joints
This commit is contained in:
parent
efeee25f38
commit
91dae873e2
6 changed files with 256 additions and 246 deletions
42
src/joint.h
42
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in a new issue