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
Download Patch File Download Diff File Expand all files Collapse all files

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