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();
	}
}
예제 #2
0
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(); 

}
예제 #4
0
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); 
}
예제 #5
0
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);
}
예제 #6
0
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;
}
예제 #7
0
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);
	}
}
예제 #8
0
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
}
예제 #9
0
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]));
}
예제 #10
0
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(); 
}
예제 #11
0
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());
}
예제 #12
0
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)));
}
예제 #13
0
void dgUpVectorConstraint::SetPinDir (const dgVector& pin)
{
	m_localMatrix1 = dgMatrix (pin);
}
예제 #14
0
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*) &regionCom[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);
}