box2d-ruby 1.0.0

data/ext/box2d/vendor/box2d/src/math_functions.c

@@ -0,0 +1,159 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "box2d/math_functions.h"
+
+#include <float.h>
+
+_Static_assert( sizeof( int32_t ) == sizeof( int ), "Box2D expects int32_t and int to be the same" );
+
+bool b2IsValidFloat( float a )
+{
+	if ( isnan( a ) )
+	{
+		return false;
+	}
+
+	if ( isinf( a ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b2IsValidVec2( b2Vec2 v )
+{
+	if ( isnan( v.x ) || isnan( v.y ) )
+	{
+		return false;
+	}
+
+	if ( isinf( v.x ) || isinf( v.y ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b2IsValidRotation( b2Rot q )
+{
+	if ( isnan( q.s ) || isnan( q.c ) )
+	{
+		return false;
+	}
+
+	if ( isinf( q.s ) || isinf( q.c ) )
+	{
+		return false;
+	}
+
+	return b2IsNormalizedRot( q );
+}
+
+bool b2IsValidPlane( b2Plane a )
+{
+	return b2IsValidVec2( a.normal ) && b2IsNormalized( a.normal ) && b2IsValidFloat( a.offset );
+}
+
+// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4
+float b2Atan2( float y, float x )
+{
+	// Added check for (0,0) to match atan2f and avoid NaN
+	if (x == 0.0f && y == 0.0f)
+	{
+		return 0.0f;
+	}
+
+	float ax = b2AbsFloat( x );
+	float ay = b2AbsFloat( y );
+	float mx = b2MaxFloat( ay, ax );
+	float mn = b2MinFloat( ay, ax );
+	float a = mn / mx;
+
+	// Minimax polynomial approximation to atan(a) on [0,1]
+	float s = a * a;
+	float c = s * a;
+	float q = s * s;
+	float r = 0.024840285f * q + 0.18681418f;
+	float t = -0.094097948f * q - 0.33213072f;
+	r = r * s + t;
+	r = r * c + a;
+
+	// Map to full circle
+	if ( ay > ax )
+	{
+		r = 1.57079637f - r;
+	}
+
+	if ( x < 0 )
+	{
+		r = 3.14159274f - r;
+	}
+
+	if ( y < 0 )
+	{
+		r = -r;
+	}
+
+	return r;
+}
+
+// Approximate cosine and sine for determinism. In my testing cosf and sinf produced
+// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust
+// this result.
+// https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula
+b2CosSin b2ComputeCosSin( float radians )
+{
+	float x = b2UnwindLargeAngle( radians );
+	float pi2 = B2_PI * B2_PI;
+
+	// cosine needs angle in [-pi/2, pi/2]
+	float c;
+	if ( x < -0.5f * B2_PI )
+	{
+		float y = x + B2_PI;
+		float y2 = y * y;
+		c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 );
+	}
+	else if ( x > 0.5f * B2_PI )
+	{
+		float y = x - B2_PI;
+		float y2 = y * y;
+		c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 );
+	}
+	else
+	{
+		float y2 = x * x;
+		c = ( pi2 - 4.0f * y2 ) / ( pi2 + y2 );
+	}
+
+	// sine needs angle in [0, pi]
+	float s;
+	if ( x < 0.0f )
+	{
+		float y = x + B2_PI;
+		s = -16.0f * y * ( B2_PI - y ) / ( 5.0f * pi2 - 4.0f * y * ( B2_PI - y ) );
+	}
+	else
+	{
+		s = 16.0f * x * ( B2_PI - x ) / ( 5.0f * pi2 - 4.0f * x * ( B2_PI - x ) );
+	}
+
+	float mag = sqrtf( s * s + c * c );
+	float invMag = mag > 0.0 ? 1.0f / mag : 0.0f;
+	b2CosSin cs = { c * invMag, s * invMag };
+	return cs;
+}
+
+b2Rot b2ComputeRotationBetweenUnitVectors(b2Vec2 v1, b2Vec2 v2)
+{
+	B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v1 ) ) < 100.0f * FLT_EPSILON );
+	B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v2 ) ) < 100.0f * FLT_EPSILON );
+
+	b2Rot rot;
+	rot.c = b2Dot( v1, v2 );
+	rot.s = b2Cross( v1, v2 );
+	return b2NormalizeRot( rot );
+}

