diff --git a/include/bounce/dynamics/contacts/contact_solver.h b/include/bounce/dynamics/contacts/contact_solver.h index 24781fa..e8d0c6c 100644 --- a/include/bounce/dynamics/contacts/contact_solver.h +++ b/include/bounce/dynamics/contacts/contact_solver.h @@ -46,13 +46,13 @@ struct b3ContactPositionConstraint { u32 indexA; float32 invMassA; - b3Mat33 invIA; + b3Mat33 localInvIA; float32 radiusA; b3Vec3 localCenterA; u32 indexB; b3Vec3 localCenterB; float32 invMassB; - b3Mat33 invIB; + b3Mat33 localInvIB; float32 radiusB; b3PositionConstraintManifold* manifolds; u32 manifoldCount; @@ -118,6 +118,7 @@ struct b3ContactSolverDef { b3Position* positions; b3Velocity* velocities; + b3Mat33* invInertias; b3Contact** contacts; u32 count; b3StackAllocator* allocator; @@ -140,6 +141,7 @@ public: protected: b3Position* m_positions; b3Velocity* m_velocities; + b3Mat33* m_inertias; b3Contact** m_contacts; b3ContactPositionConstraint* m_positionConstraints; b3ContactVelocityConstraint* m_velocityConstraints; diff --git a/include/bounce/dynamics/island.h b/include/bounce/dynamics/island.h index 7edb1fb..efdb6e0 100644 --- a/include/bounce/dynamics/island.h +++ b/include/bounce/dynamics/island.h @@ -19,7 +19,7 @@ #ifndef B3_ISLAND_H #define B3_ISLAND_H -#include +#include class b3StackAllocator; class b3Contact; @@ -67,6 +67,7 @@ private : b3Position* m_positions; b3Velocity* m_velocities; + b3Mat33* m_invInertias; }; #endif \ No newline at end of file diff --git a/include/bounce/dynamics/joints/cone_joint.h b/include/bounce/dynamics/joints/cone_joint.h index 7f111cd..663d8aa 100644 --- a/include/bounce/dynamics/joints/cone_joint.h +++ b/include/bounce/dynamics/joints/cone_joint.h @@ -110,9 +110,13 @@ private: float32 m_mB; b3Mat33 m_iA; b3Mat33 m_iB; + + b3Mat33 m_localInvIA; + b3Mat33 m_localInvIB; + b3Vec3 m_localCenterA; b3Vec3 m_localCenterB; - + // Point-to-point b3Vec3 m_rA; b3Vec3 m_rB; diff --git a/include/bounce/dynamics/joints/joint_solver.h b/include/bounce/dynamics/joints/joint_solver.h index cdbd05b..4ccb9d5 100644 --- a/include/bounce/dynamics/joints/joint_solver.h +++ b/include/bounce/dynamics/joints/joint_solver.h @@ -39,6 +39,7 @@ struct b3JointSolverDef b3Joint** joints; b3Position* positions; b3Velocity* velocities; + b3Mat33* invInertias; }; class b3JointSolver diff --git a/include/bounce/dynamics/joints/revolute_joint.h b/include/bounce/dynamics/joints/revolute_joint.h index 47214b8..336c5c9 100644 --- a/include/bounce/dynamics/joints/revolute_joint.h +++ b/include/bounce/dynamics/joints/revolute_joint.h @@ -171,6 +171,8 @@ private: b3Mat33 m_iB; b3Vec3 m_localCenterA; b3Vec3 m_localCenterB; + b3Mat33 m_localInvIA; + b3Mat33 m_localInvIB; // Hinge motor b3Vec3 m_motor_J1; // 1x3 (row) diff --git a/include/bounce/dynamics/joints/sphere_joint.h b/include/bounce/dynamics/joints/sphere_joint.h index 60e1782..086b366 100644 --- a/include/bounce/dynamics/joints/sphere_joint.h +++ b/include/bounce/dynamics/joints/sphere_joint.h @@ -76,7 +76,10 @@ private: float32 m_mB; b3Mat33 m_iA; b3Mat33 m_iB; - + + b3Mat33 m_localInvIA; + b3Mat33 m_localInvIB; + b3Vec3 m_localCenterA; b3Vec3 m_localCenterB; b3Vec3 m_rA; diff --git a/include/bounce/dynamics/joints/spring_joint.h b/include/bounce/dynamics/joints/spring_joint.h index 7af9bcc..cbd16e2 100644 --- a/include/bounce/dynamics/joints/spring_joint.h +++ b/include/bounce/dynamics/joints/spring_joint.h @@ -123,6 +123,8 @@ private: b3Mat33 m_iB; b3Vec3 m_localCenterA; b3Vec3 m_localCenterB; + b3Mat33 m_localInvIA; + b3Mat33 m_localInvIB; float32 m_bias; float32 m_gamma; diff --git a/include/bounce/dynamics/time_step.h b/include/bounce/dynamics/time_step.h index 6afe01c..670c074 100644 --- a/include/bounce/dynamics/time_step.h +++ b/include/bounce/dynamics/time_step.h @@ -39,6 +39,7 @@ struct b3SolverData { b3Position* positions; b3Velocity* velocities; + b3Mat33* invInertias; float32 dt; float32 invdt; }; @@ -87,6 +88,18 @@ inline b3Mat33 b3RotateToFrame(const b3Mat33& inertia, const b3Mat33& rotation) return rotation * inertia * b3Transpose(rotation); } +// Compute the inertia matrix of a body measured in +// inertial frame (variable over time) given the +// inertia matrix in body-fixed frame (constant) +// and a rotation matrix representing the orientation +// of the body frame relative to the inertial frame. +inline b3Mat33 b3RotateToFrame(const b3Mat33& inertia, const b3Quat& rotation) +{ + b3Mat33 R = b3QuatMat33(rotation); + + return R * inertia * b3Transpose(R); +} + // Compute the time derivative of an orientation given // the angular velocity of the rotating frame represented by the orientation. inline b3Quat b3Derivative(const b3Quat& orientation, const b3Vec3& velocity) diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 912e353..891b1f4 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -264,7 +264,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) Solve(x, iterations, A, b, S, z, sx0); b3_clothSolverIterations = iterations; - + sv = sv + x; sx = sx + sy; @@ -287,7 +287,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) WarmStart(); // Solve velocity constraints - const u32 kVelocityIterations = 8; + const u32 kVelocityIterations = 5; for (u32 i = 0; i < kVelocityIterations; ++i) { diff --git a/src/bounce/dynamics/contacts/contact_solver.cpp b/src/bounce/dynamics/contacts/contact_solver.cpp index fc3c801..ee3d7ec 100644 --- a/src/bounce/dynamics/contacts/contact_solver.cpp +++ b/src/bounce/dynamics/contacts/contact_solver.cpp @@ -32,6 +32,7 @@ b3ContactSolver::b3ContactSolver(const b3ContactSolverDef* def) m_count = def->count; m_positions = def->positions; m_velocities = def->velocities; + m_inertias = def->invInertias; m_contacts = def->contacts; m_positionConstraints = (b3ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b3ContactPositionConstraint)); m_velocityConstraints = (b3ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b3ContactVelocityConstraint)); @@ -86,13 +87,13 @@ void b3ContactSolver::InitializeConstraints() pc->indexA = bodyA->m_islandID; pc->invMassA = bodyA->m_invMass; - pc->invIA = bodyA->m_worldInvI; + pc->localInvIA = bodyA->m_invI; pc->localCenterA = bodyA->m_sweep.localCenter; pc->radiusA = shapeA->m_radius; pc->indexB = bodyB->m_islandID; pc->invMassB = bodyB->m_invMass; - pc->invIB = bodyB->m_worldInvI; + pc->localInvIB = bodyB->m_invI; pc->localCenterB = bodyB->m_sweep.localCenter; pc->radiusB = shapeB->m_radius; @@ -101,11 +102,11 @@ void b3ContactSolver::InitializeConstraints() vc->indexA = bodyA->m_islandID; vc->invMassA = bodyA->m_invMass; - vc->invIA = bodyA->m_worldInvI; + vc->invIA = m_inertias[vc->indexA]; vc->indexB = bodyB->m_islandID; vc->invMassB = bodyB->m_invMass; - vc->invIB = bodyB->m_worldInvI; + vc->invIB = m_inertias[vc->indexB]; vc->friction = b3MixFriction(shapeA->m_friction, shapeB->m_friction); vc->restitution = b3MixRestitution(shapeA->m_restitution, shapeB->m_restitution); @@ -503,19 +504,19 @@ bool b3ContactSolver::SolvePositionConstraints() u32 indexA = pc->indexA; float32 mA = pc->invMassA; - b3Mat33 iA = pc->invIA; b3Vec3 localCenterA = pc->localCenterA; u32 indexB = pc->indexB; float32 mB = pc->invMassB; - b3Mat33 iB = pc->invIB; b3Vec3 localCenterB = pc->localCenterB; b3Vec3 cA = m_positions[indexA].x; b3Quat qA = m_positions[indexA].q; + b3Mat33 iA = m_inertias[indexA]; b3Vec3 cB = m_positions[indexB].x; b3Quat qB = m_positions[indexB].q; + b3Mat33 iB = m_inertias[indexB]; u32 manifoldCount = pc->manifoldCount; @@ -565,18 +566,22 @@ bool b3ContactSolver::SolvePositionConstraints() cA -= mA * P; qA -= b3Derivative(qA, iA * b3Cross(rA, P)); qA.Normalize(); + iA = b3RotateToFrame(pc->localInvIA, qA); cB += mB * P; qB += b3Derivative(qB, iB * b3Cross(rB, P)); qB.Normalize(); + iB = b3RotateToFrame(pc->localInvIB, qB); } } m_positions[indexA].x = cA; m_positions[indexA].q = qA; + m_inertias[indexA] = iA; m_positions[indexB].x = cB; m_positions[indexB].q = qB; + m_inertias[indexB] = iB; } return minSeparation >= -3.0f * B3_LINEAR_SLOP; diff --git a/src/bounce/dynamics/island.cpp b/src/bounce/dynamics/island.cpp index b8b40f2..305c66f 100644 --- a/src/bounce/dynamics/island.cpp +++ b/src/bounce/dynamics/island.cpp @@ -35,6 +35,7 @@ b3Island::b3Island(b3StackAllocator* allocator, u32 bodyCapacity, u32 contactCap m_bodies = (b3Body**)m_allocator->Allocate(m_bodyCapacity * sizeof(b3Body*)); m_velocities = (b3Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b3Velocity)); m_positions = (b3Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b3Position)); + m_invInertias = (b3Mat33*)m_allocator->Allocate(m_bodyCapacity * sizeof(b3Mat33)); m_contacts = (b3Contact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3Contact*)); m_joints = (b3Joint**)m_allocator->Allocate(m_jointCapacity * sizeof(b3Joint*)); @@ -48,6 +49,7 @@ b3Island::~b3Island() // @note Reverse order of construction. m_allocator->Free(m_joints); m_allocator->Free(m_contacts); + m_allocator->Free(m_invInertias); m_allocator->Free(m_positions); m_allocator->Free(m_velocities); m_allocator->Free(m_bodies); @@ -171,6 +173,7 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, m_velocities[i].w = w; m_positions[i].x = x; m_positions[i].q = q; + m_invInertias[i] = b->m_worldInvI; } b3JointSolverDef jointSolverDef; @@ -178,6 +181,7 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, jointSolverDef.count = m_jointCount; jointSolverDef.positions = m_positions; jointSolverDef.velocities = m_velocities; + jointSolverDef.invInertias = m_invInertias; jointSolverDef.dt = h; b3JointSolver jointSolver(&jointSolverDef); @@ -187,6 +191,7 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, contactSolverDef.count = m_contactCount; contactSolverDef.positions = m_positions; contactSolverDef.velocities = m_velocities; + contactSolverDef.invInertias = m_invInertias; contactSolverDef.dt = h; b3ContactSolver contactSolver(&contactSolverDef); @@ -228,10 +233,13 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, // 4. Integrate positions for (u32 i = 0; i < m_bodyCount; ++i) { + b3Body* b = m_bodies[i]; + b3Vec3 x = m_positions[i].x; b3Quat q = m_positions[i].q; b3Vec3 v = m_velocities[i].v; b3Vec3 w = m_velocities[i].w; + b3Mat33 invI = m_invInertias[i]; // Prevent numerical instability due to large velocity changes. b3Vec3 translation = h * v; @@ -251,11 +259,13 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, // Integrate x += h * v; q = b3Integrate(q, w, h); + invI = b3RotateToFrame(b->m_invI, q); m_positions[i].x = x; m_positions[i].q = q; m_velocities[i].v = v; m_velocities[i].w = w; + m_invInertias[i] = invI; } // 5. Solve position constraints @@ -285,9 +295,9 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, b->m_sweep.orientation.Normalize(); b->m_linearVelocity = m_velocities[i].v; b->m_angularVelocity = m_velocities[i].w; + b->m_worldInvI = m_invInertias[i]; + b->SynchronizeTransform(); - // Transform body inertia to world inertia - b->m_worldInvI = b3RotateToFrame(b->m_invI, b->m_xf.rotation); } // 7. Put bodies under unconsiderable motion to sleep diff --git a/src/bounce/dynamics/joints/cone_joint.cpp b/src/bounce/dynamics/joints/cone_joint.cpp index 94726bd..15f41ac 100644 --- a/src/bounce/dynamics/joints/cone_joint.cpp +++ b/src/bounce/dynamics/joints/cone_joint.cpp @@ -72,12 +72,15 @@ void b3ConeJoint::InitializeConstraints(const b3SolverData* data) m_mA = m_bodyA->m_invMass; m_mB = m_bodyB->m_invMass; - m_iA = m_bodyA->m_worldInvI; - m_iB = m_bodyB->m_worldInvI; - m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; + m_localInvIA = m_bodyA->m_invI; + m_localInvIB = m_bodyB->m_invI; + + m_iA = data->invInertias[m_indexA]; + m_iB = data->invInertias[m_indexB]; + b3Vec3 xA = data->positions[m_indexA].x; b3Quat qA = data->positions[m_indexA].q; @@ -212,12 +215,12 @@ bool b3ConeJoint::SolvePositionConstraints(const b3SolverData* data) b3Quat qA = data->positions[m_indexA].q; b3Vec3 xB = data->positions[m_indexB].x; b3Quat qB = data->positions[m_indexB].q; + b3Mat33 iA = data->invInertias[m_indexA]; + b3Mat33 iB = data->invInertias[m_indexB]; float32 mA = m_mA; - b3Mat33 iA = m_iA; float32 mB = m_mB; - b3Mat33 iB = m_iB; - + // Solve point-to-point constraint. float32 linearError = 0.0f; { @@ -240,10 +243,12 @@ bool b3ConeJoint::SolvePositionConstraints(const b3SolverData* data) xA -= mA * P; qA -= b3Derivative(qA, iA * b3Cross(rA, P)); qA.Normalize(); + iA = b3RotateToFrame(m_localInvIA, qA); xB += mB * P; qB += b3Derivative(qB, iB * b3Cross(rB, P)); qB.Normalize(); + iB = b3RotateToFrame(m_localInvIB, qB); } // Solve limit constraint. @@ -280,15 +285,19 @@ bool b3ConeJoint::SolvePositionConstraints(const b3SolverData* data) qA -= b3Derivative(qA, iA * P); qA.Normalize(); + iA = b3RotateToFrame(m_localInvIA, qA); qB += b3Derivative(qB, iB * P); qB.Normalize(); + iB = b3RotateToFrame(m_localInvIB, qB); } data->positions[m_indexA].x = xA; data->positions[m_indexA].q = qA; data->positions[m_indexB].x = xB; data->positions[m_indexB].q = qB; + data->invInertias[m_indexA] = iA; + data->invInertias[m_indexB] = iB; return linearError <= B3_LINEAR_SLOP && limitError <= B3_ANGULAR_SLOP; } diff --git a/src/bounce/dynamics/joints/joint_solver.cpp b/src/bounce/dynamics/joints/joint_solver.cpp index f247d5c..234b74a 100644 --- a/src/bounce/dynamics/joints/joint_solver.cpp +++ b/src/bounce/dynamics/joints/joint_solver.cpp @@ -27,6 +27,7 @@ b3JointSolver::b3JointSolver(const b3JointSolverDef* def) m_solverData.invdt = def->dt > 0.0f ? 1.0f / def->dt : 0.0f; m_solverData.positions = def->positions; m_solverData.velocities = def->velocities; + m_solverData.invInertias = def->invInertias; } void b3JointSolver::InitializeConstraints() diff --git a/src/bounce/dynamics/joints/revolute_joint.cpp b/src/bounce/dynamics/joints/revolute_joint.cpp index dc9964b..07dbec8 100644 --- a/src/bounce/dynamics/joints/revolute_joint.cpp +++ b/src/bounce/dynamics/joints/revolute_joint.cpp @@ -239,10 +239,13 @@ void b3RevoluteJoint::InitializeConstraints(const b3SolverData* data) m_indexB = m_bodyB->m_islandID; m_mA = m_bodyA->m_invMass; m_mB = m_bodyB->m_invMass; - m_iA = m_bodyA->m_worldInvI; - m_iB = m_bodyB->m_worldInvI; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; + m_localInvIA = m_bodyA->m_invI; + m_localInvIB = m_bodyB->m_invI; + + m_iA = data->invInertias[m_indexA]; + m_iB = data->invInertias[m_indexB]; b3Quat qA = data->positions[m_indexA].q; b3Quat qB = data->positions[m_indexB].q; @@ -500,11 +503,11 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data) b3Quat qA = data->positions[m_indexA].q; b3Vec3 xB = data->positions[m_indexB].x; b3Quat qB = data->positions[m_indexB].q; + b3Mat33 iA = data->invInertias[m_indexA]; + b3Mat33 iB = data->invInertias[m_indexB]; float32 mA = m_mA; - b3Mat33 iA = m_iA; float32 mB = m_mB; - b3Mat33 iB = m_iB; // Solve limit constraint. float32 limitError = 0.0f; @@ -566,9 +569,11 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data) qA += b3Derivative(qA, iA * P1); qA.Normalize(); + iA = b3RotateToFrame(m_localInvIA, qA); qB += b3Derivative(qB, iB * P2); qB.Normalize(); + iB = b3RotateToFrame(m_localInvIB, qB); } // Solve point-to-point constraints. @@ -596,10 +601,12 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data) xA -= mA * impulse; qA -= b3Derivative(qA, iA * b3Cross(rA, impulse)); qA.Normalize(); + iA = b3RotateToFrame(m_localInvIA, qA); xB += mB * impulse; qB += b3Derivative(qB, iB * b3Cross(rB, impulse)); qB.Normalize(); + iB = b3RotateToFrame(m_localInvIB, qB); } // Solve hinge constraints. @@ -632,15 +639,19 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data) qA += b3Derivative(qA, iA * P1); qA.Normalize(); + iA = b3RotateToFrame(m_localInvIA, qA); qB += b3Derivative(qB, iB * P2); qB.Normalize(); + iB = b3RotateToFrame(m_localInvIB, qB); } data->positions[m_indexA].x = xA; data->positions[m_indexA].q = qA; data->positions[m_indexB].x = xB; data->positions[m_indexB].q = qB; + data->invInertias[m_indexA] = iA; + data->invInertias[m_indexB] = iB; return linearError <= B3_LINEAR_SLOP && angularError <= B3_ANGULAR_SLOP && diff --git a/src/bounce/dynamics/joints/sphere_joint.cpp b/src/bounce/dynamics/joints/sphere_joint.cpp index f62ef2f..ecbdd16 100644 --- a/src/bounce/dynamics/joints/sphere_joint.cpp +++ b/src/bounce/dynamics/joints/sphere_joint.cpp @@ -45,11 +45,13 @@ void b3SphereJoint::InitializeConstraints(const b3SolverData* data) m_indexB = m_bodyB->m_islandID; m_mA = m_bodyA->m_invMass; m_mB = m_bodyB->m_invMass; - m_iA = m_bodyA->m_worldInvI; - m_iB = m_bodyB->m_worldInvI; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; - + m_localInvIA = m_bodyA->m_invI; + m_localInvIB = m_bodyB->m_invI; + m_iA = data->invInertias[m_indexA]; + m_iB = data->invInertias[m_indexB]; + b3Quat qA = data->positions[m_indexA].q; b3Quat qB = data->positions[m_indexB].q; @@ -115,6 +117,8 @@ bool b3SphereJoint::SolvePositionConstraints(const b3SolverData* data) b3Quat qA = data->positions[m_indexA].q; b3Vec3 xB = data->positions[m_indexB].x; b3Quat qB = data->positions[m_indexB].q; + b3Mat33 iA = data->invInertias[m_indexA]; + b3Mat33 iB = data->invInertias[m_indexB]; // Compute effective mass b3Vec3 rA = b3Mul(qA, m_localAnchorA - m_localCenterA); @@ -126,23 +130,28 @@ bool b3SphereJoint::SolvePositionConstraints(const b3SolverData* data) b3Mat33 RB = b3Skew(rB); b3Mat33 RBT = b3Transpose(RB); - b3Mat33 mass = M + RA * m_iA * RAT + RB * m_iB * RBT; + b3Mat33 mass = M + RA * iA * RAT + RB * iB * RBT; b3Vec3 C = xB + rB - xA - rA; b3Vec3 impulse = mass.Solve(-C); xA -= m_mA * impulse; - qA -= b3Derivative(qA, b3Mul(m_iA, b3Cross(rA, impulse))); + qA -= b3Derivative(qA, b3Mul(iA, b3Cross(rA, impulse))); qA.Normalize(); + iA = b3RotateToFrame(m_localInvIA, qA); xB += m_mB * impulse; - qB += b3Derivative(qB, b3Mul(m_iB, b3Cross(rB, impulse))); + qB += b3Derivative(qB, b3Mul(iB, b3Cross(rB, impulse))); qB.Normalize(); + iB = b3RotateToFrame(m_localInvIB, qB); data->positions[m_indexA].x = xA; data->positions[m_indexA].q = qA; + data->invInertias[m_indexA] = iA; + data->positions[m_indexB].x = xB; data->positions[m_indexB].q = qB; + data->invInertias[m_indexB] = iB; return b3Length(C) <= B3_LINEAR_SLOP; } diff --git a/src/bounce/dynamics/joints/spring_joint.cpp b/src/bounce/dynamics/joints/spring_joint.cpp index 6aa667e..93d2541 100644 --- a/src/bounce/dynamics/joints/spring_joint.cpp +++ b/src/bounce/dynamics/joints/spring_joint.cpp @@ -105,12 +105,15 @@ void b3SpringJoint::InitializeConstraints(const b3SolverData* data) m_mA = m_bodyA->m_invMass; m_mB = m_bodyB->m_invMass; - m_iA = m_bodyA->m_worldInvI; - m_iB = m_bodyB->m_worldInvI; - m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; + m_localInvIA = m_bodyA->m_invI; + m_localInvIB = m_bodyB->m_invI; + + m_iA = data->invInertias[m_indexA]; + m_iB = data->invInertias[m_indexB]; + b3Vec3 xA = data->positions[m_indexA].x; b3Quat qA = data->positions[m_indexA].q; b3Vec3 xB = data->positions[m_indexB].x; @@ -218,6 +221,10 @@ bool b3SpringJoint::SolvePositionConstraints(const b3SolverData* data) b3Quat qA = data->positions[m_indexA].q; b3Vec3 xB = data->positions[m_indexB].x; b3Quat qB = data->positions[m_indexB].q; + b3Mat33 iA = data->invInertias[m_indexA]; + b3Mat33 iB = data->invInertias[m_indexB]; + float32 mA = m_mA; + float32 mB = m_mB; b3Vec3 rA = b3Mul(qA, m_localAnchorA - m_localCenterA); b3Vec3 rB = b3Mul(qB, m_localAnchorB - m_localCenterB); @@ -232,23 +239,27 @@ bool b3SpringJoint::SolvePositionConstraints(const b3SolverData* data) b3Vec3 rnA = b3Cross(rA, n); b3Vec3 rnB = b3Cross(rB, n); - float32 kMass = m_mA + m_mB + b3Dot(m_iA * rnA, rnA) + b3Dot(m_iB * rnB, rnB); + float32 kMass = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB); float32 mass = kMass > 0.0f ? 1.0f / kMass : 0.0f; float32 lambda = -mass * C; b3Vec3 impulse = lambda * n; - xA -= m_mA * impulse; - qA -= b3Derivative(qA, b3Mul(m_iA, b3Cross(rA, impulse))); + xA -= mA * impulse; + qA -= b3Derivative(qA, b3Mul(iA, b3Cross(rA, impulse))); + iA = b3RotateToFrame(m_localInvIA, qA); - xB += m_mB * impulse; - qB += b3Derivative(qB, b3Mul(m_iB, b3Cross(rB, impulse))); + xB += mB * impulse; + qB += b3Derivative(qB, b3Mul(iB, b3Cross(rB, impulse))); + iB = b3RotateToFrame(m_localInvIB, qB); data->positions[m_indexA].x = xA; data->positions[m_indexA].q = qA; data->positions[m_indexB].x = xB; data->positions[m_indexB].q = qB; - + data->invInertias[m_indexA] = iA; + data->invInertias[m_indexB] = iB; + return b3Abs(C) < B3_LINEAR_SLOP; }