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