data/ext/box2d/vendor/box2d/src/motor_joint.c

@@ -0,0 +1,283 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+void b2MotorJoint_SetLinearOffset( b2JointId jointId, b2Vec2 linearOffset )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.linearOffset = linearOffset;
+}
+
+b2Vec2 b2MotorJoint_GetLinearOffset( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.linearOffset;
+}
+
+void b2MotorJoint_SetAngularOffset( b2JointId jointId, float angularOffset )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.angularOffset = b2ClampFloat( angularOffset, -B2_PI, B2_PI );
+}
+
+float b2MotorJoint_GetAngularOffset( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.angularOffset;
+}
+
+void b2MotorJoint_SetMaxForce( b2JointId jointId, float maxForce )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.maxForce = b2MaxFloat( 0.0f, maxForce );
+}
+
+float b2MotorJoint_GetMaxForce( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.maxForce;
+}
+
+void b2MotorJoint_SetMaxTorque( b2JointId jointId, float maxTorque )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.maxTorque = b2MaxFloat( 0.0f, maxTorque );
+}
+
+float b2MotorJoint_GetMaxTorque( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.maxTorque;
+}
+
+void b2MotorJoint_SetCorrectionFactor( b2JointId jointId, float correctionFactor )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.correctionFactor = b2ClampFloat( correctionFactor, 0.0f, 1.0f );
+}
+
+float b2MotorJoint_GetCorrectionFactor( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.correctionFactor;
+}
+
+b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base )
+{
+	b2Vec2 force = b2MulSV( world->inv_h, base->motorJoint.linearImpulse );
+	return force;
+}
+
+float b2GetMotorJointTorque( b2World* world, b2JointSim* base )
+{
+	return world->inv_h * base->motorJoint.angularImpulse;
+}
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// C = angle2 - angle1 - referenceAngle
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_motorJoint );
+
+	// 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 = b2BodyArray_Get( &world->bodies, idA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
+
+	b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA );
+	b2BodySim* bodySimB = b2BodySimArray_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;
+
+	b2MotorJoint* joint = &base->motorJoint;
+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+
+	joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) );
+	joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+	joint->deltaCenter = b2Sub( b2Sub( bodySimB->center, bodySimA->center ), joint->linearOffset );
+	joint->deltaAngle = b2RelativeAngle( bodySimB->transform.q, bodySimA->transform.q ) - joint->angularOffset;
+	joint->deltaAngle = b2UnwindAngle( joint->deltaAngle );
+
+	b2Vec2 rA = joint->anchorA;
+	b2Vec2 rB = joint->anchorB;
+
+	b2Mat22 K;
+	K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	K.cx.y = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	K.cy.x = K.cx.y;
+	K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	joint->linearMass = b2GetInverse22( K );
+
+	float ka = iA + iB;
+	joint->angularMass = ka > 0.0f ? 1.0f / ka : 0.0f;
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b2Vec2_zero;
+		joint->angularImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context )
+{
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	b2MotorJoint* joint = &base->motorJoint;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB );
+
+	bodyA->linearVelocity = b2MulSub( bodyA->linearVelocity, mA, joint->linearImpulse );
+	bodyA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse );
+	bodyB->linearVelocity = b2MulAdd( bodyB->linearVelocity, mB, joint->linearImpulse );
+	bodyB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse );
+}
+
+void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias )
+{
+	B2_UNUSED( useBias );
+	B2_ASSERT( base->type == b2_motorJoint );
+
+	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;
+
+	b2MotorJoint* joint = &base->motorJoint;
+	b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 vA = bodyA->linearVelocity;
+	float wA = bodyA->angularVelocity;
+	b2Vec2 vB = bodyB->linearVelocity;
+	float wB = bodyB->angularVelocity;
+
+	// angular constraint
+	{
+		float angularSeperation = b2RelativeAngle( bodyB->deltaRotation, bodyA->deltaRotation ) + joint->deltaAngle;
+		angularSeperation = b2UnwindAngle( angularSeperation );
+
+		float angularBias = context->inv_h * joint->correctionFactor * angularSeperation;
+
+		float Cdot = wB - wA;
+		float impulse = -joint->angularMass * ( Cdot + angularBias );
+
+		float oldImpulse = joint->angularImpulse;
+		float maxImpulse = context->h * joint->maxTorque;
+		joint->angularImpulse = b2ClampFloat( joint->angularImpulse + impulse, -maxImpulse, maxImpulse );
+		impulse = joint->angularImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// linear constraint
+	{
+		b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA );
+		b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB );
+
+		b2Vec2 ds = b2Add( b2Sub( bodyB->deltaPosition, bodyA->deltaPosition ), b2Sub( rB, rA ) );
+		b2Vec2 linearSeparation = b2Add( joint->deltaCenter, ds );
+		b2Vec2 linearBias = b2MulSV( context->inv_h * joint->correctionFactor, linearSeparation );
+
+		b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );
+		b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, linearBias ) );
+		b2Vec2 impulse = { -b.x, -b.y };
+
+		b2Vec2 oldImpulse = joint->linearImpulse;
+		float maxImpulse = context->h * joint->maxForce;
+		joint->linearImpulse = b2Add( joint->linearImpulse, impulse );
+
+		if ( b2LengthSquared( joint->linearImpulse ) > maxImpulse * maxImpulse )
+		{
+			joint->linearImpulse = b2Normalize( joint->linearImpulse );
+			joint->linearImpulse.x *= maxImpulse;
+			joint->linearImpulse.y *= maxImpulse;
+		}
+
+		impulse = b2Sub( joint->linearImpulse, oldImpulse );
+
+		vA = b2MulSub( vA, mA, impulse );
+		wA -= iA * b2Cross( rA, impulse );
+		vB = b2MulAdd( vB, mB, impulse );
+		wB += iB * b2Cross( rB, impulse );
+	}
+
+	bodyA->linearVelocity = vA;
+	bodyA->angularVelocity = wA;
+	bodyB->linearVelocity = vB;
+	bodyB->angularVelocity = wB;
+}
+
+#if 0
+void b2DumpMotorJoint()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Dump("  b2MotorJointDef jd;\n");
+	b2Dump("  jd.bodyA = sims[%d];\n", indexA);
+	b2Dump("  jd.bodyB = sims[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.referenceAngle = %.9g;\n", m_referenceAngle);
+	b2Dump("  jd.stiffness = %.9g;\n", m_stiffness);
+	b2Dump("  jd.damping = %.9g;\n", m_damping);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
+#endif

