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); }