DG_INLINE void InitKinematicMassMatrix (const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgFloat32 correctionFactor) { dgAssert (0); dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0); m_bodyMass.SetZero(); m_bodyForce.SetZero(); if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) { GetInertia(); } if (m_joint) { dgAssert(m_parent); const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; dgAssert(jointInfo->m_joint == m_joint); dgAssert(jointInfo->m_joint->GetBody0() == m_body); dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body); m_dof = 0; const dgInt32 count = jointInfo->m_pairCount; const dgInt32 first = jointInfo->m_pairStart; for (dgInt32 i = 0; i < count; i++) { const dgJacobianMatrixElement* const row = &matrixRow[i + first]; m_jointForce[m_dof] = -dgClamp(row->m_penetration * correctionFactor, dgFloat32(-0.25f), dgFloat32(0.25f)); m_sourceJacobianIndex[m_dof] = dgInt8(i); m_dof++; } GetJacobians(jointInfo, matrixRow); } Factorize(); }
DG_INLINE bool ValidateForcesLCP(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, const dgForcePair& force) { dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0); bool valid = true; if (m_joint) { dgAssert(m_parent); const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; dgAssert(jointInfo->m_joint == m_joint); dgAssert(jointInfo->m_joint->GetBody0() == m_body); dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body); const dgInt32 first = jointInfo->m_pairStart; for (dgInt32 i = 0; (i < m_dof) && valid; i++) { dgInt32 k = m_sourceJacobianIndex[i]; const dgJacobianMatrixElement* const row = &matrixRow[first + k]; dgFloat32 f = force.m_joint[i] + row->m_force; valid = valid && (f >= row->m_lowerBoundFrictionCoefficent); valid = valid && (f <= row->m_upperBoundFrictionCoefficent); } } return valid; }
DG_INLINE void Factorize(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow) { dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0); m_bodyMass.SetZero(); if (m_body->GetInvMass().m_w != dgFloat32 (0.0f)) { GetInertia(); } if (m_joint) { dgAssert (m_parent); const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; dgAssert(jointInfo->m_joint == m_joint); dgAssert(jointInfo->m_joint->GetBody0() == m_body); dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body); m_dof = 0; const dgInt32 count = jointInfo->m_pairCount; const dgInt32 first = jointInfo->m_pairStart; for (dgInt32 i = 0; i < count; i++) { const dgJacobianMatrixElement* const row = &matrixRow[i + first]; if (((row->m_lowerBoundFrictionCoefficent < dgFloat32 (-1.0e9f)) && row->m_upperBoundFrictionCoefficent > dgFloat32 (1.0e9f))) { m_sourceJacobianIndex[m_dof] = dgInt8(i); m_dof ++; } } GetJacobians(jointInfo, matrixRow); } Factorize(); }
dgGoogol::dgGoogol(dgFloat64 value) :m_sign(0) ,m_exponent(0) { dgInt32 exp; dgFloat64 mantissa = fabs (frexp(value, &exp)); m_exponent = dgInt16 (exp); m_sign = (value >= 0) ? 0 : 1; memset (m_mantissa, 0, sizeof (m_mantissa)); m_mantissa[0] = (dgInt64 (dgFloat64 (dgUnsigned64(1)<<62) * mantissa)); // it looks like GCC have problems with this //dgAssert (m_mantissa[0] >= 0); dgAssert ((m_mantissa[0] & dgUnsigned64(1)<<63) == 0); }
dgKinematicBody* dgWorld::CreateKinematicBody (dgCollisionInstance* const collision, const dgMatrix& matrix) { dgKinematicBody* const body = new (m_allocator) dgKinematicBody(); dgAssert (dgInt32 (sizeof (dgBody) & 0xf) == 0); dgAssert ((dgUnsigned64 (body) & 0xf) == 0); InitBody (body, collision, matrix); return body; }
dgGoogol dgGoogol::operator* (const dgGoogol &A) const { dgAssert (dgInt64 (m_mantissa[0]) >= 0); dgAssert (dgInt64 (A.m_mantissa[0]) >= 0); if (m_mantissa[0] && A.m_mantissa[0]) { dgUnsigned64 mantissaAcc[DG_GOOGOL_SIZE * 2]; memset (mantissaAcc, 0, sizeof (mantissaAcc)); for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) { dgUnsigned64 a = m_mantissa[i]; if (a) { dgUnsigned64 mantissaScale[2 * DG_GOOGOL_SIZE]; memset (mantissaScale, 0, sizeof (mantissaScale)); A.ScaleMantissa (&mantissaScale[i], a); dgUnsigned64 carrier = 0; for (dgInt32 j = 0; j < 2 * DG_GOOGOL_SIZE; j ++) { const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - j; dgUnsigned64 m0 = mantissaAcc[k]; dgUnsigned64 m1 = mantissaScale[k]; mantissaAcc[k] = m0 + m1 + carrier; carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier); } } } dgUnsigned64 carrier = 0; dgInt32 bits = dgUnsigned64(LeadingZeros (mantissaAcc[0]) - 2); for (dgInt32 i = 0; i < 2 * DG_GOOGOL_SIZE; i ++) { const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - i; dgUnsigned64 a = mantissaAcc[k]; mantissaAcc[k] = (a << dgUnsigned64(bits)) | carrier; carrier = a >> dgUnsigned64(64 - bits); } dgInt32 exp = m_exponent + A.m_exponent - (bits - 2); dgGoogol tmp; tmp.m_sign = m_sign ^ A.m_sign; tmp.m_exponent = dgInt16 (exp); memcpy (tmp.m_mantissa, mantissaAcc, sizeof (m_mantissa)); return tmp; }
dgBody* dgWorld::CreateBody(dgCollision* const collision, const dgMatrix& matrix) { dgBody* body; _ASSERTE (collision); body = new (m_allocator) dgBody(); _ASSERTE ((sizeof (dgBody) & 0xf) == 0); _ASSERTE ((dgUnsigned64 (body) & 0xf) == 0); memset (body, 0, sizeof (dgBody)); // m_bodiesCount ++; m_bodiesUniqueID ++; body->m_world = this; body->m_freeze = false; body->m_sleeping = false; body->m_autoSleep = true; body->m_isInWorld = true; body->m_equilibrium = false; body->m_continueCollisionMode = false; body->m_collideWithLinkedBodies = true; body->m_solverInContinueCollision = false; body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false); body->m_uniqueID = dgInt32 (m_bodiesUniqueID); dgBodyMasterList::AddBody(body); // dgBodyActiveList___::AddBody(body); // _ASSERTE (body->m_activeNode); // body->m_freezeAccel2 = m_freezeAccel2; // body->m_freezeAlpha2 = m_freezeAlpha2; // body->m_freezeSpeed2 = m_freezeSpeed2; // body->m_freezeOmega2 = m_freezeOmega2; body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f))); body->SetLinearDamping (dgFloat32 (0.1045f)) ; body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f))); body->AttachCollision(collision); body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID); body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), DG_INFINITE_MASS, DG_INFINITE_MASS, DG_INFINITE_MASS); dgBroadPhaseCollision::Add (body); //body->SetMatrix (dgGetIdentityMatrix()); body->SetMatrix (matrix); body->m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f); return body; }
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgForcePair& accel) { dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0); m_bodyMass.SetZero(); if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) { GetInertia(); } if (m_joint) { dgAssert(m_parent); const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; dgAssert(jointInfo->m_joint == m_joint); dgAssert(jointInfo->m_joint->GetBody0() == m_body); dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body); const dgInt32 m0 = jointInfo->m_m0; const dgInt32 m1 = jointInfo->m_m1; const dgJacobian& y0 = internalForces[m0]; const dgJacobian& y1 = internalForces[m1]; const dgInt32 first = jointInfo->m_pairStart; dgInt32 clampedValue = m_dof - 1; //dgSpatialVector& accel = force.m_joint; for (dgInt32 i = 0; i < m_dof; i++) { dgInt32 k = m_sourceJacobianIndex[i]; const dgJacobianMatrixElement* const row = &matrixRow[first + k]; //dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar(); dgFloat32 force = row->m_force; if ((force >= row->m_lowerBoundFrictionCoefficent) && (force <= row->m_upperBoundFrictionCoefficent)) { dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) + row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular)); residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal(); accel.m_joint[i] = -residual.GetScalar(); } else { dgAssert (0); dgSwap (m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]); i --; m_dof --; clampedValue --; } } GetJacobians(jointInfo, matrixRow); } Factorize(); }
DG_INLINE void UpdateFactorizeLCP(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgForcePair& force, dgForcePair& accel) { dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0); m_bodyMass.SetZero(); if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) { GetInertia(); } if (m_joint) { dgAssert(m_parent); const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; dgAssert(jointInfo->m_joint == m_joint); dgAssert(jointInfo->m_joint->GetBody0() == m_body); dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body); const dgInt32 first = jointInfo->m_pairStart; dgInt32 clampedValue = m_dof - 1; for (dgInt32 i = 0; i < m_dof; i++) { dgInt32 k = m_sourceJacobianIndex[i]; const dgJacobianMatrixElement* const row = &matrixRow[first + k]; dgFloat32 f = force.m_joint[i] + row->m_force; if (f < row->m_lowerBoundFrictionCoefficent) { force.m_joint[i] = dgFloat32 (0.0f); dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]); dgSwap(force.m_joint[i], force.m_joint[clampedValue]); dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]); i--; m_dof--; clampedValue--; } else if (f > row->m_upperBoundFrictionCoefficent) { force.m_joint[i] = dgFloat32 (0.0f); dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]); dgSwap(force.m_joint[i], force.m_joint[clampedValue]); dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]); i--; m_dof--; clampedValue--; } } GetJacobians(jointInfo, matrixRow); } Factorize(); }
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow) { dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0); m_bodyMass.SetZero(); if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) { GetInertia(); } if (m_joint) { dgAssert(m_parent); const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index]; dgAssert(jointInfo->m_joint == m_joint); dgAssert(jointInfo->m_joint->GetBody0() == m_body); dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body); const dgInt32 m0 = jointInfo->m_m0; const dgInt32 m1 = jointInfo->m_m1; const dgJacobian& y0 = internalForces[m0]; const dgJacobian& y1 = internalForces[m1]; m_dof = 0; const dgInt32 count = jointInfo->m_pairCount; const dgInt32 first = jointInfo->m_pairStart; for (dgInt32 i = 0; i < count; i++) { const dgJacobianMatrixElement* const row = &matrixRow[i + first]; dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) + row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular)); residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal(); dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar(); if ((force > row->m_lowerBoundFrictionCoefficent) && (force < row->m_upperBoundFrictionCoefficent)) { m_sourceJacobianIndex[m_dof] = dgInt8(i); m_dof++; } } GetJacobians(jointInfo, matrixRow); } Factorize(); }
void dgPolygonSoupDatabaseBuilder::OptimizeByGroupID( dgPolygonSoupDatabaseBuilder& source, dgInt32 faceNumber, dgInt32 faceIndexNumber, dgPolygonSoupDatabaseBuilder& leftOver) { dgInt32 indexPool[1024 * 1]; dgInt32 atributeData[1024 * 1]; dgVector vertexPool[1024 * 1]; dgPolyhedra polyhedra(m_allocator); dgInt32 attribute = source.m_vertexIndex[faceIndexNumber]; for (dgInt32 i = 0; i < dgInt32(sizeof(atributeData) / sizeof(dgInt32)); i++) { indexPool[i] = i; atributeData[i] = attribute; } leftOver.Begin(); polyhedra.BeginFace(); for (dgInt32 i = faceNumber; i < source.m_faceCount; i++) { dgInt32 indexCount; indexCount = source.m_faceVertexCount[i]; _ASSERTE(indexCount < 1024); if (source.m_vertexIndex[faceIndexNumber] == attribute) { dgEdge* const face = polyhedra.AddFace(indexCount - 1, &source.m_vertexIndex[faceIndexNumber + 1]); if (!face) { dgInt32 faceArray; for (dgInt32 j = 0; j < indexCount - 1; j++) { dgInt32 index; index = source.m_vertexIndex[faceIndexNumber + j + 1]; vertexPool[j] = source.m_vertexPoints[index]; } faceArray = indexCount - 1; leftOver.AddMesh(&vertexPool[0].m_x, indexCount - 1, sizeof(dgVector), 1, &faceArray, indexPool, atributeData, dgGetIdentityMatrix()); } else { // set the attribute dgEdge* ptr = face; do { ptr->m_userData = dgUnsigned64(attribute); ptr = ptr->m_next; } while (ptr != face); } } faceIndexNumber += indexCount; } leftOver.Optimize(false); polyhedra.EndFace(); dgPolyhedra facesLeft(m_allocator); facesLeft.BeginFace(); polyhedra.ConvexPartition(&source.m_vertexPoints[0].m_x, sizeof(dgBigVector), &facesLeft); facesLeft.EndFace(); dgInt32 mark = polyhedra.IncLRU(); dgPolyhedra::Iterator iter(polyhedra); for (iter.Begin(); iter; iter++) { dgEdge* const edge = &(*iter); if (edge->m_incidentFace < 0) { continue; } if (edge->m_mark == mark) { continue; } dgEdge* ptr = edge; dgInt32 indexCount = 0; do { ptr->m_mark = mark; vertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex]; indexCount++; ptr = ptr->m_next; } while (ptr != edge); if (indexCount >= 3) { AddMesh(&vertexPool[0].m_x, indexCount, sizeof(dgVector), 1, &indexCount, indexPool, atributeData, dgGetIdentityMatrix()); } } mark = facesLeft.IncLRU(); dgPolyhedra::Iterator iter1(facesLeft); for (iter1.Begin(); iter1; iter1++) { dgEdge* const edge = &(*iter1); if (edge->m_incidentFace < 0) { continue; } if (edge->m_mark == mark) { continue; } dgEdge* ptr = edge; dgInt32 indexCount = 0; do { ptr->m_mark = mark; vertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex]; indexCount++; ptr = ptr->m_next; } while (ptr != edge); if (indexCount >= 3) { AddMesh(&vertexPool[0].m_x, indexCount, sizeof(dgVector), 1, &indexCount, indexPool, atributeData, dgGetIdentityMatrix()); } } }
dgGoogol::operator double() const { dgFloat64 mantissa = (dgFloat64(1.0f) / dgFloat64 (dgUnsigned64(1)<<62)) * dgFloat64 (m_mantissa[0]); mantissa = ldexp(mantissa, m_exponent) * (m_sign ? dgFloat64 (-1.0f) : dgFloat64 (1.0f)); return mantissa; }