This commit is contained in:
Erin Catto 2026-10-14 12:13:41 -07:00
commit 91dae873e2
6 changed files with 256 additions and 246 deletions

View file

@ -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;
}

View file

@ -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;

View file

@ -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 );

View file

@ -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 );

View file

@ -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 );

View file

@ -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