diff --git a/src/mouse_joint.c b/src/mouse_joint.c index 418ab620..ce3f2161 100644 --- a/src/mouse_joint.c +++ b/src/mouse_joint.c @@ -49,11 +49,6 @@ void b2PrepareMouse(b2Joint* base, b2StepContext* context) b2Vec2 cB = bodyB->position; b2Rot qB = bodyB->transform.q; - - b2SolverBody* solverBodyB = context->solverBodies + joint->indexB; - b2Vec2 vB = solverBodyB->linearVelocity; - float wB = solverBodyB->angularVelocity; - float mB = bodyB->invMass; float iB = bodyB->invI; @@ -88,23 +83,6 @@ void b2PrepareMouse(b2Joint* base, b2StepContext* context) joint->C = b2Add(cB, b2Sub(joint->rB, joint->targetA)); joint->C = b2MulSV(joint->beta, joint->C); - - // Cheat with some damping - wB *= B2_MAX(0.0f, 1.0f - 0.02f * (60.0f * h)); - - if (context->enableWarmStarting) - { - joint->impulse = b2MulSV(context->dtRatio, joint->impulse); - vB = b2MulAdd(vB, mB, joint->impulse); - wB += iB * b2Cross(joint->rB, joint->impulse); - } - else - { - joint->impulse = b2Vec2_zero; - } - - solverBodyB->linearVelocity = vB; - solverBodyB->angularVelocity = wB; } void b2WarmStartMouse(b2Joint* base, b2StepContext* context) diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c index cd91b057..3a510f2e 100644 --- a/src/prismatic_joint.c +++ b/src/prismatic_joint.c @@ -81,17 +81,10 @@ void b2PreparePrismatic(b2Joint* base, b2StepContext* context) joint->angleA = bodyA->angle; joint->angleB = bodyB->angle; - // This is a dummy body to represent a static body since static bodies don't have a solver body. - b2SolverBody dummyBody = {0}; - - // Note: must warm start solver bodies - b2SolverBody* solverBodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA; - float mA = solverBodyA->invMass; - float iA = solverBodyA->invI; - - b2SolverBody* solverBodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB; - float mB = solverBodyB->invMass; - float iB = solverBodyB->invI; + float mA = bodyA->invMass; + float iA = bodyA->invI; + float mB = bodyB->invMass; + float iB = bodyB->invI; b2Rot qA = bodyA->transform.q; b2Rot qB = bodyB->transform.q; @@ -129,36 +122,6 @@ void b2PreparePrismatic(b2Joint* base, b2StepContext* context) { joint->motorImpulse = 0.0f; } - - if (context->enableWarmStarting) - { - float dtRatio = context->dtRatio; - - // Soft step works best when bilateral constraints have no warm starting. - joint->impulse = b2Vec2_zero; - joint->motorImpulse *= dtRatio; - joint->lowerImpulse *= dtRatio; - joint->upperImpulse *= dtRatio; - - float axialImpulse = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; -\ - b2Vec2 P = b2MulSV(axialImpulse, axis); - float LA = axialImpulse * a1; - float LB = axialImpulse * a2; - - solverBodyA->linearVelocity = b2MulSub(solverBodyA->linearVelocity, mA, P); - solverBodyA->angularVelocity -= iA * LA; - - solverBodyB->linearVelocity = b2MulAdd(solverBodyB->linearVelocity, mB, P); - solverBodyB->angularVelocity += iB * LB; - } - else - { - joint->impulse = b2Vec2_zero; - joint->motorImpulse = 0.0f; - joint->lowerImpulse = 0.0f; - joint->upperImpulse = 0.0f; - } } void b2WarmStartPrismatic(b2Joint* base, b2StepContext* context) diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 0ac0673b..632c3a3f 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -48,15 +48,8 @@ void b2PrepareRevolute(b2Joint* base, b2StepContext* context) joint->angleA = bodyA->angle; joint->angleB = bodyB->angle; - // This is a dummy body to represent a static body since static bodies don't have a solver body. - b2SolverBody dummyBody = {0}; - - // Note: must warm start solver bodies - b2SolverBody* solverBodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA; - float iA = solverBodyA->invI; - - b2SolverBody* solverBodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB; - float iB = solverBodyB->invI; + float iA = bodyA->invI; + float iB = bodyB->invI; joint->axialMass = iA + iB; bool fixedRotation;