dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius) { dgVector p0Origin (p0 - origin); if (p0Origin.DotProduct3(p0Origin) < (dgFloat32 (100.0f) * radius * radius)) { dgVector dp (p1 - p0); dgFloat32 a = dp.DotProduct3(dp); dgFloat32 b = dgFloat32 (2.0f) * p0Origin.DotProduct3(dp); dgFloat32 c = p0Origin.DotProduct3(p0Origin) - radius * radius; dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c; if (desc >= 0.0f) { desc = dgSqrt (desc); dgFloat32 den = dgFloat32 (0.5f) / a; dgFloat32 t0 = (-b + desc) * den; dgFloat32 t1 = (-b - desc) * den; if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) { t0 = dgMin(t0, t1); if (t0 <= dgFloat32 (1.0f)) { return t0; } } else if (t0 >= dgFloat32 (0.0f)) { if (t0 <= dgFloat32 (1.0f)) { return t0; } } else { if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) { return t1; } } } } else { dgBigVector p0Origin1 (p0Origin); dgBigVector dp (p1 - p0); dgFloat64 a = dp.DotProduct3(dp); dgFloat64 b = dgFloat32 (2.0f) * p0Origin1.DotProduct3(dp); dgFloat64 c = p0Origin1.DotProduct3(p0Origin1) - dgFloat64(radius) * radius; dgFloat64 desc = b * b - dgFloat32 (4.0f) * a * c; if (desc >= 0.0f) { desc = sqrt (desc); dgFloat64 den = dgFloat32 (0.5f) / a; dgFloat64 t0 = (-b + desc) * den; dgFloat64 t1 = (-b - desc) * den; if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) { t0 = dgMin(t0, t1); if (t0 <= dgFloat32 (1.0f)) { return dgFloat32 (t0); } } else if (t0 >= dgFloat32 (0.0f)) { if (t0 <= dgFloat32 (1.0f)) { return dgFloat32 (t0); } } else { if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) { return dgFloat32 (t1); } } } } return dgFloat32 (1.2f); }
dgFloat32 dgCollisionDeformableMesh::CalculateSurfaceArea (const dgDeformableNode* const node0, const dgDeformableNode* const node1, dgVector& minBox, dgVector& maxBox) const { minBox = dgVector (dgMin (node0->m_minBox.m_x, node1->m_minBox.m_x), dgMin (node0->m_minBox.m_y, node1->m_minBox.m_y), dgMin (node0->m_minBox.m_z, node1->m_minBox.m_z), dgFloat32 (0.0f)); maxBox = dgVector (dgMax (node0->m_maxBox.m_x, node1->m_maxBox.m_x), dgMax (node0->m_maxBox.m_y, node1->m_maxBox.m_y), dgMax (node0->m_maxBox.m_z, node1->m_maxBox.m_z), dgFloat32 (0.0f)); dgVector side0 (maxBox - minBox); dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f)); return side0 % side1; }
dgFloat32 dgBody::RayCast (const dgLineBox& line, OnRayCastAction filter, OnRayPrecastAction preFilter, void* const userData, dgFloat32 maxT) const { dgAssert (filter); dgVector l0 (line.m_l0); dgVector l1 (line.m_l0 + (line.m_l1 - line.m_l0).Scale4 (dgMin(maxT, dgFloat32 (1.0f)))); if (dgRayBoxClip (l0, l1, m_minAABB, m_maxAABB)) { // if (1) { //l0 = dgVector (-20.3125000f, 3.54991579f, 34.3441200f, 0.0f); //l1 = dgVector (-19.6875000f, 3.54257250f, 35.2211456f, 0.0f); dgContactPoint contactOut; const dgMatrix& globalMatrix = m_collision->GetGlobalMatrix(); dgVector localP0 (globalMatrix.UntransformVector (l0)); dgVector localP1 (globalMatrix.UntransformVector (l1)); dgVector p1p0 (localP1 - localP0); if ((p1p0 % p1p0) > dgFloat32 (1.0e-12f)) { dgFloat32 t = m_collision->RayCast (localP0, localP1, dgFloat32 (1.0f), contactOut, preFilter, this, userData); if (t < dgFloat32 (1.0f)) { dgVector p (globalMatrix.TransformVector(localP0 + (localP1 - localP0).Scale3(t))); dgVector l1l0 (line.m_l1 - line.m_l0); t = ((p - line.m_l0) % l1l0) / (l1l0 % l1l0); if (t < maxT) { dgAssert (t >= dgFloat32 (0.0f)); dgAssert (t <= dgFloat32 (1.0f)); contactOut.m_normal = globalMatrix.RotateVector (contactOut.m_normal); maxT = filter (this, contactOut.m_collision0, p, contactOut.m_normal, contactOut.m_shapeId0, userData, t); } } } } return maxT; }
dgFloat32 dgCollisionBVH::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const { dgBVHRay ray (localP0, localP1); ray.m_t = dgMin(maxT, dgFloat32 (1.0f)); ray.m_me = this; ray.m_userData = userData; if (!m_userRayCastCallback) { ForAllSectorsRayHit (ray, maxT, RayHit, &ray); if (ray.m_t <= maxT) { maxT = ray.m_t; contactOut.m_normal = ray.m_normal.Scale3 (dgRsqrt ((ray.m_normal % ray.m_normal) + dgFloat32 (1.0e-8f))); // contactOut.m_userId = ray.m_id; contactOut.m_shapeId0 = ray.m_id; contactOut.m_shapeId1 = ray.m_id; } } else { if (body) { //ray.m_matrix = body->m_collisionWorldMatrix; ray.m_matrix = body->m_collision->GetGlobalMatrix(); } ForAllSectorsRayHit (ray, maxT, RayHitUser, &ray); if (ray.m_t <= dgFloat32 (1.0f)) { maxT = ray.m_t; contactOut.m_normal = ray.m_normal.Scale3 (dgRsqrt ((ray.m_normal % ray.m_normal) + dgFloat32 (1.0e-8f))); // contactOut.m_userId = ray.m_id; contactOut.m_shapeId0 = ray.m_id; contactOut.m_shapeId1 = ray.m_id; } } return maxT; }
void TriangleBox (const dgVector* const position, const dgInt32* const faceIndices, dgVector& minP, dgVector& maxP) const { minP = position[faceIndices[0]]; maxP = position[faceIndices[0]]; for (dgInt32 i = 1; i < 3; i ++) { dgInt32 index = faceIndices[i]; const dgVector& p = position[index]; minP.m_x = dgMin (p.m_x, minP.m_x); minP.m_y = dgMin (p.m_y, minP.m_y); minP.m_z = dgMin (p.m_z, minP.m_z); maxP.m_x = dgMax (p.m_x, maxP.m_x); maxP.m_y = dgMax (p.m_y, maxP.m_y); maxP.m_z = dgMax (p.m_z, maxP.m_z); } }
dgInt32 dgWorld::CompareJointByInvMass(const dgBilateralConstraint* const jointA, const dgBilateralConstraint* const jointB, void* notUsed) { dgInt32 modeA = jointA->m_solverModel; dgInt32 modeB = jointB->m_solverModel; if (modeA < modeB) { return -1; } else if (modeA > modeB) { return 1; } else { dgFloat32 invMassA = dgMin(jointA->GetBody0()->m_invMass.m_w, jointA->GetBody1()->m_invMass.m_w); dgFloat32 invMassB = dgMin(jointB->GetBody0()->m_invMass.m_w, jointB->GetBody1()->m_invMass.m_w); if (invMassA < invMassB) { return -1; } else if (invMassA > invMassB) { return 1; } } return 0; }
void dgContact::JointAccelerations(dgJointAccelerationDecriptor* const params) { dgJacobianMatrixElement* const rowMatrix = params->m_rowMatrix; const dgVector& bodyVeloc0 = m_body0->m_veloc; const dgVector& bodyOmega0 = m_body0->m_omega; const dgVector& bodyVeloc1 = m_body1->m_veloc; const dgVector& bodyOmega1 = m_body1->m_omega; dgInt32 count = params->m_rowsCount; dgFloat32 timestep = dgFloat32 (1.0f); dgFloat32 invTimestep = dgFloat32 (1.0f); if (params->m_timeStep > dgFloat32 (0.0f)) { timestep = params->m_timeStep; invTimestep = params->m_invTimeStep; } for (dgInt32 k = 0; k < count; k ++) { // if (!rowMatrix[k].m_accelIsMotor) // { dgJacobianMatrixElement* const row = &rowMatrix[k]; dgVector relVeloc (row->m_Jt.m_jacobianM0.m_linear.CompProduct4(bodyVeloc0) + row->m_Jt.m_jacobianM0.m_angular.CompProduct4(bodyOmega0) + row->m_Jt.m_jacobianM1.m_linear.CompProduct4(bodyVeloc1) + row->m_Jt.m_jacobianM1.m_angular.CompProduct4(bodyOmega1)); dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z; dgFloat32 aRel = row->m_deltaAccel; if (row->m_normalForceIndex < 0) { dgFloat32 restitution = (vRel <= dgFloat32 (0.0f)) ? (dgFloat32 (1.0f) + row->m_restitution) : dgFloat32 (1.0f); dgFloat32 penetrationVeloc = dgFloat32 (0.0f); if (row->m_penetration > DG_RESTING_CONTACT_PENETRATION * dgFloat32 (0.125f)) { if (vRel > dgFloat32 (0.0f)) { dgFloat32 penetrationCorrection = vRel * timestep; dgAssert (penetrationCorrection >= dgFloat32 (0.0f)); row->m_penetration = dgMax (dgFloat32 (0.0f), row->m_penetration - penetrationCorrection); } else { dgFloat32 penetrationCorrection = -vRel * timestep * row->m_restitution * dgFloat32 (8.0f); if (penetrationCorrection > row->m_penetration) { row->m_penetration = dgFloat32 (0.001f); } } penetrationVeloc = -(row->m_penetration * row->m_penetrationStiffness); } vRel *= restitution; vRel = dgMin (dgFloat32 (4.0f), vRel + penetrationVeloc); } row->m_coordenateAccel = (aRel - vRel * invTimestep); // } } }
void dgCollisionScene::CollidePair (dgCollidingPairCollector::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); 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_floatingCollision = &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_referenceCollision); //if (buffer[i].m_collision1 == proxy.m_floatingCollision) { // buffer[i].m_collision1 = myInstance; //} 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*))); } } } } else { dgVector origin; dgVector size; 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_floatingCollision = &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_referenceCollision); //if (buffer[i].m_collision1 == proxy.m_floatingCollision) { // buffer[i].m_collision1 = myInstance; //} 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; }
void dgCollisionScene::CollideCompoundPair (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy) const { const dgNodeBase* stackPool[4 * DG_COMPOUND_STACK_DEPTH][2]; dgContact* const constraint = pair->m_contact; dgBody* const myBody = constraint->GetBody1(); dgBody* const otherBody = constraint->GetBody0(); dgAssert (myBody == proxy.m_floatingBody); dgAssert (otherBody == proxy.m_referenceBody); dgCollisionInstance* const myCompoundInstance = myBody->m_collision; dgCollisionInstance* const otherCompoundInstance = otherBody->m_collision; dgAssert (myCompoundInstance->GetChildShape() == this); dgAssert (otherCompoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI)); dgCollisionCompound* const otherCompound = (dgCollisionCompound*)otherCompoundInstance->GetChildShape(); const dgContactMaterial* const material = constraint->GetMaterial(); dgMatrix myMatrix (myCompoundInstance->GetLocalMatrix() * myBody->m_matrix); dgMatrix otherMatrix (otherCompoundInstance->GetLocalMatrix() * otherBody->m_matrix); dgOOBBTestData data (otherMatrix * myMatrix.Inverse()); dgInt32 stack = 1; stackPool[0][0] = m_root; stackPool[0][1] = otherCompound->m_root; 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))) { dgAssert (0); } else { while (stack) { stack --; const dgNodeBase* const me = stackPool[stack][0]; const dgNodeBase* const other = stackPool[stack][1]; dgAssert (me && other); if (me->BoxTest (data, other)) { if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) { dgAssert (!me->m_right); bool processContacts = true; if (material->m_compoundAABBOverlap) { processContacts = material->m_compoundAABBOverlap (*material, myBody, me->m_myNode, otherBody, other->m_myNode, proxy.m_threadIndex); } if (processContacts) { const dgCollisionInstance* const mySrcInstance = me->GetShape(); const dgCollisionInstance* const otherSrcInstance = other->GetShape(); //dgCollisionInstance childInstance (*mySrcInstance, mySrcInstance->GetChildShape()); //dgCollisionInstance otherInstance (*otherSrcInstance, otherSrcInstance->GetChildShape()); dgCollisionInstance childInstance (*me->GetShape(), me->GetShape()->GetChildShape()); dgCollisionInstance otherInstance (*other->GetShape(), other->GetShape()->GetChildShape()); childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix); otherInstance.SetGlobalMatrix(otherInstance.GetLocalMatrix() * otherMatrix); proxy.m_floatingCollision = &childInstance; proxy.m_referenceCollision = &otherInstance; 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 ++) { //if (buffer[i].m_collision0 == proxy.m_floatingCollision) { // buffer[i].m_collision0 = mySrcInstance; //} //if (buffer[i].m_collision1 == proxy.m_referenceCollision) { // buffer[i].m_collision1 = otherSrcInstance; //} if (buffer[i].m_collision1->GetChildShape() == otherSrcInstance->GetChildShape()) { dgAssert(buffer[i].m_collision0->GetChildShape() == mySrcInstance->GetChildShape()); buffer[i].m_collision0 = mySrcInstance; buffer[i].m_collision1 = otherSrcInstance; } else { dgAssert(buffer[i].m_collision1->GetChildShape() == mySrcInstance->GetChildShape()); buffer[i].m_collision1 = mySrcInstance; buffer[i].m_collision0 = otherSrcInstance; } } } closestDist = dgMin(closestDist, constraint->m_closestDistance); } } else if (me->m_type == m_leaf) { dgAssert (other->m_type == m_node); stackPool[stack][0] = me; stackPool[stack][1] = other->m_left; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); stackPool[stack][0] = me; stackPool[stack][1] = other->m_right; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); } else if (other->m_type == m_leaf) { dgAssert (me->m_type == m_node); stackPool[stack][0] = me->m_left; stackPool[stack][1] = other; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); stackPool[stack][0] = me->m_right; stackPool[stack][1] = other; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); } else { dgAssert (me->m_type == m_node); dgAssert (other->m_type == m_node); stackPool[stack][0] = me->m_left; stackPool[stack][1] = other->m_left; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); stackPool[stack][0] = me->m_left; stackPool[stack][1] = other->m_right; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); stackPool[stack][0] = me->m_right; stackPool[stack][1] = other->m_left; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); stackPool[stack][0] = me->m_right; stackPool[stack][1] = other->m_right; stack++; dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*))); } } } } constraint->m_closestDistance = closestDist; }
dgAABBPointTree4d* dgConvexHull4d::BuildTree (dgAABBPointTree4d* const parent, dgHullVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** memoryPool, dgInt32& maxMemSize) const { dgAABBPointTree4d* tree = NULL; dgAssert (count); dgBigVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f)); dgBigVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f)); if (count <= DG_VERTEX_CLUMP_SIZE_4D) { dgAABBPointTree4dClump* const clump = new (*memoryPool) dgAABBPointTree4dClump; *memoryPool += sizeof (dgAABBPointTree4dClump); maxMemSize -= sizeof (dgAABBPointTree4dClump); dgAssert (maxMemSize >= 0); dgAssert (clump); clump->m_count = count; for (dgInt32 i = 0; i < count; i ++) { clump->m_indices[i] = i + baseIndex; const dgBigVector& p = points[i]; minP.m_x = dgMin (p.m_x, minP.m_x); minP.m_y = dgMin (p.m_y, minP.m_y); minP.m_z = dgMin (p.m_z, minP.m_z); minP.m_w = dgMin (p.m_w, minP.m_w); maxP.m_x = dgMax (p.m_x, maxP.m_x); maxP.m_y = dgMax (p.m_y, maxP.m_y); maxP.m_z = dgMax (p.m_z, maxP.m_z); maxP.m_w = dgMax (p.m_w, maxP.m_w); } clump->m_left = NULL; clump->m_right = NULL; tree = clump; } else { dgBigVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgBigVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); for (dgInt32 i = 0; i < count; i ++) { const dgBigVector& p = points[i]; minP.m_x = dgMin (p.m_x, minP.m_x); minP.m_y = dgMin (p.m_y, minP.m_y); minP.m_z = dgMin (p.m_z, minP.m_z); minP.m_w = dgMin (p.m_w, minP.m_w); maxP.m_x = dgMax (p.m_x, maxP.m_x); maxP.m_y = dgMax (p.m_y, maxP.m_y); maxP.m_z = dgMax (p.m_z, maxP.m_z); maxP.m_w = dgMax (p.m_w, maxP.m_w); median = median + p; varian = varian + p.CompProduct4(p); } varian = varian.Scale4 (dgFloat32 (count)) - median.CompProduct4(median); dgInt32 index = 0; dgFloat64 maxVarian = dgFloat64 (-1.0e10f); for (dgInt32 i = 0; i < 4; i ++) { if (varian[i] > maxVarian) { index = i; maxVarian = varian[i]; } } dgBigVector center = median.Scale4 (dgFloat64 (1.0f) / dgFloat64 (count)); dgFloat64 test = center[index]; dgInt32 i0 = 0; dgInt32 i1 = count - 1; do { for (; i0 <= i1; i0 ++) { dgFloat64 val = points[i0][index]; if (val > test) { break; } } for (; i1 >= i0; i1 --) { dgFloat64 val = points[i1][index]; if (val < test) { break; } } if (i0 < i1) { dgSwap(points[i0], points[i1]); i0++; i1--; } } while (i0 <= i1); if (i0 == 0){ i0 = count / 2; } if (i0 >= (count - 1)){ i0 = count / 2; } tree = new (*memoryPool) dgAABBPointTree4d; *memoryPool += sizeof (dgAABBPointTree4d); maxMemSize -= sizeof (dgAABBPointTree4d); dgAssert (maxMemSize >= 0); dgAssert (i0); dgAssert (count - i0); tree->m_left = BuildTree (tree, points, i0, baseIndex, memoryPool, maxMemSize); tree->m_right = BuildTree (tree, &points[i0], count - i0, i0 + baseIndex, memoryPool, maxMemSize); } dgAssert (tree); tree->m_parent = parent; tree->m_box[0] = minP - dgBigVector (dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f)); tree->m_box[1] = maxP + dgBigVector (dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f)); return tree; }
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance) { dgInt32 stride = strideInBytes / sizeof (dgFloat32); dgStack<dgFloat64> buffer(3 * 2 * count); for (dgInt32 i = 0; i < count; i ++) { buffer[i * 3 + 0] = vertexArray[i * stride + 0]; buffer[i * 3 + 1] = vertexArray[i * stride + 1]; buffer[i * 3 + 2] = vertexArray[i * stride + 2]; } dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance); if (!convexHull->GetCount()) { // this is a degenerated hull hull to add some thickness and for a thick plane delete convexHull; dgStack<dgVector> tmp(3 * count); for (dgInt32 i = 0; i < count; i ++) { tmp[i][0] = dgFloat32 (buffer[i*3 + 0]); tmp[i][1] = dgFloat32 (buffer[i*3 + 1]); tmp[i][2] = dgFloat32 (buffer[i*3 + 2]); tmp[i][2] = dgFloat32 (0.0f); } dgObb sphere; sphere.SetDimensions (&tmp[0][0], sizeof (dgVector), count); dgInt32 index = 0; dgFloat32 size = dgFloat32 (1.0e10f); for (dgInt32 i = 0; i < 3; i ++) { if (sphere.m_size[i] < size) { index = i; size = sphere.m_size[i]; } } dgVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); normal[index] = dgFloat32 (1.0f); dgVector step = sphere.RotateVector (normal.Scale3 (dgFloat32 (0.05f))); for (dgInt32 i = 0; i < count; i ++) { dgVector p1 (tmp[i] + step); dgVector p2 (tmp[i] - step); buffer[i * 3 + 0] = p1.m_x; buffer[i * 3 + 1] = p1.m_y; buffer[i * 3 + 2] = p1.m_z; buffer[(i + count) * 3 + 0] = p2.m_x; buffer[(i + count) * 3 + 1] = p2.m_y; buffer[(i + count) * 3 + 2] = p2.m_z; } count *= 2; convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance); if (!convexHull->GetCount()) { delete convexHull; return false; } } // check for degenerated faces for (bool success = false; !success; ) { success = true; const dgBigVector* const hullVertexArray = convexHull->GetVertexPool(); dgStack<dgInt8> mask(convexHull->GetVertexCount()); memset (&mask[0], 1, mask.GetSizeInBytes()); for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) { dgConvexHull3DFace& face = node->GetInfo(); const dgBigVector& p0 = hullVertexArray[face.m_index[0]]; const dgBigVector& p1 = hullVertexArray[face.m_index[1]]; const dgBigVector& p2 = hullVertexArray[face.m_index[2]]; dgBigVector p1p0 (p1 - p0); dgBigVector p2p0 (p2 - p0); dgBigVector normal (p2p0 * p1p0); dgFloat64 mag2 = normal % normal; if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) { success = false; dgInt32 index = -1; dgBigVector p2p1 (p2 - p1); dgFloat64 dist10 = p1p0 % p1p0; dgFloat64 dist20 = p2p0 % p2p0; dgFloat64 dist21 = p2p1 % p2p1; if ((dist10 >= dist20) && (dist10 >= dist21)) { index = 2; } else if ((dist20 >= dist10) && (dist20 >= dist21)) { index = 1; } else if ((dist21 >= dist10) && (dist21 >= dist20)) { index = 0; } dgAssert (index != -1); mask[face.m_index[index]] = 0; } } if (!success) { dgInt32 count = 0; dgInt32 vertexCount = convexHull->GetVertexCount(); for (dgInt32 i = 0; i < vertexCount; i ++) { if (mask[i]) { buffer[count * 3 + 0] = hullVertexArray[i].m_x; buffer[count * 3 + 1] = hullVertexArray[i].m_y; buffer[count * 3 + 2] = hullVertexArray[i].m_z; count ++; } } delete convexHull; convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance); } } dgAssert (convexHull); dgInt32 vertexCount = convexHull->GetVertexCount(); if (vertexCount < 4) { delete convexHull; return false; } const dgBigVector* const hullVertexArray = convexHull->GetVertexPool(); dgPolyhedra polyhedra (GetAllocator()); polyhedra.BeginFace(); for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) { dgConvexHull3DFace& face = node->GetInfo(); polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]); } polyhedra.EndFace(); if (vertexCount > 4) { // bool edgeRemoved = false; // while (RemoveCoplanarEdge (polyhedra, hullVertexArray)) { // edgeRemoved = true; // } // if (edgeRemoved) { // if (!CheckConvex (polyhedra, hullVertexArray)) { // delete convexHull; // return false; // } // } while (RemoveCoplanarEdge (polyhedra, hullVertexArray)); } dgStack<dgInt32> vertexMap(vertexCount); memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32)); dgInt32 mark = polyhedra.IncLRU(); dgPolyhedra::Iterator iter (polyhedra); for (iter.Begin(); iter; iter ++) { dgEdge* const edge = &iter.GetNode()->GetInfo(); if (edge->m_mark != mark) { if (vertexMap[edge->m_incidentVertex] == -1) { vertexMap[edge->m_incidentVertex] = m_vertexCount; m_vertexCount ++; } dgEdge* ptr = edge; do { ptr->m_mark = mark; ptr->m_userData = m_edgeCount; m_edgeCount ++; ptr = ptr->m_twin->m_next; } while (ptr != edge) ; } } m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector))); m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge))); m_vertexToEdgeMapping = (const dgConvexSimplexEdge**) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgConvexSimplexEdge*))); for (dgInt32 i = 0; i < vertexCount; i ++) { if (vertexMap[i] != -1) { m_vertex[vertexMap[i]] = hullVertexArray[i]; m_vertex[vertexMap[i]].m_w = dgFloat32 (0.0f); } } delete convexHull; vertexCount = m_vertexCount; mark = polyhedra.IncLRU();; for (iter.Begin(); iter; iter ++) { dgEdge* const edge = &iter.GetNode()->GetInfo(); if (edge->m_mark != mark) { dgEdge *ptr = edge; do { ptr->m_mark = mark; dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData]; simplexPtr->m_vertex = vertexMap[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) ; } } m_faceCount = 0; dgStack<char> faceMarks (m_edgeCount); memset (&faceMarks[0], 0, m_edgeCount * sizeof (dgInt8)); dgStack<dgConvexSimplexEdge*> faceArray (m_edgeCount); for (dgInt32 i = 0; i < m_edgeCount; i ++) { dgConvexSimplexEdge* const face = &m_simplex[i]; if (!faceMarks[i]) { dgConvexSimplexEdge* ptr = face; do { dgAssert ((ptr - m_simplex) >= 0); faceMarks[dgInt32 (ptr - m_simplex)] = '1'; ptr = ptr->m_next; } while (ptr != face); faceArray[m_faceCount] = face; m_faceCount ++; } } m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *))); memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *)); if (vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) { // create a face structure for support vertex dgStack<dgConvexBox> boxTree (vertexCount); dgTree<dgVector,dgInt32> sortTree(GetAllocator()); dgStack<dgTree<dgVector,dgInt32>::dgTreeNode*> vertexNodeList(vertexCount); dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f)); dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f)); for (dgInt32 i = 0; i < vertexCount; i ++) { const dgVector& p = m_vertex[i]; vertexNodeList[i] = sortTree.Insert (p, i); minP.m_x = dgMin (p.m_x, minP.m_x); minP.m_y = dgMin (p.m_y, minP.m_y); minP.m_z = dgMin (p.m_z, minP.m_z); maxP.m_x = dgMax (p.m_x, maxP.m_x); maxP.m_y = dgMax (p.m_y, maxP.m_y); maxP.m_z = dgMax (p.m_z, maxP.m_z); } boxTree[0].m_box[0] = minP; boxTree[0].m_box[1] = maxP; boxTree[0].m_leftBox = -1; boxTree[0].m_rightBox = -1; boxTree[0].m_vertexStart = 0; boxTree[0].m_vertexCount = vertexCount; dgInt32 boxCount = 1; dgInt32 stack = 1; dgInt32 stackBoxPool[64]; stackBoxPool[0] = 0; while (stack) { stack --; dgInt32 boxIndex = stackBoxPool[stack]; dgConvexBox& box = boxTree[boxIndex]; if (box.m_vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) { dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); for (dgInt32 i = 0; i < box.m_vertexCount; i ++) { dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo(); minP.m_x = dgMin (p.m_x, minP.m_x); minP.m_y = dgMin (p.m_y, minP.m_y); minP.m_z = dgMin (p.m_z, minP.m_z); maxP.m_x = dgMax (p.m_x, maxP.m_x); maxP.m_y = dgMax (p.m_y, maxP.m_y); maxP.m_z = dgMax (p.m_z, maxP.m_z); median += p; varian += p.CompProduct3 (p); } varian = varian.Scale3 (dgFloat32 (box.m_vertexCount)) - median.CompProduct3(median); dgInt32 index = 0; dgFloat64 maxVarian = dgFloat64 (-1.0e10f); for (dgInt32 i = 0; i < 3; i ++) { if (varian[i] > maxVarian) { index = i; maxVarian = varian[i]; } } dgVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (box.m_vertexCount)); dgFloat32 test = center[index]; dgInt32 i0 = 0; dgInt32 i1 = box.m_vertexCount - 1; do { for (; i0 <= i1; i0 ++) { dgFloat32 val = vertexNodeList[box.m_vertexStart + i0]->GetInfo()[index]; if (val > test) { break; } } for (; i1 >= i0; i1 --) { dgFloat32 val = vertexNodeList[box.m_vertexStart + i1]->GetInfo()[index]; if (val < test) { break; } } if (i0 < i1) { dgSwap(vertexNodeList[box.m_vertexStart + i0], vertexNodeList[box.m_vertexStart + i1]); i0++; i1--; } } while (i0 <= i1); if (i0 == 0){ i0 = box.m_vertexCount / 2; } if (i0 >= (box.m_vertexCount - 1)){ i0 = box.m_vertexCount / 2; } { dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f)); dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f)); for (dgInt32 i = i0; i < box.m_vertexCount; i ++) { const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo(); minP.m_x = dgMin (p.m_x, minP.m_x); minP.m_y = dgMin (p.m_y, minP.m_y); minP.m_z = dgMin (p.m_z, minP.m_z); maxP.m_x = dgMax (p.m_x, maxP.m_x); maxP.m_y = dgMax (p.m_y, maxP.m_y); maxP.m_z = dgMax (p.m_z, maxP.m_z); } box.m_rightBox = boxCount; boxTree[boxCount].m_box[0] = minP; boxTree[boxCount].m_box[1] = maxP; boxTree[boxCount].m_leftBox = -1; boxTree[boxCount].m_rightBox = -1; boxTree[boxCount].m_vertexStart = box.m_vertexStart + i0; boxTree[boxCount].m_vertexCount = box.m_vertexCount - i0; stackBoxPool[stack] = boxCount; stack ++; boxCount ++; } { dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f)); dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f)); for (dgInt32 i = 0; i < i0; i ++) { const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo(); minP.m_x = dgMin (p.m_x, minP.m_x); minP.m_y = dgMin (p.m_y, minP.m_y); minP.m_z = dgMin (p.m_z, minP.m_z); maxP.m_x = dgMax (p.m_x, maxP.m_x); maxP.m_y = dgMax (p.m_y, maxP.m_y); maxP.m_z = dgMax (p.m_z, maxP.m_z); } box.m_leftBox = boxCount; boxTree[boxCount].m_box[0] = minP; boxTree[boxCount].m_box[1] = maxP; boxTree[boxCount].m_leftBox = -1; boxTree[boxCount].m_rightBox = -1; boxTree[boxCount].m_vertexStart = box.m_vertexStart; boxTree[boxCount].m_vertexCount = i0; stackBoxPool[stack] = boxCount; stack ++; boxCount ++; } } } for (dgInt32 i = 0; i < m_vertexCount; i ++) { m_vertex[i] = vertexNodeList[i]->GetInfo(); vertexNodeList[i]->GetInfo().m_w = dgFloat32 (i); } m_supportTreeCount = boxCount; m_supportTree = (dgConvexBox*) m_allocator->Malloc(dgInt32 (boxCount * sizeof(dgConvexBox))); memcpy (m_supportTree, &boxTree[0], boxCount * sizeof(dgConvexBox)); for (dgInt32 i = 0; i < m_edgeCount; i ++) { dgConvexSimplexEdge* const ptr = &m_simplex[i]; dgTree<dgVector,dgInt32>::dgTreeNode* const node = sortTree.Find(ptr->m_vertex); dgInt32 index = dgInt32 (node->GetInfo().m_w); ptr->m_vertex = dgInt16 (index); } } for (dgInt32 i = 0; i < m_edgeCount; i ++) { dgConvexSimplexEdge* const edge = &m_simplex[i]; m_vertexToEdgeMapping[edge->m_vertex] = edge; } SetVolumeAndCG (); return true; }
dgMeshEffect * dgMeshEffect::CreateVoronoiConvexDecomposition (dgMemoryAllocator * const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32 * const pointCloud, dgInt32 materialId, const dgMatrix & textureProjectionMatrix) { dgFloat32 normalAngleInRadians = 30.0f * 3.1416f / 180.0f; dgStack<dgBigVector> buffer (pointCount + 16); dgBigVector * const pool = &buffer[0]; dgInt32 count = 0; dgFloat64 quantizeFactor = dgFloat64 (16.0f); dgFloat64 invQuantizeFactor = dgFloat64 (1.0f) / quantizeFactor; dgInt32 stride = pointStrideInBytes / sizeof (dgFloat32); dgBigVector pMin (dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (0.0f)); dgBigVector pMax (dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (0.0f)); for (dgInt32 i = 0; i < pointCount; i ++) { dgFloat64 x = pointCloud[i * stride + 0]; dgFloat64 y = pointCloud[i * stride + 1]; dgFloat64 z = pointCloud[i * stride + 2]; x = floor (x * quantizeFactor) * invQuantizeFactor; y = floor (y * quantizeFactor) * invQuantizeFactor; z = floor (z * quantizeFactor) * invQuantizeFactor; dgBigVector p (x, y, z, dgFloat64 (0.0f)); pMin = dgBigVector (dgMin (x, pMin.m_x), dgMin (y, pMin.m_y), dgMin (z, pMin.m_z), dgFloat64 (0.0f)); pMax = dgBigVector (dgMax (x, pMax.m_x), dgMax (y, pMax.m_y), dgMax (z, pMax.m_z), dgFloat64 (0.0f)); pool[count] = p; count ++; } // add the bbox as a barrier pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); count += 8; dgStack<dgInt32> indexList (count); count = dgVertexListToIndexList (&pool[0].m_x, sizeof (dgBigVector), 3, count, &indexList[0], dgFloat64 (5.0e-2f)); dgAssert (count >= 8); dgFloat64 maxSize = dgMax (pMax.m_x - pMin.m_x, pMax.m_y - pMin.m_y, pMax.m_z - pMin.m_z); pMin -= dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f)); pMax += dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f)); // add the a guard zone, so that we do no have to clip dgInt32 guadVertexKey = count; pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f)); pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f)); pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f)); count += 8; dgDelaunayTetrahedralization delaunayTetrahedras (allocator, &pool[0].m_x, count, sizeof (dgBigVector), dgFloat32 (0.0f)); delaunayTetrahedras.RemoveUpperHull (); // delaunayTetrahedras.Save("xxx0.txt"); dgInt32 tetraCount = delaunayTetrahedras.GetCount(); dgStack<dgBigVector> voronoiPoints (tetraCount + 32); dgStack<dgDelaunayTetrahedralization::dgListNode *> tetradrumNode (tetraCount); dgTree<dgList<dgInt32>, dgInt32> delanayNodes (allocator); dgInt32 index = 0; const dgHullVector * const delanayPoints = delaunayTetrahedras.GetHullVertexArray(); for (dgDelaunayTetrahedralization::dgListNode * node = delaunayTetrahedras.GetFirst(); node; node = node->GetNext()) { dgConvexHull4dTetraherum & tetra = node->GetInfo(); voronoiPoints[index] = tetra.CircumSphereCenter (delanayPoints); tetradrumNode[index] = node; for (dgInt32 i = 0; i < 4; i ++) { dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * header = delanayNodes.Find (tetra.m_faces[0].m_index[i]); if (!header) { dgList<dgInt32> list (allocator); header = delanayNodes.Insert (list, tetra.m_faces[0].m_index[i]); } header->GetInfo().Append (index); } index ++; } dgMeshEffect * const voronoiPartition = new (allocator) dgMeshEffect (allocator); voronoiPartition->BeginPolygon(); dgFloat64 layer = dgFloat64 (0.0f); dgTree<dgList<dgInt32>, dgInt32>::Iterator iter (delanayNodes); for (iter.Begin(); iter; iter ++) { dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * const nodeNode = iter.GetNode(); const dgList<dgInt32> & list = nodeNode->GetInfo(); dgInt32 key = nodeNode->GetKey(); if (key < guadVertexKey) { dgBigVector pointArray[512]; dgInt32 indexArray[512]; dgInt32 count = 0; for (dgList<dgInt32>::dgListNode * ptr = list.GetFirst(); ptr; ptr = ptr->GetNext()) { dgInt32 i = ptr->GetInfo(); pointArray[count] = voronoiPoints[i]; count ++; dgAssert (count < dgInt32 (sizeof (pointArray) / sizeof (pointArray[0]))); } count = dgVertexListToIndexList (&pointArray[0].m_x, sizeof (dgBigVector), 3, count, &indexArray[0], dgFloat64 (1.0e-3f)); if (count >= 4) { dgMeshEffect convexMesh (allocator, &pointArray[0].m_x, count, sizeof (dgBigVector), dgFloat64 (0.0f)); if (convexMesh.GetCount()) { convexMesh.CalculateNormals (normalAngleInRadians); convexMesh.UniformBoxMapping (materialId, textureProjectionMatrix); for (dgInt32 i = 0; i < convexMesh.m_pointCount; i ++) convexMesh.m_points[i].m_w = layer; for (dgInt32 i = 0; i < convexMesh.m_atribCount; i ++) convexMesh.m_attrib[i].m_vertex.m_w = layer; voronoiPartition->MergeFaces (&convexMesh); layer += dgFloat64 (1.0f); } } } } voronoiPartition->EndPolygon (dgFloat64 (1.0e-8f), false); // voronoiPartition->SaveOFF("xxx0.off"); //voronoiPartition->ConvertToPolygons(); return voronoiPartition; }
void dgWorldDynamicUpdate::ResolveClusterForces(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const { if (cluster->m_activeJointCount) { SortClusters(cluster, timestep, threadID); } if (!cluster->m_isContinueCollision) { if (cluster->m_activeJointCount) { BuildJacobianMatrix (cluster, threadID, timestep); CalculateClusterReactionForces(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); //CalculateClusterReactionForces_1(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); } else { IntegrateExternalForce(cluster, timestep, threadID); } IntegrateVelocity (cluster, DG_SOLVER_MAX_ERROR, timestep, threadID); } else { // calculate reaction forces and new velocities BuildJacobianMatrix (cluster, threadID, timestep); IntegrateReactionsForces (cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); // see if the island goes to sleep bool isAutoSleep = true; bool stackSleeping = true; dgInt32 sleepCounter = 10000; dgWorld* const world = (dgWorld*) this; const dgInt32 bodyCount = cluster->m_bodyCount; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart]; const dgFloat32 forceDamp = DG_FREEZZING_VELOCITY_DRAG; dgFloat32 maxAccel = dgFloat32 (0.0f); dgFloat32 maxAlpha = dgFloat32 (0.0f); dgFloat32 maxSpeed = dgFloat32 (0.0f); dgFloat32 maxOmega = dgFloat32 (0.0f); const dgFloat32 speedFreeze = world->m_freezeSpeed2; const dgFloat32 accelFreeze = world->m_freezeAccel2; const dgVector forceDampVect (forceDamp, forceDamp, forceDamp, dgFloat32 (0.0f)); for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { dgAssert (body->m_invMass.m_w); const dgFloat32 accel2 = body->m_accel.DotProduct3(body->m_accel); const dgFloat32 alpha2 = body->m_alpha.DotProduct3(body->m_alpha); const dgFloat32 speed2 = body->m_veloc.DotProduct3(body->m_veloc); const dgFloat32 omega2 = body->m_omega.DotProduct3(body->m_omega); maxAccel = dgMax (maxAccel, accel2); maxAlpha = dgMax (maxAlpha, alpha2); maxSpeed = dgMax (maxSpeed, speed2); maxOmega = dgMax (maxOmega, omega2); bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze); if (equilibrium) { dgVector veloc (body->m_veloc * forceDampVect); dgVector omega = body->m_omega * forceDampVect; body->m_veloc = (dgVector (veloc.DotProduct4(veloc)) > m_velocTol) & veloc; body->m_omega = (dgVector (omega.DotProduct4(omega)) > m_velocTol) & omega; } body->m_equilibrium = dgUnsigned32 (equilibrium); stackSleeping &= equilibrium; isAutoSleep &= body->m_autoSleep; sleepCounter = dgMin (sleepCounter, body->m_sleepingCounter); } // clear accel and angular acceleration body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; } if (isAutoSleep) { if (stackSleeping) { // the island went to sleep mode, for (dgInt32 i = 1; i < bodyCount; i ++) { dgBody* const body = bodyArray[i].m_body; dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; body->m_veloc = dgVector::m_zero; body->m_omega = dgVector::m_zero; } } else { // island is not sleeping but may be resting with small residual velocity for a long time // see if we can force to go to sleep if ((maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) || (maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) || (maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) || (maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega)) { for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->m_sleepingCounter = 0; } } } else { dgInt32 index = 0; for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) { if ((maxAccel <= world->m_sleepTable[i].m_maxAccel) && (maxAlpha <= world->m_sleepTable[i].m_maxAlpha) && (maxSpeed <= world->m_sleepTable[i].m_maxVeloc) && (maxOmega <= world->m_sleepTable[i].m_maxOmega)) { index = i; break; } } dgInt32 timeScaleSleepCount = dgInt32 (dgFloat32 (60.0f) * sleepCounter * timestep); if (timeScaleSleepCount > world->m_sleepTable[index].m_steps) { // force island to sleep stackSleeping = true; for (dgInt32 i = 1; i < bodyCount; i ++) { dgBody* const body = bodyArray[i].m_body; dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; body->m_veloc = dgVector::m_zero; body->m_omega = dgVector::m_zero; body->m_equilibrium = true; } } else { sleepCounter ++; for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->m_sleepingCounter = sleepCounter; } } } } } } if (!(isAutoSleep & stackSleeping)) { // island is not sleeping, need to integrate island velocity const dgUnsigned32 lru = world->GetBroadPhase()->m_lru; const dgInt32 jointCount = cluster->m_jointCount; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart]; dgFloat32 timeRemaining = timestep; const dgFloat32 timeTol = dgFloat32 (0.01f) * timestep; for (dgInt32 i = 0; (i < DG_MAX_CONTINUE_COLLISON_STEPS) && (timeRemaining > timeTol); i ++) { // calculate the closest time to impact dgFloat32 timeToImpact = timeRemaining; for (dgInt32 j = 0; (j < jointCount) && (timeToImpact > timeTol); j ++) { dgContact* const contact = (dgContact*) constraintArray[j].m_joint; if (contact->GetId() == dgConstraint::m_contactConstraint) { dgDynamicBody* const body0 = (dgDynamicBody*)contact->m_body0; dgDynamicBody* const body1 = (dgDynamicBody*)contact->m_body1; if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) { dgVector p; dgVector q; dgVector normal; timeToImpact = dgMin (timeToImpact, world->CalculateTimeToImpact (contact, timeToImpact, threadID, p, q, normal, dgFloat32 (-1.0f / 256.0f))); } } } if (timeToImpact > timeTol) { timeRemaining -= timeToImpact; for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeToImpact); body->UpdateWorlCollisionMatrix(); } } } else { if (timeToImpact >= dgFloat32 (-1.0e-5f)) { for (dgInt32 j = 1; j < bodyCount; j++) { dgDynamicBody* const body = (dgDynamicBody*)bodyArray[j].m_body; if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeToImpact); body->UpdateWorlCollisionMatrix(); } } } CalculateClusterContacts (cluster, timeRemaining, lru, threadID); BuildJacobianMatrix (cluster, threadID, 0.0f); IntegrateReactionsForces (cluster, threadID, 0.0f, DG_SOLVER_MAX_ERROR); bool clusterReceding = true; const dgFloat32 step = timestep * dgFloat32 (1.0f / DG_MAX_CONTINUE_COLLISON_STEPS); for (dgInt32 k = 0; (k < DG_MAX_CONTINUE_COLLISON_STEPS) && clusterReceding; k ++) { dgFloat32 smallTimeStep = dgMin (step, timeRemaining); timeRemaining -= smallTimeStep; for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity (smallTimeStep); body->UpdateWorlCollisionMatrix(); } } clusterReceding = false; if (timeRemaining > timeTol) { CalculateClusterContacts (cluster, timeRemaining, lru, threadID); bool isColliding = false; for (dgInt32 j = 0; (j < jointCount) && !isColliding; j ++) { dgContact* const contact = (dgContact*) constraintArray[j].m_joint; if (contact->GetId() == dgConstraint::m_contactConstraint) { const dgBody* const body0 = contact->m_body0; const dgBody* const body1 = contact->m_body1; const dgVector& veloc0 = body0->m_veloc; const dgVector& veloc1 = body1->m_veloc; const dgVector& omega0 = body0->m_omega; const dgVector& omega1 = body1->m_omega; const dgVector& com0 = body0->m_globalCentreOfMass; const dgVector& com1 = body1->m_globalCentreOfMass; for (dgList<dgContactMaterial>::dgListNode* node = contact->GetFirst(); node; node = node->GetNext()) { const dgContactMaterial* const contactMaterial = &node->GetInfo(); dgVector vel0 (veloc0 + omega0.CrossProduct3(contactMaterial->m_point - com0)); dgVector vel1 (veloc1 + omega1.CrossProduct3(contactMaterial->m_point - com1)); dgVector vRel (vel0 - vel1); dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f)); dgFloat32 speed = vRel.DotProduct4(contactMaterial->m_normal).m_w; isColliding |= (speed < dgFloat32 (0.0f)); } } } clusterReceding = !isColliding; } } } } if (timeRemaining > dgFloat32 (0.0)) { for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeRemaining); body->UpdateCollisionMatrix (timeRemaining, threadID); } } } else { for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->UpdateCollisionMatrix (timestep, threadID); } } } } } }
void dgCollisionTaperedCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) { m_rtti |= dgCollisionTaperedCylinder_RTTI; m_radio0 = dgAbsf (radio0); m_radio1 = dgAbsf (radio1); m_height = dgAbsf (height * dgFloat32 (0.5f)); m_skinthickness = dgMin (dgMin (m_radio0, m_radio1, m_height) * dgFloat32 (0.005f), dgFloat32 (1.0f / 64.0f)); m_dirVector.m_x = radio1 - radio0; m_dirVector.m_y = m_height * dgFloat32 (2.0f); m_dirVector.m_z = dgFloat32 (0.0f); m_dirVector.m_w = dgFloat32 (0.0f); m_dirVector = m_dirVector.Scale3(dgRsqrt(m_dirVector % m_dirVector)); dgFloat32 angle = dgFloat32 (0.0f); for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { dgFloat32 sinAngle = dgSin (angle); dgFloat32 cosAngle = dgCos (angle); m_vertex[i ] = dgVector (- m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f)); m_vertex[i + DG_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f)); angle += dgPI2 / DG_CYLINDER_SEGMENTS; } m_edgeCount = DG_CYLINDER_SEGMENTS * 6; m_vertexCount = DG_CYLINDER_SEGMENTS * 2; dgCollisionConvex::m_vertex = m_vertex; if (!m_shapeRefCount) { dgPolyhedra polyhedra(m_allocator); dgInt32 wireframe[DG_CYLINDER_SEGMENTS]; dgInt32 j = DG_CYLINDER_SEGMENTS - 1; polyhedra.BeginFace (); for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { wireframe[0] = j; wireframe[1] = i; wireframe[2] = i + DG_CYLINDER_SEGMENTS; wireframe[3] = j + DG_CYLINDER_SEGMENTS; j = i; polyhedra.AddFace (4, wireframe); } for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { wireframe[i] = DG_CYLINDER_SEGMENTS - 1 - i; } polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe); for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { wireframe[i] = i + DG_CYLINDER_SEGMENTS; } polyhedra.AddFace (DG_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_shapeRefCount ++; dgCollisionConvex::m_simplex = m_edgeArray; SetVolumeAndCG (); }