void dgWorld::AddSentinelBody()
{
dgCollision* const collision = new (m_allocator) dgCollisionNull (m_allocator, 0x4352fe67);
dgCollisionInstance* const instance = CreateInstance(collision, 0, dgGetIdentityMatrix());
collision->Release();
m_sentinelBody = CreateDynamicBody(instance, dgGetIdentityMatrix());
instance->Release();
}
void dgCollisionInstance::SetGlobalScale (const dgVector& scale)
{
// calculate current matrix
dgMatrix matrix(dgGetIdentityMatrix());
matrix[0][0] = m_scale.m_x;
matrix[1][1] = m_scale.m_y;
matrix[2][2] = m_scale.m_z;
matrix = m_aligmentMatrix * matrix * m_localMatrix;
// extract the original local matrix
dgMatrix transpose (matrix.Transpose());
dgVector globalScale (dgSqrt (transpose[0].DotProduct(transpose[0]).GetScalar()), dgSqrt (transpose[1].DotProduct(transpose[1]).GetScalar()), dgSqrt (transpose[2].DotProduct(transpose[2]).GetScalar()), dgFloat32 (1.0f));
dgVector invGlobalScale (dgFloat32 (1.0f) / globalScale.m_x, dgFloat32 (1.0f) / globalScale.m_y, dgFloat32 (1.0f) / globalScale.m_z, dgFloat32 (1.0f));
dgMatrix localMatrix (m_aligmentMatrix.Transpose() * m_localMatrix);
localMatrix.m_posit = matrix.m_posit * invGlobalScale;
dgAssert (localMatrix.m_posit.m_w == dgFloat32 (1.0f));
if ((dgAbs (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbs (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) {
m_localMatrix = localMatrix;
m_localMatrix.m_posit = m_localMatrix.m_posit * scale | dgVector::m_wOne;
m_aligmentMatrix = dgGetIdentityMatrix();
SetScale (scale);
} else {
// create a new scale matrix
localMatrix[0] = localMatrix[0] * scale;
localMatrix[1] = localMatrix[1] * scale;
localMatrix[2] = localMatrix[2] * scale;
localMatrix[3] = localMatrix[3] * scale;
localMatrix[3][3] = dgFloat32 (1.0f);
// decompose into to align * scale * local
localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix);
m_localMatrix = m_aligmentMatrix * m_localMatrix;
m_aligmentMatrix = m_aligmentMatrix.Transpose();
dgAssert (m_localMatrix.TestOrthogonal());
dgAssert (m_aligmentMatrix.TestOrthogonal());
//dgMatrix xxx1 (dgGetIdentityMatrix());
//xxx1[0][0] = m_scale.m_x;
//xxx1[1][1] = m_scale.m_y;
//xxx1[2][2] = m_scale.m_z;
//dgMatrix xxx (m_aligmentMatrix * xxx1 * m_localMatrix);
bool isIdentity = true;
for (dgInt32 i = 0; i < 3; i ++) {
isIdentity &= dgAbs (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f);
isIdentity &= dgAbs (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f);
}
m_scaleType = isIdentity ? m_nonUniform : m_global;
m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]);
m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f));
}
}
dgSlidingConstraint::dgSlidingConstraint ()
:dgBilateralConstraint()
{
dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0);
m_localMatrix0 = dgGetIdentityMatrix();
m_localMatrix1 = dgGetIdentityMatrix();
m_maxDOF = 6;
m_constId = m_sliderConstraint;
m_posit = dgFloat32 (0.0f);
m_jointAccelFnt = NULL;
}
dgHingeConstraint::dgHingeConstraint ()
:dgBilateralConstraint()
{
dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0);
// constraint->Init ();
m_localMatrix0 = dgGetIdentityMatrix();
m_localMatrix1 = dgGetIdentityMatrix();
m_maxDOF = 6;
m_jointAccelFnt = NULL;
m_constId = m_hingeConstraint;
m_angle = dgFloat32 (dgFloat32 (0.0f));
}
dgUniversalConstraint::dgUniversalConstraint ()
:dgBilateralConstraint()
{
dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0);
m_localMatrix0 = dgGetIdentityMatrix();
m_localMatrix1 = dgGetIdentityMatrix();
m_maxDOF = 6;
m_constId = m_universalConstraint;
m_angle0 = dgFloat32 (0.0f);
m_angle1 = dgFloat32 (0.0f);
m_jointAccelFnt = NULL;
}
dgCollisionNull::dgCollisionNull(dgMemoryAllocator* const allocator,
dgUnsigned32 signature) :
dgCollisionConvex(allocator, signature, dgGetIdentityMatrix(),
m_nullCollision)
{
m_rtti |= dgCollisionNull_RTTI;
}
void dgWorld::InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix)
{
dgAssert (collision);
m_bodiesUniqueID ++;
body->m_world = this;
body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
body->m_uniqueID = dgInt32 (m_bodiesUniqueID);
dgBodyMasterList::AddBody(body);
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);
dgMatrix inertia (dgGetIdentityMatrix());
inertia[0][0] = DG_INFINITE_MASS;
inertia[1][1] = DG_INFINITE_MASS;
inertia[2][2] = DG_INFINITE_MASS;
body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), inertia);
body->SetMatrix (matrix);
if (!body->GetCollision()->IsType (dgCollision::dgCollisionNull_RTTI)) {
m_broadPhase->Add (body);
}
}
dgMatrix dgMatrix::Symetric3by3Inverse () const
{
dgMatrix copy(*this);
dgMatrix inverse(dgGetIdentityMatrix());
for (dgInt32 i = 0; i < 3; i++) {
dgVector den(dgFloat32(1.0f) / copy[i][i]);
copy[i] = copy[i].CompProduct4(den);
inverse[i] = inverse[i].CompProduct4(den);
for (dgInt32 j = 0; j < 3; j++) {
if (j != i) {
dgVector pivot(copy[j][i]);
copy[j] -= copy[i].CompProduct4(pivot);
inverse[j] -= inverse[i].CompProduct4(pivot);
}
}
}
#ifdef _DEBUG
dgMatrix test(*this * inverse);
dgAssert(dgAbsf(test[0][0] - dgFloat32(1.0f)) < dgFloat32(0.01f));
dgAssert(dgAbsf(test[1][1] - dgFloat32(1.0f)) < dgFloat32(0.01f));
dgAssert(dgAbsf(test[2][2] - dgFloat32(1.0f)) < dgFloat32(0.01f));
#endif
return inverse;
}
void dgCollision::GetCollisionInfo(dgCollisionInfo* info) const
{
// memset (info, 0, sizeof (dgCollisionInfo));
info->m_offsetMatrix = dgGetIdentityMatrix();
info->m_collisionType = m_collsionId;
info->m_refCount = GetRefCount();
info->m_userDadaID = dgInt32 (SetUserDataID());
}
dgMatrix dgBody::CalculateLocalInertiaMatrix () const
{
dgMatrix inertia (dgGetIdentityMatrix());
inertia[0][0] = m_mass.m_x;
inertia[1][1] = m_mass.m_y;
inertia[2][2] = m_mass.m_z;
return inertia;
}
dgDynamicBodyAsymetric::dgDynamicBodyAsymetric(dgWorld* const world, const dgTree<const dgCollision*, dgInt32>* const collisionNode, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber)
:dgDynamicBody(world, collisionNode, serializeCallback, userData, revisionNumber)
, m_principalAxis(dgGetIdentityMatrix())
{
m_type = m_dynamicBody;
m_rtti |= m_dynamicBodyRTTI;
serializeCallback(userData, &m_principalAxis, sizeof(m_principalAxis));
}
dgCollisionInstance::dgCollisionInstance()
:m_globalMatrix(dgGetIdentityMatrix())
,m_localMatrix (dgGetIdentityMatrix())
,m_aligmentMatrix (dgGetIdentityMatrix())
,m_scale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_invScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_maxScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_material()
,m_world(NULL)
,m_childShape (NULL)
,m_subCollisionHandle(NULL)
,m_parent(NULL)
,m_skinThickness(dgFloat32 (0.0f))
,m_collisionMode(1)
,m_refCount(1)
,m_scaleType(m_unit)
{
}
void dgCollisionBVH::ForEachFace (dgAABBIntersectCallback callback, void* const context) const
{
dgVector p0 (-1.0e10f, -1.0e10f, -1.0e10f, 1.0f);
dgVector p1 ( 1.0e10f, 1.0e10f, 1.0e10f, 1.0f);
dgVector zero (dgFloat32 (0.0f));
dgFastAABBInfo box (dgGetIdentityMatrix(), dgVector (dgFloat32 (1.0e15f)));
//ForAllSectors (p0, p1, zero, dgFloat32 (1.0f), callback, context);
ForAllSectors (box, zero, dgFloat32 (1.0f), callback, context);
}
dgDynamicBodyAsymetric::dgDynamicBodyAsymetric()
:dgDynamicBody()
, m_principalAxis(dgGetIdentityMatrix())
{
m_type = m_dynamicBody;
m_rtti |= m_dynamicBodyAsymentricRTTI;
dgAssert(dgInt32(sizeof(dgDynamicBody) & 0x0f) == 0);
}
dgMatrix dgDynamicBody::CalculateInvInertiaMatrix() const
{
dgMatrix matrix (m_principalAxis * m_matrix);
matrix.m_posit = dgVector::m_wOne;
dgMatrix diagonal(dgGetIdentityMatrix());
diagonal[0][0] = m_invMass[0];
diagonal[1][1] = m_invMass[1];
diagonal[2][2] = m_invMass[2];
return matrix * diagonal * matrix.Inverse();
}
void dgCollisionDeformableSolidMesh::InitClusters()
{
dgStack<dgMatrix> covarianceMatrixPool(m_particles.m_count);
dgMatrix* const covarianceMatrix = &covarianceMatrixPool[0];
const dgFloat32* const masses = m_particles.m_unitMass;
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
m_clusterCom0[i] = dgVector (dgFloat32 (0.0f));
m_clusterMass[i] = dgFloat32 (0.0f);
m_clusterRotationInitialGuess[i] = dgGetIdentityMatrix();
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgVector& r = m_shapePosit[i];
dgVector mr (r.Scale4(masses[i]));
covarianceMatrix[i] = dgMatrix (mr, r);
dgAssert (covarianceMatrix[i].TestSymetric3x3());
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 index = m_clusterPosit[start + j];
m_clusterCom0[index] += mr;
m_clusterMass[index] += masses[i];
}
}
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
dgVector mcr0 (m_clusterCom0[i]);
m_clusterCom0[i] = mcr0.Scale4 (dgFloat32 (1.0f) / m_clusterMass[i]);
m_clusterAqqInv[i] = dgMatrix (mcr0.CompProduct4(dgVector::m_negOne), m_clusterCom0[i]);
dgAssert (m_clusterAqqInv[i].TestSymetric3x3());
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 index = m_clusterPosit[start + j];
dgMatrix& covariance = m_clusterAqqInv[index];
covariance.m_front += covarianceMatrix[i].m_front;
covariance.m_up += covarianceMatrix[i].m_up;
covariance.m_right += covarianceMatrix[i].m_right;
dgAssert (covariance.TestSymetric3x3());
}
}
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
dgMatrix& AqqInv = m_clusterAqqInv[i];
dgAssert (AqqInv.TestSymetric3x3());
AqqInv = AqqInv.Symetric3by3Inverse();
}
}
void dgConstraint::InitInfo (dgConstraintInfo* const info) const
{
info->m_attachBody_0 = GetBody0();
dgAssert (info->m_attachBody_0);
dgWorld* const world = info->m_attachBody_0->GetWorld();
if (info->m_attachBody_0 == (dgBody*)world->GetSentinelBody()) {
info->m_attachBody_0 = NULL;
}
info->m_attachBody_1 = GetBody1();
if (info->m_attachBody_1 == (dgBody*)world->GetSentinelBody()) {
info->m_attachBody_1 = NULL;
}
info->m_attachMatrix_0 = dgGetIdentityMatrix();
info->m_attachMatrix_1 = dgGetIdentityMatrix();
info->m_discriptionType[0] = 0;
}
void dgWorld::AddSentinelBody()
{
dgCollision* collision;
collision = new (m_allocator) dgCollisionNull (m_allocator, 0x4352fe67);
m_sentionelBody = CreateBody(collision, dgGetIdentityMatrix());
ReleaseCollision(collision);
// dgBodyMasterList::m_sentinel = m_sentionelBody;
dgCollidingPairCollector::m_sentinel = m_sentionelBody;
}
void dgDynamicBody::SetMassMatrix (dgFloat32 mass, const dgMatrix& inertia)
{
dgVector II;
m_principalAxis = inertia;
m_principalAxis.EigenVectors (II);
dgMatrix massMatrix (dgGetIdentityMatrix());
massMatrix[0][0] = II[0];
massMatrix[1][1] = II[1];
massMatrix[2][2] = II[2];
dgBody::SetMassMatrix (mass, massMatrix);
}
void dgCollisionBVH::DebugCollision (const dgMatrix& matrixPtr, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgCollisionBVHShowPolyContext context;
context.m_matrix = matrixPtr;
context.m_userData = userData;;
context.m_callback = callback;
dgFastAABBInfo box (dgGetIdentityMatrix(), dgVector (1.0e15f));
ForAllSectors (box, dgVector(dgFloat32 (0.0f)), dgFloat32 (1.0f), ShowDebugPolygon, &context);
}
dgBilateralConstraint::dgBilateralConstraint ()
:dgConstraint ()
{
dgAssert (dgInt32 (sizeof (dgBilateralConstraint) & 15) == 0);
dgAssert ((((dgUnsigned64) &m_localMatrix0) & 15) == 0);
m_maxDOF = 6;
m_contactActive = true;
m_destructor = NULL;
m_localMatrix0 = dgGetIdentityMatrix();
m_localMatrix1 = dgGetIdentityMatrix();
//SetStiffness (90.0f/99.0f);
SetStiffness (dgFloat32 (0.9f));
m_useExactSolver = false;
m_useExactSolverContactLimit = 0;
memset (m_jointForce, 0, sizeof (m_jointForce));
memset (m_rowIsMotor, 0, sizeof (m_rowIsMotor));
memset (m_motorAcceleration, 0, sizeof (m_motorAcceleration));
}
dgCollisionScene::dgCollisionScene(dgWorld* world)
:dgCollision (world->GetAllocator(), 0, dgGetIdentityMatrix(), m_sceneCollision)
,m_lock(0)
,m_list(world->GetAllocator())
,m_fitnessList(world->GetAllocator())
{
m_world = world;
m_rootNode = NULL;
m_rtti |= dgCollisionScene_RTTI;
SetCollisionBBox (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)),
dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));
}
void dgWorld::SerializeBodyArray(void* const userData, OnBodySerialize bodyCallback, dgBody** const array, dgInt32 count, dgSerialize serializeCallback, void* const fileHandle) const
{
dgSerializeMarker(serializeCallback, fileHandle);
// serialize all collisions
dgInt32 uniqueShapes = 0;
dgTree<dgInt32, const dgCollision*> shapeMap(GetAllocator());
for (dgInt32 i = 0; i < count; i++) {
dgBody* const body = array[i];
dgAssert(body->m_world == this);
dgCollisionInstance* const instance = body->GetCollision();
const dgCollision* const collision = instance->GetChildShape();
dgTree<dgInt32, const dgCollision*>::dgTreeNode* const shapeNode = shapeMap.Insert(uniqueShapes, collision);
if (shapeNode) {
uniqueShapes++;
}
}
serializeCallback(fileHandle, &uniqueShapes, sizeof (uniqueShapes));
dgTree<dgInt32, const dgCollision*>::Iterator iter(shapeMap);
for (iter.Begin(); iter; iter++) {
dgInt32 id = iter.GetNode()->GetInfo();
const dgCollision* const collision = iter.GetKey();
dgCollisionInstance instance(this, collision, 0, dgMatrix(dgGetIdentityMatrix()));
serializeCallback(fileHandle, &id, sizeof (id));
instance.Serialize(serializeCallback, fileHandle);
dgSerializeMarker(serializeCallback, fileHandle);
}
serializeCallback(fileHandle, &count, sizeof (count));
for (dgInt32 i = 0; i < count; i++) {
dgBody* const body = array[i];
dgInt32 bodyType = body->GetType();
serializeCallback(fileHandle, &bodyType, sizeof (bodyType));
// serialize the body
body->Serialize(shapeMap, serializeCallback, fileHandle);
// serialize body custom data
bodyCallback(*body, userData, serializeCallback, fileHandle);
dgSerializeMarker(serializeCallback, fileHandle);
}
}
void dgCollisionBVH::GetCollisionInfo(dgCollisionInfo* const info) const
{
dgCollision::GetCollisionInfo(info);
dgMeshVertexListIndexList data;
data.m_indexList = NULL;
data.m_userDataList = NULL;
data.m_maxIndexCount = 1000000000;
data.m_triangleCount = 0;
// dgVector p0 (-1.0e10f, -1.0e10f, -1.0e10f, 1.0f);
// dgVector p1 ( 1.0e10f, 1.0e10f, 1.0e10f, 1.0f);
dgVector zero (dgFloat32 (0.0f));
dgFastAABBInfo box (dgGetIdentityMatrix(), dgVector (dgFloat32 (1.0e15f)));
ForAllSectors (box, zero, dgFloat32 (1.0f), GetTriangleCount, &data);
info->m_bvhCollision.m_vertexCount = GetVertexCount();
info->m_bvhCollision.m_indexCount = data.m_triangleCount * 3;
}
dgCollisionInstance::dgCollisionInstance(const dgWorld* const world, const dgCollision* const childCollision, dgInt32 shapeID, const dgMatrix& matrix)
:m_globalMatrix(matrix)
,m_localMatrix (matrix)
,m_aligmentMatrix (dgGetIdentityMatrix())
,m_scale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_invScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_maxScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_userDataID(shapeID)
,m_refCount(1)
,m_userData(NULL)
,m_world(world)
,m_childShape (childCollision)
,m_subCollisionHandle(NULL)
,m_parent(NULL)
,m_collisionMode(1)
,m_scaleType(m_unit)
{
m_childShape->AddRef();
}
dgMatrix dgMatrix::Inverse4x4 () const
{
const dgFloat32 tol = 1.0e-4f;
dgMatrix tmp (*this);
dgMatrix inv (dgGetIdentityMatrix());
for (dgInt32 i = 0; i < 4; i ++) {
dgFloat32 diag = tmp[i][i];
if (dgAbsf (diag) < tol) {
dgInt32 j = 0;
for (j = i + 1; j < 4; j ++) {
dgFloat32 val = tmp[j][i];
if (dgAbsf (val) > tol) {
break;
}
}
dgAssert (j < 4);
for (dgInt32 k = 0; k < 4; k ++) {
tmp[i][k] += tmp[j][k];
inv[i][k] += inv[j][k];
}
diag = tmp[i][i];
}
dgFloat32 invDiag = dgFloat32 (1.0f) / diag;
for (dgInt32 j = 0; j < 4; j ++) {
tmp[i][j] *= invDiag;
inv[i][j] *= invDiag;
}
tmp[i][i] = dgFloat32 (1.0f);
for (dgInt32 j = 0; j < 4; j ++) {
if (j != i) {
dgFloat32 pivot = tmp[j][i];
for (dgInt32 k = 0; k < 4; k ++) {
tmp[j][k] -= pivot * tmp[i][k];
inv[j][k] -= pivot * inv[i][k];
}
tmp[j][i] = dgFloat32 (0.0f);
}
}
}
return inv;
}
dgDynamicBody::dgDynamicBody()
:dgBody()
,m_accel(dgFloat32 (0.0))
,m_alpha(dgFloat32 (0.0))
,m_prevExternalForce(dgFloat32 (0.0))
,m_prevExternalTorque(dgFloat32 (0.0))
,m_dampCoef(dgFloat32 (0.0))
,m_aparentMass(dgFloat32 (0.0))
,m_sleepingCounter(0)
,m_isInDestructionArrayLRU(0)
,m_skeleton(NULL)
,m_applyExtForces(NULL)
{
#ifdef DG_USEFULL_INERTIA_MATRIX
m_principalAxis = dgGetIdentityMatrix();
#endif
m_type = m_dynamicBody;
m_rtti |= m_dynamicBodyRTTI;
dgAssert ( dgInt32 (sizeof (dgDynamicBody) & 0x0f) == 0);
}
dgBody::dgBody()
:m_invWorldInertiaMatrix(dgGetZeroMatrix())
,m_matrix (dgGetIdentityMatrix())
,m_rotation(dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f))
,m_invMass(dgFloat32 (0.0))
,m_veloc(dgFloat32 (0.0))
,m_omega(dgFloat32 (0.0))
,m_minAABB(dgFloat32 (0.0))
,m_maxAABB(dgFloat32 (0.0))
,m_netForce(dgFloat32 (0.0))
,m_netTorque(dgFloat32 (0.0))
,m_localCentreOfMass(dgFloat32 (0.0))
,m_globalCentreOfMass(dgFloat32 (0.0))
,m_aparentMass(dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS))
,m_maxAngulaRotationPerSet2(DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP )
,m_criticalSectionLock()
,m_flags(0)
,m_userData(NULL)
,m_world(NULL)
,m_collision(NULL)
,m_broadPhaseNode(NULL)
,m_masterNode(NULL)
,m_broadPhaseaggregateNode(NULL)
,m_destructor(NULL)
,m_matrixUpdate(NULL)
,m_index(0)
,m_uniqueID(0)
,m_bodyGroupId(0)
,m_rtti(m_baseBodyRTTI)
,m_type(0)
,m_dynamicsLru(0)
,m_genericLRUMark(0)
{
m_resting = true;
m_autoSleep = true;
m_collidable = true;
m_collideWithLinkedBodies = true;
m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
}
dgFloat32 dgCollisionCone::CalculateMassProperties (dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const
{
dgFloat32 volume;
dgFloat32 inertaxx;
dgFloat32 inertayyzz;
//dgVector centerOfMass1;
//dgVector inertia1;
//dgVector crossInertia1;
//volume = dgCollisionConvex::CalculateMassProperties (inertia1, crossInertia1, centerOfMass1);
volume = dgFloat32 (3.1616f * 2.0f / 3.0f) * m_radius * m_radius * m_height;
centerOfMass = GetOffsetMatrix().m_posit - GetOffsetMatrix().m_front.Scale (dgFloat32 (0.5f) * m_height);
inertaxx = dgFloat32 (3.0f / 10.0f) * m_radius * m_radius * volume;
inertayyzz = (dgFloat32 (3.0f / 20.0f) * m_radius * m_radius + dgFloat32 (4.0f / 10.0f) * m_height * m_height) * volume;
dgMatrix inertiaTensor (dgGetIdentityMatrix());
inertiaTensor[0][0] = inertaxx;
inertiaTensor[1][1] = inertayyzz;
inertiaTensor[2][2] = inertayyzz;
inertiaTensor = GetOffsetMatrix().Inverse() * inertiaTensor * GetOffsetMatrix();
crossInertia.m_x = inertiaTensor[1][2] - volume * centerOfMass.m_y * centerOfMass.m_z;
crossInertia.m_y = inertiaTensor[0][2] - volume * centerOfMass.m_z * centerOfMass.m_x;
crossInertia.m_z = inertiaTensor[0][1] - volume * centerOfMass.m_x * centerOfMass.m_y;
dgVector central (centerOfMass.CompProduct(centerOfMass));
inertia.m_x = inertiaTensor[0][0] + volume * (central.m_y + central.m_z);
inertia.m_y = inertiaTensor[1][1] + volume * (central.m_z + central.m_x);
inertia.m_z = inertiaTensor[2][2] + volume * (central.m_x + central.m_y);
centerOfMass = centerOfMass.Scale (volume);
return volume;
}
dgBody::dgBody (dgWorld* const world, const dgTree<const dgCollision*, dgInt32>* const collisionCashe, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber)
:m_invWorldInertiaMatrix(dgGetZeroMatrix())
,m_matrix (dgGetIdentityMatrix())
,m_rotation(dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f))
,m_invMass(dgFloat32 (0.0))
,m_veloc(dgFloat32 (0.0))
,m_omega(dgFloat32 (0.0))
,m_minAABB(dgFloat32 (0.0))
,m_maxAABB(dgFloat32 (0.0))
,m_netForce(dgFloat32 (0.0))
,m_netTorque(dgFloat32 (0.0))
,m_localCentreOfMass(dgFloat32 (0.0))
,m_globalCentreOfMass(dgFloat32 (0.0))
,m_aparentMass(dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS))
,m_maxAngulaRotationPerSet2(DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP )
,m_criticalSectionLock()
,m_flags(0)
,m_userData(NULL)
,m_world(world)
,m_collision(NULL)
,m_broadPhaseNode(NULL)
,m_masterNode(NULL)
,m_broadPhaseaggregateNode(NULL)
,m_destructor(NULL)
,m_matrixUpdate(NULL)
,m_index(0)
,m_uniqueID(0)
,m_bodyGroupId(0)
,m_rtti(m_baseBodyRTTI)
,m_type(0)
,m_dynamicsLru(0)
,m_genericLRUMark(0)
{
m_autoSleep = true;
m_collidable = true;
m_collideWithLinkedBodies = true;
m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
serializeCallback (userData, &m_rotation, sizeof (m_rotation));
serializeCallback (userData, &m_matrix.m_posit, sizeof (m_matrix.m_posit));
serializeCallback (userData, &m_veloc, sizeof (m_veloc));
serializeCallback (userData, &m_omega, sizeof (m_omega));
serializeCallback (userData, &m_localCentreOfMass, sizeof (m_localCentreOfMass));
serializeCallback (userData, &m_aparentMass, sizeof (m_aparentMass));
serializeCallback (userData, &m_flags, sizeof (m_flags));
serializeCallback (userData, &m_maxAngulaRotationPerSet2, sizeof (m_maxAngulaRotationPerSet2));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
dgInt32 id;
serializeCallback (userData, &id, sizeof (id));
dgTree<const dgCollision*, dgInt32>::dgTreeNode* const node = collisionCashe->Find(id);
dgAssert (node);
const dgCollision* const collision = node->GetInfo();
collision->AddRef();
dgCollisionInstance* const instance = new (world->GetAllocator()) dgCollisionInstance (world, serializeCallback, userData, revisionNumber);
instance->m_childShape = collision;
m_collision = instance;
}
