dgFloat64 Determinant4x4 (const dgFloat64 matrix[4][4], dgFloat64* const error)
{
dgFloat64 sign = dgFloat64 (1.0f);
dgFloat64 det = dgFloat64 (0.0f);
dgFloat64 accError = dgFloat64 (0.0f);
for (dgInt32 i = 0; i < 4; i ++) {
dgFloat64 cofactor[3][3];
for (dgInt32 j = 0; j < 3; j ++) {
dgInt32 k0 = 0;
for (dgInt32 k = 0; k < 4; k ++) {
if (k != i) {
cofactor[j][k0] = matrix[j][k];
k0 ++;
}
}
}
dgFloat64 parcialError;
dgFloat64 minorDet = Determinant3x3 (cofactor, &parcialError);
accError += parcialError * Absolute (matrix[3][i]);
det += sign * minorDet * matrix[3][i];
sign *= dgFloat64 (-1.0f);
}
*error = accError;
return det;
}
dgFloat64 dgConvexHull4dTetraherum::Evalue (const dgHullVector* const pointArray, const dgBigVector& point) const
{
const dgBigVector &p0 = pointArray[m_faces[0].m_index[0]];
const dgBigVector &p1 = pointArray[m_faces[0].m_index[1]];
const dgBigVector &p2 = pointArray[m_faces[0].m_index[2]];
const dgBigVector &p3 = pointArray[m_faces[0].m_index[3]];
dgFloat64 matrix[4][4];
for (dgInt32 i = 0; i < 4; i ++) {
matrix[0][i] = p1[i] - p0[i];
matrix[1][i] = p2[i] - p0[i];
matrix[2][i] = p3[i] - p0[i];
matrix[3][i] = point[i] - p0[i];
}
dgFloat64 error;
dgFloat64 det = Determinant4x4 (matrix, &error);
dgFloat64 precision = dgFloat64 (1.0f) / dgFloat64 (1<<24);
dgFloat64 errbound = error * precision;
if (fabs(det) > errbound) {
return det;
}
dgGoogol exactMatrix[4][4];
for (dgInt32 i = 0; i < 4; i ++) {
exactMatrix[0][i] = dgGoogol(p1[i]) - dgGoogol(p0[i]);
exactMatrix[1][i] = dgGoogol(p2[i]) - dgGoogol(p0[i]);
exactMatrix[2][i] = dgGoogol(p3[i]) - dgGoogol(p0[i]);
exactMatrix[3][i] = dgGoogol(point[i]) - dgGoogol(p0[i]);
}
return Determinant4x4(exactMatrix);
}
void dgConvexHull4d::TessellateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgBigVector* const ouput, dgInt32& start) const
{
if (level) {
dgAssert (dgAbsf (p0 % p0 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p1 % p1 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p2 % p2 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
p01 = p01.Scale3 (dgRsqrt(p01 % p01));
p12 = p12.Scale3 (dgRsqrt(p12 % p12));
p20 = p20.Scale3 (dgRsqrt(p20 % p20));
dgAssert (dgAbsf (p01 % p01 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p12 % p12 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p20 % p20 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TessellateTriangle (level - 1, p0, p01, p20, count, ouput, start);
TessellateTriangle (level - 1, p1, p12, p01, count, ouput, start);
TessellateTriangle (level - 1, p2, p20, p12, count, ouput, start);
TessellateTriangle (level - 1, p01, p12, p20, count, ouput, start);
} else {
dgBigPlane n (p0, p1, p2);
n = n.Scale (dgFloat64(1.0f) / sqrt (n % n));
n.m_w = dgFloat64(0.0f);
ouput[start] = n;
start += 8;
count ++;
}
}
dgFloat64 dgSymmetricBiconjugateGradientSolve::Solve (dgInt32 size, dgFloat64 tolerance, dgFloat64* const x, const dgFloat64* const b) const
{
dgStack<dgFloat64> bufferR0(size);
dgStack<dgFloat64> bufferP0(size);
dgStack<dgFloat64> matrixTimesP0(size);
dgStack<dgFloat64> bufferConditionerInverseTimesR0(size);
dgFloat64* const r0 = &bufferR0[0];
dgFloat64* const p0 = &bufferP0[0];
dgFloat64* const MinvR0 = &bufferConditionerInverseTimesR0[0];
dgFloat64* const matrixP0 = &matrixTimesP0[0];
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
bool continueExecution = InversePrecoditionerTimeVector (p0, r0);
dgInt32 iter = 0;
dgFloat64 num = DotProduct (size, r0, p0);
dgFloat64 error2 = num;
for (dgInt32 j = 0; (j < size) && (error2 > tolerance) && continueExecution; j ++) {
MatrixTimeVector (matrixP0, p0);
dgFloat64 den = DotProduct (size, p0, matrixP0);
dgAssert (fabs(den) > dgFloat64 (0.0f));
dgFloat64 alpha = num / den;
ScaleAdd (size, x, x, alpha, p0);
if ((j % 50) != 49) {
ScaleAdd (size, r0, r0, -alpha, matrixP0);
} else {
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
}
//dgUnsigned64 xxx0 = dgGetTimeInMicrosenconds();
continueExecution = InversePrecoditionerTimeVector (MinvR0, r0);
//xxx0 = dgGetTimeInMicrosenconds() - xxx0;
//dgTrace (("%d\n", dgUnsigned64 (xxx0)));
dgFloat64 num1 = DotProduct (size, r0, MinvR0);
dgFloat64 beta = num1 / num;
ScaleAdd (size, p0, MinvR0, beta, p0);
num = DotProduct (size, r0, MinvR0);
iter ++;
error2 = num;
if (j > 10) {
error2 = dgFloat64 (0.0f);
for (dgInt32 i = 0; i < size; i ++) {
error2 = dgMax (error2, r0[i] * r0[i]);
}
}
}
dgAssert (iter < size);
return num;
}
dgConvexHull4d::dgListNode* dgConvexHull4d::FindFacingNode(const dgBigVector& vertex)
{
const dgHullVector* const hullVertexArray = &m_points[0];
dgListNode* bestNode = GetFirst();
dgConvexHull4dTetraherum* const tetra = &bestNode->GetInfo();
dgConvexHull4dTetraherum::dgTetrahedrumPlane plane (tetra->GetPlaneEquation (hullVertexArray));
dgFloat64 dist = plane.Evalue(vertex);
dgInt32 mark = IncMark();
tetra->SetMark(mark);
dgInt8 buffer[1024 * 2 * sizeof (dgFloat64)];
dgDownHeap<dgListNode*, dgFloat64> heap (buffer, sizeof (buffer));
heap.Push(bestNode, dist);
dgInt32 maxCount = heap.GetMaxCount() - 1;
dgInt32 releafCount = maxCount >> 3;
while (heap.GetCount()) {
dgListNode* const node = heap[0];
dgFloat64 dist = heap.Value();
if (dist > dgFloat64 (1.0e-5f)) {
return node;
}
heap.Pop();
dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
for (dgInt32 i = 0; i < 4; i ++) {
dgListNode* neigborghNode = tetra->m_faces[i].m_twin;
dgConvexHull4dTetraherum* const neighborgh = &neigborghNode->GetInfo();
if (neighborgh->GetMark() != mark) {
neighborgh->SetMark(mark);
if (heap.GetCount() >= maxCount) {
for (dgInt32 i = 0; i < releafCount; i ++) {
heap.Remove(heap.GetCount() - 1);
}
}
dgConvexHull4dTetraherum::dgTetrahedrumPlane plane (neighborgh->GetPlaneEquation (hullVertexArray));
heap.Push(neigborghNode, plane.Evalue(vertex));
}
}
}
for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
dgFloat64 dist = tetra->Evalue(hullVertexArray, vertex);
if (dist > dgFloat64(0.0f)) {
return node;
}
}
return NULL;
}
dgBigVector dgPointToTetrahedrumDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3)
{
const dgBigVector e10(p1 - p0);
const dgBigVector e20(p2 - p0);
const dgBigVector e30(p3 - p0);
const dgFloat64 d0 = sqrt(e10.DotProduct(e10).GetScalar());
if (d0 > dgFloat64(0.0f)) {
const dgFloat64 invd0 = dgFloat64(1.0f) / d0;
const dgFloat64 l10 = e20.DotProduct(e10).GetScalar() * invd0;
const dgFloat64 l20 = e30.DotProduct(e10).GetScalar() * invd0;
const dgFloat64 desc11 = e20.DotProduct(e20).GetScalar() - l10 * l10;
if (desc11 > dgFloat64(0.0f)) {
const dgFloat64 d1 = sqrt(desc11);
const dgFloat64 invd1 = dgFloat64(1.0f) / d1;
const dgFloat64 l21 = (e30.DotProduct(e20).GetScalar() - l20 * l10) * invd1;
const dgFloat64 desc22 = e30.DotProduct(e30).GetScalar() - l20 * l20 - l21 * l21;
if (desc22 > dgFloat64(0.0f)) {
dgBigVector p0Point (point - p0);
const dgFloat64 d2 = sqrt(desc22);
const dgFloat64 invd2 = dgFloat64(1.0f) / d2;
const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
const dgFloat64 b2 = e30.DotProduct(p0Point).GetScalar();
dgFloat64 u1 = b0 * invd0;
dgFloat64 u2 = (b1 - l10 * u1) * invd1;
dgFloat64 u3 = (b2 - l20 * u1 - l21 * u2) * invd2 * invd2;
u2 = (u2 - l21 * u3) * invd1;
u1 = (u1 - l10 * u2 - l20 * u3) * invd0;
if (u3 < dgFloat64(0.0f)) {
// this looks funny but it is correct
return dgPointToTriangleDistance(point, p0, p1, p2);
} else if (u2 < dgFloat64(0.0f)) {
return dgPointToTriangleDistance(point, p0, p1, p3);
} else if (u1 < dgFloat64(0.0f)) {
return dgPointToTriangleDistance(point, p0, p2, p3);
} else if (u1 + u2 + u3 > dgFloat64(1.0f)) {
return dgPointToTriangleDistance(point, p1, p2, p3);
}
return p0 + e10.Scale(u1) + e20.Scale(u2) + e30.Scale(u3);
}
}
}
// this is a degenerated tetra. this should never happens
dgAssert(0);
return p0;
}
dgBigVector dgPointToTriangleDistance(const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2)
{
const dgBigVector e10(p1 - p0);
const dgBigVector e20(p2 - p0);
const dgFloat64 a00 = e10.DotProduct(e10).GetScalar();
const dgFloat64 a11 = e20.DotProduct(e20).GetScalar();
const dgFloat64 a01 = e10.DotProduct(e20).GetScalar();
const dgFloat64 det = a00 * a11 - a01 * a01;
dgAssert(det >= dgFloat32(0.0f));
if (dgAbs(det) > dgFloat32(1.0e-24f)) {
dgBigVector p0Point (point - p0);
const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
const dgFloat64 beta = b1 * a00 - a01 * b0;
const dgFloat64 alpha = b0 * a11 - a01 * b1;
if (beta < dgFloat32(0.0f)) {
return dgPointToRayDistance (point, p0, p1);
} else if (alpha < dgFloat32(0.0f)) {
return dgPointToRayDistance (point, p0, p2);
} else if ((alpha + beta) > det) {
return dgPointToRayDistance (point, p1, p2);
}
return p0 + (e10.Scale(alpha) + e20.Scale(beta)).Scale(dgFloat64(1.0f) / det);
}
// this is a degenerated triangle. this should never happens
dgAssert(0);
return p0;
}
dgBigVector dgPointToRayDistance (const dgBigVector& point, const dgBigVector& ray_p0, const dgBigVector& ray_p1)
{
dgBigVector dp (ray_p1 - ray_p0);
dgAssert (dp.m_w == dgFloat32 (0.0f));
dgFloat64 t = dgClamp (dp.DotProduct3 (point - ray_p0) / dp.DotProduct3 (dp), dgFloat64(0.0f), dgFloat64 (1.0f));
return ray_p0 + dp.Scale (t);
}
DG_INLINE void CalculateBodyDiagonal(dgSkeletonGraph* const child)
{
dgAssert(child->m_joint);
dgSpatialMatrix copy;
copy.SetZero();
const dgInt32 dof = child->m_dof;
const dgSpatialMatrix& jacobianMatrix = child->m_jointJ;
const dgSpatialMatrix& childDiagonal = child->m_jointMass;
for (dgInt32 i = 0; i < dof ; i++) {
const dgSpatialVector& jacobian = jacobianMatrix[i];
for (dgInt32 j = 0; j < dof ; j++) {
dgAssert(dgAreEqual (childDiagonal[i][j], childDiagonal[j][i], dgFloat64(1.0e-5f)));
dgFloat64 val = childDiagonal[i][j];
jacobian.ScaleAdd(val, copy[j], copy[j]);
}
}
for (dgInt32 i = 0; i < dof; i++) {
const dgSpatialVector& Jacobian = copy[i];
const dgSpatialVector& JacobianTranspose = jacobianMatrix[i];
for (dgInt32 j = 0; j < 6; j++) {
dgFloat64 val = -Jacobian[j];
JacobianTranspose.ScaleAdd(val, m_bodyMass[j], m_bodyMass[j]);
}
}
}
dgFloat64 dgSymmetricBiconjugateGradientSolve::DotProduct (dgInt32 size, const dgFloat64 * const b, const dgFloat64 * const c) const
{
dgFloat64 product = dgFloat64 (0.0f);
for (dgInt32 i = 0; i < size; i ++)
product += b[i] * c[i];
return product;
}
void dgDelaunayTetrahedralization::RemoveUpperHull()
{
#ifdef _WIN32
dgUnsigned32 controlWorld = dgControlFP (0xffffffff, 0);
dgControlFP(_PC_53, _MCW_PC);
#endif
dgListNode* nextNode = NULL;
// const dgHullVector* const points = &m_points[0];
for (dgListNode* node = GetFirst(); node; node = nextNode)
{
nextNode = node->GetNext();
dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
tetra->SetMark(0);
// const dgBigVector &p0 = points[tetra->m_faces[0].m_index[0]];
// const dgBigVector &p1 = points[tetra->m_faces[0].m_index[1]];
// const dgBigVector &p2 = points[tetra->m_faces[0].m_index[2]];
// const dgBigVector &p3 = points[tetra->m_faces[0].m_otherVertex];
// dgFloat64 w = GetTetraVolume (p0, p1, p2, p3);
dgFloat64 w = GetTetraVolume(tetra);
if (w >= dgFloat64(0.0f))
{
DeleteFace(node);
}
}
#ifdef _WIN32
dgControlFP(controlWorld, _MCW_PC);
#endif
}
static void Statistics (dgObb& sphere, dgVector &eigenValues, dgVector &scaleVector, const dgFloat32 vertex[], dgInt32 vertexCount, dgInt32 stride)
{
dgBigVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgBigVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgBigVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
const dgFloat32* ptr = vertex;
for (dgInt32 i = 0; i < vertexCount; i ++) {
dgFloat32 x = ptr[0] * scaleVector.m_x;
dgFloat32 y = ptr[1] * scaleVector.m_y;
dgFloat32 z = ptr[2] * scaleVector.m_z;
ptr += stride;
massCenter += dgBigVector (x, y, z, dgFloat32 (0.0f));
var += dgBigVector (x * x, y * y, z * z, dgFloat32 (0.0f));
cov += dgBigVector (x * y, x * z, y * z, dgFloat32 (0.0f));
}
dgFloat64 k = dgFloat64 (1.0) / vertexCount;
var = var.Scale3 (k);
cov = cov.Scale3 (k);
massCenter = massCenter.Scale3 (k);
dgFloat64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
dgFloat64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
dgFloat64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
dgFloat64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
dgFloat64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
dgFloat64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector (dgFloat32(Ixx), dgFloat32(Ixy), dgFloat32(Ixz), dgFloat32 (0.0f));
sphere.m_up = dgVector (dgFloat32(Ixy), dgFloat32(Iyy), dgFloat32(Iyz), dgFloat32 (0.0f));
sphere.m_right = dgVector (dgFloat32(Ixz), dgFloat32(Iyz), dgFloat32(Izz), dgFloat32 (0.0f));
sphere.EigenVectors (eigenValues);
}
dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius)
{
dgVector p0Origin (p0 - origin);
if (p0Origin.DotProduct3(p0Origin) < (dgFloat32 (100.0f) * radius * radius)) {
dgVector dp (p1 - p0);
dgFloat32 a = dp.DotProduct3(dp);
dgFloat32 b = dgFloat32 (2.0f) * p0Origin.DotProduct3(dp);
dgFloat32 c = p0Origin.DotProduct3(p0Origin) - radius * radius;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = dgSqrt (desc);
dgFloat32 den = dgFloat32 (0.5f) / a;
dgFloat32 t0 = (-b + desc) * den;
dgFloat32 t1 = (-b - desc) * den;
if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
t0 = dgMin(t0, t1);
if (t0 <= dgFloat32 (1.0f)) {
return t0;
}
} else if (t0 >= dgFloat32 (0.0f)) {
if (t0 <= dgFloat32 (1.0f)) {
return t0;
}
} else {
if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
return t1;
}
}
}
} else {
dgBigVector p0Origin1 (p0Origin);
dgBigVector dp (p1 - p0);
dgFloat64 a = dp.DotProduct3(dp);
dgFloat64 b = dgFloat32 (2.0f) * p0Origin1.DotProduct3(dp);
dgFloat64 c = p0Origin1.DotProduct3(p0Origin1) - dgFloat64(radius) * radius;
dgFloat64 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = sqrt (desc);
dgFloat64 den = dgFloat32 (0.5f) / a;
dgFloat64 t0 = (-b + desc) * den;
dgFloat64 t1 = (-b - desc) * den;
if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
t0 = dgMin(t0, t1);
if (t0 <= dgFloat32 (1.0f)) {
return dgFloat32 (t0);
}
} else if (t0 >= dgFloat32 (0.0f)) {
if (t0 <= dgFloat32 (1.0f)) {
return dgFloat32 (t0);
}
} else {
if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
return dgFloat32 (t1);
}
}
}
}
return dgFloat32 (1.2f);
}
//dgFloat64 dgDelaunayTetrahedralization::GetTetraVolume (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) const
dgFloat64 dgDelaunayTetrahedralization::GetTetraVolume(
const dgConvexHull4dTetraherum* const tetra) const
{
// dgBigVector p1p0 (p1.Sub4(p0));
// dgBigVector p2p0 (p2.Sub4(p0));
// dgBigVector p3p0 (p3.Sub4(p0));
// dgBigVector normal (p1p0.CrossProduct4 (p2p0, p3p0));
// dgFloat64 det = normal.m_w;
const dgHullVector* const points = &m_points[0];
const dgBigVector &p0 = points[tetra->m_faces[0].m_index[0]];
const dgBigVector &p1 = points[tetra->m_faces[0].m_index[1]];
const dgBigVector &p2 = points[tetra->m_faces[0].m_index[2]];
const dgBigVector &p3 = points[tetra->m_faces[0].m_otherVertex];
dgFloat64 matrix[3][3];
for (dgInt32 i = 0; i < 3; i++)
{
matrix[0][i] = p2[i] - p0[i];
matrix[1][i] = p1[i] - p0[i];
matrix[2][i] = p3[i] - p0[i];
}
dgFloat64 error;
dgFloat64 det = Determinant3x3(matrix, &error);
dgFloat64 precision = dgFloat64(1.0f) / dgFloat64(1 << 24);
dgFloat64 errbound = error * precision;
if (fabs(det) > errbound)
{
return det;
}
dgGoogol exactMatrix[3][3];
for (dgInt32 i = 0; i < 3; i++)
{
exactMatrix[0][i] = dgGoogol(p2[i]) - dgGoogol(p0[i]);
exactMatrix[1][i] = dgGoogol(p1[i]) - dgGoogol(p0[i]);
exactMatrix[2][i] = dgGoogol(p3[i]) - dgGoogol(p0[i]);
}
dgGoogol exactDet(Determinant3x3(exactMatrix));
det = exactDet.GetAproximateValue();
return det;
}
bool dgCollisionConvexHull::CheckConvex (dgPolyhedra& polyhedra1, const dgBigVector* hullVertexArray) const
{
dgPolyhedra polyhedra(polyhedra1);
dgPolyhedra::Iterator iter (polyhedra);
dgBigVector center (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgInt32 count = 0;
dgInt32 mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
if (edge->m_mark < mark) {
count ++;
center += hullVertexArray[edge->m_incidentVertex];
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr = ptr->m_twin->m_next;
} while (ptr != edge);
}
}
center = center.Scale3 (dgFloat64 (1.0f) / dgFloat64 (count));
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgBigVector normal0 (FaceNormal (edge, hullVertexArray));
dgBigVector normal1 (FaceNormal (edge->m_twin, hullVertexArray));
dgBigPlane plane0 (normal0, - (normal0 % hullVertexArray[edge->m_incidentVertex]));
dgBigPlane plane1 (normal1, - (normal1 % hullVertexArray[edge->m_twin->m_incidentVertex]));
dgFloat64 test0 = plane0.Evalue(center);
if (test0 > dgFloat64 (1.0e-3f)) {
return false;
}
dgFloat64 test1 = plane1.Evalue(center);
// if (test1 > dgFloat64 (0.0f)) {
if (test1 > dgFloat64 (1.0e-3f)) {
return false;
}
}
return true;
}
dgBigVector LineTriangleIntersection (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& A, const dgBigVector& B, const dgBigVector& C)
{
dgHugeVector ph0 (p0);
dgHugeVector ph1 (p1);
dgHugeVector Ah (A);
dgHugeVector Bh (B);
dgHugeVector Ch (C);
dgHugeVector p1p0 (ph1 - ph0);
dgHugeVector Ap0 (Ah - ph0);
dgHugeVector Bp0 (Bh - ph0);
dgHugeVector Cp0 (Ch - ph0);
dgGoogol t0 ((Bp0 * Cp0) % p1p0);
dgFloat64 val0 = t0.GetAproximateValue();
if (val0 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol t1 ((Cp0 * Ap0) % p1p0);
dgFloat64 val1 = t1.GetAproximateValue();
if (val1 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol t2 ((Ap0 * Bp0) % p1p0);
dgFloat64 val2 = t2.GetAproximateValue();
if (val2 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol sum = t0 + t1 + t2;
dgFloat64 den = sum.GetAproximateValue();
#ifdef _DEBUG
dgBigVector testpoint (A.Scale (val0 / den) + B.Scale (val1 / den) + C.Scale(val2 / den));
dgFloat64 volume = ((B - A) * (C - A)) % (testpoint - A);
_ASSERTE (fabs (volume) < dgFloat64 (1.0e-12f));
#endif
return dgBigVector (val0 / den, val1 / den, val2 / den, dgFloat32 (0.0f));
}
dgFloat64 dgConvexHull4d::RoundToFloat (dgFloat64 val) const
{
dgInt32 exp;
dgFloat64 mantissa = frexp(val, &exp);
const dgFloat64 power = 1<<23;
const dgFloat64 invPower = dgFloat64 (1.0f) / power;
mantissa = floor(mantissa * power) * invPower;
dgFloat64 val1 = ldexp (mantissa, exp);
return val1;
}
dgBigVector dgCollisionConvexHull::FaceNormal (const dgEdge *face, const dgBigVector* const pool) const
{
const dgEdge* edge = face;
dgBigVector p0 (pool[edge->m_incidentVertex]);
edge = edge->m_next;
dgBigVector p1 (pool[edge->m_incidentVertex]);
dgBigVector e1 (p1 - p0);
dgBigVector normal (dgFloat32 (0.0f));
for (edge = edge->m_next; edge != face; edge = edge->m_next) {
dgBigVector p2 (pool[edge->m_incidentVertex]);
dgBigVector e2 (p2 - p0);
dgBigVector n1 (e1.CrossProduct(e2));
#ifdef _DEBUG
dgAssert(n1.m_w == dgFloat32(0.0f));
dgFloat64 mag = normal.DotProduct(n1).GetScalar();
dgAssert ( mag >= -dgFloat32 (0.1f));
#endif
normal += n1;
e1 = e2;
}
dgFloat64 den = sqrt (normal.DotProduct(normal).GetScalar()) + dgFloat64 (1.0e-24f);
normal = normal.Scale (dgFloat64 (1.0f)/ den);
#ifdef _DEBUG
edge = face;
dgBigVector e0 (pool[edge->m_incidentVertex] - pool[edge->m_prev->m_incidentVertex]);
do {
dgBigVector de1 (pool[edge->m_next->m_incidentVertex] - pool[edge->m_incidentVertex]);
dgBigVector dn1 (e0.CrossProduct(de1));
dgFloat64 x = normal.DotProduct(dn1).GetScalar();
dgAssert (x > -dgFloat64 (0.01f));
e0 = de1;
edge = edge->m_next;
} while (edge != face);
#endif
return normal;
}
dgBigVector dgCollisionConvexHull::FaceNormal (const dgEdge *face, const dgBigVector* const pool) const
{
const dgEdge *edge = face;
dgBigVector p0 (pool[edge->m_incidentVertex]);
edge = edge->m_next;
dgBigVector p1 (pool[edge->m_incidentVertex]);
dgBigVector e1 (p1 - p0);
dgBigVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (edge = edge->m_next; edge != face; edge = edge->m_next) {
dgBigVector p2 (pool[edge->m_incidentVertex]);
dgBigVector e2 (p2 - p0);
dgBigVector n1 (e1 * e2);
#ifdef _DEBUG
dgFloat64 mag = normal % n1;
_ASSERTE ( mag >= -dgFloat32 (0.1f));
#endif
normal += n1;
e1 = e2;
}
dgFloat64 den = sqrt (normal % normal) + dgFloat64 (1.0e-24f);
normal = normal.Scale (dgFloat64 (1.0f)/ den);
#ifdef _DEBUG
edge = face;
dgBigVector e0 (pool[edge->m_incidentVertex] - pool[edge->m_prev->m_incidentVertex]);
do {
dgBigVector e1 (pool[edge->m_next->m_incidentVertex] - pool[edge->m_incidentVertex]);
dgBigVector n1 (e0 * e1);
dgFloat64 x = normal % n1;
_ASSERTE (x > -dgFloat64 (0.01f));
e0 = e1;
edge = edge->m_next;
} while (edge != face);
#endif
return normal;
}
dgConvexHull4dTetraherum::dgTetrahedrumPlane::dgTetrahedrumPlane (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3)
:dgBigVector ((p1 - p0).CrossProduct4 (p2 - p0, p3 - p0))
{
dgBigVector& me = *this;
// dgAssert (me.DotProduct4(me) > dgFloat64 (1.0e-64f));
dgFloat64 invMag2 = dgFloat32 (0.0f);
// if (me.DotProduct4(me) > dgFloat64 (1.0e-64)) {
// if (me.DotProduct4(me) > dgFloat64 (1.0e-38)) {
dgFloat64 val = me.DotProduct4(me).m_x;
if (val > dgFloat64 (1.0e-38)) {
//invMag2 = dgFloat64 (1.0f) / sqrt (me.DotProduct4(me));
invMag2 = dgFloat64 (1.0f) / sqrt (val);
} else {
invMag2 = dgFloat32 (0.0f);
}
me.m_x *= invMag2;
me.m_y *= invMag2;
me.m_z *= invMag2;
me.m_w *= invMag2;
m_dist = - me.DotProduct4(p0).m_x;
}
dgGoogol Determinant4x4 (const dgGoogol matrix[4][4])
{
dgGoogol sign = dgFloat64 (1.0f);
dgGoogol det = dgFloat64 (0.0f);
dgGoogol negOne (dgFloat64 (-1.0f));
dgGoogol accError = dgFloat64 (0.0f);
for (dgInt32 i = 0; i < 4; i ++) {
dgGoogol cofactor[3][3];
for (dgInt32 j = 0; j < 3; j ++) {
dgInt32 k0 = 0;
for (dgInt32 k = 0; k < 4; k ++) {
if (k != i) {
cofactor[j][k0] = matrix[j][k];
k0 ++;
}
}
}
dgGoogol minorDet = Determinant3x3 (cofactor);
det = det + sign * minorDet * matrix[3][i];
sign = sign * negOne;
}
return det;
}
dgGoogol Determinant3x3 (const dgGoogol matrix[3][3])
{
dgGoogol negOne (dgFloat64 (-1.0f));
dgGoogol sign (dgFloat64 (-1.0f));
dgGoogol det = dgFloat64 (0.0f);
for (dgInt32 i = 0; i < 3; i ++) {
dgGoogol cofactor[2][2];
for (dgInt32 j = 0; j < 2; j ++) {
dgInt32 k0 = 0;
for (dgInt32 k = 0; k < 3; k ++) {
if (k != i) {
cofactor[j][k0] = matrix[j][k];
k0 ++;
}
}
}
dgGoogol minorDet (Determinant2x2 (cofactor));
det = det + sign * minorDet * matrix[2][i];
sign = sign * negOne;
}
return det;
}
dgGoogol::dgGoogol(dgFloat64 value)
:m_sign(0)
,m_exponent(0)
{
dgInt32 exp;
dgFloat64 mantissa = fabs (frexp(value, &exp));
m_exponent = dgInt16 (exp);
m_sign = (value >= 0) ? 0 : 1;
memset (m_mantissa, 0, sizeof (m_mantissa));
m_mantissa[0] = (dgInt64 (dgFloat64 (dgUnsigned64(1)<<62) * mantissa));
// it looks like GCC have problems with this
//dgAssert (m_mantissa[0] >= 0);
dgAssert ((m_mantissa[0] & dgUnsigned64(1)<<63) == 0);
}
void dgConvexHull4dTetraherum::Init (const dgHullVector* const points, dgInt32 v0, dgInt32 v1, dgInt32 v2, dgInt32 v3)
{
//{0, 1, 2, 3},
//{3, 0, 2, 1},
//{3, 2, 1, 0},
//{3, 1, 0, 2}
m_faces[0].m_index[0] = v0;
m_faces[0].m_index[1] = v1;
m_faces[0].m_index[2] = v2;
m_faces[0].m_index[3] = v3;
m_faces[1].m_index[0] = v3;
m_faces[1].m_index[1] = v0;
m_faces[1].m_index[2] = v2;
m_faces[1].m_index[3] = v1;
m_faces[2].m_index[0] = v3;
m_faces[2].m_index[1] = v2;
m_faces[2].m_index[2] = v1;
m_faces[2].m_index[3] = v0;
m_faces[3].m_index[0] = v3;
m_faces[3].m_index[1] = v1;
m_faces[3].m_index[2] = v0;
m_faces[3].m_index[3] = v2;
SetMark (0);
for (dgInt32 i = 0; i < 4; i ++) {
m_faces[i].m_twin = NULL;
}
#ifdef _DEBUG
dgBigVector p1p0 (points[v1] - points[v0]);
dgBigVector p2p0 (points[v2] - points[v0]);
dgBigVector p3p0 (points[v3] - points[v0]);
dgBigVector normal (p1p0.CrossProduct4(p2p0, p3p0));
dgFloat64 volume = normal.DotProduct4(normal).m_x;
dgAssert (volume > dgFloat64 (0.0f));
#endif
}
void dgPolygonSoupDatabaseBuilder::End(bool optimize)
{
Optimize(optimize);
// build the normal array and adjacency array
// calculate all face the normals
dgInt32 indexCount = 0;
m_normalPoints[m_faceCount].m_x = dgFloat64(0.0f);
for (dgInt32 i = 0; i < m_faceCount; i++)
{
dgInt32 faceIndexCount = m_faceVertexCount[i];
dgInt32* const ptr = &m_vertexIndex[indexCount + 1];
dgBigVector v0(&m_vertexPoints[ptr[0]].m_x);
dgBigVector v1(&m_vertexPoints[ptr[1]].m_x);
dgBigVector e0(v1 - v0);
dgBigVector normal(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
for (dgInt32 j = 2; j < faceIndexCount - 1; j++)
{
dgBigVector v2(&m_vertexPoints[ptr[j]].m_x);
dgBigVector e1(v2 - v0);
normal += e0 * e1;
e0 = e1;
}
normal = normal.Scale(dgRsqrt (normal % normal));
m_normalPoints[i].m_x = normal.m_x;
m_normalPoints[i].m_y = normal.m_y;
m_normalPoints[i].m_z = normal.m_z;
indexCount += faceIndexCount;
}
// compress normals array
m_normalIndex[m_faceCount] = 0;
m_normalCount = dgVertexListToIndexList(&m_normalPoints[0].m_x,
sizeof(dgBigVector), 3, m_faceCount, &m_normalIndex[0],
dgFloat32(1.0e-4f));
}
dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
{
dgAssert (m_p0.m_w == dgFloat32 (0.0f));
dgAssert (m_p1.m_w == dgFloat32 (0.0f));
if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) {
dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32));
dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask);
dgBigVector p0(m_p0);
dgBigVector p0v0(v0 - p0);
dgBigVector diff(m_diff);
dgBigVector normal(faceNormal);
dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar();
if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) {
dgBigVector p (p0 + diff.Scale (tOut));
dgBigVector unitDir(m_unitDir);
for (dgInt32 i = 0; i < indexCount; i++) {
dgInt32 i2 = indexArray[i] * stride;
dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask);
dgBigVector edge0(p - v0);
dgBigVector edge1(v1 - v0);
dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar();
if (area < dgFloat32 (0.0f)) {
return 1.2f;
}
v0 = v1;
}
return dgFloat32(tOut);
}
}
return dgFloat32 (1.2f);
}
dgBigVector dgConvexHull4dTetraherum::CircumSphereCenter (const dgHullVector* const pointArray) const
{
dgGoogol matrix[4][4];
dgBigVector points[4];
points[0] = pointArray[m_faces[0].m_index[0]];
points[1] = pointArray[m_faces[0].m_index[1]];
points[2] = pointArray[m_faces[0].m_index[2]];
points[3] = pointArray[m_faces[0].m_index[3]];
for (dgInt32 i = 0; i < 4; i ++) {
for (dgInt32 j = 0; j < 3; j ++) {
matrix[i][j] = dgGoogol (points[i][j]);
}
matrix[i][3] = dgGoogol (1.0f);
}
dgGoogol det (Determinant4x4(matrix));
dgFloat64 invDen = dgFloat64 (1.0f) / (dgFloat64(det) * dgFloat64 (2.0f));
dgBigVector centerOut;
dgFloat64 sign = dgFloat64 (1.0f);
for (dgInt32 k = 0; k < 3; k ++) {
for (dgInt32 i = 0; i < 4; i ++) {
matrix[i][0] = dgGoogol (points[i][3]);
for (dgInt32 j = 0; j < 2; j ++) {
dgInt32 j1 = (j < k) ? j : j + 1;
matrix[i][j + 1] = dgGoogol (points[i][j1]);
}
matrix[i][3] = dgGoogol (1.0f);
}
dgGoogol det (Determinant4x4(matrix));
dgFloat64 val = dgFloat64 (det) * sign;
sign *= dgFloat64 (-1.0f);
centerOut[k] = val * invDen;
}
centerOut[3] = dgFloat32 (0.0f);
return centerOut;
}
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 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;
}
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;
}
