void dgCollisionScene::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const { dgVector origin (matrix.TransformVector(m_boxOrigin)); dgVector size (m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING, m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING, m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING, dgFloat32 (0.0f)); p0 = origin - size; p1 = origin + size; #ifdef DG_DEBUG_AABB dgInt32 i; dgVector q0; dgVector q1; dgMatrix trans (matrix.Transpose()); for (i = 0; i < 3; i ++) { q0[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i].Scale (-1.0f)))[i]; q1[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i]))[i]; } dgVector err0 (p0 - q0); dgVector err1 (p1 - q1); dgFloat32 err; err = GetMax (size.m_x, size.m_y, size.m_z) * 0.5f; _ASSERTE ((err0 % err0) < err); _ASSERTE ((err1 % err1) < err); #endif }
DG_INLINE void dgSolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide) { const dgInt32 m0 = jointInfo->m_m0; const dgInt32 m1 = jointInfo->m_m1; const dgInt32 index = jointInfo->m_pairStart; const dgInt32 count = jointInfo->m_pairCount; const dgDynamicBody* const body0 = (dgDynamicBody*)m_bodyArray[m0].m_body; const dgDynamicBody* const body1 = (dgDynamicBody*)m_bodyArray[m1].m_body; const bool isBilateral = jointInfo->m_joint->IsBilateral(); const dgMatrix invInertia0 = body0->m_invWorldInertiaMatrix; const dgMatrix invInertia1 = body1->m_invWorldInertiaMatrix; const dgVector invMass0(body0->m_invMass[3]); const dgVector invMass1(body1->m_invMass[3]); dgSoaFloat force0(m_soaZero); if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { force0 = dgSoaFloat(body0->m_externalForce, body0->m_externalTorque); } dgSoaFloat force1(m_soaZero); if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { force1 = dgSoaFloat(body1->m_externalForce, body1->m_externalTorque); } jointInfo->m_preconditioner0 = dgFloat32(1.0f); jointInfo->m_preconditioner1 = dgFloat32(1.0f); if ((invMass0.GetScalar() > dgFloat32(0.0f)) && (invMass1.GetScalar() > dgFloat32(0.0f)) && !(body0->GetSkeleton() && body1->GetSkeleton())) { const dgFloat32 mass0 = body0->GetMass().m_w; const dgFloat32 mass1 = body1->GetMass().m_w; if (mass0 > (DG_DIAGONAL_PRECONDITIONER * mass1)) { jointInfo->m_preconditioner0 = mass0 / (mass1 * DG_DIAGONAL_PRECONDITIONER); } else if (mass1 > (DG_DIAGONAL_PRECONDITIONER * mass0)) { jointInfo->m_preconditioner1 = mass1 / (mass0 * DG_DIAGONAL_PRECONDITIONER); } } const dgFloat32 forceImpulseScale = dgFloat32(1.0f); const dgSoaFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0); const dgSoaFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0); for (dgInt32 i = 0; i < count; i++) { dgLeftHandSide* const row = &leftHandSide[index + i]; dgRightHandSide* const rhs = &rightHandSide[index + i]; 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); const dgSoaFloat& JMinvM0 = (dgSoaFloat&)row->m_JMinv.m_jacobianM0; const dgSoaFloat& JMinvM1 = (dgSoaFloat&)row->m_JMinv.m_jacobianM1; const dgSoaFloat tmpAccel((JMinvM0 * force0).MulAdd(JMinvM1, force1)); dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal(); rhs->m_deltaAccel = extenalAcceleration * forceImpulseScale; rhs->m_coordenateAccel += extenalAcceleration * forceImpulseScale; dgAssert(rhs->m_jointFeebackForce); const dgFloat32 force = rhs->m_jointFeebackForce->m_force * forceImpulseScale; rhs->m_force = isBilateral ? dgClamp(force, rhs->m_lowerBoundFrictionCoefficent, rhs->m_upperBoundFrictionCoefficent) : force; rhs->m_maxImpact = dgFloat32(0.0f); const dgSoaFloat& JtM0 = (dgSoaFloat&)row->m_Jt.m_jacobianM0; const dgSoaFloat& JtM1 = (dgSoaFloat&)row->m_Jt.m_jacobianM1; const dgSoaFloat tmpDiag((weight0 * JMinvM0 * JtM0).MulAdd(weight1, JMinvM1 * JtM1)); dgFloat32 diag = tmpDiag.AddHorizontal(); dgAssert(diag > dgFloat32(0.0f)); rhs->m_diagDamp = diag * rhs->m_stiffness; diag *= (dgFloat32(1.0f) + rhs->m_stiffness); //rhs->m_jinvMJt = diag; rhs->m_invJinvMJt = dgFloat32(1.0f) / diag; } }