int dComplemtaritySolver::BuildJacobianMatrix (int jointCount, dBilateralJoint ** const jointArray, dFloat timestep, dJacobianPair * const jacobianArray, dJacobianColum * const jacobianColumnArray, int maxRowCount)
{
int rowCount = 0;
dParamInfo constraintParams;
constraintParams.m_timestep = timestep;
constraintParams.m_timestepInv = 1.0f / timestep;
// calculate Jacobian derivative for each active joint
for (int j = 0; j < jointCount; j ++)
{
dBilateralJoint * const joint = jointArray[j];
constraintParams.m_count = 0;
joint->JacobianDerivative (&constraintParams);
int dofCount = constraintParams.m_count;
joint->m_count = dofCount;
joint->m_start = rowCount;
// complete the derivative matrix for this joint
int index = joint->m_start;
dBodyState * const state0 = joint->m_state0;
dBodyState * const state1 = joint->m_state1;
const dMatrix & invInertia0 = state0->m_invInertia;
const dMatrix & invInertia1 = state1->m_invInertia;
dFloat invMass0 = state0->m_invMass;
dFloat invMass1 = state1->m_invMass;
dFloat weight = 0.9f;
for (int i = 0; i < dofCount; i ++)
{
dJacobianPair * const row = &jacobianArray[index];
dJacobianColum * const col = &jacobianColumnArray[index];
jacobianArray[rowCount] = constraintParams.m_jacobians[i];
dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
dVector JMinvIM0angular = invInertia0.UnrotateVector (row->m_jacobian_IM0.m_angular);
dVector JMinvIM1angular = invInertia1.UnrotateVector (row->m_jacobian_IM1.m_angular);
dVector tmpDiag (JMinvIM0linear.CompProduct (row->m_jacobian_IM0.m_linear) + JMinvIM0angular.CompProduct (row->m_jacobian_IM0.m_angular) + JMinvIM1linear.CompProduct (row->m_jacobian_IM1.m_linear) + JMinvIM1angular.CompProduct (row->m_jacobian_IM1.m_angular));
dVector tmpAccel (JMinvIM0linear.CompProduct (state0->m_externalForce) + JMinvIM0angular.CompProduct (state0->m_externalTorque) + JMinvIM1linear.CompProduct (state1->m_externalForce) + JMinvIM1angular.CompProduct (state1->m_externalTorque));
dFloat extenalAcceleration = - (tmpAccel[0] + tmpAccel[1] + tmpAccel[2]);
col->m_diagDamp = 1.0f;
col->m_coordenateAccel = constraintParams.m_jointAccel[i];
col->m_jointLowFriction = constraintParams.m_jointLowFriction[i];
col->m_jointHighFriction = constraintParams.m_jointHighFriction[i];
col->m_deltaAccel = extenalAcceleration;
col->m_coordenateAccel += extenalAcceleration;
col->m_force = joint->m_jointFeebackForce[i] * weight;
dFloat stiffness = COMPLEMENTARITY_PSD_DAMP_TOL * col->m_diagDamp;
dFloat diag = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
dAssert (diag > dFloat (0.0f));
col->m_diagDamp = diag * stiffness;
diag *= (dFloat (1.0f) + stiffness);
col->m_invDJMinvJt = dFloat (1.0f) / diag;
index ++;
rowCount ++;
dAssert (rowCount < maxRowCount);
}
}
return rowCount;
}
void dgWorldDynamicUpdate::BuildJacobianMatrix (const dgBodyInfo* const bodyInfoArray, const dgJointInfo* const jointInfo, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 forceImpulseScale) const
{
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgBody* const body0 = bodyInfoArray[m0].m_body;
const dgBody* const body1 = bodyInfoArray[m1].m_body;
const bool isBilateral = jointInfo->m_joint->IsBilateral();
const dgVector invMass0(body0->m_invMass[3]);
const dgMatrix& invInertia0 = body0->m_invWorldInertiaMatrix;
const dgVector invMass1(body1->m_invMass[3]);
const dgMatrix& invInertia1 = body1->m_invWorldInertiaMatrix;
dgVector force0(dgVector::m_zero);
dgVector torque0(dgVector::m_zero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = ((dgDynamicBody*)body0)->m_externalForce;
torque0 = ((dgDynamicBody*)body0)->m_externalTorque;
}
dgVector force1(dgVector::m_zero);
dgVector torque1(dgVector::m_zero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = ((dgDynamicBody*)body1)->m_externalForce;
torque1 = ((dgDynamicBody*)body1)->m_externalTorque;
}
const dgVector scale0(jointInfo->m_scale0);
const dgVector scale1(jointInfo->m_scale1);
dgJacobian forceAcc0;
dgJacobian forceAcc1;
forceAcc0.m_linear = dgVector::m_zero;
forceAcc0.m_angular = dgVector::m_zero;
forceAcc1.m_linear = dgVector::m_zero;
forceAcc1.m_angular = dgVector::m_zero;
for (dgInt32 i = 0; i < count; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgAssert(row->m_Jt.m_jacobianM0.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM0.m_angular.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_angular.m_w == dgFloat32(0.0f));
row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0;
row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular);
row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1;
row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular);
dgVector tmpAccel(row->m_JMinv.m_jacobianM0.m_linear * force0 + row->m_JMinv.m_jacobianM0.m_angular * torque0 +
row->m_JMinv.m_jacobianM1.m_linear * force1 + row->m_JMinv.m_jacobianM1.m_angular * torque1);
dgAssert(tmpAccel.m_w == dgFloat32(0.0f));
dgFloat32 extenalAcceleration = -(tmpAccel.AddHorizontal()).GetScalar();
row->m_deltaAccel = extenalAcceleration * forceImpulseScale;
row->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
dgAssert(row->m_jointFeebackForce);
const dgFloat32 force = row->m_jointFeebackForce->m_force * forceImpulseScale;
row->m_force = isBilateral ? dgClamp(force, row->m_lowerBoundFrictionCoefficent, row->m_upperBoundFrictionCoefficent) : force;
row->m_force0 = row->m_force;
row->m_maxImpact = dgFloat32(0.0f);
dgVector jMinvM0linear (scale0 * row->m_JMinv.m_jacobianM0.m_linear);
dgVector jMinvM0angular (scale0 * row->m_JMinv.m_jacobianM0.m_angular);
dgVector jMinvM1linear (scale1 * row->m_JMinv.m_jacobianM1.m_linear);
dgVector jMinvM1angular (scale1 * row->m_JMinv.m_jacobianM1.m_angular);
dgVector tmpDiag(jMinvM0linear * row->m_Jt.m_jacobianM0.m_linear + jMinvM0angular * row->m_Jt.m_jacobianM0.m_angular +
jMinvM1linear * row->m_Jt.m_jacobianM1.m_linear + jMinvM1angular * row->m_Jt.m_jacobianM1.m_angular);
dgAssert (tmpDiag.m_w == dgFloat32 (0.0f));
dgFloat32 diag = (tmpDiag.AddHorizontal()).GetScalar();
dgAssert(diag > dgFloat32(0.0f));
row->m_diagDamp = diag * row->m_stiffness;
diag *= (dgFloat32(1.0f) + row->m_stiffness);
row->m_jMinvJt = diag;
row->m_invJMinvJt = dgFloat32(1.0f) / diag;
dgAssert(dgCheckFloat(row->m_force));
dgVector val(row->m_force);
forceAcc0.m_linear += row->m_Jt.m_jacobianM0.m_linear * val;
forceAcc0.m_angular += row->m_Jt.m_jacobianM0.m_angular * val;
forceAcc1.m_linear += row->m_Jt.m_jacobianM1.m_linear * val;
forceAcc1.m_angular += row->m_Jt.m_jacobianM1.m_angular * val;
}
forceAcc0.m_linear = forceAcc0.m_linear * scale0;
forceAcc0.m_angular = forceAcc0.m_angular * scale0;
forceAcc1.m_linear = forceAcc1.m_linear * scale1;
forceAcc1.m_angular = forceAcc1.m_angular * scale1;
if (!body0->m_equilibrium) {
internalForces[m0].m_linear += forceAcc0.m_linear;
internalForces[m0].m_angular += forceAcc0.m_angular;
}
if (!body1->m_equilibrium) {
internalForces[m1].m_linear += forceAcc1.m_linear;
internalForces[m1].m_angular += forceAcc1.m_angular;
}
}
SymmetricSchurDecomposition::SymmetricSchurDecomposition(const Matrix & s)
: diagonal_(s.rows()), eigenVectors_(s.rows(), s.columns(), 0.0) {
QL_REQUIRE(s.rows() > 0 && s.columns() > 0, "null matrix given");
QL_REQUIRE(s.rows()==s.columns(), "input matrix must be square");
Size size = s.rows();
for (Size q=0; q<size; q++) {
diagonal_[q] = s[q][q];
eigenVectors_[q][q] = 1.0;
}
Matrix ss = s;
std::vector<Real> tmpDiag(diagonal_.begin(), diagonal_.end());
std::vector<Real> tmpAccumulate(size, 0.0);
Real threshold, epsPrec = 1e-15;
bool keeplooping = true;
Size maxIterations = 100, ite = 1;
do {
//main loop
Real sum = 0;
for (Size a=0; a<size-1; a++) {
for (Size b=a+1; b<size; b++) {
sum += std::fabs(ss[a][b]);
}
}
if (sum==0) {
keeplooping = false;
} else {
/* To speed up computation a threshold is introduced to
make sure it is worthy to perform the Jacobi rotation
*/
if (ite<5) threshold = 0.2*sum/(size*size);
else threshold = 0.0;
Size j, k, l;
for (j=0; j<size-1; j++) {
for (k=j+1; k<size; k++) {
Real sine, rho, cosin, heig, tang, beta;
Real smll = std::fabs(ss[j][k]);
if(ite> 5 &&
smll<epsPrec*std::fabs(diagonal_[j]) &&
smll<epsPrec*std::fabs(diagonal_[k])) {
ss[j][k] = 0;
} else if (std::fabs(ss[j][k])>threshold) {
heig = diagonal_[k]-diagonal_[j];
if (smll<epsPrec*std::fabs(heig)) {
tang = ss[j][k]/heig;
} else {
beta = 0.5*heig/ss[j][k];
tang = 1.0/(std::fabs(beta)+
std::sqrt(1+beta*beta));
if (beta<0)
tang = -tang;
}
cosin = 1/std::sqrt(1+tang*tang);
sine = tang*cosin;
rho = sine/(1+cosin);
heig = tang*ss[j][k];
tmpAccumulate[j] -= heig;
tmpAccumulate[k] += heig;
diagonal_[j] -= heig;
diagonal_[k] += heig;
ss[j][k] = 0.0;
for (l=0; l+1<=j; l++)
jacobiRotate_(ss, rho, sine, l, j, l, k);
for (l=j+1; l<=k-1; l++)
jacobiRotate_(ss, rho, sine, j, l, l, k);
for (l=k+1; l<size; l++)
jacobiRotate_(ss, rho, sine, j, l, k, l);
for (l=0; l<size; l++)
jacobiRotate_(eigenVectors_,
rho, sine, l, j, l, k);
}
}
}
for (k=0; k<size; k++) {
tmpDiag[k] += tmpAccumulate[k];
diagonal_[k] = tmpDiag[k];
tmpAccumulate[k] = 0.0;
}
}
} while (++ite<=maxIterations && keeplooping);
QL_ENSURE(ite<=maxIterations,
"Too many iterations (" << maxIterations << ") reached");
// sort (eigenvalues, eigenvectors)
std::vector<std::pair<Real, std::vector<Real> > > temp(size);
std::vector<Real> eigenVector(size);
Size row, col;
for (col=0; col<size; col++) {
std::copy(eigenVectors_.column_begin(col),
eigenVectors_.column_end(col), eigenVector.begin());
temp[col] = std::make_pair(diagonal_[col], eigenVector);
}
std::sort(temp.begin(), temp.end(),
std::greater<std::pair<Real, std::vector<Real> > >());
Real maxEv = temp[0].first;
for (col=0; col<size; col++) {
// check for round-off errors
diagonal_[col] =
(std::fabs(temp[col].first/maxEv)<1e-16 ? 0.0 :
temp[col].first);
Real sign = 1.0;
if (temp[col].second[0]<0.0)
sign = -1.0;
for (row=0; row<size; row++) {
eigenVectors_[row][col] = sign * temp[col].second[row];
}
}
}
