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