Quaternion CreateDiagonalizer(Mat3Param matrix)
{
const unsigned cMaxSteps = 50;
const float cThetaLimit = 1.0e6f;
Quaternion quat(0.0f, 0.0f, 0.0f, 1.0f);
Matrix3 quatMatrix;
Matrix3 diagMatrix;
for(unsigned i = 0; i < cMaxSteps; ++i)
{
ToMatrix3(quat, &quatMatrix);
diagMatrix = Concat(Concat(quatMatrix, matrix), quatMatrix.Transposed());
//Elements not on the diagonal
Vector3 offDiag(diagMatrix(1, 2), diagMatrix(0, 2), diagMatrix(0, 1));
//Magnitude of the off-diagonal elements
Vector3 magDiag = Abs(offDiag);
//Index of the largest element
unsigned k = ((magDiag.x > magDiag.y) && (magDiag.x > magDiag.z)) ? 0 :
((magDiag.y > magDiag.z) ? 1 : 2);
unsigned k1 = (k + 1) % 3;
unsigned k2 = (k + 2) % 3;
//Diagonal already
if(offDiag[k] == 0.0f)
{
break;
}
float theta = (diagMatrix(k2, k2) - diagMatrix(k1, k1)) /
(2.0f * offDiag[k]);
float sign = Math::GetSign(theta);
//Make theta positive
theta *= sign;
//Large term in T
float thetaTerm = theta < 1e6f ? Math::Sqrt(Math::Sq(theta) + 1.0f)
: theta;
//Sign(T) / (|T| + sqrt(T^2 + 1))
float t = sign / (theta + thetaTerm);
//c = 1 / (t^2 + 1) t = s / c
float c = 1.0f / Math::Sqrt(Math::Sq(t) + 1.0f);
//No room for improvement - reached machine precision.
if(c == 1.0f)
{
break;
}
//Jacobi rotation for this iteration
Quaternion jacobi(0.0f, 0.0f, 0.0f, 0.0f);
//Using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
jacobi[k] = sign * Math::Sqrt((1.0f - c) / 2.0f);
//Since our quat-to-matrix convention was for v*M instead of M*v
jacobi.w = Math::Sqrt(1.0f - Math::Sq(jacobi[k]));
//Reached limits of floating point precision
if(jacobi.w == 1.0f)
{
break;
}
quat *= jacobi;
Normalize(&quat);
}
return quat;
}
dgFloat32 dgWorldDynamicUpdate::CalculateJointForceDanzig(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 restAcceleration) const
{
dgFloat32 massMatrix[DG_CONSTRAINT_MAX_ROWS * DG_CONSTRAINT_MAX_ROWS];
dgFloat32 f[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 b[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 low[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 high[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 cacheForce[DG_CONSTRAINT_MAX_ROWS + 4];
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
cacheForce[0] = dgFloat32(1.0f);
cacheForce[1] = dgFloat32(1.0f);
cacheForce[2] = dgFloat32(1.0f);
cacheForce[3] = dgFloat32(1.0f);
dgFloat32* const normalForce = &cacheForce[4];
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 rowsCount = jointInfo->m_pairCount;
dgAssert (rowsCount <= DG_CONSTRAINT_MAX_ROWS);
dgVector accNorm(dgVector::m_zero);
for (dgInt32 i = 0; i < rowsCount; i++) {
const dgJacobianMatrixElement* const row_i = &matrixRow[index + i];
dgFloat32* const massMatrixRow = &massMatrix[rowsCount * i];
dgJacobian JMinvM0(row_i->m_JMinv.m_jacobianM0);
dgJacobian JMinvM1(row_i->m_JMinv.m_jacobianM1);
dgVector diag(JMinvM0.m_linear * row_i->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_i->m_Jt.m_jacobianM0.m_angular +
JMinvM1.m_linear * row_i->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_i->m_Jt.m_jacobianM1.m_angular);
diag = diag.AddHorizontal();
dgFloat32 val = diag.GetScalar() + row_i->m_diagDamp;
dgAssert(val > dgFloat32(0.0f));
massMatrixRow[i] = val;
for (dgInt32 j = i + 1; j < rowsCount; j++) {
const dgJacobianMatrixElement* const row_j = &matrixRow[index + j];
dgVector offDiag(JMinvM0.m_linear * row_j->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_j->m_Jt.m_jacobianM0.m_angular +
JMinvM1.m_linear * row_j->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_j->m_Jt.m_jacobianM1.m_angular);
offDiag = offDiag.AddHorizontal();
dgFloat32 val1 = offDiag.GetScalar();
massMatrixRow[j] = val1;
massMatrix[j * rowsCount + i] = val1;
}
}
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
for (dgInt32 i = 0; i < rowsCount; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgJacobian JMinvM0(row->m_JMinv.m_jacobianM0);
dgJacobian JMinvM1(row->m_JMinv.m_jacobianM1);
dgVector diag(JMinvM0.m_linear * y0.m_linear + JMinvM0.m_angular * y0.m_angular +
JMinvM1.m_linear * y1.m_linear + JMinvM1.m_angular * y1.m_angular);
dgVector accel(row->m_coordenateAccel - row->m_force * row->m_diagDamp - (diag.AddHorizontal()).GetScalar());
dgInt32 frictionIndex = row->m_normalForceIndex;
dgAssert(((frictionIndex < 0) && (normalForce[frictionIndex] == dgFloat32(1.0f))) || ((frictionIndex >= 0) && (normalForce[frictionIndex] >= dgFloat32(0.0f))));
normalForce[i] = row->m_force;
dgFloat32 frictionNormal = normalForce[frictionIndex];
dgFloat32 lowerFrictionForce = frictionNormal * row->m_lowerBoundFrictionCoefficent;
dgFloat32 upperFrictionForce = frictionNormal * row->m_upperBoundFrictionCoefficent;
//f[i] = row->m_force;
f[i] = dgFloat32 (0.0f);
b[i] = accel.GetScalar();
low[i] = lowerFrictionForce - row->m_force;
high[i] = upperFrictionForce - row->m_force;
dgVector force(row->m_force + row->m_invJMinvJt * accel.GetScalar());
accel = accel.AndNot((force > upperFrictionForce) | (force < lowerFrictionForce));
accNorm = accNorm.GetMax(accel.Abs());
}
dgSolveDantzigLCP(rowsCount, massMatrix, f, b, low, high);
dgVector linearM0(internalForces[m0].m_linear);
dgVector angularM0(internalForces[m0].m_angular);
dgVector linearM1(internalForces[m1].m_linear);
dgVector angularM1(internalForces[m1].m_angular);
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgVector jointForce (f[i]);
row->m_force = f[i] + row->m_force;
linearM0 += row->m_Jt.m_jacobianM0.m_linear * jointForce;
angularM0 += row->m_Jt.m_jacobianM0.m_angular * jointForce;
linearM1 += row->m_Jt.m_jacobianM1.m_linear * jointForce;
angularM1 += row->m_Jt.m_jacobianM1.m_angular * jointForce;
}
internalForces[m0].m_linear = linearM0;
internalForces[m0].m_angular = angularM0;
internalForces[m1].m_linear = linearM1;
internalForces[m1].m_angular = angularM1;
return accNorm.GetScalar();
}
