CTransform CTransform::Scale(const CVector3& scalingVec)
{
CMatrix4x4 S(scalingVec.x, 0.0, 0.0, 0.0,
0.0, scalingVec.y, 0.0, 0.0,
0.0, 0.0, scalingVec.z, 0.0,
0.0, 0.0, 0.0, 1.0);
CMatrix4x4 invS(1.0 / scalingVec.x, 0.0, 0.0, 0.0,
0.0, 1.0 / scalingVec.y, 0.0, 0.0,
0.0, 0.0, 1.0 / scalingVec.z, 0.0,
0.0, 0.0, 0.0, 1.0);
return CTransform(S, invS);
}
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);
}
