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();
}
}
dgMatrix dgMatrix::operator* (const dgMatrix &B) const
{
#if 0
return dgMatrix (B.m_front.Scale4(m_front.m_x) + B.m_up.Scale4(m_front.m_y) + B.m_right.Scale4(m_front.m_z) + B.m_posit.Scale4 (m_front.m_w),
B.m_front.Scale4(m_up.m_x) + B.m_up.Scale4(m_up.m_y) + B.m_right.Scale4(m_up.m_z) + B.m_posit.Scale4 (m_up.m_w),
B.m_front.Scale4(m_right.m_x) + B.m_up.Scale4(m_right.m_y) + B.m_right.Scale4(m_right.m_z) + B.m_posit.Scale4 (m_right.m_w),
B.m_front.Scale4(m_posit.m_x) + B.m_up.Scale4(m_posit.m_y) + B.m_right.Scale4(m_posit.m_z) + B.m_posit.Scale4 (m_posit.m_w));
#else
return dgMatrix (B.m_front.CompProduct4(m_front.BroadcastX()) + B.m_up.CompProduct4(m_front.BroadcastY()) + B.m_right.CompProduct4(m_front.BroadcastZ()) + B.m_posit.CompProduct4 (m_front.BroadcastW()),
B.m_front.CompProduct4(m_up.BroadcastX()) + B.m_up.CompProduct4(m_up.BroadcastY()) + B.m_right.CompProduct4(m_up.BroadcastZ()) + B.m_posit.CompProduct4 (m_up.BroadcastW()),
B.m_front.CompProduct4(m_right.BroadcastX()) + B.m_up.CompProduct4(m_right.BroadcastY()) + B.m_right.CompProduct4(m_right.BroadcastZ()) + B.m_posit.CompProduct4 (m_right.BroadcastW()),
B.m_front.CompProduct4(m_posit.BroadcastX()) + B.m_up.CompProduct4(m_posit.BroadcastY()) + B.m_right.CompProduct4(m_posit.BroadcastZ()) + B.m_posit.CompProduct4 (m_posit.BroadcastW()));
#endif
}
void dgBilateralConstraint::CalculateMatrixOffset (const dgVector& pivot, const dgVector& dir, dgMatrix& matrix0, dgMatrix& matrix1)
{
dgFloat32 length;
_ASSERTE (m_body0);
_ASSERTE (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
length = dir % dir;
length = dgSqrt (length);
_ASSERTE (length > dgFloat32 (0.0f));
// matrix0.m_front = body0_Matrix.UnrotateVector (dir.Scale (dgFloat32 (1.0f) / length));
// Create__Basis (matrix0.m_front, matrix0.m_up, matrix0.m_right);
matrix0 = dgMatrix (body0_Matrix.UnrotateVector (dir.Scale (dgFloat32 (1.0f) / length)));
matrix0.m_posit = body0_Matrix.UntransformVector (pivot);
matrix0.m_front.m_w = dgFloat32 (0.0f);
matrix0.m_up.m_w = dgFloat32 (0.0f);
matrix0.m_right.m_w = dgFloat32 (0.0f);
matrix0.m_posit.m_w = dgFloat32 (1.0f);
// dgMatrix body1_Matrix (dgGetIdentityMatrix());
// if (m_body1) {
// body1_Matrix = m_body1->GetMatrix();
// }
const dgMatrix& body1_Matrix = m_body1->GetMatrix();
matrix1 = matrix0 * body0_Matrix * body1_Matrix.Inverse();
}
dgMatrix dgMatrix::Multiply3X3 (const dgMatrix &B) const
{
return dgMatrix (B.m_front.CompProduct4(m_front.BroadcastX()) + B.m_up.CompProduct4(m_front.BroadcastY()) + B.m_right.CompProduct4(m_front.BroadcastZ()),
B.m_front.CompProduct4(m_up.BroadcastX()) + B.m_up.CompProduct4(m_up.BroadcastY()) + B.m_right.CompProduct4(m_up.BroadcastZ()),
B.m_front.CompProduct4(m_right.BroadcastX()) + B.m_up.CompProduct4(m_right.BroadcastY()) + B.m_right.CompProduct4(m_right.BroadcastZ()),
dgVector::m_wOne);
}
dgMatrix dgBody::CalculateInvInertiaMatrix () const
{
dgMatrix tmp (m_matrix.Transpose4X4());
tmp[0] = tmp[0].CompProduct4(m_invMass);
tmp[1] = tmp[1].CompProduct4(m_invMass);
tmp[2] = tmp[2].CompProduct4(m_invMass);
return dgMatrix (m_matrix.RotateVector(tmp[0]), m_matrix.RotateVector(tmp[1]), m_matrix.RotateVector(tmp[2]), dgVector::m_wOne);
}
dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep)
{
dgJacobian velocStep;
if (m_gyroTorqueOn) {
dgVector dtHalf(timestep * dgVector::m_half);
dgMatrix matrix(m_gyroRotation, dgVector::m_wOne);
dgVector localOmega(matrix.UnrotateVector(m_omega));
dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega * dtHalf);
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)),
dgVector::m_wOne);
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
//dgVector gradientStep(localTorque * timestep);
dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));
dgVector omega(matrix.RotateVector(localOmega + gradientStep));
dgAssert(omega.m_w == dgFloat32(0.0f));
// integrate rotation here
dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f);
dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
dgVector omegaAxis(omega.Scale(invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar();
dgQuaternion deltaRotation(omegaAxis, omegaAngle);
m_gyroRotation = m_gyroRotation * deltaRotation;
dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne);
localOmega = matrix.UnrotateVector(omega);
//dgVector angularMomentum(inertia * localOmega);
//body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum));
//body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque);
dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
velocStep.m_angular = matrix.RotateVector(gradientStep);
} else {
velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep;
//velocStep.m_angular = velocStep.m_angular * dgVector::m_half;
}
velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;
return velocStep;
}
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 dgBody::IntegrateVelocity (dgFloat32 timestep)
{
m_globalCentreOfMass += m_veloc.Scale3 (timestep);
while (((m_omega % m_omega) * timestep * timestep) > m_maxAngulaRotationPerSet2) {
m_omega = m_omega.Scale3 (dgFloat32 (0.8f));
}
// this is correct
dgFloat32 omegaMag2 = m_omega % m_omega;
if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
dgFloat32 invOmegaMag = dgRsqrt (omegaMag2);
dgVector omegaAxis (m_omega.Scale3 (invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep;
dgQuaternion rotation (omegaAxis, omegaAngle);
m_rotation = m_rotation * rotation;
m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);
#ifdef _DEBUG
for (dgInt32 i = 0; i < 4; i ++) {
for (dgInt32 j = 0; j < 4; j ++) {
dgAssert (dgCheckFloat(m_matrix[i][j]));
}
}
dgInt32 j0 = 1;
dgInt32 j1 = 2;
for (dgInt32 i = 0; i < 3; i ++) {
dgAssert (m_matrix[i][3] == 0.0f);
dgFloat32 val = m_matrix[i] % m_matrix[i];
dgAssert (dgAbsf (val - 1.0f) < 1.0e-5f);
dgVector tmp (m_matrix[j0] * m_matrix[j1]);
val = tmp % m_matrix[i];
dgAssert (dgAbsf (val - 1.0f) < 1.0e-5f);
j0 = j1;
j1 = i;
}
#endif
}
dgMatrix dgMatrix::operator* (const dgMatrix &B) const
{
const dgMatrix& A = *this;
return dgMatrix (dgVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0],
A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
dgVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0],
A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
dgVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0],
A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
dgVector (A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0],
A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1],
A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2],
A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3]));
}
void dgBilateralConstraint::CalculateMatrixOffset (const dgVector& pivot, const dgVector& dir, dgMatrix& matrix0, dgMatrix& matrix1)
{
dgFloat32 length;
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
length = dir % dir;
length = dgSqrt (length);
dgAssert (length > dgFloat32 (0.0f));
matrix0 = dgMatrix (body0_Matrix.UnrotateVector (dir.Scale3 (dgFloat32 (1.0f) / length)));
matrix0.m_posit = body0_Matrix.UntransformVector (pivot);
matrix0.m_front.m_w = dgFloat32 (0.0f);
matrix0.m_up.m_w = dgFloat32 (0.0f);
matrix0.m_right.m_w = dgFloat32 (0.0f);
matrix0.m_posit.m_w = dgFloat32 (1.0f);
const dgMatrix& body1_Matrix = m_body1->GetMatrix();
matrix1 = matrix0 * body0_Matrix * body1_Matrix.Inverse();
}
void dgBody::IntegrateVelocity (dgFloat32 timestep)
{
m_globalCentreOfMass += m_veloc.Scale3 (timestep);
while (((m_omega % m_omega) * timestep * timestep) > m_maxAngulaRotationPerSet2) {
m_omega = m_omega.Scale4 (dgFloat32 (0.8f));
}
// this is correct
dgFloat32 omegaMag2 = m_omega % m_omega;
if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
dgFloat32 invOmegaMag = dgRsqrt (omegaMag2);
dgVector omegaAxis (m_omega.Scale4 (invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep;
dgQuaternion rotation (omegaAxis, omegaAngle);
m_rotation = m_rotation * rotation;
m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);
dgAssert (m_matrix.TestOrthogonal());
}
dgMatrix dgMatrix::Symetric3by3Inverse () const
{
const dgMatrix& mat = *this;
hacd::HaF64 det = mat[0][0] * mat[1][1] * mat[2][2] +
mat[0][1] * mat[1][2] * mat[0][2] * hacd::HaF32 (2.0f) -
mat[0][2] * mat[1][1] * mat[0][2] -
mat[0][1] * mat[0][1] * mat[2][2] -
mat[0][0] * mat[1][2] * mat[1][2];
det = hacd::HaF32 (1.0f) / det;
hacd::HaF32 x11 = (hacd::HaF32)(det * (mat[1][1] * mat[2][2] - mat[1][2] * mat[1][2]));
hacd::HaF32 x22 = (hacd::HaF32)(det * (mat[0][0] * mat[2][2] - mat[0][2] * mat[0][2]));
hacd::HaF32 x33 = (hacd::HaF32)(det * (mat[0][0] * mat[1][1] - mat[0][1] * mat[0][1]));
hacd::HaF32 x12 = (hacd::HaF32)(det * (mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]));
hacd::HaF32 x13 = (hacd::HaF32)(det * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]));
hacd::HaF32 x23 = (hacd::HaF32)(det * (mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1]));
#ifdef _DEBUG
dgMatrix matInv (dgVector (x11, x12, x13, hacd::HaF32(0.0f)),
dgVector (x12, x22, x23, hacd::HaF32(0.0f)),
dgVector (x13, x23, x33, hacd::HaF32(0.0f)),
dgVector (hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(1.0f)));
dgMatrix test (matInv * mat);
HACD_ASSERT (dgAbsf (test[0][0] - hacd::HaF32(1.0f)) < hacd::HaF32(0.01f));
HACD_ASSERT (dgAbsf (test[1][1] - hacd::HaF32(1.0f)) < hacd::HaF32(0.01f));
HACD_ASSERT (dgAbsf (test[2][2] - hacd::HaF32(1.0f)) < hacd::HaF32(0.01f));
#endif
return dgMatrix (dgVector (x11, x12, x13, hacd::HaF32(0.0f)),
dgVector (x12, x22, x23, hacd::HaF32(0.0f)),
dgVector (x13, x23, x33, hacd::HaF32(0.0f)),
dgVector (hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(1.0f)));
}
void dgUpVectorConstraint::SetPinDir (const dgVector& pin)
{
m_localMatrix1 = dgMatrix (pin);
}
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;
}
void dgCollisionDeformableSolidMesh::ResolvePositionsConstraints (dgFloat32 timestep)
{
dgAssert (m_myBody);
dgInt32 strideInBytes = sizeof (dgVector) * m_clustersCount + sizeof (dgMatrix) * m_clustersCount + sizeof (dgMatrix) * m_particles.m_count;
m_world->m_solverMatrixMemory.ExpandCapacityIfNeessesary (1, strideInBytes);
dgVector* const regionCom = (dgVector*)&m_world->m_solverMatrixMemory[0];
dgMatrix* const sumQiPi = (dgMatrix*) ®ionCom[m_clustersCount];
dgMatrix* const covarianceMatrix = (dgMatrix*) &sumQiPi[m_clustersCount];
const dgFloat32* const masses = m_particles.m_unitMass;
dgVector zero (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
regionCom[i] = zero;
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (masses[i]);
const dgVector& r = m_posit[i];
const dgVector& r0 = m_shapePosit[i];
dgVector mr (r.Scale4(masses[i]));
covarianceMatrix[i] = dgMatrix (r0, mr);
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];
regionCom[index] += mr;
}
}
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
dgVector mcr (regionCom[i]);
regionCom[i] = mcr.Scale4 (dgFloat32 (1.0f) / m_clusterMass[i]);
const dgVector& cr0 = m_clusterCom0[i];
sumQiPi[i] = dgMatrix (cr0, mcr.CompProduct4(dgVector::m_negOne));
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
const dgMatrix& covariance = covarianceMatrix[i];
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 index = m_clusterPosit[start + j];
dgMatrix& QiPi = sumQiPi[index];
QiPi.m_front += covariance.m_front;
QiPi.m_up += covariance.m_up;
QiPi.m_right += covariance.m_right;
}
}
dgVector beta0 (dgFloat32 (0.93f));
//dgVector beta0 (dgFloat32 (0.0f));
dgVector beta1 (dgVector::m_one - beta0);
dgFloat32 stiffness = dgFloat32 (0.3f);
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
dgMatrix& QiPi = sumQiPi[i];
dgMatrix S (QiPi * QiPi.Transpose4X4());
dgVector eigenValues;
S.EigenVectors (eigenValues, m_clusterRotationInitialGuess[i]);
m_clusterRotationInitialGuess[i] = S;
#ifdef _DEBUG_____
dgMatrix P0 (QiPi * QiPi.Transpose4X4());
dgMatrix D (dgGetIdentityMatrix());
D[0][0] = eigenValues[0];
D[1][1] = eigenValues[1];
D[2][2] = eigenValues[2];
dgMatrix P1 (S.Transpose4X4() * D * S);
dgAssert (P0.TestSymetric3x3());
dgAssert (P1.TestSymetric3x3());
dgMatrix xx (P1 * P0.Symetric3by3Inverse());
dgAssert (dgAbsf (xx[0][0] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[1][1] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[2][2] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[0][1]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[0][2]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[1][0]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[1][2]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[2][0]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[2][1]) < dgFloat32 (1.0e-3f));
#endif
eigenValues = eigenValues.InvSqrt();
dgMatrix m;
m.m_front = S.m_front.CompProduct4(eigenValues.BroadcastX());
m.m_up = S.m_up.CompProduct4(eigenValues.BroadcastY());
m.m_right = S.m_right.CompProduct4(eigenValues.BroadcastZ());
m.m_posit = dgVector::m_wOne;
dgMatrix invS (S.Transpose4X4() * m);
dgMatrix R (invS * QiPi);
dgMatrix A (m_clusterAqqInv[i] * QiPi);
QiPi.m_front = A.m_front.CompProduct4(beta0) + R.m_front.CompProduct4(beta1);
QiPi.m_up = A.m_up.CompProduct4(beta0) + R.m_up.CompProduct4(beta1);
QiPi.m_right = A.m_right.CompProduct4(beta0) + R.m_right.CompProduct4(beta1);
}
dgVector invTimeScale (stiffness / timestep);
dgVector* const veloc = m_particles.m_veloc;
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
dgVector v (dgFloat32 (0.0f));
const dgVector& p = m_posit[i];
const dgVector& p0 = m_shapePosit[i];
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 index = m_clusterPosit[start + j];
const dgMatrix& matrix = sumQiPi[index];
dgVector gi (matrix.RotateVector(p0 - m_clusterCom0[index]) + regionCom[index]);
v += ((gi - p).CompProduct4(invTimeScale).Scale4 (m_clusterWeight[index]));
}
veloc[i] += v;
}
// resolve collisions here
//for now just a hack a collision plane until I get the engine up an running
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector p (m_basePosit + m_posit[i].m_y);
if (p.m_y < dgFloat32 (0.0f)) {
m_posit[i].m_y = -m_basePosit.m_y;
veloc[i].m_y = dgFloat32 (0.0f);
}
}
dgVector time (timestep);
dgVector minBase(dgFloat32 (1.0e10f));
dgVector minBox (dgFloat32 (1.0e10f));
dgVector maxBox (dgFloat32 (-1.0e10f));
dgMatrix matrix (m_myBody->GetCollision()->GetGlobalMatrix().Inverse());
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_posit[i] += veloc[i].CompProduct4 (time);
m_particles.m_posit[i] = matrix.TransformVector(m_posit[i] + m_basePosit);
minBase = minBase.GetMin (m_posit[i]);
minBox = minBox.GetMin(m_particles.m_posit[i]);
maxBox = maxBox.GetMax(m_particles.m_posit[i]);
}
minBase = minBase.Floor();
dgVector mask ((minBase < dgVector (dgFloat32 (0.0f))) | (minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))));
dgInt32 test = mask.GetSignMask();
if (test & 0x07) {
dgVector offset (((minBase < dgVector (dgFloat32 (0.0f))) & dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE/2))) +
((minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))) & dgVector (dgFloat32 (-DG_SOFTBODY_BASE_SIZE/2))));
m_basePosit -= offset;
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_posit[i] += offset;
}
}
// integrate each particle by the deformation velocity, also calculate the new com
// calculate the new body average velocity
// if (m_myBody->m_matrixUpdate) {
// myBody->m_matrixUpdate (*myBody, myBody->m_matrix, threadIndex);
// }
// the collision changed shape, need to update spatial structure
// UpdateCollision ();
// SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox);
SetCollisionBBox (minBox, maxBox);
}
