dgVector dgCollisionSphere::ConvexConicSupporVertex (const dgVector& dir) const
{
return dgVector (dgFloat32 (0.0f));
}
void dgCollisionScene::MassProperties ()
{
m_inertia = dgVector (dgFloat32 (0.0f));
m_centerOfMass = dgVector (dgFloat32 (0.0f));
m_crossInertia = dgVector (dgFloat32 (0.0f));
}
void dgSolver::CalculateJointsForce(dgInt32 threadID)
{
const dgInt32* const soaRowStart = m_soaRowStart;
const dgBodyInfo* const bodyArray = m_bodyArray;
dgSoaMatrixElement* const massMatrix = &m_massMatrix[0];
dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
dgFloat32 accNorm = dgFloat32(0.0f);
const dgInt32 step = m_threadCounts;
const dgInt32 jointCount = m_jointCount;
for (dgInt32 i = threadID; i < jointCount; i += step) {
const dgInt32 rowStart = soaRowStart[i];
dgJointInfo* const jointInfo = &m_jointArray[i * DG_SOA_WORD_GROUP_SIZE];
bool isSleeping = true;
dgFloat32 accel2 = dgFloat32(0.0f);
for (dgInt32 j = 0; (j < DG_SOA_WORD_GROUP_SIZE) && isSleeping; j++) {
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
const dgBody* const body0 = bodyArray[m0].m_body;
const dgBody* const body1 = bodyArray[m1].m_body;
isSleeping &= body0->m_resting;
isSleeping &= body1->m_resting;
}
if (!isSleeping) {
accel2 = CalculateJointForce(jointInfo, &massMatrix[rowStart], internalForces);
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgInt32 const rowCount = joint->m_pairCount;
dgInt32 const rowStartBase = joint->m_pairStart;
for (dgInt32 k = 0; k < rowCount; k++) {
const dgSoaMatrixElement* const row = &massMatrix[rowStart + k];
rightHandSide[k + rowStartBase].m_force = row->m_force[j];
rightHandSide[k + rowStartBase].m_maxImpact = dgMax(dgAbs(row->m_force[j]), rightHandSide[k + rowStartBase].m_maxImpact);
}
}
}
}
dgSoaVector6 forceM0;
dgSoaVector6 forceM1;
forceM0.m_linear.m_x = m_soaZero;
forceM0.m_linear.m_y = m_soaZero;
forceM0.m_linear.m_z = m_soaZero;
forceM0.m_angular.m_x = m_soaZero;
forceM0.m_angular.m_y = m_soaZero;
forceM0.m_angular.m_z = m_soaZero;
forceM1.m_linear.m_x = m_soaZero;
forceM1.m_linear.m_y = m_soaZero;
forceM1.m_linear.m_z = m_soaZero;
forceM1.m_angular.m_x = m_soaZero;
forceM1.m_angular.m_y = m_soaZero;
forceM1.m_angular.m_z = m_soaZero;
const dgInt32 rowsCount = jointInfo->m_pairCount;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[rowStart + j];
dgSoaFloat f(row->m_force);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, f);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, f);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, f);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, f);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, f);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, f);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, f);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, f);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, f);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, f);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, f);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, f);
}
dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
dgJacobian* const tempInternalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[m_cluster->m_bodyCount];
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgJacobian m_body0Force;
dgJacobian m_body1Force;
m_body0Force.m_linear = dgVector(forceM0.m_linear.m_x[j], forceM0.m_linear.m_y[j], forceM0.m_linear.m_z[j], dgFloat32(0.0f));
m_body0Force.m_angular = dgVector(forceM0.m_angular.m_x[j], forceM0.m_angular.m_y[j], forceM0.m_angular.m_z[j], dgFloat32(0.0f));
m_body1Force.m_linear = dgVector(forceM1.m_linear.m_x[j], forceM1.m_linear.m_y[j], forceM1.m_linear.m_z[j], dgFloat32(0.0f));
m_body1Force.m_angular = dgVector(forceM1.m_angular.m_x[j], forceM1.m_angular.m_y[j], forceM1.m_angular.m_z[j], dgFloat32(0.0f));
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
if (m0) {
dgScopeSpinPause lock(&bodyProxyArray[m0].m_lock);
tempInternalForces[m0].m_linear += m_body0Force.m_linear;
tempInternalForces[m0].m_angular += m_body0Force.m_angular;
}
if (m1) {
dgScopeSpinPause lock(&bodyProxyArray[m1].m_lock);
tempInternalForces[m1].m_linear += m_body1Force.m_linear;
tempInternalForces[m1].m_angular += m_body1Force.m_angular;
}
}
}
accNorm += accel2;
}
m_accelNorm[threadID] = accNorm;
}
void dgCollisionCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionCylinder_RTTI;
m_radio0 = dgMax (dgAbsf (radio0), dgFloat32 (2.0f) * D_CYLINDER_SKIN_THINCKNESS);
m_radio1 = dgMax (dgAbsf (radio1), dgFloat32 (2.0f) * D_CYLINDER_SKIN_THINCKNESS);
m_height = dgMax (dgAbsf (height * dgFloat32 (0.5f)), dgFloat32 (2.0f) * D_CYLINDER_SKIN_THINCKNESS);
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
dgFloat32 sinAngle = dgSin (angle);
dgFloat32 cosAngle = dgCos (angle);
m_vertex[i ] = dgVector (- m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f));
m_vertex[i + DG_TAPED_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f));
angle += dgPI2 / DG_TAPED_CYLINDER_SEGMENTS;
}
m_edgeCount = DG_TAPED_CYLINDER_SEGMENTS * 6;
m_vertexCount = DG_TAPED_CYLINDER_SEGMENTS * 2;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_TAPED_CYLINDER_SEGMENTS];
dgInt32 j = DG_TAPED_CYLINDER_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = i + DG_TAPED_CYLINDER_SEGMENTS;
wireframe[3] = j + DG_TAPED_CYLINDER_SEGMENTS;
j = i;
polyhedra.AddFace (4, wireframe);
}
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = DG_TAPED_CYLINDER_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_TAPED_CYLINDER_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = i + DG_TAPED_CYLINDER_SEGMENTS;
}
polyhedra.AddFace (DG_TAPED_CYLINDER_SEGMENTS, wireframe);
polyhedra.EndFace ();
dgAssert (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_profile[0] = dgVector( m_height, m_radio1, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_profile[1] = dgVector(-m_height, m_radio0, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_profile[2] = dgVector(-m_height, -m_radio0, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_profile[3] = dgVector(m_height, -m_radio1, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
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);
}
dgFloat32 dgCollisionChamferCylinder::RayCast(const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
dgVector dq((q1 - q0) & dgVector::m_triplexMask);
// avoid NaN as a result of a division by zero
if (dq.DotProduct(dq).GetScalar() <= 0.0f) {
return dgFloat32(1.2f);
}
//dgVector dir(dq * dq.InvMagSqrt());
dgVector dir(dq.Normalize());
if (dgAbs(dir.m_x) > 0.9999f) {
return dgCollisionConvex::RayCast(q0, q1, maxT, contactOut, body, NULL, NULL);
}
dgVector p0(q0 & dgVector::m_triplexMask);
dgVector p1(q1 & dgVector::m_triplexMask);
p0.m_x = dgFloat32 (0.0f);
p1.m_x = dgFloat32 (0.0f);
dgVector dp (p1 - p0);
dgFloat32 a = dp.DotProduct(dp).GetScalar();
dgFloat32 b = dgFloat32 (2.0f) * dp.DotProduct(p0).GetScalar();
dgFloat32 c = p0.DotProduct(p0).GetScalar() - m_radius * m_radius;
dgFloat32 disc = b * b - dgFloat32 (4.0f) * a * c;
if (disc >= dgFloat32 (0.0f)) {
disc = dgSqrt (disc);
dgVector origin0(p0 + dp.Scale ((-b + disc) / (dgFloat32 (2.0f) * a)));
dgVector origin1(p0 + dp.Scale ((-b - disc) / (dgFloat32 (2.0f) * a)));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
dgFloat32 t1 = dgRayCastSphere(q0, q1, origin1, m_height);
if(t1 < t0) {
t0 = t1;
origin0 = origin1;
}
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
} else {
dgVector origin0 (dgPointToRayDistance (dgVector::m_zero, p0, p1));
origin0 = origin0.Scale(m_radius / dgSqrt(origin0.DotProduct(origin0).GetScalar()));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
}
return dgFloat32(1.2f);
}
dgVector dgCollisionNull::CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const
{
dgAssert (0);
return dgVector (dgFloat32 (0.0f));
}
void dgCollisionCone::Init (dgFloat32 radius, dgFloat32 height)
{
// dgInt32 i;
// dgInt32 j;
// dgEdge *edge;
// dgFloat32 y;
// dgFloat32 z;
// dgFloat32 angle;
m_rtti |= dgCollisionCone_RTTI;
m_radius = dgAbsf (radius);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_sinAngle = m_radius / dgSqrt (height * height + m_radius * m_radius);;
m_amp = dgFloat32 (0.5f) * m_radius / m_height;
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
dgFloat32 z = dgSin (angle) * m_radius;
dgFloat32 y = dgCos (angle) * m_radius;
m_vertex[i] = dgVector (- m_height, y, z, dgFloat32 (1.0f));
angle += dgPI2 / DG_CONE_SEGMENTS;
}
m_vertex[DG_CONE_SEGMENTS] = dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));
m_edgeCount = DG_CONE_SEGMENTS * 4;
m_vertexCount = DG_CONE_SEGMENTS + 1;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CONE_SEGMENTS];
dgInt32 j = DG_CONE_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = DG_CONE_SEGMENTS;
j = i;
polyhedra.AddFace (3, wireframe);
}
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[i] = DG_CONE_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CONE_SEGMENTS, wireframe);
polyhedra.EndFace ();
_ASSERTE (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
dgCollisionNull::dgCollisionNull(dgMemoryAllocator* const allocator, dgUnsigned32 signature)
:dgCollisionConvex(allocator, signature, m_nullCollision)
{
m_rtti |= dgCollisionNull_RTTI;
m_inertia = dgVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
}
dgVector dgCollisionNull::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (0);
return dgVector (dgFloat32 (0.0f));
}
dgVector dgCollisionScene::SupportVertex (const dgVector& dir) const
{
_ASSERTE (0);
return dgVector (0, 0, 0, 0);
}
dgVector dgCollisionScene::CalculateVolumeIntegral (const dgMatrix& globalMatrix, GetBuoyancyPlane bouyancyPlane, void* const context) const
{
_ASSERTE (0);
return dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
void dgDynamicBody::IntegrateImplicit(dgFloat32 timestep)
{
// using simple backward Euler or implicit integration, this is.
// f'(t + dt) = (f(t + dt) - f(t)) / dt
// therefore:
// f(t + dt) = f(t) + f'(t + dt) * dt
// approximate f'(t + dt) by expanding the Taylor of f(w + dt)
// f(w + dt) = f(w) + f'(w) * dt + f''(w) * dt^2 / 2! + ....
// assume dt^2 is negligible, therefore we can truncate the expansion to
// f(w + dt) ~= f(w) + f'(w) * dt
// calculating dw as the f'(w) = d(wx, wy, wz) | dt
// dw/dt = a = (Tl - (wl x (wl * Il)) * Il^1
// expanding f(w)
// f'(wx) = Ix * ax = Tx - (Iz - Iy) * wy * wz
// f'(wy) = Iy * ay = Ty - (Ix - Iz) * wz * wx
// f'(wz) = Iz * az = Tz - (Iy - Ix) * wx * wy
//
// calculation the expansion
// Ix * ax = (Tx - (Iz - Iy) * wy * wz) - ((Iz - Iy) * wy * az + (Iz - Iy) * ay * wz) * dt
// Iy * ay = (Ty - (Ix - Iz) * wz * wx) - ((Ix - Iz) * wz * ax + (Ix - Iz) * az * wx) * dt
// Iz * az = (Tz - (Iy - Ix) * wx * wy) - ((Iy - Ix) * wx * ay + (Iy - Ix) * ax * wy) * dt
//
// factorizing a we get
// Ix * ax + (Iz - Iy) * dwy * az + (Iz - Iy) * dwz * ay = Tx - (Iz - Iy) * wy * wz
// Iy * ay + (Ix - Iz) * dwz * ax + (Ix - Iz) * dwx * az = Ty - (Ix - Iz) * wz * wx
// Iz * az + (Iy - Ix) * dwx * ay + (Iy - Ix) * dwy * ax = Tz - (Iy - Ix) * wx * wy
dgVector localOmega(m_matrix.UnrotateVector(m_omega));
dgVector localTorque(m_matrix.UnrotateVector(m_externalTorque - m_gyroTorque));
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
dgVector gradientStep(localTorque.Scale(timestep));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega.Scale(dgFloat32(0.5f) * timestep));
//dgVector dw(localOmega.Scale(dgFloat32(1.0f) * timestep));
// calculates Jacobian matrix (
// dWx / dwx, dWx / dwy, dWx / dwz
// dWy / dwx, dWy / dwy, dWy / dwz
// dWz / dwx, dWz / dwy, dWz / dwz
//
// dWx / dwx = Ix, dWx / dwy = (Iz - Iy) * wz * dt, dWx / dwz = (Iz - Iy) * wy * dt)
// dWy / dwx = (Ix - Iz) * wz * dt, dWy / dwy = Iy, dWy / dwz = (Ix - Iz) * wx * dt
// dWz / dwx = (Iy - Ix) * wy * dt, dWz / dwy = (Iy - Ix) * wx * dt, dWz / dwz = Iz
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(0.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(0.0f)),
dgVector::m_wOne);
gradientStep = jacobianMatrix.SolveByGaussianElimination(gradientStep);
localOmega += gradientStep;
m_accel = m_externalForce.Scale(m_invMass.m_w);
m_alpha = m_matrix.RotateVector(localTorque * m_invMass);
m_veloc += m_accel.Scale(timestep);
m_omega = m_matrix.RotateVector(localOmega);
}
void dgCollisionScene::CollidePair (dgBroadPhase::dgPair* const pair, dgCollisionParamProxy& proxy) const
{
const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];
dgAssert (proxy.m_contactJoint == pair->m_contact);
dgContact* const constraint = pair->m_contact;
dgBody* const otherBody = constraint->GetBody0();
dgBody* const sceneBody = constraint->GetBody1();
dgAssert (sceneBody->GetCollision()->GetChildShape() == this);
dgAssert (sceneBody->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
dgCollisionInstance* const sceneInstance = sceneBody->m_collision;
dgCollisionInstance* const otherInstance = otherBody->m_collision;
dgAssert (sceneInstance->GetChildShape() == this);
dgAssert (otherInstance->IsType (dgCollision::dgCollisionConvexShape_RTTI));
const dgContactMaterial* const material = constraint->GetMaterial();
const dgMatrix& myMatrix = sceneInstance->GetGlobalMatrix();
const dgMatrix& otherMatrix = otherInstance->GetGlobalMatrix();
dgMatrix matrix (otherMatrix * myMatrix.Inverse());
const dgVector& hullVeloc = otherBody->m_veloc;
dgFloat32 baseLinearSpeed = dgSqrt (hullVeloc % hullVeloc);
dgFloat32 closestDist = dgFloat32 (1.0e10f);
if (proxy.m_continueCollision && (baseLinearSpeed > dgFloat32 (1.0e-6f))) {
dgVector p0;
dgVector p1;
otherInstance->CalcAABB (matrix, p0, p1);
const dgVector& hullOmega = otherBody->m_omega;
dgFloat32 minRadius = otherInstance->GetBoxMinRadius();
dgFloat32 maxAngularSpeed = dgSqrt (hullOmega % hullOmega);
dgFloat32 angularSpeedBound = maxAngularSpeed * (otherInstance->GetBoxMaxRadius() - minRadius);
dgFloat32 upperBoundSpeed = baseLinearSpeed + dgSqrt (angularSpeedBound);
dgVector upperBoundVeloc (hullVeloc.Scale3 (proxy.m_timestep * upperBoundSpeed / baseLinearSpeed));
dgVector boxDistanceTravelInMeshSpace (myMatrix.UnrotateVector(upperBoundVeloc.CompProduct4(otherInstance->m_invScale)));
dgInt32 stack = 1;
stackPool[0] = m_root;
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), boxDistanceTravelInMeshSpace);
dgFloat32 maxParam = proxy.m_timestep;
while (stack) {
stack--;
const dgNodeBase* const me = stackPool[stack];
dgAssert (me);
if (me->BoxIntersect (ray, p0, p1)) {
if (me->m_type == m_leaf) {
dgAssert (!me->m_right);
bool processContacts = true;
if (material->m_compoundAABBOverlap) {
processContacts = material->m_compoundAABBOverlap (*material, sceneBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex);
}
if (processContacts) {
const dgCollisionInstance* const myInstance = me->GetShape();
dgCollisionInstance childInstance (*myInstance, myInstance->GetChildShape());
childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix);
proxy.m_instance1 = &childInstance;
dgInt32 count = pair->m_contactCount;
proxy.m_timestep = maxParam;
m_world->SceneChildContacts (pair, proxy);
dgFloat32 param = proxy.m_timestep;
dgAssert(param >= dgFloat32(0.0f));
if (param < maxParam) {
maxParam = param;
}
if (pair->m_contactCount > count) {
dgContactPoint* const buffer = proxy.m_contacts;
for (dgInt32 i = count; i < pair->m_contactCount; i ++) {
dgAssert (buffer[i].m_collision0 == proxy.m_instance0);
if (buffer[i].m_collision1->GetChildShape() == myInstance->GetChildShape()) {
buffer[i].m_collision1 = myInstance;
}
}
}
closestDist = dgMin(closestDist, constraint->m_closestDistance);
}
} else {
dgAssert (me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack] = me->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
}
}
}
proxy.m_timestep = maxParam;
} else {
dgVector size;
dgVector origin;
otherInstance->CalcObb(origin, size);
dgOOBBTestData data (matrix, origin, size);
dgInt32 stack = 1;
stackPool[0] = m_root;
while (stack) {
stack --;
const dgNodeBase* const me = stackPool[stack];
dgAssert (me);
if (me->BoxTest (data)) {
if (me->m_type == m_leaf) {
dgAssert (!me->m_right);
bool processContacts = true;
if (material->m_compoundAABBOverlap) {
processContacts = material->m_compoundAABBOverlap (*material, sceneBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex);
}
if (processContacts) {
const dgCollisionInstance* const myInstance = me->GetShape();
dgCollisionInstance childInstance (*myInstance, myInstance->GetChildShape());
childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix);
proxy.m_instance1 = &childInstance;
dgInt32 count = pair->m_contactCount;
m_world->SceneChildContacts (pair, proxy);
if (pair->m_contactCount > count) {
dgContactPoint* const buffer = proxy.m_contacts;
for (dgInt32 i = count; i < pair->m_contactCount; i ++) {
dgAssert (buffer[i].m_collision0 == proxy.m_instance0);
if (buffer[i].m_collision1->GetChildShape() == myInstance->GetChildShape()) {
buffer[i].m_collision1 = myInstance;
}
}
} else if (pair->m_contactCount == -1) {
break;
}
closestDist = dgMin(closestDist, constraint->m_closestDistance);
}
} else {
dgAssert (me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack] = me->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
}
}
}
}
constraint->m_closestDistance = closestDist;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert(this == proxy.m_instance1->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const body0 = proxy.m_body0;
const dgBody* const body1 = proxy.m_body1;
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
dgFloat32 den = m_normal.DotProduct4(relativeVelocity).GetScalar();
if (den > dgFloat32 (-1.0e-10f)) {
return 0;
}
den = dgFloat32 (1.0f) / den;
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32(1.0e10f);
dgMatrix polygonMatrix;
dgVector right (m_localPoly[1] - m_localPoly[0]);
polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = dgVector::m_wOne;
dgAssert (polygonMatrix.TestOrthogonal());
dgVector polyBoxP0(dgFloat32(1.0e15f));
dgVector polyBoxP1(dgFloat32(-1.0e15f));
for (dgInt32 i = 0; i < m_count; i++) {
dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
polyBoxP0 = polyBoxP0.GetMin(point);
polyBoxP1 = polyBoxP1.GetMax(point);
}
dgVector hullBoxP0;
dgVector hullBoxP1;
dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
dgVector minBox(polyBoxP0 - hullBoxP1);
dgVector maxBox(polyBoxP1 - hullBoxP0);
dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
dgInt32 count = 0;
if (distance < dgFloat32(1.0f)) {
bool inside = false;
dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
boxSize = sphereMag2.Sqrt();
dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;
dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half));
dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half)));
sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
boxSize = sphereMag2.Sqrt();
dgAssert (boxOrigin.m_w == 0.0f);
boxOrigin = boxOrigin | dgVector::m_wOne;
if (!proxy.m_intersectionTestOnly) {
inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e & dgVector::m_triplexMask);
dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
dgPlane plane(n, -(m_localPoly[i0] % n));
dgVector p0 (boxOrigin + n.Scale4 (param));
dgVector p1 (boxOrigin - n.Scale4 (param));
dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();
if ((size0 < 0.0f) && (size1 < 0.0f)) {
return 0;
}
if ((size0 * size1) < 0.0f) {
inside = false;
break;
}
i0 = i;
}
}
dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(boxOrigin, convexSphapeUmbra);
m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
dgVector p0 (matrixInstance0.TransformVector(pointInHull));
dgFloat32 timetoImpact = dgFloat32(0.0f);
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
timetoImpact = penetration / (relativeVelocity % m_normal);
dgAssert(timetoImpact >= dgFloat32(0.0f));
}
if (timetoImpact <= proxy.m_timestep) {
dgVector contactPoints[64];
contactJoint->m_closestDistance = penetration;
proxy.m_timestep = timetoImpact;
proxy.m_normal = m_normal;
proxy.m_closestPointBody0 = p0;
proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);
if (!proxy.m_intersectionTestOnly) {
pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);
dgVector step(relativeVelocity.Scale4(timetoImpact));
penetration = dgMax(penetration, dgFloat32(0.0f));
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
return count;
}
void dgBody::SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia)
{
dgFloat32 Ixx = inertia[0][0];
dgFloat32 Iyy = inertia[1][1];
dgFloat32 Izz = inertia[2][2];
mass = dgAbsf (mass);
if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI) || m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
mass = DG_INFINITE_MASS * 2.0f;
}
if (mass < DG_MINIMUM_MASS) {
mass = DG_INFINITE_MASS * 2.0f;
}
//dgAssert (m_masterNode);
m_world->GetBroadPhase()->CheckStaticDynamic(this, mass);
if (mass >= DG_INFINITE_MASS) {
m_mass.m_x = DG_INFINITE_MASS;
m_mass.m_y = DG_INFINITE_MASS;
m_mass.m_z = DG_INFINITE_MASS;
m_mass.m_w = DG_INFINITE_MASS;
m_invMass.m_x = dgFloat32 (0.0f);
m_invMass.m_y = dgFloat32 (0.0f);
m_invMass.m_z = dgFloat32 (0.0f);
m_invMass.m_w = dgFloat32 (0.0f);
if (m_masterNode) {
dgBodyMasterList& masterList (*m_world);
if (masterList.GetFirst() != m_masterNode) {
masterList.InsertAfter (masterList.GetFirst(), m_masterNode);
}
}
SetAparentMassMatrix (m_mass);
} else {
Ixx = dgAbsf (Ixx);
Iyy = dgAbsf (Iyy);
Izz = dgAbsf (Izz);
dgFloat32 Ixx1 = dgClamp (Ixx, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgFloat32 Iyy1 = dgClamp (Iyy, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgFloat32 Izz1 = dgClamp (Izz, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgAssert (Ixx > dgFloat32 (0.0f));
dgAssert (Iyy > dgFloat32 (0.0f));
dgAssert (Izz > dgFloat32 (0.0f));
m_mass.m_x = Ixx1;
m_mass.m_y = Iyy1;
m_mass.m_z = Izz1;
m_mass.m_w = mass;
m_invMass.m_x = dgFloat32 (1.0f) / Ixx1;
m_invMass.m_y = dgFloat32 (1.0f) / Iyy1;
m_invMass.m_z = dgFloat32 (1.0f) / Izz1;
m_invMass.m_w = dgFloat32 (1.0f) / mass;
if (m_masterNode) {
dgBodyMasterList& masterList (*m_world);
masterList.RotateToEnd (m_masterNode);
}
SetAparentMassMatrix (dgVector (Ixx, Iyy, Izz, mass));
}
#ifdef _DEBUG
dgBodyMasterList& me = *m_world;
for (dgBodyMasterList::dgListNode* refNode = me.GetFirst(); refNode; refNode = refNode->GetNext()) {
dgBody* const body0 = refNode->GetInfo().GetBody();
dgVector invMass (body0->GetInvMass());
if (invMass.m_w != 0.0f) {
for (; refNode; refNode = refNode->GetNext()) {
dgBody* const body1 = refNode->GetInfo().GetBody();
dgVector invMass (body1->GetInvMass());
dgAssert (invMass.m_w != 0.0f);
}
break;
}
}
#endif
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgInt32 count = 0;
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
const dgCollisionInstance* const polygonInstance = proxy.m_instance1;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix;
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_instance0;
dgVector normalInHull(hullMatrix.UnrotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(hullMatrix.TransformVector(pointInHull));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(-1.0e-5f)) {
return 0;
}
dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL)));
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
return 0;
}
dgVector boxSize;
dgVector boxOrigin;
hull->CalcObb(boxOrigin, boxSize);
boxOrigin += dgVector::m_wOne;
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector edgeBoundaryNormal(m_normal * e);
dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar());
plane = hullMatrix.UntransformPlane(plane);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
//inside = false;
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
penetration = dgMax (dgFloat32 (0.0f), penetration);
dgAssert(penetration >= dgFloat32(0.0f));
dgVector contactPoints[64];
dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step);
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (contactsOut[0].m_normal);
if (normal.DotProduct4(m_normal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector adjacentNormal (CalculateGlobalNormal (parentMesh, dgVector(&m_vertex[index * m_stride])));
if ((m_normal.DotProduct4(adjacentNormal).GetScalar() > dgFloat32(0.9995f))) {
normal = adjacentNormal;
} else {
dgVector dir0(adjacentNormal * m_normal);
dgVector dir1(adjacentNormal * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = adjacentNormal;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
return count;
}
void dgCollisionBox::Init (dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z)
{
m_rtti |= dgCollisionBox_RTTI;
m_size[0].m_x = dgAbsf (size_x) * dgFloat32 (0.5f);
m_size[0].m_y = dgAbsf (size_y) * dgFloat32 (0.5f);
m_size[0].m_z = dgAbsf (size_z) * dgFloat32 (0.5f);
m_size[0].m_w = dgFloat32 (0.0f);
m_size[1].m_x = - m_size[0].m_x;
m_size[1].m_y = - m_size[0].m_y;
m_size[1].m_z = - m_size[0].m_z;
m_size[1].m_w = dgFloat32 (0.0f);
m_edgeCount = 24;
m_vertexCount = 8;
m_vertex[0] = dgVector ( m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[1] = dgVector (-m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[2] = dgVector ( m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[3] = dgVector (-m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[4] = dgVector ( m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[5] = dgVector (-m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[6] = dgVector ( m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[7] = dgVector (-m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
dgCollisionConvex::m_vertex = m_vertex;
dgCollisionConvex::m_simplex = m_edgeArray;
if (!m_initSimplex) {
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgInt32 i = 0; i < 6; i ++) {
polyhedra.AddFace (4, &m_faces[i][0]);
}
polyhedra.EndFace();
int index = 0;
dgInt32 mark = polyhedra.IncLRU();;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = index;
index ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
dgAssert (index == 24);
polyhedra.IncLRU();
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = ptr->m_incidentVertex;
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
m_vertexToEdgeMap[edge->m_incidentVertex] = &m_simplex[edge->m_userData];
}
dgInt32 count = 0;
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
edge->m_mark = mark;
edge->m_twin->m_mark = mark;
m_edgeEdgeMap[count] = &m_simplex[edge->m_userData];
count ++;
dgAssert (count <= 12);
}
}
m_initSimplex = 1;
}
SetVolumeAndCG ();
}
static void Statistics (
dgSphere &sphere,
dgVector &eigenValues,
dgVector &scaleVector,
const dgFloat32 vertex[],
const dgInt32 faceIndex[],
dgInt32 indexCount,
dgInt32 stride)
{
/*
dgInt32 i;
dgInt32 j;
dgFloat32 *ptr;
dgFloat32 x;
dgFloat32 z;
dgFloat32 y;
dgFloat64 k;
dgFloat64 Ixx;
dgFloat64 Iyy;
dgFloat64 Izz;
dgFloat64 Ixy;
dgFloat64 Ixz;
dgFloat64 Iyz;
dgBigVector massCenter (0, 0, 0, 0);
dgBigVector var (0, 0, 0, 0);
dgBigVector cov (0, 0, 0, 0);
ptr = (dgFloat32*)vertex;
for (i = 0; i < indexCount; i ++) {
j = index[i] * stride;
x = ptr[j + 0] * scaleVector.m_x;
y = ptr[j + 1] * scaleVector.m_y;
z = ptr[j + 2] * scaleVector.m_z;
massCenter += dgBigVector (x, y, z, 0);
var += dgBigVector (x * x, y * y, z * z, 0);
cov += dgBigVector (x * y, x * z, y * z, 0);
}
k = 1.0 / indexCount;
var = var.Scale (k);
cov = cov.Scale (k);
massCenter = massCenter.Scale (k);
Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
Izz = var.m_z - massCenter.m_z * massCenter.m_z;
Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector (dgFloat32(Ixx), dgFloat32(Ixy), dgFloat32(Ixz), dgFloat32 (0.0f));
sphere.m_up = dgVector (dgFloat32(Ixy), dgFloat32(Iyy), dgFloat32(Iyz), dgFloat32 (0.0f));
sphere.m_right = dgVector (dgFloat32(Ixz), dgFloat32(Iyz), dgFloat32(Izz), dgFloat32 (0.0f));
sphere.EigenVectors (eigenValues);
*/
const dgFloat32 *ptr;
dgFloat64 K;
dgFloat64 Ixx;
dgFloat64 Iyy;
dgFloat64 Izz;
dgFloat64 Ixy;
dgFloat64 Ixz;
dgFloat64 Iyz;
dgFloat64 area;
dgFloat64 totalArea;
// const dgFace *Face;
dgVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector centre (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
totalArea = dgFloat32 (0.0f);
ptr = vertex;
for (dgInt32 i = 0; i < indexCount; i += 3) {
// Face = &face[i];
dgInt32 index;
index = faceIndex[i] * stride;
dgVector p0 (&ptr[index]);
p0 = p0.CompProduct (scaleVector);
index = faceIndex[i + 1] * stride;;
dgVector p1 (&ptr[index]);
p1 = p1.CompProduct (scaleVector);
index = faceIndex[i + 2] * stride;;
dgVector p2 (&ptr[index]);
p2 = p2.CompProduct (scaleVector);
dgVector normal ((p1 - p0) * (p2 - p0));
area = dgFloat32 (0.5f) * sqrt (normal % normal);
centre = p0 + p1 + p2;
centre = centre.Scale (dgFloat32 (1.0f / 3.0f));
// Inertia of each point in the triangle
Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;
Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;
Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;
Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;
Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;
Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;
if (area > dgEPSILON * 10.0) {
K = area / 12.0;
//Coriollis teorem for Inercia of a triangle in an arbitrary orientation
Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
centre = centre.Scale ((dgFloat32)area);
}
totalArea += area;
massCenter += centre;
var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz, dgFloat32 (0.0f));
cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz, dgFloat32 (0.0f));
}
if (totalArea > dgEPSILON * 10.0) {
K = 1.0 / totalArea;
var = var.Scale ((dgFloat32)K);
cov = cov.Scale ((dgFloat32)K);
massCenter = massCenter.Scale ((dgFloat32)K);
}
Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
Izz = var.m_z - massCenter.m_z * massCenter.m_z;
Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz, dgFloat32 (0.0f));
sphere.m_up = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz, dgFloat32 (0.0f));
sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz, dgFloat32 (0.0f));
sphere.EigenVectors(eigenValues);
}
static void Statistics (dgObb &sphere, dgVector &eigenValues, const dgVector &scale, const dgFloat32 vertex[], const dgInt32 faceIndex[], dgInt32 indexCount, dgInt32 stride)
{
dgVector var (dgFloat32 (0.0f));
dgVector cov (dgFloat32 (0.0f));
dgVector centre (dgFloat32 (0.0f));
dgVector massCenter (dgFloat32 (0.0f));
dgVector scaleVector (scale & dgVector::m_triplexMask);
dgFloat64 totalArea = dgFloat32 (0.0f);
const dgFloat32* const ptr = vertex;
for (dgInt32 i = 0; i < indexCount; i += 3) {
dgInt32 index = faceIndex[i] * stride;
dgVector p0 (&ptr[index]);
p0 = p0 * scaleVector;
index = faceIndex[i + 1] * stride;;
dgVector p1 (&ptr[index]);
p1 = p1 * scaleVector;
index = faceIndex[i + 2] * stride;;
dgVector p2 (&ptr[index]);
p2 = p2 * scaleVector;
dgVector normal ((p1 - p0).CrossProduct(p2 - p0));
dgFloat64 area = dgFloat32 (0.5f) * sqrt (normal.DotProduct3(normal));
centre = p0 + p1 + p2;
centre = centre.Scale (dgFloat32 (1.0f / 3.0f));
// Inertia of each point in the triangle
dgFloat64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;
dgFloat64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;
dgFloat64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;
dgFloat64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;
dgFloat64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;
dgFloat64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;
if (area > dgEPSILON * 10.0) {
dgFloat64 K = area / dgFloat64 (12.0);
//Coriolis theorem for Inertia of a triangle in an arbitrary orientation
Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
centre = centre.Scale ((dgFloat32)area);
}
totalArea += area;
massCenter += centre;
var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz, dgFloat32 (0.0f));
cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz, dgFloat32 (0.0f));
}
if (totalArea > dgEPSILON * 10.0) {
dgFloat64 K = dgFloat64 (1.0) / totalArea;
var = var.Scale ((dgFloat32)K);
cov = cov.Scale ((dgFloat32)K);
massCenter = massCenter.Scale ((dgFloat32)K);
}
dgFloat64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
dgFloat64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
dgFloat64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
dgFloat64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
dgFloat64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
dgFloat64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz, dgFloat32 (0.0f));
sphere.m_up = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz, dgFloat32 (0.0f));
sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz, dgFloat32 (0.0f));
sphere.EigenVectors(eigenValues);
}
* misrepresented as being the original software. * * 3. This notice may not be removed or altered from any source distribution. */ #include "dgStdafx.h" #include "dgMatrix.h" #include "dgQuaternion.h" dgVector dgVector::m_xMask (dgInt32 (0xffffffff), dgInt32 (0), dgInt32 (0), dgInt32 (0)); dgVector dgVector::m_yMask (dgInt32 (0), dgInt32 (0xffffffff), dgInt32 (0), dgInt32 (0)); dgVector dgVector::m_zMask (dgInt32 (0), dgInt32 (0), dgInt32 (0xffffffff), dgInt32 (0)); dgVector dgVector::m_wMask (dgInt32 (0), dgInt32 (0), dgInt32 (0), dgInt32 (0xffffffff)); dgVector dgVector::m_triplexMask (dgInt32 (0xffffffff), dgInt32 (0xffffffff), dgInt32 (0xffffffff), dgInt32 (0)); dgVector dgVector::m_signMask (dgVector(dgInt32 (0xffffffff), dgInt32 (0xffffffff), dgInt32 (0xffffffff), dgInt32 (0xffffffff)).ShiftRightLogical(1)); dgVector dgVector::m_zero (dgFloat32 (0.0f)); dgVector dgVector::m_one (dgFloat32 (1.0f)); dgVector dgVector::m_two (dgFloat32 (2.0f)); dgVector dgVector::m_wOne (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)); dgVector dgVector::m_half (dgFloat32 (0.5f)); dgVector dgVector::m_three (dgFloat32 (3.0f)); dgVector dgVector::m_negOne (dgFloat32 (-1.0f)); dgMatrix dgMatrix::m_zeroMatrix (dgVector (dgFloat32(0.0f)), dgVector (dgFloat32(0.0f)), dgVector (dgFloat32(0.0f)), dgVector (dgFloat32(0.0f)));
void dgCollisionChamferCylinder::Init (dgFloat32 radius, dgFloat32 height)
{
m_rtti |= dgCollisionChamferCylinder_RTTI;
m_radius = dgMax (dgAbs (radius), D_MIN_CONVEX_SHAPE_SIZE);
m_height = dgMax (dgAbs (height * dgFloat32 (0.5f)), D_MIN_CONVEX_SHAPE_SIZE);
dgFloat32 sliceAngle = dgFloat32 (0.0f);
dgFloat32 sliceStep = dgPI / DG_CHAMFERCYLINDER_SLICES;
dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES;
dgMatrix rot (dgPitchMatrix (breakStep));
dgInt32 index = 0;
for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j ++) {
dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f));
sliceAngle += sliceStep;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
m_vertex[index] = p0;
index ++;
p0 = rot.UnrotateVector (p0);
}
}
m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2)* DG_CHAMFERCYLINDER_BRAKES;
m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1);
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10];
for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i ++) {
dgMatrix matrix (dgPitchMatrix (dgFloat32 (dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT));
m_shapesDirs[i] = matrix.RotateVector (dgVector (dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));
}
dgInt32 index0 = 0;
for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j ++) {
dgInt32 index1 = index0 + DG_CHAMFERCYLINDER_BRAKES - 1;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[0] = index0;
wireframe[1] = index1;
wireframe[2] = index1 + DG_CHAMFERCYLINDER_BRAKES;
wireframe[3] = index0 + DG_CHAMFERCYLINDER_BRAKES;
index1 = index0;
index0 ++;
polyhedra.AddFace (4, wireframe);
}
}
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[i] = i;
}
polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe);
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1) - i - 1;
}
polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe);
polyhedra.EndFace ();
dgAssert (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
dgVector dgCollisionPoint::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
return dgVector (dgFloat32 (0.0f));
}
dgCollisionDeformableMesh::dgCollisionDeformableMesh(dgWorld* const world, dgMeshEffect* const mesh, dgCollisionID collsionID)
:dgCollisionConvex (mesh->GetAllocator(), 0, collsionID)
,m_particles (mesh->GetVertexCount ())
,m_visualSegments(mesh->GetAllocator())
,m_skinThickness(DG_DEFORMABLE_DEFAULT_SKIN_THICKNESS)
,m_nodesCount(0)
,m_trianglesCount(0)
,m_visualVertexCount(0)
,m_world (world)
,m_myBody(NULL)
,m_indexList(NULL)
,m_faceNormals(NULL)
,m_rootNode(NULL)
,m_nodesMemory(NULL)
,m_visualVertexData(NULL)
,m_onDebugDisplay(NULL)
,m_isdoubleSided(false)
{
m_rtti |= dgCollisionDeformableMesh_RTTI;
dgDeformableBodiesUpdate& softBodyList = *m_world;
softBodyList.AddShape (this);
dgMeshEffect meshCopy (*mesh);
meshCopy.Triangulate();
m_trianglesCount = meshCopy.GetTotalFaceCount ();
m_nodesMemory = (dgDeformableNode*) dgMallocStack((m_trianglesCount * 2 - 1) * sizeof (dgDeformableNode));
m_indexList = (dgInt32*) dgMallocStack (3 * m_trianglesCount * sizeof (dgInt32));
m_faceNormals = (dgVector*) dgMallocStack (m_trianglesCount * sizeof (dgVector));
dgInt32 stride = meshCopy.GetVertexStrideInByte() / sizeof (dgFloat64);
dgFloat64* const vertex = meshCopy.GetVertexPool();
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_particles.m_unitMass[i] = dgFloat32 (1.0f);
m_particles.m_veloc[i] = dgVector (dgFloat32 (0.0f));
m_particles.m_posit[i] = dgVector (&vertex[i * stride]) & dgVector::m_triplexMask;
}
dgInt32 indexCount = meshCopy.GetTotalIndexCount ();
dgStack<dgInt32> faceArray (m_trianglesCount);
dgStack<dgInt32> materials (m_trianglesCount);
dgStack<void*>indexArray (indexCount);
meshCopy.GetFaces (&faceArray[0], &materials[0], &indexArray[0]);
for (dgInt32 i = 0; i < m_trianglesCount; i ++) {
dgInt32 count = faceArray[i];
dgAssert (faceArray[i]);
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 k = meshCopy.GetVertexIndex(indexArray[i * 3 + j]);
m_indexList[i * 3 + j] = k;
}
//dgTrace (("%d %d %d\n", m_indexList[i * 3 + 0], m_indexList[i * 3 + 1], m_indexList[i * 3 + 2]));
dgDeformableNode& node = m_nodesMemory[i];
node.m_left = NULL;
node.m_right = NULL;
node.m_parent = NULL;
node.m_indexStart = i * 3;
node.CalculateBox(m_particles.m_posit, &m_indexList[i * 3]);
}
m_nodesCount = m_trianglesCount;
m_rootNode = BuildTopDown (m_nodesCount, m_nodesMemory, NULL);
ImproveTotalFitness();
SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox);
// create visual vertex data
m_visualVertexCount = meshCopy.GetPropertiesCount();
m_visualVertexData = (dgVisualVertexData*) dgMallocStack(m_visualVertexCount * sizeof (dgVisualVertexData));
for (dgInt32 i = 0; i < m_visualVertexCount; i ++) {
dgMeshEffect::dgVertexAtribute& attribute = meshCopy.GetAttribute (i);
m_visualVertexData[i].m_uv0[0] = dgFloat32 (attribute.m_u0);
m_visualVertexData[i].m_uv0[1] = dgFloat32 (attribute.m_v0);
}
for (void* point = meshCopy.GetFirstPoint(); point; point = meshCopy.GetNextPoint(point)) {
dgInt32 pointIndex = meshCopy.GetPointIndex (point);
dgInt32 vertexIndex = meshCopy.GetVertexIndexFromPoint (point);
m_visualVertexData[pointIndex].m_vertexIndex = vertexIndex;
}
for (dgInt32 i = 0; i < m_trianglesCount; i ++) {
dgInt32 mat = materials[i];
if (mat != -1) {
dgInt32 count = 0;
for (dgInt32 j = i; j < m_trianglesCount; j ++) {
dgInt32 mat1 = materials[j];
if (mat == mat1) {
materials[j] = -1;
count ++;
}
}
dgMeshSegment& segment = m_visualSegments.Append()->GetInfo();
segment.m_material = mat;
segment.m_indexCount = count * 3;
segment.m_indexList = (dgInt32*) dgMallocStack( 2 * segment.m_indexCount * sizeof (dgInt32));
dgInt32 index0 = 0;
dgInt32 index1 = m_trianglesCount * 3;
for (dgInt32 j = i; j < m_trianglesCount; j ++) {
if (materials[j] == -1) {
dgInt32 m0 = meshCopy.GetPointIndex(indexArray[j * 3 + 0]);
dgInt32 m1 = meshCopy.GetPointIndex(indexArray[j * 3 + 1]);
dgInt32 m2 = meshCopy.GetPointIndex(indexArray[j * 3 + 2]);
segment.m_indexList[index0 + 0] = dgInt16 (m0);
segment.m_indexList[index0 + 1] = dgInt16 (m1);
segment.m_indexList[index0 + 2] = dgInt16 (m2);
index0 += 3;
segment.m_indexList[index1 + 0] = dgInt16 (m0);
segment.m_indexList[index1 + 1] = dgInt16 (m2);
segment.m_indexList[index1 + 2] = dgInt16 (m1);
index1 += 3;
}
}
}
}
// SetVolumeAndCG ();
}
dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection(
const dgVector& normal, const dgVector& origin,
dgVector* const contactsOut) const
{
dgInt32 count;
if (dgAbsf(normal.m_x) < dgFloat32(0.999f))
{
dgFloat32 magInv =
dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
_ASSERTE(
dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng,
dgFloat32(0.0f), dgFloat32(0.0f));
dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
dgPlane plane(normal1, -(normal1 % origin1));
count = 0;
dgVector maxDir(
(normal1.m_x > dgFloat32(0.0f)) ?
m_silhuette[0].m_x : -m_silhuette[0].m_x,
(normal1.m_y > dgFloat32(0.0f)) ?
m_silhuette[0].m_y : -m_silhuette[0].m_y, dgFloat32(0.0f),
dgFloat32(0.0f));
dgFloat32 test0 = plane.Evalue(maxDir);
dgFloat32 test1 = plane.Evalue(maxDir.Scale(dgFloat32(-1.0f)));
if ((test0 * test1) > dgFloat32(0.0f))
{
test0 = plane.m_w + plane.m_y * m_radius;
if (dgAbsf(test0) < m_height)
{
contactsOut[count] = normal1.Scale(-test0);
contactsOut[count].m_y += m_radius;
count++;
}
else
{
test0 = plane.m_w - plane.m_y * m_radius;
if (dgAbsf(test0) < m_height)
{
contactsOut[count] = normal1.Scale(-test0);
contactsOut[count].m_y -= m_radius;
count++;
}
}
}
else
{
dgVector dp(m_silhuette[1] - m_silhuette[0]);
dgFloat32 den = normal1 % dp;
_ASSERTE(dgAbsf (den) > dgFloat32 (0.0f));
test0 = -plane.Evalue(m_silhuette[0]) / den;
if ((test0 <= dgFloat32(1.0)) && (test0 >= dgFloat32(0.0f)))
{
contactsOut[count] = m_silhuette[0] + dp.Scale(test0);
count++;
}
if (count < 2)
{
test0 = plane.m_w - plane.m_y * m_radius;
if (dgAbsf(test0) < m_height)
{
dgFloat32 r = -m_radius;
dgFloat32 d = plane.m_w + r * plane.m_y;
dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
dgFloat32 b = dgFloat32(2.0f) * plane.m_y * d;
dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x;
dgFloat32 desc = b * b - dgFloat32(4.0f) * a * c;
if (desc > dgFloat32(0.0f))
{
_ASSERTE(dgAbsf (a) > dgFloat32 (0.0f));
desc = dgSqrt (desc);
a = -dgFloat32(0.5f) * b / a;
dgFloat32 y0 = a + desc;
dgFloat32 y1 = a - desc;
if (y0 > dgFloat32(0.0f))
{
y0 = y1;
}_ASSERTE(y0 < dgFloat32 (0.0f));
_ASSERTE(dgAbsf (plane.m_x) > dgFloat32 (0.0f));
dgFloat32 x = -(plane.m_y * y0 + d) / plane.m_x;
contactsOut[count] = dgVector(x, y0 + r, dgFloat32(0.0f),
dgFloat32(0.0f));
count++;
}
}
}
if (count < 2)
{
dgVector dp(m_silhuette[3] - m_silhuette[2]);
den = normal1 % dp;
_ASSERTE(dgAbsf (den) > dgFloat32 (0.0f));
test0 = -plane.Evalue(m_silhuette[2]) / den;
if ((test0 <= dgFloat32(1.0)) && (test0 >= dgFloat32(0.0f)))
{
contactsOut[count] = m_silhuette[2] + dp.Scale(test0);
count++;
}
}
if (count < 2)
{
test0 = plane.m_w + plane.m_y * m_radius;
if (dgAbsf(test0) < m_height)
{
dgFloat32 r = m_radius;
dgFloat32 d = plane.m_w + r * plane.m_y;
dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
dgFloat32 b = dgFloat32(2.0f) * plane.m_y * d;
dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x;
dgFloat32 desc = b * b - dgFloat32(4.0f) * a * c;
if (desc > dgFloat32(0.0f))
{
_ASSERTE(dgAbsf (a) > dgFloat32 (0.0f));
desc = dgSqrt (desc);
a = -dgFloat32(0.5f) * b / a;
dgFloat32 y0 = a + desc;
dgFloat32 y1 = a - desc;
if (y0 < dgFloat32(0.0f))
{
y0 = y1;
}_ASSERTE(y0 > dgFloat32 (0.0f));
_ASSERTE(dgAbsf (plane.m_x) > dgFloat32 (0.0f));
dgFloat32 x = -(plane.m_y * y0 + d) / plane.m_x;
contactsOut[count] = dgVector(x, y0 + r, dgFloat32(0.0f),
dgFloat32(0.0f));
count++;
}
}
}
}
for (dgInt32 i = 0; i < count; i++)
{
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
}
else
{
count = dgCollisionConvex::CalculatePlaneIntersection(normal, origin,
contactsOut);
}
return count;
}