data/ext/box2d/vendor/box2d/src/mouse_joint.c

@@ -0,0 +1,214 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+void b2MouseJoint_SetTarget( b2JointId jointId, b2Vec2 target )
+{
+	B2_ASSERT( b2IsValidVec2( target ) );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.targetA = target;
+}
+
+b2Vec2 b2MouseJoint_GetTarget( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.targetA;
+}
+
+void b2MouseJoint_SetSpringHertz( b2JointId jointId, float hertz )
+{
+	B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.hertz = hertz;
+}
+
+float b2MouseJoint_GetSpringHertz( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.hertz;
+}
+
+void b2MouseJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.dampingRatio = dampingRatio;
+}
+
+float b2MouseJoint_GetSpringDampingRatio( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.dampingRatio;
+}
+
+void b2MouseJoint_SetMaxForce( b2JointId jointId, float maxForce )
+{
+	B2_ASSERT( b2IsValidFloat( maxForce ) && maxForce >= 0.0f );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.maxForce = maxForce;
+}
+
+float b2MouseJoint_GetMaxForce( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.maxForce;
+}
+
+b2Vec2 b2GetMouseJointForce( b2World* world, b2JointSim* base )
+{
+	b2Vec2 force = b2MulSV( world->inv_h, base->mouseJoint.linearImpulse );
+	return force;
+}
+
+float b2GetMouseJointTorque( b2World* world, b2JointSim* base )
+{
+	return world->inv_h * base->mouseJoint.angularImpulse;
+}
+
+void b2PrepareMouseJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_mouseJoint );
+
+	// chase body id to the solver set where the body lives
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyB->setIndex == b2_awakeSet );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexB = bodyB->localIndex;
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	base->invMassB = bodySimB->invMass;
+	base->invIB = bodySimB->invInertia;
+
+	b2MouseJoint* joint = &base->mouseJoint;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+	joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+
+	joint->linearSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	float angularHertz = 0.5f;
+	float angularDampingRatio = 0.1f;
+	joint->angularSoftness = b2MakeSoft( angularHertz, angularDampingRatio, context->h );
+
+	b2Vec2 rB = joint->anchorB;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+	//   = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+	//     [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+	b2Mat22 K;
+	K.cx.x = mB + iB * rB.y * rB.y;
+	K.cx.y = -iB * rB.x * rB.y;
+	K.cy.x = K.cx.y;
+	K.cy.y = mB + iB * rB.x * rB.x;
+
+	joint->linearMass = b2GetInverse22( K );
+	joint->deltaCenter = b2Sub( bodySimB->center, joint->targetA );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b2Vec2_zero;
+		joint->angularImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartMouseJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_mouseJoint );
+
+	float mB = base->invMassB;
+	float iB = base->invIB;
+
+	b2MouseJoint* joint = &base->mouseJoint;
+
+	b2BodyState* stateB = context->states + joint->indexB;
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	b2Rot dqB = stateB->deltaRotation;
+	b2Vec2 rB = b2RotateVector( dqB, joint->anchorB );
+
+	vB = b2MulAdd( vB, mB, joint->linearImpulse );
+	wB += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse );
+
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}
+
+void b2SolveMouseJoint( b2JointSim* base, b2StepContext* context )
+{
+	float mB = base->invMassB;
+	float iB = base->invIB;
+
+	b2MouseJoint* joint = &base->mouseJoint;
+	b2BodyState* stateB = context->states + joint->indexB;
+
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	// Softness with no bias to reduce rotation speed
+	{
+		float massScale = joint->angularSoftness.massScale;
+		float impulseScale = joint->angularSoftness.impulseScale;
+
+		float impulse = iB > 0.0f ? -wB / iB : 0.0f;
+		impulse = massScale * impulse - impulseScale * joint->angularImpulse;
+		joint->angularImpulse += impulse;
+
+		wB += iB * impulse;
+	}
+
+	float maxImpulse = joint->maxForce * context->h;
+
+	{
+		b2Rot dqB = stateB->deltaRotation;
+		b2Vec2 rB = b2RotateVector( dqB, joint->anchorB );
+		b2Vec2 Cdot = b2Add( vB, b2CrossSV( wB, rB ) );
+
+		b2Vec2 separation = b2Add( b2Add( stateB->deltaPosition, rB ), joint->deltaCenter );
+		b2Vec2 bias = b2MulSV( joint->linearSoftness.biasRate, separation );
+
+		float massScale = joint->linearSoftness.massScale;
+		float impulseScale = joint->linearSoftness.impulseScale;
+
+		b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, bias ) );
+
+		b2Vec2 impulse;
+		impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x;
+		impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y;
+
+		b2Vec2 oldImpulse = joint->linearImpulse;
+		joint->linearImpulse.x += impulse.x;
+		joint->linearImpulse.y += impulse.y;
+
+		float mag = b2Length( joint->linearImpulse );
+		if ( mag > maxImpulse )
+		{
+			joint->linearImpulse = b2MulSV( maxImpulse, b2Normalize( joint->linearImpulse ) );
+		}
+
+		impulse.x = joint->linearImpulse.x - oldImpulse.x;
+		impulse.y = joint->linearImpulse.y - oldImpulse.y;
+
+		vB = b2MulAdd( vB, mB, impulse );
+		wB += iB * b2Cross( rB, impulse );
+	}
+
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}