diff --git a/src/joint.h b/src/joint.h index 64171b9..474e82b 100644 --- a/src/joint.h +++ b/src/joint.h @@ -413,3 +413,45 @@ void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); + +static inline b3Vec3 b3SolvePointConstraint( float mA, float mB, b3Matrix3 iA, b3Matrix3 iB, b3Vec3 rA, b3Vec3 rB, b3Vec3 rhs ) +{ + float axy = rA.z * iA.cy.y - rA.y * iA.cz.y; + float axz = rA.z * iA.cy.z - rA.y * iA.cz.z; + b3Vec3 ay = b3Sub( b3MulSV( rA.x, iA.cz ), b3MulSV( rA.z, iA.cx ) ); + b3Vec3 az = b3Sub( b3MulSV( rA.y, iA.cx ), b3MulSV( rA.x, iA.cy ) ); + + float bxy = rB.z * iB.cy.y - rB.y * iB.cz.y; + float bxz = rB.z * iB.cy.z - rB.y * iB.cz.z; + b3Vec3 by = b3Sub( b3MulSV( rB.x, iB.cz ), b3MulSV( rB.z, iB.cx ) ); + b3Vec3 bz = b3Sub( b3MulSV( rB.y, iB.cx ), b3MulSV( rB.x, iB.cy ) ); + + float m = mA + mB; + float kxx = ( axy * rA.z - axz * rA.y ) + ( bxy * rB.z - bxz * rB.y ) + m; + float kyy = ( ay.z * rA.x - ay.x * rA.z ) + ( by.z * rB.x - by.x * rB.z ) + m; + float kzz = ( az.x * rA.y - az.y * rA.x ) + ( bz.x * rB.y - bz.y * rB.x ) + m; + float kxy = ( ay.y * rA.z - ay.z * rA.y ) + ( by.y * rB.z - by.z * rB.y ); + float kxz = ( az.y * rA.z - az.z * rA.y ) + ( bz.y * rB.z - bz.z * rB.y ); + float kyz = ( az.z * rA.x - az.x * rA.z ) + ( bz.z * rB.x - bz.x * rB.z ); + + float cxx = kyy * kzz - kyz * kyz; + float cyy = kxx * kzz - kxz * kxz; + float czz = kxx * kyy - kxy * kxy; + float cxy = kxz * kyz - kxy * kzz; + float cxz = kxy * kyz - kyy * kxz; + float cyz = kxy * kxz - kxx * kyz; + + float det = kxx * cxx + kxy * cxy + kxz * cxz; + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Vec3 b = { + invDet * ( cxx * rhs.x + cxy * rhs.y + cxz * rhs.z ), + invDet * ( cxy * rhs.x + cyy * rhs.y + cyz * rhs.z ), + invDet * ( cxz * rhs.x + cyz * rhs.y + czz * rhs.z ), + }; + return b; + } + + return b3Vec3_zero; +} diff --git a/src/motor_joint.c b/src/motor_joint.c index 734d497..dc8913c 100644 --- a/src/motor_joint.c +++ b/src/motor_joint.c @@ -361,16 +361,7 @@ void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ) b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - b3Matrix3 sA = b3Skew( rA ); - b3Matrix3 sB = b3Skew( rB ); - b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); - b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); - b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); - k.cx.x += mA + mB; - k.cy.y += mA + mB; - k.cz.z += mA + mB; - - b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) ); b3Vec3 oldImpulse = joint->linearSpringImpulse; b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, oldImpulse ); @@ -396,16 +387,7 @@ void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ) b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); cdot = b3Sub( cdot, joint->linearVelocity ); //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - b3Matrix3 sA = b3Skew( rA ); - b3Matrix3 sB = b3Skew( rB ); - b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); - b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); - b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); - k.cx.x += mA + mB; - k.cy.y += mA + mB; - k.cz.z += mA + mB; - - b3Vec3 b = b3Solve3( k, cdot ); + b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, cdot ); b3Vec3 impulse = b3Neg( b ); b3Vec3 oldImpulse = joint->linearVelocityImpulse; diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 7856a83..0e568f1 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -411,106 +411,111 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia b3Quat relQ = b3InvMulQuat( quatA, quatB ); - // Solve spring - if ( joint->enableSpring && fixedRotation == false ) + if ( fixedRotation == false && ( joint->enableSpring || joint->enableMotor || joint->enableLimit ) ) { - // Get the substep relative rotation - float targetAngle = joint->targetAngle; - float angle = b3GetTwistAngle( relQ ); - float c = angle - targetAngle; - - float bias = joint->springSoftness.biasRate * c; - float massScale = joint->springSoftness.massScale; - float impulseScale = joint->springSoftness.impulseScale; - float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ ); - - float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * joint->springImpulse; - joint->springImpulse += deltaImpulse; - - wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) ); - wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) ); - } - - if ( joint->enableMotor && fixedRotation == false ) - { - float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ ) - joint->motorSpeed; - - float deltaImpulse = -joint->axialMass * cdot; - float newImpulse = joint->motorImpulse + deltaImpulse; - float maxImpulse = joint->maxMotorTorque * context->h; - newImpulse = b3ClampFloat( newImpulse, -maxImpulse, maxImpulse ); - deltaImpulse = newImpulse - joint->motorImpulse; - joint->motorImpulse = newImpulse; - - wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) ); - wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) ); - } - - if ( joint->enableLimit && fixedRotation == false ) - { - float angle = b3GetTwistAngle( relQ ); - - // todo does an updated twist axis help? - b3Vec3 axis = joint->rotationAxisZ; + b3Vec3 gA = b3MulMV( iA, axis ); + b3Vec3 gB = b3MulMV( iB, axis ); + float k = b3Dot( axis, b3Add( gA, gB ) ); + float axialCdot = b3Dot( b3Sub( wB, wA ), axis ); + float axialImpulse = 0.0f; - // Lower limit + // Solve spring + if ( joint->enableSpring ) { - float c = angle - joint->lowerAngle; - float bias = 0.0f; - float massScale = 1.0f; - float impulseScale = 0.0f; - if ( c > 0.0f ) - { - // speculation - bias = c * context->inv_h; - } - else if ( useBias ) - { - bias = base->constraintSoftness.biasRate * c; - massScale = base->constraintSoftness.massScale; - impulseScale = base->constraintSoftness.impulseScale; - } + // Get the substep relative rotation + float targetAngle = joint->targetAngle; + float angle = b3GetTwistAngle( relQ ); + float c = angle - targetAngle; - float cdot = b3Dot( b3Sub( wB, wA ), axis ); - float oldImpulse = joint->lowerImpulse; - float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse; - joint->lowerImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); - deltaImpulse = joint->lowerImpulse - oldImpulse; + float bias = joint->springSoftness.biasRate * c; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; - wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, axis ) ); - wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, axis ) ); + float deltaImpulse = -massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += deltaImpulse; + + axialCdot += k * deltaImpulse; + axialImpulse += deltaImpulse; } - // Upper limit + if ( joint->enableMotor ) { - float c = joint->upperAngle - angle; - float bias = 0.0f; - float massScale = 1.0f; - float impulseScale = 0.0f; - if ( c > 0.0f ) - { - // speculation - bias = c * context->inv_h; - } - else if ( useBias ) - { - bias = base->constraintSoftness.biasRate * c; - massScale = base->constraintSoftness.massScale; - impulseScale = base->constraintSoftness.impulseScale; - } + float cdot = axialCdot - joint->motorSpeed; + float oldImpulse = joint->motorImpulse; + float maxImpulse = joint->maxMotorTorque * context->h; + float deltaImpulse = b3ClampFloat( -joint->axialMass * cdot, -maxImpulse - oldImpulse, maxImpulse - oldImpulse ); + joint->motorImpulse = oldImpulse + deltaImpulse; - // sign flipped on Cdot - float cdot = b3Dot( b3Sub( wA, wB ), axis ); - float oldImpulse = joint->upperImpulse; - float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse; - joint->upperImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); - deltaImpulse = joint->upperImpulse - oldImpulse; - - // sign flipped on applied impulse - wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, axis ) ); - wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, axis ) ); + axialCdot += k * deltaImpulse; + axialImpulse += deltaImpulse; } + + if ( joint->enableLimit ) + { + float angle = b3GetTwistAngle( relQ ); + + // todo does an updated twist axis help? + + // Lower limit + { + float c = angle - joint->lowerAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float oldImpulse = joint->lowerImpulse; + float deltaImpulse = + b3MaxFloat( -massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * oldImpulse, -oldImpulse ); + joint->lowerImpulse = oldImpulse + deltaImpulse; + + axialCdot += k * deltaImpulse; + axialImpulse += deltaImpulse; + } + + // Upper limit + { + float c = joint->upperAngle - angle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float cdot = -axialCdot; + float oldImpulse = joint->upperImpulse; + float deltaImpulse = + b3MaxFloat( -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse, -oldImpulse ); + joint->upperImpulse = oldImpulse + deltaImpulse; + + // sign flipped on applied impulse + axialImpulse -= deltaImpulse; + } + } + + wA = b3MulSub( wA, axialImpulse, gA ); + wB = b3MulAdd( wB, axialImpulse, gB ); } // Collinearity constraint @@ -578,16 +583,7 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia } //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - b3Matrix3 sA = b3Skew( rA ); - b3Matrix3 sB = b3Skew( rB ); - b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); - b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); - b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); - k.cx.x += mA + mB; - k.cy.y += mA + mB; - k.cz.z += mA + mB; - - b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) ); b3Vec3 impulse = b3Sub( b3MulSV( -massScale, b ), b3MulSV( impulseScale, joint->linearImpulse ) ); joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); diff --git a/src/spherical_joint.c b/src/spherical_joint.c index c2c1054..537cc8a 100644 --- a/src/spherical_joint.c +++ b/src/spherical_joint.c @@ -454,86 +454,132 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi b3Quat relQ = b3InvMulQuat( quatA, quatB ); - // Solve spring - if ( joint->enableSpring && fixedRotation == false ) + if ( fixedRotation == false && + ( joint->enableSpring || joint->enableMotor || joint->enableTwistLimit || joint->enableConeLimit ) ) { - // Rotation constraint error - b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, joint->targetRotation ); - b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); - - b3Vec3 bias = b3MulSV( joint->springSoftness.biasRate, c ); - float massScale = joint->springSoftness.massScale; - float impulseScale = joint->springSoftness.impulseScale; + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); b3Vec3 cdot = b3Sub( wB, wA ); + b3Vec3 angularImpulse = b3Vec3_zero; - b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ), - impulseScale, joint->springImpulse ); - joint->springImpulse = b3Add( joint->springImpulse, impulse ); - - wA = b3Sub( wA, b3MulMV( iA, impulse ) ); - wB = b3Add( wB, b3MulMV( iB, impulse ) ); - } - - if ( joint->enableMotor && fixedRotation == false ) - { - b3Vec3 cdot = b3Sub( wB, wA ); - - // todo it might be more accurate to have the motor velocity in frameA due to sub-stepping. - b3Vec3 lambda = b3Neg( b3MulMV( joint->rotationMass, b3Sub( cdot, joint->motorVelocity ) ) ); - b3Vec3 newImpulse = b3Add( joint->motorImpulse, lambda ); - float length = b3Length( newImpulse ); - float maxImpulse = joint->maxMotorTorque * context->h; - if ( length > maxImpulse ) + // Solve spring + if ( joint->enableSpring ) { - newImpulse = b3MulSV( maxImpulse / length, newImpulse ); + // Rotation constraint error + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, joint->targetRotation ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + b3Vec3 bias = b3MulSV( joint->springSoftness.biasRate, c ); + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ), + impulseScale, joint->springImpulse ); + joint->springImpulse = b3Add( joint->springImpulse, impulse ); + + cdot = b3Add( cdot, b3MulMV( invInertiaSum, impulse ) ); + angularImpulse = b3Add( angularImpulse, impulse ); } - lambda = b3Sub( newImpulse, joint->motorImpulse ); - joint->motorImpulse = newImpulse; - - wA = b3Sub( wA, b3MulMV( iA, lambda ) ); - wB = b3Add( wB, b3MulMV( iB, lambda ) ); - } - - if ( joint->enableTwistLimit && fixedRotation == false ) - { - float twistAngle = b3GetTwistAngle( relQ ); - - // todo does an updated twist axis help? - - b3Vec3 twistJacobian = joint->twistJacobian; - - // Lower limit + if ( joint->enableMotor ) { - float c = twistAngle - joint->lowerTwistAngle; - float bias = 0.0f; - float massScale = 1.0f; - float impulseScale = 0.0f; - if ( c > 0.0f ) + // todo it might be more accurate to have the motor velocity in frameA due to sub-stepping. + b3Vec3 lambda = b3Neg( b3MulMV( joint->rotationMass, b3Sub( cdot, joint->motorVelocity ) ) ); + b3Vec3 newImpulse = b3Add( joint->motorImpulse, lambda ); + float length = b3Length( newImpulse ); + float maxImpulse = joint->maxMotorTorque * context->h; + if ( length > maxImpulse ) { - // speculation - bias = c * context->inv_h; - } - else if ( useBias ) - { - bias = base->constraintSoftness.biasRate * c; - massScale = base->constraintSoftness.massScale; - impulseScale = base->constraintSoftness.impulseScale; + newImpulse = b3MulSV( maxImpulse / length, newImpulse ); } - float cdot = b3Dot( b3Sub( wB, wA ), twistJacobian ); - float oldImpulse = joint->lowerTwistImpulse; - float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse; - joint->lowerTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); - deltaImpulse = joint->lowerTwistImpulse - oldImpulse; + lambda = b3Sub( newImpulse, joint->motorImpulse ); + joint->motorImpulse = newImpulse; - wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); - wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); + cdot = b3Add( cdot, b3MulMV( invInertiaSum, lambda ) ); + angularImpulse = b3Add( angularImpulse, lambda ); } - // Upper limit + if ( joint->enableTwistLimit ) { - float c = joint->upperTwistAngle - twistAngle; + float twistAngle = b3GetTwistAngle( relQ ); + + // todo does an updated twist axis help? + + b3Vec3 twistJacobian = joint->twistJacobian; + b3Vec3 twistResponse = b3MulMV( invInertiaSum, twistJacobian ); + + // Lower limit + { + float c = twistAngle - joint->lowerTwistAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float twistCdot = b3Dot( cdot, twistJacobian ); + float oldImpulse = joint->lowerTwistImpulse; + float deltaImpulse = + b3MaxFloat( -massScale * joint->twistMass * ( twistCdot + bias ) - impulseScale * oldImpulse, -oldImpulse ); + joint->lowerTwistImpulse = oldImpulse + deltaImpulse; + + cdot = b3MulAdd( cdot, deltaImpulse, twistResponse ); + angularImpulse = b3MulAdd( angularImpulse, deltaImpulse, twistJacobian ); + } + + // Upper limit + { + float c = joint->upperTwistAngle - twistAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float twistCdot = -b3Dot( cdot, twistJacobian ); + float oldImpulse = joint->upperTwistImpulse; + float deltaImpulse = + b3MaxFloat( -massScale * joint->twistMass * ( twistCdot + bias ) - impulseScale * oldImpulse, -oldImpulse ); + joint->upperTwistImpulse = oldImpulse + deltaImpulse; + + // sign flipped on applied impulse + cdot = b3MulSub( cdot, deltaImpulse, twistResponse ); + angularImpulse = b3MulSub( angularImpulse, deltaImpulse, twistJacobian ); + } + } + + if ( joint->enableConeLimit ) + { + float swingAngle = b3GetSwingAngle( relQ ); + + // todo does an updated swing axis help? + // b3Vec3 axisA = b3RotateVector( quatA, b3Vec3_axisZ ); + // b3Vec3 axisB = b3RotateVector( quatB, b3Vec3_axisZ ); + // b3Vec3 swingAxis = b3Normalize( b3Cross( axisA, axisB ) ); + // joint->swingAxis = swingAxis; + + b3Vec3 swingAxis = joint->swingAxis; + + float c = joint->coneAngle - swingAngle; float bias = 0.0f; float massScale = 1.0f; float impulseScale = 0.0f; @@ -550,56 +596,18 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi } // sign flipped on Cdot - float cdot = b3Dot( b3Sub( wA, wB ), twistJacobian ); - float oldImpulse = joint->upperTwistImpulse; - float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse; - joint->upperTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); - deltaImpulse = joint->upperTwistImpulse - oldImpulse; + float swingCdot = -b3Dot( cdot, swingAxis ); + float oldImpulse = joint->swingImpulse; + float deltaImpulse = + b3MaxFloat( -massScale * joint->swingMass * ( swingCdot + bias ) - impulseScale * oldImpulse, -oldImpulse ); + joint->swingImpulse = oldImpulse + deltaImpulse; // sign flipped on applied impulse - wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); - wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); - } - } - - if ( joint->enableConeLimit && fixedRotation == false ) - { - float swingAngle = b3GetSwingAngle( relQ ); - - // todo does an updated swing axis help? - // b3Vec3 axisA = b3RotateVector( quatA, b3Vec3_axisZ ); - // b3Vec3 axisB = b3RotateVector( quatB, b3Vec3_axisZ ); - // b3Vec3 swingAxis = b3Normalize( b3Cross( axisA, axisB ) ); - // joint->swingAxis = swingAxis; - - b3Vec3 swingAxis = joint->swingAxis; - - float c = joint->coneAngle - swingAngle; - float bias = 0.0f; - float massScale = 1.0f; - float impulseScale = 0.0f; - if ( c > 0.0f ) - { - // speculation - bias = c * context->inv_h; - } - else if ( useBias ) - { - bias = base->constraintSoftness.biasRate * c; - massScale = base->constraintSoftness.massScale; - impulseScale = base->constraintSoftness.impulseScale; + angularImpulse = b3MulSub( angularImpulse, deltaImpulse, swingAxis ); } - // sign flipped on Cdot - float cdot = b3Dot( b3Sub( wA, wB ), swingAxis ); - float oldImpulse = joint->swingImpulse; - float deltaImpulse = -massScale * joint->swingMass * ( cdot + bias ) - impulseScale * oldImpulse; - joint->swingImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); - deltaImpulse = joint->swingImpulse - oldImpulse; - - // sign flipped on applied impulse - wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, swingAxis ) ); - wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, swingAxis ) ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); } // Solve point-to-point constraint @@ -626,16 +634,7 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi } //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - b3Matrix3 sA = b3Skew( rA ); - b3Matrix3 sB = b3Skew( rB ); - b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); - b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); - b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); - k.cx.x += mA + mB; - k.cy.y += mA + mB; - k.cz.z += mA + mB; - - b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) ); b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse ); joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); diff --git a/src/weld_joint.c b/src/weld_joint.c index 86b62ed..075bbdf 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -270,16 +270,7 @@ void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ) } //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - b3Matrix3 sA = b3Skew( rA ); - b3Matrix3 sB = b3Skew( rB ); - b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); - b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); - b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); - k.cx.x += mA + mB; - k.cy.y += mA + mB; - k.cz.z += mA + mB; - - b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + b3Vec3 b = b3SolvePointConstraint( mA, mB, base->invIA, base->invIB, rA, rB, b3Add( cdot, bias ) ); b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse ); joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); diff --git a/test/test_determinism.c b/test/test_determinism.c index 96980e2..966c4ba 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -19,8 +19,8 @@ // Double precision accumulates body positions in double, so the settle/sleep step and the // state hash differ from the float build. Both modes are internally deterministic. #if defined( BOX3D_DOUBLE_PRECISION ) -#define RAGDOLL_SLEEP_STEP 264 -#define RAGDOLL_HASH 0xAB4EFAF6 +#define RAGDOLL_SLEEP_STEP 267 +#define RAGDOLL_HASH 0x01EB8FDB #define WAVE_PILE_SLEEP_STEP 333 #define WAVE_PILE_HASH 0x80D5266B #define QUERY_SPAWN_SLEEP_STEP 242 @@ -30,8 +30,8 @@ #define MESH_DROP_SLEEP_STEP 218 #define MESH_DROP_HASH 0x10051259 #else -#define RAGDOLL_SLEEP_STEP 267 -#define RAGDOLL_HASH 0xB5FF7C69 +#define RAGDOLL_SLEEP_STEP 265 +#define RAGDOLL_HASH 0xB98B3638 #define WAVE_PILE_SLEEP_STEP 288 #define WAVE_PILE_HASH 0xFC1CD2DC #define QUERY_SPAWN_SLEEP_STEP 242