void Solver::setUpBodyMatricies(DMatrix& s, DMatrix& u, DMatrix& s_next, DMatrix& u_next, DMatrix& S, DMatrix& MInverse, DMatrix& Fext)
{
for (int i = 0; i<m_rigidBodies.size(); i++)
{
const RigidBody_c* rb = m_rigidBodies[i];
s.Set(i * 7 + 0) = rb->m_position.x;
s.Set(i * 7 + 1) = rb->m_position.y;
s.Set(i * 7 + 2) = rb->m_position.z;
s.Set(i * 7 + 3) = rb->m_orientation.w;
s.Set(i * 7 + 4) = rb->m_orientation.x;
s.Set(i * 7 + 5) = rb->m_orientation.y;
s.Set(i * 7 + 6) = rb->m_orientation.z;
u.Set(i * 6 + 0) = rb->m_linearVelocity.x;
u.Set(i * 6 + 1) = rb->m_linearVelocity.y;
u.Set(i * 6 + 2) = rb->m_linearVelocity.z;
u.Set(i * 6 + 3) = rb->m_angularVelocity.x;
u.Set(i * 6 + 4) = rb->m_angularVelocity.y;
u.Set(i * 6 + 5) = rb->m_angularVelocity.z;
const XMFLOAT4& q = rb->m_orientation;
DMatrix Q(4, 3);
Q.Set(0, 0) = -q.x; Q.Set(0, 1) = -q.y; Q.Set(0, 2) = -q.z;
Q.Set(1, 0) = q.w; Q.Set(1, 1) = q.z; Q.Set(1, 2) = -q.y;
Q.Set(2, 0) = -q.z; Q.Set(2, 1) = q.w; Q.Set(2, 2) = q.x;
Q.Set(3, 0) = q.y; Q.Set(3, 1) = -q.x; Q.Set(3, 2) = q.w;
Q = Q * 0.5f;
DMatrix Idenity(3, 3);
Idenity.SetToZero();
Idenity.Set(0, 0) = Idenity.Set(1, 1) = Idenity.Set(2, 2) = 1.0f;
S.SetSubMatrix(i * 7 + 0, i * 6 + 0, Idenity);
S.SetSubMatrix(i * 7 + 3, i * 6 + 3, Q);
DMatrix M(3, 3);
M.Set(0, 0) = M.Set(1, 1) = M.Set(2, 2) = rb->m_invMass;
const XMFLOAT3X3& dxm = rb->CreateWorldII();
DMatrix I(3, 3);
I.Set(0, 0) = dxm._11; I.Set(1, 0) = dxm._12; I.Set(2, 0) = dxm._13;
I.Set(0, 1) = dxm._21; I.Set(1, 1) = dxm._22; I.Set(2, 1) = dxm._23;
I.Set(0, 2) = dxm._31; I.Set(1, 2) = dxm._32; I.Set(2, 2) = dxm._33;
/*if (rb->m_invMass == 0.0f)
{
M.Set(0, 0) = M.Set(1, 1) = M.Set(2, 2) = 0.01f;
I.Set(0, 0) = 0.01f; I.Set(1, 0) = dxm._12; I.Set(2, 0) = dxm._13;
I.Set(0, 1) = dxm._21; I.Set(1, 1) = 0.01f ; I.Set(2, 1) = dxm._23;
I.Set(0, 2) = dxm._31; I.Set(1, 2) = dxm._32; I.Set(2, 2) = 0.01f;
}*/
MInverse.SetSubMatrix(i * 6, i * 6, M);
MInverse.SetSubMatrix(i * 6 + 3, i * 6 + 3, I);
DMatrix F(3, 1);
F.Set(0, 0) = rb->m_force.x;
F.Set(1, 0) = rb->m_force.y;
F.Set(2, 0) = rb->m_force.z;
XMFLOAT3 rF = rb->m_torque;
DMatrix T(3, 1);
T.Set(0, 0) = rF.x;
T.Set(1, 0) = rF.y;
T.Set(2, 0) = rF.z;
Fext.SetSubMatrix(i * 6, 0, F);
Fext.SetSubMatrix(i * 6 + 3, 0, T);
}
}
void Solver::GaussSeidelLCP(const DMatrix& J, const DMatrix& W_Jt, const DMatrix& b, DMatrix& V, DMatrix& x, const DMatrix* lo, const DMatrix* hi)
{
int maxIterations = 20;
x.SetToZero();
const int n = x.GetNumRows();
DMatrix aDiag = J.diagonalProduct(W_Jt);
float xi_prime;
float xi;
float delta_xi;
while (maxIterations--)
{
for (int i = 0; i < n; i++)
{
// xi' = xi + 1/Aii (bi - Ai*x)
// xi' = xi + 1/Aii (bi - J*Vi)
xi = x.Get(i);
xi_prime = xi;
assert(aDiag.Get(i) != 0.0f);
xi_prime += 1 / aDiag.Get(i) * (b.Get(i) - J.rowProduct(V, i).Get(0));// A.rowProduct(x, i).Get(0));//
if (lo && (xi_prime < lo->Get(i)))
{
xi_prime = lo->Get(i);
}
if (hi && xi_prime > hi->Get(i))
{
xi_prime = hi->Get(i);
}
// Update X-component with new result
x.Set(i) = xi_prime;
// Fix V component with latest X result by using delta between current
// and previous result for the X component.
// V' = V + W*Jt * delta_x
float delta_xi = xi_prime - xi;
const DMatrix& V_adjust = W_Jt.colProduct(delta_xi, i);
V.AddSubMatrix(0, 0, V_adjust);
}
}
}
