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