void dgCollisionBVH::GetLocalAABB (const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const
{
dgAssert (0);
}
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;
}
void dgConvexHull4d::InsertNewVertex(dgInt32 vertexIndex, dgListNode* const frontFace, dgList<dgListNode*>& deletedFaces, dgList<dgListNode*>& newFaces)
{
dgAssert (Sanity());
dgList<dgListNode*> stack(GetAllocator());
dgInt32 mark = IncMark();
stack.Append(frontFace);
dgHullVector* const hullVertexArray = &m_points[0];
const dgBigVector& p = hullVertexArray[vertexIndex];
while (stack.GetCount()) {
dgList<dgListNode*>::dgListNode* const stackNode = stack.GetLast();
dgListNode* const node = stackNode->GetInfo();
stack.Remove(stackNode);
dgConvexHull4dTetraherum* const face = &node->GetInfo();
if ((face->GetMark() != mark) && (face->Evalue(hullVertexArray, p) > dgFloat64(0.0f))) {
#ifdef _DEBUG
for (dgList<dgListNode*>::dgListNode* deleteNode = deletedFaces.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) {
dgAssert (deleteNode->GetInfo() != node);
}
#endif
deletedFaces.Append(node);
face->SetMark(mark);
for (dgInt32 i = 0; i < 4; i ++) {
dgListNode* const twinNode = (dgListNode*)face->m_faces[i].m_twin;
dgAssert (twinNode);
dgConvexHull4dTetraherum* const twinFace = &twinNode->GetInfo();
if (twinFace->GetMark() != mark) {
stack.Append(twinNode);
}
}
}
}
dgTree<dgListNode*, dgInt32> perimeter(GetAllocator());
for (dgList<dgListNode*>::dgListNode* deleteNode = deletedFaces.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) {
dgListNode* const deleteTetraNode = deleteNode->GetInfo();
dgConvexHull4dTetraherum* const deletedTetra = &deleteTetraNode->GetInfo();
dgAssert (deletedTetra->GetMark() == mark);
for (dgInt32 i = 0; i < 4; i ++) {
dgListNode* const twinNode = deletedTetra->m_faces[i].m_twin;
dgConvexHull4dTetraherum* const twinTetra = &twinNode->GetInfo();
if (twinTetra->GetMark() != mark) {
if (!perimeter.Find(twinTetra->m_uniqueID)) {
perimeter.Insert(twinNode, twinTetra->m_uniqueID);
}
}
deletedTetra->m_faces[i].m_twin = NULL;
}
}
dgList<dgListNode*> coneList(GetAllocator());
dgTree<dgListNode*, dgInt32>::Iterator iter (perimeter);
for (iter.Begin(); iter; iter ++) {
dgListNode* const perimeterNode = iter.GetNode()->GetInfo();
dgConvexHull4dTetraherum* const perimeterTetra = &perimeterNode->GetInfo();
for (dgInt32 i = 0; i < 4; i ++) {
dgConvexHull4dTetraherum::dgTetrahedrumFace* const perimeterFace = &perimeterTetra->m_faces[i];
if (perimeterFace->m_twin->GetInfo().GetMark() == mark) {
dgListNode* const newNode = AddFace (vertexIndex, perimeterFace->m_index[0], perimeterFace->m_index[1], perimeterFace->m_index[2]);
newFaces.Addtop(newNode);
dgConvexHull4dTetraherum* const newTetra = &newNode->GetInfo();
newTetra->m_faces[2].m_twin = perimeterNode;
perimeterFace->m_twin = newNode;
coneList.Append (newNode);
}
}
}
for (dgList<dgListNode*>::dgListNode* coneNode = coneList.GetFirst(); coneNode->GetNext(); coneNode = coneNode->GetNext()) {
dgListNode* const coneNodeA = coneNode->GetInfo();
for (dgList<dgListNode*>::dgListNode* nextConeNode = coneNode->GetNext(); nextConeNode; nextConeNode = nextConeNode->GetNext()) {
dgListNode* const coneNodeB = nextConeNode->GetInfo();
LinkSibling (coneNodeA, coneNodeB);
}
}
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert(proxy.m_referenceCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_floatingCollision->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const polygonInstance = proxy.m_floatingCollision;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
dgInt32 count = 0;
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert(m_normal.m_w == dgFloat32(0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
dgVector savedFaceNormal(m_normal);
dgVector savedPosit (proxy.m_matrix.m_posit);
proxy.m_matrix.m_posit = dgVector::m_wOne;
dgVector hullOrigin(proxy.m_matrix.UnrotateVector (savedPosit));
for (dgInt32 i = 0; i < m_count; i++) {
m_localPoly[i] = hullOrigin + polyInstanceScale.CompProduct4(dgVector(&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert(m_localPoly[i].m_w == dgFloat32(0.0f));
}
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
contactJoint->m_closestDistance = -penetration;
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e);
//dgPlane plane(n, -(m_localPoly[i0] % n));
dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
plane = proxy.m_matrix.TransformPlane(plane);
//dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
//dgFloat32 centerDist = plane.Evalue(boxOrigin);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
dgAssert(penetration >= dgFloat32(0.0f));
dgVector pointsContacts[64];
dgAssert(penetration >= 0.0f);
dgVector point(pointInHull + normalInHull.Scale4(penetration));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
dgVector globalNormal(worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*)hull->m_childShape;
count = convexShape->CalculateConvexToConvexContact(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal(polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if (normal.DotProduct4(savedFaceNormal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n(&m_vertex[index * m_stride]);
if ((savedFaceNormal.DotProduct4(n).GetScalar() > dgFloat32(0.9995f))) {
normal = n;
} else {
dgVector dir0(n * savedFaceNormal);
dgVector dir1(n * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = n;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
proxy.m_matrix.m_posit = savedPosit;
return count;
}
dgUnsigned32 dgSlidingConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgMatrix matrix0;
dgMatrix matrix1;
//dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
matrix1.m_posit += matrix1.m_front.Scale3 (m_posit);
dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f));
const dgVector& dir1 = matrix0.m_up;
const dgVector& dir2 = matrix0.m_right;
dgVector p0 (matrix0.m_posit);
dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front));
dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector r0 (p0 + matrix0.m_up.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector r1 (p1 + matrix1.m_up.Scale3(MIN_JOINT_PIN_LENGTH));
dgPointParam pointDataP;
dgPointParam pointDataQ;
dgPointParam pointDataR;
InitPointParam (pointDataP, m_stiffness, p0, p1);
InitPointParam (pointDataQ, m_stiffness, q0, q1);
InitPointParam (pointDataR, m_stiffness, r0, r1);
CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]);
CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]);
CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]);
CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]);
CalculatePointDerivative (4, params, dir2, pointDataR, &m_jointForce[4]);
dgInt32 ret = 5;
if (m_jointAccelFnt) {
dgJointCallbackParam axisParam;
axisParam.m_accel = dgFloat32 (0.0f);
axisParam.m_timestep = params.m_timestep;
axisParam.m_minFriction = DG_MIN_BOUND;
axisParam.m_maxFriction = DG_MAX_BOUND;
if (m_jointAccelFnt (*this, &axisParam)) {
if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[5].m_low = axisParam.m_minFriction;
params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
CalculatePointDerivative (5, params, matrix0.m_front, pointDataP, &m_jointForce[5]);
//params.m_jointAccel[5] = axisParam.m_accel;
SetMotorAcceleration (5, axisParam.m_accel, params);
ret = 6;
}
}
return dgUnsigned32 (ret);
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgAssert (this == proxy.m_floatingCollision->GetChildShape());
dgAssert (m_count);
dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const floatingBody = proxy.m_floatingBody;
const dgBody* const referenceBody = proxy.m_referenceBody;
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32 (1.0e10f);
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert (m_normal.m_w == dgFloat32 (0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
const dgVector savedFaceNormal (m_normal);
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
}
dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;
dgMatrix polygonMatrix;
polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = hullOrigin;
dgAssert (polygonMatrix.TestOrthogonal());
dgMatrix savedProxyMatrix (proxy.m_matrix);
proxy.m_matrix = polygonMatrix * proxy.m_matrix;
dgVector floatingVeloc (floatingBody->m_veloc);
dgVector referenceVeloc (referenceBody->m_veloc);
const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));
dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
m_normal = polygonMatrix.UnrotateVector(m_normal);
if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
}
dgInt32 count = 0;
dgVector hullBoxP0;
dgVector hullBoxP1;
hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
dgVector minBox (polyBoxP0 - hullBoxP1);
dgVector maxBox (polyBoxP1 - hullBoxP0);
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
if (distance < dgFloat32 (1.0f)) {
dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// boxOrigin += polyRelativeVeloc.Scale4 (distance);
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector e (m_localPoly[i] - m_localPoly[i0]);
dgVector n (m_normal * e);
dgPlane plane (n, - (m_localPoly[i0] % n));
dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
dgFloat32 centerDist = plane.Evalue(boxOrigin);
if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
inside = false;
}
i0 = i;
}
// for the time being for the minkousky contact calculation
inside = false;
const dgInt32 hullId = hull->GetUserDataID();
if (inside) {
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 timetoImpact = dgFloat32 (0.0f);
//dgFloat32 closestDistance = dgFloat32 (0.0f);
dgAssert (0);
// dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32 (0.0f)) {
timetoImpact = penetration / (polyRelativeVeloc % m_normal);
dgAssert (timetoImpact >= dgFloat32 (0.0f));
// closestDistance = -penetration;
}
if (timetoImpact <= proxy.m_timestep) {
dgVector pointsContacts[64];
contactJoint->m_closestDistance = penetration;
dgAssert (0);
// dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector point (pointInHull);
count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
dgAssert (0);
// dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));
penetration = dgMax (penetration, dgFloat32 (0.0f));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping (boxOrigin, convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
count = convexShape->CalculateConvexCastContacts (proxy);
// dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n (&m_vertex[index * m_stride]);
dgVector dir0 (n * savedFaceNormal);
dgVector dir1 (n * normal);
dgFloat32 projection = dir0 % dir1;
if (projection <= dgFloat32 (0.0f)) {
normal = n;
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i ++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
#endif
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
proxy.m_matrix = savedProxyMatrix;
return count;
}
void dgCollisionConvexPolygon::Serialize(dgSerialize callback, void* const userData) const
{
dgAssert (0);
}
dgQuaternion dgQuaternion::Slerp (const dgQuaternion &QB, dgFloat32 t) const
{
dgAssert (0);
return dgQuaternion();
/*
dgFloat32 dot;
dgFloat32 ang;
dgFloat32 Sclp;
dgFloat32 Sclq;
dgFloat32 den;
dgFloat32 sinAng;
dgQuaternion Q;
dot = DotProduct (QB);
if ((dot + dgFloat32(1.0f)) > dgEPSILON) {
if (dot < (dgFloat32(1.0f) - dgEPSILON) ) {
ang = dgAcos (dot);
sinAng = dgSin (ang);
den = dgFloat32(1.0f) / sinAng;
Sclp = dgSin ((dgFloat32(1.0f) - t ) * ang) * den;
Sclq = dgSin (t * ang) * den;
} else {
Sclp = dgFloat32(1.0f) - t;
Sclq = t;
}
Q.m_q0 = m_q0 * Sclp + QB.m_q0 * Sclq;
Q.m_q1 = m_q1 * Sclp + QB.m_q1 * Sclq;
Q.m_q2 = m_q2 * Sclp + QB.m_q2 * Sclq;
Q.m_q3 = m_q3 * Sclp + QB.m_q3 * Sclq;
} else {
Q.m_q0 = m_q3;
Q.m_q1 = -m_q2;
Q.m_q2 = m_q1;
Q.m_q3 = m_q0;
Sclp = dgSin ((dgFloat32(1.0f) - t) * dgPI * dgFloat32 (0.5f));
Sclq = dgSin (t * dgPI * dgFloat32 (0.5f));
Q.m_q0 = m_q0 * Sclp + Q.m_q0 * Sclq;
Q.m_q1 = m_q1 * Sclp + Q.m_q1 * Sclq;
Q.m_q2 = m_q2 * Sclp + Q.m_q2 * Sclq;
Q.m_q3 = m_q3 * Sclp + Q.m_q3 * Sclq;
}
dot = Q.DotProduct (Q);
if ((dot) < dgFloat32(1.0f - dgEPSILON * 10.0f) ) {
//dot = dgFloat32(1.0f) / dgSqrt (dot);
dot = dgRsqrt (dot);
Q.m_q0 *= dot;
Q.m_q1 *= dot;
Q.m_q2 *= dot;
Q.m_q3 *= dot;
}
return Q;
*/
}
void dgBody::SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia)
{
dgFloat32 Ixx = inertia[0][0];
dgFloat32 Iyy = inertia[1][1];
dgFloat32 Izz = inertia[2][2];
mass = dgAbsf (mass);
if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI) || m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
mass = DG_INFINITE_MASS * 2.0f;
}
if (mass < DG_MINIMUM_MASS) {
mass = DG_INFINITE_MASS * 2.0f;
}
//dgAssert (m_masterNode);
m_world->GetBroadPhase()->CheckStaticDynamic(this, mass);
if (mass >= DG_INFINITE_MASS) {
m_mass.m_x = DG_INFINITE_MASS;
m_mass.m_y = DG_INFINITE_MASS;
m_mass.m_z = DG_INFINITE_MASS;
m_mass.m_w = DG_INFINITE_MASS;
m_invMass.m_x = dgFloat32 (0.0f);
m_invMass.m_y = dgFloat32 (0.0f);
m_invMass.m_z = dgFloat32 (0.0f);
m_invMass.m_w = dgFloat32 (0.0f);
if (m_masterNode) {
dgBodyMasterList& masterList (*m_world);
if (masterList.GetFirst() != m_masterNode) {
masterList.InsertAfter (masterList.GetFirst(), m_masterNode);
}
}
SetAparentMassMatrix (m_mass);
} else {
Ixx = dgAbsf (Ixx);
Iyy = dgAbsf (Iyy);
Izz = dgAbsf (Izz);
dgFloat32 Ixx1 = dgClamp (Ixx, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgFloat32 Iyy1 = dgClamp (Iyy, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgFloat32 Izz1 = dgClamp (Izz, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgAssert (Ixx > dgFloat32 (0.0f));
dgAssert (Iyy > dgFloat32 (0.0f));
dgAssert (Izz > dgFloat32 (0.0f));
m_mass.m_x = Ixx1;
m_mass.m_y = Iyy1;
m_mass.m_z = Izz1;
m_mass.m_w = mass;
m_invMass.m_x = dgFloat32 (1.0f) / Ixx1;
m_invMass.m_y = dgFloat32 (1.0f) / Iyy1;
m_invMass.m_z = dgFloat32 (1.0f) / Izz1;
m_invMass.m_w = dgFloat32 (1.0f) / mass;
if (m_masterNode) {
dgBodyMasterList& masterList (*m_world);
masterList.RotateToEnd (m_masterNode);
}
SetAparentMassMatrix (dgVector (Ixx, Iyy, Izz, mass));
}
#ifdef _DEBUG
dgBodyMasterList& me = *m_world;
for (dgBodyMasterList::dgListNode* refNode = me.GetFirst(); refNode; refNode = refNode->GetNext()) {
dgBody* const body0 = refNode->GetInfo().GetBody();
dgVector invMass (body0->GetInvMass());
if (invMass.m_w != 0.0f) {
for (; refNode; refNode = refNode->GetNext()) {
dgBody* const body1 = refNode->GetInfo().GetBody();
dgVector invMass (body1->GetInvMass());
dgAssert (invMass.m_w != 0.0f);
}
break;
}
}
#endif
}
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;
}
dgVector dgCollisionConvexHull::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dir.m_w == dgFloat32 (0.0f));
dgInt32 index = -1;
dgVector maxProj (dgFloat32 (-1.0e20f));
if (m_vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
dgFloat32 distPool[32];
const dgConvexBox* stackPool[32];
dgInt32 ix = (dir[0] > dgFloat64 (0.0f)) ? 1 : 0;
dgInt32 iy = (dir[1] > dgFloat64 (0.0f)) ? 1 : 0;
dgInt32 iz = (dir[2] > dgFloat64 (0.0f)) ? 1 : 0;
const dgConvexBox& leftBox = m_supportTree[m_supportTree[0].m_leftBox];
const dgConvexBox& rightBox = m_supportTree[m_supportTree[0].m_rightBox];
dgVector leftP (leftBox.m_box[ix][0], leftBox.m_box[iy][1], leftBox.m_box[iz][2], dgFloat32 (0.0f));
dgVector rightP (rightBox.m_box[ix][0], rightBox.m_box[iy][1], rightBox.m_box[iz][2], dgFloat32 (0.0f));
dgFloat32 leftDist = leftP.DotProduct4(dir).m_x;
dgFloat32 rightDist = rightP.DotProduct4(dir).m_x;
if (rightDist >= leftDist) {
distPool[0] = leftDist;
stackPool[0] = &leftBox;
distPool[1] = rightDist;
stackPool[1] = &rightBox;
} else {
distPool[0] = rightDist;
stackPool[0] = &rightBox;
distPool[1] = leftDist;
stackPool[1] = &leftBox;
}
dgInt32 stack = 2;
while (stack) {
stack--;
dgFloat32 dist = distPool[stack];
if (dist > maxProj.m_x) {
const dgConvexBox& box = *stackPool[stack];
if (box.m_leftBox > 0) {
dgAssert (box.m_rightBox > 0);
const dgConvexBox& leftBox = m_supportTree[box.m_leftBox];
const dgConvexBox& rightBox = m_supportTree[box.m_rightBox];
dgVector leftP (leftBox.m_box[ix][0], leftBox.m_box[iy][1], leftBox.m_box[iz][2], dgFloat32 (0.0f));
dgVector rightP (rightBox.m_box[ix][0], rightBox.m_box[iy][1], rightBox.m_box[iz][2], dgFloat32 (0.0f));
dgFloat32 leftDist = leftP.DotProduct4(dir).m_x;
dgFloat32 rightDist = rightP.DotProduct4(dir).m_x;
if (rightDist >= leftDist) {
distPool[stack] = leftDist;
stackPool[stack] = &leftBox;
stack ++;
dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));
distPool[stack] = rightDist;
stackPool[stack] = &rightBox;
stack ++;
dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));
} else {
distPool[stack] = rightDist;
stackPool[stack] = &rightBox;
stack ++;
dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));
distPool[stack] = leftDist;
stackPool[stack] = &leftBox;
stack ++;
dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));
}
} else {
for (dgInt32 i = 0; i < box.m_vertexCount; i ++) {
const dgVector& p = m_vertex[box.m_vertexStart + i];
dgAssert (p.m_x >= box.m_box[0].m_x);
dgAssert (p.m_x <= box.m_box[1].m_x);
dgAssert (p.m_y >= box.m_box[0].m_y);
dgAssert (p.m_y <= box.m_box[1].m_y);
dgAssert (p.m_z >= box.m_box[0].m_z);
dgAssert (p.m_z <= box.m_box[1].m_z);
dgVector dist (p.DotProduct4(dir));
//if (dist.m_x > maxProj.m_x) {
// maxProj = dist;
// index = box.m_vertexStart + i;
//}
dgVector mask (dist > maxProj);
dgInt32 intMask = *((dgInt32*) &mask.m_x);
index = ((box.m_vertexStart + i) & intMask) | (index & ~intMask);
maxProj = maxProj.GetMax(dist);
}
}
}
}
} else {
for (dgInt32 i = 0; i < m_vertexCount; i ++) {
const dgVector& p = m_vertex[i];
dgVector dist (p.DotProduct4(dir));
//if (dist.m_x > maxProj.m_x) {
// index = i;
// maxProj = dist;
//}
dgVector mask (dist > maxProj);
dgInt32 intMask = *((dgInt32*) &mask.m_x);
index = (i & intMask) | (index & ~intMask);
maxProj = maxProj.GetMax(dist);
}
}
if (vertexIndex) {
*vertexIndex = index;
}
dgAssert (index != -1);
return m_vertex[index];
}
bool dgCollisionConvexHull::RemoveCoplanarEdge (dgPolyhedra& polyhedra, const dgBigVector* const hullVertexArray) const
{
bool removeEdge = false;
// remove coplanar edges
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; ) {
dgEdge* edge0 = &(*iter);
iter ++;
if (edge0->m_incidentFace != -1) {
if (edge0->m_mark < mark) {
edge0->m_mark = mark;
edge0->m_twin->m_mark = mark;
dgBigVector normal0 (FaceNormal (edge0, &hullVertexArray[0]));
dgBigVector normal1 (FaceNormal (edge0->m_twin, &hullVertexArray[0]));
dgFloat64 test = normal0 % normal1;
if (test > dgFloat64 (0.99995f)) {
if ((edge0->m_twin->m_next->m_twin->m_next != edge0) && (edge0->m_next->m_twin->m_next != edge0->m_twin)) {
#define DG_MAX_EDGE_ANGLE dgFloat32 (1.0e-3f)
if (edge0->m_twin == &(*iter)) {
if (iter) {
iter ++;
}
}
dgBigVector e1 (hullVertexArray[edge0->m_twin->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_incidentVertex]);
dgBigVector e0 (hullVertexArray[edge0->m_incidentVertex] - hullVertexArray[edge0->m_prev->m_incidentVertex]);
dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
dgAssert ((e1 % e1) >= dgFloat64 (0.0f));
e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));
dgBigVector n1 (e0 * e1);
dgFloat64 projection = n1 % normal0;
if (projection >= DG_MAX_EDGE_ANGLE) {
dgBigVector e1 (hullVertexArray[edge0->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_incidentVertex]);
dgBigVector e0 (hullVertexArray[edge0->m_twin->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_prev->m_incidentVertex]);
dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
dgAssert ((e1 % e1) >= dgFloat64 (0.0f));
//e0 = e0.Scale3 (dgRsqrt (e0 % e0));
//e1 = e1.Scale3 (dgRsqrt (e1 % e1));
e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));
dgBigVector n1 (e0 * e1);
projection = n1 % normal0;
if (projection >= DG_MAX_EDGE_ANGLE) {
dgAssert (&(*iter) != edge0);
dgAssert (&(*iter) != edge0->m_twin);
polyhedra.DeleteEdge(edge0);
removeEdge = true;
}
}
} else {
dgEdge* next = edge0->m_next;
dgEdge* prev = edge0->m_prev;
polyhedra.DeleteEdge(edge0);
for (edge0 = next; edge0->m_prev->m_twin == edge0; edge0 = next) {
next = edge0->m_next;
polyhedra.DeleteEdge(edge0);
}
for (edge0 = prev; edge0->m_next->m_twin == edge0; edge0 = prev) {
prev = edge0->m_prev;
polyhedra.DeleteEdge(edge0);
}
iter.Begin();
removeEdge = true;
}
}
}
}
}
return removeEdge;
}
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;
}
dgMeshEffect * dgMeshEffect::CreateDelaunayTetrahedralization (dgMemoryAllocator * const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32 * const pointCloud, dgInt32 materialId, const dgMatrix & textureProjectionMatrix)
{
dgAssert (0);
return NULL;
}
void dgCollisionConvexPolygon::BeamClipping (const dgVector& origin, dgFloat32 dist)
{
dgPlane planes[4];
dgVector points[sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];
dgClippedFaceEdge clippedFace [2 * sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];
dgVector dir (m_localPoly[1] - m_localPoly[0]);
dgAssert (dir.m_w == dgFloat32 (0.0f));
dgAssert ((dir % dir) > dgFloat32 (1.0e-8f));
dir = dir.CompProduct4 (dir.InvMagSqrt());
dgFloat32 distH = origin.DotProduct4(dir).GetScalar();
planes[0] = dgPlane (dir, dist - distH);
planes[2] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distH);
dir = m_normal * dir;
dgFloat32 distV = origin.DotProduct4(dir).GetScalar();
planes[1] = dgPlane (dir, dist - distV);
planes[3] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distV);
for (dgInt32 i = 0; i < m_count; i ++) {
dgInt32 j = i << 1;
dgAssert (j < sizeof (clippedFace) / sizeof (clippedFace[0]));
points[i] = m_localPoly[i];
clippedFace[j + 0].m_twin = &clippedFace[j + 1];
clippedFace[j + 0].m_next = &clippedFace[j + 2];
clippedFace[j + 0].m_incidentVertex = i;
clippedFace[j + 0].m_incidentNormal = m_adjacentFaceEdgeNormalIndex[i];
clippedFace[j + 1].m_twin = &clippedFace[j + 0];
clippedFace[j + 1].m_next = &clippedFace[j - 2];
clippedFace[j + 1].m_incidentVertex = i + 1;
clippedFace[j + 1].m_incidentNormal = -1;
}
clippedFace[1].m_next = &clippedFace[m_count * 2 - 2 + 1];
dgAssert ((m_count * 2 - 2) >= 0);
clippedFace[m_count * 2 - 2].m_next = &clippedFace[0];
clippedFace[m_count * 2 - 2 + 1].m_incidentVertex = 0;
dgInt32 edgeCount = m_count * 2;
dgInt32 indexCount = m_count;
dgClippedFaceEdge* first = &clippedFace[0];
for (dgInt32 i = 0; i < 4; i ++) {
const dgPlane& plane = planes[i];
dgInt32 conectCount = 0;
dgClippedFaceEdge* connect[2];
dgClippedFaceEdge* ptr = first;
dgClippedFaceEdge* newFirst = first;
dgFloat32 test0 = plane.Evalue(points[ptr->m_incidentVertex]);
do {
dgFloat32 test1 = plane.Evalue(points[ptr->m_next->m_incidentVertex]);
if (test0 > dgFloat32 (1.0e-2f)) {
if (test1 <= dgFloat32 (-1.0e-2f)) {
const dgVector& p0 = points[ptr->m_incidentVertex];
const dgVector& p1 = points[ptr->m_next->m_incidentVertex];
dgVector dp (p1 - p0);
points[indexCount] = p0 - dp.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentVertex = indexCount;
newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex;
ptr->m_twin->m_incidentVertex = indexCount;
newEdge->m_next = ptr->m_next;
ptr->m_next->m_twin->m_next = newEdge->m_twin;
newEdge->m_twin->m_next = ptr->m_twin;
ptr->m_next = newEdge;
connect[conectCount] = ptr;
conectCount ++;
indexCount ++;
edgeCount += 2;
ptr = newEdge;
}
} else {
if (test1 > dgFloat32 (1.0e-2f)) {
newFirst = ptr->m_next;
const dgVector& p0 = points[ptr->m_incidentVertex];
const dgVector& p1 = points[ptr->m_next->m_incidentVertex];
dgVector dp (p1 - p0);
points[indexCount] = p0 - dp.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;;
newEdge->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentVertex = indexCount;
newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex;
ptr->m_twin->m_incidentVertex = indexCount;
newEdge->m_next = ptr->m_next;
ptr->m_next->m_twin->m_next = newEdge->m_twin;
newEdge->m_twin->m_next = ptr->m_twin;
ptr->m_next = newEdge;
connect[conectCount] = ptr;
conectCount ++;
indexCount ++;
edgeCount += 2;
ptr = newEdge;
}
}
test0 = test1;
ptr = ptr->m_next;
} while (ptr != first);
if(conectCount > 1) {
first = newFirst;
dgAssert (conectCount == 2);
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_incidentNormal = m_faceNormalIndex;;
newEdge->m_incidentVertex = connect[0]->m_next->m_incidentVertex;
newEdge->m_twin->m_next = connect[0]->m_next;
connect[0]->m_next = newEdge;
newEdge->m_twin->m_incidentNormal = m_faceNormalIndex;;
newEdge->m_twin->m_incidentVertex = connect[1]->m_next->m_incidentVertex;
newEdge->m_next = connect[1]->m_next;
connect[1]->m_next = newEdge->m_twin;
edgeCount += 2;
}
}
dgClippedFaceEdge* ptr = first;
do {
dgVector dist (points[ptr->m_next->m_incidentVertex] - points[ptr->m_incidentVertex]);
dgFloat32 error = dist % dist;
if (error < dgFloat32 (1.0e-6f)) {
ptr->m_next = ptr->m_next->m_next;
first = ptr;
}
ptr = ptr->m_next;
} while (ptr != first);
dgInt32 count = 0;
dgInt32 isConvexCap = 0;
m_adjacentFaceEdgeNormalIndex = &m_clippEdgeNormal[0];
do {
m_clippEdgeNormal[count] = ptr->m_incidentNormal;
isConvexCap |= (ptr->m_incidentNormal - m_faceNormalIndex);
m_localPoly[count] = points[ptr->m_incidentVertex];
count ++;
ptr = ptr->m_next;
} while (ptr != first);
m_count = count;
}
dgBody::dgBody (dgWorld* const world, const dgTree<const dgCollision*, dgInt32>* const collisionCashe, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber)
:m_invWorldInertiaMatrix(dgGetZeroMatrix())
,m_matrix (dgGetIdentityMatrix())
,m_rotation(dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f))
,m_invMass(dgFloat32 (0.0))
,m_veloc(dgFloat32 (0.0))
,m_omega(dgFloat32 (0.0))
,m_minAABB(dgFloat32 (0.0))
,m_maxAABB(dgFloat32 (0.0))
,m_netForce(dgFloat32 (0.0))
,m_netTorque(dgFloat32 (0.0))
,m_localCentreOfMass(dgFloat32 (0.0))
,m_globalCentreOfMass(dgFloat32 (0.0))
,m_aparentMass(dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS))
,m_maxAngulaRotationPerSet2(DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP )
,m_criticalSectionLock()
,m_flags(0)
,m_userData(NULL)
,m_world(world)
,m_collision(NULL)
,m_broadPhaseNode(NULL)
,m_masterNode(NULL)
,m_broadPhaseaggregateNode(NULL)
,m_destructor(NULL)
,m_matrixUpdate(NULL)
,m_index(0)
,m_uniqueID(0)
,m_bodyGroupId(0)
,m_rtti(m_baseBodyRTTI)
,m_type(0)
,m_dynamicsLru(0)
,m_genericLRUMark(0)
{
m_autoSleep = true;
m_collidable = true;
m_collideWithLinkedBodies = true;
m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
serializeCallback (userData, &m_rotation, sizeof (m_rotation));
serializeCallback (userData, &m_matrix.m_posit, sizeof (m_matrix.m_posit));
serializeCallback (userData, &m_veloc, sizeof (m_veloc));
serializeCallback (userData, &m_omega, sizeof (m_omega));
serializeCallback (userData, &m_localCentreOfMass, sizeof (m_localCentreOfMass));
serializeCallback (userData, &m_aparentMass, sizeof (m_aparentMass));
serializeCallback (userData, &m_flags, sizeof (m_flags));
serializeCallback (userData, &m_maxAngulaRotationPerSet2, sizeof (m_maxAngulaRotationPerSet2));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
dgInt32 id;
serializeCallback (userData, &id, sizeof (id));
dgTree<const dgCollision*, dgInt32>::dgTreeNode* const node = collisionCashe->Find(id);
dgAssert (node);
const dgCollision* const collision = node->GetInfo();
collision->AddRef();
dgCollisionInstance* const instance = new (world->GetAllocator()) dgCollisionInstance (world, serializeCallback, userData, revisionNumber);
instance->m_childShape = collision;
m_collision = instance;
}
dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgVector normal(normalIn);
dgInt32 count = 0;
dgFloat32 maxDist = dgFloat32 (1.0f);
dgFloat32 projectFactor = m_normal % normal;
if (projectFactor < dgFloat32 (0.0f)) {
projectFactor *= dgFloat32 (-1.0f);
normal = normal.Scale3 (dgFloat32 (-1.0f));
}
if (projectFactor > dgFloat32 (0.9999f)) {
for (dgInt32 i = 0; i < m_count; i ++) {
contactsOut[count] = m_localPoly[i];
count ++;
}
#ifdef _DEBUG
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
#endif
} else if (projectFactor > dgFloat32 (0.1736f)) {
maxDist = dgFloat32 (0.0f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if (side0 > dgFloat32 (0.0f)) {
maxDist = dgMax (maxDist, side0);
contactsOut[count] = p0 - plane.Scale3 (side0);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
if (side1 <= dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
} else if (side1 > dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
} else {
maxDist = dgFloat32 (1.0e10f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if ((side0 * side1) < dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
}
if (count > 1) {
if (maxDist < dgFloat32 (1.0e-3f)) {
dgVector maxPoint (contactsOut[0]);
dgVector minPoint (contactsOut[0]);
dgVector lineDir (m_normal * normal);
dgFloat32 proj = contactsOut[0] % lineDir;
dgFloat32 maxProjection = proj;
dgFloat32 minProjection = proj;
for (dgInt32 i = 1; i < count; i ++) {
proj = contactsOut[i] % lineDir;
if (proj > maxProjection) {
maxProjection = proj;
maxPoint = contactsOut[i];
}
if (proj < minProjection) {
minProjection = proj;
minPoint = contactsOut[i];
}
}
contactsOut[0] = maxPoint;
contactsOut[1] = minPoint;
count = 2;
}
dgVector error (contactsOut[count - 1] - contactsOut[0]);
if ((error % error) < dgFloat32 (1.0e-8f)) {
count --;
}
}
#ifdef _DEBUG
if (count > 1) {
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
if (count >= 3) {
dgVector n (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector e0 (contactsOut[1] - contactsOut[0]);
for (dgInt32 i = 2; i < count; i ++) {
dgVector e1 (contactsOut[i] - contactsOut[0]);
n += e0 * e1;
e0 = e1;
}
n = n.Scale3 (dgRsqrt(n % n));
dgFloat32 val = n % normal;
dgAssert (val > dgFloat32 (0.9f));
}
}
#endif
return count;
}
dgVector dgCollisionSphere::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dir.m_w == 0.0f);
return dir.Scale4 (m_radius);
}
void dgCollisionConvexPolygon::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__)
{
dgAssert (0);
}
void dgCollisionSphere::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__)
{
dgAssert (0);
}
dgFloat32 dgCollisionConvexPolygon::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* userData, OnRayPrecastAction preFilter) const
{
dgAssert (0);
return dgFloat32 (1.2f);
}
dgFloat32 dgCollisionPoint::GetVolume () const
{
dgAssert (0);
return dgFloat32 (0.0f);
}
dgFloat32 dgCollisionConvexPolygon::GetVolume () const
{
dgAssert (0);
return dgFloat32 (0.0f);
}
void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator)
{
m_rtti |= dgCollisionSphere_RTTI;
m_radius = radius;
m_edgeCount = DG_SPHERE_EDGE_COUNT;
m_vertexCount = DG_SPHERE_VERTEX_COUNT;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgInt32 indexList[256];
dgVector tmpVectex[256];
dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));
dgInt32 i = 1;
dgInt32 count = 0;
TesselateTriangle (i, p4, p0, p2, count, tmpVectex);
TesselateTriangle (i, p4, p2, p1, count, tmpVectex);
TesselateTriangle (i, p4, p1, p3, count, tmpVectex);
TesselateTriangle (i, p4, p3, p0, count, tmpVectex);
TesselateTriangle (i, p5, p2, p0, count, tmpVectex);
TesselateTriangle (i, p5, p1, p2, count, tmpVectex);
TesselateTriangle (i, p5, p3, p1, count, tmpVectex);
TesselateTriangle (i, p5, p0, p3, count, tmpVectex);
//dgAssert (count == EDGE_COUNT);
dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f);
dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT);
for (dgInt32 i = 0; i < vertexCount; i ++) {
m_unitSphere[i] = tmpVectex[i];
}
dgPolyhedra polyhedra(m_allocator);
polyhedra.BeginFace();
for (dgInt32 i = 0; i < count; i += 3) {
#ifdef _DEBUG
dgEdge* const edge = polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
dgAssert (edge);
#else
polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
#endif
}
polyhedra.EndFace();
dgUnsigned64 i1 = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i1;
i1 ++;
}
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];
}
}
for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) {
m_vertex[i] = m_unitSphere[i].Scale4 (m_radius);
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
dgInt32 dgConvexHull4d::SupportVertex (dgAABBPointTree4d** const treePointer, const dgHullVector* const points, const dgBigVector& dir) const
{
/*
dgFloat64 dist = dgFloat32 (-1.0e10f);
dgInt32 index = -1;
for (dgInt32 i = 0; i < m_count; i ++) {
dgFloat64 dist1 = dir.DotProduct4(points[i]).m_x;
if (dist1 > dist) {
dist = dist1;
index = i;
}
}
dgAssert (index != -1);
return index;
*/
#define DG_STACK_DEPTH_4D 64
dgFloat64 aabbProjection[DG_STACK_DEPTH_4D];
const dgAABBPointTree4d *stackPool[DG_STACK_DEPTH_4D];
dgInt32 index = -1;
dgInt32 stack = 1;
stackPool[0] = *treePointer;
aabbProjection[0] = dgFloat32 (1.0e20f);
dgFloat64 maxProj = dgFloat64 (-1.0e20f);
dgInt32 ix = (dir[0] > dgFloat64 (0.0f)) ? 1 : 0;
dgInt32 iy = (dir[1] > dgFloat64 (0.0f)) ? 1 : 0;
dgInt32 iz = (dir[2] > dgFloat64 (0.0f)) ? 1 : 0;
dgInt32 iw = (dir[3] > dgFloat64 (0.0f)) ? 1 : 0;
while (stack) {
stack--;
dgFloat64 boxSupportValue = aabbProjection[stack];
if (boxSupportValue > maxProj) {
const dgAABBPointTree4d* const me = stackPool[stack];
if (me->m_left && me->m_right) {
dgBigVector leftSupportPoint (me->m_left->m_box[ix].m_x, me->m_left->m_box[iy].m_y, me->m_left->m_box[iz].m_z, me->m_left->m_box[iw].m_w);
dgFloat64 leftSupportDist = leftSupportPoint.DotProduct4(dir).m_x;
dgBigVector rightSupportPoint (me->m_right->m_box[ix].m_x, me->m_right->m_box[iy].m_y, me->m_right->m_box[iz].m_z, me->m_right->m_box[iw].m_w);
dgFloat64 rightSupportDist = rightSupportPoint.DotProduct4(dir).m_x;
if (rightSupportDist >= leftSupportDist) {
aabbProjection[stack] = leftSupportDist;
stackPool[stack] = me->m_left;
stack++;
dgAssert (stack < DG_STACK_DEPTH_4D);
aabbProjection[stack] = rightSupportDist;
stackPool[stack] = me->m_right;
stack++;
dgAssert (stack < DG_STACK_DEPTH_4D);
} else {
aabbProjection[stack] = rightSupportDist;
stackPool[stack] = me->m_right;
stack++;
dgAssert (stack < DG_STACK_DEPTH_4D);
aabbProjection[stack] = leftSupportDist;
stackPool[stack] = me->m_left;
stack++;
dgAssert (stack < DG_STACK_DEPTH_4D);
}
} else {
dgAABBPointTree4dClump* const clump = (dgAABBPointTree4dClump*) me;
for (dgInt32 i = 0; i < clump->m_count; i ++) {
const dgHullVector& p = points[clump->m_indices[i]];
dgAssert (p.m_x >= clump->m_box[0].m_x);
dgAssert (p.m_x <= clump->m_box[1].m_x);
dgAssert (p.m_y >= clump->m_box[0].m_y);
dgAssert (p.m_y <= clump->m_box[1].m_y);
dgAssert (p.m_z >= clump->m_box[0].m_z);
dgAssert (p.m_z <= clump->m_box[1].m_z);
dgAssert (p.m_w >= clump->m_box[0].m_w);
dgAssert (p.m_w <= clump->m_box[1].m_w);
if (!p.m_mark) {
dgFloat64 dist = p.DotProduct4(dir).m_x;
if (dist > maxProj) {
maxProj = dist;
index = clump->m_indices[i];
}
} else {
clump->m_indices[i] = clump->m_indices[clump->m_count - 1];
clump->m_count = clump->m_count - 1;
i --;
}
}
if (clump->m_count == 0) {
dgAABBPointTree4d* const parent = clump->m_parent;
if (parent) {
dgAABBPointTree4d* const sibling = (parent->m_left != clump) ? parent->m_left : parent->m_right;
dgAssert (sibling != clump);
dgAABBPointTree4d* const grandParent = parent->m_parent;
if (grandParent) {
sibling->m_parent = grandParent;
if (grandParent->m_right == parent) {
grandParent->m_right = sibling;
} else {
grandParent->m_left = sibling;
}
} else {
sibling->m_parent = NULL;
*treePointer = sibling;
}
}
}
}
}
}
dgAssert (index != -1);
return index;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgInt32 count = 0;
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
const dgCollisionInstance* const polygonInstance = proxy.m_instance1;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix;
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_instance0;
dgVector normalInHull(hullMatrix.UnrotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(hullMatrix.TransformVector(pointInHull));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
return 0;
}
dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL)));
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
return 0;
}
dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector edgeBoundaryNormal(m_normal * e);
dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar());
plane = hullMatrix.TransformPlane(plane);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
//inside = false;
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
dgAssert(penetration >= dgFloat32(0.0f));
dgVector contactPoints[64];
dgAssert(penetration >= 0.0f);
dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step);
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (contactsOut[0].m_normal);
if (normal.DotProduct4(m_normal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector adjacentNormal (CalculateGlobalNormal (parentMesh, dgVector(&m_vertex[index * m_stride])));
if ((m_normal.DotProduct4(adjacentNormal).GetScalar() > dgFloat32(0.9995f))) {
normal = adjacentNormal;
} else {
dgVector dir0(adjacentNormal * m_normal);
dgVector dir1(adjacentNormal * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = adjacentNormal;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
return count;
}
dgInt32 dgConvexHull4d::InitVertexArray(dgHullVector* const points, const dgBigVector* const vertexCloud, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize)
{
for (dgInt32 i = 0; i < count; i ++) {
points[i] = vertexCloud[i];
points[i].m_index = i;
points[i].m_mark = 0;
}
dgSort(points, count, ConvexCompareVertex);
dgInt32 indexCount = 0;
for (int i = 1; i < count; i ++) {
for (; i < count; i ++) {
if (ConvexCompareVertex (&points[indexCount], &points[i], NULL)) {
indexCount ++;
points[indexCount] = points[i];
break;
}
}
}
count = indexCount + 1;
if (count < 4) {
m_count = 0;
return count;
}
dgAABBPointTree4d* tree = BuildTree (NULL, points, count, 0, (dgInt8**) &memoryPool, maxMemSize);
dgBigVector boxSize (tree->m_box[1] - tree->m_box[0]);
boxSize.m_w = dgFloat64 (0.0f);
m_diag = dgFloat32 (sqrt (boxSize.DotProduct4(boxSize).m_x));
m_points[4].m_x = dgFloat64 (0.0f);
dgHullVector* const convexPoints = &m_points[0];
dgStack<dgBigVector> normalArrayPool (256);
dgBigVector* const normalArray = &normalArrayPool[0];
dgInt32 normalCount = BuildNormalList (&normalArray[0]);
dgInt32 index = SupportVertex (&tree, points, normalArray[0]);
convexPoints[0] = points[index];
points[index].m_mark = 1;
bool validTetrahedrum = false;
dgBigVector e1 (dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f)) ;
for (dgInt32 i = 1; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
e1 = points[index] - convexPoints[0];
e1.m_w = dgFloat64 (0.0f);
dgFloat64 error2 = e1.DotProduct4(e1).m_x;
if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) {
convexPoints[1] = points[index];
points[index].m_mark = 1;
validTetrahedrum = true;
break;
}
}
if (!validTetrahedrum) {
m_count = 0;
return count;
}
dgInt32 bestIndex = -1;
dgFloat64 bestValue = dgFloat64 (1.0f);
validTetrahedrum = false;
dgFloat64 lenght2 = e1.DotProduct4(e1).m_x;
dgBigVector e2(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
for (dgInt32 i = 2; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
dgAssert (index < count);
e2 = points[index] - convexPoints[0];
e2.m_w = dgFloat64 (0.0f);
dgFloat64 den = e2.DotProduct4(e2).m_x;
if (fabs (den) > (dgFloat64 (1.0e-6f) * m_diag)) {
den = sqrt (lenght2 * den);
dgFloat64 num = e2.DotProduct4(e1).m_x;
dgFloat64 cosAngle = fabs (num / den);
if (cosAngle < bestValue) {
bestValue = cosAngle;
bestIndex = index;
}
if (cosAngle < 0.9f) {
break;
}
}
}
if (bestValue < dgFloat64 (0.999f)) {
convexPoints[2] = points[bestIndex];
points[bestIndex].m_mark = 1;
validTetrahedrum = true;
}
if (!validTetrahedrum) {
m_count = 0;
return count;
}
validTetrahedrum = false;
dgBigVector e3(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
for (dgInt32 i = 3; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
dgAssert (index < count);
e3 = points[index] - convexPoints[0];
e3.m_w = dgFloat64 (0.0f);
dgFloat64 volume = (e1 * e2) % e3;
if (fabs (volume) > (dgFloat64 (1.0e-4f) * m_diag * m_diag * m_diag)) {
convexPoints[3] = points[index];
points[index].m_mark = 1;
validTetrahedrum = true;
break;
}
}
m_count = 4;
if (!validTetrahedrum) {
m_count = 0;
}
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert(this == proxy.m_instance1->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const body0 = proxy.m_body0;
const dgBody* const body1 = proxy.m_body1;
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
return 0;
}
dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
if (den > dgFloat32 (1.0e-5f)) {
// this can actually happens
dgAssert(0);
return 0;
}
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32(1.0e10f);
dgMatrix polygonMatrix;
dgVector right (m_localPoly[1] - m_localPoly[0]);
polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = dgVector::m_wOne;
dgAssert (polygonMatrix.TestOrthogonal());
dgVector polyBoxP0(dgFloat32(1.0e15f));
dgVector polyBoxP1(dgFloat32(-1.0e15f));
for (dgInt32 i = 0; i < m_count; i++) {
dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
polyBoxP0 = polyBoxP0.GetMin(point);
polyBoxP1 = polyBoxP1.GetMax(point);
}
dgVector hullBoxP0;
dgVector hullBoxP1;
dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
dgVector minBox(polyBoxP0 - hullBoxP1);
dgVector maxBox(polyBoxP1 - hullBoxP0);
dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
dgInt32 count = 0;
if (distance < dgFloat32(1.0f)) {
bool inside = false;
dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
boxSize = sphereMag2.Sqrt();
dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;
dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half));
dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half)));
sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
boxSize = sphereMag2.Sqrt();
dgAssert (boxOrigin.m_w == 0.0f);
boxOrigin = boxOrigin | dgVector::m_wOne;
if (!proxy.m_intersectionTestOnly) {
inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e & dgVector::m_triplexMask);
dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
dgPlane plane(n, -(m_localPoly[i0] % n));
dgVector p0 (boxOrigin + n.Scale4 (param));
dgVector p1 (boxOrigin - n.Scale4 (param));
dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();
if ((size0 < 0.0f) && (size1 < 0.0f)) {
return 0;
}
if ((size0 * size1) < 0.0f) {
inside = false;
break;
}
i0 = i;
}
}
dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(boxOrigin, convexSphapeUmbra);
m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
dgVector p0 (matrixInstance0.TransformVector(pointInHull));
dgFloat32 timetoImpact = dgFloat32(0.0f);
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
timetoImpact = penetration / (relativeVelocity % m_normal);
dgAssert(timetoImpact >= dgFloat32(0.0f));
}
if (timetoImpact <= proxy.m_timestep) {
dgVector contactPoints[64];
contactJoint->m_closestDistance = penetration;
proxy.m_timestep = timetoImpact;
proxy.m_normal = m_normal;
proxy.m_closestPointBody0 = p0;
proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);
if (!proxy.m_intersectionTestOnly) {
pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);
dgVector step(relativeVelocity.Scale4(timetoImpact));
penetration = dgMax(penetration, dgFloat32(0.0f));
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
return count;
}
void 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;
}
dgVector dgCollisionBVH::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert(0);
return SupportVertex(dir, vertexIndex);
}
