Box2D-0.1.0.0: upstream/src/prismatic_joint.c
// SPDX-FileCopyrightText: 2023 Erin Catto
// SPDX-License-Identifier: MIT
#include "body.h"
#include "core.h"
#include "joint.h"
#include "physics_world.h"
#include "recording.h"
#include "solver.h"
#include "solver_set.h"
// needed for dll export
#include "box2d/box2d.h"
void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring )
{
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointEnableSpring, jointId, enableSpring );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
if ( enableSpring != joint->prismaticJoint.enableSpring )
{
joint->prismaticJoint.enableSpring = enableSpring;
joint->prismaticJoint.springImpulse = 0.0f;
}
}
bool b2PrismaticJoint_IsSpringEnabled( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.enableSpring;
}
void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz )
{
B2_CHECK_INPUT( b2IsValidFloat( hertz ) );
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointSetSpringHertz, jointId, hertz );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
joint->prismaticJoint.hertz = hertz;
}
float b2PrismaticJoint_GetSpringHertz( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.hertz;
}
void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )
{
B2_CHECK_INPUT( b2IsValidFloat( dampingRatio ) );
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointSetSpringDampingRatio, jointId, dampingRatio );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
joint->prismaticJoint.dampingRatio = dampingRatio;
}
float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.dampingRatio;
}
void b2PrismaticJoint_SetTargetTranslation( b2JointId jointId, float translation )
{
B2_CHECK_INPUT( b2IsValidFloat( translation ) );
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointSetTargetTranslation, jointId, translation );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
joint->prismaticJoint.targetTranslation = translation;
}
float b2PrismaticJoint_GetTargetTranslation( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.targetTranslation;
}
void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit )
{
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointEnableLimit, jointId, enableLimit );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
if ( enableLimit != joint->prismaticJoint.enableLimit )
{
joint->prismaticJoint.enableLimit = enableLimit;
joint->prismaticJoint.lowerImpulse = 0.0f;
joint->prismaticJoint.upperImpulse = 0.0f;
}
}
bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.enableLimit;
}
float b2PrismaticJoint_GetLowerLimit( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.lowerTranslation;
}
float b2PrismaticJoint_GetUpperLimit( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.upperTranslation;
}
void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper )
{
B2_CHECK_INPUT( b2IsValidFloat( lower ) );
B2_CHECK_INPUT( b2IsValidFloat( upper ) );
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointSetLimits, jointId, lower, upper );
B2_ASSERT( lower <= upper );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
joint->prismaticJoint.lowerTranslation = b2MinFloat( lower, upper );
joint->prismaticJoint.upperTranslation = b2MaxFloat( lower, upper );
}
void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor )
{
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointEnableMotor, jointId, enableMotor );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
if ( enableMotor != joint->prismaticJoint.enableMotor )
{
joint->prismaticJoint.enableMotor = enableMotor;
joint->prismaticJoint.motorImpulse = 0.0f;
}
}
bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.enableMotor;
}
void b2PrismaticJoint_SetTargetMotorSpeed( b2JointId jointId, float motorSpeed )
{
B2_CHECK_INPUT( b2IsValidFloat( motorSpeed ) );
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointSetTargetMotorSpeed, jointId, motorSpeed );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
joint->prismaticJoint.motorSpeed = motorSpeed;
}
float b2PrismaticJoint_GetTargetMotorSpeed( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.motorSpeed;
}
float b2PrismaticJoint_GetCurrentMotorForce( b2JointId jointId )
{
b2World* world = b2GetWorld( jointId.world0 );
b2JointSim* base = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return world->inv_h * base->prismaticJoint.motorImpulse;
}
void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force )
{
B2_CHECK_INPUT( b2IsValidFloat( force ) );
b2World* world = b2GetWorld( jointId.world0 );
B2_REC( world, PrismaticJointSetMaxMotorForce, jointId, force );
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
joint->prismaticJoint.maxMotorForce = force;
}
float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId )
{
b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
return joint->prismaticJoint.maxMotorForce;
}
float b2PrismaticJoint_GetCurrentTranslation( b2JointId jointId )
{
b2World* world = b2GetWorld( jointId.world0 );
b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
// Relative to body A so the difference stays in float precision far from the origin
b2WorldTransform wxfA = b2GetBodyTransform( world, jointSim->bodyIdA );
b2Transform transformA = b2ToRelativeTransform( wxfA, wxfA.p );
b2Transform transformB = b2ToRelativeTransform( b2GetBodyTransform( world, jointSim->bodyIdB ), wxfA.p );
b2Vec2 localAxisA = b2RotateVector( jointSim->localFrameA.q, (b2Vec2){ 1.0f, 0.0f } );
b2Vec2 axisA = b2RotateVector( transformA.q, localAxisA );
b2Vec2 pA = b2TransformPoint( transformA, jointSim->localFrameA.p );
b2Vec2 pB = b2TransformPoint( transformB, jointSim->localFrameB.p );
b2Vec2 d = b2Sub( pB, pA );
float translation = b2Dot( d, axisA );
return translation;
}
float b2PrismaticJoint_GetCurrentSpeed( b2JointId jointId )
{
b2World* world = b2GetWorld( jointId.world0 );
b2Joint* joint = b2GetJointFullId( world, jointId );
B2_ASSERT( joint->type == b2_prismaticJoint );
b2JointSim* base = b2GetJointSim( world, joint );
B2_ASSERT( base->type == b2_prismaticJoint );
b2Body* bodyA = b2Array_Get( world->bodies, base->bodyIdA );
b2Body* bodyB = b2Array_Get( world->bodies, base->bodyIdB );
b2BodySim* bodySimA = b2GetBodySim( world, bodyA );
b2BodySim* bodySimB = b2GetBodySim( world, bodyB );
b2BodyState* bodyStateA = b2GetBodyState( world, bodyA );
b2BodyState* bodyStateB = b2GetBodyState( world, bodyB );
b2Rot qA = bodySimA->transform.q;
b2Rot qB = bodySimB->transform.q;
b2Vec2 localAxisA = b2RotateVector( base->localFrameA.q, (b2Vec2){ 1.0f, 0.0f } );
b2Vec2 axisA = b2RotateVector( qA, localAxisA );
b2Vec2 rA = b2RotateVector( qA, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );
b2Vec2 rB = b2RotateVector( qB, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );
// Difference the centers in double so the speed stays exact far from the origin
b2Vec2 dc = b2SubPos( bodySimB->center, bodySimA->center );
b2Vec2 d = b2Add( dc, b2Sub( rB, rA ) );
b2Vec2 vA = bodyStateA ? bodyStateA->linearVelocity : b2Vec2_zero;
b2Vec2 vB = bodyStateB ? bodyStateB->linearVelocity : b2Vec2_zero;
float wA = bodyStateA ? bodyStateA->angularVelocity : 0.0f;
float wB = bodyStateB ? bodyStateB->angularVelocity : 0.0f;
b2Vec2 vRel = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );
float speed = b2Dot( d, b2CrossSV( wA, axisA ) ) + b2Dot( axisA, vRel );
return speed;
}
b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base )
{
b2Rot qA = b2GetBodyTransform( world, base->bodyIdA ).q;
b2PrismaticJoint* joint = &base->prismaticJoint;
b2Vec2 localAxisA = b2RotateVector( base->localFrameA.q, (b2Vec2){ 1.0f, 0.0f } );
b2Vec2 axisA = b2RotateVector( qA, localAxisA );
b2Vec2 perpA = b2LeftPerp( axisA );
float inv_h = world->inv_h;
float perpForce = inv_h * joint->impulse.x;
float axialForce = inv_h * ( joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse );
b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) );
return force;
}
float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base )
{
return world->inv_h * base->prismaticJoint.impulse.y;
}
// Linear constraint (point-to-line)
// d = pB - pA = xB + rB - xA - rA
// C = dot(perp, d)
// Cdot = dot(d, cross(wA, perp)) + dot(perp, vB + cross(wB, rB) - vA - cross(wA, rA))
// = -dot(perp, vA) - dot(cross(rA + d, perp), wA) + dot(perp, vB) + dot(cross(rB, perp), vB)
// J = [-perp, -cross(rA + d, perp), perp, cross(rB, perp)]
//
// Angular constraint
// C = aB - aA + a_initial
// Cdot = wB - wA
// J = [0 0 -1 0 0 1]
//
// K = J * invM * JT
//
// J = [-a -sA a sB]
// [0 -1 0 1]
// a = perp
// sA = cross(rA + d, a) = cross(pB - xA, a)
// sB = cross(rB, a) = cross(pB - xB, a)
// Motor/Limit linear constraint
// C = dot(axA, d)
// Cdot = -dot(axA, vA) - dot(cross(rA + d, axA), wA) + dot(axA, vB) + dot(cross(rB, axA), vB)
// J = [-axA -cross(rA + d, axA) axA cross(rB, ax1)]
// Predictive limit is applied even when the limit is not active.
// Prevents a constraint speed that can lead to a constraint error in one time step.
// Want C2 = C1 + h * Cdot >= 0
// Or:
// Cdot + C1/h >= 0
// I do not apply a negative constraint error because that is handled in position correction.
// So:
// Cdot + max(C1, 0)/h >= 0
// Block Solver
// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer.
//
// The Jacobian has 2 rows:
// J = [-uT -s1 uT s2] // linear
// [0 -1 0 1] // angular
//
// u = perp
// s1 = cross(d + r1, u), s2 = cross(r2, u)
// a1 = cross(d + r1, v), a2 = cross(r2, v)
void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context )
{
B2_ASSERT( base->type == b2_prismaticJoint );
// chase body id to the solver set where the body lives
int idA = base->bodyIdA;
int idB = base->bodyIdB;
b2World* world = context->world;
b2Body* bodyA = b2Array_Get( world->bodies, idA );
b2Body* bodyB = b2Array_Get( world->bodies, idB );
B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
b2SolverSet* setA = b2Array_Get( world->solverSets, bodyA->setIndex );
b2SolverSet* setB = b2Array_Get( world->solverSets, bodyB->setIndex );
int localIndexA = bodyA->localIndex;
int localIndexB = bodyB->localIndex;
b2BodySim* bodySimA = b2Array_Get( setA->bodySims, localIndexA );
b2BodySim* bodySimB = b2Array_Get( setB->bodySims, localIndexB );
float mA = bodySimA->invMass;
float iA = bodySimA->invInertia;
float mB = bodySimB->invMass;
float iB = bodySimB->invInertia;
base->invMassA = mA;
base->invMassB = mB;
base->invIA = iA;
base->invIB = iB;
b2PrismaticJoint* joint = &base->prismaticJoint;
joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
// Compute joint anchor frames with world space rotation, relative to center of mass
joint->frameA.q = b2MulRot( bodySimA->transform.q, base->localFrameA.q );
joint->frameA.p = b2RotateVector( bodySimA->transform.q, b2Sub( base->localFrameA.p, bodySimA->localCenter ) );
joint->frameB.q = b2MulRot( bodySimB->transform.q, base->localFrameB.q );
joint->frameB.p = b2RotateVector( bodySimB->transform.q, b2Sub( base->localFrameB.p, bodySimB->localCenter ) );
// Compute the initial center delta. Incremental position updates are relative to this.
joint->deltaCenter = b2SubPos( bodySimB->center, bodySimA->center );
joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );
if ( context->enableWarmStarting == false )
{
joint->impulse = b2Vec2_zero;
joint->springImpulse = 0.0f;
joint->motorImpulse = 0.0f;
joint->lowerImpulse = 0.0f;
joint->upperImpulse = 0.0f;
}
}
void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context )
{
B2_ASSERT( base->type == b2_prismaticJoint );
float mA = base->invMassA;
float mB = base->invMassB;
float iA = base->invIA;
float iB = base->invIB;
// dummy state for static bodies
b2BodyState dummyState = b2_identityBodyState;
b2PrismaticJoint* joint = &base->prismaticJoint;
b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );
b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );
b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );
b2Vec2 axisA = b2RotateVector( joint->frameA.q, (b2Vec2){ 1.0f, 0.0f } );
axisA = b2RotateVector( stateA->deltaRotation, axisA );
// impulse is applied at anchor point on body B
float a1 = b2Cross( b2Add( rA, d ), axisA );
float a2 = b2Cross( rB, axisA );
float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
// perpendicular constraint
b2Vec2 perpA = b2LeftPerp( axisA );
float s1 = b2Cross( b2Add( rA, d ), perpA );
float s2 = b2Cross( rB, perpA );
float perpImpulse = joint->impulse.x;
float angleImpulse = joint->impulse.y;
b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( perpImpulse, perpA ) );
float LA = axialImpulse * a1 + perpImpulse * s1 + angleImpulse;
float LB = axialImpulse * a2 + perpImpulse * s2 + angleImpulse;
if ( stateA->flags & b2_dynamicFlag )
{
stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P );
stateA->angularVelocity -= iA * LA;
}
if ( stateB->flags & b2_dynamicFlag )
{
stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P );
stateB->angularVelocity += iB * LB;
}
}
void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias )
{
B2_ASSERT( base->type == b2_prismaticJoint );
float mA = base->invMassA;
float mB = base->invMassB;
float iA = base->invIA;
float iB = base->invIB;
// dummy state for static bodies
b2BodyState dummyState = b2_identityBodyState;
b2PrismaticJoint* joint = &base->prismaticJoint;
b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
b2Vec2 vA = stateA->linearVelocity;
float wA = stateA->angularVelocity;
b2Vec2 vB = stateB->linearVelocity;
float wB = stateB->angularVelocity;
b2Rot qA = b2MulRot( stateA->deltaRotation, joint->frameA.q );
b2Rot qB = b2MulRot( stateB->deltaRotation, joint->frameB.q );
b2Rot relQ = b2InvMulRot( qA, qB );
// current anchors
b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->frameA.p );
b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->frameB.p );
b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );
b2Vec2 axisA = b2RotateVector( joint->frameA.q, (b2Vec2){ 1.0f, 0.0f } );
axisA = b2RotateVector( stateA->deltaRotation, axisA );
float translation = b2Dot( axisA, d );
// These scalars are for torques generated by axial forces
float a1 = b2Cross( b2Add( rA, d ), axisA );
float a2 = b2Cross( rB, axisA );
float k = mA + mB + iA * a1 * a1 + iB * a2 * a2;
float axialMass = k > 0.0f ? 1.0f / k : 0.0f;
b2Softness softness = base->constraintSoftness;
// spring constraint
if ( joint->enableSpring )
{
// This is a real spring and should be applied even during relax
float C = translation - joint->targetTranslation;
float bias = joint->springSoftness.biasRate * C;
float massScale = joint->springSoftness.massScale;
float impulseScale = joint->springSoftness.impulseScale;
float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;
float deltaImpulse = -massScale * axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse;
joint->springImpulse += deltaImpulse;
b2Vec2 P = b2MulSV( deltaImpulse, axisA );
float LA = deltaImpulse * a1;
float LB = deltaImpulse * a2;
vA = b2MulSub( vA, mA, P );
wA -= iA * LA;
vB = b2MulAdd( vB, mB, P );
wB += iB * LB;
}
// Solve motor constraint
if ( joint->enableMotor )
{
float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;
float impulse = axialMass * ( joint->motorSpeed - Cdot );
float oldImpulse = joint->motorImpulse;
float maxImpulse = context->h * joint->maxMotorForce;
joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );
impulse = joint->motorImpulse - oldImpulse;
b2Vec2 P = b2MulSV( impulse, axisA );
float LA = impulse * a1;
float LB = impulse * a2;
vA = b2MulSub( vA, mA, P );
wA -= iA * LA;
vB = b2MulAdd( vB, mB, P );
wB += iB * LB;
}
if ( joint->enableLimit )
{
// Clamp the speculative distance to a reasonable value
float speculativeDistance = 0.25f * ( joint->upperTranslation - joint->lowerTranslation );
// Lower limit
{
float C = translation - joint->lowerTranslation;
if ( C < speculativeDistance )
{
float bias = 0.0f;
float massScale = 1.0f;
float impulseScale = 0.0f;
if ( C > 0.0f )
{
// speculation
float safe = b2GetLengthUnitsPerMeter();
bias = b2MinFloat( C, safe ) * context->inv_h;
}
else if ( useBias )
{
bias = softness.biasRate * C;
massScale = softness.massScale;
impulseScale = softness.impulseScale;
}
float oldImpulse = joint->lowerImpulse;
float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;
float deltaImpulse = -axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse;
joint->lowerImpulse = b2MaxFloat( oldImpulse + deltaImpulse, 0.0f );
deltaImpulse = joint->lowerImpulse - oldImpulse;
b2Vec2 P = b2MulSV( deltaImpulse, axisA );
float LA = deltaImpulse * a1;
float LB = deltaImpulse * a2;
vA = b2MulSub( vA, mA, P );
wA -= iA * LA;
vB = b2MulAdd( vB, mB, P );
wB += iB * LB;
}
else
{
joint->lowerImpulse = 0.0f;
}
}
// Upper limit
// Note: signs are flipped to keep C positive when the constraint is satisfied.
// This also keeps the impulse positive when the limit is active.
{
// sign flipped
float C = joint->upperTranslation - translation;
if ( C < speculativeDistance )
{
float bias = 0.0f;
float massScale = 1.0f;
float impulseScale = 0.0f;
if ( C > 0.0f )
{
// speculation
float safe = b2GetLengthUnitsPerMeter();
bias = b2MinFloat( C, safe ) * context->inv_h;
}
else if ( useBias )
{
bias = softness.biasRate * C;
massScale = softness.massScale;
impulseScale = softness.impulseScale;
}
float oldImpulse = joint->upperImpulse;
// sign flipped
float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB;
float deltaImpulse = -axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse;
joint->upperImpulse = b2MaxFloat( oldImpulse + deltaImpulse, 0.0f );
deltaImpulse = joint->upperImpulse - oldImpulse;
b2Vec2 P = b2MulSV( deltaImpulse, axisA );
float LA = deltaImpulse * a1;
float LB = deltaImpulse * a2;
// sign flipped
vA = b2MulAdd( vA, mA, P );
wA += iA * LA;
vB = b2MulSub( vB, mB, P );
wB -= iB * LB;
}
else
{
joint->upperImpulse = 0.0f;
}
}
}
// Solve the prismatic constraint in block form
{
b2Vec2 perpA = b2LeftPerp( axisA );
// These scalars are for torques generated by the perpendicular constraint force
float s1 = b2Cross( b2Add( d, rA ), perpA );
float s2 = b2Cross( rB, perpA );
b2Vec2 Cdot;
Cdot.x = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA;
Cdot.y = wB - wA;
b2Vec2 bias = b2Vec2_zero;
float massScale = 1.0f;
float impulseScale = 0.0f;
if ( useBias )
{
b2Vec2 C;
C.x = b2Dot( perpA, d );
C.y = b2Rot_GetAngle( relQ );
bias = b2MulSV( softness.biasRate, C );
massScale = softness.massScale;
impulseScale = softness.impulseScale;
}
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if ( k22 == 0.0f )
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
b2Mat22 K = { { k11, k12 }, { k12, k22 } };
b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) );
b2Vec2 deltaImpulse;
deltaImpulse.x = -massScale * b.x - impulseScale * joint->impulse.x;
deltaImpulse.y = -massScale * b.y - impulseScale * joint->impulse.y;
joint->impulse.x += deltaImpulse.x;
joint->impulse.y += deltaImpulse.y;
b2Vec2 P = b2MulSV( deltaImpulse.x, perpA );
float LA = deltaImpulse.x * s1 + deltaImpulse.y;
float LB = deltaImpulse.x * s2 + deltaImpulse.y;
vA = b2MulSub( vA, mA, P );
wA -= iA * LA;
vB = b2MulAdd( vB, mB, P );
wB += iB * LB;
}
B2_ASSERT( b2IsValidVec2( vA ) );
B2_ASSERT( b2IsValidFloat( wA ) );
B2_ASSERT( b2IsValidVec2( vB ) );
B2_ASSERT( b2IsValidFloat( wB ) );
if ( stateA->flags & b2_dynamicFlag )
{
stateA->linearVelocity = vA;
stateA->angularVelocity = wA;
}
if ( stateB->flags & b2_dynamicFlag )
{
stateB->linearVelocity = vB;
stateB->angularVelocity = wB;
}
}
void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2WorldTransform transformA, b2WorldTransform transformB,
float drawScale )
{
B2_ASSERT( base->type == b2_prismaticJoint );
b2PrismaticJoint* joint = &base->prismaticJoint;
b2WorldTransform frameA = b2MulWorldTransforms( transformA, base->localFrameA );
b2WorldTransform frameB = b2MulWorldTransforms( transformB, base->localFrameB );
b2Vec2 axisA = b2RotateVector( frameA.q, (b2Vec2){ 1.0f, 0.0f } );
draw->DrawLineFcn( frameA.p, frameB.p, b2_colorDimGray, draw->context );
if ( joint->enableLimit )
{
float b = 0.25f * drawScale;
b2Pos lower = b2OffsetPos( frameA.p, b2MulSV( joint->lowerTranslation, axisA ) );
b2Pos upper = b2OffsetPos( frameA.p, b2MulSV( joint->upperTranslation, axisA ) );
b2Vec2 perp = b2LeftPerp( axisA );
draw->DrawLineFcn( lower, upper, b2_colorGray, draw->context );
draw->DrawLineFcn( b2OffsetPos( lower, b2MulSV( -b, perp ) ), b2OffsetPos( lower, b2MulSV( b, perp ) ), b2_colorGreen,
draw->context );
draw->DrawLineFcn( b2OffsetPos( upper, b2MulSV( -b, perp ) ), b2OffsetPos( upper, b2MulSV( b, perp ) ), b2_colorRed,
draw->context );
}
else
{
draw->DrawLineFcn( b2OffsetPos( frameA.p, b2Neg( axisA ) ), b2OffsetPos( frameA.p, axisA ), b2_colorGray,
draw->context );
}
if ( joint->enableSpring )
{
b2Pos p = b2OffsetPos( frameA.p, b2MulSV( joint->targetTranslation, axisA ) );
draw->DrawPointFcn( p, 8.0f, b2_colorViolet, draw->context );
}
draw->DrawPointFcn( frameA.p, 5.0f, b2_colorGray, draw->context );
draw->DrawPointFcn( frameB.p, 5.0f, b2_colorBlue, draw->context );
}