void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
if (m_frequencyHz > 0.0f)
{
float32 Cdot2 = wB - wA;
float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
m_impulse.z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);
m_impulse.x += impulse1.x;
m_impulse.y += impulse1.y;
b2Vec2 P = impulse1;
vA -= mA * P;
wA -= iA * b2Cross(m_rA, P);
vB += mB * P;
wB += iB * b2Cross(m_rB, P);
}
else
{
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
float32 Cdot2 = wB - wA;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = -b2Mul(m_mass, Cdot);
m_impulse += impulse;
b2Vec2 P(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + impulse.z);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + impulse.z);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2WeldJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
B2_NOT_USED(step);
b2Body* bA = m_bodyA;
b2Body* bB = m_bodyB;
b2Vec2 vA = bA->m_linearVelocity;
float32 wA = bA->m_angularVelocity;
b2Vec2 vB = bB->m_linearVelocity;
float32 wB = bB->m_angularVelocity;
float32 mA = bA->m_invMass, mB = bB->m_invMass;
float32 iA = bA->m_invI, iB = bB->m_invI;
b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
// Solve point-to-point constraint
b2Vec2 Cdot1 = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
float32 Cdot2 = wB - wA;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = m_mass.Solve33(-Cdot);
m_impulse += impulse;
b2Vec2 P(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(rA, P) + impulse.z);
vB += mB * P;
wB += iB * (b2Cross(rB, P) + impulse.z);
bA->m_linearVelocity = vA;
bA->m_angularVelocity = wA;
bB->m_linearVelocity = vB;
bB->m_angularVelocity = wB;
}
void b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 v1 = b1->m_linearVelocity;
float32 w1 = b1->m_angularVelocity;
b2Vec2 v2 = b2->m_linearVelocity;
float32 w2 = b2->m_angularVelocity;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = step.dt * m_maxMotorForce;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float32 L1 = impulse * m_a1;
float32 L2 = impulse * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
float32 Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float32 Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
b2Vec2 Cdot(Cdot1, Cdot2);
b2Vec2 f1 = m_impulse;
b2Vec2 df = m_K.Solve(-Cdot);
m_impulse += df;
if (m_limitState == e_atLowerLimit)
{
m_impulse.y = b2Max(m_impulse.y, 0.0f);
}
else if (m_limitState == e_atUpperLimit)
{
m_impulse.y = b2Min(m_impulse.y, 0.0f);
}
// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
float32 b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x;
float32 f2r;
if (m_K.col1.x != 0.0f)
{
f2r = b / m_K.col1.x + f1.x;
}
else
{
f2r = f1.x;
}
m_impulse.x = f2r;
df = m_impulse - f1;
b2Vec2 P = df.x * m_perp + df.y * m_axis;
float32 L1 = df.x * m_s1 + df.y * m_a1;
float32 L2 = df.x * m_s2 + df.y * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
float32 df;
if (m_K.col1.x != 0.0f)
{
df = - Cdot1 / m_K.col1.x;
}
else
{
df = 0.0f;
}
m_impulse.x += df;
b2Vec2 P = df * m_perp;
float32 L1 = df * m_s1;
float32 L2 = df * m_s2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
b1->m_linearVelocity = v1;
b1->m_angularVelocity = w1;
b2->m_linearVelocity = v2;
b2->m_angularVelocity = w2;
}
void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
{
float32 Cdot = wB - wA - m_motorSpeed;
float32 impulse = -m_motorMass * Cdot;
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
{
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
float32 Cdot2 = wB - wA;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = -m_mass.Solve33(Cdot);
if (m_limitState == e_equalLimits)
{
m_impulse += impulse;
}
else if (m_limitState == e_atLowerLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
else if (m_limitState == e_atUpperLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
b2Vec2 P(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + impulse.z);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + impulse.z);
}
else
{
// Solve point-to-point constraint
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
b2Vec2 impulse = m_mass.Solve22(-Cdot);
m_impulse.x += impulse.x;
m_impulse.y += impulse.y;
vA -= mA * impulse;
wA -= iA * b2Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * b2Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 v1 = b1->m_linearVelocity;
float32 w1 = b1->m_angularVelocity;
b2Vec2 v2 = b2->m_linearVelocity;
float32 w2 = b2->m_angularVelocity;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = step.dt * m_maxMotorForce;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float32 L1 = impulse * m_a1;
float32 L2 = impulse * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
b2Vec2 Cdot1;
Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
Cdot1.y = w2 - w1;
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float32 Cdot2;
Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 f1 = m_impulse;
b2Vec3 df = m_K.Solve33(-Cdot);
m_impulse += df;
if (m_limitState == e_atLowerLimit)
{
m_impulse.z = b2Max(m_impulse.z, 0.0f);
}
else if (m_limitState == e_atUpperLimit)
{
m_impulse.z = b2Min(m_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.col3.x, m_K.col3.y);
b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
df = m_impulse - f1;
b2Vec2 P = df.x * m_perp + df.z * m_axis;
float32 L1 = df.x * m_s1 + df.y + df.z * m_a1;
float32 L2 = df.x * m_s2 + df.y + df.z * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
b2Vec2 df = m_K.Solve22(-Cdot1);
m_impulse.x += df.x;
m_impulse.y += df.y;
b2Vec2 P = df.x * m_perp;
float32 L1 = df.x * m_s1 + df.y;
float32 L2 = df.x * m_s2 + df.y;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
b1->m_linearVelocity = v1;
b1->m_angularVelocity = w1;
b2->m_linearVelocity = v2;
b2->m_angularVelocity = w2;
}
void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = data.step.dt * m_maxMotorForce;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float32 LA = impulse * m_a1;
float32 LB = impulse * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
b2Vec2 Cdot1;
Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float32 Cdot2;
Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 f1 = m_impulse;
b2Vec3 df = m_K.Solve33(-Cdot);
m_impulse += df;
if (m_limitState == e_atLowerLimit)
{
m_impulse.z = b2Max(m_impulse.z, 0.0f);
}
else if (m_limitState == e_atUpperLimit)
{
m_impulse.z = b2Min(m_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);
b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
df = m_impulse - f1;
b2Vec2 P = df.x * m_perp + df.z * m_axis;
float32 LA = df.x * m_s1 + df.y + df.z * m_a1;
float32 LB = df.x * m_s2 + df.y + df.z * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
b2Vec2 df = m_K.Solve22(-Cdot1);
m_impulse.x += df.x;
m_impulse.y += df.y;
b2Vec2 P = df.x * m_perp;
float32 LA = df.x * m_s1 + df.y;
float32 LB = df.x * m_s2 + df.y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
b2Vec2 Cdot10 = Cdot1;
Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f)
{
b2Vec2 test = b2Mul22(m_K, df);
Cdot1.x += 0.0f;
}
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 v1 = b1->m_linearVelocity;
float32 w1 = b1->m_angularVelocity;
b2Vec2 v2 = b2->m_linearVelocity;
float32 w2 = b2->m_angularVelocity;
float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
float32 i1 = b1->m_invI, i2 = b2->m_invI;
// Solve motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = w2 - w1 - m_motorSpeed;
float32 impulse = m_motorMass * (-Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = step.dt * m_maxMotorTorque;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
// Solve point-to-point constraint
b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
float32 Cdot2 = w2 - w1;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = m_mass.Solve33(-Cdot);
if (m_limitState == e_equalLimits)
{
m_impulse += impulse;
}
else if (m_limitState == e_atLowerLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
b2Vec2 reduced = m_mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
}
else if (m_limitState == e_atUpperLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
b2Vec2 reduced = m_mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
}
b2Vec2 P(impulse.x, impulse.y);
v1 -= m1 * P;
w1 -= i1 * (b2Cross(r1, P) + impulse.z);
v2 += m2 * P;
w2 += i2 * (b2Cross(r2, P) + impulse.z);
}
else
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
// Solve point-to-point constraint
b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
b2Vec2 impulse = m_mass.Solve22(-Cdot);
m_impulse.x += impulse.x;
m_impulse.y += impulse.y;
v1 -= m1 * impulse;
w1 -= i1 * b2Cross(r1, impulse);
v2 += m2 * impulse;
w2 += i2 * b2Cross(r2, impulse);
}
b1->m_linearVelocity = v1;
b1->m_angularVelocity = w1;
b2->m_linearVelocity = v2;
b2->m_angularVelocity = w2;
}
void SimultaneousImpulseBasedConstraintSolverStrategy::Solve(float dt, std::vector<std::unique_ptr<IConstraint> >& c, Mat<float>& q, Mat<float>& qdot, SparseMat<float>& invM, SparseMat<float>& S, const Mat<float>& Fext )
{
//std::cout << "STATE :" << std::endl;
//q.afficher();
Mat<float> qdotminus(qdot);
this->dt = dt;
//computeConstraintsJacobian(c);
Mat<float> tempInvMFext( dt*(invM * Fext) ) ;
//qdot += tempInvMFext;
//computeConstraintsJacobian(c,q,qdot);
computeConstraintsANDJacobian(c,q,qdot);
//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
//std::cout << "Constraints : norme = " << norme2(C) << std::endl;
//C.afficher();
Mat<float> tConstraintsJacobian( transpose(constraintsJacobian) );
//std::cout << "t Constraints Jacobian :" << std::endl;
//tConstraintsJacobian.afficher();
//PREVIOUS METHOD :
//--------------------------------
//David Baraff 96 STAR.pdf Interactive Simulation of Rigid Body Dynamics in Computer Graphics : Lagrange Multipliers Method :
//Construct A :
/*
Mat<float> A( (-1.0f)*tConstraintsJacobian );
Mat<float> M( invGJ( invM.SM2mat() ) );
A = operatorL( M, A);
A = operatorC( A , operatorL( constraintsJacobian, Mat<float>((float)0,constraintsJacobian.getLine(), constraintsJacobian.getLine()) ) );
*/
//----------------------------
Mat<float> A( constraintsJacobian * invM.SM2mat() * tConstraintsJacobian );
//---------------------------
Mat<float> invA( invGJ(A) );//invM*tConstraintsJacobian ) * constraintsJacobian );
//Construct b and compute the solution.
//----------------------------------
//Mat<float> tempLambda( invA * operatorC( Mat<float>((float)0,invA.getLine()-constraintsJacobian.getLine(),1) , (-1.0f)*(constraintsJacobian*(invM*Fext) + offset) ) );
//-----------------------------------
Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobian*tempInvMFext + offset) ) );
//-----------------------------------
//Solutions :
//------------------------------------
//lambda = extract( &tempLambda, qdot.getLine()+1, 1, tempLambda.getLine(), 1);
//if(isnanM(lambda))
// lambda = Mat<float>(0.0f,lambda.getLine(),lambda.getColumn());
//Mat<float> udot( extract( &tempLambda, 1,1, qdot.getLine(), 1) );
//------------------------------------
lambda = tempLambda;
Mat<float> udot( tConstraintsJacobian * tempLambda);
//------------------------------------
if(isnanM(udot))
udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn());
float clampingVal = 1e4f;
for(int i=1;i<=udot.getLine();i++)
{
if(udot.get(i,1) > clampingVal)
{
udot.set( clampingVal,i,1);
}
}
#ifdef debuglvl1
std::cout << " SOLUTIONS : udot and lambda/Pc : " << std::endl;
transpose(udot).afficher();
transpose(lambda).afficher();
transpose( tConstraintsJacobian*lambda).afficher();
#endif
//Assumed model :
//qdot = tempInvMFext + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1);
//qdot = tempInvMFext + udot;
qdot += tempInvMFext + invM*udot;
//qdot += invM*udot;
//qdot += tempInvMFext + udot;
float clampingValQ = 1e3f;
for(int i=1;i<=qdot.getLine();i++)
{
if( fabs_(qdot.get(i,1)) > clampingValQ)
{
qdot.set( clampingValQ * fabs_(qdot.get(i,1))/qdot.get(i,1),i,1);
}
}
//qdot = udot;
//Assumed model if the update of the integration is applied after that constraints solver :
//qdot += dt*extract( &tempLambda, 1,1, qdot.getLine(), 1);//+tempInvMFext
Mat<float> t( dt*( S*qdot ) );
float clampQuat = 1e-1f;
float idxQuat = 3;
while(idxQuat < t.getLine())
{
for(int i=1;i<4;i++)
{
if( fabs_(t.get(idxQuat+i,1)) > clampQuat)
{
t.set( clampQuat*(t.get(idxQuat+i,1))/t.get(idxQuat+i,1), idxQuat+i,1);
}
}
idxQuat += 7;
}
//the update is done by the update via an accurate integration and we must construct q and qdot at every step
//q += t;
//--------------------------------------
//let us normalize each quaternion :
/*
idxQuat = 3;
while(idxQuat < q.getLine())
{
float scaler = q.get( idxQuat+4,1);
if(scaler != 0.0f)
{
for(int i=1;i<=4;i++)
{
q.set( q.get(idxQuat+i,1)/scaler, idxQuat+i,1);
}
}
idxQuat += 7;
}
*/
//--------------------------------------
#ifdef debuglvl2
//std::cout << " computed Pc : " << std::endl;
//(tConstraintsJacobian*tempLambda).afficher();
//std::cout << " q+ : " << std::endl;
//transpose(q).afficher();
std::cout << " qdot+ : " << std::endl;
transpose(qdot).afficher();
std::cout << " qdotminus : " << std::endl;
transpose(qdotminus).afficher();
#endif
#ifdef debuglvl3
std::cout << "SOME VERIFICATION ON : J*qdot + c = 0 : " << std::endl;
transpose(constraintsJacobian*qdot+offset).afficher();
float normC = (transpose(C)*C).get(1,1);
Mat<float> Cdot( constraintsJacobian*qdot);
float normCdot = (transpose(Cdot)*Cdot).get(1,1);
float normQdot = (transpose(qdot)*qdot).get(1,1);
//rs->ltadd(std::string("normC"),normC);
//rs->ltadd(std::string("normCdot"),normCdot);
rs->ltadd(std::string("normQdot"),normQdot);
char name[5];
for(int i=1;i<=t.getLine();i++)
{
sprintf(name,"dq%d",i);
rs->ltadd(std::string(name), t.get(i,1));
}
rs->tWriteFileTABLE();
#endif
//END OF PREVIOUS METHOD :
//--------------------------------
//--------------------------------
//--------------------------------
//Second Method :
/*
//According to Tonge Richar's Physucs For Game pdf :
Mat<float> tempLambda( (-1.0f)*invGJ( constraintsJacobian*invM.SM2mat()*tConstraintsJacobian)*constraintsJacobian*qdot );
qdot += invM*tConstraintsJacobian*tempLambda;
//qdot += tempInvMFext; //contraints not satisfied.
//qdot += tempInvMFext; //qdot+ = qdot- + dt*M-1Fext;
//Mat<float> qdotreal( qdot + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1) ); //qdotreal = qdot+ + Pc;
Mat<float> t( dt*( S*qdot ) );
q += t;
*/
//--------------------------------
//--------------------------------
//End of second method...
//--------------------------------
//--------------------------------
//--------------------------------
//THIRD METHOD :
//--------------------------------
//With reference to A Unified Framework for Rigid Body Dynamics Chap. 4.6.2.Simultaneous Force-based methods :
//which refers to Bara96 :
/*
Mat<float> iM(invM.SM2mat());
Mat<float> b((-1.0f)*constraintsJacobian*iM*Fext+offset);
Mat<float> tempLambda( invGJ( constraintsJacobian*iM*tConstraintsJacobian) * b );
//Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda+Fext));
Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda));
qdot += dt*qdoubledot;
//qdot += tempInvMFext; //contraints not satisfied.
//qdot += tempInvMFext; //qdot+ = qdot- + dt*M-1Fext;
//Mat<float> qdotreal( qdot + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1) ); //qdotreal = qdot+ + Pc;
Mat<float> t( dt*( S*qdot ) );
q += t;
std::cout << " computed Pc : " << std::endl;
(tConstraintsJacobian*tempLambda).afficher();
std::cout << " q+ : " << std::endl;
q.afficher();
std::cout << " qdot+ : " << std::endl;
qdot.afficher();
*/
//END OF THIRD METHOD :
//--------------------------------
//S.print();
//std::cout << " computed Pc : " << std::endl;
//(tConstraintsJacobian*lambda).afficher();
//std::cout << " delta state = S * qdotreal : " << std::endl;
//t.afficher();
//std::cout << " S & qdotreal : " << std::endl;
//S.print();
//qdot.afficher();
//std::cout << "invM*Fext : " << std::endl;
//tempInvMFext.afficher();
//temp.afficher();
//(constraintsJacobian*(invM*Fext)).afficher();
//(invM*Fext).afficher();
//std::cout << " A : " << std::endl;
//A.afficher();
//std::cout << " SVD A*tA : S : " << std::endl;
//SVD<float> instanceSVD(A*transpose(A));
//instanceSVD.getS().afficher();
//std::cout << " invA : " << std::endl;
//invA.afficher();
//std::cout << " LAMBDA : " << std::endl;
//lambda.afficher();
//std::cout << " qdot+ & qdot- : " << std::endl;
//qdot.afficher();
//qdotminus.afficher();
//std::cout << " q+ : " << std::endl;
//q.afficher();
#ifdef debuglvl4
//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
std::cout << "tConstraints : norme = " << norme2(C) << std::endl;
transpose(C).afficher();
std::cout << "Cdot : " << std::endl;
(constraintsJacobian*qdot).afficher();
std::cout << " JACOBIAN : " << std::endl;
//transpose(constraintsJacobian).afficher();
constraintsJacobian.afficher();
std::cout << " Qdot+ : " << std::endl;
transpose(qdot).afficher();
#endif
//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
//std::cout << "Constraints : norme = " << norme2(C) << std::endl;
//C.afficher();
}