void dgPolygonMeshDesc::SortFaceArray ()
{
dgInt32 stride = 8;
if (m_faceCount >= 8) {
dgInt32 stack[DG_MAX_COLLIDING_FACES][2];
stack[0][0] = 0;
stack[0][1] = m_faceCount - 1;
dgInt32 stackIndex = 1;
while (stackIndex) {
stackIndex --;
dgInt32 lo = stack[stackIndex][0];
dgInt32 hi = stack[stackIndex][1];
if ((hi - lo) > stride) {
dgInt32 i = lo;
dgInt32 j = hi;
dgFloat32 dist = m_hitDistance[(lo + hi) >> 1];
do {
while (m_hitDistance[i] < dist) i ++;
while (m_hitDistance[j] > dist) j --;
if (i <= j) {
dgSwap (m_hitDistance[i], m_hitDistance[j]);
dgSwap (m_faceIndexStart[i], m_faceIndexStart[j]);
dgSwap (m_faceIndexCount[i], m_faceIndexCount[j]);
i++;
j--;
}
} while (i <= j);
if (i < hi) {
stack[stackIndex][0] = i;
stack[stackIndex][1] = hi;
stackIndex ++;
}
if (lo < j) {
stack[stackIndex][0] = lo;
stack[stackIndex][1] = j;
stackIndex ++;
}
dgAssert (stackIndex < dgInt32 (sizeof (stack) / (2 * sizeof (stack[0][0]))));
}
}
dgFloat32 dgCollisionBox::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
dgAssert (localP0.m_w == dgFloat32 (0.0f));
dgAssert (localP1.m_w == dgFloat32 (0.0f));
dgInt32 index = 0;
dgFloat32 signDir = dgFloat32 (0.0f);
dgFloat32 tmin = dgFloat32 (0.0f);
dgFloat32 tmax = dgFloat32 (1.0f);
for (dgInt32 i = 0; i < 3; i++) {
dgFloat32 dp = localP1[i] - localP0[i];
if (dgAbsf (dp) < dgFloat32 (1.0e-8f)) {
if (localP0[i] <= m_size[1][i] || localP0[i] >= m_size[0][i]) {
return dgFloat32 (1.2f);
}
} else {
dp = dgFloat32 (1.0f) / dp;
dgFloat32 t1 = (m_size[1][i] - localP0[i]) * dp;
dgFloat32 t2 = (m_size[0][i] - localP0[i]) * dp;
dgFloat32 sign = dgFloat32 (-1.0f);
if (t1 > t2) {
sign = 1;
dgSwap(t1, t2);
}
if (t1 > tmin) {
signDir = sign;
index = i;
tmin = t1;
}
if (t2 < tmax) {
tmax = t2;
}
if (tmin > tmax) {
return dgFloat32 (1.2f);
}
}
}
if (tmin > dgFloat32 (0.0f)) {
dgAssert (tmin <= 1.0f);
contactOut.m_normal = dgVector (dgFloat32 (0.0f));
contactOut.m_normal[index] = signDir;
//contactOut.m_userId = SetUserDataID();
} else {
tmin = dgFloat32 (1.2f);
}
return tmin;
}
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgForcePair& accel)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
const dgInt32 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
//dgSpatialVector& accel = force.m_joint;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
//dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar();
dgFloat32 force = row->m_force;
if ((force >= row->m_lowerBoundFrictionCoefficent) && (force <= row->m_upperBoundFrictionCoefficent)) {
dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) +
row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular));
residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal();
accel.m_joint[i] = -residual.GetScalar();
} else {
dgAssert (0);
dgSwap (m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i --;
m_dof --;
clampedValue --;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
dgFloat32 dgRayCastBox (const dgVector& p0, const dgVector& p1, const dgVector& boxP0, const dgVector& boxP1, dgVector& normalOut)
{
dgInt32 index = 0;
dgFloat32 signDir = dgFloat32 (0.0f);
dgFloat32 tmin = dgFloat32 (0.0f);
dgFloat32 tmax = dgFloat32 (1.0f);
//dgVector size (boxP1 - boxP0);
for (dgInt32 i = 0; i < 3; i++) {
dgFloat32 dp = p1[i] - p0[i];
if (dgAbs (dp) < dgFloat32 (1.0e-8f)) {
if (p0[i] <= boxP0[i] || p0[i] >= boxP1[i]) {
return dgFloat32 (1.2f);
}
} else {
dp = dgFloat32 (1.0f) / dp;
dgFloat32 t1 = (boxP0[i] - p0[i]) * dp;
dgFloat32 t2 = (boxP1[i] - p0[i]) * dp;
dgFloat32 sign = dgFloat32 (-1.0f);
if (t1 > t2) {
sign = 1;
dgSwap(t1, t2);
}
if (t1 > tmin) {
signDir = sign;
index = i;
tmin = t1;
}
if (t2 < tmax) {
tmax = t2;
}
if (tmin > tmax) {
return dgFloat32 (1.2f);
}
}
}
if (tmin > dgFloat32 (0.0f)) {
dgAssert (tmin < 1.0f);
normalOut = dgVector (dgFloat32 (0.0f));
normalOut[index] = signDir;
} else {
tmin = dgFloat32 (1.2f);
}
return tmin;
}
bool dgWorld::AreBodyConnectedByJoints (dgBody* const originSrc, dgBody* const targetSrc)
{
#define DG_QEUEU_SIZE 1024
dgBody* queue[DG_QEUEU_SIZE];
m_genericLRUMark ++;
dgBody* origin1 = originSrc;
dgBody* target1 = targetSrc;
if (origin1->GetInvMass().m_w == dgFloat32 (0.0f)) {
dgSwap (origin1, target1);
}
dgAssert (origin1->GetInvMass().m_w != dgFloat32 (0.0f));
dgBody* const origin = origin1;
dgBody* const target = target1;
dgInt32 end = 1;
dgInt32 start = 0;
queue[0] = origin;
origin->m_genericLRUMark = m_genericLRUMark;
while (start != end) {
dgBody* const originVar = queue[start];
start ++;
start &= (DG_QEUEU_SIZE - 1);
for (dgBodyMasterListRow::dgListNode* jointNode = originVar->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
dgBodyMasterListCell& cell = jointNode->GetInfo();
dgBody* const body = cell.m_bodyNode;
if (body->m_genericLRUMark != m_genericLRUMark) {
dgConstraint* const constraint = cell.m_joint;
if (constraint->GetId() != dgConstraint::m_contactConstraint) {
if (body == target) {
return true;
}
body->m_genericLRUMark = m_genericLRUMark;
queue[end] = body;
end ++;
end &= (DG_QEUEU_SIZE - 1);
}
}
}
}
return false;
}
DG_INLINE void UpdateFactorizeLCP(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgForcePair& force, dgForcePair& accel)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
dgFloat32 f = force.m_joint[i] + row->m_force;
if (f < row->m_lowerBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
} else if (f > row->m_upperBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE void Factorize(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32 (0.0f)) {
GetInertia();
}
for (dgInt32 i = 0; i < sizeof (m_sourceJacobianIndex) / sizeof (m_sourceJacobianIndex[0]); i ++) {
m_sourceJacobianIndex[i] = dgInt8 (i);
}
if (m_joint) {
dgAssert (m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
if (((row->m_lowerBoundFrictionCoefficent < dgFloat32 (-1.0e9f)) && row->m_upperBoundFrictionCoefficent > dgFloat32 (1.0e9f))) {
//m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof ++;
} else {
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[count - 1]);
i--;
count--;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
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;
}
void dgContact::SwapBodies()
{
dgSwap (m_body0, m_body1);
dgSwap (m_link0, m_link1);
}
dgCollisionDeformableMesh::dgDeformableNode* dgCollisionDeformableMesh::BuildTopDown (dgInt32 count, dgDeformableNode* const children, dgDeformableNode* const parent)
{
dgDeformableNode* root = NULL;
if (count == 1) {
root = children;
root->m_left = NULL;
root->m_right = NULL;
root->m_parent = parent;
} else if (count == 2) {
root = &m_nodesMemory[m_nodesCount];
m_nodesCount ++;
root->m_indexStart = -1;
root->m_parent = parent;
root->m_left = BuildTopDown (1, children, root);
root->m_right = BuildTopDown (1, &children[1], root);
root->m_surfaceArea = CalculateSurfaceArea (root->m_left, root->m_right, root->m_minBox, root->m_maxBox);
} else {
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 < count; i ++) {
const dgDeformableNode* const node = &children[i];
dgVector p ((node->m_minBox + node->m_maxBox).Scale3 (0.5f));
median += p;
varian += p.CompProduct3 (p);
}
varian = varian.Scale3 (dgFloat32 (count)) - median.CompProduct3(median);
dgInt32 index = 0;
dgFloat32 maxVarian = dgFloat32 (-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 (count));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = count - 1;
do {
for (; i0 <= i1; i0 ++) {
const dgDeformableNode* const node = &children[i0];
dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f);
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
const dgDeformableNode* const node = &children[i1];
dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f);
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(children[i0], children[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 > 0){
i0 --;
}
if ((i0 + 1) >= count) {
i0 = count - 2;
}
dgInt32 spliteCount = i0 + 1;
root = &m_nodesMemory[m_nodesCount];
m_nodesCount ++;
root->m_indexStart = -1;
root->m_parent = parent;
root->m_left = BuildTopDown (spliteCount, children, root);
root->m_right = BuildTopDown (count - spliteCount, &children[spliteCount], root);
root->m_surfaceArea = CalculateSurfaceArea (root->m_left, root->m_right, root->m_minBox, root->m_maxBox);
}
return root;
}
void dgCollisionDeformableSolidMesh::CreateClusters (dgInt32 count, dgFloat32 overlapingWidth)
{
if (m_clusterPosit) {
dgFree (m_clusterAqqInv);
dgFree (m_clusterRotationInitialGuess);
dgFree (m_clusterCom0);
dgFree (m_clusterMass);
dgFree (m_clusterWeight);
dgFree (m_clusterPosit);
}
if (count <= 1) {
// special case of only one region
m_clustersCount = 1;
m_clusterAqqInv = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterRotationInitialGuess = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterCom0 = (dgVector*) dgMallocStack (sizeof (dgVector) * m_clustersCount);
m_clusterMass = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * m_clustersCount);
m_clusterWeight = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * (m_particles.m_count));
m_clusterPosit = (dgInt32*) dgMallocStack (sizeof (dgInt32) * (m_particles.m_count));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_clusterPositStart[i] = i;
m_clusterPosit[i] = 0;
m_clusterWeight[i] = dgFloat32 (1.0f);
}
m_clusterPositStart[m_particles.m_count] = m_particles.m_count;
} else {
dgClusterBuilder clusterList (GetAllocator());
dgCluster& cluster = clusterList.Append()->GetInfo();
cluster.m_count = m_particles.m_count;
cluster.m_points = ((dgInt32*) dgMallocStack (sizeof (dgUnsigned32) * cluster.m_count));
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
cluster.m_points[i] = i;
}
dgClusterBuilder::dgListNode* nextNode = NULL;
for (dgClusterBuilder::dgListNode* node = clusterList.GetFirst(); node && (clusterList.GetCount() < count); node = nextNode) {
dgCluster& cluster = node->GetInfo();
dgVector median (dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
median += p;
varian += p.CompProduct4(p);
}
varian = varian.Scale4 (dgFloat32 (cluster.m_count)) - median.CompProduct4(median);
dgInt32 index = 0;
dgFloat32 maxVarian = dgFloat32 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale4 (dgFloat32 (1.0f) / dgFloat32 (cluster.m_count));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = cluster.m_count - 1;
do {
for (; i0 <= i1; i0 ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i0]];
if (p[index] > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
const dgVector& p = m_shapePosit[cluster.m_points[i1]];
if (p[index] < test) {
break;
}
}
if (i0 < i1) {
dgSwap(cluster.m_points[i0], cluster.m_points[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
dgInt32 middle = i0 + 1;
dgInt32 leftSideOvelap = 0;
dgFloat32 leftBarrier = test + overlapingWidth;
for (dgInt32 i = middle; i < cluster.m_count; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
leftSideOvelap += (p[index] < leftBarrier) ? 1 : 0;
}
dgInt32 rightSideOvelap = 0;
dgFloat32 rightBarrier = test - overlapingWidth;
for (dgInt32 i = 0; i < middle; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
rightSideOvelap += (p[index] > rightBarrier) ? 1 : 0;
}
if (rightSideOvelap || leftSideOvelap) {
dgCluster& leftCluster = clusterList.Append()->GetInfo();
leftCluster.m_count = middle + leftSideOvelap;
leftCluster.m_points = ((dgInt32*) dgMallocStack (sizeof (dgUnsigned32) * leftCluster.m_count));
dgInt32 j = 0;
for (dgInt32 i = 0; i < middle; i ++) {
leftCluster.m_points[j] = cluster.m_points[i];
j ++;
}
for (dgInt32 i = middle; i < cluster.m_count; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
if (p[index] < leftBarrier) {
leftCluster.m_points[j] = cluster.m_points[i];
j ++;
dgAssert (j <= leftCluster.m_count);
}
}
j = 0;
dgCluster& rightCluster = clusterList.Append()->GetInfo();
rightCluster.m_count = cluster.m_count - middle + rightSideOvelap;
rightCluster.m_points = ((dgInt32*) dgMallocStack (sizeof (dgUnsigned32) * rightCluster.m_count));
for (dgInt32 i = middle; i < cluster.m_count; i ++) {
rightCluster.m_points[j] = cluster.m_points[i];
j ++;
}
for (dgInt32 i = 0; i < middle; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
if (p[index] > rightBarrier) {
rightCluster.m_points[j] = cluster.m_points[i];
j ++;
dgAssert (j <= rightCluster.m_count);
}
}
nextNode = node->GetNext();
clusterList.Remove(node);
} else {
dgAssert(0);
}
}
m_clustersCount = clusterList.GetCount();
m_clusterAqqInv = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterRotationInitialGuess = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterCom0 = (dgVector*) dgMallocStack (sizeof (dgVector) * m_clustersCount);
m_clusterMass = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * m_clustersCount);
dgInt32 poolSize = 0;
dgStack<dgInt32> particleClusterCountPool(m_particles.m_count);
dgInt32* const particleClusterCount = &particleClusterCountPool[0];
memset (particleClusterCount, 0, particleClusterCountPool.GetSizeInBytes());
for (dgClusterBuilder::dgListNode* node = clusterList.GetFirst(); node; node = node->GetNext()) {
dgCluster& cluster = node->GetInfo();
poolSize += cluster.m_count;
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
dgInt32 j = cluster.m_points[i];
particleClusterCount[j] ++;
}
}
m_clusterWeight = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * poolSize);
m_clusterPosit = (dgInt32*) dgMallocStack (sizeof (dgInt32) * poolSize);
dgInt32 acc = 0;
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgInt32 count = particleClusterCount[i];
m_clusterPositStart[i] = acc;
particleClusterCount[i] = acc;
acc += count;
}
m_clusterPositStart[m_particles.m_count] = acc;
dgInt32 clusterIndex = 0;
for (dgClusterBuilder::dgListNode* node = clusterList.GetFirst(); node; node = node->GetNext()) {
dgCluster& cluster = node->GetInfo();
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
dgInt32 j = cluster.m_points[i];
dgInt32 base = particleClusterCount[j];
m_clusterPosit[base] = clusterIndex;
m_clusterWeight[base] += dgFloat32 (1.0f);
particleClusterCount[j] ++;
}
clusterIndex ++;
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
dgAssert (count);
dgFloat32 weight = dgFloat32 (1.0f) / count;
for (dgInt32 j = 0; j < count; j ++) {
m_clusterWeight[start + j] = weight;
}
}
}
}
