AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ngs bugfix: update inertia after solving each constraint

Browse Source
This commit is contained in:
Irlan 2018-08-07 14:56:31 -03:00
parent cd5279e6d9
commit 86437b545d
16 changed files with 124 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/bounce/dynamics/contacts/contact_solver.h
View File

@ -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;

3
include/bounce/dynamics/island.h
View File

@ -19,7 +19,7 @@
#ifndef B3_ISLAND_H
#define B3_ISLAND_H
#include <bounce/common/math/vec3.h>
#include <bounce/common/math/mat33.h>
class b3StackAllocator;
class b3Contact;
@ -67,6 +67,7 @@ private :
b3Position* m_positions;
b3Velocity* m_velocities;
b3Mat33* m_invInertias;
};
#endif

6
include/bounce/dynamics/joints/cone_joint.h
View File

@ -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;

1
include/bounce/dynamics/joints/joint_solver.h
View File

@ -39,6 +39,7 @@ struct b3JointSolverDef
b3Joint** joints;
b3Position* positions;
b3Velocity* velocities;
b3Mat33* invInertias;
};
class b3JointSolver

2
include/bounce/dynamics/joints/revolute_joint.h
View File

@ -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)

5
include/bounce/dynamics/joints/sphere_joint.h
View File

@ -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;

2
include/bounce/dynamics/joints/spring_joint.h
View File

@ -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;

13
include/bounce/dynamics/time_step.h
View File

@ -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)

4
src/bounce/dynamics/cloth/cloth_solver.cpp
View File

@ -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)
{

17
src/bounce/dynamics/contacts/contact_solver.cpp
View File

@ -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;

14
src/bounce/dynamics/island.cpp
View File

@ -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

21
src/bounce/dynamics/joints/cone_joint.cpp
View File

@ -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;
}

1
src/bounce/dynamics/joints/joint_solver.cpp
View File

@ -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()

19
src/bounce/dynamics/joints/revolute_joint.cpp
View File

@ -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 &&

21
src/bounce/dynamics/joints/sphere_joint.cpp
View File

@ -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;
}

29
src/bounce/dynamics/joints/spring_joint.cpp
View File

@ -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;
}