B3D.NewtonDynamics. Initial Import.

newtondynamics.mod/NewtonDynamics/AUTHORS

@@ -0,0 +1,9 @@
+The following authors have all licensed their contributions to Newton Dynamics
+under the licensing terms detailed in LICENSE.
+
+(Authors keep copyright of their contributions, of course; they just grant
+a license to everyone to use it as detailed in LICENSE.)
+
+* Julio Jerez <newton*at*newtondynamics.com>
+* Alain Suero <alain*at*newtondynamics.com>
+* Manfred Manik Nerurkar <nerurkar*at*made-apps.biz> (copyright owned by MADE, GmbH)

newtondynamics.mod/NewtonDynamics/LICENSE

@@ -0,0 +1,23 @@
+Newton zlib license
+Copyright (c) <2003-2011> 
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not
+claim that you wrote the original software. If you use this software
+in a product, an acknowledgment in the product documentation would be
+appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source distribution.
+
+Julio Jerez and Alain Suero
+http://www.gzip.org/zlib/zlib_license.html

newtondynamics.mod/NewtonDynamics/README.md

@@ -0,0 +1,20 @@
+![emscripten logo](https://raw.githubusercontent.com/MADEAPPS/newton-dynamics/master/applications/newtonLogo/newtonLogo.png)
+
+Newton dynamics is a realistic, cross-platform physics simulation library. It can easily be integrated into games and game engines and provides top of it's class performance and simulation stability.
+Ongoing developement and a permissive license makes Newton Dynamics a top choice for all kinds of projects from scientific projects to game engines.
+
+* Links to **demos**, **tutorial**, **FAQ**, etc: <https://github.com/MADEAPPS/newton-dynamics/wiki>
+* Main project page: <http://newtondynamics.com>
+* Forums and public discussion: <http://newtondynamics.com/forum/index.php>
+
+
+License
+=======
+Newton Dynamics is licensed under the zlib open source license, with little if any practical difference between them.
+
+See `LICENSE` for the full content of the licenses.
+
+Authors
+=======
+Newton Dynamics is a project of Julio Jerez and Alain Suero. See `AUTHORS` for a full list of contributors.
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dLinearAlgebra.cpp

@@ -0,0 +1,875 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+#include "dLinearAlgebra.h"
+
+
+#define COMPLEMENTARITY_VEL_DAMP			dFloat(100.0f)
+#define COMPLEMENTARITY_POS_DAMP			dFloat(1500.0f)
+#define COMPLEMENTARITY_PSD_DAMP_TOL		dFloat(1.0e-4f)
+#define COMPLEMENTARITY_STACK_ENTRIES		64
+#define COMPLEMENTARITY_MAX_FRICTION_BOUND	dFloat(1.0e15f)
+#define COMPLEMENTARITY_MIN_FRICTION_BOUND	-COMPLEMENTARITY_MAX_FRICTION_BOUND
+
+
+
+
+void dSymmetricBiconjugateGradientSolve::ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const
+{
+	for (int i = 0; i < size; i ++) {
+		a[i] = b[i] + scale * c[i];
+	}
+}
+
+void dSymmetricBiconjugateGradientSolve::Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const
+{
+	for (int i = 0; i < size; i ++) {
+		a[i] = b[i] - c[i];
+	}
+}
+
+dFloat64 dSymmetricBiconjugateGradientSolve::DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const
+{
+	dFloat64 product = dFloat64 (0.0f);
+	for (int i = 0; i < size; i ++) {
+		product += b[i] * c[i];
+	}
+	return product;
+}
+
+dFloat64 dSymmetricBiconjugateGradientSolve::Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const
+{
+	dFloat64* const r0 = new dFloat64 [size];
+	dFloat64* const p0 = new dFloat64 [size];
+	dFloat64* const MinvR0 = new dFloat64 [size];
+	dFloat64* const matrixP0 = new dFloat64 [size];
+
+	MatrixTimeVector (matrixP0, x);
+	Sub(size, r0, b, matrixP0);
+	bool continueExecution = InversePrecoditionerTimeVector (p0, r0);
+
+	int iter = 0;
+	dFloat64 num = DotProduct (size, r0, p0);
+	dFloat64 error2 = num;
+	for (int j = 0; (j < size) && (error2 > tolerance) && continueExecution; j ++) {
+
+		MatrixTimeVector (matrixP0, p0);
+		dFloat64 den = DotProduct (size, p0, matrixP0);
+
+		dAssert (fabs(den) > dFloat64 (0.0f));
+		dFloat64 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);
+        }
+
+		continueExecution = InversePrecoditionerTimeVector (MinvR0, r0);
+
+		dFloat64 num1 = DotProduct (size, r0, MinvR0);
+		dFloat64 beta = num1 / num;
+		ScaleAdd (size, p0, MinvR0, beta, p0);
+		num = DotProduct (size, r0, MinvR0);
+		iter ++;
+		error2 = num;
+		if (j > 10) {
+			error2 = dFloat64 (0.0f);
+			for (int i = 0; i < size; i ++) {
+				error2 = dMax (error2, r0[i] * r0[i]);
+			}
+		}
+	}
+
+	delete[] matrixP0;
+	delete[] MinvR0;
+	delete[] p0;
+	delete[] r0;
+
+	dAssert (iter < size);
+	return num;
+}
+
+
+dComplemtaritySolver::dBodyState::dBodyState()
+	:m_matrix(dGetIdentityMatrix())
+	,m_localFrame(dGetIdentityMatrix())
+	,m_inertia(dGetZeroMatrix())
+	,m_invInertia(dGetZeroMatrix())
+	,m_localInertia (0.0f, 0.0f, 0.0f, 0.0f)
+	,m_localInvInertia(0.0f, 0.0f, 0.0f, 0.0f)
+	,m_veloc(0.0f, 0.0f, 0.0f, 0.0f)
+	,m_omega(0.0f, 0.0f, 0.0f, 0.0f)
+	,m_externalForce(0.0f, 0.0f, 0.0f, 0.0f)
+	,m_externalTorque(0.0f, 0.0f, 0.0f, 0.0f)
+	,m_globalCentreOfMass(0.0f, 0.0f, 0.0f, 0.0f)
+	,m_mass(0.0f)
+	,m_invMass(0.0f)
+	,m_myIndex(0)
+{
+}
+
+
+const dVector& dComplemtaritySolver::dBodyState::GetOmega() const
+{
+	return m_omega;
+}
+
+const dVector& dComplemtaritySolver::dBodyState::GetVelocity() const
+{
+	return m_veloc;
+}
+
+
+dFloat dComplemtaritySolver::dBodyState::GetMass () const
+{
+	return m_mass;
+}
+
+dFloat dComplemtaritySolver::dBodyState::GetInvMass () const
+{
+	return m_invMass;
+}
+
+void dComplemtaritySolver::dBodyState::SetMass (dFloat mass)
+{
+	m_mass = mass;
+	m_invMass = 1.0f / mass;
+}
+
+void dComplemtaritySolver::dBodyState::SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz)
+{
+	m_localInertia[0] = Ixx;
+	m_localInertia[1] =	Iyy;
+	m_localInertia[2] =	Izz;
+	m_localInvInertia[0] = 1.0f / Ixx;
+	m_localInvInertia[1] = 1.0f / Iyy;
+	m_localInvInertia[2] = 1.0f / Izz;
+}
+
+void dComplemtaritySolver::dBodyState::GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const
+{
+	Ixx = m_localInertia[0];
+	Iyy = m_localInertia[1];
+	Izz = m_localInertia[2];
+}
+
+
+
+void dComplemtaritySolver::dBodyState::SetMatrix (const dMatrix& matrix)
+{
+	m_matrix = matrix;
+	m_globalCentreOfMass = m_matrix.TransformVector(m_localFrame.m_posit);
+}
+
+
+const dMatrix& dComplemtaritySolver::dBodyState::GetMatrix () const
+{
+	return m_matrix;
+}
+
+void dComplemtaritySolver::dBodyState::SetLocalMatrix (const dMatrix& matrix)
+{
+	m_localFrame = matrix;
+	m_globalCentreOfMass = m_matrix.TransformVector(m_localFrame.m_posit);
+}
+
+const dMatrix& dComplemtaritySolver::dBodyState::GetLocalMatrix () const
+{
+	return m_localFrame;
+}
+
+const dVector& dComplemtaritySolver::dBodyState::GetCenterOfMass () const
+{
+	return m_globalCentreOfMass;
+}
+
+void dComplemtaritySolver::dBodyState::SetVeloc (const dVector& veloc)
+{
+	m_veloc = veloc;
+}
+
+void dComplemtaritySolver::dBodyState::SetOmega (const dVector& omega)
+{
+	m_omega = omega;
+}
+
+void dComplemtaritySolver::dBodyState::SetForce (const dVector& force)
+{
+	m_externalForce = force;
+}
+
+void dComplemtaritySolver::dBodyState::SetTorque (const dVector& torque)
+{
+	m_externalTorque = torque;
+}
+
+const dVector& dComplemtaritySolver::dBodyState::GetForce () const
+{
+	return m_externalForce;
+}
+
+const dVector& dComplemtaritySolver::dBodyState::GetTorque () const
+{
+	return m_externalTorque;
+}
+
+
+void dComplemtaritySolver::dBodyState::UpdateInertia()
+{
+	dMatrix tmpMatrix (dGetZeroMatrix());
+
+	tmpMatrix[0] = m_localInertia.CompProduct (dVector (m_matrix[0][0], m_matrix[1][0], m_matrix[2][0], 0.0f));
+	tmpMatrix[1] = m_localInertia.CompProduct (dVector (m_matrix[0][1], m_matrix[1][1], m_matrix[2][1], 0.0f));
+	tmpMatrix[2] = m_localInertia.CompProduct (dVector (m_matrix[0][2], m_matrix[1][2], m_matrix[2][2], 0.0f));
+	m_inertia = tmpMatrix * m_matrix;
+
+	tmpMatrix[0] = m_localInvInertia.CompProduct (dVector (m_matrix[0][0], m_matrix[1][0], m_matrix[2][0], 0.0f));
+	tmpMatrix[1] = m_localInvInertia.CompProduct (dVector (m_matrix[0][1], m_matrix[1][1], m_matrix[2][1], 0.0f));
+	tmpMatrix[2] = m_localInvInertia.CompProduct (dVector (m_matrix[0][2], m_matrix[1][2], m_matrix[2][2], 0.0f));
+	m_invInertia = tmpMatrix * m_matrix;
+}
+
+void dComplemtaritySolver::dBodyState::IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque)
+{
+	dVector accel (force.Scale (m_invMass));
+	dVector alpha (m_invInertia.RotateVector(torque));
+	m_veloc += accel.Scale (timestep);
+	m_omega += alpha.Scale (timestep);
+}
+
+void dComplemtaritySolver::dBodyState::IntegrateVelocity (dFloat timestep)
+{
+	const dFloat D_MAX_ANGLE_STEP = dFloat (45.0f * 3.141592f / 180.0f);
+	const dFloat D_ANGULAR_TOL = dFloat (0.0125f * 3.141592f / 180.0f);
+
+	m_globalCentreOfMass += m_veloc.Scale (timestep); 
+	while (((m_omega % m_omega) * timestep * timestep) > (D_MAX_ANGLE_STEP * D_MAX_ANGLE_STEP)) {
+		m_omega = m_omega.Scale (dFloat (0.8f));
+	}
+
+	// this is correct
+	dFloat omegaMag2 = m_omega % m_omega;
+	if (omegaMag2 > (D_ANGULAR_TOL * D_ANGULAR_TOL)) {
+		dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
+		dVector omegaAxis (m_omega.Scale (invOmegaMag));
+		dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
+		dQuaternion rotation (omegaAxis, omegaAngle);
+		dQuaternion rotMatrix (m_matrix);
+		rotMatrix = rotMatrix * rotation;
+		rotMatrix.Scale( 1.0f / dSqrt (rotMatrix.DotProduct (rotMatrix)));
+		m_matrix = dMatrix (rotMatrix, m_matrix.m_posit);
+	}
+
+	m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localFrame.m_posit);
+
+#ifdef _DEBUG
+	int j0 = 1;
+	int j1 = 2;
+	for (int i = 0; i < 3; i ++) {
+		dAssert (m_matrix[i][3] == 0.0f);
+		dFloat val = m_matrix[i] % m_matrix[i];
+		dAssert (dAbs (val - 1.0f) < 1.0e-5f);
+		dVector tmp (m_matrix[j0] * m_matrix[j1]);
+		val = tmp % m_matrix[i];
+		dAssert (dAbs (val - 1.0f) < 1.0e-5f);
+		j0 = j1;
+		j1 = i;
+	}
+#endif
+}
+
+
+void dComplemtaritySolver::dBodyState::ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega)
+{
+	dVector accel = (m_veloc - veloc).Scale(invTimestep);
+	dVector alpha = (m_omega - omega).Scale(invTimestep);
+
+	m_externalForce = accel.Scale(m_mass);
+	alpha = m_matrix.UnrotateVector(alpha);
+	m_externalTorque = m_matrix.RotateVector(alpha.CompProduct(m_localInertia));
+}
+
+
+
+void dComplemtaritySolver::dBilateralJoint::Init(dBodyState* const state0, dBodyState* const state1)
+{
+	m_start = 0;
+	m_count = 0;
+	memset (m_rowIsMotor, 0, sizeof (m_rowIsMotor));
+	memset (m_motorAcceleration, 0, sizeof (m_motorAcceleration));
+	memset (m_jointFeebackForce, 0, sizeof (m_jointFeebackForce));
+
+	m_state0 = state0;
+	m_state1 = state1;
+}
+
+
+void dComplemtaritySolver::dBilateralJoint::InitPointParam (dPointDerivativeParam& param, const dVector& pivot) const
+{
+	dAssert (m_state0);
+	dAssert (m_state1);
+
+	param.m_posit0 = pivot;
+	param.m_r0 = pivot - m_state0->m_globalCentreOfMass;
+	param.m_veloc0 = m_state0->m_omega * param.m_r0;
+	param.m_centripetal0 = m_state0->m_omega * param.m_veloc0;
+	param.m_veloc0 += m_state0->m_veloc;
+
+	param.m_posit1 = pivot;
+	param.m_r1 = pivot - m_state1->m_globalCentreOfMass;
+	param.m_veloc1 = m_state1->m_omega * param.m_r1;
+	param.m_centripetal1 = m_state1->m_omega * param.m_veloc1;
+	param.m_veloc1 += m_state1->m_veloc;
+}
+
+
+void dComplemtaritySolver::dBilateralJoint::CalculatePointDerivative (dParamInfo* const constraintParams, const dVector& dir, const dPointDerivativeParam& param)
+{
+	int index = constraintParams->m_count;
+
+	dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_IM0; 
+	dVector r0CrossDir (param.m_r0 * dir);
+	jacobian0.m_linear[0] = dir.m_x;
+	jacobian0.m_linear[1] = dir.m_y;
+	jacobian0.m_linear[2] = dir.m_z;
+	jacobian0.m_linear[3] = dFloat (0.0f);
+	jacobian0.m_angular[0] = r0CrossDir.m_x;
+	jacobian0.m_angular[1] = r0CrossDir.m_y;
+	jacobian0.m_angular[2] = r0CrossDir.m_z;
+	jacobian0.m_angular[3] = 0.0f;
+
+	dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_IM1; 
+	dVector r1CrossDir (dir * param.m_r1);
+	jacobian1.m_linear[0] = -dir.m_x;
+	jacobian1.m_linear[1] = -dir.m_y;
+	jacobian1.m_linear[2] = -dir.m_z;
+	jacobian1.m_linear[3] = dFloat (0.0f);
+	jacobian1.m_angular[0] = r1CrossDir.m_x;
+	jacobian1.m_angular[1] = r1CrossDir.m_y;
+	jacobian1.m_angular[2] = r1CrossDir.m_z;
+	jacobian1.m_angular[3] = 0.0f;
+
+	dVector velocError (param.m_veloc1 - param.m_veloc0);
+	dVector positError (param.m_posit1 - param.m_posit0);
+	dVector centrError (param.m_centripetal1 - param.m_centripetal0);
+
+	dFloat relPosit = positError % dir;
+	dFloat relVeloc = velocError % dir;
+	dFloat relCentr = centrError % dir; 
+
+	dFloat dt = constraintParams->m_timestep;
+	dFloat ks = COMPLEMENTARITY_POS_DAMP;
+	dFloat kd = COMPLEMENTARITY_VEL_DAMP;
+	dFloat ksd = dt * ks;
+	dFloat num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
+	dFloat den = dFloat (1.0f) + dt * kd + dt * ksd;
+	dFloat accelError = num / den;
+
+	m_rowIsMotor[index] = false;
+	m_motorAcceleration[index] = 0.0f;
+	constraintParams->m_jointAccel[index] = accelError + relCentr;
+	constraintParams->m_jointLowFriction[index] = COMPLEMENTARITY_MIN_FRICTION_BOUND;
+	constraintParams->m_jointHighFriction[index] = COMPLEMENTARITY_MAX_FRICTION_BOUND;
+	constraintParams->m_count = index + 1;
+}
+
+
+void dComplemtaritySolver::dBilateralJoint::AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle)
+{
+	int index = constraintParams->m_count;
+	dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_IM0; 
+
+	jacobian0.m_linear[0] = 0.0f;
+	jacobian0.m_linear[1] = 0.0f;
+	jacobian0.m_linear[2] = 0.0f;
+	jacobian0.m_linear[3] = 0.0f;
+	jacobian0.m_angular[0] = dir.m_x;
+	jacobian0.m_angular[1] = dir.m_y;
+	jacobian0.m_angular[2] = dir.m_z;
+	jacobian0.m_angular[3] = 0.0f;
+
+	dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_IM1; 
+	jacobian1.m_linear[0] = 0.0f;
+	jacobian1.m_linear[1] = 0.0f;
+	jacobian1.m_linear[2] = 0.0f;
+	jacobian1.m_linear[3] = 0.0f;
+	jacobian1.m_angular[0] = -dir.m_x;
+	jacobian1.m_angular[1] = -dir.m_y;
+	jacobian1.m_angular[2] = -dir.m_z;
+	jacobian1.m_angular[3] = 0.0f;
+
+	const dVector& omega0 = m_state0->m_omega;
+	const dVector& omega1 = m_state1->m_omega;
+	dFloat omegaError = (omega1 - omega0) % dir;
+
+
+	//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
+	dFloat dt = constraintParams->m_timestep;
+	dFloat ks = COMPLEMENTARITY_POS_DAMP;
+	dFloat kd = COMPLEMENTARITY_VEL_DAMP;
+	dFloat ksd = dt * ks;
+	dFloat num = ks * jointAngle + kd * omegaError + ksd * omegaError;
+	dFloat den = dFloat (1.0f) + dt * kd + dt * ksd;
+	dFloat alphaError = num / den;
+
+	m_rowIsMotor[index] = false;
+	m_motorAcceleration[index] = 0.0f;
+	constraintParams->m_jointAccel[index] = alphaError;
+	constraintParams->m_jointLowFriction[index] = COMPLEMENTARITY_MIN_FRICTION_BOUND;
+	constraintParams->m_jointHighFriction[index] = COMPLEMENTARITY_MAX_FRICTION_BOUND;
+	constraintParams->m_count = index + 1;
+}
+
+
+void dComplemtaritySolver::dBilateralJoint::AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir0, const dVector& dir1, dFloat accelerationRatio)
+{
+	int index = constraintParams->m_count;
+	dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_IM0; 
+
+	jacobian0.m_linear[0] = 0.0f;
+	jacobian0.m_linear[1] = 0.0f;
+	jacobian0.m_linear[2] = 0.0f;
+	jacobian0.m_linear[3] = 0.0f;
+	jacobian0.m_angular[0] = dir0.m_x;
+	jacobian0.m_angular[1] = dir0.m_y;
+	jacobian0.m_angular[2] = dir0.m_z;
+	jacobian0.m_angular[3] = 0.0f;
+
+	dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_IM1; 
+	jacobian1.m_linear[0] = 0.0f;
+	jacobian1.m_linear[1] = 0.0f;
+	jacobian1.m_linear[2] = 0.0f;
+	jacobian1.m_linear[3] = 0.0f;
+	jacobian1.m_angular[0] = dir1.m_x;
+	jacobian1.m_angular[1] = dir1.m_y;
+	jacobian1.m_angular[2] = dir1.m_z;
+	jacobian1.m_angular[3] = 0.0f;
+
+	m_rowIsMotor[index] = true;
+	m_motorAcceleration[index] = accelerationRatio;
+	constraintParams->m_jointAccel[index] = 0.0f;
+	constraintParams->m_jointLowFriction[index] = COMPLEMENTARITY_MIN_FRICTION_BOUND;
+	constraintParams->m_jointHighFriction[index] = COMPLEMENTARITY_MAX_FRICTION_BOUND;
+	constraintParams->m_count = index + 1;
+}
+
+void dComplemtaritySolver::dBilateralJoint::AddLinearRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir)
+{
+	dPointDerivativeParam pointData;
+	InitPointParam (pointData, pivot);
+	CalculatePointDerivative (constraintParams, dir, pointData); 
+}
+
+
+void dComplemtaritySolver::dBilateralJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
+{
+	dJacobianColum* const jacobianColElements = params->m_colMatrix;
+	dJacobianPair* const jacobianRowElements = params->m_rowMatrix;
+
+	const dVector& bodyVeloc0 = m_state0->m_veloc;
+	const dVector& bodyOmega0 = m_state0->m_omega;
+	const dVector& bodyVeloc1 = m_state1->m_veloc;
+	const dVector& bodyOmega1 = m_state1->m_omega;
+
+	dFloat timestep = params->m_timeStep;
+	dFloat kd = COMPLEMENTARITY_VEL_DAMP * dFloat (4.0f);
+	dFloat ks = COMPLEMENTARITY_POS_DAMP * dFloat (0.25f);
+	for (int k = 0; k < params->m_rowsCount; k ++) {
+		if (m_rowIsMotor[k]) {
+			jacobianColElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianColElements[k].m_deltaAccel;
+		} else {
+			const dJacobianPair& Jt = jacobianRowElements[k];
+			dVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0) +
+				Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0) + 
+				Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1) +
+				Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1));
+
+			dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
+			dFloat aRel = jacobianColElements[k].m_deltaAccel;
+			dFloat ksd = timestep * ks;
+			dFloat relPosit = 0.0f - vRel * timestep * params->m_firstPassCoefFlag;
+
+			dFloat num = ks * relPosit - kd * vRel - ksd * vRel;
+			dFloat den = dFloat (1.0f) + timestep * kd + timestep * ksd;
+			dFloat aRelErr = num / den;
+			jacobianColElements[k].m_coordenateAccel = aRelErr + aRel;
+		}
+	}
+}
+
+
+
+
+
+int dComplemtaritySolver::dFrictionLessContactJoint::CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy)
+{
+	if (contactA->m_point[0] < contactB->m_point[0]) {
+		return -1;
+	} else if (contactA->m_point[0] > contactB->m_point[0]) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+
+int dComplemtaritySolver::dFrictionLessContactJoint::ReduceContacts (int count, dContact* const contacts, dFloat tol)
+{
+	int mask[D_MAX_PLACEMENT_CONTACTS];
+	int index = 0;
+	int packContacts = 0;
+	dFloat window = tol;
+	dFloat window2 = window * window;
+	memset (mask, 0, size_t (count));
+	dSort (contacts, count, CompareContact, NULL);
+	dAssert (count <= D_MAX_PLACEMENT_CONTACTS);
+	for (int i = 0; i < count; i ++) {
+		if (!mask[i]) {
+			dFloat val = contacts[i].m_point[index] + window;
+			for (int j = i + 1; (j < count) && (contacts[j].m_point[index] < val) ; j ++) {
+				if (!mask[j]) {
+					dVector dp (contacts[j].m_point - contacts[i].m_point);
+					dFloat dist2 = dp % dp;
+					if (dist2 < window2) {
+						mask[j] = 1;
+						packContacts = 1;
+					}
+				}
+			}
+		}
+	}
+
+	if (packContacts) {
+		int j = 0;
+		for (int i = 0; i < count; i ++) {
+			dAssert (i < D_MAX_PLACEMENT_CONTACTS);
+			if (!mask[i]) {
+				contacts[j] = contacts[i];
+				j ++;
+			}
+		}
+		count = j;
+	}
+	return count;
+}
+
+void dComplemtaritySolver::dFrictionLessContactJoint::SetContacts (int count, dContact* const contacts, dFloat restitution)
+{
+	dFloat tol = 5.0e-3f;
+	count = ReduceContacts(count, contacts, tol);
+	while (count > D_MAX_PRAM_INFO_SIZE) {
+		tol *= 2.0f; 
+		count = ReduceContacts(count, contacts, tol);
+	}
+
+	m_count = count;
+	m_restitution = restitution;
+	memcpy (m_contacts, contacts, count * sizeof (dContact));
+}
+
+void dComplemtaritySolver::dFrictionLessContactJoint::JacobianDerivative (dParamInfo* const constraintParams)
+{
+	for (int i = 0; i < m_count; i ++) {
+		dPointDerivativeParam pointData;
+		InitPointParam (pointData, m_contacts[i].m_point);
+		CalculatePointDerivative (constraintParams, m_contacts[i].m_normal, pointData);
+
+		dVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
+
+		//dFloat restitution = 0.05f;
+		dFloat relVelocErr = velocError % m_contacts[i].m_normal;
+		dFloat penetration = 0.0f;
+		dFloat penetrationStiffness = 0.0f;
+		dFloat penetrationVeloc = penetration * penetrationStiffness;
+
+		if (relVelocErr > dFloat(1.0e-3f)) {
+			relVelocErr *= (m_restitution + dFloat (1.0f));
+		}
+
+		constraintParams->m_jointLowFriction[i] = dFloat (0.0f);
+		constraintParams->m_jointAccel[i] = dMax (dFloat (-4.0f), relVelocErr + penetrationVeloc) * constraintParams->m_timestepInv;
+	}
+}
+
+
+void dComplemtaritySolver::dFrictionLessContactJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
+{
+	dJacobianPair* const rowMatrix = params->m_rowMatrix;
+	dJacobianColum* const jacobianColElements = params->m_colMatrix;
+
+	const dVector& bodyVeloc0 = m_state0->GetVelocity();
+	const dVector& bodyOmega0 = m_state0->GetOmega();
+	const dVector& bodyVeloc1 = m_state1->GetVelocity();
+	const dVector& bodyOmega1 = m_state1->GetOmega();
+
+	int count = params->m_rowsCount;
+
+	dAssert (params->m_timeStep > dFloat (0.0f));
+	for (int k = 0; k < count; k ++) {
+		const dJacobianPair& Jt = rowMatrix[k];
+		dJacobianColum& element = jacobianColElements[k];
+
+		dVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0) + Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0) + Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1) + Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1));
+
+		dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
+		dFloat aRel = element.m_deltaAccel;
+		//dFloat restitution = (vRel <= 0.0f) ? 1.05f : 1.0f;
+		dFloat restitution = (vRel <= 0.0f) ? (dFloat (1.0f) + m_restitution) : dFloat(1.0f);
+		
+		vRel *= restitution;
+		vRel = dMin (dFloat (4.0f), vRel);
+		element.m_coordenateAccel = (aRel - vRel * params->m_invTimeStep);
+	}
+}
+
+int dComplemtaritySolver::BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount)
+{
+	int rowCount = 0;
+
+	dParamInfo constraintParams;
+	constraintParams.m_timestep = timestep;
+	constraintParams.m_timestepInv = 1.0f / timestep;
+
+	// calculate Jacobian derivative for each active joint	
+	for (int j = 0; j < jointCount; j ++) {
+		dBilateralJoint* const joint = jointArray[j];
+		constraintParams.m_count = 0;
+		joint->JacobianDerivative (&constraintParams); 
+
+		int dofCount = constraintParams.m_count;
+		joint->m_count = dofCount;
+		joint->m_start = rowCount;
+
+		// complete the derivative matrix for this joint
+		int index = joint->m_start;
+		dBodyState* const state0 = joint->m_state0;
+		dBodyState* const state1 = joint->m_state1;
+
+		const dMatrix& invInertia0 = state0->m_invInertia;
+		const dMatrix& invInertia1 = state1->m_invInertia;
+
+		dFloat invMass0 = state0->m_invMass;
+		dFloat invMass1 = state1->m_invMass;
+		dFloat weight = 0.9f;
+
+		for (int i = 0; i < dofCount; i ++) {
+			dJacobianPair* const row = &jacobianArray[index];
+			dJacobianColum* const col = &jacobianColumnArray[index];
+			jacobianArray[rowCount] = constraintParams.m_jacobians[i]; 
+
+			dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
+			dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
+			dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
+			dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);
+
+			dVector tmpDiag (JMinvIM0linear.CompProduct(row->m_jacobian_IM0.m_linear) + JMinvIM0angular.CompProduct(row->m_jacobian_IM0.m_angular) + JMinvIM1linear.CompProduct(row->m_jacobian_IM1.m_linear) + JMinvIM1angular.CompProduct(row->m_jacobian_IM1.m_angular));
+			dVector tmpAccel (JMinvIM0linear.CompProduct (state0->m_externalForce) + JMinvIM0angular.CompProduct(state0->m_externalTorque) + JMinvIM1linear.CompProduct (state1->m_externalForce) + JMinvIM1angular.CompProduct(state1->m_externalTorque));
+			dFloat extenalAcceleration = -(tmpAccel[0] + tmpAccel[1] + tmpAccel[2]);
+
+			col->m_diagDamp = 1.0f;
+			col->m_coordenateAccel = constraintParams.m_jointAccel[i];
+			col->m_jointLowFriction = constraintParams.m_jointLowFriction[i];
+			col->m_jointHighFriction = constraintParams.m_jointHighFriction[i];
+
+			col->m_deltaAccel = extenalAcceleration;
+			col->m_coordenateAccel += extenalAcceleration;
+
+			col->m_force = joint->m_jointFeebackForce[i] * weight;
+
+			dFloat stiffness = COMPLEMENTARITY_PSD_DAMP_TOL * col->m_diagDamp;
+			dFloat diag = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
+			dAssert (diag > dFloat (0.0f));
+			col->m_diagDamp = diag * stiffness;
+
+			diag *= (dFloat(1.0f) + stiffness);
+			col->m_invDJMinvJt = dFloat(1.0f) / diag;
+			index ++;
+			rowCount ++;
+			dAssert (rowCount < maxRowCount);
+		}
+	}
+	return rowCount;
+}
+
+void dComplemtaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
+{
+	dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
+	dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];
+
+	int stateIndex = 0;
+	dVector zero(dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f));
+	for (int i = 0; i < bodyCount; i ++) {
+		dBodyState* const state = bodyArray[i];
+		stateVeloc[stateIndex].m_linear = state->m_veloc;
+		stateVeloc[stateIndex].m_angular = state->m_omega;
+
+		internalForces[stateIndex].m_linear = zero;
+		internalForces[stateIndex].m_angular = zero;
+
+		state->m_myIndex = stateIndex;
+		stateIndex ++;
+		dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
+	}
+
+	for (int i = 0; i < jointCount; i ++) {
+		dJacobian y0;
+		dJacobian y1;
+		y0.m_linear = zero;
+		y0.m_angular = zero;
+		y1.m_linear = zero;
+		y1.m_angular = zero;
+		dBilateralJoint* const constraint = jointArray[i];
+		int first = constraint->m_start;
+		int count = constraint->m_count;
+		for (int j = 0; j < count; j ++) { 
+			dJacobianPair* const row = &jacobianArray[j + first];
+			const dJacobianColum* const col = &jacobianColumnArray[j + first];
+			dFloat val = col->m_force; 
+			y0.m_linear += row->m_jacobian_IM0.m_linear.Scale(val);
+			y0.m_angular += row->m_jacobian_IM0.m_angular.Scale(val);
+			y1.m_linear += row->m_jacobian_IM1.m_linear.Scale(val);
+			y1.m_angular += row->m_jacobian_IM1.m_angular.Scale(val);
+		}
+		int m0 = constraint->m_state0->m_myIndex;
+		int m1 = constraint->m_state1->m_myIndex;
+		internalForces[m0].m_linear += y0.m_linear;
+		internalForces[m0].m_angular += y0.m_angular;
+		internalForces[m1].m_linear += y1.m_linear;
+		internalForces[m1].m_angular += y1.m_angular;
+	}
+
+
+	dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
+	dFloat invStep = dFloat (0.25f);
+	dFloat timestep = timestepSrc * invStep;
+	dFloat invTimestep = invTimestepSrc * dFloat (4.0f);
+
+	int maxPasses = 5;
+	dFloat firstPassCoef = dFloat (0.0f);
+	dFloat maxAccNorm = dFloat (1.0e-2f);
+
+	for (int step = 0; step < 4; step ++) {
+		dJointAccelerationDecriptor joindDesc;
+		joindDesc.m_timeStep = timestep;
+		joindDesc.m_invTimeStep = invTimestep;
+		joindDesc.m_firstPassCoefFlag = firstPassCoef;
+
+		for (int i = 0; i < jointCount; i ++) {
+			dBilateralJoint* const constraint = jointArray[i];
+			joindDesc.m_rowsCount = constraint->m_count;
+			joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
+			joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
+			constraint->JointAccelerations (&joindDesc);
+		}
+		firstPassCoef = dFloat (1.0f);
+
+		dFloat accNorm = dFloat (1.0e10f);
+		for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
+			accNorm = dFloat (0.0f);
+			for (int i = 0; i < jointCount; i ++) {
+
+				dBilateralJoint* const constraint = jointArray[i];
+				int index = constraint->m_start;
+				int rowsCount = constraint->m_count;
+				int m0 = constraint->m_state0->m_myIndex;
+				int m1 = constraint->m_state1->m_myIndex;
+
+				dVector linearM0 (internalForces[m0].m_linear);
+				dVector angularM0 (internalForces[m0].m_angular);
+				dVector linearM1 (internalForces[m1].m_linear);
+				dVector angularM1 (internalForces[m1].m_angular);
+
+				dBodyState* const state0 = constraint->m_state0;
+				dBodyState* const state1 = constraint->m_state1;
+				const dMatrix& invInertia0 = state0->m_invInertia;
+				const dMatrix& invInertia1 = state1->m_invInertia;
+				dFloat invMass0 = state0->m_invMass;
+				dFloat invMass1 = state1->m_invMass;
+
+				for (int k = 0; k < rowsCount; k ++) {
+					dJacobianPair* const row = &jacobianArray[index];
+					dJacobianColum* const col = &jacobianColumnArray[index];
+
+					dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
+					dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
+					dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
+					dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);
+					dVector acc (JMinvIM0linear.CompProduct(linearM0) + JMinvIM0angular.CompProduct(angularM0) + JMinvIM1linear.CompProduct(linearM1) + JMinvIM1angular.CompProduct(angularM1));
+
+					dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
+					dFloat f = col->m_force + col->m_invDJMinvJt * a;
+
+					dFloat lowerFrictionForce = col->m_jointLowFriction;
+					dFloat upperFrictionForce = col->m_jointHighFriction;
+
+					if (f > upperFrictionForce) {
+						a = dFloat (0.0f);
+						f = upperFrictionForce;
+					} else if (f < lowerFrictionForce) {
+						a = dFloat (0.0f);
+						f = lowerFrictionForce;
+					}
+
+					accNorm = dMax (accNorm, dAbs (a));
+					dFloat prevValue = f - col->m_force;
+					col->m_force = f;
+
+					linearM0 += row->m_jacobian_IM0.m_linear.Scale (prevValue);
+					angularM0 += row->m_jacobian_IM0.m_angular.Scale (prevValue);
+					linearM1 += row->m_jacobian_IM1.m_linear.Scale (prevValue);
+					angularM1 += row->m_jacobian_IM1.m_angular.Scale (prevValue);
+					index ++;
+				}
+				internalForces[m0].m_linear = linearM0;
+				internalForces[m0].m_angular = angularM0;
+				internalForces[m1].m_linear = linearM1;
+				internalForces[m1].m_angular = angularM1;
+			}
+		}
+
+		for (int i = 0; i < bodyCount; i ++) {
+			dBodyState* const state = bodyArray[i];
+			//int index = state->m_myIndex;
+			dAssert (state->m_myIndex == i);
+			dVector force (state->m_externalForce + internalForces[i].m_linear);
+			dVector torque (state->m_externalTorque + internalForces[i].m_angular);
+			state->IntegrateForce(timestep, force, torque);
+		}
+	}
+
+	for (int i = 0; i < jointCount; i ++) {
+		dBilateralJoint* const constraint = jointArray[i];
+		int first = constraint->m_start;
+		int count = constraint->m_count;
+		for (int j = 0; j < count; j ++) { 
+			const dJacobianColum* const col = &jacobianColumnArray[j + first];
+			dFloat val = col->m_force; 
+			constraint->m_jointFeebackForce[j] = val;
+		}
+	}
+
+	for (int i = 0; i < jointCount; i ++) {
+		dBilateralJoint* const constraint = jointArray[i];
+		constraint->UpdateSolverForces (jacobianArray);
+	}
+
+	for (int i = 0; i < bodyCount; i ++) {
+		dBodyState* const state = bodyArray[i];
+		dAssert (state->m_myIndex == i);
+		state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
+	}
+}
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dLinearAlgebra.h

@@ -0,0 +1,267 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+#include "dVector.h"
+#include "dMatrix.h"
+#include "dQuaternion.h"
+
+
+#ifndef __D_LINEAR_ALGEBRA_H__
+#define __D_LINEAR_ALGEBRA_H__
+
+#ifdef _MSC_VER
+	#pragma warning (disable: 4100) //unreferenced formal parameter
+#endif
+
+
+#define D_MAX_PRAM_INFO_SIZE		16 
+#define D_MAX_PLACEMENT_CONTACTS	128
+
+class dSymmetricBiconjugateGradientSolve
+{
+	public:
+	dSymmetricBiconjugateGradientSolve () {}
+	virtual ~dSymmetricBiconjugateGradientSolve () {}	
+
+	virtual dFloat64 Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const;
+
+	protected:
+	virtual void MatrixTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
+	virtual bool InversePrecoditionerTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
+
+	private:
+	dFloat64 DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const;
+	void ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const;
+	void Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const;
+};
+
+class dComplemtaritySolver 
+{
+	public:
+	class dBodyState;
+	class dBilateralJoint;
+
+
+	class dContact
+	{
+		public:
+		dVector m_point;
+		dVector m_normal;
+	};
+
+	class dJacobian
+	{
+		public:
+		dVector m_linear;
+		dVector m_angular;
+	};
+
+	class dJacobianPair
+	{
+		public:
+		dJacobian m_jacobian_IM0;
+		dJacobian m_jacobian_IM1;
+	};
+
+	class dJacobianColum
+	{
+		public:
+		dFloat m_force;
+		dFloat m_diagDamp;
+		dFloat m_deltaAccel;
+		dFloat m_invDJMinvJt;
+		dFloat m_coordenateAccel;
+		dFloat m_jointLowFriction;
+		dFloat m_jointHighFriction;
+	};
+
+	class dParamInfo
+	{
+		public:
+		dJacobianPair m_jacobians[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointAccel[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointLowFriction[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointHighFriction[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_timestep;
+		dFloat m_timestepInv;
+		int m_count;
+	};
+
+	class dPointDerivativeParam
+	{
+		public:
+		dVector m_r0;
+		dVector m_posit0;
+		dVector m_veloc0;
+		dVector m_centripetal0;
+
+		dVector m_r1;
+		dVector m_posit1;
+		dVector m_veloc1;
+		dVector m_centripetal1;
+	};
+
+	class dJointAccelerationDecriptor
+	{
+		public:
+		int m_rowsCount;
+		dFloat m_timeStep;
+		dFloat m_invTimeStep;
+		dFloat m_firstPassCoefFlag;
+		dJacobianPair* m_rowMatrix;
+		dJacobianColum* m_colMatrix;
+	};
+
+	class dBilateralJoint
+	{
+		public:
+		dBilateralJoint()
+			:m_state0(NULL)
+			,m_state1(NULL)
+			,m_start(0)
+			,m_count(0)
+		{
+		}
+		virtual ~dBilateralJoint(){}
+
+		virtual void Init (dBodyState* const state0, dBodyState* const state1);
+
+		protected:
+		virtual void JacobianDerivative (dParamInfo* const constraintParams) = 0; 
+		virtual void UpdateSolverForces (const dJacobianPair* const jacobians) const = 0; 
+		virtual void JointAccelerations (dJointAccelerationDecriptor* const accelParam);
+
+		void InitPointParam (dPointDerivativeParam& param, const dVector& pivot) const;
+		void AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle);
+		void AddLinearRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir);
+		void AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir0, const dVector& dir1, dFloat ratio);
+		void CalculatePointDerivative (dParamInfo* const constraintParams, const dVector& dir, const dPointDerivativeParam& param);
+
+		dFloat m_motorAcceleration[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_jointFeebackForce[D_MAX_PRAM_INFO_SIZE];
+		int m_rowIsMotor[D_MAX_PRAM_INFO_SIZE];
+		dBodyState* m_state0;
+		dBodyState* m_state1;
+		int m_start;
+		int m_count;
+
+		friend class dBodyState;
+		friend class dComplemtaritySolver;
+	};
+
+	class dFrictionLessContactJoint: public dBilateralJoint
+	{
+		public: 
+		dFrictionLessContactJoint()
+			:dBilateralJoint()
+			,m_restitution(0.0f)
+			,m_count (0)
+		{}
+		virtual ~dFrictionLessContactJoint(){}
+
+		void SetContacts (int count, dContact* const contacts, dFloat restitution);
+
+		protected:
+		void UpdateSolverForces (const dJacobianPair* const jacobians) const {}
+
+		static inline int CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy);
+		int ReduceContacts (int count, dContact* const contacts, dFloat tol);
+		void JacobianDerivative (dParamInfo* const constraintParams);
+		void JointAccelerations (dJointAccelerationDecriptor* const params);
+
+		dContact m_contacts[D_MAX_PRAM_INFO_SIZE];
+		dFloat m_restitution;
+		int m_count;
+	};
+
+
+	class dBodyState
+	{
+		public:
+		dBodyState();
+		virtual ~dBodyState() {}
+
+		dFloat GetMass () const;
+		void SetMass (dFloat mass);
+
+		dFloat GetInvMass () const;
+
+		void SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz);
+		void GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const;
+
+		void SetVeloc (const dVector& veloc);
+		void SetOmega (const dVector& omega);
+		const dVector& GetOmega() const; 
+		const dVector& GetVelocity() const; 
+
+		void UpdateInertia();
+
+		void SetMatrix (const dMatrix& matrix);
+		const dMatrix& GetMatrix () const;
+
+		void SetLocalMatrix (const dMatrix& matrix);
+		const dMatrix& GetLocalMatrix () const;
+
+		void SetForce (const dVector& force);
+		void SetTorque (const dVector& torque);
+		const dVector& GetForce () const;
+		const dVector& GetTorque () const;
+
+		const dVector& GetCenterOfMass () const;
+
+		void IntegrateVelocity (dFloat timestep);
+
+		protected:
+		virtual void IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque);
+		virtual void ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega);
+
+		dMatrix m_matrix;
+		dMatrix m_localFrame;
+		dMatrix m_inertia;
+		dMatrix m_invInertia;
+
+		dVector m_localInertia;
+		dVector m_localInvInertia;
+
+		dVector m_veloc;
+		dVector m_omega;
+		dVector m_externalForce;
+		dVector m_externalTorque;
+		dVector m_globalCentreOfMass;
+
+		dFloat m_mass;
+		dFloat m_invMass;
+		int m_myIndex;
+
+		friend class dBilateralJoint;
+		friend class dComplemtaritySolver;
+	};
+
+
+
+	public:
+	dComplemtaritySolver() {};
+	virtual ~dComplemtaritySolver() {};
+
+	virtual int GetActiveJoints (dBilateralJoint** const jointArray, int bufferSize)
+	{
+		return 0;
+	}
+
+	virtual int BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount);
+	virtual void CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray);
+};
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dMathDefines.cpp

@@ -0,0 +1,15 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dMathDefines.h

@@ -0,0 +1,205 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __dMathDefined__
+#define __dMathDefined__
+
+#include <math.h>
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#ifndef dFloat
+	#ifdef _NEWTON_USE_DOUBLE
+		typedef double dFloat;
+	#else 
+		typedef float dFloat;
+	#endif
+#endif 
+
+#ifndef dFloat64
+	typedef double dFloat64;
+#endif 
+
+
+// transcendental functions
+#define	dAbs(x)		dFloat (fabs (dFloat(x))) 
+#define	dSqrt(x)	dFloat (sqrt (dFloat(x))) 
+#define	dFloor(x)	dFloat (floor (dFloat(x))) 
+#define	dCiel(x)	dFloat (ceil (dFloat(x))) 
+#define	dMod(x,y)	dFloat (fmod (dFloat(x), dFloat(y))) 
+#define	dPow(x,y)	dFloat (pow (dFloat(x), dFloat(y))) 
+
+#define dSin(x)		dFloat (sin (dFloat(x)))
+#define dCos(x)		dFloat (cos (dFloat(x)))
+#define dTan(x)		dFloat (tan (dFloat(x)))
+#define dAsin(x)	dFloat (asin (dFloat(x)))
+#define dAcos(x)	dFloat (acos (dFloat(x)))
+#define	dAtan2(x,y) dFloat (atan2 (dFloat(x), dFloat(y)))
+
+
+#define	D_MSC_VECTOR_ALIGMENT
+
+enum dEulerAngleOrder
+{
+	m_pitchYawRoll = (0 << 8) + (1 << 4) + (2 << 0),
+	m_rollYawpitch = (2 << 8) + (1 << 4) + (0 << 0),
+};
+
+
+
+
+#if ( defined (_MSC_VER) ) // || defined (_MINGW_32_VER) || defined (_MINGW_64_VER) )
+	#define dAssert(x) _ASSERTE(x)
+#else 
+	#define dAssert(x) assert(x)
+#endif
+
+
+
+
+template <class T>
+T dSign (T A)
+{
+	return (A >= T(0)) ? T(1) : T(-1);
+}
+
+template <class T> 
+void dSwap(T& A, T& B)
+{
+	T tmp (A);
+	A = B;
+	B = tmp;
+}	
+
+template <class T>
+T dMax(T A, T B)
+{
+	return (A > B) ? A : B; 
+}
+
+template <class T>
+T dMin(T A, T B)
+{
+	return (A < B) ? A : B; 
+}
+
+template <class T>
+T dClamp(T val, T min, T max)
+{
+	return dMax (min, dMin (max, val));
+}
+
+template <class T> 
+void dSort (T* const array, int elements, int (*compare) (const T* const  A, const T* const B, void* const context), void* const context = NULL)
+{
+	int stride = 8;
+	int stack[1024][2];
+
+	stack[0][0] = 0;
+	stack[0][1] = elements - 1;
+	int stackIndex = 1;
+	while (stackIndex) {
+		stackIndex --;
+		int lo = stack[stackIndex][0];
+		int hi = stack[stackIndex][1];
+		if ((hi - lo) > stride) {
+			int i = lo;
+			int j = hi;
+			T val (array[(lo + hi) >> 1]);
+			do {    
+				while (compare (&array[i], &val, context) < 0) i ++;
+				while (compare (&array[j], &val, context) > 0) j --;
+
+				if (i <= j)	{
+					dSwap(array[i], array[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 ++;
+			}
+			dAssert (stackIndex < int (sizeof (stack) / (2 * sizeof (stack[0][0]))));
+		}
+	}
+
+	stride = stride * 2;
+	if (elements < stride) {
+		stride = elements;
+	}
+	for (int i = 1; i < stride; i ++) {
+		if (compare (&array[0], &array[i], context) > 0) {
+			dSwap(array[0], array[i]);
+		}
+	}
+
+	for (int i = 1; i < elements; i ++) {
+		int j = i;
+		T tmp (array[i]);
+		for (; compare (&array[j - 1], &tmp, context) > 0; j --) {
+			dAssert (j > 0);
+			array[j] = array[j - 1];
+		}
+		array[j] = tmp;
+	}
+
+#ifdef _DEBUG
+	for (int i = 0; i < (elements - 1); i ++) {
+		dAssert (compare (&array[i], &array[i + 1], context) <= 0);
+	}
+#endif
+}
+
+
+
+#ifdef _MSC_VER
+	#ifdef _DEBUG
+		#include <windows.h>
+		#include <stdarg.h>
+		inline void dExpandTraceMessage (const char* const fmt, ...)
+		{
+			va_list v_args;
+			char text[4096];
+
+			text[0] = 0;
+			va_start (v_args, fmt);     
+			vsprintf(text, fmt, v_args);
+			va_end (v_args);            
+
+			OutputDebugStringA (text);
+		}
+
+		#define dTrace(x)										\
+		{														\
+			dExpandTraceMessage x;								\
+		}																	
+	#else
+		#define dTrace(x)
+	#endif
+#else
+	#define dTrace(x)
+#endif
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dMatrix.cpp

@@ -0,0 +1,739 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+#include "dVector.h"
+#include "dMatrix.h"
+#include "dQuaternion.h"
+
+// calculate an orthonormal matrix with the front vector pointing on the 
+// dir direction, and the up and right are determined by using the GramSchidth procedure
+
+
+dMatrix dGetIdentityMatrix()
+{
+	return dMatrix (dVector (1.0f, 0.0f, 0.0f, 0.0f),
+					dVector (0.0f, 1.0f, 0.0f, 0.0f),
+					dVector (0.0f, 0.0f, 1.0f, 0.0f),
+					dVector (0.0f, 0.0f, 0.0f, 1.0f));
+}
+
+dMatrix dGetZeroMatrix ()
+{
+	return dMatrix (dVector (0.0f, 0.0f, 0.0f, 0.0f),
+					dVector (0.0f, 0.0f, 0.0f, 0.0f),
+					dVector (0.0f, 0.0f, 0.0f, 0.0f),
+					dVector (0.0f, 0.0f, 0.0f, 0.0f));
+}
+
+
+
+dMatrix dGrammSchmidt(const dVector& dir)
+{
+	dVector up;
+	dVector right;
+	dVector front (dir); 
+
+	front = front.Scale(1.0f / dSqrt (front % front));
+	if (dAbs (front.m_z) > 0.577f) {
+		right = front * dVector (-front.m_y, front.m_z, 0.0f);
+	} else {
+		right = front * dVector (-front.m_y, front.m_x, 0.0f);
+	}
+	right = right.Scale (1.0f / dSqrt (right % right));
+	up = right * front;
+
+	front.m_w = 0.0f;
+	up.m_w = 0.0f;
+	right.m_w = 0.0f;
+	return dMatrix (front, up, right, dVector (0.0f, 0.0f, 0.0f, 1.0f));
+}
+
+
+dMatrix dPitchMatrix(dFloat ang)
+{
+	dFloat cosAng;
+	dFloat sinAng;
+	sinAng = dSin (ang);
+	cosAng = dCos (ang);
+	return dMatrix (dVector (1.0f,    0.0f,    0.0f, 0.0f), 
+					dVector (0.0f,  cosAng,  sinAng, 0.0f),
+					dVector (0.0f, -sinAng,  cosAng, 0.0f), 
+					dVector (0.0f,    0.0f,    0.0f, 1.0f)); 
+
+}
+
+dMatrix dYawMatrix(dFloat ang)
+{
+	dFloat cosAng;
+	dFloat sinAng;
+	sinAng = dSin (ang);
+	cosAng = dCos (ang);
+	return dMatrix (dVector (cosAng, 0.0f, -sinAng, 0.0f), 
+					dVector (0.0f,   1.0f,    0.0f, 0.0f), 
+					dVector (sinAng, 0.0f,  cosAng, 0.0f), 
+					dVector (0.0f,   0.0f,    0.0f, 1.0f)); 
+}
+
+dMatrix dRollMatrix(dFloat ang)
+{
+	dFloat cosAng;
+	dFloat sinAng;
+	sinAng = dSin (ang);
+	cosAng = dCos (ang);
+	return dMatrix (dVector ( cosAng, sinAng, 0.0f, 0.0f), 
+					dVector (-sinAng, cosAng, 0.0f, 0.0f),
+					dVector (   0.0f,   0.0f, 1.0f, 0.0f), 
+					dVector (   0.0f,   0.0f, 0.0f, 1.0f)); 
+}																		 
+
+
+
+dMatrix::dMatrix (const dQuaternion &rotation, const dVector &position)
+{
+
+	dFloat x2 = dFloat (2.0f) * rotation.m_q1 * rotation.m_q1;
+	dFloat y2 = dFloat (2.0f) * rotation.m_q2 * rotation.m_q2;
+	dFloat z2 = dFloat (2.0f) * rotation.m_q3 * rotation.m_q3;
+#ifdef _DEBUG
+	dFloat w2 = dFloat (2.0f) * rotation.m_q0 * rotation.m_q0;
+	dAssert (dAbs (w2 + x2 + y2 + z2 - dFloat(2.0f)) < dFloat (1.e-2f));
+#endif
+
+	dFloat xy = dFloat (2.0f) * rotation.m_q1 * rotation.m_q2;
+	dFloat xz = dFloat (2.0f) * rotation.m_q1 * rotation.m_q3;
+	dFloat xw = dFloat (2.0f) * rotation.m_q1 * rotation.m_q0;
+	dFloat yz = dFloat (2.0f) * rotation.m_q2 * rotation.m_q3;
+	dFloat yw = dFloat (2.0f) * rotation.m_q2 * rotation.m_q0;
+	dFloat zw = dFloat (2.0f) * rotation.m_q3 * rotation.m_q0;
+
+	m_front = dVector (dFloat(1.0f) - y2 - z2, xy + zw				 , xz - yw				  , dFloat(0.0f));
+	m_up    = dVector (xy - zw				 , dFloat(1.0f) - x2 - z2, yz + xw				  , dFloat(0.0f));
+	m_right = dVector (xz + yw				 , yz - xw				 , dFloat(1.0f) - x2 - y2 , dFloat(0.0f));
+
+	m_posit.m_x = position.m_x;
+	m_posit.m_y = position.m_y;
+	m_posit.m_z = position.m_z;
+	m_posit.m_w = dFloat(1.0f);
+}
+
+dMatrix::dMatrix (dFloat pitch, dFloat yaw, dFloat roll, const dVector& location)
+{
+	dMatrix& me = *this;
+	me = dPitchMatrix(pitch) * dYawMatrix(yaw) * dRollMatrix(roll);
+	me.m_posit = location;
+	me.m_posit.m_w = 1.0f;
+}
+
+
+bool dMatrix::TestIdentity() const 
+{
+	const dMatrix& matrix = *this;
+	const dMatrix& identity = dGetIdentityMatrix();
+	
+	bool isIdentity = true;
+	for (int i = 0; isIdentity && (i < 3); i ++) {
+		isIdentity &= dAbs (matrix[3][i]) < 1.0e-4f;
+		for (int j = i; isIdentity && (j < 3); j ++) {
+			isIdentity &= dAbs (matrix[i][j]-identity[i][j]) < 1.0e-4f;
+		}
+	}
+	return isIdentity;
+}
+
+
+bool dMatrix::TestOrthogonal() const
+{
+	dVector n (m_front * m_up);
+	dFloat a = m_right % m_right;
+	dFloat b = m_up % m_up;
+	dFloat c = m_front % m_front;
+	dFloat d = n % m_right;
+
+	return (m_front[3] == dFloat (0.0f)) & 
+		(m_up[3] == dFloat (0.0f)) & 
+		(m_right[3] == dFloat (0.0f)) & 
+		(m_posit[3] == dFloat (1.0f)) &
+		(dAbs(a - dFloat (1.0f)) < dFloat (1.0e-4f)) & 
+		(dAbs(b - dFloat (1.0f)) < dFloat (1.0e-4f)) &
+		(dAbs(c - dFloat (1.0f)) < dFloat (1.0e-4f)) &
+		(dAbs(d - dFloat (1.0f)) < dFloat (1.0e-4f)); 
+}
+
+
+//dVector dMatrix::GetEulerAngles (dEulerAngleOrder order) const
+void dMatrix::GetEulerAngles(dVector& euler0, dVector& euler1, dEulerAngleOrder order) const
+{
+	int a0 = (order>>8)&3;
+	int a1 = (order>>4)&3;
+	int a2 = (order>>0)&3;
+	const dMatrix& matrix = *this;
+
+	// Assuming the angles are in radians.
+	if (matrix[a0][a2] > 0.99995f) {
+		dFloat picth0 = 0.0f;
+		dFloat yaw0 = -3.141592f * 0.5f;
+		dFloat roll0 = - dAtan2(matrix[a2][a1], matrix[a1][a1]);
+		euler0[a0] = picth0;
+		euler0[a1] = yaw0;
+		euler0[a2] = roll0;
+
+		euler1[a0] = picth0;
+		euler1[a1] = yaw0;
+		euler1[a2] = roll0;
+
+	} else if (matrix[a0][a2] < -0.99995f) {
+		dFloat picth0 = 0.0f;
+		dFloat yaw0 = 3.141592f * 0.5f;
+		dFloat roll0 = dAtan2(matrix[a2][a1], matrix[a1][a1]);
+		euler0[a0] = picth0;
+		euler0[a1] = yaw0;
+		euler0[a2] = roll0;
+
+		euler1[a0] = picth0;
+		euler1[a1] = yaw0;
+		euler1[a2] = roll0;
+	} else {
+		//euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]);
+		//euler[a1] = -dAsin ( matrix[a0][a2]);
+		//euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]);
+
+		dFloat yaw0 = -dAsin ( matrix[a0][a2]);
+		dFloat yaw1 = 3.141592f - yaw0;
+		dFloat sign0 = dSign(dCos (yaw0));
+		dFloat sign1 = dSign(dCos (yaw1));
+
+		dFloat picth0 = dAtan2(matrix[a1][a2] * sign0, matrix[a2][a2] * sign0);
+		dFloat picth1 = dAtan2(matrix[a1][a2] * sign1, matrix[a2][a2] * sign1);
+
+		dFloat roll0 = dAtan2(matrix[a0][a1] * sign0, matrix[a0][a0] * sign0);
+		dFloat roll1 = dAtan2(matrix[a0][a1] * sign1, matrix[a0][a0] * sign1);
+
+		if (yaw1 > 3.141592f) {
+			yaw1 -= 2.0f * 3.141592f;
+		}
+
+		euler0[a0] = picth0;
+		euler0[a1] = yaw0;
+		euler0[a2] = roll0;
+
+		euler1[a0] = picth1;
+		euler1[a1] = yaw1;
+		euler1[a2] = roll1;
+	}
+	euler0[3] = dFloat(0.0f);
+	euler1[3] = dFloat(0.0f);
+
+	#ifdef _DEBUG
+		if (order == m_pitchYawRoll) {
+			dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix(euler0[1]) * dRollMatrix(euler0[2]));
+			dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix(euler1[1]) * dRollMatrix(euler1[2]));
+			for (int i = 0; i < 3; i ++) {
+				for (int j = 0; j < 3; j ++) {
+					dFloat error = dAbs (m0[i][j] - matrix[i][j]);
+					dAssert (error < 5.0e-2f);
+					error = dAbs (m1[i][j] - matrix[i][j]);
+					dAssert (error < 5.0e-2f);
+				}
+			}
+		}
+	#endif
+}
+
+
+
+dMatrix dMatrix::Inverse () const
+{
+	return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, 0.0f),
+					dVector (m_front.m_y, m_up.m_y, m_right.m_y, 0.0f),
+		            dVector (m_front.m_z, m_up.m_z, m_right.m_z, 0.0f),
+		            dVector (- (m_posit % m_front), - (m_posit % m_up), - (m_posit % m_right), 1.0f));
+}
+
+dMatrix dMatrix::Transpose () const
+{
+	return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, 0.0f),
+					dVector (m_front.m_y, m_up.m_y, m_right.m_y, 0.0f),
+					dVector (m_front.m_z, m_up.m_z, m_right.m_z, 0.0f),
+					dVector (0.0f, 0.0f, 0.0f, 1.0f));
+}
+
+dMatrix dMatrix::Transpose4X4 () const
+{
+	return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, m_posit.m_x),
+					dVector (m_front.m_y, m_up.m_y, m_right.m_y, m_posit.m_y),
+					dVector (m_front.m_z, m_up.m_z, m_right.m_z, m_posit.m_z),
+					dVector (m_front.m_w, m_up.m_w, m_right.m_w, m_posit.m_w));
+							
+}
+
+dVector dMatrix::RotateVector (const dVector &v) const
+{
+	return dVector (v.m_x * m_front.m_x + v.m_y * m_up.m_x + v.m_z * m_right.m_x,
+					v.m_x * m_front.m_y + v.m_y * m_up.m_y + v.m_z * m_right.m_y,
+					v.m_x * m_front.m_z + v.m_y * m_up.m_z + v.m_z * m_right.m_z);
+}
+
+dVector dMatrix::UnrotateVector (const dVector &v) const
+{
+	return dVector (v % m_front, v % m_up, v % m_right);
+}
+
+dVector dMatrix::RotateVector4x4 (const dVector &v) const
+{
+	dVector tmp;
+	const dMatrix& me = *this;
+	for (int i = 0; i < 4; i ++) {
+		tmp[i] = v[0] * me[0][i] + v[1] * me[1][i] +  v[2] * me[2][i] +  v[3] * me[3][i];
+	}
+	return tmp;
+}
+
+
+dVector dMatrix::TransformVector (const dVector &v) const
+{
+	return m_posit + RotateVector(v);
+}
+
+dVector dMatrix::UntransformVector (const dVector &v) const
+{
+	return UnrotateVector(v - m_posit);
+}
+
+
+void dMatrix::TransformTriplex (dFloat* const dst, int dstStrideInBytes, const dFloat* const src, int srcStrideInBytes, int count) const
+{
+	dstStrideInBytes /= sizeof (dFloat);
+	srcStrideInBytes /= sizeof (dFloat);
+	for (int i = 0 ; i < count; i ++ ) {
+		dFloat x = src[srcStrideInBytes * i + 0];
+		dFloat y = src[srcStrideInBytes * i + 1];
+		dFloat z = src[srcStrideInBytes * i + 2];
+		dst[dstStrideInBytes * i + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstStrideInBytes * i + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstStrideInBytes * i + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+	}
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+void dMatrix::TransformTriplex (dFloat64* const dst, int dstStrideInBytes, const dFloat64* const src, int srcStrideInBytes, int count) const
+{
+	dstStrideInBytes /= sizeof (dFloat64);
+	srcStrideInBytes /= sizeof (dFloat64);
+	for (int i = 0 ; i < count; i ++ ) {
+		dFloat64 x = src[srcStrideInBytes * i + 0];
+		dFloat64 y = src[srcStrideInBytes * i + 1];
+		dFloat64 z = src[srcStrideInBytes * i + 2];
+		dst[dstStrideInBytes * i + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstStrideInBytes * i + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstStrideInBytes * i + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+	}
+}
+#endif
+
+dMatrix dMatrix::operator* (const dMatrix &B) const
+{
+	const dMatrix& A = *this;
+	return dMatrix (dVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0],
+							 A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
+							 A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
+	                         A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
+					dVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0],
+						     A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
+							 A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
+							 A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
+					dVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0],
+							 A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
+							 A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
+							 A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
+					dVector (A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0],
+							 A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1],
+							 A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2],
+							 A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3]));
+}
+
+
+
+
+
+dVector dMatrix::TransformPlane (const dVector &localPlane) const
+{
+	dVector tmp (RotateVector (localPlane));  
+	tmp.m_w = localPlane.m_w - (localPlane % UnrotateVector (m_posit));  
+	return tmp;  
+}
+
+dVector dMatrix::UntransformPlane (const dVector &globalPlane) const
+{
+	dVector tmp (UnrotateVector (globalPlane));
+	tmp.m_w = globalPlane % m_posit + globalPlane.m_w;
+	return tmp;
+}
+
+bool dMatrix::SanityCheck() const
+{
+	dVector right (m_front * m_up);
+	if (dAbs (right % m_right) < 0.9999f) {
+		return false;
+	}
+	if (dAbs (m_right.m_w) > 0.0f) {
+		return false;
+	}
+	if (dAbs (m_up.m_w) > 0.0f) {
+		return false;
+	}
+	if (dAbs (m_right.m_w) > 0.0f) {
+		return false;
+	}
+
+	if (dAbs (m_posit.m_w) != 1.0f) {
+		return false;
+	}
+
+	return true;
+}
+
+
+dMatrix dMatrix::Inverse4x4 () const
+{
+	const dFloat tol = 1.0e-4f;
+	dMatrix tmp (*this);
+	dMatrix inv (dGetIdentityMatrix());
+	for (int i = 0; i < 4; i ++) {
+		dFloat diag = tmp[i][i];
+		if (dAbs (diag) < tol) {
+			int j = 0;
+			for (j = i + 1; j < 4; j ++) {
+				dFloat val = tmp[j][i];
+				if (dAbs (val) > tol) {
+					break;
+				}
+			}
+			dAssert (j < 4);
+			for (int k = 0; k < 4; k ++) {
+				tmp[i][k] += tmp[j][k];
+				inv[i][k] += inv[j][k];
+			}
+			diag = tmp[i][i];
+		}
+		dFloat invDiag = 1.0f / diag;
+		for (int j = 0; j < 4; j ++) {
+			tmp[i][j] *= invDiag;
+			inv[i][j] *= invDiag;
+		}
+		tmp[i][i] = 1.0f;
+
+		
+		for (int j = 0; j < 4; j ++) {
+			if (j != i) {
+				dFloat pivot = tmp[j][i];
+				for (int k = 0; k < 4; k ++) {
+					tmp[j][k] -= pivot * tmp[i][k];
+					inv[j][k] -= pivot * inv[i][k];
+				}
+				tmp[j][i] = 0.0f;
+			}
+		}
+	}
+	return inv;
+}
+
+
+
+
+#if 0
+class XXXX 
+{
+public:
+	XXXX ()
+	{
+		dMatrix s;
+		dFloat m = 2.0f;
+		for (int i = 0; i < 3; i ++) {
+			for (int j = 0; j < 3; j ++) {
+				s[i][j] = m; 
+				m += (i + 1) + j;
+			}
+		}
+		s.m_posit = dVector (1, 2, 3, 1);
+		dMatrix matrix;
+		dVector scale;
+		dMatrix stretch;
+		s.PolarDecomposition (matrix, scale, stretch);
+		dMatrix s1 (matrix, scale, stretch);
+
+		dMatrix xxx (dPitchMatrix(30.0f * 3.14159f/180.0f) * dRollMatrix(30.0f * 3.14159f/180.0f));
+		dMatrix xxxx (GetIdentityMatrix());
+		xxx[0] = xxx[0].Scale (-1.0f);
+		dFloat mmm = (xxx[0] * xxx[1]) % xxx[2];
+		xxxx[0][0] = 3.0f;
+		xxxx[1][1] = 3.0f;
+		xxxx[2][2] = 4.0f;
+
+		dMatrix xxx2 (xxx * xxxx);
+		mmm = (xxx2[0] * xxx2[1]) % xxx2[2];
+		xxx2.PolarDecomposition (matrix, scale, stretch);
+
+		s1 = dMatrix (matrix, scale, stretch);
+		s1 = dMatrix (matrix, scale, stretch);
+
+	}
+};
+XXXX xxx;
+#endif
+
+
+static inline void ROT(dMatrix &a, int i, int j, int k, int l, dFloat s, dFloat tau) 
+{
+	dFloat g;
+	dFloat h;
+	g = a[i][j]; 
+	h = a[k][l]; 
+	a[i][j] = g - s * (h + g * tau); 
+	a[k][l] = h + s * (g - h * tau);
+}
+
+// from numerical recipes in c
+// Jacobian method for computing the eigenvectors of a symmetric matrix
+dMatrix dMatrix::JacobiDiagonalization (dVector &eigenValues, const dMatrix& initialMatrix) const
+{
+	dFloat thresh;
+	dFloat b[3];
+	dFloat z[3];
+	dFloat d[3];
+	dFloat EPSILON = 1.0e-5f;
+
+	dMatrix mat (*this);
+	dMatrix eigenVectors (initialMatrix);
+	
+	b[0] = mat[0][0]; 
+	b[1] = mat[1][1];
+	b[2] = mat[2][2];
+
+	d[0] = mat[0][0]; 
+	d[1] = mat[1][1]; 
+	d[2] = mat[2][2]; 
+
+	z[0] = 0.0f;
+	z[1] = 0.0f;
+	z[2] = 0.0f;
+
+	int nrot = 0;
+	for (int i = 0; i < 50; i++) {
+		dFloat sm = dAbs(mat[0][1]) + dAbs(mat[0][2]) + dAbs(mat[1][2]);
+
+		if (sm < EPSILON * 1e-5) {
+			dAssert (dAbs((eigenVectors.m_front % eigenVectors.m_front) - 1.0f) <EPSILON);
+			dAssert (dAbs((eigenVectors.m_up % eigenVectors.m_up) - 1.0f) < EPSILON);
+			dAssert (dAbs((eigenVectors.m_right % eigenVectors.m_right) - 1.0f) < EPSILON);
+			eigenValues = dVector (d[0], d[1], d[2], dFloat (0.0f));
+			return eigenVectors.Inverse();
+		}
+
+		if (i < 3) {
+			thresh = (dFloat)(0.2f / 9.0f) * sm;
+		}	else {
+			thresh = 0.0;
+		}
+
+
+		// First row
+		dFloat g = 100.0f * dAbs(mat[0][1]);
+		if ((i > 3) && (dAbs(d[0]) + g == dAbs(d[0])) && (dAbs(d[1]) + g == dAbs(d[1]))) {
+			mat[0][1] = 0.0f;
+		} else if (dAbs(mat[0][1]) > thresh) {
+			dFloat t;
+			dFloat h = d[1] - d[0];
+			if (dAbs(h) + g == dAbs(h)) {
+				t = mat[0][1] / h;
+			} else {
+				dFloat theta = dFloat (0.5f) * h / mat[0][1];
+				t = dFloat(1.0f) / (dAbs(theta) + dSqrt(dFloat(1.0f) + theta * theta));
+				if (theta < 0.0f) {
+					t = -t;
+				}
+			}
+			dFloat c = dFloat(1.0f) / dSqrt (1.0f + t * t); 
+			dFloat s = t * c; 
+			dFloat tau = s / (dFloat(1.0f) + c); 
+			h = t * mat[0][1];
+			z[0] -= h; 
+			z[1] += h; 
+			d[0] -= h; 
+			d[1] += h;
+			mat[0][1] = 0.0f;
+			ROT (mat, 0, 2, 1, 2, s, tau); 
+			ROT (eigenVectors, 0, 0, 0, 1, s, tau); 
+			ROT (eigenVectors, 1, 0, 1, 1, s, tau); 
+			ROT (eigenVectors, 2, 0, 2, 1, s, tau); 
+
+			nrot++;
+		}
+
+
+		// second row
+		g = 100.0f * dAbs(mat[0][2]);
+		if ((i > 3) && (dAbs(d[0]) + g == dAbs(d[0])) && (dAbs(d[2]) + g == dAbs(d[2]))) {
+			mat[0][2] = 0.0f;
+		} else if (dAbs(mat[0][2]) > thresh) {
+			dFloat t;
+			dFloat h = d[2] - d[0];
+			if (dAbs(h) + g == dAbs(h)) {
+				t = (mat[0][2]) / h;
+			}	else {
+				dFloat theta = dFloat (0.5f) * h / mat[0][2];
+				t = dFloat(1.0f) / (dAbs(theta) + dSqrt(dFloat(1.0f) + theta * theta));
+				if (theta < 0.0f) {
+					t = -t;
+				}
+			}
+			dFloat c = dFloat(1.0f) / dSqrt(1 + t * t); 
+			dFloat s = t * c; 
+			dFloat tau = s / (dFloat(1.0f) + c); 
+			h = t * mat[0][2];
+			z[0] -= h; 
+			z[2] += h; 
+			d[0] -= h; 
+			d[2] += h;
+			mat[0][2]=0.0;
+			ROT (mat, 0, 1, 1, 2, s, tau); 
+			ROT (eigenVectors, 0, 0, 0, 2, s, tau); 
+			ROT (eigenVectors, 1, 0, 1, 2, s, tau); 
+			ROT (eigenVectors, 2, 0, 2, 2, s, tau); 
+		}
+
+		// trird row
+		g = 100.0f * dAbs(mat[1][2]);
+		if ((i > 3) && (dAbs(d[1]) + g == dAbs(d[1])) && (dAbs(d[2]) + g == dAbs(d[2]))) {
+			mat[1][2] = 0.0f;
+		} else if (dAbs(mat[1][2]) > thresh) {
+			dFloat t;
+			dFloat h = d[2] - d[1];
+			if (dAbs(h) + g == dAbs(h)) {
+				t = mat[1][2] / h;
+			}	else {
+				dFloat theta = dFloat (0.5f) * h / mat[1][2];
+				t = dFloat(1.0f) / (dAbs(theta) + dSqrt(dFloat(1.0f) + theta * theta));
+				if (theta < 0.0f) {
+					t = -t;
+				}
+			}
+			dFloat c = dFloat(1.0f) / dSqrt(1 + t*t); 
+			dFloat s = t * c; 
+			dFloat tau = s / (dFloat(1.0f) + c); 
+
+			h = t * mat[1][2];
+			z[1] -= h; 
+			z[2] += h; 
+			d[1] -= h; 
+			d[2] += h;
+			mat[1][2] = 0.0f;
+			ROT (mat, 0, 1, 0, 2, s, tau); 
+			ROT (eigenVectors, 0, 1, 0, 2, s, tau); 
+			ROT (eigenVectors, 1, 1, 1, 2, s, tau); 
+			ROT (eigenVectors, 2, 1, 2, 2, s, tau); 
+			nrot++;
+		}
+
+		b[0] += z[0]; d[0] = b[0]; z[0] = 0.0f;
+		b[1] += z[1]; d[1] = b[1]; z[1] = 0.0f;
+		b[2] += z[2]; d[2] = b[2]; z[2] = 0.0f;
+	}
+
+	dAssert (0);
+	eigenValues = dVector (d[0], d[1], d[2], dFloat (0.0f));
+	return dGetIdentityMatrix();
+} 	
+
+
+
+
+//void dMatrix::PolarDecomposition (dMatrix& orthogonal, dMatrix& symetric) const
+void dMatrix::PolarDecomposition (dMatrix& transformMatrix, dVector& scale, dMatrix& stretchAxis, const dMatrix& initialStretchAxis) const
+{
+	// a polar decomposition decompose matrix A = O * S
+	// where S = sqrt (transpose (L) * L)
+
+	// calculate transpose (L) * L 
+	dMatrix LL ((*this) * Transpose());
+
+
+	// check is this si a pure uniformScale * rotation * translation
+	dFloat det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * (1.0f / 3.0f);
+
+	dFloat invdet2 = 1.0f / det2;
+
+	dMatrix pureRotation (LL);
+	pureRotation[0] = pureRotation[0].Scale (invdet2);
+	pureRotation[1] = pureRotation[1].Scale (invdet2);
+	pureRotation[2] = pureRotation[2].Scale (invdet2);
+
+	//dFloat soureSign = ((*this)[0] * (*this)[1]) % (*this)[2];
+	dFloat sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
+	dFloat det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
+	if (dAbs (det - 1.0f) < 1.e-5f){
+		// this is a pure scale * rotation * translation
+		det = sign * dSqrt (det2);
+		scale[0] = det;
+		scale[1] = det;
+		scale[2] = det;
+		scale[3] = 1.0f;
+		det = 1.0f/ det;
+		transformMatrix.m_front = m_front.Scale (det);
+		transformMatrix.m_up = m_up.Scale (det);
+		transformMatrix.m_right = m_right.Scale (det);
+		transformMatrix[0][3] = 0.0f;
+		transformMatrix[1][3] = 0.0f;
+		transformMatrix[2][3] = 0.0f;
+		transformMatrix.m_posit = m_posit;
+		stretchAxis = dGetIdentityMatrix();
+		
+	} else {
+		stretchAxis = LL.JacobiDiagonalization(scale, initialStretchAxis);
+
+		// I need to deal with buy seeing of some of the Scale are duplicated
+		// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
+		// for now just us the matrix
+
+		scale[0] = sign * dSqrt (scale[0]);
+		scale[1] = sign * dSqrt (scale[1]);
+		scale[2] = sign * dSqrt (scale[2]);
+		scale[3] = 1.0f;
+
+		dMatrix scaledAxis;
+		scaledAxis[0] = stretchAxis[0].Scale (1.0f / scale[0]);
+		scaledAxis[1] = stretchAxis[1].Scale (1.0f / scale[1]);
+		scaledAxis[2] = stretchAxis[2].Scale (1.0f / scale[2]);
+		scaledAxis[3] = stretchAxis[3];
+		dMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
+
+		transformMatrix = symetricInv * (*this);
+		transformMatrix.m_posit = m_posit;
+	}
+}
+
+dMatrix::dMatrix (const dMatrix& transformMatrix, const dVector& scale, const dMatrix& stretchAxis)
+{
+	dMatrix scaledAxis;
+	scaledAxis[0] = stretchAxis[0].Scale (scale[0]);
+	scaledAxis[1] = stretchAxis[1].Scale (scale[1]);
+	scaledAxis[2] = stretchAxis[2].Scale (scale[2]);
+	scaledAxis[3] = stretchAxis[3];
+
+	*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
+}
+
+
+
+
+
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dMatrix.h

@@ -0,0 +1,130 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+#ifndef __dMatrix__
+#define __dMatrix__
+
+#include "dVector.h"
+
+class dMatrix ;
+class dQuaternion;
+
+// small but fully operational 4x4 matrix class
+class dQuaternion;
+
+dMatrix dGetZeroMatrix ();
+dMatrix dGetIdentityMatrix();
+
+D_MSC_VECTOR_ALIGMENT
+class dMatrix
+{
+	public:
+	dMatrix ();
+	dMatrix (const dFloat* const array);
+#ifndef _NEWTON_USE_DOUBLE
+	dMatrix (const dFloat64* const array);
+#endif
+	dMatrix (const dVector &front, const dVector &up, const dVector &right, const dVector &posit);
+	dMatrix (const dQuaternion &rotation, const dVector &position);
+	dMatrix (dFloat pitch, dFloat yaw, dFloat roll, const dVector& location);
+
+	dVector& operator[] (int i);
+	const dVector& operator[] (int i) const;
+
+	dMatrix Inverse () const;
+	dMatrix Transpose () const;
+	dMatrix Transpose4X4 () const;
+	
+	dVector RotateVector (const dVector &v) const;
+	dVector UnrotateVector (const dVector &v) const;
+	dVector TransformVector (const dVector &v) const;
+	dVector UntransformVector (const dVector &v) const;
+	dVector TransformPlane (const dVector &localPlane) const;
+	dVector UntransformPlane (const dVector &globalPlane) const;
+	//dVector GetEulerAngles(dEulerAngleOrder order = m_pitchYawRoll) const;
+    void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
+
+	bool TestIdentity() const; 
+	bool TestOrthogonal() const; 
+	dMatrix Inverse4x4 () const;
+	dVector RotateVector4x4 (const dVector &v) const;
+	dMatrix JacobiDiagonalization (dVector& eigenValues, const dMatrix& initialMatrix = dGetIdentityMatrix()) const;
+
+	// decompose this matrix into [this = transpose(stretchAxis) * matrix(scale) * stretchAxis * transformMatrix];
+	void PolarDecomposition (dMatrix& transformMatrix, dVector& scale, dMatrix& stretchAxis, const dMatrix& initialStretchAxis = dGetIdentityMatrix()) const;
+	
+	// constructor for polar composition
+	dMatrix (const dMatrix& transformMatrix, const dVector& scale, const dMatrix& stretchAxis);
+
+	void TransformTriplex (dFloat* const dst, int dstStrideInBytes, const dFloat* const src, int srcStrideInBytes, int count) const;
+#ifndef _NEWTON_USE_DOUBLE
+	void TransformTriplex (dFloat64* const dst, int dstStrideInBytes, const dFloat64* const src, int srcStrideInBytes, int count) const;
+#endif
+
+	dMatrix operator* (const dMatrix & B) const;
+
+
+	bool SanityCheck() const;
+
+	dVector m_front;
+	dVector m_up;
+	dVector m_right;
+	dVector m_posit;
+};
+
+
+
+inline dMatrix::dMatrix ()
+{
+}
+
+inline dMatrix::dMatrix (
+	const dVector &front, 
+	const dVector &up,
+	const dVector &right,
+	const dVector &posit)
+	:m_front (front), m_up(up), m_right(right), m_posit(posit)
+{
+}
+
+inline dMatrix::dMatrix (const dFloat* const array)
+{
+	memcpy (&(*this)[0][0], array, sizeof (dMatrix));
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+inline dMatrix::dMatrix (const dFloat64* const array)
+{
+	dFloat* const ptr = &(*this)[0][0];
+	for (int i = 0; i < 16; i ++) {
+		ptr[i] = dFloat (array[i]);
+	}
+}
+#endif
+
+inline dVector& dMatrix::operator[] (int  i)
+{
+	return (&m_front)[i];
+}
+
+inline const dVector& dMatrix::operator[] (int  i) const
+{
+	return (&m_front)[i];
+}
+
+
+dMatrix dRollMatrix(dFloat ang);
+dMatrix dYawMatrix(dFloat ang);
+dMatrix dPitchMatrix(dFloat ang);
+dMatrix dGrammSchmidt(const dVector& dir);
+#endif
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dQuaternion.cpp

@@ -0,0 +1,212 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+#include "dMathDefines.h"
+#include "dVector.h"
+#include "dMatrix.h"
+#include "dQuaternion.h"
+
+
+
+dQuaternion::dQuaternion (const dMatrix &matrix)
+{
+	enum QUAT_INDEX
+	{
+		X_INDEX=0,
+		Y_INDEX=1,
+		Z_INDEX=2
+	};
+	static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
+
+	dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
+	dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);
+
+	if (trace > dFloat(0.0f)) {
+		trace = dSqrt (trace + dFloat(1.0f));
+		m_q0 = dFloat (0.5f) * trace;
+		trace = dFloat (0.5f) / trace;
+		m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
+		m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
+		m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
+
+	} else {
+		QUAT_INDEX i = X_INDEX;
+		if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
+			i = Y_INDEX;
+		}
+		if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
+			i = Z_INDEX;
+		}
+		QUAT_INDEX j = QIndex [i];
+		QUAT_INDEX k = QIndex [j];
+
+		trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
+		trace = dSqrt (trace);
+
+		dFloat* const ptr = &m_q1;
+		ptr[i] = dFloat (0.5f) * trace;
+		trace = dFloat (0.5f) / trace;
+		m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
+		ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
+		ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
+	}
+
+#if _DEBUG
+
+	dMatrix tmp (*this, matrix.m_posit);
+	dMatrix unitMatrix (tmp * matrix.Inverse());
+	for (int i = 0; i < 4; i ++) {
+		dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
+		dAssert (err < dFloat (1.0e-3f));
+	}
+
+	dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
+	dAssert (err < dFloat(1.0e-3f));
+#endif
+
+}
+
+
+dQuaternion::dQuaternion (const dVector &unitAxis, dFloat angle)
+{
+	angle *= dFloat (0.5f);
+	m_q0 = dCos (angle);
+	dFloat sinAng = dSin (angle);
+
+#ifdef _DEBUG
+	if (dAbs (angle) > dFloat(1.0e-6f)) {
+		dAssert (dAbs (dFloat(1.0f) - unitAxis % unitAxis) < dFloat(1.0e-3f));
+	} 
+#endif
+	m_q1 = unitAxis.m_x * sinAng;
+	m_q2 = unitAxis.m_y * sinAng;
+	m_q3 = unitAxis.m_z * sinAng;
+}
+
+
+dVector dQuaternion::CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const
+{
+	dQuaternion q0 (*this);
+	if (q0.DotProduct (q1) < 0.0f) {
+		q0.Scale(-1.0f);
+	}
+	dQuaternion dq (q0.Inverse() * q1);
+	dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
+
+	dFloat dirMag2 = omegaDir % omegaDir;
+	if (dirMag2	< dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
+		return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
+	}
+
+	dFloat dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
+	dFloat dirMag = dirMag2 * dirMagInv;
+
+	dFloat omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) * invdt;
+	return omegaDir.Scale (dirMagInv * omegaMag);
+}
+
+
+dQuaternion dQuaternion::Slerp (const dQuaternion &QB, dFloat t) const 
+{
+	dQuaternion Q;
+
+	dFloat dot = DotProduct (QB);
+	dAssert (dot >= 0.0f);
+
+	if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) {
+		dFloat Sclp;
+		dFloat Sclq;
+		if (dot < dFloat(0.995f)) {
+
+			dFloat ang = dAcos (dot);
+			dFloat sinAng = dSin (ang);
+
+			dFloat den = dFloat(1.0f) / sinAng;
+
+			Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den;
+			Sclq = dSin (t * ang) * den;
+
+		} else  {
+			Sclp = dFloat(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;
+
+		dFloat Sclp = dSin ((dFloat(1.0f) - t) * dFloat (3.141592f *0.5f));
+		dFloat Sclq = dSin (t * dFloat (3.141592f * 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) < (1.0f - 1.0e-4f) ) {
+		dot = dFloat(1.0f) / dSqrt (dot);
+		//dot = dgRsqrt (dot);
+		Q.m_q0 *= dot;
+		Q.m_q1 *= dot;
+		Q.m_q2 *= dot;
+		Q.m_q3 *= dot;
+	}
+	return Q;
+}
+
+dVector dQuaternion::RotateVector (const dVector& point) const
+{
+	dMatrix matrix (*this, dVector (0.0f, 0.0f, 0.0f, 1.0f));
+	return matrix.RotateVector(point);
+}
+
+dVector dQuaternion::UnrotateVector (const dVector& point) const
+{
+	dMatrix matrix (*this, dVector (0.0f, 0.0f, 0.0f, 1.0f));
+	return matrix.UnrotateVector(point);
+}
+
+//dVector dQuaternion::GetEulerAngles (dEulerAngleOrder order) const
+void dQuaternion::GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order) const 
+{
+	dMatrix matrix (*this, dVector (0.0f,0.0f,0.0f,1.0f));
+	//return matrix.GetEulerAngles (order);
+    matrix.GetEulerAngles (euler1, euler2, order);
+}
+
+
+dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
+{
+	// this is correct
+	dQuaternion rotation (*this);
+	dFloat omegaMag2 = omega % omega;
+	const dFloat errAngle = 0.0125f * 3.141592f / 180.0f;
+	const dFloat errAngle2 = errAngle * errAngle;
+	if (omegaMag2 > errAngle2) {
+		dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
+		dVector omegaAxis (omega.Scale (invOmegaMag));
+		dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
+		dQuaternion deltaRotation (omegaAxis, omegaAngle);
+		rotation = rotation * deltaRotation;
+		rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
+	}
+	return rotation;
+}

newtondynamics.mod/NewtonDynamics/packages/dMath/dQuaternion.h

@@ -0,0 +1,156 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+
+
+#ifndef __dQuaternion__
+#define __dQuaternion__
+
+#include "dVector.h"
+class dMatrix;
+
+D_MSC_VECTOR_ALIGMENT
+class dQuaternion
+{
+	public:
+	dQuaternion (); 
+	dQuaternion (const dMatrix& matrix);
+	dQuaternion (dFloat q0, dFloat q1, dFloat q2, dFloat q3); 
+	dQuaternion (const dVector& unit_Axis, dFloat Angle = 0.0f);
+	
+	void Scale (dFloat scale); 
+	void Normalize (); 
+	inline dFloat DotProduct (const dQuaternion& QB) const;
+	dQuaternion Inverse () const; 
+
+	dVector RotateVector (const dVector& point) const;
+	dVector UnrotateVector (const dVector& point) const;
+
+	//dVector GetEulerAngles (dEulerAngleOrder order = m_pitchYawRoll) const;
+    void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
+	dVector CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const;
+	dQuaternion Slerp (const dQuaternion &q1, dFloat t) const;
+	dQuaternion IntegrateOmega (const dVector& omega, dFloat timestep) const;
+
+	dQuaternion operator* (const dQuaternion &B) const;
+	dQuaternion operator+ (const dQuaternion &B) const; 
+	dQuaternion operator- (const dQuaternion &B) const; 
+
+	dFloat m_q0;
+	dFloat m_q1;
+	dFloat m_q2;
+	dFloat m_q3;
+};
+
+
+
+
+inline dQuaternion::dQuaternion () 
+{
+	m_q0 = 1.0f;
+	m_q1 = 0.0f;
+	m_q2 = 0.0f;
+	m_q3 = 0.0f;
+}
+
+inline dQuaternion::dQuaternion (dFloat Q0, dFloat Q1, dFloat Q2, dFloat Q3) 
+{
+	m_q0 = Q0;
+	m_q1 = Q1;
+	m_q2 = Q2;
+	m_q3 = Q3;
+//	dAssert (dAbs (DotProduct (*this) - 1.0f) < 1.0e-4f);
+}
+
+
+
+inline void dQuaternion::Scale (dFloat scale) 
+{
+	m_q0 *= scale;
+	m_q1 *= scale;
+	m_q2 *= scale;
+	m_q3 *= scale;
+}
+
+inline void dQuaternion::Normalize () 
+{
+	Scale (1.0f / dSqrt (DotProduct (*this)));
+}
+
+inline dFloat dQuaternion::DotProduct (const dQuaternion &QB) const
+{
+	return m_q0 * QB.m_q0 + m_q1 * QB.m_q1 + m_q2 * QB.m_q2 + m_q3 * QB.m_q3;
+}
+
+inline dQuaternion dQuaternion::Inverse () const 
+{
+	return dQuaternion (m_q0, -m_q1, -m_q2, -m_q3);
+}
+
+/*
+inline dQuaternion operator+ (const dQuaternion &A, const dQuaternion &B) 
+{
+	return dQuaternion (A.m_q0 + B.m_q0, A.m_q1 + B.m_q1, A.m_q2 + B.m_q2, A.m_q3 + B.m_q3);
+}
+
+inline dQuaternion operator- (const dQuaternion &A, const dQuaternion &B) 
+{
+	return dQuaternion (A.m_q0 - B.m_q0, A.m_q1 - B.m_q1, A.m_q2 - B.m_q2, A.m_q3 - B.m_q3);
+}
+*/
+
+inline dQuaternion dQuaternion::operator+ (const dQuaternion &B) const
+{
+	return dQuaternion (m_q0 + B.m_q0, m_q1 + B.m_q1, m_q2 + B.m_q2, m_q3 + B.m_q3);
+}
+
+inline dQuaternion dQuaternion::operator- (const dQuaternion &B) const
+{
+	return dQuaternion (m_q0 - B.m_q0, m_q1 - B.m_q1, m_q2 - B.m_q2, m_q3 - B.m_q3);
+}
+
+
+inline dQuaternion dQuaternion::operator* (const dQuaternion &B) const
+{
+	return dQuaternion (B.m_q0 * m_q0 - B.m_q1 * m_q1 - B.m_q2 * m_q2 - B.m_q3 * m_q3, 
+				 		B.m_q1 * m_q0 + B.m_q0 * m_q1 - B.m_q3 * m_q2 + B.m_q2 * m_q3, 
+						B.m_q2 * m_q0 + B.m_q3 * m_q1 + B.m_q0 * m_q2 - B.m_q1 * m_q3, 
+						B.m_q3 * m_q0 - B.m_q2 * m_q1 + B.m_q1 * m_q2 + B.m_q0 * m_q3); 
+}
+
+/*
+inline dVector dQuaternion::GetXYZ_EulerAngles() const
+{
+	dFloat pitch;
+	dFloat yaw;
+	dFloat roll;
+	dFloat val = 2.0f * (m_q2 * m_q0 - m_q3 * m_q1);
+	if (val  >= 0.99995f) {
+		pitch = 2.0f * atan2(m_q1, m_q0);
+		yaw = (0.5f * 3.141592f);
+		roll = 0.0f;
+	} else if (val  <= -0.99995f) {
+		pitch = 2.0f * atan2(m_q1, m_q0);
+		yaw = -(0.5f * 3.141592f);
+		roll = 0.0f;
+	} else {
+		yaw = dAsin (val);
+		pitch = dAtan2 (2.0f * (m_q1 * m_q0 + m_q3 * m_q2), 1.0f - 2.0f * (m_q1 * m_q1 + m_q2 * m_q2));
+		roll = dAtan2 (2.0f * (m_q3 * m_q0 + m_q1 * m_q2), 1.0f - 2.0f * (m_q2 * m_q2 + m_q3 * m_q3));
+	}
+	return dVector (pitch, yaw, roll);
+}
+*/
+
+
+
+#endif 
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dStdAfxMath.cpp

@@ -0,0 +1,20 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+// stdafx.cpp : source file that includes just the standard includes
+// MinimalMath.pch will be the pre-compiled header
+// stdafx.obj will contain the pre-compiled type information
+
+#include "dStdAfxMath.h"
+
+// TODO: reference any additional headers you need in STDAFX.H
+// and not in this file
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dStdAfxMath.h

@@ -0,0 +1,36 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+// stdafx.h : include file for standard system include files,
+// or project specific include files that are used frequently, but
+// are changed infrequently
+//
+
+#ifndef __STDAFXMATH_H__
+#define __STDAFXMATH_H__
+
+
+#ifdef _MSC_VER
+	#ifndef WIN32_LEAN_AND_MEAN
+		#define WIN32_LEAN_AND_MEAN		// Exclude rarely-used stuff from Windows headers
+	#endif
+	#include <windows.h>
+	#include <crtdbg.h>
+#endif
+
+#if ( defined (_MINGW_32_VER) || defined (_MINGW_64_VER) )
+	#include <crtdbg.h>
+#endif
+
+#include <stdio.h>
+#include <assert.h>
+#endif
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dVector.cpp

@@ -0,0 +1,13 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#include "dStdAfxMath.h"
+

newtondynamics.mod/NewtonDynamics/packages/dMath/dVector.h

@@ -0,0 +1,209 @@
+/* Copyright (c) <2009> <Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely
+*/
+
+#ifndef __dVector__
+#define __dVector__
+
+
+#include "dMathDefines.h"
+
+
+// small but very effective 4 dimensional template vector class 
+
+template<class T>
+class TemplateVector
+{
+	public:
+	TemplateVector ();
+	TemplateVector (const T *ptr);
+	TemplateVector (T m_x, T m_y, T m_z, T m_w); 
+	TemplateVector Scale (T s) const;
+
+	T& operator[] (int i);
+	const T& operator[] (int i) const;
+
+	TemplateVector operator+ (const TemplateVector &A) const; 
+	TemplateVector operator- (const TemplateVector &A) const; 
+	TemplateVector &operator+= (const TemplateVector &A);
+	TemplateVector &operator-= (const TemplateVector &A); 
+
+	// return dot product
+	T operator% (const TemplateVector &A) const; 
+
+	// return cross product
+	TemplateVector operator* (const TemplateVector &B) const; 
+
+	// component wise multiplication
+	TemplateVector CompProduct (const TemplateVector &A) const;
+
+	T m_x;
+	T m_y;
+	T m_z;
+	T m_w;
+};
+
+D_MSC_VECTOR_ALIGMENT
+class dVector: public TemplateVector<dFloat>
+{
+	public:
+	dVector();
+	dVector (const TemplateVector<dFloat>& v);
+	dVector (const dFloat *ptr);
+	dVector (dFloat x, dFloat y, dFloat z, dFloat w = 1.0); 
+};
+
+
+class dBigVector: public TemplateVector<double>
+{
+	public: 
+	dBigVector(){};
+	dBigVector (const TemplateVector<dFloat>& v)
+		:TemplateVector<dFloat64> (v.m_x, v.m_y, v.m_z, v.m_w)
+	{
+	}
+
+#ifndef _NEWTON_USE_DOUBLE
+	dBigVector (const TemplateVector<dFloat64>& v)
+		:TemplateVector<dFloat64> (v.m_x, v.m_y, v.m_z, v.m_w)
+	{
+	}
+#endif
+
+	//	dBigVector (const dFloat *ptr);
+	dBigVector (dFloat x, dFloat y, dFloat z, dFloat w = 1.0)
+		:TemplateVector<dFloat64> (x, y, z, w)
+	{
+	}
+
+#ifndef _NEWTON_USE_DOUBLE
+	dBigVector (dFloat64 x, dFloat64 y, dFloat64 z, dFloat64 w = 1.0)
+		:TemplateVector<dFloat64> (x, y, z, w)
+	{
+	}
+#endif
+};
+
+
+
+
+template<class T>
+TemplateVector<T>::TemplateVector() {}
+
+template<class T>
+TemplateVector<T>::TemplateVector(const T *ptr)
+	:m_x (ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w(0.0f)
+{
+}
+
+template<class T>
+TemplateVector<T>::TemplateVector(T x, T y, T z, T w) 
+	:m_x (x), m_y(y), m_z(z), m_w(w)
+{
+}
+
+
+template<class T>
+T& TemplateVector<T>::operator[] (int i)
+{
+	return (&m_x)[i];
+}	
+
+template<class T>
+const T& TemplateVector<T>::operator[] (int i) const
+{
+	return (&m_x)[i];
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::Scale (T scale) const
+{
+	return TemplateVector<T> (m_x * scale, m_y * scale, m_z * scale, m_w);
+}
+
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::operator+ (const TemplateVector<T>& B) const
+{
+	return TemplateVector<T> (m_x + B.m_x, m_y + B.m_y, m_z + B.m_z, m_w);
+}
+
+template<class T>
+TemplateVector<T>& TemplateVector<T>::operator+= (const TemplateVector<T>& A) 
+{
+	m_x += A.m_x;
+	m_y += A.m_y;
+	m_z += A.m_z;
+	return *this;
+}
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::operator- (const TemplateVector<T>& A) const
+{
+	return TemplateVector<T> (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w);
+}
+
+template<class T>
+TemplateVector<T>& TemplateVector<T>::operator-= (const TemplateVector<T>& A) 
+{
+	m_x -= A.m_x;
+	m_y -= A.m_y;
+	m_z -= A.m_z;
+	return *this;
+}
+
+
+template<class T>
+T TemplateVector<T>::operator% (const TemplateVector<T>& A) const
+{
+	return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z;
+}
+
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::operator* (const TemplateVector<T>& B) const
+{
+	return TemplateVector<T> (m_y * B.m_z - m_z * B.m_y,
+ 							    m_z * B.m_x - m_x * B.m_z,
+								m_x * B.m_y - m_y * B.m_x, m_w);
+}
+
+
+
+template<class T>
+TemplateVector<T> TemplateVector<T>::CompProduct (const TemplateVector<T>& A) const
+{
+	return TemplateVector<T> (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w);
+}
+
+
+
+inline dVector::dVector()
+	:TemplateVector<dFloat>()
+{
+}
+
+inline dVector::dVector (const TemplateVector<dFloat>& v)
+	:TemplateVector<dFloat>(v)
+{
+}
+
+inline dVector::dVector (const dFloat *ptr)
+	:TemplateVector<dFloat>(ptr)
+{
+}
+
+inline dVector::dVector (dFloat x, dFloat y, dFloat z, dFloat w) 
+	:TemplateVector<dFloat>(x, y, z, w)
+{
+}
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/ampPhysics/dgAMP.cpp

@@ -0,0 +1,26 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgAMP.h"
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/ampPhysics/dgAMP.h

@@ -0,0 +1,41 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _DG_AMP_H_
+#define _DG_AMP_H_
+
+#pragma warning (disable: 4505) //'Concurrency::details::_Array_view_projection_helper<_T,_R>::_Project0' : unreferenced local function has been removed
+#pragma warning (disable: 4530) //: C++ exception handler used, but unwind semantics are not enabled. Specify /EHsc
+
+#include <amp.h>
+#include <ppl.h>
+#include <amprt.h>
+#include <amp_math.h>
+#include <amp_short_vectors.h>
+
+#include <dgPhysicsStdafx.h>
+#include <dgBody.h>
+#include <dgWorld.h>
+#include <dgConstraint.h>
+#include <dgDynamicBody.h>
+#include <dgWorldDynamicUpdate.h>
+
+#endif

newtondynamics.mod/NewtonDynamics/source/ampPhysics/dgAmpAllocator.h

@@ -0,0 +1,85 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _DG_AMP_ALLOCATOR_H_
+#define _DG_AMP_ALLOCATOR_H_
+
+#include "dgAMP.h"
+
+template <typename T>
+class dgAmpAllocator: public std::allocator<T>
+{
+	public:
+	typedef size_t size_type;
+	typedef T* pointer;
+	typedef const T* const_pointer;
+
+	template<typename _Tp1>
+	struct rebind
+	{
+		typedef dgAmpAllocator<_Tp1> other;
+	};
+
+	pointer allocate(size_type n, const void *hint=0)
+	{
+		return (pointer)m_allocator->Malloc(dgInt32 (n * sizeof (T)));
+	}
+
+	void deallocate(pointer p, size_type n)
+	{
+		m_allocator->Free(p);
+	}
+
+	dgAmpAllocator(dgMemoryAllocator* const allocator) throw()
+		:std::allocator<T>() 
+		,m_allocator(allocator)
+	{ 
+	}
+
+	dgAmpAllocator() throw()
+		:std::allocator<T>() 
+		,m_allocator(NULL)
+	{ 
+		dgAssert (0);
+	}
+
+	dgAmpAllocator(const dgAmpAllocator &a) throw()
+		:std::allocator<T>(a) 
+		,m_allocator(a.m_allocator)
+	{ 
+	}
+
+	template <class U>                    
+	dgAmpAllocator(const dgAmpAllocator<U> &a) throw()
+		:std::allocator<T>(a) 
+		,m_allocator(a.m_allocator)
+	{ 
+	}
+
+	~dgAmpAllocator() throw() 
+	{ 
+	}
+	
+	dgMemoryAllocator* m_allocator;
+};
+
+
+#endif

newtondynamics.mod/NewtonDynamics/source/ampPhysics/dgAmpInstance.cpp

@@ -0,0 +1,182 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "dgAMP.h"
+#include "dgAmpInstance.h"
+
+
+dgAmpBodyData::dgAmpBodyData (dgMemoryAllocator* const allocator)
+	:m_currentSize(1)
+	,m_bodyInvMass(m_currentSize)
+	,m_bodyDamp(m_currentSize)
+	,m_bodyVelocity(m_currentSize)
+	,m_bodyNetForce(m_currentSize)
+	,m_bodyInternalForce(m_currentSize)
+	,m_bodyMatrix(m_currentSize)
+	,m_bodyInvInertiaMatrix(m_currentSize)
+	,m_bodyInvMassCpu(m_bodyInvMass)
+	,m_bodyDampCpu(m_bodyDamp)
+	,m_bodyMatrixCpu(m_bodyMatrix)
+	,m_bodyVelocityCpu(m_bodyVelocity)
+{
+}
+
+void dgAmpBodyData::CLean ()
+{
+	m_currentSize = 1;
+	Alloc (2);
+
+	//m_bodyInvMassCpu.shrink_to_fit();
+	//m_bodyDampCpu.shrink_to_fit();
+	//m_bodyMatrixCpu.shrink_to_fit();
+	//m_bodyVelocityCpu.shrink_to_fit();
+}
+
+void dgAmpBodyData::Alloc (dgInt32 count)
+{
+	if (m_currentSize < count) {
+		while (m_currentSize < count) {
+			m_currentSize *= 2;
+		}
+		m_bodyInvMass = array<float_4, 1> (m_currentSize);
+		m_bodyDamp = array<dgAmpJacobian, 1>(m_currentSize);
+		m_bodyVelocity = array<dgAmpJacobian, 1>(m_currentSize);
+		m_bodyNetForce = array<dgAmpJacobian, 1>(m_currentSize);
+		m_bodyInternalForce = array<dgAmpJacobian, 1>(m_currentSize);
+		m_bodyMatrix = array<dgAmpMatrix4x4, 1> (m_currentSize);
+		m_bodyInvInertiaMatrix = array<dgAmpMatrix4x4 , 1> (m_currentSize);
+		
+		//m_bodyDampCpu.resize (m_currentSize);
+		//m_bodyMatrixCpu.resize (m_currentSize);
+		//m_bodyInvMassCpu.resize (m_currentSize);
+		//m_bodyVelocityCpu.resize (m_currentSize);
+
+		m_bodyDampCpu = array_view<dgAmpJacobian, 1>(m_bodyDamp);
+		m_bodyMatrixCpu = array_view<dgAmpMatrix4x4, 1>(m_bodyMatrix);
+		m_bodyInvMassCpu = array_view<float_4, 1>(m_bodyInvMass);
+		m_bodyVelocityCpu = array_view<dgAmpJacobian, 1>(m_bodyVelocity);
+	}
+}
+
+dgAmpConstraintData::dgAmpConstraintData (dgMemoryAllocator* const allocator)
+	:m_currentSize(1)
+	,m_matrixData(m_currentSize)
+//	,m_matrixDataCpu(dgAmpAllocator<dgAmpJacobianMatrixElement> (allocator))
+	,m_matrixDataCpu(m_matrixData)
+{
+	dgAssert ((sizeof (dgAmpJacobianMatrixElement) & 0x0f) == 0);
+}
+
+void dgAmpConstraintData::CLean ()
+{
+	m_currentSize = 1;
+	Alloc (2);
+//	m_matrixDataCpu.shrink_to_fit();
+}
+
+void dgAmpConstraintData::Alloc (dgInt32 count)
+{
+	if (m_currentSize < count) {
+		while (m_currentSize < count) {
+			m_currentSize *= 2;
+		}
+		m_matrixData = array<dgAmpJacobianMatrixElement, 1> (m_currentSize);
+
+		//m_matrixDataCpu.resize (m_currentSize);
+		m_matrixDataCpu = array_view<dgAmpJacobianMatrixElement, 1> (m_matrixData);
+	}
+}
+
+
+
+dgAmpInstance::dgAmpInstance(dgWorld* const world)
+	:dgAmpBodyData(world->GetAllocator())
+	,dgAmpConstraintData(world->GetAllocator())
+	,m_world (world)
+	,m_accelerator()
+	,m_acceleratorList(world->GetAllocator())
+{
+	const wchar_t* const reference = accelerator::direct3d_ref;
+	const wchar_t* const cpuAccel = accelerator::cpu_accelerator;
+
+    std::vector<accelerator> accs = accelerator::get_all();
+    for (int i = 0; i < int (accs.size()); i++) {
+		accelerator &accel = accs[i];
+		std::wstring path  (accel.get_device_path());
+		if (!((path == reference) || (path == cpuAccel))) { 
+			std::wstring desc  (accel.get_description());
+			std::string p (path.begin(), path.end() );
+			std::string d (desc.begin(), desc.end() );
+			dgAcceleratorDescription& info = m_acceleratorList.Append()->GetInfo();
+			strncpy (info.m_path, p.c_str(), sizeof (info.m_path));
+			strncpy (info.m_description, d.c_str(), sizeof (info.m_description));
+		}
+    }
+}
+
+
+dgAmpInstance::~dgAmpInstance(void)
+{
+}
+
+void dgAmpInstance::CleanUp()
+{
+	dgAmpBodyData::CLean();
+	dgAmpConstraintData::CLean();
+}
+
+dgInt32 dgAmpInstance::GetPlatformsCount() const
+{
+	return m_acceleratorList.GetCount();
+}
+
+
+void dgAmpInstance::SelectPlaform(dgInt32 deviceIndex)
+{
+	int index = 0;
+	for (dgList<dgAcceleratorDescription>::dgListNode* node = m_acceleratorList.GetFirst(); node; node = node->GetNext()) {
+		if (index == deviceIndex) {
+			const dgAcceleratorDescription &accel = node->GetInfo();
+			std::string p (accel.m_path);
+			std::wstring path (p.begin(), p.end());
+			accelerator::set_default (path);  
+			m_accelerator = accelerator (path);
+			break;
+		}
+		index ++;
+	}
+}
+
+
+void dgAmpInstance::GetVendorString(dgInt32 deviceIndex, char* const name, dgInt32 maxlength) const
+{
+	name[0] = 0;
+	int index = 0;
+	for (dgList<dgAcceleratorDescription>::dgListNode* node = m_acceleratorList.GetFirst(); node; node = node->GetNext()) {
+		if (index == deviceIndex) {
+			const dgAcceleratorDescription &accel = node->GetInfo();
+			strncpy (name, accel.m_description, maxlength);
+			break;
+		}
+		index ++;
+	}
+}

newtondynamics.mod/NewtonDynamics/source/ampPhysics/dgAmpInstance.h

@@ -0,0 +1,244 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _DG_AMP_INSTANCE_H_
+#define _DG_AMP_INSTANCE_H_
+
+#include "dgAMP.h"
+#include "dgAmpAllocator.h"
+
+using namespace concurrency;
+using namespace concurrency::graphics;
+
+class dgAmpJacobian
+{
+	public:
+	float_4 m_linear;
+	float_4 m_angular;
+};
+
+class dgAmpJacobianPair
+{
+	public:
+	dgAmpJacobian m_jacobianM0;
+	dgAmpJacobian m_jacobianM1;
+};
+
+class dgAmpMatrixRightSide
+{
+	public:
+	union 
+	{
+		struct 
+		{
+//	float m_force;
+//	float m_accel;
+//	float m_deltaAccel;
+//	float m_deltaForce;
+//	float m_invDJMinvJt;
+//	float m_maxImpact;
+			float m_diagDamp;
+			float m_restitution;
+			float m_penetration;
+			float m_penetrationStiffness;
+
+			float m_coordenateAccel;
+			float m_lowerBoundFrictionCoefficent;
+			float m_upperBoundFrictionCoefficent;
+
+			//dgForceImpactPair* m_jointFeebackForce;
+			int m_jointFeebackForce[2];
+			int m_normalForceIndex;
+			int m_accelIsMotor;
+		};
+		float m_data[3][4];
+	};
+};
+
+
+class dgAmpMatrix4x4 
+{
+	public:
+	dgAmpMatrix4x4()
+	{
+	}
+
+	dgAmpMatrix4x4 (const float_4& v0, const float_4& v1, const float_4& v2, const float_4& v3)  restrict(amp,cpu)
+	{
+		m_row[0] = v0;
+		m_row[1] = v1;
+		m_row[2] = v2;
+		m_row[3] = v3;
+	}
+	float_4 m_row[4];
+};
+
+
+class dgAmpBodyData
+{
+	public:
+	dgAmpBodyData (dgMemoryAllocator* const allocator);
+	void Alloc (dgInt32 count);
+	void CLean ();
+
+	dgInt32 m_currentSize;
+	array<float_4, 1> m_bodyInvMass;
+	array<dgAmpJacobian, 1> m_bodyDamp;
+	array<dgAmpJacobian, 1> m_bodyVelocity;
+	array<dgAmpJacobian, 1> m_bodyNetForce;
+	array<dgAmpJacobian, 1> m_bodyInternalForce;
+	array<dgAmpMatrix4x4 , 1> m_bodyMatrix;
+	array<dgAmpMatrix4x4 , 1> m_bodyInvInertiaMatrix;
+	
+	//std::vector<float_4, dgAmpAllocator<float_4>> m_bodyInvMassCpu;
+	//std::vector<dgAmpJacobian, dgAmpAllocator<dgAmpJacobian>> m_bodyDampCpu;
+	//std::vector<dgAmpMatrix4x4, dgAmpAllocator<dgAmpJacobian>> m_bodyMatrixCpu;
+	//std::vector<dgAmpJacobian, dgAmpAllocator<dgAmpJacobian>> m_bodyVelocityCpu;
+	array_view<float_4, 1> m_bodyInvMassCpu;
+	array_view<dgAmpJacobian, 1> m_bodyDampCpu;
+	array_view<dgAmpMatrix4x4, 1> m_bodyMatrixCpu;
+	array_view<dgAmpJacobian, 1> m_bodyVelocityCpu;
+};
+
+class dgAmpConstraintData
+{
+	public:
+	class dgAmpJacobianMatrixElement
+	{
+		public:
+		dgAmpJacobianPair m_Jt;
+		dgAmpMatrixRightSide m_data;
+	};
+
+	dgAmpConstraintData (dgMemoryAllocator* const allocator);
+	void Alloc (dgInt32 size);
+	void CLean ();
+
+	dgInt32 m_currentSize;
+	array<dgAmpJacobianMatrixElement, 1> m_matrixData;
+
+	//std::vector<dgAmpJacobianMatrixElement, dgAmpAllocator<dgAmpJacobianMatrixElement>> m_matrixDataCpu;
+	array_view<dgAmpJacobianMatrixElement, 1> m_matrixDataCpu;
+};
+
+
+class dgAmpInstance: public dgAmpBodyData, public dgAmpConstraintData
+{
+	public:
+	class dgAcceleratorDescription
+	{
+		public:
+		char m_path[128];
+		char m_description[128];
+	};
+
+	DG_CLASS_ALLOCATOR(allocator);
+
+	dgAmpInstance(dgWorld* const world);
+	~dgAmpInstance(void);
+
+	void CleanUp();
+	dgInt32 GetPlatformsCount() const;
+	void SelectPlaform(dgInt32 deviceIndex);
+	void GetVendorString(dgInt32 deviceIndex, char* const name, dgInt32 maxlength) const;
+
+	void ConstraintSolver (dgInt32 islandCount, const dgIsland* const islandArray, dgFloat32 timestep);
+	
+	private:
+	static float_4 ToFloat4 (const dgVector& v);
+
+	static dgAmpMatrix4x4 Transpose (const dgAmpMatrix4x4& matrix) restrict(amp,cpu);
+	static dgAmpMatrix4x4 Multiply (const dgAmpMatrix4x4& matrixA, const dgAmpMatrix4x4& matrixB) restrict(amp,cpu);
+	
+	static float Dot (const float_4& vectorA, const float_4& vectorB) restrict(amp,cpu);
+	static float_4 Scale (const float_4& vector, float scale) restrict(amp,cpu);
+	static float_4 RotateVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu);
+	static float_4 UnrotateVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu);
+	static float_4 TransformVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu);
+	static float_4 UntransformVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu);
+
+	
+	static void InitializeBodyArrayParallelKernel (void* const context, void* const worldContext, dgInt32 threadID);
+	static void BuildJacobianMatrixParallelKernel (void* const context, void* const worldContext, dgInt32 threadID);
+	void GetJacobianDerivativesParallel (dgJointInfo* const jointInfo, dgInt32 threadIndex, dgInt32 rowBase, dgFloat32 timestep);
+
+	void InitilizeBodyArrayParallel (dgParallelSolverSyncData* const syncData); 
+	void BuildJacobianMatrixParallel (dgParallelSolverSyncData* const syncData);
+	void CreateParallelArrayBatchArrays (dgParallelSolverSyncData* const syncData, const dgIsland* const islandArray, dgInt32 islandCount);
+	
+	static void AddDamingAccelKernel (const dgAmpMatrix4x4& bodyMatrix, const dgAmpJacobian& damping, dgAmpJacobian& veloc) restrict(amp,cpu);
+	static void CalcuateInvInertiaMatrixKernel (const dgAmpMatrix4x4& bodyMatrix, dgAmpMatrix4x4& invInertiaMatrix, const float_4& invInertia) restrict(amp,cpu);
+
+	dgWorld* m_world;
+	accelerator m_accelerator;
+	dgList<dgAcceleratorDescription> m_acceleratorList;
+};
+
+inline float_4 dgAmpInstance::ToFloat4 (const dgVector& v)
+{ 
+	return float_4 (v[0], v[1], v[2], v[3]);
+}
+
+inline float dgAmpInstance::Dot (const float_4& vectorA, const float_4& vectorB) restrict(amp,cpu)
+{
+	return vectorA.get_x() * vectorB.get_x() + vectorA.get_y() * vectorB.get_y() + vectorA.get_z() * vectorB.get_z() + vectorA.get_w() * vectorB.get_w();
+}
+
+inline dgAmpMatrix4x4 dgAmpInstance::Transpose (const dgAmpMatrix4x4& matrix) restrict(amp,cpu)
+{
+	return dgAmpMatrix4x4 (float_4 (matrix.m_row[0].get_x(), matrix.m_row[1].get_x(), matrix.m_row[2].get_x(), matrix.m_row[3].get_x()),  
+					  float_4 (matrix.m_row[0].get_y(), matrix.m_row[1].get_y(), matrix.m_row[2].get_y(), matrix.m_row[3].get_y()),
+					  float_4 (matrix.m_row[0].get_z(), matrix.m_row[1].get_z(), matrix.m_row[2].get_z(), matrix.m_row[3].get_z()),
+					  float_4 (matrix.m_row[0].get_w(), matrix.m_row[1].get_w(), matrix.m_row[2].get_w(), matrix.m_row[3].get_w()));
+}
+
+
+inline float_4 dgAmpInstance::Scale (const float_4& vector, float scale) restrict(amp,cpu)
+{
+	return float_4 (vector * float_4 (scale, scale, scale, scale));
+}
+
+inline float_4 dgAmpInstance::RotateVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu)
+{
+	return float_4 (Scale (matrix.m_row[0], vector.get_x()) + Scale (matrix.m_row[1], vector.get_y()) + Scale (matrix.m_row[2], vector.get_z()));
+}
+
+inline float_4 dgAmpInstance::UnrotateVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu)
+{
+	return float_4 (Dot (matrix.m_row[0], vector), Dot (matrix.m_row[1], vector), Dot (matrix.m_row[2], vector), vector.get_w());
+}
+
+inline float_4 dgAmpInstance::TransformVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu)
+{
+	return float_4 (Scale (matrix.m_row[0], vector.get_x()) + Scale (matrix.m_row[1], vector.get_y()) + Scale (matrix.m_row[2], vector.get_z()) + Scale (matrix.m_row[3], vector.get_w()));
+}
+
+inline float_4 dgAmpInstance::UntransformVector (const dgAmpMatrix4x4& matrix, const float_4& vector) restrict(amp,cpu)
+{
+	return UnrotateVector (matrix, vector - matrix.m_row[3]);
+}
+
+inline dgAmpMatrix4x4 dgAmpInstance::Multiply (const dgAmpMatrix4x4& matrixA, const dgAmpMatrix4x4& matrixB) restrict(amp,cpu)
+{
+	return dgAmpMatrix4x4 (TransformVector(matrixB, matrixA.m_row[0]), TransformVector(matrixB, matrixA.m_row[1]), TransformVector(matrixB, matrixA.m_row[2]), TransformVector(matrixB, matrixA.m_row[3])); 
+}
+
+#endif

newtondynamics.mod/NewtonDynamics/source/ampPhysics/dgAmpSolver.cpp

@@ -0,0 +1,1215 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "dgAMP.h"
+#include "dgAmpInstance.h"
+
+
+
+/*
+void dgWorldDynamicUpdate::CalculateReactionForcesParallel (const dgIsland* const islandArray, dgFloat32 timestep) const
+{
+	dgParallelSolverSyncData syncData;
+
+	dgWorld* const world = (dgWorld*) this;
+
+//	dgInt32 size = m_joints + m_bodies + 1024;   
+//	world->m_pairMemoryBuffer.ExpandCapacityIfNeessesary (size, sizeof (dgParallelJointMap) + sizeof (dgInt32));
+	world->m_pairMemoryBuffer.ExpandCapacityIfNeessesary (m_joints + 1024, sizeof (dgParallelJointMap));
+
+//	syncData.m_bodyAtomic = (dgThread::dgCriticalSection*) (&world->m_pairMemoryBuffer[0]);
+//	syncData.m_jointInfoMap = (dgParallelJointMap*) (&syncData.m_bodyAtomic[(m_bodies + 31) & ~0x0f]);
+	syncData.m_jointInfoMap = (dgParallelJointMap*) (&world->m_pairMemoryBuffer[0]);
+
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+
+	dgInt32 bodyCount = islandArray->m_bodyCount - 1;
+	dgInt32 jointsCount = islandArray->m_jointCount;
+	LinearizeJointParallelArray (&syncData, constraintArray, islandArray);
+	
+	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+	dgVector zero(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+	internalForces[0].m_linear = zero;
+	internalForces[0].m_angular = zero;
+
+	dgInt32 maxPasses = dgInt32 (world->m_solverMode + LINEAR_SOLVER_SUB_STEPS);
+
+
+	syncData.m_timestep = timestep;
+	syncData.m_invTimestep = (timestep > dgFloat32 (0.0f)) ? dgFloat32 (1.0f) / timestep : dgFloat32 (0.0f);
+	syncData.m_invStepRK = (dgFloat32 (1.0f) / dgFloat32 (maxPasses));
+	syncData.m_timestepRK = syncData.m_timestep * syncData.m_invStepRK;
+	syncData.m_invTimestepRK = syncData.m_invTimestep * dgFloat32 (maxPasses);
+	syncData.m_maxPasses = maxPasses;
+
+	syncData.m_bodyCount = bodyCount;
+	syncData.m_jointCount = jointsCount;
+	syncData.m_atomicIndex = 0;
+//	syncData.m_islandCount = islandsCount;
+	syncData.m_islandCount = 1;
+	syncData.m_islandArray = islandArray;
+
+	InitilizeBodyArrayParallel (&syncData);
+	BuilddgAmpJacobianMatrixParallel (&syncData);
+	SolverInitInternalForcesParallel (&syncData);
+	CalculateForcesGameModeParallel (&syncData);
+	IntegrateInslandParallel(&syncData); 
+}
+
+
+
+
+
+
+
+
+
+void dgWorldDynamicUpdate::SolverInitInternalForcesParallel (dgParallelSolverSyncData* const syncData) const
+{
+	dgWorld* const world = (dgWorld*) this;
+	dgInt32 threadCounts = world->GetThreadCount();	
+
+	for (int i = 0; i < syncData->m_batchesCount; i ++) {
+		syncData->m_atomicIndex = syncData->m_jointBatches[i].m_start;
+		syncData->m_jointsInBatch = syncData->m_jointBatches[i].m_count + syncData->m_atomicIndex;
+		for (dgInt32 j = 0; j < threadCounts; j ++) {
+			world->QueueJob (SolverInitInternalForcesParallelKernel, syncData, world);
+		}
+		world->SynchronizationBarrier();
+	}
+
+//	syncData->m_atomicIndex = 0;
+//	for (dgInt32 j = 0; j < threadCounts; j ++) {
+//		world->QueueJob (SolverInitInternalForcesParallelKernel, syncData, world);
+//	}
+	world->SynchronizationBarrier();
+
+//	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+//	internalForces[0].m_linear = dgVector (dgFloat32 (0.0f));
+//	internalForces[0].m_angular = dgVector (dgFloat32 (0.0f));
+
+}
+
+
+void dgWorldDynamicUpdate::SolverInitInternalForcesParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	const dgParallelJointMap* const jointInfoIndexArray = syncData->m_jointInfoMap;
+	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+	dgdgAmpJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0];
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex; 
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+//	dgThread::dgCriticalSection* const bodyAtomic = syncData->m_bodyAtomic;
+
+	dgVector zero(dgFloat32 (0.0f));
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointsInBatch; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+//	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		dgdgAmpJacobian y0;
+		dgdgAmpJacobian y1;
+
+		y0.m_linear = zero;
+		y0.m_angular = zero;
+		y1.m_linear = zero;
+		y1.m_angular = zero;
+
+		dgInt32 jointIndex = jointInfoIndexArray[i].m_jointIndex;
+		dgJointInfo* const jointInfo = &constraintArray[jointIndex];
+
+		dgInt32 first = jointInfo->m_autoPairstart;
+		dgInt32 count = jointInfo->m_autoPaircount;
+		for (dgInt32 j = 0; j < count; j ++) { 
+			dgdgAmpJacobianMatrixElement* const row = &matrixRow[j + first];
+
+			//dgFloat32 val = row->m_force; 
+			dgVector val (row->m_force); 
+			dgAssert (dgCheckFloat(row->m_force));
+			y0.m_linear += row->m_Jt.m_dgAmpJacobianM0.m_linear.CompProduct4(val);
+			y0.m_angular += row->m_Jt.m_dgAmpJacobianM0.m_angular.CompProduct4(val);
+			y1.m_linear += row->m_Jt.m_dgAmpJacobianM1.m_linear.CompProduct4(val);
+			y1.m_angular += row->m_Jt.m_dgAmpJacobianM1.m_angular.CompProduct4(val);
+		}
+
+		dgInt32 m0 = jointInfo->m_m0;
+		dgInt32 m1 = jointInfo->m_m1;
+		dgAssert (m0 != m1);
+
+		//bodyAtomic[m0].Lock();
+		//bodyAtomic[m1].Lock();
+		if (m0) {
+			internalForces[m0].m_linear += y0.m_linear;
+			internalForces[m0].m_angular += y0.m_angular;
+		}
+
+		if (m1) {
+			internalForces[m1].m_linear += y1.m_linear;
+			internalForces[m1].m_angular += y1.m_angular;
+		}
+
+		//bodyAtomic[m1].Unlock();
+		//bodyAtomic[m0].Unlock();
+	}
+}
+
+
+
+
+
+void dgWorldDynamicUpdate::CalculateJointsAccelParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	const dgParallelJointMap* const jointInfoIndexArray = syncData->m_jointInfoMap;
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+	dgdgAmpJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0];
+
+//	dgFloat32 invTimestepSrc = dgFloat32 (1.0f) / syncData->m_timestep;
+//	dgFloat32 invStep = (dgFloat32 (1.0f) / dgFloat32 (LINEAR_SOLVER_SUB_STEPS));
+//	dgFloat32 timestep =  syncData->m_timestep * invStep;
+//	dgFloat32 invTimestep = invTimestepSrc * dgFloat32 (LINEAR_SOLVER_SUB_STEPS);
+
+	dgJointAccelerationDecriptor joindDesc;
+	joindDesc.m_timeStep = syncData->m_invStepRK;
+	joindDesc.m_invTimeStep = syncData->m_invTimestepRK;
+	joindDesc.m_firstPassCoefFlag = syncData->m_firstPassCoef;
+
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex; 
+	if (joindDesc.m_firstPassCoefFlag == dgFloat32 (0.0f)) {
+		//for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointsInBatch;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {	
+		for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+			dgInt32 curJoint = jointInfoIndexArray[i].m_jointIndex;
+			joindDesc.m_rowsCount = constraintArray[curJoint].m_autoPaircount;
+			joindDesc.m_rowMatrix = &matrixRow[constraintArray[curJoint].m_autoPairstart];
+			constraintArray[curJoint].m_joint->JointAccelerations (&joindDesc);
+		}
+	} else {
+		//const dgBodyInfo* const bodyArray = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+		const dgIsland* const island = syncData->m_islandArray;
+		dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+		const dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
+
+		//for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointsInBatch;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {	
+		for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+			dgInt32 curJoint = jointInfoIndexArray[i].m_jointIndex;
+			dgInt32 m0 = constraintArray[curJoint].m_m0;
+			dgInt32 m1 = constraintArray[curJoint].m_m1;
+			const dgBody* const body0 = bodyArray[m0].m_body;
+			const dgBody* const body1 = bodyArray[m1].m_body;
+			if (!(body0->m_resting & body1->m_resting)) {
+				joindDesc.m_rowsCount = constraintArray[curJoint].m_autoPaircount;
+				joindDesc.m_rowMatrix = &matrixRow[constraintArray[curJoint].m_autoPairstart];
+				constraintArray[curJoint].m_joint->JointAccelerations (&joindDesc);
+			}
+		}
+	}
+}
+
+
+void dgWorldDynamicUpdate::CalculateJointsForceParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	
+	const dgParallelJointMap* const jointInfoIndexArray = syncData->m_jointInfoMap;
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+	dgdgAmpJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0];
+	const dgIsland* const island = syncData->m_islandArray;
+	dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	const dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
+//	dgThread::dgCriticalSection* const bodyAtomic = syncData->m_bodyAtomic;
+
+	dgFloat32 cacheForce[DG_CONSTRAINT_MAX_ROWS + 4];
+	cacheForce[0] = dgFloat32 (1.0f);
+	cacheForce[1] = dgFloat32 (1.0f);
+	cacheForce[2] = dgFloat32 (1.0f);
+	cacheForce[3] = dgFloat32 (1.0f);
+	dgFloat32* const normalForce = &cacheForce[4];
+
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex;
+
+	dgVector accNorm (syncData->m_accelNorm[threadID]);
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointsInBatch; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+	//for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		dgInt32 curJoint = jointInfoIndexArray[i].m_jointIndex;
+		dgInt32 m0 = constraintArray[curJoint].m_m0;
+		dgInt32 m1 = constraintArray[curJoint].m_m1;
+
+		//bodyAtomic[m0].Lock();
+		//bodyAtomic[m1].Lock();
+		//if (!m0) {
+			//bodyAtomic[m0].Unlock();
+		//}
+		//if (!m1) {
+			//bodyAtomic[m1].Unlock();
+		//}
+
+		const dgBody* const body0 = bodyArray[m0].m_body;
+		const dgBody* const body1 = bodyArray[m1].m_body;
+
+		if (!(body0->m_resting & body1->m_resting)) {
+			dgInt32 index = constraintArray[curJoint].m_autoPairstart;
+			dgInt32 rowsCount = constraintArray[curJoint].m_autoPaircount;
+
+			dgVector linearM0 (internalForces[m0].m_linear);
+			dgVector angularM0 (internalForces[m0].m_angular);
+			dgVector linearM1 (internalForces[m1].m_linear);
+			dgVector angularM1 (internalForces[m1].m_angular);
+
+			const dgVector invMass0 (body0->m_invMass[3]);
+			const dgMatrix& invInertia0 = body0->m_invWorldInertiaMatrix;
+
+			const dgVector invMass1 (body1->m_invMass[3]);
+			const dgMatrix& invInertia1 = body1->m_invWorldInertiaMatrix;
+
+			for (dgInt32 k = 0; k < rowsCount; k ++) {
+				dgdgAmpJacobianMatrixElement* const row = &matrixRow[index];
+
+				dgAssert (row->m_Jt.m_dgAmpJacobianM0.m_linear.m_w == dgFloat32 (0.0f));
+				dgAssert (row->m_Jt.m_dgAmpJacobianM0.m_angular.m_w == dgFloat32 (0.0f));
+				dgAssert (row->m_Jt.m_dgAmpJacobianM1.m_linear.m_w == dgFloat32 (0.0f));
+				dgAssert (row->m_Jt.m_dgAmpJacobianM1.m_angular.m_w == dgFloat32 (0.0f));
+
+				//dgVector JMinvdgAmpJacobianLinearM0 (row->m_Jt.m_dgAmpJacobianM0.m_linear.Scale3 (invMass0));
+				//dgVector JMinvdgAmpJacobianAngularM0 (invInertia0.UnrotateVector (row->m_Jt.m_dgAmpJacobianM0.m_angular));
+				//dgVector JMinvdgAmpJacobianLinearM1 (row->m_Jt.m_dgAmpJacobianM1.m_linear.Scale3 (invMass1));
+				//dgVector JMinvdgAmpJacobianAngularM1 (invInertia1.UnrotateVector (row->m_Jt.m_dgAmpJacobianM1.m_angular));
+
+				dgVector JMinvdgAmpJacobianLinearM0 (row->m_Jt.m_dgAmpJacobianM0.m_linear.CompProduct4 (invMass0));
+				dgVector JMinvdgAmpJacobianAngularM0 (invInertia0.RotateVector (row->m_Jt.m_dgAmpJacobianM0.m_angular));
+				dgVector JMinvdgAmpJacobianLinearM1 (row->m_Jt.m_dgAmpJacobianM1.m_linear.CompProduct4 (invMass1));
+				dgVector JMinvdgAmpJacobianAngularM1 (invInertia1.RotateVector (row->m_Jt.m_dgAmpJacobianM1.m_angular));
+
+				dgVector a (JMinvdgAmpJacobianLinearM0.CompProduct4(linearM0) + JMinvdgAmpJacobianAngularM0.CompProduct4(angularM0) + JMinvdgAmpJacobianLinearM1.CompProduct4(linearM1) + JMinvdgAmpJacobianAngularM1.CompProduct4(angularM1));
+
+				//dgFloat32 a = row->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - row->m_force * row->m_diagDamp;
+				a = dgVector (row->m_coordenateAccel  - row->m_force * row->m_diagDamp) - a.AddHorizontal();
+				
+				//dgFloat32 f = row->m_force + row->m_invDJMinvJt * a;
+				dgVector f (row->m_force + row->m_invDJMinvJt * a.m_x);
+
+				dgInt32 frictionIndex = row->m_normalForceIndex;
+				dgAssert (((frictionIndex < 0) && (normalForce[frictionIndex] == dgFloat32 (1.0f))) || ((frictionIndex >= 0) && (normalForce[frictionIndex] >= dgFloat32 (0.0f))));
+
+				dgFloat32 frictionNormal = normalForce[frictionIndex];
+				dgVector lowerFrictionForce = (frictionNormal * row->m_lowerBoundFrictionCoefficent);
+				dgVector upperFrictionForce = (frictionNormal * row->m_upperBoundFrictionCoefficent);
+
+				//if (f > upperFrictionForce) {
+				//	a = dgFloat32 (0.0f);
+				//	f = upperFrictionForce;
+				//} else if (f < lowerFrictionForce) {
+				//	a = dgFloat32 (0.0f);
+				//	f = lowerFrictionForce;
+				//}
+				a = a.AndNot((f > upperFrictionForce) | (f < lowerFrictionForce));
+				f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce);
+
+				accNorm = accNorm.GetMax(a.Abs());
+				dgAssert (accNorm.m_x >= dgAbsf (a.m_x));
+
+				// no early out for parallel solver
+				//accNorm = accNorm.GetMax(a.Abs());
+				//dgAssert (accNorm.m_x >= dgAbsf (a.m_x));
+
+				//dgFloat32 prevValue = f - row->m_force;
+				dgVector prevValue (f - dgVector (row->m_force));
+
+				row->m_force = f.m_x;
+				normalForce[k] = f.m_x;
+				row->m_maxImpact = f.Abs().GetMax (row->m_maxImpact).m_x;
+
+				linearM0 += row->m_Jt.m_dgAmpJacobianM0.m_linear.CompProduct4 (prevValue);
+				angularM0 += row->m_Jt.m_dgAmpJacobianM0.m_angular.CompProduct4 (prevValue);
+				linearM1 += row->m_Jt.m_dgAmpJacobianM1.m_linear.CompProduct4 (prevValue);
+				angularM1 += row->m_Jt.m_dgAmpJacobianM1.m_angular.CompProduct4 (prevValue);
+				index ++;
+			}
+
+			internalForces[m0].m_linear = linearM0;
+			internalForces[m0].m_angular = angularM0;
+		
+			internalForces[m1].m_linear = linearM1;
+			internalForces[m1].m_angular = angularM1;
+		}
+
+		//if (m0) {
+			//bodyAtomic[m0].Unlock();
+		//}
+		//if (m1) {
+			//bodyAtomic[m1].Unlock();
+		//}
+	}
+	syncData->m_accelNorm[threadID] = accNorm;
+}
+
+void dgWorldDynamicUpdate::CalculateJointsVelocParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+
+	//	const dgInt32* const bodyInfoIndexArray = syncData->m_bodyInfoMap;
+	//	dgBodyInfo* const bodyArray = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	const dgIsland* const island = syncData->m_islandArray;
+	dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
+
+	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+	//dgdgAmpJacobian* const internalVeloc = &world->m_solverMemory.m_internalVeloc[0];
+	//dgFloat32 invStep = (dgFloat32 (1.0f) / dgFloat32 (LINEAR_SOLVER_SUB_STEPS));
+	//dgFloat32 timestep = syncData->m_timestep * invStep;
+	//dgFloat32 invStep = syncData->m_invStepRK;
+
+	dgVector speedFreeze2 (world->m_freezeSpeed2 * dgFloat32 (0.1f));
+	dgVector freezeOmega2 (world->m_freezeOmega2 * dgFloat32 (0.1f));
+	//dgVector forceActiveMask ((jointCount <= DG_SMALL_ISLAND_COUNT) ?  dgFloat32 (-1.0f): dgFloat32 (0.0f));
+
+	dgVector timestep4 (syncData->m_timestepRK);
+
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex;
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		//dgInt32 index = bodyInfoIndexArray[i];
+		//dgInt32 index = i + 1;
+		//dgAssert (index);
+		dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
+		dgAssert (body->m_index == i);
+
+		//dgVector force (body->m_accel + internalForces[index].m_linear);
+		//dgVector torque (body->m_alpha + internalForces[index].m_angular);
+		dgVector force (internalForces[i].m_linear);
+		dgVector torque (internalForces[i].m_angular);
+		if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
+			force += body->m_accel;
+			torque += body->m_alpha;
+		}
+
+		//		dgVector accel (force.Scale4 (body->m_invMass.m_w));
+		//		dgVector alpha (body->m_invWorldInertiaMatrix.RotateVector (torque));
+		//		body->m_veloc += accel.CompProduct4(timestep4);
+		//		body->m_omega += alpha.CompProduct4(timestep4);
+		dgVector velocStep ((force.Scale4 (body->m_invMass.m_w)).CompProduct4(timestep4));
+		dgVector omegaStep ((body->m_invWorldInertiaMatrix.RotateVector (torque)).CompProduct4(timestep4));
+		if (!body->m_resting) {
+			body->m_veloc += velocStep;
+			body->m_omega += omegaStep;
+		} else {
+			dgVector velocStep2 (velocStep.DotProduct4(velocStep));
+			dgVector omegaStep2 (omegaStep.DotProduct4(omegaStep));
+			dgVector test ((velocStep2 > speedFreeze2) | (omegaStep2 > speedFreeze2));
+			if (test.GetSignMask()) {
+				body->m_resting = false;
+			}
+		}
+	}
+}
+
+
+void dgWorldDynamicUpdate::CalculateJointsImpulseVelocParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	//	const dgInt32* const bodyInfoIndexArray = syncData->m_bodyInfoMap;
+
+	//	dgBodyInfo* const bodyArray = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	const dgIsland* const island = syncData->m_islandArray;
+	dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
+
+	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+	//dgdgAmpJacobian* const internalVeloc = &world->m_solverMemory.m_internalVeloc[0];
+
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex;
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		//dgInt32 index = bodyInfoIndexArray[i];
+		dgInt32 index = i + 1;
+		dgAssert (index);
+		dgDynamicBody* const body = (dgDynamicBody*) bodyArray[index].m_body;
+		dgAssert (body->m_index == index);
+
+		const dgVector& linearMomentum = internalForces[index].m_linear;
+		const dgVector& angularMomentum = internalForces[index].m_angular;
+
+		body->m_veloc += linearMomentum.Scale4(body->m_invMass.m_w);
+		body->m_omega += body->m_invWorldInertiaMatrix.RotateVector (angularMomentum);
+
+		//internalVeloc[index].m_linear += body->m_veloc;
+		//internalVeloc[index].m_angular += body->m_omega;
+	}
+}
+
+
+void dgWorldDynamicUpdate::UpdateFeedbackForcesParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	const dgParallelJointMap* const jointInfoIndexArray = syncData->m_jointInfoMap;
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+	dgdgAmpJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0];
+
+	dgInt32 hasJointFeeback = 0;
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex;
+	//for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointsInBatch; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		dgInt32 curJoint = jointInfoIndexArray[i].m_jointIndex;
+		dgInt32 first = constraintArray[curJoint].m_autoPairstart;
+		dgInt32 count = constraintArray[curJoint].m_autoPaircount;
+
+		for (dgInt32 j = 0; j < count; j ++) { 
+			dgdgAmpJacobianMatrixElement* const row = &matrixRow[j + first];
+			dgFloat32 val = row->m_force; 
+			dgAssert (dgCheckFloat(val));
+			row->m_jointFeebackForce[0].m_force = val;
+			row->m_jointFeebackForce[0].m_impact = row->m_maxImpact * syncData->m_timestepRK;
+		}
+		hasJointFeeback |= (constraintArray[i].m_joint->m_updaFeedbackCallback ? 1 : 0);
+	}
+	syncData->m_hasJointFeeback[threadID] = hasJointFeeback;
+}
+
+
+void dgWorldDynamicUpdate::UpdateBodyVelocityParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+
+//	const dgInt32* const bodyInfoIndexArray = syncData->m_bodyInfoMap;
+//	dgBodyInfo* const bodyArray = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	const dgIsland* const island = syncData->m_islandArray;
+	dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
+
+	dgFloat32 maxAccNorm2 = DG_SOLVER_MAX_ERROR * DG_SOLVER_MAX_ERROR;
+	dgVector zero(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+
+	//dgFloat32 invTimestepSrc = dgFloat32 (1.0f) / syncData->m_timestep;
+	dgFloat32 invTimestepSrc = syncData->m_invTimestep;
+
+	dgVector invTime (invTimestepSrc);
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex;
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		//dgInt32 index = bodyInfoIndexArray[i];
+		//dgInt32 index = i + 1;
+		//dgAssert (index);
+		dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
+
+		// the initial velocity and angular velocity were stored in net force and net torque, for memory saving
+		dgVector accel = (body->m_veloc - body->m_netForce).CompProduct4 (invTime);
+		dgVector alpha = (body->m_omega - body->m_netTorque).CompProduct4 (invTime);
+
+		if ((accel % accel) < maxAccNorm2) {
+			accel = zero;
+		}
+
+		if ((alpha % alpha) < maxAccNorm2) {
+			alpha = zero;
+		}
+
+		if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
+			body->m_accel = accel;
+			body->m_alpha = alpha;
+		}
+		body->m_netForce = accel.Scale4 (body->m_mass[3]);
+
+		alpha = body->m_matrix.UnrotateVector(alpha);
+		body->m_netTorque = body->m_matrix.RotateVector (alpha.CompProduct4(body->m_mass));
+	}
+}
+
+
+void dgWorldDynamicUpdate::KinematicCallbackUpdateParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	const dgParallelJointMap* const jointInfoIndexArray = syncData->m_jointInfoMap;
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex;
+	//for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointsInBatch; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		dgInt32 curJoint = jointInfoIndexArray[i].m_jointIndex;
+		if (constraintArray[curJoint].m_joint->m_updaFeedbackCallback) {
+			constraintArray[curJoint].m_joint->m_updaFeedbackCallback (*constraintArray[curJoint].m_joint, syncData->m_timestep, threadID);
+		}
+	}
+}
+
+
+void dgWorldDynamicUpdate::IntegrateInslandParallel(dgParallelSolverSyncData* const syncData) const
+{
+	dgWorld* const world = (dgWorld*) this;
+
+//	dgInt32 threadCounts = world->GetThreadCount();	
+//	syncData->m_atomicIndex = 0;
+//	for (dgInt32 j = 0; j < threadCounts; j ++) {
+//		world->QueueJob (IntegrateInslandParallelKernel, syncData, world);
+//	}
+//	world->SynchronizationBarrier();
+	dgWorldDynamicUpdate::IntegrateInslandParallelKernel (syncData, world, 0);
+}
+
+void dgWorldDynamicUpdate::IntegrateInslandParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) 
+{
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	dgFloat32 timestep = syncData->m_timestep;
+	dgInt32* const atomicIndex = &syncData->m_islandCountCounter; 
+	const dgIsland* const islandArray = syncData->m_islandArray;
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_islandCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		const dgIsland* const island = &islandArray[i];
+		world->IntegrateArray (island, DG_SOLVER_MAX_ERROR, timestep, threadID); 
+	}
+} 
+
+
+void dgWorldDynamicUpdate::CalculateForcesGameModeParallel (dgParallelSolverSyncData* const syncData) const
+{
+	dgWorld* const world = (dgWorld*) this;
+	dgInt32 threadCounts = world->GetThreadCount();	
+
+	dgInt32 maxPasses = syncData->m_maxPasses;
+	syncData->m_firstPassCoef = dgFloat32 (0.0f);
+	for (dgInt32 step = 0; step < maxPasses; step ++) {
+
+		//		for (int i = 0; i < syncData->m_batchesCount; i ++) {
+		//			syncData->m_atomicIndex = syncData->m_jointBatches[i].m_start;
+		//			syncData->m_jointsInBatch = syncData->m_jointBatches[i].m_count + syncData->m_atomicIndex;
+		//			for (dgInt32 j = 0; j < threadCounts; j ++) {
+		//				world->QueueJob (CalculateJointsAccelParallelKernel, syncData, world);
+		//			}
+		//			world->SynchronizationBarrier();
+		//		}
+
+		syncData->m_atomicIndex = 0;
+		for (dgInt32 j = 0; j < threadCounts; j ++) {
+			world->QueueJob (CalculateJointsAccelParallelKernel, syncData, world);
+		}
+		world->SynchronizationBarrier();
+		syncData->m_firstPassCoef = dgFloat32 (1.0f);
+
+		dgFloat32 accNorm = DG_SOLVER_MAX_ERROR * dgFloat32 (2.0f);
+		for (dgInt32 passes = 0; (passes < DG_BASE_ITERATION_COUNT) && (accNorm > DG_SOLVER_MAX_ERROR); passes ++) {
+			for (dgInt32 i = 0; i < threadCounts; i ++) {
+				syncData->m_accelNorm[i] = dgVector (dgFloat32 (0.0f));
+			}
+			for (int i = 0; i < syncData->m_batchesCount; i ++) {
+				syncData->m_atomicIndex = syncData->m_jointBatches[i].m_start;
+				syncData->m_jointsInBatch = syncData->m_jointBatches[i].m_count + syncData->m_atomicIndex;
+				for (dgInt32 j = 0; j < threadCounts; j ++) {
+					world->QueueJob (CalculateJointsForceParallelKernel, syncData, world);
+				}
+				world->SynchronizationBarrier();
+			}
+			//syncData->m_atomicIndex = 0;
+			//for (dgInt32 j = 0; j < threadCounts; j ++) {
+			//	world->QueueJob (CalculateJointsForceParallelKernel, syncData, world);
+			//}
+			//world->SynchronizationBarrier();
+
+			accNorm = dgFloat32 (0.0f);
+			for (dgInt32 i = 0; i < threadCounts; i ++) {
+				accNorm = dgMax (accNorm, syncData->m_accelNorm[i].m_x);
+			}
+		}
+
+		syncData->m_atomicIndex = 1;
+		if (syncData->m_timestep != dgFloat32 (0.0f)) {
+			for (dgInt32 j = 0; j < threadCounts; j ++) {
+				world->QueueJob (CalculateJointsVelocParallelKernel, syncData, world);
+			}
+		} else {
+			for (dgInt32 j = 0; j < threadCounts; j ++) {
+				world->QueueJob (CalculateJointsImpulseVelocParallelKernel, syncData, world);
+			}
+		}
+		world->SynchronizationBarrier();
+
+	}
+
+
+	//	for (int i = 0; i < syncData->m_batchesCount; i ++) {
+	//		syncData->m_atomicIndex = syncData->m_jointBatches[i].m_start;
+	//		syncData->m_jointsInBatch = syncData->m_jointBatches[i].m_count + syncData->m_atomicIndex;
+	//		for (dgInt32 j = 0; j < threadCounts; j ++) {
+	//			world->QueueJob (UpdateFeedbackForcesParallelKernel, syncData, world);
+	//		}
+	//		world->SynchronizationBarrier();
+	//	}
+
+	syncData->m_atomicIndex = 0;
+	for (dgInt32 j = 0; j < threadCounts; j ++) {
+		world->QueueJob (UpdateFeedbackForcesParallelKernel, syncData, world);
+	}
+	world->SynchronizationBarrier();
+
+	dgInt32 hasJointFeeback = 0;
+	for (dgInt32 i = 0; i < DG_MAX_THREADS_HIVE_COUNT; i ++) {
+		hasJointFeeback |= syncData->m_hasJointFeeback[i];
+	}
+
+	syncData->m_atomicIndex = 1;
+	for (dgInt32 j = 0; j < threadCounts; j ++) {
+		world->QueueJob (UpdateBodyVelocityParallelKernel, syncData, world);
+	}
+	world->SynchronizationBarrier();
+
+	if (hasJointFeeback) {
+		//		for (int i = 0; i < syncData->m_batchesCount; i ++) {
+		//			syncData->m_atomicIndex = syncData->m_jointBatches[i].m_start;
+		//			syncData->m_jointsInBatch = syncData->m_jointBatches[i].m_count + syncData->m_atomicIndex;
+		//			for (dgInt32 j = 0; j < threadCounts; j ++) {
+		//				world->QueueJob (KinematicCallbackUpdateParallelKernel, syncData, world);
+		//			}
+		//			world->SynchronizationBarrier();
+		//		}
+
+		syncData->m_atomicIndex = 0;
+		for (dgInt32 j = 0; j < threadCounts; j ++) {
+			world->QueueJob (KinematicCallbackUpdateParallelKernel, syncData, world);
+		}
+		world->SynchronizationBarrier();
+	}
+}
+*/
+
+
+
+//void dgWorldDynamicUpdate::CalculateReactionForcesParallel (const dgIsland* const islandArray, dgFloat32 timestep) const
+void dgAmpInstance::ConstraintSolver (dgInt32 islandCount, const dgIsland* const islandArray, dgFloat32 timestep)
+{
+	dgParallelSolverSyncData syncData;
+	const dgWorldDynamicUpdate& dynUpdate = *m_world;
+	m_world->m_pairMemoryBuffer.ExpandCapacityIfNeessesary (dynUpdate.m_bodies + dynUpdate.m_joints + 1024, sizeof (dgParallelJointMap));
+
+//	dgParallelJointMap* const jointInfoMap = (dgParallelJointMap*) (&m_world->m_pairMemoryBuffer[0]);
+	syncData.m_jointInfoMap = (dgParallelJointMap*) (&m_world->m_pairMemoryBuffer[0]);
+	syncData.m_bodyInfoMap = (dgBody**) (&syncData.m_jointInfoMap[dynUpdate.m_joints + 32]);
+
+//	dgJointInfo* const constraintArray = (dgJointInfo*) &m_world->m_jointsMemory[0];
+//	dgInt32 bodyCount = islandArray->m_bodyCount - 1;
+//	dgInt32 jointsCount = islandArray->m_jointCount;
+
+	CreateParallelArrayBatchArrays (&syncData, islandArray, islandCount);
+
+/*
+	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+	dgVector zero(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+	internalForces[0].m_linear = zero;
+	internalForces[0].m_angular = zero;
+	dgInt32 maxPasses = dgInt32 (world->m_solverMode + LINEAR_SOLVER_SUB_STEPS);
+*/
+	syncData.m_timestep = timestep;
+/*
+	syncData.m_invTimestep = (timestep > dgFloat32 (0.0f)) ? dgFloat32 (1.0f) / timestep : dgFloat32 (0.0f);
+	syncData.m_invStepRK = (dgFloat32 (1.0f) / dgFloat32 (maxPasses));
+	syncData.m_timestepRK = syncData.m_timestep * syncData.m_invStepRK;
+	syncData.m_invTimestepRK = syncData.m_invTimestep * dgFloat32 (maxPasses);
+	syncData.m_maxPasses = maxPasses;
+
+	syncData.m_bodyCount = bodyCount;
+	syncData.m_jointCount = jointsCount;
+	syncData.m_atomicIndex = 0;
+//	syncData.m_islandCount = islandsCount;
+	syncData.m_islandCount = 1;
+	syncData.m_islandArray = islandArray;
+*/
+	InitilizeBodyArrayParallel (&syncData);
+	BuildJacobianMatrixParallel (&syncData);
+/*
+	SolverInitInternalForcesParallel (&syncData);
+	CalculateForcesGameModeParallel (&syncData);
+	IntegrateInslandParallel(&syncData); 
+*/
+}
+
+
+void dgAmpInstance::AddDamingAccelKernel (const dgAmpMatrix4x4& bodyMatrix, const dgAmpJacobian& damping, dgAmpJacobian& veloc) restrict(amp,cpu)
+{
+//	m_veloc -= m_veloc.Scale4 (m_dampCoef.m_w);
+//	dgVector omega (m_matrix.UnrotateVector (m_omega));
+//	omega -= omega.CompProduct4 (m_dampCoef);
+//	m_omega = m_matrix.RotateVector (omega);
+
+	float_4 localOmega (UnrotateVector (bodyMatrix, veloc.m_angular));
+	localOmega -= localOmega * damping.m_angular;
+
+	veloc.m_linear -= veloc.m_linear * damping.m_linear;
+	veloc.m_angular = RotateVector (bodyMatrix, localOmega);
+}
+
+
+void dgAmpInstance::CalcuateInvInertiaMatrixKernel (const dgAmpMatrix4x4& bodyMatrix, dgAmpMatrix4x4& invInertiaMatrix, const float_4& invInertia) restrict(amp,cpu)
+{
+//	dgMatrix tmp (m_matrix.Transpose4X4());
+//	tmp[0] = tmp[0].CompProduct4(m_invMass);
+//	tmp[1] = tmp[1].CompProduct4(m_invMass);
+//	tmp[2] = tmp[2].CompProduct4(m_invMass);
+//	return dgMatrix (m_matrix.RotateVector(tmp[0]), m_matrix.RotateVector(tmp[1]), m_matrix.RotateVector(tmp[2]), dgVector::m_wOne);
+
+	dgAmpMatrix4x4 tmp (Transpose(bodyMatrix));
+	tmp.m_row[0] *= invInertia;
+	tmp.m_row[1] *= invInertia;
+	tmp.m_row[2] *= invInertia;
+	invInertiaMatrix.m_row[0] = RotateVector(bodyMatrix, tmp.m_row[0]);
+	invInertiaMatrix.m_row[1] = RotateVector(bodyMatrix, tmp.m_row[1]);
+	invInertiaMatrix.m_row[2] = RotateVector(bodyMatrix, tmp.m_row[2]);
+	invInertiaMatrix.m_row[3] = float_4 (0.0f, 0.0f, 0.0f, 1.0f);
+}
+
+
+void dgAmpInstance::InitializeBodyArrayParallelKernel (void* const context, void* const ampContext, dgInt32 threadID)
+{
+	dgAmpInstance* const ampInstance = (dgAmpInstance*) ampContext;
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+//	dgWorld* const world = ampInstance->m_world;
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex; 
+/*
+	const dgIsland* const island = syncData->m_islandArray;
+	dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
+
+	dgdgAmpJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
+	dgVector zero(dgFloat32 (0.0f));
+
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		dgAssert (bodyArray[0].m_body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bodyArray[0].m_body)->m_accel % ((dgDynamicBody*)bodyArray[0].m_body)->m_accel) == dgFloat32 (0.0f));
+		dgAssert (bodyArray[0].m_body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bodyArray[0].m_body)->m_alpha % ((dgDynamicBody*)bodyArray[0].m_body)->m_alpha) == dgFloat32 (0.0f));
+
+		dgInt32 index = i + 1;
+		dgBody* const body = bodyArray[index].m_body;
+		dgAssert (body->m_invMass.m_w > dgFloat32 (0.0f));
+		body->AddDampingAcceleration();
+		body->CalcInvInertiaMatrix ();
+
+		// re use these variables for temp storage 
+		body->m_netForce = body->m_veloc;
+		body->m_netTorque = body->m_omega;
+
+		internalForces[index].m_linear = zero;
+		internalForces[index].m_angular = zero;
+	}
+*/
+
+	const int bodyCount = syncData->m_bodyCount;
+	dgBody** const bopyMapInfo = syncData->m_bodyInfoMap;
+	//dgAmpJacobian* const bodyDampCpu = ampInstance->m_bodyDampCpu.data();
+	dgJacobian* const bodyDampCpu = (dgJacobian*) ampInstance->m_bodyDampCpu.data();
+	dgMatrix* const bodyMatrixCpu = (dgMatrix*) ampInstance->m_bodyMatrixCpu.data();
+	dgVector* const bodyInvMassCpu = (dgVector*) ampInstance->m_bodyInvMassCpu.data();
+	dgJacobian* const bodyVelocityCpu = (dgJacobian*) ampInstance->m_bodyVelocityCpu.data();
+
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		const dgBody* const body = bopyMapInfo[i];
+
+		bodyMatrixCpu[i] = body->m_matrix;
+		bodyDampCpu[i].m_linear = dgVector (body->GetLinearDamping());
+		bodyDampCpu[i].m_angular = body->GetAngularDamping();
+		bodyVelocityCpu[i].m_linear = body->GetVelocity();
+		bodyVelocityCpu[i].m_angular = body->GetOmega();
+		bodyInvMassCpu[i] = body->GetInvMass();
+	}
+
+	dgAssert (bopyMapInfo[0]->IsRTTIType (dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bopyMapInfo[0])->m_accel % ((dgDynamicBody*)bopyMapInfo[0])->m_accel) == dgFloat32 (0.0f));
+	dgAssert (bopyMapInfo[0]->IsRTTIType (dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bopyMapInfo[0])->m_alpha % ((dgDynamicBody*)bopyMapInfo[0])->m_alpha) == dgFloat32 (0.0f));
+}
+
+
+
+void dgAmpInstance::CreateParallelArrayBatchArrays (dgParallelSolverSyncData* const syncData, const dgIsland* const islandArray, dgInt32 islanCount)
+{
+	dgBodyInfo* const bodyArray = (dgBodyInfo*) &m_world->m_bodiesMemory[0]; 
+	dgJointInfo* const constraintArray = (dgJointInfo*) &m_world->m_jointsMemory[0];
+	dgParallelJointMap* const jointInfoMap = syncData->m_jointInfoMap;
+	dgBody** const bodyInfoMap = syncData->m_bodyInfoMap;
+
+	dgInt32 bodyCount = 1;
+	dgInt32 jointCount = 0;
+	dgInt32 maxRowCount = 0;
+	bodyInfoMap[0] = m_world->GetSentinelBody();
+	for (dgInt32 i = 0; i < islanCount; i ++) {
+		const dgIsland* const island = &islandArray[i];
+		dgInt32 count = island->m_jointCount;
+		dgInt32 index = island->m_jointStart;
+		for (dgInt32 j = 0; j < count; j ++) {
+			const dgJointInfo& jointInfo = constraintArray[index];
+			dgConstraint* const joint = jointInfo.m_joint;
+			constraintArray[index].m_color = 0;
+			jointInfoMap[jointCount + j].m_jointIndex = index;
+			joint->m_index = index;
+			index ++;
+		}
+		jointCount += count;
+		maxRowCount += island->m_rowsCount;
+		
+		count = island->m_bodyCount;
+		index = island->m_bodyStart;
+		bodyArray[index].m_index = 0;
+		dgAssert (bodyArray[index].m_body == bodyInfoMap[0]);
+		for (dgInt32 j = 1; j < count; j ++) {
+			dgBodyInfo& bodyInfo = bodyArray[index + 1];
+			bodyInfo.m_index = bodyCount + j - 1;
+			bodyInfoMap[bodyInfo.m_index] = bodyInfo.m_body;
+			index ++;
+		}
+		bodyCount += count - 1;
+	}
+
+	syncData->m_bodyCount = bodyCount;
+	syncData->m_jointCount = jointCount;
+	syncData->m_rowCount = maxRowCount;
+
+	dgAmpBodyData::Alloc (syncData->m_bodyCount);
+	dgAmpConstraintData::Alloc (syncData->m_rowCount);
+
+	jointInfoMap[jointCount].m_bashIndex = 0x7fffffff;
+	jointInfoMap[jointCount].m_jointIndex= -1;
+
+	for (dgInt32 i = 0; i < jointCount; i ++) {
+		dgInt32 j = jointInfoMap[i].m_jointIndex;
+		dgJointInfo& jointInfo = constraintArray[j];
+
+		dgInt32 index = 0; 
+		dgInt32 color = jointInfo.m_color;
+		for (dgInt32 n = 1; n & color; n <<= 1) {
+			index ++;
+			dgAssert (index < 32);
+		}
+		jointInfoMap[i].m_bashIndex = index;
+
+		color = 1 << index;
+		dgConstraint* const constraint = jointInfo.m_joint;
+		dgDynamicBody* const body0 = (dgDynamicBody*) constraint->m_body0;
+		dgAssert (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI));
+
+		if (body0->m_invMass.m_w > dgFloat32 (0.0f)) {
+			for (dgBodyMasterListRow::dgListNode* jointNode = body0->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
+				dgBodyMasterListCell& cell = jointNode->GetInfo();
+
+				dgConstraint* const neiborgLink = cell.m_joint;
+				//if ((neiborgLink != constraint) && (neiborgLink->m_maxDOF)) {
+				if ((neiborgLink != constraint) & neiborgLink->IsActive()) {
+					dgJointInfo& info = constraintArray[neiborgLink->m_index];
+					info.m_color |= color;
+				}
+			}
+		}
+
+		dgDynamicBody* const body1 = (dgDynamicBody*)constraint->m_body1;
+		dgAssert (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI));
+		if (body1->m_invMass.m_w > dgFloat32 (0.0f)) {
+			for (dgBodyMasterListRow::dgListNode* jointNode = body1->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
+				dgBodyMasterListCell& cell = jointNode->GetInfo();
+
+				dgConstraint* const neiborgLink = cell.m_joint;
+				//if ((neiborgLink != constraint) && (neiborgLink->m_maxDOF)) {
+				if ((neiborgLink != constraint) & neiborgLink->IsActive()) {
+					dgJointInfo& info = constraintArray[neiborgLink->m_index];
+					info.m_color |= color;
+				}
+			}
+		}
+	}
+
+	dgSort (jointInfoMap, jointCount, dgWorldDynamicUpdate::SortJointInfoByColor, constraintArray);
+
+	dgInt32 bash = 0;
+	for (int index = 0; index < jointCount; index ++) {
+		dgInt32 count = 0; 
+		syncData->m_jointBatches[bash].m_start = index;
+		while (jointInfoMap[index].m_bashIndex == bash) {
+			count ++;
+			index ++;
+		}
+		syncData->m_jointBatches[bash].m_count = count;
+		bash ++;
+		index --;
+	}
+	syncData->m_batchesCount = bash;
+}
+
+
+void dgAmpInstance::InitilizeBodyArrayParallel (dgParallelSolverSyncData* const syncData)
+{
+	dgWorld* const world = m_world;
+	dgInt32 threadCounts = world->GetThreadCount();	
+	syncData->m_atomicIndex = 0;
+	for (dgInt32 j = 0; j < threadCounts; j ++) {
+		world->QueueJob (InitializeBodyArrayParallelKernel, syncData, this);
+	}
+	world->SynchronizationBarrier();
+
+	const int bodyCount = syncData->m_bodyCount;
+	extent<1> compute_domain(bodyCount);
+
+	//copy (m_bodyDampCpu.begin(), m_bodyDampCpu.begin() + bodyCount, m_bodyDamp);
+	//copy (m_bodyMatrixCpu.begin(), m_bodyMatrixCpu.begin() + bodyCount, m_bodyMatrix);
+	//copy (m_bodyInvMassCpu.begin(), m_bodyInvMassCpu.begin() + bodyCount, m_bodyInvMass);
+	//copy (m_bodyVelocityCpu.begin(), m_bodyVelocityCpu.begin() + bodyCount, m_bodyVelocity);
+	
+	const array<float_4, 1>& bodyInvMass = m_bodyInvMass;
+	const array<dgAmpMatrix4x4, 1>& bodyMatrix = m_bodyMatrix;
+	const array<dgAmpJacobian, 1>& bodyDamp = m_bodyDamp;
+	array<dgAmpJacobian, 1>& bodyVelocity = m_bodyVelocity;
+	array<dgAmpJacobian, 1>& bodyNetForce = m_bodyNetForce;
+	array<dgAmpJacobian, 1>& bodyInternalForce = m_bodyInternalForce;
+	array<dgAmpMatrix4x4, 1>& bodyInvInertiaMatrix = m_bodyInvInertiaMatrix;
+
+    parallel_for_each (compute_domain, [=, &bodyDamp, &bodyVelocity, &bodyMatrix, &bodyInvInertiaMatrix, &bodyInvMass, &bodyNetForce, &bodyInternalForce] (index<1> idx) restrict(amp,cpu)
+    {
+		AddDamingAccelKernel (bodyMatrix[idx], bodyDamp[idx], bodyVelocity[idx]);
+        CalcuateInvInertiaMatrixKernel (bodyMatrix[idx], bodyInvInertiaMatrix[idx], bodyInvMass[idx]);
+
+		bodyNetForce[idx] = bodyVelocity[idx];
+		bodyInternalForce[idx].m_linear = float_4 (0.0f, 0.0f, 0.0f, 0.0f);
+		bodyInternalForce[idx].m_angular = float_4 (0.0f, 0.0f, 0.0f, 0.0f);
+    });
+}
+
+
+
+void dgAmpInstance::GetJacobianDerivativesParallel (dgJointInfo* const jointInfo, dgInt32 threadIndex, dgInt32 rowBase, dgFloat32 timestep)
+{
+	dgWorld* const world = m_world;
+
+	dgContraintDescritor constraintParams;
+	constraintParams.m_world = world; 
+	constraintParams.m_threadIndex = threadIndex;
+	constraintParams.m_timestep = timestep;
+	constraintParams.m_invTimestep = dgFloat32 (1.0f / timestep);
+
+//	dgJacobianMatrixElement* const matrixRow = &m_solverMemory.m_memory[rowBase];
+	dgConstraint* const constraint = jointInfo->m_joint;
+
+	dgInt32 dof = dgInt32 (constraint->m_maxDOF);
+	dgAssert (dof <= DG_CONSTRAINT_MAX_ROWS);
+	for (dgInt32 i = 0; i < dof; i ++) {
+		constraintParams.m_forceBounds[i].m_low = DG_MIN_BOUND;
+		constraintParams.m_forceBounds[i].m_upper = DG_MAX_BOUND;
+		constraintParams.m_forceBounds[i].m_jointForce = NULL;
+		constraintParams.m_forceBounds[i].m_normalIndex = DG_BILATERAL_CONSTRAINT;
+	}
+
+	dgAssert (constraint->m_body0);
+	dgAssert (constraint->m_body1);
+
+	constraint->m_body0->m_inCallback = true;
+	constraint->m_body1->m_inCallback = true;
+
+	dof = dgInt32 (constraint->JacobianDerivative (constraintParams)); 
+
+	constraint->m_body0->m_inCallback = false;
+	constraint->m_body1->m_inCallback = false;
+
+	dgInt32 m0 = (constraint->m_body0->GetInvMass().m_w != dgFloat32(0.0f)) ? constraint->m_body0->m_index: 0;
+	dgInt32 m1 = (constraint->m_body1->GetInvMass().m_w != dgFloat32(0.0f)) ? constraint->m_body1->m_index: 0;
+
+	jointInfo->m_autoPairstart = rowBase;
+	jointInfo->m_autoPaircount = dof;
+	jointInfo->m_autoPairActiveCount = dof;
+	jointInfo->m_m0 = m0;
+	jointInfo->m_m1 = m1;
+
+	dgDoubleInt ptr; 
+	ptr.m_float = 0;
+	dgAmpJacobianMatrixElement* const matrixRow = &m_matrixDataCpu.data()[rowBase];
+	for (dgInt32 i = 0; i < dof; i ++) {
+		//dgAmpJacobianMatrixElement* const row = &matrixRow[i];
+		dgJacobianPair& jacobianPair = *((dgJacobianPair*)&matrixRow[i].m_Jt); 
+		dgAmpMatrixRightSide& rightSide = *((dgAmpMatrixRightSide*)&matrixRow[i].m_data); 
+
+		dgAssert (constraintParams.m_forceBounds[i].m_jointForce);
+		//row->m_Jt = constraintParams.m_jacobian[i]; 
+		jacobianPair = constraintParams.m_jacobian[i]; 
+
+		dgAssert (constraintParams.m_jointStiffness[i] >= dgFloat32(0.1f));
+		dgAssert (constraintParams.m_jointStiffness[i] <= dgFloat32(100.0f));
+
+		//row->m_diagDamp = constraintParams.m_jointStiffness[i];
+		//row->m_coordenateAccel = constraintParams.m_jointAccel[i];
+		//row->m_restitution = constraintParams.m_restitution[i];
+		//row->m_penetration = constraintParams.m_penetration[i];
+		//row->m_penetrationStiffness = constraintParams.m_penetrationStiffness[i];
+		//row->m_lowerBoundFrictionCoefficent = constraintParams.m_forceBounds[i].m_low;
+		//row->m_upperBoundFrictionCoefficent = constraintParams.m_forceBounds[i].m_upper;
+		//row->m_accelIsMotor = constraintParams.m_isMotor[i];
+		//row->m_normalForceIndex = constraintParams.m_forceBounds[i].m_normalIndex; 
+		//row->m_jointFeebackForce = constraintParams.m_forceBounds[i].m_jointForce;
+	
+		rightSide.m_diagDamp = constraintParams.m_jointStiffness[i];
+		rightSide.m_coordenateAccel = constraintParams.m_jointAccel[i];
+		rightSide.m_restitution = constraintParams.m_restitution[i];
+		rightSide.m_penetration = constraintParams.m_penetration[i];
+		rightSide.m_accelIsMotor = constraintParams.m_isMotor[i] ? 1 : 0;
+		rightSide.m_penetrationStiffness = constraintParams.m_penetrationStiffness[i];
+
+		ptr.m_ptr = constraintParams.m_forceBounds[i].m_jointForce;
+		rightSide.m_jointFeebackForce[0] = ptr.m_intL;
+		rightSide.m_jointFeebackForce[1] = ptr.m_intH;
+		rightSide.m_normalForceIndex = constraintParams.m_forceBounds[i].m_normalIndex; 
+		rightSide.m_lowerBoundFrictionCoefficent = constraintParams.m_forceBounds[i].m_low;
+		rightSide.m_upperBoundFrictionCoefficent = constraintParams.m_forceBounds[i].m_upper;
+	}
+}
+
+
+void dgAmpInstance::BuildJacobianMatrixParallelKernel (void* const context, void* const ampContext, dgInt32 threadID)
+{
+/*
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = (dgWorld*) worldContext;
+	const dgParallelJointMap* const jointInfoIndexArray = syncData->m_jointInfoMap;
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex; 
+	const dgIsland* const island = syncData->m_islandArray;
+	dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; 
+	dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
+
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+	dgdgAmpJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0];
+
+	dgVector zero (dgFloat32 (0.0f));
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+
+		dgInt32 jointIndex = jointInfoIndexArray[i].m_jointIndex;
+		dgJointInfo* const jointInfo = &constraintArray[jointIndex];
+
+		dgInt32 rowBase = dgAtomicExchangeAndAdd(&syncData->m_dgAmpJacobianMatrixRowAtomicIndex, jointInfo->m_autoPaircount);
+		world->GetJacobianDerivativesParallel (jointInfo, threadID, rowBase, syncData->m_timestep);
+
+		dgInt32 index = jointInfo->m_autoPairstart;
+		dgInt32 count = jointInfo->m_autoPaircount;
+		dgInt32 m0 = jointInfo->m_m0;
+		dgInt32 m1 = jointInfo->m_m1;
+		dgAssert (m0 >= 0);
+		dgAssert (m1 >= 0);
+		dgAssert (m0 != m1);
+		dgAssert (m0 < island->m_bodyCount);
+		dgAssert (m1 < island->m_bodyCount);
+
+		const dgBody* const body0 = bodyArray[m0].m_body;
+		const dgBody* const body1 = bodyArray[m1].m_body;
+
+		const dgVector invMass0 (body0->m_invMass[3]);
+		const dgMatrix& invInertia0 = body0->m_invWorldInertiaMatrix;
+		const dgVector invMass1 (body1->m_invMass[3]);
+		const dgMatrix& invInertia1 = body1->m_invWorldInertiaMatrix;
+
+		dgVector accel0 (zero); 
+		dgVector alpha0 (zero); 
+		if (body0->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
+			accel0 = ((dgDynamicBody*)body0)->m_accel;
+			alpha0 = ((dgDynamicBody*)body0)->m_alpha;
+		}
+
+		dgVector accel1 (zero); 
+		dgVector alpha1 (zero); 
+		if (body1->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
+			accel1 = ((dgDynamicBody*)body1)->m_accel;
+			alpha1 = ((dgDynamicBody*)body1)->m_alpha;
+		}
+
+		// the parallel solve will not deal with impulse phase, only for large solution solution
+		for (dgInt32 i = 0; i < count; i ++) {
+			dgdgAmpJacobianMatrixElement* const row = &matrixRow[index];
+
+			//dgVector JMinvdgAmpJacobianLinearM0 (row->m_Jt.m_dgAmpJacobianM0.m_linear.Scale3 (invMass0));
+			//dgVector JMinvdgAmpJacobianAngularM0 (invInertia0.UnrotateVector (row->m_Jt.m_dgAmpJacobianM0.m_angular));
+			//dgVector JMinvdgAmpJacobianLinearM1 (row->m_Jt.m_dgAmpJacobianM1.m_linear.Scale3 (invMass1));
+			//dgVector JMinvdgAmpJacobianAngularM1 (invInertia1.UnrotateVector (row->m_Jt.m_dgAmpJacobianM1.m_angular));
+
+			dgVector JMinvdgAmpJacobianLinearM0 (row->m_Jt.m_dgAmpJacobianM0.m_linear.CompProduct4 (invMass0));
+			dgVector JMinvdgAmpJacobianAngularM0 (invInertia0.RotateVector (row->m_Jt.m_dgAmpJacobianM0.m_angular));
+			dgVector JMinvdgAmpJacobianLinearM1 (row->m_Jt.m_dgAmpJacobianM1.m_linear.CompProduct4 (invMass1));
+			dgVector JMinvdgAmpJacobianAngularM1 (invInertia1.RotateVector (row->m_Jt.m_dgAmpJacobianM1.m_angular));
+
+			dgVector tmpDiag (JMinvdgAmpJacobianLinearM0.CompProduct4(row->m_Jt.m_dgAmpJacobianM0.m_linear) + JMinvdgAmpJacobianAngularM0.CompProduct4(row->m_Jt.m_dgAmpJacobianM0.m_angular) +
+							  JMinvdgAmpJacobianLinearM1.CompProduct4(row->m_Jt.m_dgAmpJacobianM1.m_linear) + JMinvdgAmpJacobianAngularM1.CompProduct4(row->m_Jt.m_dgAmpJacobianM1.m_angular));
+
+			dgVector tmpAccel (JMinvdgAmpJacobianLinearM0.CompProduct4(accel0) + JMinvdgAmpJacobianAngularM0.CompProduct4(alpha0) + JMinvdgAmpJacobianLinearM1.CompProduct4(accel1) + JMinvdgAmpJacobianAngularM1.CompProduct4(alpha1));
+
+			dgFloat32 extenalAcceleration = -(tmpAccel.m_x + tmpAccel.m_y + tmpAccel.m_z);
+			//row->m_extAccel = extenalAcceleration;
+			row->m_deltaAccel = extenalAcceleration;
+			row->m_coordenateAccel += extenalAcceleration;
+			row->m_force = row->m_jointFeebackForce[0].m_force;
+			row->m_maxImpact = dgFloat32 (0.0f);
+
+			dgAssert (row->m_diagDamp >= dgFloat32(0.1f));
+			dgAssert (row->m_diagDamp <= dgFloat32(100.0f));
+			dgFloat32 stiffness = DG_PSD_DAMP_TOL * row->m_diagDamp;
+
+			dgFloat32 diag = (tmpDiag.m_x + tmpDiag.m_y + tmpDiag.m_z);
+			dgAssert (diag > dgFloat32 (0.0f));
+			row->m_diagDamp = diag * stiffness;
+
+			diag *= (dgFloat32(1.0f) + stiffness);
+			//solverMemory.m_diagJMinvJt[index] = diag;
+			row->m_invDJMinvJt = dgFloat32(1.0f) / diag;
+
+			index ++;
+		}
+	}
+*/
+
+	dgAmpInstance* const ampInstance = (dgAmpInstance*) ampContext;
+	dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
+	dgWorld* const world = ampInstance->m_world;
+	dgInt32* const atomicIndex = &syncData->m_atomicIndex; 
+	dgJointInfo* const constraintArray = (dgJointInfo*) &world->m_jointsMemory[0];
+	const dgParallelJointMap* const jointInfoIndexArray = syncData->m_jointInfoMap;
+
+//	dgVector zero (dgFloat32 (0.0f));
+	for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount;  i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
+		dgInt32 jointIndex = jointInfoIndexArray[i].m_jointIndex;
+		dgJointInfo* const jointInfo = &constraintArray[jointIndex];
+		dgInt32 rowBase = dgAtomicExchangeAndAdd(&syncData->m_jacobianMatrixRowAtomicIndex, jointInfo->m_autoPaircount);
+		ampInstance->GetJacobianDerivativesParallel (jointInfo, threadID, rowBase, syncData->m_timestep);
+	}
+}
+
+
+void dgAmpInstance::BuildJacobianMatrixParallel (dgParallelSolverSyncData* const syncData)
+{
+	const int jountCount = syncData->m_jointCount;
+	if (jountCount) {
+		dgWorld* const world = m_world;
+		dgInt32 threadCounts = world->GetThreadCount();	
+
+	
+		syncData->m_atomicIndex = 0;
+		for (dgInt32 j = 0; j < threadCounts; j ++) {
+			world->QueueJob (BuildJacobianMatrixParallelKernel, syncData, this);
+		}
+		world->SynchronizationBarrier();
+
+		dgAssert (syncData->m_jacobianMatrixRowAtomicIndex == syncData->m_rowCount);
+	//	dgInt32 rowCount = syncData->m_rowCount;
+	//	copy (m_matrixDataCpu.begin(), m_matrixDataCpu.begin() + rowCount, m_matrixData);
+			
+		extent<1> compute_domain(jountCount);
+
+		array<dgAmpJacobianMatrixElement, 1>& matrixData = m_matrixData;
+		parallel_for_each (compute_domain, [=, &matrixData] (index<1> idx) restrict(amp,cpu)
+		{
+
+		});
+	}
+}
+

newtondynamics.mod/NewtonDynamics/source/core/dg.cpp

@@ -0,0 +1,27 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dg.h

@@ -0,0 +1,68 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#if !defined(AFX_DG_H__FGRTY_5GR39H_98TQ_H)
+#define AFX_DG_H__FGRTY_5GR39H_98TQ_H
+
+#include "dgTypes.h"
+
+#include "dgRef.h"
+#include "dgCRC.h"
+#include "dgObb.h"
+#include "dgRtti.h"
+#include "dgList.h"
+#include "dgTree.h"
+#include "dgNode.h"
+#include "dgHeap.h"
+#include "dgArray.h"
+#include "dgStack.h"
+#include "dgDebug.h"
+#include "dgGraph.h"
+#include "dgPlane.h"
+#include "dgGoogol.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgMemory.h"
+#include "dgRandom.h"
+#include "dgThread.h"
+#include "dgFastQueue.h"
+#include "dgSPDMatrix.h"
+#include "dgPolyhedra.h"
+#include "dgThreadHive.h"
+#include "dgPathFinder.h"
+#include "dgRefCounter.h"
+#include "dgQuaternion.h"
+#include "dgAsyncThread.h"
+#include "dgMutexThread.h"
+#include "dgConvexHull3d.h"
+#include "dgConvexHull4d.h"
+#include "dgIntersections.h"
+#include "dgGeneralVector.h"
+#include "dgGeneralMatrix.h"
+#include "dgThreadProfiler.h"
+#include "dgAABBPolygonSoup.h"
+#include "dgSmallDeterminant.h"
+#include "dgPolygonSoupBuilder.h"
+#include "dgPolygonSoupDatabase.h"
+#include "dgPolyhedraMassProperties.h"
+#include "dgDelaunayTetrahedralization.h"
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgAABBPolygonSoup.cpp

@@ -0,0 +1,1280 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgHeap.h"
+#include "dgStack.h"
+#include "dgList.h"
+#include "dgMatrix.h"
+#include "dgAABBPolygonSoup.h"
+#include "dgPolygonSoupBuilder.h"
+
+
+#define DG_STACK_DEPTH 512
+
+
+DG_MSC_VECTOR_ALIGMENT
+class dgAABBPolygonSoup::dgNodeBuilder: public dgAABBPolygonSoup::dgNode
+{
+	public:
+	dgNodeBuilder (const dgVector& p0, const dgVector& p1)
+		:dgNode()
+		,m_left (NULL)
+		,m_right (NULL)
+		,m_parent (NULL)
+		,m_indexBox0(0)
+		,m_indexBox1(0)
+		,m_enumeration(-1)
+		,m_faceIndex(0)
+		,m_indexCount(0)
+		,m_faceIndices(NULL)
+	{
+		SetBox (p0, p1);
+	}
+
+	dgNodeBuilder (const dgVector* const vertexArray, dgInt32 faceIndex, dgInt32 indexCount, const dgInt32* const indexArray)
+		:dgNode()
+		,m_left (NULL)
+		,m_right (NULL)
+		,m_parent (NULL)
+		,m_indexBox0(0)
+		,m_indexBox1(0)
+		,m_enumeration(-1)
+		,m_faceIndex(faceIndex)
+		,m_indexCount(indexCount)
+		,m_faceIndices(indexArray)
+	{
+		dgVector minP ( dgFloat32 (1.0e15f)); 
+		dgVector maxP (-dgFloat32 (1.0e15f)); 
+		for (dgInt32 i = 0; i < indexCount; i ++) {
+			dgInt32 index = indexArray[i];
+			const dgVector& p (vertexArray[index]);
+			minP = p.GetMin(minP); 
+			maxP = p.GetMax(maxP); 
+		}
+		minP -= dgVector (dgFloat32 (1.0e-3f));
+		maxP += dgVector (dgFloat32 (1.0e-3f));
+		minP = minP & dgVector::m_triplexMask;
+		maxP = maxP & dgVector::m_triplexMask;
+		SetBox (minP, maxP);
+	}
+
+	dgNodeBuilder (dgNodeBuilder* const left, dgNodeBuilder* const right)
+		:dgNode()
+		,m_left(left)
+		,m_right(right)
+		,m_parent(NULL)
+		,m_indexBox0(0)
+		,m_indexBox1(0)
+		,m_enumeration(-1)
+		,m_faceIndex(0)
+		,m_indexCount(0)
+		,m_faceIndices(NULL)
+	{
+		m_left->m_parent = this;
+		m_right->m_parent = this;
+
+		dgVector p0 (left->m_p0.GetMin(right->m_p0));
+		dgVector p1 (left->m_p1.GetMax(right->m_p1));
+		SetBox(p0, p1);
+	}
+
+	void SetBox (const dgVector& p0, const dgVector& p1)
+	{
+		m_p0 = p0;
+		m_p1 = p1;
+		m_size = m_p1 - m_p0;
+		m_origin = (m_p1 + m_p0).Scale4 (dgFloat32 (0.5f));
+		m_area = m_size.DotProduct4(m_size.ShiftTripleRight()).m_x;
+	}
+
+	static dgFloat32 CalculateSurfaceArea (dgNodeBuilder* const node0, dgNodeBuilder* const node1, dgVector& minBox, dgVector& maxBox)
+	{
+		minBox = node0->m_p0.GetMin(node1->m_p0);
+		maxBox = node0->m_p1.GetMax(node1->m_p1);
+
+		dgVector side0 ((maxBox - minBox).Scale4 (dgFloat32 (0.5f)));
+		dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
+		return side0.DotProduct4(side1).m_x;
+	}
+
+
+	dgVector m_p0;
+	dgVector m_p1;
+	dgVector m_size;
+	dgVector m_origin;
+	dgFloat32 m_area;
+	
+	dgNodeBuilder* m_left;
+	dgNodeBuilder* m_right;
+	dgNodeBuilder* m_parent;
+	dgInt32 m_indexBox0;
+	dgInt32 m_indexBox1;
+	dgInt32 m_enumeration;
+	dgInt32 m_faceIndex;
+	dgInt32 m_indexCount;
+	const dgInt32* m_faceIndices;
+} DG_GCC_VECTOR_ALIGMENT;
+
+
+
+class dgAABBPolygonSoup::dgSpliteInfo
+{
+	public:
+	dgSpliteInfo (dgNodeBuilder* const boxArray, dgInt32 boxCount)
+	{
+		dgVector minP ( dgFloat32 (1.0e15f)); 
+		dgVector maxP (-dgFloat32 (1.0e15f)); 
+
+		if (boxCount == 2) {
+			m_axis = 1;
+			for (dgInt32 i = 0; i < boxCount; i ++) {
+				const dgNodeBuilder& box = boxArray[i];
+				const dgVector& p0 = box.m_p0;
+				const dgVector& p1 = box.m_p1;
+				minP = minP.GetMin (p0); 
+				maxP = maxP.GetMax (p1); 
+			}
+
+		} else {
+			dgVector median (dgFloat32 (0.0f));
+			dgVector varian (dgFloat32 (0.0f));
+			for (dgInt32 i = 0; i < boxCount; i ++) {
+				const dgNodeBuilder& box = boxArray[i];
+
+				const dgVector& p0 = box.m_p0;
+				const dgVector& p1 = box.m_p1;
+
+				minP = minP.GetMin (p0); 
+				maxP = maxP.GetMax (p1); 
+				dgVector p ((p0 + p1).CompProduct4(dgVector::m_half));
+
+				median += p;
+				varian += p.CompProduct4(p);
+			}
+
+			varian = varian.Scale4 (dgFloat32 (boxCount)) - 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 (boxCount));
+			dgFloat32 test = center[index];
+			dgInt32 i0 = 0;
+			dgInt32 i1 = boxCount - 1;
+			do {    
+				for (; i0 <= i1; i0 ++) {
+					const dgNodeBuilder& box = boxArray[i0];
+					dgFloat32 val = (box.m_p0[index] + box.m_p1[index]) * dgFloat32 (0.5f);
+					if (val > test) {
+						break;
+					}
+				}
+
+				for (; i1 >= i0; i1 --) {
+					const dgNodeBuilder& box = boxArray[i1];
+					dgFloat32 val = (box.m_p0[index] + box.m_p1[index]) * dgFloat32 (0.5f);
+					if (val < test) {
+						break;
+					}
+				}
+
+				if (i0 < i1)	{
+					dgSwap(boxArray[i0], boxArray[i1]);
+					i0++; 
+					i1--;
+				}
+			} while (i0 <= i1);
+
+			if (i0 > 0){
+				i0 --;
+			}
+			if ((i0 + 1) >= boxCount) {
+				i0 = boxCount - 2;
+			}
+
+			m_axis = i0 + 1;
+		}
+
+		dgAssert (maxP.m_x - minP.m_x >= dgFloat32 (0.0f));
+		dgAssert (maxP.m_y - minP.m_y >= dgFloat32 (0.0f));
+		dgAssert (maxP.m_z - minP.m_z >= dgFloat32 (0.0f));
+		m_p0 = minP;
+		m_p1 = maxP;
+	}
+
+	dgInt32 m_axis;
+	dgVector m_p0;
+	dgVector m_p1;
+};
+
+
+
+dgAABBPolygonSoup::dgAABBPolygonSoup ()
+	:dgPolygonSoupDatabase()
+	,m_nodesCount(0)
+	,m_indexCount(0)
+	,m_aabb(NULL)
+	,m_indices(NULL)
+{
+}
+
+dgAABBPolygonSoup::~dgAABBPolygonSoup ()
+{
+	if (m_aabb) {
+		dgFreeStack (m_aabb);
+		dgFreeStack (m_indices);
+	}
+}
+
+
+void dgAABBPolygonSoup::ImproveNodeFitness (dgNodeBuilder* const node) const
+{
+	dgAssert (node->m_left);
+	dgAssert (node->m_right);
+
+	if (node->m_parent)	{
+		if (node->m_parent->m_left == node) {
+			dgFloat32 cost0 = node->m_area;
+
+			dgVector cost1P0;
+			dgVector cost1P1;		
+			dgFloat32 cost1 = dgNodeBuilder::CalculateSurfaceArea (node->m_right, node->m_parent->m_right, cost1P0, cost1P1);
+
+			dgVector cost2P0;
+			dgVector cost2P1;		
+			dgFloat32 cost2 = dgNodeBuilder::CalculateSurfaceArea (node->m_left, node->m_parent->m_right, cost2P0, cost2P1);
+
+			if ((cost1 <= cost0) && (cost1 <= cost2)) {
+				dgNodeBuilder* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+				node->m_size = parent->m_size;
+				node->m_origin = parent->m_origin;
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_right->m_parent = parent;
+				parent->m_left = node->m_right;
+				node->m_right = parent;
+				parent->m_p0 = cost1P0;
+				parent->m_p1 = cost1P1;		
+				parent->m_area = cost1;
+				parent->m_size = (parent->m_p1 - parent->m_p0).Scale3(dgFloat32 (0.5f));
+				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale3(dgFloat32 (0.5f));
+
+			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
+				dgNodeBuilder* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+				node->m_size = parent->m_size;
+				node->m_origin = parent->m_origin;
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_left->m_parent = parent;
+				parent->m_left = node->m_left;
+				node->m_left = parent;
+
+				parent->m_p0 = cost2P0;
+				parent->m_p1 = cost2P1;		
+				parent->m_area = cost2;
+				parent->m_size = (parent->m_p1 - parent->m_p0).Scale3(dgFloat32 (0.5f));
+				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale3(dgFloat32 (0.5f));
+			}
+		} else {
+			dgFloat32 cost0 = node->m_area;
+
+			dgVector cost1P0;
+			dgVector cost1P1;		
+			dgFloat32 cost1 = dgNodeBuilder::CalculateSurfaceArea (node->m_left, node->m_parent->m_left, cost1P0, cost1P1);
+
+			dgVector cost2P0;
+			dgVector cost2P1;		
+			dgFloat32 cost2 = dgNodeBuilder::CalculateSurfaceArea (node->m_right, node->m_parent->m_left, cost2P0, cost2P1);
+
+			if ((cost1 <= cost0) && (cost1 <= cost2)) {
+				dgNodeBuilder* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+				node->m_size = parent->m_size;
+				node->m_origin = parent->m_origin;
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_left->m_parent = parent;
+				parent->m_right = node->m_left;
+				node->m_left = parent;
+
+				parent->m_p0 = cost1P0;
+				parent->m_p1 = cost1P1;		
+				parent->m_area = cost1;
+				parent->m_size = (parent->m_p1 - parent->m_p0).Scale3(dgFloat32 (0.5f));
+				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale3(dgFloat32 (0.5f));
+
+			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
+				dgNodeBuilder* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+				node->m_size = parent->m_size;
+				node->m_origin = parent->m_origin;
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_right->m_parent = parent;
+				parent->m_right = node->m_right;
+				node->m_right = parent;
+
+				parent->m_p0 = cost2P0;
+				parent->m_p1 = cost2P1;		
+				parent->m_area = cost2;
+				parent->m_size = (parent->m_p1 - parent->m_p0).Scale3(dgFloat32 (0.5f));
+				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale3(dgFloat32 (0.5f));
+			}
+		}
+	} else {
+		// in the future I can handle this but it is too much work for little payoff
+	}
+}
+
+
+dgFloat32 dgAABBPolygonSoup::CalculateFaceMaxSize (const dgVector* const vertex, dgInt32 indexCount, const dgInt32* const indexArray) const
+{
+	dgFloat32 maxSize = dgFloat32 (0.0f);
+	dgInt32 index = indexArray[indexCount - 1];
+	dgVector p0 (vertex[index]);
+	for (dgInt32 i = 0; i < indexCount; i ++) {
+		dgInt32 index = indexArray[i];
+		dgVector p1 (vertex[index]);
+
+		dgVector dir (p1 - p0);
+		dir = dir.Scale3 (dgRsqrt (dir % dir));
+
+		dgFloat32 maxVal = dgFloat32 (-1.0e10f);
+		dgFloat32 minVal = dgFloat32 ( 1.0e10f);
+		for (dgInt32 j = 0; j < indexCount; j ++) {
+			dgInt32 index = indexArray[j];
+			dgVector q (vertex[index]);
+			dgFloat32 val = dir % q;
+			minVal = dgMin(minVal, val);
+			maxVal = dgMax(maxVal, val);
+		}
+
+		dgFloat32 size = maxVal - minVal;
+		maxSize = dgMax(maxSize, size);
+		p0 = p1;
+	}
+
+	return dgFloor (maxSize + dgFloat32 (1.0f));
+}
+
+
+
+
+void dgAABBPolygonSoup::GetAABB (dgVector& p0, dgVector& p1) const
+{
+	if (m_aabb) { 
+		GetNodeAABB (m_aabb, p0, p1);
+	} else {
+		p0 = dgVector (dgFloat32 (0.0f));
+		p1 = dgVector (dgFloat32 (0.0f));
+	}
+}
+
+
+
+void dgAABBPolygonSoup::CalculateAdjacendy ()
+{
+	dgVector p0;
+	dgVector p1;
+	GetAABB (p0, p1);
+	dgFastAABBInfo box (p0, p1);
+	ForAllSectors (box, dgVector (dgFloat32 (0.0f)), dgFloat32 (1.0f), CalculateAllFaceEdgeNormals, this);
+
+	for (dgInt32 i = 0; i < m_nodesCount; i ++) {
+		const dgNode* const node = &m_aabb[i];
+		if (node->m_left.IsLeaf()) {
+			dgInt32 vCount = node->m_left.GetCount();
+			dgInt32 index = dgInt32 (node->m_left.GetIndex());
+			dgInt32* const face = &m_indices[index];
+			for (dgInt32 j = 0; j < vCount; j ++) {
+				if (face[vCount + 2 + j] == -1) {
+					face[vCount + 2 + j] = face[vCount + 1];
+				}
+			}
+		}
+
+		if (node->m_right.IsLeaf()) {
+			dgInt32 vCount = node->m_right.GetCount();
+			dgInt32 index = dgInt32 (node->m_right.GetIndex());
+			dgInt32* const face = &m_indices[index];
+			for (dgInt32 j = 0; j < vCount; j ++) {
+				if (face[vCount + 2 + j] == -1) {
+					face[vCount + 2 + j] = face[vCount + 1];
+				}
+			}
+		}
+	}
+}
+
+
+dgIntersectStatus dgAABBPolygonSoup::CalculateAllFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+
+	AdjacentdFaces adjacentFaces;
+	adjacentFaces.m_count = indexCount;
+	adjacentFaces.m_index = (dgInt32*) indexArray;
+
+	dgVector n (&polygon[indexArray[indexCount + 1] * stride]);
+	dgVector p (&polygon[indexArray[0] * stride]);
+	adjacentFaces.m_normal = dgPlane (n, - (n % p));
+
+	dgAssert (indexCount < dgInt32 (sizeof (adjacentFaces.m_edgeMap) / sizeof (adjacentFaces.m_edgeMap[0])));
+
+	dgInt32 edgeIndex = indexCount - 1;
+	dgInt32 i0 = indexArray[indexCount - 1];
+	dgVector p0 ( dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f), dgFloat32 (0.0f)); 
+	dgVector p1 (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f)); 
+	for (dgInt32 i = 0; i < indexCount; i ++) {
+		dgInt32 i1 = indexArray[i];
+		dgInt32 index = i1 * stride;
+		dgVector p (&polygon[index]);
+//		p0.m_x = dgMin (p.m_x, p0.m_x); 
+//		p0.m_y = dgMin (p.m_y, p0.m_y); 
+//		p0.m_z = dgMin (p.m_z, p0.m_z); 
+//		p1.m_x = dgMax (p.m_x, p1.m_x); 
+//		p1.m_y = dgMax (p.m_y, p1.m_y); 
+//		p1.m_z = dgMax (p.m_z, p1.m_z); 
+		p0 = p0.GetMin(p);
+		p1 = p1.GetMax(p);
+		adjacentFaces.m_edgeMap[edgeIndex] = (dgInt64 (i1) << 32) + i0;
+		edgeIndex = i;
+		i0 = i1;
+	}
+
+	dgFloat32 padding = dgFloat32 (1.0f/16.0f);
+	p0.m_x -= padding;
+	p0.m_y -= padding;
+	p0.m_z -= padding;
+	p1.m_x += padding;
+	p1.m_y += padding;
+	p1.m_z += padding;
+
+	dgAABBPolygonSoup* const me = (dgAABBPolygonSoup*) context;
+	dgFastAABBInfo box (p0, p1);
+	me->ForAllSectors (box, dgVector (dgFloat32 (0.0f)), dgFloat32 (1.0f), CalculateDisjointedFaceEdgeNormals, &adjacentFaces);
+	return t_ContinueSearh;
+}
+
+
+dgIntersectStatus dgAABBPolygonSoup::CalculateDisjointedFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance)
+{
+	#define DG_WELDING_TOL (1.0e-2f)
+	#define DG_WELDING_TOL2 (DG_WELDING_TOL * DG_WELDING_TOL)
+
+	AdjacentdFaces& adjacentFaces = *((AdjacentdFaces*)context);
+
+	if (adjacentFaces.m_index != indexArray) {	
+		dgInt32 adjacentCount = adjacentFaces.m_count;
+		dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+
+		dgInt32 j0 = adjacentCount - 1;
+		dgInt32 indexJ0 = adjacentFaces.m_index[adjacentCount - 1];
+		for (dgInt32 j = 0; j < adjacentCount; j ++) {
+			dgInt32 indexJ1 = adjacentFaces.m_index[j];
+			dgBigVector q0 (&polygon[indexJ1 * stride]);
+			dgBigVector q1 (&polygon[indexJ0 * stride]);
+			dgBigVector q1q0 (q1 - q0);
+			dgFloat64 q1q0Mag2 = q1q0 % q1q0;
+
+			dgInt32 indexI0 = indexArray[indexCount - 1];
+			for (dgInt32 i = 0; i < indexCount; i ++) {
+				dgInt32 indexI1 = indexArray[i];
+				dgBigVector p0 (&polygon[indexI0 * stride]);
+				dgBigVector p1 (&polygon[indexI1 * stride]);
+				dgBigVector p1p0 (p1 - p0);
+				dgFloat64 dot = p1p0 % q1q0;
+				if (dot > 0.0f) {
+					dgFloat64 q1p0Mag2 = p1p0 % p1p0;
+					if ((dot * dot) >= (q1p0Mag2 * q1q0Mag2 * dgFloat64(0.99995f))) {
+						dgFloat64 x0 = q0 % q1q0;
+						dgFloat64 x1 = q1 % q1q0;
+						dgFloat64 y0 = p0 % q1q0;
+						dgFloat64 y1 = p1 % q1q0;
+						dgAssert (x1 > x0);
+						dgAssert (y1 > y0);
+						if (!((y0 >= x1) || (y1 <= x0))) {
+							dgFloat64 t = ((p0 - q0) % q1q0) / q1q0Mag2;
+							dgBigVector q (q0 + q1q0.Scale3(t));
+							dgBigVector dist (p0 - q);
+							dgFloat64 err2 = dist % dist;
+							if (err2 < DG_WELDING_TOL2) {
+								dgFloat32 maxDist = dgFloat32 (0.0f);
+								for (dgInt32 k = 0; k < indexCount; k ++) {
+									dgVector r (&polygon[indexArray[k] * stride]);
+									dgFloat32 dist = adjacentFaces.m_normal.Evalue(r);
+									if (dgAbsf (dist) > dgAbsf (maxDist)) {
+										maxDist = dist;
+									}
+								}
+
+								if (adjacentFaces.m_index[j0 + adjacentCount + 2] == -1) {
+									if (maxDist < -dgFloat32 (1.0e-3f)) {
+										adjacentFaces.m_index[j0 + adjacentCount + 2] = indexArray[indexCount + 1];
+									} else {
+										adjacentFaces.m_index[j0 + adjacentCount + 2] = adjacentFaces.m_index[adjacentCount + 1];
+									}
+								} else {
+									if (maxDist < -dgFloat32 (1.0e-3f)) {
+										dgBigVector n0 (adjacentFaces.m_normal[0], adjacentFaces.m_normal[1], adjacentFaces.m_normal[2], dgFloat64(0.0f));
+										dgBigVector n1 (&polygon[adjacentFaces.m_index[j0 + adjacentCount + 2] * stride]);
+										dgBigVector n2 (&polygon[indexArray[indexCount + 1] * stride]);
+
+										dgBigVector tilt0 (n0 * n1); 
+										dgBigVector tilt1 (n0 * n2); 
+										dgFloat64 dist0 (q1q0 % tilt0);
+										dgFloat64 dist1 (q1q0 % tilt1);
+										if (dist0 < dist1) {
+											adjacentFaces.m_index[j0 + adjacentCount + 2] = indexArray[indexCount + 1];
+										}
+									} else {
+										adjacentFaces.m_index[j0 + adjacentCount + 2] = adjacentFaces.m_index[adjacentCount + 1];
+									}
+								}
+								break;
+							}
+						}
+					}
+				}
+				indexI0 = indexI1;
+			}
+			j0 = j;
+			indexJ0 = indexJ1;
+		}
+	}
+	return t_ContinueSearh;
+}
+
+
+
+dgAABBPolygonSoup::dgNodeBuilder* dgAABBPolygonSoup::BuildTopDown (dgNodeBuilder* const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgNodeBuilder** const allocator) const
+{
+	dgAssert (firstBox >= 0);
+	dgAssert (lastBox >= 0);
+
+	if (lastBox == firstBox) {
+		return &leafArray[firstBox];
+	} else {
+		dgSpliteInfo info (&leafArray[firstBox], lastBox - firstBox + 1);
+
+		dgNodeBuilder* const parent = new (*allocator) dgNodeBuilder (info.m_p0, info.m_p1);
+		*allocator = *allocator + 1;
+
+		dgAssert (parent);
+		parent->m_right = BuildTopDown (leafArray, firstBox + info.m_axis, lastBox, allocator);
+		parent->m_right->m_parent = parent;
+
+		parent->m_left = BuildTopDown (leafArray, firstBox, firstBox + info.m_axis - 1, allocator);
+		parent->m_left->m_parent = parent;
+		return parent;
+	}
+}
+
+
+
+
+
+void dgAABBPolygonSoup::Create (const dgPolygonSoupDatabaseBuilder& builder, bool optimizedBuild)
+{
+	if (builder.m_faceCount == 0) {
+		return;
+	}
+	dgAssert (builder.m_faceCount >= 1);
+	m_strideInBytes = sizeof (dgTriplex);
+	m_nodesCount = ((builder.m_faceCount - 1) < 1) ? 1 : builder.m_faceCount - 1;
+	m_aabb = (dgNode*) dgMallocStack (sizeof (dgNode) * m_nodesCount);
+	m_indexCount = builder.m_indexCount * 2 + builder.m_faceCount;
+	if (builder.m_faceCount == 1) {
+		m_indexCount *= 2;
+	}
+	m_indices = (dgInt32*) dgMallocStack (sizeof (dgInt32) * m_indexCount);
+	dgStack<dgVector> tmpVertexArrayCont(builder.m_vertexCount + builder.m_normalCount + builder.m_faceCount * 2 + 4);
+
+	dgVector* const tmpVertexArray = &tmpVertexArrayCont[0];
+	for (dgInt32 i = 0; i < builder.m_vertexCount; i ++) {
+		tmpVertexArray[i] = builder.m_vertexPoints[i];
+	}
+
+	for (dgInt32 i = 0; i < builder.m_normalCount; i ++) {
+		tmpVertexArray[i + builder.m_vertexCount] = builder.m_normalPoints[i];
+	}
+
+	const dgInt32* const indices = &builder.m_vertexIndex[0];
+	dgStack<dgNodeBuilder> constructor (builder.m_faceCount * 2 + 16); 
+
+	dgInt32 polygonIndex = 0;
+	dgInt32 allocatorIndex = 0;
+	if (builder.m_faceCount == 1) {
+		dgInt32 indexCount = builder.m_faceVertexCount[0] - 1;
+		new (&constructor[allocatorIndex]) dgNodeBuilder (&tmpVertexArray[0], 0, indexCount, &indices[0]);
+		allocatorIndex ++;
+	}
+	for (dgInt32 i = 0; i < builder.m_faceCount; i ++) {
+		dgInt32 indexCount = builder.m_faceVertexCount[i] - 1;
+		new (&constructor[allocatorIndex]) dgNodeBuilder (&tmpVertexArray[0], i, indexCount, &indices[polygonIndex]);
+		allocatorIndex ++;
+		polygonIndex += (indexCount + 1);
+	}
+
+	dgNodeBuilder* contructorAllocator = &constructor[allocatorIndex];
+	dgNodeBuilder* root = BuildTopDown (&constructor[0], 0, allocatorIndex - 1, &contructorAllocator);
+
+	dgAssert (root);
+	if (root->m_left) {
+
+		dgAssert (root->m_right);
+		dgList<dgNodeBuilder*> list (builder.m_allocator);
+		dgList<dgNodeBuilder*> stack (builder.m_allocator);
+		stack.Append(root);
+		while (stack.GetCount()) {
+			dgList<dgNodeBuilder*>::dgListNode* const stackNode = stack.GetLast();
+			dgNodeBuilder* const node = stackNode->GetInfo();
+			stack.Remove(stackNode);
+
+			if (node->m_left) {
+				dgAssert (node->m_right);
+				list.Append(node);
+				stack.Append(node->m_right);
+				stack.Append(node->m_left);
+			} 
+		}
+
+		dgFloat64 newCost = dgFloat32 (1.0e20f);
+		dgFloat64 prevCost = newCost;
+		do {
+			prevCost = newCost;
+			for (dgList<dgNodeBuilder*>::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) {
+				dgNodeBuilder* const node = listNode->GetInfo();
+				ImproveNodeFitness (node);
+			}
+
+			newCost = dgFloat32 (0.0f);
+			for (dgList<dgNodeBuilder*>::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) {
+				dgNodeBuilder* const node = listNode->GetInfo();
+				newCost += node->m_area;
+			}
+		} while (newCost < (prevCost * dgFloat32 (0.9999f)));
+
+		root = list.GetLast()->GetInfo();
+		while (root->m_parent) {
+			root = root->m_parent;
+		}
+	}
+
+	dgList<dgNodeBuilder*> list (builder.m_allocator);
+
+	list.Append(root);
+	dgInt32 nodeIndex = 0;
+	while (list.GetCount())  {
+		dgNodeBuilder* const node = list.GetFirst()->GetInfo();
+		list.Remove(list.GetFirst());
+
+		if (node->m_left) {
+			node->m_enumeration = nodeIndex;
+			nodeIndex ++;
+			dgAssert (node->m_right);
+			list.Append(node->m_left);
+			list.Append(node->m_right);
+		}
+	}
+	dgAssert(!list.GetCount());
+
+	dgInt32 aabbBase = builder.m_vertexCount + builder.m_normalCount;
+
+
+	dgVector* const aabbPoints = &tmpVertexArray[aabbBase];
+
+
+	dgInt32 vertexIndex = 0;
+	dgInt32 aabbNodeIndex = 0;
+	list.Append(root);
+	dgInt32 indexMap = 0;
+	while (list.GetCount())  {
+
+		dgNodeBuilder* const node = list.GetFirst()->GetInfo();
+		list.Remove(list.GetFirst());
+
+		if (node->m_enumeration >= 0) {
+			dgAssert (node->m_left);
+			dgAssert (node->m_right);
+			dgNode& aabbNode = m_aabb[aabbNodeIndex];
+			aabbNodeIndex ++;
+			dgAssert (aabbNodeIndex <= m_nodesCount);
+
+			if (node->m_parent) {
+				if (node->m_parent->m_left == node) {
+					m_aabb[node->m_parent->m_enumeration].m_left = dgNode::dgLeafNodePtr (dgUnsigned32 (&m_aabb[node->m_enumeration] - m_aabb));
+				} else {
+					dgAssert (node->m_parent->m_right == node);
+					m_aabb[node->m_parent->m_enumeration].m_right = dgNode::dgLeafNodePtr (dgUnsigned32 (&m_aabb[node->m_enumeration] - m_aabb));
+				}
+			}
+
+			aabbPoints[vertexIndex + 0] = node->m_p0;
+			aabbPoints[vertexIndex + 1] = node->m_p1;
+
+			aabbNode.m_indexBox0 = aabbBase + vertexIndex;
+			aabbNode.m_indexBox1 = aabbBase + vertexIndex + 1;
+
+			vertexIndex += 2;
+
+		} else {
+			dgAssert (!node->m_left);
+			dgAssert (!node->m_right);
+
+			if (node->m_parent) {
+				if (node->m_parent->m_left == node) {
+					m_aabb[node->m_parent->m_enumeration].m_left = dgNode::dgLeafNodePtr (node->m_indexCount, indexMap);
+				} else {
+					dgAssert (node->m_parent->m_right == node);
+					m_aabb[node->m_parent->m_enumeration].m_right = dgNode::dgLeafNodePtr (node->m_indexCount, indexMap);
+				}
+			}
+
+			// index format i0, i1, i2, ... , id, normal, e0Normal, e1Normal, e2Normal, ..., faceSize
+			for (dgInt32 j = 0; j < node->m_indexCount; j ++) {
+				m_indices[indexMap + j] = node->m_faceIndices[j];
+				m_indices[indexMap + j + node->m_indexCount + 2] = -1;
+			}
+
+			// face attribute
+			m_indices[indexMap + node->m_indexCount] = node->m_faceIndices[node->m_indexCount];
+			// face normal
+			m_indices[indexMap + node->m_indexCount + 1] = builder.m_vertexCount + builder.m_normalIndex[node->m_faceIndex];
+			// face size
+			m_indices[indexMap + node->m_indexCount * 2 + 2] = dgInt32 (CalculateFaceMaxSize (&tmpVertexArray[0], node->m_indexCount, node->m_faceIndices));
+
+			indexMap += node->m_indexCount * 2 + 3;
+		}
+
+		if (node->m_left) {
+			dgAssert (node->m_right);
+			list.Append(node->m_left);
+			list.Append(node->m_right);
+		}
+	}
+
+	dgStack<dgInt32> indexArray (vertexIndex);
+	dgInt32 aabbPointCount = dgVertexListToIndexList (&aabbPoints[0].m_x, sizeof (dgVector), sizeof (dgTriplex), 0, vertexIndex, &indexArray[0], dgFloat32 (1.0e-6f));
+
+	m_vertexCount = aabbBase + aabbPointCount;
+	m_localVertex = (dgFloat32*) dgMallocStack (sizeof (dgTriplex) * m_vertexCount);
+
+	dgTriplex* const dstPoints = (dgTriplex*)m_localVertex;
+	for (dgInt32 i = 0; i < m_vertexCount; i ++) {
+		dstPoints[i].m_x = tmpVertexArray[i].m_x;
+		dstPoints[i].m_y = tmpVertexArray[i].m_y;
+		dstPoints[i].m_z = tmpVertexArray[i].m_z;
+	}
+
+	for (dgInt32 i = 0; i < m_nodesCount; i ++) {
+		dgNode& box = m_aabb[i];
+
+		dgInt32 j = box.m_indexBox0 - aabbBase;
+		box.m_indexBox0 = indexArray[j] + aabbBase;
+
+		j = box.m_indexBox1 - aabbBase;
+		box.m_indexBox1 = indexArray[j] + aabbBase;
+	}
+
+	if (builder.m_faceCount == 1) {
+		m_aabb[0].m_right = dgNode::dgLeafNodePtr (0, 0);
+	}
+//	CalculateAdjacendy();
+}
+
+void dgAABBPolygonSoup::Serialize (dgSerialize callback, void* const userData) const
+{
+	callback (userData, &m_vertexCount, sizeof (dgInt32));
+	callback (userData, &m_indexCount, sizeof (dgInt32));
+	callback (userData, &m_nodesCount, sizeof (dgInt32));
+	callback (userData, &m_nodesCount, sizeof (dgInt32));
+	if (m_aabb) {
+		callback (userData,  m_localVertex, sizeof (dgTriplex) * m_vertexCount);
+		callback (userData,  m_indices, sizeof (dgInt32) * m_indexCount);
+		callback (userData, m_aabb, sizeof (dgNode) * m_nodesCount);
+	}
+}
+
+void dgAABBPolygonSoup::Deserialize (dgDeserialize callback, void* const userData)
+{
+	m_strideInBytes = sizeof (dgTriplex);
+	callback (userData, &m_vertexCount, sizeof (dgInt32));
+	callback (userData, &m_indexCount, sizeof (dgInt32));
+	callback (userData, &m_nodesCount, sizeof (dgInt32));
+	callback (userData, &m_nodesCount, sizeof (dgInt32));
+
+	if (m_vertexCount) {
+		m_localVertex = (dgFloat32*) dgMallocStack (sizeof (dgTriplex) * m_vertexCount);
+		m_indices = (dgInt32*) dgMallocStack (sizeof (dgInt32) * m_indexCount);
+		m_aabb = (dgNode*) dgMallocStack (sizeof (dgNode) * m_nodesCount);
+
+		callback (userData, m_localVertex, sizeof (dgTriplex) * m_vertexCount);
+		callback (userData, m_indices, sizeof (dgInt32) * m_indexCount);
+		callback (userData, m_aabb, sizeof (dgNode) * m_nodesCount);
+	} else {
+		m_localVertex = NULL;
+		m_indices = NULL;
+		m_aabb = NULL;
+	}
+}
+
+
+dgVector dgAABBPolygonSoup::ForAllSectorsSupportVectex (const dgVector& dir) const
+{
+	dgVector supportVertex (dgFloat32 (0.0f));
+	if (m_aabb) {
+		dgFloat32 aabbProjection[DG_STACK_DEPTH];
+		const dgNode *stackPool[DG_STACK_DEPTH];
+
+		dgInt32 stack = 1;
+		stackPool[0] = m_aabb;
+		aabbProjection[0] = dgFloat32 (1.0e10f);
+		const dgTriplex* const boxArray = (dgTriplex*)m_localVertex;
+		
+
+		dgFloat32 maxProj = dgFloat32 (-1.0e20f); 
+		dgInt32 ix = (dir[0] > dgFloat32 (0.0f)) ? 1 : 0;
+		dgInt32 iy = (dir[1] > dgFloat32 (0.0f)) ? 1 : 0;
+		dgInt32 iz = (dir[2] > dgFloat32 (0.0f)) ? 1 : 0;
+
+		while (stack) {
+			dgFloat32 boxSupportValue;
+
+			stack--;
+			boxSupportValue = aabbProjection[stack];
+			if (boxSupportValue > maxProj) {
+				dgFloat32 backSupportDist = dgFloat32 (0.0f);
+				dgFloat32 frontSupportDist = dgFloat32 (0.0f);
+				const dgNode* const me = stackPool[stack];
+				if (me->m_left.IsLeaf()) {
+					backSupportDist = dgFloat32 (-1.0e20f);
+					dgInt32 index = dgInt32 (me->m_left.GetIndex());
+					dgInt32 vCount = me->m_left.GetCount();
+					dgVector vertex (dgFloat32 (0.0f));
+					for (dgInt32 j = 0; j < vCount; j ++) {
+						dgInt32 i0 = m_indices[index + j] * dgInt32 (sizeof (dgTriplex) / sizeof (dgFloat32));
+						dgVector p (&boxArray[i0].m_x);
+						dgFloat32 dist = p % dir;
+						if (dist > backSupportDist) {
+							backSupportDist = dist;
+							vertex = p;
+						}
+					}
+
+					if (backSupportDist > maxProj) {
+						maxProj = backSupportDist;
+						supportVertex = vertex; 
+					}
+
+				} else {
+					dgVector box[2];
+					const dgNode* const node = me->m_left.GetNode(m_aabb);
+					box[0].m_x = boxArray[node->m_indexBox0].m_x;
+					box[0].m_y = boxArray[node->m_indexBox0].m_y;
+					box[0].m_z = boxArray[node->m_indexBox0].m_z;
+					box[1].m_x = boxArray[node->m_indexBox1].m_x;
+					box[1].m_y = boxArray[node->m_indexBox1].m_y;
+					box[1].m_z = boxArray[node->m_indexBox1].m_z;
+
+					dgVector supportPoint (box[ix].m_x, box[iy].m_y, box[iz].m_z, dgFloat32 (0.0));
+					backSupportDist = supportPoint % dir;
+				}
+
+				if (me->m_right.IsLeaf()) {
+					frontSupportDist = dgFloat32 (-1.0e20f);
+					dgInt32 index = dgInt32 (me->m_right.GetIndex());
+					dgInt32 vCount = me->m_right.GetCount();
+					dgVector vertex (dgFloat32 (0.0f));
+					for (dgInt32 j = 0; j < vCount; j ++) {
+						dgInt32 i0 = m_indices[index + j] * dgInt32 (sizeof (dgTriplex) / sizeof (dgFloat32));
+						dgVector p (&boxArray[i0].m_x);
+						dgFloat32 dist = p % dir;
+						if (dist > frontSupportDist) {
+							frontSupportDist = dist;
+							vertex = p;
+						}
+					}
+					if (frontSupportDist > maxProj) {
+						maxProj = frontSupportDist;
+						supportVertex = vertex; 
+					}
+
+				} else {
+					dgVector box[2];
+					const dgNode* const node = me->m_right.GetNode(m_aabb);
+					box[0].m_x = boxArray[node->m_indexBox0].m_x;
+					box[0].m_y = boxArray[node->m_indexBox0].m_y;
+					box[0].m_z = boxArray[node->m_indexBox0].m_z;
+					box[1].m_x = boxArray[node->m_indexBox1].m_x;
+					box[1].m_y = boxArray[node->m_indexBox1].m_y;
+					box[1].m_z = boxArray[node->m_indexBox1].m_z;
+
+					dgVector supportPoint (box[ix].m_x, box[iy].m_y, box[iz].m_z, dgFloat32 (0.0f));
+					frontSupportDist = supportPoint % dir;
+				}
+
+				if (frontSupportDist >= backSupportDist) {
+					if (!me->m_left.IsLeaf()) {
+						aabbProjection[stack] = backSupportDist;
+						stackPool[stack] = me->m_left.GetNode(m_aabb);
+						stack++;
+					}
+
+					if (!me->m_right.IsLeaf()) {
+						aabbProjection[stack] = frontSupportDist;
+						stackPool[stack] = me->m_right.GetNode(m_aabb);
+						stack++;
+					}
+
+				} else {
+
+					if (!me->m_right.IsLeaf()) {
+						aabbProjection[stack] = frontSupportDist;
+						stackPool[stack] = me->m_right.GetNode(m_aabb);
+						stack++;
+					}
+
+					if (!me->m_left.IsLeaf()) {
+						aabbProjection[stack] = backSupportDist;
+						stackPool[stack] = me->m_left.GetNode(m_aabb);
+						stack++;
+					}
+				}
+			}
+		}
+	}
+	return supportVertex;
+}
+
+
+void dgAABBPolygonSoup::ForAllSectorsRayHit (const dgFastRayTest& raySrc, dgFloat32 maxParam, dgRayIntersectCallback callback, void* const context) const
+{
+	const dgNode *stackPool[DG_STACK_DEPTH];
+	dgFloat32 distance[DG_STACK_DEPTH];
+	dgFastRayTest ray (raySrc);
+
+	dgInt32 stack = 1;
+	const dgTriplex* const vertexArray = (dgTriplex*) m_localVertex;
+
+	stackPool[0] = m_aabb;
+	distance[0] = m_aabb->RayDistance(ray, vertexArray);
+	while (stack) {
+		stack --;
+		dgFloat32 dist = distance[stack];
+		if (dist > maxParam) {
+			break;
+		} else {
+			const dgNode *const me = stackPool[stack];
+			if (me->m_left.IsLeaf()) {
+				dgInt32 vCount = me->m_left.GetCount();
+				if (vCount > 0) {
+					dgInt32 index = dgInt32 (me->m_left.GetIndex());
+					dgFloat32 param = callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), &m_indices[index], vCount);
+					dgAssert (param >= dgFloat32 (0.0f));
+					if (param < maxParam) {
+						maxParam = param;
+						if (maxParam == dgFloat32 (0.0f)) {
+							break;
+						}
+					}
+				}
+
+			} else {
+				const dgNode* const node = me->m_left.GetNode(m_aabb);
+				dgFloat32 dist = node->RayDistance(ray, vertexArray);
+				if (dist < maxParam) {
+					dgInt32 j = stack;
+					for ( ; j && (dist > distance[j - 1]); j --) {
+						stackPool[j] = stackPool[j - 1];
+						distance[j] = distance[j - 1];
+					}
+					dgAssert (stack < DG_STACK_DEPTH);
+					stackPool[j] = node;
+					distance[j] = dist;
+					stack++;
+				}
+			}
+
+			if (me->m_right.IsLeaf()) {
+				dgInt32 vCount = me->m_right.GetCount();
+				if (vCount > 0) {
+					dgInt32 index = dgInt32 (me->m_right.GetIndex());
+					dgFloat32 param = callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), &m_indices[index], vCount);
+					dgAssert (param >= dgFloat32 (0.0f));
+					if (param < maxParam) {
+						maxParam = param;
+						if (maxParam == dgFloat32 (0.0f)) {
+							break;
+						}
+					}
+				}
+
+			} else {
+				const dgNode* const node = me->m_right.GetNode(m_aabb);
+				dgFloat32 dist = node->RayDistance(ray, vertexArray);
+				if (dist < maxParam) {
+					dgInt32 j = stack;
+					for ( ; j && (dist > distance[j - 1]); j --) {
+						stackPool[j] = stackPool[j - 1];
+						distance[j] = distance[j - 1];
+					}
+					dgAssert (stack < DG_STACK_DEPTH);
+					stackPool[j] = node;
+					distance[j] = dist;
+					stack++;
+				}
+			}
+		}
+	}
+}
+
+
+void dgAABBPolygonSoup::ForAllSectors (const dgFastAABBInfo& obbAabbInfo, const dgVector& boxDistanceTravel, dgFloat32 m_maxT, dgAABBIntersectCallback callback, void* const context) const
+{
+
+	dgAssert (dgAbsf(dgAbsf(obbAabbInfo[0][0]) - obbAabbInfo.m_absDir[0][0]) < dgFloat32 (1.0e-4f));
+	dgAssert (dgAbsf(dgAbsf(obbAabbInfo[1][1]) - obbAabbInfo.m_absDir[1][1]) < dgFloat32 (1.0e-4f));
+	dgAssert (dgAbsf(dgAbsf(obbAabbInfo[2][2]) - obbAabbInfo.m_absDir[2][2]) < dgFloat32 (1.0e-4f));
+
+	dgAssert (dgAbsf(dgAbsf(obbAabbInfo[0][1]) - obbAabbInfo.m_absDir[1][0]) < dgFloat32 (1.0e-4f));
+	dgAssert (dgAbsf(dgAbsf(obbAabbInfo[0][2]) - obbAabbInfo.m_absDir[2][0]) < dgFloat32 (1.0e-4f));
+	dgAssert (dgAbsf(dgAbsf(obbAabbInfo[1][2]) - obbAabbInfo.m_absDir[2][1]) < dgFloat32 (1.0e-4f));
+
+	if (m_aabb) {
+		dgFloat32 distance[DG_STACK_DEPTH];
+		const dgNode* stackPool[DG_STACK_DEPTH];
+
+		const dgInt32 stride = sizeof (dgTriplex) / sizeof (dgFloat32);
+		const dgTriplex* const vertexArray = (dgTriplex*) m_localVertex;
+
+		if ((boxDistanceTravel % boxDistanceTravel) < dgFloat32 (1.0e-8f)) {
+
+			dgInt32 stack = 1;
+			stackPool[0] = m_aabb;
+			distance[0] = m_aabb->BoxPenetration(obbAabbInfo, vertexArray);
+			while (stack) {
+				stack --;
+				dgFloat32 dist = distance[stack];
+				if (dist > dgFloat32 (0.0f)) {
+					const dgNode* const me = stackPool[stack];
+					if (me->m_left.IsLeaf()) {
+						dgInt32 index = dgInt32 (me->m_left.GetIndex());
+						dgInt32 vCount = me->m_left.GetCount();
+						if (vCount > 0) {
+							const dgInt32* const indices = &m_indices[index];
+							dgInt32 normalIndex = indices[vCount + 1];
+							dgVector faceNormal (&vertexArray[normalIndex].m_x);
+							dgFloat32 dist1 = obbAabbInfo.PolygonBoxDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x);
+							if (dist1 > dgFloat32 (0.0f)) {
+								dgAssert (vCount >= 3);
+								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, dist1) == t_StopSearh) {
+									return;
+								}
+							}
+						}
+
+					} else {
+						const dgNode* const node = me->m_left.GetNode(m_aabb);
+						dgFloat32 dist = node->BoxPenetration(obbAabbInfo, vertexArray);
+						if (dist > dgFloat32 (0.0f)) {
+							dgInt32 j = stack;
+							for ( ; j && (dist > distance[j - 1]); j --) {
+								stackPool[j] = stackPool[j - 1];
+								distance[j] = distance[j - 1];
+							}
+							dgAssert (stack < DG_STACK_DEPTH);
+							stackPool[j] = node;
+							distance[j] = dist;
+							stack++;
+						}
+					}
+
+					if (me->m_right.IsLeaf()) {
+						dgInt32 index = dgInt32 (me->m_right.GetIndex());
+						dgInt32 vCount = me->m_right.GetCount();
+						if (vCount > 0) {
+							const dgInt32* const indices = &m_indices[index];
+							dgInt32 normalIndex = indices[vCount + 1];
+							dgVector faceNormal (&vertexArray[normalIndex].m_x);
+							dgFloat32 dist1 = obbAabbInfo.PolygonBoxDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x);
+							if (dist1 > dgFloat32 (0.0f)) {
+								dgAssert (vCount >= 3);
+								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, dist1) == t_StopSearh) {
+									return;
+								}
+							}
+						}
+
+					} else {
+						const dgNode* const node = me->m_right.GetNode(m_aabb);
+						dgFloat32 dist = node->BoxPenetration(obbAabbInfo, vertexArray);
+						if (dist > dgFloat32 (0.0f)) {
+							dgInt32 j = stack;
+							for ( ; j && (dist > distance[j - 1]); j --) {
+								stackPool[j] = stackPool[j - 1];
+								distance[j] = distance[j - 1];
+							}
+							dgAssert (stack < DG_STACK_DEPTH);
+							stackPool[j] = node;
+							distance[j] = dist;
+							stack++;
+						}
+					}
+				}
+			}
+
+		} else {
+			dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), boxDistanceTravel);
+			dgFastRayTest obbRay (dgVector (dgFloat32 (0.0f)), obbAabbInfo.UnrotateVector(boxDistanceTravel));
+			dgInt32 stack = 1;
+			stackPool[0] = m_aabb;
+			distance [0] = m_aabb->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray);
+
+			while (stack) {
+				stack --;
+				const dgFloat32 dist = distance[stack];
+				const dgNode* const me = stackPool[stack];
+				if (dist < dgFloat32 (1.0f)) {
+
+					if (me->m_left.IsLeaf()) {
+						dgInt32 index = dgInt32 (me->m_left.GetIndex());
+						dgInt32 vCount = me->m_left.GetCount();
+						if (vCount > 0) {
+							const dgInt32* const indices = &m_indices[index];
+							dgInt32 normalIndex = indices[vCount + 1];
+							dgVector faceNormal (&vertexArray[normalIndex].m_x);
+							dgFloat32 hitDistance = obbAabbInfo.PolygonBoxRayDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x, ray);
+							if (hitDistance < dgFloat32 (1.0f)) {
+								dgAssert (vCount >= 3);
+								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, hitDistance) == t_StopSearh) {
+									return;
+								}
+							}
+						}
+
+					} else {
+						const dgNode* const node = me->m_left.GetNode(m_aabb);
+						dgFloat32 dist = node->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray);
+						if (dist < dgFloat32 (1.0f)) {
+							dgInt32 j = stack;
+							for ( ; j && (dist > distance[j - 1]); j --) {
+								stackPool[j] = stackPool[j - 1];
+								distance[j] = distance[j - 1];
+							}
+							dgAssert (stack < DG_STACK_DEPTH);
+							stackPool[j] = node;
+							distance[j] = dist;
+							stack++;
+						}
+					}
+
+					if (me->m_right.IsLeaf()) {
+						dgInt32 index = dgInt32 (me->m_right.GetIndex());
+						dgInt32 vCount = me->m_right.GetCount();
+						if (vCount > 0) {
+							const dgInt32* const indices = &m_indices[index];
+							dgInt32 normalIndex = indices[vCount + 1];
+							dgVector faceNormal (&vertexArray[normalIndex].m_x);
+							dgFloat32 hitDistance = obbAabbInfo.PolygonBoxRayDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x, ray);
+							if (hitDistance < dgFloat32 (1.0f)) {
+								dgAssert (vCount >= 3);
+								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, hitDistance) == t_StopSearh) {
+									return;
+								}
+							}
+						}
+
+					} else {
+						const dgNode* const node = me->m_right.GetNode(m_aabb);
+						dgFloat32 dist = node->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray);
+						if (dist < dgFloat32 (1.0f)) {
+							dgInt32 j = stack;
+							for ( ; j && (dist > distance[j - 1]); j --) {
+								stackPool[j] = stackPool[j - 1];
+								distance[j] = distance[j - 1];
+							}
+							dgAssert (stack < DG_STACK_DEPTH);
+							stackPool[j] = node;
+							distance[j] = dist;
+							stack ++;
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgAABBPolygonSoup.h

@@ -0,0 +1,241 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_AABB_POLYGON_SOUP_H_
+#define __DG_AABB_POLYGON_SOUP_H_
+
+#include "dgStdafx.h"
+#include "dgIntersections.h"
+#include "dgPolygonSoupDatabase.h"
+
+
+class dgPolygonSoupDatabaseBuilder;
+
+
+class dgAABBPolygonSoup: public dgPolygonSoupDatabase
+{
+	public:
+	class dgNode
+	{
+		public:
+		enum dgNodeType
+		{
+			m_binary = 0,
+			m_leaf,
+		};
+
+		class dgLeafNodePtr
+		{
+			#define DG_INDEX_COUNT_BITS 6
+
+			public:
+			DG_INLINE dgLeafNodePtr ()
+			{
+				dgAssert (0);
+			}
+
+			DG_INLINE dgLeafNodePtr (dgUnsigned32 node)
+			{
+				m_node = node;
+				dgAssert (!IsLeaf());
+			}
+
+			DG_INLINE dgUnsigned32 IsLeaf () const 
+			{
+				return m_node & 0x80000000;
+			}
+
+			DG_INLINE dgUnsigned32 GetCount() const 
+			{
+				dgAssert (IsLeaf());
+				return (m_node & (~0x80000000)) >> (32 - DG_INDEX_COUNT_BITS - 1);
+			}
+
+			DG_INLINE dgUnsigned32 GetIndex() const 
+			{
+				dgAssert (IsLeaf());
+				return m_node & (~(-(1 << (32 - DG_INDEX_COUNT_BITS - 1))));
+			}
+
+
+			DG_INLINE dgLeafNodePtr (dgUnsigned32 faceIndexCount, dgUnsigned32 faceIndexStart)
+			{
+				dgAssert (faceIndexCount < (1<<DG_INDEX_COUNT_BITS));
+				m_node = 0x80000000 | (faceIndexCount << (32 - DG_INDEX_COUNT_BITS - 1)) | faceIndexStart;
+			}
+
+			DG_INLINE dgNode* GetNode (const void* const root) const
+			{
+				return ((dgNode*) root) + m_node;
+			}
+
+			union {
+				dgUnsigned32 m_node;
+			};
+		};
+
+
+		dgNode ()
+			:m_indexBox0(0)
+			,m_indexBox1(0)
+			,m_left(0)
+			,m_right(0)
+		{
+		}
+
+		DG_INLINE dgFloat32 RayDistance (const dgFastRayTest& ray, const dgTriplex* const vertexArray) const
+		{
+			dgVector minBox (&vertexArray[m_indexBox0].m_x);
+			dgVector maxBox (&vertexArray[m_indexBox1].m_x);
+			return ray.BoxIntersect(minBox, maxBox);
+		}
+
+		DG_INLINE dgFloat32 BoxPenetration (const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
+		{
+			dgVector p0 (&vertexArray[m_indexBox0].m_x);
+			dgVector p1 (&vertexArray[m_indexBox1].m_x);
+			dgVector minBox (p0 - obb.m_p1);
+			dgVector maxBox (p1 - obb.m_p0);
+			dgVector mask ((minBox.CompProduct4(maxBox)) < dgVector (dgFloat32 (0.0f)));
+			//mask = mask & mask.ShiftTripleRight();
+			//mask = mask & mask.ShiftTripleRight();
+			dgVector dist (maxBox.GetMin (minBox.Abs()) & mask);
+			dist = dist.GetMin(dist.ShiftTripleRight());
+			dist = dist.GetMin(dist.ShiftTripleRight());
+
+			if (dist.GetScalar() > dgFloat32 (0.0f)) {
+				dgVector origin ((p1 + p0).CompProduct4(dgVector::m_half));
+				dgVector size ((p1 - p0).CompProduct4(dgVector::m_half));
+
+				origin = obb.UntransformVector(origin);
+				size = obb.m_absDir.RotateVector(size);
+				dgVector q0 (origin - size);
+				dgVector q1 (origin + size);
+				dgVector minBox (q0 - obb.m_size);
+				dgVector maxBox (q1 + obb.m_size);
+				dgVector mask ((minBox.CompProduct4(maxBox)) < dgVector (dgFloat32 (0.0f)));
+				//mask = mask & mask.ShiftTripleRight();
+				//mask = mask & mask.ShiftTripleRight();
+				dgVector dist1 (maxBox.GetMin (minBox.Abs()) & mask);
+				dist1 = dist1.GetMin(dist1.ShiftTripleRight());
+				dist1 = dist1.GetMin(dist1.ShiftTripleRight());
+				dist = dist.GetMin(dist1);
+			}
+			return dist.GetScalar();
+		}
+
+		DG_INLINE dgFloat32 BoxIntersect (const dgFastRayTest& ray, const dgFastRayTest& obbRay, const dgFastAABBInfo& obb, const dgTriplex* const vertexArray) const
+		{
+			dgVector p0 (&vertexArray[m_indexBox0].m_x);
+			dgVector p1 (&vertexArray[m_indexBox1].m_x);
+			dgVector minBox (p0 - obb.m_p1);
+			dgVector maxBox (p1 - obb.m_p0);
+			dgFloat32 dist = ray.BoxIntersect(minBox, maxBox);
+			if (dist < dgFloat32 (1.0f)) {
+				dgVector origin ((p1 + p0).CompProduct4(dgVector::m_half));
+				dgVector size ((p1 - p0).CompProduct4(dgVector::m_half));
+
+				origin = obb.UntransformVector(origin);
+				size = obb.m_absDir.RotateVector(size);
+				dgVector q0 (origin - size);
+				dgVector q1 (origin + size);
+
+				dgVector minBox1 (q0 - obb.m_size);
+				dgVector maxBox1 (q1 + obb.m_size);
+				dgFloat32 dist1 = obbRay.BoxIntersect(minBox1, maxBox1);
+				//dgAssert (dist1 <= 1.0f);
+				//dgAssert (dist == dist1);
+				//dist = dist1;
+				dist = (dist1  > dgFloat32 (1.0f)) ? dist1 : dgMax (dist1, dist);
+			}
+			return dist;
+		}
+
+
+
+		dgInt32 m_indexBox0;
+		dgInt32 m_indexBox1;
+		dgLeafNodePtr m_left;
+		dgLeafNodePtr m_right;
+	};
+
+	class dgSpliteInfo;
+	class dgNodeBuilder;
+
+	virtual void GetAABB (dgVector& p0, dgVector& p1) const;
+	virtual void Serialize (dgSerialize callback, void* const userData) const;
+	virtual void Deserialize (dgDeserialize callback, void* const userData);
+
+	protected:
+	dgAABBPolygonSoup ();
+	virtual ~dgAABBPolygonSoup ();
+
+	void Create (const dgPolygonSoupDatabaseBuilder& builder, bool optimizedBuild);
+	void CalculateAdjacendy ();
+	virtual void ForAllSectorsRayHit (const dgFastRayTest& ray, dgFloat32 maxT, dgRayIntersectCallback callback, void* const context) const;
+	virtual void ForAllSectors (const dgFastAABBInfo& obbAabb, const dgVector& boxDistanceTravel, dgFloat32 m_maxT, dgAABBIntersectCallback callback, void* const context) const;
+	
+
+	DG_INLINE void* GetRootNode() const 
+	{
+		return m_aabb;
+	}
+
+	DG_INLINE void* GetBackNode(const void* const root) const 
+	{
+		dgNode* const node = (dgNode*) root;
+		return node->m_left.IsLeaf() ? NULL : node->m_left.GetNode(m_aabb);
+	}
+
+	DG_INLINE void* GetFrontNode(const void* const root) const 
+	{
+		dgNode* const node = (dgNode*) root;
+		return node->m_right.IsLeaf() ? NULL : node->m_right.GetNode(m_aabb);
+	}
+
+	DG_INLINE void GetNodeAABB(const void* const root, dgVector& p0, dgVector& p1) const 
+	{
+		const dgNode* const node = (dgNode*)root;
+		p0 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox0].m_x);
+		p1 = dgVector (&((dgTriplex*)m_localVertex)[node->m_indexBox1].m_x);
+	}
+	virtual dgVector ForAllSectorsSupportVectex (const dgVector& dir) const;
+
+	
+
+	private:
+	dgNodeBuilder* BuildTopDown (dgNodeBuilder* const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgNodeBuilder** const allocator) const;
+	dgFloat32 CalculateFaceMaxSize (const dgVector* const vertex, dgInt32 indexCount, const dgInt32* const indexArray) const;
+//	static dgIntersectStatus CalculateManifoldFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount);
+	static dgIntersectStatus CalculateDisjointedFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
+	static dgIntersectStatus CalculateAllFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
+	void ImproveNodeFitness (dgNodeBuilder* const node) const;
+
+	dgInt32 m_nodesCount;
+	dgInt32 m_indexCount;
+	dgNode* m_aabb;
+	dgInt32* m_indices;
+};
+
+
+#endif
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgArray.h

@@ -0,0 +1,166 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgArray__
+#define __dgArray__
+
+#include "dgStdafx.h"
+
+template<class T>
+class dgArray
+{
+	public:
+	dgArray (dgInt32 granulatitySize, dgMemoryAllocator* const allocator, dgInt32 aligmentInBytes = DG_MEMORY_GRANULARITY);
+	~dgArray ();
+
+	DG_CLASS_ALLOCATOR(allocator)
+
+	
+	T& operator[] (dgInt32 i);
+	const T& operator[] (dgInt32 i) const;
+	void Resize (dgInt32 size) const;
+
+	dgInt32 GetElementSize() const;
+	dgInt32 GetBytesCapacity () const;
+	dgInt32 GetElementsCapacity () const; 
+
+	bool ExpandCapacityIfNeessesary (dgInt32 index, dgInt32 stride) const;
+
+	private:
+	dgInt32 m_granulatity;
+	dgInt32 m_aligmentInByte;
+	mutable dgInt32 m_maxSize;
+	mutable T *m_array;
+	dgMemoryAllocator* m_allocator; 
+};
+
+
+template<class T>
+dgArray<T>::dgArray (dgInt32 granulatitySize, dgMemoryAllocator* const allocator, dgInt32 aligmentInBytes)
+ :m_granulatity(granulatitySize), m_aligmentInByte(aligmentInBytes), m_maxSize(0), m_array(NULL), m_allocator(allocator)
+{
+	if (m_aligmentInByte <= 0) {
+		m_aligmentInByte = 8;
+	}
+	m_aligmentInByte = 1 << dgExp2(m_aligmentInByte);
+}
+
+template<class T>
+dgArray<T>::~dgArray ()
+{
+	if (m_array) {
+		m_allocator->FreeLow (m_array);
+	}
+}
+
+
+template<class T>
+const T& dgArray<T>::operator[] (dgInt32 i) const
+{ 
+	dgAssert (i >= 0);
+	while (i >= m_maxSize) {
+		Resize (i);
+	}
+	return m_array[i];
+}
+
+
+template<class T>
+T& dgArray<T>::operator[] (dgInt32 i)
+{
+	dgAssert (i >= 0);
+	while (i >= m_maxSize) {
+		Resize (i);
+	}
+	return m_array[i];
+}
+
+template<class T>
+dgInt32 dgArray<T>::GetElementSize() const
+{
+	return sizeof (T);
+}
+
+template<class T>
+dgInt32 dgArray<T>::GetElementsCapacity () const
+{
+	return m_maxSize;
+}
+
+
+template<class T>
+dgInt32 dgArray<T>::GetBytesCapacity () const
+{
+	return  m_maxSize * GetElementSize();
+}
+
+
+
+template<class T>
+void dgArray<T>::Resize (dgInt32 size) const
+{
+	if (size >= m_maxSize) {
+		size = size + m_granulatity - (size + m_granulatity) % m_granulatity;
+		T* const newArray = (T*) m_allocator->MallocLow (GetElementSize() * size, m_aligmentInByte);
+		if (m_array) {
+			for (dgInt32 i = 0; i < m_maxSize; i ++) {
+				newArray[i]	= m_array[i];
+			}
+			m_allocator->FreeLow (m_array);
+		}
+		m_array = newArray;
+		m_maxSize = size;
+	} else if (size < m_maxSize) {
+		size = size + m_granulatity - (size + m_granulatity) % m_granulatity;
+		T* const newArray = (T*) m_allocator->MallocLow (GetElementSize() * size, m_aligmentInByte);
+		if (m_array) {
+			for (dgInt32 i = 0; i < size; i ++) {
+				newArray[i]	= m_array[i];
+			}
+			m_allocator->FreeLow (m_array);
+		}
+		m_array = newArray;
+		m_maxSize = size;
+	}
+}
+
+template<class T>
+bool dgArray<T>::ExpandCapacityIfNeessesary (dgInt32 index, dgInt32 stride) const
+{
+	bool ret = false;
+	dgInt32 size = (index + 1) * stride;
+	while (size >= m_maxSize) {
+		ret = true;
+		Resize (m_maxSize * 2);
+	}
+	return ret;
+}
+
+#endif
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgAsyncThread.cpp

@@ -0,0 +1,68 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgThread.h"
+#include "dgAsyncThread.h"
+
+
+dgAsyncThread::dgAsyncThread(const char* const name, dgInt32 id)
+	:dgThread(name, id)
+	,m_myMutex()
+	,m_callerMutex()
+{
+	Init ();
+}
+
+dgAsyncThread::~dgAsyncThread(void)
+{
+	Terminate();
+}
+
+
+void dgAsyncThread::Terminate()
+{
+	if (StillBusy()) {
+		dgInterlockedExchange(&m_terminate, 1);
+		m_myMutex.Release();
+		Close();
+	}
+} 
+
+
+void dgAsyncThread::Execute (dgInt32 threadID)
+{
+	dgAssert (threadID == m_id);
+	while (!m_terminate) {
+		SuspendExecution(m_myMutex);
+		m_callerMutex.Release();
+		if (!m_terminate) {
+			TickCallback(threadID);
+		}
+	}
+}
+
+void dgAsyncThread::Tick()
+{
+	m_myMutex.Release();
+	SuspendExecution(m_callerMutex);
+}
+

newtondynamics.mod/NewtonDynamics/source/core/dgAsyncThread.h

@@ -0,0 +1,47 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_ASYNC_THREAD_H__
+#define __DG_ASYNC_THREAD_H__
+
+#include "dgTypes.h"
+#include "dgThread.h"
+
+class dgAsyncThread: public dgThread
+{
+	public:
+	dgAsyncThread(const char* const name, dgInt32 id);
+	virtual ~dgAsyncThread(void);
+
+	void Tick(); 
+	void Terminate(); 
+
+	protected:
+	virtual void Execute (dgInt32 threadID);
+	virtual void TickCallback (dgInt32 threadID) = 0;
+
+	private:
+//	dgInt32 m_terminate;
+	dgSemaphore m_myMutex;
+	dgSemaphore m_callerMutex;
+};
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgCRC.cpp

@@ -0,0 +1,390 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgCRC.h"
+#include "dgList.h"
+#include "dgTree.h"
+#include "dgDebug.h"
+#include "dgRandom.h"
+
+namespace InternalCRC
+{
+	const dgInt32 STRING_POOL_SIZE =	1024 * 8 - 256;
+
+	static dgUnsigned32 randBits[] = {
+		0x00000001, 0x2C11F801, 0xDFD8F60E, 0x6C8FA2B7, 
+		0xB573754C, 0x1522DCDD, 0x21615D3A, 0xE1B307F3, 
+		0x12AFA158, 0x53D18179, 0x70950126, 0x941702EF, 
+		0x756FE824, 0x694801D5, 0x36DF4DD2, 0x63D80FAB, 
+		0xB8AE95B0, 0x902439F1, 0x090C6F3E, 0x2B7C6A27, 
+		0x8344B5FC, 0x67D3C5CD, 0x22F5516A, 0x2FB00E63, 
+		0xFC761508, 0x87B00169, 0x27EBA056, 0x8CC0B85F, 
+		0xE33D3ED4, 0x95DA08C5, 0x13E5C802, 0x9DD9E41B, 
+		0xD4577F60, 0x3DD6B7E1, 0x096AF46E, 0x1A00CD97, 
+		0x4B10E2AC, 0x22EAAABD, 0x683F119A, 0x62D070D3, 
+		0xA8D034B8, 0xAA363D59, 0x58CECB86, 0x40F589CF, 
+		0x4F630184, 0x38918BB5, 0xB85B8E32, 0x0A6A948B, 
+		0x9A099510, 0x402871D1, 0x11E7859E, 0xEE73CD07, 
+		0x4142FB5C, 0x39D68BAD, 0x0FE19DCA, 0xE35B2F43, 
+		0x75590068, 0x66433549, 0x929182B6, 0x71EC773F, 
+		0xBBAC3034, 0xF2BD8AA5, 0x5743A062, 0x5AB120FB, 
+		0x5ABFD6C0, 0xDDD867C1, 0xDC3522CE, 0xD0EC6877, 
+		0xE106000C, 0xB7C6689D, 0xED3FF5FA, 0xC75749B3, 
+		0x126B7818, 0x1A75E939, 0x0546C5E6, 0x8A9C80AF, 
+		0x48A3CAE4, 0x756D0595, 0x7060FE92, 0xA594896B, 
+		0x12354470, 0x896599B1, 0xDAC6CBFE, 0xCB419FE7, 
+		0x9C44F0BC, 0xAFA9418D, 0xB87D1A2A, 0x428BC023, 
+		0x33229BC8, 0xC92D5929, 0xB1C19516, 0x0FBCA61F, 
+		0xE594D194, 0x716EFC85, 0x0036A8C2, 0xD7BBCDDB, 
+		0x16E4DE20, 0xD10F07A1, 0x68CF812E, 0x390A7357, 
+		0x8BAACD6C, 0x2C2E167D, 0x3E7C0A5A, 0x167F9293, 
+		0x3D596B78, 0x08888519, 0x9994F046, 0x0FC3E78F, 
+		0x008A4444, 0x87526F75, 0xB0079EF2, 0x238DEE4B, 
+		0xCA09A3D0, 0x4ED3B191, 0xFA42425E, 0x379DE2C7, 
+		0x1EA2961C, 0x1FC3E76D, 0x90DFC68A, 0x0279C103, 
+		0xF9AAE728, 0xF2666D09, 0xEF13D776, 0x92E944FF, 
+		0x364F22F4, 0x37665E65, 0x05D6E122, 0x7131EABB, 
+		0x479E9580, 0x98729781, 0x4BD20F8E, 0x1612EE37, 
+		0xCB574ACC, 0x5499B45D, 0x360B4EBA, 0x33814B73, 
+		0x43720ED8, 0x146610F9, 0x45514AA6, 0x0B23BE6F, 
+		0x026E6DA4, 0xD1B9C955, 0x94676F52, 0xCE8EC32B, 
+		0x165EB330, 0x2F6AB971, 0x92F1E8BE, 0xC54095A7, 
+		0xBEB3EB7C, 0x5C9E7D4D, 0x5921A2EA, 0xB45D31E3, 
+		0xB8C9E288, 0x5FE670E9, 0xC02049D6, 0xC42A53DF, 
+		0x6F332454, 0x661BB045, 0x2B3C4982, 0xDF4B779B, 
+		0xD7C4FCE0, 0x70FB1761, 0xADD4CDEE, 0x47BDD917, 
+		0x8C63782C, 0x8181423D, 0xFA05C31A, 0xDD947453, 
+		0x6A8D6238, 0x1A068CD9, 0x4413D506, 0x5374054F, 
+		0xC5A84704, 0xB41B1335, 0x06986FB2, 0x4CCF080B, 
+		0xF80C7290, 0x8622B151, 0x536DBF1E, 0x21E1B887, 
+		0xDED0F0DC, 0xB4B1032D, 0x1D5AAF4A, 0xC56E12C3, 
+		0x8C578DE8, 0xCBA564C9, 0xA67EEC36, 0x0837D2BF, 
+		0x3D98D5B4, 0x1B06F225, 0xFF7EE1E2, 0x3640747B, 
+		0x5E301440, 0x53A08741, 0x436FBC4E, 0xC9C333F7, 
+		0x2727558C, 0x7F5CC01D, 0xFC83677A, 0xAFF10D33, 
+		0x24836598, 0x3161F8B9, 0xDD748F66, 0x5B6CBC2F, 
+		0xAD8FD064, 0x89EE4D15, 0xBBB2A012, 0xA086BCEB, 
+		0x1BEAE1F0, 0x69F39931, 0x764DC57E, 0x17394B67, 
+		0x4D51A63C, 0xF273790D, 0x35A2EBAA, 0x7EE463A3, 
+		0xBC2BE948, 0x2B9B48A9, 0x2FC7BE96, 0x5FC9C19F, 
+		0x3AD83714, 0x6FA02405, 0xDDB6AA42, 0xE648E15B, 
+		0x1DB7DBA0, 0xF55AE721, 0x4D3ADAAE, 0xB3DAFED7, 
+		0x5FFAE2EC, 0x96A42DFD, 0xFB9C3BDA, 0x21CF1613, 
+		0x0F2C18F8, 0xAE705499, 0x650B79C6, 0x31C5E30F, 
+		0x097D09C4, 0xAAAB76F5, 0x34CE0072, 0x27EDE1CB, 
+		0xDAD20150, 0xADD57111, 0xC229FBDE, 0x8AFF4E47, 
+		0x448E0B9C, 0x5C5DDEED, 0x4612580A, 0x05F82483, 
+		0xBC1EF4A8, 0xB1C01C89, 0xF592C0F6, 0x6798207F, 
+		0xEC494874, 0x795F45E5, 0xECFBA2A2, 0xBB9CBE3B, 
+		0xF567104f, 0x47289407, 0x25683fa6, 0x2fde5836, 
+	};
+
+	struct StringPool
+	{
+		char buff[STRING_POOL_SIZE];
+
+		StringPool ()
+		{
+		}
+
+		StringPool (const StringPool &arg)
+		{
+		}
+	};
+	
+/*	
+	class CRCStringLookup: public dgTree<char *, dgUnsigned32>, public dgList<StringPool>
+	{
+	
+		dgInt32 size;
+		char *ptr;
+	
+		void AddContainer ()
+		{
+			StringPool tmp;
+			dgListNode *node;
+	
+//			Addtop (tmp);
+//			node = GetFirst();
+			node = Addtop (tmp);
+
+			ptr = node->GetInfo().buff;
+			size = STRING_POOL_SIZE;
+		}
+	
+		public:
+
+		CRCStringLookup () 
+			:dgTree<char *, dgUnsigned32>(NULL), dgList<StringPool>()
+		{
+			AddContainer();
+		}
+	
+		~CRCStringLookup () 
+		{
+		}
+
+
+		void AddString (dgUnsigned32 crc, const char *string)
+		{
+			dgInt32 length;
+	
+			if (dgTree<char *, dgUnsigned32>::Find(crc)) {
+				return;
+			}
+	
+			length = dgInt32 (strlen (string)) + 1;
+			if (size < length) {
+				AddContainer();
+			}
+	
+			strcpy (ptr, string);
+			Insert (ptr, crc);
+	
+			ptr += length;
+			size -= length;
+		}
+
+		const char* FindString (dgUnsigned32 crc) const
+		{
+			dgTreeNode *node;
+
+			node = dgTree<char *, dgUnsigned32>::Find(crc);
+//			return node ? node->GetInfo() : NULL;
+			return node ? node->GetInfo() : NULL;
+		}
+	};
+
+	static CRCStringLookup& GetDatabase ()
+	{
+		static CRCStringLookup database;
+		return database;
+	}
+*/
+
+
+
+	/*
+	dgUnsigned32 RSHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 b    = 378551;
+	dgUnsigned32 a    = 63689;
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)	{
+	hash = hash * a + (*str);
+	a    = a * b;
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of RS Hash Function 
+
+
+	dgUnsigned32 JSHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 1315423911;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash ^= ((hash << 5) + (*str) + (hash >> 2));
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of JS Hash Function 
+
+
+	dgUnsigned32 PJWHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 BitsInUnsignedInt = (dgUnsigned32)(sizeof(dgUnsigned32) * 8);
+	dgUnsigned32 ThreeQuarters     = (dgUnsigned32)((BitsInUnsignedInt  * 3) / 4);
+	dgUnsigned32 OneEighth         = (dgUnsigned32)(BitsInUnsignedInt / 8);
+	dgUnsigned32 HighBits          = (dgUnsigned32)(0xFFFFFFFF) << (BitsInUnsignedInt - OneEighth);
+	dgUnsigned32 hash              = 0;
+	dgUnsigned32 test              = 0;
+	dgUnsigned32 i                 = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (hash << OneEighth) + (*str);
+
+	if((test = hash & HighBits)  != 0)
+	{
+	hash = (( hash ^ (test >> ThreeQuarters)) & (~HighBits));
+	}
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of  P. J. Weinberger Hash Function 
+
+
+	dgUnsigned32 ELFHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 x    = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (hash << 4) + (*str);
+	if((x = hash & 0xF0000000L) != 0)
+	{
+	hash ^= (x >> 24);
+	hash &= ~x;
+	}
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of ELF Hash Function 
+
+
+	dgUnsigned32 BKDRHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 seed = 131; // 31 131 1313 13131 131313 etc.. 
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (hash * seed) + (*str);
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of BKDR Hash Function 
+
+
+	dgUnsigned32 SDBMHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = (*str) + (hash << 6) + (hash << 16) - hash;
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of SDBM Hash Function 
+
+
+	dgUnsigned32 DEKHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = len;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash = ((hash << 5) ^ (hash >> 27)) ^ (*str);
+	}
+	return (hash & 0x7FFFFFFF);
+	}
+	// End Of DEK Hash Function 
+
+
+	dgUnsigned32 APHash(char* str, dgUnsigned32 len)
+	{
+	dgUnsigned32 hash = 0;
+	dgUnsigned32 i    = 0;
+
+	for(i = 0; i < len; str++, i++)
+	{
+	hash ^= ((i & 1) == 0) ? (  (hash <<  7) ^ (*str) ^ (hash >> 3)) :
+	(~((hash << 11) ^ (*str) ^ (hash >> 5)));
+	}
+
+	return (hash & 0x7FFFFFFF);
+	}
+	*/
+
+	// End Of DJB Hash Function 
+	dgUnsigned32 DJBHash(const char* const str, dgInt32 len)
+	{
+		dgUnsigned32 hash = 5381;
+		for(dgInt32 i = 0; i < len; i++)
+		{
+			//hash = ((hash << 5) + hash) + (*str);
+			hash = ((hash << 5) + hash) + str[i];
+		}
+
+		return (hash & 0x7FFFFFFF);
+	}
+}	
+
+
+dgUnsigned32 dgApi dgCRC (const char* const name)
+{
+//	dgUnsigned32 c;
+//	dgUnsigned32 crc;
+//	unsigned char *ptr;
+//	dgUnsigned32 val;
+	if (!name) {
+		return 0;
+	}
+
+	dgUnsigned32 crc = 0;
+	//for (ptr = (unsigned char*)name; *ptr; ptr ++) {
+	for (dgInt32 i = 0; name[i]; i ++) {
+		char c = name[i];
+		dgUnsigned32 val = InternalCRC::randBits[((crc >> 24) ^ c) & 0xff];
+		crc = (crc << 8) ^ val;
+	}
+
+	dgAssert (0);
+//	InternalCRC::GetDatabase().AddString (crc, name);
+	return crc;
+}
+
+
+
+dgUnsigned32 dgApi dgCRC (const void* const buffer, dgInt32 size, dgUnsigned32 crcAcc)
+{
+	dgAssert (buffer);
+	unsigned char* const ptr = (unsigned char*)buffer;
+	for (dgInt32 i = 0; i < size; i ++) {
+		dgUnsigned32 c = ptr[i];
+		dgUnsigned32 val = InternalCRC::randBits[((crcAcc >> 24) ^ c) & 0xff];
+		crcAcc = (crcAcc << 8) ^ val;
+	}
+	return crcAcc;
+}
+
+
+const char* dgApi dgInverseCRC (dgUnsigned32 crc)
+{	
+	dgAssert (0);
+	return NULL;
+//	return InternalCRC::GetDatabase().FindString (crc);
+}
+	
+
+
+dgUnsigned32 dgApi dgHash (const void* const string, int size)
+{
+	return InternalCRC::DJBHash ((char*)string, size);
+}

newtondynamics.mod/NewtonDynamics/source/core/dgCRC.h

@@ -0,0 +1,35 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgCRC__
+#define __dgCRC__
+
+#include "dgStdafx.h"
+
+
+dgUnsigned32 dgApi dgCRC (const char* const string);
+dgUnsigned32 dgApi dgCRC (const void* const buffer, dgInt32 size, dgUnsigned32 crcAcc = 0);
+const char* dgApi dgInverseCRC (dgUnsigned32 CRC);
+
+dgUnsigned32 dgApi dgHash (const void* const string, dgInt32 size);
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgConvexHull3d.cpp

@@ -0,0 +1,981 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgStack.h"
+#include "dgTree.h"
+#include "dgGoogol.h"
+#include "dgConvexHull3d.h"
+#include "dgSmallDeterminant.h"
+
+#define DG_VERTEX_CLUMP_SIZE_3D		8 
+
+
+class dgAABBPointTree3d
+{
+	public:
+#ifdef _DEBUG
+	dgAABBPointTree3d()
+	{
+		static dgInt32 id = 0;
+		m_id = id;
+		id ++;
+	}
+	dgInt32 m_id;
+#endif
+
+	dgBigVector m_box[2];
+	dgAABBPointTree3d* m_left;
+	dgAABBPointTree3d* m_right;
+	dgAABBPointTree3d* m_parent;
+};
+
+class dgHullVertex: public dgBigVector
+{
+	public:
+	dgInt32 m_index;
+};	
+
+class dgAABBPointTree3dClump: public dgAABBPointTree3d
+{
+	public:
+	dgInt32 m_count;
+	dgInt32 m_indices[DG_VERTEX_CLUMP_SIZE_3D];
+};
+
+
+dgConvexHull3DFace::dgConvexHull3DFace()
+{
+	m_mark = 0; 
+	m_twin[0] = NULL;
+	m_twin[1] = NULL;
+	m_twin[2] = NULL;
+}
+
+dgFloat64 dgConvexHull3DFace::Evalue (const dgBigVector* const pointArray, const dgBigVector& point) const
+{
+	const dgBigVector& p0 = pointArray[m_index[0]];
+	const dgBigVector& p1 = pointArray[m_index[1]];
+	const dgBigVector& p2 = pointArray[m_index[2]];
+
+	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] = point[i] - p0[i];
+	}
+
+	dgFloat64 error;
+	dgFloat64 det = Determinant3x3 (matrix, &error);
+
+	// the code use double, however the threshold for accuracy test is the machine precision of a float.
+	// by changing this to a smaller number, the code should run faster since many small test will be considered valid
+	// the precision must be a power of two no smaller than the machine precision of a double, (1<<48)
+	// float64(1<<30) can be a good value
+
+	// dgFloat64 precision  = dgFloat64 (1.0f) / dgFloat64 (1<<30);
+	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(point[i]) - dgGoogol(p0[i]);
+	}
+	return Determinant3x3(exactMatrix);
+}
+
+dgBigPlane dgConvexHull3DFace::GetPlaneEquation (const dgBigVector* const pointArray) const
+{
+	const dgBigVector& p0 = pointArray[m_index[0]];
+	const dgBigVector& p1 = pointArray[m_index[1]];
+	const dgBigVector& p2 = pointArray[m_index[2]];
+	dgBigPlane plane (p0, p1, p2);
+	plane = plane.Scale (1.0f / sqrt (plane % plane));
+	return plane;
+}
+
+
+dgConvexHull3d::dgConvexHull3d (dgMemoryAllocator* const allocator)
+	:dgList<dgConvexHull3DFace>(allocator)
+	,m_count (0)
+	,m_diag()
+	,m_aabbP0(dgBigVector (dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0)))
+	,m_aabbP1(dgBigVector (dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0)))
+	,m_points(1024, allocator)
+{
+}
+
+dgConvexHull3d::dgConvexHull3d(const dgConvexHull3d& source)
+	:dgList<dgConvexHull3DFace>(source.GetAllocator())
+	,m_count (source.m_count)
+	,m_diag(source.m_diag)
+	,m_aabbP0 (source.m_aabbP0)
+	,m_aabbP1 (source.m_aabbP1)
+	,m_points(source.m_count, source.GetAllocator()) 
+{
+	m_points[m_count-1].m_w = dgFloat64 (0.0f);
+	for (int i = 0; i < m_count; i ++) {
+		m_points[i] = source.m_points[i];
+	}
+	dgTree<dgListNode*, dgListNode*> map (GetAllocator());
+	for(dgListNode* sourceNode = source.GetFirst(); sourceNode; sourceNode = sourceNode->GetNext() ) {
+		dgListNode* const node = Append();
+		map.Insert(node, sourceNode);
+	}
+
+	for(dgListNode* sourceNode = source.GetFirst(); sourceNode; sourceNode = sourceNode->GetNext() ) {
+		dgListNode* const node = map.Find(sourceNode)->GetInfo();
+
+		dgConvexHull3DFace& face = node->GetInfo();
+		dgConvexHull3DFace& srcFace = sourceNode->GetInfo();
+
+		face.m_mark = 0;
+		for (dgInt32 i = 0; i < 3; i ++) {
+			face.m_index[i] = srcFace.m_index[i];
+			face.m_twin[i] = map.Find (srcFace.m_twin[i])->GetInfo();
+		}
+	}
+}
+
+dgConvexHull3d::dgConvexHull3d(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount)
+	:dgList<dgConvexHull3DFace>(allocator)
+	,m_count (0)
+	,m_diag()
+	,m_aabbP0 (dgBigVector (dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0)))
+	,m_aabbP1 (dgBigVector (dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0), dgFloat64 (0.0)))
+	,m_points(count, allocator) 
+{
+	BuildHull (vertexCloud, strideInBytes, count, distTol, maxVertexCount);
+}
+
+dgConvexHull3d::~dgConvexHull3d(void)
+{
+}
+
+void dgConvexHull3d::BuildHull (const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount)
+{
+	dgSetPrecisionDouble precision;
+
+	dgInt32 treeCount = count / (DG_VERTEX_CLUMP_SIZE_3D>>1); 
+	if (treeCount < 4) {
+		treeCount = 4;
+	}
+	treeCount *= 2;
+
+	dgStack<dgHullVertex> points (count);
+	dgStack<dgAABBPointTree3dClump> treePool (treeCount + 256);
+	count = InitVertexArray(&points[0], vertexCloud, strideInBytes, count, &treePool[0], treePool.GetSizeInBytes());
+
+	if (m_count >= 4) {
+		CalculateConvexHull (&treePool[0], &points[0], count, distTol, maxVertexCount);
+	}
+}
+
+dgInt32 dgConvexHull3d::ConvexCompareVertex(const dgHullVertex* const  A, const dgHullVertex* const B, void* const context)
+{
+	for (dgInt32 i = 0; i < 3; i ++) {
+		if ((*A)[i] < (*B)[i]) {
+			return -1;
+		} else if ((*A)[i] > (*B)[i]) {
+			return 1;
+		}
+	}
+	return 0;
+}
+
+
+
+dgAABBPointTree3d* dgConvexHull3d::BuildTree (dgAABBPointTree3d* const parent, dgHullVertex* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** memoryPool, dgInt32& maxMemSize) const
+{
+	dgAABBPointTree3d* tree = NULL;
+
+	dgAssert (count);
+	dgBigVector minP ( dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f), dgFloat32 (0.0f)); 
+	dgBigVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f)); 
+	if (count <= DG_VERTEX_CLUMP_SIZE_3D) {
+
+		dgAABBPointTree3dClump* const clump = new (*memoryPool) dgAABBPointTree3dClump;
+		*memoryPool += sizeof (dgAABBPointTree3dClump);
+		maxMemSize -= sizeof (dgAABBPointTree3dClump);
+		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); 
+
+			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); 
+		}
+
+		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); 
+
+			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 (count)) - 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];
+			}
+		}
+		dgBigVector center = median.Scale3 (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) dgAABBPointTree3d;
+		*memoryPool += sizeof (dgAABBPointTree3d);
+		maxMemSize -= sizeof (dgAABBPointTree3d);
+		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 (0.0f));
+	tree->m_box[1] = maxP + dgBigVector (dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (0.0f));
+	return tree;
+}
+
+
+
+
+
+dgInt32 dgConvexHull3d::InitVertexArray(dgHullVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+	if (stride >= 4) {
+		for (dgInt32 i = 0; i < count; i ++) {
+			dgInt32 index = i * stride;
+			dgBigVector& vertex = points[i];
+			vertex = dgBigVector (vertexCloud[index], vertexCloud[index + 1], vertexCloud[index + 2], vertexCloud[index + 3]);
+			dgAssert (dgCheckVector(vertex));
+			points[i].m_index = 0;
+		}
+	} else {
+		for (dgInt32 i = 0; i < count; i ++) {
+			dgInt32 index = i * stride;
+			dgBigVector& vertex = points[i];
+			vertex = dgBigVector (vertexCloud[index], vertexCloud[index + 1], vertexCloud[index + 2], dgFloat64 (0.0f));
+			dgAssert (dgCheckVector(vertex));
+			points[i].m_index = 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;
+	}
+
+	dgAABBPointTree3d* tree = BuildTree (NULL, points, count, 0, (dgInt8**) &memoryPool, maxMemSize);
+
+	m_aabbP0 = tree->m_box[0];
+	m_aabbP1 = tree->m_box[1];
+
+	dgBigVector boxSize (tree->m_box[1] - tree->m_box[0]);	
+	m_diag = dgFloat32 (sqrt (boxSize % boxSize));
+
+#if 0
+	dgStack<dgBigVector> normalMapPool (256);
+	dgBigVector* const normalMap = &normalMapPool[0];
+	dgInt32 normalMapCount = BuildNormalList (&normalMap[0]);
+#else 
+	static dgBigVector normalMap[32];
+	static dgInt32 normalMapCount = BuildNormalList (&normalMap[0]);
+#endif
+	dgAssert (normalMapCount <= sizeof (normalMap)/sizeof (normalMap[0]));
+
+	dgInt32 index = SupportVertex (&tree, points, normalMap[0]);
+	m_points[0] = points[index];
+	points[index].m_index = 1;
+
+	bool validTetrahedrum = false;
+	dgBigVector e1 (dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f)) ;
+	for (dgInt32 i = 1; i < normalMapCount; i ++) {
+		dgInt32 index = SupportVertex (&tree, points, normalMap[i]);
+		dgAssert (index >= 0);
+
+		e1 = points[index] - m_points[0];
+		dgFloat64 error2 = e1 % e1;
+		if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) {
+			m_points[1] = points[index];
+			points[index].m_index = 1;
+			validTetrahedrum = true;
+			break;
+		}
+	}
+	if (!validTetrahedrum) {
+		m_count = 0;
+		dgAssert (0);
+		return count;
+	}
+
+	validTetrahedrum = false;
+	dgBigVector e2(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
+	dgBigVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+	for (dgInt32 i = 2; i < normalMapCount; i ++) {
+		dgInt32 index = SupportVertex (&tree, points, normalMap[i]);
+		dgAssert (index >= 0);
+		e2 = points[index] - m_points[0];
+		normal = e1 * e2;
+		dgFloat64 error2 = sqrt (normal % normal);
+		if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) {
+			m_points[2] = points[index];
+			points[index].m_index = 1;
+			validTetrahedrum = true;
+			break;
+		}
+	}
+
+	if (!validTetrahedrum) {
+		m_count = 0;
+		dgAssert (0);
+		return count;
+	}
+
+	// find the largest possible tetrahedron
+	validTetrahedrum = false;
+	dgBigVector e3(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+
+	index = SupportVertex (&tree, points, normal);
+	e3 = points[index] - m_points[0];
+	dgFloat64 error2 = normal % e3;
+	if (fabs (error2) > (dgFloat64 (1.0e-6f) * m_diag * m_diag)) {
+		// we found a valid tetrahedra, about and start build the hull by adding the rest of the points
+		m_points[3] = points[index];
+		points[index].m_index = 1;
+		validTetrahedrum = true;
+	}
+	if (!validTetrahedrum) {
+		dgVector n (normal.Scale3(dgFloat64 (-1.0f)));
+		dgInt32 index = SupportVertex (&tree, points, n);
+		e3 = points[index] - m_points[0];
+		dgFloat64 error2 = normal % e3;
+		if (fabs (error2) > (dgFloat64 (1.0e-6f) * m_diag * m_diag)) {
+			// we found a valid tetrahedral, about and start build the hull by adding the rest of the points
+			m_points[3] = points[index];
+			points[index].m_index = 1;
+			validTetrahedrum = true;
+		}
+	}
+	if (!validTetrahedrum) {
+	for (dgInt32 i = 3; i < normalMapCount; i ++) {
+		dgInt32 index = SupportVertex (&tree, points, normalMap[i]);
+		dgAssert (index >= 0);
+
+		//make sure the volume of the fist tetrahedral is no negative
+		e3 = points[index] - m_points[0];
+		dgFloat64 error2 = normal % e3;
+		if (fabs (error2) > (dgFloat64 (1.0e-6f) * m_diag * m_diag)) {
+			// we found a valid tetrahedral, about and start build the hull by adding the rest of the points
+			m_points[3] = points[index];
+			points[index].m_index = 1;
+			validTetrahedrum = true;
+			break;
+		}
+	}
+	}
+	if (!validTetrahedrum) {
+		// the points do not form a convex hull
+		m_count = 0;
+		//dgAssert (0);
+		return count;
+	}
+
+	m_count = 4;
+	dgFloat64 volume = TetrahedrumVolume (m_points[0], m_points[1], m_points[2], m_points[3]);
+	if (volume > dgFloat64 (0.0f)) {
+		dgSwap(m_points[2], m_points[3]);
+	}
+	dgAssert (TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]) < dgFloat64(0.0f));
+
+	return count;
+}
+
+dgFloat64 dgConvexHull3d::TetrahedrumVolume (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) const
+{
+	dgBigVector p1p0 (p1 - p0);
+	dgBigVector p2p0 (p2 - p0);
+	dgBigVector p3p0 (p3 - p0);
+	return (p1p0 * p2p0) % p3p0;
+}
+
+
+void dgConvexHull3d::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 ++;
+	}
+}
+
+
+dgInt32 dgConvexHull3d::SupportVertex (dgAABBPointTree3d** const treePointer, const dgHullVertex* const points, const dgBigVector& dir) const
+{
+	#define DG_STACK_DEPTH_3D 64
+	dgFloat64 aabbProjection[DG_STACK_DEPTH_3D];
+	const dgAABBPointTree3d *stackPool[DG_STACK_DEPTH_3D];
+
+	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;
+	while (stack) {
+		stack--;
+		dgFloat64 boxSupportValue = aabbProjection[stack];
+		if (boxSupportValue > maxProj) {
+			const dgAABBPointTree3d* 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, dgFloat32 (0.0));
+				dgFloat64 leftSupportDist = leftSupportPoint % dir;
+
+				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, dgFloat32 (0.0));
+				dgFloat64 rightSupportDist = rightSupportPoint % dir;
+
+
+				if (rightSupportDist >= leftSupportDist) {
+					aabbProjection[stack] = leftSupportDist;
+					stackPool[stack] = me->m_left;
+					stack++;
+					dgAssert (stack < DG_STACK_DEPTH_3D);
+					aabbProjection[stack] = rightSupportDist;
+					stackPool[stack] = me->m_right;
+					stack++;
+					dgAssert (stack < DG_STACK_DEPTH_3D);
+				} else {
+					aabbProjection[stack] = rightSupportDist;
+					stackPool[stack] = me->m_right;
+					stack++;
+					dgAssert (stack < DG_STACK_DEPTH_3D);
+					aabbProjection[stack] = leftSupportDist;
+					stackPool[stack] = me->m_left;
+					stack++;
+					dgAssert (stack < DG_STACK_DEPTH_3D);
+				}
+
+			} else {
+				dgAABBPointTree3dClump* const clump = (dgAABBPointTree3dClump*) me;
+				for (dgInt32 i = 0; i < clump->m_count; i ++) {
+					const dgHullVertex& 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);
+					if (!p.m_index) {
+						dgFloat64 dist = p % dir;
+						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) {
+					dgAABBPointTree3d* const parent = clump->m_parent;
+					if (parent) {	
+						dgAABBPointTree3d* const sibling = (parent->m_left != clump) ? parent->m_left : parent->m_right;
+						dgAssert (sibling != clump);
+						dgAABBPointTree3d* 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 dgConvexHull3d::BuildNormalList (dgBigVector* const normalArray) const
+{
+	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 count = 0;
+	dgInt32 subdivitions = 1;
+
+	dgInt32 start = 0;
+	TessellateTriangle  (subdivitions, p4, p0, p2, count, normalArray, start);
+	start = 1;
+	TessellateTriangle  (subdivitions, p5, p3, p1, count, normalArray, start);
+	start = 2;
+	TessellateTriangle  (subdivitions, p5, p1, p2, count, normalArray, start);
+	start = 3;
+	TessellateTriangle  (subdivitions, p4, p3, p0, count, normalArray, start);
+	start = 4;
+	TessellateTriangle  (subdivitions, p4, p2, p1, count, normalArray, start);
+	start = 5;
+	TessellateTriangle  (subdivitions, p5, p0, p3, count, normalArray, start);
+	start = 6;
+	TessellateTriangle  (subdivitions, p5, p2, p0, count, normalArray, start);
+	start = 7;
+	TessellateTriangle  (subdivitions, p4, p1, p3, count, normalArray, start);
+	return count;
+}
+
+dgConvexHull3d::dgListNode* dgConvexHull3d::AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2)
+{
+	dgListNode* const node = Append();
+	dgConvexHull3DFace& face = node->GetInfo();
+
+	face.m_index[0] = i0; 
+	face.m_index[1] = i1; 
+	face.m_index[2] = i2; 
+	return node;
+}
+
+void dgConvexHull3d::DeleteFace (dgListNode* const node) 
+{
+	Remove (node);
+}
+
+bool dgConvexHull3d::Sanity() const
+{
+/*
+	for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+		dgConvexHull3DFace* const face = &node->GetInfo();		
+		for (dgInt32 i = 0; i < 3; i ++) {
+			dgListNode* const twinNode = face->m_twin[i];
+			if (!twinNode) {
+				return false;
+			}
+
+			dgInt32 count = 0;
+			dgListNode* me = NULL;
+			dgConvexHull3DFace* const twinFace = &twinNode->GetInfo();
+			for (dgInt32 j = 0; j < 3; j ++) {
+				if (twinFace->m_twin[j] == node) {
+					count ++;
+					me = twinFace->m_twin[j];
+				}
+			}
+			if (count != 1) {
+				return false;
+			}
+			if (me != node) {
+				return false;
+			}
+		}
+	}
+*/
+	return true;
+}
+
+void dgConvexHull3d::CalculateConvexHull (dgAABBPointTree3d* vertexTree, dgHullVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount)
+{
+	distTol = fabs (distTol) * m_diag;
+	dgListNode* const f0Node = AddFace (0, 1, 2);
+	dgListNode* const f1Node = AddFace (0, 2, 3);
+	dgListNode* const f2Node = AddFace (2, 1, 3);
+	dgListNode* const f3Node = AddFace (1, 0, 3);
+
+	dgConvexHull3DFace* const f0 = &f0Node->GetInfo();
+	dgConvexHull3DFace* const f1 = &f1Node->GetInfo();
+	dgConvexHull3DFace* const f2 = &f2Node->GetInfo();
+	dgConvexHull3DFace* const f3 = &f3Node->GetInfo();
+
+	f0->m_twin[0] = (dgList<dgConvexHull3DFace>::dgListNode*)f3Node; 
+	f0->m_twin[1] = (dgList<dgConvexHull3DFace>::dgListNode*)f2Node; 
+	f0->m_twin[2] = (dgList<dgConvexHull3DFace>::dgListNode*)f1Node;
+
+	f1->m_twin[0] = (dgList<dgConvexHull3DFace>::dgListNode*)f0Node; 
+	f1->m_twin[1] = (dgList<dgConvexHull3DFace>::dgListNode*)f2Node; 
+	f1->m_twin[2] = (dgList<dgConvexHull3DFace>::dgListNode*)f3Node;
+
+	f2->m_twin[0] = (dgList<dgConvexHull3DFace>::dgListNode*)f0Node; 
+	f2->m_twin[1] = (dgList<dgConvexHull3DFace>::dgListNode*)f3Node; 
+	f2->m_twin[2] = (dgList<dgConvexHull3DFace>::dgListNode*)f1Node;
+
+	f3->m_twin[0] = (dgList<dgConvexHull3DFace>::dgListNode*)f0Node; 
+	f3->m_twin[1] = (dgList<dgConvexHull3DFace>::dgListNode*)f1Node; 
+	f3->m_twin[2] = (dgList<dgConvexHull3DFace>::dgListNode*)f2Node;
+	
+	dgList<dgListNode*> boundaryFaces(GetAllocator());
+
+	boundaryFaces.Append(f0Node);
+	boundaryFaces.Append(f1Node);
+	boundaryFaces.Append(f2Node);
+	boundaryFaces.Append(f3Node);
+
+	dgStack<dgListNode*> stackPool(1024 + m_count);
+	dgStack<dgListNode*> coneListPool(1024 + m_count);
+	dgStack<dgListNode*> deleteListPool(1024 + m_count);
+
+	dgListNode** const stack = &stackPool[0];
+	dgListNode** const coneList = &stackPool[0];
+	dgListNode** const deleteList = &deleteListPool[0];
+
+	count -= 4;
+	maxVertexCount -= 4;
+	dgInt32 currentIndex = 4;
+
+	while (boundaryFaces.GetCount() && count && (maxVertexCount > 0)) {
+		// my definition of the optimal convex hull of a given vertex count, 
+		// is the convex hull formed by a subset of the input vertex that minimizes the volume difference 
+		// between the perfect hull formed from all input vertex and the hull of the sub set of vertex.
+		// When using a priority heap this algorithms will generate the an optimal of a fix vertex count.
+		// Since all Newton's tools do not have a limit on the point count of a convex hull, I can use either a stack or a queue.
+		// a stack maximize construction speed, a Queue tend to maximize the volume of the generated Hull approaching a perfect Hull. 
+		// For now we use a queue. 
+		// For general hulls it does not make a difference if we use a stack, queue, or a priority heap. 
+		// perfect optimal hull only apply for when build hull of a limited vertex count.
+		//
+		// Also when building Hulls of a limited vertex count, this function runs in constant time.
+		// yes that is correct, it does not makes a difference if you build a N point hull from 100 vertex 
+		// or from 100000 vertex input array.
+		
+		#if 0
+			// using stack (faster)
+			dgListNode* const faceNode = boundaryFaces.GetFirst()->GetInfo();
+		#else 
+			// using a queue (some what slower by better hull for reduce vertex count)
+			dgListNode* const faceNode = boundaryFaces.GetLast()->GetInfo();
+		#endif
+
+		dgConvexHull3DFace* const face = &faceNode->GetInfo();
+		dgBigPlane planeEquation (face->GetPlaneEquation (&m_points[0]));
+
+		dgInt32 index = SupportVertex (&vertexTree, points, planeEquation);
+		const dgBigVector& p = points[index];
+		dgFloat64 dist = planeEquation.Evalue(p);
+
+		if ((dist >= distTol) && (face->Evalue(&m_points[0], p) > dgFloat64(0.0f))) {
+			dgAssert (Sanity());
+			
+			dgAssert (faceNode);
+			stack[0] = faceNode;
+
+			dgInt32 stackIndex = 1;
+			dgInt32 deletedCount = 0;
+
+			while (stackIndex) {
+				stackIndex --;
+				dgListNode* const node = stack[stackIndex];
+				dgConvexHull3DFace* const face = &node->GetInfo();
+
+				if (!face->m_mark && (face->Evalue(&m_points[0], p) > dgFloat64(0.0f))) { 
+					#ifdef _DEBUG
+					for (dgInt32 i = 0; i < deletedCount; i ++) {
+						dgAssert (deleteList[i] != node);
+					}
+					#endif
+
+					deleteList[deletedCount] = node;
+					deletedCount ++;
+					dgAssert (deletedCount < dgInt32 (deleteListPool.GetElementsCount()));
+					face->m_mark = 1;
+					for (dgInt32 i = 0; i < 3; i ++) {
+						dgListNode* const twinNode = (dgListNode*)face->m_twin[i];
+						dgAssert (twinNode);
+						dgConvexHull3DFace* const twinFace = &twinNode->GetInfo();
+						if (!twinFace->m_mark) {
+							stack[stackIndex] = twinNode;
+							stackIndex ++;
+							dgAssert (stackIndex < dgInt32 (stackPool.GetElementsCount()));
+						}
+					}
+				}
+			}
+
+			m_points[currentIndex] = points[index];
+			points[index].m_index = 1;
+
+			dgInt32 newCount = 0;
+			for (dgInt32 i = 0; i < deletedCount; i ++) {
+				dgListNode* const node = deleteList[i];
+				dgConvexHull3DFace* const face = &node->GetInfo();
+				dgAssert (face->m_mark == 1);
+				for (dgInt32 j0 = 0; j0 < 3; j0 ++) {
+					dgListNode* const twinNode = face->m_twin[j0];
+					dgConvexHull3DFace* const twinFace = &twinNode->GetInfo();
+					if (!twinFace->m_mark) {
+						dgInt32 j1 = (j0 == 2) ? 0 : j0 + 1;
+						dgListNode* const newNode = AddFace (currentIndex, face->m_index[j0], face->m_index[j1]);
+						boundaryFaces.Addtop(newNode);
+
+						dgConvexHull3DFace* const newFace = &newNode->GetInfo();
+						newFace->m_twin[1] = twinNode;
+						for (dgInt32 k = 0; k < 3; k ++) {
+							if (twinFace->m_twin[k] == node) {
+								twinFace->m_twin[k] = newNode;
+							}
+						}
+						coneList[newCount] = newNode;
+						newCount ++;
+						dgAssert (newCount < dgInt32 (coneListPool.GetElementsCount()));
+					}
+				}
+			}
+			
+			for (dgInt32 i = 0; i < newCount - 1; i ++) {
+				dgListNode* const nodeA = coneList[i];
+				dgConvexHull3DFace* const faceA = &nodeA->GetInfo();
+				dgAssert (faceA->m_mark == 0);
+				for (dgInt32 j = i + 1; j < newCount; j ++) {
+					dgListNode* const nodeB = coneList[j];
+					dgConvexHull3DFace* const faceB = &nodeB->GetInfo();
+					dgAssert (faceB->m_mark == 0);
+					if (faceA->m_index[2] == faceB->m_index[1]) {
+						faceA->m_twin[2] = nodeB;
+						faceB->m_twin[0] = nodeA;
+						break;
+					}
+				}
+
+				for (dgInt32 j = i + 1; j < newCount; j ++) {
+					dgListNode* const nodeB = coneList[j];
+					dgConvexHull3DFace* const faceB = &nodeB->GetInfo();
+					dgAssert (faceB->m_mark == 0);
+					if (faceA->m_index[1] == faceB->m_index[2]) {
+						faceA->m_twin[0] = nodeB;
+						faceB->m_twin[2] = nodeA;
+						break;
+					}
+				}
+			}
+
+			for (dgInt32 i = 0; i < deletedCount; i ++) {
+				dgListNode* const node = deleteList[i];
+				boundaryFaces.Remove (node);
+				DeleteFace (node); 
+			}
+
+			maxVertexCount --;
+			currentIndex ++;
+			count --;
+		} else {
+			boundaryFaces.Remove (faceNode);
+		}
+	}
+	m_count = currentIndex;
+}
+
+
+void dgConvexHull3d::CalculateVolumeAndSurfaceArea (dgFloat64& volume, dgFloat64& surfaceArea) const
+{
+	dgFloat64 areaAcc = dgFloat32 (0.0f);
+	dgFloat64  volumeAcc = dgFloat32 (0.0f);
+	for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+		const dgConvexHull3DFace* const face = &node->GetInfo();
+		dgInt32 i0 = face->m_index[0];
+		dgInt32 i1 = face->m_index[1];
+		dgInt32 i2 = face->m_index[2];
+		const dgBigVector& p0 = m_points[i0];
+		const dgBigVector& p1 = m_points[i1];
+		const dgBigVector& p2 = m_points[i2];
+		dgBigVector normal ((p1 - p0) * (p2 - p0));
+		dgFloat64 area = sqrt (normal % normal);
+		areaAcc += area;
+		volumeAcc += (p0 * p1) % p2;
+	}
+	dgAssert (volumeAcc >= dgFloat64 (0.0f));
+	volume = volumeAcc * dgFloat64 (1.0f/6.0f);
+	surfaceArea = areaAcc * dgFloat64 (0.5f);
+}
+
+// this code has linear time complexity on the number of faces
+dgFloat64 dgConvexHull3d::RayCast (const dgBigVector& localP0, const dgBigVector& localP1) const
+{
+	dgFloat64 interset = dgFloat32 (1.2f);
+	dgFloat64 tE = dgFloat64 (0.0f);    // for the maximum entering segment parameter;
+	dgFloat64 tL = dgFloat64 (1.0f);    // for the minimum leaving segment parameter;
+	dgBigVector dS (localP1 - localP0); // is the segment direction vector;
+	dgInt32 hasHit = 0;
+
+	for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+		const dgConvexHull3DFace* const face = &node->GetInfo();
+
+		dgInt32 i0 = face->m_index[0];
+		dgInt32 i1 = face->m_index[1];
+		dgInt32 i2 = face->m_index[2];
+
+		const dgBigVector& p0 = m_points[i0];
+		dgBigVector normal ((m_points[i1] - p0) * (m_points[i2] - p0));
+
+		dgFloat64 N = -((localP0 - p0) % normal);
+		dgFloat64 D = dS % normal;
+
+		if (fabs(D) < dgFloat64 (1.0e-12f)) { // 
+			if (N < dgFloat64 (0.0f)) {
+				return dgFloat64 (1.2f);
+			} else {
+				continue; 
+			}
+		}
+
+		dgFloat64 t = N / D;
+		if (D < dgFloat64 (0.0f)) {
+			if (t > tE) {
+				tE = t;
+				hasHit = 1;
+			}
+			if (tE > tL) {
+				return dgFloat64 (1.2f);
+			}
+		} else {
+			dgAssert (D >= dgFloat64 (0.0f));
+			tL = dgMin (tL, t);
+			if (tL < tE) {
+				return dgFloat64 (1.2f);
+			}
+		}
+	}
+
+	if (hasHit) {
+		interset = tE;	
+	}
+
+	return interset;
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgConvexHull3d.h

@@ -0,0 +1,129 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_CONVEXHULL_3D__
+#define __DG_CONVEXHULL_3D__
+
+#include "dgStdafx.h"
+#include "dgList.h"
+#include "dgArray.h"
+#include "dgPlane.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+class dgMemoryAllocator;
+class dgAABBPointTree3d;
+
+class dgConvexHull3DFace
+{
+	public:
+	dgConvexHull3DFace();
+	
+	
+	void SetMark(dgInt32 mark) {m_mark = mark;}
+	dgInt32 GetMark() const {return m_mark;}
+	dgList<dgConvexHull3DFace>::dgListNode* GetTwin(dgInt32 index) const { return m_twin[index];}
+
+	private:
+	dgFloat64 Evalue (const dgBigVector* const pointArray, const dgBigVector& point) const;
+	dgBigPlane GetPlaneEquation (const dgBigVector* const pointArray) const;
+
+	public:
+	dgInt32 m_index[3]; 
+	private:
+	dgInt32 m_mark;
+	dgList<dgConvexHull3DFace>::dgListNode* m_twin[3];
+	friend class dgConvexHull3d;
+};
+
+class dgHullVertex;
+
+class dgConvexHull3d: public dgList<dgConvexHull3DFace>
+{
+	public:
+	dgConvexHull3d(const dgConvexHull3d& source);
+	dgConvexHull3d(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount = 0x7fffffff);
+	virtual ~dgConvexHull3d();
+
+	dgInt32 GetVertexCount() const;
+	const dgBigVector* GetVertexPool() const;
+	const dgBigVector& GetVertex(dgInt32 i) const;
+
+	dgFloat64 GetDiagonal() const;
+	void GetAABB (dgBigVector& boxP0, dgBigVector& boxP1) const;
+	dgFloat64 RayCast (const dgBigVector& localP0, const dgBigVector& localP1) const;
+	void CalculateVolumeAndSurfaceArea (dgFloat64& volume, dgFloat64& surcafeArea) const;
+
+	protected:
+	dgConvexHull3d(dgMemoryAllocator* const allocator);
+	void BuildHull (const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
+
+	virtual dgListNode* AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2);
+	virtual void DeleteFace (dgListNode* const node) ;
+	virtual dgInt32 InitVertexArray(dgHullVertex* const points, const dgFloat64* const vertexCloud, dgInt32 strideInBytes, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
+
+	void CalculateConvexHull (dgAABBPointTree3d* vertexTree, dgHullVertex* const points, dgInt32 count, dgFloat64 distTol, dgInt32 maxVertexCount);
+	dgInt32 BuildNormalList (dgBigVector* const normalArray) const;
+	dgInt32 SupportVertex (dgAABBPointTree3d** const tree, const dgHullVertex* const points, const dgBigVector& dir) const;
+	dgFloat64 TetrahedrumVolume (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) const;
+	void TessellateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgBigVector* const ouput, dgInt32& start) const;
+
+	dgAABBPointTree3d* BuildTree (dgAABBPointTree3d* const parent, dgHullVertex* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** const memoryPool, dgInt32& maxMemSize) const;
+	static dgInt32 ConvexCompareVertex(const dgHullVertex* const  A, const dgHullVertex* const B, void* const context);
+	bool Sanity() const;
+
+	dgInt32 m_count;
+	dgFloat64 m_diag;
+	dgBigVector m_aabbP0;
+	dgBigVector m_aabbP1;
+	dgArray<dgBigVector> m_points;
+};
+
+
+inline dgInt32 dgConvexHull3d::GetVertexCount() const
+{
+	return m_count;
+}
+
+inline const dgBigVector* dgConvexHull3d::GetVertexPool() const
+{
+	return &m_points[0];
+}
+
+inline const dgBigVector& dgConvexHull3d::GetVertex(dgInt32 index) const
+{
+	return m_points[index];
+}
+
+inline dgFloat64 dgConvexHull3d::GetDiagonal() const
+{
+	return m_diag;
+}
+
+
+inline void dgConvexHull3d::GetAABB (dgBigVector& boxP0, dgBigVector& boxP1) const
+{
+	boxP0 = m_aabbP0;
+	boxP1 = m_aabbP1;
+}
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgConvexHull4d.cpp

@@ -0,0 +1,1089 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgStack.h"
+#include "dgTree.h"
+#include "dgHeap.h"
+#include "dgGoogol.h"
+#include "dgConvexHull4d.h"
+#include "dgSmallDeterminant.h"
+
+#define DG_VERTEX_CLUMP_SIZE_4D		8 
+
+
+
+
+class dgAABBPointTree4d
+{
+	public:
+	#ifdef _DEBUG
+	dgAABBPointTree4d()
+	{
+		static dgInt32 id = 0;
+		m_id = id;
+		id ++;
+	}
+	dgInt32 m_id;
+	#endif
+
+		dgBigVector m_box[2];
+		dgAABBPointTree4d* m_left;
+		dgAABBPointTree4d* m_right;
+		dgAABBPointTree4d* m_parent;
+
+};
+
+class dgAABBPointTree4dClump: public dgAABBPointTree4d
+{
+	public:
+	dgInt32 m_count;
+	dgInt32 m_indices[DG_VERTEX_CLUMP_SIZE_4D];
+};
+
+
+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;
+}
+
+dgFloat64 dgConvexHull4dTetraherum::dgTetrahedrumPlane::Evalue (const dgBigVector& point) const
+{
+	const dgBigVector& me = *this;
+	return me.DotProduct4(point).m_x + m_dist;
+}
+
+
+
+dgConvexHull4dTetraherum::dgConvexHull4dTetraherum()
+{
+#ifdef _DEBUG
+	static dgInt32 debugID;
+	m_debugID = debugID;
+	debugID ++;
+#endif
+	static dgInt32 m_monotonicID;
+
+	m_uniqueID = m_monotonicID;
+	m_monotonicID ++;
+}
+
+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
+}
+
+
+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);
+}
+
+
+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;
+}
+
+dgConvexHull4dTetraherum::dgTetrahedrumPlane dgConvexHull4dTetraherum::GetPlaneEquation (const dgHullVector* const points) const
+{
+	const dgBigVector &p0 = points[m_faces[0].m_index[0]];
+	const dgBigVector &p1 = points[m_faces[0].m_index[1]];
+	const dgBigVector &p2 = points[m_faces[0].m_index[2]];
+	const dgBigVector &p3 = points[m_faces[0].m_index[3]];
+	return dgTetrahedrumPlane (p0, p1, p2, p3);
+}
+
+dgConvexHull4d::dgConvexHull4d (dgMemoryAllocator* const allocator)
+	:dgList<dgConvexHull4dTetraherum>(allocator), m_mark(0), m_count (0), m_diag(), m_points(1024, allocator) 
+{
+}
+
+
+dgConvexHull4d::dgConvexHull4d (dgMemoryAllocator* const allocator, const dgBigVector* const vertexCloud, dgInt32 count, dgFloat32 distTol)
+	:dgList<dgConvexHull4dTetraherum>(allocator), m_mark(0), m_count (0), m_diag(), m_points(count, allocator) 
+{
+	BuildHull (allocator, vertexCloud, count, distTol);
+}
+
+
+dgConvexHull4d::~dgConvexHull4d(void)
+{
+}
+
+
+void dgConvexHull4d::Save (const char* const filename) const
+{
+	FILE* const file = fopen (filename, "wb");
+	int index = 0;
+	fprintf (file, "final\n");
+	for (dgListNode* nodePtr = GetFirst(); nodePtr; nodePtr = nodePtr->GetNext()) {
+		fprintf (file, "tetra %d\n", index);
+		index ++;
+		const dgConvexHull4dTetraherum& face = nodePtr->GetInfo();
+		const dgBigVector& p0 = m_points[face.m_faces[0].m_index[0]];
+		const dgBigVector& p1 = m_points[face.m_faces[0].m_index[1]];
+		const dgBigVector& p2 = m_points[face.m_faces[0].m_index[2]];
+		const dgBigVector& p3 = m_points[face.m_faces[0].m_index[3]];
+		fprintf (file, "p0(%f %f %f %f)\n", p0[0], p0[1], p0[2], p0[3]);
+		fprintf (file, "p1(%f %f %f %f)\n", p1[0], p1[1], p1[2], p1[3]);
+		fprintf (file, "p2(%f %f %f %f)\n", p2[0], p2[1], p2[2], p2[3]);
+		fprintf (file, "p3(%f %f %f %f)\n", p3[0], p3[1], p3[2], p3[3]);
+	}
+	fprintf (file, "\n");
+
+	fclose (file);
+}
+
+
+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;
+}
+
+
+
+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 ++;
+	}
+}
+
+
+dgInt32 dgConvexHull4d::BuildNormalList (dgBigVector* const normalArray) const
+{
+	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 count = 0;
+	dgInt32 subdivitions = 1;
+
+	dgInt32 start = 0;
+	TessellateTriangle  (subdivitions, p4, p0, p2, count, normalArray, start);
+	start = 1;
+	TessellateTriangle  (subdivitions, p5, p3, p1, count, normalArray, start);
+	start = 2;
+	TessellateTriangle  (subdivitions, p5, p1, p2, count, normalArray, start);
+	start = 3;
+	TessellateTriangle  (subdivitions, p4, p3, p0, count, normalArray, start);
+	start = 4;
+	TessellateTriangle  (subdivitions, p4, p2, p1, count, normalArray, start);
+	start = 5;
+	TessellateTriangle  (subdivitions, p5, p0, p3, count, normalArray, start);
+	start = 6;
+	TessellateTriangle  (subdivitions, p5, p2, p0, count, normalArray, start);
+	start = 7;
+	TessellateTriangle  (subdivitions, p4, p1, p3, count, normalArray, start);
+
+	return count;
+}
+
+
+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 dgConvexHull4d::ConvexCompareVertex(const dgHullVector* const  A, const dgHullVector* const B, void* const context)
+{
+	for (dgInt32 i = 0; i < 4; i ++) {
+		if ((*A)[i] < (*B)[i]) {
+			return -1;
+		} else if ((*A)[i] > (*B)[i]) {
+			return 1;
+		}
+	}
+	return 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;
+}
+
+
+
+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;
+}
+
+
+
+dgConvexHull4d::dgListNode* dgConvexHull4d::AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2, dgInt32 i3)
+{
+	dgListNode* const node = Append();
+	dgConvexHull4dTetraherum& face = node->GetInfo();
+	face.Init (&m_points[0], i0, i1, i2, i3);
+	return node;
+}
+
+
+void dgConvexHull4d::DeleteFace (dgListNode* const node) 
+{
+	Remove (node);
+}
+
+
+bool dgConvexHull4d::Sanity() const
+{
+	for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+		dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+
+		for (dgInt32 i = 0; i < 4; i ++) {
+			dgConvexHull4dTetraherum::dgTetrahedrumFace* const face = &tetra->m_faces[i];
+			dgListNode* const twinNode = face->m_twin;
+			if (!twinNode) {
+				return false;
+			}
+		}
+	}
+
+/*
+	dgList<dgListNode*> tetraList(GetAllocator());
+	const dgHullVector* const points = &m_points[0];
+	for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+		dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+		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_index[3]];
+
+		dgBigVector p1p0 (p1.Sub4(p0));
+		dgBigVector p2p0 (p2.Sub4(p0));
+		dgBigVector p3p0 (p3.Sub4(p0));
+		dgBigVector normal (p1p0.CrossProduct4 (p2p0, p3p0));
+
+		if (normal.m_w < dgFloat64 (0.0f)) {
+			tetraList.Append(node);
+		}
+	}
+
+	for (dgList<dgListNode*>::dgListNode* node0 = tetraList.GetFirst(); node0; node0 = node0->GetNext()) {
+		dgListNode* const tetraNode0 = node0->GetInfo();
+		dgConvexHull4dTetraherum* const tetra0 = &tetraNode0->GetInfo();
+
+		dgInt32 index0[4];
+		index0[0] = tetra0->m_faces[0].m_index[0];
+		index0[1] = tetra0->m_faces[0].m_index[1];
+		index0[2] = tetra0->m_faces[0].m_index[2];
+		index0[3] = tetra0->m_faces[0].m_index[3];
+
+		const dgBigVector &p0 = points[index0[0]];
+		const dgBigVector &p1 = points[index0[1]];
+		const dgBigVector &p2 = points[index0[2]];
+		const dgBigVector &p3 = points[index0[3]];
+		for (dgList<dgListNode*>::dgListNode* node1 = node0->GetNext(); node1; node1 = node1->GetNext()) {
+			dgListNode* const tetraNode1 = node1->GetInfo();
+			dgConvexHull4dTetraherum* const tetra1 = &tetraNode1->GetInfo();
+
+			dgInt32 index1[4];
+			index1[0] = tetra1->m_faces[0].m_index[0];
+			index1[1] = tetra1->m_faces[0].m_index[1];
+			index1[2] = tetra1->m_faces[0].m_index[2];
+			index1[3] = tetra1->m_faces[0].m_index[3];
+
+			for (dgInt32 i = 0; i < 4; i ++) {
+				dgInt32 count = 0;
+				dgInt32 k = index1[i];
+				for (dgInt32 j = 0; j < 4; j ++) {
+					count += (k == index0[j]);
+				}
+				if (!count){
+//					const dgBigVector &p = points[k];
+//					dgFloat64 size = -insphere(&p0.m_x, &p1.m_x, &p2.m_x, &p3.m_x, &p.m_x);
+//					if (size < dgFloat64 (0.0f)) {
+//						return false;
+//					}
+				}
+			}
+		}
+	}
+*/
+
+	//dgTrace (("\n"));
+	return true;
+}
+
+
+void dgConvexHull4d::LinkSibling (dgListNode* node0, dgListNode* node1)	const
+{
+	dgConvexHull4dTetraherum* const tetra0 = &node0->GetInfo();
+	dgConvexHull4dTetraherum* const tetra1 = &node1->GetInfo();
+	for (int i = 0; i < 4; i ++) {
+		dgConvexHull4dTetraherum::dgTetrahedrumFace* const face0 = &tetra0->m_faces[i];
+		if (!face0->m_twin) {
+			dgInt32 i0 = face0->m_index[0];
+			dgInt32 i1 = face0->m_index[1];
+			dgInt32 i2 = face0->m_index[2];
+			for (int j = 0; j < 4; j ++) {
+				dgConvexHull4dTetraherum::dgTetrahedrumFace* const face1 = &tetra1->m_faces[j];
+				if (!face1->m_twin) {
+					dgInt32 j2 = face1->m_index[0];
+					dgInt32 j1 = face1->m_index[1];
+					dgInt32 j0 = face1->m_index[2];
+
+					if (((i0 == j0) && (i1 == j1) && (i2 == j2)) || ((i1 == j0) && (i2 == j1) && (i0 == j2)) || ((i2 == j0) && (i0 == j1) && (i1 == j2))) {
+						face0->m_twin = node1;
+						face1->m_twin = node0;
+						return;
+					}
+				}
+			}
+		}
+	}
+}
+
+
+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);
+		}
+	}
+}
+
+
+
+
+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;
+}
+
+dgInt32 dgConvexHull4d::AddVertex (const dgBigVector& vertex)
+{
+	dgSetPrecisionDouble precision;
+	dgInt32 index = -1;
+	dgListNode* const faceNode = FindFacingNode(vertex);
+	if (faceNode) {
+		index = m_count;
+		m_points[index] = vertex;
+		m_points[index].m_index = index;
+		m_count ++;
+
+		dgList<dgListNode*> newFaces(GetAllocator());
+		dgList<dgListNode*> deleteList(GetAllocator());
+		
+		InsertNewVertex(index, faceNode, deleteList, newFaces);
+		for (dgList<dgListNode*>::dgListNode* deleteNode = deleteList.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) {
+			dgListNode* const node = deleteNode->GetInfo();
+			DeleteFace (node); 
+		}
+	}
+	return index;
+}
+
+
+void dgConvexHull4d::BuildHull (dgMemoryAllocator* const allocator, const dgBigVector* const vertexCloud, dgInt32 count, dgFloat64 distTol)
+{
+	dgInt32 treeCount = count / (DG_VERTEX_CLUMP_SIZE_4D>>1); 
+	if (treeCount < 4) {
+		treeCount = 4;
+	}
+	treeCount *= 2;
+
+	dgStack<dgHullVector> points (count);
+	dgStack<dgAABBPointTree4dClump> treePool (treeCount + 256);
+
+
+	count = InitVertexArray(&points[0], vertexCloud, count, &treePool[0], treePool.GetSizeInBytes());
+	if (m_count >= 4) {
+		CalculateConvexHull (&treePool[0], &points[0], count, distTol);
+	}
+}
+
+
+void dgConvexHull4d::CalculateConvexHull (dgAABBPointTree4d* vertexTree, dgHullVector* const points, dgInt32 count, dgFloat64 distTol)
+{
+	distTol = fabs (distTol) * m_diag;
+	dgListNode* const nodes0 = AddFace (0, 1, 2, 3);
+	dgListNode* const nodes1 = AddFace (0, 1, 3, 2);
+
+	dgList<dgListNode*> boundaryFaces(GetAllocator());
+	boundaryFaces.Append(nodes0);
+	boundaryFaces.Append(nodes1);
+
+	LinkSibling (nodes0, nodes1);
+	LinkSibling (nodes0, nodes1);
+	LinkSibling (nodes0, nodes1);
+	LinkSibling (nodes0, nodes1);
+
+	IncMark();
+	count -= 4;
+	dgInt32 currentIndex = 4;
+	while (boundaryFaces.GetCount() && count) {
+		dgHullVector* const hullVertexArray = &m_points[0]; 
+		dgListNode* const faceNode = boundaryFaces.GetFirst()->GetInfo();
+		dgConvexHull4dTetraherum* const face = &faceNode->GetInfo();
+		dgConvexHull4dTetraherum::dgTetrahedrumPlane planeEquation (face->GetPlaneEquation (hullVertexArray));
+
+		dgInt32 index = SupportVertex (&vertexTree, points, planeEquation);
+
+		const dgHullVector& p = points[index];
+		dgFloat64 dist = planeEquation.Evalue(p);
+		if ((dist > distTol) && (face->Evalue(hullVertexArray, p) > dgFloat64(0.0f))) {
+
+			m_points[currentIndex] = p;
+			points[index].m_mark = 1;
+
+			dgList<dgListNode*> deleteList(GetAllocator());
+			InsertNewVertex(currentIndex, faceNode, deleteList, boundaryFaces);
+
+			for (dgList<dgListNode*>::dgListNode* deleteNode = deleteList.GetFirst(); deleteNode; deleteNode = deleteNode->GetNext()) {
+				dgListNode* const node = deleteNode->GetInfo();
+				boundaryFaces.Remove (node);
+				DeleteFace (node); 
+			}
+
+			currentIndex ++;
+			count --;
+		} else {
+			boundaryFaces.Remove (faceNode);
+		}
+	}
+	m_count = currentIndex;
+}

newtondynamics.mod/NewtonDynamics/source/core/dgConvexHull4d.h

@@ -0,0 +1,175 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_CONVEXHULL_4D__
+#define __DG_CONVEXHULL_4D__
+
+#include "dgStdafx.h"
+#include "dgList.h"
+#include "dgArray.h"
+#include "dgPlane.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+class dgMemoryAllocator;
+class dgAABBPointTree4d;
+
+class dgHullVector: public dgBigVector
+{
+	public:
+	void operator = (const dgBigVector& a) 
+	{
+		m_x = a.m_x;
+		m_y = a.m_y;
+		m_z = a.m_z;
+		m_w = a.m_w;
+		m_index = 0;
+		m_mark = 0;
+	}
+
+	dgInt32 m_index;
+	dgInt32 m_mark;
+};
+
+
+class dgConvexHull4dTetraherum
+{
+	public:
+	class dgTetrahedrumFace 
+	{
+		public:
+		dgInt32 m_index[4];
+		dgList<dgConvexHull4dTetraherum>::dgListNode* m_twin;
+	};
+
+
+	class dgTetrahedrumPlane: public dgBigVector
+	{
+		public: 
+		dgTetrahedrumPlane (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3);
+		dgFloat64 Evalue (const dgBigVector& point) const;
+		dgFloat64 m_dist;
+	};
+
+	dgConvexHull4dTetraherum();
+	dgTetrahedrumPlane GetPlaneEquation (const dgHullVector* const points) const;
+	dgFloat64 Evalue (const dgHullVector* const pointArray, const dgBigVector& point) const;
+
+	dgBigVector CircumSphereCenter (const dgHullVector* const pointArray) const;
+
+	dgInt32 GetMark() const { return m_mark; }
+	void SetMark(dgInt32 mark) { m_mark = mark; }
+
+	private:
+	void Init (const dgHullVector* const points, dgInt32 v0, dgInt32 v1, dgInt32 v2, dgInt32 v3);
+
+	public:
+	dgTetrahedrumFace m_faces[4];
+	dgInt32 m_mark;
+	dgInt32 m_uniqueID;
+
+#ifdef _DEBUG
+	dgInt32 m_debugID;
+#endif
+	friend class dgConvexHull4d;
+	friend class dgDelaunayTetrahedralization;
+};
+
+
+class dgConvexHull4d: public dgList<dgConvexHull4dTetraherum>
+{
+	public:
+	dgConvexHull4d(dgMemoryAllocator* const allocator, const dgBigVector* const vertexCloud, dgInt32 count, dgFloat32 distTol);
+	virtual ~dgConvexHull4d();
+
+	dgInt32 GetVertexCount() const;
+	dgInt32 GetVertexIndex(dgInt32 i) const;
+	const dgBigVector& GetVertex(dgInt32 i) const;
+
+	const dgHullVector* GetHullVertexArray() const;
+
+	dgInt32 IncMark (); 
+	void Save (const char* const filename) const;
+
+	protected:
+	dgConvexHull4d(dgMemoryAllocator* const allocator);
+
+	void BuildHull (dgMemoryAllocator* const allocator, const dgBigVector* const vertexCloud, dgInt32 count, dgFloat64 distTol);
+
+	virtual dgInt32 AddVertex (const dgBigVector& vertex);
+	virtual dgInt32 InitVertexArray(dgHullVector* const points, const dgBigVector* const vertexCloud, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize);
+	virtual dgListNode* AddFace (dgInt32 i0, dgInt32 i1, dgInt32 i2, dgInt32 i3);
+	virtual void DeleteFace (dgListNode* const node);
+
+	dgListNode* FindFacingNode(const dgBigVector& vertex);
+	dgInt32 BuildNormalList (dgBigVector* const normalArray) const;
+	void InsertNewVertex(dgInt32 vertexIndex, dgListNode* const frontFace, dgList<dgListNode*>& deletedFaces, dgList<dgListNode*>& newFaces);
+	dgInt32 SupportVertex (dgAABBPointTree4d** const tree, const dgHullVector* const points, const dgBigVector& dir) const;
+	void TessellateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgBigVector* const ouput, dgInt32& start) const;
+	void CalculateConvexHull (dgAABBPointTree4d* vertexTree, dgHullVector* const points, dgInt32 count, dgFloat64 distTol);
+	void LinkSibling (dgListNode* node0, dgListNode* node1)	const;
+	bool Sanity() const;
+	dgAABBPointTree4d* BuildTree (dgAABBPointTree4d* const parent, dgHullVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** const memoryPool, dgInt32& maxMemSize) const;
+
+	static dgInt32 ConvexCompareVertex(const dgHullVector* const  A, const dgHullVector* const B, void* const context);
+	
+	dgFloat64 RoundToFloat (dgFloat64 val) const;
+	dgInt32 m_mark;
+	dgInt32 m_count;
+	dgFloat64 m_diag;
+	dgArray<dgHullVector> m_points;
+};
+
+
+inline dgInt32 dgConvexHull4d::IncMark ()
+{
+	m_mark ++;
+	return m_mark;
+}
+
+inline dgInt32 dgConvexHull4d::GetVertexCount() const
+{
+	return m_count;
+}
+
+inline dgInt32 dgConvexHull4d::GetVertexIndex(dgInt32 index) const
+{
+	dgAssert (index >= 0);
+	dgAssert (index < m_count);
+	return m_points[index].m_index;
+}
+
+
+inline const dgBigVector& dgConvexHull4d::GetVertex(dgInt32 index) const
+{
+	dgAssert (index >= 0);
+	dgAssert (index < m_count);
+//	return dgVector (dgFloat32 (m_points[index].m_x), dgFloat32 (m_points[index].m_y), dgFloat32 (m_points[index].m_z), dgFloat32 (m_points[index].m_w));
+	return m_points[index];
+}
+
+inline const dgHullVector* dgConvexHull4d::GetHullVertexArray() const
+{
+	return &m_points[0];
+}
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgDebug.cpp

@@ -0,0 +1,43 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+
+#ifdef DG_TRACE
+	void dgApi dgExpandTraceMessage (const char *fmt, ...)
+	{
+		va_list v_args;
+		char text[4096];
+
+		text[0] = 0;
+		va_start (v_args, fmt);     
+		vsprintf(text, fmt, v_args);
+		va_end (v_args);            
+
+		#ifdef _WIN32 
+			OutputDebugStringA (text);
+		#else 
+			printf ("%s\n", text);
+		#endif
+	}
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgDebug.h

@@ -0,0 +1,57 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgDebug__
+#define __dgDebug__
+
+#include "dgStdafx.h"
+#ifdef _MSC_VER 
+	#ifdef _DEBUG 
+		#define DG_TRACE
+	#endif
+#endif
+
+#ifdef DG_TRACE
+	void dgApi dgExpandTraceMessage (const char *fmt, ...);
+	#define dgTrace(x)	dgExpandTraceMessage x;
+#else
+	#define dgTrace(x)
+#endif
+
+
+
+#ifdef _DEBUG
+	inline void TraceFuntionName (const char *name)
+	{
+		//	static int trace;
+		//	dgTrace (("%d %s\n", trace, name));
+		dgTrace (("%s\n", name));
+	}
+
+	//#define TRACE_FUNCTION(name) TraceFuntionName (name)
+	#define TRACE_FUNCTION(name)
+#else
+	#define TRACE_FUNCTION(name)
+#endif
+
+	
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgDelaunayTetrahedralization.cpp

@@ -0,0 +1,221 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgStack.h"
+#include "dgGoogol.h"
+#include "dgSmallDeterminant.h"
+#include "dgDelaunayTetrahedralization.h"
+
+
+
+dgDelaunayTetrahedralization::dgDelaunayTetrahedralization(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol)
+	:dgConvexHull4d(allocator)
+{
+	dgSetPrecisionDouble precision;
+
+	dgStack<dgBigVector> pool(count);
+
+	dgBigVector* const points = &pool[0];
+	dgInt32 stride = dgInt32 (strideInByte / sizeof (dgFloat64));
+	for (dgInt32 i = 0; i < count; i ++) {
+		dgFloat64 x = RoundToFloat (vertexCloud[i * stride + 0]);
+		dgFloat64 y = RoundToFloat (vertexCloud[i * stride + 1]);
+		dgFloat64 z = RoundToFloat (vertexCloud[i * stride + 2]);
+		points[i] = dgBigVector (x, y, z, x * x + y * y + z * z);
+	}
+
+	dgInt32 oldCount = count;
+	BuildHull (allocator, &pool[0], count, distTol);
+#if 1
+//	if ((oldCount > m_count) && (m_count >= 4)) {
+	if (oldCount > m_count) {
+		// this is probably a regular convex solid, which will have a zero volume hull
+		// add the rest of the points by incremental insertion with small perturbation
+		dgInt32 hullCount = m_count;
+		
+		for (dgInt32 i = 0; i < count; i ++) {
+			bool inHull = false;
+			const dgHullVector* const hullPoints = &m_points[0];
+			for (dgInt32 j = 0; j < hullCount; j ++) {
+				if (hullPoints[j].m_index == i) {
+					inHull = true;
+					break;
+				}			
+			}
+			if (!inHull) {
+				dgBigVector q (points[i]);
+				dgInt32 index = AddVertex(q);
+				if (index == -1) {
+					q.m_x += dgFloat64 (1.0e-3f);
+					q.m_y += dgFloat64 (1.0e-3f);
+					q.m_z += dgFloat64 (1.0e-3f);
+					index = AddVertex(q);
+					dgAssert (index != -1);
+				}
+				dgAssert (index != -1);
+//				m_points[index] = points[i];
+				m_points[index].m_index = i;
+			}
+		}
+	}
+#else
+	if (oldCount > m_count) {
+		// this is probably a regular convex solid, which will have a zero volume hull
+		// perturbate a point and try again
+		dgBigVector p (points[0]);
+		points[0].m_x += dgFloat64 (1.0e-0f);
+		points[0].m_y += dgFloat64 (1.0e-0f);
+		points[0].m_z += dgFloat64 (1.0e-0f);
+		points[0].m_w = points[0].m_x * points[0].m_x + points[0].m_y * points[0].m_y + points[0].m_z * points[0].m_z;
+		BuildHull (allocator, &pool[0], oldCount, distTol);
+		dgAssert (oldCount == m_count);
+		// restore the old point
+		//points[0].m_w = points[0].m_x * points[0].m_x + points[0].m_y * points[0].m_y + points[0].m_z * points[0].m_z;
+	}
+#endif
+
+	SortVertexArray ();
+}
+
+dgDelaunayTetrahedralization::~dgDelaunayTetrahedralization()
+{
+}
+
+
+
+dgInt32 dgDelaunayTetrahedralization::AddVertex (const dgBigVector& vertex)
+{
+	dgSetPrecisionDouble precision;
+
+	dgBigVector p (vertex);
+	p.m_w = p % p;
+	dgInt32 index = dgConvexHull4d::AddVertex(p);
+
+	return index;
+}
+
+
+
+dgInt32 dgDelaunayTetrahedralization::CompareVertexByIndex(const dgHullVector* const  A, const dgHullVector* const B, void* const context)
+{
+	if (A->m_index < B ->m_index) {
+		return -1;
+	} else if (A->m_index > B->m_index) {
+		return 1;
+	}
+	return 0;
+}
+
+
+void dgDelaunayTetrahedralization::SortVertexArray ()
+{
+	dgHullVector* const points = &m_points[0];
+	for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {	
+		dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+		for (dgInt32 i = 0; i < 4; i ++) {
+			dgConvexHull4dTetraherum::dgTetrahedrumFace& face = tetra->m_faces[i];
+			for (dgInt32 j = 0; j < 4; j ++) {
+				dgInt32 index = face.m_index[j];
+				face.m_index[j] = points[index].m_index;
+			}
+		}
+	}
+
+	dgSort(points, m_count, CompareVertexByIndex);
+}
+
+
+
+void dgDelaunayTetrahedralization::RemoveUpperHull ()
+{
+	dgSetPrecisionDouble precision;
+
+	dgListNode* nextNode = NULL;
+	for (dgListNode* node = GetFirst(); node; node = nextNode) {
+		nextNode = node->GetNext();
+
+		dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+		tetra->SetMark(0);
+		dgFloat64 w = GetTetraVolume (tetra);
+		if (w >= dgFloat64 (0.0f)) {
+			DeleteFace(node);
+		}
+	}
+}
+
+
+void dgDelaunayTetrahedralization::DeleteFace (dgListNode* const node)
+{
+	dgConvexHull4dTetraherum* const tetra = &node->GetInfo();
+	for (dgInt32 i = 0; i < 4; i ++) {
+		dgListNode* const twinNode = tetra->m_faces[i].m_twin;
+		if (twinNode) {
+			dgConvexHull4dTetraherum* const twinTetra = &twinNode->GetInfo();
+			for (dgInt32 j = 0; j < 4; j ++) {
+				if (twinTetra->m_faces[j].m_twin == node) {
+					twinTetra->m_faces[j].m_twin = NULL;
+					break;
+				}
+			}
+		}
+	}
+	dgConvexHull4d::DeleteFace (node);
+}
+
+
+dgFloat64 dgDelaunayTetrahedralization::GetTetraVolume (const dgConvexHull4dTetraherum* const tetra) const
+{
+	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_index[3]];
+
+	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]);
+	}
+
+	return Determinant3x3(exactMatrix);
+}
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgDelaunayTetrahedralization.h

@@ -0,0 +1,46 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_DEALUNAY_TETRAHEDRALIZAION_4D__
+#define __DG_DEALUNAY_TETRAHEDRALIZAION_4D__
+
+#include "dgStdafx.h"
+#include "dgConvexHull4d.h"
+
+class dgDelaunayTetrahedralization: public dgConvexHull4d
+{
+	public:
+	dgDelaunayTetrahedralization(dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol);
+	virtual ~dgDelaunayTetrahedralization();
+	void RemoveUpperHull ();
+
+	dgInt32 AddVertex (const dgBigVector& vertex);
+
+	protected:
+	virtual void DeleteFace (dgListNode* const node) ;
+	dgFloat64 GetTetraVolume (const dgConvexHull4dTetraherum* const tetra) const;
+
+	void SortVertexArray ();
+	static dgInt32 CompareVertexByIndex(const dgHullVector* const  A, const dgHullVector* const B, void* const context);
+
+};
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgFastQueue.h

@@ -0,0 +1,113 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgFastQueue__
+#define __dgFastQueue__
+
+#include "dgStdafx.h"
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+class dgFastQueue
+{
+	public:
+    DG_CLASS_ALLOCATOR_NEW(allocator)
+
+	dgFastQueue (dgMemoryAllocator* const allocator);
+	~dgFastQueue ();
+
+	bool IsEmpty() const;
+	bool IsFull() const;
+	const T& GetHead() const;
+	void Pop();
+	void Push(T& object);
+
+	private:
+	T* m_pool;
+    dgMemoryAllocator* m_allocator;
+    dgInt32 m_head;
+    dgInt32 m_tail;
+};
+
+
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+dgFastQueue<T, sizeInPowerOfTwo>::dgFastQueue (dgMemoryAllocator* const allocator)
+    :m_allocator(allocator)
+	,m_head(0)
+	,m_tail(0)
+{
+    dgAssert (((sizeInPowerOfTwo -1) & (-sizeInPowerOfTwo)) == 0);
+    m_pool = (T*) m_allocator->MallocLow(sizeInPowerOfTwo * sizeof (T));
+}
+
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+dgFastQueue<T, sizeInPowerOfTwo>::~dgFastQueue ()
+{
+    m_allocator->FreeLow(m_pool); 
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+bool dgFastQueue<T, sizeInPowerOfTwo>::IsEmpty() const
+{
+	return (m_head == m_tail);
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+bool dgFastQueue<T, sizeInPowerOfTwo>::IsFull() const
+{
+	return (((m_tail + 1) & (sizeInPowerOfTwo - 1)) == m_head);
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+const T& dgFastQueue<T, sizeInPowerOfTwo>::GetHead() const
+{
+	dgAssert (!IsEmpty());
+	return m_pool[m_head];
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+void dgFastQueue<T, sizeInPowerOfTwo>::Pop()
+{
+	dgAssert (!IsEmpty());
+	m_head = (m_head + 1) & (sizeInPowerOfTwo - 1);
+}
+
+template<class T, dgInt32 sizeInPowerOfTwo>
+void dgFastQueue<T, sizeInPowerOfTwo>::Push(T& object)
+{
+	dgAssert (!IsFull());
+	m_pool[m_tail] = object; 
+	m_tail = (m_tail + 1) & (sizeInPowerOfTwo - 1);
+}
+
+
+
+#endif
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgGeneralMatrix.cpp

@@ -0,0 +1,25 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgGeneralMatrix.h"
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgGeneralMatrix.h

@@ -0,0 +1,661 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgGeneralMatrix__
+#define __dgGeneralMatrix__
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+#include "dgGeneralVector.h"
+
+
+
+
+template <class T> dgInt32 dgGeneralMatrixCalcBufferSizeInBytes (dgInt32 row, dgInt32 column)
+{
+	dgInt32 columnPad;
+	columnPad = ((column * sizeof (T) + 0x0f) & -0x0f) / sizeof (T);
+	return row * columnPad * sizeof (T) + row * sizeof (dgGeneralVector<T>);
+}
+
+
+template<class T>
+class dgGeneralMatrix
+{
+	public:
+	dgGeneralMatrix (dgInt32 row, dgInt32 column);
+	dgGeneralMatrix (const dgGeneralMatrix<T>& src);
+	dgGeneralMatrix (const dgGeneralMatrix<T>& src, T *elemBuffer);
+	dgGeneralMatrix (dgInt32 row, dgInt32 column, T *elemBuffer);
+	~dgGeneralMatrix ();
+
+	dgGeneralVector<T>& operator[] (dgInt32 i);
+	const dgGeneralVector<T>& operator[] (dgInt32 i) const;
+
+	dgInt32 GetRowCount() const;
+	dgInt32 GetColCount() const;
+
+	void Clear (T val);
+	void Identity ();
+
+	void SwapRows (dgInt32 i, dgInt32 j); 
+	void SwapColumns (dgInt32 i, dgInt32 j); 
+
+//	dgGeneralMatrix Transpose ();
+//	void Inverse (dgGeneralMatrix& inverseOut);
+	void GaussianPivotStep (dgInt32 srcRow, dgInt32 pivotRow, dgInt32 pivotCol, T tol = T (1.0e-6f));
+
+
+	// calculate out = V * A;
+	void VectorTimeMatrix (const dgGeneralVector<T> &v, dgGeneralVector<T> &out);
+
+	// calculate out = A * transpose (V);
+	void MatrixTimeVectorTranspose (const dgGeneralVector<T> &v, dgGeneralVector<T> &out);
+
+
+	// calculate M = A * B;
+	void MatrixTimeMatrix (const dgGeneralMatrix<T>& A, const dgGeneralMatrix<T>& B);
+
+	// calculate M = A * transpose (B);
+	void MatrixTimeMatrixTranspose (const dgGeneralMatrix<T>& A, const dgGeneralMatrix<T>& Bt);
+
+	bool Solve (dgGeneralVector<T> &b, T tol = T(0.0f));
+
+	bool CholeskyDecomposition ();
+	bool TestPSD () const;
+	bool TestSymetry () const;
+
+
+	void Trace () const;
+
+
+	protected:
+	bool m_ownMemory;
+	dgInt32 m_rowCount;
+	dgInt32 m_colCount;
+	dgGeneralVector<T>* m_rows;
+
+	private:
+	T* m_buffer;
+};
+
+
+// ***********************************************************************************************
+//
+//	LinearSystem
+//
+// ***********************************************************************************************
+template<class T>
+dgGeneralMatrix<T>::dgGeneralMatrix (dgInt32 row, dgInt32 column)
+{
+	dgInt32 i;
+	dgInt32 columnPad;
+	dgAssert  (row > 0);
+	dgAssert  (column > 0);
+
+	m_rowCount = row;
+	m_colCount = column;
+	m_ownMemory = true;
+
+
+	columnPad = ((column * sizeof (T) + 0x0f) & -0x0f) / sizeof (T);
+	m_buffer = new T [row * columnPad];
+
+	m_rows = new dgGeneralVector<T>[row];
+	for (i = 0; i < row; i ++) {
+		m_rows[i] = dgGeneralVector<T> (column, &m_buffer[i * columnPad]);
+	}
+}
+
+
+template<class T>
+dgGeneralMatrix<T>::dgGeneralMatrix (const dgGeneralMatrix<T>& src)
+{
+	dgInt32 i;
+	dgInt32 columnPad;
+
+	m_ownMemory = true;
+	m_rowCount = src.m_rowCount;
+	m_colCount = src.m_colCount;
+
+	columnPad = ((m_colCount * sizeof (T) + 0x0f) & -0x0f) / sizeof (T);
+	m_buffer = new T [m_rowCount * columnPad];
+	m_rows = new dgGeneralVector<T>[m_rowCount];
+
+	for (i = 0; i < m_rowCount; i ++) {
+		m_rows[i] = dgGeneralVector<T> (src[i], &m_buffer[i * columnPad]);
+	}
+}
+
+template<class T>
+dgGeneralMatrix<T>::dgGeneralMatrix (
+	dgInt32 row, 
+	dgInt32 column,
+	T *elemBuffer) 
+{
+	dgInt32 i;
+	dgInt32 columnPad;
+
+	m_ownMemory = false;
+	m_rowCount = row;
+	m_colCount = column;
+
+
+	dgAssert ((((dgUnsigned32) elemBuffer) & 0x0f) == 0);
+
+	m_buffer = elemBuffer;
+	columnPad = ((m_colCount * sizeof (T) + 0x0f) & -0x0f) / sizeof (T);
+	m_rows = (dgGeneralVector<T>*) &elemBuffer[m_rowCount * columnPad];
+	for (i = 0; i < row; i ++) {
+		m_rows[i] = dgGeneralVector<T> (column, &m_buffer[i * columnPad]);
+	}
+}
+
+
+template<class T>
+dgGeneralMatrix<T>::dgGeneralMatrix (
+	const dgGeneralMatrix<T>& src, 
+	T *elemBuffer) 
+{
+	dgInt32 i;
+	dgInt32 columnPad;
+
+	m_ownMemory = false;
+	m_rowCount = src.m_rowCount;
+	m_colCount = src.m_colCount;
+
+	dgAssert ((((dgUnsigned32) elemBuffer) & 0x0f) == 0);
+	m_buffer = elemBuffer;
+
+	columnPad = ((m_colCount * sizeof (T) + 0x0f) & -0x0f) / sizeof (T);
+	m_rows = (dgGeneralVector<T>*) &elemBuffer[m_rowCount * columnPad];
+	for (i = 0; i < m_rowCount; i ++) {
+		m_rows[i] = dgGeneralVector<T> (src[i], &m_buffer[i * columnPad]);
+	}
+}
+
+
+
+template<class T>  
+dgGeneralMatrix<T>::~dgGeneralMatrix ()
+{
+	if (m_ownMemory) {
+		delete[] m_rows;
+		delete[] m_buffer;
+	}
+}
+
+
+template<class T>  
+dgInt32 dgGeneralMatrix<T>::GetRowCount() const
+{
+	return m_rowCount;
+}
+
+template<class T>  
+dgInt32 dgGeneralMatrix<T>::GetColCount() const
+{
+	return m_colCount;
+}
+
+
+template<class T>
+void dgGeneralMatrix<T>::Trace () const
+{
+	dgInt32 i;
+
+	for (i = 0; i < m_rowCount; i ++) {
+		m_rows[i].Trace ();
+	}
+}
+
+
+
+template<class T>
+dgGeneralVector<T>& dgGeneralMatrix<T>::operator[] (dgInt32 i)
+{
+	dgAssert (i < m_rowCount);
+	dgAssert (i >= 0);
+	return m_rows[i];
+}	
+
+template<class T>
+const dgGeneralVector<T>& dgGeneralMatrix<T>::operator[] (dgInt32 i) const
+{
+	dgAssert (i < m_rowCount);
+	dgAssert (i >= 0);
+	return m_rows[i];
+}
+
+template<class T>
+void dgGeneralMatrix<T>::Clear (T val)
+{
+	dgInt32 i;
+	for (i = 0; i < m_rowCount; i ++ ) {
+		m_rows[i].Clear(val);
+	}
+}
+
+template<class T>
+void dgGeneralMatrix<T>::Identity ()
+{
+	dgInt32 i;
+
+	for (i = 0; i < m_rowCount; i ++ ) {
+		m_rows[i].Clear(T (0.0f));
+		m_rows[i][i] = T (1.0f);
+	}
+}
+
+
+//template<class T>
+//void dgGeneralMatrix<T>::Transpose ()
+//{
+//	dgInt32 i;
+//	dgInt32 j;
+//
+//	dgAssert (m_rowCount	== 
+//	dgGeneralMatrix<T>& me = *this;
+//	for (i = 0; i < m_rowCount; i ++) {
+//		for (j = i + 1; j < m_rowCount; j ++) {
+//			T tmp (me[i][j]);
+//			me[i][j] = me[j][i];
+//			me[j][i] = tmp;
+//
+//			#ifdef DG_COUNT_FLOAT_OPS
+//			dgGeneralVector<T>::m_memoryWrite += 2;
+//			#endif
+//		}
+//	}
+//}
+
+
+template<class T>
+void dgGeneralMatrix<T>::GaussianPivotStep (
+	dgInt32 srcRow, 
+	dgInt32 pivotRow, 
+	dgInt32 pivotCol,
+	T tol)
+{
+	dgGeneralMatrix<T>& me = *this;
+
+	T num (me[pivotRow][pivotCol]);
+	if (T(dgAbsf (num)) > tol) {
+		T den (me[srcRow][pivotCol]);
+		dgAssert (T(dgAbsf (den)) > T (0.0f));
+
+		#ifdef DG_COUNT_FLOAT_OPS
+		dgGeneralVector<T>::m_floatsOp += 2;
+		#endif
+	
+		den = - num / den;
+		me[pivotRow].LinearCombine (den, me[srcRow], me[pivotRow]);
+	}
+}
+
+
+//template<class T>
+//void dgGeneralMatrix<T>::Inverse (dgGeneralMatrix& inverseOut)
+//{
+//	dgAssert (m_colCount == m_rowCount);
+//}
+
+
+template<class T>
+void dgGeneralMatrix<T>::VectorTimeMatrix (const dgGeneralVector<T> &v, dgGeneralVector<T> &out)
+{
+	dgInt32 i;
+	dgInt32 j;
+	T acc;
+	T* outMem;
+	const T* inMem;
+
+	dgAssert (&v != &out);
+	dgAssert (m_rowCount	== v.m_colCount);
+	dgAssert (m_colCount == out.m_colCount);
+
+	inMem = &v[0];
+	outMem = &out[0];
+	const dgGeneralMatrix<T>& me = *this;
+	for (i = 0; i < m_colCount; i ++) {
+		acc = T (0.0f);
+		for (j = 0; j < m_rowCount; j ++) {
+			acc = acc + inMem[j] * me[j][i];
+
+			#ifdef DG_COUNT_FLOAT_OPS
+			dgGeneralVector<T>::m_floatsOp += 2;
+			#endif
+		}
+		outMem[i] = acc;
+		#ifdef DG_COUNT_FLOAT_OPS
+		dgGeneralVector<T>::m_memoryWrite += 1;
+		#endif
+	}
+}
+
+
+template<class T>
+void dgGeneralMatrix<T>::MatrixTimeVectorTranspose (const dgGeneralVector<T> &v, dgGeneralVector<T> &out)
+{
+	dgInt32 i;
+
+	dgAssert (&v != &out);
+	dgAssert (m_rowCount	== out.m_colCount);
+	dgAssert (m_colCount	== v.m_colCount);
+
+	for (i = 0; i < m_rowCount; i ++) {
+		out[i] = v.DotProduct (m_rows[i]);
+	}
+}
+
+template<class T>
+void dgGeneralMatrix<T>::MatrixTimeMatrix (const dgGeneralMatrix<T>& A, const dgGeneralMatrix<T>& B)
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	dgInt32 count;
+ 	T* out;
+	T* rowA;
+
+	dgAssert (m_rowCount	== A.m_rowCount);
+	dgAssert (m_colCount	== B.m_colCount);
+	dgAssert (A.m_colCount == B.m_rowCount);
+
+	dgAssert (this != &A);
+
+	count = A.m_colCount;
+	for (i = 0; i < m_rowCount; i ++) {
+		out = &m_rows[i][0];
+		rowA = &A.m_rows[i][0];
+		for (j = 0; j < m_colCount; j ++) {
+			T acc (0.0f);
+			for (k = 0; k < count; k ++) {
+				acc = acc + rowA[k] * B.m_rows[k][j]; 
+
+				#ifdef DG_COUNT_FLOAT_OPS
+				dgGeneralVector<T>::m_floatsOp += 2;
+				#endif
+			}
+			out[j] = acc;
+
+			#ifdef DG_COUNT_FLOAT_OPS
+			dgGeneralVector<T>::m_memoryWrite += 1;
+			#endif
+		}
+	}
+}
+
+
+template<class T>
+void dgGeneralMatrix<T>::MatrixTimeMatrixTranspose (const dgGeneralMatrix<T>& A, const dgGeneralMatrix<T>& Bt)
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	dgInt32 count;
+	T* out;
+	T* rowA;
+	T* rowB;
+
+	dgAssert (m_rowCount	== A.m_rowCount);
+	dgAssert (m_colCount	== Bt.m_rowCount);
+	dgAssert (A.m_colCount == Bt.m_colCount);
+
+	dgAssert (this != &A);
+	dgAssert (this != &Bt);
+
+	count = A.m_colCount;
+	for (i = 0; i < m_rowCount; i ++) {
+		out = &m_rows[i][0];
+		rowA = &A.m_rows[i][0];
+		for (j = 0; j < m_colCount; j ++) {
+			T acc (0.0f);
+			rowB = &Bt.m_rows[j][0];
+			for (k = 0; k < count; k ++) {
+				acc = acc + rowA[k] * rowB[k]; 
+
+				#ifdef DG_COUNT_FLOAT_OPS
+				dgGeneralVector<T>::m_floatsOp += 2;
+				#endif
+			}
+			out[j] = acc;
+
+			#ifdef DG_COUNT_FLOAT_OPS
+			dgGeneralVector<T>::m_memoryWrite += 1;
+			#endif
+		}
+	}
+}
+
+
+
+
+template<class T>
+bool dgGeneralMatrix<T>::Solve (dgGeneralVector<T> &b, T tol)
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	T* B;
+	T* rowI;
+	T* rowK;
+
+	dgAssert (m_rowCount	== m_colCount);
+	dgAssert (b.m_colCount == m_colCount);
+
+	B = &b[0];
+	// convert to upper triangular matrix by applying gauss pivoting
+	for (i = 0; i < m_rowCount - 1; i ++) {
+		rowI = &m_rows[i][0];
+		T den (rowI[i]);
+
+		if (T (dgAbsf (den)) < T (0.0f)) {
+		 	return false;
+		}
+
+		for (k = i + 1; k < m_rowCount; k ++) {
+			rowK = &m_rows[k][0];
+			T num (rowK[i]);
+
+			if (T (dgAbsf (num)) > tol) {
+				num = num / den;
+				for (j = i + 1; j < m_rowCount; j ++) {
+					rowK[j] = rowK[j] - rowI[j] * num;
+
+					#ifdef DG_COUNT_FLOAT_OPS
+					dgGeneralVector<T>::m_floatsOp += 2;
+					dgGeneralVector<T>::m_memoryWrite += 1;
+					#endif
+				}
+				B[k] = B[k] - B[i] * num;
+
+				#ifdef DG_COUNT_FLOAT_OPS
+				dgGeneralVector<T>::m_floatsOp += 3;
+				dgGeneralVector<T>::m_memoryWrite += 1;
+				#endif
+			}
+		}
+	}
+
+
+	B[m_rowCount-1] = B[m_rowCount-1] / m_rows[m_rowCount-1][m_rowCount-1];
+	for (i =	m_rowCount - 2; i >= 0; i --) {
+		T acc (0);
+		rowI = &m_rows[i][0];
+		for (j = i + 1 ; j < m_rowCount; j ++) {
+			acc = acc + rowI[j] * B[j];
+
+			#ifdef DG_COUNT_FLOAT_OPS
+			dgGeneralVector<T>::m_floatsOp += 2;
+			#endif
+		}
+		B[i] = (B[i] - acc) / rowI[i];
+
+		#ifdef DG_COUNT_FLOAT_OPS
+		dgGeneralVector<T>::m_floatsOp += 2;
+		dgGeneralVector<T>::m_memoryWrite += 1;
+		#endif
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgGeneralVector<T>::m_floatsOp += 1;
+	dgGeneralVector<T>::m_memoryWrite += 1;
+	#endif
+		
+	return true;
+}
+
+template<class T>
+void dgGeneralMatrix<T>::SwapRows (dgInt32 i, dgInt32 j)
+{
+	dgAssert (i	>= 0);
+	dgAssert (j	>= 0);
+	dgAssert (i	< m_rowCount);
+	dgAssert (j	< m_rowCount);
+	dgAssert (j	!= i);
+	dgSwap(m_rows[i].m_columns, m_rows[j].m_columns);
+}
+
+template<class T>
+void dgGeneralMatrix<T>::SwapColumns (dgInt32 i, dgInt32 j)
+{
+	dgInt32 k;
+	dgAssert (i	>= 0);
+	dgAssert (j	>= 0);
+	dgAssert (i	< m_colCount);
+	dgAssert (j	< m_colCount);
+	for (k = 0; k < m_colCount; k ++) {
+		dgSwap(m_rows[k][i], m_rows[k][j]);
+	}
+}
+
+
+
+template<class T>
+bool dgGeneralMatrix<T>::TestSymetry () const
+{
+	dgInt32 i;
+	dgInt32 j;
+
+	if (m_colCount	!= m_rowCount) {
+		return false;
+	}
+
+	dgGeneralMatrix<T> mat = *this;
+	for (i = 0; i < m_rowCount; i ++) {
+		for (j = i + 1; j < m_rowCount; j ++) {
+			if (dgAbsf (mat[i][j] - mat[j][i]) > 1.0e-12f) {
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
+template<class T>
+bool dgGeneralMatrix<T>::TestPSD () const
+{
+	if (!TestSymetry ()) {
+		return false;
+	}
+
+	dgGeneralMatrix<T> tmp (*this);
+	return tmp.CholeskyDecomposition();
+}
+
+
+
+template<class T>
+bool dgGeneralMatrix<T>::CholeskyDecomposition()
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	T factor;
+	T* rowK;
+	T* rowJ;
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgInt32 memCount;
+	dgInt32 floatCount;
+
+	memCount = dgGeneralVector<T>::GetMemWrites();
+	floatCount = dgGeneralVector<T>::GetFloatOps();
+	#endif
+
+	for (j = 0; j < m_rowCount; j++) {
+//Trace ();
+//dgTrace (("\n"));
+		rowJ = &m_rows[j].m_columns[0];
+
+		for (k = 0; k < j; k ++ ) {
+			rowK = &m_rows[k].m_columns[0];
+
+			factor = rowK[j];
+			if (dgAbsf (factor) > 1.0e-6f) {
+				for (i = j; i < m_rowCount; i ++) {
+					rowJ[i] -= rowK[i] * factor;
+					#ifdef DG_COUNT_FLOAT_OPS
+					memCount += 1;
+					floatCount += 2;
+					#endif
+				}
+			}
+		}
+
+		factor = rowJ[j];
+		if (factor <= T (0.0f)) {
+			if (factor <= T(-5.0e-4f)) {
+	 			return false;
+			}
+			factor = T(1.0e-12f);
+		}
+
+		factor = T (dgFloat64 (1.0) / T (sqrt (factor)));
+		rowJ[j] = factor;
+		factor = T(1.0f / factor);
+
+		#ifdef DG_COUNT_FLOAT_OPS
+		memCount += 1;
+		floatCount += 1;
+		#endif
+
+		for (k = j + 1; k < m_rowCount; k ++) {
+			rowJ[k] *= factor;
+			#ifdef DG_COUNT_FLOAT_OPS
+			memCount += 1;
+			floatCount += 1;
+			#endif
+		}
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgGeneralVector<T>::SetMemWrites(memCount); 
+	dgGeneralVector<T>::SetFloatOps(floatCount); 
+	#endif
+
+	return true;
+}
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgGeneralVector.cpp

@@ -0,0 +1,25 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgGeneralVector.h"
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgGeneralVector.h

@@ -0,0 +1,364 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgGeneralVector__
+#define __dgGeneralVector__
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+
+#define DG_COUNT_FLOAT_OPS
+
+template<class T> class dgSPDMatrix;
+template<class T> class dgGeneralMatrix;
+
+template<class T>
+class dgGeneralVector
+{
+	friend class dgSPDMatrix<T>;
+	friend class dgGeneralMatrix<T>;
+
+	public:
+	dgGeneralVector (dgInt32 size);
+	dgGeneralVector (dgInt32 size, T *mem);
+	dgGeneralVector (const dgGeneralVector<T> &src);
+	dgGeneralVector (const dgGeneralVector<T> &src, T *mem);
+
+	~dgGeneralVector ();
+
+	T& operator[] (dgInt32 i);
+	const T& operator[] (dgInt32 i) const;
+
+	dgInt32 GetRowCount() const;
+
+	void Clear (T val);
+	void Copy (const dgGeneralVector<T> &src);
+	T DotProduct (const dgGeneralVector &b) const;
+	T Norm () const;
+	T Norm2 () const;
+
+	void operator += (const dgGeneralVector &A);
+	void operator -= (const dgGeneralVector &A); 
+
+	void Scale (T scale);
+	void LinearCombine (T scale, const dgGeneralVector &A, const dgGeneralVector &B);
+
+
+	static dgInt32 GetFloatOps();
+	static dgInt32 GetMemWrites();
+
+	static void SetFloatOps(dgInt32 initialCount = 0);
+	static void SetMemWrites(dgInt32 initialCount = 0);
+
+	void Trace () const;
+
+
+	protected:
+	bool m_ownMemory;
+	dgInt32 m_colCount;
+	T* m_columns;
+
+	static dgInt32 m_floatsOp;
+	static dgInt32 m_memoryWrite;
+
+
+	dgGeneralVector();
+};
+
+
+// ***********************************************************************************************
+//
+//	vector
+//
+// ***********************************************************************************************
+template<class T>
+dgGeneralVector<T>::dgGeneralVector() 
+{
+	m_ownMemory = false;
+	m_colCount = 0;
+	m_columns = NULL;
+}
+
+template<class T>
+dgGeneralVector<T>::dgGeneralVector (dgInt32 size)
+{
+	dgAssert  (size > 0);
+	m_ownMemory = true;
+	m_colCount = size;
+	m_columns = new T[size];
+	dgAssert ((((dgUnsigned32) m_columns) & 0x0f) == 0);
+}
+
+template<class T>
+dgGeneralVector<T>::dgGeneralVector (dgInt32 size, T *mem)
+{
+	m_ownMemory = false;
+	m_colCount = size;
+	m_columns = mem;
+	dgAssert ((((dgUnsigned32) m_columns) & 0x0f) == 0);
+}
+
+template<class T>
+dgGeneralVector<T>::dgGeneralVector (const dgGeneralVector<T> &src)
+{
+	m_ownMemory = true;
+	m_colCount = src.m_colCount;
+	m_columns = new T[m_colCount];
+	dgAssert ((((dgUnsigned32) m_columns) & 0x0f) == 0);
+
+	Copy (src);
+}
+
+template<class T>
+dgGeneralVector<T>::dgGeneralVector (const dgGeneralVector<T> &src,  T *mem)
+{
+	m_ownMemory = false;
+	m_colCount = src.m_colCount;
+	m_columns = mem;
+	dgAssert ((((dgUnsigned32) m_columns) & 0x0f) == 0);
+
+	Copy (src);
+}
+
+
+template<class T>
+dgInt32 dgGeneralVector<T>::GetFloatOps()
+{
+	return m_floatsOp;
+}
+
+template<class T>
+void dgGeneralVector<T>::SetFloatOps(dgInt32 initialCount)
+{
+	m_floatsOp = initialCount;
+}
+
+
+template<class T>
+dgInt32 dgGeneralVector<T>::GetMemWrites()
+{
+	return m_memoryWrite;
+}
+
+
+template<class T>
+void dgGeneralVector<T>::SetMemWrites(dgInt32 initialCount)
+{
+	m_memoryWrite = initialCount;
+}
+
+
+
+template<class T>
+dgGeneralVector<T>::~dgGeneralVector ()
+{
+	if (m_ownMemory) {
+		delete[] m_columns;
+	}
+}
+
+template<class T>
+void dgGeneralVector<T>::Trace () const
+{
+	dgInt32 i;
+	for (i = 0; i < m_colCount; i ++) {
+		dgTrace (("%f ", m_columns[i]));
+		
+	}
+	dgTrace (("\n"));
+}
+
+
+
+template<class T>
+T& dgGeneralVector<T>::operator[] (dgInt32 i)
+{
+	dgAssert (i < m_colCount);
+	dgAssert (i >= 0);
+	return m_columns[i];
+}	
+
+template<class T>
+const T& dgGeneralVector<T>::operator[] (dgInt32 i) const
+{
+	dgAssert (i < m_colCount);
+	dgAssert (i >= 0);
+	return m_columns[i];
+}
+
+template<class T>
+dgInt32 dgGeneralVector<T>::GetRowCount() const
+{
+	return m_colCount;
+}
+
+
+// return dot product
+template<class T>
+T dgGeneralVector<T>::DotProduct (const dgGeneralVector<T> &A) const 
+{
+	dgInt32 i;
+
+	dgAssert (m_colCount == A.m_colCount);
+	T val (0.0);
+	for (i = 0; i < m_colCount; i ++) {
+		val = val + m_columns[i] * A.m_columns[i];
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_floatsOp += 2 * m_colCount;
+	m_memoryWrite += m_colCount;
+	#endif
+
+	return val;
+}
+
+
+
+
+template<class T>
+void dgGeneralVector<T>::Clear (T val)
+{
+	dgInt32 i;
+	for (i = 0; i < m_colCount; i ++ ) {
+		m_columns[i] = val;
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_memoryWrite += m_colCount;
+	#endif
+}
+
+template<class T>
+void dgGeneralVector<T>::Copy (const dgGeneralVector<T> &src)
+{
+	dgInt32 i;
+
+	dgAssert (m_colCount == src.m_colCount);
+	for (i = 0; i < m_colCount; i ++ ) {
+		m_columns[i] = src.m_columns[i];
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_memoryWrite += m_colCount;
+	#endif
+}
+
+
+
+template<class T>
+void dgGeneralVector<T>::Scale (T scale)
+{
+	dgInt32 i;
+	for (i = 0; i < m_colCount; i ++ ) {
+		m_columns[i] = m_columns[i] * scale;
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_floatsOp += m_colCount;
+	m_memoryWrite += m_colCount;
+	#endif
+}
+
+
+template<class T>
+T dgGeneralVector<T>::Norm2 () const
+{
+	dgInt32 i;
+	T max;
+
+	max = 0;
+	for (i = 0; i < m_colCount; i ++ ) {
+		 max = dgMax (m_columns[i] * m_columns[i], max);
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_floatsOp += m_colCount;
+	m_memoryWrite += m_colCount;
+	#endif
+
+	return max;
+}
+
+template<class T>
+T dgGeneralVector<T>::Norm () const
+{
+	return T (sqrt (Norm2 ()));
+}
+
+
+template<class T>
+void dgGeneralVector<T>::LinearCombine (T scale, const dgGeneralVector<T> &A, const dgGeneralVector<T> &B)
+{
+	dgInt32 i;
+	
+	dgAssert (A.m_colCount == m_colCount);
+	dgAssert (B.m_colCount == m_colCount);
+	for (i = 0; i < m_colCount; i ++ ) {
+		m_columns[i] = A.m_columns[i] * scale + B.m_columns[i];
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_floatsOp += 3 * m_colCount;
+	m_memoryWrite += m_colCount;
+	#endif
+}
+
+
+template<class T>
+void dgGeneralVector<T>::operator+= (const dgGeneralVector<T> &A)
+{
+	dgInt32 i;
+
+	dgAssert (A.m_colCount == m_colCount);
+	for (i = 0; i < m_colCount; i ++ ) {
+		m_columns[i] += A.m_columns[i];
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_floatsOp += m_colCount;
+	m_memoryWrite += m_colCount;
+	#endif
+}
+
+template<class T>
+void dgGeneralVector<T>::operator-= (const dgGeneralVector<T> &A) 
+{
+	dgInt32 i;
+
+	dgAssert (A.m_colCount == m_colCount);
+	for (i = 0; i < m_colCount; i ++ ) {
+		m_columns[i] -= A.m_columns[i];
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	m_floatsOp += m_colCount;
+	m_memoryWrite += m_colCount;
+	#endif
+}
+
+
+template<class T> dgInt32 dgGeneralVector <T>::m_floatsOp = 0;
+template<class T> dgInt32 dgGeneralVector <T>::m_memoryWrite = 0;
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgGoogol.cpp

@@ -0,0 +1,587 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "dgStdafx.h"
+#include "dgGoogol.h"
+
+dgGoogol dgGoogol::m_zero(0.0); 
+dgGoogol dgGoogol::m_one(1.0); 
+dgGoogol dgGoogol::m_two(2.0);  
+dgGoogol dgGoogol::m_three(3.0);   
+dgGoogol dgGoogol::m_half(0.5);   
+
+#ifdef _IMPLEMENT_USING_INTEGER_ARITHMETIC_
+
+	dgGoogol::dgGoogol(void)
+		:m_sign(0)
+		,m_exponent(0)
+	{
+		memset (m_mantissa, 0, sizeof (m_mantissa));
+	}
+
+	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 dgGoogol::CopySignedMantissa (dgUnsigned64* const mantissa) const
+	{
+		memcpy (mantissa, m_mantissa, sizeof (m_mantissa));
+		if (m_sign) {
+			NegateMantissa (mantissa);
+		}
+	}
+
+	dgGoogol::operator double() const
+	{
+		dgFloat64 mantissa = (dgFloat64(1.0f) / dgFloat64 (dgUnsigned64(1)<<62)) * dgFloat64 (m_mantissa[0]);
+		mantissa = ldexp(mantissa, m_exponent) * (m_sign ?  dgFloat64 (-1.0f) : dgFloat64 (1.0f));
+		return mantissa;
+	}
+
+	dgGoogol dgGoogol::operator+ (const dgGoogol &A) const
+	{
+		dgGoogol tmp;
+		dgAssert (dgInt64 (m_mantissa[0]) >= 0);
+		dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
+
+		if (m_mantissa[0] && A.m_mantissa[0]) {
+			dgUnsigned64 mantissa0[DG_GOOGOL_SIZE];
+			dgUnsigned64 mantissa1[DG_GOOGOL_SIZE];
+			dgUnsigned64 mantissa[DG_GOOGOL_SIZE];
+
+			CopySignedMantissa (mantissa0);
+			A.CopySignedMantissa (mantissa1);
+
+			dgInt32 exponetDiff = m_exponent - A.m_exponent;
+			dgInt32 exponent = m_exponent;
+			if (exponetDiff > 0) {
+				ShiftRightMantissa (mantissa1, exponetDiff);
+			} else if (exponetDiff < 0) {
+				exponent = A.m_exponent;
+				ShiftRightMantissa (mantissa0, -exponetDiff);
+			} 
+
+			dgUnsigned64 carrier = 0;
+			for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+				dgUnsigned64 m0 = mantissa0[i];
+				dgUnsigned64 m1 = mantissa1[i];
+				mantissa[i] = m0 + m1 + carrier;
+				carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
+			}
+
+			dgInt8 sign = 0;
+			if (dgInt64 (mantissa[0]) < 0) {
+				sign = 1;
+				NegateMantissa (mantissa);
+			}
+
+			dgInt32 bits = NormalizeMantissa (mantissa);
+			if (bits <= (-64 * DG_GOOGOL_SIZE)) {
+				tmp.m_sign = 0;
+				tmp.m_exponent = 0;
+			} else {
+				tmp.m_sign = sign;
+				tmp.m_exponent =  dgInt16 (exponent + bits);
+			}
+
+			memcpy (tmp.m_mantissa, mantissa, sizeof (m_mantissa));
+
+
+		} else if (A.m_mantissa[0]) {
+			tmp = A;
+		} else {
+			tmp = *this;
+		}
+
+		dgAssert (dgInt64 (tmp.m_mantissa[0]) >= 0);
+		return tmp;
+	}
+
+	dgGoogol dgGoogol::operator- (const dgGoogol &A) const
+	{
+		dgGoogol tmp (A);
+		tmp.m_sign = !tmp.m_sign;
+		return *this + tmp;
+	}
+
+	void dgGoogol::ScaleMantissa (dgUnsigned64* const dst, dgUnsigned64 scale) const
+	{
+		dgUnsigned64 carrier = 0;
+		for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+			if (m_mantissa[i]) {
+				dgUnsigned64 low;
+				dgUnsigned64 high;
+				ExtendeMultiply (scale, m_mantissa[i], high, low);
+				dgUnsigned64 acc = low + carrier;
+				carrier = CheckCarrier (low, carrier);	
+				dgAssert (CheckCarrier (carrier, high) == 0);
+				carrier += high;
+				dst[i + 1] = acc;
+			} else {
+				dst[i + 1] = carrier;
+				carrier = 0;
+			}
+
+		}
+		dst[0] = carrier;
+	}
+
+	dgGoogol dgGoogol::operator* (const dgGoogol &A) const
+	{
+		dgAssert (dgInt64 (m_mantissa[0]) >= 0);
+		dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
+
+		if (m_mantissa[0] && A.m_mantissa[0]) {
+			dgUnsigned64 mantissaAcc[DG_GOOGOL_SIZE * 2];
+			memset (mantissaAcc, 0, sizeof (mantissaAcc));
+			for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+				dgUnsigned64 a = m_mantissa[i];
+				if (a) {
+					dgUnsigned64 mantissaScale[2 * DG_GOOGOL_SIZE];
+					memset (mantissaScale, 0, sizeof (mantissaScale));
+					A.ScaleMantissa (&mantissaScale[i], a);
+
+					dgUnsigned64 carrier = 0;
+					for (dgInt32 j = 0; j < 2 * DG_GOOGOL_SIZE; j ++) {
+						const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - j;
+						dgUnsigned64 m0 = mantissaAcc[k];
+						dgUnsigned64 m1 = mantissaScale[k];
+						mantissaAcc[k] = m0 + m1 + carrier;
+						carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
+					}
+				}
+			}
+
+			dgUnsigned64 carrier = 0;
+			dgInt32 bits = dgUnsigned64(LeadingZeros (mantissaAcc[0]) - 2);
+			for (dgInt32 i = 0; i < 2 * DG_GOOGOL_SIZE; i ++) {
+				const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - i;
+				dgUnsigned64 a = mantissaAcc[k];
+				mantissaAcc[k] = (a << dgUnsigned64(bits)) | carrier;
+				carrier = a >> dgUnsigned64(64 - bits);
+			}
+
+			dgInt32 exp = m_exponent + A.m_exponent - (bits - 2);
+
+			dgGoogol tmp;
+			tmp.m_sign = m_sign ^ A.m_sign;
+			tmp.m_exponent = dgInt16 (exp);
+			memcpy (tmp.m_mantissa, mantissaAcc, sizeof (m_mantissa));
+
+			return tmp;
+		} 
+		return dgGoogol(0.0);
+	}
+
+	dgGoogol dgGoogol::operator/ (const dgGoogol &A) const
+	{
+		dgGoogol tmp (1.0 / A);
+		tmp = tmp * (m_two - A * tmp);
+		tmp = tmp * (m_two - A * tmp);
+		int test = 0;
+		dgInt32 passes = 0;
+		do {
+			passes ++;
+			dgGoogol tmp0 (tmp);
+			tmp = tmp * (m_two - A * tmp);
+			test = memcmp (&tmp0, &tmp, sizeof (dgGoogol));
+		} while (test && (passes < (2 * DG_GOOGOL_SIZE)));	
+		dgAssert (passes <= (2 * DG_GOOGOL_SIZE));
+		return (*this) * tmp;
+	}
+
+
+	dgGoogol dgGoogol::Abs () const
+	{
+		dgGoogol tmp (*this);
+		tmp.m_sign = 0;
+		return tmp;
+	}
+
+	dgGoogol dgGoogol::Floor () const
+	{
+		if (m_exponent < 1) {
+			return dgGoogol (0.0);
+		} 
+		dgInt32 bits = m_exponent + 2;
+		dgInt32 start = 0;
+		while (bits >= 64) {
+			bits -= 64;
+			start ++;
+		}
+
+		dgGoogol tmp (*this);
+		for (dgInt32 i = DG_GOOGOL_SIZE - 1; i > start; i --) {
+			tmp.m_mantissa[i] = 0;
+		}
+		// some compilers do no like this and I do not know why is that
+		//dgUnsigned64 mask = (-1LL) << (64 - bits);
+		dgUnsigned64 mask (~0ULL);
+		mask <<= (64 - bits);
+		tmp.m_mantissa[start] &= mask;
+		if (m_sign) {
+			dgAssert (0);
+		}
+
+		return tmp;
+	}
+
+	dgGoogol dgGoogol::InvSqrt () const
+	{
+		const dgGoogol& me = *this;
+		dgGoogol x (1.0f / sqrt (me));
+
+		dgInt32 test = 0;
+		dgInt32 passes = 0;
+		do {
+			passes ++;
+			dgGoogol tmp (x);
+			x = m_half * x * (m_three - me * x * x);
+			test = memcmp (&x, &tmp, sizeof (dgGoogol));
+		} while (test && (passes < (2 * DG_GOOGOL_SIZE)));	
+		dgAssert (passes <= (2 * DG_GOOGOL_SIZE));
+		return x;
+	}
+
+	dgGoogol dgGoogol::Sqrt () const
+	{
+		return *this * InvSqrt();
+	}
+
+	void dgGoogol::ToString (char* const string) const
+	{
+		dgGoogol tmp (*this);
+		dgGoogol base (10.0);
+		while (dgFloat64 (tmp) > 1.0) {
+			tmp = tmp/base;
+		}
+
+		dgInt32 index = 0;
+		while (tmp.m_mantissa[0]) {
+			tmp = tmp * base;
+			dgGoogol digit (tmp.Floor());
+			tmp -= digit;
+			dgFloat64 val = digit;
+			string[index] = char (val) + '0';
+			index ++;
+		}
+		string[index] = 0;
+	}
+
+
+#else 
+
+	dgGoogol::dgGoogol(void)
+		:m_value(0.0)
+	{
+	}
+
+	dgGoogol::dgGoogol(dgFloat64 value)
+		:m_value (value)
+	{
+	}
+
+	void dgGoogol::CopySignedMantissa (dgUnsigned64* const mantissa) const
+	{
+	}
+
+	dgGoogol::operator double() const
+	{
+		return m_value;
+	}
+
+	dgGoogol dgGoogol::operator+ (const dgGoogol &A) const
+	{
+		return m_value + A.m_value;
+	}
+
+
+	dgGoogol dgGoogol::operator- (const dgGoogol &A) const
+	{
+		return m_value - A.m_value;
+	}
+
+	dgGoogol dgGoogol::operator* (const dgGoogol &A) const
+	{
+		return m_value * A.m_value;
+	}
+
+	dgGoogol dgGoogol::operator/ (const dgGoogol &A) const
+	{
+		return m_value / A.m_value;
+	}
+
+
+	dgGoogol dgGoogol::Abs () const
+	{
+		return fabs (m_value);
+	}
+
+	dgGoogol dgGoogol::InvSqrt () const
+	{
+		return 1.0 / sqrt (m_value);
+	}
+
+	dgGoogol dgGoogol::Sqrt () const
+	{
+		return sqrt(m_value);
+	}
+
+
+	dgGoogol dgGoogol::Floor () const
+	{
+		return floor (m_value);
+	}
+
+	void dgGoogol::ToString (char* const string) const
+	{
+		sprintf (string, "%f", m_value);
+	}
+
+	void dgGoogol::ScaleMantissa (dgUnsigned64* const dst, dgUnsigned64 scale) const
+	{
+	}
+
+#endif
+
+
+
+void dgGoogol::NegateMantissa (dgUnsigned64* const mantissa) const
+{
+	dgUnsigned64 carrier = 1;
+	for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
+		dgUnsigned64 a = ~mantissa[i] + carrier;
+		if (a) {
+			carrier = 0;
+		}
+		mantissa[i] = a;
+	}
+}
+
+
+void dgGoogol::ShiftRightMantissa (dgUnsigned64* const mantissa, dgInt32 bits) const
+{
+	dgUnsigned64 carrier = 0;
+	if (dgInt64 (mantissa[0]) < dgInt64 (0)) {
+		carrier = dgUnsigned64 (-1);
+	}
+	
+	while (bits >= 64) {
+		for (dgInt32 i = DG_GOOGOL_SIZE - 2; i >= 0; i --) {
+			mantissa[i + 1] = mantissa[i];
+		}
+		mantissa[0] = carrier;
+		bits -= 64;
+	}
+
+	if (bits > 0) {
+		carrier <<= (64 - bits);
+		for (dgInt32 i = 0; i < DG_GOOGOL_SIZE; i ++) {
+			dgUnsigned64 a = mantissa[i];
+			mantissa[i] = (a >> bits) | carrier;
+			carrier = a << (64 - bits);
+		}
+	}
+}
+
+dgInt32 dgGoogol::LeadingZeros (dgUnsigned64 a) const
+{
+	#define dgCOUNTBIT(mask,add)		\
+	{									\
+		dgUnsigned64 test = a & mask;	\
+		n += test ? 0 : add;			\
+		a = test ? test : (a & ~mask);	\
+	}
+
+	dgInt32 n = 0;
+    dgAssert (a);
+	dgCOUNTBIT (0xffffffff00000000LL, 32);
+	dgCOUNTBIT (0xffff0000ffff0000LL, 16);
+	dgCOUNTBIT (0xff00ff00ff00ff00LL,  8);
+	dgCOUNTBIT (0xf0f0f0f0f0f0f0f0LL,  4);
+	dgCOUNTBIT (0xccccccccccccccccLL,  2);
+	dgCOUNTBIT (0xaaaaaaaaaaaaaaaaLL,  1);
+
+	return n;
+}
+
+dgInt32 dgGoogol::NormalizeMantissa (dgUnsigned64* const mantissa) const
+{
+	dgAssert (dgInt64 (mantissa[0]) >= 0);
+
+	dgInt32 bits = 0;
+	if(dgInt64 (mantissa[0] * 2) < 0) {
+		bits = 1;
+		ShiftRightMantissa (mantissa, 1);
+	} else {
+		while (!mantissa[0] && bits > (-64 * DG_GOOGOL_SIZE)) {
+			bits -= 64;
+			for (dgInt32 i = 1; i < DG_GOOGOL_SIZE; i ++) {
+				mantissa[i - 1] = mantissa[i];
+			}
+			mantissa[DG_GOOGOL_SIZE - 1] = 0;
+		}
+
+		if (bits > (-64 * DG_GOOGOL_SIZE)) {
+			dgInt32 n = LeadingZeros (mantissa[0]) - 2;
+			if (n > 0) {
+				dgAssert (n > 0);
+				dgUnsigned64 carrier = 0;
+				for (dgInt32 i = DG_GOOGOL_SIZE-1; i >= 0; i --) {
+					dgUnsigned64 a = mantissa[i];
+					mantissa[i] = (a << n) | carrier;
+					carrier = a >> (64 - n);
+				}
+				bits -= n;
+			} else if (n < 0) {
+				// this is very rare but it does happens, whee the leading zeros of the mantissa is an exact multiple of 64
+				dgAssert (mantissa[0] & dgUnsigned64(3)<<62);
+				dgUnsigned64 carrier = 0;
+				dgInt32 shift = -n;
+				for (dgInt32 i = 0; i < DG_GOOGOL_SIZE; i ++) {
+					dgUnsigned64 a = mantissa[i];
+					mantissa[i] = (a >> shift) | carrier;
+					carrier = a << (64 - shift);
+				}
+				bits -= n;
+
+			}
+		}
+	}
+	return bits;
+}
+
+dgUnsigned64 dgGoogol::CheckCarrier (dgUnsigned64 a, dgUnsigned64 b) const
+{
+	return ((dgUnsigned64 (-1) - b) < a) ? 1 : 0;
+}
+
+
+void dgGoogol::ExtendeMultiply (dgUnsigned64 a, dgUnsigned64 b, dgUnsigned64& high, dgUnsigned64& low) const
+{
+	dgUnsigned64 bLow = b & 0xffffffff; 
+	dgUnsigned64 bHigh = b >> 32; 
+	dgUnsigned64 aLow = a & 0xffffffff; 
+	dgUnsigned64 aHigh = a >> 32; 
+
+	dgUnsigned64 l = bLow * aLow;
+
+	dgUnsigned64 c1 = bHigh * aLow;
+	dgUnsigned64 c2 = bLow * aHigh;
+	dgUnsigned64 m = c1 + c2;
+	dgUnsigned64 carrier = CheckCarrier (c1, c2) << 32;
+
+	dgUnsigned64 h = bHigh * aHigh + carrier;
+
+	dgUnsigned64 ml = m << 32;	
+	dgUnsigned64 ll = l + ml;
+	dgUnsigned64 mh = (m >> 32) + CheckCarrier (l, ml);	
+	dgAssert ((mh & ~0xffffffff) == 0);
+
+	dgUnsigned64 hh = h + mh;
+
+	low = ll;
+	high = hh;
+}
+
+
+
+
+
+dgGoogol dgGoogol::operator+= (const dgGoogol &A)
+{
+	*this = *this + A;
+	return *this;
+}
+
+dgGoogol dgGoogol::operator-= (const dgGoogol &A)
+{
+	*this = *this - A;
+	return *this;
+}
+
+
+bool dgGoogol::operator> (const dgGoogol &A) const
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64(tmp) > 0.0;
+}
+
+bool dgGoogol::operator>= (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) >= 0.0;
+}
+
+bool dgGoogol::operator< (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) < 0.0;
+}
+
+bool dgGoogol::operator<= (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) <= 0.0;
+}
+
+bool dgGoogol::operator== (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) == 0.0;
+}
+
+bool dgGoogol::operator!= (const dgGoogol &A) const 
+{
+	dgGoogol tmp (*this - A);
+	return dgFloat64 (tmp) != 0.0;
+}
+
+
+
+
+
+void dgGoogol::Trace () const
+{
+	dgTrace (("%f ", dgFloat64 (*this)));
+}
+
+
+
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgGoogol.h

@@ -0,0 +1,138 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgGoogol__
+#define __dgGoogol__
+
+
+#include "dgStdafx.h"
+#include "dgMemory.h"
+#include "dgArray.h"
+#include "dgVector.h"
+
+#define _IMPLEMENT_USING_INTEGER_ARITHMETIC_
+
+//#define DG_GOOGOL_SIZE	16
+#define DG_GOOGOL_SIZE		4
+
+class dgGoogol
+{
+	public:
+	dgGoogol(void);
+	dgGoogol(dgFloat64 value);
+
+	operator double() const;	
+	dgGoogol operator+ (const dgGoogol &A) const; 
+	dgGoogol operator- (const dgGoogol &A) const; 
+	dgGoogol operator* (const dgGoogol &A) const; 
+	dgGoogol operator/ (const dgGoogol &A) const; 
+
+	dgGoogol operator+= (const dgGoogol &A); 
+	dgGoogol operator-= (const dgGoogol &A); 
+
+	bool operator> (const dgGoogol &A) const; 
+	bool operator>= (const dgGoogol &A) const; 
+	bool operator< (const dgGoogol &A) const; 
+	bool operator<= (const dgGoogol &A) const; 
+	bool operator== (const dgGoogol &A) const; 
+	bool operator!= (const dgGoogol &A) const; 
+
+	dgGoogol Abs () const;
+	dgGoogol Sqrt () const;
+	dgGoogol InvSqrt () const;
+	dgGoogol Floor () const;
+	
+	void Trace () const;
+	void ToString (char* const string) const;
+
+
+	private:
+	void InitFloatFloat (dgFloat64 value);
+	void NegateMantissa (dgUnsigned64* const mantissa) const;
+	void CopySignedMantissa (dgUnsigned64* const mantissa) const;
+	dgInt32 NormalizeMantissa (dgUnsigned64* const mantissa) const;
+	dgUnsigned64 CheckCarrier (dgUnsigned64 a, dgUnsigned64 b) const;
+	void ShiftRightMantissa (dgUnsigned64* const mantissa, dgInt32 bits) const;
+
+	dgInt32 LeadingZeros (dgUnsigned64 a) const;
+	void ExtendeMultiply (dgUnsigned64 a, dgUnsigned64 b, dgUnsigned64& high, dgUnsigned64& low) const;
+	void ScaleMantissa (dgUnsigned64* const out, dgUnsigned64 scale) const;
+
+#ifdef _IMPLEMENT_USING_INTEGER_ARITHMETIC_
+	dgInt32 m_sign;
+	dgInt32 m_exponent;
+	dgUnsigned64 m_mantissa[DG_GOOGOL_SIZE];
+#else
+	dgFloat64 m_value;
+#endif
+	
+
+	public:
+	static dgGoogol m_zero; 
+	static dgGoogol m_one; 
+	static dgGoogol m_two; 
+	static dgGoogol m_three; 
+	static dgGoogol m_half; 
+};
+
+
+class dgHugeVector: public dgTemplateVector<dgGoogol>
+{
+	public:
+	dgHugeVector ()
+		:dgTemplateVector<dgGoogol>()
+	{
+	}
+
+	dgHugeVector (const dgBigVector& a)
+		:dgTemplateVector<dgGoogol>(dgGoogol (a.m_x), dgGoogol (a.m_y), dgGoogol (a.m_z), dgGoogol (a.m_w))
+	{
+	}
+
+	dgHugeVector (const dgTemplateVector<dgGoogol>& a)
+		:dgTemplateVector<dgGoogol>(a)
+	{
+	}
+
+	dgHugeVector (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w)
+		:dgTemplateVector<dgGoogol>(x, y, z, w)
+	{
+	}
+
+	dgGoogol EvaluePlane (const dgHugeVector& point) const 
+	{
+		return (point % (*this)) + m_w;
+	}
+
+#ifdef _DEBUG
+	void Trace () const
+	{
+		m_x.Trace();
+		m_y.Trace();
+		m_z.Trace();
+		m_w.Trace();
+		dgTrace (("\n"));
+	}
+#endif
+};
+
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgGraph.h

@@ -0,0 +1,221 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgGraph__
+#define __dgGraph__
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgList.h"
+
+
+template<class dgNodeData, class dgEdgeData> class dgGraphEdge;
+template<class dgNodeData, class dgEdgeData> class dgGraphNode;
+
+
+template<class dgNodeData, class dgEdgeData>
+class dgGraph: public dgList<dgGraphNode<dgNodeData, dgEdgeData> >
+{
+	public:
+	dgGraph (dgMemoryAllocator* const allocator);
+	~dgGraph ();
+
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* AddNode ();
+	void DeleteNode (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node);
+
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* GetNodeFromNodeData(dgNodeData* const nodeData);
+
+	//void Trace () const;
+};
+
+template<class dgNodeData, class dgEdgeData> 
+class dgGraphNode: public dgList<dgGraphEdge<dgNodeData, dgEdgeData> >
+{
+	public:
+	dgGraphNode ();
+	~dgGraphNode ();
+
+	typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* AddEdge(typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node);
+	void DeleteHalfEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge);
+	void DeleteEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge);
+
+	//void Trace () const;
+
+	dgNodeData m_nodeData;
+};
+
+template<class dgNodeData, class dgEdgeData> 
+class dgGraphEdge
+{
+	public:
+	dgGraphEdge();
+	~dgGraphEdge();
+
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* m_node;
+	dgEdgeData m_edgeData;
+};
+
+
+
+template<class dgNodeData, class dgEdgeData>
+dgGraph<dgNodeData, dgEdgeData>::dgGraph (dgMemoryAllocator* const allocator) 
+	:dgList<dgGraphNode<dgNodeData, dgEdgeData> >(allocator)
+{
+}
+
+
+template<class dgNodeData, class dgEdgeData>
+dgGraph<dgNodeData, dgEdgeData>::~dgGraph () 
+{
+}
+
+template<class dgNodeData, class dgEdgeData>
+typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* dgGraph<dgNodeData, dgEdgeData>::AddNode ()
+{
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node = dgGraph<dgNodeData, dgEdgeData>::Append();
+
+	node->GetInfo().SetAllocator(dgGraph<dgNodeData, dgEdgeData>::GetAllocator());
+
+	return node;
+}
+
+template<class dgNodeData, class dgEdgeData>
+void dgGraph<dgNodeData, dgEdgeData>::DeleteNode (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node)
+{
+	for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* link = node->GetInfo().GetFirst(); link; link = link->GetNext()) {	
+		typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const twinNode = link->GetInfo().m_node;
+		for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* link1 = twinNode->GetInfo().GetFirst(); link1; link1 = link1->GetNext()) {	
+			if (link1->GetInfo().m_node == node) {
+				twinNode->GetInfo().Remove (link1);
+				break;
+			}
+		}
+	}
+	dgList<dgGraphNode<dgNodeData, dgEdgeData> >::Remove (node);
+}
+
+template<class dgNodeData, class dgEdgeData>
+typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* dgGraph<dgNodeData, dgEdgeData>::GetNodeFromNodeData(dgNodeData* const nodeData)
+{
+	dgInt32 size1 = sizeof (*nodeData);
+	dgInt32 size0 = sizeof (dgGraphNode<dgNodeData, dgEdgeData>);
+	void* const ptr = ((char*)nodeData) - (size0 - size1);
+	dgGraphNode<dgNodeData, dgEdgeData>* const nodeInfo = (dgGraphNode<dgNodeData, dgEdgeData>*) ptr;
+	return this->GetNodeFromInfo(*nodeInfo);
+}
+
+/*
+template<class dgNodeData, class dgEdgeData>
+void dgGraph<dgNodeData, dgEdgeData>::Trace () const
+{
+	for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* link = dgGraphNode<dgNodeData, dgEdgeData>::GetFirst(); link; link = link->GetNext()) {	
+		link->GetInfo().Trace ();
+		dgTrace (("%d: ", link->GetInfo().m_index));
+		for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* edge = link->GetInfo().GetFirst(); edge; edge = edge->GetNext()) {	
+			dgListNode* const node = edge->GetInfo().m_node;
+			dgTrace (("%d ", node->GetInfo().m_index));
+		}
+		dgTrace (("\n"));
+	}
+	dgTrace (("\n"));
+}
+*/
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphNode<dgNodeData, dgEdgeData>::dgGraphNode() 
+	:dgList<dgGraphEdge<dgNodeData, dgEdgeData> >(NULL)
+{
+
+}
+
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphNode<dgNodeData, dgEdgeData>::~dgGraphNode() 
+{
+
+}
+
+template<class dgNodeData, class dgEdgeData> 
+typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* dgGraphNode<dgNodeData, dgEdgeData>::AddEdge (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node)
+{
+	typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge = dgGraphNode<dgNodeData, dgEdgeData>::Append();
+
+	edge->GetInfo().m_node = node;
+	return edge;
+}
+
+template<class dgNodeData, class dgEdgeData> 
+void dgGraphNode<dgNodeData, dgEdgeData>::DeleteHalfEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge)
+{
+	dgList<dgGraphEdge<dgNodeData, dgEdgeData> >::Remove (edge);
+}
+
+template<class dgNodeData, class dgEdgeData> 
+void dgGraphNode<dgNodeData, dgEdgeData>::DeleteEdge(typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* const edge)
+{
+	typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node = edge->GetInfo().m_node;
+	
+	for (typename dgGraphNode<dgNodeData, dgEdgeData>::dgListNode* twinEdge = node->GetInfo().GetFirst(); twinEdge; twinEdge = twinEdge->GetNext()) {	
+		if (&twinEdge->GetInfo().m_node->GetInfo() == this) {
+			node->GetInfo().DeleteHalfEdge(twinEdge);
+			break;
+		}
+	}
+
+	DeleteHalfEdge(edge);
+}	
+
+/*
+template<class dgNodeData, class dgEdgeData> 
+void dgGraphNode<dgNodeData, dgEdgeData>::Trace () const
+{
+	dgTrace (("%d: ", m_index));
+	for (typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* edge = typename dgGraph<dgNodeData, dgEdgeData>::GetFirst(); edge; edge = edge->GetNext()) {	
+		typename dgGraph<dgNodeData, dgEdgeData>::dgListNode* const node;
+		node = edge->GetInfo().m_node;
+		dgTrace (("%d ", node->GetInfo().m_index));
+	}
+	dgTrace (("\n"));
+
+}
+*/
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphEdge<dgNodeData, dgEdgeData>::dgGraphEdge() 
+{
+
+}
+
+template<class dgNodeData, class dgEdgeData> 
+dgGraphEdge<dgNodeData, dgEdgeData>::~dgGraphEdge() 
+{
+
+}
+
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgHeap.h

@@ -0,0 +1,529 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgHeapBase__
+#define __dgHeapBase__
+
+#include "dgStdafx.h"
+#include "dgMemory.h"
+
+
+//#define DG_HEAP_SANITY_CHECK
+
+template <class OBJECT, class KEY>
+class dgHeapBase
+{
+	protected:
+	struct RECORD 
+	{
+		KEY m_key;
+		OBJECT m_obj;
+
+		RECORD (KEY key, const OBJECT& obj)
+			:m_key(key), m_obj(obj)
+		{
+		}
+	};
+
+	dgHeapBase (dgInt32 maxElements, dgMemoryAllocator* const allocator);
+	dgHeapBase (const void * const buffer, dgInt32 sizeInBytes);
+	~dgHeapBase ();
+	
+	public:
+	DG_CLASS_ALLOCATOR(allocator)
+
+	void Flush (); 
+	KEY MaxValue() const; 
+	KEY Value(dgInt32 i = 0) const;
+	dgInt32 GetCount() const;
+	dgInt32 GetMaxCount() const;
+	const OBJECT& operator[] (dgInt32 i) const;
+	dgInt32 Find (OBJECT &obj);
+	dgInt32 Find (KEY key);
+
+	dgInt32 m_curCount;
+	dgInt32 m_maxCount;
+	dgMemoryAllocator* m_allocator;
+	RECORD *m_pool;
+};
+
+template <class OBJECT, class KEY>
+class dgDownHeap: public dgHeapBase<OBJECT, KEY>
+{
+	public:
+	dgDownHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator);
+	dgDownHeap (const void * const buffer, dgInt32 sizeInBytes);
+
+	void Pop ();
+	void Push (OBJECT &obj, KEY key);
+	void Sort ();
+	void Remove (dgInt32 Index);
+
+#ifdef DG_HEAP_SANITY_CHECK
+	bool SanityCheck();
+#endif
+};
+
+template <class OBJECT, class KEY>
+class dgUpHeap: public dgHeapBase<OBJECT, KEY>
+{
+	public:
+	dgUpHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator);
+	dgUpHeap (const void * const buffer, dgInt32 sizeInBytes);
+
+	void Pop ();
+	void Push (OBJECT &obj, KEY key);
+	void Sort ();
+	void Remove (dgInt32 Index);
+
+#ifdef DG_HEAP_SANITY_CHECK
+	bool SanityCheck();
+#endif
+};
+
+
+
+template <class OBJECT, class KEY>
+dgHeapBase<OBJECT,KEY>::dgHeapBase (dgInt32 maxElements, dgMemoryAllocator* const allocator)
+{
+	m_allocator = allocator;
+	m_pool = (RECORD *)m_allocator->Malloc (maxElements * sizeof (RECORD));
+	m_maxCount = maxElements;
+	Flush();
+}
+
+template <class OBJECT, class KEY>
+dgHeapBase<OBJECT,KEY>::dgHeapBase (const void * const buffer, dgInt32 sizeInBytes)
+{
+	m_allocator = NULL;
+	m_pool = (RECORD *) buffer;
+	m_maxCount = dgInt32 (sizeInBytes / sizeof (RECORD));
+	Flush();
+}
+
+template <class OBJECT, class KEY>
+dgHeapBase<OBJECT,KEY>::~dgHeapBase ()
+{   
+	if (m_allocator) {
+		m_allocator->Free (m_pool);
+	}
+}
+
+
+template <class OBJECT, class KEY>
+KEY dgHeapBase<OBJECT,KEY>::Value(dgInt32 i) const
+{
+	return m_pool[i].m_key;
+}
+
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::GetCount() const
+{ 
+	return m_curCount;
+}
+
+
+template <class OBJECT, class KEY>
+void dgHeapBase<OBJECT,KEY>::Flush () 
+{
+	m_curCount = 0;
+
+	#ifdef _DEBUG
+//	dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = KEY (0);
+	#endif
+}
+
+
+template <class OBJECT, class KEY>
+KEY dgHeapBase<OBJECT,KEY>::MaxValue() const 
+{
+	return m_pool[0].m_key;
+}
+
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::GetMaxCount() const
+{ 
+	return m_maxCount;
+}
+
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::Find (OBJECT &obj)
+{
+	// For now let perform a linear search
+	// this is efficient if the size of the heap is small
+	// ex: m_curCount < 32
+	// this will be change to a binary search in the heap should the 
+	// the size of the heap get larger than 32
+	//	dgAssert (m_curCount <= 32);
+	for (dgInt32 i = 0; i < m_curCount; i ++) {
+		if (m_pool[i].obj == obj) {
+			return i;
+		}
+	}
+	return - 1;
+}
+
+
+template <class OBJECT, class KEY>
+dgInt32 dgHeapBase<OBJECT,KEY>::Find (KEY key)
+{
+	// ex: m_curCount < 32
+	// this will be change to a binary search in the heap shoud the 
+	// the size of the heap get larger than 32
+	dgAssert (m_curCount <= 32);
+	for (dgInt32 i = 0; i < m_curCount; i ++)	{
+		if (m_pool[i].m_key == key) {
+			return i;
+		}
+	}
+	return - 1;
+}
+
+
+template <class OBJECT, class KEY>
+const OBJECT& dgHeapBase<OBJECT,KEY>::operator[] (dgInt32 i) const
+{ 
+	dgAssert (i<= m_curCount);
+	return m_pool[i].m_obj;
+}
+
+
+// **************************************************************************
+//
+// down Heap
+//
+// **************************************************************************
+template <class OBJECT, class KEY>
+dgDownHeap<OBJECT,KEY>::dgDownHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator)
+	:dgHeapBase<OBJECT, KEY> (maxElements, allocator)
+{
+}
+
+template <class OBJECT, class KEY>
+dgDownHeap<OBJECT,KEY>::dgDownHeap (const void * const buffer, dgInt32 sizeInBytes)
+	:dgHeapBase<OBJECT, KEY> (buffer, sizeInBytes)
+{
+}
+
+
+template <class OBJECT, class KEY>
+void dgDownHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
+{
+#ifdef _DEBUG
+	dgInt32 cc = dgHeapBase<OBJECT,KEY>::m_curCount;
+	dgInt32 cm = dgHeapBase<OBJECT,KEY>::m_maxCount;
+	dgAssert (cc < cm);
+#endif
+
+	dgHeapBase<OBJECT,KEY>::m_curCount ++;
+
+	dgInt32 j;
+	dgInt32 i = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (; i; i = j) {
+		j = i >> 1;
+		if (!j || (dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key > key)) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgAssert (i);
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
+
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+}
+
+
+template <class OBJECT, class KEY>
+void dgDownHeap<OBJECT,KEY>::Remove (dgInt32 index)
+{
+	dgHeapBase<OBJECT,KEY>::m_curCount--;
+	KEY key (dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key);
+
+	dgInt32 j;
+	dgInt32 i = index + 1;
+	for (; i <= (dgHeapBase<OBJECT,KEY>::m_curCount>>1); i = j) {
+		j = i + i;
+		if ((j < dgHeapBase<OBJECT,KEY>::m_curCount) && 
+			(dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key < dgHeapBase<OBJECT,KEY>::m_pool[j].m_key)) {
+				j ++;
+		}
+		if (key >= dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj;
+
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+
+}
+
+template <class OBJECT, class KEY>
+void dgDownHeap<OBJECT,KEY>::Pop ()
+{
+	dgHeapBase<OBJECT,KEY>::m_curCount--;
+	KEY key (dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key);
+
+	dgInt32 j;
+	dgInt32 i = 1;
+	for (; i <= (dgHeapBase<OBJECT,KEY>::m_curCount>>1); i = j) {
+		j = i + i;
+		if ((j < dgHeapBase<OBJECT,KEY>::m_curCount) && 
+			(dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key < dgHeapBase<OBJECT,KEY>::m_pool[j].m_key)) {
+			j ++;
+		}
+		if (key >= dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj;
+
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+}
+
+
+
+template <class OBJECT, class KEY>
+void dgDownHeap<OBJECT,KEY>::Sort ()
+{
+
+	dgInt32 count = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (dgInt32 i = 1; i < count; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[0].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
+
+		Pop();
+
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
+	}
+
+	dgHeapBase<OBJECT,KEY>::m_curCount = count;
+	for (dgInt32 i = 0; i < count / 2; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
+
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_key = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
+
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
+	}
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+}
+
+#ifdef DG_HEAP_SANITY_CHECK
+template <class OBJECT, class KEY>
+bool dgDownHeap<OBJECT,KEY>::SanityCheck()
+{
+	for (dgInt32 i = 0; i < dgHeapBase<OBJECT,KEY>::m_curCount / 2; i ++) {
+		if (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key < dgHeapBase<OBJECT,KEY>::m_pool[i * 2 + 1].m_key) {
+			return false;
+		}
+		if ((i * 2 + 2) < dgHeapBase<OBJECT,KEY>::m_curCount) {
+			if (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key < dgHeapBase<OBJECT,KEY>::m_pool[i * 2 + 2].m_key) {
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+#endif
+
+
+
+
+// **************************************************************************
+//
+// down Heap
+//
+// **************************************************************************
+template <class OBJECT, class KEY>
+dgUpHeap<OBJECT,KEY>::dgUpHeap (dgInt32 maxElements, dgMemoryAllocator* const allocator)
+	:dgHeapBase<OBJECT, KEY> (maxElements, allocator)
+{
+}
+
+template <class OBJECT, class KEY>
+dgUpHeap<OBJECT,KEY>::dgUpHeap (const void * const buffer, dgInt32 sizeInBytes)
+	:dgHeapBase<OBJECT, KEY> (buffer, sizeInBytes)
+{
+}
+
+#ifdef DG_HEAP_SANITY_CHECK
+template <class OBJECT, class KEY>
+bool dgUpHeap<OBJECT,KEY>::SanityCheck()
+{
+	for (dgInt32 i = 0; i < dgHeapBase<OBJECT,KEY>::m_curCount / 2; i ++) {
+		if (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key > dgHeapBase<OBJECT,KEY>::m_pool[i * 2 + 1].m_key) {
+			return false;
+		}
+		if ((i * 2 + 2) < dgHeapBase<OBJECT,KEY>::m_curCount) {
+			if (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key > dgHeapBase<OBJECT,KEY>::m_pool[i * 2 + 2].m_key) {
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+#endif
+
+template <class OBJECT, class KEY>
+void dgUpHeap<OBJECT,KEY>::Push (OBJECT &obj, KEY key)
+{
+#ifdef _DEBUG
+	dgInt32 cc = dgHeapBase<OBJECT,KEY>::m_curCount;
+	dgInt32 cm = dgHeapBase<OBJECT,KEY>::m_maxCount;
+	dgAssert (cc < cm);
+#endif
+	dgHeapBase<OBJECT,KEY>::m_curCount ++;
+
+	dgInt32 j;
+	dgInt32 i = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (; i; i = j) {
+		j = i >> 1;
+		if (!j || (dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key < key)) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgAssert (i);
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = obj;
+
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+}
+
+
+template <class OBJECT, class KEY>
+void dgUpHeap<OBJECT,KEY>::Sort ()
+{
+	dgInt32 count = dgHeapBase<OBJECT,KEY>::m_curCount;
+	for (dgInt32 i = 1; i < count; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[0].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[0].m_obj);
+
+		Pop();
+
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj = obj;
+	}
+
+	dgHeapBase<OBJECT,KEY>::m_curCount = count;
+	for (dgInt32 i = 0; i < count / 2; i ++) {
+		KEY key (dgHeapBase<OBJECT,KEY>::m_pool[i].m_key);
+		OBJECT obj (dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj);
+
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_key = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key;
+		dgHeapBase<OBJECT,KEY>::m_pool[i].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj;
+
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_key = key;
+		dgHeapBase<OBJECT,KEY>::m_pool[count - i - 1].m_obj = obj;
+	}
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+}
+
+
+template <class OBJECT, class KEY>
+void dgUpHeap<OBJECT,KEY>::Remove (dgInt32 index)
+{
+	dgHeapBase<OBJECT,KEY>::m_curCount--;
+
+	dgInt32 j;
+	dgInt32 i = index + 1;
+	KEY key (dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key);
+	for (; i <= (dgHeapBase<OBJECT,KEY>::m_curCount>>1); i = j) {
+		j = i + i;
+		if ((j < dgHeapBase<OBJECT,KEY>::m_curCount) && 
+			(dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key > dgHeapBase<OBJECT,KEY>::m_pool[j].m_key)) {
+				j ++;
+		}
+		if (key <= dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj;
+
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+}
+
+
+template <class OBJECT, class KEY>
+void dgUpHeap<OBJECT,KEY>::Pop ()
+{
+	dgHeapBase<OBJECT,KEY>::m_curCount--;
+	KEY key (dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_key);
+
+	dgInt32 j;
+	dgInt32 i = 1;
+	for (; i <= (dgHeapBase<OBJECT,KEY>::m_curCount>>1); i = j) {
+		j = i + i;
+		if ((j < dgHeapBase<OBJECT,KEY>::m_curCount) && 
+			(dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key > dgHeapBase<OBJECT,KEY>::m_pool[j].m_key)) {
+				j ++;
+		}
+		if (key <= dgHeapBase<OBJECT,KEY>::m_pool[j - 1].m_key) {
+			break;
+		}
+		dgHeapBase<OBJECT,KEY>::m_pool[i - 1] = dgHeapBase<OBJECT,KEY>::m_pool[j - 1];
+	}
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_key = key;
+	dgHeapBase<OBJECT,KEY>::m_pool[i - 1].m_obj = dgHeapBase<OBJECT,KEY>::m_pool[dgHeapBase<OBJECT,KEY>::m_curCount].m_obj;
+
+#ifdef DG_HEAP_SANITY_CHECK
+	dgAssert (SanityCheck());
+#endif
+}
+
+
+#endif
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgIntersections.cpp

@@ -0,0 +1,1010 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgMemory.h"
+#include "dgVector.h"
+#include "dgPlane.h"
+#include "dgGoogol.h"
+#include "dgIntersections.h"
+
+#define USE_FLOAT_VERSION
+
+
+#define DG_RAY_TOL_ERROR (dgFloat32 (-1.0e-3f))
+#define DG_RAY_TOL_ADAPTIVE_ERROR (dgFloat32 (1.0e-1f))
+
+
+
+dgFloat32 dgFastRayTest::PolygonIntersectFallback (const dgVector& normal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+
+	dgBigVector v0 (&polygon[indexArray[indexCount - 1] * stride]);
+	dgBigVector m_p0_ (m_p0);
+	dgBigVector p0v0_ (v0 - m_p0_);
+	dgBigVector normal_ (normal);
+	dgBigVector diff_ (m_diff);
+	dgFloat64 tOut = normal_ % p0v0_;
+	dgFloat64 dist = normal_ % diff_;
+	if (tOut >= dist * maxT) {
+		if ((tOut < dgFloat64 (0.0f)) && (tOut > dist)) {
+			for (dgInt32 i = 0; i < indexCount; i ++) {
+				dgInt32 i2 = indexArray[i] * stride;
+				dgBigVector v1 (&polygon[i2]);
+				dgBigVector p0v1_ (v1 - m_p0_);
+				// calculate the volume formed by the line and the edge of the polygon
+				dgFloat64 alpha = (diff_ * p0v1_) % p0v0_;
+				// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
+				if (alpha < DG_RAY_TOL_ERROR) {
+					return 1.2f;
+				}
+				p0v0_ = p0v1_;
+			}
+
+			//the line is to the left of all the polygon edges, 
+			//then the intersection is the point we the line intersect the plane of the polygon
+			tOut = tOut / dist;
+			dgAssert (tOut >= dgFloat32 (0.0f));
+			dgAssert (tOut <= dgFloat32 (1.0f));
+			return dgFloat32 (tOut);
+		}
+	}
+	return dgFloat32 (1.2f);
+}
+
+
+
+dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& normal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
+{
+#if 1
+	dgAssert (m_p0.m_w == dgFloat32 (0.0f));
+	dgAssert (m_p1.m_w == dgFloat32 (0.0f));
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+
+	dgBigVector v0 (&polygon[indexArray[indexCount - 1] * stride]);
+	dgBigVector m_p0_ (m_p0);
+	dgBigVector p0v0_ (v0 - m_p0_);
+	dgBigVector normal_ (normal);
+	dgBigVector diff_ (m_diff);
+	dgFloat64 tOut = normal_ % p0v0_;
+	dgFloat64 dist = normal_ % diff_;
+	if (tOut >= dist * maxT) {
+		if ((tOut < dgFloat64 (0.0f)) && (tOut > dist)) {
+			for (dgInt32 i = 0; i < indexCount; i ++) {
+				dgInt32 i2 = indexArray[i] * stride;
+				dgBigVector v1 (&polygon[i2]);
+				dgBigVector p0v1_ (v1 - m_p0_);
+				// calculate the volume formed by the line and the edge of the polygon
+				dgFloat64 alpha = (diff_ * p0v1_) % p0v0_;
+				// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
+				if (alpha < DG_RAY_TOL_ERROR) {
+					return 1.2f;
+				}
+				p0v0_ = p0v1_;
+			}
+
+			//the line is to the left of all the polygon edges, 
+			//then the intersection is the point we the line intersect the plane of the polygon
+			tOut = tOut / dist;
+			dgAssert (tOut >= dgFloat32 (0.0f));
+			dgAssert (tOut <= dgFloat32 (1.0f));
+			return dgFloat32 (tOut);
+		}
+	}
+	return dgFloat32 (1.2f);
+
+#else 
+	dgAssert (m_p0.m_w == dgFloat32 (0.0f));
+	dgAssert (m_p1.m_w == dgFloat32 (0.0f));
+
+	dgFloat32 dist = normal % m_diff;
+	if (dist < m_dirError) {
+
+		dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+
+		dgVector v0 (&polygon[indexArray[indexCount - 1] * stride]);
+		dgVector p0v0 (v0 - m_p0);
+		dgFloat32 tOut = normal % p0v0;
+		if (tOut >= dist * maxT) {
+			// this only work for convex polygons and for single side faces 
+			// walk the polygon around the edges and calculate the volume 
+			dgFloat32 errorTest = m_magRayTest * DG_RAY_TOL_ADAPTIVE_ERROR;
+			if ((tOut < dgFloat32 (0.0f)) && (tOut > dist)) {
+				for (dgInt32 i = 0; i < indexCount; i ++) {
+					dgInt32 i2 = indexArray[i] * stride;
+					dgVector v1 (&polygon[i2]);
+					dgVector p0v1 (v1 - m_p0);
+					// calculate the volume formed by the line and the edge of the polygon
+					dgFloat32 alpha = (m_diff * p0v1) % p0v0;
+					// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
+					if (dgAbsf (alpha) < errorTest) {
+						return PolygonIntersectFallback (normal, maxT, polygon, strideInBytes, indexArray, indexCount);
+					} else if (alpha < 0.0f) {
+						return dgFloat32 (1.2f);
+					}
+					p0v0 = p0v1;
+				}
+
+				tOut = tOut / dist;
+				dgAssert (tOut >= dgFloat32 (0.0f));
+				dgAssert (tOut <= dgFloat32 (1.0f));
+				return tOut;
+			}
+		}
+	}
+	return dgFloat32 (1.2f);
+#endif
+}
+
+
+
+bool dgApi dgRayBoxClip (dgVector& p0, dgVector& p1, const dgVector& boxP0, const dgVector& boxP1) 
+{	
+	dgAssert (p0.m_w == dgFloat32(0.0f));
+	dgAssert (p1.m_w == dgFloat32(0.0f));
+	dgAssert (boxP0.m_w == dgFloat32(0.0f));
+	dgAssert (boxP1.m_w == dgFloat32(0.0f));
+	for (int i = 0; i < 3; i ++) {
+		dgFloat32 tmp0 = boxP1[i] - p0[i];
+		if (tmp0 > dgFloat32 (0.0f)) {
+			dgFloat32 tmp1 = boxP1[i] - p1[i];
+			if (tmp1 < dgFloat32 (0.0f)) {
+				p1 = p0 + (p1 - p0).Scale4 (tmp0 / (p1[i] - p0[i])); 
+				p1[i] = boxP1[i];
+			}
+		} else {
+			dgFloat32 tmp1 = boxP1[i] - p1[i];
+			if (tmp1 > dgFloat32 (0.0f)) {
+				p0 += (p1 - p0).Scale4 (tmp0 / (p1[i] - p0[i])); 
+				p0[i] = boxP1[i];
+			} else {
+				return false;
+			}
+		}
+
+		tmp0 = boxP0[i] - p0[i];
+		if (tmp0 < dgFloat32 (0.0f)) {
+			dgFloat32 tmp1 = boxP0[i] - p1[i];
+			if (tmp1 > dgFloat32 (0.0f)) {
+				p1 = p0 + (p1 - p0).Scale4 (tmp0 / (p1[i] - p0[i])); 
+				p1[i] = boxP0[i];
+			}
+		} else {
+			dgFloat32 tmp1 = boxP0[i] - p1[i];
+			if (tmp1 < dgFloat32 (0.0f)) {
+				p0 += (p1 - p0).Scale4 (tmp0 / (p1[i] - p0[i])); 
+				p0[i] = boxP0[i];
+			} else {
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
+
+dgVector dgApi dgPointToRayDistance (const dgVector& point, const dgVector& ray_p0, const dgVector& ray_p1)
+{
+	dgFloat32 t;
+	dgVector dp (ray_p1 - ray_p0);
+	t = dgClamp (((point - ray_p0) % dp) / (dp % dp), dgFloat32(dgFloat32 (0.0f)), dgFloat32 (1.0f));
+	return ray_p0 + dp.Scale3 (t);
+}
+
+void dgApi dgRayToRayDistance (const dgVector& ray_p0, const dgVector& ray_p1, const dgVector& ray_q0, const dgVector& ray_q1, dgVector& pOut, dgVector& qOut)
+{
+	dgFloat32 sN;
+	dgFloat32 tN;
+
+	dgVector u (ray_p1 - ray_p0);
+	dgVector v (ray_q1 - ray_q0);
+	dgVector w (ray_p0 - ray_q0);
+
+	dgFloat32 a = u % u;        // always >= 0
+	dgFloat32 b = u % v;
+	dgFloat32 c = v % v;        // always >= 0
+	dgFloat32 d = u % w;
+	dgFloat32 e = v % w;
+	dgFloat32 D = a*c - b*b;   // always >= 0
+	dgFloat32 sD = D;			// sc = sN / sD, default sD = D >= 0
+	dgFloat32 tD = D;			// tc = tN / tD, default tD = D >= 0
+
+	// compute the line parameters of the two closest points
+	if (D < dgFloat32 (1.0e-8f)) { // the lines are almost parallel
+		sN = dgFloat32 (0.0f);        // force using point P0 on segment S1
+		sD = dgFloat32 (1.0f);        // to prevent possible division by 0.0 later
+		tN = e;
+		tD = c;
+	} else {                // get the closest points on the infinite lines
+		sN = (b*e - c*d);
+		tN = (a*e - b*d);
+		if (sN < dgFloat32 (0.0f)) {       // sc < 0 => the s=0 edge is visible
+			sN = dgFloat32 (0.0f);
+			tN = e;
+			tD = c;
+		}
+		else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
+			sN = sD;
+			tN = e + b;
+			tD = c;
+		}
+	}
+
+
+	if (tN < dgFloat32 (0.0f)) {           // tc < 0 => the t=0 edge is visible
+		tN = dgFloat32 (0.0f);
+		// recompute sc for this edge
+		if (-d < dgFloat32 (0.0f))
+			sN = dgFloat32 (0.0f);
+		else if (-d > a)
+			sN = sD;
+		else {
+			sN = -d;
+			sD = a;
+		}
+	}
+	else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
+		tN = tD;
+		// recompute sc for this edge
+		if ((-d + b) < dgFloat32 (0.0f))
+			sN = dgFloat32 (0.0f);
+		else if ((-d + b) > a)
+			sN = sD;
+		else {
+			sN = (-d + b);
+			sD = a;
+		}
+	}
+
+	// finally do the division to get sc and tc
+	dgFloat32 sc = (dgAbsf(sN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : sN / sD);
+	dgFloat32 tc = (dgAbsf(tN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : tN / tD);
+
+	pOut = ray_p0 + u.Scale3 (sc);
+	qOut = ray_q0 + v.Scale3 (tc);
+}
+
+/*
+dgVector dgPointToTriangleDistance____ (const dgVector& point, const dgVector& p0, const dgVector& p1, const dgVector& p2)
+{
+	const dgVector p10 (p1 - p0);
+	const dgVector p20 (p2 - p0);
+	const dgVector p_p0 (point - p0);
+
+	dgFloat32 alpha1 = p10 % p_p0;
+	dgFloat32 alpha2 = p20 % p_p0;
+	if ((alpha1 <= dgFloat32 (0.0f)) && (alpha2 <= dgFloat32 (0.0f))) {
+		return p0;
+	}
+
+	dgVector p_p1 (point - p1);
+	dgFloat32 alpha3 = p10 % p_p1;
+	dgFloat32 alpha4 = p20 % p_p1;
+	if ((alpha3 >= dgFloat32 (0.0f)) && (alpha4 <= alpha3)) {
+		return p1;
+	}
+
+	dgFloat32 vc = alpha1 * alpha4 - alpha3 * alpha2;
+	if ((vc <= dgFloat32 (0.0f)) && (alpha1 >= dgFloat32 (0.0f)) && (alpha3 <= dgFloat32 (0.0f))) {
+		dgFloat32 t = alpha1 / (alpha1 - alpha3);
+		dgAssert (t >= dgFloat32 (0.0f));
+		dgAssert (t <= dgFloat32 (1.0f));
+		return p0 + p10.Scale3 (t);
+	}
+
+
+	dgVector p_p2 (point - p2);
+	dgFloat32 alpha5 = p10 % p_p2;
+	dgFloat32 alpha6 = p20 % p_p2;
+	if ((alpha6 >= dgFloat32 (0.0f)) && (alpha5 <= alpha6)) {
+		return p2;
+	}
+
+
+	dgFloat32 vb = alpha5 * alpha2 - alpha1 * alpha6;
+	if ((vb <= dgFloat32 (0.0f)) && (alpha2 >= dgFloat32 (0.0f)) && (alpha6 <= dgFloat32 (0.0f))) {
+		dgFloat32 t = alpha2 / (alpha2 - alpha6);
+		dgAssert (t >= dgFloat32 (0.0f));
+		dgAssert (t <= dgFloat32 (1.0f));
+		return p0 + p20.Scale3 (t);
+	}
+
+
+	dgFloat32 va = alpha3 * alpha6 - alpha5 * alpha4;
+	if ((va <= dgFloat32 (0.0f)) && ((alpha4 - alpha3) >= dgFloat32 (0.0f)) && ((alpha5 - alpha6) >= dgFloat32 (0.0f))) {
+		dgFloat32 t = (alpha4 - alpha3) / ((alpha4 - alpha3) + (alpha5 - alpha6));
+		dgAssert (t >= dgFloat32 (0.0f));
+		dgAssert (t <= dgFloat32 (1.0f));
+		return p1 + (p2 - p1).Scale3 (t);
+	}
+
+	dgFloat32 den = float(dgFloat32 (1.0f)) / (va + vb + vc);
+	dgFloat32 t = vb * den;
+	dgFloat32 s = vc * den;
+	dgAssert (t >= dgFloat32 (0.0f));
+	dgAssert (s >= dgFloat32 (0.0f));
+	dgAssert (t <= dgFloat32 (1.0f));
+	dgAssert (s <= dgFloat32 (1.0f));
+	return p0 + p10.Scale3 (t) + p20.Scale3 (s);
+}
+*/
+
+dgVector dgPointToTriangleDistance (const dgVector& point, const dgVector& p0, const dgVector& p1, const dgVector& p2, const dgVector& normal)
+{
+//	return  dgPointToTriangleDistance____ (point, p0, p1, p2);
+
+#ifdef _DEBUG
+	dgVector faceNormal ((p1 - p0) * (p2 - p0));
+	dgFloat64 faceNormal2 = faceNormal % faceNormal;
+	dgFloat64 normal2 = normal % normal;
+	dgFloat64 faceNormalNormal = faceNormal % normal;
+	dgFloat64 error = (faceNormalNormal * faceNormalNormal - faceNormal2 * normal2) / (faceNormalNormal * faceNormalNormal);
+	dgAssert (fabsf (error < dgFloat32 (1.0e-5f)));
+	dgAssert (faceNormalNormal > dgFloat32 (0.0f));
+#endif
+
+	dgVector array[3];
+	array[0] = p0;
+	array[1] = p1;
+	array[2] = p2;
+
+	dgInt32 i0 = 2;
+	dgInt32 closestIndex = -1;
+	dgVector p1p0 (array[2] - point);
+	for (dgInt32 i1 = 0; i1 < 3; i1 ++) {
+		dgVector p2p0 (array[i1] - point);
+
+		dgFloat32 volume = (p1p0 * p2p0) % normal;
+		if (volume < dgFloat32 (0.0f)) {
+			dgVector segment (array[i1] - array[i0]);
+			dgVector poinP0 (point - array[i0]);
+			dgFloat32 den = segment % segment;
+			dgAssert (den > dgFloat32 (0.0f));
+			dgFloat32 num = poinP0 % segment;
+			if (num < dgFloat32 (0.0f)) {
+				closestIndex = i0;
+			} else if (num > den) {
+				closestIndex = i1;
+			} else {
+				return array[i0] + segment.Scale3 (num / den);
+			}
+		}
+		p1p0 = p2p0;
+		i0 = i1;
+	}
+
+	if (closestIndex >= 0) {
+		return array[closestIndex];
+	} else {
+		return point - normal.Scale3((normal % (point - p0)) / (normal % normal));
+	}
+}
+
+
+dgBigVector dgPointToTriangleDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& normal)
+{
+#ifdef _DEBUG
+	dgBigVector faceNormal ((p1 - p0) * (p2 - p0));
+	dgFloat64 faceNormal2 = faceNormal % faceNormal;
+	dgFloat64 normal2 = normal % normal;
+	dgFloat64 faceNormalNormal = faceNormal % normal;
+	dgFloat64 error = (faceNormalNormal * faceNormalNormal - faceNormal2 * normal2) / (faceNormalNormal * faceNormalNormal);
+	dgAssert (fabsf (error < dgFloat32 (1.0e-5f)));
+	dgAssert (faceNormalNormal > dgFloat32 (0.0f));
+#endif
+
+	dgBigVector array[3];
+	array[0] = p0;
+	array[1] = p1;
+	array[2] = p2;
+
+	dgInt32 i0 = 2;
+	dgInt32 closestIndex = -1;
+	dgBigVector p1p0 (array[2] - point);
+	for (dgInt32 i1 = 0; i1 < 3; i1 ++) {
+		dgBigVector p2p0 (array[i1] - point);
+
+		dgFloat64 volume = (p1p0 * p2p0) % normal;
+		if (volume < dgFloat32 (0.0f)) {
+			dgBigVector segment (array[i1] - array[i0]);
+			dgBigVector poinP0 (point - array[i0]);
+			dgFloat64 den = segment % segment;
+			dgAssert (den > dgFloat32 (0.0f));
+			dgFloat64 num = poinP0 % segment;
+			if (num < dgFloat32 (0.0f)) {
+				closestIndex = i0;
+			} else if (num > den) {
+				closestIndex = i1;
+			} else {
+				return array[i0] + segment.Scale3 (num / den);
+			}
+		}
+		p1p0 = p2p0;
+		i0 = i1;
+	}
+
+	if (closestIndex >= 0) {
+		return array[closestIndex];
+	} else {
+		return point - normal.Scale3((normal % (point - p0)) / (normal % normal));
+	}
+}
+
+
+/*
+bool dgApi dgPointToPolygonDistance (const dgVector& p, const dgFloat32* const polygon, dgInt32 strideInBytes,
+									 const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 bailDistance, dgVector& out)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+
+	dgInt32 i0 = indexArray[0] * stride;
+	dgInt32 i1 = indexArray[1] * stride;
+
+	const dgVector v0 (&polygon[i0]);
+	dgVector v1 (&polygon[i1]);
+	dgVector closestPoint (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+	dgFloat32 minDist = dgFloat32 (1.0e20f);
+	for (dgInt32 i = 2; i < indexCount; i ++) {
+		dgInt32 i2 = indexArray[i] * stride;
+		const dgVector v2 (&polygon[i2]);
+		const dgVector q (dgPointToTriangleDistance (p, v0, v1, v2));
+		const dgVector error (q - p);
+		dgFloat32 dist = error % error;
+		if (dist < minDist) {
+			minDist = dist;
+			closestPoint = q;
+		}
+		v1 = v2;
+	}
+
+	if (minDist > (bailDistance * bailDistance)) {
+		return false;
+	}
+
+	out = closestPoint;
+	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.Scale3 (val0 / den) + B.Scale3 (val1 / den) + C.Scale3(val2 / den));
+	dgFloat64 volume = ((B - A) * (C - A)) % (testpoint - A);
+	dgAssert (fabs (volume) < dgFloat64 (1.0e-12f));
+#endif
+	return dgBigVector (val0 / den, val1 / den, val2 / den, dgFloat32 (0.0f));
+}
+*/
+
+
+
+
+
+// using my old method which clip the line again the voronoi regions of the polygon first, then find the 
+// shortest of the first time to impact of each segment to each respective voronoi region region. 
+// the method is much slower than Paul Nettle method but is is more reliably and accurate.
+// in fact I believe could never get Paul Nettle's method to work correctly in all cases, 
+// I start to have strong suspicion that Paul Nettle method has an analytical flaw and does not really extend to 3d.
+class dgSweepSphereToPolygon
+{
+	public: 
+	dgSweepSphereToPolygon (dgInt32 count, const dgVector* const polygon, const dgVector& normal)
+		:m_count(count) 
+		,m_polygon(polygon) 
+		,m_normal(normal) 
+	{
+		m_vertexIndex[0] = 0;
+		for (dgInt32 i = 0; i < count; i ++) {
+			m_prevVertex[i] = i - 1;
+			m_nextVertex[i] = i + 1;
+			m_edgeVoronoi[i] = i + 1;
+			m_vertexVoronoi[i] = count + i + 1;
+			m_vertexIndex[i + 1] = i;
+			m_vertexIndex[m_count + i + 1] = i;
+		}
+		m_prevVertex[0] = m_count - 1;
+		dgAssert ((m_count - 1) > 0);
+		m_nextVertex[m_count - 1] = 0;
+	}
+
+	dgFloat32 CalculateTimeOfImpact(const dgVector& p0, const dgVector& p1, dgFloat32 radius, dgVector& normalOut, dgVector& contactOut) 
+	{
+		dgBigVector q0(p0); 
+		dgBigVector q1(p1); 
+		dgBigVector diff (p1 - p0);
+
+		dgBigVector r0 (q0);
+		dgInt32 startRegion = GetRegionIndex (r0);
+		for (dgInt32 i = 0; i < 10; i ++) {
+			dgFloat64 clipParam;
+			dgInt32 exitRegion = GetExitRegion (startRegion, r0, q1, clipParam);
+			dgBigVector r1 (r0 + (q1 - r0).Scale3 (clipParam));
+			dgFloat64 param = CalculateInterstionParam(startRegion, r0, r1, radius, normalOut, contactOut);
+			if (param >= dgFloat32 (0.0f)) {
+				dgBigVector q2 (r0 + (r1 - r0).Scale3 (param));
+				return dgFloat32 (((q2 - q0) % diff) / (diff % diff));
+			}
+			if (startRegion == exitRegion) {
+				break;
+			}
+
+			r0 = r1;
+			startRegion = exitRegion;
+		}
+		return dgFloat32 (-1.0f);
+	} 
+
+	private:
+
+	void CalculateVertexPlanes (dgBigPlane& planeA, dgBigPlane& planeB, dgInt32 vertexIndex) const
+	{
+		dgBigVector v0 (m_polygon[vertexIndex]);
+
+		dgInt32 j = m_nextVertex[vertexIndex];
+		dgBigVector v1 (m_polygon[j]);
+		dgBigVector dir0 (v1 - v0);
+		dir0 = dir0.Scale3 (sqrt (dgFloat64(1.0f) / (dir0 % dir0)));
+		planeA = dgBigPlane (dir0, -(dir0 % v0));
+
+		j = m_prevVertex[vertexIndex];
+		dgBigVector v2 (m_polygon[j]);
+		dgBigVector dir2 (v2 - v0);
+		dir2 = dir2.Scale3 (sqrt (dgFloat64(1.0f) / (dir2 % dir2)));
+		planeB = dgBigPlane (dir2, -(dir2 % v0));
+	}
+
+
+	dgInt32 GetRegionIndex (const dgBigVector& point) const
+	{
+		dgInt32 voronoiRegion = 0;
+		dgBigVector v0 (m_polygon[m_count-1]);
+		dgVector pointInPlane (point - m_normal.Scale3 ((point - v0) % m_normal));
+
+		dgBigVector p0 (point);
+		dgBigVector p1 (pointInPlane);
+		dgBigVector diff (p1 - p0);
+		dgBigVector p0v0 (v0 - p0);
+
+		for (dgInt32 i = 0; i < m_count; i ++) {
+			dgBigVector v1 (m_polygon[i]);
+			dgBigVector p0v1 (v1 - p0);
+
+			// calculate the volume formed by the line and the edge of the polygon
+			dgFloat64 alpha = (diff * p0v1) % p0v0;
+			if (alpha < dgFloat32 (0.0f)) {
+
+				// calculate the voronoi regions for the edge
+				dgInt32 k = m_prevVertex[i];
+				dgBigPlane plane0;
+				dgBigPlane plane1;
+				dgBigPlane plane2;
+				dgBigPlane plane3;
+				CalculateVertexPlanes (plane0, plane1, k);
+				CalculateVertexPlanes (plane2, plane3, i);
+
+				if ((plane0.Evalue(p0) >= dgFloat32 (0.0f)) && (plane3.Evalue(p0) >= dgFloat32 (0.0f))) {
+					return m_edgeVoronoi[m_prevVertex[i]];
+				} else if ((plane0.Evalue(p0) <= dgFloat32 (0.0f)) && (plane1.Evalue(p0) <= dgFloat32 (0.0f))) {
+					return m_vertexVoronoi[m_prevVertex[i]];
+				} else {
+					dgAssert ((plane2.Evalue(p0) <= dgFloat32 (0.0f)) && (plane3.Evalue(p0) <= dgFloat32 (0.0f)));
+					return m_vertexVoronoi[i];
+				}
+			}
+
+			v0 = v1;
+			p0v0 = p0v1;
+		}
+		return voronoiRegion;
+	}
+
+
+	dgInt32 dgCalculateVoronoiIntersectionParam (dgInt32 region, const dgBigVector& p0, const dgBigVector& p1, dgInt32 planesCount, dgBigPlane* const planes, const dgInt32* const planeRegions, dgFloat64& param)
+	{
+		param = dgFloat32 (1.0f);
+		dgInt32 changedRegion = region; 
+		dgInt32 planeIndex = -1;
+		dgFloat64 minT = dgFloat64 (1.0f);
+		dgBigVector p1p0 (p1 - p0);
+		for (dgInt32 i = 0; i < planesCount; i ++) {
+			dgFloat64 den = planes[i] % p1p0;
+			if (fabs (den) > dgFloat64 (1.0e-20f)) {
+				dgFloat64 t = -planes[i].Evalue(p0) / den;
+				if (t > dgFloat32 (1.0e-10f)) {
+					if (t < minT) {
+						planeIndex = i;
+						changedRegion = planeRegions[i];
+						minT = t;
+					}
+				}
+			}
+		}
+		if (planeIndex >= 0) {
+			param = minT;
+		}
+		return changedRegion;
+	}
+
+
+	dgInt32 GetExitRegion (dgInt32 orginRegion, const dgBigVector& p0, const dgBigVector& p1, dgFloat64& param)
+	{
+		param = dgFloat32 (1.0f);
+		if (orginRegion == 0) {
+			// inside the plane
+			// the exit region is the line the point intersect if there is any
+
+			dgInt32 adjacentRegion[32];
+			dgBigPlane planes[32];
+
+			dgBigVector diff (p1 - p0);
+			// deal we special case that the line is parallel to the normal
+			dgFloat64 diff2 = diff % diff;
+			dgFloat64 parallelTest = (diff % m_normal);
+			if ((parallelTest * parallelTest) < (diff2 * dgFloat32 (0.9999f))) {
+				dgBigVector v0 (m_polygon[m_count-1]);
+				for (dgInt32 i = 0; i < m_count; i ++) {
+					dgBigVector v1 (m_polygon[i]);
+					dgBigVector edge (v1 - v0);
+					dgBigVector wallNormal (edge * m_normal);
+					wallNormal = wallNormal.Scale3 (sqrt (dgFloat64(1.0f) / (wallNormal % wallNormal)));
+					planes[i] =  dgBigPlane (wallNormal, - (wallNormal % v1));
+					adjacentRegion[i] = m_edgeVoronoi[m_prevVertex[i]];
+					v0 = v1;
+				}
+				return dgCalculateVoronoiIntersectionParam (orginRegion, p0, p1, m_count, planes, adjacentRegion, param);
+			}
+
+		} else if (orginRegion <= m_count) {
+			// edge region
+			dgInt32 i0 = m_vertexIndex[orginRegion];
+			dgInt32 i1 = m_nextVertex[i0];
+
+			dgInt32 adjacentRegion[3];
+			dgBigPlane planes[3];
+
+			dgBigVector q0 (m_polygon[i0]);
+			dgBigVector q1 (m_polygon[i1]);
+			dgBigVector dir (q0 - q1);
+			dir = dir * m_normal;
+			dir = dir.Scale3 (sqrt (dgFloat64(1.0f) / (dir % dir)));
+			planes[0] =  dgBigPlane (dir, - (dir % q0));
+			adjacentRegion[0] = 0;
+
+			dgBigPlane plane0;
+			CalculateVertexPlanes (planes[1], plane0, i0);
+			CalculateVertexPlanes (plane0, planes[2], i1);
+
+			adjacentRegion[1] = m_vertexVoronoi[i0];
+			adjacentRegion[2] = m_vertexVoronoi[i1];
+
+			planes[1] = planes[1].Scale(dgFloat32 (-1.0f));
+			planes[2] = planes[2].Scale(dgFloat32 (-1.0f));
+			return dgCalculateVoronoiIntersectionParam (orginRegion, p0, p1, 3, planes, adjacentRegion, param);
+		} else {
+			// point region
+			dgInt32 index = m_vertexIndex[orginRegion];
+			dgBigPlane planes[2];
+			dgInt32 adjacentRegion[2];
+			CalculateVertexPlanes (planes[1], planes[0], index);
+			adjacentRegion[0] = m_edgeVoronoi[m_prevVertex[index]];
+			adjacentRegion[1] = m_edgeVoronoi[index];
+			return dgCalculateVoronoiIntersectionParam (orginRegion, p0, p1, 2, planes, adjacentRegion, param);
+		}
+		return orginRegion;
+	}
+
+	bool dgQuadraticRoots (dgFloat64 a, dgFloat64 b, dgFloat64 c, dgFloat64& t0, dgFloat64& t1) const 
+	{
+		dgFloat64 desc = b * b - dgFloat64 (4.0f) * a * c;
+		if (desc > dgFloat64 (0.0f)) {
+			desc = sqrt (desc);
+			dgFloat64 den = dgFloat64 (0.5f) / a;
+			t0 = (-b + desc) * den;
+			t1 = (-b - desc) * den;
+			return true;
+		}
+		return false;
+	}
+
+
+	dgFloat64 CalculateInterstionParam (dgInt32 region, const dgBigVector& p0, const dgBigVector& p1, dgFloat32 radius, dgVector& normalOut, dgVector& contactOut) const 
+	{
+		if (region == 0) {
+			// inside the plane
+			dgBigVector diff (p1 - p0);
+			dgBigVector dist (p0 - dgBigVector(m_polygon[0]));
+			dgFloat64 num = radius - m_normal % dist;
+			dgFloat64 den = m_normal % diff;
+			dgFloat64 t = num / den;
+
+			if (t < dgFloat32 (0.0f)) {
+				dgBigVector r (diff.Scale3 (t));
+				if ((r % r) < radius * radius) {
+					t = dgFloat64 (0.0f);
+				}
+			}
+
+			if ((t < dgFloat32 (0.0f)) || (t > dgFloat32 (1.0f))) {
+				//t = dgFloat32 (0.0f);
+				t = dgFloat32 (-1.0f);
+			} else {
+				dgBigVector p (p0 + diff.Scale3 (t));
+				contactOut = dgVector (p - m_normal.Scale3 (m_normal % (p - dgBigVector(m_polygon[0]))));
+				normalOut = m_normal;
+			}
+			return t;
+			
+		} else if (region <= m_count) {
+			// edge region
+			dgInt32 i0 = m_vertexIndex [region];
+			dgInt32 i1 = m_nextVertex [i0];
+			
+			dgBigVector q0 (m_polygon[i0]);
+			dgBigVector q1 (m_polygon[i1]);
+
+			dgBigVector q1q0 (q1 - q0);
+			dgBigVector p0q0 (p0 - q0);
+			dgBigVector p1p0 (p1 - p0);
+			dgFloat64 den = dgFloat64 (1.0f) / (q1q0 % q1q0);
+
+			dgBigVector B (q1q0.Scale3 ((p0q0 % q1q0) * den));
+			dgBigVector C (q1q0.Scale3 ((p1p0 % q1q0) * den));
+			
+			dgBigVector r0 (p0q0 - B);
+			dgBigVector r1 (p1p0 - C);
+
+			dgFloat64 a = r1 % r1;
+			dgFloat64 b = dgFloat64 (2.0f) * (r0 % r1);
+			dgFloat64 c = r0 % r0 - radius * radius;
+
+			dgFloat64 t0 = dgFloat64 (-1.0f);
+			dgFloat64 t1;
+			if (dgQuadraticRoots (a, b, c, t0, t1)) {
+				t0 = dgMin(t0, t1);
+				if (t0 < dgFloat32 (0.0f)) {
+					dgBigVector r ((p1 - p0).Scale3 (t0));
+					if ((r % r) < radius * radius) {
+						t0 = dgFloat64 (0.0f);
+					}
+				}
+
+				if ((t0 < dgFloat32 (0.0f)) || (t0 > dgFloat32 (1.0f))) {
+					t0 = dgFloat32 (-1.0f);
+				} else {
+					dgBigVector p (p0 + p1p0.Scale3 (t0));
+					dgBigVector q (q0 + B + C.Scale3 (t0));
+					
+					contactOut = q;
+					normalOut = dgVector (p - q);
+					normalOut = normalOut.Scale3 (dgRsqrt(normalOut % normalOut));
+				}
+			}
+			return t0;
+
+
+		} else {
+			// point region
+			dgInt32 index = m_vertexIndex [region];
+			dgBigVector q (m_polygon[index]);
+
+			dgBigVector p1p0 (p1 - p0);
+			dgBigVector p0q (p0 - q);
+			dgFloat64 a = p1p0 % p1p0;
+			dgFloat64 b = dgFloat64 (2.0f) * (p1p0 % p0q);
+			dgFloat64 c = p0q % p0q - radius * radius;
+			dgFloat64 t0 = dgFloat64 (-1.0f);
+			dgFloat64 t1;
+			if (dgQuadraticRoots (a, b, c, t0, t1)) {
+				t0 = dgMin(t0, t1);
+
+				if (t0 < dgFloat32 (0.0f)) {
+					dgBigVector r ((p1 - p0).Scale3 (t0));
+					if ((r % r) < radius * radius) {
+						t0 = dgFloat64 (0.0f);
+					}
+				}
+
+				if ((t0 < dgFloat32 (0.0f)) || (t0 > dgFloat32 (1.0f))) {
+					t0 = dgFloat32 (-1.0f);
+				} else {
+					contactOut = q;
+					normalOut = dgVector (p0 + p1p0.Scale3 (t0) - q);
+					normalOut = normalOut.Scale3 (dgRsqrt (normalOut % normalOut));
+				}
+			}
+			return t0;
+		}
+	}
+
+	dgFloat32 m_radius;
+	dgInt32 m_count; 
+	const dgVector* m_polygon; 
+	dgBigVector m_normal; 
+
+	dgInt32 m_prevVertex[32];
+	dgInt32 m_nextVertex[32];
+	dgInt32 m_edgeVoronoi[32];
+	dgInt32 m_vertexVoronoi[32];
+	dgInt32 m_vertexIndex[64];
+};
+
+
+dgFloat32 dgSweepLineToPolygonTimeOfImpact (const dgVector& p0, const dgVector& p1, dgFloat32 radius, dgInt32 count, const dgVector* const polygon, const dgVector& normal, dgVector& normalOut, dgVector& contactOut)
+{
+	dgVector diff (p1 - p0);
+	dgFloat32 projectVeloc = normal % diff;
+	if (projectVeloc >= dgFloat32 (0.0f)) {
+		return dgFloat32 (-1.0f);
+	}
+
+	dgFloat32 planeSide = (p0 - polygon[0]) % normal;
+	if ((planeSide) <= dgFloat32 (0.0f)) {
+		return dgFloat32 (-1.0f);
+	}
+
+	dgSweepSphereToPolygon sweepSphere (count, polygon, normal);
+	return sweepSphere.CalculateTimeOfImpact(p0, p1, radius, normalOut, contactOut);
+}
+
+
+dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius)
+{
+	dgVector p0Origin (p0 - origin);
+
+	if ((p0Origin % p0Origin) < (100.0f * radius * radius)) {
+		dgVector dp (p1 - p0);
+		dgFloat32 a = dp % dp;
+		dgFloat32 b = dgFloat32 (2.0f) * (p0Origin % dp);
+		dgFloat32 c = p0Origin % 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 % dp;
+		dgFloat64 b = dgFloat32 (2.0f) * (p0Origin1 % dp);
+		dgFloat64 c = p0Origin1 % p0Origin1 - dgFloat64(radius) * radius;
+		dgFloat64 desc = b * b - dgFloat32 (4.0f) * a * c;
+		if (desc >= 0.0f) {
+			desc = sqrt (desc);
+			dgFloat64 den = dgFloat32 (0.5f) / a;
+			dgFloat64 t0 = (-b + desc) * den;
+			dgFloat64 t1 = (-b - desc) * den;
+			if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
+				t0 =  dgMin(t0, t1);
+				if (t0 <= dgFloat32 (1.0f)) {
+					return dgFloat32 (t0);
+				}
+			} else if (t0 >= dgFloat32 (0.0f)) {
+				if (t0 <= dgFloat32 (1.0f)) {
+					return dgFloat32 (t0);
+				}
+			} else {
+				if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
+					return dgFloat32 (t1);
+				}
+			}
+		}
+	}
+	return dgFloat32 (1.2f);
+}
+
+dgFloat32 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 (dgAbsf (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;
+
+}
+

newtondynamics.mod/NewtonDynamics/source/core/dgIntersections.h

@@ -0,0 +1,383 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgIntersections__
+#define __dgIntersections__
+
+#include "dgStdafx.h"
+#include "dgObb.h"
+#include "dgDebug.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+
+class dgPlane;
+class dgObject;
+class dgPolyhedra;
+
+enum dgIntersectStatus
+{
+	t_StopSearh,
+	t_ContinueSearh
+};
+
+typedef dgIntersectStatus (*dgAABBIntersectCallback) (void* const context, 
+													  const dgFloat32* const polygon, dgInt32 strideInBytes,
+													  const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance);
+
+typedef dgFloat32 (*dgRayIntersectCallback) (void* const context, 
+											 const dgFloat32* const polygon, dgInt32 strideInBytes,
+											 const dgInt32* const indexArray, dgInt32 indexCount);
+
+
+
+DG_MSC_VECTOR_ALIGMENT 
+class dgFastRayTest
+{
+	public:
+	DG_INLINE dgFastRayTest(const dgVector& l0, const dgVector& l1)
+		:m_p0 (l0)
+		,m_p1(l1)
+		,m_diff ((l1 - l0) | dgVector::m_wOne)
+		,m_minT(dgFloat32 (0.0f))  
+		,m_maxT(dgFloat32 (1.0f))
+		,m_zero(dgFloat32 (0.0f)) 
+	{
+		dgAssert (m_p0.m_w == dgFloat32 (0.0f));
+		dgAssert (m_p1.m_w == dgFloat32 (0.0f));
+		dgAssert (m_diff.m_w == dgFloat32 (1.0f));
+
+		m_isParallel = (m_diff.Abs() < dgVector (1.0e-8f));
+
+		m_dpInv = (((dgVector (dgFloat32 (1.0e-20)) & m_isParallel) | m_diff.AndNot(m_isParallel)).Reciproc ()) & dgVector::m_triplexMask;
+		m_dpBaseInv = m_dpInv;
+
+		dgFloat32 mag = dgSqrt (m_diff % m_diff);
+		m_dirError = -dgFloat32 (0.0175f) * mag;
+		m_magRayTest = dgMax (mag, dgFloat32 (1.0f));
+	}
+	
+
+	dgFloat32 PolygonIntersect (const dgVector& normal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const;
+	dgFloat32 PolygonIntersectFallback (const dgVector& normal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const;
+
+	DG_INLINE dgInt32 BoxTest (const dgVector& minBox, const dgVector& maxBox) const
+	{
+		#if 1
+			dgVector test (((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
+			if (test.GetSignMask() & 0x07) {
+				return 0;
+			}
+			dgVector tt0 ((minBox - m_p0).CompProduct4(m_dpInv));
+			dgVector tt1 ((maxBox - m_p0).CompProduct4(m_dpInv));
+			dgVector t0 (m_minT.GetMax(tt0.GetMin(tt1)));
+			dgVector t1 (m_maxT.GetMin(tt0.GetMax(tt1)));
+			t0 = t0.GetMax(t0.ShiftTripleRight());
+			t1 = t1.GetMin(t1.ShiftTripleRight());
+			t0 = t0.GetMax(t0.ShiftTripleRight());
+			t1 = t1.GetMin(t1.ShiftTripleRight());
+			return ((t0 < t1).GetSignMask() & 1);
+
+		#else
+
+			dgFloat32 tmin = 0.0f;          
+			dgFloat32 tmax = 1.0f;
+
+			for (dgInt32 i = 0; i < 3; i++) {
+				if (m_isParallel[i]) {
+					if (m_p0[i] <= minBox[i] || m_p0[i] >= maxBox[i]) {
+						return 0;
+					}
+				} else {
+					dgFloat32 t1 = (minBox[i] - m_p0[i]) * m_dpInv[i];
+					dgFloat32 t2 = (maxBox[i] - m_p0[i]) * m_dpInv[i];
+
+					if (t1 > t2) {
+						dgSwap(t1, t2);
+					}
+					if (t1 > tmin) {
+						tmin = t1;
+					}
+					if (t2 < tmax) {
+						tmax = t2;
+					}
+					if (tmin > tmax) {
+						return 0;
+					}
+				}
+			}
+			return 0x1;
+		#endif
+	}
+
+	DG_INLINE dgFloat32 BoxIntersect (const dgVector& minBox, const dgVector& maxBox) const
+	{
+		dgVector test (((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
+		if (test.GetSignMask() & 0x07) {
+			return dgFloat32 (1.2f);
+		}
+		dgVector tt0 ((minBox - m_p0).CompProduct4(m_dpInv));
+		dgVector tt1 ((maxBox - m_p0).CompProduct4(m_dpInv));
+		dgVector t0 (m_minT.GetMax(tt0.GetMin(tt1)));
+		dgVector t1 (m_maxT.GetMin(tt0.GetMax(tt1)));
+		t0 = t0.GetMax(t0.ShiftTripleRight());
+		t1 = t1.GetMin(t1.ShiftTripleRight());
+		t0 = t0.GetMax(t0.ShiftTripleRight());
+		t1 = t1.GetMin(t1.ShiftTripleRight());
+		dgVector mask (t0 < t1);
+		dgVector maxDist (dgFloat32 (1.2f));
+		t0 = (t0 & mask) | maxDist.AndNot(mask);
+		dgAssert ((mask.GetSignMask() & 1) == (t0.m_x < dgFloat32 (1.0f)));
+		return t0.GetScalar();
+	}
+
+
+	dgVector m_p0;
+	dgVector m_p1;
+	dgVector m_diff;
+	dgVector m_dpInv;
+	dgVector m_dpBaseInv;
+	dgVector m_minT;
+	dgVector m_maxT;
+	dgVector m_zero;
+	dgVector m_isParallel;
+	dgFloat32 m_dirError;
+	dgFloat32 m_magRayTest;
+} DG_GCC_VECTOR_ALIGMENT;
+
+
+
+bool dgRayBoxClip (dgVector& ray_p0, dgVector& ray_p1, const dgVector& boxP0, const dgVector& boxP1); 
+dgVector dgPointToRayDistance (const dgVector& point, const dgVector& ray_p0, const dgVector& ray_p1); 
+void dgRayToRayDistance (const dgVector& ray_p0, const dgVector& ray_p1, const dgVector& ray_q0, const dgVector& ray_q1, dgVector& p0Out, dgVector& p1Out); 
+dgVector dgPointToTriangleDistance (const dgVector& point, const dgVector& p0, const dgVector& p1, const dgVector& p2, const dgVector& normal);
+dgBigVector dgPointToTriangleDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& normal);
+dgFloat32 dgSweepLineToPolygonTimeOfImpact (const dgVector& p0, const dgVector& p1, dgFloat32 radius, dgInt32 count, const dgVector* const polygon, const dgVector& normal, dgVector& normalOut, dgVector& contactOut);
+dgFloat32 dgRayCastBox (const dgVector& p0, const dgVector& p1, const dgVector& boxP0, const dgVector& boxP1, dgVector& normalOut);
+dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius);
+
+DG_INLINE dgInt32 dgOverlapTest (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
+{
+//	dgInt32 test = ((p0.m_x < q1.m_x) && (p1.m_x > q0.m_x) && (p0.m_z < q1.m_z) && (p1.m_z > q0.m_z) && (p0.m_y < q1.m_y) && (p1.m_y > q0.m_y));
+//	return  test
+	dgVector val ((p0 < q1) & (p1 > q0));
+	dgInt32 mask = val.GetSignMask();
+	return ((mask & 0x07) == 0x07);
+}
+
+
+DG_INLINE dgInt32 dgBoxInclusionTest (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
+{
+//	dgInt32 test = (p0.m_x >= q0.m_x) && (p0.m_y >= q0.m_y) && (p0.m_z >= q0.m_z) && (p1.m_x <= q1.m_x) && (p1.m_y <= q1.m_y) && (p1.m_z <= q1.m_z);
+//	return test;
+
+	dgVector val ((p0 >= q0) & (p1 <= q1));
+	dgInt32 mask = val.GetSignMask();
+	return ((mask & 0x07) == 0x07);
+}
+
+
+DG_INLINE dgInt32 dgCompareBox (const dgVector& p0, const dgVector& p1, const dgVector& q0, const dgVector& q1)
+{
+	dgAssert(0);
+	return (p0.m_x != q0.m_x) || (p0.m_y != q0.m_y) || (p0.m_z != q0.m_z) || (p1.m_x != q1.m_x) || (p1.m_y != q1.m_y) || (p1.m_z != q1.m_z);
+}
+
+
+DG_INLINE void dgMovingAABB (dgVector& p0, dgVector& p1, const dgVector& veloc, const dgVector& omega, dgFloat32 timestep, dgFloat32 maxRadius, dgFloat32 minRadius)
+{
+	dgVector linearStep (veloc.Scale3 (timestep));
+
+	// estimate the maximum effect of the angular velocity and enlarge that box by that value (use 45 degrees as max angle not 90)
+	dgFloat32 maxAngle = dgMin (dgSqrt ((omega % omega) * timestep * timestep), dgFloat32 (45.0f * 3.14159f / 180.0f));
+
+	dgFloat32 angularTravel = (maxRadius - minRadius) * maxAngle;
+	dgVector angularStep (angularTravel, angularTravel, angularTravel, dgFloat32 (0.0f));
+	
+	dgVector r0 (p0 - angularStep);
+	dgVector r1 (p1 + angularStep);
+	dgVector q0 (r0 + linearStep);
+	dgVector q1 (r1 + linearStep);
+	//p0 = dgVector (dgMin (r0.m_x, q0.m_x), dgMin (r0.m_y, q0.m_y), dgMin (r0.m_z, q0.m_z), dgFloat32 (0.0f));
+	p0 = r0.GetMin (q0);
+	//p1 = dgVector (dgMax (r1.m_x, q1.m_x), dgMax (r1.m_y, q1.m_y), dgMax (r1.m_z, q1.m_z), dgFloat32 (0.0f));
+	p1 = r1.GetMax (q1);
+	p0.m_w = dgFloat32 (0.0f);
+	p1.m_w = dgFloat32 (0.0f);
+}
+
+
+DG_INLINE dgFloat32 BoxPenetration (const dgVector& minBox, const dgVector& maxBox)
+{
+	dgVector mask ((minBox.CompProduct4(maxBox)) < dgVector (dgFloat32 (0.0f)));
+	dgVector dist (maxBox.GetMin (minBox.Abs()) & mask);
+	dist = dist.GetMin(dist.ShiftTripleRight());
+	dist = dist.GetMin(dist.ShiftTripleRight());
+	return dist.GetScalar();
+}
+
+DG_INLINE dgFloat32 dgBoxDistanceToOrigin2 (const dgVector& minBox, const dgVector& maxBox)
+{
+	dgVector mask ((minBox.CompProduct4(maxBox)) > dgVector (dgFloat32 (0.0f)));
+	dgVector dist (maxBox.Abs().GetMin (minBox.Abs()) & mask);
+	return dist.DotProduct4(dist).GetScalar();
+}
+
+
+DG_MSC_VECTOR_ALIGMENT 
+class dgFastAABBInfo: public dgObb
+{
+	public:
+	DG_INLINE dgFastAABBInfo()
+		:dgObb()
+		,m_absDir(dgGetIdentityMatrix())
+	{
+	}
+
+	DG_INLINE dgFastAABBInfo(const dgMatrix& matrix, const dgVector& size)
+		:dgObb(matrix, size)
+	{
+		SetTransposeAbsMatrix (matrix);
+		dgVector size1 (matrix[0].Abs().Scale4(size.m_x) + matrix[1].Abs().Scale4(size.m_y) + matrix[2].Abs().Scale4(size.m_z));
+		m_p0 = (matrix[3] - size1) & dgVector::m_triplexMask;
+		m_p1 = (matrix[3] + size1) & dgVector::m_triplexMask;
+	}
+
+	DG_INLINE dgFastAABBInfo(const dgVector& p0, const dgVector& p1)
+		:dgObb(dgGetIdentityMatrix(), (p1 - p0).CompProduct4(dgVector::m_half))
+		,m_absDir(dgGetIdentityMatrix())
+		,m_p0(p0)
+		,m_p1(p1)
+	{
+		m_posit = ((p1 + p0).CompProduct4(dgVector::m_half) & dgVector::m_triplexMask) | dgVector::m_wOne;
+	}
+
+	DG_INLINE void SetTransposeAbsMatrix (const dgMatrix& matrix)
+	{
+		m_absDir = matrix.Transpose();
+		m_absDir[0] = m_absDir[0].Abs();
+		m_absDir[1] = m_absDir[1].Abs();
+		m_absDir[2] = m_absDir[2].Abs();
+		m_absDir[3] = dgVector::m_wOne;
+	}
+
+	DG_INLINE dgFloat32 PolygonBoxRayDistance (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, const dgFastRayTest& ray) const
+	{
+		dgVector minBox;
+		dgVector maxBox;
+		MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+		dgFloat32 dist0 = ray.BoxIntersect(minBox, maxBox);
+		if (dist0 < dgFloat32 (1.0f)) {
+			dgMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
+
+			MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+			dgVector veloc (faceMatrix.RotateVector(ray.m_diff) & dgVector::m_triplexMask);
+			dgFastRayTest localRay (dgVector (dgFloat32 (0.0f)), veloc);
+			dgFloat32 dist1 = localRay.BoxIntersect(minBox, maxBox);
+			dist0 = dgMax (dist1, dist0);
+		}
+		return dist0;
+	}
+
+
+	DG_INLINE dgFloat32 PolygonBoxDistance (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray) const
+	{
+		dgVector minBox;
+		dgVector maxBox;
+		MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+		dgFloat32 dist0 = BoxPenetration (minBox, maxBox);
+		if (dist0 > dgFloat32 (0.0f)) {
+			dgMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
+			MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
+			dgFloat32 dist1 = BoxPenetration (minBox, maxBox);
+			dist0 = (dist1 > dgFloat32 (0.0f)) ? dgMax (dist0, dist1) : dgFloat32 (0.0f);
+		}
+		return dist0;
+	}
+
+	private:
+	DG_INLINE void MakeBox1 (dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, dgVector& minBox, dgVector& maxBox) const
+	{
+		//const dgMatrix& matrix = *this;
+		//dgVector faceBoxP0 (m_scale.CompProduct4(matrix.UntransformVector(dgVector(&vertexArray[indexArray[0] * stride]))));
+		//dgVector faceBoxP0 (m_scale.CompProduct4(dgVector(&vertexArray[indexArray[0] * stride])));
+		dgVector faceBoxP0 (&vertexArray[indexArray[0] * stride]);
+		dgVector faceBoxP1 (faceBoxP0);
+		for (dgInt32 i = 1; i < indexCount; i ++) {
+			//dgVector p (m_scale.CompProduct4 (matrix.UntransformVector(dgVector(&vertexArray[indexArray[i] * stride]))));
+			//dgVector p (m_scale.CompProduct4 (dgVector(&vertexArray[indexArray[i] * stride])));
+			dgVector p (&vertexArray[indexArray[i] * stride]);
+			faceBoxP0 = faceBoxP0.GetMin(p); 
+			faceBoxP1 = faceBoxP1.GetMax(p); 
+		}
+		//minBox = faceBoxP0 - m_size;
+		//maxBox = faceBoxP1 + m_size;
+		minBox = faceBoxP0 - m_p1;
+		maxBox = faceBoxP1 - m_p0;
+	}
+
+	DG_INLINE void MakeBox2 (const dgMatrix& faceMatrix, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray, dgVector& minBox, dgVector& maxBox) const
+	{
+		dgVector faceBoxP0 (faceMatrix.TransformVector (dgVector (&vertexArray[indexArray[0] * stride])));
+		dgVector faceBoxP1 (faceBoxP0);
+		for (dgInt32 i = 1; i < indexCount; i ++) {
+			dgVector p (faceMatrix.TransformVector (dgVector (&vertexArray[indexArray[i] * stride])));
+			faceBoxP0 = faceBoxP0.GetMin(p); 
+			faceBoxP1 = faceBoxP1.GetMax(p); 
+		}
+		faceBoxP0 = faceBoxP0 & dgVector::m_triplexMask;
+		faceBoxP1 = faceBoxP1 & dgVector::m_triplexMask;
+
+		dgMatrix matrix = *this * faceMatrix;
+		dgVector size (matrix[0].Abs().Scale4(m_size.m_x) + matrix[1].Abs().Scale4(m_size.m_y) + matrix[2].Abs().Scale4(m_size.m_z));
+		dgVector boxP0 ((matrix.m_posit - size) & dgVector::m_triplexMask);
+		dgVector boxP1 ((matrix.m_posit + size) & dgVector::m_triplexMask);
+
+		minBox = faceBoxP0 - boxP1;
+		maxBox = faceBoxP1 - boxP0;
+	}
+
+
+	DG_INLINE dgMatrix MakeFaceMatrix (const dgVector& faceNormal, dgInt32 indexCount, const dgInt32* const indexArray, dgInt32 stride, const dgFloat32* const vertexArray) const
+	{
+		dgMatrix faceMatrix;
+		dgVector origin (&vertexArray[indexArray[0] * stride]);
+
+		faceMatrix[0] = faceNormal;
+		faceMatrix[1] = dgVector (&vertexArray[indexArray[1] * stride]) - origin;
+		faceMatrix[1] = faceMatrix[1].CompProduct4 (faceMatrix[1].DotProduct4(faceMatrix[1]).InvSqrt());
+		faceMatrix[2] = faceMatrix[0] * faceMatrix[1];
+		faceMatrix[3] = origin | dgVector::m_wOne; 
+		return faceMatrix.Inverse();
+	}
+
+	protected:
+	dgMatrix m_absDir;
+	dgVector m_p0;
+	dgVector m_p1;
+
+	friend class dgAABBPolygonSoup;
+	friend class dgCollisionUserMesh;
+	friend class dgCollisionHeightField;
+} DG_GCC_VECTOR_ALIGMENT;
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgList.h

@@ -0,0 +1,680 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgList__
+#define __dgList__
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+
+
+template<class T>
+class dgList 
+{
+	public:
+	class dgListNode
+	{
+		DG_CLASS_ALLOCATOR(allocator)
+
+		dgListNode (dgListNode* const prev, dgListNode* const next) 
+			:m_info () 
+		{
+//			dgAssert ((dgUnsigned64 (&m_info) & 0x0f) == 0);
+			m_prev = prev;
+			m_next = next;
+			if (m_prev) {
+				m_prev->m_next = this;
+			}
+			if (m_next) {
+				m_next->m_prev = this;
+			}
+		}
+
+		dgListNode (const T &info, dgListNode* const prev, dgListNode* const next) 
+			:m_info (info) 
+		{
+			m_prev = prev;
+			m_next = next;
+			if (m_prev) {
+				m_prev->m_next = this;
+			}
+			if (m_next) {
+				m_next->m_prev = this;
+			}
+		}
+
+		virtual ~dgListNode()
+		{
+		}
+
+		void Unlink ()
+		{
+			if (m_prev) {
+				m_prev->m_next = m_next;
+			}
+
+			if (m_next) {
+				m_next->m_prev = m_prev;
+			}
+			m_prev = NULL;
+			m_next = NULL;
+		}
+
+		void AddLast(dgListNode* const node) 
+		{
+			m_next = node;
+			node->m_prev = this;
+		}
+
+		void AddFirst(dgListNode* const node) 
+		{
+			m_prev = node;
+			node->m_next = this;
+		}
+
+		public:
+		T& GetInfo()
+		{
+			return m_info;
+		}
+
+		dgListNode *GetNext() const
+		{
+			return m_next;
+		}
+
+		dgListNode *GetPrev() const
+		{
+			return m_prev;
+		}
+
+		private:
+		T m_info;
+		dgListNode *m_next;
+		dgListNode *m_prev;
+		friend class dgList<T>;
+
+	};
+
+	class Iterator
+	{
+		public:
+		Iterator (const dgList<T> &me)
+		{
+			m_ptr = NULL;
+			m_list = (dgList *)&me;
+		}
+
+		~Iterator ()
+		{
+		}
+
+		operator dgInt32() const
+		{
+			return m_ptr != NULL;
+		}
+
+		bool operator== (const Iterator &target) const
+		{
+			return (m_ptr == target.m_ptr) && (m_list == target.m_list);
+		}
+
+		void Begin()
+		{
+			m_ptr = m_list->GetFirst();
+		}
+
+		void End()
+		{
+			m_ptr = m_list->GetLast();
+		}
+
+		void Set (dgListNode* const node)
+		{
+			m_ptr = node;
+		}
+
+		void operator++ ()
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->m_next();
+		}
+
+		void operator++ (dgInt32)
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->GetNext();
+		}
+
+		void operator-- () 
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->GetPrev();
+		}
+
+		void operator-- (dgInt32) 
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->GetPrev();
+		}
+
+		T &operator* () const
+		{
+			return m_ptr->GetInfo();
+		}
+
+		dgListNode *GetNode() const
+		{
+			return m_ptr;
+		}
+
+		private:
+		dgList *m_list;
+		dgListNode *m_ptr;
+	};
+
+	// ***********************************************************
+	// member functions
+	// ***********************************************************
+	public:
+	DG_CLASS_ALLOCATOR(allocator)
+
+//	dgList ();
+	dgList (dgMemoryAllocator* const allocator);
+	virtual ~dgList ();
+
+	dgMemoryAllocator* GetAllocator () const;
+	void SetAllocator (dgMemoryAllocator* const allocator);
+
+	operator dgInt32() const;
+	dgInt32 GetCount() const;
+	dgListNode *GetLast() const;
+	dgListNode *GetFirst() const;
+	dgListNode *Append ();
+	dgListNode *Append (dgListNode* const node);
+	dgListNode *Append (const T &element);
+	dgListNode *Addtop ();
+	dgListNode *Addtop (dgListNode* const node);
+	dgListNode *Addtop (const T &element);
+	
+	void RotateToEnd (dgListNode* const node);
+	void RotateToBegin (dgListNode* const node);
+	void InsertAfter (dgListNode* const root, dgListNode* const node);
+	void InsertBefore (dgListNode* const root, dgListNode* const node);
+
+
+	dgListNode *Find (const T &element) const;
+	dgListNode *GetNodeFromInfo (T &m_info) const;
+	void Remove (dgListNode* const node);
+	void Remove (const T &element);
+	void RemoveAll ();
+
+	void Merge (dgList<T>& list);
+	void Unlink (dgListNode* const node);
+	bool SanityCheck () const;
+
+
+	// ***********************************************************
+	// member variables
+	// ***********************************************************
+	private:
+	dgInt32 m_count;
+	dgListNode *m_first;
+	dgListNode *m_last;
+	dgMemoryAllocator* m_allocator;
+
+//	static dgInt32 m_size;
+//	static dgMemoryAllocator* m_staticAllocator;
+	friend class dgListNode;
+};
+
+/*
+template<class T>
+dgList<T>::dgList ()
+{
+	m_count = 0;
+	m_first = NULL;
+	m_last = NULL;
+	m_allocator = NULL;
+}
+*/
+
+template<class T>
+dgList<T>::dgList (dgMemoryAllocator* const allocator)
+	:m_count(0)
+	,m_first(NULL)
+	,m_last(NULL)
+	,m_allocator(allocator)
+{
+}
+
+
+template<class T>
+dgList<T>::~dgList () 
+{
+	RemoveAll ();
+}
+
+template<class T>
+void dgList<T>::SetAllocator (dgMemoryAllocator* const allocator)
+{
+	if ((m_count == 0) && (m_allocator == NULL)) {
+		m_allocator = allocator;
+	}
+}
+
+template<class T>
+dgMemoryAllocator* dgList<T>::GetAllocator () const
+{
+	return m_allocator;
+}
+
+
+template<class T>
+dgInt32 dgList<T>::GetCount() const
+{
+	return m_count;
+}
+
+template<class T>
+dgList<T>::operator dgInt32() const
+{
+	return m_first != NULL;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::GetFirst() const
+{
+	return m_first;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::GetLast() const
+{
+	return m_last;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Append (dgListNode* const node)
+{
+	dgAssert (node->m_next == NULL);
+	dgAssert (node->m_prev == NULL);
+	m_count	++;
+	if (m_first == NULL) {
+		m_last = node;
+		m_first = node;
+	} else {
+		m_last->AddLast (node);
+		m_last = node;
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_last;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Append ()
+{
+	m_count	++;
+	if (m_first == NULL) {
+		m_first = new (m_allocator) dgListNode(NULL, NULL);
+		m_last = m_first;
+	} else {
+		m_last = new (m_allocator) dgListNode(m_last, NULL);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_last;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Append (const T &element)
+{
+	m_count	++;
+	if (m_first == NULL) {
+		m_first = new (m_allocator) dgListNode(element, NULL, NULL);
+		m_last = m_first;
+	} else {
+		m_last = new (m_allocator) dgListNode(element, m_last, NULL);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+
+	return m_last;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Addtop (dgListNode* const node)
+{
+	dgAssert (node->m_next == NULL);
+	dgAssert (node->m_prev == NULL);
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = node;
+		m_first = node;
+	} else {
+		m_first->AddFirst(node);
+		m_first = node;
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_first;
+}
+
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Addtop ()
+{
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = new (m_allocator) dgListNode(NULL, NULL);
+		m_first = m_last;
+	} else {
+		m_first = new (m_allocator) dgListNode(NULL, m_first);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_first;
+}
+
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Addtop (const T &element)
+{
+	m_count	++;
+	if (m_last == NULL) {
+		m_last = new (m_allocator) dgListNode(element, NULL, NULL);
+		m_first = m_last;
+	} else {
+		m_first = new (m_allocator) dgListNode(element, NULL, m_first);
+	}
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	return m_first;
+}
+
+template<class T>
+void dgList<T>::InsertAfter (dgListNode* const root, dgListNode* const node)
+{
+	dgAssert (root);
+	dgAssert (node != root);
+	
+	if (root->m_next != node) {
+		if (node == m_first) {
+			m_first = node->m_next;
+		}
+		if (node == m_last) {
+			m_last = node->m_prev;
+		}
+		node->Unlink ();
+		
+		node->m_prev = root;
+		node->m_next = root->m_next;
+		if (root->m_next) {
+			root->m_next->m_prev = node;
+		} 
+		root->m_next = node;
+
+		if (node->m_next == NULL) {
+			m_last = node;
+		}
+
+		dgAssert (m_last);
+		dgAssert (!m_last->m_next);
+		dgAssert (m_first);
+		dgAssert (!m_first->m_prev);
+		dgAssert (SanityCheck ());
+	}
+}
+
+
+template<class T>
+void dgList<T>::InsertBefore (dgListNode* const root, dgListNode* const node)
+{
+	dgAssert (root);
+	dgAssert (node != root);
+	
+	if (root->m_prev != node) {
+		if (node == m_last) {
+			m_last = node->m_prev;
+		}
+		if (node == m_first) {
+			m_first = node->m_next;
+		}
+		node->Unlink ();
+		
+		node->m_next = root;
+		node->m_prev = root->m_prev;
+		if (root->m_prev) {
+			root->m_prev->m_next = node;
+		} 
+		root->m_prev = node;
+
+		if (node->m_prev == NULL) {
+			m_first = node;
+		}
+
+		dgAssert (m_first);
+		dgAssert (!m_first->m_prev);
+		dgAssert (m_last);
+		dgAssert (!m_last->m_next);
+		dgAssert (SanityCheck ());
+	}
+}
+
+
+template<class T>
+void dgList<T>::RotateToEnd (dgListNode* const node)
+{
+	if (node != m_last) {
+		if (m_last != m_first) {
+			if (node == m_first) {
+				m_first = m_first->GetNext();
+			}
+			node->Unlink();
+			m_last->AddLast(node);
+			m_last = node; 
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+template<class T>
+void dgList<T>::RotateToBegin (dgListNode* const node)
+{
+	if (node != m_first) {
+		if (m_last != m_first) {
+			if (node == m_last) {
+				m_last = m_last->GetPrev();
+			}
+			node->Unlink();
+			m_first->AddFirst(node);
+			m_first = node; 
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::Find (const T &element) const
+{
+	dgListNode *node;
+	for (node = m_first; node; node = node->GetNext()) {
+		if (element	== node->m_info) {
+			break;
+		}
+	}
+	return node;
+}
+
+template<class T>
+typename dgList<T>::dgListNode *dgList<T>::GetNodeFromInfo (T &info) const
+{
+	dgListNode* const node = (dgListNode *) &info;
+	dgInt64 offset = ((char*) &node->m_info) - ((char *) node);
+	dgListNode* const retnode = (dgListNode *) (((char *) node) - offset);
+
+	dgAssert (&retnode->GetInfo () == &info);
+	return retnode;
+}
+
+
+template<class T> 
+void dgList<T>::Remove (const T &element)
+{
+	dgListNode *const node = Find (element);
+	if (node) {
+		Remove (node);
+	}
+}
+
+template<class T> 
+void dgList<T>::Unlink (dgListNode* const node)
+{
+	dgAssert (node);
+
+	m_count --;
+	dgAssert (m_count >= 0);
+
+	if (node == m_first) {
+		m_first = m_first->GetNext();
+	}
+	if (node == m_last) {
+		m_last = m_last->GetPrev();
+	}
+	node->Unlink();
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+template<class T> 
+void dgList<T>::Merge (dgList<T>& list)
+{
+	m_count += list.m_count;
+	if (list.m_first) {
+		list.m_first->m_prev = m_last; 
+	}
+	if (m_last) {
+		m_last->m_next = list.m_first;
+	}
+	m_last = list.m_last;
+	if (!m_first) {
+		m_first = list.m_first;
+	}
+
+	list.m_count = 0;
+	list.m_last = NULL;
+	list.m_first = NULL;
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+
+template<class T>
+void dgList<T>::Remove (dgListNode* const node)
+{
+	Unlink (node);
+	delete node;
+}
+
+
+template<class T>
+void dgList<T>::RemoveAll ()
+{
+	for (dgListNode *node = m_first; node; node = m_first) {
+		m_count --;
+		m_first = node->GetNext();
+		node->Unlink();
+		delete node;
+	}
+	dgAssert (m_count == 0);
+	m_last = NULL;
+	m_first = NULL;
+}
+
+template<class T>
+bool dgList<T>::SanityCheck () const
+{
+	#ifdef _DEBUG
+	dgInt32 tCount = 0;
+	for (dgListNode * node = m_first; node; node = node->GetNext()) {
+		tCount ++;
+		if (node->GetPrev()) {
+			dgAssert (node->GetPrev() != node->GetNext());
+			if (node->GetPrev()->GetNext() != node) {
+				dgAssert (0);
+				return false; 
+			}
+		}
+		if (node->GetNext()) {
+			dgAssert (node->GetPrev() != node->GetNext());
+			if (node->GetNext()->GetPrev() != node)	{
+				dgAssert (0);
+				return false;
+			}
+		}
+	}
+	if (tCount != m_count) {
+		dgAssert (0);
+		return false;
+	}
+	#endif
+	return true;
+}
+
+
+//template<class T>
+//void dgList<T>::SetAllocator (dgMemoryAllocator * allocator)
+//{
+//	m_allocator = allocator;
+//}
+
+//template<class T>
+//dgMemoryAllocator* dgList<T>::GetAllocator () const
+//{
+//	return m_allocator;
+//}
+//template<class T> dgInt32 dgList <T>::m_size = 0;
+//template<class T> dgMemoryAllocator* dgList<T>::m_staticAllocator = NULL;
+
+
+#endif
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgMatrix.cpp

@@ -0,0 +1,605 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+
+
+dgVector dgVector::m_triplexMask (0xffffffff, 0xffffffff, 0xffffffff, 0);
+dgVector dgVector::m_signMask (0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff);
+dgVector dgVector::m_one  (dgFloat32 (1.0f));
+dgVector dgVector::m_two  (dgFloat32 (2.0f));
+dgVector dgVector::m_wOne (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));
+dgVector dgVector::m_half (dgFloat32 (0.5f));
+dgVector dgVector::m_three (dgFloat32 (3.0f));
+dgVector dgVector::m_negOne (dgFloat32 (-1.0f));
+
+dgMatrix dgMatrix::m_zeroMatrix (dgVector (dgFloat32(0.0f)),
+								 dgVector (dgFloat32(0.0f)),
+								 dgVector (dgFloat32(0.0f)),
+								 dgVector (dgFloat32(0.0f)));
+
+dgMatrix dgMatrix::m_identityMatrix (dgVector (dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
+									 dgVector (dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
+									 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)),
+									 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
+
+const dgMatrix& dgGetIdentityMatrix()
+{
+	return dgMatrix::m_identityMatrix;
+}
+
+const dgMatrix& dgGetZeroMatrix ()
+{
+	return dgMatrix::m_zeroMatrix;
+}
+
+
+dgMatrix::dgMatrix (const dgQuaternion &rotation, const dgVector &position)
+{
+	dgFloat32 x2 = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q1;
+	dgFloat32 y2 = dgFloat32 (2.0f) * rotation.m_q2 * rotation.m_q2;
+	dgFloat32 z2 = dgFloat32 (2.0f) * rotation.m_q3 * rotation.m_q3;
+
+#ifdef _DEBUG
+	dgFloat32 w2 = dgFloat32 (2.0f) * rotation.m_q0 * rotation.m_q0;
+	dgAssert (dgAbsf (w2 + x2 + y2 + z2 - dgFloat32(2.0f)) <dgFloat32 (1.0e-3f));
+#endif
+
+	dgFloat32 xy = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q2;
+	dgFloat32 xz = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q3;
+	dgFloat32 xw = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q0;
+	dgFloat32 yz = dgFloat32 (2.0f) * rotation.m_q2 * rotation.m_q3;
+	dgFloat32 yw = dgFloat32 (2.0f) * rotation.m_q2 * rotation.m_q0;
+	dgFloat32 zw = dgFloat32 (2.0f) * rotation.m_q3 * rotation.m_q0;
+
+	m_front = dgVector (dgFloat32(1.0f) - y2 - z2, xy + zw,                   xz - yw				    , dgFloat32(0.0f));
+	m_up    = dgVector (xy - zw,                   dgFloat32(1.0f) - x2 - z2, yz + xw					, dgFloat32(0.0f));
+	m_right = dgVector (xz + yw,                   yz - xw,                   dgFloat32(1.0f) - x2 - y2 , dgFloat32(0.0f));
+
+
+	m_posit.m_x = position.m_x;
+	m_posit.m_y = position.m_y;
+	m_posit.m_z = position.m_z;
+	m_posit.m_w = dgFloat32(1.0f);
+}
+
+
+dgMatrix::dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis)
+{
+	dgMatrix scaledAxis;
+	scaledAxis[0] = stretchAxis[0].Scale4 (scale[0]);
+	scaledAxis[1] = stretchAxis[1].Scale4 (scale[1]);
+	scaledAxis[2] = stretchAxis[2].Scale4 (scale[2]);
+	scaledAxis[3] = stretchAxis[3];
+
+	*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
+}
+
+dgMatrix dgMatrix::operator* (const dgMatrix &B) const
+{
+	return dgMatrix (dgVector (B.m_front.Scale4(m_front.m_x) + B.m_up.Scale4(m_front.m_y) + B.m_right.Scale4(m_front.m_z) + B.m_posit.Scale4 (m_front.m_w)), 
+					 dgVector (B.m_front.Scale4(m_up.m_x)    + B.m_up.Scale4(m_up.m_y)    + B.m_right.Scale4(m_up.m_z)    + B.m_posit.Scale4 (m_up.m_w)), 
+					 dgVector (B.m_front.Scale4(m_right.m_x) + B.m_up.Scale4(m_right.m_y) + B.m_right.Scale4(m_right.m_z) + B.m_posit.Scale4 (m_right.m_w)), 
+					 dgVector (B.m_front.Scale4(m_posit.m_x) + B.m_up.Scale4(m_posit.m_y) + B.m_right.Scale4(m_posit.m_z) + B.m_posit.Scale4 (m_posit.m_w))); 
+}
+
+
+
+void dgMatrix::TransformTriplex (dgFloat32* const dst, dgInt32 dstStrideInBytes, const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
+{
+	dgInt32 dstStride = dstStrideInBytes /sizeof (dgFloat32);
+	dgInt32 srcStride = srcStrideInBytes / sizeof (dgFloat32);
+
+	dgInt32 dstIndex = 0;
+	dgInt32 srcIndex = 0;
+	for (dgInt32 i = 0 ; i < count; i ++ ) {
+		dgFloat32 x = src[srcIndex + 0];
+		dgFloat32 y = src[srcIndex + 1];
+		dgFloat32 z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+void dgMatrix::TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes, const dgFloat64* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
+{
+	dgInt32 dstStride = dstStrideInBytes /sizeof (dgFloat64);
+	dgInt32 srcStride = srcStrideInBytes / sizeof (dgFloat64);
+
+	dgInt32 dstIndex = 0;
+	dgInt32 srcIndex = 0;
+	for (dgInt32 i = 0 ; i < count; i ++ ) {
+		dgFloat64 x = src[srcIndex + 0];
+		dgFloat64 y = src[srcIndex + 1];
+		dgFloat64 z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+
+void dgMatrix::TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes, const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const
+{
+	dgInt32 dstStride = dstStrideInBytes /sizeof (dgFloat64);
+	dgInt32 srcStride = srcStrideInBytes / sizeof (dgFloat32);
+
+	dgInt32 dstIndex = 0;
+	dgInt32 srcIndex = 0;
+	for (dgInt32 i = 0 ; i < count; i ++ ) {
+		dgFloat64 x = src[srcIndex + 0];
+		dgFloat64 y = src[srcIndex + 1];
+		dgFloat64 z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+#endif
+
+void dgMatrix::TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const
+{
+	const dgMatrix& matrix = *this;
+	dgVector size ((p1local - p0local).Scale3 (dgFloat32 (0.5f)));
+	dgVector center (TransformVector ((p1local + p0local).Scale3 (dgFloat32 (0.5f))));
+	dgVector extends (size.m_x * dgAbsf(matrix[0][0]) + size.m_y * dgAbsf(matrix[1][0]) + size.m_z * dgAbsf(matrix[2][0]),  
+					  size.m_x * dgAbsf(matrix[0][1]) + size.m_y * dgAbsf(matrix[1][1]) + size.m_z * dgAbsf(matrix[2][1]),  
+	                  size.m_x * dgAbsf(matrix[0][2]) + size.m_y * dgAbsf(matrix[1][2]) + size.m_z * dgAbsf(matrix[2][2]), dgFloat32 (0.0f));  
+
+	p0 = center - extends;
+	p1 = center + extends;
+
+}
+
+dgMatrix dgMatrix::Inverse4x4 () const
+{
+	const dgFloat32 tol = 1.0e-4f;
+	dgMatrix tmp (*this);
+	dgMatrix inv (dgGetIdentityMatrix());
+	for (dgInt32 i = 0; i < 4; i ++) {
+		dgFloat32 diag = tmp[i][i];
+		if (dgAbsf (diag) < tol) {
+			dgInt32 j = 0;
+			for (j = i + 1; j < 4; j ++) {
+				dgFloat32 val = tmp[j][i];
+				if (dgAbsf (val) > tol) {
+					break;
+				}
+			}
+			dgAssert (j < 4);
+			for (dgInt32 k = 0; k < 4; k ++) {
+				tmp[i][k] += tmp[j][k];
+				inv[i][k] += inv[j][k];
+			}
+			diag = tmp[i][i];
+		}
+		dgFloat32 invDiag = dgFloat32 (1.0f) / diag;
+		for (dgInt32 j = 0; j < 4; j ++) {
+			tmp[i][j] *= invDiag;
+			inv[i][j] *= invDiag;
+		}
+		tmp[i][i] = dgFloat32 (1.0f);
+
+		for (dgInt32 j = 0; j < 4; j ++) {
+			if (j != i) {
+				dgFloat32 pivot = tmp[j][i];
+				for (dgInt32 k = 0; k < 4; k ++) {
+					tmp[j][k] -= pivot * tmp[i][k];
+					inv[j][k] -= pivot * inv[i][k];
+				}
+				tmp[j][i] = dgFloat32 (0.0f);
+			}
+		}
+	}
+	return inv;
+}
+
+dgMatrix dgMatrix::Symetric3by3Inverse () const
+{
+	const dgMatrix& mat = *this;
+	dgFloat64 det = mat[0][0] * mat[1][1] * mat[2][2] + 
+			mat[0][1] * mat[1][2] * mat[0][2] * dgFloat32 (2.0f) -
+			mat[0][2] * mat[1][1] * mat[0][2] -
+			mat[0][1] * mat[0][1] * mat[2][2] -
+			mat[0][0] * mat[1][2] * mat[1][2];
+
+	det = dgFloat32 (1.0f) / det;
+
+	dgFloat32 x11 = (dgFloat32)(det * (mat[1][1] * mat[2][2] - mat[1][2] * mat[1][2]));  
+	dgFloat32 x22 = (dgFloat32)(det * (mat[0][0] * mat[2][2] - mat[0][2] * mat[0][2]));  
+	dgFloat32 x33 = (dgFloat32)(det * (mat[0][0] * mat[1][1] - mat[0][1] * mat[0][1]));  
+
+	dgFloat32 x12 = (dgFloat32)(det * (mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]));  
+	dgFloat32 x13 = (dgFloat32)(det * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]));  
+	dgFloat32 x23 = (dgFloat32)(det * (mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1]));  
+
+
+#ifdef _DEBUG
+	dgMatrix matInv (dgVector (x11, x12, x13, dgFloat32(0.0f)),
+				     dgVector (x12, x22, x23, dgFloat32(0.0f)),
+					 dgVector (x13, x23, x33, dgFloat32(0.0f)),
+					 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
+
+	dgMatrix test (matInv * mat);
+	dgAssert (dgAbsf (test[0][0] - dgFloat32(1.0f)) < dgFloat32(0.01f));
+	dgAssert (dgAbsf (test[1][1] - dgFloat32(1.0f)) < dgFloat32(0.01f));
+	dgAssert (dgAbsf (test[2][2] - dgFloat32(1.0f)) < dgFloat32(0.01f));
+#endif
+
+	return dgMatrix (dgVector (x11, x12, x13, dgFloat32(0.0f)),
+					 dgVector (x12, x22, x23, dgFloat32(0.0f)),
+					 dgVector (x13, x23, x33, dgFloat32(0.0f)),
+					 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
+}
+
+
+
+
+
+void dgMatrix::CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const
+{
+	const dgMatrix& matrix = *this;
+	dgAssert (dgAbsf (((matrix[0] * matrix[1]) % matrix[2]) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
+
+	// Assuming the angles are in radians.
+	if (matrix[0][2] > dgFloat32 (0.99995f)) {
+		dgFloat32 picth0 = dgFloat32 (0.0f);
+		dgFloat32 yaw0 = dgFloat32 (-3.141592f * 0.5f);
+		dgFloat32 roll0 = - dgAtan2(matrix[2][1], matrix[1][1]);
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth0;
+		euler1[1] = yaw0;
+		euler1[2] = roll0;
+
+	} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
+		dgFloat32 picth0 = dgFloat32 (0.0f);
+		dgFloat32 yaw0 = dgFloat32 (3.141592f * 0.5f);
+		dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth0;
+		euler1[1] = yaw0;
+		euler1[2] = roll0;
+	} else {
+		dgFloat32 yaw0 = -dgAsin ( matrix[0][2]);
+		dgFloat32 yaw1 = dgFloat32 (3.141592f) - yaw0;
+		dgFloat32 sign0 = dgSign(dgCos (yaw0));
+		dgFloat32 sign1 = dgSign(dgCos (yaw1));
+
+		dgFloat32 picth0 = dgAtan2(matrix[1][2] * sign0, matrix[2][2] * sign0);
+		dgFloat32 picth1 = dgAtan2(matrix[1][2] * sign1, matrix[2][2] * sign1);
+
+		dgFloat32 roll0 = dgAtan2(matrix[0][1] * sign0, matrix[0][0] * sign0);
+		dgFloat32 roll1 = dgAtan2(matrix[0][1] * sign1, matrix[0][0] * sign1);
+
+		if (yaw1 > dgFloat32 (3.141592f)) {
+			yaw1 -= dgFloat32 (2.0f * 3.141592f);
+		}
+
+		euler0[0] = picth0;
+		euler0[1] = yaw0;
+		euler0[2] = roll0;
+
+		euler1[0] = picth1;
+		euler1[1] = yaw1;
+		euler1[2] = roll1;
+	}
+	euler0[3] = dgFloat32(0.0f);
+	euler1[3] = dgFloat32(0.0f);
+
+#ifdef _DEBUG
+	dgMatrix m0 (dgPitchMatrix (euler0[0]) * dgYawMatrix(euler0[1]) * dgRollMatrix(euler0[2]));
+	dgMatrix m1 (dgPitchMatrix (euler1[0]) * dgYawMatrix(euler1[1]) * dgRollMatrix(euler1[2]));
+	for (int i = 0; i < 3; i ++) {
+		for (int j = 0; j < 3; j ++) {
+			dgFloat32 error = dgAbsf (m0[i][j] - matrix[i][j]);
+			dgAssert (error < 5.0e-2f);
+			error = dgAbsf (m1[i][j] - matrix[i][j]);
+			dgAssert (error < 5.0e-2f);
+		}
+	}
+#endif
+}
+
+
+
+void dgMatrix::PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis, const dgMatrix& initialStretchAxis) const
+{
+	// a polar decomposition decompose matrix A = O * S
+	// where S = sqrt (transpose (L) * L)
+
+/*
+	// calculate transpose (L) * L 
+	dgMatrix LL ((*this) * Transpose());
+
+	// check is this is a pure uniformScale * rotation * translation
+	dgFloat32 det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * dgFloat32 (1.0f / 3.0f);
+
+	dgFloat32 invdet2 = 1.0f / det2;
+
+	dgMatrix pureRotation (LL);
+	pureRotation[0] = pureRotation[0].Scale3 (invdet2);
+	pureRotation[1] = pureRotation[1].Scale3 (invdet2);
+	pureRotation[2] = pureRotation[2].Scale3 (invdet2);
+
+	dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
+	dgFloat32 det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
+	if (dgAbsf (det - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)) {
+		// this is a pure scale * rotation * translation
+		det = sign * dgSqrt (det2);
+		scale[0] = det;
+		scale[1] = det;
+		scale[2] = det;
+		det = dgFloat32 (1.0f)/ det;
+		transformMatrix.m_front = m_front.Scale3 (det);
+		transformMatrix.m_up = m_up.Scale3 (det);
+		transformMatrix.m_right = m_right.Scale3 (det);
+		transformMatrix[0][3] = dgFloat32 (0.0f);
+		transformMatrix[1][3] = dgFloat32 (0.0f);
+		transformMatrix[2][3] = dgFloat32 (0.0f);
+		transformMatrix.m_posit = m_posit;
+		stretchAxis = dgGetIdentityMatrix();
+
+	} else {
+		stretchAxis = LL;
+		stretchAxis.EigenVectors (scale);
+
+		// I need to deal with by seeing of some of the Scale are duplicated
+		// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
+		// for now just us the matrix
+
+		scale[0] = sign * dgSqrt (scale[0]);
+		scale[1] = sign * dgSqrt (scale[1]);
+		scale[2] = sign * dgSqrt (scale[2]);
+		scale[3] = dgFloat32 (0.0f);
+
+		dgMatrix scaledAxis;
+		scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
+		scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
+		scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
+		scaledAxis[3] = stretchAxis[3];
+		dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
+
+		transformMatrix = symetricInv * (*this);
+		transformMatrix.m_posit = m_posit;
+	}
+*/
+
+	//dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
+	dgFloat32 sign = dgSign(((*this)[0] * (*this)[1]) % (*this)[2]);
+	stretchAxis = (*this) * Transpose();
+	stretchAxis.EigenVectors (scale);
+
+	// I need to deal with by seeing of some of the Scale are duplicated
+	// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
+	// for now just us the matrix
+
+	scale[0] = sign * dgSqrt (scale[0]);
+	scale[1] = sign * dgSqrt (scale[1]);
+	scale[2] = sign * dgSqrt (scale[2]);
+	scale[3] = dgFloat32 (0.0f);
+
+	dgMatrix scaledAxis;
+	scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
+	scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
+	scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
+	scaledAxis[3] = stretchAxis[3];
+	dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
+
+	transformMatrix = symetricInv * (*this);
+	transformMatrix.m_posit = m_posit;
+
+}
+
+
+/*
+for some reason I cannot get this to work
+void dgMatrix::EigenVectors (dgVector& eigenValues, const dgMatrix& initialGuess)
+{
+	dgVector xxx1;
+	dgMatrix xxx0 (*this);
+	xxx0.EigenVectors____ (xxx1, initialGuess);
+
+	static dgInt32 offDiagonalIndex[][2] = {{0, 1}, {0, 2}, {1, 2}};
+
+	dgMatrix eigenVector (dgGetIdentityMatrix());
+	dgMatrix& mat = *this;
+
+	for (dgInt32 m = 0; m < 20; m ++) {
+		dgFloat32 ajk[3];
+		ajk[0] = dgAbsf(mat[0][1]);
+		ajk[1] = dgAbsf(mat[0][2]);
+		ajk[2] = dgAbsf(mat[1][2]);
+		dgFloat32 sm = ajk[0] + ajk[1] + ajk[2];
+		if (sm < dgFloat32 (1.0e-12f)) {
+			eigenValues = dgVector (mat[0][0], mat[1][1], mat[2][2], dgFloat32 (1.0f));
+			*this = eigenVector;
+			return;
+		}
+
+		dgInt32 index = 0;
+		dgFloat32 maxA = ajk[0];
+		if (maxA < ajk[1]) {
+			index = 1;
+			maxA = ajk[1];
+		}
+
+		if (maxA < ajk[2]) {
+			index = 2;
+			maxA = ajk[2];
+		}
+
+		dgInt32 j = offDiagonalIndex[index][0];
+		dgInt32 k = offDiagonalIndex[index][1];
+
+		dgFloat32 Ajj = mat[j][j];
+		dgFloat32 Akk = mat[k][k];
+		dgFloat32 Ajk = mat[j][k];
+		dgFloat32 phi = dgFloat32 (0.5f) * dgAtan2 (dgFloat32 (2.0f) * Ajk, Akk - Ajj);
+
+		dgFloat32 c = dgCos (phi);
+		dgFloat32 s = dgSin (phi);
+
+		dgMatrix givensRotation (dgGetIdentityMatrix());
+		givensRotation[j][j] = c;
+		givensRotation[k][k] = c;
+		givensRotation[j][k] = -s;
+		givensRotation[k][j] = s;
+
+		eigenVector = eigenVector * givensRotation;
+		mat = givensRotation * mat * givensRotation.Transpose();
+		mat[j][k] = dgFloat32 (0.0f);
+		mat[k][j] = dgFloat32 (0.0f);
+	}
+	dgAssert (0);
+}
+*/
+
+void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess)
+{
+	dgFloat32 b[3];
+	dgFloat32 z[3];
+	dgFloat32 d[3];
+
+	dgMatrix& mat = *this;
+	dgMatrix eigenVectors (initialGuess.Transpose4X4());
+	mat = initialGuess * mat * eigenVectors;
+
+	b[0] = mat[0][0]; 
+	b[1] = mat[1][1];
+	b[2] = mat[2][2];
+
+	d[0] = mat[0][0]; 
+	d[1] = mat[1][1]; 
+	d[2] = mat[2][2]; 
+
+	z[0] = dgFloat32 (0.0f);
+	z[1] = dgFloat32 (0.0f);
+	z[2] = dgFloat32 (0.0f);
+
+	for (dgInt32 i = 0; i < 50; i++) {
+		dgFloat32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
+
+		if (sm < dgFloat32 (1.0e-12f)) {
+			//dgAssert (dgAbsf((eigenVectors.m_front % eigenVectors.m_front) - dgFloat32(1.0f)) < dgEPSILON);
+			//dgAssert (dgAbsf((eigenVectors.m_up % eigenVectors.m_up) - dgFloat32(1.0f)) < dgEPSILON);
+			//dgAssert (dgAbsf((eigenVectors.m_right % eigenVectors.m_right) - dgFloat32(1.0f)) < dgEPSILON);
+
+			// order the eigenvalue vectors	
+			dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
+			if (tmp % eigenVectors.m_right < dgFloat32(0.0f)) {
+				dgAssert (0.0f);
+				eigenVectors.m_right = eigenVectors.m_right.Scale3 (-dgFloat32(1.0f));
+			}
+
+			//eigenValues = dgVector (d[0], d[1], d[2], dgFloat32 (1.0f));
+			//*this = eigenVectors.Inverse();
+			//return;
+			break;
+		}
+
+		dgFloat32 thresh = dgFloat32 (0.0f);
+		if (i < 3) {
+			thresh = (dgFloat32)(0.2f / 9.0f) * sm;
+		}
+
+		for (dgInt32 ip = 0; ip < 2; ip ++) {
+			for (dgInt32 iq = ip + 1; iq < 3; iq ++) {
+				dgFloat32 g = dgFloat32 (100.0f) * dgAbsf(mat[ip][iq]);
+				if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
+					mat[ip][iq] = dgFloat32 (0.0f);
+				} else if (dgAbsf(mat[ip][iq]) > thresh) {
+
+					dgFloat32 t;
+					dgFloat32 h = d[iq] - d[ip];
+					if (dgAbsf(h) + g == dgAbsf(h)) {
+						t = mat[ip][iq] / h;
+					} else {
+						dgFloat32 theta = dgFloat32 (0.5f) * h / mat[ip][iq];
+						t = dgFloat32(1.0f) / (dgAbsf(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
+						if (theta < dgFloat32 (0.0f)) {
+							t = -t;
+						}
+					}
+					dgFloat32 c = dgRsqrt (dgFloat32 (1.0f) + t * t); 
+					dgFloat32 s = t * c; 
+					dgFloat32 tau = s / (dgFloat32(1.0f) + c); 
+					h = t * mat[ip][iq];
+					z[ip] -= h; 
+					z[iq] += h; 
+					d[ip] -= h; 
+					d[iq] += h;
+					mat[ip][iq] = dgFloat32(0.0f);
+
+					for (dgInt32 j = 0; j <= ip - 1; j ++) {
+						dgFloat32 g = mat[j][ip]; 
+						dgFloat32 h = mat[j][iq]; 
+						mat[j][ip] = g - s * (h + g * tau); 
+						mat[j][iq] = h + s * (g - h * tau);
+
+					}
+					for (dgInt32 j = ip + 1; j <= iq - 1; j ++) {
+						dgFloat32 g = mat[ip][j]; 
+						dgFloat32 h = mat[j][iq]; 
+						mat[ip][j] = g - s * (h + g * tau); 
+						mat[j][iq] = h + s * (g - h * tau);
+					}
+					for (dgInt32 j = iq + 1; j < 3; j ++) {
+						dgFloat32 g = mat[ip][j]; 
+						dgFloat32 h = mat[iq][j]; 
+						mat[ip][j] = g - s * (h + g * tau); 
+						mat[iq][j] = h + s * (g - h * tau);
+					}
+
+					for (dgInt32 j = 0; j < 3; j ++) {
+						dgFloat32 g = eigenVectors[j][ip]; 
+						dgFloat32 h = eigenVectors[j][iq]; 
+						eigenVectors[j][ip] = g - s * (h + g * tau); 
+						eigenVectors[j][iq] = h + s * (g - h * tau);
+					}
+				}
+			}
+		}
+		b[0] += z[0]; d[0] = b[0]; z[0] = dgFloat32 (0.0f);
+		b[1] += z[1]; d[1] = b[1]; z[1] = dgFloat32 (0.0f);
+		b[2] += z[2]; d[2] = b[2]; z[2] = dgFloat32 (0.0f);
+	}
+
+	eigenValues = dgVector (d[0], d[1], d[2], dgFloat32 (1.0f));
+	*this = eigenVectors.Inverse();
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgMatrix.h

@@ -0,0 +1,332 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgMatrix__
+#define __dgMatrix__
+
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+#include "dgVector.h"
+#include "dgPlane.h"
+#include <math.h>
+
+class dgMatrix;
+class dgQuaternion;
+
+const dgMatrix& dgGetZeroMatrix ();
+const dgMatrix& dgGetIdentityMatrix();
+
+
+DG_MSC_VECTOR_ALIGMENT
+class dgMatrix
+{
+	public:
+	DG_CLASS_ALLOCATOR(allocator)
+
+	dgMatrix ();
+	dgMatrix (const dgFloat32* const array);
+	dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit);
+	dgMatrix (const dgQuaternion &rotation, const dgVector &position);
+
+	// create a orthonormal normal vector basis, front become m_front vector, and m_up and m_right are mutualiperpendicular to fron and to each other
+	dgMatrix (const dgVector &front);
+
+	// create a covariance Matrix = transpose(p) * q 
+	dgMatrix (const dgVector& p, const dgVector& q);
+
+	dgVector& operator[] (dgInt32 i);
+	const dgVector& operator[] (dgInt32 i) const;
+
+	dgMatrix Inverse () const;
+	dgMatrix Inverse4x4 () const;
+	dgMatrix Transpose () const;
+	dgMatrix Transpose4X4 () const;
+	dgMatrix Symetric3by3Inverse () const;
+	dgVector RotateVector (const dgVector &v) const;
+	dgVector UnrotateVector (const dgVector &v) const;
+	dgVector TransformVector (const dgVector &v) const;
+	dgVector UntransformVector (const dgVector &v) const;
+	dgPlane TransformPlane (const dgPlane &localPlane) const;
+	dgPlane UntransformPlane (const dgPlane &globalPlane) const;
+	void TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const; 
+
+	void CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const;
+	void TransformTriplex (dgFloat32* const dst, dgInt32 dstStrideInBytes,
+						   const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
+
+#ifndef _NEWTON_USE_DOUBLE
+	void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
+						   const dgFloat64* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
+
+	void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
+						   const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
+#endif
+
+	bool TestOrthogonal() const;
+	bool TestSymetric3x3() const;
+
+	dgMatrix operator* (const dgMatrix &B) const;
+
+
+	
+	
+
+	// these function can only be called when dgMatrix is a PDS matrix
+	void EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess = dgGetIdentityMatrix());
+	void EigenVectors (const dgMatrix& initialGuess = dgGetIdentityMatrix());
+	void PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis, const dgMatrix& initialStretchAxis = dgGetIdentityMatrix()) const;
+
+	// constructor for polar composition
+	dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis);
+
+	dgVector m_front;
+	dgVector m_up;
+	dgVector m_right;
+	dgVector m_posit;
+
+	static dgMatrix m_zeroMatrix;
+	static dgMatrix m_identityMatrix;
+} DG_GCC_VECTOR_ALIGMENT;
+
+
+
+DG_INLINE dgMatrix::dgMatrix ()
+{
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgFloat32* const array)
+{
+	memcpy (&m_front.m_x, array, sizeof (dgMatrix)) ;
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit)
+	:m_front (front), m_up(up), m_right(right), m_posit(posit)
+{
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgVector& p, const dgVector& q)
+	:m_front(q.CompProduct4(p.BroadcastX()))
+	,m_up(q.CompProduct4(p.BroadcastY()))
+	,m_right(q.CompProduct4(p.BroadcastZ()))
+	,m_posit (dgVector::m_wOne)
+{
+}
+
+DG_INLINE dgMatrix::dgMatrix (const dgVector& front)
+{
+	m_front = front; 
+	if (dgAbsf (front.m_z) > dgFloat32 (0.577f)) {
+		m_right = front * dgVector (-front.m_y, front.m_z, dgFloat32(0.0f), dgFloat32(0.0f));
+	} else {
+	  	m_right = front * dgVector (-front.m_y, front.m_x, dgFloat32(0.0f), dgFloat32(0.0f));
+	}
+  	m_right = m_right.Scale3 (dgRsqrt (m_right % m_right));
+  	m_up = m_right * m_front;
+
+	m_front.m_w = dgFloat32(0.0f);
+	m_up.m_w = dgFloat32(0.0f);
+	m_right.m_w = dgFloat32(0.0f);
+	m_posit = dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f));
+
+	dgAssert ((dgAbsf (m_front % m_front) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+	dgAssert ((dgAbsf (m_up % m_up) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+	dgAssert ((dgAbsf (m_right % m_right) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+	dgAssert ((dgAbsf (m_right % (m_front * m_up)) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); 
+}
+
+
+DG_INLINE dgVector& dgMatrix::operator[] (dgInt32  i)
+{
+	dgAssert (i < 4);
+	dgAssert (i >= 0);
+	return (&m_front)[i];
+}
+
+DG_INLINE const dgVector& dgMatrix::operator[] (dgInt32  i) const
+{
+	dgAssert (i < 4);
+	dgAssert (i >= 0);
+	return (&m_front)[i];
+}
+
+
+DG_INLINE dgMatrix dgMatrix::Transpose () const
+{
+#ifdef DG_SCALAR_VECTOR_CLASS
+	return dgMatrix (dgVector (m_front.m_x, m_up.m_x, m_right.m_x, dgFloat32(0.0f)),
+					 dgVector (m_front.m_y, m_up.m_y, m_right.m_y, dgFloat32(0.0f)),
+					 dgVector (m_front.m_z, m_up.m_z, m_right.m_z, dgFloat32(0.0f)),
+					 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
+#else
+	dgMatrix tmp;
+	dgVector::Transpose4x4 (tmp.m_front, tmp.m_up, tmp.m_right, tmp.m_posit, m_front, m_up, m_right, dgVector::m_wOne); 
+	return tmp;
+#endif
+}
+
+DG_INLINE dgMatrix dgMatrix::Transpose4X4 () const
+{
+#ifdef DG_SCALAR_VECTOR_CLASS
+	return dgMatrix (dgVector (m_front.m_x, m_up.m_x, m_right.m_x, m_posit.m_x),
+					 dgVector (m_front.m_y, m_up.m_y, m_right.m_y, m_posit.m_y),
+					 dgVector (m_front.m_z, m_up.m_z, m_right.m_z, m_posit.m_z),
+					 dgVector (m_front.m_w, m_up.m_w, m_right.m_w, m_posit.m_w));
+#else 
+	dgMatrix tmp;
+	dgVector::Transpose4x4 (tmp.m_front, tmp.m_up, tmp.m_right, tmp.m_posit, m_front, m_up, m_right, m_posit); 
+	return tmp;
+#endif
+}
+
+DG_INLINE dgVector dgMatrix::RotateVector (const dgVector &v) const
+{
+	return dgVector (m_front.Scale4(v.m_x) + m_up.Scale4(v.m_y) + m_right.Scale4(v.m_z));
+}
+
+
+DG_INLINE dgVector dgMatrix::UnrotateVector (const dgVector &v) const
+{
+#ifdef DG_SCALAR_VECTOR_CLASS
+	return dgVector (v % m_front, v % m_up, v % m_right, dgFloat32 (0.0f));
+#else
+	dgVector x (v.CompProduct4(m_front));
+	dgVector y (v.CompProduct4(m_up));
+	dgVector z (v.CompProduct4(m_right));
+	dgVector w (dgFloat32 (0.0f));
+	dgVector::Transpose4x4 (x, y, z, w, x, y, z, w); 
+	return x + y + z;
+
+#endif
+}
+
+
+DG_INLINE dgVector dgMatrix::TransformVector (const dgVector &v) const
+{
+/*
+	dgVector tmp0 (v.m_x * m_front.m_x + v.m_y * m_up.m_x + v.m_z * m_right.m_x + m_posit.m_x,
+				   v.m_x * m_front.m_y + v.m_y * m_up.m_y + v.m_z * m_right.m_y + m_posit.m_y,
+				   v.m_x * m_front.m_z + v.m_y * m_up.m_z + v.m_z * m_right.m_z + m_posit.m_z, v.m_w);
+	reurn dgVector (m_front.Scale4(v.m_x) + m_up.Scale4(v.m_y) + m_right.Scale4(v.m_z) + m_posit.Scale4(v.m_w));
+*/
+	return dgVector (m_front.Scale4(v.m_x) + m_up.Scale4(v.m_y) + m_right.Scale4(v.m_z) + m_posit);
+}
+
+DG_INLINE dgVector dgMatrix::UntransformVector (const dgVector &v) const
+{
+	return UnrotateVector(v - m_posit);
+}
+
+DG_INLINE dgPlane dgMatrix::TransformPlane (const dgPlane &localPlane) const
+{
+	return dgPlane (RotateVector (localPlane), localPlane.m_w - (localPlane % UnrotateVector (m_posit)));  
+}
+
+DG_INLINE dgPlane dgMatrix::UntransformPlane (const dgPlane &globalPlane) const
+{
+	return dgPlane (UnrotateVector (globalPlane), globalPlane.Evalue(m_posit));
+}
+
+DG_INLINE void dgMatrix::EigenVectors (const dgMatrix& initialGuess)
+{
+	dgVector eigenValues;
+	EigenVectors (eigenValues, initialGuess);
+}
+
+
+DG_INLINE dgMatrix dgMatrix::Inverse () const
+{
+	return dgMatrix (dgVector (m_front.m_x, m_up.m_x, m_right.m_x, dgFloat32(0.0f)),
+					 dgVector (m_front.m_y, m_up.m_y, m_right.m_y, dgFloat32(0.0f)),
+					 dgVector (m_front.m_z, m_up.m_z, m_right.m_z, dgFloat32(0.0f)),
+					 dgVector (- (m_posit % m_front), - (m_posit % m_up), - (m_posit % m_right), dgFloat32(1.0f)));
+}
+
+
+DG_INLINE bool dgMatrix::TestOrthogonal() const
+{
+	dgVector n (m_front * m_up);
+	dgFloat32 a = m_right % m_right;
+	dgFloat32 b = m_up % m_up;
+	dgFloat32 c = m_front % m_front;
+	dgFloat32 d = n % m_right;
+
+	return (m_front[3] == dgFloat32 (0.0f)) & 
+		   (m_up[3] == dgFloat32 (0.0f)) & 
+		   (m_right[3] == dgFloat32 (0.0f)) & 
+		   (m_posit[3] == dgFloat32 (1.0f)) &
+		   (dgAbsf(a - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)) & 
+		   (dgAbsf(b - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)) &
+		   (dgAbsf(c - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)) &
+		   (dgAbsf(d - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); 
+}
+
+DG_INLINE bool dgMatrix::TestSymetric3x3() const
+{
+	const dgMatrix& me = *this;
+	return (dgAbsf (me[0][1] - me[1][0]) < dgFloat32 (1.0e-5f)) && 
+		   (dgAbsf (me[0][2] - me[2][0]) < dgFloat32 (1.0e-5f)) &&
+		   (dgAbsf (me[1][2] - me[2][1]) < dgFloat32 (1.0e-5f)) &&
+		   (me[0][3] == dgFloat32 (0.0f)) &&
+		   (me[1][3] == dgFloat32 (0.0f)) &&
+		   (me[2][3] == dgFloat32 (0.0f)) &&
+		   (me[3][0] == dgFloat32 (0.0f)) &&
+		   (me[3][1] == dgFloat32 (0.0f)) &&
+		   (me[3][2] == dgFloat32 (0.0f)) &&
+		   (me[3][3] == dgFloat32 (1.0f));
+}
+
+
+DG_INLINE dgMatrix dgPitchMatrix(dgFloat32 ang)
+{
+	dgFloat32 sinAng = dgSin (ang);
+	dgFloat32 cosAng = dgCos (ang);
+	return dgMatrix (dgVector (dgFloat32(1.0f),  dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f),  cosAng,          sinAng,          dgFloat32(0.0f)),
+		dgVector (dgFloat32(0.0f), -sinAng,          cosAng,          dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f),  dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f))); 
+
+}
+
+DG_INLINE dgMatrix dgYawMatrix(dgFloat32 ang)
+{
+	dgFloat32 sinAng = dgSin (ang);
+	dgFloat32 cosAng = dgCos (ang);
+	return dgMatrix (dgVector (cosAng,          dgFloat32(0.0f), -sinAng,          dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f), dgFloat32(1.0f),  dgFloat32(0.0f), dgFloat32(0.0f)), 
+		dgVector (sinAng,          dgFloat32(0.0f),  cosAng,          dgFloat32(0.0f)), 
+		dgVector (dgFloat32(0.0f), dgFloat32(0.0f),  dgFloat32(0.0f), dgFloat32(1.0f))); 
+}
+
+DG_INLINE dgMatrix dgRollMatrix(dgFloat32 ang)
+{
+	dgFloat32 sinAng = dgSin (ang);
+	dgFloat32 cosAng = dgCos (ang);
+	return dgMatrix (dgVector ( cosAng,          sinAng,          dgFloat32(0.0f), dgFloat32(0.0f)), 
+		dgVector (-sinAng,          cosAng,          dgFloat32(0.0f), dgFloat32(0.0f)),
+		dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)), 
+		dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f))); 
+}																		 
+
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgMemory.cpp

@@ -0,0 +1,472 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgList.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+
+#ifdef _DEBUG
+dgInt32 m_threadSanityCheck = 0;
+#define DG_MEMORY_THREAD_SANITY_CHECK_LOCK()		\
+	dgAssert (!m_threadSanityCheck);				\
+	dgAtomicExchangeAndAdd(&m_threadSanityCheck, 1);
+	#define DG_MEMORY_THREAD_SANITY_CHECK_UNLOCK()	  dgAtomicExchangeAndAdd(&m_threadSanityCheck, -1);	
+#else 
+	#define DG_MEMORY_THREAD_SANITY_CHECK_LOCK()
+	#define DG_MEMORY_THREAD_SANITY_CHECK_UNLOCK()
+#endif
+
+class dgGlobalAllocator: public dgMemoryAllocator, public dgList<dgMemoryAllocator*>
+{
+	public:
+	dgGlobalAllocator ()
+		:dgMemoryAllocator (__malloc__, __free__), dgList<dgMemoryAllocator*> (NULL)
+	{
+		SetAllocator (this);
+	}
+
+	~dgGlobalAllocator ()
+	{
+		dgAssert (GetCount() == 0);
+	}
+
+	static void* dgApi __malloc__ (dgUnsigned32 size) 
+	{
+		return malloc (size);
+	}
+
+	static void dgApi __free__ (void* const ptr, dgUnsigned32 size)
+	{
+		free (ptr);
+	}
+
+	void operator delete (void* const ptr)
+	{
+		dgAssert (0);
+		//::delete (ptr);
+		free (ptr);
+	}
+
+
+	dgInt32 GetMemoryUsed () const
+	{
+		dgInt32 mem = m_memoryUsed;
+		for (dgList<dgMemoryAllocator*>::dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+			mem += node->GetInfo()->GetMemoryUsed();
+		}
+		return mem;
+	}
+
+	static dgGlobalAllocator m_globalAllocator;
+};
+
+
+
+dgGlobalAllocator dgGlobalAllocator::m_globalAllocator;
+
+
+
+dgMemoryAllocator::dgMemoryAllocator ()
+{
+	m_memoryUsed = 0;
+	m_emumerator = 0;
+	SetAllocatorsCallback (dgGlobalAllocator::m_globalAllocator.m_malloc, dgGlobalAllocator::m_globalAllocator.m_free);
+	memset (m_memoryDirectory, 0, sizeof (m_memoryDirectory));
+	dgGlobalAllocator::m_globalAllocator.Append(this);
+}
+
+dgMemoryAllocator::dgMemoryAllocator (dgMemAlloc memAlloc, dgMemFree memFree)
+{
+	m_memoryUsed = 0;
+	m_emumerator = 0;
+	SetAllocatorsCallback (memAlloc, memFree);
+	memset (m_memoryDirectory, 0, sizeof (m_memoryDirectory));
+}
+
+
+dgMemoryAllocator::~dgMemoryAllocator  ()
+{
+	dgGlobalAllocator::m_globalAllocator.Remove(this);
+	dgAssert (m_memoryUsed == 0);
+}
+
+
+void *dgMemoryAllocator::operator new (size_t size) 
+{ 
+	return dgMallocStack(size);
+}
+
+void dgMemoryAllocator::operator delete (void* const ptr) 
+{ 
+	dgFreeStack(ptr); 
+}
+
+
+dgInt32 dgMemoryAllocator::GetMemoryUsed() const
+{
+	return m_memoryUsed;
+}
+
+void dgMemoryAllocator::SetAllocatorsCallback (dgMemAlloc memAlloc, dgMemFree memFree)
+{
+	m_free = memFree;
+	m_malloc = memAlloc;
+}
+
+
+
+void *dgMemoryAllocator::MallocLow (dgInt32 workingSize, dgInt32 alignment)
+{
+	dgAssert (alignment >= DG_MEMORY_GRANULARITY);
+	dgAssert (((-alignment) & (alignment - 1)) == 0);
+	dgInt32 size = workingSize + alignment * 2;
+	void* const ptr = m_malloc(dgUnsigned32 (size));
+	dgAssert (ptr);
+	dgUnsigned64 val = dgUnsigned64 (PointerToInt(ptr));
+	val = (val & dgUnsigned64(-alignment)) + alignment * 2;
+	void* const retPtr = IntToPointer (val);
+
+	dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
+	info->SaveInfo(this, ptr, size, m_emumerator, workingSize);
+
+	dgAtomicExchangeAndAdd (&m_memoryUsed, size);
+	return retPtr;
+}
+
+void dgMemoryAllocator::FreeLow (void* const retPtr)
+{
+	dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
+	dgAssert (info->m_allocator == this);
+
+	dgAtomicExchangeAndAdd (&m_memoryUsed, -info->m_size);
+
+#ifdef _DEBUG
+	memset (retPtr, 0, info->m_workingSize);
+#endif
+
+	m_free (info->m_ptr, dgUnsigned32 (info->m_size));
+}
+
+// alloca memory on pool that are quantized to DG_MEMORY_GRANULARITY
+// if memory size is larger than DG_MEMORY_BIN_ENTRIES then the memory is not placed into a pool
+void *dgMemoryAllocator::Malloc (dgInt32 memsize)
+{
+	dgAssert (dgInt32 (sizeof (dgMemoryCacheEntry) + sizeof (dgInt32) + sizeof(dgInt32)) <= DG_MEMORY_GRANULARITY);
+
+	dgInt32 size = memsize + DG_MEMORY_GRANULARITY - 1;
+	size &= (-DG_MEMORY_GRANULARITY);
+
+	dgInt32 paddedSize = size + DG_MEMORY_GRANULARITY; 
+	dgInt32 entry = paddedSize >> DG_MEMORY_GRANULARITY_BITS;	
+
+	void* ptr;
+	if (entry >= DG_MEMORY_BIN_ENTRIES) {
+		ptr = MallocLow (size);
+	} else {
+		if (!m_memoryDirectory[entry].m_cache) {
+			dgMemoryBin* const bin = (dgMemoryBin*) MallocLow (sizeof (dgMemoryBin));
+
+			dgInt32 count = dgInt32 (sizeof (bin->m_pool) / paddedSize);
+			bin->m_info.m_count = 0;
+			bin->m_info.m_totalCount = count;
+			bin->m_info.m_stepInBites = paddedSize;
+			bin->m_info.m_next = m_memoryDirectory[entry].m_first;
+			bin->m_info.m_prev = NULL;
+			if (bin->m_info.m_next) {
+				bin->m_info.m_next->m_info.m_prev = bin;
+			}
+
+			m_memoryDirectory[entry].m_first = bin;
+
+			dgInt8* charPtr = reinterpret_cast<dgInt8*>(bin->m_pool);
+			m_memoryDirectory[entry].m_cache = (dgMemoryCacheEntry*)charPtr;
+
+			for (dgInt32 i = 0; i < count; i ++) {
+				dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) charPtr;
+				cashe->m_next = (dgMemoryCacheEntry*) (charPtr + paddedSize);
+				cashe->m_prev = (dgMemoryCacheEntry*) (charPtr - paddedSize);
+				dgMemoryInfo* const info = ((dgMemoryInfo*) (charPtr + DG_MEMORY_GRANULARITY)) - 1;						
+				info->SaveInfo(this, bin, entry, m_emumerator, memsize);
+				charPtr += paddedSize;
+			}
+			dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) (charPtr - paddedSize);
+			cashe->m_next = NULL;
+			m_memoryDirectory[entry].m_cache->m_prev = NULL;
+		}
+
+
+		dgAssert (m_memoryDirectory[entry].m_cache);
+
+		dgMemoryCacheEntry* const cashe = m_memoryDirectory[entry].m_cache;
+		m_memoryDirectory[entry].m_cache = cashe->m_next;
+		if (cashe->m_next) {
+			cashe->m_next->m_prev = NULL;
+		}
+
+		ptr = ((dgInt8*)cashe) + DG_MEMORY_GRANULARITY;
+
+		dgMemoryInfo* info;
+		info = ((dgMemoryInfo*) (ptr)) - 1;
+		dgAssert (info->m_allocator == this);
+
+		dgMemoryBin* const bin = (dgMemoryBin*) info->m_ptr;
+		bin->m_info.m_count ++;
+
+		#ifdef __TRACK_MEMORY_LEAKS__
+		m_leaklTracker.InsertBlock (dgInt32 (memsize), ptr);
+		#endif
+
+	}
+	return ptr;
+}
+
+// alloca memory on pool that are quantized to DG_MEMORY_GRANULARITY
+// if memory size is larger than DG_MEMORY_BIN_ENTRIES then the memory is not placed into a pool
+void dgMemoryAllocator::Free (void* const retPtr)
+{
+	dgMemoryInfo* const info = ((dgMemoryInfo*) (retPtr)) - 1;
+	dgAssert (info->m_allocator == this);
+
+	dgInt32 entry = info->m_size;
+
+	if (entry >= DG_MEMORY_BIN_ENTRIES) {
+		FreeLow (retPtr);
+	} else {
+		#ifdef __TRACK_MEMORY_LEAKS__
+		m_leaklTracker.RemoveBlock (retPtr);
+		#endif
+
+		dgMemoryCacheEntry* const cashe = (dgMemoryCacheEntry*) (((char*)retPtr) - DG_MEMORY_GRANULARITY) ;
+
+		dgMemoryCacheEntry* const tmpCashe = m_memoryDirectory[entry].m_cache;
+		if (tmpCashe) {
+			dgAssert (!tmpCashe->m_prev);
+			tmpCashe->m_prev = cashe;
+		}
+		cashe->m_next = tmpCashe;
+		cashe->m_prev = NULL;
+
+		m_memoryDirectory[entry].m_cache = cashe;
+
+		dgMemoryBin* const bin = (dgMemoryBin *) info->m_ptr;
+
+		dgAssert (bin);
+#ifdef _DEBUG
+		dgAssert ((bin->m_info.m_stepInBites - DG_MEMORY_GRANULARITY) > 0);
+		memset (retPtr, 0, bin->m_info.m_stepInBites - DG_MEMORY_GRANULARITY);
+#endif
+
+		bin->m_info.m_count --;
+		if (bin->m_info.m_count == 0) {
+
+			dgInt32 count = bin->m_info.m_totalCount;
+			dgInt32 sizeInBytes = bin->m_info.m_stepInBites;
+			char* charPtr = bin->m_pool;
+			for (dgInt32 i = 0; i < count; i ++) {
+				dgMemoryCacheEntry* const tmpCashe = (dgMemoryCacheEntry*)charPtr;
+				charPtr += sizeInBytes;
+
+				if (tmpCashe == m_memoryDirectory[entry].m_cache) {
+					m_memoryDirectory[entry].m_cache = tmpCashe->m_next;
+				}
+
+				if (tmpCashe->m_prev) {
+					tmpCashe->m_prev->m_next = tmpCashe->m_next;
+				}
+
+				if (tmpCashe->m_next) {
+					tmpCashe->m_next->m_prev = tmpCashe->m_prev;
+				}
+			}
+
+			if (m_memoryDirectory[entry].m_first == bin) {
+				m_memoryDirectory[entry].m_first = bin->m_info.m_next;
+			}
+
+			if (bin->m_info.m_next) {
+				bin->m_info.m_next->m_info.m_prev = bin->m_info.m_prev;
+			}
+			if (bin->m_info.m_prev) {
+				bin->m_info.m_prev->m_info.m_next = bin->m_info.m_next;
+			}
+
+			FreeLow (bin);
+		}
+	}
+}
+
+
+
+	// this is a simple memory leak tracker, it uses an flat array of two megabyte indexed by a hatch code
+#ifdef __TRACK_MEMORY_LEAKS__
+
+dgMemoryAllocator::dgMemoryLeaksTracker::dgMemoryLeaksTracker()
+{
+	m_density = 0;
+	m_totalAllocatedBytes = 0; 
+	m_totalAllocatedCalls = 0; 
+	m_leakAllocationCounter = 0;
+
+	memset (m_pool, 0, sizeof (m_pool));
+}
+
+dgMemoryAllocator::dgMemoryLeaksTracker::~dgMemoryLeaksTracker ()
+{
+	if (m_totalAllocatedBytes) {
+		for (dgInt32 i = 0; i < DG_TRACK_MEMORY_LEAKS_ENTRIES; i ++) {
+			if (m_pool[i].m_ptr) {
+				dgTrace (("MemoryLeak: (0x%08x), size (%d)  allocationNumber (%d)\n", m_pool[i].m_ptr, m_pool[i].m_size, m_pool[i].m_allocationNumber));
+			}
+		}
+	}
+}
+
+void dgMemoryAllocator::dgMemoryLeaksTracker::InsertBlock (dgInt32 size, void* const ptr)
+{
+	dgUnsigned32 key = dgHash (&ptr, sizeof (void*));
+	dgUnsigned32 index = key % DG_TRACK_MEMORY_LEAKS_ENTRIES;
+
+	dgInt32 i = 0;
+	for (; m_pool[index].m_ptr && (i < DG_TRACK_MEMORY_LEAKS_ENTRIES); i ++) {
+		index = ((index + 1) < DG_TRACK_MEMORY_LEAKS_ENTRIES) ? index + 1 : 0;
+	}
+
+	dgAssert (i < 8);
+	dgAssert (i < DG_TRACK_MEMORY_LEAKS_ENTRIES);
+
+	m_density ++;
+	m_pool[index].m_size = size;
+	m_pool[index].m_ptr = ptr;
+	m_pool[index].m_allocationNumber = m_leakAllocationCounter;
+
+//	dgAssert (m_leakAllocationCounter != 129);
+
+	m_leakAllocationCounter ++;
+	m_totalAllocatedBytes += size; 
+	m_totalAllocatedCalls ++;
+}
+
+void dgMemoryAllocator::dgMemoryLeaksTracker::RemoveBlock (void* const ptr)
+{
+	dgUnsigned32 key = dgHash (&ptr, sizeof (void*));
+	dgUnsigned32 index = key % DG_TRACK_MEMORY_LEAKS_ENTRIES;
+
+	dgInt32 i = 0;
+	for (; i < DG_TRACK_MEMORY_LEAKS_ENTRIES; i ++) {
+		if (m_pool[index].m_ptr == ptr) {
+			m_density --;
+			m_totalAllocatedCalls--;
+			m_totalAllocatedBytes -= m_pool[index].m_size; 
+			dgAssert (m_totalAllocatedBytes >= 0);
+			m_pool[index].m_size = 0;
+			m_pool[index].m_ptr = NULL;
+			break;
+		}
+		index = ((index + 1) < DG_TRACK_MEMORY_LEAKS_ENTRIES) ? index + 1 : 0;
+	}
+
+	dgAssert (i < DG_TRACK_MEMORY_LEAKS_ENTRIES);
+}
+#endif
+
+
+
+// Set the pointer of memory allocation functions
+void dgSetGlobalAllocators (dgMemAlloc malloc, dgMemFree free)
+{
+	dgGlobalAllocator::m_globalAllocator.SetAllocatorsCallback (malloc, free);
+}
+
+dgInt32 dgGetMemoryUsed ()
+{
+	return dgGlobalAllocator::m_globalAllocator.GetMemoryUsed();
+}
+
+// this can be used by function that allocates large memory pools memory locally on the stack
+// this by pases the pool allocation because this should only be used for very large memory blocks.
+// this was using virtual memory on windows but 
+// but because of many complaint I changed it to use malloc and free
+void* dgApi dgMallocStack (size_t size)
+{
+	DG_MEMORY_THREAD_SANITY_CHECK_LOCK();
+	void * const ptr = dgGlobalAllocator::m_globalAllocator.MallocLow (dgInt32 (size));
+	DG_MEMORY_THREAD_SANITY_CHECK_UNLOCK();
+	return ptr;
+}
+
+void* dgApi dgMallocAligned (size_t size, dgInt32 align)
+{
+	DG_MEMORY_THREAD_SANITY_CHECK_LOCK();
+	void * const ptr = dgGlobalAllocator::m_globalAllocator.MallocLow (dgInt32 (size), align);
+	DG_MEMORY_THREAD_SANITY_CHECK_UNLOCK();
+	return ptr;
+	
+}
+
+// this can be used by function that allocates large memory pools memory locally on the stack
+// this by pases the pool allocation because this should only be used for very large memory blocks.
+// this was using virtual memory on windows but 
+// but because of many complaint I changed it to use malloc and free
+void  dgApi dgFreeStack (void* const ptr)
+{
+	DG_MEMORY_THREAD_SANITY_CHECK_LOCK();
+	dgGlobalAllocator::m_globalAllocator.FreeLow (ptr);
+	DG_MEMORY_THREAD_SANITY_CHECK_UNLOCK();
+}
+
+
+// general memory allocation for all data in the library
+void* dgApi dgMalloc (size_t size, dgMemoryAllocator* const allocator) 
+{
+	void* ptr = NULL;
+	dgAssert (allocator);
+
+	DG_MEMORY_THREAD_SANITY_CHECK_LOCK();
+	if (size) {
+		ptr = allocator->Malloc (dgInt32 (size));
+	}
+
+	DG_MEMORY_THREAD_SANITY_CHECK_UNLOCK();
+	return ptr;
+
+	
+}
+
+
+// general deletion allocation for all data in the library
+void dgApi dgFree (void* const ptr)
+{
+	if (ptr) {
+		DG_MEMORY_THREAD_SANITY_CHECK_LOCK();
+		dgMemoryAllocator::dgMemoryInfo* info;
+		info = ((dgMemoryAllocator::dgMemoryInfo*) ptr) - 1; 
+		dgAssert (info->m_allocator);
+		info->m_allocator->Free (ptr);
+		DG_MEMORY_THREAD_SANITY_CHECK_UNLOCK();
+	}
+}
+
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgMemory.h

@@ -0,0 +1,201 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgMemory__
+#define __dgMemory__
+
+#include "dgStdafx.h"
+
+#ifdef _DEBUG
+//#define __TRACK_MEMORY_LEAKS__
+#endif
+
+class dgMemoryAllocator;
+
+void* dgApi dgMalloc (size_t size, dgMemoryAllocator* const allocator);
+void  dgApi dgFree (void* const ptr);
+
+
+void* dgApi dgMallocStack (size_t size);
+void* dgApi dgMallocAligned (size_t size, dgInt32 alignmentInBytes);
+void  dgApi dgFreeStack (void* const ptr);
+
+typedef void* (dgApi *dgMemAlloc) (dgUnsigned32 size);
+typedef void (dgApi *dgMemFree) (void* const ptr, dgUnsigned32 size);
+
+
+//void dgSetMemoryDrivers (dgMemAlloc alloc, dgMemFree free);
+void dgSetGlobalAllocators (dgMemAlloc alloc, dgMemFree free);
+dgInt32 dgGetMemoryUsed ();
+
+
+#define DG_CLASS_ALLOCATOR_NEW(allocator)			inline void *operator new (size_t size, dgMemoryAllocator* const allocator) { return dgMalloc(size, allocator);}
+#define DG_CLASS_ALLOCATOR_NEW_ARRAY(allocator)		inline void *operator new[] (size_t size, dgMemoryAllocator* const allocator) { return dgMalloc(size, allocator);}
+#define DG_CLASS_ALLOCATOR_DELETE(allocator)		inline void operator delete (void* const ptr, dgMemoryAllocator* const allocator) { dgFree(ptr); }
+#define DG_CLASS_ALLOCATOR_DELETE_ARRAY(allocator)	inline void operator delete[] (void* const ptr, dgMemoryAllocator* const allocator) { dgFree(ptr); }
+#define DG_CLASS_ALLOCATOR_NEW_DUMMY				inline void *operator new (size_t size) { dgAssert (0); return dgMalloc(size, NULL);}
+#define DG_CLASS_ALLOCATOR_NEW_ARRAY_DUMMY			inline void *operator new[] (size_t size) { dgAssert (0); return dgMalloc(size, NULL);}
+#define DG_CLASS_ALLOCATOR_DELETE_DUMMY				inline void operator delete (void* const ptr) { dgFree(ptr); }
+#define DG_CLASS_ALLOCATOR_DELETE_ARRAY_DUMMY		inline void operator delete[] (void* const ptr) { dgFree(ptr); }
+
+
+#define DG_CLASS_ALLOCATOR(allocator)				\
+	DG_CLASS_ALLOCATOR_DELETE(allocator)			\
+	DG_CLASS_ALLOCATOR_DELETE_ARRAY(allocator)		\
+	DG_CLASS_ALLOCATOR_NEW(allocator)				\
+	DG_CLASS_ALLOCATOR_NEW_ARRAY(allocator)			\
+	DG_CLASS_ALLOCATOR_NEW_DUMMY					\
+	DG_CLASS_ALLOCATOR_NEW_ARRAY_DUMMY				\
+	DG_CLASS_ALLOCATOR_DELETE_DUMMY					\
+	DG_CLASS_ALLOCATOR_DELETE_ARRAY_DUMMY
+
+
+
+
+class dgMemoryAllocator
+{
+
+	#if (defined (_WIN_64_VER) || defined (_MINGW_64_VER) || defined (_POSIX_VER_64) || defined (_MACOSX_VER))
+		#define DG_MEMORY_GRANULARITY_BITS		6	
+	#else
+		#define DG_MEMORY_GRANULARITY_BITS		5	
+	#endif
+	#define DG_MEMORY_GRANULARITY				(1 << DG_MEMORY_GRANULARITY_BITS)	
+	#define DG_MEMORY_SIZE						(1024 - 64)
+	#define DG_MEMORY_BIN_SIZE					(1024 * 16)
+	#define DG_MEMORY_BIN_ENTRIES				(DG_MEMORY_SIZE / DG_MEMORY_GRANULARITY)
+
+	public: 
+
+	class dgMemoryBin
+	{
+		public: 
+		class dgMemoryBinInfo
+		{
+			public: 
+			dgInt32 m_count;
+			dgInt32 m_totalCount;
+			dgInt32 m_stepInBites;
+			dgMemoryBin* m_next;
+			dgMemoryBin* m_prev;
+		};
+
+		char m_pool[DG_MEMORY_BIN_SIZE - sizeof (dgMemoryBinInfo) - DG_MEMORY_GRANULARITY * 2];
+		dgMemoryBinInfo m_info;
+	};
+
+
+	class dgMemoryCacheEntry
+	{
+		public: 
+		dgMemoryCacheEntry* m_next;
+		dgMemoryCacheEntry* m_prev;
+	};
+
+	class dgMemoryInfo
+	{
+		public:
+		void *m_ptr;
+		dgMemoryAllocator* m_allocator;
+		dgInt32 m_size;
+		dgInt32 m_enum;
+#ifdef _DEBUG
+		dgInt32 m_workingSize;
+#endif
+
+		DG_INLINE void SaveInfo(dgMemoryAllocator* const allocator, void* const ptr, dgInt32 size, dgInt32& enumerator, dgInt32 workingSize = 0)
+		{
+			m_ptr = ptr;
+			m_size = size;
+			m_enum = enumerator;
+			enumerator ++;
+			m_allocator = allocator;
+#ifdef _DEBUG
+			m_workingSize = workingSize;
+#endif
+		}
+	};
+
+	class dgMemDirectory
+	{	
+		public: 
+		dgMemoryBin* m_first;
+		dgMemoryCacheEntry* m_cache;
+	};
+
+
+	// this is a simple memory leak tracker, it uses an flat array of two megabyte indexed by a hatch code
+#ifdef __TRACK_MEMORY_LEAKS__
+	class dgMemoryLeaksTracker
+	{
+		#define DG_TRACK_MEMORY_LEAKS_ENTRIES (1024 * 1024 * 4)
+		class Pool
+		{	
+			public: 
+			void* m_ptr;
+			dgInt32 m_size;
+			dgInt32 m_allocationNumber;
+		};
+
+		public:
+		dgMemoryLeaksTracker();
+		~dgMemoryLeaksTracker ();
+		void RemoveBlock (void* const ptr);
+		void InsertBlock (dgInt32 size, void* const ptr);
+
+		dgInt32 m_density;
+		dgInt32 m_totalAllocatedBytes; 
+		dgInt32 m_totalAllocatedCalls; 
+		dgInt32 m_leakAllocationCounter;
+		Pool m_pool[DG_TRACK_MEMORY_LEAKS_ENTRIES];
+
+	};
+#endif
+
+	dgMemoryAllocator ();
+	~dgMemoryAllocator ();
+	void *operator new (size_t size);
+	void operator delete (void* const ptr);
+	dgInt32 GetMemoryUsed() const;
+	void SetAllocatorsCallback (dgMemAlloc memAlloc, dgMemFree memFree);
+	void *MallocLow (dgInt32 size, dgInt32 alignment = DG_MEMORY_GRANULARITY);
+	void FreeLow (void* const retPtr);
+	void *Malloc (dgInt32 memsize);
+	void Free (void* const retPtr);
+
+
+	protected:
+	dgMemoryAllocator (dgMemAlloc memAlloc, dgMemFree memFree);
+
+	dgInt32 m_emumerator;
+	dgInt32 m_memoryUsed;
+	dgMemFree m_free;
+	dgMemAlloc m_malloc;
+	dgMemDirectory m_memoryDirectory[DG_MEMORY_BIN_ENTRIES + 1]; 
+
+#ifdef __TRACK_MEMORY_LEAKS__
+	dgMemoryLeaksTracker m_leaklTracker;
+#endif
+};
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgMutexThread.cpp

@@ -0,0 +1,83 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgThread.h"
+#include "dgMutexThread.h"
+
+
+dgMutexThread::dgMutexThread(const char* const name, dgInt32 id)
+	:dgThread(name, id)
+	,m_isBusy(0)
+	,m_myMutex()
+	,m_callerMutex()
+{
+	Init ();
+}
+
+dgMutexThread::~dgMutexThread(void)
+{
+	Terminate();
+}
+
+void dgMutexThread::Terminate()
+{
+	if (StillBusy()) {
+		dgInterlockedExchange(&m_terminate, 1);
+		m_callerMutex.Release();
+		m_myMutex.Release();
+		Close();
+	}
+} 
+
+
+void dgMutexThread::Execute (dgInt32 threadID)
+{
+	// suspend this tread until the call thread decide to 
+	dgAssert (threadID == m_id);
+	while (!m_terminate) {
+		// wait for the main thread to signal an update
+		dgInterlockedExchange(&m_isBusy, 0);
+		SuspendExecution(m_myMutex);
+		dgInterlockedExchange(&m_isBusy, 1);
+		if (!m_terminate) {
+			TickCallback(threadID);
+			// let main thread resume execution
+			m_callerMutex.Release();
+		}
+	}
+	dgInterlockedExchange(&m_isBusy, 0);
+}
+
+bool dgMutexThread::IsBusy() const
+{
+	return m_isBusy ? true : false;
+}
+
+void dgMutexThread::Tick()
+{
+	// let the thread run until the update function return  
+	m_myMutex.Release();
+
+	// wait for the thread function to return
+	SuspendExecution(m_callerMutex);
+}
+

newtondynamics.mod/NewtonDynamics/source/core/dgMutexThread.h

@@ -0,0 +1,49 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_MUTEX_THREAD_H__
+#define __DG_MUTEX_THREAD_H__
+
+#include "dgTypes.h"
+#include "dgThread.h"
+
+class dgMutexThread: public dgThread
+{
+	public:
+	dgMutexThread(const char* const name, dgInt32 id);
+	virtual ~dgMutexThread(void);
+
+	void Tick(); 
+	void Terminate(); 
+	bool IsBusy() const;
+
+	protected:
+	virtual void Execute (dgInt32 threadID);
+	virtual void TickCallback (dgInt32 threadID) = 0;
+
+	private:
+	dgInt32 m_isBusy;
+//	dgInt32 m_terminate;
+	dgSemaphore m_myMutex;
+	dgSemaphore m_callerMutex;
+};
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgNode.cpp

@@ -0,0 +1,231 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgNode.h"
+
+
+dgInitRtti(dgBaseNode);
+
+dgBaseNode::dgBaseNode (const dgBaseNode &clone)
+	:dgRef (clone)
+{
+	Clear ();
+
+	for (dgBaseNode* obj = clone.child; obj; obj = obj->sibling) {
+		dgBaseNode* newObj = (dgBaseNode *)obj->CreateClone ();
+		newObj->Attach (this);
+		newObj->Release();
+	}
+}
+
+
+dgBaseNode::~dgBaseNode () 
+{
+	if (child) {
+		dgBaseNode* tmp;
+		for (dgBaseNode* ptr = child; ptr && ptr->Release(); ptr = tmp) {
+			ptr->Kill();
+			tmp = ptr->sibling;
+			ptr->parent = NULL;
+			ptr->sibling = NULL;
+		}
+	}
+	
+	dgBaseNode* tmp;
+	for (dgBaseNode* ptr = sibling; ptr && ptr->Release(); ptr = tmp) {
+		ptr->Kill();
+		tmp = ptr->sibling;
+		ptr->parent = NULL;
+		ptr->sibling = NULL;
+	}
+}
+
+void dgBaseNode::CloneFixUp (const dgBaseNode &clone)
+{
+	dgAssert (GetNameID() == clone.GetNameID());
+
+	dgBaseNode* cloneChild = clone.GetChild();
+	for (dgBaseNode* obj = child; obj; obj = obj->sibling) {
+		obj->CloneFixUp (*cloneChild);
+		cloneChild = cloneChild->GetSibling();
+	}
+}
+
+
+void dgBaseNode::Attach (dgBaseNode *parentArg, bool addFirst)
+{
+	dgAssert (!parent);
+	dgAssert (!sibling);
+	dgAssert (parentArg);
+
+	
+	parent = parentArg;
+	if (parent->child) {
+		if (addFirst) {
+			sibling = parent->child;
+			parent->child = this;
+		} else {
+			dgBaseNode* obj = parent->child;
+			for (; obj->sibling; obj = obj->sibling)
+			{
+				
+			}
+			obj->sibling = this;
+		}
+	} else {
+		parent->child = this;
+	}
+
+	AddRef();
+}
+
+
+void dgBaseNode::Detach ()
+{
+ 	if (parent) {
+		if (parent->child == this) {
+			parent->child = sibling;
+		} else {
+			dgBaseNode* ptr = parent->child;
+			for (; ptr->sibling != this; ptr = ptr->sibling)
+			{
+				
+			}
+			ptr->sibling = sibling;
+		}
+		parent = NULL;
+		sibling = NULL;
+		Release();
+	}
+}
+	
+
+dgBaseNode* dgBaseNode::GetRoot() const
+{
+	const dgBaseNode* root = this;
+	for (; root->parent; root = root->parent)
+	{
+		
+	}
+	return (dgBaseNode*)root;
+}
+
+
+dgBaseNode* dgBaseNode::GetFirst() const
+{
+	dgBaseNode* ptr = (dgBaseNode *)this;
+	for (; ptr->child; ptr = ptr->child)
+	{
+		
+	}
+	return ptr;
+}
+
+dgBaseNode* dgBaseNode::GetNext() const
+{
+	if (sibling) {
+		return sibling->GetFirst();
+	}
+
+	dgBaseNode* ptr = parent;
+	dgBaseNode* x = (dgBaseNode *)this;
+	for (; ptr && (x == ptr->sibling); ptr = ptr->parent) {
+		x = ptr;
+	}
+	return ptr;
+}
+
+
+
+dgBaseNode* dgBaseNode::GetLast() const
+{
+
+	dgBaseNode* ptr = (dgBaseNode *)this;	
+	for (ptr = (dgBaseNode *)this; ptr->sibling; ptr = ptr->sibling)
+	{
+		
+	}
+	return ptr;
+}
+
+
+dgBaseNode* dgBaseNode::GetPrev() const
+{
+	if (child) {
+		return child->GetNext();
+	}
+
+	dgBaseNode* ptr = parent;
+	dgBaseNode* x = (dgBaseNode *)this;
+	for (; ptr && (x == ptr->child); ptr = ptr->child) {
+		x = ptr;
+	}
+	return ptr;
+}
+
+
+
+dgBaseNode* dgBaseNode::Find (dgUnsigned32 nameCRC) const 
+{
+	dgBaseNode *ptr = GetFirst();
+	for (; ptr; ptr = ptr->GetNext()) {
+		if (nameCRC == ptr->GetNameID()) {
+			break;
+		}
+	}
+	return ptr;
+}
+
+bool dgBaseNode::SanityCheck()
+{
+	return true;
+}
+
+void dgBaseNode::PrintHierarchy (
+	dgFile &file, 
+	char *indent) const 
+{
+	char newIndent[1024];
+
+	sprintf (newIndent, "%s   ", indent);
+	for (dgBaseNode *node = child; node; node = node->sibling) {
+		node->PrintHierarchy (file, newIndent);
+	} 
+}
+
+
+void dgBaseNode::DebugPrint (const char *fileName)
+{
+/*
+	char indent[512];
+
+	indent[0] = '\0';
+	dgFile file (fileName, "w");
+	PrintHierarchy (file, indent); 
+*/
+}
+
+
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgNode.h

@@ -0,0 +1,262 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgNode__
+#define __dgNode__
+
+#include "dgStdafx.h"
+#include "dgCRC.h"
+#include "dgRef.h"
+#include "dgRtti.h"
+
+
+class dgFile;
+//enum dgSaveType;
+
+
+class dgBaseNode: public dgRef
+{
+	public:
+	dgBaseNode *GetChild () const;
+	dgBaseNode *GetParent () const;
+	dgBaseNode *GetSibling () const;
+
+	void Detach ();
+	void Attach (dgBaseNode* const parent, bool addFirst = false);
+	
+	dgBaseNode *GetRoot () const;
+	dgBaseNode *GetFirst() const;
+	dgBaseNode *GetLast() const;
+	dgBaseNode *GetNext() const;
+	dgBaseNode *GetPrev() const;
+
+	dgBaseNode *Find (dgUnsigned32 nameCRC) const; 
+	dgBaseNode *Find (const char* const name) const;
+	void DebugPrint (const char* const fileName); 
+	bool SanityCheck();
+
+
+	protected:
+	dgBaseNode ();
+	dgBaseNode (const char* const name);
+	dgBaseNode (const dgBaseNode &clone);
+	~dgBaseNode ();
+
+//	virtual void Save (dgFile &file, dgSaveType saveType, void* const context) const; 
+	virtual void CloneFixUp (const dgBaseNode &clone);
+	virtual void PrintHierarchy (dgFile &file, char* indentation) const; 
+
+	private:
+	inline void Clear();
+
+	dgAddRtti(dgRef);
+	dgBaseNode* parent;
+	dgBaseNode* child;
+	dgBaseNode* sibling;
+
+};
+
+template<class T>
+class dgNode: public dgBaseNode
+{
+	public:
+	dgNode ();
+	dgNode (const char* const name);
+	void Attach (T* parent, bool addFirst = false);
+	void Detach ();
+	T* GetChild () const;
+	T* GetSibling () const;
+	T* GetParent () const;
+	T* GetRoot () const;
+	T* GetFirst() const;
+	T* GetLast() const;
+	T* GetNext() const;
+	T* GetPrev() const;
+	T* Find (dgUnsigned32 nameCRC) const;
+	T* Find (const char* const name) const;
+
+	protected:
+	dgNode (const T &clone);
+	virtual ~dgNode ();
+	dgRef *CreateClone () const;
+
+};
+
+
+
+
+
+inline dgBaseNode::dgBaseNode ()
+	:dgRef ()
+{
+	Clear ();
+}
+
+inline dgBaseNode::dgBaseNode (const char* const name)
+	:dgRef (name)
+{
+	Clear ();
+}
+
+
+inline void dgBaseNode::Clear()
+{
+	child = NULL;
+	parent = NULL;
+	sibling = NULL;
+}
+
+
+inline dgBaseNode *dgBaseNode::GetChild () const
+{
+	return child;
+}
+
+inline dgBaseNode *dgBaseNode::GetSibling () const
+{
+	return sibling;
+}
+
+inline dgBaseNode *dgBaseNode::GetParent () const
+{
+	return parent;
+}
+
+
+inline dgBaseNode *dgBaseNode::Find (const char* const name) const
+{
+	return Find (dgCRC (name)); 
+} 
+
+
+
+
+template<class T>
+dgNode<T>::dgNode ()
+	:dgBaseNode ()
+{
+}
+
+template<class T>
+dgNode<T>::dgNode (const T &clone)
+	:dgBaseNode (clone)
+{
+}
+
+template<class T>
+dgNode<T>::dgNode (const char* const name)
+	:dgBaseNode (name)
+{
+}
+
+template<class T>
+dgNode<T>::~dgNode () 
+{
+}
+
+
+template<class T>
+dgRef *dgNode<T>::CreateClone () const
+{
+	return new T (*(T*)this);
+}
+
+template<class T>
+void dgNode<T>::Attach (T* const parent, bool addFirst)
+{
+	dgBaseNode::Attach(parent, addFirst);
+}
+
+template<class T>
+void dgNode<T>::Detach ()
+{
+	dgBaseNode::Detach ();
+}
+
+template<class T>
+T* dgNode<T>::GetChild () const
+{
+	return (T*) dgBaseNode::GetChild();
+}
+
+template<class T>
+T* dgNode<T>::GetSibling () const
+{
+	return (T*) dgBaseNode::GetSibling ();
+}
+
+template<class T>
+T* dgNode<T>::GetParent () const
+{
+	return (T*) dgBaseNode::GetParent ();
+}
+
+
+template<class T>
+T* dgNode<T>::GetRoot () const
+{
+	return (T*) dgBaseNode::GetRoot ();
+}
+
+
+template<class T>
+T* dgNode<T>::GetFirst() const
+{
+	return (T*) dgBaseNode::GetFirst ();
+}
+
+template<class T>
+T* dgNode<T>::GetLast() const
+{
+	return (T*) dgBaseNode::GetLast ();
+}
+
+
+template<class T>
+T* dgNode<T>::GetNext() const
+{
+	return (T*) dgBaseNode::GetNext ();
+}
+
+template<class T>
+T* dgNode<T>::GetPrev() const
+{
+	return (T*) dgBaseNode::GetPrev ();
+}
+
+
+template<class T>
+T* dgNode<T>::Find (dgUnsigned32 nameCRC) const 
+{
+	return (T*) dgBaseNode::Find (nameCRC);
+}
+
+template<class T>
+T* dgNode<T>::Find (const char* const name) const
+{
+	return (T*) dgBaseNode::Find (name);
+} 
+
+
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgObb.cpp

@@ -0,0 +1,864 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgObb.h"
+#include "dgDebug.h"
+#include "dgPlane.h"
+#include "dgMatrix.h"
+
+
+namespace InternalSphere
+{
+	const dgFloat32 SPHERE_TOL = 0.002f;	
+
+	static dgFloat32 AspectRatio (dgFloat32 x, dgFloat32 y)
+	{
+		dgFloat32 tmp;
+		x = dgAbsf (x);
+		y = dgAbsf (y);
+
+		if (y < x) {
+			tmp = y;
+			y = x;
+			x = tmp;
+		}
+		if (y < 1.0e-12)	{
+			y = 1.0e-12f;
+		}
+		return x / y;
+	}
+	
+
+	static void BoundingBox (const dgMatrix& matrix, const dgFloat32 vertex[], dgInt32 stride, const dgInt32 index[], dgInt32 indexCount, dgVector &min, dgVector &max)
+	{
+	   dgFloat32 xmin = dgFloat32 (1.0e10f);
+	   dgFloat32 ymin = dgFloat32 (1.0e10f);
+	   dgFloat32 zmin = dgFloat32 (1.0e10f);
+
+	   dgFloat32 xmax = dgFloat32 (-1.0e10f);
+	   dgFloat32 ymax = dgFloat32 (-1.0e10f);
+	   dgFloat32 zmax = dgFloat32 (-1.0e10f);
+
+	   const dgFloat32*	const ptr = vertex;
+	   for (dgInt32 j = 0 ; j < indexCount; j ++ ) {
+			dgInt32 i = index[j] * stride;
+			dgVector tmp (ptr[i + 0], ptr[i + 1], ptr[i + 2], dgFloat32 (0.0f));
+			tmp = matrix.UnrotateVector (tmp);
+			if (tmp.m_x < xmin) xmin = tmp.m_x;
+			if (tmp.m_y < ymin) ymin = tmp.m_y;
+			if (tmp.m_z < zmin) zmin = tmp.m_z;
+			if (tmp.m_x > xmax) xmax = tmp.m_x;
+			if (tmp.m_y > ymax) ymax = tmp.m_y;
+			if (tmp.m_z > zmax) zmax = tmp.m_z;
+	   }
+		
+	   min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
+	   max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
+	}
+
+	// Compute axis aligned box
+	static void BoundingBox (const dgMatrix &Mat, const dgFloat32 vertex[], dgInt32 vertexCount, dgInt32 stride, dgVector &min, dgVector &max)
+	{
+		dgFloat32 xmin = dgFloat32 (1.0e10f);
+		dgFloat32 ymin = dgFloat32 (1.0e10f);
+		dgFloat32 zmin = dgFloat32 (1.0e10f);
+
+		dgFloat32 xmax = dgFloat32 (-1.0e10f);
+		dgFloat32 ymax = dgFloat32 (-1.0e10f);
+		dgFloat32 zmax = dgFloat32 (-1.0e10f);
+
+		const dgFloat32* ptr = vertex;
+		for (dgInt32 i = 0 ; i < vertexCount; i ++ ) {
+			dgVector tmp (ptr[0], ptr[1], ptr[2], dgFloat32 (0.0f));
+			ptr += stride;
+			tmp = Mat.UnrotateVector (tmp);
+			if (tmp.m_x < xmin) xmin = tmp.m_x;
+			if (tmp.m_y < ymin) ymin = tmp.m_y;
+			if (tmp.m_z < zmin) zmin = tmp.m_z;
+			if (tmp.m_x > xmax) xmax = tmp.m_x;
+			if (tmp.m_y > ymax) ymax = tmp.m_y;
+			if (tmp.m_z > zmax) zmax = tmp.m_z;
+		}
+
+		min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
+		max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
+	}
+
+
+	static void Statistics (dgObb &sphere, dgVector &eigenValues, dgVector &scaleVector, const dgFloat32 vertex[], const dgInt32 faceIndex[], dgInt32 indexCount, dgInt32 stride)
+	{
+		dgVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector centre (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+
+		dgFloat64 totalArea = dgFloat32 (0.0f);
+		const dgFloat32* const ptr = vertex;
+		for (dgInt32 i = 0; i < indexCount; i += 3) {
+			dgInt32 index = faceIndex[i] * stride;
+			dgVector p0 (&ptr[index]);
+			p0 = p0.CompProduct3 (scaleVector);
+
+			index = faceIndex[i + 1] * stride;;
+			dgVector p1 (&ptr[index]);
+			p1 = p1.CompProduct3 (scaleVector);
+
+			index = faceIndex[i + 2] * stride;;
+			dgVector p2 (&ptr[index]);
+			p2 = p2.CompProduct3 (scaleVector);
+
+			dgVector normal ((p1 - p0) * (p2 - p0));
+
+			dgFloat64 area = dgFloat32 (0.5f) * sqrt (normal % normal);
+
+			centre = p0 + p1 + p2;
+			centre = centre.Scale3 (dgFloat32  (1.0f / 3.0f));
+
+			// Inertia of each point in the triangle
+			dgFloat64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;	
+			dgFloat64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;	
+			dgFloat64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;	
+
+			dgFloat64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;	
+			dgFloat64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;	
+			dgFloat64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;	
+
+			if (area > dgEPSILON * 10.0) {
+				dgFloat64 K = area / dgFloat64 (12.0);
+				//Coriolis theorem for Inertia of a triangle in an arbitrary orientation
+				Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
+				Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
+				Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
+
+				Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
+				Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
+				Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
+				centre = centre.Scale3 ((dgFloat32)area);
+			} 
+
+			totalArea += area;
+			massCenter += centre;
+			var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz, dgFloat32 (0.0f));
+			cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz, dgFloat32 (0.0f));
+		}
+
+		if (totalArea > dgEPSILON * 10.0) {
+			dgFloat64 K = dgFloat64 (1.0) / totalArea; 
+			var = var.Scale3 ((dgFloat32)K);
+			cov = cov.Scale3 ((dgFloat32)K);
+			massCenter = massCenter.Scale3 ((dgFloat32)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);
+	}
+
+	
+	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);
+	}
+
+/*
+	static void Statistics (
+		dgObb &sphere,
+		dgVector &eigenValues,
+		const dgVector &scaleVector,
+		const dgFloat32 vertex[],
+		dgInt32 stride,
+		const dgFace face[],
+		dgInt32 faceCount)
+	{
+		dgAssert (0);
+
+		dgInt32 i;
+		dgInt32 index;
+		const dgFloat32 *ptr;
+		dgFloat64 K;
+		dgFloat64 Ixx;
+		dgFloat64 Iyy;
+		dgFloat64 Izz;
+		dgFloat64 Ixy;
+		dgFloat64 Ixz;
+		dgFloat64 Iyz;
+		dgFloat64 area;
+		dgFloat64 totalArea;
+		const dgFace *Face;
+	
+		dgVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector centre (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		
+		totalArea = 0.0;
+		ptr = vertex;
+		for (i = 0; i < faceCount; i ++) {
+			 Face = &face[i];
+		
+			 index = Face->m_wireFrame[0] * stride;
+			 dgVector p0 (&ptr[index]);
+			 p0 = p0.CompProduct3 (scaleVector);
+	
+			 index = Face->m_wireFrame[1] * stride;
+			 dgVector p1 (&ptr[index]);
+			 p1 = p1.CompProduct3 (scaleVector);
+		
+			 index = Face->m_wireFrame[2] * stride;
+			 dgVector p2 (&ptr[index]);
+			 p2 = p2.CompProduct3 (scaleVector);
+		
+			 dgVector normal ((p1 - p0) * (p2 - p0));
+		
+			 area = 0.5 * sqrt (normal % normal);
+		
+			 centre = p0 + p1 + p2;
+			 centre = centre.Scale3 (1.0f / 3.0f);
+		
+			 // Inercia of each point in the triangle
+			 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;	
+			 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;	
+			 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;	
+		
+			 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;	
+			 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;	
+			 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;	
+		
+			 if (area > dgEPSILON * 10.0) {
+			     K = area / 12.0;
+			     //Coriollis teorem for Inercia of a triangle in an arbitrary orientation
+			     Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
+			     Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
+			     Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
+		
+			     Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
+			     Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
+			     Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
+			     centre = centre.Scale3 ((dgFloat32)area);
+			 } 
+		
+			 totalArea += area;
+			 massCenter += centre;
+			 var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz);
+			 cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz);
+		}
+		
+		if (totalArea > dgEPSILON * 10.0) {
+			K = 1.0 / totalArea; 
+			var = var.Scale3 ((dgFloat32)K);
+			cov = cov.Scale3 ((dgFloat32)K);
+			massCenter = massCenter.Scale3 ((dgFloat32)K);
+		}
+	
+		Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
+		Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
+		Izz = var.m_z - massCenter.m_z * massCenter.m_z;
+		
+		Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
+		Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
+		Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
+	
+		sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz);
+		sphere.m_up    = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz);
+		sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz);
+		sphere.EigenVectors(eigenValues);
+	}
+*/
+}
+
+
+
+void dgObb::SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, const dgInt32 triangles[], dgInt32 indexCount, const dgMatrix *basis)
+{
+	dgVector eigen;
+	dgVector scaleVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
+
+	if (indexCount < 3) {
+		return;
+	}
+
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+	if (!basis)	{
+
+		InternalSphere::Statistics (*this, eigen, scaleVector, vertex, triangles, indexCount, stride);
+
+		dgInt32 k = 0;
+		for (dgInt32 i = 0; i < 3; i ++) {
+			if (k >= 6) {
+				 break;
+			}
+			for (dgInt32 j = i + 1; j < 3; j ++) {
+				dgFloat32 aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
+				if (aspect > dgFloat32 (0.9f)) {
+					scaleVector[i] *= dgFloat32 (2.0f); 
+					InternalSphere::Statistics (*this, eigen, scaleVector, vertex, triangles, indexCount, stride);
+					k ++;
+					i = -1;
+					break;
+				}
+			}
+		}
+	} else {
+		*this = *basis;
+	}
+
+	dgVector min; 
+	dgVector max;
+	InternalSphere::BoundingBox (*this, vertex, stride, triangles, indexCount, min, max);
+
+	dgVector massCenter	(max + min);
+	massCenter = massCenter.Scale3 (dgFloat32 (0.5f));
+	m_posit = TransformVector (massCenter);
+
+	dgVector dim (max - min);
+	dim = dim.Scale3 (dgFloat32(0.5f));
+	SetDimensions (dim.m_x, dim.m_y, dim.m_z);
+}
+
+
+void dgObb::SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, dgInt32 count, const dgMatrix *basis) 
+{
+	dgVector eigen;
+	dgVector scaleVector (dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32 (0.0f));
+
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+	if (!basis)	{
+		InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
+
+		dgInt32 k = 0;
+		for (dgInt32 i = 0; i < 3; i ++) {
+			if (k >= 6) {
+				 break;
+			}
+			for (dgInt32 j = i + 1; j < 3; j ++) {
+				dgFloat32 aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
+				if (aspect > dgFloat32 (0.9f)) {
+					scaleVector[i] *= dgFloat32 (2.0f); 
+					InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
+					k ++;
+					i = -1;
+					break;
+				}
+			}
+		}
+	} else {
+		*this = *basis;
+	}
+		
+   dgVector min; 
+   dgVector max;
+   InternalSphere::BoundingBox (*this, vertex, count, stride, min, max);
+
+	dgVector massCenter (max + min);
+	massCenter = massCenter.Scale3 (0.5);
+	m_posit = TransformVector (massCenter);
+
+	dgVector dim (max - min);
+	dim = dim.Scale3 (dgFloat32(0.5f));
+	SetDimensions (dim.m_x + InternalSphere::SPHERE_TOL, 
+				   dim.m_y + InternalSphere::SPHERE_TOL, 
+				   dim.m_z + InternalSphere::SPHERE_TOL);
+
+}
+
+/*
+void dgObb::SetDimensions (
+	const dgFloat32 vertex[],  
+	dgInt32 strideInBytes,
+	const dgInt32 index[],
+	dgInt32 indexCount,
+	const dgMatrix *basis)
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	dgInt32 stride;
+	dgFloat32 aspect;
+	dgVector eigen;
+	dgVector scaleVector (dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32 (0.0f));
+
+	stride = strideInBytes / sizeof (dgFloat32);
+	if (!basis)	{
+		InternalSphere::Statistics (*this, eigen, scaleVector, vertex, index, indexCount, stride);
+
+		k = 0;
+		for (i = 0; i < 3; i ++) {
+			if (k >= 6) {
+				 break;
+			}
+			for (j = i + 1; j < 3; j ++) {
+				aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
+				if (aspect > dgFloat32 (0.9f)) {
+					scaleVector[i] *= dgFloat32 (2.0f); 
+					InternalSphere::Statistics (*this, eigen, scaleVector, vertex, index, indexCount, stride);
+					i = -1;
+					k ++;
+					break;
+				}
+			}
+		}
+	} else {
+		*this = *basis;
+	}
+		
+   dgVector min; 
+   dgVector max;
+   InternalSphere::BoundingBox (*this, vertex, stride, index, indexCount, min, max);
+
+	dgVector massCenter (max + min);
+	massCenter = massCenter.Scale3 (dgFloat32(0.5f));
+	m_posit = TransformVector (massCenter);
+
+	dgVector dim (max - min);
+	dim = dim.Scale3 (dgFloat32(0.5f));
+	SetDimensions (dim.m_x + InternalSphere::SPHERE_TOL, 
+				   dim.m_y + InternalSphere::SPHERE_TOL, 
+				   dim.m_z + InternalSphere::SPHERE_TOL);
+}
+*/
+
+
+
+/*
+dgObb::dgObb (
+	const dgObb &dgObb, 
+	const dgVector &Dir)
+{
+	if ((Dir % Dir) < EPSILON * 0.01f) {
+		*this = dgObb;
+		return;
+	}
+
+	front = Dir;
+	front.Fast_Normalize(); 
+
+	if (dgAbsf (front % dgObb.right) < 0.995) {
+		up = front * dgObb.right;
+		up.Fast_Normalize(); 
+	} else {
+		up	= dgObb.up;
+	}
+	right = up * front;
+
+	dgVector Step (Dir.Scale3(0.5));
+	size.m_x = (dgFloat32)(dgObb.size.m_x * dgAbsf (right % dgObb.right) +
+						 dgObb.size.m_y * dgAbsf (right % dgObb.up) +
+						 dgObb.size.m_z * dgAbsf (right % dgObb.front));
+
+	size.m_y = (dgFloat32)(dgObb.size.m_x * dgAbsf (up % dgObb.right) +
+						  dgObb.size.m_y * dgAbsf (up % dgObb.up) +
+						  dgObb.size.m_z * dgAbsf (up % dgObb.front));
+
+	size.m_z = (dgFloat32)(sqrt (Step % Step) +
+							dgObb.size.m_x * dgAbsf (front % dgObb.right) +
+							dgObb.size.m_y * dgAbsf (front % dgObb.up) +
+							dgObb.size.m_z * dgAbsf (front % dgObb.front));
+	posit = dgObb.posit + Step;
+
+}
+
+
+
+bool dgObb::dgObb_Overlap_Test (const dgObb &dgObb)
+{
+	dgFloat64 R;
+	dgVector Dir (dgObb.posit - posit);
+
+  	R = size.m_x * dgAbsf (right % Dir) + dgObb.size.m_x * dgAbsf (dgObb.right % Dir) + 
+	    size.m_y * dgAbsf (up % Dir) + dgObb.size.m_y * dgAbsf (dgObb.up % Dir) + 
+		 size.m_z * dgAbsf (front %Dir) + dgObb.size.m_z * dgAbsf (dgObb.front % Dir);
+	if (R	< (Dir % Dir)) {
+		return false;
+	}
+
+	R = size.m_x * dgAbsf (right % dgObb.right) + 
+	    size.m_y * dgAbsf (up % dgObb.right) + 
+		 size.m_z * dgAbsf (front % dgObb.right) + dgObb.size.m_x;
+	if (R	< dgAbsf (Dir % dgObb.right)) {
+		return false;
+	}
+
+	R = size.m_x * dgAbsf (right % dgObb.up) + 
+	    size.m_y * dgAbsf (up % dgObb.up) + 
+		 size.m_z * dgAbsf (front % dgObb.up) + dgObb.size.m_y;
+	if (R	< dgAbsf (Dir % dgObb.up)) {
+		return false;
+	}
+
+	R = size.m_x * dgAbsf (right % dgObb.front) + 
+	    size.m_y * dgAbsf (up % dgObb.front) + 
+		 size.m_z * dgAbsf (front % dgObb.front) + dgObb.size.m_z; 
+	if (R	< dgAbsf (Dir % dgObb.front)) {
+		return false;
+	}
+
+	R = dgObb.size.m_x * dgAbsf (dgObb.right % right) + 
+	    dgObb.size.m_y * dgAbsf (dgObb.up % right) + 
+		 dgObb.size.m_z * dgAbsf (dgObb.front % right) + size.m_x;
+	if (R	< dgAbsf (Dir % right)) {
+		return false;
+	}
+
+	R = dgObb.size.m_x * dgAbsf (dgObb.right % up) + 
+	    dgObb.size.m_y * dgAbsf (dgObb.up % up) + 
+		 dgObb.size.m_z * dgAbsf (dgObb.front % up) + size.m_y;
+	if (R	< dgAbsf (Dir % up)) {
+		return false;
+	}
+
+	R = dgObb.size.m_x * dgAbsf (dgObb.right % front) + 
+	    dgObb.size.m_y * dgAbsf (dgObb.up % front) + 
+		 dgObb.size.m_z * dgAbsf (dgObb.front % front) + size.m_z;
+	if (R	< dgAbsf (Dir % front)) {
+		return false;
+	}
+
+	return true;
+}
+
+
+void dgObb::Swept_Volume (
+	dgVector &min, 
+	dgVector &max)
+{
+	dgFloat32 w;
+	dgFloat32 h;
+	dgFloat32 b;
+
+	w = (dgFloat32)(size.m_x * dgAbsf(right.m_x) + size.m_y * dgAbsf(up.m_x) + size.m_z * dgAbsf(front.m_x));  
+	h = (dgFloat32)(size.m_x * dgAbsf(right.m_y) + size.m_y * dgAbsf(up.m_y) + size.m_z * dgAbsf(front.m_y));  
+	b = (dgFloat32)(size.m_x * dgAbsf(right.m_z) + size.m_y * dgAbsf(up.m_z) + size.m_z * dgAbsf(front.m_z));  
+
+	min.m_x = posit.m_x - w;
+	min.m_y = posit.m_y - h;
+	min.m_z = posit.m_z - b;
+
+	max.m_x = posit.m_x + w;
+	max.m_y = posit.m_y + h;
+	max.m_z = posit.m_z + b;
+}
+*/
+
+
+/*
+dgInt32 dgObb::FrontTest (
+	const dgMatrix& matrix, 
+	const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbsf (matrix.m_front.m_x) + m_size.m_y * dgAbsf (matrix.m_up.m_x) + m_size.m_z * dgAbsf (matrix.m_right.m_x);
+	R = plane[5].m_x * matrix.m_posit.m_x + plane[5].m_w; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = flag0.i >> 30 & 2;
+	flag1.i = flag1.i >> 31 & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::RearTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbsf (matrix.m_front.m_x) + m_size.m_y * dgAbsf (matrix.m_up.m_x) + m_size.m_z * dgAbsf (matrix.m_right.m_x);
+	R = plane[4].m_x * matrix.m_posit.m_x + plane[4].m_w; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = flag0.i >> 30 & 2;
+	flag1.i = flag1.i >> 31 & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+
+dgInt32 dgObb::LeftTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbsf (matrix.m_front.m_x * plane[0].m_x + matrix.m_front.m_z * plane[0].m_z) + 
+		 m_size.m_y * dgAbsf (matrix.m_up.m_x    * plane[0].m_x + matrix.m_up.m_z    * plane[0].m_z) +
+		 m_size.m_z * dgAbsf (matrix.m_right.m_x * plane[0].m_x + matrix.m_right.m_z * plane[0].m_z);
+	R = plane[0].m_x * matrix.m_posit.m_x + plane[0].m_z * matrix.m_posit.m_z; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::RightTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbsf (matrix.m_front.m_x * plane[1].m_x + matrix.m_front.m_z * plane[1].m_z) + 
+		 m_size.m_y * dgAbsf (matrix.m_up.m_x    * plane[1].m_x + matrix.m_up.m_z    * plane[1].m_z) +
+		 m_size.m_z * dgAbsf (matrix.m_right.m_x * plane[1].m_x + matrix.m_right.m_z * plane[1].m_z);
+	R = plane[1].m_x * matrix.m_posit.m_x + plane[1].m_z * matrix.m_posit.m_z; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::BottomTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbsf (matrix.m_front.m_x * plane[2].m_x + matrix.m_front.m_y * plane[2].m_y) + 
+		 m_size.m_y * dgAbsf (matrix.m_up.m_x    * plane[2].m_x + matrix.m_up.m_y    * plane[2].m_y) +
+		 m_size.m_z * dgAbsf (matrix.m_right.m_x * plane[2].m_x + matrix.m_right.m_y * plane[2].m_y);
+
+	R = plane[2].m_x * matrix.m_posit.m_x + plane[2].m_y * matrix.m_posit.m_y; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+dgInt32 dgObb::TopTest (const dgMatrix& matrix, const dgPlane* plane) const
+{
+	dgFloat32 R;
+	dgFloat32 dR;
+	InternalSphere::dgFloatSign flag0;
+	InternalSphere::dgFloatSign flag1;
+
+	dR = m_size.m_x * dgAbsf (matrix.m_front.m_x * plane[3].m_x + matrix.m_front.m_y * plane[3].m_y) + 
+		 m_size.m_y * dgAbsf (matrix.m_up.m_x    * plane[3].m_x + matrix.m_up.m_y    * plane[3].m_y) +
+		 m_size.m_z * dgAbsf (matrix.m_right.m_x * plane[3].m_x + matrix.m_right.m_y * plane[3].m_y);
+
+	R = plane[3].m_x * matrix.m_posit.m_x + plane[3].m_y * matrix.m_posit.m_y; 
+
+	flag0.f = R + dR;
+	flag1.f = R - dR;
+	flag0.i = (flag0.i >> 30) & 2;
+	flag1.i = (flag1.i >> 31) & 1;
+	return InternalSphere::CodeTbl[flag0.i | flag1.i];
+}
+
+
+
+dgInt32 dgObb::VisibilityTestLow (
+	const dgCamera* camera,
+	const dgMatrix& matrix) const
+{
+	dgInt32 i;
+	dgInt32 code;
+	const dgPlane* planes;
+	const dgPlane* guardPlanes;
+
+	planes = camera->GetViewVolume();
+
+	code = (this->*planeTest) (matrix, planes);
+	if (code != -1) {
+		for (i = 0; i < 6; i ++) {
+			code |= (this->*planeTestArray[i]) (matrix, planes);
+			if (code == -1) {
+				planeTest = planeTestArray[i];
+				return -1;
+			}
+		}
+
+		if (code) {
+			guardPlanes = camera->GetGuardViewVolume();
+			if (guardPlanes) {
+				code = 0;
+				for (i = 0; i < 6; i ++) {
+					code |= (this->*planeTestArray[i]) (matrix, guardPlanes);
+					dgAssert (code >= 0);
+					if (code) {
+						return code;
+					}
+				}
+			}
+		}
+	}
+
+   return code;
+}
+
+
+dgInt32 dgObb::VisibilityTest (const dgCamera* camera) const
+{
+	dgMatrix viewMatrix (*this * camera->GetViewMatrix());
+	return VisibilityTestLow (camera, viewMatrix);
+}
+
+dgInt32 dgObb::VisibilityTest (const dgCamera* camera, const dgMatrix &worldMatrix) const 
+{
+	dgMatrix viewMatrix (*this * worldMatrix * camera->GetViewMatrix());
+	return VisibilityTestLow (camera, viewMatrix);
+}
+
+void dgObb::Render (
+	const dgCamera* camera, 
+	const dgMatrix &worldMatrix, 
+	dgUnsigned32 rgb) const
+{
+	dgInt32 i;
+	struct ColorVertex
+	{
+		dgFloat32 m_x;
+		dgFloat32 m_y;
+		dgFloat32 m_z;
+		dgColor m_color;
+	};
+
+	dgUnsigned32 index [][2] = {
+		{0, 4}, {1, 5}, {2, 6}, {3, 7},
+		{0, 1}, {4, 5}, {7, 6}, {3, 2},
+		{1, 2}, {5, 6}, {4, 7}, {0, 3},
+	};
+
+	ColorVertex* ptr;
+	ColorVertex box[8];
+
+	box[0].m_x = -size.m_x;
+	box[0].m_y = -size.m_y;
+	box[0].m_z = -size.m_z;
+	box[0].m_color.m_val = rgb;
+
+	box[1].m_x =  size.m_x;
+	box[1].m_y = -size.m_y;
+	box[1].m_z = -size.m_z;
+	box[1].m_color.m_val = rgb;
+
+	box[2].m_x =  size.m_x;
+	box[2].m_y = -size.m_y;
+	box[2].m_z =  size.m_z;
+	box[2].m_color.m_val = rgb;
+
+	box[3].m_x = -size.m_x;
+	box[3].m_y = -size.m_y;
+	box[3].m_z =  size.m_z;
+	box[3].m_color.m_val = rgb;
+
+	box[4].m_x = -size.m_x;
+	box[4].m_y =  size.m_y;
+	box[4].m_z = -size.m_z;
+	box[4].m_color.m_val = rgb;
+
+	box[5].m_x =  size.m_x;
+	box[5].m_y =  size.m_y;
+	box[5].m_z = -size.m_z;
+	box[5].m_color.m_val = rgb;
+
+	box[6].m_x =  size.m_x;
+	box[6].m_y =  size.m_y;
+	box[6].m_z =  size.m_z;
+	box[6].m_color.m_val = rgb;
+
+	box[7].m_x = -size.m_x;
+	box[7].m_y =  size.m_y;
+	box[7].m_z =  size.m_z;
+	box[7].m_color.m_val = rgb;
+
+	dgRenderDescriptorParams param;
+	param.m_indexCount = 0;
+	param.m_vertexCount = sizeof (index) / sizeof (dgInt32); 
+	param.m_descType = dgDynamicVertex;
+	param.m_primitiveType = RENDER_LINELIST;
+	param.m_vertexFlags = VERTEX_ENABLE_XYZ | COLOR_ENABLE;
+
+	dgRenderDescriptor desc (param);
+
+	dgMatrix tmpMat (*this * worldMatrix);
+	camera->SetWorldMatrix (&tmpMat);
+
+	desc.m_material = dgMaterial::UseDebugMaterial();
+
+  	dgVertexRecord vertexRecord (desc.LockVertex());
+	ptr = (ColorVertex*) vertexRecord.vertex.ptr;
+	for (i = 0; i < (sizeof (index) / (2 * sizeof (dgUnsigned32))); i ++) {
+		 ptr[0] = box[index[i][0]]; 
+		 ptr[1] = box[index[i][1]]; 
+		 ptr += 2;
+	}
+	desc.UnlockVertex();
+
+	camera->Render (desc);
+
+	desc.m_material->Release();
+}
+*/
+

newtondynamics.mod/NewtonDynamics/source/core/dgObb.h

@@ -0,0 +1,115 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgOOBB_H__
+#define __dgOOBB_H__
+
+#include "dgStdafx.h"
+#include "dgTypes.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+
+class dgPlane;
+
+DG_MSC_VECTOR_ALIGMENT
+class dgObb: public dgMatrix
+{
+	public:
+	DG_INLINE dgObb (){};
+	dgObb (const dgQuaternion &quat, const dgVector &position, const dgVector& dim = dgVector(0, 0, 0, 0));
+	dgObb (const dgMatrix& matrix, const dgVector& dim = dgVector(0, 0, 0, 0));
+
+	dgObb &operator= (const dgMatrix &arg);
+	void Scale (dgFloat32 Ws, dgFloat32 Hs, dgFloat32 Bs) ;
+	void SetDimensions (dgFloat32 W, dgFloat32 H, dgFloat32 B);
+	void SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, dgInt32 vertexCount, const dgMatrix *basis = NULL);
+	void SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, const dgInt32 triangles[], dgInt32 indexCount, const dgMatrix *basis);
+//	void SetDimensions (const dgFloat32 vertex[], dgInt32 strideInBytes, const dgInt32 index[], dgInt32 indexCount, const dgMatrix *basis = NULL);
+
+	// return:  0 if the sphere is wholly inside the view port
+	//          1 if the sphere is partially inside the view port
+	//         -1 if the sphere is wholly outside the view port
+//	dgInt32 VisibilityTest (const dgCamera* camera) const;
+//	dgInt32 VisibilityTest (const dgCamera* camera, const dgMatrix &worldMatrix) const; 
+//	void Render (const dgCamera* camera, const dgMatrix &transform, unsigned rgb) const;
+
+	private:
+/*
+ 	typedef dgInt32 (dgSphere::*CachedVisibilityTest) (const dgMatrix &point, const dgPlane* plane) const;
+
+	mutable CachedVisibilityTest planeTest;
+	static CachedVisibilityTest planeTestArray[6];
+
+	void ChangeCachedVisibilityTest (CachedVisibilityTest	fnt);
+	dgInt32 FrontTest  (const dgMatrix &point, const dgPlane* plane) const;
+	dgInt32 RearTest   (const dgMatrix &point, const dgPlane* plane) const;
+	dgInt32 LeftTest   (const dgMatrix &point, const dgPlane* plane) const;
+	dgInt32 RightTest  (const dgMatrix &point, const dgPlane* plane) const;
+	dgInt32 TopTest    (const dgMatrix &point, const dgPlane* plane) const;
+	dgInt32 BottomTest (const dgMatrix &point, const dgPlane* plane) const;
+	dgInt32 VisibilityTestLow (const dgCamera* camera, const dgMatrix& viewMNatrix) const;
+*/
+
+	public:
+	dgVector m_size;
+} DG_GCC_VECTOR_ALIGMENT; 
+
+
+inline dgObb::dgObb (const dgQuaternion &quat, const dgVector &position, const dgVector& dim)
+	:dgMatrix(quat, position)
+{
+	SetDimensions (dim.m_x, dim.m_y, dim.m_z);
+	dgAssert (0);
+}
+
+inline dgObb::dgObb(const dgMatrix& matrix, const dgVector& dim)
+	:dgMatrix(matrix)
+{
+	SetDimensions (dim.m_x, dim.m_y, dim.m_z);
+}
+
+
+
+
+inline dgObb &dgObb::operator= (const dgMatrix &arg)
+{
+	m_front = arg.m_front;
+	m_up = arg.m_up;
+	m_right = arg.m_right;
+	m_posit = arg.m_posit;
+	return *this;
+}
+
+inline void dgObb::SetDimensions (dgFloat32 W, dgFloat32 H, dgFloat32 B)
+{
+	m_size = dgVector (dgAbsf(W), dgAbsf(H), dgAbsf(B), dgSqrt (W * W + H * H + B * B));
+}
+
+inline void dgObb::Scale (dgFloat32 Ws, dgFloat32 Hs, dgFloat32 Bs) 
+{
+	SetDimensions (m_size.m_x * Ws, m_size.m_y * Hs, m_size.m_z * Bs);
+}
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgPathFinder.h

@@ -0,0 +1,226 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgPathFinder__
+#define __dgPathFinder__
+
+#include "dgStdafx.h"
+#include "dgTree.h"
+#include "dgHeap.h"
+
+template<class NODEID, class COST> class dgPathFinder;
+
+
+#define DG_MAX_PATH_ENUMERATION_CHILDREN	128
+
+template<class NODEID, class COST>
+class dgPathNode
+{
+	friend class dgPathFinder<NODEID, COST>;
+	NODEID m_Id;
+	COST m_NodeCostToGoal;
+	COST m_NodeCostFromSource;
+	dgPathNode* m_Next;
+
+	public:
+	dgPathNode();
+	NODEID GetId() const;
+	const dgPathNode* GetNext() const;
+	const dgPathNode* GetParent() const;
+};
+
+template<class NODEID, class COST>
+class dgPathCloseList: public dgTree<dgPathNode<NODEID, COST>, NODEID>
+{
+	protected:
+	dgPathCloseList(): dgTree<dgPathNode<NODEID, COST>, NODEID>() {}
+};
+
+
+template<class NODEID, class COST>
+class dgPathOpenHeap: public dgUpHeap<typename dgPathCloseList<NODEID, COST>::dgTreeNode*, COST>
+{
+	protected:
+	dgPathOpenHeap(dgInt32 maxElements)
+		:dgUpHeap<typename dgPathCloseList<NODEID, COST>::dgTreeNode*, COST>(maxElements)
+	{
+	}
+
+	friend class dgPathFinder<NODEID, COST>;
+};
+
+
+template<class NODEID, class COST>
+class dgPathFinder: public dgPathCloseList<NODEID, COST>
+{
+	dgInt32 maxChildren;
+
+	public:
+	dgPathFinder(dgInt32 maxElementsInOpenList, dgInt32 nodeMaxChildren);
+	virtual ~dgPathFinder();
+
+	virtual const dgPathNode<NODEID, COST>* CalCulatePath (NODEID source, NODEID goal);
+
+	// this funtions must be overloaded by the user
+	virtual COST GetCostFromParent(const dgPathNode<NODEID, COST>& node) const;
+	virtual COST GetEstimatedCostToGoal(NODEID id) const;
+	virtual dgInt32 EnumerateChildren(NODEID parent, NODEID array[]) const;
+
+	dgPathOpenHeap<NODEID, COST> m_openList;
+};
+
+
+template<class NODEID, class COST>
+dgPathNode<NODEID, COST>::dgPathNode ()
+{
+}
+
+template<class NODEID, class COST>
+NODEID dgPathNode<NODEID, COST>::GetId () const
+{
+	return m_Id;
+}
+
+
+template<class NODEID, class COST>
+const dgPathNode<NODEID, COST>* dgPathNode<NODEID, COST>::GetNext() const
+{
+	return m_Next;
+}
+
+
+template<class NODEID, class COST>
+const dgPathNode<NODEID, COST>* dgPathNode<NODEID, COST>::GetParent() const
+{
+	return m_Next;
+}
+
+
+template<class NODEID, class COST>
+dgPathFinder<NODEID, COST>::dgPathFinder(
+	dgInt32 maxElementsInOpenList, 
+	dgInt32 nodeMaxChildren)
+	:dgPathCloseList<NODEID, COST>(),
+	 m_openList(maxElementsInOpenList)
+{
+	maxChildren	= nodeMaxChildren;
+}
+
+
+template<class NODEID, class COST>
+dgPathFinder<NODEID, COST>::~dgPathFinder()
+{
+}
+
+template<class NODEID, class COST>
+const dgPathNode<NODEID, COST>* dgPathFinder<NODEID, COST>::CalCulatePath (NODEID source, NODEID goal)
+{
+	dgInt32 count;
+	dgInt32 heapMaxCount;
+	dgPathNode<NODEID, COST> cell;
+	dgPathNode<NODEID, COST>* next;
+	dgPathNode<NODEID, COST>* prev;
+	dgPathNode<NODEID, COST>* link;
+	typename dgPathCloseList<NODEID, COST>::dgTreeNode* node;
+	NODEID idArray[DG_MAX_PATH_ENUMERATION_CHILDREN];
+
+	dgPathCloseList<NODEID, COST>& close = *this;
+
+	m_openList.Flush();
+	close.RemoveAll();
+
+	cell.m_Id = source;
+	cell.m_Next = NULL;
+	cell.m_NodeCostFromSource = COST (0);
+	cell.m_NodeCostToGoal = GetEstimatedCostToGoal(cell.m_Id);
+		  
+	node = close.Insert (cell, source);
+	
+	heapMaxCount = m_openList.GetMaxCount();
+	m_openList.Push (node, cell.m_NodeCostFromSource + cell.m_NodeCostToGoal);
+	while (m_openList.GetCount()) {
+		node = m_openList[0];
+		dgPathNode<NODEID, COST>& parent = node->GetInfo();
+		if (parent.m_Id == goal) {
+			link = &parent;
+			next = NULL;
+			do {
+				prev = link->m_Next;
+				link->m_Next = next;
+				next = link;
+				link = prev;
+			} while (link);
+			return next;
+		}
+
+		m_openList.Pop();
+		count = EnumerateChildren(node->GetKey(), idArray);
+		cell.m_Next = &parent;
+		for (int i = 0; i < count; i ++) {
+			bool state;
+			cell.m_Id = idArray[i];
+
+			COST newCostFromSource (GetCostFromParent(cell) + parent.m_NodeCostFromSource);
+			node = close.Insert (cell, cell.m_Id, state);
+			dgPathNode<NODEID, COST>& newCell = node->GetInfo();
+			if (state) {
+				if (newCell.m_NodeCostFromSource <= newCostFromSource) {
+					continue;
+				}
+				//newCell.m_Next = cell.m_Next;
+			}
+
+			newCell.m_NodeCostFromSource = newCostFromSource;
+			newCell.m_NodeCostToGoal = GetEstimatedCostToGoal(newCell.m_Id);
+			if (m_openList.GetCount() >= heapMaxCount) {
+				m_openList.Remove (heapMaxCount);
+			}
+			m_openList.Push (node, newCell.m_NodeCostFromSource + newCell.m_NodeCostToGoal);
+		}
+	}
+	return NULL;
+}
+
+template<class NODEID, class COST>
+COST dgPathFinder<NODEID, COST>::GetCostFromParent(const dgPathNode<NODEID, COST>& node) const
+{
+	return COST (1);
+}
+
+template<class NODEID, class COST>
+COST dgPathFinder<NODEID, COST>::GetEstimatedCostToGoal(NODEID id) const
+{
+	return COST (1);
+}
+
+template<class NODEID, class COST>
+dgInt32 dgPathFinder<NODEID, COST>::EnumerateChildren(NODEID parent, NODEID array[]) const
+{
+	return 0;
+}
+
+
+
+
+#endif
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgPlane.h

@@ -0,0 +1,165 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgPlane__
+#define __dgPlane__
+
+#include "dgStdafx.h"
+#include "dgVector.h"
+
+DG_MSC_VECTOR_ALIGMENT
+class dgPlane: public dgVector
+{
+	public:
+	dgPlane ();
+	dgPlane (const dgVector& point);
+	dgPlane (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w);
+	dgPlane (const dgVector &normal, dgFloat32 distance); 
+	dgPlane (const dgVector &P0, const dgVector &P1, const dgVector &P2);
+	dgPlane Scale (dgFloat32 s) const;
+	dgFloat32 Evalue (const dgFloat32* const point) const;
+	dgFloat32 Evalue (const dgVector &point) const;
+
+
+}DG_GCC_VECTOR_ALIGMENT;
+
+DG_MSC_VECTOR_ALIGMENT
+class dgBigPlane: public dgBigVector
+{
+	public:
+	dgBigPlane ();
+	dgBigPlane (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w);
+	dgBigPlane (const dgBigVector &normal, dgFloat64 distance); 
+	dgBigPlane (const dgBigVector &P0, const dgBigVector &P1, const dgBigVector &P2);
+	dgBigPlane Scale (dgFloat64 s) const;
+	dgFloat64 Evalue (const dgFloat32* const point) const;
+#ifndef _NEWTON_USE_DOUBLE
+	dgFloat64 Evalue (const dgFloat64* const point) const;
+#endif
+	dgFloat64 Evalue (const dgVector &point) const;
+	dgFloat64 Evalue (const dgBigVector &point) const;
+}DG_GCC_VECTOR_ALIGMENT;
+
+
+
+
+DG_INLINE dgPlane::dgPlane () 
+	:dgVector () 
+{
+}
+
+DG_INLINE dgPlane::dgPlane (const dgVector& point)
+	:dgVector (point)
+{
+}
+
+DG_INLINE dgPlane::dgPlane (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w)
+	:dgVector (x, y, z, w) 
+{
+}
+
+DG_INLINE dgPlane::dgPlane (const dgVector &normal, dgFloat32 distance) 
+	:dgVector (normal)
+{
+	m_w = distance;
+}
+
+DG_INLINE dgPlane::dgPlane (const dgVector &P0, const dgVector &P1, const dgVector &P2)
+	:dgVector ((P1 - P0) * (P2 - P0)) 
+{
+	m_w = - (*this % P0);
+}
+
+DG_INLINE dgPlane dgPlane::Scale (dgFloat32 s)	const
+{
+//	return dgPlane (m_x * s, m_y * s, m_z * s, m_w * s);
+	return dgPlane (CompProduct4 (dgVector (s)));
+}
+
+
+DG_INLINE dgFloat32 dgPlane::Evalue (const dgFloat32* const point) const
+{
+//	return m_x * point[0] + m_y * point[1] + m_z * point[2] + m_w;
+	return DotProduct4 (dgVector (point) | m_wOne).m_x;
+}
+
+DG_INLINE dgFloat32 dgPlane::Evalue (const dgVector& point) const
+{
+//	return m_x * point.m_x + m_y * point.m_y + m_z * point.m_z + m_w;
+	return DotProduct4 ((point & m_triplexMask) | m_wOne).m_x;
+}
+
+
+
+DG_INLINE dgBigPlane::dgBigPlane () 
+	:dgBigVector () 
+{
+}
+
+DG_INLINE dgBigPlane::dgBigPlane (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w)
+	:dgBigVector (x, y, z, w) 
+{
+}
+
+
+DG_INLINE dgBigPlane::dgBigPlane (const dgBigVector &normal, dgFloat64 distance) 
+	:dgBigVector (normal)
+{
+	m_w = distance;
+}
+
+DG_INLINE dgBigPlane::dgBigPlane (const dgBigVector &P0, const dgBigVector &P1, const dgBigVector &P2)
+	:dgBigVector ((P1 - P0) * (P2 - P0)) 
+{
+	m_w = - (*this % P0);
+}
+
+DG_INLINE dgBigPlane dgBigPlane::Scale (dgFloat64 s) const
+{
+	return dgBigPlane (m_x * s, m_y * s, m_z * s, m_w * s);
+}
+
+DG_INLINE dgFloat64 dgBigPlane::Evalue (const dgFloat32* const point) const
+{
+	return m_x * point[0] + m_y * point[1] + m_z * point[2] + m_w;
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+DG_INLINE dgFloat64 dgBigPlane::Evalue (const dgFloat64* const point) const
+{
+	return m_x * point[0] + m_y * point[1] + m_z * point[2] + m_w;
+}
+#endif
+
+DG_INLINE dgFloat64 dgBigPlane::Evalue (const dgVector &point) const
+{
+	return m_x * point.m_x + m_y * point.m_y + m_z * point.m_z + m_w;
+}
+
+DG_INLINE dgFloat64 dgBigPlane::Evalue (const dgBigVector &point) const
+{
+	return m_x * point.m_x + m_y * point.m_y + m_z * point.m_z + m_w;
+}
+
+
+#endif
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgPolygonSoupBuilder.cpp

@@ -0,0 +1,1043 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#include "dgStdafx.h"
+#include "dgStack.h"
+#include "dgMatrix.h"
+#include "dgMemory.h"
+#include "dgPolyhedra.h"
+#include "dgPolygonSoupBuilder.h"
+
+#define DG_POINTS_RUN (512 * 1024)
+
+
+
+class dgPolygonSoupDatabaseBuilder::dgFaceInfo
+{
+	public:
+	dgInt32 indexCount;
+	dgInt32 indexStart;
+};
+
+class dgPolygonSoupDatabaseBuilder::dgFaceBucket: public dgList<dgFaceInfo>
+{
+	public: 
+	dgFaceBucket (dgMemoryAllocator* const allocator)
+		:dgList<dgFaceInfo>(allocator)
+	{
+	}
+};
+
+class dgPolygonSoupDatabaseBuilder::dgFaceMap: public dgTree<dgFaceBucket, dgInt32>
+{
+	public:
+	dgFaceMap (dgMemoryAllocator* const allocator, dgPolygonSoupDatabaseBuilder& builder)
+		:dgTree<dgFaceBucket, dgInt32>(allocator)
+	{
+		dgInt32 polygonIndex = 0;
+		dgInt32 faceCount = builder.m_faceCount;
+		const dgInt32* const faceVertexCounts = &builder.m_faceVertexCount[0];
+		const dgInt32* const faceVertexIndex = &builder.m_vertexIndex[0];
+		for (dgInt32 i = 0; i < faceCount; i ++) {
+			dgInt32 count = faceVertexCounts[i];
+			dgInt32 attribute = faceVertexIndex[polygonIndex + count - 1];
+			
+			dgTreeNode* node = Find(attribute);
+			if (!node) {
+				dgFaceBucket tmp (GetAllocator());
+				node = Insert(tmp, attribute);
+			}
+
+			dgFaceBucket& bucket = node->GetInfo();
+			dgFaceInfo& face = bucket.Append()->GetInfo();
+			face.indexCount = count;
+			face.indexStart = polygonIndex;
+			polygonIndex += count;
+		}
+	}
+
+};
+
+
+class dgPolygonSoupDatabaseBuilder::dgPolySoupFilterAllocator: public dgPolyhedra
+{
+	public: 
+	dgPolySoupFilterAllocator (dgMemoryAllocator* const allocator)
+		:dgPolyhedra (allocator)
+	{
+	}
+
+	~dgPolySoupFilterAllocator ()
+	{
+	}
+
+	dgInt32 AddFilterFace (dgUnsigned32 count, dgInt32* const pool)
+	{
+		BeginFace();
+		dgAssert (count);
+		bool reduction = true;
+		while (reduction && !AddFace (dgInt32 (count), pool)) {
+			reduction = false;
+			if (count >3) {
+				for (dgUnsigned32 i = 0; i < count; i ++) {
+					for (dgUnsigned32 j = i + 1; j < count; j ++) {
+						if (pool[j] == pool[i]) {
+							for (i = j; i < count - 1; i ++) {
+								pool[i] =  pool[i + 1];
+							}
+							count --;
+							i = count;
+							reduction = true;
+							break;
+						}
+					}
+				}
+			}
+		}
+		EndFace();
+		return reduction ? dgInt32 (count) : 0;
+	}
+};
+
+
+dgPolygonSoupDatabaseBuilder::dgPolygonSoupDatabaseBuilder (dgMemoryAllocator* const allocator)
+	:m_faceVertexCount(allocator)
+	,m_vertexIndex(allocator)
+	,m_normalIndex(allocator)
+	,m_vertexPoints(allocator)
+	,m_normalPoints(allocator)
+{
+	m_run = DG_POINTS_RUN;
+	m_faceCount = 0;
+	m_indexCount = 0;
+	m_vertexCount = 0;
+	m_normalCount = 0;
+	m_allocator = allocator;
+}
+
+dgPolygonSoupDatabaseBuilder::dgPolygonSoupDatabaseBuilder (const dgPolygonSoupDatabaseBuilder& source)
+	:m_faceVertexCount(source.m_allocator)
+	,m_vertexIndex(source.m_allocator)
+	,m_normalIndex(source.m_allocator)
+	,m_vertexPoints(source.m_allocator)
+	,m_normalPoints(source.m_allocator)
+{
+	m_run = DG_POINTS_RUN;
+	m_faceCount = source.m_faceCount;
+	m_indexCount = source.m_indexCount;
+	m_vertexCount = source.m_vertexCount;
+	m_normalCount = source.m_normalCount;
+	m_allocator = source.m_allocator;
+	
+	m_vertexIndex[m_indexCount-1] = 0;
+	m_faceVertexCount[m_faceCount-1] = 0;
+	m_vertexPoints[m_vertexCount-1].m_w = 0;
+
+	memcpy (&m_vertexIndex[0], &source.m_vertexIndex[0], sizeof (dgInt32) * m_indexCount);
+	memcpy (&m_faceVertexCount[0], &source.m_faceVertexCount[0], sizeof (dgInt32) * m_faceCount);
+	memcpy (&m_vertexPoints[0], &source.m_vertexPoints[0], sizeof (dgBigVector) * m_vertexCount);
+
+	if (m_normalCount) {
+		m_normalIndex[m_faceCount-1] = 0;
+		m_normalPoints[m_normalCount - 1].m_w = 0;
+
+		memcpy (&m_normalIndex[0], &source.m_normalIndex[0], sizeof (dgInt32) * m_faceCount);
+		memcpy (&m_normalPoints[0], &source.m_normalPoints[0], sizeof (dgBigVector) * m_normalCount);
+	} else {
+		m_normalIndex[0] = 0;
+		m_normalPoints[0].m_w = 0;
+	}
+}
+
+
+dgPolygonSoupDatabaseBuilder::~dgPolygonSoupDatabaseBuilder ()
+{
+}
+
+
+void dgPolygonSoupDatabaseBuilder::Begin()
+{
+	m_run = DG_POINTS_RUN;
+	m_faceCount = 0;
+	m_indexCount = 0;
+	m_vertexCount = 0;
+	m_normalCount = 0;
+}
+
+
+void dgPolygonSoupDatabaseBuilder::AddMesh (const dgFloat32* const vertex, dgInt32 vertexCount, dgInt32 strideInBytes, dgInt32 faceCount,	
+	const dgInt32* const faceArray, const dgInt32* const indexArray, const dgInt32* const faceMaterialId, const dgMatrix& worldMatrix) 
+{
+	dgInt32 faces[256];
+	dgInt32 pool[2048];
+
+
+	m_vertexPoints[m_vertexCount + vertexCount].m_x = dgFloat64 (0.0f);
+	dgBigVector* const vertexPool = &m_vertexPoints[m_vertexCount];
+
+	worldMatrix.TransformTriplex (&vertexPool[0].m_x, sizeof (dgBigVector), vertex, strideInBytes, vertexCount);
+	for (dgInt32 i = 0; i < vertexCount; i ++) {
+		vertexPool[i].m_w = dgFloat64 (0.0f);
+	}
+
+	dgInt32 totalIndexCount = faceCount;
+	for (dgInt32 i = 0; i < faceCount; i ++) {
+		totalIndexCount += faceArray[i];
+	}
+
+	m_vertexIndex[m_indexCount + totalIndexCount] = 0;
+	m_faceVertexCount[m_faceCount + faceCount] = 0;
+
+	dgInt32 k = 0;
+	for (dgInt32 i = 0; i < faceCount; i ++) {
+		dgInt32 count = faceArray[i];
+		for (dgInt32 j = 0; j < count; j ++) {
+			dgInt32 index = indexArray[k];
+			pool[j] = index + m_vertexCount;
+			k ++;
+		}
+
+		dgInt32 convexFaces = 0;
+		if (count == 3) {
+			convexFaces = 1;
+			dgBigVector p0 (m_vertexPoints[pool[2]]);
+			for (dgInt32 i = 0; i < 3; i ++) {
+				dgBigVector p1 (m_vertexPoints[pool[i]]);
+				dgBigVector edge (p1 - p0);
+				dgFloat64 mag2 = edge % edge;
+				if (mag2 < dgFloat32 (1.0e-6f)) {
+					convexFaces = 0;
+				}
+				p0 = p1;
+			}
+
+			if (convexFaces) {
+				dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
+				dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
+				dgBigVector normal (edge0 * edge1);
+				dgFloat64 mag2 = normal % normal;
+				if (mag2 < dgFloat32 (1.0e-8f)) {
+					convexFaces = 0;
+				}
+			}
+
+			if (convexFaces) {
+				faces[0] = 3;
+			}
+
+		} else {
+			convexFaces = AddConvexFace (count, pool, faces);
+		}
+
+		dgInt32 indexAcc = 0;
+		for (dgInt32 k = 0; k < convexFaces; k ++) {
+			dgInt32 count = faces[k];
+			m_vertexIndex[m_indexCount + count] = faceMaterialId[i];
+			for (dgInt32 j = 0; j < count; j ++) {
+				m_vertexIndex[m_indexCount + j] = pool[indexAcc + j];
+			}
+			indexAcc += count;
+			m_indexCount += (count + 1);
+			m_faceVertexCount[m_faceCount] = count + 1;
+			m_faceCount ++;
+		}
+	}
+	m_vertexCount += vertexCount;
+	m_run -= vertexCount;
+	if (m_run <= 0) {
+		PackArray();
+	}
+}
+
+void dgPolygonSoupDatabaseBuilder::PackArray()
+{
+	dgStack<dgInt32> indexMapPool (m_vertexCount);
+	dgInt32* const indexMap = &indexMapPool[0];
+	m_vertexCount = dgVertexListToIndexList (&m_vertexPoints[0].m_x, sizeof (dgBigVector), 3, m_vertexCount, &indexMap[0], dgFloat32 (1.0e-6f));
+
+	dgInt32 k = 0;
+	for (dgInt32 i = 0; i < m_faceCount; i ++) {
+		dgInt32 count = m_faceVertexCount[i] - 1;
+		for (dgInt32 j = 0; j < count; j ++) {
+			dgInt32 index = m_vertexIndex[k];
+			index = indexMap[index];
+			m_vertexIndex[k] = index;
+			k ++;
+		}
+		k ++;
+	}
+
+	m_run = DG_POINTS_RUN;
+}
+
+void dgPolygonSoupDatabaseBuilder::Finalize()
+{
+	if (m_faceCount) {
+		dgStack<dgInt32> indexMapPool (m_indexCount + m_vertexCount);
+
+		dgInt32* const indexMap = &indexMapPool[0];
+		m_vertexCount = dgVertexListToIndexList (&m_vertexPoints[0].m_x, sizeof (dgBigVector), 3, m_vertexCount, &indexMap[0], dgFloat32 (1.0e-4f));
+
+		dgInt32 k = 0;
+		for (dgInt32 i = 0; i < m_faceCount; i ++) {
+			dgInt32 count = m_faceVertexCount[i] - 1;
+			for (dgInt32 j = 0; j < count; j ++) {
+				dgInt32 index = m_vertexIndex[k];
+				index = indexMap[index];
+				m_vertexIndex[k] = index;
+				k ++;
+			}
+			k ++;
+		}
+		OptimizeByIndividualFaces();
+	}
+}
+
+
+
+void dgPolygonSoupDatabaseBuilder::FinalizeAndOptimize()
+{
+	Finalize();
+	dgPolyhedra polyhedra(m_allocator);
+	dgPolygonSoupDatabaseBuilder source(*this);
+	dgPolygonSoupDatabaseBuilder leftOver(m_allocator);
+	dgInt32 tmpIndexPool[1024];
+	dgVector tmpVertexPool[1024];
+	
+	Begin();
+	leftOver.Begin();
+	polyhedra.BeginFace ();
+	dgInt32 faceIndexNumber = 0;
+	dgInt32 attribute = m_vertexIndex[0];
+
+	for (dgInt32 i = 0; i < source.m_faceCount; i ++) {
+		dgInt32 indexCount = source.m_faceVertexCount[i];
+		dgAssert (indexCount < 1024);
+
+		dgEdge* const face = polyhedra.AddFace(indexCount - 1, &source.m_vertexIndex[faceIndexNumber]);
+		if (!face) {
+			for (dgInt32 j = 0; j < indexCount - 1; j ++) {
+				dgInt32 index = source.m_vertexIndex[faceIndexNumber + j];
+				tmpVertexPool[j] = source.m_vertexPoints[index];
+				tmpIndexPool[j] = j;
+			}
+			dgInt32 faceArray = indexCount - 1;
+			leftOver.AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (tmpVertexPool[0]), 1, &faceArray, tmpIndexPool, &attribute, dgGetIdentityMatrix());
+		} else {
+			// set the attribute
+			dgEdge* ptr = face;
+			do {
+				ptr->m_userData = dgUnsigned64 (attribute);
+				ptr = ptr->m_next;
+			} while (ptr != face);
+		}
+		faceIndexNumber += indexCount; 
+	} 
+	polyhedra.EndFace();
+
+	dgPolyhedra facesLeft(m_allocator);
+	facesLeft.BeginFace();
+	polyhedra.ConvexPartition (&source.m_vertexPoints[0].m_x, source.m_vertexPoints.GetElementSize(), &facesLeft);
+	facesLeft.EndFace();
+
+	dgInt32 mark = polyhedra.IncLRU();
+	dgPolyhedra::Iterator iter (polyhedra);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		dgEdge* ptr = edge;
+		dgInt32 indexCount = 0;
+		do {
+			ptr->m_mark = mark;
+			tmpVertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex];
+			tmpIndexPool[indexCount] = indexCount;
+			indexCount ++;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+
+		if (indexCount >= 3) {
+			AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (tmpVertexPool[0]), 1, &indexCount, tmpIndexPool, &attribute, dgGetIdentityMatrix());
+		}
+	}
+
+
+	mark = facesLeft.IncLRU();
+	dgPolyhedra::Iterator iter1 (facesLeft);
+	for (iter1.Begin(); iter1; iter1 ++) {
+		dgEdge* const edge = &(*iter1);
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		dgEdge* ptr = edge;
+		dgInt32 indexCount = 0;
+		do {
+			ptr->m_mark = mark;
+			tmpVertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex];
+			tmpIndexPool[indexCount] = indexCount;
+			indexCount ++;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+		if (indexCount >= 3) {
+			AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (dgVector), 1, &indexCount, tmpIndexPool, &attribute, dgGetIdentityMatrix());
+		}
+	}
+
+	faceIndexNumber = 0;
+	for (dgInt32 i = 0; i < leftOver.m_faceCount; i ++) {
+		dgInt32 indexCount = leftOver.m_faceVertexCount[i] - 1;
+		for (dgInt32 j = 0; j < indexCount; j ++) {
+			dgInt32 index = leftOver.m_vertexIndex[faceIndexNumber + j];
+			tmpVertexPool[j] = leftOver.m_vertexPoints[index];
+			tmpIndexPool[j] = j;
+		}
+		dgInt32 faceArray = indexCount;
+		AddMesh (&tmpVertexPool[0].m_x, indexCount, sizeof (tmpVertexPool[0]), 1, &faceArray, tmpIndexPool, &attribute, dgGetIdentityMatrix());
+
+		faceIndexNumber += (indexCount + 1); 
+	}
+
+	Finalize();
+}
+
+
+void dgPolygonSoupDatabaseBuilder::OptimizeByIndividualFaces()
+{
+	dgInt32* const faceArray = &m_faceVertexCount[0];
+	dgInt32* const indexArray = &m_vertexIndex[0];
+
+	dgInt32* const oldFaceArray = &m_faceVertexCount[0];
+	dgInt32* const oldIndexArray = &m_vertexIndex[0];
+
+	dgInt32 polygonIndex = 0;
+	dgInt32 newFaceCount = 0;
+	dgInt32 newIndexCount = 0;
+	for (dgInt32 i = 0; i < m_faceCount; i ++) {
+		dgInt32 oldCount = oldFaceArray[i];
+		dgInt32 count = FilterFace (oldCount - 1, &oldIndexArray[polygonIndex]);
+		if (count) {
+			faceArray[newFaceCount] = count + 1;
+			for (dgInt32 j = 0; j < count; j ++) {
+				indexArray[newIndexCount + j] = oldIndexArray[polygonIndex + j];
+			}
+			indexArray[newIndexCount + count] = oldIndexArray[polygonIndex + oldCount - 1];
+			newFaceCount ++;
+			newIndexCount += (count + 1);
+		}
+		polygonIndex += oldCount;
+	}
+	dgAssert (polygonIndex == m_indexCount);
+	m_faceCount = newFaceCount;
+	m_indexCount = newIndexCount;
+}
+
+
+void dgPolygonSoupDatabaseBuilder::End(bool optimize)
+{
+	if (optimize) {
+		dgPolygonSoupDatabaseBuilder copy (*this);
+		dgFaceMap faceMap (m_allocator, copy);
+
+		Begin();
+		dgFaceMap::Iterator iter (faceMap);
+		for (iter.Begin(); iter; iter ++) {
+			const dgFaceBucket& bucket = iter.GetNode()->GetInfo();
+			Optimize(iter.GetNode()->GetKey(), bucket, copy);
+		}
+	}
+	Finalize();
+
+	// 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];
+
+		const dgInt32* const ptr = &m_vertexIndex[indexCount];
+		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.Scale3 (dgFloat64 (1.0f) / sqrt (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;
+		m_normalPoints[i].m_w = dgFloat32 (0.0f);
+		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));
+}
+
+
+void dgPolygonSoupDatabaseBuilder::Optimize(dgInt32 faceId, const dgFaceBucket& faceBucket, const dgPolygonSoupDatabaseBuilder& source)
+{
+	#define DG_MESH_PARTITION_SIZE (1024 * 4)
+
+	const dgInt32* const indexArray = &source.m_vertexIndex[0];
+	const dgBigVector* const points = &source.m_vertexPoints[0];
+
+	dgVector face[256];
+	dgInt32 faceIndex[256];
+	if (faceBucket.GetCount() >= DG_MESH_PARTITION_SIZE) {
+		dgStack<dgFaceBucket::dgListNode*> array(faceBucket.GetCount());
+		dgInt32 count = 0;
+		for (dgFaceBucket::dgListNode* node = faceBucket.GetFirst(); node; node = node->GetNext()) {
+			array[count] = node;
+			count ++;
+		}
+
+		dgInt32 stack = 1;
+		dgInt32 segments[32][2];
+			
+		segments[0][0] = 0;
+		segments[0][1] = count;
+	
+		while (stack) {
+			stack --;
+			dgInt32 faceStart = segments[stack][0];
+			dgInt32 faceCount = segments[stack][1];
+
+			if (faceCount <= DG_MESH_PARTITION_SIZE) {
+
+				dgPolygonSoupDatabaseBuilder tmpBuilder (m_allocator);
+				for (dgInt32 i = 0; i < faceCount; i ++) {
+					const dgFaceInfo& faceInfo = array[faceStart + i]->GetInfo();
+
+					dgInt32 count = faceInfo.indexCount - 1;
+					dgInt32 start = faceInfo.indexStart;
+					dgAssert (faceId == indexArray[start + count]);
+					for (dgInt32 j = 0; j < count; j ++) {
+						dgInt32 index = indexArray[start + j];
+						face[j] = points[index];
+						faceIndex[j] = j;
+					}
+					dgInt32 faceIndexCount = count;
+					tmpBuilder.AddMesh (&face[0].m_x, count, sizeof (dgVector), 1, &faceIndexCount, &faceIndex[0], &faceId, dgGetIdentityMatrix()); 
+				}
+				tmpBuilder.FinalizeAndOptimize ();
+
+				dgInt32 faceIndexNumber = 0;
+				for (dgInt32 i = 0; i < tmpBuilder.m_faceCount; i ++) {
+					dgInt32 indexCount = tmpBuilder.m_faceVertexCount[i] - 1;
+					for (dgInt32 j = 0; j < indexCount; j ++) {
+						dgInt32 index = tmpBuilder.m_vertexIndex[faceIndexNumber + j];
+						face[j] = tmpBuilder.m_vertexPoints[index];
+						faceIndex[j] = j;
+					}
+					dgInt32 faceArray = indexCount;
+					AddMesh (&face[0].m_x, indexCount, sizeof (dgVector), 1, &faceArray, faceIndex, &faceId, dgGetIdentityMatrix());
+
+					faceIndexNumber += (indexCount + 1); 
+				}
+
+			} 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 < faceCount; i ++) {
+					const dgFaceInfo& faceInfo = array[faceStart + i]->GetInfo();
+					dgInt32 count = faceInfo.indexCount - 1;
+					dgInt32 start = faceInfo.indexStart;
+					dgBigVector p0 (dgFloat32 ( 1.0e10f), dgFloat32 ( 1.0e10f), dgFloat32 ( 1.0e10f), dgFloat32 (0.0f));
+					dgBigVector p1 (dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (0.0f));
+					for (dgInt32 j = 0; j < count; j ++) {
+						dgInt32 index = indexArray[start + j];
+						const dgBigVector& p = points[index];
+						p0.m_x = dgMin (p0.m_x, p.m_x);
+						p0.m_y = dgMin (p0.m_y, p.m_y);
+						p0.m_z = dgMin (p0.m_z, p.m_z);
+						p1.m_x = dgMax (p1.m_x, p.m_x);
+						p1.m_y = dgMax (p1.m_y, p.m_y);
+						p1.m_z = dgMax (p1.m_z, p.m_z);
+					}
+					dgBigVector p ((p0 + p1).Scale3 (0.5f));
+					median += p;
+					varian += p.CompProduct3 (p);
+				}
+
+				varian = varian.Scale3 (dgFloat32 (faceCount)) - median.CompProduct3(median);
+
+				dgInt32 axis = 0;
+				dgFloat32 maxVarian = dgFloat32 (-1.0e10f);
+				for (dgInt32 i = 0; i < 3; i ++) {
+					if (varian[i] > maxVarian) {
+						axis = i;
+						maxVarian = dgFloat32 (varian[i]);
+					}
+				}
+				dgBigVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (faceCount));
+				dgFloat64 axisVal = center[axis];
+
+				dgInt32 leftCount = 0;
+				dgInt32 lastFace = faceCount;
+
+				for (dgInt32 i = 0; i < lastFace; i ++) {
+					dgInt32 side = 0;
+					const dgFaceInfo& faceInfo = array[faceStart + i]->GetInfo();
+
+					dgInt32 start = faceInfo.indexStart;
+					dgInt32 count = faceInfo.indexCount - 1;
+					for (dgInt32 j = 0; j < count; j ++) {
+						dgInt32 index = indexArray[start + j];
+						const dgBigVector& p = points[index];
+						if (p[axis] > axisVal) {
+							side = 1;
+							break;
+						}
+					}
+
+					if (side) {
+						dgSwap (array[faceStart + i], array[faceStart + lastFace - 1]);
+						lastFace --;
+						i --;
+					} else {
+						leftCount ++;
+					}
+				}
+				dgAssert (leftCount);
+				dgAssert (leftCount < faceCount);
+
+				segments[stack][0] = faceStart;
+				segments[stack][1] = leftCount;
+				stack ++;
+
+				segments[stack][0] = faceStart + leftCount;
+				segments[stack][1] = faceCount - leftCount;
+				stack ++;
+			}
+		}
+	
+	} else {
+		dgPolygonSoupDatabaseBuilder tmpBuilder (m_allocator);
+		for (dgFaceBucket::dgListNode* node = faceBucket.GetFirst(); node; node = node->GetNext()) {
+			const dgFaceInfo& faceInfo = node->GetInfo();
+
+			dgInt32 count = faceInfo.indexCount - 1;
+			dgInt32 start = faceInfo.indexStart;
+			dgAssert (faceId == indexArray[start + count]);
+			for (dgInt32 j = 0; j < count; j ++) {
+				dgInt32 index = indexArray[start + j];
+				face[j] = points[index];
+				faceIndex[j] = j;
+			}
+			dgInt32 faceIndexCount = count;
+			tmpBuilder.AddMesh (&face[0].m_x, count, sizeof (dgVector), 1, &faceIndexCount, &faceIndex[0], &faceId, dgGetIdentityMatrix()); 
+		}
+		tmpBuilder.FinalizeAndOptimize ();
+
+		dgInt32 faceIndexNumber = 0;
+		for (dgInt32 i = 0; i < tmpBuilder.m_faceCount; i ++) {
+			dgInt32 indexCount = tmpBuilder.m_faceVertexCount[i] - 1;
+			for (dgInt32 j = 0; j < indexCount; j ++) {
+				dgInt32 index = tmpBuilder.m_vertexIndex[faceIndexNumber + j];
+				face[j] = tmpBuilder.m_vertexPoints[index];
+				faceIndex[j] = j;
+			}
+			dgInt32 faceArray = indexCount;
+			AddMesh (&face[0].m_x, indexCount, sizeof (dgVector), 1, &faceArray, faceIndex, &faceId, dgGetIdentityMatrix());
+
+			faceIndexNumber += (indexCount + 1); 
+		}
+	}
+}
+
+
+dgInt32 dgPolygonSoupDatabaseBuilder::FilterFace (dgInt32 count, dgInt32* const pool)
+{
+	if (count == 3) {
+		dgBigVector p0 (m_vertexPoints[pool[2]]);
+		for (dgInt32 i = 0; i < 3; i ++) {
+			dgBigVector p1 (m_vertexPoints[pool[i]]);
+			dgBigVector edge (p1 - p0);
+			dgFloat64 mag2 = edge % edge;
+			if (mag2 < dgFloat32 (1.0e-6f)) {
+				count = 0;
+			}
+			p0 = p1;
+		}
+
+		if (count == 3) {
+			dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
+			dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
+			dgBigVector normal (edge0 * edge1);
+			dgFloat64 mag2 = normal % normal;
+			if (mag2 < dgFloat32 (1.0e-8f)) {
+				count = 0;
+			}
+		}
+	} else {
+		dgPolySoupFilterAllocator polyhedra(m_allocator);
+		count = polyhedra.AddFilterFace (dgUnsigned32 (count), pool);
+
+		if (!count) {
+			return 0;
+		}
+
+		dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
+		if (edge->m_incidentFace < 0) {
+			edge = edge->m_twin;
+		}
+
+		bool flag = true;
+		while (flag) {
+			flag = false;
+			if (count >= 3) {
+				dgEdge* ptr = edge;
+
+				dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
+				do {
+					dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
+					dgBigVector e0 (p1 - p0);
+					dgFloat64 mag2 = e0 % e0;
+					if (mag2 < dgFloat32 (1.0e-6f)) {
+						count --;
+						flag = true;
+						edge = ptr->m_next;
+						ptr->m_prev->m_next = ptr->m_next;
+						ptr->m_next->m_prev = ptr->m_prev;
+						ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
+						ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
+						break;
+					}
+					p0 = p1;
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+			}
+		}
+		if (count >= 3) {
+			flag = true;
+			dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
+
+			dgAssert ((normal % normal) > dgFloat32 (1.0e-10f)); 
+			normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (normal % normal + dgFloat32 (1.0e-24f)));
+
+			while (flag) {
+				flag = false;
+				if (count >= 3) {
+					dgEdge* ptr = edge;
+
+					dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
+					dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
+					dgBigVector e0 (p1 - p0);
+					e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0 + dgFloat32(1.0e-24f)));
+					do {
+						dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
+						dgBigVector e1 (p2 - p1);
+
+						e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1 + dgFloat32(1.0e-24f)));
+						dgFloat64 mag2 = e1 % e0;
+						if (mag2 > dgFloat32 (0.9999f)) {
+							count --;
+							flag = true;
+							edge = ptr->m_next;
+							ptr->m_prev->m_next = ptr->m_next;
+							ptr->m_next->m_prev = ptr->m_prev;
+							ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
+							ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
+							break;
+						}
+
+						dgBigVector n (e0 * e1);
+						mag2 = n % normal;
+						if (mag2 < dgFloat32 (1.0e-5f)) {
+							count --;
+							flag = true;
+							edge = ptr->m_next;
+							ptr->m_prev->m_next = ptr->m_next;
+							ptr->m_next->m_prev = ptr->m_prev;
+							ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
+							ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
+							break;
+						}
+
+						e0 = e1;
+						p1 = p2;
+						ptr = ptr->m_next;
+					} while (ptr != edge);
+				}
+			}
+		}
+
+		dgEdge* first = edge;
+		if (count >= 3) {
+			dgFloat64 best = dgFloat32 (2.0f);
+			dgEdge* ptr = edge;
+
+			dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
+			dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
+			dgBigVector e0 (p1 - p0);
+			e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0 + dgFloat32(1.0e-24f)));
+			do {
+				dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x);
+				dgBigVector e1 (p2 - p1);
+
+				e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1 + dgFloat32(1.0e-24f)));
+				dgFloat64 mag2 = fabs (e1 % e0);
+				if (mag2 < best) {
+					best = mag2;
+					first = ptr;
+				}
+
+				e0 = e1;
+				p1 = p2;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+
+			count = 0;
+			ptr = first;
+			do {
+				pool[count] = ptr->m_incidentVertex;
+				count ++;
+				ptr = ptr->m_next;
+			} while (ptr != first);
+		}
+
+	#ifdef _DEBUG
+		if (count >= 3) {
+			dgInt32 j0 = count - 2;  
+			dgInt32 j1 = count - 1;  
+			dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
+			for (dgInt32 j2 = 0; j2 < count; j2 ++) { 
+				dgBigVector p0 (&m_vertexPoints[pool[j0]].m_x);
+				dgBigVector p1 (&m_vertexPoints[pool[j1]].m_x);
+				dgBigVector p2 (&m_vertexPoints[pool[j2]].m_x);
+				dgBigVector e0 ((p0 - p1));
+				dgBigVector e1 ((p2 - p1));
+
+				dgBigVector n (e1 * e0);
+				dgAssert ((n % normal) > dgFloat32 (0.0f));
+				j0 = j1;
+				j1 = j2;
+			}
+		}
+	#endif
+	}
+
+	return (count >= 3) ? count : 0;
+}
+
+
+dgInt32 dgPolygonSoupDatabaseBuilder::AddConvexFace (dgInt32 count, dgInt32* const pool, dgInt32* const facesArray)
+{
+	dgPolySoupFilterAllocator polyhedra(m_allocator);
+
+	count = polyhedra.AddFilterFace(dgUnsigned32 (count), pool);
+
+	dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
+	if (edge->m_incidentFace < 0) {
+		edge = edge->m_twin;
+	}
+
+	
+	dgInt32 isconvex = 1;
+	dgInt32 facesCount = 0;
+
+	dgInt32 flag = 1;
+	while (flag) {
+		flag = 0;
+		if (count >= 3) {
+			dgEdge* ptr = edge;
+
+			dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
+			do {
+				dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
+				dgBigVector e0 (p1 - p0);
+				dgFloat64 mag2 = e0 % e0;
+				if (mag2 < dgFloat32 (1.0e-6f)) {
+					count --;
+					flag = 1;
+					edge = ptr->m_next;
+					ptr->m_prev->m_next = ptr->m_next;
+					ptr->m_next->m_prev = ptr->m_prev;
+					ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
+					ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
+					break;
+				}
+				p0 = p1;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+	}
+	if (count >= 3) {
+		flag = 1;
+
+		while (flag) {
+			flag = 0;
+			if (count >= 3) {
+				dgEdge* ptr = edge;
+
+				dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
+				dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
+				dgBigVector e0 (p1 - p0);
+				e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0 + dgFloat32(1.0e-24f)));
+				do {
+					dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
+					dgBigVector e1 (p2 - p1);
+
+					e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1 + dgFloat32(1.0e-24f)));
+					dgFloat64 mag2 = e1 % e0;
+					if (mag2 > dgFloat32 (0.9999f)) {
+						count --;
+						flag = 1;
+						edge = ptr->m_next;
+						ptr->m_prev->m_next = ptr->m_next;
+						ptr->m_next->m_prev = ptr->m_prev;
+						ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
+						ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
+						break;
+					}
+
+					e0 = e1;
+					p1 = p2;
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+			}
+		}
+
+		dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
+		dgFloat64 mag2 = normal % normal;
+		if (mag2 < dgFloat32 (1.0e-8f)) {
+			return 0;
+		}
+		normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (mag2));
+
+
+		if (count >= 3) {
+			dgEdge* ptr = edge;
+			dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
+			dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
+			dgBigVector e0 (p1 - p0);
+			e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0 + dgFloat32(1.0e-24f)));
+			do {
+				dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
+				dgBigVector e1 (p2 - p1);
+
+				e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1 + dgFloat32(1.0e-24f)));
+
+				dgBigVector n (e0 * e1);
+				dgFloat64 mag2 = n % normal;
+				if (mag2 < dgFloat32 (1.0e-5f)) {
+					isconvex = 0;
+					break;
+				}
+
+				e0 = e1;
+				p1 = p2;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+	}
+
+	if (isconvex) {
+		dgEdge* const first = edge;
+		if (count >= 3) {
+			count = 0;
+			dgEdge* ptr = first;
+			do {
+				pool[count] = ptr->m_incidentVertex;
+				count ++;
+				ptr = ptr->m_next;
+			} while (ptr != first);
+			facesArray[facesCount] = count;
+			facesCount = 1;
+		}
+	} else {
+		dgPolyhedra leftOver(m_allocator);
+		dgPolyhedra polyhedra2(m_allocator);
+		dgEdge* ptr = edge;
+		count = 0;
+		do {
+			pool[count] = ptr->m_incidentVertex;
+			count ++;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+
+
+		polyhedra2.BeginFace();
+		polyhedra2.AddFace (count, pool);
+		polyhedra2.EndFace();
+		leftOver.BeginFace();
+		polyhedra2.ConvexPartition (&m_vertexPoints[0].m_x, m_vertexPoints.GetElementSize(), &leftOver);
+		leftOver.EndFace();
+
+#if _DEBUG
+		if (leftOver.GetCount()) {
+			dgTrace (("warning: %d faces with more that a one shared edge\n", leftOver.GetCount()));
+			dgTrace (("         this mesh is not a manifold and may lead to collision malfunctions\n"));
+		}
+#endif
+
+		dgInt32 mark = polyhedra2.IncLRU();
+		dgInt32 index = 0;
+		dgPolyhedra::Iterator iter (polyhedra2);
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			if (edge->m_incidentFace < 0) {
+				continue;
+			}
+			if (edge->m_mark == mark) {
+				continue;
+			}
+
+			ptr = edge;
+			count = 0;
+			do {
+				ptr->m_mark = mark;
+				pool[index] = ptr->m_incidentVertex;
+				index ++;
+				count ++;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+
+			facesArray[facesCount] = count;
+			facesCount ++;
+		}
+	}
+
+	return facesCount;
+}
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgPolygonSoupBuilder.h

@@ -0,0 +1,115 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgPolygonSoupDatabaseBuilder0x23413452233__
+#define __dgPolygonSoupDatabaseBuilder0x23413452233__
+
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgArray.h"
+#include "dgIntersections.h"
+
+
+class AdjacentdFaces
+{
+	public:
+	dgInt32 m_count;
+	dgInt32 *m_index;
+	dgPlane m_normal;
+	dgInt64 m_edgeMap[256];
+};
+
+class dgPolygonSoupDatabaseBuilder 
+{
+	class dgFaceMap;
+	class dgFaceInfo;
+	class dgFaceBucket;
+	class dgPolySoupFilterAllocator;
+	public:
+
+	dgPolygonSoupDatabaseBuilder (dgMemoryAllocator* const allocator);
+	dgPolygonSoupDatabaseBuilder (const dgPolygonSoupDatabaseBuilder& sopurce);
+	~dgPolygonSoupDatabaseBuilder ();
+
+	DG_CLASS_ALLOCATOR(allocator)
+
+	void Begin();
+	void End(bool optimize);
+	void AddMesh (const dgFloat32* const vertex, dgInt32 vertexCount, dgInt32 strideInBytes, dgInt32 faceCount, 
+		          const dgInt32* const faceArray, const dgInt32* const indexArray, const dgInt32* const faceTagsData, const dgMatrix& worldMatrix); 
+
+	private:
+	void Optimize(dgInt32 faceId, const dgFaceBucket& faceBucket, const dgPolygonSoupDatabaseBuilder& source);
+
+	void Finalize();
+	void FinalizeAndOptimize();
+	void OptimizeByIndividualFaces();
+	dgInt32 FilterFace (dgInt32 count, dgInt32* const indexArray);
+	dgInt32 AddConvexFace (dgInt32 count, dgInt32* const indexArray, dgInt32* const  facesArray);
+	void PackArray();
+
+
+
+	public:
+	class dgVertexArray: public dgArray<dgBigVector>
+	{	
+		public:
+		dgVertexArray(dgMemoryAllocator* const allocator)
+			:dgArray<dgBigVector>(1024 * 32, allocator)
+		{
+		}
+	};
+
+	class dgIndexArray: public dgArray<dgInt32>
+	{
+		public:
+		dgIndexArray(dgMemoryAllocator* const allocator)
+			:dgArray<dgInt32>(1024 * 32, allocator)
+		{
+		}
+	};
+
+	dgInt32 m_run;
+	dgInt32 m_faceCount;
+	dgInt32 m_indexCount;
+	dgInt32 m_vertexCount;
+	dgInt32 m_normalCount;
+	dgIndexArray m_faceVertexCount;
+	dgIndexArray m_vertexIndex;
+	dgIndexArray m_normalIndex;
+	dgVertexArray m_vertexPoints;
+	dgVertexArray m_normalPoints;
+	dgMemoryAllocator* m_allocator;
+
+};
+
+
+
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgPolygonSoupDatabase.h

@@ -0,0 +1,113 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+/****************************************************************************
+*
+*  Visual C++ 6.0 created by: Julio Jerez
+*
+****************************************************************************/
+#ifndef __dgPolygonSoupDatabase_H_
+#define __dgPolygonSoupDatabase_H_
+
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgArray.h"
+#include "dgIntersections.h"
+
+class dgMatrix;
+
+
+
+class dgPolygonSoupDatabase
+{
+	public:
+	dgFloat32 GetRadius() const;
+	dgInt32 GetVertexCount() const;
+	dgInt32 GetStrideInBytes() const;
+	dgFloat32* GetLocalVertexPool() const;
+
+	dgUnsigned32 GetTagId(const dgInt32* const face, dgInt32 indexCount) const;
+	void SetTagId(const dgInt32* const face, dgInt32 indexCount, dgUnsigned32 newID) const;
+
+	virtual void Serialize (dgSerialize callback, void* const userData) const = 0;
+	virtual void Deserialize (dgDeserialize callback, void* const userData) = 0;
+
+	
+	protected:
+	dgPolygonSoupDatabase(const char* const name = NULL);
+	virtual ~dgPolygonSoupDatabase ();
+
+	dgInt32 m_vertexCount;
+	dgInt32 m_strideInBytes;
+	dgFloat32* m_localVertex;
+};
+
+
+inline dgPolygonSoupDatabase::dgPolygonSoupDatabase(const char* const name)
+{
+	m_vertexCount = 0;
+	m_strideInBytes = 0;
+	m_localVertex = NULL;
+}
+
+inline dgPolygonSoupDatabase::~dgPolygonSoupDatabase ()
+{
+	if (m_localVertex) {
+		dgFreeStack (m_localVertex);
+	}
+}
+
+
+inline dgUnsigned32 dgPolygonSoupDatabase::GetTagId(const dgInt32* const face, dgInt32 indexCount) const
+{
+	return dgUnsigned32 (face[indexCount]);
+}
+
+inline void dgPolygonSoupDatabase::SetTagId(const dgInt32* const facePtr, dgInt32 indexCount, dgUnsigned32 newID) const
+{
+	dgUnsigned32* const face = (dgUnsigned32*) facePtr;
+	face[indexCount] = newID;
+}
+
+inline dgInt32 dgPolygonSoupDatabase::GetVertexCount()	const
+{
+	return m_vertexCount;
+}
+
+inline dgFloat32* dgPolygonSoupDatabase::GetLocalVertexPool() const
+{
+	return m_localVertex;
+}
+
+inline dgInt32 dgPolygonSoupDatabase::GetStrideInBytes() const
+{
+	return m_strideInBytes;
+}
+
+inline dgFloat32 dgPolygonSoupDatabase::GetRadius() const
+{
+	return 0.0f;
+}
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgPolyhedra.cpp

@@ -0,0 +1,2804 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgObb.h"
+#include "dgHeap.h"
+#include "dgDebug.h"
+#include "dgStack.h"
+#include "dgPolyhedra.h"
+#include "dgConvexHull3d.h"
+#include "dgSmallDeterminant.h"
+
+
+//#define DG_MIN_EDGE_ASPECT_RATIO  dgFloat64 (0.02f)
+
+class dgDiagonalEdge
+{
+	public:
+	dgDiagonalEdge (dgEdge* const edge)
+		:m_i0(edge->m_incidentVertex), m_i1(edge->m_twin->m_incidentVertex)
+	{
+	}
+	dgInt32 m_i0;
+	dgInt32 m_i1;
+};
+
+
+struct dgEdgeCollapseEdgeHandle
+{
+	dgEdgeCollapseEdgeHandle (dgEdge* const newEdge)
+		:m_inList(false), m_edge(newEdge)
+	{
+	}
+
+	dgEdgeCollapseEdgeHandle (const dgEdgeCollapseEdgeHandle &dataHandle)
+		:m_inList(true), m_edge(dataHandle.m_edge)
+	{
+		dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData);
+		if (handle) {
+			dgAssert (handle != this);
+			handle->m_edge = NULL;
+		}
+		m_edge->m_userData = dgUnsigned64 (PointerToInt(this));
+	}
+
+	~dgEdgeCollapseEdgeHandle ()
+	{
+		if (m_inList) {
+			if (m_edge) {
+				dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData);
+				if (handle == this) {
+					m_edge->m_userData = PointerToInt (NULL);
+				}
+			}
+		}
+		m_edge = NULL;
+	}
+
+	dgUnsigned32 m_inList;
+	dgEdge* m_edge;
+};
+
+
+class dgVertexCollapseVertexMetric
+{
+	public:
+	dgVertexCollapseVertexMetric (const dgBigPlane &plane) 
+	{
+		elem[0] = plane.m_x * plane.m_x;  
+		elem[1] = plane.m_y * plane.m_y;  
+		elem[2] = plane.m_z * plane.m_z;  
+		elem[3] = plane.m_w * plane.m_w;  
+		elem[4] = dgFloat64 (2.0) * plane.m_x * plane.m_y;  
+		elem[5] = dgFloat64 (2.0) * plane.m_x * plane.m_z;  
+		elem[6] = dgFloat64 (2.0) * plane.m_x * plane.m_w;  
+		elem[7] = dgFloat64 (2.0) * plane.m_y * plane.m_z;  
+		elem[8] = dgFloat64 (2.0) * plane.m_y * plane.m_w;  
+		elem[9] = dgFloat64 (2.0) * plane.m_z * plane.m_w;  
+	}
+
+	void Clear ()
+	{
+		memset (elem, 0, 10 * sizeof (dgFloat64));
+	}
+
+	void Accumulate (const dgVertexCollapseVertexMetric& p) 
+	{
+		elem[0] += p.elem[0]; 
+		elem[1] += p.elem[1]; 
+		elem[2] += p.elem[2]; 
+		elem[3] += p.elem[3]; 
+		elem[4] += p.elem[4]; 
+		elem[5] += p.elem[5]; 
+		elem[6] += p.elem[6]; 
+		elem[7] += p.elem[7]; 
+		elem[8] += p.elem[8]; 
+		elem[9] += p.elem[9]; 
+	}
+
+	void Accumulate (const dgBigPlane& plane) 
+	{
+		elem[0] += plane.m_x * plane.m_x;  
+		elem[1] += plane.m_y * plane.m_y;  
+		elem[2] += plane.m_z * plane.m_z;  
+		elem[3] += plane.m_w * plane.m_w;  
+
+		elem[4] += dgFloat64 (2.0f) * plane.m_x * plane.m_y;  
+		elem[5] += dgFloat64 (2.0f) * plane.m_x * plane.m_z;  
+		elem[7] += dgFloat64 (2.0f) * plane.m_y * plane.m_z;  
+
+		elem[6] += dgFloat64 (2.0f) * plane.m_x * plane.m_w;  
+		elem[8] += dgFloat64 (2.0f) * plane.m_y * plane.m_w;  
+		elem[9] += dgFloat64 (2.0f) * plane.m_z * plane.m_w;  
+	}
+
+
+	dgFloat64 Evalue (const dgBigVector &p) const 
+	{
+		dgFloat64 acc = elem[0] * p.m_x * p.m_x + elem[1] * p.m_y * p.m_y + elem[2] * p.m_z * p.m_z + 
+						elem[4] * p.m_x * p.m_y + elem[5] * p.m_x * p.m_z + elem[7] * p.m_y * p.m_z + 
+						elem[6] * p.m_x + elem[8] * p.m_y + elem[9] * p.m_z + elem[3];  
+		return fabs (acc);
+	}
+
+	dgFloat64 elem[10];
+};
+
+
+
+dgPolyhedra::dgPolyhedra (dgMemoryAllocator* const allocator)
+	:dgTree <dgEdge, dgInt64>(allocator)
+	,m_baseMark(0)
+	,m_edgeMark(0)
+	,m_faceSecuence(0)
+{
+}
+
+dgPolyhedra::dgPolyhedra (const dgPolyhedra &polyhedra)
+	:dgTree <dgEdge, dgInt64>(polyhedra.GetAllocator())
+	,m_baseMark(0)
+	,m_edgeMark(0)
+	,m_faceSecuence(0)
+{
+	dgStack<dgInt32> indexPool (1024 * 16);
+	dgStack<dgUnsigned64> userPool (1024 * 16);
+	dgInt32* const index = &indexPool[0];
+	dgUnsigned64* const user = &userPool[0];
+
+	BeginFace ();
+	Iterator iter(polyhedra);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+
+		if (!FindEdge(edge->m_incidentVertex, edge->m_twin->m_incidentVertex))	{
+			dgInt32 indexCount = 0;
+			dgEdge* ptr = edge;
+			do {
+				user[indexCount] = ptr->m_userData;
+				index[indexCount] = ptr->m_incidentVertex;
+				indexCount ++;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+
+			dgEdge* const face = AddFace (indexCount, index, (dgInt64*) user);
+			ptr = face;
+			do {
+				ptr->m_incidentFace = edge->m_incidentFace;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+		}
+	}
+	EndFace();
+
+	m_faceSecuence = polyhedra.m_faceSecuence;
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck());
+#endif
+}
+
+dgPolyhedra::~dgPolyhedra ()
+{
+}
+
+
+dgInt32 dgPolyhedra::GetFaceCount() const
+{
+	Iterator iter (*this);
+	dgInt32 count = 0;
+	dgInt32 mark = IncLRU();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+
+		count ++;
+		dgEdge* ptr = edge;
+		do {
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+	}
+	return count;
+}
+
+
+
+dgEdge* dgPolyhedra::AddFace ( dgInt32 count, const dgInt32* const index, const dgInt64* const userdata)
+{
+	class IntersectionFilter
+	{
+		public:
+		IntersectionFilter ()
+		{
+			m_count = 0;
+		}
+
+		bool Insert (dgInt64 value)
+		{
+			dgInt32 i = 0;				
+			for (; i < m_count; i ++) {
+				if (m_array[i] == value) {
+					return false;
+				}
+			}
+			m_array[i] = value;
+			m_count ++;
+			return true;
+		}
+
+		dgInt32 m_count;
+		dgInt64 m_array[2048];
+	};
+
+	IntersectionFilter selfIntersectingFaceFilter;
+
+	dgInt32 i0 = index[count-1];
+	for (dgInt32 i = 0; i < count; i ++) {
+		dgInt32 i1 = index[i];
+
+		dgPairKey code0 (i0, i1);
+		if (!selfIntersectingFaceFilter.Insert (code0.GetVal())) {
+			return NULL;
+		}
+
+		dgPairKey code1 (i1, i0);
+		if (!selfIntersectingFaceFilter.Insert (code1.GetVal())) {
+			return NULL;
+		}
+
+		if (i0 == i1) {
+			return NULL;
+		}
+		if (FindEdge (i0, i1)) {
+			return NULL;
+		}
+		i0 = i1;
+	}
+
+	m_faceSecuence ++;
+
+	i0 = index[count-1];
+	dgInt32 i1 = index[0];
+	dgUnsigned64 udata0 = 0;
+	dgUnsigned64 udata1 = 0;
+	if (userdata) {
+		udata0 = dgUnsigned64 (userdata[count-1]);
+		udata1 = dgUnsigned64 (userdata[0]);
+	} 
+
+	bool state;
+	dgPairKey code (i0, i1);
+	dgEdge tmpEdge (i0, m_faceSecuence, udata0);
+	dgTreeNode* const node = Insert (tmpEdge, code.GetVal(), state); 
+	dgAssert (!state);
+	dgEdge* edge0 = &node->GetInfo();
+	dgEdge* const first = edge0;
+
+	for (dgInt32 i = 1; i < count; i ++) {
+		i0 = i1;
+		i1 = index[i];
+		udata0 = udata1;
+		udata1 = dgUnsigned64 (userdata ? userdata[i] : 0);
+
+		dgPairKey code (i0, i1);
+		dgEdge tmpEdge (i0, m_faceSecuence, udata0);
+		dgTreeNode* const node = Insert (tmpEdge, code.GetVal(), state); 
+		dgAssert (!state);
+
+		dgEdge* const edge1 = &node->GetInfo();
+		edge0->m_next = edge1;
+		edge1->m_prev = edge0;
+		edge0 = edge1;
+	}
+
+	first->m_prev = edge0;
+	edge0->m_next = first;
+
+	return first->m_next;
+}
+
+
+void dgPolyhedra::EndFace ()
+{
+	dgPolyhedra::Iterator iter (*this);
+
+	// Connect all twin edge
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (!edge->m_twin) {
+			edge->m_twin = FindEdge (edge->m_next->m_incidentVertex, edge->m_incidentVertex);
+			if (edge->m_twin) {
+				edge->m_twin->m_twin = edge; 
+			}
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck());
+#endif
+	dgStack<dgEdge*> edgeArrayPool(GetCount() * 2 + 256);
+
+	dgInt32 edgeCount = 0;
+	dgEdge** const edgeArray = &edgeArrayPool[0];
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (!edge->m_twin) {
+			bool state;
+			dgPolyhedra::dgPairKey code (edge->m_next->m_incidentVertex, edge->m_incidentVertex);
+			dgEdge tmpEdge (edge->m_next->m_incidentVertex, -1);
+			tmpEdge.m_incidentFace = -1; 
+			dgPolyhedra::dgTreeNode* const node = Insert (tmpEdge, code.GetVal(), state); 
+			dgAssert (!state);
+			edge->m_twin = &node->GetInfo();
+			edge->m_twin->m_twin = edge; 
+			edgeArray[edgeCount] = edge->m_twin;
+			edgeCount ++;
+		}
+	}
+
+	for (dgInt32 i = 0; i < edgeCount; i ++) {
+		dgEdge* const edge = edgeArray[i];
+		dgAssert (!edge->m_prev);
+		dgEdge *ptr = edge->m_twin;
+		for (; ptr->m_next; ptr = ptr->m_next->m_twin){}
+		ptr->m_next = edge;
+		edge->m_prev = ptr;
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+}
+
+
+void dgPolyhedra::DeleteFace(dgEdge* const face)
+{
+	dgEdge* edgeList[1024 * 16];
+
+	if (face->m_incidentFace > 0) {
+		dgInt32 count = 0;
+		dgEdge* ptr = face;
+		do {
+			ptr->m_incidentFace = -1;
+			dgInt32 i = 0;
+			for (; i < count; i ++) {
+				if ((edgeList[i] == ptr) || (edgeList[i]->m_twin == ptr)) {
+					break;
+				}
+			}
+			if (i == count) {
+				edgeList[count] = ptr;
+				count ++;
+			}
+			ptr = ptr->m_next;
+		} while (ptr != face);
+
+
+		for (dgInt32 i = 0; i < count; i ++) {
+			dgEdge* const ptr = edgeList[i];
+			if (ptr->m_twin->m_incidentFace < 0) {
+				DeleteEdge (ptr);
+			}
+		}
+	}
+}
+
+
+
+dgBigVector dgPolyhedra::FaceNormal (const dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes) const
+{
+	dgInt32 stride = strideInBytes / sizeof (dgFloat64);
+	const dgEdge* edge = face;
+	dgBigVector p0 (&pool[edge->m_incidentVertex * stride]);
+	edge = edge->m_next;
+	dgBigVector p1 (&pool[edge->m_incidentVertex * stride]);
+	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 * stride]);
+		dgBigVector e2 (p2 - p0);
+		normal += e1 * e2;
+		e1 = e2;
+	} 
+	return normal;
+}
+
+
+dgEdge* dgPolyhedra::AddHalfEdge (dgInt32 v0, dgInt32 v1)
+{
+	if (v0 != v1) {
+		dgPairKey pairKey (v0, v1);
+		dgEdge tmpEdge (v0, -1);
+
+		dgTreeNode* node = Insert (tmpEdge, pairKey.GetVal()); 
+		return node ? &node->GetInfo() : NULL;
+	} else {
+		return NULL;
+	}
+}
+
+
+void dgPolyhedra::DeleteEdge (dgEdge* const edge)
+{
+	dgEdge *const twin = edge->m_twin;
+
+	edge->m_prev->m_next = twin->m_next;
+	twin->m_next->m_prev = edge->m_prev;
+	edge->m_next->m_prev = twin->m_prev;
+	twin->m_prev->m_next = edge->m_next;
+
+	dgTreeNode *const nodeA = GetNodeFromInfo (*edge);
+	dgTreeNode *const nodeB = GetNodeFromInfo (*twin);
+
+	dgAssert (&nodeA->GetInfo() == edge);
+	dgAssert (&nodeB->GetInfo() == twin);
+
+	Remove (nodeA);
+	Remove (nodeB);
+}
+
+
+dgEdge* dgPolyhedra::ConnectVertex (dgEdge* const e0, dgEdge* const e1)
+{
+	dgEdge* const edge = AddHalfEdge(e1->m_incidentVertex, e0->m_incidentVertex);
+	dgEdge* const twin = AddHalfEdge(e0->m_incidentVertex, e1->m_incidentVertex);
+	dgAssert ((edge && twin) || !(edge || twin));
+	if (edge) {
+		edge->m_twin = twin;
+		twin->m_twin = edge;
+
+		edge->m_incidentFace = e0->m_incidentFace;
+		twin->m_incidentFace = e1->m_incidentFace;
+
+		edge->m_userData = e1->m_userData;
+		twin->m_userData = e0->m_userData;
+
+		edge->m_next = e0;
+		edge->m_prev = e1->m_prev;
+
+		twin->m_next = e1;
+		twin->m_prev = e0->m_prev;
+
+		e0->m_prev->m_next = twin;
+		e0->m_prev = edge;
+
+		e1->m_prev->m_next = edge;
+		e1->m_prev = twin;
+	}
+
+	return edge;
+}
+
+dgEdge* dgPolyhedra::SpliteEdge (dgInt32 newIndex,	dgEdge* const edge)
+{
+	dgEdge* const edge00 = edge->m_prev;
+	dgEdge* const edge01 = edge->m_next;
+	dgEdge* const twin00 = edge->m_twin->m_next;
+	dgEdge* const twin01 = edge->m_twin->m_prev;
+
+	dgInt32 i0 = edge->m_incidentVertex;
+	dgInt32 i1 = edge->m_twin->m_incidentVertex;
+
+	dgInt32 f0 = edge->m_incidentFace;
+	dgInt32 f1 = edge->m_twin->m_incidentFace;
+
+	DeleteEdge (edge);
+
+	dgEdge* const edge0 = AddHalfEdge (i0, newIndex);
+	dgEdge* const edge1 = AddHalfEdge (newIndex, i1);
+
+	dgEdge* const twin0 = AddHalfEdge (newIndex, i0);
+	dgEdge* const twin1 = AddHalfEdge (i1, newIndex);
+	dgAssert (edge0);
+	dgAssert (edge1);
+	dgAssert (twin0);
+	dgAssert (twin1);
+
+	edge0->m_twin = twin0;
+	twin0->m_twin = edge0;
+
+	edge1->m_twin = twin1;
+	twin1->m_twin = edge1;
+
+	edge0->m_next = edge1;
+	edge1->m_prev = edge0;
+
+	twin1->m_next = twin0;
+	twin0->m_prev = twin1;
+
+	edge0->m_prev = edge00;
+	edge00 ->m_next = edge0;
+
+	edge1->m_next = edge01;
+	edge01->m_prev = edge1;
+
+	twin0->m_next = twin00;
+	twin00->m_prev = twin0;
+
+	twin1->m_prev = twin01;
+	twin01->m_next = twin1;
+
+	edge0->m_incidentFace = f0;
+	edge1->m_incidentFace = f0;
+
+	twin0->m_incidentFace = f1;
+	twin1->m_incidentFace = f1;
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	//	dgAssert (SanityCheck ());
+#endif
+
+	return edge0;
+}
+
+
+
+bool dgPolyhedra::FlipEdge (dgEdge* const edge)
+{
+	//	dgTreeNode *node;
+	if (edge->m_next->m_next->m_next != edge) {
+		return false;
+	}
+
+	if (edge->m_twin->m_next->m_next->m_next != edge->m_twin) {
+		return false;
+	}
+
+	if (FindEdge(edge->m_prev->m_incidentVertex, edge->m_twin->m_prev->m_incidentVertex)) {
+		return false;
+	}
+
+	dgEdge *const prevEdge = edge->m_prev;
+	dgEdge *const prevTwin = edge->m_twin->m_prev;
+
+	dgPairKey edgeKey (prevTwin->m_incidentVertex, prevEdge->m_incidentVertex);
+	dgPairKey twinKey (prevEdge->m_incidentVertex, prevTwin->m_incidentVertex);
+
+	ReplaceKey (GetNodeFromInfo (*edge), edgeKey.GetVal());
+	//	dgAssert (node);
+
+	ReplaceKey (GetNodeFromInfo (*edge->m_twin), twinKey.GetVal());
+	//	dgAssert (node);
+
+	edge->m_incidentVertex = prevTwin->m_incidentVertex;
+	edge->m_twin->m_incidentVertex = prevEdge->m_incidentVertex;
+
+	edge->m_userData = prevTwin->m_userData;
+	edge->m_twin->m_userData = prevEdge->m_userData;
+
+	prevEdge->m_next = edge->m_twin->m_next;
+	prevTwin->m_prev->m_prev = edge->m_prev;
+
+	prevTwin->m_next = edge->m_next;
+	prevEdge->m_prev->m_prev = edge->m_twin->m_prev;
+
+	edge->m_prev = prevTwin->m_prev;
+	edge->m_next = prevEdge;
+
+	edge->m_twin->m_prev = prevEdge->m_prev;
+	edge->m_twin->m_next = prevTwin;
+
+	prevTwin->m_prev->m_next = edge;
+	prevTwin->m_prev = edge->m_twin;
+
+	prevEdge->m_prev->m_next = edge->m_twin;
+	prevEdge->m_prev = edge;
+
+	edge->m_next->m_incidentFace = edge->m_incidentFace;
+	edge->m_prev->m_incidentFace = edge->m_incidentFace;
+
+	edge->m_twin->m_next->m_incidentFace = edge->m_twin->m_incidentFace;
+	edge->m_twin->m_prev->m_incidentFace = edge->m_twin->m_incidentFace;
+
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+
+	return true;
+}
+
+
+
+bool dgPolyhedra::GetConectedSurface (dgPolyhedra &polyhedra) const
+{
+	if (!GetCount()) {
+		return false;
+	}
+
+	dgEdge* edge = NULL;
+	Iterator iter(*this);
+	for (iter.Begin (); iter; iter ++) {
+		edge = &(*iter);
+		if ((edge->m_mark < m_baseMark) && (edge->m_incidentFace > 0)) {
+			break;
+		}
+	}
+
+	if (!iter) {
+		return false;
+	}
+
+	dgInt32 faceIndex[4096];
+	dgInt64 faceDataIndex[4096];
+	dgStack<dgEdge*> stackPool (GetCount()); 
+	dgEdge** const stack = &stackPool[0];
+
+	dgInt32 mark = IncLRU();
+
+	polyhedra.BeginFace ();
+	stack[0] = edge;
+	dgInt32 index = 1;
+	while (index) {
+		index --;
+		dgEdge* const edge = stack[index];
+		dgAssert (edge);
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		dgInt32 count = 0;
+		dgEdge* ptr = edge;
+		do {
+			dgAssert (ptr);
+			ptr->m_mark = mark;
+			faceIndex[count] = ptr->m_incidentVertex;
+			faceDataIndex[count] = dgInt64 (ptr->m_userData);
+			count ++;
+			dgAssert (count <  dgInt32 ((sizeof (faceIndex)/sizeof(faceIndex[0]))));
+
+			if ((ptr->m_twin->m_incidentFace > 0) && (ptr->m_twin->m_mark != mark)) {
+				stack[index] = ptr->m_twin;
+				index ++;
+				dgAssert (index < GetCount());
+			}
+
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+
+		polyhedra.AddFace (count, &faceIndex[0], &faceDataIndex[0]);
+	}
+
+	polyhedra.EndFace ();
+
+	return true;
+}
+
+
+void dgPolyhedra::ChangeEdgeIncidentVertex (dgEdge* const edge, dgInt32 newIndex)
+{
+	dgEdge* ptr = edge;
+	do {
+		dgTreeNode* node = GetNodeFromInfo(*ptr);
+		dgPairKey Key0 (newIndex, ptr->m_twin->m_incidentVertex);
+		ReplaceKey (node, Key0.GetVal());
+
+		node = GetNodeFromInfo(*ptr->m_twin);
+		dgPairKey Key1 (ptr->m_twin->m_incidentVertex, newIndex);
+		ReplaceKey (node, Key1.GetVal());
+
+		ptr->m_incidentVertex = newIndex;
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != edge);
+}
+
+
+void dgPolyhedra::DeleteDegenerateFaces (const dgFloat64* const pool, dgInt32 strideInBytes, dgFloat64 area)
+{
+	if (!GetCount()) {
+		return;
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+	dgStack <dgPolyhedra::dgTreeNode*> faceArrayPool(GetCount() / 2 + 100);
+
+	dgInt32 count = 0;
+	dgPolyhedra::dgTreeNode** const faceArray = &faceArrayPool[0];
+	dgInt32 mark = IncLRU();
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+
+		if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) {
+			faceArray[count] = iter.GetNode();
+			count ++;
+			dgEdge* ptr = edge;
+			do	{
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+	}
+
+	dgFloat64 area2 = area * area;
+	area2 *= dgFloat64 (4.0f);
+
+	for (dgInt32 i = 0; i < count; i ++) {
+		dgPolyhedra::dgTreeNode* const faceNode = faceArray[i];
+		dgEdge* const edge = &faceNode->GetInfo();
+
+		dgBigVector normal (FaceNormal (edge, pool, strideInBytes));
+
+		dgFloat64 faceArea = normal % normal;
+		if (faceArea < area2) {
+			DeleteFace (edge);
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	mark = IncLRU();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) {
+			//dgAssert (edge->m_next->m_next->m_next == edge);
+			dgEdge* ptr = edge;
+			do	{
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+
+			dgBigVector normal (FaceNormal (edge, pool, strideInBytes));
+
+			dgFloat64 faceArea = normal % normal;
+			dgAssert (faceArea >= area2);
+		}
+	}
+	dgAssert (SanityCheck ());
+#endif
+}
+
+
+
+
+
+static dgBigPlane UnboundedLoopPlane (dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const pool)
+{
+	const dgBigVector p0 = pool[i0];
+	const dgBigVector p1 = pool[i1];
+	const dgBigVector p2 = pool[i2];
+	dgBigVector E0 (p1 - p0); 
+	dgBigVector E1 (p2 - p0); 
+
+	dgBigVector N ((E0 * E1) * E0); 
+	dgFloat64 dist = - (N % p0);
+	dgBigPlane plane (N, dist);
+
+	dgFloat64 mag = sqrt (plane % plane);
+	if (mag < dgFloat64 (1.0e-12f)) {
+		mag = dgFloat64 (1.0e-12f);
+	}
+	mag = dgFloat64 (10.0f) / mag;
+
+	plane.m_x *= mag;
+	plane.m_y *= mag;
+	plane.m_z *= mag;
+	plane.m_w *= mag;
+
+	return plane;
+}
+
+dgEdge* dgPolyhedra::CollapseEdge(dgEdge* const edge)
+{
+	dgInt32 v0 = edge->m_incidentVertex;
+	dgInt32 v1 = edge->m_twin->m_incidentVertex;
+
+	dgEdge* retEdge = edge->m_twin->m_prev->m_twin;
+	if (retEdge	== edge->m_twin->m_next) {
+		return NULL;
+	}
+	if (retEdge	== edge->m_twin) {
+		return NULL;
+	}
+	if (retEdge	== edge->m_next) {
+		retEdge = edge->m_prev->m_twin;
+		if (retEdge	== edge->m_twin->m_next) {
+			return NULL;
+		}
+		if (retEdge	== edge->m_twin) {
+			return NULL;
+		}
+	}
+
+	dgEdge* lastEdge = NULL;
+	dgEdge* firstEdge = NULL;
+	if ((edge->m_incidentFace >= 0)	&& (edge->m_twin->m_incidentFace >= 0)) {	
+		lastEdge = edge->m_prev->m_twin;
+		firstEdge = edge->m_twin->m_next->m_twin->m_next;
+	} else if (edge->m_twin->m_incidentFace >= 0) {
+		firstEdge = edge->m_twin->m_next->m_twin->m_next;
+		lastEdge = edge;
+	} else {
+		lastEdge = edge->m_prev->m_twin;
+		firstEdge = edge->m_twin->m_next;
+	}
+
+	for (dgEdge* ptr = firstEdge; ptr != lastEdge; ptr = ptr->m_twin->m_next) {
+		dgEdge* const badEdge = FindEdge (edge->m_twin->m_incidentVertex, ptr->m_twin->m_incidentVertex);
+		if (badEdge) {
+			return NULL;
+		}
+	} 
+
+	dgEdge* const twin = edge->m_twin;
+	if (twin->m_next == twin->m_prev->m_prev) {
+		twin->m_prev->m_twin->m_twin = twin->m_next->m_twin;
+		twin->m_next->m_twin->m_twin = twin->m_prev->m_twin;
+
+		Remove (GetNodeFromInfo(*twin->m_prev));
+		Remove (GetNodeFromInfo(*twin->m_next));
+	} else {
+		twin->m_next->m_userData = twin->m_userData;
+		twin->m_next->m_prev = twin->m_prev;
+		twin->m_prev->m_next = twin->m_next;
+	}
+
+	if (edge->m_next == edge->m_prev->m_prev) {
+		edge->m_next->m_twin->m_twin = edge->m_prev->m_twin;
+		edge->m_prev->m_twin->m_twin = edge->m_next->m_twin;
+		Remove (GetNodeFromInfo(*edge->m_next));
+		Remove (GetNodeFromInfo(*edge->m_prev));
+	} else {
+		edge->m_next->m_prev = edge->m_prev;
+		edge->m_prev->m_next = edge->m_next;
+	}
+
+	dgAssert (twin->m_twin->m_incidentVertex == v0);
+	dgAssert (edge->m_twin->m_incidentVertex == v1);
+	Remove (GetNodeFromInfo(*twin));
+	Remove (GetNodeFromInfo(*edge));
+
+	dgEdge* ptr = retEdge;
+	do {
+		dgPolyhedra::dgPairKey pairKey (v0, ptr->m_twin->m_incidentVertex);
+
+		dgPolyhedra::dgTreeNode* node = Find (pairKey.GetVal());
+		if (node) {
+			if (&node->GetInfo() == ptr) {
+				dgPolyhedra::dgPairKey key (v1, ptr->m_twin->m_incidentVertex);
+				ptr->m_incidentVertex = v1;
+				node = ReplaceKey (node, key.GetVal());
+				dgAssert (node);
+			} 
+		}
+
+		dgPolyhedra::dgPairKey TwinKey (ptr->m_twin->m_incidentVertex, v0);
+		node = Find (TwinKey.GetVal());
+		if (node) {
+			if (&node->GetInfo() == ptr->m_twin) {
+				dgPolyhedra::dgPairKey key (ptr->m_twin->m_incidentVertex, v1);
+				node = ReplaceKey (node, key.GetVal());
+				dgAssert (node);
+			}
+		}
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != retEdge);
+
+	return retEdge;
+}
+
+
+
+void dgPolyhedra::RemoveHalfEdge (dgEdge* const edge)
+{
+	dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *) IntToPointer (edge->m_userData);
+	if (handle) { 
+		handle->m_edge = NULL;
+	}
+
+	dgPolyhedra::dgTreeNode* const node = GetNodeFromInfo(*edge);
+	dgAssert (node);
+	Remove (node);
+}
+
+
+dgEdge* dgPolyhedra::FindEarTip (dgEdge* const face, const dgFloat64* const pool, dgInt32 stride, dgDownHeap<dgEdge*, dgFloat64>& heap, const dgBigVector &normal) const
+{
+	dgEdge* ptr = face;
+	dgBigVector p0 (&pool[ptr->m_prev->m_incidentVertex * stride]);
+	dgBigVector p1 (&pool[ptr->m_incidentVertex * stride]);
+	dgBigVector d0 (p1 - p0);
+	dgFloat64 f = sqrt (d0 % d0);
+	if (f < dgFloat64 (1.0e-10f)) {
+		f = dgFloat64 (1.0e-10f);
+	}
+	d0 = d0.Scale3 (dgFloat64 (1.0f) / f);
+
+	dgFloat64 minAngle = dgFloat32 (10.0f);
+	do {
+		dgBigVector p2 (&pool [ptr->m_next->m_incidentVertex * stride]);
+		dgBigVector d1 (p2 - p1);
+		dgFloat64 f = dgFloat64 (1.0f) / sqrt (d1 % d1);
+		if (f < dgFloat64 (1.0e-10f)) {
+			f = dgFloat64 (1.0e-10f);
+		}
+		d1 = d1.Scale3 (dgFloat32 (1.0f) / f);
+		dgBigVector n (d0 * d1);
+
+		dgFloat64 angle = normal %  n;
+		if (angle >= dgFloat64 (0.0f)) {
+			heap.Push (ptr, angle);
+		}
+
+		if (angle < minAngle) {
+			minAngle = angle;
+		}
+
+		d0 = d1;
+		p1 = p2;
+		ptr = ptr->m_next;
+	} while (ptr != face);
+
+	if (minAngle > dgFloat32 (0.1f)) {
+		return heap[0];
+	}
+
+	dgEdge* ear = NULL;
+	while (heap.GetCount()) {
+		ear = heap[0];
+		heap.Pop();
+
+		if (FindEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex)) {
+			continue;
+		}
+
+		dgBigVector p0 (&pool [ear->m_prev->m_incidentVertex * stride]);
+		dgBigVector p1 (&pool [ear->m_incidentVertex * stride]);
+		dgBigVector p2 (&pool [ear->m_next->m_incidentVertex * stride]);
+
+		dgBigVector p10 (p1 - p0);
+		dgBigVector p21 (p2 - p1);
+		dgBigVector p02 (p0 - p2);
+
+		for (ptr = ear->m_next->m_next; ptr != ear->m_prev; ptr = ptr->m_next) {
+			dgBigVector p (&pool [ptr->m_incidentVertex * stride]);
+
+			dgFloat64 side = ((p - p0) * p10) % normal;
+			if (side < dgFloat64 (0.05f)) {
+				side = ((p - p1) * p21) % normal;
+				if (side < dgFloat64 (0.05f)) {
+					side = ((p - p2) * p02) % normal;
+					if (side < dgFloat32 (0.05f)) {
+						break;
+					}
+				}
+			}
+		}
+
+		if (ptr == ear->m_prev) {
+			break;
+		}
+	}
+
+	return ear;
+}
+
+
+
+dgEdge* dgPolyhedra::TriangulateFace (dgEdge* const faceIn, const dgFloat64* const pool, dgInt32 stride, dgDownHeap<dgEdge*, dgFloat64>& heap, dgBigVector* const faceNormalOut)
+{
+	dgEdge* face = faceIn;
+//	dgEdge* perimeter [1024 * 16]; 
+//	dgEdge* ptr = face;
+//	dgInt32 perimeterCount = 0;
+//	do {
+//		perimeter[perimeterCount] = ptr;
+//		perimeterCount ++;
+//		dgAssert (perimeterCount < dgInt32 (sizeof (perimeter) / sizeof (perimeter[0])));
+//		ptr = ptr->m_next;
+//	} while (ptr != face);
+//	perimeter[perimeterCount] = face;
+//	dgAssert ((perimeterCount + 1) < dgInt32 (sizeof (perimeter) / sizeof (perimeter[0])));
+
+	dgBigVector normal (FaceNormal (face, pool, dgInt32 (stride * sizeof (dgFloat64))));
+
+	dgFloat64 dot = normal % normal;
+	if (dot < dgFloat64 (1.0e-12f)) {
+		if (faceNormalOut) {
+			*faceNormalOut = dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
+		}
+		return face;
+	}
+	normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (dot));
+	if (faceNormalOut) {
+		*faceNormalOut = normal;
+	}
+
+	while (face->m_next->m_next->m_next != face) {
+		dgEdge* const ear = FindEarTip (face, pool, stride, heap, normal); 
+		if (!ear) {
+			return face;
+		}
+		if ((face == ear)	|| (face == ear->m_prev)) {
+			face = ear->m_prev->m_prev;
+		}
+		dgEdge* const edge = AddHalfEdge (ear->m_next->m_incidentVertex, ear->m_prev->m_incidentVertex);
+		if (!edge) {
+			return face;
+		}
+		dgEdge* const twin = AddHalfEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex);
+		if (!twin) {
+			return face;
+		}
+		dgAssert (twin);
+
+
+		edge->m_mark = ear->m_mark;
+		edge->m_userData = ear->m_next->m_userData;
+		edge->m_incidentFace = ear->m_incidentFace;
+
+		twin->m_mark = ear->m_mark;
+		twin->m_userData = ear->m_prev->m_userData;
+		twin->m_incidentFace = ear->m_incidentFace;
+
+		edge->m_twin = twin;
+		twin->m_twin = edge;
+
+		twin->m_prev = ear->m_prev->m_prev;
+		twin->m_next = ear->m_next;
+		ear->m_prev->m_prev->m_next = twin;
+		ear->m_next->m_prev = twin;
+
+		edge->m_next = ear->m_prev;
+		edge->m_prev = ear;
+		ear->m_prev->m_prev = edge;
+		ear->m_next = edge;
+
+		heap.Flush ();
+	}
+	return NULL;
+}
+
+void dgPolyhedra::PolygonizeFace (dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes)
+{
+	if (face->m_next->m_next->m_next != face) {
+		dgInt32 mark = IncLRU();
+		dgEdge* ptr = face;
+		do {
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != face);
+		char memPool [1024 * (sizeof (dgEdge*) + sizeof (dgFloat64))]; 
+		dgDownHeap<dgEdge*, dgFloat64> heap(&memPool[0], sizeof (memPool));
+
+		dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+		dgEdge* const edge = TriangulateFace (face, pool, stride, heap, NULL);
+		dgAssert (!edge);
+		if (edge) {
+			dgAssert (0);
+		}
+
+		//dgAssert (0);
+		//OptimizeTriangulation (pool, strideInBytes);
+	}
+}
+
+
+void dgPolyhedra::MarkAdjacentCoplanarFaces (dgPolyhedra& polyhedraOut, dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes)
+{
+	const dgFloat64 normalDeviation = dgFloat64 (0.9999f);
+	const dgFloat64 distanceFromPlane = dgFloat64 (1.0f / 128.0f);
+
+	dgInt32 faceIndex[1024 * 4];
+	dgEdge* stack[1024 * 4];
+	dgEdge* deleteEdge[1024 * 4];
+
+	dgInt32 deleteCount = 1;
+	deleteEdge[0] = face;
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+
+	dgAssert (face->m_incidentFace > 0);
+
+	dgBigVector normalAverage (FaceNormal (face, pool, strideInBytes));
+	dgFloat64 dot = normalAverage % normalAverage;
+	if (dot > dgFloat64 (1.0e-12f)) {
+		dgInt32 testPointsCount = 1;
+		dot = dgFloat64 (1.0f) / sqrt (dot);
+		dgBigVector normal (normalAverage.Scale3 (dot));
+
+		dgBigVector averageTestPoint (&pool[face->m_incidentVertex * stride]);
+		dgBigPlane testPlane(normal, - (averageTestPoint % normal));
+
+		polyhedraOut.BeginFace();
+
+		IncLRU();
+		dgInt32 faceMark = IncLRU();
+
+		dgInt32 faceIndexCount = 0;
+		dgEdge* ptr = face;
+		do {
+			ptr->m_mark = faceMark;
+			faceIndex[faceIndexCount] = ptr->m_incidentVertex;
+			faceIndexCount ++;
+			dgAssert (faceIndexCount < dgInt32 (sizeof (faceIndex) / sizeof (faceIndex[0])));
+			ptr = ptr ->m_next;
+		} while (ptr != face);
+		polyhedraOut.AddFace(faceIndexCount, faceIndex);
+
+		dgInt32 index = 1;
+		deleteCount = 0;
+		stack[0] = face;
+		while (index) {
+			index --;
+			dgEdge* const face = stack[index];
+			deleteEdge[deleteCount] = face;
+			deleteCount ++;
+			dgAssert (deleteCount < dgInt32 (sizeof (deleteEdge) / sizeof (deleteEdge[0])));
+			dgAssert (face->m_next->m_next->m_next == face);
+
+			dgEdge* edge = face;
+			do {
+				dgEdge* const ptr = edge->m_twin;
+				if (ptr->m_incidentFace > 0) {
+					if (ptr->m_mark != faceMark) {
+						dgEdge* ptr1 = ptr;
+						faceIndexCount = 0;
+						do {
+							ptr1->m_mark = faceMark;
+							faceIndex[faceIndexCount] = ptr1->m_incidentVertex;
+							dgAssert (faceIndexCount < dgInt32 (sizeof (faceIndex) / sizeof (faceIndex[0])));
+							faceIndexCount ++;
+							ptr1 = ptr1 ->m_next;
+						} while (ptr1 != ptr);
+
+						dgBigVector normal1 (FaceNormal (ptr, pool, strideInBytes));
+						dot = normal1 % normal1;
+						if (dot < dgFloat64 (1.0e-12f)) {
+							deleteEdge[deleteCount] = ptr;
+							deleteCount ++;
+							dgAssert (deleteCount < dgInt32 (sizeof (deleteEdge) / sizeof (deleteEdge[0])));
+						} else {
+							//normal1 = normal1.Scale3 (dgFloat64 (1.0f) / sqrt (dot));
+							dgBigVector testNormal (normal1.Scale3 (dgFloat64 (1.0f) / sqrt (dot)));
+							dot = normal % testNormal;
+							if (dot >= normalDeviation) {
+								dgBigVector testPoint (&pool[ptr->m_prev->m_incidentVertex * stride]);
+								dgFloat64 dist = fabs (testPlane.Evalue (testPoint));
+								if (dist < distanceFromPlane) {
+									testPointsCount ++;
+
+									averageTestPoint += testPoint;
+									testPoint = averageTestPoint.Scale3 (dgFloat64 (1.0f) / dgFloat64(testPointsCount));
+
+									normalAverage += normal1;
+									testNormal = normalAverage.Scale3 (dgFloat64 (1.0f) / sqrt (normalAverage % normalAverage));
+									testPlane = dgBigPlane (testNormal, - (testPoint % testNormal));
+
+									polyhedraOut.AddFace(faceIndexCount, faceIndex);;
+									stack[index] = ptr;
+									index ++;
+									dgAssert (index < dgInt32 (sizeof (stack) / sizeof (stack[0])));
+								}
+							}
+						}
+					}
+				}
+
+				edge = edge->m_next;
+			} while (edge != face);
+		}
+		polyhedraOut.EndFace();
+	}
+
+	for (dgInt32 index = 0; index < deleteCount; index ++) {
+		DeleteFace (deleteEdge[index]);
+	}
+}
+
+
+void dgPolyhedra::RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride, dgBigVector* const normal, dgInt32 perimeterCount, dgEdge** const perimeter)
+{
+	dgList<dgDiagonalEdge> dignonals(GetAllocator());
+
+	for (dgInt32 i = 1; i <= perimeterCount; i ++) {
+		dgEdge* const last = perimeter[i - 1];
+		for (dgEdge* ptr = perimeter[i]->m_prev; ptr != last; ptr = ptr->m_twin->m_prev) {
+			dgList<dgDiagonalEdge>::dgListNode* node = dignonals.GetFirst();
+			for (; node; node = node->GetNext()) {
+				const dgDiagonalEdge& key = node->GetInfo();
+				if (((key.m_i0 == ptr->m_incidentVertex) && (key.m_i1 == ptr->m_twin->m_incidentVertex)) ||
+					((key.m_i1 == ptr->m_incidentVertex) && (key.m_i0 == ptr->m_twin->m_incidentVertex))) {
+						break;
+				}
+			}
+			if (!node) {
+				dgDiagonalEdge key (ptr);
+				dignonals.Append(key);
+			}
+		}
+	}
+
+	dgEdge* const face = perimeter[0];
+	dgInt32 i0 = face->m_incidentVertex * stride;
+	dgInt32 i1 = face->m_next->m_incidentVertex * stride;
+	dgBigVector p0 (vertex[i0], vertex[i0 + 1], vertex[i0 + 2], dgFloat32 (0.0f));
+	dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], dgFloat32 (0.0f));
+
+	dgBigVector p1p0 (p1 - p0);
+	dgFloat64 mag2 = p1p0 % p1p0;
+	for (dgEdge* ptr = face->m_next->m_next; mag2 < dgFloat32 (1.0e-12f); ptr = ptr->m_next) {
+		dgInt32 i1 = ptr->m_incidentVertex * stride;
+		dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], dgFloat32 (0.0f));
+		p1p0 = p1 - p0;
+		mag2 = p1p0 % p1p0;
+	}
+
+	dgMatrix matrix (dgGetIdentityMatrix());
+	matrix.m_posit = p0;
+	matrix.m_front = dgVector (p1p0.Scale3 (dgFloat64 (1.0f) / sqrt (mag2)));
+	matrix.m_right = dgVector (normal->Scale3 (dgFloat64 (1.0f) / sqrt (*normal % *normal)));
+	matrix.m_up = matrix.m_right * matrix.m_front;
+	matrix = matrix.Inverse();
+	matrix.m_posit.m_w = dgFloat32 (1.0f);
+
+	dgInt32 maxCount = dignonals.GetCount() * dignonals.GetCount();
+	while (dignonals.GetCount() && maxCount) {
+		maxCount --;
+		dgList<dgDiagonalEdge>::dgListNode* const node = dignonals.GetFirst();
+		dgDiagonalEdge key (node->GetInfo());
+		dignonals.Remove(node);
+		dgEdge* const edge = FindEdge(key.m_i0, key.m_i1);
+		if (edge) {
+			dgInt32 i0 = edge->m_incidentVertex * stride;
+			dgInt32 i1 = edge->m_next->m_incidentVertex * stride;
+			dgInt32 i2 = edge->m_next->m_next->m_incidentVertex * stride;
+			dgInt32 i3 = edge->m_twin->m_prev->m_incidentVertex * stride;
+
+			dgBigVector p0 (vertex[i0], vertex[i0 + 1], vertex[i0 + 2], dgFloat64 (1.0f));
+			dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], dgFloat64 (1.0f));
+			dgBigVector p2 (vertex[i2], vertex[i2 + 1], vertex[i2 + 2], dgFloat64 (1.0f));
+			dgBigVector p3 (vertex[i3], vertex[i3 + 1], vertex[i3 + 2], dgFloat64 (1.0f));
+
+			p0 = matrix.TransformVector(p0);
+			p1 = matrix.TransformVector(p1);
+			p2 = matrix.TransformVector(p2);
+			p3 = matrix.TransformVector(p3);
+
+			dgFloat64 circleTest[3][3];
+			circleTest[0][0] = p0[0] - p3[0];
+			circleTest[0][1] = p0[1] - p3[1];
+			circleTest[0][2] = circleTest[0][0] * circleTest[0][0] + circleTest[0][1] * circleTest[0][1];
+
+			circleTest[1][0] = p1[0] - p3[0];
+			circleTest[1][1] = p1[1] - p3[1];
+			circleTest[1][2] = circleTest[1][0] * circleTest[1][0] + circleTest[1][1] * circleTest[1][1];
+
+			circleTest[2][0] = p2[0] - p3[0];
+			circleTest[2][1] = p2[1] - p3[1];
+			circleTest[2][2] = circleTest[2][0] * circleTest[2][0] + circleTest[2][1] * circleTest[2][1];
+
+			dgFloat64 error;
+			dgFloat64 det = Determinant3x3 (circleTest, &error);
+			if (det < dgFloat32 (0.0f)) {
+				dgEdge* frontFace0 = edge->m_prev;
+				dgEdge* backFace0 = edge->m_twin->m_prev;
+
+				FlipEdge(edge);
+
+				if (perimeterCount > 4) {
+					dgEdge* backFace1 = backFace0->m_next;
+					dgEdge* frontFace1 = frontFace0->m_next;
+					for (dgInt32 i = 0; i < perimeterCount; i ++) {
+						if (frontFace0 == perimeter[i]) {
+							frontFace0 = NULL;
+						}
+						if (frontFace1 == perimeter[i]) {
+							frontFace1 = NULL;
+						}
+
+						if (backFace0 == perimeter[i]) {
+							backFace0 = NULL;
+						}
+						if (backFace1 == perimeter[i]) {
+							backFace1 = NULL;
+						}
+					}
+
+					if (backFace0 && (backFace0->m_incidentFace > 0) && (backFace0->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (backFace0);
+						dignonals.Append(key);
+					}
+					if (backFace1 && (backFace1->m_incidentFace > 0) && (backFace1->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (backFace1);
+						dignonals.Append(key);
+					}
+
+					if (frontFace0 && (frontFace0->m_incidentFace > 0) && (frontFace0->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (frontFace0);
+						dignonals.Append(key);
+					}
+
+					if (frontFace1 && (frontFace1->m_incidentFace > 0) && (frontFace1->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (frontFace1);
+						dignonals.Append(key);
+					}
+				}
+			}
+		}
+	}
+}
+
+
+void dgPolyhedra::RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride)
+{
+	if (GetCount() <= 6) {
+		return;
+	}
+
+	dgInt32 mark = IncLRU();
+	dgInt32 loopCount = 0;
+	
+	dgPolyhedra::Iterator iter (*this);
+	dgEdge* edgePerimeters[1024 * 16];
+	dgInt32 perimeterCount = 0;
+	dgTree<dgEdge*, dgInt32> filter (GetAllocator());
+	for (iter.Begin(); iter && (loopCount <= 1) ; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_incidentFace < 0) && (edge->m_mark != mark)){
+			loopCount ++;
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				if (!filter.Insert(ptr, ptr->m_incidentVertex)) {
+					loopCount = 2;
+					break;
+				}
+				edgePerimeters[perimeterCount] = ptr->m_twin;
+				perimeterCount ++;
+				dgAssert (perimeterCount < dgInt32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
+				ptr = ptr->m_prev;
+			} while (ptr != edge);
+		}
+	}
+
+	if (loopCount == 1) {
+		#ifdef _DEBUG
+		for (dgInt32 i = 0; i < perimeterCount; i ++) {
+			for (dgInt32 j = i + 1; j < perimeterCount; j ++) {
+				dgAssert (edgePerimeters[i]->m_incidentVertex != edgePerimeters[j]->m_incidentVertex);
+			}
+		}
+		#endif
+
+		dgAssert (perimeterCount);
+		dgAssert (perimeterCount < dgInt32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
+		edgePerimeters[perimeterCount] = edgePerimeters[0];
+
+		dgBigVector normal (FaceNormal(edgePerimeters[0], vertex, dgInt32 (stride * sizeof (dgFloat64))));
+		if ((normal % normal) > dgFloat32 (1.0e-12f)) {
+			RefineTriangulation (vertex, stride, &normal, perimeterCount, edgePerimeters);
+		}
+	}
+}
+
+
+void dgPolyhedra::OptimizeTriangulation (const dgFloat64* const vertex, dgInt32 strideInBytes)
+{
+	dgInt32 polygon[1024 * 8];
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+
+	dgPolyhedra leftOver(GetAllocator());
+	dgPolyhedra buildConvex(GetAllocator());
+
+	buildConvex.BeginFace();
+	dgPolyhedra::Iterator iter (*this);
+
+	for (iter.Begin(); iter; ) {
+		dgEdge* const edge = &(*iter);
+		iter++;
+
+		if (edge->m_incidentFace > 0) {
+			dgPolyhedra flatFace(GetAllocator());
+			MarkAdjacentCoplanarFaces (flatFace, edge, vertex, strideInBytes);
+			//dgAssert (flatFace.GetCount());
+
+			if (flatFace.GetCount()) {
+				flatFace.RefineTriangulation (vertex, stride);
+
+				dgInt32 mark = flatFace.IncLRU();
+				dgPolyhedra::Iterator iter (flatFace);
+				for (iter.Begin(); iter; iter ++) {
+					dgEdge* const edge = &(*iter);
+					if (edge->m_mark != mark) {
+						if (edge->m_incidentFace > 0) {
+							dgEdge* ptr = edge;
+							dgInt32 vertexCount = 0;
+							do {
+								polygon[vertexCount] = ptr->m_incidentVertex;				
+								vertexCount ++;
+								dgAssert (vertexCount < dgInt32 (sizeof (polygon) / sizeof (polygon[0])));
+								ptr->m_mark = mark;
+								ptr = ptr->m_next;
+							} while (ptr != edge);
+							if (vertexCount >= 3) {
+								buildConvex.AddFace (vertexCount, polygon);
+							}
+						}
+					}
+				}
+			}
+			iter.Begin();
+		}
+	}
+	buildConvex.EndFace();
+	dgAssert (GetCount() == 0);
+	SwapInfo(buildConvex);
+}
+
+
+void dgPolyhedra::Triangulate (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOver)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+
+	dgInt32 count = GetCount() / 2;
+	dgStack<char> memPool (dgInt32 ((count + 512) * (2 * sizeof (dgFloat64)))); 
+	dgDownHeap<dgEdge*, dgFloat64> heap(&memPool[0], memPool.GetSizeInBytes());
+
+	dgInt32 mark = IncLRU();
+	Iterator iter (*this);
+	for (iter.Begin(); iter; ) {
+		dgEdge* const thisEdge = &(*iter);
+		iter ++;
+
+		if (thisEdge->m_mark == mark) {
+			continue;
+		}
+		if (thisEdge->m_incidentFace < 0) {
+			continue;
+		}
+
+		count = 0;
+		dgEdge* ptr = thisEdge;
+		do {
+			count ++;
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != thisEdge);
+
+		if (count > 3) {
+			dgEdge* const edge = TriangulateFace (thisEdge, vertex, stride, heap, NULL);
+			heap.Flush ();
+
+			if (edge) {
+				dgAssert (edge->m_incidentFace > 0);
+
+				if (leftOver) {
+					dgInt32* const index = (dgInt32 *) &heap[0];
+					dgInt64* const data = (dgInt64 *)&index[count];
+					dgInt32 i = 0;
+					dgEdge* ptr = edge;
+					do {
+						index[i] = ptr->m_incidentVertex;
+						data[i] = dgInt64 (ptr->m_userData);
+						i ++;
+						ptr = ptr->m_next;
+					} while (ptr != edge);
+					leftOver->AddFace(i, index, data);
+
+				} else {
+					dgTrace (("Deleting face:"));					
+					ptr = edge;
+					do {
+						dgTrace (("%d ", ptr->m_incidentVertex));
+					} while (ptr != edge);
+					dgTrace (("\n"));					
+				}
+
+				DeleteFace (edge);
+				iter.Begin();
+			}
+		}
+	}
+
+	OptimizeTriangulation (vertex, strideInBytes);
+
+	mark = IncLRU();
+	m_faceSecuence = 1;
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* edge = &(*iter);
+		if (edge->m_mark == mark) {
+			continue;
+		}
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+		dgAssert (edge == edge->m_next->m_next->m_next);
+
+		for (dgInt32 i = 0; i < 3; i ++) { 
+			edge->m_incidentFace = m_faceSecuence; 
+			edge->m_mark = mark;
+			edge = edge->m_next;
+		}
+		m_faceSecuence ++;
+	}
+}
+
+
+static void RemoveColinearVertices (dgPolyhedra& flatFace, const dgFloat64* const vertex, dgInt32 stride)
+{
+	dgEdge* edgePerimeters[1024];
+
+	dgInt32 perimeterCount = 0;
+	dgInt32 mark = flatFace.IncLRU();
+	dgPolyhedra::Iterator iter (flatFace);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_incidentFace < 0) && (edge->m_mark != mark)) {
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+			edgePerimeters[perimeterCount] = edge;
+			perimeterCount ++;
+			dgAssert (perimeterCount < dgInt32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
+		}
+	}
+
+	for (dgInt32 i = 0; i < perimeterCount; i ++) {
+		dgEdge* edge = edgePerimeters[i];
+		dgEdge* ptr = edge;
+		dgVector p0 (&vertex[ptr->m_incidentVertex * stride]);
+		dgVector p1 (&vertex[ptr->m_next->m_incidentVertex * stride]);
+		dgVector e0 (p1 - p0) ;
+		e0 = e0.Scale3 (dgRsqrt (e0 % e0) + dgFloat32 (1.0e-12f));
+		dgInt32 ignoreTest = 1;
+		do {
+			ignoreTest = 0;
+			dgVector p2 (&vertex[ptr->m_next->m_next->m_incidentVertex * stride]);
+			dgVector e1 (p2 - p1);
+			e1 = e1.Scale3 (dgRsqrt (e1 % e1) + dgFloat32 (1.0e-12f));
+			dgFloat32 dot = e1 % e0;
+			if (dot > dgFloat32 (dgFloat32 (0.9999f))) {
+
+				for (dgEdge* interiorEdge = ptr->m_next->m_twin->m_next; interiorEdge != ptr->m_twin; interiorEdge = ptr->m_next->m_twin->m_next) {
+					flatFace.DeleteEdge (interiorEdge);
+				} 
+
+				if (ptr->m_twin->m_next->m_next->m_next == ptr->m_twin) {
+					dgAssert (ptr->m_twin->m_next->m_incidentFace > 0);
+					flatFace.DeleteEdge (ptr->m_twin->m_next);
+				}
+
+				dgAssert (ptr->m_next->m_twin->m_next->m_twin == ptr);
+				edge = ptr->m_next;
+
+				if (!flatFace.FindEdge (ptr->m_incidentVertex, edge->m_twin->m_incidentVertex) && 
+					!flatFace.FindEdge (edge->m_twin->m_incidentVertex, ptr->m_incidentVertex)) {
+						ptr->m_twin->m_prev = edge->m_twin->m_prev;
+						edge->m_twin->m_prev->m_next = ptr->m_twin;
+
+						edge->m_next->m_prev = ptr;
+						ptr->m_next = edge->m_next;
+
+						edge->m_next = edge->m_twin;
+						edge->m_prev = edge->m_twin;
+						edge->m_twin->m_next = edge;
+						edge->m_twin->m_prev = edge;
+						flatFace.DeleteEdge (edge);								
+						flatFace.ChangeEdgeIncidentVertex (ptr->m_twin, ptr->m_next->m_incidentVertex);
+
+						e1 = e0;
+						p1 = p2;
+						edge = ptr;
+						ignoreTest = 1;
+						continue;
+				}
+			}
+
+			e0 = e1;
+			p1 = p2;
+			ptr = ptr->m_next;
+		} while ((ptr != edge) || ignoreTest);
+	}
+}
+
+
+static dgInt32 GetInteriorDiagonals (dgPolyhedra& polyhedra, dgEdge** const diagonals, dgInt32 maxCount)
+{
+	dgInt32 count = 0;
+	dgInt32 mark = polyhedra.IncLRU();
+	dgPolyhedra::Iterator iter (polyhedra);
+	for (iter.Begin(); iter; iter++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark != mark) { 
+			if (edge->m_incidentFace > 0) {
+				if (edge->m_twin->m_incidentFace > 0) {
+					edge->m_twin->m_mark = mark;
+					if (count < maxCount){
+						diagonals[count] = edge;
+						count ++;
+					}
+					dgAssert (count <= maxCount);
+				}
+			}
+		}
+		edge->m_mark = mark;
+	}
+
+	return count;
+}
+
+static bool IsEssensialPointDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const dgFloat64* const pool, dgInt32 stride)
+{
+	dgFloat64 dot;
+	dgBigVector p0 (&pool[diagonal->m_incidentVertex * stride]);
+	dgBigVector p1 (&pool[diagonal->m_twin->m_next->m_twin->m_incidentVertex * stride]);
+	dgBigVector p2 (&pool[diagonal->m_prev->m_incidentVertex * stride]);
+
+	dgBigVector e1 (p1 - p0);
+	dot = e1 % e1;
+	if (dot < dgFloat64 (1.0e-12f)) {
+		return false;
+	}
+	e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt(dot));
+
+	dgBigVector e2 (p2 - p0);
+	dot = e2 % e2;
+	if (dot < dgFloat64 (1.0e-12f)) {
+		return false;
+	}
+	e2 = e2.Scale3 (dgFloat64 (1.0f) / sqrt(dot));
+
+	dgBigVector n1 (e1 * e2); 
+
+	dot = normal % n1;
+	//if (dot > dgFloat64 (dgFloat32 (0.1f)f)) {
+	//if (dot >= dgFloat64 (-1.0e-6f)) {
+	if (dot >= dgFloat64 (0.0f)) {
+		return false;
+	}
+	return true;
+}
+
+
+static bool IsEssensialDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const dgFloat64* const pool,  dgInt32 stride)
+{
+	return IsEssensialPointDiagonal (diagonal, normal, pool, stride) || IsEssensialPointDiagonal (diagonal->m_twin, normal, pool, stride); 
+}
+
+
+void dgPolyhedra::ConvexPartition (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOversOut)
+{
+	if (GetCount()) {
+		Triangulate (vertex, strideInBytes, leftOversOut);
+		DeleteDegenerateFaces (vertex, strideInBytes, dgFloat32 (1.0e-5f));
+		Optimize (vertex, strideInBytes, NULL, NULL, dgFloat32 (1.0e-4f));
+		DeleteDegenerateFaces (vertex, strideInBytes, dgFloat32 (1.0e-5f));
+
+		if (GetCount()) {
+			dgInt32 removeCount = 0;
+			dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+
+			dgInt32 polygon[1024 * 8];
+			dgEdge* diagonalsPool[1024 * 8];
+			dgPolyhedra buildConvex(GetAllocator());
+
+			buildConvex.BeginFace();
+			dgPolyhedra::Iterator iter (*this);
+			for (iter.Begin(); iter; ) {
+				dgEdge* edge = &(*iter);
+				iter++;
+				if (edge->m_incidentFace > 0) {
+
+					dgPolyhedra flatFace(GetAllocator());
+					MarkAdjacentCoplanarFaces (flatFace, edge, vertex, strideInBytes);
+
+					if (flatFace.GetCount()) {
+						flatFace.RefineTriangulation (vertex, stride);
+						RemoveColinearVertices (flatFace, vertex, stride);
+
+						dgInt32 diagonalCount = GetInteriorDiagonals (flatFace, diagonalsPool, sizeof (diagonalsPool) / sizeof (diagonalsPool[0]));
+						if (diagonalCount) {
+							edge = &flatFace.GetRoot()->GetInfo();
+							if (edge->m_incidentFace < 0) {
+								edge = edge->m_twin;
+							}
+							dgAssert (edge->m_incidentFace > 0);
+
+							dgBigVector normal (FaceNormal (edge, vertex, strideInBytes));
+							normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (normal % normal));
+
+							edge = NULL;
+							dgPolyhedra::Iterator iter (flatFace);
+							for (iter.Begin(); iter; iter ++) {
+								edge = &(*iter);
+								if (edge->m_incidentFace < 0) {
+									break;
+								}
+							}
+							dgAssert (edge);
+
+							dgInt32 isConvex = 1;
+							dgEdge* ptr = edge;
+							dgInt32 mark = flatFace.IncLRU();
+
+							dgBigVector normal2 (normal);
+							dgBigVector p0 (&vertex[ptr->m_prev->m_incidentVertex * stride]);
+							dgBigVector p1 (&vertex[ptr->m_incidentVertex * stride]);
+							dgBigVector e0 (p1 - p0);
+							e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt ((e0 % e0) + dgFloat64 (1.0e-24f)));
+							do {
+								dgBigVector p2 (&vertex[ptr->m_next->m_incidentVertex * stride]);
+								dgBigVector e1 (p2 - p1);
+								e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt ((e1 % e1) + dgFloat32 (1.0e-24f)));
+								dgFloat64 dot = (e0 * e1) % normal2;
+								if (dot > dgFloat32 (5.0e-3f)) {
+									isConvex = 0;
+									break;
+								}
+								ptr->m_mark = mark;
+								e0 = e1;
+								p1 = p2;
+								ptr = ptr->m_next;
+							} while (ptr != edge);
+
+							if (isConvex) {
+								dgPolyhedra::Iterator iter (flatFace);
+								for (iter.Begin(); iter; iter ++) {
+									ptr = &(*iter);
+									if (ptr->m_incidentFace < 0) {
+										if (ptr->m_mark < mark) {
+											isConvex = 0;
+											break;
+										}
+									}
+								}
+							}
+
+							if (isConvex) {
+								if (diagonalCount > 2) {
+									dgInt32 count = 0;
+									ptr = edge;
+									do {
+										polygon[count] = ptr->m_incidentVertex;				
+										count ++;
+										dgAssert (count < dgInt32 (sizeof (polygon) / sizeof (polygon[0])));
+										ptr = ptr->m_next;
+									} while (ptr != edge);
+
+									for (dgInt32 i = 0; i < count - 1; i ++) {
+										for (dgInt32 j = i + 1; j < count; j ++) {
+											if (polygon[i] == polygon[j]) {
+												i = count;
+												isConvex = 0;
+												break ;
+											}
+										}
+									}
+								}
+							}
+
+							if (isConvex) {
+								for (dgInt32 j = 0; j < diagonalCount; j ++) {
+									dgEdge* const diagonal = diagonalsPool[j];
+									removeCount ++;
+									flatFace.DeleteEdge (diagonal);
+								}
+							} else {
+								for (dgInt32 j = 0; j < diagonalCount; j ++) {
+									dgEdge* const diagonal = diagonalsPool[j];
+									if (!IsEssensialDiagonal(diagonal, normal, vertex, stride)) {
+										removeCount ++;
+										flatFace.DeleteEdge (diagonal);
+									}
+								}
+							}
+						}
+
+						dgInt32 mark = flatFace.IncLRU();
+						dgPolyhedra::Iterator iter (flatFace);
+						for (iter.Begin(); iter; iter ++) {
+							dgEdge* const edge = &(*iter);
+							if (edge->m_mark != mark) {
+								if (edge->m_incidentFace > 0) {
+									dgEdge* ptr = edge;
+									dgInt32 diagonalCount = 0;
+									do {
+										polygon[diagonalCount] = ptr->m_incidentVertex;				
+										diagonalCount ++;
+										dgAssert (diagonalCount < dgInt32 (sizeof (polygon) / sizeof (polygon[0])));
+										ptr->m_mark = mark;
+										ptr = ptr->m_next;
+									} while (ptr != edge);
+									if (diagonalCount >= 3) {
+ 										buildConvex.AddFace (diagonalCount, polygon);
+									}
+								}
+							}
+						}
+					}
+					iter.Begin();
+				}
+			}
+
+			buildConvex.EndFace();
+			dgAssert (GetCount() == 0);
+			SwapInfo(buildConvex);
+		}
+	}
+}
+
+
+dgObb dgPolyhedra::CalculateSphere (const dgFloat64* const vertex, dgInt32 strideInBytes, const dgMatrix* const basis) const
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));	
+
+	dgInt32 vertexCount = 0;
+	dgInt32 mark = IncLRU();
+	dgPolyhedra::Iterator iter(*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark != mark) {
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != edge);
+			vertexCount ++;
+		}
+	}
+	dgAssert (vertexCount);
+
+	mark = IncLRU();
+	dgInt32 vertexCountIndex = 0;
+	dgStack<dgBigVector> pool (vertexCount);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark != mark) {
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != edge);
+			dgInt32 incidentVertex = edge->m_incidentVertex * stride;
+			pool[vertexCountIndex] = dgBigVector (vertex[incidentVertex + 0], vertex[incidentVertex + 1], vertex[incidentVertex + 2], dgFloat32 (0.0f));
+			vertexCountIndex ++;
+		}
+	}
+	dgAssert (vertexCountIndex <= vertexCount);
+
+	dgMatrix axis (dgGetIdentityMatrix());
+	dgObb sphere (axis);
+	dgConvexHull3d convexHull (GetAllocator(), &pool[0].m_x, sizeof (dgBigVector), vertexCountIndex, 0.0f);
+	if (convexHull.GetCount()) {
+		dgStack<dgInt32> triangleList (convexHull.GetCount() * 3); 				
+		dgInt32 trianglesCount = 0;
+		for (dgConvexHull3d::dgListNode* node = convexHull.GetFirst(); node; node = node->GetNext()) {
+			dgConvexHull3DFace* const face = &node->GetInfo();
+			triangleList[trianglesCount * 3 + 0] = face->m_index[0];
+			triangleList[trianglesCount * 3 + 1] = face->m_index[1];
+			triangleList[trianglesCount * 3 + 2] = face->m_index[2];
+			trianglesCount ++;
+			dgAssert ((trianglesCount * 3) <= triangleList.GetElementsCount());
+		}
+
+		dgVector* const dst = (dgVector*) &pool[0].m_x;
+		for (dgInt32 i = 0; i < convexHull.GetVertexCount(); i ++) {
+			dst[i] = convexHull.GetVertex(i);
+		}
+		sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL);
+
+	} else if (vertexCountIndex >= 3) {
+		dgStack<dgInt32> triangleList (GetCount() * 3 * 2); 
+		dgInt32 mark = IncLRU();
+		dgInt32 trianglesCount = 0;
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			if (edge->m_mark != mark) {
+				dgEdge* ptr = edge;
+				do {
+					ptr->m_mark = mark;
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != edge);
+
+				ptr = edge->m_next->m_next;
+				do {
+					triangleList[trianglesCount * 3 + 0] = edge->m_incidentVertex;
+					triangleList[trianglesCount * 3 + 1] = ptr->m_prev->m_incidentVertex;
+					triangleList[trianglesCount * 3 + 2] = ptr->m_incidentVertex;
+					trianglesCount ++;
+					dgAssert ((trianglesCount * 3) <= triangleList.GetElementsCount());
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != edge);
+
+				dgVector* const dst = (dgVector*) &pool[0].m_x;
+				for (dgInt32 i = 0; i < vertexCountIndex; i ++) {
+					dst[i] = pool[i];
+				}
+				sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL);
+			}
+		}
+	}
+	return sphere;
+}
+
+dgBigPlane dgPolyhedra::EdgePlane (dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const pool) const
+{
+	const dgBigVector& p0 = pool[i0];
+	const dgBigVector& p1 = pool[i1];
+	const dgBigVector& p2 = pool[i2];
+
+	dgBigPlane plane (p0, p1, p2);
+	dgFloat64 mag = sqrt (plane % plane);
+	if (mag < dgFloat64 (1.0e-12f)) {
+		mag = dgFloat64 (1.0e-12f);
+	}
+	mag = dgFloat64 (1.0f) / mag;
+
+	plane.m_x *= mag;
+	plane.m_y *= mag;
+	plane.m_z *= mag;
+	plane.m_w *= mag;
+
+	return plane;
+}
+
+
+void dgPolyhedra::CalculateVertexMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool, dgEdge* const edge) const
+{
+	dgInt32 i0 = edge->m_incidentVertex;
+
+	table[i0].Clear ();
+	dgEdge* ptr = edge;
+	do {
+
+		if (ptr->m_incidentFace > 0) {
+			dgInt32 i1 = ptr->m_next->m_incidentVertex;
+			dgInt32 i2 = ptr->m_prev->m_incidentVertex;
+			dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool));
+			table[i0].Accumulate (constrainPlane);
+
+		} else {
+			dgInt32 i1 = ptr->m_twin->m_incidentVertex;
+			dgInt32 i2 = ptr->m_twin->m_prev->m_incidentVertex;
+			dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool));
+			table[i0].Accumulate (constrainPlane);
+
+			i1 = ptr->m_prev->m_incidentVertex;
+			i2 = ptr->m_prev->m_twin->m_prev->m_incidentVertex;
+			constrainPlane = UnboundedLoopPlane (i0, i1, i2, pool);
+			table[i0].Accumulate (constrainPlane);
+		}
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != edge);
+}
+
+
+void dgPolyhedra::CalculateAllMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool) const
+{
+	dgInt32 edgeMark = IncLRU();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+
+		dgAssert (edge);
+		if (edge->m_mark != edgeMark) {
+
+			if (edge->m_incidentFace > 0) {
+				dgInt32 i0 = edge->m_incidentVertex;
+				dgInt32 i1 = edge->m_next->m_incidentVertex;
+				dgInt32 i2 = edge->m_prev->m_incidentVertex;
+
+				dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool));
+				dgVertexCollapseVertexMetric tmp (constrainPlane);
+
+				dgEdge* ptr = edge;
+				do {
+					ptr->m_mark = edgeMark;
+					i0 = ptr->m_incidentVertex;
+					table[i0].Accumulate(tmp);
+
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+
+			} else {
+				dgAssert (edge->m_twin->m_incidentFace > 0);
+				dgInt32 i0 = edge->m_twin->m_incidentVertex;
+				dgInt32 i1 = edge->m_twin->m_next->m_incidentVertex;
+				dgInt32 i2 = edge->m_twin->m_prev->m_incidentVertex;
+
+				edge->m_mark = edgeMark;
+				dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool));
+				dgVertexCollapseVertexMetric tmp (constrainPlane);
+
+				i0 = edge->m_incidentVertex;
+				table[i0].Accumulate(tmp);
+
+				i0 = edge->m_twin->m_incidentVertex;
+				table[i0].Accumulate(tmp);
+			}
+		}
+	}
+}
+
+
+
+bool dgPolyhedra::IsOkToCollapse (const dgBigVector* const pool, dgEdge* const edge) const
+{
+	const dgBigVector& q = pool[edge->m_incidentVertex];
+	const dgBigVector& p = pool[edge->m_twin->m_incidentVertex];
+	for (dgEdge* triangle = edge->m_prev->m_twin; triangle != edge->m_twin->m_next; triangle = triangle->m_prev->m_twin) {
+		if (triangle->m_incidentFace > 0) {
+			dgAssert ((edge->m_incidentFace < 0) || (edge->m_incidentVertex == edge->m_next->m_next->m_next->m_incidentVertex));
+
+			dgBigVector originalArea ((pool[triangle->m_next->m_incidentVertex] - q) * (pool[triangle->m_prev->m_incidentVertex] - q));
+			dgBigVector newArea ((pool[triangle->m_next->m_incidentVertex] - p) * (pool[triangle->m_prev->m_incidentVertex] - p));
+
+			dgFloat64 projectedArea = newArea % originalArea;
+			if (projectedArea <= dgFloat64 (0.0f)) {
+				return false;
+			}
+
+			dgFloat64 mag20 = newArea % newArea;
+			dgFloat64 mag21 = originalArea % originalArea;
+			if ((projectedArea * projectedArea)  < (mag20 * mag21 * dgFloat64 (1.0e-10f)))  {
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+
+dgEdge* dgPolyhedra::OptimizeCollapseEdge (dgEdge* const edge)
+{
+	dgInt32 v0 = edge->m_incidentVertex;
+	dgInt32 v1 = edge->m_twin->m_incidentVertex;
+
+#ifdef _DEBUG
+	dgPolyhedra::dgPairKey TwinKey (v1, v0);
+	dgPolyhedra::dgTreeNode* const node = Find (TwinKey.GetVal());
+	dgEdge* const twin1 = node ? &node->GetInfo() : NULL;
+	dgAssert (twin1);
+	dgAssert (edge->m_twin == twin1);
+	dgAssert (twin1->m_twin == edge);
+	dgAssert (edge->m_incidentFace != 0);
+	dgAssert (twin1->m_incidentFace != 0);
+	dgAssert ((edge->m_incidentFace < 0) || (edge->m_incidentVertex == edge->m_next->m_next->m_next->m_incidentVertex));
+	dgAssert ((edge->m_twin->m_incidentFace < 0) || (edge->m_twin->m_incidentVertex == edge->m_twin->m_next->m_next->m_next->m_incidentVertex));
+#endif
+
+	dgEdge* retEdge = edge->m_twin->m_prev->m_twin;
+	if (retEdge	== edge->m_twin->m_next) {
+		return NULL;
+	}
+	if (retEdge	== edge->m_twin) {
+		return NULL;
+	}
+	if (retEdge	== edge->m_next) {
+		retEdge = edge->m_prev->m_twin;
+		if (retEdge	== edge->m_twin->m_next) {
+			return NULL;
+		}
+		if (retEdge	== edge->m_twin) {
+			return NULL;
+		}
+	}
+
+	dgEdge* lastEdge = NULL;
+	dgEdge* firstEdge = NULL;
+	if ((edge->m_incidentFace >= 0)	&& (edge->m_twin->m_incidentFace >= 0)) {	
+		lastEdge = edge->m_prev->m_twin;
+		firstEdge = edge->m_twin->m_next->m_twin->m_next;
+	} else if (edge->m_twin->m_incidentFace >= 0) {
+		firstEdge = edge->m_twin->m_next->m_twin->m_next;
+		lastEdge = edge;
+	} else {
+		lastEdge = edge->m_prev->m_twin;
+		firstEdge = edge->m_twin->m_next;
+	}
+
+	for (dgEdge* ptr = firstEdge; ptr != lastEdge; ptr = ptr->m_twin->m_next) {
+		dgEdge* badEdge = FindEdge (edge->m_twin->m_incidentVertex, ptr->m_twin->m_incidentVertex);
+		if (badEdge) {
+			return NULL;
+		}
+	} 
+
+	dgEdge* const twin = edge->m_twin;
+	if (twin->m_next == twin->m_prev->m_prev) {
+		twin->m_prev->m_twin->m_twin = twin->m_next->m_twin;
+		twin->m_next->m_twin->m_twin = twin->m_prev->m_twin;
+
+		RemoveHalfEdge (twin->m_prev);
+		RemoveHalfEdge (twin->m_next);
+	} else {
+		twin->m_next->m_prev = twin->m_prev;
+		twin->m_prev->m_next = twin->m_next;
+	}
+
+	if (edge->m_next == edge->m_prev->m_prev) {
+		edge->m_next->m_twin->m_twin = edge->m_prev->m_twin;
+		edge->m_prev->m_twin->m_twin = edge->m_next->m_twin;
+		RemoveHalfEdge (edge->m_next);
+		RemoveHalfEdge (edge->m_prev);
+	} else {
+		edge->m_next->m_prev = edge->m_prev;
+		edge->m_prev->m_next = edge->m_next;
+	}
+
+	dgAssert (twin->m_twin->m_incidentVertex == v0);
+	dgAssert (edge->m_twin->m_incidentVertex == v1);
+	RemoveHalfEdge (twin);
+	RemoveHalfEdge (edge);
+
+	dgEdge* remapPtr = retEdge;
+	do {
+		dgPolyhedra::dgPairKey pairKey (v0, remapPtr->m_twin->m_incidentVertex);
+		dgPolyhedra::dgTreeNode* const pairEdgeNode = Find (pairKey.GetVal());
+		if (pairEdgeNode) {
+			if (&pairEdgeNode->GetInfo() == remapPtr) {
+				dgPolyhedra::dgPairKey key (v1, remapPtr->m_twin->m_incidentVertex);
+				remapPtr->m_incidentVertex = v1;
+				ReplaceKey (pairEdgeNode, key.GetVal());
+			} 
+		}
+
+		dgPolyhedra::dgPairKey TwinKey (remapPtr->m_twin->m_incidentVertex, v0);
+		dgPolyhedra::dgTreeNode* const pairTwinNode = Find (TwinKey.GetVal());
+		if (pairTwinNode) {
+			if (&pairTwinNode->GetInfo() == remapPtr->m_twin) {
+				dgPolyhedra::dgPairKey key (remapPtr->m_twin->m_incidentVertex, v1);
+				ReplaceKey (pairTwinNode, key.GetVal());
+			}
+		}
+
+		remapPtr = remapPtr->m_twin->m_next;
+	} while (remapPtr != retEdge);
+
+	return retEdge;
+}
+
+#if 0
+dgFloat64 dgPolyhedra::EdgePenalty (const dgBigVector* const pool, dgEdge* const edge, dgFloat64 dist___) const
+{
+	dgInt32 i0 = edge->m_incidentVertex;
+	dgInt32 i1 = edge->m_next->m_incidentVertex;
+
+	const dgBigVector& p0 = pool[i0];
+	const dgBigVector& p1 = pool[i1];
+	dgBigVector dp (p1 - p0);
+
+	dgFloat64 dot = dp % dp;
+	if (dot < dgFloat64(1.0e-6f)) {
+		return dgFloat64 (-1.0f);
+	}
+
+	if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) {
+		dgBigVector edgeNormal (FaceNormal (edge, &pool[0].m_x, sizeof (dgBigVector)));
+		dgBigVector twinNormal (FaceNormal (edge->m_twin, &pool[0].m_x, sizeof (dgBigVector)));
+
+		dgFloat64 mag0 = edgeNormal % edgeNormal;
+		dgFloat64 mag1 = twinNormal % twinNormal;
+		if ((mag0 < dgFloat64 (1.0e-24f)) || (mag1 < dgFloat64 (1.0e-24f))) {
+			return dgFloat64 (-1.0f);
+		}
+
+		edgeNormal = edgeNormal.Scale3 (dgFloat64 (1.0f) / sqrt(mag0));
+		twinNormal = twinNormal.Scale3 (dgFloat64 (1.0f) / sqrt(mag1));
+
+		dot = edgeNormal % twinNormal;
+		if (dot < dgFloat64 (-0.9f)) {
+			return dgFloat32 (-1.0f);
+		}
+
+		dgEdge* ptr = edge;
+		do {
+			if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
+				dgEdge* const adj = edge->m_twin;
+				ptr = edge;
+				do {
+					if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
+						return dgFloat32 (-1.0);
+					}
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != adj);
+			}
+			ptr = ptr->m_twin->m_next;
+		} while (ptr != edge);
+	}
+
+	dgInt32 faceA = edge->m_incidentFace;
+	dgInt32 faceB = edge->m_twin->m_incidentFace;
+
+	i0 = edge->m_twin->m_incidentVertex;
+	dgBigVector p (pool[i0].m_x, pool[i0].m_y, pool[i0].m_z, dgFloat32 (0.0f));
+
+	bool penalty = false;
+	dgEdge* ptr = edge;
+	do {
+		dgEdge* const adj = ptr->m_twin;
+
+		dgInt32 face = adj->m_incidentFace;
+		if ((face != faceB) && (face != faceA) && (face >= 0) && (adj->m_next->m_incidentFace == face) && (adj->m_prev->m_incidentFace == face)){
+
+			dgInt32 i0 = adj->m_next->m_incidentVertex;
+			const dgBigVector& p0 = pool[i0];
+
+			dgInt32 i1 = adj->m_incidentVertex;
+			const dgBigVector& p1 = pool[i1];
+
+			dgInt32 i2 = adj->m_prev->m_incidentVertex;
+			const dgBigVector& p2 = pool[i2];
+
+			dgBigVector n0 ((p1 - p0) * (p2 - p0));
+			dgBigVector n1 ((p1 - p) * (p2 - p));
+			dgFloat64 dot = n0 % n1;
+			if (dot < dgFloat64 (0.0f)) {
+				penalty = true;
+				break;
+			}
+		}
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != edge);
+
+	dgFloat64 aspect = dgFloat32 (-1.0f);
+	if (!penalty) {
+		dgInt32 i0 = edge->m_twin->m_incidentVertex;
+		dgBigVector p0 (pool[i0]);
+
+		aspect = dgFloat32 (1.0f);
+		for (dgEdge* ptr = edge->m_twin->m_next->m_twin->m_next; ptr != edge; ptr = ptr->m_twin->m_next) {
+			if (ptr->m_incidentFace > 0) {
+				dgInt32 i0 = ptr->m_next->m_incidentVertex;
+				const dgBigVector& p1 = pool[i0];
+
+				dgInt32 i1 = ptr->m_prev->m_incidentVertex;
+				const dgBigVector& p2 = pool[i1];
+
+				dgBigVector e0 (p1 - p0);
+				dgBigVector e1 (p2 - p1);
+				dgBigVector e2 (p0 - p2);
+
+				dgFloat64 mag0 = e0 % e0;
+				dgFloat64 mag1 = e1 % e1;
+				dgFloat64 mag2 = e2 % e2;
+				dgFloat64 maxMag = dgMax (mag0, mag1, mag2);
+				dgFloat64 minMag = dgMin (mag0, mag1, mag2);
+				dgFloat64 ratio = minMag / maxMag;
+
+				if (ratio < aspect) {
+					aspect = ratio;
+				}
+			}
+		}
+		aspect = sqrt (aspect);
+		//aspect = 1.0f;
+	}
+
+	return aspect;
+}
+
+void dgPolyhedra::Optimize (const dgFloat64* const array, dgInt32 strideInBytes, dgFloat64 tol, dgInt32 maxFaceCount___)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+
+	dgInt32 edgeCount = GetEdgeCount() * 4 + 1024 * 16;
+	dgInt32 maxVertexIndex = GetLastVertexIndex();
+
+	dgStack<dgBigVector> vertexPool (maxVertexIndex); 
+	dgStack<dgVertexCollapseVertexMetric> vertexMetrics (maxVertexIndex + 512); 
+
+	dgList <dgEdgeCollapseEdgeHandle> edgeHandleList(GetAllocator());
+	dgStack<char> heapPool (2 * edgeCount * dgInt32 (sizeof (dgFloat64) + sizeof (dgEdgeCollapseEdgeHandle*) + sizeof (dgInt32))); 
+	dgDownHeap<dgList <dgEdgeCollapseEdgeHandle>::dgListNode* , dgFloat64> bigHeapArray(&heapPool[0], heapPool.GetSizeInBytes());
+
+	for (dgInt32 i = 0; i < maxVertexIndex; i ++) {
+		vertexPool[i].m_x = array[i * stride + 0];
+		vertexPool[i].m_y = array[i * stride + 1];
+		vertexPool[i].m_z = array[i * stride + 2];
+		vertexPool[i].m_w= dgFloat64 (0.0f);
+	}
+
+	memset (&vertexMetrics[0], 0, maxVertexIndex * sizeof (dgVertexCollapseVertexMetric));
+	CalculateAllMetrics (&vertexMetrics[0], &vertexPool[0]);
+
+	dgFloat64 tol2 = tol * tol;
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+
+		edge->m_userData = 0;
+		dgInt32 index0 = edge->m_incidentVertex;
+		dgInt32 index1 = edge->m_twin->m_incidentVertex;
+
+		dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+		const dgBigVector& p = vertexPool[index1];
+		dgFloat64 cost = metric.Evalue (p); 
+		if (cost < tol2) {
+			cost = EdgePenalty (&vertexPool[0], edge, 0);
+
+			if (cost > dgFloat64 (0.0f)) {
+				dgEdgeCollapseEdgeHandle handle (edge);
+				dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+				bigHeapArray.Push (handleNodePtr, cost);
+			}
+		}
+	}
+
+	while (bigHeapArray.GetCount()) {
+		dgList <dgEdgeCollapseEdgeHandle>::dgListNode* const handleNodePtr = bigHeapArray[0];
+
+		dgEdge* edge = handleNodePtr->GetInfo().m_edge;
+		bigHeapArray.Pop();
+		edgeHandleList.Remove (handleNodePtr);
+
+		if (edge) {
+			CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge);
+
+			dgInt32 index0 = edge->m_incidentVertex;
+			dgInt32 index1 = edge->m_twin->m_incidentVertex;
+			dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+			const dgBigVector& p = vertexPool[index1];
+
+			if ((metric.Evalue (p) < tol2) && IsOkToCollapse (&vertexPool[0], edge) && (EdgePenalty (&vertexPool[0], edge, 0)  > dgFloat64 (0.0f))) {
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+				dgAssert (SanityCheck ());
+#endif
+
+				edge = OptimizeCollapseEdge(edge);
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+				dgAssert (SanityCheck ());
+#endif
+				if (edge) {
+					// Update vertex metrics
+					CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge);
+
+					// Update metrics for all surrounding vertex
+					dgEdge* ptr = edge;
+					do {
+						CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], ptr->m_twin);
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+
+					// calculate edge cost of all incident edges
+					dgInt32 mark = IncLRU();
+					ptr = edge;
+					do {
+						dgAssert (ptr->m_mark != mark);
+						ptr->m_mark = mark;
+
+						index0 = ptr->m_incidentVertex;
+						index1 = ptr->m_twin->m_incidentVertex;
+
+						dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+						const dgBigVector& p = vertexPool[index1];
+
+						dgFloat64 cost = dgFloat32 (-1.0f);
+						if (metric.Evalue (p) < tol2) {
+							cost = EdgePenalty (&vertexPool[0], ptr, 0);
+						}
+
+						if (cost  > dgFloat64 (0.0f)) {
+							dgEdgeCollapseEdgeHandle handle (ptr);
+							dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+							bigHeapArray.Push (handleNodePtr, cost);
+						} else {
+							dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle*)IntToPointer (ptr->m_userData);
+							if (handle) {
+								handle->m_edge = NULL;
+							}
+							ptr->m_userData = dgUnsigned32 (NULL);
+
+						}
+
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+
+
+					// calculate edge cost of all incident edges to a surrounding vertex
+					ptr = edge;
+					do {
+						dgEdge* const incidentEdge = ptr->m_twin;		
+
+						dgEdge* ptr1 = incidentEdge;
+						do {
+							index0 = ptr1->m_incidentVertex;
+							index1 = ptr1->m_twin->m_incidentVertex;
+
+							if (ptr1->m_mark != mark) {
+								ptr1->m_mark = mark;
+								dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+								const dgBigVector& p = vertexPool[index1];
+
+								dgFloat64 cost = dgFloat32 (-1.0f);
+								if (metric.Evalue (p) < tol2) {
+									cost = EdgePenalty (&vertexPool[0], ptr1, 0);
+								}
+
+								if (cost  > dgFloat64 (0.0f)) {
+									dgAssert (cost > dgFloat64(0.0f));
+									dgEdgeCollapseEdgeHandle handle (ptr1);
+									dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+									bigHeapArray.Push (handleNodePtr, cost);
+								} else {
+									dgEdgeCollapseEdgeHandle *handle;
+									handle = (dgEdgeCollapseEdgeHandle*)IntToPointer (ptr1->m_userData);
+									if (handle) {
+										handle->m_edge = NULL;
+									}
+									ptr1->m_userData = dgUnsigned32 (NULL);
+
+								}
+							}
+
+							if (ptr1->m_twin->m_mark != mark) {
+								ptr1->m_twin->m_mark = mark;
+								dgVertexCollapseVertexMetric &metric = vertexMetrics[index1];
+								const dgBigVector& p = vertexPool[index0];
+
+								dgFloat64 cost = dgFloat32 (-1.0f);
+								if (metric.Evalue (p) < tol2) {
+									cost = EdgePenalty (&vertexPool[0], ptr1->m_twin, 0);
+								}
+
+								if (cost  > dgFloat64 (0.0f)) {
+									dgAssert (cost > dgFloat64(0.0f));
+									dgEdgeCollapseEdgeHandle handle (ptr1->m_twin);
+									dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+									bigHeapArray.Push (handleNodePtr, cost);
+								} else {
+									dgEdgeCollapseEdgeHandle *handle;
+									handle = (dgEdgeCollapseEdgeHandle*) IntToPointer (ptr1->m_twin->m_userData);
+									if (handle) {
+										handle->m_edge = NULL;
+									}
+									ptr1->m_twin->m_userData = dgUnsigned32 (NULL);
+
+								}
+							}
+
+							ptr1 = ptr1->m_twin->m_next;
+						} while (ptr1 != incidentEdge);
+
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+				}
+			}
+		}
+	}
+}
+
+#else
+
+
+
+dgFloat64 dgPolyhedra::EdgePenalty (const dgBigVector* const pool, dgEdge* const edge, dgFloat64 dist) const
+{
+	dgInt32 i0 = edge->m_incidentVertex;
+	dgInt32 i1 = edge->m_next->m_incidentVertex;
+
+	dgFloat32 maxPenalty = dgFloat32 (1.0e14f);
+
+	const dgBigVector& p0 = pool[i0];
+	const dgBigVector& p1 = pool[i1];
+	dgBigVector dp (p1 - p0);
+
+	dgFloat64 dot = dp % dp;
+	if (dot < dgFloat64(1.0e-6f)) {
+		return dist * maxPenalty;
+	}
+
+	if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) {
+		dgBigVector edgeNormal (FaceNormal (edge, &pool[0].m_x, sizeof (dgBigVector)));
+		dgBigVector twinNormal (FaceNormal (edge->m_twin, &pool[0].m_x, sizeof (dgBigVector)));
+
+		dgFloat64 mag0 = edgeNormal % edgeNormal;
+		dgFloat64 mag1 = twinNormal % twinNormal;
+		if ((mag0 < dgFloat64 (1.0e-24f)) || (mag1 < dgFloat64 (1.0e-24f))) {
+			return dist * maxPenalty;
+		}
+
+		edgeNormal = edgeNormal.Scale3 (dgFloat64 (1.0f) / sqrt(mag0));
+		twinNormal = twinNormal.Scale3 (dgFloat64 (1.0f) / sqrt(mag1));
+
+		dot = edgeNormal % twinNormal;
+		if (dot < dgFloat64 (-0.9f)) {
+			return dist * maxPenalty;
+		}
+
+		dgEdge* ptr = edge;
+		do {
+			if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
+				dgEdge* const adj = edge->m_twin;
+				ptr = edge;
+				do {
+					if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
+						return dist * maxPenalty;
+					}
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != adj);
+			}
+			ptr = ptr->m_twin->m_next;
+		} while (ptr != edge);
+	}
+
+	dgInt32 faceA = edge->m_incidentFace;
+	dgInt32 faceB = edge->m_twin->m_incidentFace;
+
+	i0 = edge->m_twin->m_incidentVertex;
+	dgBigVector p (pool[i0].m_x, pool[i0].m_y, pool[i0].m_z, dgFloat32 (0.0f));
+
+	bool penalty = false;
+	dgEdge* ptr = edge;
+	do {
+		dgEdge* const adj = ptr->m_twin;
+
+		dgInt32 face = adj->m_incidentFace;
+		if ((face != faceB) && (face != faceA) && (face >= 0) && (adj->m_next->m_incidentFace == face) && (adj->m_prev->m_incidentFace == face)){
+
+			dgInt32 i0 = adj->m_next->m_incidentVertex;
+			const dgBigVector& p0 = pool[i0];
+
+			dgInt32 i1 = adj->m_incidentVertex;
+			const dgBigVector& p1 = pool[i1];
+
+			dgInt32 i2 = adj->m_prev->m_incidentVertex;
+			const dgBigVector& p2 = pool[i2];
+
+			dgBigVector n0 ((p1 - p0) * (p2 - p0));
+			dgBigVector n1 ((p1 - p) * (p2 - p));
+			dgFloat64 dot = n0 % n1;
+			if (dot < dgFloat64 (0.0f)) {
+				penalty = true;
+				break;
+			}
+		}
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != edge);
+
+	dgFloat64 aspect = dgFloat32 (0.0f);
+	if (!penalty) {
+		dgInt32 i0 = edge->m_twin->m_incidentVertex;
+		dgBigVector p0 (pool[i0]);
+
+		aspect = dgFloat32 (1.0f);
+		for (dgEdge* ptr = edge->m_twin->m_next->m_twin->m_next; ptr != edge; ptr = ptr->m_twin->m_next) {
+			if (ptr->m_incidentFace > 0) {
+				dgInt32 i0 = ptr->m_next->m_incidentVertex;
+				const dgBigVector& p1 = pool[i0];
+
+				dgInt32 i1 = ptr->m_prev->m_incidentVertex;
+				const dgBigVector& p2 = pool[i1];
+
+				dgBigVector e0 (p1 - p0);
+				dgBigVector e1 (p2 - p1);
+				dgBigVector e2 (p0 - p2);
+
+				dgFloat64 mag0 = e0 % e0;
+				dgFloat64 mag1 = e1 % e1;
+				dgFloat64 mag2 = e2 % e2;
+				dgFloat64 maxMag = dgMax (mag0, mag1, mag2);
+				dgFloat64 minMag = dgMin (mag0, mag1, mag2);
+				dgFloat64 ratio = minMag / maxMag;
+
+				if (ratio < aspect) {
+					aspect = ratio;
+				}
+			}
+		}
+		aspect = dgFloat32 (1.0f) - aspect;
+	}
+	return aspect * aspect * dist;
+}
+
+
+bool dgPolyhedra::Optimize (const dgFloat64* const array, dgInt32 strideInBytes, dgReportProgress normalizedProgress, void* const reportProgressUserData, dgFloat64 tol, dgInt32 maxFaceCount)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+	dgAssert (SanityCheck ());
+#endif
+
+	dgFloat32 progressDen = dgFloat32 (1.0f / GetEdgeCount());
+	dgInt32 edgeCount = GetEdgeCount() * 4 + 1024 * 16;
+	dgInt32 maxVertexIndex = GetLastVertexIndex();
+	
+	dgStack<dgBigVector> vertexPool (maxVertexIndex); 
+	dgStack<dgVertexCollapseVertexMetric> vertexMetrics (maxVertexIndex + 512); 
+
+	dgList <dgEdgeCollapseEdgeHandle> edgeHandleList(GetAllocator());
+	dgStack<char> heapPool (2 * edgeCount * dgInt32 (sizeof (dgFloat64) + sizeof (dgEdgeCollapseEdgeHandle*) + sizeof (dgInt32))); 
+	dgUpHeap<dgList <dgEdgeCollapseEdgeHandle>::dgListNode* , dgFloat64> bigHeapArray(&heapPool[0], heapPool.GetSizeInBytes());
+
+	for (dgInt32 i = 0; i < maxVertexIndex; i ++) {
+		vertexPool[i].m_x = array[i * stride + 0];
+		vertexPool[i].m_y = array[i * stride + 1];
+		vertexPool[i].m_z = array[i * stride + 2];
+		vertexPool[i].m_w= dgFloat64 (0.0f);
+	}
+
+	memset (&vertexMetrics[0], 0, maxVertexIndex * sizeof (dgVertexCollapseVertexMetric));
+	CalculateAllMetrics (&vertexMetrics[0], &vertexPool[0]);
+
+	const dgFloat64 maxCost = dgFloat32 (1.0e-3f);
+	dgFloat64 tol2 = tol * tol;
+	dgFloat64 distTol = dgMax (tol2, dgFloat64 (1.0e-12f));
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+
+		edge->m_userData = 0;
+		dgInt32 index0 = edge->m_incidentVertex;
+		dgInt32 index1 = edge->m_twin->m_incidentVertex;
+
+		dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+		const dgBigVector& p = vertexPool[index1];
+		dgFloat64 faceCost = metric.Evalue (p); 
+		dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], edge, distTol);
+		dgAssert (edgePenalty >= dgFloat32 (0.0f));
+		dgEdgeCollapseEdgeHandle handle (edge);
+		dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+		bigHeapArray.Push (handleNodePtr, faceCost + edgePenalty);
+	}
+
+	bool progress = true;
+	dgInt32 interPasses = 0;
+	dgInt32 faceCount = GetFaceCount();
+	while (bigHeapArray.GetCount() && (bigHeapArray.Value() < maxCost) && ((bigHeapArray.Value() < tol2) || (faceCount > maxFaceCount)) && progress ) {
+		dgList <dgEdgeCollapseEdgeHandle>::dgListNode* const handleNodePtr = bigHeapArray[0];
+
+		dgEdge* edge = handleNodePtr->GetInfo().m_edge;
+		bigHeapArray.Pop();
+		edgeHandleList.Remove (handleNodePtr);
+
+		if (edge) {
+			if (IsOkToCollapse (&vertexPool[0], edge)) {
+
+				interPasses ++;
+				faceCount -= 2;
+				if (interPasses >= 400) {
+					interPasses = 0;
+					faceCount = GetFaceCount();
+					progress = normalizedProgress ? normalizedProgress(dgFloat32 (1.0f) - GetEdgeCount() * progressDen, reportProgressUserData) : true;
+				}
+
+				if (bigHeapArray.GetCount() > (bigHeapArray.GetMaxCount() - 100)) {
+					for(dgInt32 i = bigHeapArray.GetCount() - 1; i >= 0; i --) {
+						dgList <dgEdgeCollapseEdgeHandle>::dgListNode* const emptyHandle = bigHeapArray[i];
+						if (!emptyHandle->GetInfo().m_edge) {
+							bigHeapArray.Remove(i);
+							edgeHandleList.Remove (emptyHandle);
+						}
+					}
+				}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+				dgAssert (SanityCheck ());
+#endif
+
+				edge = OptimizeCollapseEdge(edge);
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+				dgAssert (SanityCheck ());
+#endif
+				if (edge) {
+					// Update vertex metrics
+					CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge);
+
+					// Update metrics for all surrounding vertex
+					dgEdge* ptr = edge;
+					do {
+						CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], ptr->m_twin);
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+
+					// calculate edge cost of all incident edges
+					dgInt32 mark = IncLRU();
+					ptr = edge;
+					do {
+						dgAssert (ptr->m_mark != mark);
+						ptr->m_mark = mark;
+
+						dgInt32 index0 = ptr->m_incidentVertex;
+						dgInt32 index1 = ptr->m_twin->m_incidentVertex;
+
+						dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+						const dgBigVector& p = vertexPool[index1];
+
+						dgFloat64 faceCost = metric.Evalue (p); 
+						dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], ptr, distTol);
+						dgAssert (edgePenalty >= dgFloat32 (0.0f));
+						dgEdgeCollapseEdgeHandle handle (ptr);
+						dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+						bigHeapArray.Push (handleNodePtr, faceCost + edgePenalty);
+
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+
+
+					// calculate edge cost of all incident edges to a surrounding vertex
+					ptr = edge;
+					do {
+						dgEdge* const incidentEdge = ptr->m_twin;		
+
+						dgEdge* ptr1 = incidentEdge;
+						do {
+							dgInt32 index0 = ptr1->m_incidentVertex;
+							dgInt32 index1 = ptr1->m_twin->m_incidentVertex;
+
+							if (ptr1->m_mark != mark) {
+								ptr1->m_mark = mark;
+								dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+								const dgBigVector& p = vertexPool[index1];
+
+								dgFloat64 faceCost = metric.Evalue (p); 
+								dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], ptr1, distTol);
+								dgAssert (edgePenalty >= dgFloat32 (0.0f));
+								dgEdgeCollapseEdgeHandle handle (ptr1);
+								dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+								bigHeapArray.Push (handleNodePtr, faceCost + edgePenalty);
+							}
+
+							if (ptr1->m_twin->m_mark != mark) {
+								ptr1->m_twin->m_mark = mark;
+								dgVertexCollapseVertexMetric &metric = vertexMetrics[index1];
+								const dgBigVector& p = vertexPool[index0];
+								dgFloat64 faceCost = metric.Evalue (p); 
+								dgFloat64 edgePenalty = EdgePenalty (&vertexPool[0], ptr1->m_twin, distTol);
+								dgAssert (edgePenalty >= dgFloat32 (0.0f));
+								dgEdgeCollapseEdgeHandle handle (ptr1->m_twin);
+								dgList <dgEdgeCollapseEdgeHandle>::dgListNode* handleNodePtr = edgeHandleList.Addtop (handle);
+								bigHeapArray.Push (handleNodePtr, faceCost + edgePenalty);
+							}
+
+							ptr1 = ptr1->m_twin->m_next;
+						} while (ptr1 != incidentEdge);
+
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+				}
+			}
+		}
+	}
+
+	if (normalizedProgress && progress) {
+		progress = normalizedProgress(dgFloat32 (1.0f), reportProgressUserData);
+	}
+	return progress;
+}
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgPolyhedra.h

@@ -0,0 +1,300 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgPolyhedra__
+#define __dgPolyhedra__
+
+#include "dgStdafx.h"
+#include "dgList.h"
+#include "dgTree.h"
+#include "dgHeap.h"
+#include "dgDebug.h"
+
+
+
+
+class dgEdge;
+class dgPlane;
+class dgObb;
+class dgMatrix;
+class dgPolyhedra;
+class dgVertexCollapseVertexMetric;
+
+typedef dgInt64 dgEdgeKey;
+
+
+DG_MSC_VECTOR_ALIGMENT
+class dgEdge
+{
+	public:
+	dgEdge ();
+	dgEdge (dgInt32 vertex, dgInt32 face, dgUnsigned64 userdata = 0);
+	~dgEdge ();
+
+	dgInt32 m_incidentVertex;
+	dgInt32 m_incidentFace;
+	dgUnsigned64 m_userData;
+	dgEdge* m_next;
+	dgEdge* m_prev;
+	dgEdge* m_twin;
+	dgInt32 m_mark;
+} DG_GCC_VECTOR_ALIGMENT;
+
+
+class dgPolyhedra: public dgTree <dgEdge, dgEdgeKey>
+{
+	public:
+	class dgPairKey
+	{
+		public:
+		dgPairKey ();
+		dgPairKey (dgInt64 val);
+		dgPairKey (dgInt32 v0, dgInt32 v1);
+		dgInt64 GetVal () const; 
+		dgInt32 GetLowKey () const;
+		dgInt32 GetHighKey () const;
+
+		private:
+		dgUnsigned64 m_key;
+	};
+
+	dgPolyhedra (dgMemoryAllocator* const allocator);
+	dgPolyhedra (const dgPolyhedra &polyhedra);
+	virtual ~dgPolyhedra();
+
+	virtual void BeginFace();
+	dgEdge* AddFace (dgInt32 v0, dgInt32 v1, dgInt32 v2);
+	dgEdge* AddFace (dgInt32 count, const dgInt32* const index);
+	dgEdge* AddFace (dgInt32 count, const dgInt32* const index, const dgInt64* const userdata);
+	virtual void EndFace ();
+	virtual void DeleteFace(dgEdge* const edge);
+
+	dgInt32 GetFaceCount() const;
+	dgInt32 GetEdgeCount() const;
+	dgInt32 GetLastVertexIndex() const;
+
+	dgInt32 IncLRU() const;
+	void SetLRU(dgInt32 lru) const;
+
+	dgEdge* FindEdge (dgInt32 v0, dgInt32 v1) const;
+	dgTreeNode* FindEdgeNode (dgInt32 v0, dgInt32 v1) const;
+
+	dgEdge* AddHalfEdge (dgInt32 v0, dgInt32 v1);
+	void DeleteEdge (dgEdge* const edge);
+	void DeleteEdge (dgInt32 v0, dgInt32 v1);
+
+	dgEdge* ConnectVertex (dgEdge* const e0, dgEdge* const e1);
+	
+	bool FlipEdge (dgEdge* const edge);
+	dgEdge* SpliteEdge (dgInt32 newIndex, dgEdge* const edge);
+	dgBigVector FaceNormal (const dgEdge* const face, const dgFloat64* const vertex, dgInt32 strideInBytes) const;
+
+	void BeginConectedSurface() const;
+	bool GetConectedSurface (dgPolyhedra &polyhedra) const;
+	void EndConectedSurface() const;
+
+	dgObb CalculateSphere (const dgFloat64* const vertex, dgInt32 strideInBytes, const dgMatrix* const basis = NULL) const;
+	void ChangeEdgeIncidentVertex (dgEdge* const edge, dgInt32 newIndex);	
+	void DeleteDegenerateFaces (const dgFloat64* const pool, dgInt32 dstStrideInBytes, dgFloat64 minArea);
+
+	bool Optimize (const dgFloat64* const pool, dgInt32 strideInBytes, dgReportProgress normalizedProgress, void* const reportProgressUserData, dgFloat64 tol, dgInt32 maxFaceCount = 1<<28);
+	void Triangulate (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOversOut);
+	void ConvexPartition (const dgFloat64* const vertex, dgInt32 strideInBytes, dgPolyhedra* const leftOversOut);
+	dgEdge* CollapseEdge(dgEdge* const edge);
+	void PolygonizeFace (dgEdge* face, const dgFloat64* const pool, dgInt32 stride);
+
+	private:
+	void RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride);
+	void RefineTriangulation (const dgFloat64* const vertex, dgInt32 stride, dgBigVector* const normal, dgInt32 perimeterCount, dgEdge** const perimeter);
+	void OptimizeTriangulation (const dgFloat64* const vertex, dgInt32 strideInBytes);
+	void MarkAdjacentCoplanarFaces (dgPolyhedra& polyhedraOut, dgEdge* const face, const dgFloat64* const pool, dgInt32 strideInBytes);
+	dgEdge* FindEarTip (dgEdge* const face, const dgFloat64* const pool, dgInt32 stride, dgDownHeap<dgEdge*, dgFloat64>& heap, const dgBigVector &normal) const;
+	dgEdge* TriangulateFace (dgEdge* const face, const dgFloat64* const pool, dgInt32 stride, dgDownHeap<dgEdge*, dgFloat64>& heap, dgBigVector* const faceNormalOut);
+	
+
+	void RemoveHalfEdge (dgEdge* const edge);
+	dgEdge* OptimizeCollapseEdge (dgEdge* const edge);
+	bool IsOkToCollapse (const dgBigVector* const pool, dgEdge* const edge) const;
+	dgFloat64 EdgePenalty (const dgBigVector* const pool, dgEdge* const edge, dgFloat64 dist) const;
+	dgBigPlane EdgePlane (dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const pool) const;
+	void CalculateAllMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool) const;
+	void CalculateVertexMetrics (dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool, dgEdge* const edge) const;
+	
+
+	mutable dgInt32 m_baseMark;
+	mutable dgInt32 m_edgeMark;
+	mutable dgInt32 m_faceSecuence;
+
+	friend class dgPolyhedraDescriptor;
+	
+};
+
+
+
+DG_INLINE dgEdge::dgEdge ()
+{
+}
+
+DG_INLINE dgEdge::dgEdge (dgInt32 vertex, dgInt32 face, dgUnsigned64 userdata)
+	:m_incidentVertex(vertex)
+	,m_incidentFace(face)
+	,m_userData(userdata)
+	,m_next(NULL)
+	,m_prev(NULL)
+	,m_twin(NULL)
+	,m_mark(0)
+{
+}
+
+DG_INLINE dgEdge::~dgEdge ()
+{
+}
+
+DG_INLINE dgPolyhedra::dgPairKey::dgPairKey ()
+{
+}
+
+DG_INLINE dgPolyhedra::dgPairKey::dgPairKey (dgInt64 val)
+	:m_key(dgUnsigned64 (val))
+{
+}
+
+DG_INLINE dgPolyhedra::dgPairKey::dgPairKey (dgInt32 v0, dgInt32 v1)
+	:m_key (dgUnsigned64 ((dgInt64 (v0) << 32) | v1))
+{
+}
+
+DG_INLINE dgInt64 dgPolyhedra::dgPairKey::GetVal () const 
+{
+	return dgInt64 (m_key);
+}
+
+DG_INLINE dgInt32 dgPolyhedra::dgPairKey::GetLowKey () const 
+{
+	return dgInt32 (m_key>>32);
+}
+
+DG_INLINE dgInt32 dgPolyhedra::dgPairKey::GetHighKey () const 
+{
+	return dgInt32 (m_key & 0xffffffff);
+}
+
+DG_INLINE void dgPolyhedra::BeginFace ()
+{
+}
+
+DG_INLINE dgEdge* dgPolyhedra::AddFace (dgInt32 count, const dgInt32* const index) 
+{
+	return AddFace (count, index, NULL);
+}
+
+DG_INLINE dgEdge* dgPolyhedra::AddFace (dgInt32 v0, dgInt32 v1, dgInt32 v2)
+{
+	dgInt32 vertex[3];
+
+	vertex [0] = v0;
+	vertex [1] = v1;
+	vertex [2] = v2;
+	return AddFace (3, vertex, NULL);
+}
+
+DG_INLINE dgInt32 dgPolyhedra::GetEdgeCount() const
+{
+#ifdef _DEBUG
+	dgInt32 edgeCount = 0;
+	Iterator iter(*this);
+	for (iter.Begin(); iter; iter ++) {
+		edgeCount ++;
+	}
+	dgAssert (edgeCount == GetCount());;
+#endif
+	return GetCount();
+}
+
+DG_INLINE dgInt32 dgPolyhedra::GetLastVertexIndex() const
+{
+	dgInt32 maxVertexIndex = -1;
+	Iterator iter(*this);
+	for (iter.Begin(); iter; iter ++) {
+		const dgEdge* const edge = &(*iter);
+		if (edge->m_incidentVertex > maxVertexIndex) {
+			maxVertexIndex = edge->m_incidentVertex;
+		}
+	}
+	return maxVertexIndex + 1;
+}
+
+
+DG_INLINE dgInt32 dgPolyhedra::IncLRU() const
+{	
+	m_edgeMark ++;
+	dgAssert (m_edgeMark < 0x7fffffff);
+	return m_edgeMark;
+}
+
+DG_INLINE void dgPolyhedra::SetLRU(dgInt32 lru) const
+{
+	if (lru > m_edgeMark) {
+		m_edgeMark = lru;
+	}
+}
+
+DG_INLINE void dgPolyhedra::BeginConectedSurface() const
+{
+	m_baseMark = IncLRU();
+}
+
+DG_INLINE void dgPolyhedra::EndConectedSurface() const
+{
+}
+
+
+DG_INLINE dgPolyhedra::dgTreeNode* dgPolyhedra::FindEdgeNode (dgInt32 i0, dgInt32 i1) const
+{
+	dgPairKey key (i0, i1);
+	return Find (key.GetVal());
+}
+
+
+DG_INLINE dgEdge *dgPolyhedra::FindEdge (dgInt32 i0, dgInt32 i1) const
+{
+	//	dgTreeNode *node;
+	//	dgPairKey key (i0, i1);
+	//	node = Find (key.GetVal());
+	//	return node ? &node->GetInfo() : NULL;
+	dgTreeNode* const node = FindEdgeNode (i0, i1);
+	return node ? &node->GetInfo() : NULL;
+}
+
+DG_INLINE void dgPolyhedra::DeleteEdge (dgInt32 v0, dgInt32 v1)
+{
+	dgPairKey pairKey (v0, v1);
+	dgTreeNode* const node = Find(pairKey.GetVal());
+	dgEdge* const edge = node ? &node->GetInfo() : NULL;
+	if (!edge) {
+		return;
+	}
+	DeleteEdge (edge);
+}
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgPolyhedraMassProperties.cpp

@@ -0,0 +1,570 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgVector.h"
+#include "dgPolyhedraMassProperties.h"
+
+
+#if 0
+class dgPolyhedraMassProperties
+{
+#define X 0
+#define Y 1
+#define Z 2
+#define SQR(x) ((x)*(x))
+#define CUBE(x) ((x)*(x)*(x))
+
+
+public:
+	dgPolyhedraMassProperties()
+	{
+		memset (this, 0, sizeof (dgPolyhedraMassProperties));
+	}
+
+	void ProjectionIntegrals(
+		dgInt32 indexCount, 
+		const dgVector* faceVertex)
+	{
+		dgInt32 i0;
+		dgInt32 i1;
+		dgFloat32 a0, a1, da;
+		dgFloat32 b0, b1, db;
+
+		dgFloat32 C1;
+		dgFloat32 Ca;
+		dgFloat32 Cb;
+		dgFloat32 Caa;
+		dgFloat32 Cbb;
+		dgFloat32 Cab;
+		dgFloat32 Kab;
+		dgFloat32 a0_2;
+		dgFloat32 a0_3;
+		dgFloat32 a1_2;
+		dgFloat32 b0_2;
+		dgFloat32 b0_3;
+
+		m_P1 = dgFloat32 (0.0f);
+		m_Pa = dgFloat32 (0.0f);
+		m_Pb = dgFloat32 (0.0f);
+		m_Paa = dgFloat32 (0.0f);
+		m_Pbb = dgFloat32 (0.0f);
+		m_Pab = dgFloat32 (0.0f);
+
+		i0 = indexCount - 1;
+		for (i1 = 0; i1 < indexCount; i1 ++) {
+			a0 = faceVertex[i0][m_A];
+			b0 = faceVertex[i0][m_B];
+
+			a1 = faceVertex[i1][m_A];
+			b1 = faceVertex[i1][m_B];
+			i0 = i1;			
+
+			da = a1 - a0;
+			db = b1 - b0;
+
+			a0_2 = a0 * a0; 
+			a0_3 = a0_2 * a0; 
+
+
+			b0_2 = b0 * b0; 
+			b0_3 = b0_2 * b0; 
+
+			a1_2 = a1 * a1; 
+
+			C1 = a1 + a0;
+			Ca = a1 * C1 + a0_2; 
+			Caa = a1*Ca + a0_3; 
+
+			Cb = b1 * (b1 + b0) + b0_2; 
+			Cbb = b1*Cb + b0_3; 
+
+			Cab = dgFloat32 (3.0f) * a1_2 + dgFloat32 (2.0f) * a1 * a0 + a0_2; 
+			Kab = a1_2 + dgFloat32 (2.0f) * a1 * a0 + dgFloat32 (3.0f) * a0_2;
+
+			m_P1 += db * C1;
+			m_Pa += db * Ca;
+			m_Paa += db * Caa;
+
+			m_Pb += da * Cb;
+			m_Pbb += da * Cbb;
+			m_Pab += db * (b1 * Cab + b0 * Kab);
+		} 
+
+		m_P1 *= dgFloat32 (0.5f);
+		m_Pa *= dgFloat32 ( 1.0f / 6.0f);
+		m_Pb *= dgFloat32 (-1.0f / 6.0f);
+		m_Paa *= dgFloat32 (1.0f / 12.0f);
+		m_Pbb *= dgFloat32 (-1.0f / 12.0f);
+		m_Pab *= dgFloat32 ( 1.0f / 24.0f);
+	}
+
+
+	void FaceIntegrals (
+		dgInt32 count, 
+		const dgPlane& plane, 
+		const dgVector* faceVertex)
+	{
+
+		dgFloat32 k1, k2, k3, k4;
+
+		ProjectionIntegrals (count, faceVertex);
+
+		k1 = dgFloat32 (1.0f) / plane[m_C]; 
+		k2 = k1 * k1; 
+		k3 = k2 * k1; 
+		k4 = k3 * k1;
+
+		m_Fa = k1 * m_Pa;
+		m_Fb = k1 * m_Pb;
+		m_Fc = -k2 * (plane[m_A] * m_Pa + plane[m_B] * m_Pb + plane[3] * m_P1);
+
+		m_Faa = k1 * m_Paa;
+		m_Fbb = k1 * m_Pbb;
+		m_Fcc = k3 * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab + 
+			SQR(plane[m_B]) * m_Pbb + plane[3] * (dgFloat32 (2.0f) *(plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));
+	}
+
+
+	void VolumeIntegrals(
+		dgInt32 indexCount, 
+		const dgPlane& plane, 
+		const dgVector* faceVertex)
+	{
+		dgFloat32 mag2;
+
+		mag2 = plane % plane;
+		if (mag2 > dgFloat32 (1.0e-8f)) {
+			if ((dgAbsf (plane.m_x) > dgAbsf (plane.m_y)) && (dgAbsf (plane.m_x) > dgAbsf (plane.m_z))) {
+				m_C = X;
+			} else {
+				m_C = (dgAbsf (plane.m_y) > dgAbsf (plane.m_z)) ? Y : Z;
+			}
+			m_A = (m_C + 1) % 3;
+			m_B = (m_A + 1) % 3;
+			FaceIntegrals (indexCount, plane, faceVertex);
+
+			m_T0 += plane[X] * ((m_A == X) ? m_Fa : ((m_B == X) ? m_Fb : m_Fc));
+
+			m_T1[m_A] += plane[m_A] * m_Faa;
+			m_T1[m_B] += plane[m_B] * m_Fbb;
+			m_T1[m_C] += plane[m_C] * m_Fcc;
+			//dgTrace (("(%f %f %f) (%f %f %f) (%f %f %f)\n", m_T1[m_A], m_T1[m_B], m_T1[m_C], plane[m_A], plane[m_B], plane[m_C], m_Faa, m_Fbb, m_Fcc))
+		}
+	}
+
+
+	void AddInertia (int indexCount, const dgFloat32* faceVertex)
+	{
+		dgInt32 i0;
+		dgInt32 i1;
+		dgFloat32 a0, a1, da;
+		dgFloat32 b0, b1, db;
+		dgFloat32 C1;
+		dgFloat32 Ca;
+		dgFloat32 Cb;
+		dgFloat32 Caa;
+		dgFloat32 Cbb;
+		dgFloat32 Cab;
+		dgFloat32 Kab;
+		dgFloat32 Caaa;
+		dgFloat32 Cbbb;
+		dgFloat32 Cabb;
+		dgFloat32 Caab;
+		dgFloat32 Kabb;
+		dgFloat32 Kaab;
+		dgFloat32 a0_2;
+		dgFloat32 a0_3;
+		dgFloat32 a0_4;
+		dgFloat32 a1_2;
+		dgFloat32 a1_3;
+		dgFloat32 b0_2;
+		dgFloat32 b0_3;
+		dgFloat32 b0_4;
+		dgFloat32 b1_2;
+		dgFloat32 b1_3;
+		dgFloat32 mag2;
+
+		dgVector p0 (&faceVertex[0]);
+		dgVector p1 (&faceVertex[3]);
+		dgVector p2 (&faceVertex[6]);
+		dgPlane plane (p0, p1, p2);
+
+		mag2 = plane % plane;
+		if (mag2 > dgFloat32 (1.0e-8f)) {
+			plane = plane.Scale (dgRsqrt ((plane % plane)));
+			if ((dgAbsf (plane.m_x) > dgAbsf (plane.m_y)) && (dgAbsf (plane.m_x) > dgAbsf (plane.m_z))) {
+				m_C = X;
+			} else {
+				m_C = (dgAbsf (plane.m_y) > dgAbsf (plane.m_z)) ? Y : Z;
+			}
+			m_A = (m_C + 1) % 3;
+			m_B = (m_A + 1) % 3;
+
+			//	FaceIntegrals (indexCount, plane, faceVertex);
+			dgFloat32 k1, k2, k3, k4;
+			{
+				{
+					//ProjectionIntegrals (count, faceVertex);
+					m_P1 = dgFloat32 (0.0f);
+					m_Pa = dgFloat32 (0.0f);
+					m_Pb = dgFloat32 (0.0f);
+					m_Paa = dgFloat32 (0.0f);
+					m_Pbb = dgFloat32 (0.0f);
+					m_Pab = dgFloat32 (0.0f);
+
+					m_Paaa = dgFloat32 (0.0f);
+					m_Pbbb = dgFloat32 (0.0f);
+					m_Paab = dgFloat32 (0.0f);
+					m_Pabb = dgFloat32 (0.0f);
+
+					i0 = indexCount - 1;
+					for (i1 = 0; i1 < indexCount; i1 ++) {
+						a0 = faceVertex[i0 * 3 + m_A];
+						b0 = faceVertex[i0 * 3 + m_B];
+
+						a1 = faceVertex[i1 * 3 + m_A];
+						b1 = faceVertex[i1 * 3 + m_B];
+
+						i0 = i1;			
+
+						da = a1 - a0;
+						db = b1 - b0;
+
+						a0_2 = a0 * a0; 
+						a0_3 = a0_2 * a0; 
+						a0_4 = a0_3 * a0;
+
+						b0_2 = b0 * b0; 
+						b0_3 = b0_2 * b0; 
+						b0_4 = b0_3 * b0;
+
+						a1_2 = a1 * a1; 
+						a1_3 = a1_2 * a1; 
+
+						b1_2 = b1 * b1; 
+						b1_3 = b1_2 * b1;
+
+						C1 = a1 + a0;
+
+						Ca = a1 * C1 + a0_2; 
+						Caa = a1 * Ca + a0_3; 
+						Caaa = a1 * Caa + a0_4;
+
+						Cb = b1 * (b1 + b0) + b0_2; 
+						Cbb = b1 * Cb + b0_3;
+						Cbbb = b1 * Cbb + b0_4;
+
+						Cab = dgFloat32 (3.0f) * a1_2 + dgFloat32 (2.0f) * a1 * a0 + a0_2; 
+						Kab = a1_2 + dgFloat32 (2.0f) * a1 * a0 + dgFloat32 (3.0f) * a0_2;
+
+						Caab = a0 * Cab + dgFloat32 (4.0f) * a1_3; 
+						Kaab = a1 * Kab + dgFloat32 (4.0f) * a0_3;
+						Cabb = dgFloat32 (4.0f) * b1_3 + dgFloat32 (3.0f) * b1_2 * b0 + dgFloat32 (2.0f) * b1 * b0_2 + b0_3;
+						Kabb = b1_3 + dgFloat32 (2.0f) * b1_2 * b0 + dgFloat32 (3.0f) * b1 * b0_2 + dgFloat32 (4.0f) * b0_3;
+
+						m_P1 += (db * C1);
+						m_Pa += (db * Ca);
+						m_Paa += (db * Caa);
+
+						m_Pb += (da * Cb);
+						m_Pbb += (da * Cbb);
+						m_Pab += (db * (b1 * Cab + b0 * Kab));
+
+						m_Paaa += (db * Caaa);
+						m_Pbbb += (da * Cbbb);
+						m_Paab += (db * (b1 * Caab + b0 * Kaab));
+						m_Pabb += (da * (a1 * Cabb + a0 * Kabb));
+					} 
+
+					m_P1 *= dgFloat32 (0.5f);
+					m_Pa *= dgFloat32 ( 1.0f / 6.0f);
+					m_Pb *= dgFloat32 (-1.0f / 6.0f);
+					m_Paa *= dgFloat32 (1.0f / 12.0f);
+					m_Pbb *= dgFloat32 (-1.0f / 12.0f);
+					m_Pab *= dgFloat32 ( 1.0f / 24.0f);
+
+					m_Paaa *= dgFloat32 (1.0f / 20.0);
+					m_Pbbb *= dgFloat32 (-1.0f / 20.0);
+					m_Paab *= dgFloat32 (1.0f / 60.0);
+					m_Pabb *= dgFloat32 (-1.0f / 60.0);
+				}
+
+				k1 = dgFloat32 (1.0f) / plane[m_C]; 
+				k2 = k1 * k1; 
+				k3 = k2 * k1; 
+				k4 = k3 * k1;
+
+				m_Fa = k1 * m_Pa;
+				m_Fb = k1 * m_Pb;
+				m_Fc = -k2 * (plane[m_A] * m_Pa + plane[m_B] * m_Pb + plane[3] * m_P1);
+
+				m_Faa = k1 * m_Paa;
+				m_Fbb = k1 * m_Pbb;
+				m_Fcc = k3 * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab + 
+					SQR(plane[m_B]) * m_Pbb + plane[3] * (dgFloat32 (2.0f) *(plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));
+
+				m_Faaa = k1 * m_Paaa;
+				m_Fbbb = k1 * m_Pbbb;
+				m_Fccc = -k4 * (CUBE(plane[m_A]) * m_Paaa + 
+					dgFloat32(3.0f) * SQR(plane[m_A]) * plane[m_B] * m_Paab +
+					dgFloat32(3.0f) * plane[m_A] * SQR(plane[m_B]) * m_Pabb + CUBE(plane[m_B]) * m_Pbbb +
+					dgFloat32(3.0f) * plane[3]  * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab + SQR(plane[m_B]) * m_Pbb) +
+					SQR(plane[3]) * (dgFloat32(3.0f) * (plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));
+
+				m_Faab = k1 * m_Paab;
+				m_Fbbc = -k2 * (plane[m_A] * m_Pabb + plane[m_B] * m_Pbbb + plane[3] * m_Pbb);
+				m_Fcca = k3 * (SQR(plane[m_A]) * m_Paaa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Paab + SQR(plane[m_B]) * m_Pabb +
+					plane[3] * (dgFloat32 (2.0f) * (plane[m_A] * m_Paa + plane[m_B] * m_Pab) + plane[3] * m_Pa));
+			}
+
+			m_T0 += (plane[X] * ((m_A == X) ? m_Fa : ((m_B == X) ? m_Fb : m_Fc)));
+
+			m_T1[m_A] += (plane[m_A] * m_Faa);
+			m_T1[m_B] += (plane[m_B] * m_Fbb);
+			m_T1[m_C] += (plane[m_C] * m_Fcc);
+
+			m_T2[m_A] += (plane[m_A] * m_Faaa);
+			m_T2[m_B] += (plane[m_B] * m_Fbbb);
+			m_T2[m_C] += (plane[m_C] * m_Fccc);
+
+			m_TP[m_A] += (plane[m_A] * m_Faab);
+			m_TP[m_B] += (plane[m_B] * m_Fbbc);
+			m_TP[m_C] += (plane[m_C] * m_Fcca);
+		}
+	}
+
+
+	dgInt32 m_A;   // alpha 
+	dgInt32 m_B;   // beta 
+	dgInt32 m_C;   // gamma 
+
+	dgFloat32 m_T0;
+	dgFloat32 m_T1[3];
+	dgFloat32 m_T2[3];
+	dgFloat32 m_TP[3];
+
+	dgFloat32 m_P1;
+	dgFloat32 m_Pa;
+	dgFloat32 m_Pb;
+
+	dgFloat32 m_Paa;
+	dgFloat32 m_Pbb;
+	dgFloat32 m_Pab;
+
+	dgFloat32 m_Paaa;
+	dgFloat32 m_Pbbb;
+	dgFloat32 m_Paab;
+	dgFloat32 m_Pabb;
+
+	dgFloat32 m_Fa;
+	dgFloat32 m_Fb;
+	dgFloat32 m_Fc;
+
+	dgFloat32 m_Faa;
+	dgFloat32 m_Fbb;
+	dgFloat32 m_Fcc;
+
+	dgFloat32 m_Faaa;
+	dgFloat32 m_Fbbb;
+	dgFloat32 m_Fccc;
+
+	dgFloat32 m_Faab;
+	dgFloat32 m_Fbbc;
+	dgFloat32 m_Fcca;
+};
+#endif
+
+
+
+dgPolyhedraMassProperties::dgPolyhedraMassProperties()
+{
+	memset (this, 0, sizeof (dgPolyhedraMassProperties));
+	mult[0] = dgFloat32 (1.0f/6.0f); 
+	mult[1] = dgFloat32 (1.0f/24.0f);
+	mult[2] = dgFloat32 (1.0f/24.0f);
+	mult[3] = dgFloat32 (1.0f/24.0f);
+	mult[4] = dgFloat32 (1.0f/60.0f);
+	mult[5] = dgFloat32 (1.0f/60.0f);
+	mult[6] = dgFloat32 (1.0f/60.0f);
+	mult[7] = dgFloat32 (1.0f/120.0f);
+	mult[8] = dgFloat32 (1.0f/120.0f);
+	mult[9] = dgFloat32 (1.0f/120.0f);
+}
+
+void dgPolyhedraMassProperties::AddCGFace (dgInt32 indexCount, const dgVector* const faceVertex)
+{
+	#define CDSubexpressions(w0,w1,w2,f1,f2) \
+	{					\
+		dgFloat32 temp0 = w0 + w1; \
+		f1 = temp0 + w2; \
+		f2 = w0 * w0 + w1 * temp0 + w2 * f1; \
+	}					
+
+	const dgVector& p0 = faceVertex[0];
+	dgVector p1 (faceVertex[1]);
+
+	for (dgInt32 i = 2; i < indexCount; i++) {
+		const dgVector& p2 = faceVertex[i];
+
+		dgVector e01 (p1 - p0);
+		dgVector e02 (p2 - p0);
+		dgVector d (e01 * e02);
+
+		dgVector f1;
+		dgVector f2;
+		CDSubexpressions (p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x);
+		CDSubexpressions (p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y);
+		CDSubexpressions (p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z);
+
+		// update integrals
+		intg[0] += d[0] * f1.m_x;
+
+		intg[1] += d[0] * f2.m_x; 
+		intg[2] += d[1] * f2.m_y; 
+		intg[3] += d[2] * f2.m_z;
+
+		p1 = p2;
+	}
+}
+
+void dgPolyhedraMassProperties::AddInertiaFace (dgInt32 indexCount, const dgFloat32* const faceVertex)
+{
+	#define InertiaSubexpression(w0,w1,w2,f1,f2,f3) \
+	{					 \
+		dgFloat32 temp0 = w0 + w1; \
+		dgFloat32 temp1 = w0 * w0; \
+		dgFloat32 temp2 = temp1 + w1 * temp0; \
+		f1 = temp0 + w2; \
+		f2 = temp2 + w2 * f1;  \
+		f3 = w0 * temp1 + w1 * temp2 + w2 * f2; \
+	}
+
+	dgVector p0 (faceVertex[0], faceVertex[1], faceVertex[2], 0.0f);
+	dgVector p1 (faceVertex[3], faceVertex[4], faceVertex[5], 0.0f);
+
+	for (dgInt32 i = 2; i < indexCount; i++) {
+		dgVector p2 (faceVertex[i * 3], faceVertex[i * 3 + 1], faceVertex[i * 3 + 2], 0.0f);
+
+		dgVector e01 (p1 - p0);
+		dgVector e02 (p2 - p0);
+		dgVector d (e01 * e02);
+
+		dgVector f1;
+		dgVector f2;
+		dgVector f3;
+		InertiaSubexpression (p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x, f3.m_x);
+		InertiaSubexpression (p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y, f3.m_y);
+		InertiaSubexpression (p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z, f3.m_z);
+
+		// update integrals
+		intg[0] += d[0] * f1.m_x;
+
+		intg[1] += d[0] * f2.m_x; 
+		intg[2] += d[1] * f2.m_y; 
+		intg[3] += d[2] * f2.m_z;
+
+		intg[4] += d[0] * f3.m_x; 
+		intg[5] += d[1] * f3.m_y; 
+		intg[6] += d[2] * f3.m_z;
+
+		p1 = p2;
+	}
+}
+
+
+void dgPolyhedraMassProperties::AddInertiaAndCrossFace (dgInt32 indexCount, const dgFloat32* const faceVertex)
+{
+	#define Subexpressions(w0,w1,w2,f1,f2,f3,g0,g1,g2) \
+	{												   \
+		dgFloat32 temp0 = w0 + w1; \
+		dgFloat32 temp1 = w0 * w0; \
+		dgFloat32 temp2 = temp1 + w1 * temp0; \
+		f1 = temp0 + w2; \
+		f2 = temp2 + w2 * f1;  \
+		f3 = w0 * temp1 + w1 * temp2 + w2 * f2; \
+		g0 = f2 + w0 * (f1 + w0); \
+		g1 = f2 + w1 * (f1 + w1); \
+		g2 = f2 + w2 * (f1 + w2); \
+	}
+
+	dgVector p0 (&faceVertex[0]);
+	dgVector p1 (&faceVertex[3]);
+	for (dgInt32 i = 2; i < indexCount; i++) {
+		dgVector p2 (&faceVertex[i * 3]);
+
+		dgVector e01 (p1 - p0);
+		dgVector e02 (p2 - p0);
+		dgVector d (e01 * e02);
+
+		dgVector f1;
+		dgVector f2;
+		dgVector f3;
+		dgVector g0;
+		dgVector g1;
+		dgVector g2;
+		Subexpressions (p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x, f3.m_x, g0.m_x, g1.m_x, g2.m_x);
+		Subexpressions (p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y, f3.m_y, g0.m_y, g1.m_y, g2.m_y);
+		Subexpressions (p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z, f3.m_z, g0.m_z, g1.m_z, g2.m_z);
+
+		// update integrals
+		intg[0] += d[0] * f1.m_x;
+
+		intg[1] += d[0] * f2.m_x; 
+		intg[2] += d[1] * f2.m_y; 
+		intg[3] += d[2] * f2.m_z;
+
+		intg[4] += d[0] * f3.m_x; 
+		intg[5] += d[1] * f3.m_y; 
+		intg[6] += d[2] * f3.m_z;
+
+		intg[7] += d[0] * (p0.m_y * g0.m_x + p1.m_y * g1.m_x + p2.m_y * g2.m_x);
+		intg[8] += d[1] * (p0.m_z * g0.m_y + p1.m_z * g1.m_y + p2.m_z * g2.m_y);
+		intg[9] += d[2] * (p0.m_x * g0.m_z + p1.m_x * g1.m_z + p2.m_x * g2.m_z);
+
+		p1 = p2;
+	}
+}
+
+
+dgFloat32 dgPolyhedraMassProperties::MassProperties (dgVector& cg, dgVector& inertia, dgVector& crossInertia)
+{
+	for (dgInt32 i = 0; i < 10; i++) {
+		intg[i] *= mult[i];
+	}
+
+	cg.m_x = intg[1];
+	cg.m_y = intg[2];
+	cg.m_z = intg[3];
+	cg.m_w = dgFloat32 (0.0f);
+	inertia.m_x = intg[5] + intg[6];
+	inertia.m_y = intg[4] + intg[6];
+	inertia.m_z = intg[4] + intg[5];
+	inertia.m_w = dgFloat32 (0.0f);
+	crossInertia.m_x = -intg[8];
+	crossInertia.m_y = -intg[9];
+	crossInertia.m_z = -intg[7];
+	crossInertia.m_w = dgFloat32 (0.0f);
+	return intg[0];
+}
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgPolyhedraMassProperties.h

@@ -0,0 +1,41 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgPolyhedraMassProperties__
+#define __dgPolyhedraMassProperties__
+
+class dgPolyhedraMassProperties
+{
+	public:
+	dgPolyhedraMassProperties();
+
+	void AddCGFace (dgInt32 indexCount, const dgVector* const faceVertex);
+	void AddInertiaFace (dgInt32 indexCount, const dgFloat32* const faceVertex);
+	void AddInertiaAndCrossFace (dgInt32 indexCount, const dgFloat32* const faceVertex);
+	
+	dgFloat32 MassProperties (dgVector& cg, dgVector& inertia, dgVector& crossInertia);
+
+	private:
+	dgFloat32 intg[10];
+	dgFloat32 mult[10];
+};
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgQuaternion.cpp

@@ -0,0 +1,186 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgVector.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+
+dgQuaternion::dgQuaternion (const dgMatrix& matrix)
+{
+	enum QUAT_INDEX
+	{
+		X_INDEX=0,
+		Y_INDEX=1,
+		Z_INDEX=2
+	};
+	static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
+
+	dgFloat32 trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
+	if (trace > dgFloat32(0.0f)) {
+		trace = dgSqrt (trace + dgFloat32(1.0f));
+		m_q0 = dgFloat32 (0.5f) * trace;
+		trace = dgFloat32 (0.5f) / trace;
+		m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
+		m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
+		m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
+
+	} else {
+		QUAT_INDEX i = X_INDEX;
+		if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
+			i = Y_INDEX;
+		}
+		if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
+			i = Z_INDEX;
+		}
+		QUAT_INDEX j = QIndex [i];
+		QUAT_INDEX k = QIndex [j];
+
+		trace = dgFloat32(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
+		trace = dgSqrt (trace);
+
+		dgFloat32* const ptr = &m_q1;
+		ptr[i] = dgFloat32 (0.5f) * trace;
+		trace  = dgFloat32 (0.5f) / trace;
+		m_q0   = (matrix[j][k] - matrix[k][j]) * trace;
+		ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
+		ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
+	}
+
+#ifdef _DEBUG
+	dgMatrix tmp (*this, matrix.m_posit);
+	dgMatrix unitMatrix (tmp * matrix.Inverse());
+	for (dgInt32 i = 0; i < 4; i ++) {
+		dgFloat32 err = dgAbsf (unitMatrix[i][i] - dgFloat32(1.0f));
+		dgAssert (err < dgFloat32 (1.0e-2f));
+	}
+
+	dgFloat32 err = dgAbsf (DotProduct(*this) - dgFloat32(1.0f));
+	dgAssert (err < dgFloat32(dgEPSILON * 100.0f));
+#endif
+}
+
+
+dgQuaternion::dgQuaternion (const dgVector &unitAxis, dgFloat32 angle)
+{
+	angle *= dgFloat32 (0.5f);
+	m_q0 = dgCos (angle);
+	dgFloat32 sinAng = dgSin (angle);
+
+#ifdef _DEBUG
+	if (dgAbsf (angle) > dgFloat32(dgEPSILON / 10.0f)) {
+		dgAssert (dgAbsf (dgFloat32(1.0f) - unitAxis % unitAxis) < dgFloat32(dgEPSILON * 10.0f));
+	} 
+#endif
+	m_q1 = unitAxis.m_x * sinAng;
+	m_q2 = unitAxis.m_y * sinAng;
+	m_q3 = unitAxis.m_z * sinAng;
+
+}
+
+
+dgVector dgQuaternion::CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const
+{
+	dgQuaternion q0 (*this);
+	if (q0.DotProduct (q1) < 0.0f) {
+		q0.Scale(-1.0f);
+	}
+	dgQuaternion dq (q0.Inverse() * q1);
+	dgVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3, dgFloat32 (0.0f));
+
+	dgFloat32 dirMag2 = omegaDir % omegaDir;
+	if (dirMag2	< dgFloat32(dgFloat32 (1.0e-5f) * dgFloat32 (1.0e-5f))) {
+		return dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
+	}
+
+	dgFloat32 dirMagInv = dgRsqrt (dirMag2);
+	dgFloat32 dirMag = dirMag2 * dirMagInv;
+
+	dgFloat32 omegaMag = dgFloat32(2.0f) * dgAtan2 (dirMag, dq.m_q0) * invdt;
+	return omegaDir.Scale3 (dirMagInv * omegaMag);
+}
+
+
+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;
+*/
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgQuaternion.h

@@ -0,0 +1,124 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgQuaternion__
+#define __dgQuaternion__
+
+#include "dgStdafx.h"
+
+class dgVector;
+class dgMatrix;
+
+DG_MSC_VECTOR_ALIGMENT
+class dgQuaternion
+{
+	public:
+	dgQuaternion (); 
+	dgQuaternion (const dgMatrix& matrix);
+	dgQuaternion (dgFloat32 q0, dgFloat32 q1, dgFloat32 q2, dgFloat32 q3); 
+	dgQuaternion (const dgVector &unit_Axis, dgFloat32 angle = dgFloat32 (0.0f));
+	
+	void Scale (dgFloat32 scale); 
+	void Normalize (); 
+	inline dgFloat32 DotProduct (const dgQuaternion &QB) const;
+	dgQuaternion Inverse () const; 
+
+	dgQuaternion Slerp (const dgQuaternion &q1, dgFloat32 t) const;
+	dgVector CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const;
+
+	dgQuaternion operator* (const dgQuaternion &B) const;
+	dgQuaternion operator+ (const dgQuaternion &B) const; 
+	dgQuaternion operator- (const dgQuaternion &B) const; 
+
+	dgFloat32 m_q0;
+	dgFloat32 m_q1;
+	dgFloat32 m_q2;
+	dgFloat32 m_q3;
+}DG_GCC_VECTOR_ALIGMENT;
+
+
+
+
+inline dgQuaternion::dgQuaternion () 
+{
+	m_q0 = dgFloat32 (1.0f);
+	m_q1 = dgFloat32 (0.0f);
+	m_q2 = dgFloat32 (0.0f);
+	m_q3 = dgFloat32 (0.0f);
+}
+
+inline dgQuaternion::dgQuaternion (dgFloat32 Q0, dgFloat32 Q1, dgFloat32 Q2, dgFloat32 Q3) 
+{
+	m_q0 = Q0;
+	m_q1 = Q1;
+	m_q2 = Q2;
+	m_q3 = Q3;
+//	dgAssert (dgAbsf (DotProduct (*this) -dgFloat32 (1.0f)) < dgFloat32(1.0e-4f));
+}
+
+
+
+inline void dgQuaternion::Scale (dgFloat32 scale) 
+{
+	m_q0 *= scale;
+	m_q1 *= scale;
+	m_q2 *= scale;
+	m_q3 *= scale;
+}
+
+inline void dgQuaternion::Normalize () 
+{
+	Scale (dgRsqrt (DotProduct (*this)));
+}
+
+inline dgFloat32 dgQuaternion::DotProduct (const dgQuaternion &QB) const
+{
+	return m_q0 * QB.m_q0 + m_q1 * QB.m_q1 + m_q2 * QB.m_q2 + m_q3 * QB.m_q3;
+}
+
+inline dgQuaternion dgQuaternion::Inverse () const 
+{
+	return dgQuaternion (m_q0, -m_q1, -m_q2, -m_q3);
+}
+
+inline dgQuaternion dgQuaternion::operator+ (const dgQuaternion &B) const
+{
+	return dgQuaternion (m_q0 + B.m_q0, m_q1 + B.m_q1, m_q2 + B.m_q2, m_q3 + B.m_q3);
+}
+
+inline dgQuaternion dgQuaternion::operator- (const dgQuaternion &B) const
+{
+	return dgQuaternion (m_q0 - B.m_q0, m_q1 - B.m_q1, m_q2 - B.m_q2, m_q3 - B.m_q3);
+}
+
+
+inline dgQuaternion dgQuaternion::operator* (const dgQuaternion &B) const
+{
+	return dgQuaternion (B.m_q0 * m_q0 - B.m_q1 * m_q1 - B.m_q2 * m_q2 - B.m_q3 * m_q3, 
+				 		 B.m_q1 * m_q0 + B.m_q0 * m_q1 - B.m_q3 * m_q2 + B.m_q2 * m_q3, 
+						 B.m_q2 * m_q0 + B.m_q3 * m_q1 + B.m_q0 * m_q2 - B.m_q1 * m_q3, 
+						 B.m_q3 * m_q0 - B.m_q2 * m_q1 + B.m_q1 * m_q2 + B.m_q0 * m_q3); 
+}
+
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgRandom.cpp

@@ -0,0 +1,39 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgRandom.h"
+
+#define RAND_MUL 31415821u
+static dgUnsigned32 randSeed = RAND_MUL;
+
+void dgApi dgRandomize (dgUnsigned32 Seed)
+{
+	randSeed = Seed;
+}
+
+dgUnsigned32 dgApi dgRandom()
+{
+	randSeed = RAND_MUL * randSeed + 1; 
+	return randSeed;
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgRandom.h

@@ -0,0 +1,41 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgRandom__
+#define __dgRandom__
+
+#include "dgStdafx.h"
+
+const dgFloat64 fRandom = (dgFloat64 (1.0) / dgFloat64 ((dgUnsigned32)(0xffffffff))); 
+
+// return a random number between 0 and 0xffffffff;
+dgUnsigned32 dgApi dgRandom();
+
+inline dgFloat32 dgfRandom()
+{
+	return (dgFloat32) (dgRandom() * fRandom);
+}
+
+
+void dgApi dgRandomize (dgUnsigned32 Seed);
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgRef.cpp

@@ -0,0 +1,45 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgList.h"
+#include "dgTree.h"
+
+
+dgRtti dgRef::m_rtti ("dgRef");
+
+
+void dgRef::AttachRef (
+	dgRef **oldObj, 
+	dgRef *newObj)
+{
+	if (*oldObj) {
+	   (*oldObj)->Release();
+	}
+	*oldObj = newObj;
+	if (newObj) {
+		newObj->AddRef();
+	}
+}
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgRef.h

@@ -0,0 +1,249 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgRef__
+#define __dgRef__
+
+#include "dgStdafx.h"
+#include "dgCRC.h"
+#include "dgRtti.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+
+struct dgRefFlags
+{
+	dgRefFlags ();
+	inline dgInt32 operator = (dgInt32 val);
+
+	dgUnsigned8 m_alive; 
+	dgUnsigned8 m_userFlag0;
+	dgUnsigned8 m_userFlag1;
+	dgUnsigned8 m_userFlag2;
+//	dgUnsigned32 m_userFlag3	: 1;
+//	dgUnsigned32 m_userFlag4	: 1;
+//	dgUnsigned32 m_userFlag5	: 1;
+//	dgUnsigned32 m_userFlag6	: 1;
+
+	dgUnsigned32 m_ref;
+};
+
+
+class dgRef: public dgRefFlags
+{
+   public:
+	dgRef ();
+	dgRef (const char *name);
+	dgRef (dgUnsigned32 idArg);
+	dgRef(const dgRef &Clone);
+	dgRef *AddRef () ;
+	dgInt32 Release ();
+	dgInt32 GetRefCount() const;
+
+	DG_CLASS_ALLOCATOR(allocator)
+
+	virtual dgRef *CreateClone ()	const;
+	virtual dgUnsigned32 GetTypeId () const;
+	virtual bool IsType (dgUnsigned32 typeId) const;
+
+	bool GetUserFlag0 () const;
+	bool GetUserFlag1 () const;
+	void SetUserFlag0 (bool flags);
+	void SetUserFlag1 (bool flags);
+
+	bool IsAlive() const;
+	virtual void Kill(); 
+	virtual void Unkill();
+
+	const char* GetName () const;
+	dgUnsigned32 GetNameID () const;
+	inline void SetNameID (dgUnsigned32 newID);
+	virtual inline void SetName (const char *name);
+
+	void AttachRef (dgRef **oldRef, dgRef *newRef);
+
+	
+	bool IsTypeByName (const char *typeName) const;
+	static dgUnsigned32 GetRttiType();
+
+	protected:
+	virtual ~dgRef (); 
+
+	private:
+	dgUnsigned32 m_id;
+	static dgRtti m_rtti;
+};
+
+
+
+inline dgRefFlags::dgRefFlags ()
+{
+	*this = 0;
+	m_alive = true;
+	m_ref = 1;
+}
+
+inline dgInt32 dgRefFlags::operator = (dgInt32 val)
+{
+	dgInt32* ptr;
+	ptr = &(*(dgInt32*)this);
+	*ptr = val;
+	return val;
+}
+
+
+
+inline dgRef::dgRef ()
+{
+	m_id = 0;
+}
+
+inline dgRef::dgRef (const char *name)
+{
+	SetName (name);
+}
+
+inline dgRef::dgRef (dgUnsigned32 idArg)
+{
+	SetNameID (idArg);
+}
+
+inline dgRef::dgRef(const dgRef &Clone)
+{
+	m_id = Clone.m_id;
+}
+
+inline dgRef::~dgRef () 
+{
+}
+
+inline dgRef *dgRef::AddRef () 
+{
+	m_ref ++;
+	dgAssert (m_ref < ((1<<24) - 1));
+	return this;
+}
+
+inline dgInt32 dgRef::Release ()
+{
+	m_ref --;
+	if (m_ref) {
+		return dgInt32 (m_ref);
+	}
+	delete this;
+	return 0;
+}
+
+inline dgRef *dgRef::CreateClone () const
+{
+	dgAssert (0);
+	return NULL;
+}
+
+
+inline dgUnsigned32 dgRef::GetTypeId () const
+{
+	return m_rtti.GetTypeId ();
+}
+
+inline bool dgRef::IsType (dgUnsigned32 typeId) const
+{
+	return m_rtti.IsTypeID (typeId);
+}
+
+inline dgUnsigned32 dgRef::GetRttiType()
+{
+	return m_rtti.GetTypeId();
+}
+
+inline bool dgRef::IsTypeByName (const char *typeName) const
+{
+	return IsType (dgCRC (typeName,  (dgInt32) strlen (typeName)));
+}
+
+
+inline bool dgRef::GetUserFlag0 () const
+{
+	return m_userFlag0 ? true : false;
+}
+
+inline bool dgRef::GetUserFlag1 () const
+{
+	return m_userFlag1 ? true : false;
+}
+
+
+inline void dgRef::SetUserFlag0 (bool flags)
+{
+	m_userFlag0 = dgUnsigned8 (flags);
+}
+
+inline void dgRef::SetUserFlag1 (bool flags)
+{
+	m_userFlag1 = dgUnsigned8 (flags);
+}
+
+
+inline bool dgRef::IsAlive() const
+{
+	return m_alive ? true : false;
+}
+
+inline void dgRef::Kill() 
+{
+	m_alive = false;
+}
+
+inline void dgRef::Unkill() 
+{
+	m_alive = true;
+}
+
+inline void dgRef::SetNameID (dgUnsigned32 newID)
+{
+	m_id = newID;
+}
+
+inline dgUnsigned32 dgRef::GetNameID () const
+{
+	return m_id;
+}
+
+inline void dgRef::SetName (const char *name)
+{
+	SetNameID (0);
+	if (name) {
+		SetNameID (dgCRC (name));
+	}
+}
+
+inline const char* dgRef::GetName () const
+{
+	return dgInverseCRC (GetNameID ());
+}
+
+inline dgInt32 dgRef::GetRefCount() const
+{
+	return dgInt32 (m_ref);
+}
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgRefCounter.cpp

@@ -0,0 +1,57 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "dgStdafx.h"
+#include "dgRefCounter.h"
+
+/*
+dgRefCounter::dgRefCounter(void)
+{
+	m_refCount = 1;
+}
+
+dgRefCounter::~dgRefCounter(void)
+{
+}
+
+
+int dgRefCounter::GetRef() const
+{
+	return m_refCount;
+}
+
+int dgRefCounter::Release()
+{
+	m_refCount --;
+	if (!m_refCount) {
+		delete this;
+		return 0;
+	}
+	return m_refCount;
+}
+
+void dgRefCounter::AddRef()
+{
+	m_refCount ++;
+}
+*/
+

newtondynamics.mod/NewtonDynamics/source/core/dgRefCounter.h

@@ -0,0 +1,72 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DGREF_COUNTER__
+#define __DGREF_COUNTER__
+
+class dgRefCounter
+{
+	public:
+	dgRefCounter(void);
+	int GetRef() const;
+	int Release();
+	void AddRef();
+
+	protected:
+	virtual ~dgRefCounter(void);
+
+	private:
+	int m_refCount;
+};
+
+
+inline dgRefCounter::dgRefCounter(void)
+{
+	m_refCount = 1;
+}
+
+inline dgRefCounter::~dgRefCounter(void)
+{
+	dgAssert (m_refCount <= 1);
+}
+
+
+inline int dgRefCounter::GetRef() const
+{
+	return m_refCount;
+}
+
+inline int dgRefCounter::Release()
+{
+	m_refCount --;
+	if (!m_refCount) {
+		delete this;
+		return 0;
+	}
+	return m_refCount;
+}
+
+inline void dgRefCounter::AddRef()
+{
+	m_refCount ++;
+}
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgRtti.h

@@ -0,0 +1,81 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgRtti__
+#define __dgRtti__
+
+#include "dgStdafx.h"
+#include "dgCRC.h"
+
+class dgRtti
+{
+	public:
+	dgRtti(const char* typeName);
+	dgUnsigned32 GetTypeId() const;
+	bool IsTypeID(dgUnsigned32 id) const;
+
+	private:
+	dgUnsigned32 m_TypeId;
+};
+
+inline dgRtti::dgRtti(const char* typeName)
+{
+	m_TypeId = dgCRC (typeName,  (dgInt32) strlen (typeName));
+}
+
+inline dgUnsigned32 dgRtti::GetTypeId() const
+{
+	return m_TypeId;
+}
+
+inline bool dgRtti::IsTypeID (dgUnsigned32 id) const
+{
+	return m_TypeId == id;
+}
+
+
+
+#define dgAddRtti(baseClass)								\
+	private:												\
+	static dgRtti rtti; 									\
+	public:													\
+	virtual bool IsType (dgUnsigned32 typeId) const			\
+	{														\
+		if (rtti.IsTypeID (typeId)) {						\
+			return true;									\
+		}													\
+		return baseClass::IsType (typeId);					\
+	}														\
+	virtual dgUnsigned32 GetTypeId () const					\
+	{														\
+		return rtti.GetTypeId ();							\
+	}														\
+	static dgUnsigned32 GetRttiType()						\
+	{														\
+		return rtti.GetTypeId();							\
+	}
+
+
+#define dgInitRtti(className)								\
+	dgRtti className::rtti (#className)
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgSPDMatrix.cpp

@@ -0,0 +1,395 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgStack.h"
+#include "dgMemory.h"
+#include "dgSPDMatrix.h"
+
+
+//static bool CholeskyDecomposition (dgFloat32 **rowPointers, dgInt32 size);
+//static void BackAndForwardSustitition (dgFloat32 **rowPointers, dgInt32 size, dgFloat32 *rightsideVector);
+
+
+/*
+bool _CholeskyDecomposition (
+	void *rowPointers, 
+	dgInt32 rowStrideInBytes,
+	dgInt32 typeSizeInBytes,
+	dgInt32 size)
+{
+	dgUnsigned8 *rowArray;
+
+	rowArray	= (dgUnsigned8*)rowPointers;
+	if (typeSizeInBytes == dgInt32 (sizeof (dgFloat32))) {
+		dgInt32 i;
+		dgFloat32 **rows;
+		rows = (dgFloat32**) dgStackAlloc (size * sizeof (dgFloat32*));
+		for (i = 0; i < size; i ++) {
+			rows[i] = *((dgFloat32 **)rowArray);
+			rowArray += rowStrideInBytes;
+		}
+		return CholeskyDecomposition (rows, size);
+
+	} else {
+		dgAssert (0);
+		dgAssert (typeSizeInBytes == dgInt32 (sizeof (dgFloat64)));
+	}
+
+	return true;
+}
+*/
+
+/*
+void _BackAndForwardSustitition (
+	void *rightsideVector,
+	void *rowPointers, 
+	dgInt32 rowStrideInBytes, 
+	dgInt32 typeSizeInBytes, 
+	dgInt32 size)
+{
+	dgUnsigned8 *rowArray;
+
+	rowArray	= (dgUnsigned8*)rowPointers;
+	if (typeSizeInBytes == dgInt32 (sizeof (dgFloat32))) {
+		dgInt32 i;
+		dgFloat32 **rows;
+		rows = (dgFloat32**) dgStackAlloc (size * sizeof (dgFloat32*));
+		for (i = 0; i < size; i ++) {
+			rows[i] = *((dgFloat32 **)rowArray);
+			rowArray += rowStrideInBytes;
+		}
+		BackAndForwardSustitition (rows, size, (dgFloat32*)rightsideVector);
+
+	} else {
+		dgAssert (0);
+		dgAssert (typeSizeInBytes == sizeof (dgFloat64));
+	}
+}
+*/
+
+/*
+bool _SolveByCholeskyDecomposition (
+	void *rightsideVector,
+	void *rowPointers, 
+	dgInt32 rowStrideInBytes, 
+	dgInt32 typeSizeInBytes, 
+	dgInt32 size)
+{
+	dgUnsigned8 *rowArray;
+
+	rowArray	= (dgUnsigned8*)rowPointers;
+	if (typeSizeInBytes == dgInt32 (sizeof (dgFloat32))) {
+		dgInt32 i;
+		dgFloat32 **rows;
+		rows = (dgFloat32**) dgStackAlloc (size * sizeof (dgFloat32*));
+		for (i = 0; i < size; i ++) {
+			rows[i] = *((dgFloat32 **)rowArray);
+			rowArray += rowStrideInBytes;
+		}
+
+
+		if (CholeskyDecomposition (rows, size)) {
+			BackAndForwardSustitition (rows, size, (dgFloat32*)rightsideVector);
+			return true;
+		}
+	} else {
+		dgAssert (0);
+		dgAssert (typeSizeInBytes == dgInt32 (sizeof (dgFloat64)));
+	}
+
+	return false;
+}
+*/
+
+
+/*
+void BackAndForwardSustitition (
+	dgFloat32 **rows, 
+	dgInt32 size, 
+	dgFloat32 *B)
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgFloat32 acc;
+
+//dgSPDMatrix<dgFloat32> M (8);
+//M.CholeskyDecomposition();
+
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgInt32 memCount;
+	dgInt32 floatCount;
+
+	memCount = dgGeneralVector<dgFloat32>::GetMemWrites();
+	floatCount = dgGeneralVector<dgFloat32>::GetFloatOps();
+	#endif
+
+	B[0] = B[0] / rows[0][0];
+	for (i =	1; i < size; i ++) {
+		acc = 0.0f;
+		for (j =	0; j < i; j ++) {
+			acc = acc + rows[j][i] * B[j];
+			#ifdef DG_COUNT_FLOAT_OPS
+			floatCount += 2;
+			#endif
+		}
+		B[i] = (B[i] - acc) / rows[i][i];
+		#ifdef DG_COUNT_FLOAT_OPS
+		floatCount += 2;
+		memCount  += 1;
+		#endif
+	}
+
+	B[size-1] = B[size-1] / rows[size-1][size-1];
+	for (i =	size - 2; i >= 0; i --) {
+		acc = 0.0f;
+		dgFloat32 *row; 
+
+		row = rows[i];
+		for (j = i + 1; j < size; j ++) {
+			acc = acc + row[j] * B[j];
+
+			#ifdef DG_COUNT_FLOAT_OPS
+			floatCount += 2;
+			#endif
+		}
+		B[i] = (B[i] - acc) / rows[i][i];
+
+		#ifdef DG_COUNT_FLOAT_OPS
+		floatCount += 2;
+		memCount  += 1;
+		#endif
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgGeneralVector<dgFloat32>::SetMemWrites(memCount); 
+	dgGeneralVector<dgFloat32>::SetFloatOps(floatCount); 
+	#endif
+}
+
+*/
+/*
+bool CholeskyDecomposition (dgFloat32 **rows, dgInt32 size)
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	dgFloat32 factor;
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgInt32 memCount;
+	dgInt32 floatCount;
+
+	memCount = dgGeneralVector<dgFloat32>::GetMemWrites();
+	floatCount = dgGeneralVector<dgFloat32>::GetFloatOps();
+	#endif
+
+	for (j = 0; j < size; j++) {
+		for (k = 0; k < j; k ++ ) {
+			factor = rows[k][j];
+			if (dgAbsf (factor) > 1.0e-6f) {
+				for (i = j; i < size; i ++) {
+					rows[j][i] -= rows[k][i] * factor;
+					#ifdef DG_COUNT_FLOAT_OPS
+					memCount += 1;
+					floatCount += 2;
+					#endif
+				}
+			}
+		}
+
+		factor = rows[j][j];
+		if (factor <= 0.0f) {
+			if (factor <= -5.0e-4f) {
+	 			return false;
+			}
+			factor = 1.0e-12f;
+		}
+
+		factor = dgSqrt (factor);
+		rows[j][j] = factor;
+		factor = 1.0f / factor;
+
+		#ifdef DG_COUNT_FLOAT_OPS
+		memCount += 1;
+		floatCount += 1;
+		#endif
+
+		for (k = j + 1; k < size; k ++) {
+			rows[j][k] *= factor;
+			#ifdef DG_COUNT_FLOAT_OPS
+			memCount += 1;
+			floatCount += 1;
+			#endif
+		}
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgGeneralVector<dgFloat32>::SetMemWrites(memCount); 
+	dgGeneralVector<dgFloat32>::SetFloatOps(floatCount); 
+	#endif
+
+	return true;
+}
+*/
+
+
+/*
+class TestSolver_xxxxxxx: public SymmetricBiconjugateGradientSolve
+{
+	public:
+	dgFloat64 a[4][4];
+
+	TestSolver_xxxxxxx()
+		:SymmetricBiconjugateGradientSolve()
+	{
+		dgFloat64 b[] = {1, 2, 3, 4};
+		dgFloat64 x[] = {0, 0, 0, 0};
+		dgFloat64 c[4];
+
+		memset (a, 0, sizeof (a));
+		a[0][0] = 2;
+		a[1][1] = 3;
+		a[2][2] = 4;
+		a[0][3] = 1;
+		a[1][3] = 1;
+		a[2][3] = 1;
+		a[3][0] = 1;
+		a[3][1] = 1;
+		a[3][2] = 1;
+
+
+		Solve (4, dgFloat64  (1.0e-10f), x, b);
+
+		MatrixTimeVector (c, x);
+		MatrixTimeVector (c, x);
+	}
+
+	void MatrixTimeVector (dgFloat64* const out, const dgFloat64* const v) const
+	{
+		out[0] = a[0][0] * v[0] + a[0][1] * v[1] + a[0][2] * v[2] + a[0][3] * v[3];
+		out[1] = a[1][0] * v[0] + a[1][1] * v[1] + a[1][2] * v[2] + a[1][3] * v[3];
+		out[2] = a[2][0] * v[0] + a[2][1] * v[1] + a[2][2] * v[2] + a[2][3] * v[3];
+		out[3] = a[3][0] * v[0] + a[3][1] * v[1] + a[3][2] * v[2] + a[3][3] * v[3];
+	}
+
+	void InversePrecoditionerTimeVector (dgFloat64* const out, const dgFloat64* const v) const
+	{
+		out[0] = v[0]/a[0][0];
+		out[1] = v[1]/a[1][1];
+		out[2] = v[2]/a[2][2];
+		out[3] = v[3];
+	}
+};
+*/
+
+
+dgSymmetricBiconjugateGradientSolve::dgSymmetricBiconjugateGradientSolve ()
+{
+}
+
+dgSymmetricBiconjugateGradientSolve::~dgSymmetricBiconjugateGradientSolve ()
+{
+}
+
+
+void dgSymmetricBiconjugateGradientSolve::ScaleAdd (dgInt32 size, dgFloat64* const a, const dgFloat64* const b, dgFloat64 scale, const dgFloat64* const c) const
+{
+	for (dgInt32 i = 0; i < size; i ++) {
+		a[i] = b[i] + scale * c[i];
+	}
+}
+
+void dgSymmetricBiconjugateGradientSolve::Sub (dgInt32 size, dgFloat64* const a, const dgFloat64* const b, const dgFloat64* const c) const
+{
+	for (dgInt32 i = 0; i < size; i ++) {
+		a[i] = b[i] - c[i];
+	}
+}
+
+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;
+}
+
+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;
+}

newtondynamics.mod/NewtonDynamics/source/core/dgSPDMatrix.h

@@ -0,0 +1,261 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgSPDMatrix__
+#define __dgSPDMatrix__
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+#include "dgGeneralMatrix.h"
+
+/*
+void _BackAndForwardSustitition (void *rightsideVector,
+										   void *rowPointers, 
+										   dgInt32 rowStrideInBytes, 
+										   dgInt32 typeSizeInBytes, 
+										   dgInt32 size);
+
+bool _SolveByCholeskyDecomposition (void *rightsideVector,
+												void *rowPointers, 
+												dgInt32 rowStrideInBytes, 
+												dgInt32 typeSizeInBytes, 
+												dgInt32 size);
+
+
+
+template<class T>
+class dgSPDMatrix: public dgGeneralMatrix<T>
+{
+	public:
+	dgSPDMatrix (dgInt32 size);
+	dgSPDMatrix (const dgSPDMatrix<T>& src);
+	dgSPDMatrix (dgInt32 size, T *elemBuffer, dgGeneralVector<T>* m_rowBuffer);
+	dgSPDMatrix (const dgSPDMatrix<T>& src, T *elemBuffer, dgGeneralVector<T>* m_rowBuffer);
+
+	~dgSPDMatrix ();
+
+	bool TestPSD () const;
+	bool CholeskyDecomposition();
+	void DownDateCholeskyDecomposition (dgInt32 column);
+
+
+	bool Solve (dgGeneralVector<T> &b);
+	void BackAndForwardSustitition(dgGeneralVector<T> &b);
+};
+
+
+// ***********************************************************************************************
+//
+//	LinearSystem
+//
+// ***********************************************************************************************
+template<class T>
+dgSPDMatrix<T>::dgSPDMatrix (dgInt32 size)
+	:dgGeneralMatrix<T>(size, size)
+{
+}
+
+template<class T>
+dgSPDMatrix<T>::dgSPDMatrix (const dgSPDMatrix<T>& src)
+	:dgGeneralMatrix<T>(src)
+{
+}
+
+
+template<class T>
+dgSPDMatrix<T>::dgSPDMatrix (
+	dgInt32 size, 
+	T *elemBuffer, 
+	dgGeneralVector<T>* m_rowBuffer)
+	:dgGeneralMatrix<T>(size, size, elemBuffer, m_rowBuffer)
+{
+}
+
+template<class T>
+dgSPDMatrix<T>::dgSPDMatrix (
+	const dgSPDMatrix<T>& src, 
+	T *elemBuffer, 
+	dgGeneralVector<T>* m_rowBuffer)
+	:dgGeneralMatrix<T>(src, elemBuffer, m_rowBuffer)
+{
+}
+
+
+template<class T>  
+dgSPDMatrix<T>::~dgSPDMatrix ()
+{
+}
+
+template<class T>
+bool dgSPDMatrix<T>::TestPSD () const
+{
+	if (!TestSymetry ()) {
+		return false;
+	}
+
+	dgSPDMatrix<T> tmp (*this);
+	return tmp.CholeskyDecomposition();
+}
+
+
+template<class T>
+void dgSPDMatrix<T>::BackAndForwardSustitition(dgGeneralVector<T> &b)
+{
+	_BackAndForwardSustitition (b.m_columns, &m_rows[0].m_columns, sizeof (dgGeneralVector<T>), sizeof (T), m_rowCount);
+}
+
+
+template<class T>
+bool dgSPDMatrix<T>::Solve (dgGeneralVector<T> &b)
+{
+	return _SolveByCholeskyDecomposition (b.m_columns, &m_rows[0].m_columns, sizeof (dgGeneralVector<T>), sizeof (T), m_rowCount);
+}
+
+
+
+template<class T>
+bool dgSPDMatrix<T>::CholeskyDecomposition()
+{
+	dgInt32 i;
+	dgInt32 j;
+	dgInt32 k;
+	T factor;
+	T* rowK;
+	T* rowJ;
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgInt32 memCount;
+	dgInt32 floatCount;
+
+	memCount = dgGeneralVector<T>::GetMemWrites();
+	floatCount = dgGeneralVector<T>::GetFloatOps();
+	#endif
+
+	for (j = 0; j < m_rowCount; j++) {
+		rowJ = &m_rows[j].m_columns[0];
+
+		for (k = 0; k < j; k ++ ) {
+			rowK = &m_rows[k].m_columns[0];
+
+			factor = rowK[j];
+			if (dgAbsf (factor) > 1.0e-6f) {
+				for (i = j; i < m_rowCount; i ++) {
+					rowJ[i] -= rowK[i] * factor;
+					#ifdef DG_COUNT_FLOAT_OPS
+					memCount += 1;
+					floatCount += 2;
+					#endif
+				}
+			}
+		}
+
+		factor = rowJ[j];
+		if (factor <= T (0.0f)) {
+			if (factor <= T(-5.0e-4f)) {
+	 			return false;
+			}
+			factor = T(1.0e-12f);
+		}
+
+		factor = T (dgSqrt (dgFloat32(factor)));
+		rowJ[j] = factor;
+		factor = T(1.0f / dgFloat32(factor));
+
+		#ifdef DG_COUNT_FLOAT_OPS
+		memCount += 1;
+		floatCount += 1;
+		#endif
+
+		for (k = j + 1; k < m_rowCount; k ++) {
+			rowJ[k] *= factor;
+			#ifdef DG_COUNT_FLOAT_OPS
+			memCount += 1;
+			floatCount += 1;
+			#endif
+		}
+	}
+
+	#ifdef DG_COUNT_FLOAT_OPS
+	dgGeneralVector<T>::SetMemWrites(memCount); 
+	dgGeneralVector<T>::SetFloatOps(floatCount); 
+	#endif
+
+	return true;
+
+}
+
+
+template<class T>
+void dgSPDMatrix<T>::DownDateCholeskyDecomposition (dgInt32 column)
+{
+	dgInt32 i;
+	dgInt32 j;
+
+	T *buffer;
+	dgGeneralVector<T>* rowsBuffer;
+
+	rowsBuffer = (dgGeneralVector<dgFloat32>*) dgStackAlloc (m_rowCount * sizeof (dgGeneralVector<T>));
+	buffer = (T *) dgStackAlloc ((m_rowCount + 4) * (m_rowCount + 4) * sizeof (T));
+
+	dgGeneralMatrix<T> tmp (m_rowCount, m_rowCount, buffer, rowsBuffer);
+	dgGeneralMatrix<T>& me = *this;
+	for (i = 0; i < m_rowCount; i ++) {
+		tmp[i][i] =  me[i][i];
+		for (j = i + 1; j < m_rowCount; j ++) {
+			tmp[i][j] = T (0.0f);
+			tmp[j][i] =  me[i][j];
+		}
+	}
+
+	me.MatrixTimeMatrixTranspose (tmp, tmp);
+	for (i = 0; i < m_rowCount; i ++) {
+		me[i][column] = T (0.0f);
+		me[column][i] = T (0.0f);
+	}
+	me[column][column] = T (1.0f);
+
+	CholeskyDecomposition();
+}
+*/
+
+class dgSymmetricBiconjugateGradientSolve
+{
+	public:
+	dgSymmetricBiconjugateGradientSolve ();
+	~dgSymmetricBiconjugateGradientSolve ();	
+
+	dgFloat64 Solve (dgInt32 size, dgFloat64 tolerance, dgFloat64* const x, const dgFloat64* const b) const;
+
+	protected:
+	virtual void MatrixTimeVector (dgFloat64* const out, const dgFloat64* const v) const = 0;
+	virtual bool InversePrecoditionerTimeVector (dgFloat64* const out, const dgFloat64* const v) const = 0;
+
+	private:
+	dgFloat64 DotProduct (dgInt32 size, const dgFloat64* const b, const dgFloat64* const c) const;
+	void ScaleAdd (dgInt32 size, dgFloat64* const a, const dgFloat64* const b, dgFloat64 scale, const dgFloat64* const c) const;
+	void Sub (dgInt32 size, dgFloat64* const a, const dgFloat64* const b, const dgFloat64* const c) const;
+
+};
+
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgSmallDeterminant.cpp

@@ -0,0 +1,153 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgGoogol.h"
+#include "dgSmallDeterminant.h"
+
+#define Absolute(a)  ((a) >= 0.0 ? (a) : -(a))
+
+dgFloat64 Determinant2x2 (const dgFloat64 matrix[2][2], dgFloat64* const error)
+{
+	dgFloat64 a00xa11 = matrix[0][0] * matrix[1][1];
+	dgFloat64 a01xa10 = matrix[0][1] * matrix[1][0];
+	*error = Absolute(a00xa11) + Absolute(a01xa10);
+	return a00xa11 - a01xa10;
+}
+
+dgGoogol Determinant2x2 (const dgGoogol matrix[2][2])
+{
+	dgGoogol a00xa11 (matrix[0][0] * matrix[1][1]);
+	dgGoogol a01xa10 (matrix[0][1] * matrix[1][0]);
+	return a00xa11 - a01xa10;
+}
+
+
+
+dgFloat64 Determinant3x3 (const dgFloat64 matrix[3][3], dgFloat64* const error)
+{
+	dgFloat64 sign = dgFloat64 (-1.0f);
+	dgFloat64 det = dgFloat64 (0.0f);
+	dgFloat64 accError = dgFloat64 (0.0f); 
+	for (dgInt32 i = 0; i < 3; i ++)  {
+		dgFloat64 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 ++;
+				}
+			}
+		}
+
+		dgFloat64 parcialError;
+		dgFloat64 minorDet = Determinant2x2 (cofactor, &parcialError);
+		accError += parcialError * Absolute (matrix[2][i]);
+		det += sign * minorDet * matrix[2][i];
+		sign *= dgFloat64 (-1.0f);
+	}
+
+	*error = accError;
+	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;
+}
+
+
+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;
+}
+
+
+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;
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgSmallDeterminant.h

@@ -0,0 +1,37 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgSMALLDETERMINANT__
+#define __dgSMALLDETERMINANT__
+
+#include "dgStdafx.h"
+
+class dgGoogol;
+dgFloat64 Determinant2x2 (const dgFloat64 matrix[2][2], dgFloat64* const error);
+dgFloat64 Determinant3x3 (const dgFloat64 matrix[3][3], dgFloat64* const error);
+dgFloat64 Determinant4x4 (const dgFloat64 matrix[4][4], dgFloat64* const error);
+
+
+dgGoogol Determinant2x2 (const dgGoogol matrix[2][2]);
+dgGoogol Determinant3x3 (const dgGoogol matrix[3][3]);
+dgGoogol Determinant4x4 (const dgGoogol matrix[4][4]);
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgStack.h

@@ -0,0 +1,119 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgStack__
+#define __dgStack__
+
+#include "dgStdafx.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+class dgStackBase
+{
+	protected:
+	dgStackBase (dgInt32 size);
+	~dgStackBase ();
+
+	const void *m_ptr;
+};
+
+inline dgStackBase::dgStackBase (dgInt32 size)
+{
+	m_ptr = dgMallocStack (size_t (size));
+}
+
+inline dgStackBase::~dgStackBase ()
+{
+	dgFreeStack ((void*)m_ptr);
+}
+
+
+
+
+template<class T>
+class dgStack: public dgStackBase
+{
+	public:
+	dgStack (dgInt32 size);
+	~dgStack ();
+	dgInt32 GetSizeInBytes() const;
+	dgInt32 GetElementsCount() const;
+	
+	T& operator[] (dgInt32 entry);
+	const T& operator[] (dgInt32 entry) const;
+
+	private:
+	dgInt32 m_size;
+};
+
+template<class T>
+dgStack<T>::dgStack (dgInt32 size)
+	:dgStackBase (dgInt32 (size * sizeof(T)))
+{
+	m_size = size;
+}
+
+template<class T>
+dgStack<T>::~dgStack ()
+{
+}
+
+template<class T>
+dgInt32 dgStack<T>::GetElementsCount() const
+{
+	return m_size;
+}
+
+template<class T>
+dgInt32 dgStack<T>::GetSizeInBytes() const
+{
+	return dgInt32 (m_size * sizeof(T));
+}
+ 
+
+template<class T>
+T& dgStack<T>::operator[] (dgInt32 entry) 
+{
+	T *mem;
+
+	dgAssert (entry >= 0);
+	dgAssert ((entry < m_size) || ((m_size == 0) && (entry == 0)));
+
+	mem = (T*) m_ptr;
+	return mem[entry];
+}
+
+template<class T>
+const T& dgStack<T>::operator[] (dgInt32 entry) const
+{
+	T *mem;
+
+	dgAssert (0);
+	dgAssert (entry >= 0);
+	dgAssert ((entry < m_size) || ((m_size == 0) && (entry == 0)));
+
+	mem = (T*) m_ptr;
+	return mem[entry];
+}
+
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgStdafx.h

@@ -0,0 +1,28 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _DG_TYPES_H_
+#define _DG_TYPES_H_
+
+#include "dgTypes.h"
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgThread.cpp

@@ -0,0 +1,255 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgThread.h"
+
+
+
+dgThread::dgThread ()
+	:m_id(0)
+	,m_terminate(0)
+	,m_threadRunning(0)
+{
+	m_name[0] = 0;
+}
+
+dgThread::dgThread (const char* const name, dgInt32 id)
+	:m_id(id)
+	,m_terminate(0)
+	,m_threadRunning(0)
+{
+	strncpy (m_name, name, sizeof (m_name) - 1);
+}
+
+
+void dgThread::Init (const char* const name, dgInt32 id)
+{
+	m_id = id;
+	strncpy (m_name, name, sizeof (m_name) - 1);
+	Init();
+}
+
+
+bool dgThread::StillBusy() const
+{
+	return m_threadRunning ? true : false;
+}
+
+
+
+#ifdef DG_USE_THREAD_EMULATION
+
+dgThread::dgSemaphore::dgSemaphore ()
+{
+	m_sem = NULL;
+}
+
+dgThread::dgSemaphore::~dgSemaphore ()
+{
+}
+
+void dgThread::dgSemaphore::Release ()
+{
+}
+
+dgThread::dgCriticalSection::dgCriticalSection()
+{
+}
+
+dgThread::dgCriticalSection::~dgCriticalSection()
+{
+}
+
+
+dgThread::~dgThread ()
+{
+}
+
+void dgThread::Init ()
+{
+}
+
+void dgThread::Close ()
+{
+}
+
+void dgThread::SuspendExecution (dgSemaphore& mutex)
+{
+}
+
+
+void dgThread::SuspendExecution (dgInt32 count, dgSemaphore* const semArray)
+{
+}
+
+
+int dgThread::GetPriority() const
+{
+	return 0;
+}
+
+void dgThread::SetPriority(int priority)
+{
+}
+
+
+void* dgThread::dgThreadSystemCallback(void* threadData)
+{
+	return 0;
+}
+
+#else  
+
+
+#if defined (_MACOSX_VER) || defined (IOS) || defined (__APPLE__)
+	#define DG_SEMAPHORE_NAME "/semaphore"
+#endif
+
+dgThread::dgSemaphore::dgSemaphore ()
+{
+	#if defined (_MACOSX_VER) || defined (IOS) || defined (__APPLE__)
+		char temp[256];
+		static int semID = 1;
+		m_nameId = semID ++;
+		sprintf (temp, "%s%d", DG_SEMAPHORE_NAME, m_nameId);
+        sem_unlink(temp);
+		m_sem = sem_open(temp, O_CREAT, S_IRUSR | S_IWUSR, 0);
+	#else
+		sem_init (&m_sem, 0, 0);
+	#endif
+}
+
+dgThread::dgSemaphore::~dgSemaphore ()
+{
+	#if defined (_MACOSX_VER) || defined (IOS) || defined (__APPLE__)
+		char temp[256];
+		sprintf (temp, "%s%d", DG_SEMAPHORE_NAME, m_nameId);
+		sem_close(m_sem);
+		sem_unlink(temp) ;
+	#else 
+		sem_destroy (&m_sem);
+	#endif
+}
+
+void dgThread::dgSemaphore::Release ()
+{
+	#if defined (_MACOSX_VER) || defined (IOS) || defined (__APPLE__)
+		sem_post (m_sem);
+	#else 
+		sem_post (&m_sem);
+	#endif
+}
+
+void dgThread::dgSemaphore::Wait()
+{
+	#if defined (_MACOSX_VER) || defined (IOS) || defined (__APPLE__)
+		sem_wait (m_sem);
+	#else 
+		sem_wait (&m_sem);
+	#endif
+}
+
+dgThread::dgCriticalSection::dgCriticalSection()
+	#ifdef DG_USE_MUTEX_CRITICAL_SECTION
+		:m_mutex (PTHREAD_MUTEX_INITIALIZER)
+	#else 
+		:m_mutex(0)
+	#endif
+{
+}
+
+dgThread::dgCriticalSection::~dgCriticalSection()
+{
+	#ifdef DG_USE_MUTEX_CRITICAL_SECTION
+		pthread_mutex_destroy(&m_mutex);
+	#endif
+};
+
+
+
+dgThread::~dgThread ()
+{
+}
+
+void dgThread::Init ()
+{
+	pthread_create (&m_handle, NULL, dgThreadSystemCallback, this);
+}
+
+void dgThread::Close ()
+{
+	pthread_join (m_handle, NULL);
+}
+
+
+dgInt32 dgThread::GetPriority() const
+{
+	dgInt32 policy;
+	sched_param param;
+	pthread_getschedparam (m_handle, &policy, &param);
+	return param.sched_priority;
+}
+
+void dgThread::SetPriority(int priority)
+{
+	dgInt32 policy;
+	sched_param param;
+
+	pthread_getschedparam (m_handle, &policy, &param);
+
+	param.sched_priority = priority;
+	pthread_setschedparam (m_handle, policy, &param);
+}
+
+
+void dgThread::SuspendExecution (dgSemaphore& mutex)
+{
+	mutex.Wait();
+}
+
+
+void dgThread::SuspendExecution (dgInt32 count, dgSemaphore* const semArray)
+{
+	for (dgInt32 i = 0; i < count; i ++) {
+		SuspendExecution (semArray[i]);
+	}
+}
+
+
+void* dgThread::dgThreadSystemCallback(void* threadData)
+{
+	dgFloatExceptions exception;
+	dgSetPrecisionDouble precision;
+
+	dgThread* const me = (dgThread*) threadData;
+	dgInterlockedExchange(&me->m_threadRunning, 1);
+	me->Execute(me->m_id);
+	dgInterlockedExchange(&me->m_threadRunning, 0);
+	dgThreadYield();
+	return 0;
+}
+
+
+#endif
+
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgThread.h

@@ -0,0 +1,128 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_THREAD_API_H__
+#define __DG_THREAD_API_H__
+
+// by default newton is run on a separate thread, optionally concurrent with the calling thread, it also uses a thread job pool for multi core systems.
+// define DG_USE_THREAD_EMULATION on the command line for platform that do not support hardware multi threading or if multi threading is not stable 
+//#define DG_USE_THREAD_EMULATION
+
+//
+//#define DG_USE_MUTEX_CRITICAL_SECTION
+
+class dgThread
+{
+public:
+	class dgCriticalSection
+	{
+		public:
+		dgCriticalSection();
+		~dgCriticalSection();
+		void Lock();
+		void Unlock();
+
+		private:
+		#if (defined (DG_USE_THREAD_EMULATION) || !defined (DG_USE_MUTEX_CRITICAL_SECTION))
+			dgInt32 m_mutex;
+		#else 
+			pthread_mutex_t  m_mutex;
+		#endif
+	};
+
+	class dgSemaphore
+	{
+		public:
+		dgSemaphore ();
+		~dgSemaphore ();
+		void Wait();
+		void Release();
+
+		private:
+		#ifdef DG_USE_THREAD_EMULATION
+			dgInt32 m_sem;
+		#else 
+			#if defined (_MACOSX_VER) || defined (IOS) || defined (__APPLE__)
+				sem_t* m_sem;
+				dgInt32 m_nameId;
+			#else
+				sem_t m_sem;
+			#endif
+		#endif
+		friend class dgThread;
+	};
+
+	dgThread ();
+	dgThread (const char* const name, dgInt32 id);
+	virtual ~dgThread ();
+
+	virtual void Execute (dgInt32 threadId) = 0;
+	
+	bool StillBusy() const;
+	void SuspendExecution (dgSemaphore& mutex);
+	void SuspendExecution (dgInt32 count, dgSemaphore* const mutexes);
+
+	dgInt32 GetPriority() const;
+	void SetPriority(int priority);
+
+	protected:
+	void Init ();
+	void Init (const char* const name, dgInt32 id);
+	void Close ();
+
+	static void* dgThreadSystemCallback(void* threadData);
+
+	pthread_t m_handle;
+	dgInt32 m_id;
+	dgInt32 m_terminate;
+	dgInt32 m_threadRunning;
+	
+	char m_name[32];
+};
+
+
+DG_INLINE void dgThread::dgCriticalSection::Lock()
+{
+	#ifndef DG_USE_THREAD_EMULATION 
+		#ifdef DG_USE_MUTEX_CRITICAL_SECTION
+			pthread_mutex_lock(&m_mutex);
+		#else 
+			while (dgInterlockedExchange(&m_mutex, 1)) {
+				dgThreadYield();
+			}
+		
+		#endif
+	#endif
+}
+
+DG_INLINE void dgThread::dgCriticalSection::Unlock()
+{
+	#ifndef DG_USE_THREAD_EMULATION 
+		#ifdef DG_USE_MUTEX_CRITICAL_SECTION
+			pthread_mutex_unlock(&m_mutex);
+		#else 
+			dgInterlockedExchange(&m_mutex, 0);
+		#endif
+	#endif
+}
+
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgThreadHive.cpp

@@ -0,0 +1,243 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgTypes.h"
+#include "dgMemory.h"
+#include "dgThreadHive.h"
+
+
+
+
+dgThreadHive::dgThreadBee::dgThreadBee()
+	:dgThread()
+	,m_isBusy(0)
+	,m_ticks (0)
+	,m_myMutex()
+	,m_hive(NULL)
+	,m_allocator(NULL)
+	,m_getPerformanceCount(NULL)
+{
+}
+
+dgThreadHive::dgThreadBee::~dgThreadBee()
+{
+	while (IsBusy());
+
+	dgInterlockedExchange(&m_terminate, 1);
+	m_myMutex.Release();
+	Close();
+}
+
+void dgThreadHive::dgThreadBee::SetUp(dgMemoryAllocator* const allocator, const char* const name, dgInt32 id, dgThreadHive* const hive)
+{
+	m_allocator = allocator;
+	m_hive = hive;
+	Init (name, id);
+
+	int priority = GetPriority();
+	SetPriority(priority + 1);
+}
+
+bool dgThreadHive::dgThreadBee::IsBusy() const
+{
+	return m_isBusy ? true : false;
+}
+
+void dgThreadHive::dgThreadBee::SetPerfomanceCounter(OnGetPerformanceCountCallback callback)
+{
+	m_getPerformanceCount = callback;
+}
+
+
+void dgThreadHive::dgThreadBee::Execute (dgInt32 threadId)
+{
+	m_hive->OnBeginWorkerThread (threadId);
+
+	while (!m_terminate) {
+		dgInterlockedExchange(&m_isBusy, 0);
+		SuspendExecution(m_myMutex);
+		dgInterlockedExchange(&m_isBusy, 1);
+		if (!m_terminate) {
+			RunNextJobInQueue(threadId);
+			m_hive->m_myMutex[threadId].Release();
+		}
+	}
+
+	dgInterlockedExchange(&m_isBusy, 0);
+
+	m_hive->OnEndWorkerThread (threadId);
+}
+
+
+void dgThreadHive::dgThreadBee::RunNextJobInQueue(dgInt32 threadId)
+{
+	dgUnsigned32 ticks = m_getPerformanceCount();
+
+	bool isEmpty = false;
+	do {
+		dgThreadJob job;
+		dgAssert (threadId == m_id);
+		m_hive->m_jobsCriticalSection.Lock();
+		isEmpty = m_hive->m_jobsPool.IsEmpty();
+		if (!isEmpty) {
+			job = m_hive->m_jobsPool.GetHead();
+			m_hive->m_jobsPool.Pop();
+		}
+		m_hive->m_jobsCriticalSection.Unlock();
+
+		if (!isEmpty) {
+			job.m_callback (job.m_context0, job.m_context1, m_id);
+		}
+	} while (!isEmpty);
+
+	
+	m_ticks += (m_getPerformanceCount() - ticks);
+}
+
+
+dgThreadHive::dgThreadHive(dgMemoryAllocator* const allocator)
+	:m_beesCount(0)
+	,m_currentIdleBee(0)
+	,m_workerBees(NULL)
+	,m_myMasterThread(NULL)
+	,m_allocator(allocator)
+	,m_jobsCriticalSection()
+	,m_globalCriticalSection()
+    ,m_jobsPool(allocator)
+{
+}
+
+dgThreadHive::~dgThreadHive()
+{
+	DestroyThreads();
+}
+
+void dgThreadHive::SetMatertThread (dgThread* const mastertThread)
+{
+	m_myMasterThread = mastertThread;
+}
+
+void dgThreadHive::DestroyThreads()
+{
+	if (m_beesCount) {
+		delete[] m_workerBees;
+		m_workerBees = NULL;
+		m_beesCount = 0;
+	}
+}
+
+
+void dgThreadHive::SetPerfomanceCounter(OnGetPerformanceCountCallback callback)
+{
+	for (dgInt32 i = 0; i < m_beesCount; i ++) {
+		m_workerBees[i].SetPerfomanceCounter(callback);
+	}
+}
+
+
+dgUnsigned32 dgThreadHive::GetPerfomanceTicks (dgUnsigned32 threadIndex) const
+{
+	return (m_beesCount && (threadIndex < dgUnsigned32(m_beesCount))) ? m_workerBees[threadIndex].m_ticks : 0;
+}
+
+
+
+
+dgInt32 dgThreadHive::GetThreadCount() const
+{
+	return m_beesCount ? m_beesCount : 1;
+}
+
+void dgThreadHive::ClearTimers()
+{
+	for (dgInt32 i = 0; i < m_beesCount; i ++) {
+		m_workerBees[i].m_ticks = 0;
+	}
+}
+
+dgInt32 dgThreadHive::GetMaxThreadCount() const
+{
+	return DG_MAX_THREADS_HIVE_COUNT;
+}
+
+
+void dgThreadHive::SetThreadsCount (dgInt32 threads)
+{
+	DestroyThreads();
+
+	m_beesCount = dgMin (threads, DG_MAX_THREADS_HIVE_COUNT);
+	if (m_beesCount == 1) {
+		m_beesCount = 0;
+	}
+
+	if (m_beesCount) {
+		m_workerBees = new (m_allocator) dgThreadBee[dgUnsigned32 (m_beesCount)];
+
+		for (dgInt32 i = 0; i < m_beesCount; i ++) {
+			char name[256];
+			sprintf (name, "dgThreadBee%d", i);
+			m_workerBees[i].SetUp(m_allocator, name, i, this);
+		}
+	}
+}
+
+
+void dgThreadHive::QueueJob (dgWorkerThreadTaskCallback callback, void* const context0, void* const context1)
+{
+	if (!m_beesCount) {
+		callback (context0, context1, 0);
+	} else {
+		#ifdef DG_USE_THREAD_EMULATION
+			callback (context0, context1, 0);
+		#else 
+			dgThreadJob job (context0, context1, callback);
+			m_jobsPool.Push(job);
+			if (m_jobsPool.IsFull()) {
+				SynchronizationBarrier ();
+			}
+		#endif
+	}
+}
+
+
+void dgThreadHive::OnBeginWorkerThread (dgInt32 threadId)
+{
+}
+
+void dgThreadHive::OnEndWorkerThread (dgInt32 threadId)
+{
+}
+
+
+void dgThreadHive::SynchronizationBarrier ()
+{
+	if (m_beesCount) {
+		for (dgInt32 i = 0; i < m_beesCount; i ++) {
+			m_workerBees[i].m_myMutex.Release();
+		}
+
+		m_myMasterThread->SuspendExecution(m_beesCount, m_myMutex);
+		dgAssert (m_jobsPool.IsEmpty());
+	}
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgThreadHive.h

@@ -0,0 +1,171 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef __DG_THREAD_HIVE_H__
+#define __DG_THREAD_HIVE_H__
+
+#include "dgThread.h"
+#include "dgMemory.h"
+#include "dgFastQueue.h"
+
+
+
+//#define DG_THREAD_POOL_JOB_SIZE (512)
+#define DG_THREAD_POOL_JOB_SIZE (1024 * 8)
+
+typedef void (*dgWorkerThreadTaskCallback) (void* const context0, void* const context1, dgInt32 threadID);
+
+class dgThreadHive  
+{
+	public:
+
+	class dgThreadJob
+	{
+		public:
+		dgThreadJob()
+		{
+		}
+
+		dgThreadJob (void* const context0, void* const context1, dgWorkerThreadTaskCallback callback)
+			:m_context0(context0)
+			,m_context1(context1)
+			,m_callback(callback)
+		{
+		}
+		void* m_context0;
+		void* m_context1;
+		dgWorkerThreadTaskCallback m_callback;
+	};
+
+
+	class dgThreadBee: public dgThread
+	{
+		public:
+		DG_CLASS_ALLOCATOR(allocator)
+
+		dgThreadBee();
+		~dgThreadBee();
+
+		bool IsBusy() const;
+		void SetPerfomanceCounter(OnGetPerformanceCountCallback callback);
+		void SetUp(dgMemoryAllocator* const allocator, const char* const name, dgInt32 id, dgThreadHive* const hive);
+		virtual void Execute (dgInt32 threadId);
+
+		void RunNextJobInQueue(dgInt32 threadId);
+
+		dgInt32 m_isBusy;
+
+		dgUnsigned32 m_ticks;
+		dgSemaphore m_myMutex;
+		dgThreadHive* m_hive;
+		dgMemoryAllocator* m_allocator; 
+		OnGetPerformanceCountCallback m_getPerformanceCount;	
+	};
+
+	dgThreadHive(dgMemoryAllocator* const allocator);
+	virtual ~dgThreadHive();
+
+	virtual void OnBeginWorkerThread (dgInt32 threadId);
+	virtual void OnEndWorkerThread (dgInt32 threadId);
+
+	void SetMatertThread (dgThread* const mastertThread);
+
+	void GlobalLock() const;
+	void GlobalUnlock() const;
+
+	void GetIndirectLock (dgThread::dgCriticalSection* const criticalSectionLock) const;
+	void ReleaseIndirectLock (dgThread::dgCriticalSection* const criticalSectionLock) const;
+
+	void ClearTimers();
+	dgInt32 GetThreadCount() const;
+
+	dgInt32 GetMaxThreadCount() const;
+	void SetThreadsCount (dgInt32 count);
+
+	void QueueJob (dgWorkerThreadTaskCallback callback, void* const context0, void* const context1);
+	void SynchronizationBarrier ();
+
+	void SetPerfomanceCounter(OnGetPerformanceCountCallback callback);
+	dgUnsigned32 GetPerfomanceTicks (dgUnsigned32 threadIndex) const;
+
+	private:
+	void DestroyThreads();
+
+	dgInt32 m_beesCount;
+	dgInt32 m_currentIdleBee;
+	dgThreadBee* m_workerBees;
+	dgThread* m_myMasterThread;
+	dgMemoryAllocator* m_allocator;
+	dgThread::dgCriticalSection m_jobsCriticalSection;
+	mutable dgThread::dgCriticalSection m_globalCriticalSection;
+	dgThread::dgSemaphore m_myMutex[DG_MAX_THREADS_HIVE_COUNT];
+	dgFastQueue<dgThreadJob, DG_THREAD_POOL_JOB_SIZE> m_jobsPool;
+};
+
+
+inline void dgThreadHive::GlobalLock() const
+{
+	GetIndirectLock(&m_globalCriticalSection);
+}
+
+inline void dgThreadHive::GlobalUnlock() const
+{	
+	ReleaseIndirectLock(&m_globalCriticalSection);
+}
+
+
+inline void dgThreadHive::GetIndirectLock (dgThread::dgCriticalSection* const criticalSectionLock) const
+{
+	if (m_beesCount) {	
+		criticalSectionLock->Lock();
+	}
+}
+
+inline void dgThreadHive::ReleaseIndirectLock (dgThread::dgCriticalSection* const criticalSectionLock) const
+{
+	if (m_beesCount) {	
+		criticalSectionLock->Unlock();
+	}
+}
+
+class dgThreadHiveScopeLock
+{
+	public:
+	dgThreadHiveScopeLock(const dgThreadHive* const me, dgThread::dgCriticalSection* const criticalSectionLock)
+		:m_me(me)
+		,m_lock (criticalSectionLock)
+	{
+		me->GetIndirectLock (criticalSectionLock);
+	}
+
+	~dgThreadHiveScopeLock()
+	{
+		m_me->ReleaseIndirectLock (m_lock);
+	}
+
+	const dgThreadHive* m_me;
+	dgThread::dgCriticalSection* m_lock; 
+};
+
+
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgThreadProfiler.h

@@ -0,0 +1,51 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DG_THREAD_PROFILER_H__
+#define __DG_THREAD_PROFILER_H__
+
+#include "dgList.h"
+
+#define DG_PROFILER_EVENT_NAME_SIZE  32
+
+class dgThreadProfiler
+{
+	public:
+	class dgEvent
+	{
+		public:
+		char m_name[ DG_PROFILER_EVENT_NAME_SIZE];
+		dgUnsigned32 m_startTime;
+		dgUnsigned32 m_duration;
+	};
+
+	dgThreadProfiler ();
+	dgThreadProfiler (const char* const name, dgInt32 id);
+	virtual ~dgThreadProfiler ();
+	
+	dgUnsigned32 m_startTime;
+	dgUnsigned32 m_endTime;
+	dgList<dgEvent> m_events;
+};
+
+
+
+#endif

newtondynamics.mod/NewtonDynamics/source/core/dgTree.cpp

@@ -0,0 +1,423 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgTree.h"
+
+
+dgRedBackNode *dgRedBackNode::Minimum () const
+{
+	dgRedBackNode* ptr = (dgRedBackNode *)this;
+	for (; ptr->m_left; ptr = ptr->m_left){}
+	return ptr;
+}
+
+dgRedBackNode *dgRedBackNode::Maximum () const
+{
+	dgRedBackNode* ptr = (dgRedBackNode *)this;
+	for (; ptr->m_right; ptr = ptr->m_right){}
+	return ptr;
+}
+
+
+dgRedBackNode *dgRedBackNode::Prev () const
+{
+	if (m_left) {
+		return m_left->Maximum ();
+	}
+
+	dgRedBackNode* node = (dgRedBackNode *)this;
+	dgRedBackNode* ptr = m_parent;
+	for (; ptr && node == ptr->m_left; ptr = ptr->m_parent) {
+		node = ptr;
+	}
+	return ptr;
+
+}
+
+dgRedBackNode *dgRedBackNode::Next () const
+{
+
+	if (m_right) {
+		return m_right->Minimum ();
+	}
+
+	dgRedBackNode* node = (dgRedBackNode *)this;
+	dgRedBackNode* ptr = m_parent;
+	for (; ptr && node == ptr->m_right; ptr = ptr->m_parent) {
+		node = ptr;
+	}
+	return ptr;
+}
+
+// rotate node me to left 
+void dgRedBackNode::RotateLeft(dgRedBackNode** const head) 
+{
+	dgRedBackNode* const me = this;
+	dgRedBackNode* const child = me->m_right;
+	
+	//establish me->m_right link 
+	me->m_right = child->m_left;
+	if (child->m_left != NULL) {
+		child->m_left->m_parent = me;
+	}
+	
+	// establish child->m_parent link 
+	if (child != NULL) {
+		child->m_parent = me->m_parent;
+	}
+	if (me->m_parent) {
+		if (me == me->m_parent->m_left) {
+			me->m_parent->m_left = child;
+		} else {
+			me->m_parent->m_right = child;
+		}
+	} else {
+		*head = child;
+	}
+	
+	// link child and me 
+	dgAssert (child);
+	child->m_left = me;
+	if (me != NULL) {
+		me->m_parent = child;
+	}
+}
+
+
+// rotate node me to right  *
+void dgRedBackNode::RotateRight(dgRedBackNode ** const head) 
+{
+	dgRedBackNode* const me = this;
+	dgRedBackNode* const child = me->m_left;
+
+	// establish me->m_left link 
+	me->m_left = child->m_right;
+	if (child->m_right != NULL) {
+		child->m_right->m_parent = me;
+	}
+
+	// establish child->m_parent link 
+	if (child != NULL) {
+		child->m_parent = me->m_parent;
+	}
+	if (me->m_parent) {
+		if (me == me->m_parent->m_right) {
+			me->m_parent->m_right = child;
+		} else {
+			me->m_parent->m_left = child;
+		}
+	} else {
+		*head = child;
+	}
+
+	// link me and child 
+	dgAssert (child);
+	child->m_right = me;
+	if (me != NULL) {
+		me->m_parent = child;
+	}
+}
+
+
+// maintain Red-Black tree balance after inserting node ptr   
+void dgRedBackNode::InsertFixup(dgRedBackNode ** const head) 
+{
+	dgRedBackNode* ptr = this;
+	// check Red-Black properties 
+	while ((ptr != *head) && (ptr->m_parent->GetColor() == RED)) {
+		// we have a violation 
+		dgAssert (ptr->m_parent);
+		dgAssert (ptr->m_parent->m_parent);
+		if (ptr->m_parent == ptr->m_parent->m_parent->m_left) {
+			dgRedBackNode* const tmp = ptr->m_parent->m_parent->m_right;
+			if (tmp && (tmp->GetColor() == RED)) {
+				// uncle is RED 
+				ptr->m_parent->SetColor(BLACK);
+				tmp->SetColor(BLACK) ;
+				ptr->m_parent->m_parent->SetColor(RED) ;
+				ptr = ptr->m_parent->m_parent;
+			} else {
+				// uncle is BLACK 
+				if (ptr == ptr->m_parent->m_right) {
+					// make ptr a left child 
+					ptr = ptr->m_parent;
+					ptr->RotateLeft(head);
+				}
+
+				ptr->m_parent->SetColor(BLACK);
+				if (ptr->m_parent->m_parent) {
+					ptr->m_parent->m_parent->SetColor(RED);
+					ptr->m_parent->m_parent->RotateRight(head);
+				}
+			}
+		} else {
+			dgAssert (ptr->m_parent == ptr->m_parent->m_parent->m_right);
+			// mirror image of above code 
+			dgRedBackNode* const tmp = ptr->m_parent->m_parent->m_left;
+			if (tmp && (tmp->GetColor() == RED)) {
+				//uncle is RED 
+				ptr->m_parent->SetColor(BLACK);
+				tmp->SetColor(BLACK) ;
+				ptr->m_parent->m_parent->SetColor(RED) ;
+				ptr = ptr->m_parent->m_parent;
+			} else {
+				// uncle is BLACK 
+				if (ptr == ptr->m_parent->m_left) {
+					ptr = ptr->m_parent;
+					ptr->RotateRight(head);
+				}
+				ptr->m_parent->SetColor(BLACK);
+				if (ptr->m_parent->m_parent->GetColor() == BLACK) {
+					ptr->m_parent->m_parent->SetColor(RED) ;
+				   ptr->m_parent->m_parent->RotateLeft (head); 
+				}
+			}
+		}
+	}
+	(*head)->SetColor(BLACK);
+}
+
+
+//maintain Red-Black tree balance after deleting node x 
+void dgRedBackNode::RemoveFixup (dgRedBackNode* const thisNode, dgRedBackNode ** const head) 
+{
+	dgRedBackNode* ptr = this;
+	dgRedBackNode* node = thisNode;
+	while ((node != *head) && (!node || node->GetColor() == BLACK)) {
+		if (node == ptr->m_left) {
+			if (!ptr) {
+				return;
+			}
+			dgRedBackNode* tmp = ptr->m_right;
+			if (!tmp) {
+				return;
+			}
+			if (tmp->GetColor() == RED) {
+				tmp->SetColor(BLACK) ;
+				ptr->SetColor(RED) ;
+				ptr->RotateLeft (head);
+				tmp = ptr->m_right;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+			if ((!tmp->m_left  || (tmp->m_left->GetColor() == BLACK)) && 
+				 (!tmp->m_right || (tmp->m_right->GetColor() == BLACK))) {
+				tmp->SetColor(RED);
+				node = ptr;
+				ptr = ptr->m_parent;
+				continue;
+			} else if (!tmp->m_right || (tmp->m_right->GetColor() == BLACK)) {
+				tmp->m_left->SetColor(BLACK);
+				tmp->SetColor(RED);
+				tmp->RotateRight (head);
+				tmp = ptr->m_right;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+			tmp->SetColor (ptr->GetColor());
+			if (tmp->m_right) {
+				tmp->m_right->SetColor(BLACK) ;
+			}
+			if (ptr) {
+				ptr->SetColor(BLACK) ;
+				ptr->RotateLeft (head);
+			}
+			node = *head;
+
+		} else {
+		  	if (!ptr) {
+				return;
+		  	}
+			dgRedBackNode* tmp = ptr->m_left;
+			if (!tmp) {
+				return;
+			}
+			if (tmp->GetColor() == RED) {
+				tmp->SetColor(BLACK) ;
+				ptr->SetColor(RED) ;
+				ptr->RotateRight (head);
+				tmp = ptr->m_left;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+
+			if ((!tmp->m_right || (tmp->m_right->GetColor() == BLACK)) && 
+				 (!tmp->m_left  || (tmp->m_left->GetColor() == BLACK))) {
+				tmp->SetColor(RED) ;
+				node = ptr;
+				ptr = ptr->m_parent;
+				continue;
+			} else if (!tmp->m_left || (tmp->m_left->GetColor() == BLACK)) {
+				tmp->m_right->SetColor(BLACK) ;
+				tmp->SetColor(RED) ;
+				tmp->RotateLeft (head);
+				tmp = ptr->m_left;
+				//if (!ptr || !tmp) {
+				if (!tmp) {
+					return;
+				}
+			}
+			tmp->SetColor (ptr->GetColor());
+			if (tmp->m_left) {
+				tmp->m_left->SetColor(BLACK);
+			}
+			if (ptr) {
+				ptr->SetColor(BLACK) ;
+				ptr->RotateRight (head);
+			}
+			node = *head;
+		}
+	}
+	if (node) {
+		node->SetColor(BLACK);
+	}
+}
+
+void dgRedBackNode::Unlink (dgRedBackNode ** const head) 
+{
+//	dgRedBackNode *child;
+//	dgRedBackNode *endNode;
+//	dgRedBackNode *endNodeParent;
+//	dgRedBackNode::REDBLACK_COLOR oldColor;
+
+	dgRedBackNode* const node = this;
+//	node->Kill();
+	node->SetInTreeFlag(false);
+
+	if (!node->m_left || !node->m_right) {
+		// y has a NULL node as a child 
+		dgRedBackNode* const endNode = node;
+
+		// x is y's only child 
+		dgRedBackNode* child = endNode->m_right;
+		if (endNode->m_left) {
+			child = endNode->m_left;
+		}
+
+		// remove y from the parent chain 
+		if (child) {
+			child->m_parent = endNode->m_parent;
+		}
+
+		if (endNode->m_parent)	{
+			if (endNode == endNode->m_parent->m_left) {
+				endNode->m_parent->m_left = child;
+			} else {
+				endNode->m_parent->m_right = child;
+			}
+		} else {
+			*head = child;
+		}
+
+		if (endNode->GetColor() == BLACK) {
+			endNode->m_parent->RemoveFixup (child, head);
+		}
+//		endNode->Release();
+//		delete endNode;
+
+	} else {
+
+		// find tree successor with a NULL node as a child 
+		dgRedBackNode* endNode = node->m_right;
+		while (endNode->m_left != NULL) {
+			endNode = endNode->m_left;
+		}
+
+		dgAssert (endNode);
+		dgAssert (endNode->m_parent);
+		dgAssert (!endNode->m_left);
+
+		// x is y's only child 
+		dgRedBackNode* const child = endNode->m_right;
+
+		dgAssert ((endNode != node->m_right) || !child || (child->m_parent == endNode));
+
+		endNode->m_left = node->m_left;
+		node->m_left->m_parent = endNode;
+
+		dgRedBackNode* endNodeParent = endNode;
+		if (endNode != node->m_right) {
+			if (child) {
+				child->m_parent = endNode->m_parent;
+			}
+			endNode->m_parent->m_left = child;
+			endNode->m_right = node->m_right;
+			node->m_right->m_parent = endNode;
+			endNodeParent = endNode->m_parent;
+		}
+
+
+		if (node == *head) {
+			*head = endNode;
+		} else if (node == node->m_parent->m_left) {
+			node->m_parent->m_left = endNode;
+		} else {
+			node->m_parent->m_right = endNode;
+		}
+		endNode->m_parent = node->m_parent;
+
+		dgRedBackNode::REDBLACK_COLOR oldColor = endNode->GetColor();
+		endNode->SetColor (node->GetColor());
+		node->SetColor	(oldColor);
+
+		if (oldColor == BLACK) {
+			endNodeParent->RemoveFixup (child, head);
+		}
+//		node->Release();
+//		delete node;
+	}
+}
+
+void dgRedBackNode::Remove (dgRedBackNode ** const head) 
+{
+	Unlink (head);
+	delete this;	
+}
+
+void dgRedBackNode::RemoveAllLow ()
+{
+	if (m_left) {
+		m_left->RemoveAllLow();
+	}
+	if (m_right) {
+		m_right->RemoveAllLow ();
+	}
+	SetInTreeFlag(false);
+//	Kill();
+//	Release();
+	delete this;
+}
+
+void dgRedBackNode::RemoveAll ()
+{
+	dgRedBackNode* root = this;
+	for (; root->m_parent; root = root->m_parent);
+	root->RemoveAllLow();
+}
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgTree.h

@@ -0,0 +1,848 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgTree__
+#define __dgTree__
+
+#include "dgStdafx.h"
+#include "dgRef.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+
+
+// Note: this is a low level class for dgTree use only
+// unpredictable result will happen if you attempt to manipulate
+// any member of this class
+class dgRedBackNode
+{
+	public:
+	enum REDBLACK_COLOR
+	{
+		RED = true,
+		BLACK = false
+	};
+
+	DG_CLASS_ALLOCATOR(allocator)
+
+	dgRedBackNode()
+	{
+	}
+
+	virtual ~dgRedBackNode () 
+	{
+	}
+
+	void RemoveAllLow ();
+	void RotateLeft(dgRedBackNode** const head); 
+	void RotateRight(dgRedBackNode** const head); 
+	void RemoveFixup (dgRedBackNode* const node, dgRedBackNode* * const head); 
+
+	dgRedBackNode* GetLeft() const;
+	dgRedBackNode* GetRight() const;
+	dgRedBackNode* GetParent() const;
+
+	dgRedBackNode (dgRedBackNode* const parent);
+	inline void Initdata (dgRedBackNode* const parent);
+	inline void SetColor (REDBLACK_COLOR color);
+	REDBLACK_COLOR GetColor () const;
+	dgUnsigned32 IsInTree () const;
+	inline void SetInTreeFlag (dgUnsigned32 flag);
+
+	void RemoveAll ();
+	dgRedBackNode* Prev() const;
+	dgRedBackNode* Next() const;
+	dgRedBackNode* Minimum() const;
+	dgRedBackNode* Maximum() const;
+	void Remove (dgRedBackNode** const head);
+	void Unlink (dgRedBackNode** const head);
+	void InsertFixup(dgRedBackNode** const head); 
+
+	dgRedBackNode* m_left;
+	dgRedBackNode* m_right;
+	dgRedBackNode* m_parent;
+	dgUnsigned32  m_color	: 1;
+	dgUnsigned32  m_inTree	: 1;
+//	dgUnsigned32  m_pad[2];
+};
+
+template<class OBJECT, class KEY>
+class dgTree 
+{
+	public:
+	class dgTreeNode: public dgRedBackNode
+	{
+		dgTreeNode (
+			const OBJECT &info, 
+			const KEY &key, 
+			dgTreeNode* parentNode)
+			:dgRedBackNode(parentNode), m_info (info), m_key (key)
+		{
+//			dgAssert ((dgUnsigned64 (&m_info) & 0x0f) == 0);
+		}
+
+		~dgTreeNode () 
+		{
+		}
+
+		dgTreeNode* GetLeft () const
+		{
+			return (dgTreeNode* )dgRedBackNode::m_left;
+		}
+
+		dgTreeNode* GetRight () const
+		{
+			return (dgTreeNode* )dgRedBackNode::m_right;
+		}
+
+		dgTreeNode* GetParent ()
+		{
+			return (dgTreeNode* )dgRedBackNode::m_parent;
+		}
+
+		void SetLeft (dgTreeNode* const node)
+		{
+			dgRedBackNode::m_left = node;
+		}
+
+		void SetRight (dgTreeNode* const node)
+		{
+			dgRedBackNode::m_right = node;
+		}
+
+		void SetParent (dgTreeNode* const node)
+		{
+			dgRedBackNode::m_parent = node;
+		}
+
+		public:
+		const KEY& GetKey() const
+		{
+			return m_key;
+		}
+
+		OBJECT& GetInfo()
+		{
+			return m_info;
+		}
+
+		private:
+		OBJECT m_info;
+		KEY m_key; 
+		friend class dgTree<OBJECT, KEY>;
+
+	};
+
+	class Iterator
+	{
+
+		public:
+		Iterator(const dgTree<OBJECT,KEY> &me)
+		{
+			m_ptr = NULL;
+			m_tree = &me;
+		}
+
+		~Iterator()
+		{
+		}
+
+		void Begin() 
+		{
+			m_ptr = m_tree->Minimum();
+		}
+
+		void End()  
+		{
+			m_ptr = m_tree->Maximum();
+		}
+
+		void Set (dgTreeNode* const node)
+		{
+			m_ptr = node;
+		}
+
+		operator dgInt32() const 
+		{
+			return m_ptr != NULL;
+		}
+
+		void operator++ ()
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->Next();
+		}
+
+		void operator++ (dgInt32)
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->Next();
+		}
+
+		void operator-- () 
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->Prev();
+		}
+
+		void operator-- (dgInt32) 
+		{
+			dgAssert (m_ptr);
+			m_ptr = m_ptr->Prev();
+		}
+
+		OBJECT &operator* () const 
+		{
+			return ((dgTreeNode*)m_ptr)->GetInfo();
+		}
+
+		dgTreeNode* GetNode() const
+		{
+			return (dgTreeNode*)m_ptr;
+		}
+
+		KEY GetKey () const
+		{
+			dgTreeNode* const tmp = (dgTreeNode*)m_ptr;
+			return tmp ? tmp->GetKey() : KEY(0);
+		}
+
+		private:
+		dgRedBackNode* m_ptr;
+		const dgTree* m_tree;
+
+	};
+
+
+	// ***********************************************************
+	// member functions
+	// ***********************************************************
+	public:
+	DG_CLASS_ALLOCATOR(allocator)
+
+//	dgTree ();
+	dgTree (dgMemoryAllocator* const allocator);
+	virtual ~dgTree (); 
+
+	dgMemoryAllocator* GetAllocator () const;
+	void SetAllocator (dgMemoryAllocator* const allocator);
+	
+
+	operator dgInt32() const;
+	dgInt32 GetCount() const;
+
+	dgTreeNode* GetRoot () const;
+	dgTreeNode* Minimum () const;
+	dgTreeNode* Maximum () const;
+
+	dgTreeNode* Find (KEY key) const;
+	dgTreeNode* FindGreater (KEY key) const;
+	dgTreeNode* FindGreaterEqual (KEY key) const;
+	dgTreeNode* FindLessEqual (KEY key) const;
+
+	dgTreeNode* GetNodeFromInfo (OBJECT &info) const;
+
+	dgTreeNode* Insert (const OBJECT &element, KEY key, bool& elementWasInTree);
+	dgTreeNode* Insert (const OBJECT &element, KEY key);
+	dgTreeNode* Insert (dgTreeNode* const node, KEY key);
+
+	dgTreeNode* Replace (OBJECT &element, KEY key);
+	dgTreeNode* ReplaceKey (KEY oldKey, KEY newKey);
+	dgTreeNode* ReplaceKey (dgTreeNode* const node, KEY key);
+
+	void Remove (KEY key);
+	void Remove (dgTreeNode* const node);
+	void RemoveAll (); 
+
+	void Unlink (dgTreeNode* const node);
+	void SwapInfo (dgTree& tree);
+
+	bool SanityCheck () const;
+
+	// ***********************************************************
+	// member variables
+	// ***********************************************************
+	private:
+	dgInt32 m_count;
+	dgTreeNode* m_head;
+	dgMemoryAllocator* m_allocator;
+
+	dgInt32 CompareKeys (const KEY &key0, const KEY &key1) const;
+	bool SanityCheck (dgTreeNode* const ptr, dgInt32 height) const;
+
+	friend class dgTreeNode;
+};
+
+
+inline dgRedBackNode::dgRedBackNode (dgRedBackNode* const parent)
+{
+	Initdata (parent);
+}
+
+inline void dgRedBackNode::Initdata (dgRedBackNode* const parent)
+{
+	SetColor (RED);
+	SetInTreeFlag (true);
+	m_left = NULL;
+	m_right = NULL;
+	m_parent = parent;
+}
+
+inline void dgRedBackNode::SetColor (dgRedBackNode::REDBLACK_COLOR color)
+{
+	m_color = color;
+}
+
+
+
+inline dgRedBackNode::REDBLACK_COLOR  dgRedBackNode::GetColor () const
+{
+	return REDBLACK_COLOR (m_color);
+}
+
+
+inline void dgRedBackNode::SetInTreeFlag (dgUnsigned32 flag)
+{
+	m_inTree = flag;
+}
+
+inline dgUnsigned32 dgRedBackNode::IsInTree () const
+{
+	return m_inTree;
+}
+
+/*
+template<class OBJECT, class KEY>
+dgTree<OBJECT, KEY>::dgTree ()
+{
+	m_count	= 0;
+	m_head = NULL;
+	m_allocator = NULL;
+}
+*/
+
+template<class OBJECT, class KEY>
+dgTree<OBJECT, KEY>::dgTree (dgMemoryAllocator* const allocator)
+	:m_count(0)
+	,m_head(NULL)
+	,m_allocator(allocator)
+{
+}
+
+
+template<class OBJECT, class KEY>
+dgTree<OBJECT, KEY>::~dgTree () 
+{
+	RemoveAll();
+}
+
+template<class OBJECT, class KEY>
+dgMemoryAllocator* dgTree<OBJECT, KEY>::GetAllocator () const
+{
+	return m_allocator;
+}
+
+template<class OBJECT, class KEY>
+void dgTree<OBJECT, KEY>::SetAllocator (dgMemoryAllocator* const allocator)
+{
+	if ((m_count == 0) && (m_allocator == NULL)) {
+		m_allocator = allocator;
+	}
+}
+
+
+template<class OBJECT, class KEY>
+dgTree<OBJECT, KEY>::operator dgInt32() const
+{
+	return m_head != NULL;
+}
+
+template<class OBJECT, class KEY>
+dgInt32 dgTree<OBJECT, KEY>::GetCount() const
+{
+	return m_count;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::Minimum () const
+{
+	return m_head ? (dgTreeNode* )m_head->Minimum() : NULL;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::Maximum () const
+{
+	return m_head ? (dgTreeNode* )m_head->Maximum() : NULL;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::GetRoot () const
+{
+	return m_head;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::Find (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dgTreeNode* ptr = m_head;
+	while (ptr != NULL) {
+		if (key < ptr->m_key) {
+			dgAssert (CompareKeys (ptr->m_key, key) == -1) ;
+			ptr = ptr->GetLeft();
+		} else if (key > ptr->m_key) {
+			dgAssert (CompareKeys (ptr->m_key, key) == 1) ;
+			ptr = ptr->GetRight();
+		} else {
+			dgAssert (CompareKeys (ptr->m_key, key) == 0) ;
+			break;
+		}
+	}
+	return ptr;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::GetNodeFromInfo (OBJECT &info) const
+{
+	dgTreeNode* const node = (dgTreeNode* ) &info;
+	dgInt64 offset = ((char*) &node->m_info) - ((char *) node);
+	dgTreeNode* const retnode = (dgTreeNode* ) (((char *) node) - offset);
+
+	dgAssert (retnode->IsInTree ());
+	dgAssert (&retnode->GetInfo () == &info);
+	return (retnode->IsInTree ()) ? retnode : NULL;
+
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::FindGreater (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dgTreeNode* prev = NULL;
+	dgTreeNode* ptr = m_head;
+
+	while (ptr != NULL) {
+		if (key < ptr->m_key) {
+		prev = ptr;
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
+	if (prev) {
+		Iterator iter (*this);
+		for (iter.Begin(); iter.GetNode() != prev; iter ++) {
+			KEY key1 = iter.GetKey(); 
+			dgAssert (key1 <= key);
+		}
+		for (; iter.GetNode(); iter ++) {
+			KEY key1 = iter.GetKey(); 
+			dgAssert (key1 > key);
+		}
+	}
+#endif
+
+	return (dgTreeNode* )prev; 
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::FindGreaterEqual (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dgTreeNode* prev = NULL;
+	dgTreeNode* ptr = m_head;
+	
+	while (ptr != NULL) {
+		if (key == ptr->m_key) {
+			return ptr;
+		}
+		if (key < ptr->m_key) {
+		prev = ptr;
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
+	if (prev) {
+		Iterator iter (*this);
+		for (iter.Begin(); iter.GetNode() != prev; iter ++) {
+			KEY key1 = iter.GetKey(); 
+			dgAssert (key1 <= key);
+		}
+		for (; iter.GetNode(); iter ++) {
+			KEY key1 = iter.GetKey(); 
+			dgAssert (key1 >= key);
+		}
+	}
+#endif
+
+	return (dgTreeNode* )prev; 
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::FindLessEqual (KEY key) const
+{
+	if (m_head == NULL) {
+		return NULL;
+	}
+
+	dgTreeNode* prev = NULL;
+	dgTreeNode* ptr = m_head;
+
+	while (ptr != NULL) {
+		if (key == ptr->m_key) {
+			return ptr;
+		}
+
+		if (key < ptr->m_key) {
+			ptr = ptr->GetLeft();
+		} else {
+			prev = ptr;
+			ptr = ptr->GetRight();
+		}
+
+	}
+
+#ifdef __ENABLE_DG_CONTAINERS_SANITY_CHECK
+	if (prev) {
+		Iterator iter (*this);
+		for (iter.End(); iter.GetNode() != prev; iter --) {
+			KEY key1 = iter.GetKey(); 
+			dgAssert (key1 >= key);
+		}
+		for (; iter.GetNode(); iter --) {
+			KEY key1 = iter.GetKey(); 
+			dgAssert (key1 < key);
+		}
+	}
+#endif
+
+	return (dgTreeNode* )prev; 
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::Insert (const OBJECT &element, KEY key, bool& elementWasInTree)
+{
+	dgTreeNode* parent = NULL;
+	dgTreeNode* ptr = m_head;
+	dgInt32 val = 0;
+	elementWasInTree = false;
+	while (ptr != NULL) {
+		parent = ptr;
+
+		if (key < ptr->m_key) {
+			dgAssert (CompareKeys (ptr->m_key, key) == -1) ;
+			val = -1;
+			ptr = ptr->GetLeft();
+		} else if (key > ptr->m_key) {
+			dgAssert (CompareKeys (ptr->m_key, key) == 1) ;
+			val = 1;
+			ptr = ptr->GetRight();
+		} else {
+			dgAssert (CompareKeys (ptr->m_key, key) == 0) ;
+			elementWasInTree = true;
+			return ptr;
+		}
+	}
+
+	m_count	++;
+	dgAssert (m_allocator);
+	ptr = new (m_allocator) dgTreeNode (element, key, parent);
+	if (!parent) {
+		m_head = ptr;
+	} else {
+		if (val < 0) {
+			parent->m_left = ptr; 
+		} else {
+			parent->m_right = ptr;
+		}
+	}
+
+	dgTreeNode** const headPtr = (dgTreeNode**) &m_head;
+//	ptr->InsertFixup ((dgRedBackNode**)&m_head);
+	ptr->InsertFixup ((dgRedBackNode**)headPtr);
+	return ptr;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::Insert (const OBJECT &element, KEY key)
+{
+	bool foundState;
+
+	dgTreeNode* const node = Insert (element, key, foundState);
+	if (foundState) {
+		return NULL;
+	}
+	return node;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::Insert (typename dgTree<OBJECT, KEY>::dgTreeNode* const node, KEY key)
+{
+	dgInt32 val = 0;
+	dgTreeNode* ptr = m_head;
+	dgTreeNode* parent = NULL;
+	while (ptr != NULL) {
+		parent = ptr;
+//		val = CompareKeys (ptr->m_key, key);
+//		if (val < 0) {
+//			ptr = ptr->GetLeft();
+//		} else if (val > 0) {
+//			ptr = ptr->GetRight();
+//		} else {
+//			return NULL;
+//		}
+
+		if (key < ptr->m_key) {
+			dgAssert (CompareKeys (ptr->m_key, key) == -1) ;
+			val = -1;
+			ptr = ptr->GetLeft();
+		} else if (key > ptr->m_key) {
+			dgAssert (CompareKeys (ptr->m_key, key) == 1) ;
+			val = 1;
+			ptr = ptr->GetRight();
+		} else {
+			dgAssert (CompareKeys (ptr->m_key, key) == 0) ;
+			return NULL;
+		}
+	}
+
+	m_count	++;
+
+	ptr = node;
+	ptr->m_key = key;
+	ptr->Initdata (parent);
+
+	if (!parent) {
+		m_head = ptr;
+	} else {
+		if (val < 0) {
+			parent->m_left = ptr; 
+		} else {
+			parent->m_right = ptr;
+		}
+	}
+
+	dgTreeNode** const headPtr = (dgTreeNode**) &m_head;
+//	ptr->InsertFixup ((dgRedBackNode**)&m_head);
+	ptr->InsertFixup ((dgRedBackNode**)headPtr);
+	return ptr;
+}
+
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::Replace (OBJECT &element, KEY key)
+{
+	dgTreeNode* parent = NULL;
+	dgTreeNode* ptr = m_head;
+	dgInt32 val = 0;
+	while (ptr != NULL) {
+		parent = ptr;
+
+		dgAssert (0);
+		val = CompareKeys (ptr->m_key, key);
+		if (val == 0) {
+			ptr->m_info = element;
+			return ptr;
+		}
+		if (val < 0) {
+			ptr = ptr->GetLeft();
+		} else {
+			ptr = ptr->GetRight();
+		}
+	}
+
+	dgAssert (m_allocator);
+	ptr = new (m_allocator) dgTreeNode (element, key, parent);
+	if (!parent) {
+		m_head = ptr;
+	} else {
+		if (val < 0) {
+			parent->m_left = ptr; 
+		} else {
+			parent->m_right = ptr;
+		}
+	}
+
+	dgTreeNode** const headPtr = (dgTreeNode**) &m_head;
+//	ptr->InsertFixup ((dgRedBackNode**)&m_head);
+	ptr->InsertFixup ((dgRedBackNode**)headPtr );
+	return ptr;
+}
+
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::ReplaceKey (typename dgTree<OBJECT, KEY>::dgTreeNode* const node, KEY key)
+{
+	Unlink (node);
+	dgTreeNode* const ptr = Insert (node, key);
+	dgAssert (ptr);
+	return ptr;
+}
+
+template<class OBJECT, class KEY>
+typename dgTree<OBJECT, KEY>::dgTreeNode* dgTree<OBJECT, KEY>::ReplaceKey (KEY oldKey, KEY newKey)
+{
+	dgTreeNode* const node = Find (oldKey);
+	return node ? ReplaceKey (node, newKey) : NULL;
+}
+
+template<class OBJECT, class KEY>
+void dgTree<OBJECT, KEY>::Unlink (typename dgTree<OBJECT, KEY>::dgTreeNode* const node)
+{
+	m_count	--;
+
+	dgTreeNode** const headPtr = (dgTreeNode**) &m_head;
+	node->Unlink ((dgRedBackNode**)headPtr);
+	dgAssert (!Find (node->GetKey()));
+}
+
+
+template<class OBJECT, class KEY>
+void dgTree<OBJECT, KEY>::Remove (typename dgTree<OBJECT, KEY>::dgTreeNode* const node)
+{
+	m_count	--;
+	dgTreeNode** const headPtr = (dgTreeNode**) &m_head;
+	node->Remove ((dgRedBackNode**)headPtr);
+}
+
+template<class OBJECT, class KEY>
+void dgTree<OBJECT, KEY>::Remove (KEY key) 
+{
+	dgTreeNode* node;
+
+	// find node in tree 
+	node = Find (key);
+	if (node == NULL) {
+		return;
+	}
+	Remove (node);
+}
+
+template<class OBJECT, class KEY>
+void dgTree<OBJECT, KEY>::RemoveAll () 
+{
+	if (m_head) {
+		m_count	 = 0;
+		m_head->RemoveAll ();
+		m_head = NULL;
+	}
+}
+
+template<class OBJECT, class KEY>
+bool dgTree<OBJECT, KEY>::SanityCheck () const
+{
+	return SanityCheck (m_head, 0);
+}
+
+
+template<class OBJECT, class KEY>
+bool dgTree<OBJECT, KEY>::SanityCheck (typename dgTree<OBJECT, KEY>::dgTreeNode* const ptr, dgInt32 height) const
+{
+	if (!ptr) {
+		return true;
+	}
+
+	if (!ptr->IsInTree()) {
+		return false;
+	}
+
+	if (ptr->m_left) {
+		if (CompareKeys (ptr->m_key, ptr->GetLeft()->m_key) > 0) {
+			return false;
+		}
+	}
+
+	if (ptr->m_right) {
+		if (CompareKeys (ptr->m_key, ptr->GetRight()->m_key) < 0) {
+			return false;
+		}
+	}
+
+	if (ptr->GetColor() == dgTreeNode::BLACK) {
+		height ++;
+	} else if (!((!ptr->m_left  || (ptr->m_left->GetColor() == dgTreeNode::BLACK)) &&
+			       (!ptr->m_right || (ptr->m_right->GetColor() == dgTreeNode::BLACK)))) {
+	  	return false;
+	}
+
+	if (!ptr->m_left && !ptr->m_right) {
+		dgInt32 bh = 0;
+		for (dgTreeNode* x = ptr; x; x = x->GetParent()) {
+	 		if (x->GetColor() == dgTreeNode::BLACK) {
+				bh ++;
+			}
+		}
+		if (bh != height) {
+			return false;
+		}
+	}
+
+	if (ptr->m_left && !SanityCheck (ptr->GetLeft(), height)) {
+		return false;
+	}
+
+	if (ptr->m_right && !SanityCheck (ptr->GetRight(), height)) {
+		return false;
+	}
+	return true;
+}
+
+
+template<class OBJECT, class KEY>
+dgInt32 dgTree<OBJECT, KEY>::CompareKeys (const KEY &key0, const KEY &key1) const
+{
+	if (key1 < key0) {
+		return - 1;
+	}
+	if (key1 > key0) {
+		return 1;
+	}
+	return 0;
+}
+
+template<class OBJECT, class KEY>
+void dgTree<OBJECT, KEY>::SwapInfo (dgTree<OBJECT, KEY>& tree)
+{
+	dgSwap (m_head, tree.m_head);
+	dgSwap (m_count, tree.m_count);
+}
+
+//template<class OBJECT, class KEY> dgInt32 dgTree<OBJECT,KEY>::m_size = 0;
+//template<class OBJECT, class KEY> dgMemoryAllocator* dgTree<OBJECT,KEY>::m_staticAllocator = NULL;
+
+
+#endif
+
+

newtondynamics.mod/NewtonDynamics/source/core/dgTypes.cpp

@@ -0,0 +1,519 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgStdafx.h"
+#include "dgTypes.h"
+#include "dgDebug.h"
+#include "dgVector.h"
+#include "dgMemory.h"
+#include "dgStack.h"
+
+
+
+
+dgUnsigned64 dgGetTimeInMicrosenconds()
+{
+
+#ifdef _MSC_VER
+	static LARGE_INTEGER frequency;
+	static LARGE_INTEGER baseCount;
+	if (!frequency.QuadPart) {
+		QueryPerformanceFrequency(&frequency);
+		QueryPerformanceCounter (&baseCount);
+	}
+
+
+	LARGE_INTEGER count;
+	QueryPerformanceCounter (&count);
+	count.QuadPart -= baseCount.QuadPart;
+	dgUnsigned64 ticks = dgUnsigned64 (count.QuadPart * LONGLONG (1000000) / frequency.QuadPart);
+	return ticks;
+#endif
+
+#if (defined (_POSIX_VER) || defined (_POSIX_VER_64))
+	timespec ts;
+	static dgUnsigned64 baseCount = 0;
+	if (!baseCount) {
+		clock_gettime(CLOCK_REALTIME, &ts);
+		baseCount = dgUnsigned64 (ts.tv_sec) * 1000000 + ts.tv_nsec / 1000;
+	}
+	clock_gettime(CLOCK_REALTIME, &ts); // Works on Linux
+	return dgUnsigned64 (ts.tv_sec) * 1000000 + ts.tv_nsec / 1000 - baseCount;
+#endif
+
+
+#ifdef _MACOSX_VER
+	timeval tp;
+	static dgUnsigned64 baseCount = 0;
+	if (!baseCount) {
+		gettimeofday(&tp, NULL);
+		baseCount = dgUnsigned64 (tp.tv_sec) * 1000000 + tp.tv_usec;
+	}
+
+	gettimeofday(&tp, NULL);
+	dgUnsigned64 microsecunds = dgUnsigned64 (tp.tv_sec) * 1000000 + tp.tv_usec;
+	return microsecunds - baseCount;
+#endif
+}
+
+
+
+void GetMinMax (dgVector &minOut, dgVector &maxOut, const dgFloat32* const vertexArray, dgInt32 vCount, dgInt32 strideInBytes)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
+	const dgFloat32* vArray = vertexArray + stride;
+
+	dgAssert (stride >= 3);
+ 	minOut = dgVector (vertexArray[0], vertexArray[1], vertexArray[2], dgFloat32 (0.0f)); 
+	maxOut = dgVector (vertexArray[0], vertexArray[1], vertexArray[2], dgFloat32 (0.0f)); 
+
+	for (dgInt32 i = 1; i < vCount; i ++) {
+		minOut.m_x = dgMin (minOut.m_x, vArray[0]);
+		minOut.m_y = dgMin (minOut.m_y, vArray[1]);
+		minOut.m_z = dgMin (minOut.m_z, vArray[2]);
+
+		maxOut.m_x = dgMax (maxOut.m_x, vArray[0]);
+		maxOut.m_y = dgMax (maxOut.m_y, vArray[1]);
+		maxOut.m_z = dgMax (maxOut.m_z, vArray[2]);
+
+		vArray += stride;
+	}
+}
+
+
+void GetMinMax (dgBigVector &minOut, dgBigVector &maxOut, const dgFloat64* const vertexArray, dgInt32 vCount, dgInt32 strideInBytes)
+{
+	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat64));
+	const dgFloat64* vArray = vertexArray + stride;
+
+	dgAssert (stride >= 3);
+	minOut = dgBigVector (vertexArray[0], vertexArray[1], vertexArray[2], dgFloat64 (0.0f)); 
+	maxOut = dgBigVector (vertexArray[0], vertexArray[1], vertexArray[2], dgFloat64 (0.0f)); 
+
+	for (dgInt32 i = 1; i < vCount; i ++) {
+		minOut.m_x = dgMin (minOut.m_x, vArray[0]);
+		minOut.m_y = dgMin (minOut.m_y, vArray[1]);
+		minOut.m_z = dgMin (minOut.m_z, vArray[2]);
+
+		maxOut.m_x = dgMax (maxOut.m_x, vArray[0]);
+		maxOut.m_y = dgMax (maxOut.m_y, vArray[1]);
+		maxOut.m_z = dgMax (maxOut.m_z, vArray[2]);
+
+		vArray += stride;
+	}
+}
+
+
+
+static inline dgInt32 cmp_vertex (const dgFloat64* const v1, const dgFloat64* const v2, dgInt32 firstSortAxis)
+{
+	if (v1[firstSortAxis] < v2[firstSortAxis]) {
+		return -1;
+	}
+
+	if (v1[firstSortAxis] > v2[firstSortAxis]){
+		return 1;
+	}
+
+	return 0;
+}
+
+static dgInt32 SortVertices (dgFloat64* const vertexList,  dgInt32 stride, dgInt32 compareCount, dgInt32 vertexCount, dgFloat64 tolerance)
+{
+	dgFloat64 xc = dgFloat64 (0.0f);
+	dgFloat64 yc = dgFloat64 (0.0f);
+	dgFloat64 zc = dgFloat64 (0.0f);
+	dgFloat64 x2c = dgFloat64 (0.0f);
+	dgFloat64 y2c = dgFloat64 (0.0f);
+	dgFloat64 z2c = dgFloat64 (0.0f);
+
+	dgBigVector minP (dgFloat64 (1.0e10f), dgFloat64 (1.0e10f), dgFloat64 (1.0e10f), dgFloat64 (0.0f));
+	dgBigVector maxP (dgFloat64 (-1.0e10f), dgFloat64 (-1.0e10f), dgFloat64 (-1.0e10f), dgFloat64 (0.0f));
+	//dgInt32 k = 0;
+	for (dgInt32 k = 0, i = 0; i < vertexCount; i ++) {
+		dgFloat64 x  = vertexList[k + 2];
+		dgFloat64 y  = vertexList[k + 3];
+		dgFloat64 z  = vertexList[k + 4];
+		k += stride;
+
+		xc += x;
+		yc += y;
+		zc += z;
+		x2c += x * x;
+		y2c += y * y; 
+		z2c += z * z;
+
+		if (x < minP.m_x) {
+			minP.m_x = x; 
+		}
+		if (y < minP.m_y) {
+			minP.m_y = y; 
+		}
+
+		if (z < minP.m_z) {
+			minP.m_z = z; 
+		}
+
+		if (x > maxP.m_x) {
+			maxP.m_x = x; 
+		}
+		if (y > maxP.m_y) {
+			maxP.m_y = y; 
+		}
+
+		if (z > maxP.m_z) {
+			maxP.m_z = z; 
+		}
+	}
+
+	dgBigVector del (maxP - minP);
+	dgFloat64 minDist = dgMin (del.m_x, del.m_y, del.m_z);
+	if (minDist < dgFloat64 (1.0e-3f)) {
+		minDist = dgFloat64 (1.0e-3f);
+	}
+
+	dgFloat64 tol = tolerance * minDist + dgFloat64 (1.0e-12f);
+	dgFloat64 sweptWindow = dgFloat64 (2.0f) * tol;
+	sweptWindow += dgFloat64 (1.0e-4f);
+
+	x2c = vertexCount * x2c - xc * xc;
+	y2c = vertexCount * y2c - yc * yc;
+	z2c = vertexCount * z2c - zc * zc;
+
+	dgInt32 firstSortAxis = 2;
+	if ((y2c >= x2c) && (y2c >= z2c)) {
+		firstSortAxis = 3;
+	} else if ((z2c >= x2c) && (z2c >= y2c)) {
+		firstSortAxis = 4;
+	}
+
+
+	dgInt32 stack[1024][2];
+	stack[0][0] = 0;
+	stack[0][1] = vertexCount - 1;
+	dgInt32 stackIndex = 1;
+	while (stackIndex) {
+		stackIndex --;
+		dgInt32 lo = stack[stackIndex][0];
+		dgInt32 hi = stack[stackIndex][1];
+		if ((hi - lo) > 8) {
+			dgInt32 i = lo;
+			dgInt32 j = hi;
+			dgFloat64 val[64]; 
+			memcpy (val, &vertexList[((lo + hi) >> 1) * stride], stride * sizeof (dgFloat64));
+			do {    
+				while (cmp_vertex (&vertexList[i * stride], val, firstSortAxis) < 0) i ++;
+				while (cmp_vertex (&vertexList[j * stride], val, firstSortAxis) > 0) j --;
+
+				if (i <= j)	{
+					if (i < j) {
+						dgFloat64 tmp[64]; 
+						memcpy (tmp, &vertexList[i * stride], stride * sizeof (dgFloat64));
+						memcpy (&vertexList[i * stride], &vertexList[j * stride], stride * sizeof (dgFloat64)); 
+						memcpy (&vertexList[j * stride], tmp, stride * sizeof (dgFloat64)); 
+					}
+					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]))));
+		} else {
+			for (dgInt32 i = lo + 1; i <= hi ; i++) {
+				dgFloat64 tmp[64]; 
+				memcpy (tmp, &vertexList[i * stride], stride * sizeof (dgFloat64));
+
+				dgInt32 j = i;
+				for (; j && (cmp_vertex (&vertexList[(j - 1) * stride], tmp, firstSortAxis) > 0); j --) {
+					memcpy (&vertexList[j * stride], &vertexList[(j - 1)* stride], stride * sizeof (dgFloat64));
+				}
+				memcpy (&vertexList[j * stride], tmp, stride * sizeof (dgFloat64)); 
+			}
+		}
+	}
+
+
+#ifdef _DEBUG
+	for (dgInt32 i = 0; i < (vertexCount - 1); i ++) {
+		dgAssert (cmp_vertex (&vertexList[i * stride], &vertexList[(i + 1) * stride], firstSortAxis) <= 0);
+	}
+#endif
+
+	dgInt32 count = 0;
+	for (dgInt32 i = 0; i < vertexCount; i ++) {
+		dgInt32 m = i * stride;
+		dgInt32 index = dgInt32 (vertexList[m + 0]);
+		if (index == dgInt32 (0xffffffff)) {
+			dgFloat64 swept = vertexList[m + firstSortAxis] + sweptWindow;
+			dgInt32 k = i * stride + stride;
+			for (dgInt32 i1 = i + 1; i1 < vertexCount; i1 ++) {
+
+				index = dgInt32 (vertexList[k + 0]);
+				if (index == dgInt32 (0xffffffff)) {
+					dgFloat64 val = vertexList[k + firstSortAxis];
+					if (val >= swept) {
+						break;
+					}
+					bool test = true;
+					for (dgInt32 t = 0; test && (t < compareCount); t ++) {
+						val = fabs (vertexList[m + t + 2] - vertexList[k + t + 2]);
+						test = test && (val <= tol);
+					}
+					if (test) {
+						vertexList[k + 0] = dgFloat64 (count);
+					}
+				}
+				k += stride;
+			}
+
+			dgAssert (&vertexList[count * stride + 2] <= &vertexList[m + 2]);
+			if (&vertexList[count * stride + 2] < &vertexList[m + 2]) {
+				memcpy (&vertexList[count * stride + 2], &vertexList[m + 2], (stride - 2) * sizeof (dgFloat64));
+			}
+			vertexList[m + 0] = dgFloat64 (count);
+			count ++;
+		}
+	}
+
+	return count;
+}
+
+
+
+
+static dgInt32 QuickSortVertices (dgFloat64* const vertList, dgInt32 stride, dgInt32 compareCount, dgInt32 vertexCount, dgFloat64 tolerance)
+{
+	dgInt32 count = 0;
+	if (vertexCount > (1024 * 256)) {
+		dgFloat64 x = dgFloat32 (0.0f);
+		dgFloat64 y = dgFloat32 (0.0f);
+		dgFloat64 z = dgFloat32 (0.0f);
+		dgFloat64 xd = dgFloat32 (0.0f);
+		dgFloat64 yd = dgFloat32 (0.0f);
+		dgFloat64 zd = dgFloat32 (0.0f);
+
+		for (dgInt32 i = 0; i < vertexCount; i ++) {
+			dgFloat64 x0 = vertList[i * stride + 2];
+			dgFloat64 y0 = vertList[i * stride + 3];
+			dgFloat64 z0 = vertList[i * stride + 4];
+			x += x0;
+			y += y0;
+			z += z0;
+			xd += x0 * x0;
+			yd += y0 * y0;
+			zd += z0 * z0;
+		}
+
+		xd = vertexCount * xd - x * x;
+		yd = vertexCount * yd - y * y;
+		zd = vertexCount * zd - z * z;
+
+		dgInt32 axis = 2;
+		dgFloat64 axisVal = x / vertexCount;
+		if ((yd > xd) && (yd > zd)) {
+			axis = 3;
+			axisVal = y / vertexCount;
+		}
+		if ((zd > xd) && (zd > yd)) {
+			axis = 4;
+			axisVal = z / vertexCount;
+		}
+
+		dgInt32 i0 = 0;
+		dgInt32 i1 = vertexCount - 1;
+		do {    
+			for ( ;vertList[i0 * stride + axis] < axisVal; i0 ++); 
+			for ( ;vertList[i1 * stride + axis] > axisVal; i1 --);
+			if (i0 <= i1) {
+				for (dgInt32 i = 0; i < stride; i ++) {
+					dgSwap (vertList[i0 * stride + i], vertList[i1 * stride + i]);
+				}
+				i0 ++; 
+				i1 --;
+			}
+		} while (i0 <= i1);
+		dgAssert (i0 < vertexCount);
+
+		dgInt32 count0 = QuickSortVertices (&vertList[ 0 * stride], stride, compareCount, i0, tolerance);
+		dgInt32 count1 = QuickSortVertices (&vertList[i0 * stride], stride, compareCount, vertexCount - i0, tolerance);
+
+		count = count0 + count1;
+
+		for (dgInt32 i = 0; i < count1; i ++) {
+			memcpy (&vertList[(count0 + i) * stride + 2], &vertList[(i0 + i) * stride + 2], (stride - 2) * sizeof (dgFloat64));
+		}
+
+
+		//		dgFloat64* const indexPtr = (dgInt64*)vertList;
+		for (dgInt32 i = i0; i < vertexCount; i ++) {
+			//			indexPtr[i * stride] += count0;
+			vertList[i * stride] += dgFloat64 (count0);
+		}
+
+	} else {
+		count = SortVertices (vertList, stride, compareCount, vertexCount, tolerance);
+	}
+
+	return count;
+}
+
+
+dgInt32 dgVertexListToIndexList (dgFloat64* const vertList, dgInt32 strideInBytes, dgInt32 compareCount, dgInt32 vertexCount, dgInt32* const indexListOut, dgFloat64 tolerance)
+{
+	dgSetPrecisionDouble precision;
+
+	if (strideInBytes < 3 * dgInt32 (sizeof (dgFloat64))) {
+		return 0;
+	}
+	if (compareCount < 3) {
+		return 0;
+	}
+	dgAssert (compareCount <= dgInt32 (strideInBytes / sizeof (dgFloat64)));
+	dgAssert (strideInBytes == dgInt32 (sizeof (dgFloat64) * (strideInBytes / sizeof (dgFloat64))));
+
+	dgInt32 stride = strideInBytes / dgInt32 (sizeof (dgFloat64));
+	dgInt32 stride2 = stride + 2;
+
+	dgStack<dgFloat64>pool (stride2  * vertexCount);
+	dgFloat64* const tmpVertexList = &pool[0];
+
+//	dgInt64* const indexPtr = (dgInt64*)tmpVertexList;
+
+	dgInt32 k = 0;
+	dgInt32 m = 0;
+	for (dgInt32 i = 0; i < vertexCount; i ++) {
+		memcpy (&tmpVertexList[m + 2], &vertList[k], stride * sizeof (dgFloat64));
+		tmpVertexList[m + 0] = dgFloat64 (- 1.0f);
+		tmpVertexList[m + 1] = dgFloat64 (i);
+		k += stride;
+		m += stride2;
+	}
+	
+	dgInt32 count = QuickSortVertices (tmpVertexList, stride2, compareCount, vertexCount, tolerance);
+
+	k = 0;
+	m = 0;
+	for (dgInt32 i = 0; i < count; i ++) {
+		k = i * stride;
+		m = i * stride2;
+		memcpy (&vertList[k], &tmpVertexList[m + 2], stride * sizeof (dgFloat64));
+		k += stride;
+		m += stride2;
+	}
+
+	m = 0;
+	for (dgInt32 i = 0; i < vertexCount; i ++) {
+		dgInt32 i1 = dgInt32 (tmpVertexList [m + 1]);
+		dgInt32 index = dgInt32 (tmpVertexList [m + 0]);
+		indexListOut[i1] = index;
+		m += stride2;
+	}
+
+	return count;
+}
+
+
+
+
+dgInt32 dgVertexListToIndexList (dgFloat32* const vertList, dgInt32 strideInBytes, dgInt32 floatSizeInBytes, dgInt32 unsignedSizeInBytes, dgInt32 vertexCount, dgInt32* const indexList, dgFloat32 tolerance)
+{
+	dgInt32 stride = strideInBytes / sizeof (dgFloat32);
+
+	dgAssert (!unsignedSizeInBytes);
+	dgStack<dgFloat64> pool(vertexCount * stride);
+
+	dgInt32 floatCount = floatSizeInBytes / sizeof (dgFloat32);
+
+	dgFloat64* const data = &pool[0];
+	for (dgInt32 i = 0; i < vertexCount; i ++) {
+
+		dgFloat64* const dst = &data[i * stride];
+		dgFloat32* const src = &vertList[i * stride];
+		for (dgInt32 j = 0; j < stride; j ++) {
+			dst[j] = src[j];
+		}
+	}
+
+	dgInt32 count = dgVertexListToIndexList (data, stride * sizeof (dgFloat64), floatCount, vertexCount, indexList, dgFloat64 (tolerance));
+	for (dgInt32 i = 0; i < count; i ++) {
+		dgFloat64* const src = &data[i * stride];
+		dgFloat32* const dst = &vertList[i * stride];
+		for (dgInt32 j = 0; j < stride; j ++) {
+			dst[j] = dgFloat32 (src[j]);
+		}
+	}
+	
+	return count;
+}
+
+
+//#define SERIALIZE_END	'dne '
+#define SERIALIZE_END   0x646e6520
+
+void dgSerializeMarker(dgSerialize serializeCallback, void* const userData)
+{
+	dgInt32 end = SERIALIZE_END;
+	serializeCallback (userData, &end, sizeof (dgInt32));
+	serializeCallback (userData, &end, sizeof (dgInt32));
+}
+
+void dgDeserializeMarker(dgDeserialize serializeCallback, void* const userData)
+{
+	dgInt32 state = 0;
+	while (state != 2) {
+		dgInt32 marker;
+		serializeCallback (userData, &marker, sizeof (marker));
+		switch (state) 
+		{
+			case 0:
+			{
+				if (marker == SERIALIZE_END) {
+					state = 1;
+					break;
+				} else {
+					state = 0;
+				}
+				break;
+			}
+
+			case 1:
+			{
+				if (marker == SERIALIZE_END) {
+					state = 2;
+					break;
+				} else {
+					state = 0;
+				}
+				break;
+			}
+		}
+	} 
+}

newtondynamics.mod/NewtonDynamics/source/core/dgTypes.h

@@ -0,0 +1,831 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __DGTYPES_H__
+#define __DGTYPES_H__
+
+
+#ifdef DG_SSE4_INSTRUCTIONS_SET
+	#undef DG_SCALAR_VECTOR_CLASS
+#endif
+
+
+#if defined (_NEWTON_USE_DOUBLE) || defined (__ppc__) || defined (ANDROID) || defined (IOS)
+	#undef DG_SSE4_INSTRUCTIONS_SET
+	#ifndef DG_SCALAR_VECTOR_CLASS
+		#define DG_SCALAR_VECTOR_CLASS
+	#endif		
+#endif
+
+
+
+#ifdef _MSC_VER 
+	#ifdef _M_X64
+		#ifndef _WIN_64_VER
+			#define _WIN_64_VER
+		#endif
+	#else 		
+		#ifndef _WIN_32_VER
+			#define _WIN_32_VER
+		#endif
+	#endif
+#endif
+
+
+#ifdef _MSC_VER 
+	#if !(defined (_WIN_32_VER) || defined (_WIN_64_VER))
+		#error "most define _WIN_32_VER or _WIN_64_VER for a correct CPU target"
+	#endif
+#endif
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+
+	#pragma warning (disable: 4100) //unreferenced formal parameter
+	#pragma warning (disable: 4201) //nonstandard extension used : nameless struct/union
+	#pragma warning (disable: 4324) //structure was padded due to __declspec(align())
+	#pragma warning (disable: 4530) //: C++ exception handler used, but unwind semantics are not enabled. Specify /EHsc
+
+	#include <io.h> 
+	#include <direct.h> 
+	#include <malloc.h>
+	#include <float.h>
+	#include <stdarg.h>
+	#include <process.h>
+
+
+	#ifdef _DEBUG
+		#pragma warning (disable: 4127)	//conditional expression is constant
+	#endif
+
+	#pragma warning (push, 3) 
+		#include <windows.h>
+		#include <crtdbg.h>
+		#ifndef _DURANGO
+			#include <tlhelp32.h>
+		#endif
+	#pragma warning (pop) 
+#endif
+
+#include <new>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include <time.h>
+#include <math.h>
+#include <float.h>
+#include <ctype.h>
+#include <pthread.h>
+#include <sched.h>
+#include <semaphore.h>
+
+
+
+#if (defined (_MINGW_32_VER) || defined (_MINGW_64_VER))
+	#include <io.h> 
+	#include <direct.h> 
+	#include <malloc.h>
+	#include <float.h>
+	#include <windows.h>
+	#include <process.h>
+#endif
+
+
+
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#include <intrin.h>
+#endif
+
+
+
+#ifdef __ppc__
+	#include <vecLib/veclib.h>
+#endif
+
+
+#if (defined (_POSIX_VER) || defined (_POSIX_VER_64) || defined (_MINGW_32_VER) || defined (_MINGW_64_VER))
+	#include <unistd.h>
+	#include <assert.h>
+	extern "C" 
+	{ 
+		// for SSE3 and up
+		#include <pmmintrin.h> 
+		#include <emmintrin.h> 
+		#include <mmintrin.h> 
+	} 
+#endif
+
+#ifdef _MACOSX_VER
+	#include <unistd.h>
+	#include <sys/sysctl.h>
+    #include <assert.h> 
+    #if (defined __i386__ || defined __x86_64__)
+		#include <pmmintrin.h> 
+		#include <emmintrin.h>  //sse3
+        #include <mmintrin.h>    
+    #endif
+#endif
+
+//************************************************************
+#ifdef DG_DISABLE_ASSERT
+	#define dgAssert(x)
+#else 
+	#if defined (_WIN_32_VER) || defined (_WIN_64_VER)
+		#define dgAssert(x) _ASSERTE(x)
+	#else 
+		#ifdef _DEBUG
+			#define dgAssert(x) assert(x)
+		#else 
+			#define dgAssert(x)
+		#endif
+	#endif
+#endif
+
+
+#define	DG_MAX_THREADS_HIVE_COUNT		16
+
+#ifdef _DEBUG
+//#define __ENABLE_DG_CONTAINERS_SANITY_CHECK 
+#endif
+
+
+#ifdef DLL_DECLSPEC
+#undef DLL_DECLSPEC
+#endif
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#define DG_INLINE	__forceinline 
+#else 
+	#define DG_INLINE	inline
+	//#define DG_INLINE	 __attribute__((always_inline))
+#endif
+
+
+#define DG_VECTOR_SIMD_SIZE		16
+
+#if ((defined (_WIN_32_VER) || defined (_WIN_64_VER)) && (_MSC_VER  >= 1700)) && !defined(_DURANGO)
+	// starting Visual studio 2012 an up we can use high performance computing using AMP
+	#define _NEWTON_AMP
+#endif
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#define	DG_GCC_VECTOR_ALIGMENT	
+	#define	DG_MSC_VECTOR_ALIGMENT			__declspec(align(DG_VECTOR_SIMD_SIZE))
+#else
+	#define	DG_MSC_VECTOR_ALIGMENT			
+	#define	DG_GCC_VECTOR_ALIGMENT			__attribute__ ((aligned (DG_VECTOR_SIMD_SIZE)))
+#endif
+
+#if ((defined (_WIN_32_VER) || defined (_WIN_64_VER)) && (_MSC_VER  >= 1600))
+	#include <stdint.h>
+	typedef int8_t dgInt8;
+	typedef uint8_t dgUnsigned8;
+
+	typedef int16_t dgInt16;
+	typedef uint16_t dgUnsigned16;
+
+	typedef int32_t dgInt32;
+	typedef uint32_t dgUnsigned32;
+
+	typedef int64_t dgInt64;
+	typedef uint64_t dgUnsigned64;
+#else
+	typedef char dgInt8;
+	typedef unsigned char dgUnsigned8;
+
+	typedef short dgInt16;
+	typedef unsigned short dgUnsigned16;
+
+	typedef int dgInt32;
+	typedef unsigned dgUnsigned32;
+	typedef unsigned int dgUnsigned32;
+
+	typedef long long dgInt64;
+	typedef unsigned long long dgUnsigned64;
+	typedef double dgFloat64;
+#endif
+
+
+typedef double dgFloat64;
+
+#ifdef _NEWTON_USE_DOUBLE
+	typedef double dgFloat32;
+#else
+	typedef float dgFloat32;
+#endif
+
+
+class dgTriplex
+{
+	public:
+	dgFloat32 m_x;
+	dgFloat32 m_y;
+	dgFloat32 m_z;
+};
+
+
+#define dgPI			 	dgFloat32 (3.14159f)
+#define dgPI2			 	dgFloat32 (dgPI * 2.0f)
+#define dgEXP			 	dgFloat32 (2.71828f)
+#define dgEPSILON	  	 	dgFloat32 (1.0e-5f)
+#define dgGRAVITY	  	 	dgFloat32 (9.8f)
+#define dgDEG2RAD	  	 	dgFloat32 (dgPI / 180.0f)
+#define dgRAD2DEG	  	 	dgFloat32 (180.0f / dgPI)
+#define dgKMH2MPSEC		 	dgFloat32 (0.278f)
+
+
+class dgVector;
+class dgBigVector;
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#define dgApi __cdecl 	
+	#define dgStdApi __stdcall 	
+#else
+	#define dgApi 	
+	#define dgStdApi
+#endif
+
+
+
+#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+	#define dgCheckFloat(x) (_finite(x) && !_isnan(x))
+//	#define dgCheckFloat(x) 1
+#else
+	#define dgCheckFloat(x) (isfinite(x) && !isnan(x))
+//		#define dgCheckFloat(x) 1
+#endif
+
+
+
+typedef void (dgApi *dgDeserialize) (void* const userData, void* buffer, size_t size);
+typedef void (dgApi *dgSerialize) (void* const userData, const void* const buffer, size_t size);
+typedef bool (dgApi *dgReportProgress) (dgFloat32 progressNormalzedPercent, void* const userData);
+
+DG_INLINE dgInt32 dgExp2 (dgInt32 x)
+{
+	dgInt32 exp;
+	for (exp = -1; x; x >>= 1) {
+		exp ++;
+	}
+	return exp;
+}
+
+template <class T> 
+DG_INLINE T dgMin(T A, T B)
+{
+	return (A < B) ? A : B; 
+}
+
+template <class T> 
+DG_INLINE T dgMax(T A, T B)
+{
+	return (A > B) ? A : B; 
+}
+
+template <class T>
+DG_INLINE T dgMin(T A, T B, T C)
+{
+	return dgMin(dgMin (A, B), C);
+}
+
+template <class T>
+DG_INLINE T dgMax(T A, T B, T C)
+{
+	return dgMax(dgMax (A, B), C);
+}
+
+template <class T>
+DG_INLINE T dgClamp(T val, T min, T max)
+{
+	return dgMax (min, dgMin (max, val));
+}
+
+template <class T> 
+DG_INLINE void dgSwap(T& A, T& B)
+{
+	T tmp (A);
+	A = B;
+	B = tmp;
+}	
+
+template <class T>
+DG_INLINE T dgSign(T A)
+{
+//	T sign (1.0f);
+//	if (A < T (0.0f)) {
+//		sign = T (-1.0f);
+//	}
+//	return sign;
+	return (A >= T(0)) ? T(1) : T(-1);
+}
+
+template <class T> 
+dgInt32 dgBinarySearch (T const* array, dgInt32 elements, dgInt32 entry)
+{
+	dgInt32 index0 = 0;
+	dgInt32 index2 = elements - 1;
+	dgInt32 entry0 = array[index0].m_Key;
+	dgInt32 entry2 = array[index2].m_Key;
+
+	while ((index2 - index0) > 1) {
+		dgInt32 index1 = (index0 + index2) >> 1;
+		dgInt32 entry1 = array[index1].m_Key;
+		if (entry1 == entry) {
+			dgAssert (array[index1].m_Key <= entry);
+			dgAssert (array[index1 + 1].m_Key >= entry);
+			return index1;
+		} else if (entry < entry1) {
+			index2 = index1;
+			entry2 = entry1;
+		} else {
+			index0 = index1;
+			entry0 = entry1;
+		}
+	}
+
+	if (array[index0].m_Key > index0) {
+		index0 --;
+	}
+
+	dgAssert (array[index0].m_Key <= entry);
+	dgAssert (array[index0 + 1].m_Key >= entry);
+	return index0;
+}
+
+
+
+
+template <class T> 
+void dgRadixSort (T* const array, T* const tmpArray, dgInt32 elements, dgInt32 radixPass, 
+				  dgInt32 (*getRadixKey) (const T* const  A, void* const context), void* const context = NULL)
+{
+	dgInt32 scanCount[256]; 
+	dgInt32 histogram[256][4];
+
+	dgAssert (radixPass >= 1);
+	dgAssert (radixPass <= 4);
+	
+	memset (histogram, 0, sizeof (histogram));
+	for (dgInt32 i = 0; i < elements; i ++) {
+		dgInt32 key = getRadixKey (&array[i], context);
+		for (dgInt32 j = 0; j < radixPass; j ++) {
+			dgInt32 radix = (key >> (j << 3)) & 0xff;
+			histogram[radix][j] = histogram[radix][j] + 1;
+		}
+	}
+
+	for (dgInt32 radix = 0; radix < radixPass; radix += 2) {
+		scanCount[0] = 0;
+		for (dgInt32 i = 1; i < 256; i ++) {
+			scanCount[i] = scanCount[i - 1] + histogram[i - 1][radix];
+		}
+		dgInt32 radixShift = radix << 3;
+		for (dgInt32 i = 0; i < elements; i ++) {
+			dgInt32 key = (getRadixKey (&array[i], context) >> radixShift) & 0xff;
+			dgInt32 index = scanCount[key];
+			tmpArray[index] = array[i];
+			scanCount[key] = index + 1;
+		}
+
+		if ((radix + 1) < radixPass) { 
+			scanCount[0] = 0;
+			for (dgInt32 i = 1; i < 256; i ++) {
+				scanCount[i] = scanCount[i - 1] + histogram[i - 1][radix + 1];
+			}
+			
+			dgInt32 radixShift = (radix + 1) << 3;
+			for (dgInt32 i = 0; i < elements; i ++) {
+				dgInt32 key = (getRadixKey (&array[i], context) >> radixShift) & 0xff;
+				dgInt32 index = scanCount[key];
+				array[index] = tmpArray[i];
+				scanCount[key] = index + 1;
+			}
+		} else {
+			memcpy (array, tmpArray, elements * sizeof (T)); 
+		}
+	}
+
+
+#ifdef _DEBUG
+	for (dgInt32 i = 0; i < (elements - 1); i ++) {
+		dgAssert (getRadixKey (&array[i], context) <= getRadixKey (&array[i + 1], context));
+	}
+#endif
+
+}
+
+
+template <class T> 
+void dgSort (T* const array, dgInt32 elements, dgInt32 (*compare) (const T* const  A, const T* const B, void* const context), void* const context = NULL)
+{
+	dgInt32 stride = 8;
+	dgInt32 stack[1024][2];
+
+	stack[0][0] = 0;
+	stack[0][1] = elements - 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;
+			T val (array[(lo + hi) >> 1]);
+			do {    
+				while (compare (&array[i], &val, context) < 0) i ++;
+				while (compare (&array[j], &val, context) > 0) j --;
+
+				if (i <= j)	{
+					dgSwap(array[i], array[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]))));
+		}
+	}
+
+	stride = stride * 2;
+	if (elements < stride) {
+		stride = elements;
+	}
+	for (dgInt32 i = 1; i < stride; i ++) {
+		if (compare (&array[0], &array[i], context) > 0) {
+			dgSwap(array[0], array[i]);
+		}
+	}
+
+	for (dgInt32 i = 1; i < elements; i ++) {
+		dgInt32 j = i;
+		T tmp (array[i]);
+		for (; compare (&array[j - 1], &tmp, context) > 0; j --) {
+			dgAssert (j > 0);
+			array[j] = array[j - 1];
+		}
+		array[j] = tmp;
+	}
+
+#ifdef _DEBUG
+	for (dgInt32 i = 0; i < (elements - 1); i ++) {
+		dgAssert (compare (&array[i], &array[i + 1], context) <= 0);
+	}
+#endif
+}
+
+
+template <class T> 
+void dgSortIndirect (T** const array, dgInt32 elements, dgInt32 (*compare) (const T* const  A, const T* const B, void* const context), void* const context = NULL)
+{
+	dgInt32 stride = 8;
+	dgInt32 stack[1024][2];
+
+	stack[0][0] = 0;
+	stack[0][1] = elements - 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;
+			T* val (array[(lo + hi) >> 1]);
+			do {    
+				while (compare (array[i], val, context) < 0) i ++;
+				while (compare (array[j], val, context) > 0) j --;
+
+				if (i <= j)	{
+					dgSwap(array[i], array[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]))));
+		}
+	}
+
+	stride = stride * 2;
+	if (elements < stride) {
+		stride = elements;
+	}
+	for (dgInt32 i = 1; i < stride; i ++) {
+		if (compare (&array[0], &array[i], context) > 0) {
+			dgSwap(array[0], array[i]);
+		}
+	}
+
+	for (dgInt32 i = 1; i < elements; i ++) {
+		dgInt32 j = i;
+		T* tmp (array[i]);
+		for (; compare (&array[j - 1], &tmp, context) > 0; j --) {
+			dgAssert (j > 0);
+			array[j] = array[j - 1];
+		}
+		array[j] = tmp;
+	}
+
+#ifdef _DEBUG
+	for (dgInt32 i = 0; i < (elements - 1); i ++) {
+		dgAssert (compare (array[i], array[i + 1], context) <= 0);
+	}
+#endif
+}
+
+
+
+#ifdef _NEWTON_USE_DOUBLE
+	union dgFloatSign
+	{
+		struct {
+			dgInt32 m_dommy;
+			dgInt32 m_iVal;
+		} m_integer;
+		dgFloat64 m_fVal;
+	};
+#else
+	union dgFloatSign
+	{
+		struct {
+			dgInt32 m_iVal;
+		} m_integer;
+		dgFloat32 m_fVal;
+	};
+#endif
+
+union dgDoubleInt
+{
+	struct {
+		dgInt32 m_intL;
+		dgInt32 m_intH;
+	};
+	void* m_ptr;
+	dgInt64 m_int;
+	dgFloat64 m_float;
+};
+
+
+
+void GetMinMax (dgVector &Min, dgVector &Max, const dgFloat32* const vArray, dgInt32 vCount, dgInt32 StrideInBytes);
+void GetMinMax (dgBigVector &Min, dgBigVector &Max, const dgFloat64* const vArray, dgInt32 vCount, dgInt32 strideInBytes);
+
+dgInt32 dgVertexListToIndexList (dgFloat32* const vertexList, dgInt32 strideInBytes, dgInt32 floatSizeInBytes, dgInt32 unsignedSizeInBytes, dgInt32 vertexCount, dgInt32* const indexListOut, dgFloat32 tolerance = dgEPSILON);
+
+dgInt32 dgVertexListToIndexList (dgFloat64* const vertexList, dgInt32 strideInBytes, dgInt32 compareCount, dgInt32 vertexCount, dgInt32* const indexListOut, dgFloat64 tolerance = dgEPSILON);
+
+
+#define PointerToInt(x) ((size_t)x)
+#define IntToPointer(x) ((void*)(size_t(x)))
+
+
+#ifndef _MSC_VER 
+	#define _stricmp(x,y) strcasecmp(x,y)
+#endif
+
+
+DG_INLINE dgFloat32 dgAbsf(dgFloat32 x)
+{
+#if 0
+	dgDoubleInt val;
+	val.m_float = x;
+	val.m_intH &= ~(dgUnsigned64 (1)<<31);
+	dgAssert (val.m_float == fabs (x));
+	return dgFloat32 (val.m_float);
+#else
+	// according to Intel this is better because is doe not read after write
+	return (x >= dgFloat32 (0.0f)) ? x : -x;
+#endif
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+DG_INLINE dgInt32 dgFastInt (dgFloat64 x)
+{
+	dgInt32 i = dgInt32 (x);
+	if (dgFloat64 (i) > x) {
+		i --;
+	}
+	return i;
+}
+#endif
+
+DG_INLINE dgInt32 dgFastInt (dgFloat32 x)
+{
+	dgInt32 i = dgInt32 (x);
+	if (dgFloat32 (i) > x) {
+		i --;
+	}
+	return i;
+}
+
+DG_INLINE dgFloat32 dgFloor(dgFloat32 x)
+{
+#ifdef _MSC_VER
+	dgFloat32 ret = dgFloat32 (dgFastInt (x));
+	dgAssert (ret == floor (x));
+	return  ret;
+#else 
+	return floor (x);
+#endif
+}
+
+DG_INLINE dgFloat32 dgCeil(dgFloat32 x)
+{
+#ifdef _MSC_VER
+	dgFloat32 ret = dgFloor(x);
+	if (ret < x) {
+		ret += dgFloat32 (1.0f);
+	}
+	dgAssert (ret == ceil (x));
+	return  ret;
+#else 
+	return ceil (x);
+#endif
+}
+
+DG_INLINE dgFloat32 dgRsqrt(dgFloat32 x)	
+{
+	return dgFloat32 (1.0f) / dgFloat32 (sqrt(x));		
+}
+
+#define dgSqrt(x)			dgFloat32 (sqrt(x))	
+#define dgSin(x)			dgFloat32 (sin(x))
+#define dgCos(x)			dgFloat32 (cos(x))
+#define dgAsin(x)			dgFloat32 (asin(x))
+#define dgAcos(x)			dgFloat32 (acos(x))
+#define dgAtan2(x,y)		dgFloat32 (atan2(x,y))
+#define dgLog(x)			dgFloat32 (log(x))
+#define dgPow(x,y)			dgFloat32 (pow(x,y))
+#define dgFmod(x,y)			dgFloat32 (fmod(x,y))
+#define dgClearFP()			_clearfp() 
+#define dgControlFP(x,y)	_controlfp(x,y)
+
+
+void dgSerializeMarker(dgSerialize serializeCallback, void* const userData);
+void dgDeserializeMarker(dgDeserialize serializeCallback, void* const userData);
+
+
+typedef dgUnsigned32 (dgApi *OnGetPerformanceCountCallback) ();
+
+
+dgUnsigned64 dgGetTimeInMicrosenconds();
+
+
+class dgFloatExceptions
+{
+	public:
+	//#define DG_FLOAT_EXECTIONS_MASK (_EM_INVALID | _EM_DENORMAL | _EM_ZERODIVIDE | _EM_OVERFLOW | _EM_UNDERFLOW| _EM_INEXACT)
+	//#define DG_FLOAT_EXECTIONS_MASK (_EM_INVALID | _EM_DENORMAL | _EM_ZERODIVIDE)
+	//#define DG_FLOAT_EXECTIONS_MASK (_EM_DENORMAL | _EM_ZERODIVIDE)
+	
+	enum dgFloatExceptionMask
+	{
+		#if (defined (_MSC_VER) && defined (_DEBUG))
+			m_InvalidDenormalAndivideByZero = _EM_INVALID | _EM_DENORMAL | _EM_ZERODIVIDE,
+			m_allExepctions = _EM_INVALID | _EM_DENORMAL | _EM_ZERODIVIDE | _EM_OVERFLOW | _EM_UNDERFLOW,
+		#else
+			m_InvalidDenormalAndivideByZero,
+			m_allExepctions,
+		#endif			
+	};
+
+	dgFloatExceptions(dgFloatExceptionMask mask = m_InvalidDenormalAndivideByZero)
+		:m_mask (0)
+	{
+		#if (defined (_MSC_VER) && defined (_DEBUG))
+			dgClearFP();
+			m_mask = dgControlFP(0, 0);
+			dgControlFP (m_mask & dgUnsigned32 (~mask), _MCW_EM);
+		#endif
+	}
+
+	~dgFloatExceptions()
+	{
+		#if (defined (_MSC_VER) && defined (_DEBUG))
+			dgClearFP();
+			dgControlFP(m_mask, _MCW_EM);
+		#endif
+	}
+
+	dgInt32 m_mask;
+};
+
+
+class dgSetPrecisionDouble 
+{
+	public:
+	dgSetPrecisionDouble()
+	{
+		#if (defined (_MSC_VER) && defined (_WIN_32_VER))
+			dgClearFP();
+			m_mask = dgControlFP(0, 0);
+			dgControlFP (_PC_53, _MCW_PC);
+		#endif
+	}
+
+	~dgSetPrecisionDouble()
+	{
+		#if (defined (_MSC_VER) && defined (_WIN_32_VER))
+			dgClearFP();
+			dgControlFP (m_mask, _MCW_PC);
+		#endif
+	}
+	dgInt32 m_mask; 
+};
+
+
+
+
+DG_INLINE dgInt32 dgAtomicExchangeAndAdd (dgInt32* const addend, dgInt32 amount)
+{
+	// it is a pity that pthread does not supports cross platform atomics, it would be nice if it did
+	#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+		return _InterlockedExchangeAdd((long*) addend, long (amount));
+	#endif
+
+	#if (defined (_MINGW_32_VER) || defined (_MINGW_64_VER))
+		return InterlockedExchangeAdd((long*) addend, long (amount));
+	#endif
+
+
+	#if (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER))
+		return __sync_fetch_and_add ((int32_t*)addend, amount );
+	#endif
+}
+
+DG_INLINE dgInt32 dgInterlockedExchange(dgInt32* const ptr, dgInt32 value)
+{
+	// it is a pity that pthread does not supports cross platform atomics, it would be nice if it did
+	#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
+		return _InterlockedExchange((long*) ptr, value);
+	#endif
+
+	#if (defined (_MINGW_32_VER) || defined (_MINGW_64_VER))
+		return InterlockedExchange((long*) ptr, value);
+	#endif
+
+
+	#if (defined (_POSIX_VER) || defined (_POSIX_VER_64) ||defined (_MACOSX_VER))
+		//__sync_synchronize();
+		return __sync_lock_test_and_set((int32_t*)ptr, value);
+	#endif
+}
+
+DG_INLINE void dgThreadYield()
+{
+#if defined (DG_USE_THREAD_EMULATION)
+	return;
+#else
+	sched_yield();
+#endif
+}
+
+DG_INLINE void dgPrefetchMem(const void* const mem)
+{
+	#if !(defined (__ppc__) || defined (ANDROID) || defined (IOS))
+		_mm_prefetch ((const char*)mem, _MM_HINT_T0);
+	#endif
+}
+
+#endif
+

newtondynamics.mod/NewtonDynamics/source/core/dgVector.h

@@ -0,0 +1,1050 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgVector__
+#define __dgVector__
+
+#include "dgStdafx.h"
+#include "dgTypes.h"
+#include "dgDebug.h"
+#include "dgMemory.h"
+
+
+
+#define dgCheckVector(x) (dgCheckFloat(x[0]) && dgCheckFloat(x[1]) && dgCheckFloat(x[2]) && dgCheckFloat(x[3]))
+
+
+template<class T>
+class dgTemplateVector
+{
+	public:
+	DG_INLINE dgTemplateVector () 
+	{
+	}
+	
+	DG_INLINE dgTemplateVector (const T* const ptr)
+		:m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (T(0.0f))
+	{
+		//	dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgTemplateVector (const dgTemplateVector<T>& copy)
+		:m_x(copy.m_x), m_y(copy.m_y), m_z(copy.m_z), m_w (copy.m_w)
+	{
+		//	dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgTemplateVector (T x, T y, T z, T w) 
+		:m_x(x), m_y(y), m_z(z), m_w (w)
+	{
+	}
+	
+	DG_INLINE T& operator[] (dgInt32 i)
+	{
+		dgAssert (i < 4);
+		dgAssert (i >= 0);
+		return (&m_x)[i];
+	}	
+
+	DG_INLINE const T& operator[] (dgInt32 i) const
+	{
+		dgAssert (i < 4);
+		dgAssert (i >= 0);
+		return (&m_x)[i];
+	}
+
+	DG_INLINE dgTemplateVector<T> Scale3 (T scale) const
+	{
+		return dgTemplateVector<T> (m_x * scale, m_y * scale, m_z * scale, m_w);
+	}
+
+	DG_INLINE dgTemplateVector<T> Scale4 (T scale) const
+	{
+		return dgTemplateVector<T> (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
+	}
+
+	DG_INLINE dgTemplateVector<T> operator+ (const dgTemplateVector<T>& B) const
+	{
+		return dgTemplateVector<T> (m_x + B.m_x, m_y + B.m_y, m_z + B.m_z, m_w + B.m_w);
+	}
+
+	DG_INLINE dgTemplateVector<T>& operator+= (const dgTemplateVector<T>& A) 
+	{
+		return (*this = dgTemplateVector<T> (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
+	}
+
+	DG_INLINE dgTemplateVector<T> operator- (const dgTemplateVector<T>& A) const
+	{
+		return dgTemplateVector<T> (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
+	}
+
+	DG_INLINE dgTemplateVector<T>& operator-= (const dgTemplateVector<T>& A) 
+	{
+		return (*this = dgTemplateVector<T> (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
+	}
+
+	// return dot product
+	DG_INLINE T operator% (const dgTemplateVector<T>& A) const
+	{
+		return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z;
+	}
+
+	// return cross product
+	DG_INLINE dgTemplateVector<T> operator* (const dgTemplateVector<T>& B) const
+	{
+		return dgTemplateVector<T> (m_y * B.m_z - m_z * B.m_y,
+									m_z * B.m_x - m_x * B.m_z,
+									m_x * B.m_y - m_y * B.m_x, m_w);
+	}
+
+	DG_INLINE dgTemplateVector<T> AddHorizontal () const
+	{
+		T val (m_x + m_y + m_z + m_w); 
+		return dgTemplateVector<T> (val, val, val, val);
+	}
+
+	// return dot 4d dot product
+	DG_INLINE dgTemplateVector<T> DotProduct4 (const dgTemplateVector &A) const
+	{
+		T val (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
+		return dgTemplateVector<T> (val, val, val, val);
+	}
+
+	
+	DG_INLINE dgTemplateVector<T> CrossProduct4 (const dgTemplateVector &A, const dgTemplateVector &B) const
+	{
+		T cofactor[3][3];
+		T array[4][4];
+
+		const dgTemplateVector<T>& me = *this;
+		for (dgInt32 i = 0; i < 4; i ++) {
+			array[0][i] = me[i];
+			array[1][i] = A[i];
+			array[2][i] = B[i];
+			array[3][i] = T (1.0f);
+		}
+
+		dgTemplateVector<T> normal;
+		T sign = T (-1.0f);
+		for (dgInt32 i = 0; i < 4; i ++)  {
+
+			for (dgInt32 j = 0; j < 3; j ++) {
+				dgInt32 k0 = 0;
+				for (dgInt32 k = 0; k < 4; k ++) {
+					if (k != i) {
+						cofactor[j][k0] = array[j][k];
+						k0 ++;
+					}
+				}
+			}
+			T x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
+			T y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
+			T z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
+			T det = x + y + z;
+
+			normal[i] = sign * det;
+			sign *= T (-1.0f);
+		}
+
+		return normal;
+	}
+
+	// component wise multiplication
+	DG_INLINE dgTemplateVector<T> CompProduct3 (const dgTemplateVector<T>& A) const
+	{
+		return dgTemplateVector<T> (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w);
+	}
+
+	// component wise 4d multiplication
+	DG_INLINE dgTemplateVector<T> CompProduct4 (const dgTemplateVector<T>& A) const
+	{
+		return dgTemplateVector<T> (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
+	}
+
+
+	// check validity of floats
+#ifdef _DEBUG
+	void Trace () const
+	{
+		dgTrace (("%f %f %f %f\n", m_x, m_y, m_z, m_w));
+	}
+#endif
+
+	DG_CLASS_ALLOCATOR(allocator)
+
+	T m_x;
+	T m_y;
+	T m_z;
+	T m_w;
+};
+
+class dgVector;
+
+// *****************************************************************************************
+//
+// 256 bit double precision vector class declaration
+//
+// *****************************************************************************************
+DG_MSC_VECTOR_ALIGMENT
+class dgBigVector: public dgTemplateVector<dgFloat64>
+{
+	public:
+	DG_INLINE dgBigVector()
+		:dgTemplateVector<dgFloat64>()
+	{
+	}
+
+	DG_INLINE dgBigVector (const dgVector& v)
+		:dgTemplateVector<dgFloat64>(((dgTemplateVector<dgFloat32>&)v).m_x, ((dgTemplateVector<dgFloat32>&)v).m_y, ((dgTemplateVector<dgFloat32>&)v).m_z, ((dgTemplateVector<dgFloat32>&)v).m_w)
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgBigVector (const dgTemplateVector<dgFloat64>& v)
+		:dgTemplateVector<dgFloat64>(v)
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgBigVector (const dgBigVector& copy)
+		:dgTemplateVector<dgFloat64>(copy)
+	{
+		//	dgAssert (dgCheckVector ((*this)));
+	}
+
+
+	DG_INLINE dgBigVector (const dgFloat32* const ptr)
+		:dgTemplateVector<dgFloat64>(ptr[0], ptr[1], ptr[2], dgFloat64 (0.0f))
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+
+#ifndef _NEWTON_USE_DOUBLE
+	DG_INLINE dgBigVector (const dgFloat64* const ptr)
+		:dgTemplateVector<dgFloat64>(ptr)
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+#endif
+
+	DG_INLINE dgBigVector (dgFloat64 x, dgFloat64 y, dgFloat64 z, dgFloat64 w) 
+		:dgTemplateVector<dgFloat64>(x, y, z, w)
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+
+} DG_GCC_VECTOR_ALIGMENT;
+
+
+
+// *****************************************************************************************
+//
+// 128 bit single precision vector class declaration
+//
+// *****************************************************************************************
+#ifdef DG_SCALAR_VECTOR_CLASS
+
+DG_MSC_VECTOR_ALIGMENT
+class dgVector
+{
+	public:
+	DG_INLINE dgVector()
+	{
+	}
+
+	DG_INLINE dgVector(dgFloat32 val)
+		:m_x(val), m_y(val), m_z(val), m_w(val)
+	{
+	}
+
+	DG_INLINE dgVector (const dgVector& v)
+		:m_x(v.m_x), m_y(v.m_y), m_z(v.m_z), m_w(v.m_w)
+	{
+		//dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgVector (const dgFloat32* const ptr)
+		:m_x(ptr[0]), m_y(ptr[1]), m_z(ptr[2]), m_w (dgFloat32 (0.0f))
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgVector (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w) 
+		:m_x(x), m_y(y), m_z(z), m_w(w)
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgVector (dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw)
+		:m_x(*((dgFloat32*)&ix)), m_y(*((dgFloat32*)&iy)), m_z(*((dgFloat32*)&iz)), m_w(*((dgFloat32*)&iw))
+	{
+	}
+
+	DG_INLINE dgVector (const dgBigVector& copy)
+		:m_x(dgFloat32 (copy.m_x)), m_y(dgFloat32 (copy.m_y)), m_z(dgFloat32 (copy.m_z)), m_w(dgFloat32 (copy.m_w))
+	{
+		dgAssert (dgCheckVector ((*this)));
+	}
+
+	DG_INLINE dgFloat32 GetScalar () const
+	{
+		return m_x;
+	}
+
+	DG_INLINE void Store (dgFloat32* const dst) const
+	{
+		dst[0] = m_x;
+		dst[1] = m_y;
+		dst[2] = m_z;
+		dst[3] = m_w;
+	}
+
+	DG_INLINE dgVector BroadcastX () const
+	{
+		return dgVector (m_x);
+	}
+
+	DG_INLINE dgVector BroadcastY () const
+	{
+		return dgVector (m_y);
+	}
+
+	DG_INLINE dgVector BroadcastZ () const
+	{
+		return dgVector (m_z);
+	}
+
+	DG_INLINE dgVector BroadcastW () const
+	{
+		return dgVector (m_w);
+	}
+
+
+	DG_INLINE dgFloat32& operator[] (dgInt32 i)
+	{
+		dgAssert (i < 4);
+		dgAssert (i >= 0);
+		return (&m_x)[i];
+	}
+
+	DG_INLINE const dgFloat32& operator[] (dgInt32 i) const
+	{
+		dgAssert (i < 4);
+		dgAssert (i >= 0);
+		return (&m_x)[i];
+	}
+
+	DG_INLINE dgVector operator+ (const dgVector& A) const
+	{
+		return dgVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w);
+	}
+
+	DG_INLINE dgVector operator- (const dgVector& A) const 
+	{
+		return dgVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w);
+	}
+
+	DG_INLINE dgVector &operator+= (const dgVector& A)
+	{
+		return (*this = dgVector (m_x + A.m_x, m_y + A.m_y, m_z + A.m_z, m_w + A.m_w));
+	}
+
+	DG_INLINE dgVector &operator-= (const dgVector& A)
+	{
+		return (*this = dgVector (m_x - A.m_x, m_y - A.m_y, m_z - A.m_z, m_w - A.m_w));
+	}
+
+	DG_INLINE dgVector AddHorizontal () const
+	{
+		return dgVector (m_x + m_y + m_z + m_w);
+	}
+
+
+	DG_INLINE dgVector Scale3 (dgFloat32 scale) const
+	{
+		return dgVector (m_x * scale, m_y * scale, m_z * scale, m_w);
+	}
+
+	DG_INLINE dgVector Scale4 (dgFloat32 scale) const
+	{
+		return dgVector (m_x * scale, m_y * scale, m_z * scale, m_w * scale);
+	}
+
+
+	// component wise multiplication
+	DG_INLINE dgVector CompProduct3 (const dgVector& A) const
+	{
+		return dgVector (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, A.m_w);
+	}
+
+	// component wise 4d multiplication
+	DG_INLINE dgVector CompProduct4 (const dgVector& A) const
+	{
+		return dgVector (m_x * A.m_x, m_y * A.m_y, m_z * A.m_z, m_w * A.m_w);
+	}
+
+
+	// return dot product
+	DG_INLINE dgFloat32 operator% (const dgVector& A) const
+	{
+		return m_x * A.m_x + m_y * A.m_y + m_z * A.m_z;
+	}
+
+	// return cross product
+	DG_INLINE dgVector operator* (const dgVector& B) const
+	{
+		return dgVector (m_y * B.m_z - m_z * B.m_y,
+						 m_z * B.m_x - m_x * B.m_z,
+						 m_x * B.m_y - m_y * B.m_x, m_w);
+	}
+
+	
+	DG_INLINE dgVector GetInt () const
+	{
+		return dgVector (dgInt32 (dgFloor (m_x)), dgInt32(dgFloor (m_y)), dgInt32(dgFloor (m_z)), dgInt32 (dgFloor (m_w)));
+	}
+
+	DG_INLINE dgVector TestZero() const
+	{
+		const dgInt32* const a = (dgInt32*)&m_x;
+		return dgVector ((a[0] == 0) ? dgInt32 (0xffffffff) : dgInt32 (0),
+						 (a[1] == 0) ? dgInt32 (0xffffffff) : dgInt32 (0),
+						 (a[2] == 0) ? dgInt32 (0xffffffff) : dgInt32 (0),
+					     (a[3] == 0) ? dgInt32 (0xffffffff) : dgInt32 (0));
+	}
+
+
+	DG_INLINE dgVector Floor () const
+	{
+		return dgVector (dgFloor (m_x), dgFloor (m_y), dgFloor (m_z), dgFloor (m_w));
+	}
+
+	DG_INLINE dgVector DotProduct4 (const dgVector &A) const
+	{
+		return dgVector (m_x * A.m_x + m_y * A.m_y + m_z * A.m_z + m_w * A.m_w);
+	}
+
+	DG_INLINE dgVector InvMagSqrt () const
+	{
+		return dgVector (dgRsqrt (DotProduct4(*this).m_x));
+	}
+
+	DG_INLINE dgVector Reciproc () const
+	{
+		return dgVector (dgFloat32 (1.0f) / m_x, dgFloat32 (1.0f) / m_y, dgFloat32 (1.0f) / m_z, dgFloat32 (1.0f) / m_w);
+	}
+
+	DG_INLINE dgVector Sqrt () const
+	{
+		return dgVector (dgSqrt (m_x), dgSqrt (m_y), dgSqrt (m_z), dgSqrt (m_w));
+	}
+
+	DG_INLINE dgVector InvSqrt () const
+	{
+		return dgVector (dgRsqrt (m_x), dgRsqrt (m_y), dgRsqrt (m_z), dgRsqrt (m_w));
+	}
+
+	dgVector Abs () const
+	{
+		return dgVector ((m_x > dgFloat32 (0.0f)) ? m_x : -m_x,
+			             (m_y > dgFloat32 (0.0f)) ? m_y : -m_y,
+			             (m_z > dgFloat32 (0.0f)) ? m_z : -m_z,
+			             (m_w > dgFloat32 (0.0f)) ? m_w : -m_w);
+	}
+
+	dgVector GetMax (const dgVector& data) const
+	{
+		return dgVector ((m_x > data.m_x) ? m_x : data.m_x,
+			             (m_y > data.m_y) ? m_y : data.m_y,
+			             (m_z > data.m_z) ? m_z : data.m_z,
+			             (m_w > data.m_w) ? m_w : data.m_w);
+	}
+
+	dgVector GetMin (const dgVector& data) const
+	{
+		return dgVector ((m_x < data.m_x) ? m_x : data.m_x,
+			             (m_y < data.m_y) ? m_y : data.m_y,
+			             (m_z < data.m_z) ? m_z : data.m_z,
+			             (m_w < data.m_w) ? m_w : data.m_w);
+	}
+
+
+	// relational operators
+	DG_INLINE dgVector operator== (const dgVector& data) const
+	{
+		return dgVector ((m_x == data.m_x) ? dgInt32 (0xffffffff) : 0,
+					     (m_y == data.m_y) ? dgInt32 (0xffffffff) : 0,
+			             (m_z == data.m_z) ? dgInt32 (0xffffffff) : 0,
+			             (m_w == data.m_w) ? dgInt32 (0xffffffff) : 0);
+	}
+
+	DG_INLINE dgVector operator> (const dgVector& data) const
+	{
+		return dgVector ((m_x > data.m_x) ? dgInt32 (0xffffffff) : 0,
+					     (m_y > data.m_y) ? dgInt32 (0xffffffff) : 0,
+			             (m_z > data.m_z) ? dgInt32 (0xffffffff) : 0,
+			             (m_w > data.m_w) ? dgInt32 (0xffffffff) : 0);
+	}
+
+	DG_INLINE dgVector operator< (const dgVector& data) const
+	{
+		return dgVector ((m_x < data.m_x) ? dgInt32 (0xffffffff) : 0,
+					     (m_y < data.m_y) ? dgInt32 (0xffffffff) : 0,
+			             (m_z < data.m_z) ? dgInt32 (0xffffffff) : 0,
+			             (m_w < data.m_w) ? dgInt32 (0xffffffff) : 0);
+	}
+
+	DG_INLINE dgVector operator>= (const dgVector& data) const
+	{
+		return dgVector ((m_x >= data.m_x) ? dgInt32 (0xffffffff) : 0, 
+						 (m_y >= data.m_y) ? dgInt32 (0xffffffff) : 0,
+						 (m_z >= data.m_z) ? dgInt32 (0xffffffff) : 0,
+						 (m_w >= data.m_w) ? dgInt32 (0xffffffff) : 0);
+	}
+
+	DG_INLINE dgVector operator<= (const dgVector& data) const
+	{
+		return dgVector ((m_x <= data.m_x) ? dgInt32 (0xffffffff) : 0,
+						 (m_y <= data.m_y) ? dgInt32 (0xffffffff) : 0,
+						 (m_z <= data.m_z) ? dgInt32 (0xffffffff) : 0,
+						 (m_w <= data.m_w) ? dgInt32 (0xffffffff) : 0);
+	}
+
+
+	// logical operations
+	DG_INLINE dgVector operator& (const dgVector& data) const
+	{
+		const dgInt32* const a = (dgInt32*)&m_x;
+		const dgInt32* const b = (dgInt32*)&data.m_x;
+		return dgVector (a[0] & b[0], a[1] & b[1], a[2] & b[2], a[3] & b[3]); 
+	}
+
+	DG_INLINE dgVector operator| (const dgVector& data) const
+	{
+		const dgInt32* const a = (dgInt32*)&m_x;
+		const dgInt32* const b = (dgInt32*)&data.m_x;
+		return dgVector (a[0] | b[0], a[1] | b[1], a[2] | b[2], a[3] | b[3]); 
+	}
+
+	DG_INLINE dgVector operator^ (const dgVector& data) const
+	{
+		const dgInt32* const a = (dgInt32*)&m_x;
+		const dgInt32* const b = (dgInt32*)&data.m_x;
+		return dgVector (a[0] ^ b[0], a[1] ^ b[1], a[2] ^ b[2], a[3] ^ b[3]); 
+	}
+
+	DG_INLINE dgVector AndNot (const dgVector& data) const
+	{
+		const dgInt32* const a = (dgInt32*)&m_x;
+		const dgInt32* const b = (dgInt32*)&data.m_x;
+		return dgVector (a[0] & ~b[0], a[1] & ~b[1], a[2] & ~b[2], a[3] & ~b[3]); 
+	}
+
+	DG_INLINE dgInt32 GetSignMask() const
+	{
+		const dgInt32* const a = (dgInt32*)&m_x;
+		return (((a[0] & 0x80000000) ? 1 : 0) | ((a[1] & 0x80000000) ? 2 : 0) | ((a[2] & 0x80000000) ? 4 : 0) | ((a[3] & 0x80000000) ? 8 : 0));
+	} 
+
+	DG_INLINE dgVector ShiftTripleRight () const
+	{
+		return dgVector (m_z, m_x, m_y, m_w); 
+	}
+
+
+	DG_INLINE dgVector MoveLow (const dgVector& data) const
+	{
+		return dgVector (m_x, m_y, data.m_x, data.m_y); 
+	}
+
+	DG_INLINE dgVector MoveHigh (const dgVector& data) const
+	{
+		return dgVector (data.m_z, data.m_w, m_z, m_w); 
+	}
+
+	DG_INLINE dgVector PackLow (const dgVector& data) const
+	{
+		return dgVector (m_x, data.m_x, m_y, data.m_y); 
+	}
+
+	DG_INLINE dgVector PackHigh (const dgVector& data) const
+	{
+		return dgVector (m_z, data.m_z, m_w, data.m_w); 
+	}
+
+	DG_INLINE static void Transpose4x4 (dgVector& dst0, dgVector& dst1, dgVector& dst2, dgVector& dst3, 
+										const dgVector& src0, const dgVector& src1, const dgVector& src2, const dgVector& src3)
+	{
+		dgVector tmp0 (src0.PackLow(src1));
+		dgVector tmp1 (src2.PackLow(src3));
+		dgVector tmp2 (src0.PackHigh(src1));
+		dgVector tmp3 (src2.PackHigh(src3));
+
+		dst0 = tmp0.MoveLow (tmp1);
+		dst1 = tmp1.MoveHigh (tmp0);
+		dst2 = tmp2.MoveLow (tmp3);
+		dst3 = tmp3.MoveHigh (tmp2);
+	}
+
+	DG_CLASS_ALLOCATOR(allocator)
+
+	union {
+		struct {
+			dgFloat32 m_x;
+			dgFloat32 m_y;
+			dgFloat32 m_z;
+			dgFloat32 m_w;
+		};
+		struct {
+			dgInt32 m_ix;
+			dgInt32 m_iy;
+			dgInt32 m_iz;
+			dgInt32 m_iw;
+		};
+	};
+
+
+	static dgVector m_one;
+	static dgVector m_wOne;
+	static dgVector m_half;
+	static dgVector m_two;
+	static dgVector m_three;
+	static dgVector m_negOne;
+	static dgVector m_signMask;
+	static dgVector m_triplexMask;
+}DG_GCC_VECTOR_ALIGMENT;
+
+
+#else
+
+DG_MSC_VECTOR_ALIGMENT
+class dgVector
+{
+	#define PURMUT_MASK(w, z, y, x)		_MM_SHUFFLE (w, z, y, x)
+
+	public:
+	DG_INLINE dgVector() 
+	{
+	}
+
+	DG_INLINE dgVector(const __m128 type)
+		:m_type (type)
+	{
+	}
+
+	DG_INLINE dgVector(const __m128i type)
+		:m_typeInt (type)
+	{
+	}
+
+	DG_INLINE dgVector (const dgFloat32 a)
+		: m_type(_mm_set_ps1(a)) 
+	{
+	}
+
+	DG_INLINE dgVector (const dgFloat32* const ptr)
+		:m_type(_mm_loadu_ps (ptr))
+	{
+		m_type = _mm_and_ps (m_type, m_triplexMask.m_type);
+	}
+
+	DG_INLINE dgVector (const dgVector& copy)
+		:m_type(copy.m_type)
+	{
+	}
+
+
+	DG_INLINE dgVector (const dgBigVector& copy)
+		:m_type(_mm_set_ps(dgFloat32 (copy.m_w), dgFloat32 (copy.m_z), dgFloat32 (copy.m_y), dgFloat32 (copy.m_x)))
+	{
+	}
+
+	DG_INLINE dgVector (dgFloat32 x, dgFloat32 y, dgFloat32 z, dgFloat32 w)
+		:m_type(_mm_set_ps(w, z, y, x))
+	{
+	}
+
+	DG_INLINE dgVector (dgInt32 ix, dgInt32 iy, dgInt32 iz, dgInt32 iw)
+		:m_type(_mm_set_ps(*(dgFloat32*)&iw, *(dgFloat32*)&iz, *(dgFloat32*)&iy, *(dgFloat32*)&ix))
+	{
+	}
+	
+
+	DG_INLINE dgFloat32 GetScalar () const
+	{
+		dgFloat32 scalar;
+		_mm_store_ss(&scalar, m_type);
+		return scalar;
+	}
+
+	DG_INLINE void Store (dgFloat32* const dst) const
+	{
+		_mm_storeu_ps(dst, m_type);
+	}
+
+	DG_INLINE dgVector BroadcastX () const
+	{
+		return _mm_shuffle_ps (m_type, m_type, PURMUT_MASK(0, 0, 0, 0));
+	}
+
+	DG_INLINE dgVector BroadcastY () const
+	{
+		return _mm_shuffle_ps (m_type, m_type, PURMUT_MASK(1, 1, 1, 1));
+	}
+
+	DG_INLINE dgVector BroadcastZ () const
+	{
+		return _mm_shuffle_ps (m_type, m_type, PURMUT_MASK(2, 2, 2, 2));
+	}
+
+	DG_INLINE dgVector BroadcastW () const
+	{
+		return _mm_shuffle_ps (m_type, m_type, PURMUT_MASK(3, 3, 3, 3));
+	}
+	
+
+	DG_INLINE dgVector Scale3 (dgFloat32 s) const
+	{
+		dgVector tmp (s, s, s, dgFloat32 (1.0f));
+		return _mm_mul_ps (m_type, tmp.m_type);
+	}
+
+	DG_INLINE dgVector Scale4 (dgFloat32 s) const
+	{
+		return _mm_mul_ps (m_type, _mm_set_ps1(s));
+	}
+
+
+	DG_INLINE dgFloat32& operator[] (dgInt32 i)
+	{
+		dgAssert (i < 4);
+		dgAssert (i >= 0);
+		return m_f[i];
+	}
+
+	DG_INLINE const dgFloat32& operator[] (dgInt32 i) const
+	{
+		dgAssert (i < 4);
+		dgAssert (i >= 0);
+		return m_f[i];
+	}
+
+	DG_INLINE dgVector operator+ (const dgVector& A) const
+	{
+		return _mm_add_ps (m_type, A.m_type);	
+	}
+
+
+	DG_INLINE dgVector operator- (const dgVector& A) const 
+	{
+		return _mm_sub_ps (m_type, A.m_type);	
+	}
+
+	DG_INLINE dgVector &operator+= (const dgVector& A)
+	{
+		return (*this = _mm_add_ps (m_type, A.m_type));
+	}
+
+	DG_INLINE dgVector &operator-= (const dgVector& A)
+	{
+		return (*this = _mm_sub_ps (m_type, A.m_type));
+	}
+
+	// return dot product
+	DG_INLINE dgFloat32 operator% (const dgVector& A) const
+	{
+		#ifdef DG_SSE4_INSTRUCTIONS_SET 
+			return dgVector (_mm_dp_ps (m_type, A.m_type, 0x77)).m_x; 
+		#else
+			dgVector tmp (A & m_triplexMask);
+			dgAssert ((m_w * tmp.m_w) == dgFloat32 (0.0f));
+			return CompProduct4(tmp).AddHorizontal().m_x;
+		#endif
+	}
+
+	DG_INLINE dgVector DotProduct4 (const dgVector& A) const
+	{
+		#ifdef DG_SSE4_INSTRUCTIONS_SET 
+			return _mm_dp_ps (m_type, A.m_type, 0xff); 
+		#else 
+			return CompProduct4(A).AddHorizontal();
+		#endif
+	}
+
+	// return cross product
+	DG_INLINE dgVector operator* (const dgVector& B) const
+	{
+		return _mm_sub_ps (_mm_mul_ps (_mm_shuffle_ps (m_type, m_type, PURMUT_MASK(3, 0, 2, 1)), _mm_shuffle_ps (B.m_type, B.m_type, PURMUT_MASK(3, 1, 0, 2))),
+						   _mm_mul_ps (_mm_shuffle_ps (m_type, m_type, PURMUT_MASK(3, 1, 0, 2)), _mm_shuffle_ps (B.m_type, B.m_type, PURMUT_MASK(3, 0, 2, 1))));
+
+	}
+
+	DG_INLINE dgVector CrossProduct4 (const dgVector& A, const dgVector& B) const
+	{
+		dgFloat32 cofactor[3][3];
+		dgFloat32 array[4][4];
+
+		const dgVector& me = *this;
+		for (dgInt32 i = 0; i < 4; i ++) {
+			array[0][i] = me[i];
+			array[1][i] = A[i];
+			array[2][i] = B[i];
+			array[3][i] = dgFloat32 (1.0f);
+		}
+
+		dgVector normal;
+		dgFloat32  sign = dgFloat32 (-1.0f);
+		for (dgInt32 i = 0; i < 4; i ++)  {
+
+			for (dgInt32 j = 0; j < 3; j ++) {
+				dgInt32 k0 = 0;
+				for (dgInt32 k = 0; k < 4; k ++) {
+					if (k != i) {
+						cofactor[j][k0] = array[j][k];
+						k0 ++;
+					}
+				}
+			}
+			dgFloat32  x = cofactor[0][0] * (cofactor[1][1] * cofactor[2][2] - cofactor[1][2] * cofactor[2][1]);
+			dgFloat32  y = cofactor[0][1] * (cofactor[1][2] * cofactor[2][0] - cofactor[1][0] * cofactor[2][2]);
+			dgFloat32  z = cofactor[0][2] * (cofactor[1][0] * cofactor[2][1] - cofactor[1][1] * cofactor[2][0]);
+			dgFloat32  det = x + y + z;
+
+			normal[i] = sign * det;
+			sign *= dgFloat32 (-1.0f);
+		}
+
+		return normal;
+	}
+
+	// component wise multiplication
+	DG_INLINE dgVector CompProduct3 (const dgVector& A) const
+	{
+		dgVector tmp ((A & m_triplexMask) | m_wOne);
+		return _mm_mul_ps (m_type, tmp.m_type);
+	}
+
+	DG_INLINE dgVector Reciproc () const
+	{
+		return _mm_div_ps (m_one.m_type, m_type);
+	}
+
+	// component wise multiplication
+	DG_INLINE dgVector CompProduct4 (const dgVector& A) const
+	{
+		return _mm_mul_ps (m_type, A.m_type);
+	}
+
+	DG_INLINE dgVector AddHorizontal () const
+	{
+		dgVector tmp (_mm_hadd_ps (m_type, m_type));
+		return _mm_hadd_ps (tmp.m_type, tmp.m_type);
+	}
+
+	DG_INLINE dgVector Abs () const
+	{
+//		__m128i shitSign = _mm_srli_epi32 (_mm_slli_epi32 (*((__m128i*) &m_type), 1), 1);
+//		return *(__m128*)&shitSign;
+		return _mm_and_ps (m_type, m_signMask.m_type);
+	}
+
+	dgVector GetMax (const dgVector& data) const
+	{
+		return _mm_max_ps (m_type, data.m_type);
+	}
+
+	dgVector GetMin (const dgVector& data) const
+	{
+		return _mm_min_ps (m_type, data.m_type);
+	}
+
+	//DG_INLINE dgInt32 GetInt () const
+	DG_INLINE dgVector GetInt () const
+	{
+		//return _mm_cvtss_si32(m_type);
+		return dgVector(_mm_cvtps_epi32(Floor().m_type));
+	}
+
+	DG_INLINE dgVector TestZero() const
+	{
+		return _mm_cmpeq_epi32 (m_typeInt, dgVector (dgFloat32 (0.0f)).m_typeInt);
+	}
+
+	DG_INLINE dgVector Floor () const
+	{
+//		dgVector mask ((dgFloat32 (1.5f) * dgFloat32 (1<<23)));
+//		dgVector ret (_mm_sub_ps(_mm_add_ps(m_type, mask.m_type), mask.m_type));
+//		dgVector adjust (_mm_cmplt_ps (m_type, ret.m_type));
+//		ret = _mm_sub_ps (ret.m_type, _mm_and_ps(_mm_set_ps1(1.0), adjust.m_type));
+
+		dgVector truncated (_mm_cvtepi32_ps (_mm_cvttps_epi32 (m_type)));
+		dgVector ret (truncated - (dgVector::m_one & (*this < truncated)));
+		dgAssert (ret.m_f[0] == dgFloor(m_f[0]));
+		dgAssert (ret.m_f[1] == dgFloor(m_f[1]));
+		dgAssert (ret.m_f[2] == dgFloor(m_f[2]));
+		dgAssert (ret.m_f[3] == dgFloor(m_f[3]));
+		return ret;
+	}
+
+	DG_INLINE dgVector Sqrt () const
+	{
+		return dgVector (_mm_sqrt_ps(m_type));
+	}
+
+	DG_INLINE dgVector InvSqrt () const
+	{
+		dgVector tmp0 (_mm_rsqrt_ps(m_type));
+		return m_half.CompProduct4(tmp0).CompProduct4((m_three - CompProduct4(tmp0).CompProduct4(tmp0)));
+	}
+
+	DG_INLINE dgVector InvMagSqrt () const
+	{
+		return DotProduct4(*this).InvSqrt();
+	}
+
+	
+	// relational operators
+	DG_INLINE dgVector operator> (const dgVector& data) const
+	{
+		return _mm_cmpgt_ps (m_type, data.m_type);	
+	}
+
+	DG_INLINE dgVector operator== (const dgVector& data) const
+	{
+		return _mm_cmpeq_ps (m_type, data.m_type);	
+	}
+
+	DG_INLINE dgVector operator< (const dgVector& data) const
+	{
+		return _mm_cmplt_ps (m_type, data.m_type);	
+	}
+
+	DG_INLINE dgVector operator>= (const dgVector& data) const
+	{
+		return _mm_cmpge_ps (m_type, data.m_type);	
+	}
+
+	DG_INLINE dgVector operator<= (const dgVector& data) const
+	{
+		return _mm_cmple_ps (m_type, data.m_type);	
+	}
+
+	// logical operations
+	DG_INLINE dgVector operator& (const dgVector& data) const
+	{
+		return _mm_and_ps (m_type, data.m_type);	
+	}
+
+	DG_INLINE dgVector operator| (const dgVector& data) const
+	{
+		return _mm_or_ps (m_type, data.m_type);	
+	}
+
+	DG_INLINE dgVector operator^ (const dgVector& data) const
+	{
+		return _mm_xor_ps (m_type, data.m_type);	
+	}
+
+	DG_INLINE dgVector AndNot (const dgVector& data) const
+	{
+		return _mm_andnot_ps (data.m_type, m_type);	
+	}
+
+	DG_INLINE dgInt32 GetSignMask() const
+	{
+		return _mm_movemask_ps(m_type);
+	} 
+
+	DG_INLINE dgVector ShiftTripleRight () const
+	{
+		return _mm_shuffle_ps(m_type, m_type, PURMUT_MASK(3, 1, 0, 2));
+	}
+
+
+	DG_INLINE dgVector MoveLow (const dgVector& data) const
+	{
+		return _mm_movelh_ps (m_type, data.m_type);
+	}
+
+	DG_INLINE dgVector MoveHigh (const dgVector& data) const
+	{
+		return _mm_movehl_ps (m_type, data.m_type);
+	}
+
+	DG_INLINE dgVector PackLow (const dgVector& data) const
+	{
+		return _mm_unpacklo_ps (m_type, data.m_type);
+	}
+
+	DG_INLINE dgVector PackHigh (const dgVector& data) const
+	{
+		return _mm_unpackhi_ps (m_type, data.m_type);
+	}
+
+
+	DG_INLINE static void Transpose4x4 (dgVector& dst0, dgVector& dst1, dgVector& dst2, dgVector& dst3, 
+										const dgVector& src0, const dgVector& src1, const dgVector& src2, const dgVector& src3)
+	{
+		dgVector tmp0 (src0.PackLow(src1));
+		dgVector tmp1 (src2.PackLow(src3));
+		dgVector tmp2 (src0.PackHigh(src1));
+		dgVector tmp3 (src2.PackHigh(src3));
+
+		dst0 = tmp0.MoveLow (tmp1);
+		dst1 = tmp1.MoveHigh (tmp0);
+		dst2 = tmp2.MoveLow (tmp3);
+		dst3 = tmp3.MoveHigh (tmp2);
+	}
+
+
+	DG_CLASS_ALLOCATOR(allocator)
+	
+	union {
+		__m128 m_type;
+		__m128i m_typeInt;
+		dgFloat32 m_f[4];
+		struct {
+			dgFloat32 m_x;
+			dgFloat32 m_y;
+			dgFloat32 m_z;
+			dgFloat32 m_w;
+		};
+		struct {
+			dgInt32 m_ix;
+			dgInt32 m_iy;
+			dgInt32 m_iz;
+			dgInt32 m_iw;
+		};
+	};
+
+	static dgVector m_one;
+	static dgVector m_wOne;
+	static dgVector m_two;
+	static dgVector m_half;
+	static dgVector m_three;
+	static dgVector m_negOne;
+	static dgVector m_signMask;
+	static dgVector m_triplexMask;
+} DG_GCC_VECTOR_ALIGMENT;
+
+#endif
+#endif
+

newtondynamics.mod/NewtonDynamics/source/meshUtil/dgMeshEffect.h

@@ -0,0 +1,427 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __dgMeshEffect_H__
+#define __dgMeshEffect_H__
+
+#include <dgRefCounter.h>
+
+class dgWorld;
+class dgMeshEffect;
+class dgCollisionInstance;
+
+#define DG_MESH_EFFECT_PRECISION_BITS		48
+#define DG_MESH_EFFECT_PRECISION_SCALE		dgFloat64(dgInt64(1)<<DG_MESH_EFFECT_PRECISION_BITS)
+#define DG_MESH_EFFECT_PRECISION_SCALE_INV	(dgFloat64 (1.0f) / DG_MESH_EFFECT_PRECISION_SCALE)
+
+
+#define DG_MESH_EFFECT_INITIAL_VERTEX_SIZE	8
+#define DG_VERTEXLIST_INDEXLIST_TOL			(dgFloat64 (0.0f))
+#define DG_MESH_EFFECT_POINT_SPLITED		512
+
+
+
+#define DG_MESH_EFFECT_BVH_STACK_DEPTH		256
+
+
+class dgMeshEffect: public dgPolyhedra, public dgRefCounter
+{
+	public:
+
+	class dgVertexAtribute 
+	{
+		public:
+		dgBigVector m_vertex;
+		dgFloat64 m_normal_x;
+		dgFloat64 m_normal_y;
+		dgFloat64 m_normal_z;
+		dgFloat64 m_u0;
+		dgFloat64 m_v0;
+		dgFloat64 m_u1;
+		dgFloat64 m_v1;
+		dgFloat64 m_material;
+	};
+
+	class dgIndexArray 
+	{
+		public:
+		dgInt32 m_materialCount;
+		dgInt32 m_indexCount;
+		dgInt32 m_materials[256];
+		dgInt32 m_materialsIndexCount[256];
+		dgInt32* m_indexList;
+	};
+
+
+
+	public:
+	class dgMeshBVH
+	{
+		public:
+		class dgMeshBVHNode
+		{
+			public:
+			dgMeshBVHNode (const dgMeshEffect* const mesh, dgEdge* const face, void* const userData);
+			dgMeshBVHNode (dgMeshBVHNode* const left, dgMeshBVHNode* const right);
+			~dgMeshBVHNode ();
+			void SetBox (const dgVector& p0, const dgVector& p1);
+
+			DG_CLASS_ALLOCATOR(allocator)
+			dgVector m_p0;
+			dgVector m_p1;
+
+			dgFloat32 m_area;
+			dgEdge* m_face;
+			void* m_userData;
+			dgMeshBVHNode* m_left;
+			dgMeshBVHNode* m_right;
+			dgMeshBVHNode* m_parent;
+		};
+
+		class dgFitnessList: public dgTree <dgMeshBVHNode*, dgMeshBVHNode*>
+		{
+			public:
+			dgFitnessList (dgMemoryAllocator* const allocator);
+			dgFloat64 TotalCost () const;
+		};
+
+		
+		dgMeshBVH (dgMeshEffect* const mesh);
+		virtual ~dgMeshBVH();
+
+		virtual void Build ();
+		virtual void Cleanup ();
+
+		void GetOverlapNodes (dgList<dgMeshBVHNode*>& overlapNodes, const dgBigVector& p0, const dgBigVector& p1) const;
+		dgMeshBVHNode* FaceRayCast (const dgBigVector& l0, const dgBigVector& l1, dgFloat64& paramOut, bool doubleSidedFaces) const;
+
+		protected:
+		dgMeshBVHNode* AddFaceNode (dgEdge* const face, void* const userData);
+		void RemoveNode (dgMeshBVHNode* const treeNode);
+		void ImproveNodeFitness ();
+		void ImproveNodeFitness (dgMeshBVHNode* const node);
+		dgFloat32 CalculateSurfaceArea (dgMeshBVHNode* const node0, dgMeshBVHNode* const node1, dgVector& minBox, dgVector& maxBox) const;
+		virtual bool SanityCheck() const;
+
+		virtual dgFloat64 VertexRayCast (const dgBigVector& l0, const dgBigVector& l1) const;
+		virtual dgFloat64 RayFaceIntersect (const dgMeshBVHNode* const face, const dgBigVector& p0, const dgBigVector& p1, bool dobleSidedFaces) const;
+		virtual bool RayRayIntersect (dgEdge* const edge, const dgMeshEffect* const otherMesh, dgEdge* const otherEdge, dgFloat64& param, dgFloat64& otherParam) const;
+		
+		dgMeshEffect* m_mesh;
+		dgMeshBVHNode* m_rootNode;
+		dgFitnessList m_fitness;
+		friend class dgMeshEffect;
+	};
+
+
+
+	dgMeshEffect(dgMemoryAllocator* const allocator);
+	dgMeshEffect(dgCollisionInstance* const collision);
+	dgMeshEffect(const dgMeshEffect& source);
+	dgMeshEffect(dgPolyhedra& mesh, const dgMeshEffect& source);
+	dgMeshEffect (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData);
+
+	// create from OFF or PLY file format
+	dgMeshEffect(dgMemoryAllocator* const allocator, const char* const fileName);
+
+	// Create a convex hull Mesh form point cloud
+	dgMeshEffect (dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol);
+
+	// create a planar Mesh
+	dgMeshEffect(dgMemoryAllocator* const allocator, const dgMatrix& planeMatrix, dgFloat32 witdth, dgFloat32 breadth, dgInt32 material, const dgMatrix& textureMatrix0, const dgMatrix& textureMatrix1);
+	virtual ~dgMeshEffect(void);
+
+	void Trace () const;
+	void SaveOFF (const char* const fileName) const;
+
+	void ApplyTransform (const dgMatrix& matrix);
+	dgMatrix CalculateOOBB (dgBigVector& size) const;
+	void CalculateAABB (dgBigVector& min, dgBigVector& max) const;
+
+	void CalculateNormals (dgFloat64 angleInRadians);
+	void SphericalMapping (dgInt32 material);
+	void BoxMapping (dgInt32 front, dgInt32 side, dgInt32 top);
+	void UniformBoxMapping (dgInt32 material, const dgMatrix& textruMatrix);
+	void CylindricalMapping (dgInt32 cylinderMaterial, dgInt32 capMaterial);
+	void AngleBaseFlatteningMapping (dgInt32 cylinderMaterial, dgReportProgress progressReportCallback, void* const userData);
+
+	dgEdge* InsertEdgeVertex (dgEdge* const edge, dgFloat64 param);
+
+	dgMeshEffect* Union (const dgMatrix& matrix, const dgMeshEffect* const clipper) const;
+	dgMeshEffect* Difference (const dgMatrix& matrix, const dgMeshEffect* const clipper) const;
+	dgMeshEffect* Intersection (const dgMatrix& matrix, const dgMeshEffect* const clipper) const;
+	void ClipMesh (const dgMatrix& matrix, const dgMeshEffect* const clipper, dgMeshEffect** const top, dgMeshEffect** const bottom) const;
+
+	//bool PlaneClip (const dgBigPlane& plane);
+	
+	dgMeshEffect* ConvexMeshIntersection (const dgMeshEffect* const convexMesh) const;
+
+	dgMeshEffect* GetFirstLayer ();
+	dgMeshEffect* GetNextLayer (dgMeshEffect* const layer);
+
+	void Triangulate ();
+	void ConvertToPolygons ();
+	void RemoveUnusedVertices(dgInt32* const vertexRemapTable);
+	
+	void BeginPolygon ();
+	void AddPolygon (dgInt32 count, const dgFloat32* const vertexList, dgInt32 stride, dgInt32 material);
+#ifndef _NEWTON_USE_DOUBLE
+	void AddPolygon (dgInt32 count, const dgFloat64* const vertexList, dgInt32 stride, dgInt32 material);
+#endif
+	void EndPolygon (dgFloat64 tol, bool fixTjoint = true);
+
+	void PackVertexArrays ();
+
+	void BuildFromVertexListIndexList(dgInt32 faceCount, const dgInt32 * const faceIndexCount, const dgInt32 * const faceMaterialIndex, 
+		const dgFloat32* const vertex, dgInt32  vertexStrideInBytes, const dgInt32 * const vertexIndex,
+		const dgFloat32* const normal, dgInt32  normalStrideInBytes, const dgInt32 * const normalIndex,
+		const dgFloat32* const uv0, dgInt32  uv0StrideInBytes, const dgInt32 * const uv0Index,
+		const dgFloat32* const uv1, dgInt32  uv1StrideInBytes, const dgInt32 * const uv1Index);
+
+
+	dgInt32 GetVertexCount() const;
+	dgInt32 GetVertexStrideInByte() const;
+	dgFloat64* GetVertexPool () const;
+
+	dgInt32 GetPropertiesCount() const;
+	dgInt32 GetPropertiesStrideInByte() const;
+	dgFloat64* GetAttributePool() const;
+	dgFloat64* GetNormalPool() const;
+	dgFloat64* GetUV0Pool() const;
+	dgFloat64* GetUV1Pool() const;
+
+	dgEdge* SpliteFace (dgInt32 v0, dgInt32 v1);
+
+	dgInt32 GetTotalFaceCount() const;
+	dgInt32 GetTotalIndexCount() const;
+	void GetFaces (dgInt32* const faceCount, dgInt32* const materials, void** const faceNodeList) const;
+
+	void RepairTJoints ();
+	bool SeparateDuplicateLoops (dgEdge* const face);
+
+	bool HasOpenEdges () const;
+
+	dgFloat64 CalculateVolume () const;
+
+	void GetVertexStreams (dgInt32 vetexStrideInByte, dgFloat32* const vertex, 
+						   dgInt32 normalStrideInByte, dgFloat32* const normal, 
+						   dgInt32 uvStrideInByte0, dgFloat32* const uv0, 
+						   dgInt32 uvStrideInByte1, dgFloat32* const uv1);
+
+	void GetIndirectVertexStreams(dgInt32 vetexStrideInByte, dgFloat64* const vertex, dgInt32* const vertexIndices, dgInt32* const vertexCount,
+								  dgInt32 normalStrideInByte, dgFloat64* const normal, dgInt32* const normalIndices, dgInt32* const normalCount,
+								  dgInt32 uvStrideInByte0, dgFloat64* const uv0, dgInt32* const uvIndices0, dgInt32* const uvCount0,
+								  dgInt32 uvStrideInByte1, dgFloat64* const uv1, dgInt32* const uvIndices1, dgInt32* const uvCount1);
+
+	
+
+	dgIndexArray* MaterialGeometryBegin();
+	void MaterialGeomteryEnd(dgIndexArray* const handle);
+	dgInt32 GetFirstMaterial (dgIndexArray* const handle) const;
+	dgInt32 GetNextMaterial (dgIndexArray* const handle, dgInt32 materialHandle) const;
+	dgInt32 GetMaterialID (dgIndexArray* const handle, dgInt32 materialHandle) const;
+	dgInt32 GetMaterialIndexCount (dgIndexArray* const handle, dgInt32 materialHandle) const;
+	void GetMaterialGetIndexStream (dgIndexArray* const handle, dgInt32 materialHandle, dgInt32* const index) const;
+	void GetMaterialGetIndexStreamShort (dgIndexArray* const handle, dgInt32 materialHandle, dgInt16* const index) const;
+	
+	dgCollisionInstance* CreateCollisionTree(dgWorld* const world, dgInt32 shapeID) const;
+	dgCollisionInstance* CreateConvexCollision(dgWorld* const world, dgFloat64 tolerance, dgInt32 shapeID, const dgMatrix& matrix = dgGetIdentityMatrix()) const;
+
+	dgMeshEffect* CreateSimplification (dgInt32 maxVertexCount, dgReportProgress reportProgressCallback, void* const userData) const;
+	dgMeshEffect* CreateConvexApproximation (dgFloat32 maxConcavity, dgFloat32 backFaceDistanceFactor, dgInt32 maxHullOuputCount, dgInt32 maxVertexPerHull, dgReportProgress reportProgressCallback, void* const userData) const;
+
+	static dgMeshEffect* CreateDelaunayTetrahedralization (dgMemoryAllocator* const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32* const pointCloud, dgInt32 materialId, const dgMatrix& textureProjectionMatrix);
+	static dgMeshEffect* CreateVoronoiConvexDecomposition (dgMemoryAllocator* const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32* const pointCloud, dgInt32 materialId, const dgMatrix& textureProjectionMatrix);
+	static dgMeshEffect* CreateFromSerialization (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData);
+
+	void Serialize (dgSerialize callback, void* const userData) const;
+
+	dgBigVector& GetVertex (dgInt32 index) const;
+	dgVertexAtribute& GetAttribute (dgInt32 index) const;
+	void TransformMesh (const dgMatrix& matrix);
+
+
+	void* GetFirstVertex () const;
+	void* GetNextVertex (const void* const vertex) const;
+	int GetVertexIndex (const void* const vertex) const;
+
+	void* GetFirstPoint () const;
+	void* GetNextPoint (const void* const point) const;
+	int GetPointIndex (const void* const point) const;
+	int GetVertexIndexFromPoint (const void* const point) const;
+
+
+	void* GetFirstEdge () const;
+	void* GetNextEdge (const void* const edge) const;
+	void GetEdgeIndex (const void* const edge, dgInt32& v0, dgInt32& v1) const;
+//	void GetEdgeAttributeIndex (const void* edge, dgInt32& v0, dgInt32& v1) const;
+
+	void* GetFirstFace () const;
+	void* GetNextFace (const void* const face) const;
+	int IsFaceOpen (const void* const face) const;
+	int GetFaceMaterial (const void* const face) const;
+	int GetFaceIndexCount (const void* const face) const;
+	void GetFaceIndex (const void* const face, dgInt32* const indices) const;
+	void GetFaceAttributeIndex (const void* const face, dgInt32* const indices) const;
+	dgBigVector CalculateFaceNormal (const void* const face) const;
+
+	void SetFaceMaterial (const void* const face, int materialID) const;
+
+	
+	dgVertexAtribute InterpolateVertex (const dgBigVector& point, const dgEdge* const face) const;
+
+	bool Sanity () const;
+	void AddVertex(const dgBigVector& vertex);
+	void AddAtribute (const dgVertexAtribute& attib);
+	void AddPoint(const dgFloat64* vertexList, dgInt32 material);
+
+	protected:
+	virtual void BeginFace();
+	virtual void EndFace ();
+
+	void Init ();
+	dgBigVector GetOrigin ()const;
+	dgInt32 CalculateMaxAttributes () const;
+	dgFloat64 QuantizeCordinade(dgFloat64 val) const;
+
+	void ClearAttributeArray ();
+	dgInt32 EnumerateAttributeArray (dgVertexAtribute* const attib);
+	void ApplyAttributeArray (dgVertexAtribute* const attib, dgInt32 maxCount);
+	
+	void MergeFaces (const dgMeshEffect* const source);
+//	void ReverseMergeFaces (dgMeshEffect* const source);
+	dgVertexAtribute InterpolateEdge (dgEdge* const edge, dgFloat64 param) const;
+
+	bool PlaneClip (const dgMeshEffect& convexMesh, const dgEdge* const face);
+
+	dgMeshEffect* GetNextLayer (dgInt32 mark);
+	dgMeshEffect* CreateVoronoiConvex (const dgBigVector* const conevexPointCloud, dgInt32 count, dgInt32 materialId, const dgMatrix& textureProjectionMatrix, dgFloat32 normalAngleInRadians) const;
+
+
+	dgInt32 m_pointCount;
+	dgInt32 m_maxPointCount;
+
+	dgInt32 m_atribCount;
+	dgInt32 m_maxAtribCount;
+
+	dgBigVector* m_points;
+	dgVertexAtribute* m_attrib;
+	
+	friend class dgConvexHull3d;
+	friend class dgConvexHull4d;
+	friend class dgBooleanMeshBVH;
+	friend class dgTriangleAnglesToUV;
+	friend class dgCollisionCompoundFractured;
+};
+
+
+
+DG_INLINE dgInt32 dgMeshEffect::GetVertexCount() const
+{
+	return m_pointCount;
+}
+
+DG_INLINE dgInt32 dgMeshEffect::GetPropertiesCount() const
+{
+	return m_atribCount;
+}
+
+DG_INLINE dgInt32 dgMeshEffect::GetMaterialID (dgIndexArray* const handle, dgInt32 materialHandle) const
+{
+	return handle->m_materials[materialHandle];
+}
+
+DG_INLINE dgInt32 dgMeshEffect::GetMaterialIndexCount (dgIndexArray* const handle, dgInt32 materialHandle) const
+{
+	return handle->m_materialsIndexCount[materialHandle];
+}
+
+DG_INLINE dgMeshEffect::dgVertexAtribute& dgMeshEffect::GetAttribute (dgInt32 index) const 
+{
+	return m_attrib[index];
+}
+
+DG_INLINE dgBigVector& dgMeshEffect::GetVertex (dgInt32 index) const
+{
+	return m_points[index];
+}
+
+DG_INLINE dgInt32 dgMeshEffect::GetPropertiesStrideInByte() const 
+{
+	return sizeof (dgVertexAtribute);
+}
+
+DG_INLINE dgFloat64* dgMeshEffect::GetAttributePool() const 
+{
+	return &m_attrib->m_vertex.m_x;
+}
+
+DG_INLINE dgFloat64* dgMeshEffect::GetNormalPool() const 
+{
+	return &m_attrib->m_normal_x;
+}
+
+DG_INLINE dgFloat64* dgMeshEffect::GetUV0Pool() const 
+{
+	return &m_attrib->m_u0;
+}
+
+DG_INLINE dgFloat64* dgMeshEffect::GetUV1Pool() const 
+{
+	return &m_attrib->m_u1;
+}
+
+DG_INLINE dgInt32 dgMeshEffect::GetVertexStrideInByte() const 
+{
+	return sizeof (dgBigVector);
+}
+
+DG_INLINE dgFloat64* dgMeshEffect::GetVertexPool () const 
+{
+	return &m_points[0].m_x;
+}
+
+
+DG_INLINE dgMeshEffect* dgMeshEffect::GetFirstLayer ()
+{
+	return GetNextLayer (IncLRU());
+}
+
+DG_INLINE dgMeshEffect* dgMeshEffect::GetNextLayer (dgMeshEffect* const layerSegment)
+{
+	if (!layerSegment) {
+		return NULL;
+	}
+	return GetNextLayer (layerSegment->IncLRU() - 1);
+}
+
+
+DG_INLINE dgFloat64 dgMeshEffect::QuantizeCordinade(dgFloat64 x) const
+{
+	dgInt32 exp;
+	dgFloat64 mantissa = frexp(x, &exp);
+	mantissa = DG_MESH_EFFECT_PRECISION_SCALE_INV * floor (mantissa * DG_MESH_EFFECT_PRECISION_SCALE);
+
+	dgFloat64 x1 = ldexp(mantissa, exp);
+	return x1;
+}
+
+#endif

newtondynamics.mod/NewtonDynamics/source/meshUtil/dgMeshEffect1.cpp

@@ -0,0 +1,3464 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgPhysicsStdafx.h"
+#include "dgBody.h"
+#include "dgWorld.h"
+#include "dgMeshEffect.h"
+#include "dgCollisionBVH.h"
+#include "dgCollisionCompound.h"
+#include "dgCollisionConvexHull.h"
+
+
+dgMeshEffect::dgMeshBVH::dgFitnessList::dgFitnessList (dgMemoryAllocator* const allocator)
+	:dgTree <dgMeshBVHNode*, dgMeshBVHNode*>(allocator)
+{
+}
+
+dgFloat64 dgMeshEffect::dgMeshBVH::dgFitnessList::TotalCost () const
+{
+	dgFloat64 cost = dgFloat32 (0.0f);
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgTreeNode* const node = iter.GetNode();
+		dgMeshBVHNode* const box = node->GetInfo();
+		cost += box->m_area;
+	}
+	return cost;
+}
+
+
+dgMeshEffect::dgMeshBVH::dgMeshBVHNode::dgMeshBVHNode (const dgMeshEffect* const mesh, dgEdge* const face, void* const userData)
+	:m_area(dgFloat32 (0.0f))
+	,m_face (face)
+	,m_userData(userData)
+	,m_left (NULL)
+	,m_right(NULL)
+	,m_parent(NULL)
+{
+	dgBigVector p0(1.0e30, 1.0e30, 1.0e30, 0.0);
+	dgBigVector p1(-1.0e30, -1.0e30, -1.0e30, 0.0);
+
+	const dgBigVector* const points = (dgBigVector*) mesh->GetVertexPool();
+
+	dgEdge* ptr = m_face;
+	do {
+		dgInt32 i = ptr->m_incidentVertex;
+		const dgBigVector& p = points[i];
+		p0.m_x = dgMin(p.m_x, p0.m_x);
+		p0.m_y = dgMin(p.m_y, p0.m_y);
+		p0.m_z = dgMin(p.m_z, p0.m_z);
+
+		p1.m_x = dgMax(p.m_x, p1.m_x);
+		p1.m_y = dgMax(p.m_y, p1.m_y);
+		p1.m_z = dgMax(p.m_z, p1.m_z);
+
+		ptr = ptr->m_next;
+	} while (ptr != face);
+
+
+	SetBox (dgVector (dgFloat32 (p0.m_x) - dgFloat32 (0.1f), dgFloat32 (p0.m_y) - dgFloat32 (0.1f), dgFloat32 (p0.m_z) - dgFloat32 (0.1f), 0.0f), 
+			dgVector (dgFloat32 (p1.m_x) + dgFloat32 (0.1f), dgFloat32 (p1.m_y) + dgFloat32 (0.1f), dgFloat32 (p1.m_z) + dgFloat32 (0.1f), 0.0f));
+}
+
+dgMeshEffect::dgMeshBVH::dgMeshBVHNode::dgMeshBVHNode (dgMeshBVHNode* const left, dgMeshBVHNode* const right)
+	:m_area(dgFloat32 (0.0f))
+	,m_face (NULL)
+	,m_left (left)
+	,m_right(right)
+	,m_parent(NULL)
+{
+	m_left->m_parent = this;
+	m_right->m_parent = this;
+
+	dgVector p0 (dgMin (left->m_p0.m_x, right->m_p0.m_x), dgMin (left->m_p0.m_y, right->m_p0.m_y), dgMin (left->m_p0.m_z, right->m_p0.m_z), dgFloat32 (0.0f));
+	dgVector p1 (dgMax (left->m_p1.m_x, right->m_p1.m_x), dgMax (left->m_p1.m_y, right->m_p1.m_y), dgMax (left->m_p1.m_z, right->m_p1.m_z), dgFloat32 (0.0f));
+	SetBox(p0, p1);
+}
+
+
+dgMeshEffect::dgMeshBVH::dgMeshBVHNode::~dgMeshBVHNode ()
+{
+	if (m_left) {
+		delete m_left;
+	}
+	if (m_right) {
+		delete m_right;
+	}
+}
+
+
+void dgMeshEffect::dgMeshBVH::dgMeshBVHNode::SetBox (const dgVector& p0, const dgVector& p1)
+{
+	m_p0 = p0;
+	m_p1 = p1;
+	m_p0.m_w = 0.0f;
+	m_p1.m_w = 0.0f;
+
+	dgVector size ((m_p1 - m_p0).Scale3 (dgFloat32 (0.5f)));
+	dgVector size1(size.m_y, size.m_z, size.m_x, dgFloat32 (0.0f));
+	m_area = size % size1;
+}
+
+
+
+dgMeshEffect::dgMeshBVH::dgMeshBVH (dgMeshEffect* const mesh)
+	:m_mesh(mesh)
+	,m_rootNode(NULL)
+	,m_fitness(m_mesh->GetAllocator())
+{
+}
+
+
+dgMeshEffect::dgMeshBVH::~dgMeshBVH()
+{
+	Cleanup ();
+}
+
+dgMeshEffect* dgMeshEffect::CreateFromSerialization (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData)
+{
+	return new (allocator) dgMeshEffect(allocator, deserialization, userData);
+}
+
+void dgMeshEffect::Serialize (dgSerialize callback, void* const userData) const
+{
+	dgInt32 faceCount = 0;
+	dgTree<dgEdge*, dgEdge*>filter(GetAllocator());
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const face = &iter.GetNode()->GetInfo();
+		if (!filter.Find(face) && (face->m_incidentFace > 0)) {
+			faceCount ++;
+			dgEdge* edge = face; 
+			do {
+				filter.Insert(edge, edge);
+				edge = edge->m_next;
+			} while (edge != face);
+		}
+	}
+
+	callback (userData, &faceCount, sizeof (dgInt32));
+	callback (userData, &m_pointCount, sizeof (dgInt32));
+	callback (userData, &m_atribCount, sizeof (dgInt32));
+	callback (userData, &m_atribCount, sizeof (dgInt32));
+
+	callback (userData, m_points, m_pointCount * sizeof (dgBigVector));
+	callback (userData, m_attrib, m_atribCount * sizeof (dgVertexAtribute));
+
+	filter.RemoveAll();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const face = &iter.GetNode()->GetInfo();
+		if (!filter.Find(face) && (face->m_incidentFace > 0)) {
+			dgInt32 indices[1024];
+			dgInt64 attibuteIndex[1024];
+			dgInt32 vertexCount = 0;
+			dgEdge* edge = face; 
+			do {
+				indices[vertexCount] = edge->m_incidentVertex;
+				attibuteIndex[vertexCount] = edge->m_userData;
+				vertexCount ++;
+				filter.Insert(edge, edge);
+				edge = edge->m_next;
+			} while (edge != face);
+
+			callback (userData, &vertexCount, sizeof (dgInt32));
+			callback (userData, indices, vertexCount * sizeof (dgInt32));
+			callback (userData, attibuteIndex, vertexCount * sizeof (dgInt64));
+		}
+	}
+}
+
+void dgMeshEffect::dgMeshBVH::Build ()
+{
+	for (void* faceNode = m_mesh->GetFirstFace (); faceNode; faceNode = m_mesh->GetNextFace(faceNode)) {
+		if (!m_mesh->IsFaceOpen(faceNode)) {
+			dgEdge* const face = &((dgTreeNode*)faceNode)->GetInfo();
+			AddFaceNode(face, NULL);
+		}
+	}
+	ImproveNodeFitness ();
+}
+
+void dgMeshEffect::dgMeshBVH::Cleanup ()
+{
+	if (m_rootNode) {
+		delete m_rootNode;
+	}
+}
+
+dgFloat32 dgMeshEffect::dgMeshBVH::CalculateSurfaceArea (dgMeshBVHNode* const node0, dgMeshBVHNode* const node1, dgVector& minBox, dgVector& maxBox) const
+{
+	minBox = dgVector (dgMin (node0->m_p0.m_x, node1->m_p0.m_x), dgMin (node0->m_p0.m_y, node1->m_p0.m_y), dgMin (node0->m_p0.m_z, node1->m_p0.m_z), dgFloat32 (0.0f));
+	maxBox = dgVector (dgMax (node0->m_p1.m_x, node1->m_p1.m_x), dgMax (node0->m_p1.m_y, node1->m_p1.m_y), dgMax (node0->m_p1.m_z, node1->m_p1.m_z), dgFloat32 (0.0f));		
+	dgVector side0 ((maxBox - minBox).Scale3 (dgFloat32 (0.5f)));
+	dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
+	return side0 % side1;
+}
+
+
+
+void dgMeshEffect::dgMeshBVH::ImproveNodeFitness (dgMeshBVHNode* const node)
+{
+	dgAssert (node->m_left);
+	dgAssert (node->m_right);
+
+	if (node->m_parent)	{
+		if (node->m_parent->m_left == node) {
+			dgFloat32 cost0 = node->m_area;
+
+			dgVector cost1P0;
+			dgVector cost1P1;		
+			dgFloat32 cost1 = CalculateSurfaceArea (node->m_right, node->m_parent->m_right, cost1P0, cost1P1);
+
+			dgVector cost2P0;
+			dgVector cost2P1;		
+			dgFloat32 cost2 = CalculateSurfaceArea (node->m_left, node->m_parent->m_right, cost2P0, cost2P1);
+
+			if ((cost1 <= cost0) && (cost1 <= cost2)) {
+				dgMeshBVHNode* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				} else {
+					m_rootNode = node;
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_right->m_parent = parent;
+				parent->m_left = node->m_right;
+				node->m_right = parent;
+				parent->m_p0 = cost1P0;
+				parent->m_p1 = cost1P1;		
+				parent->m_area = cost1;
+
+
+			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
+				dgMeshBVHNode* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				} else {
+					m_rootNode = node;
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_left->m_parent = parent;
+				parent->m_left = node->m_left;
+				node->m_left = parent;
+
+				parent->m_p0 = cost2P0;
+				parent->m_p1 = cost2P1;		
+				parent->m_area = cost2;
+			}
+		} else {
+			dgFloat32 cost0 = node->m_area;
+
+			dgVector cost1P0;
+			dgVector cost1P1;		
+			dgFloat32 cost1 = CalculateSurfaceArea (node->m_left, node->m_parent->m_left, cost1P0, cost1P1);
+
+			dgVector cost2P0;
+			dgVector cost2P1;		
+			dgFloat32 cost2 = CalculateSurfaceArea (node->m_right, node->m_parent->m_left, cost2P0, cost2P1);
+
+			if ((cost1 <= cost0) && (cost1 <= cost2)) {
+				dgMeshBVHNode* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				} else {
+					m_rootNode = node;
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_left->m_parent = parent;
+				parent->m_right = node->m_left;
+				node->m_left = parent;
+
+				parent->m_p0 = cost1P0;
+				parent->m_p1 = cost1P1;		
+				parent->m_area = cost1;
+
+			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
+				dgMeshBVHNode* const parent = node->m_parent;
+				node->m_p0 = parent->m_p0;
+				node->m_p1 = parent->m_p1;
+				node->m_area = parent->m_area; 
+
+				if (parent->m_parent) {
+					if (parent->m_parent->m_left == parent) {
+						parent->m_parent->m_left = node;
+					} else {
+						dgAssert (parent->m_parent->m_right == parent);
+						parent->m_parent->m_right = node;
+					}
+				} else {
+					m_rootNode = node;
+				}
+				node->m_parent = parent->m_parent;
+				parent->m_parent = node;
+				node->m_right->m_parent = parent;
+				parent->m_right = node->m_right;
+				node->m_right = parent;
+
+				parent->m_p0 = cost2P0;
+				parent->m_p1 = cost2P1;		
+				parent->m_area = cost2;
+			}
+		}
+	}
+	//	dgAssert (SanityCheck());
+}
+
+
+void dgMeshEffect::dgMeshBVH::ImproveNodeFitness ()
+{
+	dgFloat64 cost0 = m_fitness.TotalCost ();
+	dgFloat64 cost1 = cost0;
+	do {
+		cost0 = cost1;
+		dgFitnessList::Iterator iter (m_fitness);
+		for (iter.Begin(); iter; iter ++) {
+			dgFitnessList::dgTreeNode* const node = iter.GetNode();
+			ImproveNodeFitness (node->GetInfo());
+		}
+		cost1 = m_fitness.TotalCost ();
+	} while (cost1 < (dgFloat32 (0.95f)) * cost0);
+}
+
+
+
+
+dgMeshEffect::dgMeshBVH::dgMeshBVHNode* dgMeshEffect::dgMeshBVH::AddFaceNode (dgEdge* const face, void* const userData)
+{
+	dgMemoryAllocator* const allocator = m_mesh->GetAllocator();
+
+	dgMeshBVHNode* const newNode = new (allocator) dgMeshBVHNode (m_mesh, face, userData);
+	if (!m_rootNode) {
+		m_rootNode = newNode;
+	} else {
+
+		dgVector p0;
+		dgVector p1;		
+		dgMeshBVHNode* sibling = m_rootNode;
+
+		dgFloat32 surfaceArea = dgMeshBVH::CalculateSurfaceArea (newNode, sibling, p0, p1);
+		while(sibling->m_left && sibling->m_right) {
+
+			if (surfaceArea > sibling->m_area) {
+				break;
+			} 
+
+			sibling->SetBox (p0, p1);
+
+			dgVector leftP0;
+			dgVector leftP1;		
+			dgFloat32 leftSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_left, leftP0, leftP1);
+
+			dgVector rightP0;
+			dgVector rightP1;		
+			dgFloat32 rightSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_right, rightP0, rightP1);
+
+			if (leftSurfaceArea < rightSurfaceArea) {
+				sibling = sibling->m_left;
+				p0 = leftP0;
+				p1 = leftP1;
+				surfaceArea = leftSurfaceArea;
+			} else {
+				sibling = sibling->m_right;
+				p0 = rightP0;
+				p1 = rightP1;
+				surfaceArea = rightSurfaceArea;
+			}
+		} 
+
+		if (!sibling->m_parent) {
+			m_rootNode = new (allocator) dgMeshBVHNode (sibling, newNode);
+			m_fitness.Insert(m_rootNode, m_rootNode);
+		} else {
+			dgMeshBVHNode* const parent = sibling->m_parent;
+			if (parent->m_left == sibling) {
+				dgMeshBVHNode* const node = new (allocator) dgMeshBVHNode (sibling, newNode);
+				m_fitness.Insert(node, node);
+				parent->m_left = node;
+				node->m_parent = parent;
+			} else {
+				dgAssert (parent->m_right == sibling); 
+				dgMeshBVHNode* const node = new (allocator) dgMeshBVHNode (sibling, newNode);
+				m_fitness.Insert(node, node);
+				parent->m_right = node;
+				node->m_parent = parent;
+			}
+		}
+	}
+
+	return newNode;
+}
+
+
+void dgMeshEffect::dgMeshBVH::RemoveNode (dgMeshBVHNode* const treeNode)
+{
+	if (!treeNode->m_parent) {
+		delete (m_rootNode);
+		m_rootNode = NULL;
+	} else if (!treeNode->m_parent->m_parent) {
+		dgMeshBVHNode* const root = m_rootNode;
+		if (treeNode->m_parent->m_left == treeNode) {
+			m_rootNode = treeNode->m_parent->m_right;
+			treeNode->m_parent->m_right = NULL;
+		} else {
+			dgAssert (treeNode->m_parent->m_right == treeNode);
+			m_rootNode = treeNode->m_parent->m_left;
+			treeNode->m_parent->m_left= NULL;
+		}
+		m_rootNode->m_parent = NULL;
+		dgAssert (m_fitness.Find(root));
+		m_fitness.Remove(root);
+		delete (root);
+
+	} else {
+		dgMeshBVHNode* const root = treeNode->m_parent->m_parent;
+		if (treeNode->m_parent == root->m_left) {
+			if (treeNode->m_parent->m_right == treeNode) {
+				root->m_left = treeNode->m_parent->m_left;
+				treeNode->m_parent->m_left = NULL;
+			} else {
+				dgAssert (treeNode->m_parent->m_left == treeNode);
+				root->m_left = treeNode->m_parent->m_right;
+				treeNode->m_parent->m_right = NULL;
+			}
+			root->m_left->m_parent = root;
+		} else {
+			if (treeNode->m_parent->m_right == treeNode) {
+				root->m_right = treeNode->m_parent->m_left;
+				treeNode->m_parent->m_left = NULL;
+			} else {
+				dgAssert (treeNode->m_parent->m_left == treeNode);
+				root->m_right = treeNode->m_parent->m_right;
+				treeNode->m_parent->m_right = NULL;
+			}
+			root->m_right->m_parent = root;
+		}
+
+		dgAssert (m_fitness.Find(treeNode->m_parent));
+		m_fitness.Remove(treeNode->m_parent);
+		delete (treeNode->m_parent);
+	}
+
+	//dgAssert (SanityCheck());
+}
+
+
+
+bool dgMeshEffect::dgMeshBVH::SanityCheck() const
+{
+	#ifdef _DEBUG
+	dgAssert (m_mesh->Sanity ());
+
+	if (!m_rootNode) {
+		return false;
+	}
+
+	if ((!m_rootNode->m_left && m_rootNode->m_right) || (m_rootNode->m_left && !m_rootNode->m_right)) {
+		return false;
+	}
+
+	if (m_rootNode->m_left && m_rootNode->m_right) {
+		if (m_rootNode->m_left->m_parent != m_rootNode) {
+			return false;
+		}
+		if (m_rootNode->m_right->m_parent != m_rootNode) {
+			return false;
+		}
+
+		dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+
+		dgInt32 stack = 2;
+		stackPool[0] = m_rootNode->m_left;
+		stackPool[1] = m_rootNode->m_right;
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const node = stackPool[stack];
+
+			if ((node->m_parent->m_left != node) && (node->m_parent->m_right != node)){
+				return false;
+			}
+
+			if (node->m_left) {
+				dgAssert (node->m_right);
+				stackPool[stack] = node->m_left;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				stackPool[stack] = node->m_right;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+			}
+		}
+	}
+	#endif
+	return true;
+}
+
+
+void dgMeshEffect::dgMeshBVH::GetOverlapNodes (dgList<dgMeshBVHNode*>& overlapNodes, const dgBigVector& p0, const dgBigVector& p1) const
+{
+	dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+
+	dgInt32 stack = 1;
+	stackPool[0] = m_rootNode;
+
+	dgVector l0(p0);
+	dgVector l1(p1);
+
+	while (stack) {
+		stack --;
+		dgMeshBVHNode* const me = stackPool[stack];
+
+		if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) {
+
+			if (!me->m_left) {
+				dgAssert (!me->m_right);
+				overlapNodes.Append(me);
+			} else {
+				dgAssert (me->m_left);
+				dgAssert (me->m_right);
+				stackPool[stack] = me->m_left;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				stackPool[stack] = me->m_right;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+			}
+		}
+	}
+}
+
+dgFloat64 dgMeshEffect::dgMeshBVH::VertexRayCast (const dgBigVector& p0, const dgBigVector& p1) const
+{
+	dgAssert (0);
+/*
+	dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+
+	dgInt32 stack = 1;
+	stackPool[0] = m_rootNode;
+
+	dgVector l0(p0);
+	dgVector l1(p1);
+	dgBigVector p1p0 (p1 - p0);
+	dgFloat64 den = p1p0 % p1p0;
+
+	const dgBigVector* const points = (dgBigVector*) m_mesh->GetVertexPool();
+	while (stack) {
+		stack --;
+		dgMeshBVHNode* const me = stackPool[stack];
+
+		if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) {
+			if (!me->m_left) {
+				dgAssert (!me->m_right);
+
+				dgEdge* ptr = me->m_face;
+				do {
+					dgInt32 index = ptr->m_incidentVertex;
+					const dgBigVector& q0 = points[index];
+					dgBigVector q0p0 (q0 - p0);
+					dgFloat64 alpha = q0p0 % p1p0;
+					if ((alpha > (DG_BOOLEAN_ZERO_TOLERANCE * den)) && (alpha < (den - DG_BOOLEAN_ZERO_TOLERANCE))) {
+						dgBigVector dist (p0 + p1p0.Scale3 (alpha / den) - q0);
+						dgFloat64 dist2 = dist % dist;
+						if (dist2 < (DG_BOOLEAN_ZERO_TOLERANCE * DG_BOOLEAN_ZERO_TOLERANCE)) {
+							return alpha / den;
+						}
+					}
+
+					ptr = ptr->m_next;
+				} while (ptr != me->m_face);
+			} else {
+				dgAssert (me->m_left);
+				dgAssert (me->m_right);
+				stackPool[stack] = me->m_left;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				stackPool[stack] = me->m_right;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+			}
+		}
+	}
+*/
+	return 1.2f;
+}
+
+
+
+bool dgMeshEffect::dgMeshBVH::RayRayIntersect (dgEdge* const edge, const dgMeshEffect* const otherMesh, dgEdge* const otherEdge, dgFloat64& param, dgFloat64& otherParam) const
+{
+	dgAssert (0);
+/*
+	dgBigVector ray_p0 (m_mesh->m_points[edge->m_incidentVertex]);
+	dgBigVector ray_p1 (m_mesh->m_points[edge->m_twin->m_incidentVertex]);
+
+	dgBigVector ray_q0 (otherMesh->m_points[otherEdge->m_incidentVertex]);
+	dgBigVector ray_q1 (otherMesh->m_points[otherEdge->m_twin->m_incidentVertex]);
+
+	dgBigVector p1p0 (ray_p1 - ray_p0);
+	dgBigVector q1q0 (ray_q1 - ray_q0);
+	dgBigVector p0q0 (ray_p0 - ray_q0);
+
+	dgFloat64 a = p1p0 % p1p0;        // always >= 0
+	dgFloat64 c = q1q0 % q1q0;        // always >= 0
+	dgFloat64 b = p1p0 % q1q0;
+
+	dgFloat64 d = (p1p0 % p0q0);
+	dgFloat64 e = (q1q0 % p0q0);
+	dgFloat64 den = a * c - b * b;   // always >= 0
+	// compute the line parameters of the two closest points
+	if (den < DG_BOOLEAN_ZERO_TOLERANCE) { 
+		// the lines are almost parallel
+		return false;
+	} else {         
+		// get the closest points on the infinite lines
+		dgFloat64 t = b * e - c * d;
+		dgFloat64 s = a * e - b * d;
+
+		if (t < (DG_BOOLEAN_ZERO_TOLERANCE * den) || (s < (DG_BOOLEAN_ZERO_TOLERANCE * den)) || (t > (den - DG_BOOLEAN_ZERO_TOLERANCE)) ||  (s > (den - DG_BOOLEAN_ZERO_TOLERANCE))) {
+			return false;
+		}
+		//dgBigVector normal (p1p0 * q1q0);
+		//dgFloat64 dist0 = normal % (p1p0.Scale3 (t / den) - ray_p0);
+		//dgFloat64 dist1 = normal % (q1q0.Scale3 (s / den) - ray_q0);
+		dgBigVector r0 = ray_p0 + p1p0.Scale3 (t / den);
+		dgBigVector r1 = ray_q0 + q1q0.Scale3 (s / den);
+		dgBigVector r1r0 (r1 - r0);
+		dgFloat64 dist2 = r1r0 % r1r0;
+		if (dist2 > (DG_BOOLEAN_ZERO_TOLERANCE * DG_BOOLEAN_ZERO_TOLERANCE)) {
+			return false;
+		}
+
+		param = t / den;
+		otherParam = s / den;
+	}
+*/
+	return true;
+}
+
+
+dgFloat64 dgMeshEffect::dgMeshBVH::RayFaceIntersect (const dgMeshBVHNode* const faceNode, const dgBigVector& p0, const dgBigVector& p1, bool doubleSidedFaces) const
+{
+	dgBigVector normal (m_mesh->FaceNormal(faceNode->m_face, m_mesh->GetVertexPool(), sizeof(dgBigVector)));
+
+	dgBigVector diff (p1 - p0);
+
+	dgFloat64 tOut = 2.0f;
+	const dgBigVector* const points = (dgBigVector*) m_mesh->GetVertexPool();
+	dgFloat64 dir = normal % diff;
+	if (dir < 0.0f) {
+		dgEdge* ptr = faceNode->m_face;
+		do {
+			dgInt32 index0 = ptr->m_incidentVertex;
+			dgInt32 index1 = ptr->m_next->m_incidentVertex;
+			dgBigVector p0v0 (points[index0] - p0);
+			dgBigVector p0v1 (points[index1] - p0);
+			dgFloat64 alpha = (diff * p0v1) % p0v0;
+			if (alpha <= 0.0f) {
+				return 1.2f;
+			}
+
+			ptr = ptr->m_next;
+		} while (ptr != faceNode->m_face);
+
+		dgInt32 index0 = ptr->m_incidentVertex;
+		dgBigVector p0v0 (points[index0] - p0);
+		tOut = normal % p0v0;
+		dgFloat64 dist = normal % diff;
+		tOut = tOut / dist;
+
+	} else if (doubleSidedFaces && (dir > 0.0f)) {
+		dgEdge* ptr = faceNode->m_face;
+		do {
+			dgInt32 index0 = ptr->m_incidentVertex;
+			dgInt32 index1 = ptr->m_prev->m_incidentVertex;
+			dgBigVector p0v0 (points[index0] - p0);
+			dgBigVector p0v1 (points[index1] - p0);
+			dgFloat64 alpha = (diff * p0v1) % p0v0;
+			if (alpha <= 0.0f) {
+				return 1.2f;
+			}
+
+			ptr = ptr->m_prev;
+		} while (ptr != faceNode->m_face);
+
+		dgInt32 index0 = ptr->m_incidentVertex;
+		dgBigVector p0v0 (points[index0] - p0);
+		tOut = normal % p0v0;
+		dgFloat64 dist = normal % diff;
+		tOut = tOut / dist;
+	}
+
+	if (tOut < 1.e-12f) {
+		tOut = 2.0f;
+	} else if (tOut > (1.0 - 1.e-12f)) {
+		tOut = 2.0f;
+	}
+	return tOut;
+}
+
+
+dgMeshEffect::dgMeshBVH::dgMeshBVHNode* dgMeshEffect::dgMeshBVH::FaceRayCast (const dgBigVector& p0, const dgBigVector& p1, dgFloat64& paramOut, bool doubleSidedFaces) const
+{
+	dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+
+	dgInt32 stack = 1;
+	dgMeshBVHNode* node = NULL;
+
+	stackPool[0] = m_rootNode;
+	dgFloat64 maxParam = dgFloat32 (1.2f);
+
+	dgVector l0(p0);
+	dgVector l1(p1);
+	l0 = l0 & dgVector::m_triplexMask;
+	l1 = l1 & dgVector::m_triplexMask;
+	dgFastRayTest ray (l0, l1);
+	while (stack) {
+		stack --;
+		dgMeshBVHNode* const me = stackPool[stack];
+
+		if (me && ray.BoxTest (me->m_p0, me->m_p1)) {
+
+			if (!me->m_left) {
+				dgAssert (!me->m_right);
+				dgFloat64 param = RayFaceIntersect (me, p0, p1, doubleSidedFaces);
+				if (param < maxParam) {
+					node = me;
+					maxParam = param;
+				}
+
+			} else {
+				dgAssert (me->m_left);
+				dgAssert (me->m_right);
+				stackPool[stack] = me->m_left;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				stackPool[stack] = me->m_right;
+				stack++;
+				dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+			}
+		}
+	}
+
+	paramOut = maxParam;
+	return node;
+}
+
+
+
+
+dgMeshEffect::dgMeshEffect(dgMemoryAllocator* const allocator)
+	:dgPolyhedra(allocator)
+{
+	Init();
+}
+
+dgMeshEffect::dgMeshEffect (dgMemoryAllocator* const allocator, const dgMatrix& planeMatrix, dgFloat32 witdth, dgFloat32 breadth, dgInt32 material, const dgMatrix& textureMatrix0, const dgMatrix& textureMatrix1)
+	:dgPolyhedra(allocator)
+{
+	dgInt32 index[4];
+	dgInt64 attrIndex[4];
+	dgBigVector face[4];
+
+	Init();
+
+	face[0] = dgBigVector (dgFloat32 (0.0f), -witdth, -breadth, dgFloat32 (0.0f));
+	face[1] = dgBigVector (dgFloat32 (0.0f),  witdth, -breadth, dgFloat32 (0.0f));
+	face[2] = dgBigVector (dgFloat32 (0.0f),  witdth,  breadth, dgFloat32 (0.0f));
+	face[3] = dgBigVector (dgFloat32 (0.0f), -witdth,  breadth, dgFloat32 (0.0f));
+
+	for (dgInt32 i = 0; i < 4; i ++) {
+		dgBigVector uv0 (textureMatrix0.TransformVector(face[i]));
+		dgBigVector uv1 (textureMatrix1.TransformVector(face[i]));
+
+		m_points[i] = planeMatrix.TransformVector(face[i]);
+
+		m_attrib[i].m_vertex.m_x = m_points[i].m_x;
+		m_attrib[i].m_vertex.m_y = m_points[i].m_y;
+		m_attrib[i].m_vertex.m_z = m_points[i].m_z;
+		m_attrib[i].m_vertex.m_w = dgFloat64 (0.0f);
+
+		m_attrib[i].m_normal_x = planeMatrix.m_front.m_x;
+		m_attrib[i].m_normal_y = planeMatrix.m_front.m_y;
+		m_attrib[i].m_normal_z = planeMatrix.m_front.m_z;
+		
+		m_attrib[i].m_u0 = uv0.m_y;
+		m_attrib[i].m_v0 = uv0.m_z;
+
+		m_attrib[i].m_u1 = uv1.m_y;
+		m_attrib[i].m_v1 = uv1.m_z;
+
+		m_attrib[i].m_material = material;
+
+		index[i] = i;
+		attrIndex[i] = i;
+	}
+
+	m_pointCount = 4;
+	m_atribCount = 4;
+	BeginFace();
+	AddFace (4, index, attrIndex);
+	EndFace();
+}
+
+
+dgMeshEffect::dgMeshEffect(dgPolyhedra& mesh, const dgMeshEffect& source)
+	:dgPolyhedra (mesh) 
+{
+	m_pointCount = source.m_pointCount;
+	m_maxPointCount = source.m_maxPointCount;
+	m_points = (dgBigVector*) GetAllocator()->MallocLow(dgInt32 (m_maxPointCount * sizeof(dgBigVector)));
+	memcpy (m_points, source.m_points, m_pointCount * sizeof(dgBigVector));
+
+	m_atribCount = source.m_atribCount;
+	m_maxAtribCount = source.m_maxAtribCount;
+	m_attrib = (dgVertexAtribute*) GetAllocator()->MallocLow(dgInt32 (m_maxAtribCount * sizeof(dgVertexAtribute)));
+	memcpy (m_attrib, source.m_attrib, m_atribCount * sizeof(dgVertexAtribute));
+}
+
+
+dgMeshEffect::dgMeshEffect(const dgMeshEffect& source)
+	:dgPolyhedra (source) 
+{
+	m_pointCount = source.m_pointCount;
+	m_maxPointCount = source.m_maxPointCount;
+	m_points = (dgBigVector*) GetAllocator()->MallocLow(dgInt32 (m_maxPointCount * sizeof(dgBigVector)));
+	memcpy (m_points, source.m_points, m_pointCount * sizeof(dgBigVector));
+
+	m_atribCount = source.m_atribCount;
+	m_maxAtribCount = source.m_maxAtribCount;
+	m_attrib = (dgVertexAtribute*) GetAllocator()->MallocLow(dgInt32 (m_maxAtribCount * sizeof(dgVertexAtribute)));
+	memcpy (m_attrib, source.m_attrib, m_atribCount * sizeof(dgVertexAtribute));
+}
+
+
+dgMeshEffect::dgMeshEffect(dgCollisionInstance* const collision)
+	:dgPolyhedra (collision->GetAllocator()) 
+{
+	class dgMeshEffectBuilder
+	{
+		public:
+		dgMeshEffectBuilder ()
+		{
+			m_brush = 0;
+			m_faceCount = 0;
+			m_vertexCount = 0;
+			m_maxFaceCount = 32;
+			m_maxVertexCount = 32;
+			m_vertex = (dgVector*) dgMallocStack(m_maxVertexCount * sizeof(dgVector));
+			m_faceIndexCount = (dgInt32*) dgMallocStack(m_maxFaceCount * sizeof(dgInt32));
+		}
+
+		~dgMeshEffectBuilder ()
+		{
+			dgFreeStack (m_faceIndexCount);
+			dgFreeStack (m_vertex);
+		}
+
+		static void GetShapeFromCollision (void* userData, dgInt32 vertexCount, const dgFloat32* faceVertex, dgInt32 id)
+		{
+			dgInt32 vertexIndex; 
+			dgMeshEffectBuilder& builder = *((dgMeshEffectBuilder*)userData);
+
+
+			if (builder.m_faceCount >= builder.m_maxFaceCount) {
+				dgInt32* index;
+
+				builder.m_maxFaceCount *= 2;
+				index = (dgInt32*) dgMallocStack(builder.m_maxFaceCount * sizeof(dgInt32));
+				memcpy (index, builder.m_faceIndexCount, builder.m_faceCount * sizeof(dgInt32));
+				dgFreeStack(builder.m_faceIndexCount);
+				builder.m_faceIndexCount = index;
+			}
+			builder.m_faceIndexCount[builder.m_faceCount] = vertexCount;
+			builder.m_faceCount = builder.m_faceCount + 1;
+
+			vertexIndex = builder.m_vertexCount; 
+			dgFloat32 brush = dgFloat32 (builder.m_brush);
+			for (dgInt32 i = 0; i < vertexCount; i ++) {
+				if (vertexIndex >= builder.m_maxVertexCount) {
+					builder.m_maxVertexCount *= 2;
+					dgVector* const points = (dgVector*) dgMallocStack(builder.m_maxVertexCount * sizeof(dgVector));
+					memcpy (points, builder.m_vertex, vertexIndex * sizeof(dgVector));
+					dgFreeStack(builder.m_vertex);
+					builder.m_vertex = points;
+				}
+
+				builder.m_vertex[vertexIndex].m_x = faceVertex[i * 3 + 0];
+				builder.m_vertex[vertexIndex].m_y = faceVertex[i * 3 + 1];
+				builder.m_vertex[vertexIndex].m_z = faceVertex[i * 3 + 2];
+				builder.m_vertex[vertexIndex].m_w = brush;
+				vertexIndex ++;
+			}
+
+			builder.m_vertexCount = vertexIndex;
+		}
+
+		dgInt32 m_brush;
+		dgInt32 m_vertexCount;
+		dgInt32 m_maxVertexCount;
+
+		dgInt32 m_faceCount;
+		dgInt32 m_maxFaceCount;
+
+		dgVector* m_vertex;
+		dgInt32* m_faceIndexCount;
+	};
+
+	dgMeshEffectBuilder builder;
+
+	if (collision->IsType (dgCollision::dgCollisionCompound_RTTI)) {
+		dgCollisionInfo collisionInfo;
+		collision->GetCollisionInfo (&collisionInfo);
+
+		dgInt32 brush = 0;
+		dgMatrix matrix (collisionInfo.m_offsetMatrix);
+		dgCollisionCompound* const compoundCollision = (dgCollisionCompound*) collision->GetChildShape();
+		for (dgTree<dgCollisionCompound::dgNodeBase*, dgInt32>::dgTreeNode* node = compoundCollision->GetFirstNode(); node; node = compoundCollision->GetNextNode(node)) {
+			builder.m_brush = brush;
+			brush ++;
+			dgCollisionInstance* const childShape = compoundCollision->GetCollisionFromNode(node);
+			childShape->DebugCollision (matrix, (dgCollision::OnDebugCollisionMeshCallback) dgMeshEffectBuilder::GetShapeFromCollision, &builder);
+		}
+
+	} else {
+		dgMatrix matrix (dgGetIdentityMatrix());
+		collision->DebugCollision (matrix, (dgCollision::OnDebugCollisionMeshCallback) dgMeshEffectBuilder::GetShapeFromCollision, &builder);
+	}
+
+	dgStack<dgInt32>indexList (builder.m_vertexCount);
+
+	dgVertexListToIndexList (&builder.m_vertex[0].m_x, sizeof (dgVector), sizeof (dgVector), 0, builder.m_vertexCount, &indexList[0], DG_VERTEXLIST_INDEXLIST_TOL);	
+
+	dgStack<dgInt32> materialIndex(builder.m_faceCount);
+	dgStack<dgInt32> m_normalUVIndex(builder.m_vertexCount);
+
+	dgVector normalUV(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+
+	memset (&materialIndex[0], 0, size_t (materialIndex.GetSizeInBytes()));
+	memset (&m_normalUVIndex[0], 0, size_t (m_normalUVIndex.GetSizeInBytes()));
+
+	Init();
+	BuildFromVertexListIndexList(builder.m_faceCount, builder.m_faceIndexCount, &materialIndex[0],
+								 &builder.m_vertex[0].m_x, sizeof (dgVector), &indexList[0],
+								 &normalUV.m_x, sizeof (dgVector), &m_normalUVIndex[0],
+								 &normalUV.m_x, sizeof (dgVector), &m_normalUVIndex[0],
+								 &normalUV.m_x, sizeof (dgVector), &m_normalUVIndex[0]);
+
+    RepairTJoints();
+	CalculateNormals(dgFloat32 (45.0f * 3.141592f/180.0f));
+}
+
+
+dgMeshEffect::dgMeshEffect(dgMemoryAllocator* const allocator, const char* const fileName)
+	:dgPolyhedra (allocator) 
+{
+	class ParceOFF
+	{
+		public:
+		enum Token
+		{
+			m_off,	
+			m_value,
+			m_end,
+		};
+
+		ParceOFF (FILE* const file)
+			:m_file (file)
+		{
+		}
+
+		Token GetToken(char* const buffer) const
+		{
+			while (!feof (m_file) && fscanf (m_file, "%s", buffer)) {
+				if (buffer[0] == '#') {
+					SkipLine();
+				} else {
+					if (!_stricmp (buffer, "OFF")) {
+						return m_off;
+					}
+					return m_value;
+				}
+			}
+			return m_end;
+		}
+
+		char* SkipLine() const
+		{
+			char tmp[1024];
+			return fgets (tmp, sizeof (tmp), m_file);
+		}
+
+		dgInt32 GetInteger() const
+		{
+			char buffer[1024];
+			GetToken(buffer);
+			return atoi (buffer);	
+		}
+
+		dgFloat64 GetFloat() const
+		{
+			char buffer[1024];
+			GetToken(buffer);
+			return atof (buffer);	
+		}
+
+		FILE* m_file;
+	};
+
+	Init();
+	FILE* const file = fopen (fileName, "rb");
+	if (file) {
+		ParceOFF parcel (file);
+
+		dgInt32 vertexCount = 0;
+		dgInt32 faceCount = 0;
+//		dgInt32 edgeCount = 0;
+
+		char buffer[1024];
+		bool stillData = true;
+		while (stillData) {
+			ParceOFF::Token token = parcel.GetToken(buffer);
+			switch (token) 
+			{
+				case ParceOFF::m_off:
+				{
+					vertexCount = parcel.GetInteger();
+					faceCount = parcel.GetInteger();
+//					edgeCount = parcel.GetInteger();
+					parcel.SkipLine();
+
+					dgVertexAtribute attribute;
+					memset (&attribute, 0, sizeof (dgVertexAtribute));
+					attribute.m_normal_y = 1.0f;
+					//AddAtribute(attribute);
+					for (dgInt32 i = 0; i < vertexCount; i ++) {
+						//dgBigVector point;
+						attribute.m_vertex.m_x = parcel.GetFloat();
+						attribute.m_vertex.m_y = parcel.GetFloat();
+						attribute.m_vertex.m_z = parcel.GetFloat();
+						attribute.m_vertex.m_w = 0.0;
+						parcel.SkipLine();
+						//AddVertex(point);
+						AddPoint(&attribute.m_vertex.m_x, 0);
+					}
+
+					BeginFace();
+					for (dgInt32 i = 0; i < faceCount; i ++) {
+						dgInt32 face[256];
+						dgInt64 attrib[256];
+						dgInt32 faceVertexCount = parcel.GetInteger();
+						for (dgInt32 j = 0; j < faceVertexCount; j ++) {
+							face[j] = parcel.GetInteger();
+							attrib[j] = face[j];
+						}
+						parcel.SkipLine();
+						AddFace(faceVertexCount, face, attrib);
+					}
+					EndFace();
+
+					CalculateNormals (3.1416f * 30.0f / 180.0f);
+					stillData = false;
+					break;
+				}
+					
+				default:;
+			}
+		}
+
+		fclose (file);
+	}
+}
+
+dgMeshEffect::dgMeshEffect (dgMemoryAllocator* const allocator, dgDeserialize deserialization, void* const userData)
+	:dgPolyhedra (allocator) 
+{
+	dgInt32 faceCount;
+	deserialization (userData, &faceCount, sizeof (dgInt32));
+	deserialization (userData, &m_pointCount, sizeof (dgInt32));
+	deserialization (userData, &m_atribCount, sizeof (dgInt32));
+	deserialization (userData, &m_atribCount, sizeof (dgInt32));
+
+	m_maxPointCount = m_pointCount;
+	m_maxAtribCount = m_atribCount;
+
+	m_points = (dgBigVector*) GetAllocator()->MallocLow(dgInt32 (m_pointCount * sizeof(dgBigVector)));
+	m_attrib = (dgVertexAtribute*) GetAllocator()->MallocLow(dgInt32 (m_atribCount * sizeof(dgVertexAtribute)));
+
+	deserialization (userData, m_points, m_pointCount * sizeof (dgBigVector));
+	deserialization (userData, m_attrib, m_atribCount * sizeof (dgVertexAtribute));
+
+	BeginFace();
+	for (dgInt32 i = 0; i < faceCount; i ++) {
+		dgInt32 vertexCount;
+		dgInt32 face[1024];
+		dgInt64 attrib[1024];
+		deserialization (userData, &vertexCount, sizeof (dgInt32));
+		deserialization (userData, face, vertexCount * sizeof (dgInt32));
+		deserialization (userData, attrib, vertexCount * sizeof (dgInt64));
+		AddFace (vertexCount, face, attrib);
+	}
+	EndFace();
+}
+
+
+dgMeshEffect::~dgMeshEffect(void)
+{
+	GetAllocator()->FreeLow (m_points);
+	GetAllocator()->FreeLow (m_attrib);
+}
+
+
+void dgMeshEffect::BeginFace()
+{
+	dgPolyhedra::BeginFace();
+}
+
+void dgMeshEffect::EndFace ()
+{
+	dgPolyhedra::EndFace();
+
+	for (bool hasVertexCollision = true; hasVertexCollision;) {
+		hasVertexCollision = false;
+
+		const dgInt32 currentCount = m_pointCount;
+		dgStack<dgInt8> verterCollision (currentCount);
+		memset (&verterCollision[0], 0, verterCollision.GetSizeInBytes());
+
+		Iterator iter (*this);
+		dgInt32 mark = IncLRU();
+		dgList<dgTreeNode*> collisionFound(GetAllocator());
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &iter.GetNode()->GetInfo();
+			if (edge->m_mark != mark) {
+				if ((edge->m_incidentVertex < currentCount) && (verterCollision[edge->m_incidentVertex] == 0)) {
+					verterCollision[edge->m_incidentVertex] = 1;
+				} else {
+					hasVertexCollision = true;
+					collisionFound.Append(iter.GetNode());
+				}
+				dgEdge* ptr = edge;
+				do {
+					ptr->m_mark = mark;
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != edge);
+			}
+		}
+
+		if (hasVertexCollision) {
+			dgAssert (Sanity());
+			for (dgList<dgTreeNode*>::dgListNode* node = collisionFound.GetFirst(); node; node = node->GetNext()) {
+				dgEdge* const edge = &node->GetInfo()->GetInfo();
+
+				// this is a vertex collision
+				dgBigVector point (m_points[edge->m_incidentVertex]);
+				point.m_w += dgFloat64 (1.0f);
+				AddVertex (point);
+
+				dgEdge* ptr = edge;
+				do {
+					ptr->m_incidentVertex = m_pointCount - 1;
+
+					dgTreeNode* const edgeNode = GetNodeFromInfo (*ptr);
+					dgPairKey edgeKey (ptr->m_incidentVertex, ptr->m_twin->m_incidentVertex);
+					ReplaceKey (edgeNode, edgeKey.GetVal());
+
+					dgTreeNode* const twinNode = GetNodeFromInfo (*(ptr->m_twin));
+					dgPairKey twinKey (ptr->m_twin->m_incidentVertex, ptr->m_incidentVertex);
+					ReplaceKey (twinNode, twinKey.GetVal());
+
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != edge);
+			}
+			dgAssert (Sanity());
+		}
+	}
+}
+
+
+void dgMeshEffect::Init()
+{
+	m_pointCount = 0;
+	m_atribCount = 0;
+	m_maxPointCount = DG_MESH_EFFECT_INITIAL_VERTEX_SIZE;
+	m_maxAtribCount = DG_MESH_EFFECT_INITIAL_VERTEX_SIZE;
+
+	m_points = (dgBigVector*) GetAllocator()->MallocLow(dgInt32 (m_maxPointCount * sizeof(dgBigVector)));
+	m_attrib = (dgVertexAtribute*) GetAllocator()->MallocLow(dgInt32 (m_maxAtribCount * sizeof(dgVertexAtribute)));
+}
+
+void dgMeshEffect::Trace () const
+{
+	for (dgInt32 i = 0; i < m_pointCount; i ++ ) {
+		dgTrace (("%d-> %f %f %f\n", i, m_points[i].m_x, m_points[i].m_y, m_points[i].m_z));
+	}
+
+
+	dgTree<dgEdge*, dgEdge*>filter(GetAllocator());
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &iter.GetNode()->GetInfo();
+		if (!filter.Find(edge)) {
+			dgEdge* ptr = edge;
+			do {
+				filter.Insert(edge, ptr);
+				dgTrace (("%d ", ptr->m_incidentVertex));
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+			if (edge->m_incidentFace <= 0) {
+				dgTrace (("open"));
+			}
+			dgTrace (("\n"));
+		}
+	}
+	dgTrace (("\n"));
+};
+
+
+void dgMeshEffect::SaveOFF (const char* const fileName) const
+{
+	FILE* const file = fopen (fileName, "wb");
+
+	fprintf (file, "OFF\n");
+
+	dgInt32 faceCount = 0;
+	dgTree<dgEdge*, dgEdge*>filter(GetAllocator());
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const face = &iter.GetNode()->GetInfo();
+		if (!filter.Find(face) && (face->m_incidentFace > 0)) {
+			faceCount ++;
+			dgEdge* edge = face; 
+			do {
+				filter.Insert(edge, edge);
+				edge = edge->m_next;
+			} while (edge != face);
+		}
+	}
+	fprintf (file, "%d %d 0\n", m_pointCount, faceCount);
+
+	for (dgInt32 i = 0; i < m_pointCount; i ++) {
+		fprintf (file, "%f %f %f\n", m_points[i].m_x, m_points[i].m_y, m_points[i].m_z);
+	}
+
+	filter.RemoveAll();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const face = &iter.GetNode()->GetInfo();
+		if (!filter.Find(face) && (face->m_incidentFace > 0)) {
+			dgInt32 indices[1024];
+			dgInt32 vertexCount = 0;
+			dgEdge* edge = face; 
+			do {
+				indices[vertexCount] = edge->m_incidentVertex;
+				vertexCount ++;
+				filter.Insert(edge, edge);
+				edge = edge->m_next;
+			} while (edge != face);
+
+			fprintf (file, "%d", vertexCount);
+			for (dgInt32 j = 0; j < vertexCount; j ++) {
+				fprintf (file, " %d", indices[j]);
+			}
+			fprintf (file, "\n");
+		}
+	}
+	fclose (file);
+}
+
+void dgMeshEffect::Triangulate  ()
+{
+	dgPolyhedra polygon(GetAllocator());
+
+	dgInt32 mark = IncLRU();
+	polygon.BeginFace();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+
+		if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+			dgInt32	index[DG_MESH_EFFECT_POINT_SPLITED];
+
+			dgEdge* ptr = face;
+			dgInt32 indexCount = 0;
+			do {
+				dgInt32 attribIndex = dgInt32 (ptr->m_userData);
+				m_attrib[attribIndex].m_vertex.m_w = dgFloat64 (ptr->m_incidentVertex);
+				ptr->m_mark = mark;
+				index[indexCount] = attribIndex;
+				indexCount ++;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			polygon.AddFace(indexCount, index);
+		}
+	}
+	polygon.EndFace();
+
+
+	dgPolyhedra leftOversOut(GetAllocator());
+	polygon.Triangulate(&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), &leftOversOut);
+	dgAssert (leftOversOut.GetCount() == 0);
+
+
+	RemoveAll();
+	SetLRU (0);
+
+	mark = polygon.IncLRU();
+	BeginFace();
+	dgPolyhedra::Iterator iter1 (polygon);
+	for (iter1.Begin(); iter1; iter1 ++){
+		dgEdge* const face = &(*iter1);
+		if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+			dgInt32	index[DG_MESH_EFFECT_POINT_SPLITED];
+			dgInt64	userData[DG_MESH_EFFECT_POINT_SPLITED];
+
+			dgEdge* ptr = face;
+			dgInt32 indexCount = 0;
+			do {
+				ptr->m_mark = mark;
+				index[indexCount] = dgInt32 (m_attrib[ptr->m_incidentVertex].m_vertex.m_w);
+
+				userData[indexCount] = ptr->m_incidentVertex;
+				indexCount ++;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			AddFace(indexCount, index, userData);
+		}
+	}
+	EndFace();
+
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+		if (face->m_incidentFace > 0) {
+			dgInt32 attribIndex = dgInt32 (face->m_userData);
+			m_attrib[attribIndex].m_vertex.m_w = m_points[face->m_incidentVertex].m_w;
+		}
+	}
+
+	RepairTJoints ();
+	dgAssert (Sanity ());
+}
+
+void dgMeshEffect::ConvertToPolygons ()
+{
+	dgPolyhedra polygon(GetAllocator());
+
+	dgInt32 mark = IncLRU();
+	polygon.BeginFace();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+
+		if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+			dgInt32	index[DG_MESH_EFFECT_POINT_SPLITED];
+
+			dgEdge* ptr = face;
+			dgInt32 indexCount = 0;
+			do {
+				dgInt32 attribIndex = dgInt32 (ptr->m_userData);
+
+				m_attrib[attribIndex].m_vertex.m_w = dgFloat32 (ptr->m_incidentVertex);
+				ptr->m_mark = mark;
+				index[indexCount] = attribIndex;
+				indexCount ++;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			polygon.AddFace(indexCount, index);
+		}
+	}
+	polygon.EndFace();
+
+	dgPolyhedra leftOversOut(GetAllocator());
+	polygon.ConvexPartition (&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), &leftOversOut);
+	dgAssert (leftOversOut.GetCount() == 0);
+
+	RemoveAll();
+	SetLRU (0);
+
+	mark = polygon.IncLRU();
+	BeginFace();
+	dgPolyhedra::Iterator iter1 (polygon);
+	for (iter1.Begin(); iter1; iter1 ++){
+		dgEdge* const face = &(*iter1);
+		if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+			dgInt32	index[DG_MESH_EFFECT_POINT_SPLITED];
+			dgInt64	userData[DG_MESH_EFFECT_POINT_SPLITED];
+
+			dgEdge* ptr = face;
+			dgInt32 indexCount = 0;
+			do {
+				ptr->m_mark = mark;
+				index[indexCount] = dgInt32 (m_attrib[ptr->m_incidentVertex].m_vertex.m_w);
+				userData[indexCount] = ptr->m_incidentVertex;
+				indexCount ++;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			AddFace(indexCount, index, userData);
+		}
+	}
+	EndFace();
+
+
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+		if (face->m_incidentFace > 0) {
+			dgInt32 attribIndex = dgInt32 (face->m_userData);
+			m_attrib[attribIndex].m_vertex.m_w = m_points[face->m_incidentVertex].m_w;
+		}
+	}
+
+	RepairTJoints ();
+	dgAssert (Sanity ());
+}
+
+void dgMeshEffect::RemoveUnusedVertices(dgInt32* const vertexMapResult)
+{
+	dgPolyhedra polygon(GetAllocator());
+	dgStack<dgInt32>attrbMap(m_atribCount);
+	dgStack<dgInt32>vertexMap(m_pointCount);
+
+	dgInt32 savedPointCount = m_pointCount;
+	memset(&vertexMap[0], -1, m_pointCount * sizeof (int));
+	memset(&attrbMap[0], -1, m_atribCount * sizeof (int));
+
+	int attribCount = 0;
+	int vertexCount = 0;
+
+	dgStack<dgBigVector>points (m_pointCount);
+	dgStack<dgVertexAtribute>atributes (m_atribCount);
+
+	dgInt32 mark = IncLRU();
+	polygon.BeginFace();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+		if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+			dgInt32	vertex[DG_MESH_EFFECT_POINT_SPLITED];
+			dgInt64	userData[DG_MESH_EFFECT_POINT_SPLITED];
+			int indexCount = 0;
+			dgEdge* ptr = face;
+			do {
+				ptr->m_mark = mark;
+
+				int index = ptr->m_incidentVertex;
+				if (vertexMap[index] == -1) {
+					vertexMap[index] = vertexCount;
+					points[vertexCount] = m_points[index];
+					vertexCount ++;
+				}
+				vertex[indexCount] = vertexMap[index];
+
+				index = int (ptr->m_userData);
+				if (attrbMap[index] == -1) {
+					attrbMap[index] = attribCount;
+					atributes[attribCount] = m_attrib[index];
+					attribCount ++;
+				}
+				userData[indexCount] = attrbMap[index];
+				indexCount ++;
+
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			polygon.AddFace(indexCount, vertex, userData);
+		}
+	}
+	polygon.EndFace();
+
+	m_pointCount = vertexCount;
+	memcpy (&m_points[0].m_x, &points[0].m_x, m_pointCount * sizeof (dgBigVector));
+	 
+	m_atribCount = attribCount;
+	memcpy (&m_attrib[0].m_vertex.m_x, &atributes[0].m_vertex.m_x, m_atribCount * sizeof (dgVertexAtribute));
+
+
+	RemoveAll();
+	SetLRU (0);
+
+	BeginFace();
+	dgPolyhedra::Iterator iter1 (polygon);
+	for (iter1.Begin(); iter1; iter1 ++){
+		dgEdge* const face = &(*iter1);
+		if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+			dgInt32	index[DG_MESH_EFFECT_POINT_SPLITED];
+			dgInt64	userData[DG_MESH_EFFECT_POINT_SPLITED];
+
+			dgEdge* ptr = face;
+			dgInt32 indexCount = 0;
+			do {
+				ptr->m_mark = mark;
+				index[indexCount] = ptr->m_incidentVertex;
+				userData[indexCount] = dgInt64 (ptr->m_userData);
+				indexCount ++;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			AddFace(indexCount, index, userData);
+		}
+	}
+	EndFace();
+	PackVertexArrays ();
+
+	if (vertexMapResult) {
+		memcpy (vertexMapResult, &vertexMap[0], savedPointCount * sizeof (dgInt32));
+	}
+}
+
+
+void dgMeshEffect::ApplyTransform (const dgMatrix& matrix)
+{
+	matrix.TransformTriplex(&m_points[0].m_x, sizeof (dgBigVector), &m_points[0].m_x, sizeof (dgBigVector), m_pointCount);
+	matrix.TransformTriplex(&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), &m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), m_atribCount);
+
+	dgMatrix rotation ((matrix.Inverse4x4()).Transpose4X4());
+	for (dgInt32 i = 0; i < m_atribCount; i ++) {
+		dgVector n (dgFloat32 (m_attrib[i].m_normal_x), dgFloat32 (m_attrib[i].m_normal_y), dgFloat32 (m_attrib[i].m_normal_z), dgFloat32 (0.0f));
+		n = rotation.RotateVector(n);
+		dgAssert ((n % n) > dgFloat32 (0.0f));
+		n = n.Scale3 (dgRsqrt (n % n));
+		m_attrib[i].m_normal_x = n.m_x;
+		m_attrib[i].m_normal_y = n.m_y;
+		m_attrib[i].m_normal_z = n.m_z;
+	}
+}
+
+dgMatrix dgMeshEffect::CalculateOOBB (dgBigVector& size) const
+{
+	dgObb sphere (CalculateSphere (&m_points[0].m_x, sizeof (dgBigVector), NULL));
+	size = sphere.m_size;
+	size.m_w = 0.0f;
+
+//	dgMatrix permuation (dgGetIdentityMatrix());
+//	permuation[0][0] = dgFloat32 (0.0f);
+//	permuation[0][1] = dgFloat32 (1.0f);
+//	permuation[1][1] = dgFloat32 (0.0f);
+//	permuation[1][2] = dgFloat32 (1.0f);
+//	permuation[2][2] = dgFloat32 (0.0f);
+//	permuation[2][0] = dgFloat32 (1.0f);
+//	while ((size.m_x < size.m_y) || (size.m_x < size.m_z)) {
+//		sphere = permuation * sphere;
+//		size = permuation.UnrotateVector(size);
+//	}
+
+	return sphere;
+}
+
+void dgMeshEffect::CalculateAABB (dgBigVector& minBox, dgBigVector& maxBox) const
+{
+	dgBigVector minP ( dgFloat64 (1.0e15f),  dgFloat64 (1.0e15f),  dgFloat64 (1.0e15f), dgFloat64 (0.0f)); 
+	dgBigVector maxP (-dgFloat64 (1.0e15f), -dgFloat64 (1.0e15f), -dgFloat64 (1.0e15f), dgFloat64 (0.0f)); 
+
+	dgPolyhedra::Iterator iter (*this);
+	const dgBigVector* const points = &m_points[0];
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const edge = &(*iter);
+		const dgBigVector& p (points[edge->m_incidentVertex]);
+
+		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); 
+	}
+
+	minBox = minP;
+	maxBox = maxP;
+}
+
+
+void dgMeshEffect::BeginPolygon ()
+{
+	m_pointCount = 0;
+	m_atribCount = 0;
+	RemoveAll();
+	BeginFace();
+}
+
+
+void dgMeshEffect::AddAtribute (const dgVertexAtribute& attib)
+{
+	if (m_atribCount >= m_maxAtribCount) {
+		m_maxAtribCount *= 2;
+		dgVertexAtribute* const attibArray = (dgVertexAtribute*) GetAllocator()->MallocLow(dgInt32 (m_maxAtribCount * sizeof(dgVertexAtribute)));
+		memcpy (attibArray, m_attrib, m_atribCount * sizeof(dgVertexAtribute));
+		GetAllocator()->FreeLow(m_attrib);
+		m_attrib = attibArray;
+	}
+
+	m_attrib[m_atribCount] = attib;
+
+	dgBigVector n (attib.m_normal_x, attib.m_normal_y, attib.m_normal_z, dgFloat64 (0.0f));
+	dgFloat64 mag2 = n % n ; 
+	if (mag2 < dgFloat64 (1.0e-16f)) {
+		n.m_x = dgFloat64 (0.0f);
+		n.m_y = dgFloat64 (1.0f);
+		n.m_z = dgFloat64 (0.0f);
+	}
+	m_attrib[m_atribCount].m_normal_x = n.m_x;
+	m_attrib[m_atribCount].m_normal_y = n.m_y;
+	m_attrib[m_atribCount].m_normal_z = n.m_z;
+
+	m_attrib[m_atribCount].m_vertex.m_x = QuantizeCordinade(m_attrib[m_atribCount].m_vertex.m_x);
+	m_attrib[m_atribCount].m_vertex.m_y = QuantizeCordinade(m_attrib[m_atribCount].m_vertex.m_y);
+	m_attrib[m_atribCount].m_vertex.m_z = QuantizeCordinade(m_attrib[m_atribCount].m_vertex.m_z);
+	m_atribCount ++;
+}
+
+void dgMeshEffect::AddVertex(const dgBigVector& vertex)
+{
+	if (m_pointCount >= m_maxPointCount) {
+		m_maxPointCount *= 2;
+		dgBigVector* const points = (dgBigVector*) GetAllocator()->MallocLow(dgInt32 (m_maxPointCount * sizeof(dgBigVector)));
+		memcpy (points, m_points, m_pointCount * sizeof(dgBigVector));
+		GetAllocator()->FreeLow(m_points);
+		m_points = points;
+	}
+	
+	m_points[m_pointCount].m_x = QuantizeCordinade(vertex[0]);
+	m_points[m_pointCount].m_y = QuantizeCordinade(vertex[1]);
+	m_points[m_pointCount].m_z = QuantizeCordinade(vertex[2]);
+	m_points[m_pointCount].m_w = vertex.m_w;
+	m_pointCount ++;
+}
+
+
+void dgMeshEffect::AddPoint(const dgFloat64* vertex, dgInt32 material)
+{
+	dgVertexAtribute attib;
+	AddVertex(dgBigVector (vertex[0], vertex[1], vertex[2], vertex[3]));
+	
+	attib.m_vertex.m_x = m_points[m_pointCount - 1].m_x;
+	attib.m_vertex.m_y = m_points[m_pointCount - 1].m_y;
+	attib.m_vertex.m_z = m_points[m_pointCount - 1].m_z;
+	attib.m_vertex.m_w = m_points[m_pointCount - 1].m_w;
+
+	attib.m_normal_x = vertex[4];
+	attib.m_normal_y = vertex[5];
+	attib.m_normal_z = vertex[6];
+	attib.m_u0 = vertex[7];
+	attib.m_v0 = vertex[8];
+	attib.m_u1 = vertex[9];
+	attib.m_v1 = vertex[10];
+	attib.m_material = material;
+
+	AddAtribute (attib);
+}
+
+void dgMeshEffect::PackVertexArrays ()
+{
+	if (m_maxPointCount > m_pointCount) {
+		dgBigVector* const points = (dgBigVector*) GetAllocator()->MallocLow(dgInt32 (m_pointCount * sizeof(dgBigVector)));
+		memcpy (points, m_points, m_pointCount * sizeof(dgBigVector));
+		GetAllocator()->FreeLow(m_points);
+		m_points = points;
+		m_maxPointCount = m_pointCount;
+	}
+
+
+	if (m_maxAtribCount > m_atribCount) {
+		dgVertexAtribute* const attibArray = (dgVertexAtribute*) GetAllocator()->MallocLow(dgInt32 (m_atribCount * sizeof(dgVertexAtribute)));
+		memcpy (attibArray, m_attrib, m_atribCount * sizeof(dgVertexAtribute));
+		GetAllocator()->FreeLow(m_attrib);
+		m_attrib = attibArray;
+		m_maxAtribCount = m_atribCount;
+	}
+};
+
+
+void dgMeshEffect::AddPolygon (dgInt32 count, const dgFloat64* const vertexList, dgInt32 strideIndBytes, dgInt32 material)
+{
+	dgAssert (strideIndBytes >= sizeof (dgBigVector));
+	dgInt32 stride = dgInt32 (strideIndBytes / sizeof (dgFloat64));
+	
+	if (count > 3) {
+		dgPolyhedra polygon (GetAllocator());
+		dgInt32 indexList[256];
+		dgAssert (count < dgInt32 (sizeof (indexList)/sizeof(indexList[0])));
+		for (dgInt32 i = 0; i < count; i ++) {
+			indexList[i] = i;
+		}
+
+		polygon.BeginFace();
+		polygon.AddFace(count, indexList, NULL);
+		polygon.EndFace();
+		polygon.Triangulate(vertexList, strideIndBytes, NULL);
+
+		dgInt32 mark = polygon.IncLRU();
+		dgPolyhedra::Iterator iter (polygon);
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &iter.GetNode()->GetInfo();
+			if ((edge->m_incidentFace > 0) && (edge->m_mark < mark)) {
+				dgInt32 i0 = edge->m_incidentVertex;
+				dgInt32 i1 = edge->m_next->m_incidentVertex;
+				dgInt32 i2 = edge->m_next->m_next->m_incidentVertex;
+				edge->m_mark = mark;
+				edge->m_next->m_mark = mark;
+				edge->m_next->m_next->m_mark = mark;
+
+//				#ifdef _DEBUG
+//					dgBigVector p0_ (&vertexList[i0 * stride]);
+//					dgBigVector p1_ (&vertexList[i1 * stride]);
+//					dgBigVector p2_ (&vertexList[i2 * stride]);
+//					dgBigVector e1_ (p1_ - p0_);
+//					dgBigVector e2_ (p2_ - p0_);
+//					dgBigVector n_ (e1_ * e2_);
+//					dgFloat64 mag2_ = n_ % n_;
+//					dgAssert (mag2_ > dgFloat32 (DG_MESH_EFFECT_PRECISION_SCALE_INV * DG_MESH_EFFECT_PRECISION_SCALE_INV)); 
+//				#endif
+
+				AddPoint(vertexList + i0 * stride, material);
+				AddPoint(vertexList + i1 * stride, material);
+				AddPoint(vertexList + i2 * stride, material);
+
+				#ifdef _DEBUG
+					const dgBigVector& p0 = m_points[m_pointCount - 3];
+					const dgBigVector& p1 = m_points[m_pointCount - 2];
+					const dgBigVector& p2 = m_points[m_pointCount - 1];
+					dgBigVector e1 (p1 - p0);
+					dgBigVector e2 (p2 - p0);
+					dgBigVector n (e1 * e2);
+					dgFloat64 mag3 = n % n;
+					dgAssert (mag3 > dgFloat64 (DG_MESH_EFFECT_PRECISION_SCALE_INV * DG_MESH_EFFECT_PRECISION_SCALE_INV));
+				#endif
+			}
+		}
+
+	} else {
+
+		AddPoint(vertexList, material);
+		AddPoint(vertexList + stride, material);
+		AddPoint(vertexList + stride + stride, material);
+
+		const dgBigVector& p0 = m_points[m_pointCount - 3];
+		const dgBigVector& p1 = m_points[m_pointCount - 2];
+		const dgBigVector& p2 = m_points[m_pointCount - 1];
+		dgBigVector e1 (p1 - p0);
+		dgBigVector e2 (p2 - p0);
+		dgBigVector n (e1 * e2);
+		dgFloat64 mag3 = n % n;
+		if (mag3 < dgFloat64 (DG_MESH_EFFECT_PRECISION_SCALE_INV * DG_MESH_EFFECT_PRECISION_SCALE_INV)) {
+			m_pointCount -= 3;
+			m_atribCount -= 3;
+		}
+	}
+}
+
+#ifndef _NEWTON_USE_DOUBLE
+
+void dgMeshEffect::AddPolygon (dgInt32 count, const dgFloat32* const vertexList, dgInt32 strideIndBytes, dgInt32 material)
+{
+	dgVertexAtribute points[256];
+	dgAssert (count < dgInt32 (sizeof (points)/sizeof (points[0])));
+
+	dgInt32 stride = strideIndBytes / sizeof (dgFloat32);
+	if (stride < 4) {
+		for (dgInt32 i = 0; i < count; i ++) {
+			points[i].m_vertex.m_x = vertexList[i * stride + 0];
+			points[i].m_vertex.m_y = vertexList[i * stride + 1];
+			points[i].m_vertex.m_z = vertexList[i * stride + 2];
+			points[i].m_vertex.m_w = dgFloat64(0.0f);
+			points[i].m_normal_x = dgFloat64(0.0f);
+			points[i].m_normal_y = dgFloat64(1.0f);
+			points[i].m_normal_z = dgFloat64(0.0f);
+			points[i].m_u0 = dgFloat64(0.0f);
+			points[i].m_v0 = dgFloat64(0.0f);
+			points[i].m_u1 = dgFloat64(0.0f);
+			points[i].m_v1 = dgFloat64(0.0f);
+			points[i].m_material = dgFloat64(material);
+		}
+	} else {
+		for (dgInt32 i = 0; i < count; i ++) {
+			points[i].m_vertex.m_x = vertexList[i * stride + 0];
+			points[i].m_vertex.m_y = vertexList[i * stride + 1];
+			points[i].m_vertex.m_z = vertexList[i * stride + 2];
+			points[i].m_vertex.m_w = vertexList[i * stride + 3];
+			points[i].m_normal_x = vertexList[i * stride + 4];
+			points[i].m_normal_y = vertexList[i * stride + 5];
+			points[i].m_normal_z = vertexList[i * stride + 6];
+			points[i].m_u0 = vertexList[i * stride + 7];
+			points[i].m_v0 = vertexList[i * stride + 8];
+			points[i].m_u1 = vertexList[i * stride + 9];
+			points[i].m_v1 = vertexList[i * stride + 10];
+			points[i].m_material = dgFloat64(material);
+		}
+	}
+	AddPolygon (count, &points[0].m_vertex.m_x, sizeof (dgVertexAtribute), material);
+}
+#endif
+
+void dgMeshEffect::EndPolygon (dgFloat64 tol, bool fixTjoint)
+{
+	dgStack<dgInt32>indexMap(m_pointCount);
+	dgStack<dgInt32>attrIndexMap(m_atribCount);
+
+#ifdef _DEBUG
+	for (dgInt32 i = 0; i < m_pointCount; i += 3) {
+		dgBigVector p0 (m_points[i + 0]);
+		dgBigVector p1 (m_points[i + 1]);
+		dgBigVector p2 (m_points[i + 2]);
+		dgBigVector e1 (p1 - p0);
+		dgBigVector e2 (p2 - p0);
+		dgBigVector n (e1 * e2);
+		dgFloat64 mag2 = n % n;
+		dgAssert (mag2 > dgFloat32 (0.0f));
+	}
+#endif
+
+	dgInt32 triangCount = m_pointCount / 3;
+	m_pointCount = dgVertexListToIndexList (&m_points[0].m_x, sizeof (dgBigVector), sizeof (dgBigVector)/sizeof (dgFloat64), m_pointCount, &indexMap[0], tol);
+	m_atribCount = dgVertexListToIndexList (&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), sizeof (dgVertexAtribute)/sizeof (dgFloat64), m_atribCount, &attrIndexMap[0], tol);
+
+	for (dgInt32 i = 0; i < triangCount; i ++) {
+		dgInt32 index[3];
+		dgInt64 userdata[3];
+
+		index[0] = indexMap[i * 3 + 0];
+		index[1] = indexMap[i * 3 + 1];
+		index[2] = indexMap[i * 3 + 2];
+
+
+		dgBigVector e1 (m_points[index[1]] - m_points[index[0]]);
+		dgBigVector e2 (m_points[index[2]] - m_points[index[0]]);
+
+		dgBigVector n (e1 * e2);
+		dgFloat64 mag2 = n % n;
+		if (mag2 > dgFloat64 (1.0e-12f)) {
+			userdata[0] = attrIndexMap[i * 3 + 0];
+			userdata[1] = attrIndexMap[i * 3 + 1];
+			userdata[2] = attrIndexMap[i * 3 + 2];
+			dgEdge* const edge = AddFace (3, index, userdata);
+			if (!edge) {
+				dgAssert ((m_pointCount + 3) <= m_maxPointCount);
+
+				m_points[m_pointCount + 0] = m_points[index[0]];
+				m_points[m_pointCount + 1] = m_points[index[1]];
+				m_points[m_pointCount + 2] = m_points[index[2]];
+
+				index[0] = m_pointCount + 0;
+				index[1] = m_pointCount + 1;
+				index[2] = m_pointCount + 2;
+
+				m_pointCount += 3;
+
+				#ifdef _DEBUG
+					dgEdge* test = AddFace (3, index, userdata);
+					dgAssert (test);
+				#else 
+					AddFace (3, index, userdata);
+				#endif
+			}
+		}
+	}
+	EndFace();
+
+	if (fixTjoint) {
+		RepairTJoints ();
+	}
+
+#ifdef _DEBUG
+	dgPolyhedra::Iterator iter (*this);	
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+		if (face->m_incidentFace > 0) {
+			dgBigVector p0 (m_points[face->m_incidentVertex]);
+			dgBigVector p1 (m_points[face->m_next->m_incidentVertex]);
+			dgBigVector p2 (m_points[face->m_next->m_next->m_incidentVertex]);
+			dgBigVector e1 (p1 - p0);
+			dgBigVector e2 (p2 - p0);
+			dgBigVector n (e1 * e2);
+			dgFloat64 mag2 = n % n;
+			dgAssert (mag2 >= dgFloat32 (0.0f));
+		}
+	}
+#endif
+}
+
+
+void dgMeshEffect::BuildFromVertexListIndexList(
+	dgInt32 faceCount, const dgInt32* const faceIndexCount, const dgInt32* const faceMaterialIndex, 
+	const dgFloat32* const vertex, dgInt32 vertexStrideInBytes, const dgInt32* const vertexIndex,
+	const dgFloat32* const normal, dgInt32  normalStrideInBytes, const dgInt32* const normalIndex,
+	const dgFloat32* const uv0, dgInt32  uv0StrideInBytes, const dgInt32* const uv0Index,
+	const dgFloat32* const uv1, dgInt32  uv1StrideInBytes, const dgInt32* const uv1Index)
+{
+	BeginPolygon ();
+
+	// calculate vertex Count
+	dgInt32 acc = 0;
+	dgInt32 vertexCount = 0;
+	for (dgInt32 j = 0; j < faceCount; j ++) {
+		dgInt32 count = faceIndexCount[j];
+		for (dgInt32 i = 0; i < count; i ++) {
+			vertexCount = dgMax(vertexCount, vertexIndex[acc + i] + 1);
+		}
+		acc += count;
+	}
+
+	dgInt32 layerCountBase = 0;
+	dgInt32 vertexStride = dgInt32 (vertexStrideInBytes / sizeof (dgFloat32));
+	for (dgInt32 i = 0; i < vertexCount; i ++) {
+		dgInt32 index = i * vertexStride;
+		dgBigVector v (vertex[index + 0], vertex[index + 1], vertex[index + 2], vertex[index + 3]);
+		AddVertex (v);
+		layerCountBase += (vertex[index + 3]) > dgFloat32(layerCountBase);
+	}
+
+
+	dgInt32 maxAttribCount = 0;
+	for (dgInt32 j = 0; j < faceCount; j ++) {
+		maxAttribCount += faceIndexCount[j];
+	}
+	dgStack<dgInt32>attrIndexMap(maxAttribCount);
+
+	acc = 0;
+	dgInt32 currentCount = 0;
+	dgInt32 attributeCount = 0;
+	dgInt32 attributeCountMarker = 0;
+	dgInt32 normalStride = dgInt32 (normalStrideInBytes / sizeof (dgFloat32));
+	dgInt32 uv0Stride = dgInt32 (uv0StrideInBytes / sizeof (dgFloat32));
+	dgInt32 uv1Stride = dgInt32 (uv1StrideInBytes / sizeof (dgFloat32));
+	for (dgInt32 j = 0; j < faceCount; j ++) {
+		dgInt32 indexCount = faceIndexCount[j];
+		dgInt32 materialIndex = faceMaterialIndex[j];
+		for (dgInt32 i = 0; i < indexCount; i ++) {
+			dgVertexAtribute point;
+			dgInt32 index = vertexIndex[acc + i];
+			point.m_vertex = m_points[index];
+			
+			index = normalIndex[(acc + i)] * normalStride;
+			point.m_normal_x =  normal[index + 0];
+			point.m_normal_y =  normal[index + 1];
+			point.m_normal_z =  normal[index + 2];
+
+			index = uv0Index[(acc + i)] * uv0Stride;
+			point.m_u0 = uv0[index + 0];
+			point.m_v0 = uv0[index + 1];
+			
+			index = uv1Index[(acc + i)] * uv1Stride;
+			point.m_u1 = uv1[index + 0];
+			point.m_v1 = uv1[index + 1];
+
+			point.m_material = materialIndex;
+			AddAtribute(point);
+
+			attrIndexMap[attributeCount] = attributeCount;
+			attributeCount ++;
+		}
+
+		acc += indexCount;
+		if (attributeCount >= (attributeCountMarker + 1024 * 256)) {
+			dgInt32 count = attributeCount - attributeCountMarker;
+			dgInt32 newCount = dgVertexListToIndexList (&m_attrib[currentCount].m_vertex.m_x, sizeof (dgVertexAtribute), sizeof (dgVertexAtribute) / sizeof (dgFloat64), count, &attrIndexMap[attributeCountMarker], DG_VERTEXLIST_INDEXLIST_TOL);
+			for (dgInt32 i = 0; i < count; i ++) {
+				attrIndexMap[attributeCountMarker + i] += currentCount;
+			}
+			currentCount += newCount;
+			m_atribCount = currentCount;
+			attributeCountMarker = attributeCount;
+		}
+	}
+
+
+	if (attributeCountMarker) {
+		dgInt32 count = attributeCount - attributeCountMarker;
+		dgInt32 newCount = dgVertexListToIndexList (&m_attrib[currentCount].m_vertex.m_x, sizeof (dgVertexAtribute), sizeof (dgVertexAtribute) / sizeof (dgFloat64), count, &attrIndexMap[attributeCountMarker], DG_VERTEXLIST_INDEXLIST_TOL);
+		for (dgInt32 i = 0; i < count; i ++) {
+			attrIndexMap[attributeCountMarker + i] += currentCount;
+		}
+		currentCount += newCount;
+		m_atribCount = currentCount;
+		attributeCountMarker = attributeCount;
+
+
+		dgStack<dgInt32>indirectAttrIndexMap(m_atribCount);
+		m_atribCount = dgVertexListToIndexList (&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), sizeof (dgVertexAtribute) / sizeof (dgFloat64), m_atribCount, &indirectAttrIndexMap[0], DG_VERTEXLIST_INDEXLIST_TOL);
+
+		for (dgInt32 i = 0; i < maxAttribCount; i ++) {
+			dgInt32 j = attrIndexMap[i];
+			attrIndexMap[i] = indirectAttrIndexMap[j];
+		}
+
+	} else {
+		m_atribCount = dgVertexListToIndexList (&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), sizeof (dgVertexAtribute) / sizeof (dgFloat64), m_atribCount, &attrIndexMap[0], DG_VERTEXLIST_INDEXLIST_TOL);
+	}
+
+
+	bool hasFaces = true;
+	dgStack<dgInt8> faceMark (faceCount);
+	memset (&faceMark[0], 1, size_t (faceMark.GetSizeInBytes()));
+	
+	dgInt32 layerCount = 0;
+	while (hasFaces) {
+		acc = 0;
+		hasFaces = false;
+		dgInt32 vertexBank = layerCount * vertexCount;
+		for (dgInt32 j = 0; j < faceCount; j ++) {
+			int indexCount = faceIndexCount[j];
+			if (indexCount > 0) {
+				dgInt32 index[256];
+				dgInt64 userdata[256];
+				dgAssert (indexCount >= 3);
+				dgAssert (indexCount < dgInt32 (sizeof (index) / sizeof (index[0])));
+
+				if (faceMark[j]) {
+					for (int i = 0; i < indexCount; i ++) {
+						index[i] = vertexIndex[acc + i] + vertexBank;
+						userdata[i] = attrIndexMap[acc + i];
+					}
+					dgEdge* const edge = AddFace (indexCount, index, userdata);
+					if (edge) {
+						faceMark[j] = 0;
+					} else {
+						// check if the face is not degenerated
+						bool degeneratedFace = false;
+						for (int i = 0; i < indexCount - 1; i ++) {
+							for (int k = i + 1; k < indexCount; k ++) {
+								if (index[i] == index[k]) {
+									degeneratedFace = true;		
+								}
+							}
+						}
+						if (degeneratedFace) {
+							faceMark[j] = 0;
+						} else {
+							hasFaces = true;
+						}
+					}
+				}
+				acc += indexCount;
+			}
+		}
+		if (hasFaces) {
+			layerCount ++;
+			for (int i = 0; i < vertexCount; i ++) {
+				int index = i * vertexStride;
+				AddVertex (dgBigVector (vertex[index + 0], vertex[index + 1], vertex[index + 2], dgFloat64 (layerCount + layerCountBase)));
+			}
+		}
+	}
+
+	EndFace();
+	PackVertexArrays ();
+}
+
+
+dgInt32 dgMeshEffect::GetTotalFaceCount() const
+{
+	return GetFaceCount();
+}
+
+dgInt32 dgMeshEffect::GetTotalIndexCount() const
+{
+	Iterator iter (*this);
+	dgInt32 count = 0;
+	dgInt32 mark = IncLRU();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+		
+		dgEdge* ptr = edge;
+		do {
+			count ++;
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+	}
+	return count;
+}
+
+void dgMeshEffect::GetFaces (dgInt32* const facesIndex, dgInt32* const materials, void** const faceNodeList) const
+{
+	Iterator iter (*this);
+
+	dgInt32 faces = 0;
+	dgInt32 indexCount = 0;
+	dgInt32 mark = IncLRU();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+
+		dgInt32 faceCount = 0;
+		dgEdge* ptr = edge;
+		do {
+//			indexList[indexCount] = dgInt32 (ptr->m_userData);
+			faceNodeList[indexCount] = GetNodeFromInfo (*ptr);
+			indexCount ++;
+			faceCount ++;
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+
+		facesIndex[faces] = faceCount;
+		materials[faces] = dgFastInt(m_attrib[dgInt32 (edge->m_userData)].m_material);
+		faces ++;
+	}
+}
+
+void* dgMeshEffect::GetFirstVertex () const
+{
+	Iterator iter (*this);
+	iter.Begin();
+
+	dgTreeNode* node = NULL;
+	if (iter) {
+		dgInt32 mark = IncLRU();
+		node = iter.GetNode();
+
+		dgEdge* const edge = &node->GetInfo();
+		dgEdge* ptr = edge;
+		do {
+			ptr->m_mark = mark;
+			ptr = ptr->m_twin->m_next;
+		} while (ptr != edge);
+	}
+	return node; 
+}
+
+void* dgMeshEffect::GetNextVertex (const void* const vertex) const
+{
+	dgTreeNode* node = (dgTreeNode*) vertex;
+	dgInt32 mark = node->GetInfo().m_mark;
+
+	Iterator iter (*this);
+	iter.Set (node);
+	for (iter ++; iter; iter ++) {
+		dgTreeNode* node = iter.GetNode();
+		if (node->GetInfo().m_mark != mark) {
+			dgEdge* const edge = &node->GetInfo();
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != edge);
+			return node; 
+		}
+	}
+	return NULL; 
+}
+
+dgInt32 dgMeshEffect::GetVertexIndex (const void* const vertex) const
+{
+	dgTreeNode* const node = (dgTreeNode*) vertex;
+	dgEdge* const edge = &node->GetInfo();
+	return edge->m_incidentVertex;
+}
+
+
+void* dgMeshEffect::GetFirstPoint () const
+{
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgTreeNode* const node = iter.GetNode();
+		dgEdge* const edge = &node->GetInfo();
+		if (edge->m_incidentFace > 0) {
+			return node;
+		}
+	}
+	return NULL; 
+}
+
+void* dgMeshEffect::GetNextPoint (const void* const point) const
+{
+	Iterator iter (*this);
+	iter.Set ((dgTreeNode*) point);
+	for (iter ++; iter; iter ++) {
+		dgTreeNode* const node = iter.GetNode();
+		dgEdge* const edge = &node->GetInfo();
+		if (edge->m_incidentFace > 0) {
+			return node; 
+		}
+	}
+	return NULL; 
+}
+
+dgInt32 dgMeshEffect::GetPointIndex (const void* const point) const
+{
+	dgTreeNode* const node = (dgTreeNode*) point;
+	dgEdge* const edge = &node->GetInfo();
+	return int (edge->m_userData);
+}
+
+dgInt32 dgMeshEffect::GetVertexIndexFromPoint (const void* const point) const
+{
+	return GetVertexIndex (point);
+}
+
+
+dgEdge* dgMeshEffect::SpliteFace (dgInt32 v0, dgInt32 v1)
+{
+	if (!FindEdge(v0, v1)) {
+		dgPolyhedra::dgPairKey key (v0, 0);
+		dgTreeNode* const node = FindGreaterEqual(key.GetVal());
+		if (node) {
+			dgEdge* const edge = &node->GetInfo();
+			dgEdge* edge0 = edge;
+			do {
+				if (edge0->m_incidentFace > 0) {
+					for (dgEdge* edge1 = edge0->m_next->m_next; edge1 != edge0->m_prev; edge1 = edge1->m_next) {
+						if (edge1->m_incidentVertex == v1) {
+							return ConnectVertex (edge0, edge1);
+						}
+					};
+				}
+				edge0 = edge0->m_twin->m_next;
+			} while (edge0 != edge);
+		}
+	}
+	return NULL;
+}
+
+
+void* dgMeshEffect::GetFirstEdge () const
+{
+	Iterator iter (*this);
+	iter.Begin();
+
+	dgTreeNode* node = NULL;
+	if (iter) {
+		dgInt32 mark = IncLRU();
+
+		node = iter.GetNode();
+
+		dgEdge* const edge = &node->GetInfo();
+		edge->m_mark = mark;
+		edge->m_twin->m_mark = mark;
+	}
+	return node; 
+}
+
+void* dgMeshEffect::GetNextEdge (const void* const edge) const
+{
+	dgTreeNode* node = (dgTreeNode*) edge;
+	dgInt32 mark = node->GetInfo().m_mark;
+
+	Iterator iter (*this);
+	iter.Set (node);
+	for (iter ++; iter; iter ++) {
+		dgTreeNode* node = iter.GetNode();
+		if (node->GetInfo().m_mark != mark) {
+			node->GetInfo().m_mark = mark;
+			node->GetInfo().m_twin->m_mark = mark;
+			return node; 
+		}
+	}
+	return NULL; 
+}
+
+void dgMeshEffect::GetEdgeIndex (const void* const edge, dgInt32& v0, dgInt32& v1) const
+{
+	dgTreeNode* node = (dgTreeNode*) edge;
+	v0 = node->GetInfo().m_incidentVertex;
+	v1 = node->GetInfo().m_twin->m_incidentVertex;
+}
+
+//void dgMeshEffect::GetEdgeAttributeIndex (const void* edge, dgInt32& v0, dgInt32& v1) const
+//{
+//	dgTreeNode* node = (dgTreeNode*) edge;
+//	v0 = int (node->GetInfo().m_userData);
+//	v1 = int (node->GetInfo().m_twin->m_userData);
+//}
+
+
+void* dgMeshEffect::GetFirstFace () const
+{
+	Iterator iter (*this);
+	iter.Begin();
+
+	dgTreeNode* node = NULL;
+	if (iter) {
+		dgInt32 mark = IncLRU();
+		node = iter.GetNode();
+
+		dgEdge* const edge = &node->GetInfo();
+		dgEdge* ptr = edge;
+		do {
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+	}
+
+	return node;
+}
+
+void* dgMeshEffect::GetNextFace (const void* const face) const
+{
+	dgTreeNode* node = (dgTreeNode*) face;
+	dgInt32 mark = node->GetInfo().m_mark;
+
+	Iterator iter (*this);
+	iter.Set (node);
+	for (iter ++; iter; iter ++) {
+		dgTreeNode* node = iter.GetNode();
+		if (node->GetInfo().m_mark != mark) {
+			dgEdge* const edge = &node->GetInfo();
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+			return node; 
+		}
+	}
+	return NULL; 
+}
+
+
+dgInt32 dgMeshEffect::IsFaceOpen (const void* const face) const
+{
+	dgTreeNode* const node = (dgTreeNode*) face;
+	dgEdge* const edge = &node->GetInfo();
+	return (edge->m_incidentFace > 0) ? 0 : 1;
+}
+
+dgInt32 dgMeshEffect::GetFaceMaterial (const void* const face) const
+{
+	dgTreeNode* const node = (dgTreeNode*) face;
+	dgEdge* const edge = &node->GetInfo();
+	return dgInt32 (m_attrib[edge->m_userData].m_material);
+}
+
+void dgMeshEffect::SetFaceMaterial (const void* const face, int mateialID) const
+{
+	dgTreeNode* const node = (dgTreeNode*) face;
+	dgEdge* const edge = &node->GetInfo();
+	if (edge->m_incidentFace > 0) {
+		dgEdge* ptr = edge;
+		do {
+			dgVertexAtribute* const attrib = &m_attrib[ptr->m_userData];
+			attrib->m_material = dgFloat64 (mateialID);
+			ptr = ptr->m_next;
+		} while (ptr != edge) ;
+	}
+}
+
+
+dgInt32 dgMeshEffect::GetFaceIndexCount (const void* const face) const
+{
+	int count = 0;
+	dgTreeNode* node = (dgTreeNode*) face;
+	dgEdge* const edge = &node->GetInfo();
+	dgEdge* ptr = edge;
+	do {
+		count ++;
+		ptr = ptr->m_next;
+	} while (ptr != edge);
+	return count; 
+}
+
+void dgMeshEffect::GetFaceIndex (const void* const face, dgInt32* const indices) const
+{
+	int count = 0;
+	dgTreeNode* node = (dgTreeNode*) face;
+	dgEdge* const edge = &node->GetInfo();
+	dgEdge* ptr = edge;
+	do {
+		indices[count] =  ptr->m_incidentVertex;
+		count ++;
+		ptr = ptr->m_next;
+	} while (ptr != edge);
+}
+
+void dgMeshEffect::GetFaceAttributeIndex (const void* const face, dgInt32* const indices) const
+{
+	int count = 0;
+	dgTreeNode* node = (dgTreeNode*) face;
+	dgEdge* const edge = &node->GetInfo();
+	dgEdge* ptr = edge;
+	do {
+		indices[count] = int (ptr->m_userData);
+		count ++;
+		ptr = ptr->m_next;
+	} while (ptr != edge);
+}
+
+
+dgBigVector dgMeshEffect::CalculateFaceNormal (const void* const face) const
+{
+	dgTreeNode* const node = (dgTreeNode*) face;
+	dgEdge* const faceEdge = &node->GetInfo();
+	dgBigVector normal (FaceNormal (faceEdge, &m_points[0].m_x, sizeof (m_points[0])));
+	normal = normal.Scale3 (1.0f / sqrt (normal % normal));
+	return normal;
+}
+
+/*
+dgInt32 GetTotalFaceCount() const;
+{
+	dgInt32 mark;
+	dgInt32 count;
+	dgInt32 materialCount;
+	dgInt32 materials[256];
+	dgInt32 streamIndexMap[256];
+	dgIndexArray* array; 
+
+	count = 0;
+	materialCount = 0;
+
+	array = (dgIndexArray*) GetAllocator()->MallocLow (4 * sizeof (dgInt32) * GetCount() + sizeof (dgIndexArray) + 2048);
+	array->m_indexList = (dgInt32*)&array[1];
+
+	mark = IncLRU();
+	dgPolyhedra::Iterator iter (*this);	
+	memset(streamIndexMap, 0, sizeof (streamIndexMap));
+	for(iter.Begin(); iter; iter ++){
+
+		dgEdge* const edge;
+		edge = &(*iter);
+		if ((edge->m_incidentFace >= 0) && (edge->m_mark != mark)) {
+			dgEdge* ptr;
+			dgInt32 hashValue;
+			dgInt32 index0;
+			dgInt32 index1;
+
+			ptr = edge;
+			ptr->m_mark = mark;
+			index0 = dgInt32 (ptr->m_userData);
+
+			ptr = ptr->m_next;
+			ptr->m_mark = mark;
+			index1 = dgInt32 (ptr->m_userData);
+
+			ptr = ptr->m_next;
+			do {
+				ptr->m_mark = mark;
+
+				array->m_indexList[count * 4 + 0] = index0;
+				array->m_indexList[count * 4 + 1] = index1;
+				array->m_indexList[count * 4 + 2] = dgInt32 (ptr->m_userData);
+				array->m_indexList[count * 4 + 3] = m_attrib[dgInt32 (edge->m_userData)].m_material;
+				index1 = dgInt32 (ptr->m_userData);
+
+				hashValue = array->m_indexList[count * 4 + 3] & 0xff;
+				streamIndexMap[hashValue] ++;
+				materials[hashValue] = array->m_indexList[count * 4 + 3];
+				count ++;
+
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+	}
+*/
+
+
+
+
+void dgMeshEffect::GetVertexStreams (dgInt32 vetexStrideInByte, dgFloat32* const vertex, 
+									 dgInt32 normalStrideInByte, dgFloat32* const normal, 
+									 dgInt32 uvStrideInByte0, dgFloat32* const uv0, 
+									 dgInt32 uvStrideInByte1, dgFloat32* const uv1)
+{
+	uvStrideInByte0 /= sizeof (dgFloat32);
+	uvStrideInByte1 /= sizeof (dgFloat32);
+	vetexStrideInByte /= sizeof (dgFloat32);
+	normalStrideInByte /= sizeof (dgFloat32);
+	for (dgInt32 i = 0; i < m_atribCount; i ++)	{
+		dgInt32 j = i * vetexStrideInByte;
+		vertex[j + 0] = dgFloat32 (m_attrib[i].m_vertex.m_x);
+		vertex[j + 1] = dgFloat32 (m_attrib[i].m_vertex.m_y);
+		vertex[j + 2] = dgFloat32 (m_attrib[i].m_vertex.m_z);
+
+		j = i * normalStrideInByte;
+		normal[j + 0] = dgFloat32 (m_attrib[i].m_normal_x);
+		normal[j + 1] = dgFloat32 (m_attrib[i].m_normal_y);
+		normal[j + 2] = dgFloat32 (m_attrib[i].m_normal_z);
+
+		j = i * uvStrideInByte1;
+		uv1[j + 0] = dgFloat32 (m_attrib[i].m_u1);
+		uv1[j + 1] = dgFloat32 (m_attrib[i].m_v1);
+
+		j = i * uvStrideInByte0;
+		uv0[j + 0] = dgFloat32 (m_attrib[i].m_u0);
+		uv0[j + 1] = dgFloat32 (m_attrib[i].m_v0);
+	}
+}
+
+
+void dgMeshEffect::GetIndirectVertexStreams(
+	dgInt32 vetexStrideInByte, dgFloat64* const vertex, dgInt32* const vertexIndices, dgInt32* const vertexCount,
+	dgInt32 normalStrideInByte, dgFloat64* const normal, dgInt32* const normalIndices, dgInt32* const normalCount,
+	dgInt32 uvStrideInByte0, dgFloat64* const uv0, dgInt32* const uvIndices0, dgInt32* const uvCount0,
+	dgInt32 uvStrideInByte1, dgFloat64* const uv1, dgInt32* const uvIndices1, dgInt32* const uvCount1)
+{
+/*
+	GetVertexStreams (vetexStrideInByte, vertex, normalStrideInByte, normal, uvStrideInByte0, uv0, uvStrideInByte1, uv1);
+
+	*vertexCount = dgVertexListToIndexList(vertex, vetexStrideInByte, vetexStrideInByte, 0, m_atribCount, vertexIndices, dgFloat32 (0.0f));
+	*normalCount = dgVertexListToIndexList(normal, normalStrideInByte, normalStrideInByte, 0, m_atribCount, normalIndices, dgFloat32 (0.0f));
+
+	dgTriplex* const tmpUV = (dgTriplex*) GetAllocator()->MallocLow (dgInt32 (sizeof (dgTriplex) * m_atribCount));
+	dgInt32 stride = dgInt32 (uvStrideInByte1 /sizeof (dgFloat32));
+	for (dgInt32 i = 0; i < m_atribCount; i ++){
+		tmpUV[i].m_x = uv1[i * stride + 0];
+		tmpUV[i].m_y = uv1[i * stride + 1];
+		tmpUV[i].m_z = dgFloat32 (0.0f);
+	}
+
+	dgInt32 count = dgVertexListToIndexList(&tmpUV[0].m_x, sizeof (dgTriplex), sizeof (dgTriplex), 0, m_atribCount, uvIndices1, dgFloat32 (0.0f));
+	for (dgInt32 i = 0; i < count; i ++){
+		uv1[i * stride + 0] = tmpUV[i].m_x;
+		uv1[i * stride + 1] = tmpUV[i].m_y;
+	}
+	*uvCount1 = count;
+
+	stride = dgInt32 (uvStrideInByte0 /sizeof (dgFloat32));
+	for (dgInt32 i = 0; i < m_atribCount; i ++){
+		tmpUV[i].m_x = uv0[i * stride + 0];
+		tmpUV[i].m_y = uv0[i * stride + 1];
+		tmpUV[i].m_z = dgFloat32 (0.0f);
+	}
+	count = dgVertexListToIndexList(&tmpUV[0].m_x, sizeof (dgTriplex), sizeof (dgTriplex), 0, m_atribCount, uvIndices0, dgFloat32 (0.0f));
+	for (dgInt32 i = 0; i < count; i ++){
+		uv0[i * stride + 0] = tmpUV[i].m_x;
+		uv0[i * stride + 1] = tmpUV[i].m_y;
+	}
+	*uvCount0 = count;
+
+	GetAllocator()->FreeLow (tmpUV);
+*/
+}
+
+dgMeshEffect::dgIndexArray* dgMeshEffect::MaterialGeometryBegin()
+{
+	dgInt32 materials[256];
+	dgInt32 streamIndexMap[256];
+
+	dgInt32 count = 0;
+	dgInt32 materialCount = 0;
+	
+	dgIndexArray* const array = (dgIndexArray*) GetAllocator()->MallocLow (dgInt32 (4 * sizeof (dgInt32) * GetCount() + sizeof (dgIndexArray) + 2048));
+	array->m_indexList = (dgInt32*)&array[1];
+	
+	dgInt32 mark = IncLRU();
+	dgPolyhedra::Iterator iter (*this);	
+	memset(streamIndexMap, 0, sizeof (streamIndexMap));
+	for(iter.Begin(); iter; iter ++){
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_incidentFace >= 0) && (edge->m_mark != mark)) {
+			dgEdge* ptr = edge;
+			ptr->m_mark = mark;
+			dgInt32 index0 = dgInt32 (ptr->m_userData);
+
+			ptr = ptr->m_next;
+			ptr->m_mark = mark;
+			dgInt32 index1 = dgInt32 (ptr->m_userData);
+
+			ptr = ptr->m_next;
+			do {
+				ptr->m_mark = mark;
+
+				array->m_indexList[count * 4 + 0] = index0;
+				array->m_indexList[count * 4 + 1] = index1;
+				array->m_indexList[count * 4 + 2] = dgInt32 (ptr->m_userData);
+				array->m_indexList[count * 4 + 3] = dgInt32 (m_attrib[dgInt32 (edge->m_userData)].m_material);
+				index1 = dgInt32 (ptr->m_userData);
+
+				dgInt32 hashValue = array->m_indexList[count * 4 + 3] & 0xff;
+				streamIndexMap[hashValue] ++;
+				materials[hashValue] = array->m_indexList[count * 4 + 3];
+				count ++;
+
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+	}
+
+	array->m_indexCount = count;
+	array->m_materialCount = materialCount;
+
+	count = 0;
+	for (dgInt32 i = 0; i < 256;i ++) {
+		if (streamIndexMap[i]) {
+			array->m_materials[count] = materials[i];
+			array->m_materialsIndexCount[count] = streamIndexMap[i] * 3;
+			count ++;
+		}
+	}
+
+	array->m_materialCount = count;
+
+	return array;
+}
+
+void dgMeshEffect::MaterialGeomteryEnd(dgIndexArray* const handle)
+{
+	GetAllocator()->FreeLow (handle);
+}
+
+
+dgInt32 dgMeshEffect::GetFirstMaterial (dgIndexArray* const handle) const
+{
+	return GetNextMaterial (handle, -1);
+}
+
+dgInt32 dgMeshEffect::GetNextMaterial (dgIndexArray* const handle, dgInt32 materialId) const
+{
+	materialId ++;
+	if(materialId >= handle->m_materialCount) {
+		materialId = -1;
+	}
+	return materialId;
+}
+
+void dgMeshEffect::GetMaterialGetIndexStream (dgIndexArray* const handle, dgInt32 materialHandle, dgInt32* const indexArray) const
+{
+
+	dgInt32 index = 0;
+	dgInt32 textureID = handle->m_materials[materialHandle];
+	for (dgInt32 j = 0; j < handle->m_indexCount; j ++) {
+		if (handle->m_indexList[j * 4 + 3] == textureID) {
+			indexArray[index + 0] = handle->m_indexList[j * 4 + 0];
+			indexArray[index + 1] = handle->m_indexList[j * 4 + 1];
+			indexArray[index + 2] = handle->m_indexList[j * 4 + 2];
+
+			index += 3;
+		}
+	}
+}
+
+void dgMeshEffect::GetMaterialGetIndexStreamShort (dgIndexArray* const handle, dgInt32 materialHandle, dgInt16* const indexArray) const
+{
+	dgInt32 index = 0;
+	dgInt32 textureID = handle->m_materials[materialHandle];
+	for (dgInt32 j = 0; j < handle->m_indexCount; j ++) {
+		if (handle->m_indexList[j * 4 + 3] == textureID) {
+			indexArray[index + 0] = (dgInt16)handle->m_indexList[j * 4 + 0];
+			indexArray[index + 1] = (dgInt16)handle->m_indexList[j * 4 + 1];
+			indexArray[index + 2] = (dgInt16)handle->m_indexList[j * 4 + 2];
+			index += 3;
+		}
+	}
+}
+
+dgCollisionInstance* dgMeshEffect::CreateCollisionTree(dgWorld* const world, dgInt32 shapeID) const
+{
+	dgCollisionBVH* const collision = new  (GetAllocator()) dgCollisionBVH (world);
+
+	collision->BeginBuild();
+
+	dgInt32 mark = IncLRU();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+		if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+			dgInt32 count = 0;
+			dgVector polygon[256]; 
+			dgEdge* ptr = face;
+			do {
+				polygon[count] = dgVector (m_points[ptr->m_incidentVertex]);
+				polygon[count].m_w = dgFloat32 (0.0f);
+				count ++;
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			collision->AddFace(count, &polygon[0].m_x, sizeof (dgVector), dgInt32 (m_attrib[face->m_userData].m_material));
+		}
+	}
+	collision->EndBuild(0);
+
+	dgCollisionInstance* const instance = world->CreateInstance(collision, shapeID, dgGetIdentityMatrix());
+	collision->Release();
+	return instance;
+}
+
+dgCollisionInstance* dgMeshEffect::CreateConvexCollision(dgWorld* const world, dgFloat64 tolerance, dgInt32 shapeID, const dgMatrix& srcMatrix) const
+{
+	dgStack<dgVector> poolPtr (m_pointCount * 2); 
+	dgVector* const pool = &poolPtr[0];
+
+	dgBigVector minBox;
+	dgBigVector maxBox;
+	CalculateAABB (minBox, maxBox);
+	dgVector com ((minBox + maxBox).Scale3 (dgFloat32 (0.5f)));
+
+	dgInt32 count = 0;
+	dgInt32 mark = IncLRU();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const vertex = &(*iter);
+		if (vertex->m_mark != mark) {
+			dgEdge* ptr = vertex;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != vertex);
+
+			if (count < dgInt32 (poolPtr.GetElementsCount())) {
+				const dgBigVector p = m_points[vertex->m_incidentVertex];
+				pool[count] = dgVector (p) - com;
+				count ++;
+			}
+		}
+	}
+
+	dgMatrix matrix (srcMatrix);
+	matrix.m_posit += matrix.RotateVector(com);
+	matrix.m_posit.m_w = dgFloat32 (1.0f);
+
+	dgUnsigned32 crc = dgCollisionConvexHull::CalculateSignature (count, &pool[0].m_x, sizeof (dgVector));
+	dgCollisionConvexHull* const collision = new (GetAllocator()) dgCollisionConvexHull (GetAllocator(), crc, count, sizeof (dgVector), dgFloat32 (tolerance), &pool[0].m_x);
+	if (!collision->GetConvexVertexCount()) {
+		collision->Release();
+		return NULL;
+	}
+	dgCollisionInstance* const instance = world->CreateInstance(collision, shapeID, matrix);
+	collision->Release();
+	return instance;
+}
+
+
+void dgMeshEffect::TransformMesh (const dgMatrix& matrix)
+{
+	dgMatrix normalMatrix (matrix);
+	normalMatrix.m_posit = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));
+
+	matrix.TransformTriplex (&m_points->m_x, sizeof (dgBigVector), &m_points->m_x, sizeof (dgBigVector), m_pointCount);
+	matrix.TransformTriplex (&m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), &m_attrib[0].m_vertex.m_x, sizeof (dgVertexAtribute), m_atribCount);
+	normalMatrix.TransformTriplex (&m_attrib[0].m_normal_x, sizeof (dgVertexAtribute), &m_attrib[0].m_normal_x, sizeof (dgVertexAtribute), m_atribCount);
+}
+
+
+dgMeshEffect::dgVertexAtribute dgMeshEffect::InterpolateEdge (dgEdge* const edge, dgFloat64 param) const
+{
+	dgVertexAtribute attrEdge;
+	dgFloat64 t1 = param;
+	dgFloat64 t0 = dgFloat64 (1.0f) - t1;
+	dgAssert (t1 >= dgFloat64(0.0f));
+	dgAssert (t1 <= dgFloat64(1.0f));
+
+	const dgVertexAtribute& attrEdge0 = m_attrib[edge->m_userData];
+	const dgVertexAtribute& attrEdge1 = m_attrib[edge->m_next->m_userData];
+
+	attrEdge.m_vertex.m_x = attrEdge0.m_vertex.m_x * t0 + attrEdge1.m_vertex.m_x * t1;
+	attrEdge.m_vertex.m_y = attrEdge0.m_vertex.m_y * t0 + attrEdge1.m_vertex.m_y * t1;
+	attrEdge.m_vertex.m_z = attrEdge0.m_vertex.m_z * t0 + attrEdge1.m_vertex.m_z * t1;
+	attrEdge.m_vertex.m_w = dgFloat32(0.0f);
+	attrEdge.m_normal_x = attrEdge0.m_normal_x * t0 +  attrEdge1.m_normal_x * t1; 
+	attrEdge.m_normal_y = attrEdge0.m_normal_y * t0 +  attrEdge1.m_normal_y * t1; 
+	attrEdge.m_normal_z = attrEdge0.m_normal_z * t0 +  attrEdge1.m_normal_z * t1; 
+	attrEdge.m_u0 = attrEdge0.m_u0 * t0 +  attrEdge1.m_u0 * t1;
+	attrEdge.m_v0 = attrEdge0.m_v0 * t0 +  attrEdge1.m_v0 * t1;
+	attrEdge.m_u1 = attrEdge0.m_u1 * t0 +  attrEdge1.m_u1 * t1;
+	attrEdge.m_v1 = attrEdge0.m_v1 * t0 +  attrEdge1.m_v1 * t1;
+	attrEdge.m_material = attrEdge0.m_material;
+	return attrEdge;
+}
+
+bool dgMeshEffect::Sanity () const
+{
+	#ifdef  _DEBUG
+		dgMeshEffect::Iterator iter (*this);
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &iter.GetNode()->GetInfo();
+
+			dgAssert (edge->m_twin->m_twin == edge);
+			dgAssert (edge->m_next->m_incidentVertex == edge->m_twin->m_incidentVertex);
+			dgAssert (edge->m_incidentVertex == edge->m_twin->m_next->m_incidentVertex);
+
+			if (edge->m_incidentFace > 0) {
+				dgBigVector p0 (m_points[edge->m_incidentVertex]);
+				dgBigVector p1 (m_attrib[edge->m_userData].m_vertex);
+				dgBigVector p1p0 (p1 - p0);
+				dgFloat64 mag2 (p1p0 % p1p0);
+				dgAssert (mag2 < 1.0e-16f);
+			}
+		}
+	#endif
+	return true;
+}
+
+
+dgEdge* dgMeshEffect::InsertEdgeVertex (dgEdge* const edge, dgFloat64 param)
+{
+	dgEdge* const twin = edge->m_twin;
+	dgVertexAtribute attrEdge (InterpolateEdge (edge, param));
+	dgVertexAtribute attrTwin (InterpolateEdge (twin, dgFloat32 (1.0f) - param));
+
+	attrTwin.m_vertex = attrEdge.m_vertex;
+	AddPoint(&attrEdge.m_vertex.m_x, dgFastInt (attrEdge.m_material));
+	AddAtribute (attrTwin);
+
+	dgInt32 edgeAttrV0 = dgInt32 (edge->m_userData);
+	dgInt32 twinAttrV0 = dgInt32 (twin->m_userData);
+
+	dgEdge* const faceA0 = edge->m_next;
+	dgEdge* const faceA1 = edge->m_prev;
+	dgEdge* const faceB0 = twin->m_next;
+	dgEdge* const faceB1 = twin->m_prev;
+
+//	SpliteEdgeAndTriangulate (m_pointCount - 1, edge);
+	SpliteEdge (m_pointCount - 1, edge);
+
+	faceA0->m_prev->m_userData = dgUnsigned64 (m_atribCount - 2);
+	faceA1->m_next->m_userData = dgUnsigned64 (edgeAttrV0);
+
+	faceB0->m_prev->m_userData = dgUnsigned64 (m_atribCount - 1);
+	faceB1->m_next->m_userData = dgUnsigned64 (twinAttrV0);
+
+	return faceA1->m_next;
+}
+
+
+
+dgMeshEffect::dgVertexAtribute dgMeshEffect::InterpolateVertex (const dgBigVector& srcPoint, const dgEdge* const face) const
+{
+	const dgBigVector point (srcPoint);
+
+	dgVertexAtribute attribute;
+	memset (&attribute, 0, sizeof (attribute));
+
+//	dgBigVector normal (FaceNormal(face, &m_points[0].m_x, sizeof(dgBigVector)));
+//	normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (normal % normal));
+//	attribute.m_vertex = srcPoint;
+//	attribute.m_normal_x = normal.m_x;
+//	attribute.m_normal_y = normal.m_y;
+//	attribute.m_normal_z = normal.m_z;
+
+	dgFloat64 tol = dgFloat32 (1.0e-4f);
+	for (dgInt32 i = 0; i < 4; i ++) {
+		const dgEdge* ptr = face;
+		const dgEdge* const edge0 = ptr;
+		dgBigVector q0 (m_points[ptr->m_incidentVertex]);
+
+		ptr = ptr->m_next;
+		const dgEdge* edge1 = ptr;
+		dgBigVector q1 (m_points[ptr->m_incidentVertex]);
+
+		ptr = ptr->m_next;
+		const dgEdge* edge2 = ptr;
+		do {
+			const dgBigVector q2 (m_points[ptr->m_incidentVertex]);
+
+			dgBigVector p10 (q1 - q0);
+			dgBigVector p20 (q2 - q0);
+
+			dgFloat64 dot = p20 % p10;
+			dgFloat64 mag1 = p10 % p10;
+			dgFloat64 mag2 = p20 % p20;
+			dgFloat64 collinear = dot * dot - mag2 * mag1;
+			if (fabs (collinear) > dgFloat64 (1.0e-8f)) {
+				dgBigVector p_p0 (point - q0);
+				dgBigVector p_p1 (point - q1);
+				dgBigVector p_p2 (point - q2);
+
+				dgFloat64 alpha1 = p10 % p_p0;
+				dgFloat64 alpha2 = p20 % p_p0;
+				dgFloat64 alpha3 = p10 % p_p1;
+				dgFloat64 alpha4 = p20 % p_p1;
+				dgFloat64 alpha5 = p10 % p_p2;
+				dgFloat64 alpha6 = p20 % p_p2;
+
+				dgFloat64 vc = alpha1 * alpha4 - alpha3 * alpha2;
+				dgFloat64 vb = alpha5 * alpha2 - alpha1 * alpha6;
+				dgFloat64 va = alpha3 * alpha6 - alpha5 * alpha4;
+				dgFloat64 den = va + vb + vc;
+				dgFloat64 minError = den * (-tol);
+				dgFloat64 maxError = den * (dgFloat32 (1.0f) + tol);
+				if ((va > minError) && (vb > minError) && (vc > minError) && (va < maxError) && (vb < maxError) && (vc < maxError)) {
+					edge2 = ptr;
+
+					den = dgFloat64 (1.0f) / (va + vb + vc);
+
+					dgFloat64 alpha0 = dgFloat32 (va * den);
+					dgFloat64 alpha1 = dgFloat32 (vb * den);
+					dgFloat64 alpha2 = dgFloat32 (vc * den);
+
+					const dgVertexAtribute& attr0 = m_attrib[edge0->m_userData];
+					const dgVertexAtribute& attr1 = m_attrib[edge1->m_userData];
+					const dgVertexAtribute& attr2 = m_attrib[edge2->m_userData];
+					dgBigVector normal (attr0.m_normal_x * alpha0 + attr1.m_normal_x * alpha1 + attr2.m_normal_x * alpha2,
+										attr0.m_normal_y * alpha0 + attr1.m_normal_y * alpha1 + attr2.m_normal_y * alpha2,
+										attr0.m_normal_z * alpha0 + attr1.m_normal_z * alpha1 + attr2.m_normal_z * alpha2, dgFloat32 (0.0f));
+					normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (normal % normal));
+
+		#ifdef _DEBUG
+					dgBigVector testPoint (attr0.m_vertex.m_x * alpha0 + attr1.m_vertex.m_x * alpha1 + attr2.m_vertex.m_x * alpha2,
+										   attr0.m_vertex.m_y * alpha0 + attr1.m_vertex.m_y * alpha1 + attr2.m_vertex.m_y * alpha2,
+										   attr0.m_vertex.m_z * alpha0 + attr1.m_vertex.m_z * alpha1 + attr2.m_vertex.m_z * alpha2, dgFloat32 (0.0f));
+					dgAssert (fabs (testPoint.m_x - point.m_x) < dgFloat32 (1.0e-2f));
+					dgAssert (fabs (testPoint.m_y - point.m_y) < dgFloat32 (1.0e-2f));
+					dgAssert (fabs (testPoint.m_z - point.m_z) < dgFloat32 (1.0e-2f));
+		#endif
+
+
+					attribute.m_vertex.m_x = point.m_x;
+					attribute.m_vertex.m_y = point.m_y;
+					attribute.m_vertex.m_z = point.m_z;
+					attribute.m_vertex.m_w = point.m_w;
+					attribute.m_normal_x = normal.m_x;
+					attribute.m_normal_y = normal.m_y;
+					attribute.m_normal_z = normal.m_z;
+					attribute.m_u0 = attr0.m_u0 * alpha0 +  attr1.m_u0 * alpha1 + attr2.m_u0 * alpha2;
+					attribute.m_v0 = attr0.m_v0 * alpha0 +  attr1.m_v0 * alpha1 + attr2.m_v0 * alpha2;
+					attribute.m_u1 = attr0.m_u1 * alpha0 +  attr1.m_u1 * alpha1 + attr2.m_u1 * alpha2;
+					attribute.m_v1 = attr0.m_v1 * alpha0 +  attr1.m_v1 * alpha1 + attr2.m_v1 * alpha2;
+
+					attribute.m_material = attr0.m_material;
+					dgAssert (attr0.m_material == attr1.m_material);
+					dgAssert (attr0.m_material == attr2.m_material);
+					return attribute; 
+				}
+			}
+				
+			q1 = q2;
+			edge1 = ptr;
+
+			ptr = ptr->m_next;
+		} while (ptr != face);
+		tol *= dgFloat64 (2.0f);
+	}
+	// this should never happens
+	dgAssert (0);
+	return attribute;
+}
+
+bool dgMeshEffect::HasOpenEdges () const
+{
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++){
+		dgEdge* const face = &(*iter);
+		if (face->m_incidentFace < 0){
+			return true;
+		}
+	}
+	return false;
+}
+
+dgFloat64 dgMeshEffect::CalculateVolume () const
+{
+	dgAssert (0);
+	return 0;
+	/*
+
+	dgPolyhedraMassProperties localData;
+
+	dgInt32 mark = IncLRU();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++){
+		dgInt32 count;
+		dgEdge* ptr;
+		dgEdge* face;
+		dgVector points[256];
+		
+		face = &(*iter);
+		if ((face->m_incidentFace > 0) && (face->m_mark != mark)) {
+			count = 0;
+			ptr = face;
+			do {
+				points[count] = m_points[ptr->m_incidentVertex];
+				count ++;
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			localData.AddCGFace (count, points);
+		}
+	}
+
+	dgFloat32 volume;
+	dgVector p0;
+	dgVector p1;
+	dgVector com;
+	dgVector inertia;
+	dgVector crossInertia;
+	volume = localData.MassProperties (com, inertia, crossInertia);
+	return volume;
+*/
+}
+
+
+
+
+dgMeshEffect* dgMeshEffect::GetNextLayer (dgInt32 mark)
+{
+	Iterator iter(*this);
+	dgEdge* edge = NULL;
+	for (iter.Begin (); iter; iter ++) {
+		edge = &(*iter);
+		if ((edge->m_mark < mark) && (edge->m_incidentFace > 0)) {
+			break;
+		}
+	}
+
+	if (!edge) {
+		return NULL;
+	}
+
+	dgInt32 layer = dgInt32 (m_points[edge->m_incidentVertex].m_w);
+	dgPolyhedra polyhedra(GetAllocator());
+
+	polyhedra.BeginFace ();
+	for (iter.Begin (); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_mark < mark) && (edge->m_incidentFace > 0)) {
+			dgInt32 thislayer = dgInt32 (m_points[edge->m_incidentVertex].m_w);
+			if (thislayer == layer) {
+				dgEdge* ptr = edge;
+				dgInt32 count = 0;
+				dgInt32 faceIndex[256];
+				dgInt64 faceDataIndex[256];
+				do {
+					ptr->m_mark = mark;
+					faceIndex[count] = ptr->m_incidentVertex;
+					faceDataIndex[count] = ptr->m_userData;
+					count ++;
+					dgAssert (count < dgInt32 (sizeof (faceIndex)/ sizeof(faceIndex[0])));
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+				polyhedra.AddFace (count, &faceIndex[0], &faceDataIndex[0]);
+			}
+		}
+	}
+	polyhedra.EndFace ();
+
+	dgMeshEffect* solid = NULL;
+	if (polyhedra.GetCount()) {
+		solid = new (GetAllocator()) dgMeshEffect(polyhedra, *this);
+		solid->SetLRU(mark);
+	}
+	return solid;
+}
+
+
+
+void dgMeshEffect::MergeFaces (const dgMeshEffect* const source)
+{
+	dgInt32 mark = source->IncLRU();
+	dgPolyhedra::Iterator iter (*source);
+	for(iter.Begin(); iter; iter ++){
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_incidentFace > 0) && (edge->m_mark < mark)) {
+			dgVertexAtribute face[DG_MESH_EFFECT_POINT_SPLITED];
+
+			dgInt32 count = 0;
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				face[count] = source->m_attrib[ptr->m_userData];
+				count ++;
+				dgAssert (count < dgInt32 (sizeof (face) / sizeof (face[0])));
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+			AddPolygon(count, &face[0].m_vertex.m_x, sizeof (dgVertexAtribute), dgFastInt (face[0].m_material));
+		}
+	}
+}
+
+
+bool dgMeshEffect::SeparateDuplicateLoops (dgEdge* const face)
+{
+	for (dgEdge* ptr0 = face; ptr0 != face->m_prev; ptr0 = ptr0->m_next) {
+		dgInt32 index = ptr0->m_incidentVertex;
+
+		dgEdge* ptr1 = ptr0->m_next; 
+		do {
+			if (ptr1->m_incidentVertex == index) {
+				dgEdge* const ptr00 = ptr0->m_prev;
+				dgEdge* const ptr11 = ptr1->m_prev;
+
+				ptr00->m_next = ptr1;
+				ptr1->m_prev = ptr00;
+
+				ptr11->m_next = ptr0;
+				ptr0->m_prev = ptr11;
+
+				return true;
+			}
+
+			ptr1 = ptr1->m_next;
+		} while (ptr1 != face);
+	}
+
+	return false;
+}
+
+
+
+void dgMeshEffect::RepairTJoints ()
+{
+	dgAssert (Sanity ());
+
+	// delete edge of zero length
+	bool dirty = true;
+	while (dirty) {
+		dgFloat64 tol = 1.0e-5;
+		dgFloat64 tol2 = tol * tol;
+		dirty = false;
+		dgPolyhedra::Iterator iter (*this);
+		for (iter.Begin(); iter; ) {
+			dgEdge* const edge = &(*iter);
+			iter ++;
+			const dgBigVector& p0 = m_points[edge->m_incidentVertex];
+			const dgBigVector& p1 = m_points[edge->m_twin->m_incidentVertex];
+			dgBigVector dist (p1 - p0);
+			dgFloat64 mag2 = dist % dist;
+			if (mag2 < tol2) {
+				bool move = true;
+				while (move) {
+					move = false;
+					dgEdge* ptr = edge->m_twin;
+					do {
+						if ((&(*iter) == ptr) || (&(*iter) == ptr->m_twin)) {
+							move = true;
+							iter ++;
+						}
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge->m_twin);
+
+					ptr = edge;
+					do {
+						if ((&(*iter) == ptr) || (&(*iter) == ptr->m_twin)) {
+							move = true;
+							iter ++;
+						}
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+				}
+				
+				dgEdge* const collapsedEdge = CollapseEdge(edge);
+				if (collapsedEdge) {
+					dirty = true;
+					dgBigVector q (m_points[collapsedEdge->m_incidentVertex]);
+					dgEdge* ptr = collapsedEdge;
+					do {
+						if (ptr->m_incidentFace > 0) {
+							m_attrib[ptr->m_userData].m_vertex = q;
+						}
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != collapsedEdge);
+				}
+			}
+		}
+	}
+	dgAssert (Sanity ());
+
+	// repair straight open edges
+	dgInt32 mark = IncLRU();
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_mark) != mark && (edge->m_incidentFace < 0)) {
+
+			while (SeparateDuplicateLoops (edge));
+			dgEdge* ptr = edge; 
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+	}
+
+	dgAssert (Sanity ());
+	DeleteDegenerateFaces(&m_points[0].m_x, sizeof (m_points[0]), dgFloat64 (1.0e-7f));
+	dgAssert (Sanity ());
+
+	// delete straight line edges
+	dirty = true;
+	while (dirty) {
+		dgFloat64 tol = 1.0 - 1.0e-8;
+		dgFloat64 tol2 = tol * tol;
+
+		dirty = false;
+		dgAssert (Sanity ());
+
+		dgPolyhedra::Iterator iter (*this);
+		for (iter.Begin(); iter; ) {
+			dgEdge* const edge = &(*iter);
+			iter ++;
+
+			const dgBigVector& p0 = m_points[edge->m_incidentVertex];
+			const dgBigVector& p1 = m_points[edge->m_next->m_incidentVertex];
+			const dgBigVector& p2 = m_points[edge->m_next->m_next->m_incidentVertex];
+
+			dgBigVector A (p1 - p0);
+			dgBigVector B (p2 - p1);
+			dgFloat64 ab = A % B;
+			if (ab >= 0.0f) {
+				dgFloat64 aa = A % A;
+				dgFloat64 bb = B % B;
+
+				dgFloat64 magab2 = ab * ab;
+				dgFloat64 magaabb = aa * bb * tol2;
+				if (magab2 >= magaabb) {
+					if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) {
+						if (edge->m_twin->m_prev == edge->m_next->m_twin) {
+							dgEdge* const newEdge = AddHalfEdge(edge->m_incidentVertex, edge->m_next->m_next->m_incidentVertex);
+							if (newEdge) {
+								dirty = true;
+								dgEdge* const newTwin = AddHalfEdge(edge->m_next->m_next->m_incidentVertex, edge->m_incidentVertex);
+								dgAssert (newEdge);
+								dgAssert (newTwin);
+
+								newEdge->m_twin = newTwin;
+								newTwin->m_twin = newEdge;
+
+								newEdge->m_userData = edge->m_userData;
+								newTwin->m_userData = edge->m_twin->m_prev->m_userData;
+
+								newEdge->m_incidentFace = edge->m_incidentFace;
+								newTwin->m_incidentFace = edge->m_twin->m_incidentFace;
+
+								dgEdge* const nextEdge = edge->m_next;
+
+								nextEdge->m_twin->m_prev->m_next = newTwin;
+								newTwin->m_prev = nextEdge->m_twin->m_prev;
+
+								edge->m_twin->m_next->m_prev = newTwin;
+								newTwin->m_next = edge->m_twin->m_next;
+
+								nextEdge->m_next->m_prev = newEdge;
+								newEdge->m_next = nextEdge->m_next;
+
+								edge->m_prev->m_next = newEdge;
+								newEdge->m_prev = edge->m_prev;
+
+								while ((&(*iter) == edge->m_twin) || (&(*iter) == nextEdge) || (&(*iter) == nextEdge->m_twin)) {
+									iter ++;
+								}
+
+								nextEdge->m_twin->m_prev = nextEdge;
+								nextEdge->m_twin->m_next = nextEdge;
+								nextEdge->m_prev = nextEdge->m_twin;
+								nextEdge->m_next = nextEdge->m_twin;
+
+								edge->m_twin->m_prev = edge;
+								edge->m_twin->m_next = edge;
+								edge->m_prev = edge->m_twin;
+								edge->m_next = edge->m_twin;
+
+								DeleteEdge(edge);
+								DeleteEdge(nextEdge);
+								//dgAssert (Sanity ());
+
+							} else if (edge->m_next->m_next->m_next == edge) {
+								dirty = true;
+								dgEdge* const openEdge = edge;
+								dgEdge* const nextEdge = openEdge->m_next;
+								dgEdge* const deletedEdge = openEdge->m_prev;
+								while ((&(*iter) == deletedEdge) || (&(*iter) == deletedEdge->m_twin)) {
+									iter ++;
+								}
+
+								openEdge->m_userData = deletedEdge->m_twin->m_userData;
+
+								dgBigVector p2p0 (p2 - p0);
+								dgFloat64 den = p2p0 % p2p0;
+								dgFloat64 param1 = ((p1 - p0) % p2p0) / den;
+								dgVertexAtribute attib1 = InterpolateEdge (deletedEdge->m_twin, param1);
+								AddAtribute(attib1);
+								openEdge->m_next->m_userData = m_atribCount  - 1;
+
+								openEdge->m_incidentFace = deletedEdge->m_twin->m_incidentFace;
+								openEdge->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace;
+
+								deletedEdge->m_twin->m_prev->m_next = openEdge;
+								openEdge->m_prev = deletedEdge->m_twin->m_prev;
+
+								deletedEdge->m_twin->m_next->m_prev = nextEdge;
+								nextEdge->m_next = deletedEdge->m_twin->m_next;
+
+								deletedEdge->m_twin->m_next = deletedEdge;
+								deletedEdge->m_twin->m_prev = deletedEdge;
+								deletedEdge->m_next = deletedEdge->m_twin;
+								deletedEdge->m_prev = deletedEdge->m_twin;
+								DeleteEdge(deletedEdge);
+								//dgAssert (Sanity ());
+							}
+						}
+					} else if (FindEdge(edge->m_incidentVertex, edge->m_next->m_next->m_incidentVertex)) {
+						dgEdge* const openEdge = edge;
+						dgAssert (openEdge->m_incidentFace <= 0);
+						dgEdge* const nextEdge = openEdge->m_next;
+						dgEdge* const deletedEdge = openEdge->m_prev;
+						if (deletedEdge == openEdge->m_next->m_next) {
+							dirty = true;
+							while ((&(*iter) == deletedEdge) || (&(*iter) == deletedEdge->m_twin)) {
+								iter ++;
+							}
+
+							dgAssert (deletedEdge->m_twin->m_incidentFace > 0);
+							openEdge->m_incidentFace = deletedEdge->m_twin->m_incidentFace;
+							openEdge->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace;
+
+							openEdge->m_userData = deletedEdge->m_twin->m_userData;
+							dgBigVector p2p0 (p2 - p0);
+							dgFloat64 den = p2p0 % p2p0;
+							dgFloat64 param1 = ((p1 - p0) % p2p0) / den;
+							dgVertexAtribute attib1 = InterpolateEdge (deletedEdge->m_twin, param1);
+							attib1.m_vertex = m_points[openEdge->m_next->m_incidentVertex];
+							AddAtribute(attib1);
+							openEdge->m_next->m_userData = m_atribCount  - 1;
+							
+							deletedEdge->m_twin->m_prev->m_next = openEdge;
+							openEdge->m_prev = deletedEdge->m_twin->m_prev;
+
+							deletedEdge->m_twin->m_next->m_prev = nextEdge;
+							nextEdge->m_next = deletedEdge->m_twin->m_next;
+
+							deletedEdge->m_twin->m_next = deletedEdge;
+							deletedEdge->m_twin->m_prev = deletedEdge;
+							deletedEdge->m_next = deletedEdge->m_twin;
+							deletedEdge->m_prev = deletedEdge->m_twin;
+							DeleteEdge(deletedEdge);
+							//dgAssert (Sanity ());
+						}
+
+					} else {
+
+						dgEdge* const openEdge = (edge->m_incidentFace <= 0) ? edge : edge->m_twin;
+						dgAssert (openEdge->m_incidentFace <= 0);
+
+						const dgBigVector& p3 = m_points[openEdge->m_next->m_next->m_next->m_incidentVertex];
+
+						dgBigVector A (p3 - p2);
+						dgBigVector B (p2 - p1);
+						dgFloat64 ab (A % B);
+						if (ab >= 0.0) {
+							dgFloat64 aa (A % A);
+							dgFloat64 bb (B % B);
+
+							dgFloat64 magab2 = ab * ab;
+							dgFloat64 magaabb = aa * bb * tol2;
+							if (magab2 >= magaabb) {
+								if (openEdge->m_next->m_next->m_next->m_next != openEdge) {
+									const dgBigVector& p4 = m_points[openEdge->m_prev->m_incidentVertex];
+									dgBigVector A (p1 - p0);
+									dgBigVector B (p1 - p4);
+									dgFloat64 ab (A % B);
+									if (ab < 0.0f) {
+										dgFloat64 magab2 = ab * ab;
+										dgFloat64 magaabb = aa * bb * tol2;
+										if (magab2 >= magaabb) {
+											dgEdge* const newFace = ConnectVertex (openEdge->m_prev, openEdge->m_next);
+											dirty |= newFace ? true : false;
+										}
+									}
+									//dgAssert (Sanity ());
+								} else if (openEdge->m_prev->m_twin->m_incidentFace > 0) {
+									dirty = true;
+
+									dgEdge* const nextEdge = openEdge->m_next->m_next;
+									dgEdge* const deletedEdge = openEdge->m_prev;
+									while ((&(*iter) == deletedEdge) || (&(*iter) == deletedEdge->m_twin)) {
+										iter ++;
+									}
+
+									openEdge->m_incidentFace = deletedEdge->m_twin->m_incidentFace;
+									openEdge->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace;
+									openEdge->m_next->m_next->m_incidentFace = deletedEdge->m_twin->m_incidentFace;
+
+									openEdge->m_userData = deletedEdge->m_twin->m_userData;
+
+									dgBigVector p3p0 (p3 - p0);
+									dgFloat64 den = p3p0 % p3p0;
+									dgFloat64 param1 = ((p1 - p0) % p3p0) / den;
+									dgVertexAtribute attib1 = InterpolateEdge (deletedEdge->m_twin, param1);
+									attib1.m_vertex = m_points[openEdge->m_next->m_incidentVertex];
+									AddAtribute(attib1);
+									openEdge->m_next->m_userData = m_atribCount  - 1;
+
+									dgFloat64 param2 = ((p2 - p0) % p3p0) / den;
+									dgVertexAtribute attib2 = InterpolateEdge (deletedEdge->m_twin, param2);
+									attib2.m_vertex = m_points[openEdge->m_next->m_next->m_incidentVertex];
+									AddAtribute(attib2);
+									openEdge->m_next->m_next->m_userData = m_atribCount  - 1;
+
+									deletedEdge->m_twin->m_prev->m_next = openEdge;
+									openEdge->m_prev = deletedEdge->m_twin->m_prev;
+
+									deletedEdge->m_twin->m_next->m_prev = nextEdge;
+									nextEdge->m_next = deletedEdge->m_twin->m_next;
+									
+									deletedEdge->m_twin->m_next = deletedEdge;
+									deletedEdge->m_twin->m_prev = deletedEdge;
+									deletedEdge->m_next = deletedEdge->m_twin;
+									deletedEdge->m_prev = deletedEdge->m_twin;
+									DeleteEdge(deletedEdge);
+									//dgAssert (Sanity ());
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+	dgAssert (Sanity ());
+
+	DeleteDegenerateFaces(&m_points[0].m_x, sizeof (m_points[0]), dgFloat64 (1.0e-7f));
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &iter.GetNode()->GetInfo();
+		if (edge->m_incidentFace > 0) {
+			dgBigVector p0 (m_points[edge->m_incidentVertex]);
+			m_attrib[edge->m_userData].m_vertex.m_x = p0.m_x;
+			m_attrib[edge->m_userData].m_vertex.m_y = p0.m_y;
+			m_attrib[edge->m_userData].m_vertex.m_z = p0.m_z;
+		}
+	}
+	dgAssert (Sanity ());
+}
+
+
+
+
+

newtondynamics.mod/NewtonDynamics/source/meshUtil/dgMeshEffect2.cpp

@@ -0,0 +1,224 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgPhysicsStdafx.h"
+#include "dgBody.h"
+#include "dgWorld.h"
+#include "dgMeshEffect.h"
+#include "dgCollisionConvexHull.h"
+
+
+// create a convex hull
+dgMeshEffect::dgMeshEffect (dgMemoryAllocator* const allocator, const dgFloat64* const vertexCloud, dgInt32 count, dgInt32 strideInByte, dgFloat64 distTol)
+	:dgPolyhedra(allocator)
+{
+	Init();
+	if (count >= 4) {
+		dgConvexHull3d convexHull (allocator, vertexCloud, strideInByte, count, distTol);
+		if (convexHull.GetCount()) {
+
+			dgInt32 vertexCount = convexHull.GetVertexCount();
+			dgStack<dgVector> pointsPool (convexHull.GetVertexCount());
+			dgVector* const points = &pointsPool[0];
+			for (dgInt32 i = 0; i < vertexCount; i ++) {
+				points[i] = convexHull.GetVertex(i);
+			}
+			dgVector uv(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+			dgVector normal (dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+
+			dgInt32 triangleCount = convexHull.GetCount();
+			dgStack<dgInt32> faceCountPool (triangleCount);
+			dgStack<dgInt32> materialsPool (triangleCount);
+			dgStack<dgInt32> vertexIndexListPool (triangleCount * 3);
+			dgStack<dgInt32> normalIndexListPool (triangleCount * 3);
+
+
+			memset (&materialsPool[0], 0, triangleCount * sizeof (dgInt32));
+			memset (&normalIndexListPool[0], 0, 3 * triangleCount * sizeof (dgInt32));
+
+			dgInt32 index = 0;
+			dgInt32* const faceCount = &faceCountPool[0];
+			dgInt32* const vertexIndexList = &vertexIndexListPool[0];
+			for (dgConvexHull3d::dgListNode* faceNode = convexHull.GetFirst(); faceNode; faceNode = faceNode->GetNext()) {
+				dgConvexHull3DFace& face = faceNode->GetInfo();
+				faceCount[index] = 3;
+				vertexIndexList[index * 3 + 0] = face.m_index[0]; 
+				vertexIndexList[index * 3 + 1] = face.m_index[1]; 
+				vertexIndexList[index * 3 + 2] = face.m_index[2]; 
+				index ++;
+			}
+
+			BuildFromVertexListIndexList(triangleCount, faceCount, &materialsPool[0], 
+				&points[0].m_x, sizeof (dgVector), vertexIndexList,
+				&normal.m_x, sizeof (dgVector), &normalIndexListPool[0],
+				&uv.m_x, sizeof (dgVector), &normalIndexListPool[0],
+				&uv.m_x, sizeof (dgVector), &normalIndexListPool[0]);
+
+            RepairTJoints ();
+		}
+	}
+}
+
+
+dgMeshEffect* dgMeshEffect::CreateDelaunayTetrahedralization (dgMemoryAllocator* const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32* const pointCloud, dgInt32 materialId, const dgMatrix& textureProjectionMatrix)
+{
+	dgAssert (0);
+	return NULL;
+}
+
+
+
+
+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;
+}

newtondynamics.mod/NewtonDynamics/source/meshUtil/dgMeshEffect3.cpp

@@ -0,0 +1,1389 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+*
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+*
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+*
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+*
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgPhysicsStdafx.h"
+#include "dgBody.h"
+#include "dgWorld.h"
+#include "dgMeshEffect.h"
+#include "dgCollisionConvexHull.h"
+
+
+
+// based of the paper Hierarchical Approximate Convex Decomposition by Khaled Mamou 
+// with his permission to adapt his algorithm to be more efficient.
+// also making some addition to his heuristic for better convex clusters selections
+// for the details http://kmamou.blogspot.com/
+
+
+#define DG_BUILD_HIERACHICAL_HACD
+
+#define DG_CONCAVITY_SCALE				dgFloat64 (100.0f)
+#define DG_CONCAVITY_PERIMETER_HANDICAP	dgFloat64 (0.5f)
+
+
+
+class dgHACDEdge
+{
+	public:
+	dgHACDEdge ()
+		:m_mark(0)
+		,m_proxyListNode(NULL)
+		,m_backFaceHandicap(dgFloat64 (1.0))
+	{
+	}
+	~dgHACDEdge ()
+	{
+	}
+
+	dgInt32 m_mark;
+	void* m_proxyListNode;
+	dgFloat64 m_backFaceHandicap;
+};
+
+class dgHACDClusterFace
+{
+	public:
+	dgHACDClusterFace()
+		:m_edge(NULL)
+		,m_area(dgFloat64(0.0f))
+	{
+	}
+	~dgHACDClusterFace()
+	{
+	}
+
+	dgEdge* m_edge;
+	dgFloat64 m_area;
+	dgBigVector m_normal;
+};
+
+
+class dgHACDCluster: public dgList<dgHACDClusterFace>
+{
+	public:
+	dgHACDCluster ()
+		:dgList<dgHACDClusterFace>(NULL)
+		,m_color(0)
+		,m_hierachicalClusterIndex(0)
+		,m_area(dgFloat64 (0.0f))
+		,m_concavity(dgFloat64 (0.0f))
+	{
+	}
+
+	bool IsCoplanar(const dgBigPlane& plane, const dgMeshEffect& mesh, dgFloat64 tolerance) const
+	{
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+			const dgHACDClusterFace& info = node->GetInfo();
+			dgEdge* ptr = info.m_edge;
+			do {
+				const dgBigVector& p = points[ptr->m_incidentVertex];
+				dgFloat64 dist = fabs(plane.Evalue(p));
+				if (dist > tolerance) {
+					return false;
+				}
+				ptr = ptr->m_next;
+			} while (ptr != info.m_edge);
+		}
+		return true;
+	}
+
+
+	dgInt32 m_color;
+	dgInt32 m_hierachicalClusterIndex;
+	dgFloat64 m_area;
+	dgFloat64 m_concavity;
+};
+
+
+class dgHACDClusterGraph: public dgGraph<dgHACDCluster, dgHACDEdge> 
+//	,public dgAABBPolygonSoup 
+//	,public dgMeshEffect::dgMeshBVH
+{
+	public:
+
+	class dgHACDConveHull: public dgConvexHull3d
+	{
+		class dgConvexHullRayCastData
+		{
+			public:
+			dgFloat64 m_normalProjection;
+			dgConvexHull3DFace* m_face;
+		};
+
+		public: 
+		dgHACDConveHull (const dgHACDConveHull& hull)
+			:dgConvexHull3d(hull)
+			,m_mark(1)
+		{
+		}
+
+		dgHACDConveHull (dgMemoryAllocator* const allocator, const dgBigVector* const points, dgInt32 count)
+			:dgConvexHull3d(allocator, &points[0].m_x, sizeof (dgBigVector),count, dgFloat64 (0.0f))
+			,m_mark(1)
+		{
+		}
+
+
+		dgFloat64 CalculateTriangleConcavity(const dgBigVector& normal, dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const points)
+		{
+			dgUnsigned32 head = 1;
+			dgUnsigned32 tail = 0;
+			dgBigVector pool[1<<8][3];
+
+			pool[0][0] = points[i0];
+			pool[0][1] = points[i1];
+			pool[0][2] = points[i2];
+
+			const dgFloat64 rayLength = dgFloat64(4.0f) * GetDiagonal();
+			const dgBigVector step(normal.Scale3(rayLength));
+
+			dgFloat64 concavity = dgFloat32(0.0f);
+			dgFloat64 minArea = dgFloat32(0.125f);
+			dgFloat64 minArea2 = minArea * minArea * 0.5f;
+
+			dgInt32 maxCount = 4;
+			dgUnsigned32 mask = (sizeof (pool) / (3 * sizeof (pool[0][0]))) - 1;
+
+			dgConvexHull3DFace* firstGuess = NULL;
+			while ((tail != head) && (maxCount >= 0)) {
+				maxCount --;
+				dgBigVector p0(pool[tail][0]);
+				dgBigVector p1(pool[tail][1]);
+				dgBigVector p2(pool[tail][2]);
+				p0.m_w = dgFloat32 (0.0f);
+				p1.m_w = dgFloat32 (0.0f);
+				p2.m_w = dgFloat32 (0.0f);
+
+				tail = (tail + 1) & mask;
+
+				dgBigVector q1((p0 + p1 + p2).Scale3(dgFloat64(1.0f / 3.0f)));
+				dgBigVector q0(q1 + step);
+
+				dgFloat64 param = RayCast(q0, q1, &firstGuess);
+				if (param > dgFloat64(1.0f)) {
+					param = dgFloat64(1.0f);
+				}
+				dgBigVector dq(step.Scale3(dgFloat32(1.0f) - param));
+				dgFloat64 lenght2 = sqrt (dq % dq);
+				if (lenght2 > concavity) {
+					concavity = lenght2;
+				}
+
+				if (((head + 1) & mask) != tail) {
+					dgBigVector edge10(p1 - p0);
+					dgBigVector edge20(p2 - p0);
+					dgBigVector n(edge10 * edge20);
+					dgFloat64 area2 = n % n;
+					if (area2 > minArea2) {
+						dgBigVector p01((p0 + p1).Scale3(dgFloat64(0.5f)));
+						dgBigVector p12((p1 + p2).Scale3(dgFloat64(0.5f)));
+						dgBigVector p20((p2 + p0).Scale3(dgFloat64(0.5f)));
+
+						pool[head][0] = p0;
+						pool[head][1] = p01;
+						pool[head][2] = p20;
+						head = (head + 1) & mask;
+
+						if (((head + 1) & mask) != tail) {
+							pool[head][0] = p1;
+							pool[head][1] = p12;
+							pool[head][2] = p01;
+							head = (head + 1) & mask;
+
+							if (((head + 1) & mask) != tail)	{
+								pool[head][0] = p2;
+								pool[head][1] = p20;
+								pool[head][2] = p12;
+								head = (head + 1) & mask;
+							}
+						}
+					}
+				}
+			}
+			return concavity;
+		}
+
+
+
+		dgFloat64 FaceRayCast (const dgConvexHull3DFace* const face, const dgBigVector& origin, const dgBigVector& dist, dgFloat64& normalProjection) const
+		{
+			dgInt32 i0 = face->m_index[0];
+			dgInt32 i1 = face->m_index[1];
+			dgInt32 i2 = face->m_index[2];
+
+			const dgBigVector& p0 = m_points[i0];
+			dgBigVector normal ((m_points[i1] - p0) * (m_points[i2] - p0));
+
+			dgFloat64 N = (origin - p0) % normal;
+			dgFloat64 D = dist % normal;
+
+			if (fabs(D) < dgFloat64 (1.0e-16f)) { // 
+				normalProjection = dgFloat32 (0.0);
+				if (N > dgFloat64 (0.0f)) {
+					return dgFloat32 (-1.0e30);
+				} else {
+
+					return dgFloat32 (1.0e30);
+				}
+			}
+			normalProjection = D;
+			return - N / D;
+		}
+
+		dgConvexHull3DFace* ClosestFaceVertexToPoint (const dgBigVector& point)
+		{
+			// note, for this function to be effective point should be an already close point to the Hull.
+			// for example casting the point to the OBB or the AABB of the full is a good first guess. 
+			dgConvexHull3DFace* closestFace = &GetFirst()->GetInfo();	
+			dgInt8 pool[256 * (sizeof (dgConvexHull3DFace*) + sizeof (dgFloat64))];
+			dgUpHeap<dgConvexHull3DFace*,dgFloat64> heap (pool, sizeof (pool));
+
+			for (dgInt32 i = 0; i < 3; i ++) {
+				dgBigVector dist (m_points[closestFace->m_index[i]] - point);
+				heap.Push(closestFace, dist % dist);
+			}
+
+			m_mark ++;	
+			dgFloat64 minDist = heap.Value();
+			while (heap.GetCount()) {
+				dgConvexHull3DFace* const face = heap[0];	
+				if (heap.Value() < minDist) {
+					minDist = heap.Value();
+					closestFace = face;
+				}
+				heap.Pop();
+				//face->m_mark = m_mark;
+				face->SetMark(m_mark);
+				for (dgInt32 i = 0; i < 3; i ++) {
+					dgConvexHull3DFace* twin = &face->GetTwin(i)->GetInfo();	
+					if (twin->GetMark() != m_mark) {
+						dgBigVector dist (m_points[twin->m_index[i]] - point);
+						// use hysteresis to prevent stops at a local minimal, but at the same time fast descend
+						dgFloat64 dist2 = dist % dist;
+						if (dist2 < (minDist * dgFloat64 (1.001f))) {
+							heap.Push(twin, dist2);
+						}
+					}
+				}
+			}
+
+			return closestFace;
+		}
+
+
+		// this version have input sensitive complexity (approximately  log2)
+		// when casting parallel rays and using the last face as initial guess this version has const time complexity 
+		dgFloat64 RayCast (const dgBigVector& localP0, const dgBigVector& localP1, dgConvexHull3DFace** firstFaceGuess)
+		{
+			dgConvexHull3DFace* face = &GetFirst()->GetInfo();
+			if (firstFaceGuess && *firstFaceGuess) {
+				face = *firstFaceGuess;
+			} else {
+				if (GetCount() > 32) {
+					dgVector q0 (localP0);
+					dgVector q1 (localP1);
+					if (dgRayBoxClip (q0, q1, m_aabbP0, m_aabbP1)) {
+						face = ClosestFaceVertexToPoint (q0);
+					}
+				}
+			}
+
+			m_mark ++;	
+			face->SetMark (m_mark);
+			dgInt8 pool[256 * (sizeof (dgConvexHullRayCastData) + sizeof (dgFloat64))];
+			dgDownHeap<dgConvexHullRayCastData,dgFloat64> heap (pool, sizeof (pool));
+
+			dgFloat64 t0 = dgFloat64 (-1.0e20);			//for the maximum entering segment parameter;
+			dgFloat64 t1 = dgFloat64 ( 1.0e20);			//for the minimum leaving segment parameter;
+			dgBigVector dS (localP1 - localP0);		// is the segment direction vector;
+			dgConvexHullRayCastData data;
+			data.m_face = face;
+			dgFloat64 t = FaceRayCast (face, localP0, dS, data.m_normalProjection);
+			if (data.m_normalProjection >= dgFloat32 (0.0)) {
+				t = dgFloat64 (-1.0e30);
+			}
+
+			heap.Push (data, t);
+			while (heap.GetCount()) {
+				dgConvexHullRayCastData data (heap[0]);
+				dgFloat64 t = heap.Value();
+				dgConvexHull3DFace* face = data.m_face;
+				dgFloat64 normalDistProjection = data.m_normalProjection;
+				heap.Pop();
+				bool foundThisBestFace = true;
+				if (normalDistProjection < dgFloat64 (0.0f)) {
+					if (t > t0) {
+						t0 = t;
+					}
+					if (t0 > t1) {
+						return dgFloat64 (1.2f);
+					}
+				} else {
+					foundThisBestFace = false;
+				}
+
+				for (dgInt32 i = 0; i < 3; i ++) {
+					dgConvexHull3DFace* const face1 = &face->GetTwin(i)->GetInfo();
+
+					if (face1->GetMark() != m_mark) {
+						face1->SetMark (m_mark);
+						dgConvexHullRayCastData data;
+						data.m_face = face1;
+						dgFloat64 t = FaceRayCast (face1, localP0, dS, data.m_normalProjection);
+						if (data.m_normalProjection >= dgFloat32 (0.0)) {
+							t = dgFloat64 (-1.0e30);
+						} else if (t > t0) {
+							foundThisBestFace = false;
+						} else if (fabs (t - t0) < dgFloat64 (1.0e-10f)) {
+							return dgConvexHull3d::RayCast (localP0, localP1);
+						}
+						if ((heap.GetCount() + 2)>= heap.GetMaxCount()) {
+							// remove t values that are old and way outside interval [0.0, 1.0]  
+							for (dgInt32 i = heap.GetCount() - 1; i >= 0; i--) {
+								dgFloat64 val = heap.Value(i);
+								if ((val < dgFloat64 (-100.0f)) || (val > dgFloat64 (100.0f))) {
+									heap.Remove(i);
+								}
+							}
+						}
+						heap.Push (data, t);
+					}
+				}
+				if (foundThisBestFace) {
+					if ((t0 >= dgFloat64 (0.0f)) && (t0 <= dgFloat64 (1.0f))) {
+						if (firstFaceGuess) {
+							*firstFaceGuess = face;
+						}
+						return t0;
+					}
+					break;
+				}
+			}
+			return dgFloat64 (1.2f);
+		}
+
+
+		dgInt32 m_mark;
+	};
+
+	class dgHACDConvacityLookAheadTree
+	{
+		public:
+		DG_CLASS_ALLOCATOR(allocator)
+
+			dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgEdge* const face, dgFloat64 concavity)
+			:m_concavity(concavity)	
+			,m_faceList (allocator)
+			,m_left (NULL)
+			,m_right (NULL)
+		{
+			m_faceList.Append(face);
+		}
+
+
+		dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgHACDConvacityLookAheadTree* const leftChild, dgHACDConvacityLookAheadTree* const rightChild, dgFloat64 concavity)
+			:m_concavity(concavity)	
+			,m_faceList (allocator)
+			,m_left (leftChild)
+			,m_right (rightChild)
+		{
+			dgAssert (leftChild);
+			dgAssert (rightChild);
+
+			dgFloat64 concavityTest = m_concavity - dgFloat64 (1.0e-5f);
+			if ((((m_left->m_faceList.GetCount() == 1) || (m_right->m_faceList.GetCount() == 1))) ||
+				((concavityTest <= m_left->m_concavity) && (concavityTest <= m_right->m_concavity))) {
+					//The the parent has lower concavity this mean that the two do no add more detail, 
+					//the can be deleted and replaced the parent node
+					// for example the two children can be two convex strips that are part of a larger convex piece
+					// but each part has a non zero concavity, while the convex part has a lower concavity 
+					m_faceList.Merge (m_left->m_faceList);
+					m_faceList.Merge (m_right->m_faceList);
+
+					delete m_left;
+					delete m_right;
+					m_left = NULL;
+					m_right = NULL;
+			} else {
+				for (dgList<dgEdge*>::dgListNode* node = m_left->m_faceList.GetFirst(); node; node = node->GetNext()) {
+					m_faceList.Append(node->GetInfo());
+				}
+				for (dgList<dgEdge*>::dgListNode* node = m_right->m_faceList.GetFirst(); node; node = node->GetNext()) {
+					m_faceList.Append(node->GetInfo());
+				}
+			}
+		}
+
+		~dgHACDConvacityLookAheadTree ()
+		{
+			if (m_left) {
+				dgAssert (m_right);
+				delete m_left;
+				delete m_right;
+			}
+		}
+
+		dgInt32 GetNodesCount () const
+		{
+			dgInt32 count = 0;
+			dgInt32 stack = 1;
+			const dgHACDConvacityLookAheadTree* pool[1024];
+			pool[0] = this;
+			while (stack) {
+				stack --;
+				count ++;
+				const dgHACDConvacityLookAheadTree* const root = pool[stack];
+				if (root->m_left) {
+					dgAssert (root->m_right);
+					pool[stack] = root->m_left;
+					stack ++;
+					dgAssert (stack < sizeof (pool)/sizeof (pool[0]));
+					pool[stack] = root->m_right;
+					stack ++;
+					dgAssert (stack < sizeof (pool)/sizeof (pool[0]));
+				}
+			}
+			return count;
+		}
+
+		void ReduceByCount (dgInt32 count, dgDownHeap<dgHACDConvacityLookAheadTree*, dgFloat64>& approximation)
+		{
+			if (count < 1) {
+				count = 1;
+			}
+
+			approximation.Flush();
+			dgHACDConvacityLookAheadTree* tmp = this;
+			approximation.Push(tmp, m_concavity);
+			while ((approximation.GetCount() < count) && (approximation.Value() >= dgFloat32 (0.0f))) {
+				dgHACDConvacityLookAheadTree* worseCluster = approximation[0];
+				dgFloat64 concavity = approximation.Value();
+				if (!worseCluster->m_left && (concavity >= dgFloat32 (0.0f))) {
+					dgAssert (!worseCluster->m_right);
+					approximation.Pop();
+					approximation.Push(worseCluster, concavity - dgFloat64 (1.0e10f));
+				} else {
+					dgAssert (worseCluster->m_left);
+					dgAssert (worseCluster->m_right);
+					approximation.Pop();
+					approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity);
+					approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity);
+				}
+			}
+		}
+
+
+		void ReduceByConcavity (dgFloat64 concavity, dgDownHeap<dgHACDConvacityLookAheadTree*, dgFloat64>& approximation)
+		{
+			approximation.Flush();
+			dgHACDConvacityLookAheadTree* tmp = this;
+
+			approximation.Push(tmp, m_concavity);
+			while (approximation.Value() > concavity) {
+				dgHACDConvacityLookAheadTree* worseCluster = approximation[0];
+				if (!worseCluster->m_left && approximation.Value() >= dgFloat32 (0.0f)) {
+					approximation.Pop();
+					approximation.Push(worseCluster, dgFloat32 (-1.0f));
+				} else {
+					dgAssert (worseCluster->m_left);
+					dgAssert (worseCluster->m_right);
+					approximation.Pop();
+					approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity);
+					approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity);
+				}
+			}
+		}
+
+		dgFloat64 m_concavity; 
+		dgList<dgEdge*> m_faceList;
+		dgHACDConvacityLookAheadTree* m_left;
+		dgHACDConvacityLookAheadTree* m_right;
+	};
+
+	class dgPairProxy
+	{
+		public:
+		dgPairProxy()
+			:m_nodeA(NULL)
+			,m_nodeB(NULL)
+			,m_hierachicalClusterIndexA(0)
+			,m_hierachicalClusterIndexB(0)
+			,m_area(dgFloat64(0.0f))
+		{
+		}
+
+		~dgPairProxy()
+		{
+		}
+
+		dgListNode* m_nodeA;
+		dgListNode* m_nodeB;
+		dgInt32 m_hierachicalClusterIndexA;
+		dgInt32 m_hierachicalClusterIndexB;
+		dgFloat64 m_area;
+		dgFloat64 m_distanceConcavity;
+	};
+
+
+	class dgBackFaceFinder: public dgMeshEffect::dgMeshBVH
+	{
+		public:
+		dgBackFaceFinder(dgMeshEffect* const mesh, dgHACDClusterGraph* const graph)
+			:dgMeshEffect::dgMeshBVH(mesh)
+			,m_clusterA(NULL)
+			,m_graph(graph)
+		{
+			for (dgListNode* clusterNode = graph->GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) {
+				dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData;
+				dgHACDClusterFace& face = cluster.GetFirst()->GetInfo();
+				dgEdge* const edge = face.m_edge;
+				AddFaceNode(edge, &cluster);
+			}
+		}
+
+		dgFloat64 RayFaceIntersect (const dgMeshBVHNode* const face, const dgBigVector& p0, const dgBigVector& p1, bool doublesided) const
+		{
+			dgHACDCluster* const clusterFace = (dgHACDCluster*) face->m_userData;
+
+			dgFloat64 param = dgFloat32 (100.0f);
+			if (clusterFace->m_color != m_clusterA->m_color) {
+				param = dgMeshEffect::dgMeshBVH::RayFaceIntersect (face, p1, p0, false);
+				if ((param >= dgFloat32 (0.0f)) && (param <= dgFloat32(1.0f))) {
+					param = dgFloat32 (1.0f) - param;
+				}
+			}
+			return param;
+		}
+
+		void CastBackFace (dgListNode* const clusterNodeA, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, dgFloat32 distanceThreshold)
+		{
+			dgBigVector origin ((p0 + p1 + p2).Scale3 (dgFloat32 (1.0f/3.0f)));
+
+			dgFloat32 rayDistance = distanceThreshold * dgFloat32 (2.0f);
+
+
+			m_clusterA = &clusterNodeA->GetInfo().m_nodeData;
+			dgHACDClusterFace& faceA = m_clusterA->GetFirst()->GetInfo();
+			dgBigVector end (origin - faceA.m_normal.Scale3 (rayDistance));
+
+			dgFloat64 paramOut;
+			dgMeshBVHNode* const node = FaceRayCast (origin, end, paramOut, false);
+
+			if (node) {
+				dgHACDCluster* const clusterB = (dgHACDCluster*) node->m_userData;
+				dgAssert (clusterB->m_color != m_clusterA->m_color);
+				dgFloat64 distance = rayDistance * paramOut;
+
+				if (distance < distanceThreshold) {
+					dgHACDClusterFace& faceB = clusterB->GetFirst()->GetInfo();
+					dgEdge* const edgeB = faceB.m_edge;
+
+					
+					bool isAdjacent = false;
+					dgEdge* ptrA = faceA.m_edge;
+					do {
+						dgEdge* ptrB = edgeB;
+						do {
+							if (ptrB->m_twin == ptrA) {
+								ptrA = faceA.m_edge->m_prev;
+								isAdjacent = true;
+								break;
+							}
+							ptrB = ptrB->m_next;
+						} while (ptrB != edgeB);
+
+						ptrA = ptrA->m_next;
+					} while (ptrA != faceA.m_edge);
+
+					if (!isAdjacent) {
+						isAdjacent = false;
+						dgHACDClusterGraph::dgListNode* const clusterNodeB = m_graph->GetNodeFromNodeData (clusterB);
+						for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNode = clusterNodeA->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) {
+							if (edgeNode->GetInfo().m_node == clusterNodeB) {
+								isAdjacent = true;
+								break;
+							}
+						}
+
+						if (!isAdjacent) {
+							dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* const edgeNodeAB = clusterNodeA->GetInfo().AddEdge (clusterNodeB);
+							dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* const edgeNodeBA = clusterNodeB->GetInfo().AddEdge (clusterNodeA);
+
+							dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+							dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+							edgeAB.m_backFaceHandicap = DG_CONCAVITY_PERIMETER_HANDICAP;
+							edgeBA.m_backFaceHandicap = DG_CONCAVITY_PERIMETER_HANDICAP;
+						}
+					}
+				}
+			}
+		}
+
+		
+		dgHACDCluster* m_clusterA;
+		dgHACDClusterGraph* m_graph;
+	};
+
+    dgHACDClusterGraph(dgMeshEffect& mesh, dgFloat32 backFaceDistanceFactor, dgReportProgress reportProgressCallback, void* const reportProgressUserData)
+		:dgGraph<dgHACDCluster, dgHACDEdge> (mesh.GetAllocator())
+		,m_mark(0)
+		,m_faceCount(0)
+		,m_vertexMark(0)
+		,m_progress(0)
+		,m_concavityTreeIndex(0)
+		,m_invFaceCount(dgFloat32 (1.0f))
+		,m_diagonal(dgFloat64(1.0f))
+		,m_vertexMarks(NULL)
+		,m_vertexPool(NULL)
+		,m_proxyList(mesh.GetAllocator())
+		,m_concavityTreeArray(NULL)
+		,m_convexProximation(mesh.GetAllocator())
+		,m_priorityHeap (mesh.GetCount() * 2 + 2048, mesh.GetAllocator())
+        ,m_reportProgressCallback(reportProgressCallback)
+        ,m_reportProgressUserData(reportProgressUserData)
+	{
+		m_faceCount = mesh.GetTotalFaceCount();
+
+		dgMemoryAllocator* const allocator = mesh.GetAllocator();
+		m_invFaceCount = dgFloat32 (1.0f) / (m_faceCount);
+
+		// init some auxiliary structures
+		dgInt32 vertexCount = mesh.GetVertexCount();
+		m_vertexMarks = (dgInt32*) dgMallocStack(vertexCount * sizeof(dgInt32));
+		m_vertexPool = (dgBigVector*) dgMallocStack(vertexCount * sizeof(dgBigVector));
+		memset(m_vertexMarks, 0, vertexCount * sizeof(dgInt32));
+
+		m_concavityTreeIndex = m_faceCount + 1;
+		m_concavityTreeArray = (dgHACDConvacityLookAheadTree**) dgMallocStack(2 * m_concavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*));
+		memset(m_concavityTreeArray, 0, 2 * m_concavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*));
+
+		// scan the mesh and and add a node for each face
+		dgInt32 color = 1;
+		dgMeshEffect::Iterator iter(mesh);
+
+		dgInt32 meshMask = mesh.IncLRU();
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (iter.Begin(); iter; iter++) {
+			dgEdge* const edge = &(*iter);
+			if ((edge->m_mark != meshMask) && (edge->m_incidentFace > 0)) {
+
+				dgListNode* const clusterNode = AddNode ();
+				dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData;
+				cluster.SetAllocator(mesh.GetAllocator());
+
+				dgFloat64 perimeter = dgFloat64(0.0f);
+				dgEdge* ptr = edge;
+				do {
+					dgBigVector p1p0(points[ptr->m_incidentVertex] - points[ptr->m_prev->m_incidentVertex]);
+					perimeter += sqrt(p1p0 % p1p0);
+					ptr->m_incidentFace = color;
+					ptr->m_userData = dgUnsigned64 (clusterNode);
+					ptr->m_mark = meshMask;
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+
+				dgBigVector normal = mesh.FaceNormal(edge, &points[0][0], sizeof(dgBigVector));
+				dgFloat64 mag = sqrt(normal % normal);
+
+				cluster.m_color = color;
+				cluster.m_hierachicalClusterIndex = color;
+				cluster.m_area = dgFloat64(0.5f) * mag;
+				cluster.m_concavity = CalculateConcavityMetric (dgFloat64 (0.0f), cluster.m_area, perimeter, 1, 0);
+
+				dgHACDClusterFace& face = cluster.Append()->GetInfo();
+				face.m_edge = edge;
+				face.m_area = dgFloat64(0.5f) * mag;
+				face.m_normal = normal.Scale3(dgFloat64(1.0f) / mag);
+
+				m_concavityTreeArray[color] = new (allocator) dgHACDConvacityLookAheadTree (allocator, edge, dgFloat64 (0.0f));
+
+				color ++;
+			}
+		}
+
+		// add all link adjacent faces links
+		for (dgListNode* clusterNode = GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) {
+
+			dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData;
+			dgHACDClusterFace& face = cluster.GetFirst()->GetInfo();
+			dgEdge* const edge = face.m_edge;
+			dgEdge* ptr = edge; 
+			do {
+				if (ptr->m_twin->m_incidentFace > 0) {
+					dgAssert (ptr->m_twin->m_userData);
+					dgListNode* const twinClusterNode = (dgListNode*) ptr->m_twin->m_userData;
+					dgAssert (twinClusterNode);
+
+					bool doubleEdge = false;
+					for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNode = clusterNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) {
+						if (edgeNode->GetInfo().m_node == twinClusterNode) {
+							doubleEdge = true;
+							break;
+						}
+					}
+					if (!doubleEdge) {
+						clusterNode->GetInfo().AddEdge (twinClusterNode);
+					}
+				}
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+
+		Trace();
+
+		// add links to back faces
+		dgBigVector minAABB;
+		dgBigVector maxAABB;
+		mesh.CalculateAABB (minAABB, maxAABB);
+		maxAABB -= minAABB;
+		dgFloat32 rayDiagonalLength = dgFloat32 (sqrt (maxAABB % maxAABB));
+		m_diagonal = rayDiagonalLength;
+
+		dgBackFaceFinder backFaces(&mesh, this);
+		dgFloat32 distanceThreshold = rayDiagonalLength * backFaceDistanceFactor;
+		dgAssert (distanceThreshold >= dgFloat32 (0.0f));
+		for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) {
+
+			dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+			dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo();
+			dgEdge* const edgeA = faceA.m_edge;
+			dgEdge* ptr = edgeA;
+
+			dgBigVector p0 (points[ptr->m_incidentVertex]);
+			dgBigVector p1 (points[ptr->m_next->m_incidentVertex]);
+			ptr = ptr->m_next->m_next;
+			do {
+				dgBigVector p2 (points[ptr->m_incidentVertex]);
+				dgBigVector p01 ((p0 + p1).Scale3 (dgFloat32 (0.5f)));
+				dgBigVector p12 ((p1 + p2).Scale3 (dgFloat32 (0.5f)));
+				dgBigVector p20 ((p2 + p0).Scale3 (dgFloat32 (0.5f)));
+
+				backFaces.CastBackFace (clusterNodeA, p0, p01, p20, distanceThreshold);
+				backFaces.CastBackFace (clusterNodeA, p1, p12, p01, distanceThreshold);
+				backFaces.CastBackFace (clusterNodeA, p2, p20, p12, distanceThreshold);
+				backFaces.CastBackFace (clusterNodeA, p01, p12, p20, distanceThreshold);
+
+				p1 = p2;
+				ptr = ptr->m_next;
+			} while (ptr != edgeA);
+		}
+
+		Trace();
+	}
+
+	~dgHACDClusterGraph ()
+	{
+		for (dgInt32 i = 0; i < m_faceCount * 2; i ++) {
+			if (m_concavityTreeArray[i]) {
+				delete m_concavityTreeArray[i];
+			}
+		}
+
+		dgFreeStack(m_concavityTreeArray);
+		dgFreeStack(m_vertexPool);
+		dgFreeStack(m_vertexMarks);
+	}
+
+
+
+
+	void Trace() const
+	{
+#if 0
+		for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) {
+			dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+			//dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo();
+			//dgEdge* const edgeA = faceA.m_edge;
+
+			dgTrace (("cluster node: %d\n", clusterA.m_color));
+			dgTrace (("            links: "));
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeA = clusterNodeA->GetInfo().GetFirst(); edgeNodeA; edgeNodeA = edgeNodeA->GetNext()) {
+				dgListNode* const clusterNodeB = edgeNodeA->GetInfo().m_node;
+				dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData;
+				dgTrace (("%d ", clusterB.m_color));
+			}
+			dgTrace (("\n"));
+		}
+		dgTrace (("\n"));
+#endif
+	}
+
+
+	// you can insert callback here to print the progress as it collapse clusters
+	bool ReportProgress ()
+	{
+		bool state = true;
+		m_progress ++;
+		if (m_reportProgressCallback) {
+			dgFloat32 progress = dgFloat32(m_progress) * m_invFaceCount;
+			state = m_reportProgressCallback (progress * dgFloat32 (0.5f) + 0.5f, m_reportProgressUserData);
+		}
+		return state;
+	}
+/*
+	static bool ReportProgress (dgFloat32 progressNormalzedPercent)
+	{
+		bool state = true;
+		if (m_reportProgressCallback) {
+			state = m_reportProgressCallback(progressNormalzedPercent * 0.5f);
+		}
+		return state;
+	};
+*/
+
+
+	dgMeshEffect* CreatePartitionMesh (dgMeshEffect& mesh, dgInt32 maxVertexPerHull)
+	{
+		dgMemoryAllocator* const allocator = mesh.GetAllocator();
+		dgMeshEffect* const convexPartionMesh = new (allocator) dgMeshEffect(allocator);
+
+		dgMeshEffect::dgVertexAtribute polygon[256];
+		memset(polygon, 0, sizeof(polygon));
+		dgArray<dgBigVector> convexVertexBuffer(mesh.GetCount(), GetAllocator());
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+
+		convexPartionMesh->BeginPolygon();
+		dgFloat64 layer = dgFloat64 (0.0f);
+		for (dgList<dgHACDConvacityLookAheadTree*>::dgListNode* clusterNode = m_convexProximation.GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) {
+			dgHACDConvacityLookAheadTree* const cluster = clusterNode->GetInfo();
+
+			dgInt32 vertexCount = 0;
+			for (dgList<dgEdge*>::dgListNode* faceNode = cluster->m_faceList.GetFirst(); faceNode; faceNode = faceNode->GetNext()) {
+				dgEdge* const edge = faceNode->GetInfo();
+				dgEdge* ptr = edge;
+				do {
+					dgInt32 index = ptr->m_incidentVertex;
+					convexVertexBuffer[vertexCount] = points[index];
+					vertexCount++;
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+			}
+			dgConvexHull3d convexHull(allocator, &convexVertexBuffer[0].m_x, sizeof(dgBigVector), vertexCount, 0.0, maxVertexPerHull);
+			if (convexHull.GetCount()) {
+				const dgBigVector* const vertex = convexHull.GetVertexPool();
+				for (dgConvexHull3d::dgListNode* node = convexHull.GetFirst(); node; node = node->GetNext()) {
+					const dgConvexHull3DFace* const face = &node->GetInfo();
+
+					dgInt32 i0 = face->m_index[0];
+					dgInt32 i1 = face->m_index[1];
+					dgInt32 i2 = face->m_index[2];
+
+					polygon[0].m_vertex = vertex[i0];
+					polygon[0].m_vertex.m_w = layer;
+
+					polygon[1].m_vertex = vertex[i1];
+					polygon[1].m_vertex.m_w = layer;
+
+					polygon[2].m_vertex = vertex[i2];
+					polygon[2].m_vertex.m_w = layer;
+
+					convexPartionMesh->AddPolygon(3, &polygon[0].m_vertex.m_x, sizeof(dgMeshEffect::dgVertexAtribute), 0);
+				}
+				layer += dgFloat64 (1.0f);
+			}
+		}
+		convexPartionMesh->EndPolygon(1.0e-5f);
+
+		m_progress = m_faceCount - 1;
+		ReportProgress();
+
+		return convexPartionMesh;
+	}
+
+	dgFloat64 ConcavityByFaceMedian (dgInt32 faceCountA, dgInt32 faceCountB) const
+	{
+		dgFloat64 faceCountCost = DG_CONCAVITY_SCALE * dgFloat64 (0.1f) * (faceCountA + faceCountB) * m_invFaceCount;
+		return faceCountCost;
+	}
+
+	dgFloat64 CalculateConcavityMetric (dgFloat64 convexConcavity, dgFloat64 area, dgFloat64 perimeter, dgInt32 faceCountA, dgInt32 faceCountB) const 
+	{
+		dgFloat64 edgeCost = perimeter * perimeter / (dgFloat64(4.0f * 3.141592f) * area);
+		return convexConcavity * DG_CONCAVITY_SCALE + edgeCost + ConcavityByFaceMedian (faceCountA, faceCountB);
+	}
+
+	void SubmitInitialEdgeCosts (dgMeshEffect& mesh) 
+	{
+		m_mark ++;
+		for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) {
+			// call the progress callback
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) {
+				dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+				dgFloat64 weight = edgeAB.m_backFaceHandicap; 
+				if (edgeAB.m_mark != m_mark) {
+					edgeAB.m_mark = m_mark;
+					dgListNode* const clusterNodeB = edgeNodeAB->GetInfo().m_node;
+					for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) {
+						dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node;
+						if (clusterNode == clusterNodeA) {
+							dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+							edgeBA.m_mark = m_mark;
+							dgAssert (!edgeAB.m_proxyListNode);
+							dgAssert (!edgeBA.m_proxyListNode);
+
+							dgAssert (edgeBA.m_backFaceHandicap == weight);
+							dgList<dgPairProxy>::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB, weight * edgeBA.m_backFaceHandicap);
+							edgeAB.m_proxyListNode = proxyNode;
+							edgeBA.m_proxyListNode = proxyNode;
+							break;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	dgInt32 CopyVertexToPool(const dgMeshEffect& mesh, const dgHACDCluster& cluster, dgInt32 start)
+	{
+		dgInt32 count = start;
+
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			const dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			do {
+				dgInt32 index = edge->m_incidentVertex;
+				if (m_vertexMarks[index] != m_vertexMark) {
+					m_vertexMarks[index] = m_vertexMark;
+					m_vertexPool[count] = points[index];
+					count++;
+				}
+				edge = edge->m_next;
+			} while (edge != clusterFace.m_edge);
+		}
+		return count;
+	}
+
+
+	void MarkInteriorClusterEdges (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const
+	{
+		dgAssert (colorA != colorB);
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			do {
+				if ((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB)) {
+					edge->m_mark = mark;
+					edge->m_twin->m_mark = mark;
+				}
+				edge = edge->m_next;
+			} while (edge != clusterFace.m_edge);
+		}
+	}
+
+	dgFloat64 CalculateClusterPerimeter (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const
+	{
+		dgAssert (colorA != colorB);
+		dgFloat64 perimeter = dgFloat64 (0.0f);
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			do {
+				if (!((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB))) {
+					dgBigVector p1p0(points[edge->m_twin->m_incidentVertex] - points[edge->m_incidentVertex]);
+					perimeter += sqrt(p1p0 % p1p0);
+				}
+				edge = edge->m_next;
+			} while (edge != clusterFace.m_edge);
+		}
+
+		return perimeter;
+	}
+
+	void HeapCollectGarbage () 
+	{
+		if ((m_priorityHeap.GetCount() + 20) > m_priorityHeap.GetMaxCount()) {
+			for (dgInt32 i = m_priorityHeap.GetCount() - 1; i >= 0; i--) {
+				dgList<dgPairProxy>::dgListNode* const emptyNode = m_priorityHeap[i];
+				dgPairProxy& emptyPair = emptyNode->GetInfo();
+				if ((emptyPair.m_nodeA == NULL) && (emptyPair.m_nodeB == NULL)) {
+					m_priorityHeap.Remove(i);
+				}
+			}
+		}
+	}
+
+
+	dgFloat64 CalculateConcavity(dgHACDConveHull& hull, const dgMeshEffect& mesh, const dgHACDCluster& cluster)
+	{
+		dgFloat64 concavity = dgFloat32(0.0f);
+
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			dgInt32 i0 = edge->m_incidentVertex;
+			dgInt32 i1 = edge->m_next->m_incidentVertex;
+			for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) {
+				dgInt32 i2 = ptr->m_incidentVertex;
+				dgFloat64 val = hull.CalculateTriangleConcavity(clusterFace.m_normal, i0, i1, i2, points);
+				if (val > concavity) {
+					concavity = val;
+				}
+				i1 = i2;
+			}
+		}
+
+		return concavity;
+	}
+
+	dgFloat64 CalculateConcavity (dgHACDConveHull& hull, dgMeshEffect& mesh, dgHACDCluster& clusterA, dgHACDCluster& clusterB)
+	{
+		return dgMax(CalculateConcavity(hull, mesh, clusterA), CalculateConcavity(hull, mesh, clusterB));
+	}
+
+
+	dgList<dgPairProxy>::dgListNode* SubmitEdgeCost (dgMeshEffect& mesh, dgListNode* const clusterNodeA, dgListNode* const clusterNodeB, dgFloat64 perimeterHandicap)
+	{
+		dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+		dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData;
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+
+		bool flatStrip = true;
+		dgFloat64 tol = dgFloat64 (1.0e-5f) * m_diagonal;
+		dgHACDClusterFace& clusterFaceA = clusterA.GetFirst()->GetInfo();
+		dgBigPlane plane(clusterFaceA.m_normal, -(points[clusterFaceA.m_edge->m_incidentVertex] % clusterFaceA.m_normal));
+
+		if (clusterA.GetCount() > 1) {
+			flatStrip = clusterA.IsCoplanar(plane, mesh, tol);
+		}
+
+		if (flatStrip) {
+			flatStrip = clusterB.IsCoplanar(plane, mesh, tol);
+		}
+
+		dgList<dgPairProxy>::dgListNode* pairNode = NULL;
+		if (!flatStrip) {
+			m_vertexMark ++;
+			dgInt32 vertexCount = CopyVertexToPool(mesh, clusterA, 0);
+			vertexCount = CopyVertexToPool(mesh, clusterB, vertexCount);
+
+			dgHACDConveHull convexHull(mesh.GetAllocator(), m_vertexPool, vertexCount);
+
+			if (convexHull.GetVertexCount()) {
+				dgInt32 mark = mesh.IncLRU();
+				MarkInteriorClusterEdges (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color);
+				MarkInteriorClusterEdges (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color);
+
+				dgFloat64 area = clusterA.m_area + clusterB.m_area;
+				dgFloat64 perimeter = CalculateClusterPerimeter (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color) +
+									  CalculateClusterPerimeter (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color);
+				dgFloat64 concavity = CalculateConcavity (convexHull, mesh, clusterA, clusterB);
+
+				if (concavity < dgFloat64(1.0e-3f)) {
+					concavity = dgFloat64(0.0f);
+				}
+
+				// see if the heap will overflow
+				HeapCollectGarbage ();
+
+				// add a new pair to the heap
+				dgList<dgPairProxy>::dgListNode* pairNode = m_proxyList.Append();
+				dgPairProxy& pair = pairNode->GetInfo();
+				pair.m_nodeA = clusterNodeA;
+				pair.m_nodeB = clusterNodeB;
+				pair.m_distanceConcavity = concavity;
+				pair.m_hierachicalClusterIndexA = clusterA.m_hierachicalClusterIndex;
+				pair.m_hierachicalClusterIndexB = clusterB.m_hierachicalClusterIndex;
+
+				pair.m_area = area;
+				dgFloat64 cost = CalculateConcavityMetric (concavity, area * perimeterHandicap, perimeter * perimeterHandicap, clusterA.GetCount(), clusterB.GetCount());
+				m_priorityHeap.Push(pairNode, cost);
+
+				return pairNode;
+			}
+		}
+		return pairNode;
+	}
+
+
+	bool CollapseEdge (dgList<dgPairProxy>::dgListNode* const pairNode, dgMeshEffect& mesh, dgFloat64 concavity)
+	{
+		dgListNode* adjacentNodes[1024];
+		dgPairProxy& pair = pairNode->GetInfo();
+
+		dgMemoryAllocator* const allocator = mesh.GetAllocator();
+
+		bool continueColapsing = true;
+		dgAssert((pair.m_nodeA && pair.m_nodeB) || (!pair.m_nodeA && !pair.m_nodeB));
+		if (pair.m_nodeA && pair.m_nodeB && continueColapsing) {
+			// call the progress callback
+			continueColapsing = ReportProgress();
+
+			dgListNode* const clusterNodeA = pair.m_nodeA;
+			dgListNode* const clusterNodeB = pair.m_nodeB;
+			dgAssert (clusterNodeA != clusterNodeB);
+
+			dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+			dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData;
+
+			dgAssert (&clusterA != &clusterB);
+			dgAssert(clusterA.m_color != clusterB.m_color);
+
+			dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexA];
+			dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexB];
+			dgAssert (leftTree);
+			dgAssert (rightTree);
+			m_concavityTreeArray[pair.m_hierachicalClusterIndexA] = NULL;
+			m_concavityTreeArray[pair.m_hierachicalClusterIndexB] = NULL;
+			dgAssert (m_concavityTreeIndex < (2 * (m_faceCount + 1)));
+
+			dgFloat64 treeConcavity = pair.m_distanceConcavity;
+//			 dgAssert (treeConcavity < 0.1);
+			m_concavityTreeArray[m_concavityTreeIndex] = new (allocator) dgHACDConvacityLookAheadTree (allocator, leftTree, rightTree, treeConcavity);
+			clusterA.m_hierachicalClusterIndex = m_concavityTreeIndex;
+			clusterB.m_hierachicalClusterIndex = m_concavityTreeIndex;
+			m_concavityTreeIndex ++;
+
+			// merge two clusters
+			while (clusterB.GetCount()) {
+
+				dgHACDCluster::dgListNode* const nodeB = clusterB.GetFirst();
+				clusterB.Unlink(nodeB);
+	
+				// now color code all faces of the merged cluster
+				dgHACDClusterFace& faceB = nodeB->GetInfo();
+				dgEdge* ptr = faceB.m_edge;
+				do {
+					ptr->m_incidentFace = clusterA.m_color;
+					ptr = ptr->m_next;
+				} while (ptr != faceB.m_edge);
+				clusterA.Append(nodeB);
+			}
+			clusterA.m_area = pair.m_area;
+			clusterA.m_concavity = concavity;
+
+			// invalidate all proxies that are still in the heap
+			dgInt32 adjacentCount = 1;
+			adjacentNodes[0] = clusterNodeA;
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) {
+				dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+				dgList<dgPairProxy>::dgListNode* const proxyNode = (dgList<dgPairProxy>::dgListNode*) edgeAB.m_proxyListNode;
+				if (proxyNode) {
+					dgPairProxy& pairProxy = proxyNode->GetInfo();
+					dgAssert ((edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeA) || (edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeB));
+					pairProxy.m_nodeA = NULL;
+					pairProxy.m_nodeB = NULL;
+					edgeAB.m_proxyListNode = NULL;
+				}
+
+				adjacentNodes[adjacentCount] = edgeNodeAB->GetInfo().m_node;
+				adjacentCount ++;
+				dgAssert (adjacentCount < sizeof (adjacentNodes)/ sizeof (adjacentNodes[0]));
+			}
+
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) {
+				dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+				dgList<dgPairProxy>::dgListNode* const proxyNode = (dgList<dgPairProxy>::dgListNode*) edgeBA.m_proxyListNode;
+				if (proxyNode) {
+					dgPairProxy& pairProxy = proxyNode->GetInfo();
+					pairProxy.m_nodeA = NULL;
+					pairProxy.m_nodeB = NULL;
+					edgeBA.m_proxyListNode = NULL;
+				}
+
+				bool alreadyLinked = false;
+				dgListNode* const node = edgeNodeBA->GetInfo().m_node;
+				for (dgInt32 i = 0; i < adjacentCount; i ++) {
+					if (node == adjacentNodes[i]) {
+						alreadyLinked = true;
+						break;
+					}
+				}
+				if (!alreadyLinked) {
+					clusterNodeA->GetInfo().AddEdge (node);
+					node->GetInfo().AddEdge (clusterNodeA);
+				}
+			}
+			DeleteNode (clusterNodeB);
+
+			// submit all new costs for each edge connecting this new node to any other node 
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) {
+				dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+				dgListNode* const clusterNodeB = edgeNodeAB->GetInfo().m_node;
+				dgFloat64 weigh = edgeAB.m_backFaceHandicap;
+				for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) {
+					dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node;
+					if (clusterNode == clusterNodeA) {
+						dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+						dgList<dgPairProxy>::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB, weigh * edgeBA.m_backFaceHandicap);
+						if (proxyNode) {
+							edgeBA.m_proxyListNode = proxyNode;
+							edgeAB.m_proxyListNode = proxyNode;
+						}
+						break;
+					}
+				}
+			}
+		}
+		m_proxyList.Remove(pairNode);
+
+		return continueColapsing;
+	}
+
+#ifdef DG_BUILD_HIERACHICAL_HACD
+	bool CollapseClusters (dgMeshEffect& mesh, dgFloat64 maxConcavity___, dgInt32 maxClustesCount)
+	{
+		bool collapseEdgeState = true;
+		while (m_priorityHeap.GetCount() && collapseEdgeState) {
+			dgFloat64 concavity =  m_priorityHeap.Value();
+			dgList<dgPairProxy>::dgListNode* const pairNode = m_priorityHeap[0];
+			m_priorityHeap.Pop();
+			collapseEdgeState = CollapseEdge (pairNode, mesh, concavity);
+		}
+
+		if (collapseEdgeState) {
+			dgInt32 treeCounts = 0;
+			for (dgInt32 i = 0; i < m_concavityTreeIndex; i ++) {
+				if (m_concavityTreeArray[i]) {
+					m_concavityTreeArray[treeCounts] = m_concavityTreeArray[i];
+					m_concavityTreeArray[i] = NULL;
+					treeCounts ++;
+				}
+			}
+
+			if (treeCounts > 1) {
+				for (dgInt32 i = 0; i < treeCounts; i ++) {
+					dgAssert (m_concavityTreeArray[i]);
+					if (m_concavityTreeArray[i]->m_faceList.GetCount()==1) {
+						delete m_concavityTreeArray[i];
+						m_concavityTreeArray[i] = m_concavityTreeArray[treeCounts-1];
+						m_concavityTreeArray[treeCounts-1]= NULL;
+						treeCounts --;
+						i--;
+					}
+				}
+
+
+				dgFloat32 largeConcacvity = 10000;
+				while (treeCounts > 1)	 {
+					dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[treeCounts-1];
+					dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[treeCounts-2];
+					m_concavityTreeArray[treeCounts-1] = NULL;
+					m_concavityTreeArray[treeCounts-2] = new (mesh.GetAllocator()) dgHACDConvacityLookAheadTree (mesh.GetAllocator(), leftTree, rightTree, largeConcacvity);
+					largeConcacvity *= 2;
+					treeCounts --;
+				}
+
+			}
+
+			dgHACDConvacityLookAheadTree* const tree = m_concavityTreeArray[0];
+			if (tree) {
+				dgDownHeap<dgHACDConvacityLookAheadTree*, dgFloat64> approximation(maxClustesCount * 2, mesh.GetAllocator());
+
+				tree->ReduceByCount (maxClustesCount, approximation);
+				//tree->ReduceByConcavity (maxConcavity, approximation);
+
+//while ((approximation.Value() + dgFloat64 (1.0e10f)) > 1.0e-5) {
+//approximation.Pop();
+//}
+
+				while (approximation.GetCount()) {
+					m_convexProximation.Append(approximation[0]);
+					approximation.Pop();
+				}
+			}
+		}
+		return collapseEdgeState;
+	}
+
+#else 
+	void CollapseClusters (dgMeshEffect& mesh, dgFloat64 maxConcavity, dgInt32 maxClustesCount)
+	{
+		maxConcavity *= (m_diagonal * DG_CONCAVITY_SCALE);
+
+		bool terminate = false;
+		while (m_priorityHeap.GetCount() && !terminate) {
+			dgFloat64 concavity =  m_priorityHeap.Value();
+			dgList<dgPairProxy>::dgListNode* const pairNode = m_priorityHeap[0];
+			if ((concavity < maxConcavity) && (GetCount() < maxClustesCount)) {
+				terminate  = true;
+			} else {
+				m_priorityHeap.Pop();
+				CollapseEdge (pairNode, mesh, concavity);
+			}
+		}
+	}
+#endif
+
+	dgInt32 m_mark;
+	dgInt32 m_faceCount;
+	dgInt32 m_vertexMark;
+	dgInt32 m_progress;
+	dgInt32 m_concavityTreeIndex;
+	dgFloat32 m_invFaceCount;
+	dgFloat64 m_diagonal;
+	dgInt32* m_vertexMarks;
+	dgBigVector* m_vertexPool;
+	dgList<dgPairProxy> m_proxyList;
+	dgHACDConvacityLookAheadTree** m_concavityTreeArray;	
+	dgList<dgHACDConvacityLookAheadTree*> m_convexProximation;
+	dgUpHeap<dgList<dgPairProxy>::dgListNode*, dgFloat64> m_priorityHeap;
+	dgReportProgress m_reportProgressCallback;
+    void* m_reportProgressUserData;
+};
+
+dgMeshEffect* dgMeshEffect::CreateConvexApproximation(dgFloat32 maxConcavity, dgFloat32 backFaceDistanceFactor, dgInt32 maxHullsCount, dgInt32 maxVertexPerHull, dgReportProgress reportProgressCallback, void* const progressReportUserData) const
+{
+	//	dgMeshEffect triangleMesh(*this);
+	if (maxHullsCount <= 1) {
+		maxHullsCount = 1;
+	}
+	if (maxConcavity <= dgFloat32 (1.0e-5f)) {
+		maxConcavity = dgFloat32 (1.0e-5f);
+	}
+
+	if (maxVertexPerHull < 4) {
+		maxVertexPerHull = 4;
+	}
+	backFaceDistanceFactor = dgClamp(backFaceDistanceFactor, dgFloat32 (1.0e-6f), dgFloat32 (1.0f));
+
+	dgMeshEffect* partition = NULL;
+
+	// make a copy of the mesh
+	dgMeshEffect mesh(*this);
+	mesh.ClearAttributeArray();
+	mesh.Triangulate ();
+	if (mesh.Optimize (&mesh.m_points->m_x, sizeof (dgBigVector), reportProgressCallback, progressReportUserData, dgFloat32 (1.0e-3f), 1500)) {
+		mesh.ClearAttributeArray();
+		mesh.DeleteDegenerateFaces (&mesh.m_points->m_x, sizeof (dgBigVector), dgFloat32 (1.0e-12f));
+		mesh.RepairTJoints();
+		mesh.ConvertToPolygons();
+		//mesh.SaveOFF ("xxxxxx.off");
+
+		// create a general connectivity graph    
+		dgHACDClusterGraph graph (mesh, backFaceDistanceFactor, reportProgressCallback, progressReportUserData);
+
+		// calculate initial edge costs
+		graph.SubmitInitialEdgeCosts (mesh);
+
+		// collapse the graph
+		if (graph.CollapseClusters (mesh, maxConcavity, maxHullsCount)) {
+			// Create Partition Mesh
+			partition = graph.CreatePartitionMesh (mesh, maxVertexPerHull);
+		}
+	}
+
+	return partition;
+
+}

newtondynamics.mod/NewtonDynamics/source/meshUtil/dgMeshEffect4.cpp

@@ -0,0 +1,1625 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+*
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+*
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+*
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+*
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgPhysicsStdafx.h"
+#include "dgBody.h"
+#include "dgWorld.h"
+#include "dgMeshEffect.h"
+#include "dgCollisionConvexHull.h"
+
+
+
+#if 0
+#define DG_BUILD_HIERACHICAL_HACD
+
+#define DG_CONCAVITY_MAX_THREADS	  8
+#define DG_CONCAVITY_SCALE dgFloat64 (100.0f)
+
+
+
+
+class dgHACDEdge
+{
+	public:
+	dgHACDEdge ()
+		:m_mark(0)
+		,m_proxyListNode(NULL)
+		,m_backFaceHandicap(dgFloat64 (1.0))
+	{
+	}
+	~dgHACDEdge ()
+	{
+	}
+
+	dgInt32 m_mark;
+	void* m_proxyListNode;
+	dgFloat64 m_backFaceHandicap;
+};
+
+class dgHACDClusterFace
+{
+	public:
+	dgHACDClusterFace()
+		:m_edge(NULL)
+		,m_area(dgFloat64(0.0f))
+	{
+	}
+	~dgHACDClusterFace()
+	{
+	}
+
+	dgEdge* m_edge;
+	dgFloat64 m_area;
+	dgBigVector m_normal;
+};
+
+
+class dgHACDCluster: public dgList<dgHACDClusterFace>
+{
+	public:
+	dgHACDCluster ()
+		:dgList<dgHACDClusterFace>(NULL)
+		,m_color(0)
+		,m_hierachicalClusterIndex(0)
+		,m_area(dgFloat64 (0.0f))
+		,m_concavity(dgFloat64 (0.0f))
+	{
+	}
+
+	bool IsCoplanar(const dgBigPlane& plane, const dgMeshEffect& mesh, dgFloat64 tolerance) const
+	{
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgListNode* node = GetFirst(); node; node = node->GetNext()) {
+			const dgHACDClusterFace& info = node->GetInfo();
+			dgEdge* ptr = info.m_edge;
+			do {
+				const dgBigVector& p = points[ptr->m_incidentVertex];
+				dgFloat64 dist = fabs(plane.Evalue(p));
+				if (dist > tolerance) {
+					return false;
+				}
+				ptr = ptr->m_next;
+			} while (ptr != info.m_edge);
+		}
+		return true;
+	}
+
+
+	dgInt32 m_color;
+	dgInt32 m_hierachicalClusterIndex;
+	dgFloat64 m_area;
+	dgFloat64 m_concavity;
+};
+
+
+class dgHACDClusterGraph
+	:public dgGraph<dgHACDCluster, dgHACDEdge> 
+	,public dgAABBPolygonSoup 
+{
+	public:
+
+	class dgHACDConveHull: public dgConvexHull3d
+	{
+		class dgConvexHullRayCastData
+		{
+			public:
+			dgFloat64 m_normalProjection;
+			dgConvexHull3DFace* m_face;
+		};
+
+		public: 
+		dgHACDConveHull (const dgHACDConveHull& hull)
+			:dgConvexHull3d(hull)
+			,m_mark(1)
+		{
+		}
+
+		dgHACDConveHull (dgMemoryAllocator* const allocator, const dgBigVector* const points, dgInt32 count)
+			:dgConvexHull3d(allocator, &points[0].m_x, sizeof (dgBigVector),count, dgFloat64 (0.0f))
+			,m_mark(1)
+		{
+
+		}
+
+		dgFloat64 CalculateTriangleConcavity(const dgBigVector& normal, dgInt32 i0, dgInt32 i1, dgInt32 i2, const dgBigVector* const points)
+		{
+			dgUnsigned32 head = 1;
+			dgUnsigned32 tail = 0;
+			dgBigVector pool[1<<8][3];
+
+			pool[0][0] = points[i0];
+			pool[0][1] = points[i1];
+			pool[0][2] = points[i2];
+
+			const dgBigVector step(normal.Scale(dgFloat64(4.0f) * GetDiagonal()));
+
+			dgFloat64 concavity = dgFloat32(0.0f);
+			dgFloat64 minArea = dgFloat32(0.125f);
+			dgFloat64 minArea2 = minArea * minArea * 0.5f;
+
+			dgInt32 maxCount = 4;
+			dgUnsigned32 mask = (sizeof (pool) / (3 * sizeof (pool[0][0]))) - 1;
+
+			dgConvexHull3DFace* firstGuess = NULL;
+			while ((tail != head) && (maxCount >= 0)) {
+				maxCount --;
+				dgBigVector p0(pool[tail][0]);
+				dgBigVector p1(pool[tail][1]);
+				dgBigVector p2(pool[tail][2]);
+				tail = (tail + 1) & mask;
+
+				dgBigVector q1((p0 + p1 + p2).Scale(dgFloat64(1.0f / 3.0f)));
+				dgBigVector q0(q1 + step);
+
+				//dgFloat64 param = convexHull.RayCast(q0, q1, &firstGuess);
+				dgFloat64 param = FastRayCast(q0, q1, &firstGuess);
+				if (param > dgFloat64(1.0f)) {
+					param = dgFloat64(1.0f);
+				}
+				dgBigVector dq(step.Scale(dgFloat32(1.0f) - param));
+				dgFloat64 lenght2 = sqrt (dq % dq);
+				//dgAssert (lenght2 < GetDiagonal());
+				if (lenght2 > concavity) {
+					concavity = lenght2;
+				}
+
+				if (((head + 1) & mask) != tail) {
+					dgBigVector edge10(p1 - p0);
+					dgBigVector edge20(p2 - p0);
+					dgBigVector n(edge10 * edge20);
+					dgFloat64 area2 = n % n;
+					if (area2 > minArea2) {
+						dgBigVector p01((p0 + p1).Scale(dgFloat64(0.5f)));
+						dgBigVector p12((p1 + p2).Scale(dgFloat64(0.5f)));
+						dgBigVector p20((p2 + p0).Scale(dgFloat64(0.5f)));
+
+						pool[head][0] = p0;
+						pool[head][1] = p01;
+						pool[head][2] = p20;
+						head = (head + 1) & mask;
+
+						if (((head + 1) & mask) != tail) {
+							pool[head][0] = p1;
+							pool[head][1] = p12;
+							pool[head][2] = p01;
+							head = (head + 1) & mask;
+
+							if (((head + 1) & mask) != tail)	{
+								pool[head][0] = p2;
+								pool[head][1] = p20;
+								pool[head][2] = p12;
+								head = (head + 1) & mask;
+							}
+						}
+					}
+				}
+			}
+			return concavity;
+		}
+
+
+
+		dgFloat64 FaceRayCast (const dgConvexHull3DFace* const face, const dgBigVector& origin, const dgBigVector& dist, dgFloat64& normalProjection) const
+		{
+			dgInt32 i0 = face->m_index[0];
+			dgInt32 i1 = face->m_index[1];
+			dgInt32 i2 = face->m_index[2];
+
+			const dgBigVector& p0 = m_points[i0];
+			dgBigVector normal ((m_points[i1] - p0) * (m_points[i2] - p0));
+
+			dgFloat64 N = (origin - p0) % normal;
+			dgFloat64 D = dist % normal;
+
+			if (fabs(D) < dgFloat64 (1.0e-16f)) { // 
+				normalProjection = dgFloat32 (0.0);
+				if (N > dgFloat64 (0.0f)) {
+					return dgFloat32 (-1.0e30);
+				} else {
+
+					return dgFloat32 (1.0e30);
+				}
+			}
+			normalProjection = D;
+			return - N / D;
+		}
+
+		dgConvexHull3DFace* ClosestFaceVertexToPoint (const dgBigVector& point)
+		{
+			// note, for this function to be effective point should be an already close point to the Hull.
+			// for example casting the point to the OBB or the AABB of the full is a good first guess. 
+			dgConvexHull3DFace* closestFace = &GetFirst()->GetInfo();	
+			dgInt8 pool[256 * (sizeof (dgConvexHull3DFace*) + sizeof (dgFloat64))];
+			dgUpHeap<dgConvexHull3DFace*,dgFloat64> heap (pool, sizeof (pool));
+
+			for (dgInt32 i = 0; i < 3; i ++) {
+				dgBigVector dist (m_points[closestFace->m_index[i]] - point);
+				heap.Push(closestFace, dist % dist);
+			}
+
+			m_mark ++;	
+			dgFloat64 minDist = heap.Value();
+			while (heap.GetCount()) {
+				dgConvexHull3DFace* const face = heap[0];	
+				if (heap.Value() < minDist) {
+					minDist = heap.Value();
+					closestFace = face;
+				}
+				heap.Pop();
+				//face->m_mark = m_mark;
+				face->SetMark(m_mark);
+				for (dgInt32 i = 0; i < 3; i ++) {
+					//const dgConvexHull3DFace* twin = &face->m_twin[i]->GetInfo();	
+					dgConvexHull3DFace* twin = &face->GetTwin(i)->GetInfo();	
+					//if (twin->m_mark != m_mark) {
+					if (twin->GetMark() != m_mark) {
+						dgBigVector dist (m_points[twin->m_index[i]] - point);
+						// use hysteresis to prevent stops at a local minimal, but at the same time fast descend
+						dgFloat64 dist2 = dist % dist;
+						if (dist2 < (minDist * dgFloat64 (1.001f))) {
+							heap.Push(twin, dist2);
+						}
+					}
+				}
+			}
+
+			return closestFace;
+		}
+
+
+		// this version have input sensitive complexity (approximately  log2)
+		// when casting parallel rays and using the last face as initial guess this version has const time complexity 
+		dgFloat64 RayCast (const dgBigVector& localP0, const dgBigVector& localP1, dgConvexHull3DFace** firstFaceGuess)
+		{
+			dgConvexHull3DFace* face = &GetFirst()->GetInfo();
+			if (firstFaceGuess && *firstFaceGuess) {
+				face = *firstFaceGuess;
+			} else {
+				if (GetCount() > 32) {
+					dgVector q0 (localP0);
+					dgVector q1 (localP1);
+					if (dgRayBoxClip (q0, q1, m_aabbP0, m_aabbP1)) {
+						face = ClosestFaceVertexToPoint (q0);
+					}
+				}
+			}
+
+			m_mark ++;	
+			//face->m_mark = m_mark;
+			face->SetMark (m_mark);
+			dgInt8 pool[256 * (sizeof (dgConvexHullRayCastData) + sizeof (dgFloat64))];
+			dgDownHeap<dgConvexHullRayCastData,dgFloat64> heap (pool, sizeof (pool));
+
+			dgFloat64 t0 = dgFloat64 (-1.0e20);			//for the maximum entering segment parameter;
+			dgFloat64 t1 = dgFloat64 ( 1.0e20);			//for the minimum leaving segment parameter;
+			dgBigVector dS (localP1 - localP0);		// is the segment direction vector;
+			dgConvexHullRayCastData data;
+			data.m_face = face;
+			dgFloat64 t = FaceRayCast (face, localP0, dS, data.m_normalProjection);
+			if (data.m_normalProjection >= dgFloat32 (0.0)) {
+				t = dgFloat64 (-1.0e30);
+			}
+
+			heap.Push (data, t);
+			while (heap.GetCount()) {
+				dgConvexHullRayCastData data (heap[0]);
+				dgFloat64 t = heap.Value();
+				dgConvexHull3DFace* face = data.m_face;
+				dgFloat64 normalDistProjection = data.m_normalProjection;
+				heap.Pop();
+				bool foundThisBestFace = true;
+				if (normalDistProjection < dgFloat64 (0.0f)) {
+					if (t > t0) {
+						t0 = t;
+					}
+					if (t0 > t1) {
+						return dgFloat64 (1.2f);
+					}
+				} else {
+					foundThisBestFace = false;
+				}
+
+				for (dgInt32 i = 0; i < 3; i ++) {
+					//dgConvexHull3DFace* const face1 = &face->m_twin[i]->GetInfo();
+					dgConvexHull3DFace* const face1 = &face->GetTwin(i)->GetInfo();
+
+					//if (face1->m_mark != m_mark) {
+					if (face1->GetMark() != m_mark) {
+						//face1->m_mark = m_mark;
+						face1->SetMark (m_mark);
+						dgConvexHullRayCastData data;
+						data.m_face = face1;
+						dgFloat64 t = FaceRayCast (face1, localP0, dS, data.m_normalProjection);
+						if (data.m_normalProjection >= dgFloat32 (0.0)) {
+							t = dgFloat64 (-1.0e30);
+						} else if (t > t0) {
+							foundThisBestFace = false;
+						} else if (fabs (t - t0) < dgFloat64 (1.0e-10f)) {
+							return dgConvexHull3d::RayCast (localP0, localP1);
+						}
+						if ((heap.GetCount() + 2)>= heap.GetMaxCount()) {
+							// remove t values that are old and way outside interval [0.0, 1.0]  
+							for (dgInt32 i = heap.GetCount() - 1; i >= 0; i--) {
+								dgFloat64 val = heap.Value(i);
+								if ((val < dgFloat64 (-100.0f)) || (val > dgFloat64 (100.0f))) {
+									heap.Remove(i);
+								}
+							}
+						}
+						heap.Push (data, t);
+					}
+				}
+				if (foundThisBestFace) {
+					if ((t0 >= dgFloat64 (0.0f)) && (t0 <= dgFloat64 (1.0f))) {
+						if (firstFaceGuess) {
+							*firstFaceGuess = face;
+						}
+						return t0;
+					}
+					break;
+				}
+			}
+
+			return dgFloat64 (1.2f);
+
+		}
+
+		dgFloat64 FastRayCast (const dgBigVector& localP0, const dgBigVector& localP1, dgConvexHull3DFace** guess)
+		{
+#if 0
+	#ifdef _DEBUG
+			dgFloat64 t0 = dgConvexHull3d::RayCast (localP0, localP1);
+			dgFloat64 t1 = RayCast (localP0, localP1, guess);
+			dgAssert (fabs(t0 - t1) < dgFloat64 (1.0e-5f));
+	#endif
+#endif
+
+			//return dgConvexHull3d::RayCast (localP0, localP1);
+			return RayCast (localP0, localP1, guess);
+		}
+
+		dgInt32 m_mark;
+	};
+
+	class dgHACDConvacityLookAheadTree
+	{
+		public:
+		DG_CLASS_ALLOCATOR(allocator)
+
+			dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgEdge* const face, dgFloat64 concavity)
+			:m_concavity(concavity)	
+			,m_faceList (allocator)
+			,m_left (NULL)
+			,m_right (NULL)
+		{
+			m_faceList.Append(face);
+		}
+
+
+		dgHACDConvacityLookAheadTree (dgMemoryAllocator* const allocator, dgHACDConvacityLookAheadTree* const leftChild, dgHACDConvacityLookAheadTree* const rightChild, dgFloat64 concavity)
+			:m_concavity(concavity)	
+			,m_faceList (allocator)
+			,m_left (leftChild)
+			,m_right (rightChild)
+		{
+			dgAssert (leftChild);
+			dgAssert (rightChild);
+
+			dgFloat64 concavityTest = m_concavity - dgFloat64 (1.0e-5f);
+			//if ((m_left->m_faceList.GetCount() == 1) || (m_right->m_faceList.GetCount() == 1)) {
+			if ((((m_left->m_faceList.GetCount() == 1) || (m_right->m_faceList.GetCount() == 1))) ||
+				((concavityTest <= m_left->m_concavity) && (concavityTest <= m_right->m_concavity))) {
+					//The the parent has lower concavity this mean that the two do no add more detail, 
+					//the can be deleted and replaced the parent node
+					// for example the two children can be two convex strips that are part of a larger convex piece
+					// but each part has a non zero concavity, while the convex part has a lower concavity 
+					m_faceList.Merge (m_left->m_faceList);
+					m_faceList.Merge (m_right->m_faceList);
+
+					delete m_left;
+					delete m_right;
+					m_left = NULL;
+					m_right = NULL;
+			} else {
+				for (dgList<dgEdge*>::dgListNode* node = m_left->m_faceList.GetFirst(); node; node = node->GetNext()) {
+					m_faceList.Append(node->GetInfo());
+				}
+				for (dgList<dgEdge*>::dgListNode* node = m_right->m_faceList.GetFirst(); node; node = node->GetNext()) {
+					m_faceList.Append(node->GetInfo());
+				}
+			}
+		}
+
+		~dgHACDConvacityLookAheadTree ()
+		{
+			if (m_left) {
+				dgAssert (m_right);
+				delete m_left;
+				delete m_right;
+			}
+		}
+
+		dgInt32 GetNodesCount () const
+		{
+			dgInt32 count = 0;
+			dgInt32 stack = 1;
+			const dgHACDConvacityLookAheadTree* pool[1024];
+			pool[0] = this;
+			while (stack) {
+				stack --;
+				count ++;
+				const dgHACDConvacityLookAheadTree* const root = pool[stack];
+				if (root->m_left) {
+					dgAssert (root->m_right);
+					pool[stack] = root->m_left;
+					stack ++;
+					dgAssert (stack < sizeof (pool)/sizeof (pool[0]));
+					pool[stack] = root->m_right;
+					stack ++;
+					dgAssert (stack < sizeof (pool)/sizeof (pool[0]));
+				}
+			}
+			return count;
+		}
+
+		void ReduceByCount (dgInt32 count, dgDownHeap<dgHACDConvacityLookAheadTree*, dgFloat64>& approximation)
+		{
+			if (count < 1) {
+				count = 1;
+			}
+//			dgInt32 nodesCount = GetNodesCount();
+
+			approximation.Flush();
+			dgHACDConvacityLookAheadTree* tmp = this;
+			approximation.Push(tmp, m_concavity);
+//			nodesCount --;
+			//while (nodesCount && (approximation.GetCount() < count) && (approximation.Value() >= dgFloat32 (0.0f))) {
+			while ((approximation.GetCount() < count) && (approximation.Value() >= dgFloat32 (0.0f))) {
+				dgHACDConvacityLookAheadTree* worseCluster = approximation[0];
+				if (!worseCluster->m_left && approximation.Value() >= dgFloat32 (0.0f)) {
+					approximation.Pop();
+					approximation.Push(worseCluster, dgFloat32 (-1.0f));
+				} else {
+					dgAssert (worseCluster->m_left);
+					dgAssert (worseCluster->m_right);
+					approximation.Pop();
+					approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity);
+					approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity);
+//					nodesCount -= 2;
+				}
+			}
+		}
+
+
+		void ReduceByConcavity (dgFloat64 concavity, dgDownHeap<dgHACDConvacityLookAheadTree*, dgFloat64>& approximation)
+		{
+			approximation.Flush();
+			dgHACDConvacityLookAheadTree* tmp = this;
+
+			approximation.Push(tmp, m_concavity);
+			while (approximation.Value() > concavity) {
+				dgHACDConvacityLookAheadTree* worseCluster = approximation[0];
+				if (!worseCluster->m_left && approximation.Value() >= dgFloat32 (0.0f)) {
+					approximation.Pop();
+					approximation.Push(worseCluster, dgFloat32 (-1.0f));
+				} else {
+					dgAssert (worseCluster->m_left);
+					dgAssert (worseCluster->m_right);
+					approximation.Pop();
+					approximation.Push(worseCluster->m_left, worseCluster->m_left->m_concavity);
+					approximation.Push(worseCluster->m_right, worseCluster->m_right->m_concavity);
+				}
+			}
+		}
+
+		dgFloat64 m_concavity; 
+		dgList<dgEdge*> m_faceList;
+		dgHACDConvacityLookAheadTree* m_left;
+		dgHACDConvacityLookAheadTree* m_right;
+	};
+
+	class dgPairProxy
+	{
+		public:
+		dgPairProxy()
+			:m_nodeA(NULL)
+			,m_nodeB(NULL)
+			,m_hierachicalClusterIndexA(0)
+			,m_hierachicalClusterIndexB(0)
+			,m_area(dgFloat64(0.0f))
+		{
+		}
+
+		~dgPairProxy()
+		{
+		}
+
+		dgListNode* m_nodeA;
+		dgListNode* m_nodeB;
+		dgInt32 m_hierachicalClusterIndexA;
+		dgInt32 m_hierachicalClusterIndexB;
+		dgFloat64 m_area;
+		dgFloat64 m_distanceConcavity;
+	};
+
+	class dgHACDRayCasterContext: public dgFastRayTest
+	{
+		public:
+		dgHACDRayCasterContext (const dgVector& l0, const dgVector& l1, dgHACDClusterGraph* const me, dgInt32 mycolor)
+			:dgFastRayTest (l0, l1)
+			,m_myColor(mycolor)
+			,m_colorHit(-1)
+			,m_param (1.0f) 
+			,m_me (me) 
+		{
+		}
+
+		dgInt32 m_myColor;
+		dgInt32 m_colorHit;
+		dgFloat32 m_param;
+		dgHACDClusterGraph* m_me;
+	};
+
+
+	dgHACDClusterGraph(dgMeshEffect& mesh, dgFloat32 backFaceDistanceFactor, dgReportProgress reportProgressCallback)
+		:dgGraph<dgHACDCluster, dgHACDEdge> (mesh.GetAllocator())
+		,dgAABBPolygonSoup()
+		,m_mark(0)
+		,m_faceCount(0)
+		,m_vertexMark(0)
+		,m_progress(0)
+		,m_cancavityTreeIndex(0)
+		,m_invFaceCount(dgFloat32 (1.0f))
+		,m_vertexMarks(NULL)
+		,m_diagonal(dgFloat64(1.0f))
+		,m_vertexPool(NULL)
+		,m_proxyList(mesh.GetAllocator())
+		,m_concavityTreeArray(NULL)
+		,m_convexProximation(mesh.GetAllocator())
+		,m_priorityHeap (mesh.GetCount() + 2048, mesh.GetAllocator())
+		,m_reportProgressCallback (reportProgressCallback)
+		,m_parallerConcavityCalculator(mesh.GetAllocator())
+	{
+		
+		m_parallerConcavityCalculator.SetThreadsCount(DG_CONCAVITY_MAX_THREADS);
+
+		// precondition the mesh for better approximation
+		mesh.ConvertToPolygons();
+
+		m_faceCount = mesh.GetTotalFaceCount();
+
+		dgMemoryAllocator* const allocator = mesh.GetAllocator();
+		m_invFaceCount = dgFloat32 (1.0f) / (m_faceCount);
+
+		// init some auxiliary structures
+		dgInt32 vertexCount = mesh.GetVertexCount();
+		m_vertexMarks =  (dgInt32*) dgMallocStack(vertexCount * sizeof(dgInt32));
+		m_vertexPool =  (dgBigVector*) dgMallocStack(vertexCount * sizeof(dgBigVector));
+		memset(m_vertexMarks, 0, vertexCount * sizeof(dgInt32));
+
+		m_cancavityTreeIndex = m_faceCount + 1;
+		m_concavityTreeArray = (dgHACDConvacityLookAheadTree**) dgMallocStack(2 * m_cancavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*));
+		memset(m_concavityTreeArray, 0, 2 * m_cancavityTreeIndex * sizeof(dgHACDConvacityLookAheadTree*));
+
+		// scan the mesh and and add a node for each face
+		dgInt32 color = 1;
+		dgMeshEffect::Iterator iter(mesh);
+
+		dgInt32 meshMask = mesh.IncLRU();
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (iter.Begin(); iter; iter++) {
+			dgEdge* const edge = &(*iter);
+			if ((edge->m_mark != meshMask) && (edge->m_incidentFace > 0)) {
+
+				// call the progress callback
+				//ReportProgress();
+
+				dgListNode* const clusterNode = AddNode ();
+				dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData;
+				cluster.SetAllocator(mesh.GetAllocator());
+
+				dgFloat64 perimeter = dgFloat64(0.0f);
+				dgEdge* ptr = edge;
+				do {
+					dgBigVector p1p0(points[ptr->m_incidentVertex] - points[ptr->m_prev->m_incidentVertex]);
+					perimeter += sqrt(p1p0 % p1p0);
+					ptr->m_incidentFace = color;
+					ptr->m_userData = dgUnsigned64 (clusterNode);
+					ptr->m_mark = meshMask;
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+
+				dgBigVector normal = mesh.FaceNormal(edge, &points[0][0], sizeof(dgBigVector));
+				dgFloat64 mag = sqrt(normal % normal);
+
+				cluster.m_color = color;
+				cluster.m_hierachicalClusterIndex = color;
+				cluster.m_area = dgFloat64(0.5f) * mag;
+				cluster.m_concavity = CalculateConcavityMetric (dgFloat64 (0.0f), cluster.m_area, perimeter, 1, 0);
+
+				dgHACDClusterFace& face = cluster.Append()->GetInfo();
+				face.m_edge = edge;
+				face.m_area = dgFloat64(0.5f) * mag;
+				face.m_normal = normal.Scale3(dgFloat64(1.0f) / mag);
+
+				//m_concavityTreeArray[color] = new (allocator) dgHACDConvacityLookAheadTree (allocator, edge, cluster.m_concavity);
+				m_concavityTreeArray[color] = new (allocator) dgHACDConvacityLookAheadTree (allocator, edge, dgFloat64 (0.0f));
+
+				color ++;
+			}
+		}
+
+		// add all link adjacent faces links
+		for (dgListNode* clusterNode = GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) {
+
+			// call the progress callback
+			//ReportProgress();
+
+			dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData;
+			dgHACDClusterFace& face = cluster.GetFirst()->GetInfo();
+			dgEdge* const edge = face.m_edge;
+			dgEdge* ptr = edge; 
+			do {
+				if (ptr->m_twin->m_incidentFace > 0) {
+					dgAssert (ptr->m_twin->m_userData);
+					dgListNode* const twinClusterNode = (dgListNode*) ptr->m_twin->m_userData;
+					dgAssert (twinClusterNode);
+
+					bool doubleEdge = false;
+					for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNode = clusterNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) {
+						if (edgeNode->GetInfo().m_node == twinClusterNode) {
+							doubleEdge = true;
+							break;
+						}
+					}
+					if (!doubleEdge) {
+						clusterNode->GetInfo().AddEdge (twinClusterNode);
+					}
+				}
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+
+		Trace();
+
+		// add links to back faces
+		dgPolygonSoupDatabaseBuilder builder (mesh.GetAllocator());
+		dgVector polygon[64];
+		dgInt32 indexList[64];
+
+		dgMatrix matrix (dgGetIdentityMatrix());
+		for (dgInt32 i = 0; i < sizeof (polygon) / sizeof (polygon[0]); i ++) {
+			indexList[i] = i;
+		}
+
+		dgBigVector minAABB;
+		dgBigVector maxAABB;
+		mesh.CalculateAABB (minAABB, maxAABB);
+		maxAABB -= minAABB;
+		dgFloat32 rayDiagonalLength = dgFloat32 (sqrt (maxAABB % maxAABB));
+		m_diagonal = rayDiagonalLength;
+
+		builder.Begin();
+		dgTree<dgListNode*,dgInt32> clusterMap (GetAllocator());
+		for (dgListNode* clusterNode = GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) {
+
+			// call the progress callback
+			//ReportProgress();
+
+			dgHACDCluster& cluster = clusterNode->GetInfo().m_nodeData;
+			clusterMap.Insert(clusterNode, cluster.m_color);
+			dgHACDClusterFace& face = cluster.GetFirst()->GetInfo();
+			dgEdge* const edge = face.m_edge;
+			dgInt32 count = 0;
+			dgEdge* ptr = edge;
+			do {
+				polygon[count] = points[ptr->m_incidentVertex];
+				count ++;
+				ptr = ptr->m_prev;
+			} while (ptr != edge);
+
+			builder.AddMesh(&polygon[0].m_x, count, sizeof (dgVector), 1, &count, indexList, &cluster.m_color, matrix);
+		}
+		builder.End(false);
+		Create (builder, false);
+
+
+		dgFloat32 distanceThreshold = rayDiagonalLength * backFaceDistanceFactor;
+		for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) {
+
+			// call the progress callback
+			//ReportProgress();
+			dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+			dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo();
+			dgEdge* const edgeA = faceA.m_edge;
+			dgEdge* ptr = edgeA;
+
+			dgVector p0 (points[ptr->m_incidentVertex]);
+			dgVector p1 (points[ptr->m_next->m_incidentVertex]);
+			ptr = ptr->m_next->m_next;
+			do {
+				dgVector p2 (points[ptr->m_incidentVertex]);
+				dgVector p01 ((p0 + p1).Scale (dgFloat32 (0.5f)));
+				dgVector p12 ((p1 + p2).Scale (dgFloat32 (0.5f)));
+				dgVector p20 ((p2 + p0).Scale (dgFloat32 (0.5f)));
+
+				CastBackFace (clusterNodeA, p0, p01, p20, distanceThreshold, clusterMap);
+				CastBackFace (clusterNodeA, p1, p12, p01, distanceThreshold, clusterMap);
+				CastBackFace (clusterNodeA, p2, p20, p12, distanceThreshold, clusterMap);
+				CastBackFace (clusterNodeA, p01, p12, p20, distanceThreshold, clusterMap);
+
+				p1 = p2;
+				ptr = ptr->m_next;
+			} while (ptr != edgeA);
+		}
+
+		Trace();
+	}
+
+	~dgHACDClusterGraph ()
+	{
+		for (dgInt32 i = 0; i < m_faceCount * 2; i ++) {
+			if (m_concavityTreeArray[i]) {
+				delete m_concavityTreeArray[i];
+			}
+		}
+
+		dgFreeStack(m_concavityTreeArray);
+		dgFreeStack(m_vertexPool);
+		dgFreeStack(m_vertexMarks);
+	}
+
+
+	void CastBackFace (
+		dgListNode* const clusterNodeA,
+		const dgVector& p0, 
+		const dgVector& p1, 
+		const dgVector& p2,
+		dgFloat32 distanceThreshold,
+		dgTree<dgListNode*,dgInt32>& clusterMap)
+	{
+		dgVector origin ((p0 + p1 + p2).Scale (dgFloat32 (1.0f/3.0f)));
+
+		dgFloat32 rayDistance = distanceThreshold * dgFloat32 (2.0f);
+
+		dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+		dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo();
+		dgVector end (origin - dgVector (faceA.m_normal).Scale (rayDistance));
+
+		dgHACDRayCasterContext ray (origin, end, this, clusterA.m_color);
+		ForAllSectorsRayHit(ray, RayHit, &ray);
+
+		if (ray.m_colorHit != -1) {
+			dgAssert (ray.m_colorHit != ray.m_myColor);
+			dgFloat32 distance = rayDistance * ray.m_param;
+
+			if (distance < distanceThreshold) {
+
+				dgAssert (ray.m_colorHit != clusterA.m_color);
+				dgAssert (clusterMap.Find(ray.m_colorHit));
+				dgListNode* const clusterNodeB = clusterMap.Find(ray.m_colorHit)->GetInfo();
+				dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData;
+
+				dgHACDClusterFace& faceB = clusterB.GetFirst()->GetInfo();
+				dgEdge* const edgeB = faceB.m_edge;
+
+				bool isAdjacent = false;
+				dgEdge* ptrA = faceA.m_edge;
+				do {
+					dgEdge* ptrB = edgeB;
+					do {
+						if (ptrB->m_twin == ptrA) {
+							ptrA = faceA.m_edge->m_prev;
+							isAdjacent = true;
+							break;
+						}
+						ptrB = ptrB->m_next;
+					} while (ptrB != edgeB);
+
+					ptrA = ptrA->m_next;
+				} while (ptrA != faceA.m_edge);
+
+				if (!isAdjacent) {
+
+					isAdjacent = false;
+					for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNode = clusterNodeA->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) {
+						if (edgeNode->GetInfo().m_node == clusterNodeB) {
+							isAdjacent = true;
+							break;
+						}
+					}
+
+					if (!isAdjacent) {
+
+						dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* const edgeNodeAB = clusterNodeA->GetInfo().AddEdge (clusterNodeB);
+						dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* const edgeNodeBA = clusterNodeB->GetInfo().AddEdge (clusterNodeA);
+
+						dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+						dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+						edgeAB.m_backFaceHandicap = dgFloat64 (0.5f);
+						edgeBA.m_backFaceHandicap = dgFloat64 (0.5f);
+					}
+				}
+			}
+		}
+	}
+
+
+	void Trace() const
+	{
+		/*
+		for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) {
+		dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+		//dgHACDClusterFace& faceA = clusterA.GetFirst()->GetInfo();
+		//dgEdge* const edgeA = faceA.m_edge;
+
+		dgTrace (("cluster node: %d\n", clusterA.m_color));
+		dgTrace (("            links: "));
+		for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeA = clusterNodeA->GetInfo().GetFirst(); edgeNodeA; edgeNodeA = edgeNodeA->GetNext()) {
+		dgListNode* const clusterNodeB = edgeNodeA->GetInfo().m_node;
+		dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData;
+		dgTrace (("%d ", clusterB.m_color));
+		}
+		dgTrace (("\n"));
+		}
+		dgTrace (("\n"));
+		*/
+	}
+
+
+	// you can insert cal callback here  to print the progress as it collapse clusters
+	void ReportProgress ()
+	{
+		m_progress ++;
+		if (m_reportProgressCallback) {
+			dgFloat32 progress = dgFloat32(m_progress) * m_invFaceCount;
+			m_reportProgressCallback (progress);
+		}
+	}
+
+	dgMeshEffect* CreatePatitionMesh (dgMeshEffect& mesh, dgInt32 maxVertexPerHull)
+	{
+		dgMemoryAllocator* const allocator = mesh.GetAllocator();
+		dgMeshEffect* const convexPartionMesh = new (allocator) dgMeshEffect(allocator, true);
+
+		dgMeshEffect::dgVertexAtribute polygon[256];
+		memset(polygon, 0, sizeof(polygon));
+		dgArray<dgBigVector> convexVertexBuffer(mesh.GetCount(), GetAllocator());
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+
+		convexPartionMesh->BeginPolygon();
+		dgFloat64 layer = dgFloat64 (0.0f);
+		for (dgList<dgHACDConvacityLookAheadTree*>::dgListNode* clusterNode = m_convexProximation.GetFirst(); clusterNode; clusterNode = clusterNode->GetNext()) {
+			dgHACDConvacityLookAheadTree* const cluster = clusterNode->GetInfo();
+
+			dgInt32 vertexCount = 0;
+			for (dgList<dgEdge*>::dgListNode* faceNode = cluster->m_faceList.GetFirst(); faceNode; faceNode = faceNode->GetNext()) {
+				dgEdge* const edge = faceNode->GetInfo();
+				dgEdge* ptr = edge;
+				do {
+					dgInt32 index = ptr->m_incidentVertex;
+					convexVertexBuffer[vertexCount] = points[index];
+					vertexCount++;
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+			}
+			dgConvexHull3d convexHull(allocator, &convexVertexBuffer[0].m_x, sizeof(dgBigVector), vertexCount, 0.0, maxVertexPerHull);
+			if (convexHull.GetCount()) {
+				const dgBigVector* const vertex = convexHull.GetVertexPool();
+				for (dgConvexHull3d::dgListNode* node = convexHull.GetFirst(); node; node = node->GetNext()) {
+					const dgConvexHull3DFace* const face = &node->GetInfo();
+
+					dgInt32 i0 = face->m_index[0];
+					dgInt32 i1 = face->m_index[1];
+					dgInt32 i2 = face->m_index[2];
+
+					polygon[0].m_vertex = vertex[i0];
+					polygon[0].m_vertex.m_w = layer;
+
+					polygon[1].m_vertex = vertex[i1];
+					polygon[1].m_vertex.m_w = layer;
+
+					polygon[2].m_vertex = vertex[i2];
+					polygon[2].m_vertex.m_w = layer;
+
+					convexPartionMesh->AddPolygon(3, &polygon[0].m_vertex.m_x, sizeof(dgMeshEffect::dgVertexAtribute), 0);
+				}
+				layer += dgFloat64 (1.0f);
+			}
+		}
+		convexPartionMesh->EndPolygon(1.0e-5f);
+
+		m_progress = m_faceCount - 1;
+		ReportProgress();
+
+		return convexPartionMesh;
+	}
+
+
+
+	static dgFloat32 RayHit (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount)
+	{
+		dgHACDRayCasterContext& me = *((dgHACDRayCasterContext*) context);
+		dgVector normal (&polygon[indexArray[indexCount] * (strideInBytes / sizeof (dgFloat32))]);
+		dgFloat32 t = me.PolygonIntersect (normal, polygon, strideInBytes, indexArray, indexCount);
+		if (t < me.m_param) {
+			dgInt32 faceColor = me.m_me->GetTagId(indexArray);
+			if (faceColor != me.m_myColor) {
+				me.m_param = t;
+				me.m_colorHit = faceColor;
+			}
+		}
+		return t;
+	}
+
+
+	dgFloat64 ConcavityByFaceMedian (dgInt32 faceCountA, dgInt32 faceCountB) const
+	{
+		dgFloat64 faceCountCost = DG_CONCAVITY_SCALE * dgFloat64 (0.1f) * (faceCountA + faceCountB) * m_invFaceCount;
+		//faceCountCost *= 0;
+		return faceCountCost;
+	}
+
+	dgFloat64 CalculateConcavityMetric (dgFloat64 convexConcavity, dgFloat64 area, dgFloat64 perimeter, dgInt32 faceCountA, dgInt32 faceCountB) const 
+	{
+		dgFloat64 edgeCost = perimeter * perimeter / (dgFloat64(4.0f * 3.141592f) * area);
+		return convexConcavity * DG_CONCAVITY_SCALE + edgeCost + ConcavityByFaceMedian (faceCountA, faceCountB);
+	}
+
+	void SubmitInitialEdgeCosts (dgMeshEffect& mesh) 
+	{
+		m_mark ++;
+		for (dgListNode* clusterNodeA = GetFirst(); clusterNodeA; clusterNodeA = clusterNodeA->GetNext()) {
+			// call the progress callback
+			//ReportProgress();
+
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) {
+				dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+				dgFloat64 weight = edgeAB.m_backFaceHandicap; 
+				if (edgeAB.m_mark != m_mark) {
+					edgeAB.m_mark = m_mark;
+					dgListNode* const clusterNodeB = edgeNodeAB->GetInfo().m_node;
+					for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) {
+						dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node;
+						if (clusterNode == clusterNodeA) {
+							dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+							edgeBA.m_mark = m_mark;
+							dgAssert (!edgeAB.m_proxyListNode);
+							dgAssert (!edgeBA.m_proxyListNode);
+
+							dgList<dgPairProxy>::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB, weight * edgeBA.m_backFaceHandicap);
+							edgeAB.m_proxyListNode = proxyNode;
+							edgeBA.m_proxyListNode = proxyNode;
+							break;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	dgInt32 CopyVertexToPool(const dgMeshEffect& mesh, const dgHACDCluster& cluster, dgInt32 start)
+	{
+		dgInt32 count = start;
+
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			const dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			do {
+				dgInt32 index = edge->m_incidentVertex;
+				if (m_vertexMarks[index] != m_vertexMark) {
+					m_vertexMarks[index] = m_vertexMark;
+					m_vertexPool[count] = points[index];
+					count++;
+				}
+				edge = edge->m_next;
+			} while (edge != clusterFace.m_edge);
+		}
+		return count;
+	}
+
+
+	void MarkInteriorClusterEdges (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const
+	{
+		dgAssert (colorA != colorB);
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			do {
+				if ((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB)) {
+					edge->m_mark = mark;
+					edge->m_twin->m_mark = mark;
+				}
+				edge = edge->m_next;
+			} while (edge != clusterFace.m_edge);
+		}
+	}
+
+	dgFloat64 CalculateClusterPerimeter (dgMeshEffect& mesh, dgInt32 mark, const dgHACDCluster& cluster, dgInt32 colorA, dgInt32 colorB) const
+	{
+		dgAssert (colorA != colorB);
+		dgFloat64 perimeter = dgFloat64 (0.0f);
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			do {
+				if (!((edge->m_twin->m_incidentFace == colorA) || (edge->m_twin->m_incidentFace == colorB))) {
+					dgBigVector p1p0(points[edge->m_twin->m_incidentVertex] - points[edge->m_incidentVertex]);
+					perimeter += sqrt(p1p0 % p1p0);
+				}
+				edge = edge->m_next;
+			} while (edge != clusterFace.m_edge);
+		}
+
+		return perimeter;
+	}
+
+	void HeapCollectGarbage () 
+	{
+		if ((m_priorityHeap.GetCount() + 20) > m_priorityHeap.GetMaxCount()) {
+			for (dgInt32 i = m_priorityHeap.GetCount() - 1; i >= 0; i--) {
+				dgList<dgPairProxy>::dgListNode* const emptyNode = m_priorityHeap[i];
+				dgPairProxy& emptyPair = emptyNode->GetInfo();
+				if ((emptyPair.m_nodeA == NULL) && (emptyPair.m_nodeB == NULL)) {
+					m_priorityHeap.Remove(i);
+				}
+			}
+		}
+	}
+
+
+	dgFloat64 CalculateConcavity(dgHACDConveHull& hull, const dgMeshEffect& mesh, const dgHACDCluster& cluster)
+	{
+		dgFloat64 concavity = dgFloat32(0.0f);
+
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+		for (dgList<dgHACDClusterFace>::dgListNode* node = cluster.GetFirst(); node; node = node->GetNext()) {
+			dgHACDClusterFace& clusterFace = node->GetInfo();
+			dgEdge* edge = clusterFace.m_edge;
+			dgInt32 i0 = edge->m_incidentVertex;
+			dgInt32 i1 = edge->m_next->m_incidentVertex;
+			for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) {
+				dgInt32 i2 = ptr->m_incidentVertex;
+				dgFloat64 val = hull.CalculateTriangleConcavity(clusterFace.m_normal, i0, i1, i2, points);
+				if (val > concavity) {
+					concavity = val;
+				}
+				i1 = i2;
+			}
+		}
+
+		return concavity;
+	}
+
+	dgFloat64 CalculateConcavitySingleThread (dgHACDConveHull& hull, dgMeshEffect& mesh, dgHACDCluster& clusterA, dgHACDCluster& clusterB)
+	{
+		return dgMax(CalculateConcavity(hull, mesh, clusterA), CalculateConcavity(hull, mesh, clusterB));
+	}
+
+
+	class dgConvexHullRayCastContext
+	{
+		public: 
+		dgConvexHullRayCastContext (dgHACDConveHull& hull, dgMeshEffect& mesh, dgThreadHive* const manager)
+			:m_atomicLock(0)
+			,m_mesh(&mesh)
+			,m_cluster(NULL)
+			,m_threadManager(manager)
+			,m_faceNode(NULL)
+		{
+			for(dgInt32 i = 0; i < DG_CONCAVITY_MAX_THREADS; i ++) {
+				hullArray[i] = new (mesh.GetAllocator()) dgHACDConveHull (hull);
+			}
+		}
+
+		~dgConvexHullRayCastContext ()
+		{
+			for(dgInt32 i = 0; i < DG_CONCAVITY_MAX_THREADS; i ++) {
+				delete hullArray[i];
+			}
+		}
+
+		void SetCluster (dgHACDCluster& cluster)
+		{
+			m_cluster = &cluster;
+			m_node = m_cluster->GetFirst();
+			memset (m_concavity, 0, sizeof (m_concavity));
+		}
+
+		dgFloat64 GetConcavity() const 
+		{
+			dgFloat64 concavity = dgFloat32(0.0f);
+			for (dgInt32 i = 0; i < DG_CONCAVITY_MAX_THREADS; i ++) {	
+				if (concavity < m_concavity[i]) {
+					concavity = m_concavity[i];
+				}
+			}
+			return concavity;
+		}
+
+
+		static void RayCastKernel (void* const context, dgInt32 threadID)
+		{
+			dgConvexHullRayCastContext* const data = (dgConvexHullRayCastContext*) context;
+			const dgBigVector* const points = (dgBigVector*) data->m_mesh->GetVertexPool();
+			
+			data->m_threadManager->GetIndirectLock(&data->m_atomicLock, threadID);
+			dgList<dgHACDClusterFace>::dgListNode* node = data->m_node;
+			if (node) {
+				data->m_node = node->GetNext();
+			}
+			data->m_threadManager->ReleaseIndirectLock (&data->m_atomicLock);
+			for (; node;) {
+
+				dgHACDClusterFace& clusterFace = node->GetInfo();
+				dgEdge* edge = clusterFace.m_edge;
+				dgInt32 i0 = edge->m_incidentVertex;
+				dgInt32 i1 = edge->m_next->m_incidentVertex;
+				for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) {
+					dgInt32 i2 = ptr->m_incidentVertex;
+					dgFloat64 val = data->hullArray[threadID]->CalculateTriangleConcavity(clusterFace.m_normal, i0, i1, i2, points);
+					if (val > data->m_concavity[threadID]) {
+						data->m_concavity[threadID] = val;
+					}
+					i1 = i2;
+				}
+
+				data->m_threadManager->GetIndirectLock(&data->m_atomicLock, threadID);
+				node = data->m_node;
+				if (node) {
+					data->m_node = node->GetNext();;
+				}
+				data->m_threadManager->ReleaseIndirectLock (&data->m_atomicLock);
+			}
+		}
+
+
+		dgInt32 m_atomicLock;
+		dgMeshEffect* m_mesh;
+		dgHACDCluster* m_cluster;
+		dgThreadHive* m_threadManager;
+		dgList<dgHACDClusterFace>::dgListNode* m_node;
+
+		dgList<dgHACDClusterFace>::dgListNode* m_faceNode;
+		dgFloat64 m_concavity[DG_CONCAVITY_MAX_THREADS];
+		dgHACDConveHull* hullArray[DG_CONCAVITY_MAX_THREADS];		
+	};
+
+
+	dgFloat64 CalculateConcavityMultiThread (dgHACDConveHull& hull, dgMeshEffect& mesh, dgHACDCluster& clusterA, dgHACDCluster& clusterB)
+	{
+		dgConvexHullRayCastContext data (hull, mesh, &m_parallerConcavityCalculator);
+
+		dgInt32 threadsCount = m_parallerConcavityCalculator.GetThreadCount();	
+		data.SetCluster (clusterA);
+		for (dgInt32 i = 0; i < threadsCount; i ++) {		
+			m_parallerConcavityCalculator.QueueJob(dgConvexHullRayCastContext::RayCastKernel, &data);
+		}
+		m_parallerConcavityCalculator.SynchronizationBarrier();
+		dgFloat64 concavity = data.GetConcavity();
+
+		data.SetCluster (clusterB);
+		for (dgInt32 i = 0; i < threadsCount; i ++) {		
+			m_parallerConcavityCalculator.QueueJob(dgConvexHullRayCastContext::RayCastKernel, &data);
+		}
+		m_parallerConcavityCalculator.SynchronizationBarrier();
+		
+		concavity = dgMax(concavity, data.GetConcavity());
+		//dgFloat64 xxx = CalculateConcavitySingleThread (hull, mesh, clusterA, clusterB);
+		//dgAssert (fabs(concavity - xxx) < dgFloat64 (1.0e-5f));
+		return concavity;
+	}
+
+	dgList<dgPairProxy>::dgListNode* SubmitEdgeCost (dgMeshEffect& mesh, dgListNode* const clusterNodeA, dgListNode* const clusterNodeB, dgFloat64 perimeterHandicap)
+	{
+		dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+		dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData;
+		const dgBigVector* const points = (dgBigVector*) mesh.GetVertexPool();
+
+		bool flatStrip = true;
+		dgFloat64 tol = dgFloat64 (1.0e-5f) * m_diagonal;
+		dgHACDClusterFace& clusterFaceA = clusterA.GetFirst()->GetInfo();
+		dgBigPlane plane(clusterFaceA.m_normal, -(points[clusterFaceA.m_edge->m_incidentVertex] % clusterFaceA.m_normal));
+
+		if (clusterA.GetCount() > 1) {
+			flatStrip = clusterA.IsCoplanar(plane, mesh, tol);
+		}
+
+		if (flatStrip) {
+			flatStrip = clusterB.IsCoplanar(plane, mesh, tol);
+		}
+
+		dgList<dgPairProxy>::dgListNode* pairNode = NULL;
+		if (!flatStrip) {
+			m_vertexMark ++;
+			dgInt32 vertexCount = CopyVertexToPool(mesh, clusterA, 0);
+			vertexCount = CopyVertexToPool(mesh, clusterB, vertexCount);
+
+			dgHACDConveHull convexHull(mesh.GetAllocator(), m_vertexPool, vertexCount);
+
+			if (convexHull.GetVertexCount()) {
+				dgInt32 mark = mesh.IncLRU();
+				MarkInteriorClusterEdges (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color);
+				MarkInteriorClusterEdges (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color);
+
+				dgFloat64 area = clusterA.m_area + clusterB.m_area;
+				dgFloat64 perimeter = CalculateClusterPerimeter (mesh, mark, clusterA, clusterA.m_color, clusterB.m_color) +
+									  CalculateClusterPerimeter (mesh, mark, clusterB, clusterA.m_color, clusterB.m_color);
+
+	
+				dgFloat64 concavity = dgFloat64 (0.0f);
+				if ((convexHull.GetCount() > 128) && ((clusterA.GetCount() > 256) || (clusterB.GetCount() > 256))) { 
+					concavity = CalculateConcavityMultiThread (convexHull, mesh, clusterA, clusterB);
+				} else {
+					concavity = CalculateConcavitySingleThread (convexHull, mesh, clusterA, clusterB);
+				}
+
+				if (concavity < dgFloat64(1.0e-3f)) {
+					concavity = dgFloat64(0.0f);
+				}
+
+				// see if the heap will overflow
+				HeapCollectGarbage ();
+
+				// add a new pair to the heap
+				dgList<dgPairProxy>::dgListNode* pairNode = m_proxyList.Append();
+				dgPairProxy& pair = pairNode->GetInfo();
+				pair.m_nodeA = clusterNodeA;
+				pair.m_nodeB = clusterNodeB;
+				pair.m_distanceConcavity = concavity;
+				pair.m_hierachicalClusterIndexA = clusterA.m_hierachicalClusterIndex;
+				pair.m_hierachicalClusterIndexB = clusterB.m_hierachicalClusterIndex;
+
+				pair.m_area = area;
+				dgFloat64 cost = CalculateConcavityMetric (concavity, area, perimeter * perimeterHandicap, clusterA.GetCount(), clusterB.GetCount());
+				m_priorityHeap.Push(pairNode, cost);
+
+				return pairNode;
+			}
+		}
+		return pairNode;
+	}
+
+
+	void CollapseEdge (dgList<dgPairProxy>::dgListNode* const pairNode, dgMeshEffect& mesh, dgFloat64 concavity)
+	{
+		dgListNode* adjacentNodes[1024];
+		dgPairProxy& pair = pairNode->GetInfo();
+
+		dgMemoryAllocator* const allocator = mesh.GetAllocator();
+
+
+		dgAssert((pair.m_nodeA && pair.m_nodeB) || (!pair.m_nodeA && !pair.m_nodeB));
+		if (pair.m_nodeA && pair.m_nodeB) {
+			// call the progress callback
+			ReportProgress();
+
+			dgListNode* const clusterNodeA = pair.m_nodeA;
+			dgListNode* const clusterNodeB = pair.m_nodeB;
+			dgAssert (clusterNodeA != clusterNodeB);
+
+			dgHACDCluster& clusterA = clusterNodeA->GetInfo().m_nodeData;
+			dgHACDCluster& clusterB = clusterNodeB->GetInfo().m_nodeData;
+
+			dgAssert (&clusterA != &clusterB);
+			dgAssert(clusterA.m_color != clusterB.m_color);
+
+			dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexA];
+			dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[pair.m_hierachicalClusterIndexB];
+			dgAssert (leftTree);
+			dgAssert (rightTree);
+			m_concavityTreeArray[pair.m_hierachicalClusterIndexA] = NULL;
+			m_concavityTreeArray[pair.m_hierachicalClusterIndexB] = NULL;
+			dgAssert (m_cancavityTreeIndex < (2 * (m_faceCount + 1)));
+
+			dgFloat64 treeConcavity = pair.m_distanceConcavity;
+//			 dgAssert (treeConcavity < 0.1);
+			m_concavityTreeArray[m_cancavityTreeIndex] = new (allocator) dgHACDConvacityLookAheadTree (allocator, leftTree, rightTree, treeConcavity);
+			clusterA.m_hierachicalClusterIndex = m_cancavityTreeIndex;
+			clusterB.m_hierachicalClusterIndex = m_cancavityTreeIndex;
+			m_cancavityTreeIndex ++;
+
+			// merge two clusters
+			while (clusterB.GetCount()) {
+
+				dgHACDCluster::dgListNode* const nodeB = clusterB.GetFirst();
+				clusterB.Unlink(nodeB);
+	
+				// now color code all faces of the merged cluster
+				dgHACDClusterFace& faceB = nodeB->GetInfo();
+				dgEdge* ptr = faceB.m_edge;
+				do {
+					ptr->m_incidentFace = clusterA.m_color;
+					ptr = ptr->m_next;
+				} while (ptr != faceB.m_edge);
+				clusterA.Append(nodeB);
+			}
+			clusterA.m_area = pair.m_area;
+			clusterA.m_concavity = concavity;
+
+			// invalidate all proxies that are still in the heap
+			dgInt32 adjacentCount = 1;
+			adjacentNodes[0] = clusterNodeA;
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) {
+				dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+				dgList<dgPairProxy>::dgListNode* const proxyNode = (dgList<dgPairProxy>::dgListNode*) edgeAB.m_proxyListNode;
+				if (proxyNode) {
+					dgPairProxy& pairProxy = proxyNode->GetInfo();
+					dgAssert ((edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeA) || (edgeNodeAB->GetInfo().m_node == pairProxy.m_nodeB));
+					pairProxy.m_nodeA = NULL;
+					pairProxy.m_nodeB = NULL;
+					edgeAB.m_proxyListNode = NULL;
+				}
+
+				adjacentNodes[adjacentCount] = edgeNodeAB->GetInfo().m_node;
+				adjacentCount ++;
+				dgAssert (adjacentCount < sizeof (adjacentNodes)/ sizeof (adjacentNodes[0]));
+			}
+
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) {
+				dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+				dgList<dgPairProxy>::dgListNode* const proxyNode = (dgList<dgPairProxy>::dgListNode*) edgeBA.m_proxyListNode;
+				if (proxyNode) {
+					dgPairProxy& pairProxy = proxyNode->GetInfo();
+					pairProxy.m_nodeA = NULL;
+					pairProxy.m_nodeB = NULL;
+					edgeBA.m_proxyListNode = NULL;
+				}
+
+				bool alreadyLinked = false;
+				dgListNode* const node = edgeNodeBA->GetInfo().m_node;
+				for (dgInt32 i = 0; i < adjacentCount; i ++) {
+					if (node == adjacentNodes[i]) {
+						alreadyLinked = true;
+						break;
+					}
+				}
+				if (!alreadyLinked) {
+					clusterNodeA->GetInfo().AddEdge (node);
+					node->GetInfo().AddEdge (clusterNodeA);
+				}
+			}
+			DeleteNode (clusterNodeB);
+
+			// submit all new costs for each edge connecting this new node to any other node 
+			for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeAB = clusterNodeA->GetInfo().GetFirst(); edgeNodeAB; edgeNodeAB = edgeNodeAB->GetNext()) {
+				dgHACDEdge& edgeAB = edgeNodeAB->GetInfo().m_edgeData;
+				dgListNode* const clusterNodeB = edgeNodeAB->GetInfo().m_node;
+				dgFloat64 weigh = edgeAB.m_backFaceHandicap;
+				for (dgGraphNode<dgHACDCluster, dgHACDEdge>::dgListNode* edgeNodeBA = clusterNodeB->GetInfo().GetFirst(); edgeNodeBA; edgeNodeBA = edgeNodeBA->GetNext()) {
+					dgListNode* const clusterNode = edgeNodeBA->GetInfo().m_node;
+					if (clusterNode == clusterNodeA) {
+						dgHACDEdge& edgeBA = edgeNodeBA->GetInfo().m_edgeData;
+						dgList<dgPairProxy>::dgListNode* const proxyNode = SubmitEdgeCost (mesh, clusterNodeA, clusterNodeB, weigh * edgeBA.m_backFaceHandicap);
+						if (proxyNode) {
+							edgeBA.m_proxyListNode = proxyNode;
+							edgeAB.m_proxyListNode = proxyNode;
+						}
+						break;
+					}
+				}
+			}
+		}
+		m_proxyList.Remove(pairNode);
+	}
+
+#ifdef DG_BUILD_HIERACHICAL_HACD
+	void CollapseClusters (dgMeshEffect& mesh, dgFloat64 maxConcavity, dgInt32 maxClustesCount)
+	{
+
+		maxConcavity *= (m_diagonal * DG_CONCAVITY_SCALE);
+		while (m_priorityHeap.GetCount()) {
+			dgFloat64 concavity =  m_priorityHeap.Value();
+			dgList<dgPairProxy>::dgListNode* const pairNode = m_priorityHeap[0];
+			m_priorityHeap.Pop();
+			CollapseEdge (pairNode, mesh, concavity);
+
+//if (m_progress == 24)
+//break;
+
+		}
+
+
+
+		dgInt32 treeCounts = 0;
+		for (dgInt32 i = 0; i < m_cancavityTreeIndex; i ++) {
+			if (m_concavityTreeArray[i]) {
+				m_concavityTreeArray[treeCounts] = m_concavityTreeArray[i];
+				m_concavityTreeArray[i] = NULL;
+				treeCounts ++;
+			}
+		}
+
+		if (treeCounts > 1) {
+
+			for (dgInt32 i = 0; i < treeCounts; i ++) {
+				if (m_concavityTreeArray[i]->m_faceList.GetCount()==1) {
+					delete m_concavityTreeArray[i];
+					m_concavityTreeArray[i] = m_concavityTreeArray[treeCounts-1];
+					m_concavityTreeArray[treeCounts-1]= NULL;
+					treeCounts --;
+					i--;
+				}
+			}
+
+
+			dgFloat32 C = 10000;
+			while (treeCounts > 1)	 {
+				dgHACDConvacityLookAheadTree* const leftTree = m_concavityTreeArray[treeCounts-1];
+				dgHACDConvacityLookAheadTree* const rightTree = m_concavityTreeArray[treeCounts-2];
+				m_concavityTreeArray[treeCounts-1] = NULL;
+				m_concavityTreeArray[treeCounts-2] = new (mesh.GetAllocator()) dgHACDConvacityLookAheadTree (mesh.GetAllocator(), leftTree, rightTree, C);
+				C *= 2;
+				treeCounts --;
+			}
+
+		}
+
+		dgHACDConvacityLookAheadTree* const tree = m_concavityTreeArray[0];
+		dgDownHeap<dgHACDConvacityLookAheadTree*, dgFloat64> approximation(maxClustesCount * 2, mesh.GetAllocator());
+
+		tree->ReduceByCount (maxClustesCount, approximation);
+		//		tree->ReduceByConcavity (maxConcavity, approximation);
+
+		while (approximation.GetCount()) {
+			m_convexProximation.Append(approximation[0]);
+			approximation.Pop();
+		}
+	}
+#else 
+	void CollapseClusters (dgMeshEffect& mesh, dgFloat64 maxConcavity, dgInt32 maxClustesCount)
+	{
+		maxConcavity *= (m_diagonal * DG_CONCAVITY_SCALE);
+
+		bool terminate = false;
+		while (m_priorityHeap.GetCount() && !terminate) {
+			dgFloat64 concavity =  m_priorityHeap.Value();
+			dgList<dgPairProxy>::dgListNode* const pairNode = m_priorityHeap[0];
+			if ((concavity < maxConcavity) && (GetCount() < maxClustesCount)) {
+				terminate  = true;
+			} else {
+				m_priorityHeap.Pop();
+				CollapseEdge (pairNode, mesh, concavity);
+			}
+		}
+	}
+#endif
+
+	dgInt32 m_mark;
+	dgInt32 m_faceCount;
+	dgInt32 m_vertexMark;
+	dgInt32 m_progress;
+	dgInt32 m_cancavityTreeIndex;
+	dgInt32* m_vertexMarks;
+	dgFloat32 m_invFaceCount;
+	dgFloat64 m_diagonal;
+	dgBigVector* m_vertexPool;
+	dgList<dgPairProxy> m_proxyList;
+	dgHACDConvacityLookAheadTree** m_concavityTreeArray;	
+	dgList<dgHACDConvacityLookAheadTree*> m_convexProximation;
+	dgUpHeap<dgList<dgPairProxy>::dgListNode*, dgFloat64> m_priorityHeap;
+	dgReportProgress m_reportProgressCallback;
+	dgThreadHive m_parallerConcavityCalculator;
+};
+
+#endif
+
+dgMeshEffect* dgMeshEffect::CreateSimplification(dgInt32 maxVertexCount, dgReportProgress reportProgressCallback, void* const reportPrgressUserData) const
+{
+	if (GetVertexCount() <= maxVertexCount) {
+		return new (GetAllocator()) dgMeshEffect(*this); 
+	}
+dgAssert (0);
+return new (GetAllocator()) dgMeshEffect(*this); 
+/*
+	//	dgMeshEffect triangleMesh(*this);
+	if (maxHullsCount <= 1) {
+		maxHullsCount = 1;
+	}
+	if (maxConcavity <= dgFloat32 (1.0e-5f)) {
+		maxConcavity = dgFloat32 (1.0e-5f);
+	}
+
+	if (maxVertexPerHull < 4) {
+		maxVertexPerHull = 4;
+	}
+	ClampValue(backFaceDistanceFactor, dgFloat32 (0.01f), dgFloat32 (1.0f));
+
+	if (reportProgressCallback) {
+		reportProgressCallback (0.0f);
+	}
+
+
+	// make a copy of the mesh
+	dgMeshEffect mesh(*this);
+	mesh.ClearAttributeArray();
+
+
+	dgInt32 faceCount = mesh.GetTotalFaceCount();
+	if (faceCount  > meshSimplicationMaxFaceCount) {
+		mesh.Triangulate();
+
+		dgPolyhedra polygon(GetAllocator());
+		dgInt32 mark = mesh.IncLRU();
+		polygon.BeginFace();
+		dgPolyhedra::Iterator iter (mesh);
+		for (iter.Begin(); iter; iter ++){
+			dgEdge* const face = &(*iter);
+
+			if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+				dgInt32	index[DG_MESH_EFFECT_POINT_SPLITED];
+
+				dgEdge* ptr = face;
+				dgInt32 indexCount = 0;
+				do {
+					index[indexCount] = ptr->m_incidentVertex;
+					ptr->m_mark = mark;
+					indexCount ++;
+					ptr = ptr->m_next;
+				} while (ptr != face);
+				polygon.AddFace(indexCount, index);
+			}
+		}
+		polygon.EndFace();
+
+		polygon.Optimize(&mesh.m_points[0].m_x, sizeof (dgFloat64), 1000.0f, meshSimplicationMaxFaceCount);
+
+		mesh.RemoveAll();
+		
+		mark = polygon.IncLRU();
+		mesh.BeginFace();
+		dgPolyhedra::Iterator iter1 (polygon);
+		for (iter1.Begin(); iter1; iter1 ++){
+			dgEdge* const face = &(*iter1);
+			if ((face->m_mark != mark) && (face->m_incidentFace > 0)) {
+				dgInt32	index[DG_MESH_EFFECT_POINT_SPLITED];
+				dgEdge* ptr = face;
+				dgInt32 indexCount = 0;
+				do {
+					ptr->m_mark = mark;
+					index[indexCount] = dgInt32 (ptr->m_incidentVertex);
+					indexCount ++;
+					ptr = ptr->m_next;
+				} while (ptr != face);
+				mesh.AddFace(indexCount, index);
+			}
+		}
+		mesh.EndFace();
+
+		faceCount = mesh.GetTotalFaceCount();
+		mesh.ClearAttributeArray();
+	}
+
+	// create a general connectivity graph    
+	dgHACDClusterGraph graph (mesh, backFaceDistanceFactor, reportProgressCallback);
+
+	// calculate initial edge costs
+	graph.SubmitInitialEdgeCosts (mesh);
+
+	// collapse the graph
+	graph.CollapseClusters (mesh, maxConcavity, maxHullsCount);
+
+	// Create Partition Mesh
+	return graph.CreatePatitionMesh (mesh, maxVertexPerHull);
+*/
+}

newtondynamics.mod/NewtonDynamics/source/meshUtil/dgMeshEffect5.cpp

@@ -0,0 +1,3304 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+*
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+*
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+*
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+*
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgPhysicsStdafx.h"
+#include "dgBody.h"
+#include "dgWorld.h"
+#include "dgMeshEffect.h"
+#include "dgCollisionConvexHull.h"
+
+
+#define DG_BOOLEAN_ZERO_TOLERANCE	(1.0e-30)
+
+#if 0
+#define DG_BOOLEAN_COMBINE_MARK		(1 << 8)
+#define DG_REMAP_VERTEX_MARK		(1 << 9)
+#define DG_INTERIOR_FACE_MARK		(1 << 10)
+#define DG_LOOP_BORDER_MARK			(1 << 11)
+class dgBooleanMeshClipper: public dgMeshEffect::dgMeshBVH
+{
+	public:
+	class dgFaceVertexPair
+	{
+		public:
+		dgFaceVertexPair (dgEdge* const vertex, dgMeshBVHNode* const faceNode)
+			:m_vertex (vertex), m_faceNode(faceNode)
+		{
+		}
+		dgEdge* m_vertex;	
+		dgMeshBVHNode* m_faceNode;
+	};
+
+	dgBooleanMeshClipper (dgMeshEffect* const mesh) 
+		:dgMeshBVH (mesh)
+		,m_vertexBase(mesh->GetVertexCount())
+		,m_vertexMap(mesh->GetAllocator())
+		,m_nodeEdgeMap(mesh->GetAllocator())
+		,m_vertexAlias(mesh->GetVertexCount() + 512, mesh->GetAllocator())
+	{
+		const dgBigVector* const points = (dgBigVector*) m_mesh->GetVertexPool(); 
+		for (dgInt32 i = 0; i < m_mesh->GetVertexCount(); i ++) {
+			m_vertexAlias[i] = dgHugeVector (points[i]);
+		}
+
+		
+		dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+		dgInt32 stack = 1;
+		stackPool[0] = m_rootNode;
+		dgVector l0(dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (0.0f)); 
+		dgVector l1(dgFloat32 (1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 (0.0f)); 
+
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const me = stackPool[stack];
+			if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) {
+				if (!me->m_left) {
+					dgAssert (!me->m_right);
+					dgPolyhedra::dgPairKey key (me->m_face->m_incidentVertex, me->m_face->m_twin->m_incidentVertex);
+					m_nodeEdgeMap.Insert(me, key.GetVal());
+				} else {
+					dgAssert (me->m_left);
+					dgAssert (me->m_right);
+					stackPool[stack] = me->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack] = me->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				}
+			}
+		}
+
+		dgAssert (SanityCheck());
+	}
+
+	~dgBooleanMeshClipper () 
+	{
+	}
+
+	bool SanityCheck() const
+	{
+		#ifdef _DEBUG
+			dgAssert (dgMeshBVH::SanityCheck()); 
+			dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+			dgInt32 stack = 1;
+			stackPool[0] = m_rootNode;
+			dgVector l0(dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (0.0f)); 
+			dgVector l1(dgFloat32 (1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 (0.0f)); 
+
+			while (stack) {
+				stack --;
+				dgMeshBVHNode* const me = stackPool[stack];
+				if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) {
+					if (!me->m_left) {
+						dgAssert (!me->m_right);
+						dgPolyhedra::dgPairKey key (me->m_face->m_incidentVertex, me->m_face->m_twin->m_incidentVertex);
+						dgTree<dgMeshBVHNode*, dgUnsigned64>::dgTreeNode* const node = m_nodeEdgeMap.Find(key.GetVal());
+						dgAssert (node);
+						dgAssert (node->GetInfo() == me);
+					} else {
+						dgAssert (me->m_left);
+						dgAssert (me->m_right);
+						stackPool[stack] = me->m_left;
+						stack++;
+						dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+						stackPool[stack] = me->m_right;
+						stack++;
+						dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+					}
+				}
+			}
+		#endif
+		return true;
+	}
+
+	dgHugeVector FaceNormal (const dgEdge* const face) const
+	{
+		const dgEdge* edge = face;
+		const dgHugeVector& p0 = m_vertexAlias[edge->m_incidentVertex];
+
+		edge = edge->m_next;
+		const dgHugeVector& p1 = m_vertexAlias[edge->m_incidentVertex];
+		dgHugeVector e1 (p1 - p0);
+
+		dgHugeVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		for (edge = edge->m_next; edge != face; edge = edge->m_next) {
+			const dgHugeVector& p2 = m_vertexAlias[edge->m_incidentVertex]; 
+			dgHugeVector e2 (p2 - p0);
+			normal += e1 * e2;
+			e1 = e2;
+		} 
+		return normal;
+	}
+
+	
+	bool RayVertexIntersectQuickTest (dgEdge* const edge, const dgBooleanMeshClipper* const otherMeshBVH, dgEdge* const otherVertex) const 
+	{
+		dgTree<dgInt32,dgInt32>::dgTreeNode* const node = m_vertexMap.Find(otherVertex->m_incidentVertex);
+		dgInt32 v = node ? node->GetInfo() : -1;
+		if ((v == edge->m_incidentVertex) || (v == edge->m_twin->m_incidentVertex)) {
+			return false;
+		}
+
+		dgBigVector q0 (otherMeshBVH->m_mesh->m_points[otherVertex->m_incidentVertex]);
+		dgBigVector ray_p0 (m_mesh->m_points[edge->m_incidentVertex]);
+		dgBigVector ray_p1 (m_mesh->m_points[edge->m_twin->m_incidentVertex]);
+
+		dgBigVector p1p0 (ray_p1 - ray_p0);
+		dgBigVector q0p0 (q0 - ray_p0);
+
+		dgFloat64 tol = dgFloat64(1.0e-5f);
+		dgFloat64 den = p1p0 % p1p0;        // always >= 0
+		if (den < tol * tol) { 
+			return ((q0p0 % q0p0) < (1.0e-6f));
+		}
+
+		dgFloat64 num = q0p0 % p1p0;        
+		dgFloat64 tolDen = -tol * den;
+		dgFloat64 tolDen1 = den + tol;
+		if ((num < tolDen) || (num > tolDen1)) {
+			return false;
+		}
+
+		dgBigVector r (ray_p0 + p1p0.Scale3 (num / den));
+		dgBigVector q0r (q0 - r);
+		dgFloat64 dist2 (q0r % q0r);
+		return dist2 <= (tol * tol);
+	}
+
+
+	bool RayRayIntersectQuickTest (dgEdge* const edge, const dgBooleanMeshClipper* const otherMeshBVH, dgEdge* const otherEdge) const
+	{
+		dgTree<dgInt32,dgInt32>::dgTreeNode* const node0 = otherMeshBVH->m_vertexMap.Find(otherEdge->m_incidentVertex);
+		dgTree<dgInt32,dgInt32>::dgTreeNode* const node1 = otherMeshBVH->m_vertexMap.Find(otherEdge->m_twin->m_incidentVertex);
+		dgInt32 v0 = node0 ? node0->GetInfo() : -1;
+		dgInt32 v1 = node1 ? node1->GetInfo() : -1;
+
+		if ((v0 == edge->m_incidentVertex) || (v0 == edge->m_twin->m_incidentVertex) || (v1 == edge->m_incidentVertex) || (v1 == edge->m_twin->m_incidentVertex)) {
+			return false;
+		}
+
+		dgBigVector ray_p0 (m_mesh->m_points[edge->m_incidentVertex]);
+		dgBigVector ray_p1 (m_mesh->m_points[edge->m_twin->m_incidentVertex]);
+
+		dgBigVector ray_q0 (otherMeshBVH->m_mesh->m_points[otherEdge->m_incidentVertex]);
+		dgBigVector ray_q1 (otherMeshBVH->m_mesh->m_points[otherEdge->m_twin->m_incidentVertex]);
+
+		dgBigVector p1p0 (ray_p1 - ray_p0);
+		dgBigVector q1q0 (ray_q1 - ray_q0);
+		dgBigVector p0q0 (ray_p0 - ray_q0);
+
+		dgFloat64 tol = dgFloat64(1.0e-5f);
+
+		dgFloat64 a = p1p0 % p1p0;        // always >= 0
+		if ( a < tol * tol) {
+			return true;
+		}
+
+		dgFloat64 c = q1q0 % q1q0;        // always >= 0
+		if ( c < tol * tol) {
+			return true;
+		}
+		dgFloat64 b = p1p0 % q1q0;
+
+		dgFloat64 d = (p1p0 % p0q0);
+		dgFloat64 e = (q1q0 % p0q0);
+		dgFloat64 den = a * c - b * b;   // always >= 0
+		
+
+		// compute the line parameters of the two closest points
+		if (den < tol) { 
+			// almost parallel
+			dgBigVector r = ray_q0 + q1q0.Scale3 (e / c);
+			dgBigVector r1r0 (ray_p0 - r);
+			dgFloat64 dist2 (r1r0 % r1r0);
+			// almost parallel, intersection can not be tested, let the the extend precision routine figure it out 
+			return dist2 <= (tol * tol);
+
+		} else {         
+			// get the closest points on the infinite lines
+			dgFloat64 t = b * e - c * d;
+			dgFloat64 s = a * e - b * d;
+
+			dgFloat64 tolDen = -tol * den;
+			dgFloat64 tolDen1 = den + tol;
+
+			if ((t < tolDen) || (s < tolDen) || (t > tolDen1) || (s > tolDen1)) {
+				return false;
+			}
+
+			dgBigVector r0 = ray_p0 + p1p0.Scale3 (t / den);
+			dgBigVector r1 = ray_q0 + q1q0.Scale3 (s / den);
+			dgBigVector r1r0 (r1 - r0);
+			dgFloat64 dist2 (r1r0 % r1r0);
+			if (dist2 > tol * tol) {
+				return false;
+			}
+		}
+		return true;
+	}
+
+	bool VertexFaceIntersectQuickTest (dgEdge* const vertex, const dgBooleanMeshClipper* const otherMeshBVH, dgEdge* const otherface) const
+	{
+		if (vertex->m_incidentVertex >= m_vertexBase) {
+			return false;
+		}
+		if (m_vertexMap.Find(vertex->m_incidentVertex)) {
+			return false;
+		}
+
+		const dgBigVector* const otherPoints = &otherMeshBVH->m_mesh->m_points[0];
+		dgBigVector normal (otherMeshBVH->m_mesh->FaceNormal (otherface, &otherPoints[0].m_x, sizeof (dgBigVector)));
+		dgFloat64 mag2 = normal % normal;
+		if (mag2 < dgFloat64 (1.0e-12)) {
+			// can not make decision let the extended precision test determine this
+			return true;
+		}
+		normal = normal.Scale3 (1.0 / sqrt (mag2));
+		
+
+		dgBigVector p0 (m_mesh->m_points[vertex->m_incidentVertex]);
+		dgFloat64 dist (normal % (p0 - otherPoints[otherface->m_incidentVertex]));
+		if (fabs(dist) > dgFloat64 (1.0e-4f)) {
+			return false;
+		}
+		
+		dgEdge* ptr = otherface;
+		do {
+			dgInt32 index0 = ptr->m_incidentVertex;
+			dgInt32 index1 = ptr->m_next->m_incidentVertex;
+			dgBigVector p0v0 (otherPoints[index0] - p0);
+			dgBigVector p0v1 (otherPoints[index1] - p0);
+			dgFloat64 alpha = (normal * p0v0) % p0v1;
+			if (alpha < -1.0e-2f) {
+				return false;
+			}
+			
+			ptr = ptr->m_next;
+		} while (ptr != otherface);
+
+		return true;
+	}
+
+
+	bool VertexFaceIntersect (dgEdge* const vertex, const dgBooleanMeshClipper* const otherMeshBVH, dgEdge* const otherface) const
+	{
+		const dgHugeVector* const otherPoints = &otherMeshBVH->m_vertexAlias[0];
+		dgHugeVector normal (otherMeshBVH->FaceNormal (otherface));
+		dgGoogol mag2 = normal % normal;
+		dgAssert ((normal % normal) > m_tol2);
+
+		dgHugeVector p0 (m_vertexAlias[vertex->m_incidentVertex]);
+		dgGoogol dist (normal % (p0 - otherPoints[otherface->m_incidentVertex]));
+		if (dist.Abs() > m_tol) {
+			return false;
+		}
+
+		dgEdge* ptr = otherface;
+		do {
+			dgInt32 index0 = ptr->m_incidentVertex;
+			dgInt32 index1 = ptr->m_next->m_incidentVertex;
+			dgHugeVector p0v0 (otherPoints[index0] - p0);
+			dgHugeVector p0v1 (otherPoints[index1] - p0);
+			dgGoogol alpha ((normal * p0v0) % p0v1);
+			if (alpha <= m_tol2) {
+				return false;
+			}
+
+			ptr = ptr->m_next;
+		} while (ptr != otherface);
+
+		return true;
+	}
+
+
+
+	bool RayVertexIntersect (dgEdge* const edge, const dgBooleanMeshClipper* const otherMeshBVH, dgEdge* const otherVertex, dgGoogol& param) const 
+	{
+		dgHugeVector ray_p0 (m_vertexAlias[edge->m_incidentVertex]);
+		dgHugeVector ray_p1 (m_vertexAlias[edge->m_twin->m_incidentVertex]);
+		dgHugeVector q0 (otherMeshBVH->m_vertexAlias[otherVertex->m_incidentVertex]);
+
+		dgHugeVector p1p0 (ray_p1 - ray_p0);
+		dgHugeVector q0p0 (q0 - ray_p0);
+
+		dgGoogol den = p1p0 % p1p0;        // always >= 0
+		if (den < m_tol2) { 
+			return false;
+		}
+
+		dgGoogol num = q0p0 % p1p0;        
+		param = num / den;
+		if ((param < m_tol) || (param > m_oneMinusTol)) {
+			return false;
+		}
+
+		dgHugeVector r (ray_p0 + p1p0.Scale3 (param));
+		dgHugeVector q0r (q0 - r);
+		dgGoogol dist2 (q0r % q0r);
+		return (dist2 <= m_tol2);
+	}
+
+	bool RayRayIntersect (dgEdge* const edge, const dgBooleanMeshClipper* const otherMeshBVH, dgEdge* const otherEdge, dgGoogol& param, dgGoogol& otherParam) const
+	{
+		dgHugeVector ray_p0 (m_vertexAlias[edge->m_incidentVertex]);
+		dgHugeVector ray_p1 (m_vertexAlias[edge->m_twin->m_incidentVertex]);
+
+		dgHugeVector ray_q0 (otherMeshBVH->m_vertexAlias[otherEdge->m_incidentVertex]);
+		dgHugeVector ray_q1 (otherMeshBVH->m_vertexAlias[otherEdge->m_twin->m_incidentVertex]);
+
+		dgHugeVector p1p0 (ray_p1 - ray_p0);
+		dgHugeVector q1q0 (ray_q1 - ray_q0);
+		dgHugeVector p0q0 (ray_p0 - ray_q0);
+
+		dgGoogol a = p1p0 % p1p0;        // always >= 0
+		dgGoogol c = q1q0 % q1q0;        // always >= 0
+		dgGoogol b = p1p0 % q1q0;
+
+		dgGoogol d = (p1p0 % p0q0);
+		dgGoogol e = (q1q0 % p0q0);
+		dgGoogol den = a * c - b * b;   // always >= 0
+
+		// compute the line parameters of the two closest points
+		if (den < m_tol4) { 
+			// the lines are almost parallel
+			return false;
+		} else {         
+			// get the closest points on the infinite lines
+			dgGoogol t = (b * e - c * d) / den;
+			dgGoogol s = (a * e - b * d) / den;
+
+			if ((t < m_tol) || (s < m_tol) || (t > m_oneMinusTol) || (s > m_oneMinusTol)) {
+				return false;
+			}
+
+			dgHugeVector r0 = ray_p0 + p1p0.Scale3 (t);
+			dgHugeVector r1 = ray_q0 + q1q0.Scale3 (s);
+			dgHugeVector r1r0 (r1 - r0);
+			dgGoogol dist2 (r1r0 % r1r0);
+			if (dist2 > m_tol2) {
+				return false;
+			}
+			param = t;
+			otherParam = s;
+		}
+
+		return true;
+	}
+
+	dgGoogol RayFaceIntersect (const dgMeshBVHNode* const faceNode, const dgHugeVector& p0, const dgHugeVector& p1) const
+	{
+		dgHugeVector normal (FaceNormal(faceNode->m_face));
+		const dgHugeVector* const points = &m_vertexAlias[0];
+	
+	
+		dgHugeVector diff (p1 - p0);
+		dgGoogol tOut (dgGoogol::m_two);
+		dgGoogol dir = normal % diff;
+		if (dir < dgGoogol::m_zero) {
+			dgEdge* ptr = faceNode->m_face;
+			do {
+				dgInt32 index0 = ptr->m_incidentVertex;
+				dgInt32 index1 = ptr->m_next->m_incidentVertex;
+				dgHugeVector p0v0 (points[index0] - p0);
+				dgHugeVector p0v1 (points[index1] - p0);
+				dgGoogol alpha ((diff * p0v1) % p0v0);
+				if (alpha <= m_tol2) {
+					return 1.2f;
+				}
+
+				ptr = ptr->m_next;
+			} while (ptr != faceNode->m_face);
+
+			dgInt32 index0 = ptr->m_incidentVertex;
+			dgHugeVector p0v0 (points[index0] - p0);
+			tOut = normal % p0v0;
+			dgGoogol dist = normal % diff;
+			tOut = tOut / dist;
+
+		} else if (dir > dgGoogol::m_zero) {
+			dgEdge* ptr = faceNode->m_face;
+			do {
+				dgInt32 index0 = ptr->m_incidentVertex;
+				dgInt32 index1 = ptr->m_prev->m_incidentVertex;
+				dgHugeVector p0v0 (points[index0] - p0);
+				dgHugeVector p0v1 (points[index1] - p0);
+				dgGoogol alpha ((diff * p0v1) % p0v0);
+				if (alpha <= m_tol2) {
+					return 1.2f;
+				}
+
+				ptr = ptr->m_prev;
+			} while (ptr != faceNode->m_face);
+
+			dgInt32 index0 = ptr->m_incidentVertex;
+			dgHugeVector p0v0 (points[index0] - p0);
+			tOut = normal % p0v0;
+			dgGoogol dist = normal % diff;
+			tOut = tOut / dist;
+		}
+
+
+		if (tOut < m_tol) {
+			tOut = dgGoogol::m_two;
+		} else if (tOut > (dgGoogol::m_one - m_tol)) {
+			tOut = dgGoogol::m_two;
+		}
+		return tOut;
+	}
+
+	dgMeshBVHNode* FaceRayCast (const dgBooleanMeshClipper* const rayMeshBVH, const dgEdge* const edge, dgGoogol& paramOut) const
+	{
+		dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+
+		dgInt32 stack = 1;
+		dgMeshBVHNode* node = NULL;
+
+		stackPool[0] = m_rootNode;
+		dgGoogol maxParam = dgFloat32 (1.2f);
+
+		dgTree<dgInt32,dgInt32>::dgTreeNode* const node0 = rayMeshBVH->m_vertexMap.Find(edge->m_incidentVertex);
+		dgTree<dgInt32,dgInt32>::dgTreeNode* const node1 = rayMeshBVH->m_vertexMap.Find(edge->m_twin->m_incidentVertex);
+		dgInt32 v0 = node0 ? node0->GetInfo() : -1;
+		dgInt32 v1 = node1 ? node1->GetInfo() : -1;
+
+		const dgMeshEffect* const mesh = rayMeshBVH->m_mesh;
+		const dgBigVector* points = (dgBigVector*)mesh->GetVertexPool();
+		const dgBigVector& q0 = points[edge->m_incidentVertex];
+		const dgBigVector& q1 = points[edge->m_twin->m_incidentVertex];
+
+		const dgHugeVector* const linePoints = &rayMeshBVH->m_vertexAlias[0];
+		dgHugeVector p0 (linePoints[edge->m_incidentVertex]);
+		dgHugeVector p1 (linePoints[edge->m_twin->m_incidentVertex]);
+
+		dgVector l0(dgFloat32 (q0.m_x), dgFloat32 (q0.m_y), dgFloat32 (q0.m_z), dgFloat32 (0.0f));
+		dgVector l1(dgFloat32 (q1.m_x), dgFloat32 (q1.m_y), dgFloat32 (q1.m_z), dgFloat32 (0.0f));
+		dgFastRayTest ray (l0, l1);
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const me = stackPool[stack];
+
+			if (me && ray.BoxTest (me->m_p0, me->m_p1)) {
+
+				if (!me->m_left) {
+					dgAssert (!me->m_right);
+					dgEdge* ptr = me->m_face;
+					do {
+						if ((ptr->m_incidentVertex == v0) || (ptr->m_incidentVertex == v1)) {
+							break;
+						}
+						ptr = ptr->m_next;
+					} while (ptr != me->m_face);
+					if (ptr == me->m_face) {
+						dgGoogol param = RayFaceIntersect (me, p0, p1);
+						if (param < maxParam) {
+							node = me;
+							maxParam = param;
+							ray.Reset (dgFloat32 (maxParam) + dgFloat32(0.01f));
+						}
+					}
+
+				} else {
+					dgAssert (me->m_left);
+					dgAssert (me->m_right);
+					stackPool[stack] = me->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack] = me->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				}
+			}
+		}
+
+		paramOut = maxParam;
+		return node;
+	}
+
+	void InsertFaceVertex (dgEdge* const face, const dgHugeVector& point)
+	{
+		dgMeshEffect::dgVertexAtribute attribute;
+		memset (&attribute, 0, sizeof (attribute));
+
+		dgGoogol tol(m_tol);
+		const dgMeshEffect::dgVertexAtribute* const attrib = (dgMeshEffect::dgVertexAtribute*)m_mesh->GetAttributePool(); 
+		for (dgInt32 i = 0; i < 4; i ++) {
+
+			dgGoogol posTol (tol + dgGoogol::m_one);
+			dgGoogol negTol (dgGoogol::m_zero - m_tol);
+
+			dgEdge* ptr = face;
+			dgEdge* const edge0 = ptr;
+			dgHugeVector q0 (m_vertexAlias[ptr->m_incidentVertex]);
+
+			ptr = ptr->m_next;
+			const dgEdge* edge1 = ptr;
+			dgHugeVector q1 (m_vertexAlias[ptr->m_incidentVertex]);
+
+			ptr = ptr->m_next;
+			const dgEdge* edge2 = ptr;
+			do {
+				const dgHugeVector& q2 = m_vertexAlias[ptr->m_incidentVertex];
+
+				dgHugeVector p10 (q1 - q0);
+				dgHugeVector p20 (q2 - q0);
+
+				dgGoogol dot = p20 % p10;
+				dgGoogol mag1 = p10 % p10;
+				dgGoogol mag2 = p20 % p20;
+				dgGoogol collinear = dot * dot - mag2 * mag1;
+				if (collinear.Abs() > m_tol) {
+					dgHugeVector p_p0 (point - q0);
+					dgHugeVector p_p1 (point - q1);
+					dgHugeVector p_p2 (point - q2);
+
+					dgGoogol alpha1 = p10 % p_p0;
+					dgGoogol alpha2 = p20 % p_p0;
+					dgGoogol alpha3 = p10 % p_p1;
+					dgGoogol alpha4 = p20 % p_p1;
+					dgGoogol alpha5 = p10 % p_p2;
+					dgGoogol alpha6 = p20 % p_p2;
+
+					dgGoogol vc = alpha1 * alpha4 - alpha3 * alpha2;
+					dgGoogol vb = alpha5 * alpha2 - alpha1 * alpha6;
+					dgGoogol va = alpha3 * alpha6 - alpha5 * alpha4;
+					dgGoogol den = va + vb + vc;
+					dgGoogol minError = den * negTol;
+					dgGoogol maxError = den * posTol;
+
+					if ((va > minError) && (vb > minError) && (vc > minError) && (va < maxError) && (vb < maxError) && (vc < maxError)) {
+
+						edge2 = ptr;
+
+						dgGoogol alpha0 = va / den;
+						dgGoogol alpha1 = vb / den;
+						dgGoogol alpha2 = vc / den;
+
+						dgFloat64 falpha0 = alpha0;
+						dgFloat64 falpha1 = alpha1;
+						dgFloat64 falpha2 = alpha2;
+
+						const dgMeshEffect::dgVertexAtribute& attr0 = attrib[edge0->m_userData];
+						const dgMeshEffect::dgVertexAtribute& attr1 = attrib[edge1->m_userData];
+						const dgMeshEffect::dgVertexAtribute& attr2 = attrib[edge2->m_userData];
+						dgBigVector normal (attr0.m_normal_x * falpha0 + attr1.m_normal_x * falpha1 + attr2.m_normal_x * falpha2,
+											attr0.m_normal_y * falpha0 + attr1.m_normal_y * falpha1 + attr2.m_normal_y * falpha2,
+											attr0.m_normal_z * falpha0 + attr1.m_normal_z * falpha1 + attr2.m_normal_z * falpha2, dgFloat32 (0.0f));
+						normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (normal % normal));
+
+
+						attribute.m_vertex.m_x = point.m_x;
+						attribute.m_vertex.m_y = point.m_y;
+						attribute.m_vertex.m_z = point.m_z;
+						attribute.m_vertex.m_w = point.m_w;
+						attribute.m_normal_x = normal.m_x;
+						attribute.m_normal_y = normal.m_y;
+						attribute.m_normal_z = normal.m_z;
+						attribute.m_u0 = attr0.m_u0 * falpha0 +  attr1.m_u0 * falpha1 + attr2.m_u0 * falpha2;
+						attribute.m_v0 = attr0.m_v0 * falpha0 +  attr1.m_v0 * falpha1 + attr2.m_v0 * falpha2;
+						attribute.m_u1 = attr0.m_u1 * falpha0 +  attr1.m_u1 * falpha1 + attr2.m_u1 * falpha2;
+						attribute.m_v1 = attr0.m_v1 * falpha0 +  attr1.m_v1 * falpha1 + attr2.m_v1 * falpha2;
+
+						attribute.m_material = attr0.m_material;
+						dgAssert (attr0.m_material == attr1.m_material);
+						dgAssert (attr0.m_material == attr2.m_material);
+
+						m_mesh->AddPoint(&attribute.m_vertex.m_x, dgInt32 (attribute.m_material)); 
+						m_vertexAlias[m_mesh->GetVertexCount()-1] = point;
+						return;
+					}
+				}
+
+				q1 = q2;
+				edge1 = ptr;
+
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			dgAssert (0);
+			tol = tol * dgGoogol::m_two;
+		}
+		// this should never happens
+		dgAssert (0);
+	}
+
+
+	void SpliteEdge(dgEdge* const edge, dgGoogol param, dgEdge** edge0, dgEdge** edge1)
+	{
+		dgInt32 v0 = edge->m_incidentVertex;
+		dgInt32 v1 = edge->m_twin->m_incidentVertex;
+		dgPolyhedra::dgPairKey edgeKey (v0, v1);
+		dgPolyhedra::dgPairKey twinKey (v1, v0);
+
+		dgEdge* const newEdge = m_mesh->InsertEdgeVertex (edge, param);
+		dgGoogol t0(dgGoogol::m_one - param);
+		dgGoogol t1(param);
+		dgInt32 v01 = newEdge->m_twin->m_incidentVertex;
+		m_vertexAlias[v01] = m_vertexAlias[v0].Scale3 (t0) + m_vertexAlias[v1].Scale3 (t1);
+
+		dgTree<dgMeshBVHNode*, dgUnsigned64>::dgTreeNode* const mapNode = m_nodeEdgeMap.Find(edgeKey.GetVal());
+		if (mapNode) {
+			dgMeshBVHNode* const node = mapNode->GetInfo();
+			node->m_face = newEdge;
+			m_nodeEdgeMap.Remove(mapNode);
+			dgPolyhedra::dgPairKey key (newEdge->m_incidentVertex, newEdge->m_twin->m_incidentVertex);
+			m_nodeEdgeMap.Insert(node, key.GetVal());
+		}
+		
+		dgTree<dgMeshBVHNode*, dgUnsigned64>::dgTreeNode* const twinMapNode = m_nodeEdgeMap.Find(twinKey.GetVal());
+		if (twinMapNode) {
+			dgMeshBVHNode* const node = twinMapNode->GetInfo();
+			node->m_face = newEdge->m_twin;
+			m_nodeEdgeMap.Remove(twinMapNode);
+			dgPolyhedra::dgPairKey key (newEdge->m_twin->m_incidentVertex, newEdge->m_incidentVertex);
+			m_nodeEdgeMap.Insert(node, key.GetVal());
+		}
+
+		*edge0 = newEdge;
+		*edge1 = newEdge->m_next;
+
+		dgAssert (SanityCheck());
+	}
+
+	bool CalculateEdgeVertexIntersetions (dgBooleanMeshClipper& otherVertexMeshBVH)
+	{
+		dgMeshBVHNode* stackPool[4 * DG_MESH_EFFECT_BVH_STACK_DEPTH][2];
+
+		dgInt32 stack = 1;
+		stackPool[0][0] = m_rootNode;
+		stackPool[0][1] = otherVertexMeshBVH.m_rootNode;
+
+		dgList<dgEdge*> potencialEdge (m_mesh->GetAllocator());
+		dgList<dgEdge*> otherPotencialVertex (m_mesh->GetAllocator());
+		dgInt32 mark = dgMax(m_mesh->IncLRU(), otherVertexMeshBVH.m_mesh->IncLRU());
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const me = stackPool[stack][0];
+			dgMeshBVHNode* const other = stackPool[stack][1];
+
+			dgAssert (me && other);
+			if (dgOverlapTest (me->m_p0, me->m_p1, other->m_p0, other->m_p1)) {
+
+				if (!me->m_left && !other->m_left) {
+					dgAssert (!me->m_right);
+					dgAssert (!other->m_right);
+
+					dgEdge* myPtr = me->m_face;
+					do {
+						dgEdge* otherPtr = other->m_face;
+						do {
+							if ((myPtr->m_mark != mark) || (otherPtr->m_mark != mark)) {
+				
+								if (RayVertexIntersectQuickTest(myPtr, &otherVertexMeshBVH, otherPtr)) {
+									if (myPtr->m_mark != mark) {
+										potencialEdge.Append(myPtr);
+									}
+
+									if (otherPtr->m_mark != mark) {
+										otherPotencialVertex.Append(otherPtr);
+									}
+
+									myPtr->m_mark = mark;
+									myPtr->m_twin->m_mark = mark;
+
+									dgEdge* ptr = otherPtr;
+									do {
+										ptr->m_mark = mark;
+										ptr = ptr->m_twin->m_next;
+
+									} while (ptr != otherPtr);
+								}
+							}
+							otherPtr = otherPtr->m_next;
+						} while (otherPtr != other->m_face);
+						myPtr = myPtr->m_next;
+					} while (myPtr != me->m_face);
+
+
+				} else if (!me->m_left) {
+					dgAssert (other->m_left);
+					dgAssert (other->m_right);
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				} else if (!other->m_right) {
+					dgAssert (me->m_left);
+					dgAssert (me->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				} else {
+					dgAssert (me->m_left && me->m_right);
+					dgAssert (other->m_left && other->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				}
+			}
+		}
+
+
+		bool intersectionFound = false;
+		while (potencialEdge.GetFirst()) {
+			dgEdge* const edge = potencialEdge.GetFirst()->GetInfo();
+			potencialEdge.Remove(potencialEdge.GetFirst());
+			for (dgList<dgEdge*>::dgListNode* otherNode = otherPotencialVertex.GetFirst(); otherNode; otherNode = otherNode->GetNext()) {
+				dgEdge* const otherVertexEdge = otherNode->GetInfo();
+
+				if (RayVertexIntersectQuickTest(edge, &otherVertexMeshBVH, otherVertexEdge)) {
+					dgGoogol param;
+					if (RayVertexIntersect (edge, &otherVertexMeshBVH, otherVertexEdge, param)) {
+
+						intersectionFound = true;
+
+						dgEdge* tmp0;
+						dgEdge* tmp1;
+						SpliteEdge (edge, param, &tmp0, &tmp1);
+						m_vertexAlias[m_mesh->GetVertexCount()-1] = otherVertexMeshBVH.m_vertexAlias[otherVertexEdge->m_incidentVertex];
+						potencialEdge.Addtop(tmp0);
+						potencialEdge.Addtop(tmp1);
+						m_vertexMap.Insert(otherVertexEdge->m_incidentVertex, m_mesh->GetVertexCount() - 1);
+
+						otherVertexMeshBVH.m_vertexMap.Insert(m_mesh->GetVertexCount() - 1, otherVertexEdge->m_incidentVertex);
+						break;
+					}
+				}
+			}
+		}
+		return intersectionFound;
+	}
+
+	bool CalculateEdgeEdgeIntersetions (dgBooleanMeshClipper& otherEdgeBVH)
+	{
+		dgMeshBVHNode* stackPool[4 * DG_MESH_EFFECT_BVH_STACK_DEPTH][2];
+
+		dgInt32 stack = 1;
+		stackPool[0][0] = m_rootNode;
+		stackPool[0][1] = otherEdgeBVH.m_rootNode;
+
+		dgList<dgEdge*> potencialEdge (m_mesh->GetAllocator());
+		dgList<dgEdge*> otherPotencialEdge (m_mesh->GetAllocator());
+		dgInt32 mark = dgMax(m_mesh->IncLRU(), otherEdgeBVH.m_mesh->IncLRU());
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const me = stackPool[stack][0];
+			dgMeshBVHNode* const other = stackPool[stack][1];
+
+			dgAssert (me && other);
+			if (dgOverlapTest (me->m_p0, me->m_p1, other->m_p0, other->m_p1)) {
+
+				if (!me->m_left && !other->m_left) {
+					dgAssert (!me->m_right);
+					dgAssert (!other->m_right);
+
+					dgEdge* myPtr = me->m_face;
+					do {
+						dgEdge* otherPtr = other->m_face;
+						do {
+							if ((myPtr->m_mark != mark) || (otherPtr->m_mark != mark)) {
+								if (RayRayIntersectQuickTest(myPtr, &otherEdgeBVH, otherPtr)) {
+									if (myPtr->m_mark != mark) {
+										potencialEdge.Append(myPtr);
+									}
+									if (otherPtr->m_mark != mark) {
+										otherPotencialEdge.Append(otherPtr);
+									}
+
+									myPtr->m_mark = mark;
+									myPtr->m_twin->m_mark = mark;
+									otherPtr->m_mark = mark;
+									otherPtr->m_twin->m_mark = mark;
+								}
+							}
+							otherPtr = otherPtr->m_next;
+						} while (otherPtr != other->m_face);
+						myPtr = myPtr->m_next;
+					} while (myPtr != me->m_face);
+
+
+				} else if (!me->m_left) {
+					dgAssert (other->m_left);
+					dgAssert (other->m_right);
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				} else if (!other->m_right) {
+					dgAssert (me->m_left);
+					dgAssert (me->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				} else {
+					dgAssert (me->m_left && me->m_right);
+					dgAssert (other->m_left && other->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				}
+			}
+		}
+
+		bool intersectionFound = false;
+		while (potencialEdge.GetFirst()) {
+			dgEdge* const edge = potencialEdge.GetFirst()->GetInfo();
+			potencialEdge.Remove(potencialEdge.GetFirst());
+			for (dgList<dgEdge*>::dgListNode* otherNode = otherPotencialEdge.GetFirst(); otherNode; otherNode = otherNode->GetNext()) {
+				dgEdge* const otherEdge = otherNode->GetInfo();
+				if (RayRayIntersectQuickTest(edge, &otherEdgeBVH, otherEdge)) {
+
+					dgGoogol param; 
+					dgGoogol otherParam;
+					if (RayRayIntersect (edge, &otherEdgeBVH, otherEdge, param, otherParam)) {
+
+						intersectionFound = true;
+
+						dgEdge* tmp0;
+						dgEdge* tmp1;
+						SpliteEdge (edge, param, &tmp0, &tmp1);
+						potencialEdge.Addtop(tmp0);
+						potencialEdge.Addtop(tmp1);
+
+						otherEdgeBVH.SpliteEdge (otherEdge, otherParam, &tmp0, &tmp1);
+						otherPotencialEdge.Remove(otherNode);
+						otherPotencialEdge.Addtop(tmp0);
+						otherPotencialEdge.Addtop(tmp1);
+
+						m_vertexMap.Insert(otherEdgeBVH.m_mesh->GetVertexCount() - 1, m_mesh->GetVertexCount() - 1);
+						otherEdgeBVH.m_vertexMap.Insert(m_mesh->GetVertexCount() - 1, otherEdgeBVH.m_mesh->GetVertexCount() - 1);
+						break;
+					}
+				}
+			}
+		}
+		return intersectionFound;
+	}
+
+
+	void InsertFaceVertex (dgMeshBVH::dgMeshBVHNode* const nodeFace, const dgHugeVector& point) 
+	{
+		InsertFaceVertex (nodeFace->m_face, point);
+
+		dgInt32 attibIndex = m_mesh->GetPropertiesCount() - 1;
+		dgInt32 vertexIndex = m_mesh->GetVertexCount() - 1;
+
+		dgInt32 count = 0;
+		dgEdge* perimeter[256];
+
+		dgEdge* ptr = nodeFace->m_face;
+		do {
+			perimeter[count] = ptr;
+			count ++;
+			ptr = ptr->m_next;
+		} while (ptr != nodeFace->m_face);
+
+		for(dgInt32 i = 0; i < count; i ++) {
+			dgEdge* const perimeterFace = perimeter[i];
+			dgEdge* const edge = m_mesh->AddHalfEdge(vertexIndex, perimeterFace->m_incidentVertex);
+			dgEdge* const twin = m_mesh->AddHalfEdge(perimeterFace->m_incidentVertex, vertexIndex);
+
+			edge->m_incidentFace = perimeterFace->m_incidentFace;
+			twin->m_incidentFace = perimeterFace->m_incidentFace;
+
+			edge->m_userData = attibIndex;
+			twin->m_userData = perimeterFace->m_userData;
+
+			twin->m_twin = edge;
+			edge->m_twin = twin;
+
+			perimeterFace->m_prev->m_next = twin;
+			twin->m_prev = perimeterFace->m_prev;
+
+			perimeterFace->m_prev = edge;
+			edge->m_next = perimeterFace;
+		}
+
+		dgPolyhedra::dgPairKey key(nodeFace->m_face->m_incidentVertex, nodeFace->m_face->m_twin->m_incidentVertex);
+		dgAssert (m_nodeEdgeMap.Find(key.GetVal()));
+		m_nodeEdgeMap.Remove(key.GetVal());
+		RemoveNode (nodeFace);
+
+		for(dgInt32 i = 0; i < count; i ++) {
+			dgEdge* const perimeterFace = perimeter[i];
+			perimeterFace->m_next->m_next = perimeterFace->m_prev;
+			perimeterFace->m_prev->m_prev = perimeterFace->m_next;
+
+			dgMeshBVHNode* const newNode = AddNode (perimeterFace->m_next);
+			dgPolyhedra::dgPairKey key(newNode->m_face->m_incidentVertex, newNode->m_face->m_twin->m_incidentVertex);
+			m_nodeEdgeMap.Insert(newNode, key.GetVal());
+		}
+	}
+
+
+	dgEdge* EdgeFaceIntersetion (dgEdge* const edgeSegment, dgBooleanMeshClipper& faceMeshBVH)
+	{
+		dgGoogol param;
+		dgEdge* edge = NULL;
+		dgMeshBVH::dgMeshBVHNode* const node = faceMeshBVH.FaceRayCast (this, edgeSegment, param);
+		if (node) {
+			// clip the line edge
+			dgAssert (param > dgGoogol::m_zero);
+			dgAssert (param < dgGoogol::m_one);
+			dgEdge* next;
+			SpliteEdge (edgeSegment, param, &edge, &next);
+			dgAssert (!edge || edge->m_next == next);
+
+			// tessellate the face
+			faceMeshBVH.InsertFaceVertex (node, m_vertexAlias[m_mesh->GetVertexCount()-1]) ;
+
+			m_vertexMap.Insert(faceMeshBVH.m_mesh->GetVertexCount() - 1, m_mesh->GetVertexCount() - 1);
+			faceMeshBVH.m_vertexMap.Insert(m_mesh->GetVertexCount() - 1, faceMeshBVH.m_mesh->GetVertexCount() - 1);
+		}
+		return edge;
+	}
+
+
+	bool CalculateEdgeFacesIntersetions (dgBooleanMeshClipper& faceMeshBVH)
+	{
+		dgList<dgEdge*> edgeList (m_mesh->GetAllocator()) ;
+
+		for (void* edgeNode = m_mesh->GetfirstClipperEdge(); edgeNode; edgeNode = m_mesh->GetNextEdge(edgeNode)) {
+			dgEdge* const edge = &((dgMeshEffect::dgTreeNode*)edgeNode)->GetInfo();
+			edgeList.Append(edge);
+		}
+
+
+		bool intersectionFound = false;
+		while (edgeList.GetCount()) {
+			dgEdge* const edgeSegment = edgeList.GetFirst()->GetInfo();
+			edgeList.Remove(edgeList.GetFirst());
+
+			dgEdge* const edge = EdgeFaceIntersetion (edgeSegment, faceMeshBVH);
+			if (edge) {
+				intersectionFound = true;
+				edgeList.Addtop(edge);
+				edgeList.Addtop(edge->m_next);
+			}
+		}
+		if (intersectionFound) {
+			faceMeshBVH.ImproveNodeFitness();
+			dgAssert (faceMeshBVH.SanityCheck());
+		}
+		return intersectionFound;
+	}
+
+
+	bool CalculateVertexFacesIntersetions (dgBooleanMeshClipper& faceMeshBVH)
+	{
+		dgMeshBVHNode* stackPool[4 * DG_MESH_EFFECT_BVH_STACK_DEPTH][2];
+
+		dgInt32 stack = 1;
+		stackPool[0][0] = m_rootNode;
+		stackPool[0][1] = faceMeshBVH.m_rootNode;
+
+		dgList<dgFaceVertexPair> collidingPairs (m_mesh->GetAllocator());
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const me = stackPool[stack][0];
+			dgMeshBVHNode* const other = stackPool[stack][1];
+
+			dgAssert (me && other);
+			if (dgOverlapTest (me->m_p0, me->m_p1, other->m_p0, other->m_p1)) {
+
+				if (!me->m_left && !other->m_left) {
+					dgAssert (!me->m_right);
+					dgAssert (!other->m_right);
+
+					dgEdge* vertexEdge = me->m_face;
+					do {
+						if (VertexFaceIntersectQuickTest (vertexEdge, &faceMeshBVH, other->m_face)) {
+							bool found = false;
+							for (dgList<dgFaceVertexPair>::dgListNode* node = collidingPairs.GetFirst(); node; node = node->GetNext()) {
+								const dgFaceVertexPair& pair = node->GetInfo();
+								if ((other == pair.m_faceNode) && (pair.m_vertex->m_incidentVertex == vertexEdge->m_incidentVertex)) { 
+									found = true;
+									break;
+								}
+							}
+							if (!found) {
+								dgFaceVertexPair pair (vertexEdge, other);
+								collidingPairs.Append(pair);
+							}
+						}
+						vertexEdge = vertexEdge->m_next;
+					} while (vertexEdge != me->m_face);
+	
+				} else if (!me->m_left) {
+					dgAssert (other->m_left);
+					dgAssert (other->m_right);
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				} else if (!other->m_right) {
+					dgAssert (me->m_left);
+					dgAssert (me->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				} else {
+					dgAssert (me->m_left && me->m_right);
+					dgAssert (other->m_left && other->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				}
+			}
+		}
+
+		bool intersectionFound = false;
+		while (collidingPairs.GetFirst()) {
+			dgFaceVertexPair pair (collidingPairs.GetFirst()->GetInfo());
+			collidingPairs.Remove(collidingPairs.GetFirst());
+
+			if (VertexFaceIntersect (pair.m_vertex, &faceMeshBVH, pair.m_faceNode->m_face)) {
+				dgList<dgFaceVertexPair>::dgListNode* nextNode;
+				for (dgList<dgFaceVertexPair>::dgListNode* node = collidingPairs.GetFirst(); node; node = nextNode) {
+					nextNode = node->GetNext();
+
+					const dgFaceVertexPair& conflictingPair = node->GetInfo();
+					if ((pair.m_faceNode == conflictingPair.m_faceNode) || (pair.m_vertex->m_incidentVertex == conflictingPair.m_vertex->m_incidentVertex)) { 
+						collidingPairs.Remove(node);
+					}
+				}
+
+				intersectionFound = true;
+				faceMeshBVH.InsertFaceVertex (pair.m_faceNode, m_vertexAlias[pair.m_vertex->m_incidentVertex]) ;
+				m_vertexMap.Insert(faceMeshBVH.m_mesh->GetVertexCount() - 1, pair.m_vertex->m_incidentVertex);
+				faceMeshBVH.m_vertexMap.Insert(pair.m_vertex->m_incidentVertex, faceMeshBVH.m_mesh->GetVertexCount() - 1);
+			}
+		}
+
+		if (intersectionFound) {
+			faceMeshBVH.ImproveNodeFitness();
+			dgAssert (faceMeshBVH.SanityCheck());
+		}
+		return intersectionFound;
+	}
+
+
+	dgEdge* FindMatchEdge (dgEdge* const vertex, const dgBooleanMeshClipper& otherMeshBVH) const
+	{
+		dgInt32 vA0 = vertex->m_incidentVertex;
+		dgAssert (m_vertexMap.Find (vA0));
+		dgAssert (otherMeshBVH.m_vertexMap.Find(m_vertexMap.Find (vA0)->GetInfo()));
+		dgAssert (m_vertexMap.Find (vA0)->GetInfo() == otherMeshBVH.m_vertexMap.Find(m_vertexMap.Find (vA0)->GetInfo())->GetKey());
+
+		dgInt32 vB0 = m_vertexMap.Find (vA0)->GetInfo();
+		dgEdge* ptr = vertex;
+		do {
+			dgTree<dgInt32,dgInt32>::dgTreeNode* const node = m_vertexMap.Find(ptr->m_twin->m_incidentVertex);
+			if (node) {
+				dgInt32 vA1 = node->GetInfo();
+				dgAssert (otherMeshBVH.m_vertexMap.Find(vA1));
+				dgInt32 vB1 = otherMeshBVH.m_vertexMap.Find (vA1)->GetInfo();
+				if (otherMeshBVH.m_mesh->FindEdge(vB0, vB1)) {
+					return ptr;
+				}
+			}
+			ptr = ptr->m_twin->m_next;
+		} while (ptr != vertex);
+
+		dgAssert (0);
+		return NULL;
+	}
+
+	bool ConnectExistingEdges (dgBooleanMeshClipper* const otherMesh) const
+	{
+		bool foundEdge = false;
+		dgTree<dgInt32,dgInt32>::Iterator iter (m_vertexMap);
+		for (iter.Begin(); iter; iter ++) {
+			dgInt32 vA0 = iter.GetKey();
+			dgInt32 vB0 = iter.GetNode()->GetInfo();
+			dgAssert (otherMesh->m_vertexMap.Find(vB0));
+
+			dgPolyhedra::dgPairKey key (vA0, 0);
+			dgAssert (m_mesh->FindGreaterEqual(key.GetVal()));
+
+			dgEdge* const edgeA0 = &m_mesh->FindGreaterEqual(key.GetVal())->GetInfo();
+			dgEdge* ptr = edgeA0;
+			do {
+				dgInt32 vA1 = ptr->m_twin->m_incidentVertex;
+				dgTree<dgInt32,dgInt32>::dgTreeNode* const node = m_vertexMap.Find(vA1);
+				if (node) {
+					dgInt32 vB1 = node->GetInfo();
+					const dgEdge* const otherEdge = otherMesh->m_mesh->FindEdge(vB1, vB0);
+					if (!otherEdge) {
+						dgEdge* const ret = otherMesh->m_mesh->SpliteFace (vB1, vB0);
+						foundEdge |= ret ? true : false;
+					}
+				}
+			
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != edgeA0); 
+		}
+		return foundEdge;
+	}
+
+	void CalculateNewEdgeIntestions (dgEdge* const edgeSegment, dgBooleanMeshClipper* const otherMeshBVH)
+	{
+		dgEdge* const edge = EdgeFaceIntersetion (edgeSegment, *otherMeshBVH);
+		if (edge) {
+			dgEdge* const next = edge->m_next; 
+			CalculateNewEdgeIntestions (edge, otherMeshBVH);
+			CalculateNewEdgeIntestions (next, otherMeshBVH);
+			return;
+		}
+
+		const dgHugeVector& q0 = m_vertexAlias[edgeSegment->m_incidentVertex];
+		const dgHugeVector& q1 = m_vertexAlias[edgeSegment->m_twin->m_incidentVertex];
+
+		dgBigVector l0 (q0.m_x, q0.m_y, q0.m_z, 0.0);
+		dgBigVector l1 (q1.m_x, q1.m_y, q1.m_z, 0.0);
+		dgBigVector p0 (dgMin (l0.m_x, l1.m_x) - 0.1, dgMin (l0.m_y, l1.m_y) - 0.1, dgMin (l0.m_z, l1.m_z) - 0.1, 0.0);
+		dgBigVector p1 (dgMax (l0.m_x, l1.m_x) + 0.1, dgMax (l0.m_y, l1.m_y) + 0.1, dgMin (l0.m_z, l1.m_z) + 0.1, 0.0);
+
+		dgList<dgMeshBVHNode*> nodeList (m_mesh->GetAllocator());
+		otherMeshBVH->GetOverlapNodes (nodeList, p0, p1);
+
+		for (dgList<dgMeshBVHNode*>::dgListNode* nodePtr = nodeList.GetFirst(); nodePtr; nodePtr = nodePtr->GetNext()) {
+			dgMeshBVHNode* const node = nodePtr->GetInfo();
+
+			dgEdge* const otherEdge = node->m_face; 
+			dgEdge* ptr = otherEdge; 
+			do {
+				if (RayRayIntersectQuickTest(edgeSegment, otherMeshBVH, ptr)) {
+
+					dgGoogol param; 
+					dgGoogol otherParam;
+					if (RayRayIntersect (edgeSegment, otherMeshBVH, ptr, param, otherParam)) {
+						dgAssert (0);
+/*
+						dgEdge* tmp0;
+						dgEdge* tmp1;
+						SpliteEdge (edge, param, &tmp0, &tmp1);
+						potencialEdge.Addtop(tmp0);
+						potencialEdge.Addtop(tmp1);
+
+						otherEdgeBVH.SpliteEdge (otherEdge, otherParam, &tmp0, &tmp1);
+						otherPotencialEdge.Remove(otherNode);
+						otherPotencialEdge.Addtop(tmp0);
+						otherPotencialEdge.Addtop(tmp1);
+
+						m_vertexMap.Insert(otherEdgeBVH.m_mesh->GetVertexCount() - 1, m_mesh->GetVertexCount() - 1);
+						otherEdgeBVH.m_vertexMap.Insert(m_mesh->GetVertexCount() - 1, otherEdgeBVH.m_mesh->GetVertexCount() - 1);
+*/
+						return;
+					}
+				}
+
+				ptr = ptr->m_next;
+			} while (ptr != otherEdge);
+		}
+
+		for (dgList<dgMeshBVHNode*>::dgListNode* nodePtr = nodeList.GetFirst(); nodePtr; nodePtr = nodePtr->GetNext()) {
+			dgMeshBVHNode* const node = nodePtr->GetInfo();
+
+			dgEdge* const otherEdge = node->m_face; 
+			dgEdge* ptr = otherEdge; 
+			do {
+
+				if (RayVertexIntersectQuickTest(edgeSegment, otherMeshBVH, ptr)) {
+					dgGoogol param;
+					if (RayVertexIntersect (edgeSegment, otherMeshBVH, ptr, param)) {
+						dgEdge* edge;
+						dgEdge* next;
+						SpliteEdge (edgeSegment, param, &edge, &next);
+						m_vertexAlias[m_mesh->GetVertexCount()-1] = otherMeshBVH->m_vertexAlias[ptr->m_incidentVertex];
+
+						m_vertexMap.Insert(ptr->m_incidentVertex, m_mesh->GetVertexCount() - 1);
+						otherMeshBVH->m_vertexMap.Insert(m_mesh->GetVertexCount() - 1, ptr->m_incidentVertex);
+
+						nodeList.RemoveAll();
+						CalculateNewEdgeIntestions (edge, otherMeshBVH);
+						CalculateNewEdgeIntestions (next, otherMeshBVH);
+						return;
+					}
+				}
+				ptr = ptr->m_next;
+			} while (ptr != otherEdge);
+		}
+		return;
+	}
+
+/*
+	bool GetFirtAndLastFan (const dgEdge* const edge, dgEdge** const firstClipperEdge, dgEdge** const lastEdge) const
+	{
+		dgEdge* first = edge->m_next->m_next;
+		dgEdge* last = edge->m_prev;
+
+		const dgHugeVector& p0 = m_vertexAlias[edge->m_incidentVertex];
+		const dgHugeVector& p1 = m_vertexAlias[edge->m_next->m_incidentVertex];
+		dgHugeVector p1p0 (p1 - p0);
+		dgGoogol p1p0Mag2 (p1p0 % p1p0);
+		while (first != last) {
+			const dgHugeVector& p2 = m_vertexAlias[first->m_incidentVertex];
+			dgHugeVector p2p0 (p2 - p0);
+			dgGoogol p2p0Mag2 (p2p0 % p2p0);
+			dgGoogol dot (p1p0 % p2p0);
+			dgGoogol error (p2p0Mag2 * p1p0Mag2 -  dot * dot);
+			if (error > m_tol2) {
+				dgAssert (first != last);
+				const dgHugeVector& p1 = m_vertexAlias[last->m_incidentVertex];
+				dgHugeVector p1p0 (p1 - p0);
+				dgGoogol p1p0Mag2 (p1p0 % p1p0);
+				do {
+					const dgHugeVector& p2 = m_vertexAlias[last->m_prev->m_incidentVertex];
+					dgHugeVector p2p0 (p2 - p0);
+					dgGoogol p2p0Mag2 (p2p0 % p2p0);
+					dgGoogol dot (p1p0 % p2p0);
+					dgGoogol error (p2p0Mag2 * p1p0Mag2 -  dot * dot);
+					if (error > m_tol2) {
+						*firstClipperEdge = first;
+						*lastEdge = last;
+						return true;
+					}
+					last = last->m_prev;
+				} while (last != first);
+				break;
+			}
+			first = first->m_next;
+		};
+
+		return false;
+	}
+*/
+
+	dgGoogol ColinealError (dgEdge* const edge) const
+	{
+		const dgHugeVector& p0 = m_vertexAlias[edge->m_incidentVertex];
+		const dgHugeVector& p1 = m_vertexAlias[edge->m_next->m_incidentVertex];
+		const dgHugeVector& p2 = m_vertexAlias[edge->m_next->m_next->m_incidentVertex];
+		dgHugeVector p1p0 (p1 - p0);
+		dgHugeVector p2p0 (p2 - p0);
+		dgGoogol p1p0Mag2 (p1p0 % p1p0);
+		dgGoogol p2p0Mag2 (p2p0 % p2p0);
+		dgGoogol dot (p1p0 % p2p0);
+		return p2p0Mag2 * p1p0Mag2 -  dot * dot;
+	}
+
+	dgEdge* SpliteFace (dgInt32 v1, dgInt32 v0)
+	{
+		dgEdge* const edge = m_mesh->SpliteFace(v1, v0);
+		if (edge) {
+			dgGoogol error0 (ColinealError (edge));
+			dgGoogol error1 (ColinealError (edge->m_twin));
+			if ((error0 < m_tol2) || (error1 < m_tol2)) {
+				m_mesh->DeleteEdge(edge);
+				return NULL;
+			}
+		}
+
+		return edge;
+	}
+
+	bool ConnectMissingEdges (dgBooleanMeshClipper* const otherMeshBVH)
+	{
+		bool foundEdge = false;
+		dgTree<dgInt32,dgInt32>::Iterator iter (m_vertexMap);
+		for (iter.Begin(); iter; iter ++) {
+			dgInt32 vA0 = iter.GetKey();
+			dgInt32 vB0 = iter.GetNode()->GetInfo();
+			dgAssert (otherMeshBVH->m_vertexMap.Find(vB0));
+
+			dgPolyhedra::dgPairKey key (vA0, 0);
+			dgAssert (m_mesh->FindGreaterEqual(key.GetVal()));
+
+			dgEdge* const edgeA0 = &m_mesh->FindGreaterEqual(key.GetVal())->GetInfo();
+			dgEdge* ptr = edgeA0;
+			do {
+				if (ptr->m_incidentFace > 0) {
+					for (dgEdge* edgeA1 = ptr->m_next->m_next; edgeA1 != ptr->m_prev; edgeA1 = edgeA1->m_next) {
+						dgInt32 vA1 = edgeA1->m_incidentVertex;
+						if (m_vertexMap.Find(vA1)) {
+							if (!m_mesh->FindEdge(vA1, vA0)) {
+								dgTree<dgInt32,dgInt32>::dgTreeNode* const node = m_vertexMap.Find(vA1);
+								if (node) {
+									dgInt32 vB1 = node->GetInfo();
+									dgEdge* const newOtherEdge = otherMeshBVH->SpliteFace(vB1, vB0);
+									if (newOtherEdge) {
+										foundEdge = true;
+										otherMeshBVH->CalculateNewEdgeIntestions (newOtherEdge, this);
+									}
+								}
+							}
+						}
+					}
+				}
+			
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != edgeA0); 
+		}
+		return foundEdge;
+	}
+
+	const dgHugeVector& GetTestPoint(const dgEdge* const edge) const
+	{
+		const dgBigVector& p0 = m_mesh->m_points[edge->m_incidentVertex];
+		const dgBigVector& p1 = m_mesh->m_points[edge->m_twin->m_incidentVertex];
+		dgBigVector p1p0 (p1 - p0);
+		dgInt32 index = -1;
+		dgFloat64 mag2 = p1p0 % p1p0;
+		if (mag2 > 1.0e-8) {
+			dgFloat64 den = 1.0f / mag2;
+			dgFloat64 maxDist = -1.0e10f;
+			for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) {
+				const dgBigVector& q = m_mesh->m_points[ptr->m_incidentVertex];
+				dgBigVector qp0 (q - p0);
+				dgBigVector diff (qp0 - p1p0.Scale3 ((qp0 % p1p0) * den));
+				dgFloat64 dist2 = diff % diff;
+				if (dist2 > maxDist) {
+					index = ptr->m_incidentVertex;
+					maxDist = dist2;
+				}
+			}
+		} else {
+			const dgHugeVector& p0 = m_vertexAlias[edge->m_incidentVertex];
+			const dgHugeVector& p1 = m_vertexAlias[edge->m_twin->m_incidentVertex];
+			dgHugeVector p1p0 (p1 - p0);
+			dgGoogol mag2 (p1p0 % p1p0);
+			dgAssert (mag2 > dgGoogol::m_zero);
+			dgGoogol den = dgGoogol::m_one / mag2;
+			dgGoogol maxDist (-1.0e10f);
+			for (dgEdge* ptr = edge->m_next->m_next; ptr != edge; ptr = ptr->m_next) {
+				const dgHugeVector& q = m_mesh->m_points[ptr->m_incidentVertex];
+				dgHugeVector qp0 (q - p0);
+				dgHugeVector diff (qp0 - p1p0.Scale3 ((qp0 % p1p0) * den));
+				dgGoogol dist2 = diff % diff;
+				if (dist2 > maxDist) {
+					index = ptr->m_incidentVertex;
+					maxDist = dist2;
+				}
+			}
+		}
+
+		dgAssert (index >= 0);
+		return m_vertexAlias[index];
+	}
+
+	void ColorInteriorFaces (const dgBooleanMeshClipper* const meshBvhB)
+	{
+
+		dgPolyhedra::Iterator iter (*m_mesh);
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			edge->m_mark = 0;
+		}
+
+		dgInt32 mark = m_mesh->IncLRU();
+		dgAssert (mark < DG_INTERIOR_FACE_MARK);
+
+
+		dgList<dgEdge*> borderStack (m_mesh->GetAllocator());
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edgeA = &iter.GetNode()->GetInfo();
+			if (~edgeA->m_mark & mark) {
+				edgeA->m_mark |= mark;
+				edgeA->m_twin->m_mark |= mark;
+				dgInt32 vA0 = edgeA->m_incidentVertex;
+				dgInt32 vA1 = edgeA->m_twin->m_incidentVertex;
+				dgTree<dgInt32, dgInt32>::dgTreeNode* const nodeA0 = m_vertexMap.Find(vA0);
+				if (nodeA0) {
+					dgTree<dgInt32, dgInt32>::dgTreeNode* const nodeA1 = m_vertexMap.Find(vA1);
+					if (nodeA1) {
+						dgInt32 vB0 = nodeA0->GetInfo();
+						dgInt32 vB1 = nodeA1->GetInfo();
+						dgEdge* const edgeB = meshBvhB->m_mesh->FindEdge(vB0, vB1);
+						if (edgeB) {
+							dgHugeVector normalB0 (meshBvhB->FaceNormal(edgeB));
+							dgHugeVector normalB1 (meshBvhB->FaceNormal(edgeB->m_twin));
+							const dgHugeVector& origin (m_vertexAlias[vA0]);
+							//const dgHugeVector& testPoint1 (m_vertexAlias[edgeA->m_prev->m_incidentVertex]);
+							const dgHugeVector& testPoint1 = GetTestPoint(edgeA);
+							dgHugeVector dist0 (testPoint1 - origin);
+							dgGoogol side10 (normalB0 % dist0);
+							dgGoogol side11 (normalB1 % dist0);
+
+							//const dgHugeVector& testPoint2 (m_vertexAlias[edgeA->m_twin->m_prev->m_incidentVertex]);
+							const dgHugeVector& testPoint2 = GetTestPoint(edgeA->m_twin);
+							dgHugeVector dist2 (testPoint2 - origin);
+							dgGoogol side20 (normalB0 % dist2);
+							dgGoogol side21 (normalB1 % dist2);
+
+							bool edgeMarkA = false;
+							bool twinMarkA = false;
+							if ((side10 < m_negTol) || (side11 < m_negTol)) {
+								edgeMarkA = true;
+								if ((side20 < m_negTol) || (side21 < m_negTol)) {
+									twinMarkA = true;
+								}
+
+							} else {
+								if ((side20 < m_negTol) || (side21 < m_negTol)) {
+									twinMarkA = true;
+								}
+							}
+
+							if (edgeMarkA & !twinMarkA) {
+								borderStack.Append(edgeA);
+								dgEdge* ptr = edgeA->m_twin; 
+								do {
+									ptr->m_mark |= DG_LOOP_BORDER_MARK;
+									ptr = ptr->m_next;
+								} while (ptr != edgeA->m_twin);
+
+							} else if (!edgeMarkA & twinMarkA) {
+								borderStack.Append(edgeA->m_twin);
+								dgEdge* ptr = edgeA; 
+								do {
+									ptr->m_mark |= DG_LOOP_BORDER_MARK;
+									ptr = ptr->m_next;
+								} while (ptr != edgeA);
+							}
+						}
+					}
+				}
+			}
+		}
+
+
+		while (borderStack.GetCount()) {
+			dgEdge* const edge = borderStack.GetFirst()->GetInfo();
+			borderStack.Remove(borderStack.GetFirst());
+
+			if ((~edge->m_mark & DG_LOOP_BORDER_MARK) && (~edge->m_mark & DG_INTERIOR_FACE_MARK)) {
+				dgEdge* ptr = edge;
+				do {
+					ptr->m_mark |= DG_INTERIOR_FACE_MARK;
+					if ((~ptr->m_twin->m_mark & DG_LOOP_BORDER_MARK) && (~ptr->m_twin->m_mark & DG_INTERIOR_FACE_MARK)) {
+						borderStack.Addtop(ptr->m_twin);
+					}
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+			}
+		}
+	}
+
+
+	static void ConnectEntersections (dgBooleanMeshClipper* const meshBvhA, dgBooleanMeshClipper* const meshBvhB)
+	{
+		dgAssert (meshBvhA->m_vertexMap.GetCount() == meshBvhB->m_vertexMap.GetCount());				
+
+		for (bool edgeFound = true; edgeFound;) {
+			edgeFound = false;
+			edgeFound |= meshBvhA->ConnectExistingEdges (meshBvhB);
+			edgeFound |= meshBvhB->ConnectExistingEdges (meshBvhA);
+		}
+
+		for (bool edgeFound = true; edgeFound;) {
+			edgeFound = false;
+			while (meshBvhA->ConnectMissingEdges (meshBvhB)) {
+				edgeFound = true;
+				meshBvhB->ConnectExistingEdges (meshBvhA);
+			}
+			while (meshBvhB->ConnectMissingEdges (meshBvhA)) {
+				edgeFound = true;
+				meshBvhA->ConnectExistingEdges (meshBvhB);
+			}
+		}
+
+		meshBvhA->ColorInteriorFaces (meshBvhB);
+		meshBvhB->ColorInteriorFaces (meshBvhA);
+	}
+
+	static void ClipMeshesAndColorize (dgMeshEffect* const meshA, dgMeshEffect* const meshB)
+	{
+		dgBooleanMeshClipper BVHmeshA (meshA);
+		dgBooleanMeshClipper BVHmeshB (meshB);
+
+		dgInt32 baseAttibuteCountB = BVHmeshB.m_mesh->GetPropertiesCount();
+
+
+BVHmeshA.m_mesh->SaveOFF("xxxA0.off");
+BVHmeshB.m_mesh->SaveOFF("xxxB0.off");
+
+		// edge-face, edge-edge and edge-vertex intersections until not more intersections are found 
+		for (bool intersectionFound = true; intersectionFound;) {
+			intersectionFound = false;
+
+			intersectionFound |= BVHmeshA.CalculateEdgeFacesIntersetions (BVHmeshB);
+			intersectionFound |= BVHmeshB.CalculateEdgeFacesIntersetions (BVHmeshA);
+
+			intersectionFound |= BVHmeshA.CalculateVertexFacesIntersetions (BVHmeshB);
+			intersectionFound |= BVHmeshB.CalculateVertexFacesIntersetions (BVHmeshA);
+
+
+BVHmeshA.m_mesh->SaveOFF("xxxA1.off");
+BVHmeshB.m_mesh->SaveOFF("xxxB1.off");
+
+			intersectionFound |= BVHmeshA.CalculateEdgeEdgeIntersetions (BVHmeshB);
+
+BVHmeshA.m_mesh->SaveOFF("xxxA2.off");
+BVHmeshB.m_mesh->SaveOFF("xxxB2.off");
+
+			intersectionFound |= BVHmeshA.CalculateEdgeVertexIntersetions (BVHmeshB);
+			intersectionFound |= BVHmeshB.CalculateEdgeVertexIntersetions (BVHmeshA);
+
+BVHmeshA.m_mesh->SaveOFF("xxxA3.off");
+BVHmeshB.m_mesh->SaveOFF("xxxB3.off");
+
+		};
+
+BVHmeshA.m_mesh->SaveOFF("xxxA4.off");
+BVHmeshB.m_mesh->SaveOFF("xxxB4.off");
+
+		ConnectEntersections (&BVHmeshA, &BVHmeshB);
+
+BVHmeshA.m_mesh->SaveOFF("xxxA5.off");
+BVHmeshB.m_mesh->SaveOFF("xxxB5.off");
+
+
+		// copy the vertex from the other mesh
+		dgInt32 baseAttibuteCountA = BVHmeshA.m_mesh->GetPropertiesCount();
+		const dgMeshEffect::dgVertexAtribute* const property = (dgMeshEffect::dgVertexAtribute*)BVHmeshB.m_mesh->GetAttributePool();
+		for (dgInt32 i = 0; i < baseAttibuteCountB; i ++) {
+			BVHmeshA.m_mesh->AddAtribute(property [i]);
+		}
+
+		dgMeshEffect::Iterator iter (*BVHmeshB.m_mesh);
+		const dgBigVector* const vertexB = (dgBigVector*)BVHmeshB.m_mesh->GetVertexPool();
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const face = &iter.GetNode()->GetInfo();
+			if (~face->m_mark & DG_REMAP_VERTEX_MARK) {
+				
+				dgInt32 newIndex = face->m_incidentVertex;
+				if (newIndex < BVHmeshB.m_vertexBase) {
+					BVHmeshA.m_mesh->AddVertex(vertexB[newIndex]);
+					newIndex = BVHmeshA.m_mesh->GetVertexCount() - 1;
+				} else {
+					dgAssert (BVHmeshB.m_vertexMap.Find(newIndex));
+					newIndex = BVHmeshB.m_vertexMap.Find(newIndex)->GetInfo();
+				}
+
+				dgEdge* ptr = face;
+				do {
+					if (dgInt32 (ptr->m_userData) < baseAttibuteCountB) {
+						ptr->m_userData += baseAttibuteCountA;
+					} else {
+						BVHmeshA.m_mesh->AddAtribute(BVHmeshB.m_mesh->GetAttribute(dgInt32 (ptr->m_userData)));
+						ptr->m_userData = BVHmeshA.m_mesh->GetPropertiesCount() - 1;
+					}
+
+					ptr->m_incidentVertex = newIndex;
+					ptr->m_mark |= DG_REMAP_VERTEX_MARK;
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != face);
+			}
+		}
+	}
+
+	static void CopyPoints(dgMeshEffect* const dest, const dgMeshEffect* const source)
+	{
+		dgInt32 vertexCount = source->GetVertexCount();
+		const dgBigVector* const vertex = (dgBigVector*)source->GetVertexPool();
+		for (dgInt32 i = 0; i < vertexCount; i ++) {
+			dest->AddVertex(vertex[i]);
+		}
+
+		dgInt32 propertyCount = source->GetPropertiesCount();
+		const dgMeshEffect::dgVertexAtribute* const property = (dgMeshEffect::dgVertexAtribute*)source->GetAttributePool();
+		for (dgInt32 i = 0; i < propertyCount; i ++) {
+			dest->AddAtribute(property [i]);
+		}
+	}
+
+	static void AddFace (dgMeshEffect* const dest, dgPolyhedra* const source, dgEdge* const face)
+	{
+		dgInt32 faceIndex[1024];
+		dgInt64 attibIndex[1024];
+
+		dgInt32 count = 0;
+		dgEdge* ptr = face;
+		do {
+			ptr->m_mark |= DG_BOOLEAN_COMBINE_MARK;
+			faceIndex[count] = ptr->m_incidentVertex;
+			attibIndex[count] = ptr->m_userData;
+			count ++;
+			dgAssert (count < dgInt32 (sizeof (faceIndex) / sizeof (faceIndex[0])));
+			ptr = ptr->m_next;
+		} while (ptr != face);
+		dest->AddFace(count, faceIndex, attibIndex);
+	}
+
+	static void AddExteriorFaces (dgMeshEffect* const dest, dgPolyhedra* const source)
+	{
+		dgMeshEffect::Iterator iter (*source);
+		for (iter.Begin(); iter; iter ++) {
+
+			dgEdge* const face = &iter.GetNode()->GetInfo();
+			if ((~face->m_mark & DG_BOOLEAN_COMBINE_MARK) && (face->m_incidentFace > 0) && (~face->m_mark & DG_INTERIOR_FACE_MARK)) {
+				AddFace (dest, source, face);
+			}
+		}
+	}
+
+	static void AddInteriorFaces (dgMeshEffect* const dest, dgPolyhedra* const source)
+	{
+		dgMeshEffect::Iterator iter (*source);
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const face = &iter.GetNode()->GetInfo();
+			if ((~face->m_mark & DG_BOOLEAN_COMBINE_MARK) && (face->m_incidentFace > 0) && (face->m_mark & DG_INTERIOR_FACE_MARK)) {
+				AddFace (dest, source, face);
+			}
+		}
+	}
+
+	static void AddInteriorFacesInvertWinding (dgMeshEffect* const dest, dgPolyhedra* const source)
+	{
+		dgStack<dgInt32> marks(dest->GetPropertiesCount());
+		memset (&marks[0], 0,  marks.GetSizeInBytes());
+
+		dgMeshEffect::dgVertexAtribute* const property = (dgMeshEffect::dgVertexAtribute*)dest->GetAttributePool();
+		dgMeshEffect::Iterator iter (*source);
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const face = &iter.GetNode()->GetInfo();
+			if ((~face->m_mark & DG_BOOLEAN_COMBINE_MARK) && (face->m_incidentFace > 0) && (face->m_mark & DG_INTERIOR_FACE_MARK)) {
+				dgInt32 faceIndex[1024];
+				dgInt64 attibIndex[1024];
+
+				dgInt32 count = 0;
+				dgEdge* ptr = face;
+				do {
+					ptr->m_mark |= DG_BOOLEAN_COMBINE_MARK;
+					faceIndex[count] = ptr->m_incidentVertex;
+					attibIndex[count] = ptr->m_userData;
+					if (!marks[dgInt32 (ptr->m_userData)]) {
+						marks[dgInt32 (ptr->m_userData)] = 1;
+						property[ptr->m_userData].m_normal_x *= dgFloat32 (-1.0f);
+						property[ptr->m_userData].m_normal_y *= dgFloat32 (-1.0f);
+						property[ptr->m_userData].m_normal_z *= dgFloat32 (-1.0f);
+					}
+
+					count ++;
+					dgAssert (count < dgInt32 (sizeof (faceIndex) / sizeof (faceIndex[0])));
+					ptr = ptr->m_prev;
+				} while (ptr != face);
+				dest->AddFace(count, faceIndex, attibIndex);
+			}
+		}
+	}
+
+	dgInt32 m_vertexBase;
+	dgTree<dgInt32,dgInt32> m_vertexMap;
+	dgTree<dgMeshBVHNode*, dgUnsigned64> m_nodeEdgeMap;
+	dgArray<dgHugeVector> m_vertexAlias;
+
+	static dgGoogol m_tol;
+	static dgGoogol m_tol2;
+	static dgGoogol m_tol3;
+	static dgGoogol m_tol4;
+	static dgGoogol m_negTol;
+	static dgGoogol m_oneMinusTol;
+};
+dgGoogol dgBooleanMeshClipper::m_tol(DG_BOOLEAN_ZERO_TOLERANCE);
+dgGoogol dgBooleanMeshClipper::m_oneMinusTol (dgGoogol(1.0) - m_tol);
+dgGoogol dgBooleanMeshClipper::m_negTol(-DG_BOOLEAN_ZERO_TOLERANCE);
+dgGoogol dgBooleanMeshClipper::m_tol2(m_tol * m_tol);
+dgGoogol dgBooleanMeshClipper::m_tol3(m_tol2 * m_tol);
+dgGoogol dgBooleanMeshClipper::m_tol4(m_tol2 * m_tol2);
+#endif
+
+
+
+void dgMeshEffect::ClipMesh (const dgMatrix& matrix, const dgMeshEffect* const clipMesh, dgMeshEffect** const back, dgMeshEffect** const front) const
+{
+	dgAssert (0);
+/*
+	dgMeshEffect clipper (*clipMesh);
+	clipper.TransformMesh (matrix);
+
+	dgMeshEffect* backMeshSource = NULL;
+	dgMeshEffect* frontMeshSource = NULL;
+	dgMeshEffect* backMeshClipper = NULL;
+	dgMeshEffect* frontMeshClipper = NULL;
+
+	ClipMesh (&clipper, &backMeshSource, &frontMeshSource);
+	if (backMeshSource && frontMeshSource) {
+		clipper.ClipMesh (this, &backMeshClipper, &frontMeshClipper);
+		if (backMeshSource && frontMeshSource) {
+
+			dgMeshEffect* backMesh;
+			dgMeshEffect* frontMesh;
+
+			backMesh = new (GetAllocator()) dgMeshEffect (GetAllocator(), true);
+			frontMesh = new (GetAllocator()) dgMeshEffect (GetAllocator(), true);
+
+			backMesh->BeginPolygon();
+			frontMesh->BeginPolygon();
+
+			backMesh->MergeFaces(backMeshSource);
+			backMesh->MergeFaces(backMeshClipper);
+
+			frontMesh->MergeFaces(frontMeshSource);
+			frontMesh->ReverseMergeFaces(backMeshClipper);
+
+			backMesh->EndPolygon(dgFloat64 (1.0e-5f));
+			frontMesh->EndPolygon(dgFloat64 (1.0e-5f));
+
+			*back = backMesh;
+			*front = frontMesh;
+		}
+	}
+
+	if (backMeshClipper) {
+		delete backMeshClipper;
+	}
+
+	if (frontMeshClipper) {
+		delete frontMeshClipper;
+	}
+
+	if (backMeshSource) {
+		delete backMeshSource;
+	}
+
+	if (frontMeshSource) {
+		delete frontMeshSource;
+	}
+*/
+}
+
+
+
+
+dgMeshEffect* dgMeshEffect::Union (const dgMatrix& matrix, const dgMeshEffect* const clipperMesh) const
+{
+	dgAssert (0);
+	return NULL;
+/*
+	dgMeshEffect copy (*this);
+	dgMeshEffect clipper (*clipperMesh);
+	clipper.TransformMesh (matrix);
+
+	dgBooleanMeshClipper::ClipMeshesAndColorize (&copy, &clipper);
+
+	dgMeshEffect* const mesh = new (GetAllocator()) dgMeshEffect (GetAllocator());
+	mesh->BeginFace();
+	dgBooleanMeshClipper::CopyPoints(mesh, &copy);
+	dgBooleanMeshClipper::AddExteriorFaces (mesh, &copy);
+
+	dgBooleanMeshClipper::AddExteriorFaces (mesh, &clipper);
+	mesh->EndFace ();
+	mesh->RepairTJoints();
+	mesh->RemoveUnusedVertices(NULL);
+	return mesh;
+*/
+}
+
+dgMeshEffect* dgMeshEffect::Difference (const dgMatrix& matrix, const dgMeshEffect* const clipperMesh) const
+{
+/*
+	dgMeshEffect copy (*this);
+	dgMeshEffect clipper (*clipperMesh);
+	clipper.TransformMesh (matrix);
+
+	dgBooleanMeshClipper::ClipMeshesAndColorize (&copy, &clipper);
+
+	dgMeshEffect* const mesh = new (GetAllocator()) dgMeshEffect (GetAllocator());
+	mesh->BeginFace();
+	dgBooleanMeshClipper::CopyPoints(mesh, &copy);
+	dgBooleanMeshClipper::AddExteriorFaces (mesh, &copy);
+	dgBooleanMeshClipper::AddInteriorFacesInvertWinding (mesh, &clipper);
+	mesh->EndFace ();
+	mesh->RepairTJoints();
+	mesh->RemoveUnusedVertices(NULL);
+	return mesh;
+*/
+
+	dgAssert (0);
+	return NULL;
+}
+
+
+class dgBooleanMeshClipper: public dgMeshEffect::dgMeshBVH
+{
+	public:
+
+	class dgClusterFace: public dgList<dgMeshBVHNode*>
+	{
+		public:
+		dgClusterFace(dgMeshEffect* const mesh, dgEdge* const face)
+			:dgList<dgMeshBVHNode*>(mesh->GetAllocator())
+			,m_plane (mesh->FaceNormal (face, mesh->GetVertexPool(), sizeof (dgBigVector)), dgFloat32(0.0))
+		{
+			const dgBigVector* const vertex = (dgBigVector*) mesh->GetVertexPool();
+			m_plane = m_plane.Scale(1.0 / sqrt(m_plane % m_plane));
+			m_plane.m_w = - (m_plane % vertex[face->m_incidentVertex]);
+		}
+
+		bool AddFace (const dgMeshEffect* const mesh, dgMeshBVHNode* const faceNode, const dgMeshEffect* const myMesh, dgMeshBVHNode* const myFaceNode) 
+		{
+			dgAssert (!Find(faceNode));
+
+			bool added = false;
+			dgInt32 posCount = 0;
+			dgInt32 negCount = 0;
+			const dgBigVector* const vertex = (dgBigVector*) mesh->GetVertexPool();
+			dgEdge* ptr = faceNode->m_face;
+			do {
+				dgFloat64 test = m_plane.Evalue(vertex[ptr->m_incidentVertex]);
+				posCount += (test > -1.0e-3);
+				negCount += (test <  1.0e-3);
+				ptr = ptr->m_next;
+			} while (ptr != faceNode->m_face);
+
+			if (posCount * negCount) {
+				dgBigPlane plane (mesh->FaceNormal (faceNode->m_face, &vertex[0].m_x, sizeof (dgBigVector)), dgFloat32 (0.0f));
+				plane = plane.Scale(1.0 / sqrt(plane % plane));
+				plane.m_w = - (plane % vertex[faceNode->m_face->m_incidentVertex]);
+
+				dgInt32 posCount = 0;
+				dgInt32 negCount = 0;
+				const dgBigVector* const myVertex = (dgBigVector*) myMesh->GetVertexPool();
+				dgEdge* ptr = myFaceNode->m_face;
+				do {
+					dgFloat64 test = plane.Evalue (myVertex[ptr->m_incidentVertex]);
+					posCount += (test > -1.0e-3);
+					negCount += (test <  1.0e-3);
+					ptr = ptr->m_next;
+				} while (ptr != myFaceNode->m_face);
+
+				if (posCount * negCount) {
+					Append(faceNode);
+					added = true;
+				}
+			}
+
+			return added;
+		}
+
+		dgEdge* FindFirstClipperEdge (const dgHugeVector& normal, const dgHugeVector& origin, const dgBooleanMeshClipper* const clipperMeshBVH, dgInt32 clipperMark) const
+		{
+			dgEdge* vertexClipperEdge = NULL;
+//normal.Trace();
+//origin.Trace();
+
+			for (dgListNode* clipperNode = GetFirst(); clipperNode; clipperNode = clipperNode->GetNext()) {
+				dgMeshBVHNode* const clipperFaceNode = clipperNode ->GetInfo();
+
+				dgEdge* ptr = clipperFaceNode->m_face;
+//clipperMeshBVH->m_vertexAlias[ptr->m_incidentVertex].Trace();
+				dgGoogol test0 (normal % (clipperMeshBVH->m_vertexAlias[ptr->m_incidentVertex] - origin));
+				do {
+					dgGoogol test1 (normal % (clipperMeshBVH->m_vertexAlias[ptr->m_next->m_incidentVertex] - origin));
+//test0.Trace();
+//test1.Trace();
+					if (ptr->m_mark != clipperMark) {
+						if ((test0 <= dgGoogol (m_negTol)) && (test1 >= dgGoogol (m_posTol))) {
+							return ptr;
+						} else if ((test0.Abs() < dgGoogol (m_posTol)) && (test1 >= dgGoogol (m_posTol))) {
+							dgAssert (0);
+							vertexClipperEdge = ptr;
+						}
+					}
+
+					test0 = test1;
+					ptr = ptr->m_next;
+				} while (ptr != clipperFaceNode->m_face);
+			}
+			return vertexClipperEdge;
+		}
+
+
+		dgEdge* EmitPointOnPlaneAndGetNextCrossingEdge (const dgHugeVector& normal, const dgHugeVector& origin, const dgBooleanMeshClipper* const clipperMeshBVH, dgEdge* const clipperEdge, dgInt32 clipperMark, dgList<dgHugeVector>& curve) const
+		{
+			dgHugeVector& point = curve.Addtop()->GetInfo();
+			point = clipperMeshBVH->m_vertexAlias[clipperEdge->m_twin->m_incidentVertex];
+
+			dgEdge* ptr = clipperEdge;
+			do {
+				ptr->m_mark = clipperMark;
+				ptr->m_twin->m_mark = clipperMark;
+				ptr = ptr->m_next;
+			} while (ptr != clipperEdge);
+
+			dgEdge* nexEdge = NULL;
+			ptr = clipperEdge->m_next->m_twin->m_next;
+			do {
+				ptr->m_mark = clipperMark;
+				ptr->m_twin->m_mark = clipperMark;
+
+				if (!nexEdge) {
+					dgEdge* ptr1 = ptr->m_next;
+					dgAssert (ptr1);
+					dgGoogol test0 (normal % (clipperMeshBVH->m_vertexAlias[ptr1->m_incidentVertex] - origin));
+					do {
+						dgAssert (ptr1->m_next);
+						dgGoogol test1 (normal % (clipperMeshBVH->m_vertexAlias[ptr1->m_next->m_incidentVertex] - origin));
+						if (test0 >= m_posTol) {
+							if (test1.Abs() < m_posTol) {
+								nexEdge = EmitPointOnPlaneAndGetNextCrossingEdge (normal, origin, clipperMeshBVH, ptr1, clipperMark, curve);
+							} else if (test1 <= m_negTol) {
+								nexEdge = ptr1;
+							}
+						}
+
+						test1 = test0;
+						ptr1 = ptr1->m_next;
+					} while (ptr1 != ptr->m_prev);
+				}
+
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != clipperEdge->m_next);
+
+			dgAssert (nexEdge);
+			return nexEdge;
+		}
+
+
+		bool Get2DCurve (const dgHugeVector& normal, const dgHugeVector& origin, const dgBooleanMeshClipper* const clipperMeshBVH, dgEdge* const clipperEdge, dgInt32 clipperMark, dgList<dgHugeVector>& curve) const
+		{
+			bool isClosedLoop = false;
+
+static int xxx;
+xxx ++;
+if (xxx == 3){
+clipperMeshBVH->m_mesh->Trace();
+}
+
+			dgInt32 clusterColor = clipperMark -1;	
+
+			dgEdge* firstClipperEdge = clipperEdge;
+			dgAssert (firstClipperEdge->m_mark >= clusterColor);
+
+			for (bool state = true; state; ) {
+				state = false;
+
+				dgHugeVector p1p0 (clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_next->m_incidentVertex] - clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_incidentVertex]);
+				dgGoogol num (normal % (clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_incidentVertex] - origin));
+				dgGoogol den (normal % p1p0);
+				dgHugeVector& point = curve.Addtop()->GetInfo();
+				point = clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_incidentVertex] - p1p0.Scale3(num / den);
+
+				if (firstClipperEdge->m_mark >= clusterColor) {
+					firstClipperEdge->m_mark = clipperMark;
+					firstClipperEdge->m_twin->m_mark = clipperMark;
+
+					dgEdge* ptr = firstClipperEdge->m_next;
+					dgGoogol test0 (normal % (clipperMeshBVH->m_vertexAlias[ptr->m_incidentVertex] - origin));
+					do {
+						dgGoogol test1 (normal % (clipperMeshBVH->m_vertexAlias[ptr->m_next->m_incidentVertex] - origin));
+						dgAssert (ptr->m_mark >= clusterColor);
+						if (test1.Abs() < m_posTol) {
+							dgAssert (test0 >= m_posTol);
+							state = true;
+							firstClipperEdge = EmitPointOnPlaneAndGetNextCrossingEdge (normal, origin, clipperMeshBVH, ptr, clipperMark, curve);
+							firstClipperEdge = firstClipperEdge->m_twin;
+							break;
+
+						} else if ((test0 >= m_posTol) && (test1 <= m_negTol)) {
+							if (ptr->m_mark != clipperMark) {
+								state = true;
+								firstClipperEdge = ptr->m_twin;
+							} else {
+								isClosedLoop = true;
+							}
+							break;
+						}
+
+						test0 = test1;
+						ptr = ptr->m_next;
+					} while (ptr != firstClipperEdge);
+				}
+			}
+
+			if (!isClosedLoop) {
+				dgEdge* firstClipperEdge = clipperEdge->m_twin->m_next;
+				for (bool state = true; state; ) {
+					state = false;
+					if (firstClipperEdge->m_mark >= clusterColor) {
+						dgAssert (0);
+/*
+						firstClipperEdge->m_mark = clipperMark;
+						firstClipperEdge->m_twin->m_mark = clipperMark;
+
+						dgEdge* ptr = firstClipperEdge->m_next;
+						dgGoogol test0 (normal % (clipperMeshBVH->m_vertexAlias[ptr->m_incidentVertex] - origin));
+						do {
+							dgGoogol test1 (normal % (clipperMeshBVH->m_vertexAlias[ptr->m_next->m_incidentVertex] - origin));
+							dgAssert (ptr->m_mark >= clusterColor);
+							if ((test0 >= dgGoogol (m_posTol)) && (test1 <= dgGoogol (m_negTol))) {
+								if (ptr->m_mark != clipperMark) {
+									state = true;
+									firstClipperEdge = ptr->m_twin;
+								} else {
+									isClosedLoop = true;
+								}
+								break;
+							}
+
+							test0 = test1;
+							ptr = ptr->m_next;
+						} while (ptr != firstClipperEdge);
+
+//						dgHugeVector p1p0 (clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_next->m_incidentVertex] - clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_incidentVertex]);
+//						dgGoogol num (normal % (clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_incidentVertex] - origin  ));
+//						dgGoogol den (normal % p1p0);
+//						dgHugeVector& point = curve.Addtop()->GetInfo();
+//						point = clipperMeshBVH->m_vertexAlias[firstClipperEdge->m_incidentVertex] - p1p0.Scale(num / den);
+*/
+					}
+				}
+			}
+			return isClosedLoop;
+		}
+
+		bool TestPlanar (const dgHugeVector& normal, const dgList<dgHugeVector>& curve) const
+		{
+			dgHugeVector n (0.0, 0.0, 0.0, 0.0);
+			const dgHugeVector& p0 = curve.GetFirst()->GetInfo();
+			const dgHugeVector& p1 = curve.GetFirst()->GetNext()->GetInfo();
+
+//			p0.Trace();
+//			p1.Trace();
+			dgHugeVector e10 (p1 - p0);
+			for (dgList<dgHugeVector>::dgListNode* node = curve.GetFirst()->GetNext()->GetNext(); node; node = node->GetNext()) {
+				const dgHugeVector& p2 = node->GetInfo();
+//				p2.Trace();
+				dgHugeVector e20 (p2 - p0);
+				n += e10 * e20;
+				e10 = e20;
+			}
+
+			dgGoogol dir (n % normal);
+			return dir > dgGoogol::m_zero;
+		}
+
+
+		void RayRayIntersect (const dgHugeVector& ray_p0, const dgHugeVector& ray_p1, dgGoogol& paramP, const dgHugeVector& ray_q0, const dgHugeVector& ray_q1, dgGoogol& paramQ) const
+		{
+			dgHugeVector p1p0 (ray_p1 - ray_p0);
+			dgHugeVector q1q0 (ray_q1 - ray_q0);
+			dgHugeVector p0q0 (ray_p0 - ray_q0);
+
+			dgGoogol a = p1p0 % p1p0;        // always >= 0
+			dgGoogol c = q1q0 % q1q0;        // always >= 0
+			dgGoogol b = p1p0 % q1q0;
+
+			dgGoogol d = (p1p0 % p0q0);
+			dgGoogol e = (q1q0 % p0q0);
+			dgGoogol den = a * c - b * b;   // always >= 0
+
+			// compute the line parameters of the two closest points
+			dgAssert (den > m_tol3);
+
+			// get the closest points on the infinite lines
+			paramP = (b * e - c * d) / den;
+			paramQ = (a * e - b * d) / den;
+			paramP = dgClamp (paramP, dgGoogol::m_zero, dgGoogol::m_one);
+			paramQ = dgClamp (paramQ, dgGoogol::m_zero, dgGoogol::m_one);
+		}
+
+		void InsertOpenCurve (dgBooleanMeshClipper* const meshBVH, dgList<dgEdge*>& facePerimeter, const dgHugeVector& normal, const dgHugeVector& origin, dgList<dgHugeVector>& curve) const
+		{
+			dgInt32 edgeConnectCount = 0;
+			dgEdge* edgeConnect[4];
+			const dgHugeVector* const vertex = &meshBVH->m_vertexAlias[0];
+			for (dgList<dgEdge*>::dgListNode* node = facePerimeter.GetFirst(); node; node = node->GetNext()) {
+				dgEdge* const edge = node->GetInfo();
+
+				dgInt32 index0 = edge->m_incidentVertex;
+				dgInt32 index1 = edge->m_next->m_incidentVertex;
+				const dgHugeVector& q0 (vertex[index0]);
+				const dgHugeVector& q1 (vertex[index1]);
+
+				for (dgList<dgHugeVector>::dgListNode* curveNode = curve.GetFirst(); curveNode != curve.GetLast(); curveNode = curveNode->GetNext()) {
+					const dgHugeVector& p0 = curveNode->GetInfo();
+					const dgHugeVector& p1 = curveNode->GetNext()->GetInfo();
+
+					dgHugeVector q0p0 (q0 - p0);
+					dgHugeVector q1p0 (q1 - p0);
+					dgGoogol alpha0 ((normal * q0p0) % q1p0);
+
+					dgHugeVector q0p1 (q0 - p1);
+					dgHugeVector q1p1 (q1 - p1);
+					dgGoogol alpha1 ((normal * q0p1) % q1p1);
+
+					bool test0 = (alpha0 <= m_tol2);
+					bool test1 = (alpha1 <= m_tol2);
+					if (test0 ^ test1) {
+						dgGoogol paramP;
+						dgGoogol paramQ;
+						RayRayIntersect (q0, q1, paramQ, p0, p1, paramP);
+
+						bool test0ParamP = paramP >= m_posTol;
+						bool test1ParamP = paramP <= m_oneMinusTol;
+						bool test0ParamQ = paramQ >= m_posTol;
+						bool test1ParamQ = paramQ <= m_oneMinusTol;
+
+						if (test0ParamP & test1ParamP & test0ParamQ & test1ParamQ) {
+							dgEdge* edge0;
+							dgEdge* edge1;
+
+							meshBVH->SpliteEdge(edge, paramQ, &edge0, &edge1);
+							edgeConnect[edgeConnectCount] = edge1;
+							edgeConnectCount ++; 
+
+							node->GetInfo() = edge0;
+							facePerimeter.InsertAfter(node, facePerimeter.Append(edge1));
+							curve.InsertAfter (curveNode, curve.Append(meshBVH->m_vertexAlias[meshBVH->m_mesh->GetVertexCount()-1]));
+
+						} else if (!((test0ParamP ^ test1ParamP) & (test0ParamQ ^ test1ParamQ))) {
+							dgHugeVector p (p0 + (p1 - p0).Scale3 (paramP));
+							dgHugeVector q (q0 + (q1 - q0).Scale3 (paramQ));
+							dgHugeVector pq = (p - q);
+							dgGoogol dist2 (pq % pq);
+							if (dist2 < m_tol2) {
+								if (test0ParamQ ^ test1ParamQ) {
+									dgAssert (test0ParamP & test1ParamP);
+									if (!test0ParamQ) {
+										dgAssert (test1ParamQ);
+										edgeConnect[edgeConnectCount] = edge;
+										edgeConnectCount ++; 
+										curve.InsertAfter (curveNode, curve.Append(meshBVH->m_vertexAlias[edge->m_next->m_incidentVertex]));
+									} else {
+										dgAssert (!test1ParamQ);
+										edgeConnect[edgeConnectCount] = edge->m_next;
+										edgeConnectCount ++; 
+										curve.InsertAfter (curveNode, curve.Append(meshBVH->m_vertexAlias[edge->m_next->m_incidentVertex]));
+									}
+								} else {
+									dgAssert (0);
+									dgAssert (test0ParamP ^ test1ParamP);
+									dgAssert (test0ParamQ & test1ParamQ);
+								}
+							}
+						}
+
+						break;
+					}
+				}
+			}
+
+			dgList<dgHugeVector>::dgListNode* nextNode;
+			for (dgList<dgHugeVector>::dgListNode* node = curve.GetFirst(); node; node = nextNode) {
+				nextNode = node->GetNext();
+				const dgHugeVector& p0 = node->GetInfo();
+
+				for (dgList<dgEdge*>::dgListNode* faceNode = facePerimeter.GetFirst(); faceNode; faceNode = faceNode->GetNext()) {
+					dgEdge* const edge = faceNode->GetInfo();
+
+					dgInt32 index0 = edge->m_incidentVertex;
+					dgInt32 index1 = edge->m_next->m_incidentVertex;
+					const dgHugeVector& q0 (vertex[index0]);
+					const dgHugeVector& q1 (vertex[index1]);
+					dgHugeVector q0p0 (q0 - p0);
+					dgHugeVector q1p0 (q1 - p0);
+					dgGoogol alpha0 ((normal * q1p0) % q0p0);
+					if (alpha0 >= m_tol2) {
+						curve.Remove(node);
+						break;
+					}
+				}
+			}
+
+
+			dgAssert (edgeConnectCount == 2);
+			if (curve.GetCount() == 2) {
+				meshBVH->m_mesh->ConnectVertex(edgeConnect[0], edgeConnect[1]);
+			} else {
+				dgAssert (0);
+			}
+		}
+
+
+		void InsertClosedCurve (dgBooleanMeshClipper* const meshBVH, dgList<dgEdge*>& facePerimeter, const dgHugeVector& normal, const dgHugeVector& origin, const dgList<dgHugeVector>& clipperCurve) const
+		{
+			dgAssert (TestPlanar (normal, clipperCurve));
+
+			bool isInside = true;
+			const dgHugeVector* const vertex = &meshBVH->m_vertexAlias[0];
+			for (dgList<dgEdge*>::dgListNode* node = facePerimeter.GetFirst(); isInside && node; node = node->GetNext()) {
+				dgEdge* const edge = node->GetInfo();
+				dgInt32 index0 = edge->m_incidentVertex;
+				dgInt32 index1 = edge->m_next->m_incidentVertex;
+
+				const dgHugeVector& q0 (vertex[index0]);
+				const dgHugeVector& q1 (vertex[index1]);
+				for (dgList<dgHugeVector>::dgListNode* clipperNode = clipperCurve.GetFirst(); clipperNode; clipperNode = clipperNode->GetNext()) {
+					const dgHugeVector& p0 = clipperNode->GetInfo();
+					dgHugeVector q0p0 (q0 - p0);
+					dgHugeVector q1p0 (q1 - p0);
+					dgGoogol alpha ((normal * q0p0) % q1p0);
+					if (alpha <= m_tol2) {
+						isInside = false;
+						break;
+					}
+				}
+			} 
+
+			if (isInside) {
+				dgInt32 face[1024];
+				dgInt64 attrb[1024];
+
+				dgInt32 indexCount = 0;
+				dgEdge* const exteriorFace = facePerimeter.GetFirst()->GetInfo();
+				for (dgList<dgHugeVector>::dgListNode* clipperNode = clipperCurve.GetFirst(); clipperNode; clipperNode = clipperNode->GetNext()) {
+					meshBVH->InsertFaceVertex (exteriorFace, clipperNode->GetInfo());
+					face[indexCount] = meshBVH->m_mesh->GetVertexCount() - 1;
+					attrb[indexCount] = meshBVH->m_mesh->GetPropertiesCount() - 1;
+					indexCount ++;
+				}
+				dgEdge* const interiorFace = meshBVH->m_mesh->AddFace(indexCount, face, attrb);
+				dgEdge* ptr = interiorFace;
+				do {
+					ptr->m_twin = meshBVH->m_mesh->AddHalfEdge(ptr->m_next->m_incidentVertex, ptr->m_incidentVertex);
+					dgAssert (ptr->m_twin);
+					ptr->m_twin->m_twin = ptr;
+					ptr = ptr->m_next;
+				} while (ptr != interiorFace);
+
+				ptr = interiorFace;
+				do {
+					ptr->m_twin->m_incidentFace = ptr->m_incidentFace;
+					ptr->m_twin->m_userData = ptr->m_next->m_userData;
+
+					ptr->m_twin->m_prev = ptr->m_next->m_twin;
+					ptr->m_next->m_twin->m_next = ptr->m_twin;
+
+					ptr = ptr->m_next;
+				} while (ptr != interiorFace);
+
+				dgGoogol error2 (1.0e20);
+				dgEdge* closestEdge = NULL;
+				const dgHugeVector* const vertex = &meshBVH->m_vertexAlias[0];
+				const dgHugeVector& p0 = vertex[exteriorFace->m_incidentVertex];
+				ptr = interiorFace;
+				do {
+					const dgHugeVector& q0 = vertex[ptr->m_incidentVertex];
+					dgHugeVector dist (q0 - p0);
+					dgGoogol mag2 (dist % dist);
+
+					if (mag2 < error2) {
+						error2 = mag2;
+						closestEdge = ptr;
+					}
+
+					ptr->m_twin->m_incidentFace = interiorFace->m_incidentFace;
+					ptr = ptr->m_next;
+				} while (ptr != interiorFace);
+
+				dgAssert (closestEdge);
+				closestEdge = closestEdge->m_twin->m_next;
+				closestEdge = meshBVH->m_mesh->ConnectVertex (exteriorFace, closestEdge);
+				dgAssert (closestEdge);
+
+				meshBVH->m_mesh->PolygonizeFace(interiorFace, meshBVH->m_mesh->GetVertexPool(), sizeof (dgBigVector));
+				meshBVH->m_mesh->PolygonizeFace(closestEdge, meshBVH->m_mesh->GetVertexPool(), sizeof (dgBigVector));
+
+			} else {
+				dgAssert (0);
+			}
+		}
+
+
+
+		bool ClipFace (dgMeshBVHNode* const faceNode, dgBooleanMeshClipper* const meshBVH, const dgBooleanMeshClipper* const clipperMeshBVH)
+		{
+			dgHugeVector normal (meshBVH->FaceNormal (faceNode->m_face));
+			dgGoogol mag2 (normal % normal);
+			normal = normal.Scale3 (mag2.InvSqrt());
+			const dgHugeVector& origin (meshBVH->m_vertexAlias[faceNode->m_face->m_incidentVertex]);
+
+			dgClusterFace::dgListNode* nextNode;
+			dgInt32 clusterColor = clipperMeshBVH->m_mesh->IncLRU();
+			for (dgClusterFace::dgListNode* node = GetFirst(); node; node = nextNode) {
+				dgMeshBVHNode* const clipperFaceNode = node->GetInfo();
+				nextNode = node->GetNext();
+
+				dgInt32 posCount = 0;
+				dgInt32 negCount = 0;
+				dgInt32 onPlaneCount = 0;
+				dgEdge* ptr = clipperFaceNode->m_face;
+				do {
+					ptr->m_mark = clusterColor;
+					dgGoogol test (normal % (clipperMeshBVH->m_vertexAlias[ptr->m_incidentVertex] - origin));
+					bool pos = (test > dgBooleanMeshClipper::m_posTol);
+					bool neg = (test < dgBooleanMeshClipper::m_negTol);
+					posCount += pos;
+					negCount += neg;
+					onPlaneCount += !(pos || neg);
+					ptr = ptr->m_next;
+				} while (ptr != clipperFaceNode->m_face);
+
+
+				if (!((posCount && negCount) || (posCount && onPlaneCount) || (negCount && onPlaneCount))) {
+					dgAssert (0);
+					Remove(node);
+				}
+			}
+
+
+			if (GetCount()) {
+				dgList<dgEdge*> facePerimeter (clipperMeshBVH->m_mesh->GetAllocator());
+				dgEdge* perimterEdge = faceNode->m_face;
+				do {
+					facePerimeter.Append(perimterEdge);
+					perimterEdge = perimterEdge->m_next;
+				} while (perimterEdge != faceNode->m_face);
+
+				dgPolyhedra::dgPairKey key (faceNode->m_face->m_incidentVertex, faceNode->m_face->m_twin->m_incidentVertex);
+
+				dgInt32 clipperMark = clipperMeshBVH->m_mesh->IncLRU();
+				for (bool found = true; found;) {
+					found = false;
+					dgList<dgHugeVector> clipperCurve (clipperMeshBVH->m_mesh->GetAllocator());
+
+					dgEdge* const firstClipperEdge = FindFirstClipperEdge (normal, origin, clipperMeshBVH, clipperMark);
+					if (firstClipperEdge) {
+						dgAssert (firstClipperEdge->m_mark == clusterColor);
+						found = true;
+						bool isClosedLoop = Get2DCurve (normal, origin, clipperMeshBVH, firstClipperEdge, clipperMark, clipperCurve);
+						if (clipperCurve.GetCount() >= 2) {
+							if (isClosedLoop) {
+								InsertClosedCurve (meshBVH, facePerimeter, normal, origin, clipperCurve);
+							} else {
+								InsertOpenCurve (meshBVH, facePerimeter, normal, origin, clipperCurve);
+							}
+						}
+					}
+				}
+
+				meshBVH->m_nodeEdgeMap.Remove(key.GetVal());
+				meshBVH->RemoveNode(faceNode);
+
+				dgInt32 stack = 0;
+				dgEdge* stackPool[512];
+				dgInt32 mark = meshBVH->m_mesh->IncLRU();
+				for (dgList<dgEdge*>::dgListNode* node = facePerimeter.GetFirst(); node; node = node->GetNext()) {
+					dgEdge* const edge = node->GetInfo();
+					edge->m_twin->m_mark = mark;
+					stackPool[stack] = edge;
+					stack ++;
+				}
+
+				while (stack) {
+					stack --;
+					dgEdge* const face = stackPool[stack];
+					if (face->m_mark != mark) {
+						dgMeshBVHNode* const newNode = meshBVH->AddFaceNode (face, NULL);
+						dgPolyhedra::dgPairKey key(newNode->m_face->m_incidentVertex, newNode->m_face->m_twin->m_incidentVertex);
+						meshBVH->m_nodeEdgeMap.Insert(newNode, key.GetVal());
+					}
+
+					dgEdge* ptr = face;
+					do {
+						ptr->m_mark = mark;
+						if (ptr->m_twin->m_mark != mark) {
+							stackPool[stack] = ptr->m_twin;
+							stack ++;
+						}
+						ptr = ptr->m_next;
+					} while (ptr != face);
+				}
+			}
+
+			return false;
+		}
+
+
+		dgBigPlane m_plane;
+	};
+
+	class dgBooleanFaceCluster: public dgTree<dgClusterFace, dgMeshBVHNode*>
+	{
+		public:
+		dgBooleanFaceCluster (dgMemoryAllocator* const allocator)
+			:dgTree<dgClusterFace, dgMeshBVHNode*>(allocator)
+		{
+		}
+
+		bool ClipCluster (dgTreeNode* const clusterNode, dgBooleanMeshClipper* const meshBVH, const dgBooleanMeshClipper* const clipperMeshBVH)
+		{
+			dgClusterFace& cluster = clusterNode->GetInfo();
+			dgMeshBVHNode* const faceNode = clusterNode->GetKey();
+			bool ret = cluster.ClipFace(faceNode, meshBVH, clipperMeshBVH);
+			Remove (clusterNode);
+			return ret;
+		}
+	};
+
+
+	dgBooleanMeshClipper (dgMeshEffect* const mesh) 
+		:dgMeshBVH (mesh)
+		,m_vertexBase(mesh->GetVertexCount())
+		,m_vertexAlias(mesh->GetVertexCount() + 512, mesh->GetAllocator())
+		,m_nodeEdgeMap(mesh->GetAllocator())
+	{
+		Build ();
+	}
+
+	~dgBooleanMeshClipper () 
+	{
+	}
+
+
+	void Build ()
+	{
+		dgMeshBVH::Build();
+
+		const dgBigVector* const points = (dgBigVector*) m_mesh->GetVertexPool(); 
+		for (dgInt32 i = 0; i < m_mesh->GetVertexCount(); i ++) {
+			m_vertexAlias[i] = dgHugeVector (points[i]);
+		}
+
+
+		dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+		dgInt32 stack = 1;
+		stackPool[0] = m_rootNode;
+		dgVector l0(dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (0.0f)); 
+		dgVector l1(dgFloat32 (1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 (0.0f)); 
+
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const me = stackPool[stack];
+			if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) {
+				if (!me->m_left) {
+					dgAssert (!me->m_right);
+					dgPolyhedra::dgPairKey key (me->m_face->m_incidentVertex, me->m_face->m_twin->m_incidentVertex);
+					m_nodeEdgeMap.Insert(me, key.GetVal());
+				} else {
+					dgAssert (me->m_left);
+					dgAssert (me->m_right);
+					stackPool[stack] = me->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack] = me->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				}
+			}
+		}
+		dgAssert (SanityCheck());
+	}
+
+	void Cleanup ()
+	{
+		dgMeshBVH::Cleanup();
+	}
+
+
+	bool SanityCheck() const
+	{
+		#ifdef _DEBUG
+			dgAssert (dgMeshBVH::SanityCheck()); 
+			dgMeshBVHNode* stackPool[DG_MESH_EFFECT_BVH_STACK_DEPTH];
+			dgInt32 stack = 1;
+			stackPool[0] = m_rootNode;
+			dgVector l0(dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (-1.0e15f), dgFloat32 (0.0f)); 
+			dgVector l1(dgFloat32 (1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 ( 1.0e15f), dgFloat32 (0.0f)); 
+
+			while (stack) {
+				stack --;
+				dgMeshBVHNode* const me = stackPool[stack];
+				if (me && dgOverlapTest (me->m_p0, me->m_p1, l0, l1)) {
+					if (!me->m_left) {
+						dgAssert (!me->m_right);
+						dgPolyhedra::dgPairKey key (me->m_face->m_incidentVertex, me->m_face->m_twin->m_incidentVertex);
+						dgTree<dgMeshBVHNode*, dgUnsigned64>::dgTreeNode* const node = m_nodeEdgeMap.Find(key.GetVal());
+						dgAssert (node);
+						dgAssert (node->GetInfo() == me);
+					} else {
+						dgAssert (me->m_left);
+						dgAssert (me->m_right);
+						stackPool[stack] = me->m_left;
+						stack++;
+						dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+						stackPool[stack] = me->m_right;
+						stack++;
+						dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+					}
+				}
+			}
+		#endif
+		return true;
+	}
+
+
+	dgHugeVector FaceNormal (const dgEdge* const face) const
+	{
+		const dgEdge* edge = face;
+		const dgHugeVector& p0 = m_vertexAlias[edge->m_incidentVertex];
+
+		edge = edge->m_next;
+		const dgHugeVector& p1 = m_vertexAlias[edge->m_incidentVertex];
+		dgHugeVector e1 (p1 - p0);
+
+		dgHugeVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		for (edge = edge->m_next; edge != face; edge = edge->m_next) {
+			const dgHugeVector& p2 = m_vertexAlias[edge->m_incidentVertex]; 
+			dgHugeVector e2 (p2 - p0);
+			normal += e1 * e2;
+			e1 = e2;
+		} 
+		return normal;
+	}
+
+	void InsertFaceVertex (dgEdge* const face, const dgHugeVector& point)
+	{
+		dgMeshEffect::dgVertexAtribute attribute;
+		memset (&attribute, 0, sizeof (attribute));
+
+		dgGoogol tol(m_posTol);
+		const dgMeshEffect::dgVertexAtribute* const attrib = (dgMeshEffect::dgVertexAtribute*)m_mesh->GetAttributePool(); 
+		for (dgInt32 i = 0; i < 4; i ++) {
+
+			dgGoogol posTol (tol + dgGoogol::m_one);
+			dgGoogol negTol (dgGoogol::m_zero - m_posTol);
+
+			dgEdge* ptr = face;
+			dgEdge* const edge0 = ptr;
+			dgHugeVector q0 (m_vertexAlias[ptr->m_incidentVertex]);
+
+			ptr = ptr->m_next;
+			const dgEdge* edge1 = ptr;
+			dgHugeVector q1 (m_vertexAlias[ptr->m_incidentVertex]);
+
+			ptr = ptr->m_next;
+			const dgEdge* edge2 = ptr;
+			do {
+				const dgHugeVector& q2 = m_vertexAlias[ptr->m_incidentVertex];
+
+				dgHugeVector p10 (q1 - q0);
+				dgHugeVector p20 (q2 - q0);
+
+				dgGoogol dot = p20 % p10;
+				dgGoogol mag1 = p10 % p10;
+				dgGoogol mag2 = p20 % p20;
+				dgGoogol collinear = dot * dot - mag2 * mag1;
+				if (collinear.Abs() > m_posTol) {
+					dgHugeVector p_p0 (point - q0);
+					dgHugeVector p_p1 (point - q1);
+					dgHugeVector p_p2 (point - q2);
+
+					dgGoogol alpha1 = p10 % p_p0;
+					dgGoogol alpha2 = p20 % p_p0;
+					dgGoogol alpha3 = p10 % p_p1;
+					dgGoogol alpha4 = p20 % p_p1;
+					dgGoogol alpha5 = p10 % p_p2;
+					dgGoogol alpha6 = p20 % p_p2;
+
+					dgGoogol vc = alpha1 * alpha4 - alpha3 * alpha2;
+					dgGoogol vb = alpha5 * alpha2 - alpha1 * alpha6;
+					dgGoogol va = alpha3 * alpha6 - alpha5 * alpha4;
+					dgGoogol den = va + vb + vc;
+					dgGoogol minError = den * negTol;
+					dgGoogol maxError = den * posTol;
+
+					if ((va > minError) && (vb > minError) && (vc > minError) && (va < maxError) && (vb < maxError) && (vc < maxError)) {
+
+						edge2 = ptr;
+
+						dgGoogol alpha0 = va / den;
+						dgGoogol alpha1 = vb / den;
+						dgGoogol alpha2 = vc / den;
+
+						dgFloat64 falpha0 = alpha0;
+						dgFloat64 falpha1 = alpha1;
+						dgFloat64 falpha2 = alpha2;
+
+						const dgMeshEffect::dgVertexAtribute& attr0 = attrib[edge0->m_userData];
+						const dgMeshEffect::dgVertexAtribute& attr1 = attrib[edge1->m_userData];
+						const dgMeshEffect::dgVertexAtribute& attr2 = attrib[edge2->m_userData];
+						dgBigVector normal (attr0.m_normal_x * falpha0 + attr1.m_normal_x * falpha1 + attr2.m_normal_x * falpha2,
+											attr0.m_normal_y * falpha0 + attr1.m_normal_y * falpha1 + attr2.m_normal_y * falpha2,
+											attr0.m_normal_z * falpha0 + attr1.m_normal_z * falpha1 + attr2.m_normal_z * falpha2, dgFloat32 (0.0f));
+						normal = normal.Scale3 (dgFloat64 (1.0f) / sqrt (normal % normal));
+
+
+						attribute.m_vertex.m_x = point.m_x;
+						attribute.m_vertex.m_y = point.m_y;
+						attribute.m_vertex.m_z = point.m_z;
+						attribute.m_vertex.m_w = point.m_w;
+						attribute.m_normal_x = normal.m_x;
+						attribute.m_normal_y = normal.m_y;
+						attribute.m_normal_z = normal.m_z;
+						attribute.m_u0 = attr0.m_u0 * falpha0 +  attr1.m_u0 * falpha1 + attr2.m_u0 * falpha2;
+						attribute.m_v0 = attr0.m_v0 * falpha0 +  attr1.m_v0 * falpha1 + attr2.m_v0 * falpha2;
+						attribute.m_u1 = attr0.m_u1 * falpha0 +  attr1.m_u1 * falpha1 + attr2.m_u1 * falpha2;
+						attribute.m_v1 = attr0.m_v1 * falpha0 +  attr1.m_v1 * falpha1 + attr2.m_v1 * falpha2;
+
+						attribute.m_material = attr0.m_material;
+						dgAssert (attr0.m_material == attr1.m_material);
+						dgAssert (attr0.m_material == attr2.m_material);
+
+						m_mesh->AddPoint(&attribute.m_vertex.m_x, dgInt32 (attribute.m_material)); 
+						m_vertexAlias[m_mesh->GetVertexCount()-1] = point;
+						return;
+					}
+				}
+
+				q1 = q2;
+				edge1 = ptr;
+
+				ptr = ptr->m_next;
+			} while (ptr != face);
+			dgAssert (0);
+			tol = tol * dgGoogol::m_two;
+		}
+		// this should never happens
+		dgAssert (0);
+	}
+
+	void SpliteEdge(dgEdge* const edge, dgGoogol param, dgEdge** edge0, dgEdge** edge1)
+	{
+		dgInt32 v0 = edge->m_incidentVertex;
+		dgInt32 v1 = edge->m_twin->m_incidentVertex;
+		dgPolyhedra::dgPairKey edgeKey (v0, v1);
+		dgPolyhedra::dgPairKey twinKey (v1, v0);
+
+		dgEdge* const newEdge = m_mesh->InsertEdgeVertex (edge, param);
+		dgGoogol t0(dgGoogol::m_one - param);
+		dgGoogol t1(param);
+		dgInt32 v01 = newEdge->m_twin->m_incidentVertex;
+		m_vertexAlias[v01] = m_vertexAlias[v0].Scale3 (t0) + m_vertexAlias[v1].Scale3 (t1);
+
+		dgTree<dgMeshBVHNode*, dgUnsigned64>::dgTreeNode* const mapNode = m_nodeEdgeMap.Find(edgeKey.GetVal());
+		if (mapNode) {
+			dgMeshBVHNode* const node = mapNode->GetInfo();
+			node->m_face = newEdge;
+			m_nodeEdgeMap.Remove(mapNode);
+			dgPolyhedra::dgPairKey key (newEdge->m_incidentVertex, newEdge->m_twin->m_incidentVertex);
+			m_nodeEdgeMap.Insert(node, key.GetVal());
+		}
+
+		dgTree<dgMeshBVHNode*, dgUnsigned64>::dgTreeNode* const twinMapNode = m_nodeEdgeMap.Find(twinKey.GetVal());
+		if (twinMapNode) {
+			dgMeshBVHNode* const node = twinMapNode->GetInfo();
+			node->m_face = newEdge->m_twin;
+			m_nodeEdgeMap.Remove(twinMapNode);
+			dgPolyhedra::dgPairKey key (newEdge->m_twin->m_incidentVertex, newEdge->m_incidentVertex);
+			m_nodeEdgeMap.Insert(node, key.GetVal());
+		}
+
+		*edge0 = newEdge;
+		*edge1 = newEdge->m_next;
+
+		dgAssert (SanityCheck());
+	}
+
+
+
+	bool ClippMesh (const dgBooleanMeshClipper& otherMeshBVH)
+	{
+		dgMeshBVHNode* stackPool[4 * DG_MESH_EFFECT_BVH_STACK_DEPTH][2];
+	
+
+		dgInt32 stack = 1;
+		stackPool[0][0] = m_rootNode;
+		stackPool[0][1] = otherMeshBVH.m_rootNode;
+
+		dgBooleanFaceCluster bundaryClusters (m_mesh->GetAllocator());
+		while (stack) {
+			stack --;
+			dgMeshBVHNode* const me = stackPool[stack][0];
+			dgMeshBVHNode* const other = stackPool[stack][1];
+
+			dgAssert (me && other);
+			if (dgOverlapTest (me->m_p0, me->m_p1, other->m_p0, other->m_p1)) {
+
+				if (!me->m_left && !other->m_left) {
+					dgAssert (!me->m_right);
+					dgAssert (!other->m_right);
+
+					dgBooleanFaceCluster::dgTreeNode* node = bundaryClusters.Find(me);
+					if (!node) {
+						dgClusterFace tmp (m_mesh, me->m_face);
+						node = bundaryClusters.Insert(tmp, me);
+					}
+					if (!node->GetInfo().AddFace (otherMeshBVH.m_mesh, other, m_mesh, me)) {
+						if (!node->GetInfo().GetCount()) {
+							bundaryClusters.Remove(node);
+						}
+					}
+
+				} else if (!me->m_left) {
+					dgAssert (other->m_left);
+					dgAssert (other->m_right);
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+				} else if (!other->m_right) {
+					dgAssert (me->m_left);
+					dgAssert (me->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				} else {
+					dgAssert (me->m_left && me->m_right);
+					dgAssert (other->m_left && other->m_right);
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_left;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_left;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+
+					stackPool[stack][0] = me->m_right;
+					stackPool[stack][1] = other->m_right;
+					stack++;
+					dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgMeshBVHNode*)));
+				}
+			}
+		}
+
+		bool intersectionFound = false;
+		while (bundaryClusters.GetCount()) {
+			intersectionFound |= bundaryClusters.ClipCluster (bundaryClusters.GetRoot(), this, &otherMeshBVH);
+		}
+		ImproveNodeFitness();
+		return intersectionFound;
+	}
+
+
+
+	static void ClipMeshesAndColorize (dgMeshEffect* const meshA, dgMeshEffect* const meshB)
+	{
+		dgBooleanMeshClipper BVHmeshA (meshA);
+		dgBooleanMeshClipper BVHmeshB (meshB);
+
+
+		BVHmeshA.ClippMesh (BVHmeshB);
+BVHmeshA.m_mesh->SaveOFF("xxxA0.off");
+		BVHmeshB.ClippMesh (BVHmeshA);
+BVHmeshB.m_mesh->SaveOFF("xxxB0.off");
+		BVHmeshA.ClippMesh (BVHmeshB);
+		dgAssert (!BVHmeshB.ClippMesh (BVHmeshA));
+		dgAssert (!BVHmeshA.ClippMesh (BVHmeshB));
+		
+	}
+
+
+	dgInt32 m_vertexBase;
+	dgArray<dgHugeVector> m_vertexAlias;
+//	dgTree<dgInt32,dgInt32> m_vertexMap;
+	dgTree<dgMeshBVHNode*, dgUnsigned64> m_nodeEdgeMap;
+
+	static dgGoogol m_posTol;
+	static dgGoogol m_negTol;
+	static dgGoogol m_tol2;
+	static dgGoogol m_tol3;
+	static dgGoogol m_oneMinusTol;
+};
+
+dgGoogol dgBooleanMeshClipper::m_posTol ( DG_BOOLEAN_ZERO_TOLERANCE);
+dgGoogol dgBooleanMeshClipper::m_negTol (-DG_BOOLEAN_ZERO_TOLERANCE);
+dgGoogol dgBooleanMeshClipper::m_tol2 (m_posTol * m_posTol);
+dgGoogol dgBooleanMeshClipper::m_tol3 (m_posTol * m_tol2);
+dgGoogol dgBooleanMeshClipper::m_oneMinusTol (dgGoogol(1.0) - m_posTol);
+
+
+dgMeshEffect* dgMeshEffect::Intersection (const dgMatrix& matrix, const dgMeshEffect* const clipperMesh) const
+{
+	dgMeshEffect copy (*this);
+	dgMeshEffect clipper (*clipperMesh);
+	clipper.TransformMesh (matrix);
+
+	dgBooleanMeshClipper::ClipMeshesAndColorize (&copy, &clipper);
+/*
+	dgMeshEffect* const mesh = new (GetAllocator()) dgMeshEffect (GetAllocator());
+	mesh->BeginFace();
+	dgBooleanMeshClipper::CopyPoints(mesh, &copy);
+	dgBooleanMeshClipper::AddInteriorFaces (mesh, &copy);
+	dgBooleanMeshClipper::AddInteriorFaces (mesh, &clipper);
+	mesh->EndFace ();
+	mesh->RepairTJoints();
+	mesh->RemoveUnusedVertices(NULL);
+
+	return mesh;
+*/
+
+	dgAssert (0);
+	return NULL;
+}
+
+
+bool dgMeshEffect::PlaneClip (const dgMeshEffect& convexMesh, const dgEdge* const convexFace)
+{
+	dgAssert (convexFace->m_incidentFace > 0);
+	
+	dgBigVector normal (convexMesh.FaceNormal(convexFace, &convexMesh.m_points[0].m_x, sizeof(dgBigVector)));
+	dgFloat64 mag2 = normal % normal;
+	if (mag2 < dgFloat64 (1.0e-30)) {
+		dgAssert (0);
+		return true;
+	}  
+
+	normal = normal.Scale3(dgFloat64 (1.0f) / sqrt (mag2));
+	dgBigVector origin (convexMesh.m_points[convexFace->m_incidentVertex]);
+	dgBigPlane plane (normal, - (origin % normal));
+/*
+static int xxxx;
+static int xxx;
+xxx ++;
+int xxxxxxxxxx = 2028;
+if (xxx == xxxxxxxxxx){
+
+char xxxx[128];
+sprintf (xxxx, "xxx__%d.off", xxx) ;
+
+SaveOFF(xxxx);
+dgInt32 xxxxx = IncLRU();
+dgPolyhedra::Iterator iter (*this);
+for (iter.Begin(); iter; iter ++) {
+	dgEdge* const edge = &(*iter);
+
+	if (edge->m_mark != xxxxx) {
+		dgEdge* end = edge;
+		do {
+			end->m_mark = xxxxx;
+			dgTrace (("%d ", end->m_incidentVertex));
+			end = end->m_next;
+		} while (end != edge);
+		dgTrace (("\n"));
+	}
+}
+}
+*/
+	dgAssert (!HasOpenEdges());
+	
+	dgInt32 pointCount = GetVertexCount();
+	dgStack <dgFloat64> testPool (2 * pointCount + 1024);
+	dgFloat64* const test = &testPool[0];
+	for (dgInt32 i = 0; i < pointCount; i ++) {
+		test[i] = plane.Evalue (m_points[i]);
+		if (fabs (test[i]) < dgFloat32 (1.0e-5f)) {
+			test[i] = dgFloat32 (0.0f);
+		}
+	}
+
+	dgInt32 positive = 0;
+	dgInt32 negative = 0;
+	dgPolyhedra::Iterator iter (*this); 
+	for (iter.Begin(); iter && !(positive && negative); iter ++){
+		dgEdge* const edge = &(*iter);
+		positive += test[edge->m_incidentVertex] > dgFloat32 (0.0f);
+		negative += test[edge->m_incidentVertex] < dgFloat32 (0.0f);
+	}
+	if (positive  && !negative) {
+		return false;
+	}
+	
+	if (positive && negative) {
+
+        const dgEdge* e0 = convexFace;
+        const dgEdge* e1 = e0->m_next;
+        const dgEdge* e2 = e1->m_next;
+
+		dgMatrix matrix;
+		dgBigVector p1 (convexMesh.m_points[e1->m_incidentVertex]);
+
+		dgBigVector xDir (p1 - origin);
+        dgAssert ((xDir % xDir) > dgFloat32 (0.0f));
+		matrix[2] = dgVector (normal);
+		matrix[0] = dgVector(xDir.Scale3(dgFloat64 (1.0f) / sqrt (xDir% xDir)));
+		matrix[1] = matrix[2] * matrix[0];
+		matrix[3] = dgVector (origin);
+		matrix[3][3] = dgFloat32 (1.0f);
+
+		dgVector q0 (matrix.UntransformVector(dgVector(convexMesh.m_points[e0->m_incidentVertex])));
+        dgVector q1 (matrix.UntransformVector(dgVector(convexMesh.m_points[e1->m_incidentVertex])));
+        dgVector q2 (matrix.UntransformVector(dgVector(convexMesh.m_points[e2->m_incidentVertex])));
+
+        dgVector p10 (q1 - q0);
+        dgVector p20 (q2 - q0);
+        dgVector faceNormal (matrix.UnrotateVector (dgVector(normal)));
+        dgFloat32 areaInv = (p10 * p20) % faceNormal;
+        if (e2->m_next != e0) {
+            const dgEdge* edge = e2;
+            dgVector r1 (q2);
+            dgVector p10 (p20);
+            do {
+                dgVector r2 (matrix.UntransformVector(dgVector(convexMesh.m_points[edge->m_next->m_incidentVertex])));
+                dgVector p20 (r2 - q0);
+                dgFloat32 areaInv1 = (p10 * p20) % faceNormal;
+                if (areaInv1 > areaInv) {
+                    e1 = edge;
+                    e2 = edge->m_next;
+                    q1 = r1;
+                    q2 = r2;
+                    areaInv = areaInv1;
+                }
+                r1 = r2;
+                p10 = p20;
+                edge = edge->m_next;
+            } while (edge->m_next != e0);
+        }
+		
+        dgAssert (areaInv > dgFloat32 (0.0f));
+        areaInv = dgFloat32 (1.0f) / areaInv;
+
+		dgVector uv0_0 (dgFloat32 (convexMesh.m_attrib[e0->m_userData].m_u0), dgFloat32 (convexMesh.m_attrib[e0->m_userData].m_v0), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector uv0_1 (dgFloat32 (convexMesh.m_attrib[e1->m_userData].m_u0), dgFloat32 (convexMesh.m_attrib[e1->m_userData].m_v0), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector uv0_2 (dgFloat32 (convexMesh.m_attrib[e2->m_userData].m_u0), dgFloat32 (convexMesh.m_attrib[e2->m_userData].m_v0), dgFloat32 (0.0f), dgFloat32 (0.0f));
+
+		dgVector uv1_0 (dgFloat32 (convexMesh.m_attrib[e0->m_userData].m_u1), dgFloat32 (convexMesh.m_attrib[e0->m_userData].m_v1), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector uv1_1 (dgFloat32 (convexMesh.m_attrib[e1->m_userData].m_u1), dgFloat32 (convexMesh.m_attrib[e1->m_userData].m_v1), dgFloat32 (0.0f), dgFloat32 (0.0f));
+		dgVector uv1_2 (dgFloat32 (convexMesh.m_attrib[e2->m_userData].m_u1), dgFloat32 (convexMesh.m_attrib[e2->m_userData].m_v1), dgFloat32 (0.0f), dgFloat32 (0.0f));
+
+		for (iter.Begin(); iter; iter ++){
+			dgEdge* const edge = &(*iter);
+			
+			dgFloat64 side0 = test[edge->m_prev->m_incidentVertex];
+			dgFloat64 side1 = test[edge->m_incidentVertex];
+
+			if ((side0 < dgFloat32 (0.0f)) && (side1 > dgFloat64 (0.0f))) {
+				dgBigVector dp (m_points[edge->m_incidentVertex] - m_points[edge->m_prev->m_incidentVertex]);
+				dgFloat64 param = - side0 / (plane % dp);
+
+				dgEdge* const splitEdge = InsertEdgeVertex (edge->m_prev, param);
+				test[splitEdge->m_next->m_incidentVertex] = dgFloat64 (0.0f);
+			} 
+		}
+
+/*
+if (xxx == xxxxxxxxxx){
+dgInt32 xxxxx = IncLRU();
+dgPolyhedra::Iterator iter (*this);
+for (iter.Begin(); iter; iter ++) {
+	dgEdge* const edge = &(*iter);
+
+	if (edge->m_mark != xxxxx) {
+		dgEdge* end = edge;
+		do {
+			end->m_mark = xxxxx;
+			dgTrace (("%d ", end->m_incidentVertex));
+			end = end->m_next;
+		} while (end != edge);
+		dgTrace (("\n"));
+	}
+}
+}
+*/
+
+		dgInt32 colorMark = IncLRU();
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			dgFloat64 side0 = test[edge->m_incidentVertex];
+			dgFloat64 side1 = test[edge->m_next->m_incidentVertex];
+
+			if ((side0 > dgFloat32 (0.0f)) || (side1 > dgFloat64 (0.0f))) {
+				edge->m_mark = colorMark;
+			}
+		}
+
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			dgFloat64 side0 = test[edge->m_incidentVertex];
+			dgFloat64 side1 = test[edge->m_next->m_incidentVertex];
+			if ((side0 == dgFloat32 (0.0f)) && (side1 == dgFloat64 (0.0f))) {
+				dgEdge* ptr = edge->m_next;
+				do {
+					if (ptr->m_mark == colorMark) {
+						edge->m_mark = colorMark;
+						break;
+					}
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+			}
+		}
+
+
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			if ((edge->m_mark == colorMark) && (edge->m_next->m_mark < colorMark)) {
+				dgEdge* const startEdge = edge->m_next;
+				dgEdge* end = startEdge;
+				do {
+					if (end->m_mark == colorMark) {
+						break;
+					}
+
+					end = end->m_next;
+				} while (end != startEdge);
+				dgAssert (end != startEdge);
+				dgEdge* const devideEdge = ConnectVertex (startEdge, end);
+				dgAssert (devideEdge);
+				dgAssert (devideEdge->m_next->m_mark != colorMark);
+				dgAssert (devideEdge->m_prev->m_mark != colorMark);
+				dgAssert (devideEdge->m_twin->m_next->m_mark == colorMark);
+				dgAssert (devideEdge->m_twin->m_prev->m_mark == colorMark);
+				devideEdge->m_mark = colorMark - 1;
+				devideEdge->m_twin->m_mark = colorMark;
+			}
+		}
+
+		dgInt32 mark = IncLRU();
+		dgList<dgEdge*> faceList (GetAllocator());
+		for (iter.Begin(); iter; iter ++){
+			dgEdge* const face = &(*iter);
+			if ((face->m_mark >= colorMark) && (face->m_mark != mark)) {
+				faceList.Append(face);
+				dgEdge* edge = face;
+				do {
+					edge->m_mark = mark;
+					edge = edge->m_next;
+				} while (edge != face);
+			}
+		}
+
+		for (dgList<dgEdge*>::dgListNode* node = faceList.GetFirst(); node; node = node->GetNext()) {
+			dgEdge* const face = node->GetInfo();
+			DeleteFace(face);
+		}
+
+		mark = IncLRU();
+		faceList.RemoveAll();
+		for (iter.Begin(); iter; iter ++){
+			dgEdge* const face = &(*iter);
+			if ((face->m_mark != mark) && (face->m_incidentFace < 0)) {
+				faceList.Append(face);
+				dgEdge* edge = face;
+				do {
+					edge->m_mark = mark;
+					edge = edge->m_next;
+				} while (edge != face);
+			}
+		}
+
+		const dgFloat64 capAttribute = convexMesh.m_attrib[convexFace->m_userData].m_material;
+		for (dgList<dgEdge*>::dgListNode* node = faceList.GetFirst(); node; node = node->GetNext()) {
+			dgEdge* const face = node->GetInfo();
+
+			dgEdge* edge = face;
+			do {
+				dgVertexAtribute attibute;
+				attibute.m_vertex = m_points[edge->m_incidentVertex];
+				attibute.m_normal_x = normal.m_x;
+				attibute.m_normal_y = normal.m_y;
+				attibute.m_normal_z = normal.m_z;
+				attibute.m_material = capAttribute;
+
+				dgVector p (matrix.UntransformVector (attibute.m_vertex));
+
+				dgVector p_p0 (p - q0);
+				dgVector p_p1 (p - q1);
+				dgVector p_p2 (p - q2);
+				//dgFloat32 alpha1 = p10 % p_p0;
+				//dgFloat32 alpha2 = p20 % p_p0;
+				//dgFloat32 alpha3 = p10 % p_p1;
+				//dgFloat32 alpha4 = p20 % p_p1;
+				//dgFloat32 alpha5 = p10 % p_p2;
+				//dgFloat32 alpha6 = p20 % p_p2;
+				//dgFloat32 vc = alpha1 * alpha4 - alpha3 * alpha2;
+				//dgFloat32 vb = alpha5 * alpha2 - alpha1 * alpha6;
+				//dgFloat32 va = alpha3 * alpha6 - alpha5 * alpha4;
+				//dgFloat32 den = va + vb + vc;
+				//dgAssert (den > 0.0f);
+                //den = dgFloat32 (1.0f) / (va + vb + vc);
+                //dgFloat32 alpha0 = dgFloat32 (va * den);
+                //alpha1 = dgFloat32 (vb * den);
+                //alpha2 = dgFloat32 (vc * den);
+
+				dgFloat32 alpha0 = ((p_p1 * p_p2) % faceNormal) * areaInv;
+				dgFloat32 alpha1 = ((p_p2 * p_p0) % faceNormal) * areaInv;
+				dgFloat32 alpha2 = ((p_p0 * p_p1) % faceNormal) * areaInv;
+
+				attibute.m_u0 = uv0_0.m_x * alpha0 + uv0_1.m_x * alpha1 + uv0_2.m_x * alpha2; 
+				attibute.m_v0 = uv0_0.m_y * alpha0 + uv0_1.m_y * alpha1 + uv0_2.m_y * alpha2; 
+				attibute.m_u1 = uv1_0.m_x * alpha0 + uv1_1.m_x * alpha1 + uv1_2.m_x * alpha2; 
+				attibute.m_v1 = uv1_0.m_y * alpha0 + uv1_1.m_y * alpha1 + uv1_2.m_y * alpha2; 
+
+				AddAtribute (attibute);
+				edge->m_incidentFace = 1;
+				edge->m_userData = m_atribCount - 1;
+
+				//faceIndices[indexCount] = edge->m_incidentVertex;
+				//indexCount ++;
+				//dgAssert (indexCount < sizeof (faceIndices) / sizeof (faceIndices[0]));
+
+				edge = edge->m_next;
+			} while (edge != face);
+
+			//facePolygedra.AddFace(indexCount, faceIndices);
+			//facePolygedra.EndFace();
+
+			//dgPolyhedra leftOversOut(GetAllocator());
+			//facePolygedra.ConvexPartition (&m_points[0].m_x, sizeof (dgBigVector), &leftOversOut);
+			//dgAssert (leftOversOut.GetCount() == 0);
+		}
+	}
+
+	return true;
+}
+
+
+dgMeshEffect* dgMeshEffect::ConvexMeshIntersection (const dgMeshEffect* const convexMeshSrc) const
+{
+	dgMeshEffect convexMesh (*convexMeshSrc);
+	convexMesh.ConvertToPolygons();
+	//return new (GetAllocator()) dgMeshEffect (*convexMesh);
+
+	dgMeshEffect* const convexIntersection = new (GetAllocator()) dgMeshEffect (*this);
+	//convexIntersection->ConvertToPolygons();
+	//convexIntersection->Triangulate();
+	convexIntersection->RemoveUnusedVertices(NULL);
+
+	dgInt32 mark = convexMesh.IncLRU();
+	dgPolyhedra::Iterator iter (convexMesh);
+
+	for (iter.Begin(); iter; iter ++){
+		 dgEdge* const convexFace = &(*iter);
+		if ((convexFace->m_incidentFace > 0) && (convexFace->m_mark != mark)) {
+			dgEdge* ptr = convexFace;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != convexFace);
+			if (!convexIntersection->PlaneClip(convexMesh, convexFace)) {
+				delete convexIntersection;
+				return NULL;
+			}
+		}
+	}
+
+	convexIntersection->RemoveUnusedVertices(NULL);
+	if (!convexIntersection->GetVertexCount()) {
+		delete convexIntersection;
+		return NULL;
+	}
+
+	convexIntersection->RemoveUnusedVertices(NULL);
+	
+	return convexIntersection;
+}