void dgCollisionBVH::GetLocalAABB (const dgVector& p0, const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const
{
	dgAssert (0);
}

dgAABBPointTree4d* dgConvexHull4d::BuildTree (dgAABBPointTree4d* const parent, dgHullVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** memoryPool, dgInt32& maxMemSize) const
{
	dgAABBPointTree4d* tree = NULL;

	dgAssert (count);
	dgBigVector minP ( dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f)); 
	dgBigVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f)); 
	if (count <= DG_VERTEX_CLUMP_SIZE_4D) {

		dgAABBPointTree4dClump* const clump = new (*memoryPool) dgAABBPointTree4dClump;
		*memoryPool += sizeof (dgAABBPointTree4dClump);
		maxMemSize -= sizeof (dgAABBPointTree4dClump);
		dgAssert (maxMemSize >= 0);

		dgAssert (clump);
		clump->m_count = count;
		for (dgInt32 i = 0; i < count; i ++) {
			clump->m_indices[i] = i + baseIndex;

			const dgBigVector& p = points[i];
			minP.m_x = dgMin (p.m_x, minP.m_x); 
			minP.m_y = dgMin (p.m_y, minP.m_y); 
			minP.m_z = dgMin (p.m_z, minP.m_z); 
			minP.m_w = dgMin (p.m_w, minP.m_w); 

			maxP.m_x = dgMax (p.m_x, maxP.m_x); 
			maxP.m_y = dgMax (p.m_y, maxP.m_y); 
			maxP.m_z = dgMax (p.m_z, maxP.m_z); 
			maxP.m_w = dgMax (p.m_w, maxP.m_w); 
		}

		clump->m_left = NULL;
		clump->m_right = NULL;
		tree = clump;

	} else {
		dgBigVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
		dgBigVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
		for (dgInt32 i = 0; i < count; i ++) {

			const dgBigVector& p = points[i];
			minP.m_x = dgMin (p.m_x, minP.m_x); 
			minP.m_y = dgMin (p.m_y, minP.m_y); 
			minP.m_z = dgMin (p.m_z, minP.m_z); 
			minP.m_w = dgMin (p.m_w, minP.m_w); 

			maxP.m_x = dgMax (p.m_x, maxP.m_x); 
			maxP.m_y = dgMax (p.m_y, maxP.m_y); 
			maxP.m_z = dgMax (p.m_z, maxP.m_z); 
			maxP.m_w = dgMax (p.m_w, maxP.m_w); 

			median = median + p;
			varian = varian + p.CompProduct4(p);
		}

		varian = varian.Scale4 (dgFloat32 (count)) - median.CompProduct4(median);

		dgInt32 index = 0;
		dgFloat64 maxVarian = dgFloat64 (-1.0e10f);
		for (dgInt32 i = 0; i < 4; i ++) {
			if (varian[i] > maxVarian) {
				index = i;
				maxVarian = varian[i];
			}
		}
		dgBigVector center = median.Scale4 (dgFloat64 (1.0f) / dgFloat64 (count));

		dgFloat64 test = center[index];

		dgInt32 i0 = 0;
		dgInt32 i1 = count - 1;
		do {    
			for (; i0 <= i1; i0 ++) {
				dgFloat64 val = points[i0][index];
				if (val > test) {
					break;
				}
			}

			for (; i1 >= i0; i1 --) {
				dgFloat64 val = points[i1][index];
				if (val < test) {
					break;
				}
			}

			if (i0 < i1)	{
				dgSwap(points[i0], points[i1]);
				i0++; 
				i1--;
			}
		} while (i0 <= i1);

		if (i0 == 0){
			i0 = count / 2;
		}
		if (i0 >= (count - 1)){
			i0 = count / 2;
		}

//.........这里部分代码省略.........

dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
    dgAssert(proxy.m_referenceCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI));
    dgAssert(proxy.m_floatingCollision->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));

    const dgCollisionInstance* const polygonInstance = proxy.m_floatingCollision;
    dgAssert(this == polygonInstance->GetChildShape());
    dgAssert(m_count);
    dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

    dgInt32 count = 0;


    m_normal = m_normal.CompProduct4(polyInstanceInvScale);
    dgAssert(m_normal.m_w == dgFloat32(0.0f));
    m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
    dgVector savedFaceNormal(m_normal);

    dgVector savedPosit (proxy.m_matrix.m_posit);
    proxy.m_matrix.m_posit = dgVector::m_wOne;

    dgVector hullOrigin(proxy.m_matrix.UnrotateVector (savedPosit));
    for (dgInt32 i = 0; i < m_count; i++) {
        m_localPoly[i] = hullOrigin + polyInstanceScale.CompProduct4(dgVector(&m_vertex[m_vertexIndex[i] * m_stride]));
        dgAssert(m_localPoly[i].m_w == dgFloat32(0.0f));
    }

    dgContact* const contactJoint = proxy.m_contactJoint;
    const dgCollisionInstance* const hull = proxy.m_referenceCollision;

    dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
    dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
    dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
    dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));

    dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
    if (penetration < dgFloat32(0.0f)) {
        contactJoint->m_closestDistance = -penetration;
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    contactJoint->m_closestDistance = dgFloat32(0.0f);
    dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
    if (distance >= dgFloat32(0.0f)) {
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
    dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);

    bool inside = true;
    dgInt32 i0 = m_count - 1;
    for (dgInt32 i = 0; i < m_count; i++) {

        dgVector e(m_localPoly[i] - m_localPoly[i0]);
        dgVector n(m_normal * e);
        //dgPlane plane(n, -(m_localPoly[i0] % n));
        dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
        plane = proxy.m_matrix.TransformPlane(plane);

        //dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
        //dgFloat32 centerDist = plane.Evalue(boxOrigin);
        dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
        dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();

        if ((centerDist + supportDist) < dgFloat32(0.0f)) {
            proxy.m_matrix.m_posit = savedPosit;
            return 0;
        }

        if ((centerDist - supportDist) < dgFloat32(0.0f)) {
            inside = false;
            break;
        }
        i0 = i;
    }

    const dgInt32 hullId = hull->GetUserDataID();
    if (inside & !proxy.m_intersectionTestOnly) {
        dgAssert(penetration >= dgFloat32(0.0f));
        dgVector pointsContacts[64];

        dgAssert(penetration >= 0.0f);
        dgVector point(pointInHull + normalInHull.Scale4(penetration));

        count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
        dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));

        const dgMatrix& worldMatrix = hull->m_globalMatrix;
        dgContactPoint* const contactsOut = proxy.m_contacts;
        dgAssert(contactsOut);
        dgVector globalNormal(worldMatrix.RotateVector(normalInHull));
        for (dgInt32 i = 0; i < count; i++) {
            contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step);
            contactsOut[i].m_normal = globalNormal;
            contactsOut[i].m_shapeId0 = hullId;
            contactsOut[i].m_shapeId1 = m_faceId;
            contactsOut[i].m_penetration = penetration;
//.........这里部分代码省略.........

dgUnsigned32 dgSlidingConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;

	//dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));
	CalculateGlobalMatrixAndAngle (matrix0, matrix1);
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
	matrix1.m_posit += matrix1.m_front.Scale3 (m_posit);

	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); 

	const dgVector& dir1 = matrix0.m_up;
	const dgVector& dir2 = matrix0.m_right;

	dgVector p0 (matrix0.m_posit);
	dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front));

	dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));

	dgVector r0 (p0 + matrix0.m_up.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector r1 (p1 + matrix1.m_up.Scale3(MIN_JOINT_PIN_LENGTH));

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	dgPointParam pointDataR;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);
	InitPointParam (pointDataR, m_stiffness, r0, r1);

	CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); 
	CalculatePointDerivative (4, params, dir2, pointDataR, &m_jointForce[4]); 

	dgInt32 ret = 5;
	if (m_jointAccelFnt) {
		dgJointCallbackParam axisParam;
		axisParam.m_accel = dgFloat32 (0.0f);
		axisParam.m_timestep = params.m_timestep;
		axisParam.m_minFriction = DG_MIN_BOUND;
		axisParam.m_maxFriction = DG_MAX_BOUND;

		if (m_jointAccelFnt (*this, &axisParam)) {
			if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[5].m_low = axisParam.m_minFriction;
				params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
				params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

			CalculatePointDerivative (5, params, matrix0.m_front, pointDataP, &m_jointForce[5]); 
			//params.m_jointAccel[5] = axisParam.m_accel;
			SetMotorAcceleration (5, axisParam.m_accel, params);
			ret = 6;
		}
	}

	return dgUnsigned32 (ret);
}

dgQuaternion dgQuaternion::Slerp (const dgQuaternion &QB, dgFloat32 t) const 
{
dgAssert (0);
return dgQuaternion();
/*
	dgFloat32 dot;
	dgFloat32 ang;
	dgFloat32 Sclp;
	dgFloat32 Sclq;
	dgFloat32 den;
	dgFloat32 sinAng;
	dgQuaternion Q;

	dot = DotProduct (QB);

	if ((dot + dgFloat32(1.0f)) > dgEPSILON) {
		if (dot < (dgFloat32(1.0f) - dgEPSILON) ) {
			ang = dgAcos (dot);

			sinAng = dgSin (ang);
			den = dgFloat32(1.0f) / sinAng;

			Sclp = dgSin ((dgFloat32(1.0f) - t ) * ang) * den;
			Sclq = dgSin (t * ang) * den;

		} else  {
			Sclp = dgFloat32(1.0f) - t;
			Sclq = t;
		}

		Q.m_q0 = m_q0 * Sclp + QB.m_q0 * Sclq;
		Q.m_q1 = m_q1 * Sclp + QB.m_q1 * Sclq;
		Q.m_q2 = m_q2 * Sclp + QB.m_q2 * Sclq;
		Q.m_q3 = m_q3 * Sclp + QB.m_q3 * Sclq;

	} else {
		Q.m_q0 =  m_q3;
		Q.m_q1 = -m_q2;
		Q.m_q2 =  m_q1;
		Q.m_q3 =  m_q0;

		Sclp = dgSin ((dgFloat32(1.0f) - t) * dgPI * dgFloat32 (0.5f));
		Sclq = dgSin (t * dgPI * dgFloat32 (0.5f));

		Q.m_q0 = m_q0 * Sclp + Q.m_q0 * Sclq;
		Q.m_q1 = m_q1 * Sclp + Q.m_q1 * Sclq;
		Q.m_q2 = m_q2 * Sclp + Q.m_q2 * Sclq;
		Q.m_q3 = m_q3 * Sclp + Q.m_q3 * Sclq;
	}

	dot = Q.DotProduct (Q);
	if ((dot) < dgFloat32(1.0f - dgEPSILON * 10.0f) ) {
		//dot = dgFloat32(1.0f) / dgSqrt (dot);
		dot = dgRsqrt (dot);
		Q.m_q0 *= dot;
		Q.m_q1 *= dot;
		Q.m_q2 *= dot;
		Q.m_q3 *= dot;
	}
	return Q;
*/
}

void dgBody::SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia)
{
	dgFloat32 Ixx = inertia[0][0];
	dgFloat32 Iyy = inertia[1][1];
	dgFloat32 Izz = inertia[2][2];
	mass = dgAbsf (mass);
	if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI) || m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		mass = DG_INFINITE_MASS * 2.0f;
	}

	if (mass < DG_MINIMUM_MASS) {
		mass = DG_INFINITE_MASS * 2.0f;
	}

	//dgAssert (m_masterNode);
	m_world->GetBroadPhase()->CheckStaticDynamic(this, mass);

	if (mass >= DG_INFINITE_MASS) {
		m_mass.m_x = DG_INFINITE_MASS;
		m_mass.m_y = DG_INFINITE_MASS;
		m_mass.m_z = DG_INFINITE_MASS;
		m_mass.m_w = DG_INFINITE_MASS;
		m_invMass.m_x = dgFloat32 (0.0f);
		m_invMass.m_y = dgFloat32 (0.0f);
		m_invMass.m_z = dgFloat32 (0.0f);
		m_invMass.m_w = dgFloat32 (0.0f);

		if (m_masterNode) {
			dgBodyMasterList& masterList (*m_world);
			if (masterList.GetFirst() != m_masterNode) {
				masterList.InsertAfter (masterList.GetFirst(), m_masterNode);
			}
		}
		SetAparentMassMatrix (m_mass);

	} else {
		Ixx = dgAbsf (Ixx);
		Iyy = dgAbsf (Iyy);
		Izz = dgAbsf (Izz);

		dgFloat32 Ixx1 = dgClamp (Ixx, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
		dgFloat32 Iyy1 = dgClamp (Iyy, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
		dgFloat32 Izz1 = dgClamp (Izz, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);

		dgAssert (Ixx > dgFloat32 (0.0f));
		dgAssert (Iyy > dgFloat32 (0.0f));
		dgAssert (Izz > dgFloat32 (0.0f));

		m_mass.m_x = Ixx1;
		m_mass.m_y = Iyy1;
		m_mass.m_z = Izz1;
		m_mass.m_w = mass;

		m_invMass.m_x = dgFloat32 (1.0f) / Ixx1;
		m_invMass.m_y = dgFloat32 (1.0f) / Iyy1;
		m_invMass.m_z = dgFloat32 (1.0f) / Izz1;
		m_invMass.m_w = dgFloat32 (1.0f) / mass;

		if (m_masterNode) {
			dgBodyMasterList& masterList (*m_world);
			masterList.RotateToEnd (m_masterNode);
		}
		SetAparentMassMatrix (dgVector (Ixx, Iyy, Izz, mass));
	}

#ifdef _DEBUG
	dgBodyMasterList& me = *m_world;
	for (dgBodyMasterList::dgListNode* refNode = me.GetFirst(); refNode; refNode = refNode->GetNext()) {
		dgBody* const body0 = refNode->GetInfo().GetBody();
		dgVector invMass (body0->GetInvMass());
		if (invMass.m_w != 0.0f) {
			for (; refNode; refNode = refNode->GetNext()) {
				dgBody* const body1 = refNode->GetInfo().GetBody();
				dgVector invMass (body1->GetInvMass());
				dgAssert (invMass.m_w != 0.0f);
			}
			break;
		}
	}
#endif
}

bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
	dgInt32 stride = strideInBytes / sizeof (dgFloat32);
	dgStack<dgFloat64> buffer(3 * 2 * count);
	for (dgInt32 i = 0; i < count; i ++) {
		buffer[i * 3 + 0] = vertexArray[i * stride + 0];
		buffer[i * 3 + 1] = vertexArray[i * stride + 1];
		buffer[i * 3 + 2] = vertexArray[i * stride + 2];
	}

	dgConvexHull3d* convexHull =  new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
	if (!convexHull->GetCount()) {
		// this is a degenerated hull hull to add some thickness and for a thick plane
		delete convexHull;

		dgStack<dgVector> tmp(3 * count);
		for (dgInt32 i = 0; i < count; i ++) {
			tmp[i][0] = dgFloat32 (buffer[i*3 + 0]);
			tmp[i][1] = dgFloat32 (buffer[i*3 + 1]);
			tmp[i][2] = dgFloat32 (buffer[i*3 + 2]);
			tmp[i][2] = dgFloat32 (0.0f);
		}
	
		dgObb sphere;
		sphere.SetDimensions (&tmp[0][0], sizeof (dgVector), count);

		dgInt32 index = 0;
		dgFloat32 size = dgFloat32 (1.0e10f);
		for (dgInt32 i = 0; i < 3; i ++) {
			if (sphere.m_size[i] < size) {
				index = i;
				size = sphere.m_size[i];
			}
		}
		dgVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
		normal[index] = dgFloat32 (1.0f);
		dgVector step = sphere.RotateVector (normal.Scale3 (dgFloat32 (0.05f)));
		for (dgInt32 i = 0; i < count; i ++) {
			dgVector p1 (tmp[i] + step);
			dgVector p2 (tmp[i] - step);

			buffer[i * 3 + 0] = p1.m_x;
			buffer[i * 3 + 1] = p1.m_y;
			buffer[i * 3 + 2] = p1.m_z;
			buffer[(i + count) * 3 + 0] = p2.m_x;
			buffer[(i + count) * 3 + 1] = p2.m_y;
			buffer[(i + count) * 3 + 2] = p2.m_z;
		}
		count *= 2;
		convexHull =  new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
		if (!convexHull->GetCount()) {
			delete convexHull;
			return false;
		}
	}

	// check for degenerated faces
	for (bool success = false; !success;  ) {
		success = true;
		const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();

		dgStack<dgInt8> mask(convexHull->GetVertexCount());
		memset (&mask[0], 1, mask.GetSizeInBytes());
		for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
			dgConvexHull3DFace& face = node->GetInfo();
			const dgBigVector& p0 = hullVertexArray[face.m_index[0]];
			const dgBigVector& p1 = hullVertexArray[face.m_index[1]];
			const dgBigVector& p2 = hullVertexArray[face.m_index[2]];
			dgBigVector p1p0 (p1 - p0);
			dgBigVector p2p0 (p2 - p0);
			dgBigVector normal (p2p0 * p1p0);
			dgFloat64 mag2 = normal % normal;
			if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) {
				success = false;
				dgInt32 index = -1;
				dgBigVector p2p1 (p2 - p1);
				dgFloat64 dist10 = p1p0 % p1p0;
				dgFloat64 dist20 = p2p0 % p2p0;
				dgFloat64 dist21 = p2p1 % p2p1;
				if ((dist10 >= dist20) && (dist10 >= dist21)) {
					index = 2;
				} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
					index = 1;
				} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
					index = 0;
				}
				dgAssert (index != -1);
				mask[face.m_index[index]] = 0;
			}
		}
		if (!success) {
			dgInt32 count = 0;
			dgInt32 vertexCount = convexHull->GetVertexCount();
			for (dgInt32 i = 0; i < vertexCount; i ++) {
				if (mask[i]) {
					buffer[count * 3 + 0] = hullVertexArray[i].m_x;
					buffer[count * 3 + 1] = hullVertexArray[i].m_y;
					buffer[count * 3 + 2] = hullVertexArray[i].m_z;
					count ++;
				}
//.........这里部分代码省略.........

dgVector dgCollisionConvexHull::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
	dgAssert (dir.m_w == dgFloat32 (0.0f));
	dgInt32 index = -1;
	dgVector maxProj (dgFloat32 (-1.0e20f)); 
	if (m_vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
		dgFloat32 distPool[32];
		const dgConvexBox* stackPool[32];

		dgInt32 ix = (dir[0] > dgFloat64 (0.0f)) ? 1 : 0;
		dgInt32 iy = (dir[1] > dgFloat64 (0.0f)) ? 1 : 0;
		dgInt32 iz = (dir[2] > dgFloat64 (0.0f)) ? 1 : 0;

		const dgConvexBox& leftBox = m_supportTree[m_supportTree[0].m_leftBox];
		const dgConvexBox& rightBox = m_supportTree[m_supportTree[0].m_rightBox];
		
		dgVector leftP (leftBox.m_box[ix][0], leftBox.m_box[iy][1], leftBox.m_box[iz][2], dgFloat32 (0.0f));
		dgVector rightP (rightBox.m_box[ix][0], rightBox.m_box[iy][1], rightBox.m_box[iz][2], dgFloat32 (0.0f));

		dgFloat32 leftDist = leftP.DotProduct4(dir).m_x;
		dgFloat32 rightDist = rightP.DotProduct4(dir).m_x;
		if (rightDist >= leftDist) {
			distPool[0] = leftDist;
			stackPool[0] = &leftBox; 

			distPool[1] = rightDist;
			stackPool[1] = &rightBox; 
		} else {
			distPool[0] = rightDist;
			stackPool[0] = &rightBox; 

			distPool[1] = leftDist;
			stackPool[1] = &leftBox; 
		}
		
		dgInt32 stack = 2;
		
		while (stack) {
			stack--;
			dgFloat32 dist = distPool[stack];
			if (dist > maxProj.m_x) {
				const dgConvexBox& box = *stackPool[stack];

				if (box.m_leftBox > 0) {
					dgAssert (box.m_rightBox > 0);
					const dgConvexBox& leftBox = m_supportTree[box.m_leftBox];
					const dgConvexBox& rightBox = m_supportTree[box.m_rightBox];

					dgVector leftP (leftBox.m_box[ix][0], leftBox.m_box[iy][1], leftBox.m_box[iz][2], dgFloat32 (0.0f));
					dgVector rightP (rightBox.m_box[ix][0], rightBox.m_box[iy][1], rightBox.m_box[iz][2], dgFloat32 (0.0f));

					dgFloat32 leftDist = leftP.DotProduct4(dir).m_x;
					dgFloat32 rightDist = rightP.DotProduct4(dir).m_x;
					if (rightDist >= leftDist) {
						distPool[stack] = leftDist;
						stackPool[stack] = &leftBox; 
						stack ++;
						dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));

						distPool[stack] = rightDist;
						stackPool[stack] = &rightBox; 
						stack ++;
						dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));

					} else {
						distPool[stack] = rightDist;
						stackPool[stack] = &rightBox; 
						stack ++;
						dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));

						distPool[stack] = leftDist;
						stackPool[stack] = &leftBox; 
						stack ++;
						dgAssert (stack < sizeof (distPool)/sizeof (distPool[0]));
					}
				} else {
					for (dgInt32 i = 0; i < box.m_vertexCount; i ++) {
						const dgVector& p = m_vertex[box.m_vertexStart + i];
						dgAssert (p.m_x >= box.m_box[0].m_x);
						dgAssert (p.m_x <= box.m_box[1].m_x);
						dgAssert (p.m_y >= box.m_box[0].m_y);
						dgAssert (p.m_y <= box.m_box[1].m_y);
						dgAssert (p.m_z >= box.m_box[0].m_z);
						dgAssert (p.m_z <= box.m_box[1].m_z);
						dgVector dist (p.DotProduct4(dir));
						//if (dist.m_x > maxProj.m_x) {
						//	maxProj = dist;
						//	index = box.m_vertexStart + i;
						//}
						dgVector mask (dist > maxProj);
						dgInt32 intMask = *((dgInt32*) &mask.m_x);
						index = ((box.m_vertexStart + i) & intMask) | (index & ~intMask);
						maxProj = maxProj.GetMax(dist);
					}
				}
			}
		}
	} else {
		for (dgInt32 i = 0; i < m_vertexCount; i ++) {
			const dgVector& p = m_vertex[i];
//.........这里部分代码省略.........

bool dgCollisionConvexHull::RemoveCoplanarEdge (dgPolyhedra& polyhedra, const dgBigVector* const hullVertexArray) const
{
	bool removeEdge = false;
	// remove coplanar edges
	dgInt32 mark = polyhedra.IncLRU();
	dgPolyhedra::Iterator iter (polyhedra);
	for (iter.Begin(); iter; ) {
		dgEdge* edge0 = &(*iter);
		iter ++;

		if (edge0->m_incidentFace != -1) {

			if (edge0->m_mark < mark) {
				edge0->m_mark = mark;
				edge0->m_twin->m_mark = mark;
				dgBigVector normal0 (FaceNormal (edge0, &hullVertexArray[0]));
				dgBigVector normal1 (FaceNormal (edge0->m_twin, &hullVertexArray[0]));

				dgFloat64 test = normal0 % normal1;
				if (test > dgFloat64 (0.99995f)) {

					if ((edge0->m_twin->m_next->m_twin->m_next != edge0) && (edge0->m_next->m_twin->m_next != edge0->m_twin)) {
						#define DG_MAX_EDGE_ANGLE dgFloat32 (1.0e-3f)

						if (edge0->m_twin == &(*iter)) {
							if (iter) {
								iter ++;
							}
						}

						dgBigVector e1 (hullVertexArray[edge0->m_twin->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_incidentVertex]);
						dgBigVector e0 (hullVertexArray[edge0->m_incidentVertex] - hullVertexArray[edge0->m_prev->m_incidentVertex]);

						dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
						dgAssert ((e1 % e1) >= dgFloat64 (0.0f));

						e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
						e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));
						dgBigVector n1 (e0 * e1);

						dgFloat64 projection = n1 % normal0;
						if (projection >= DG_MAX_EDGE_ANGLE) {

							dgBigVector e1 (hullVertexArray[edge0->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_incidentVertex]);
							dgBigVector e0 (hullVertexArray[edge0->m_twin->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_prev->m_incidentVertex]);
							dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
							dgAssert ((e1 % e1) >= dgFloat64 (0.0f));
							//e0 = e0.Scale3 (dgRsqrt (e0 % e0));
							//e1 = e1.Scale3 (dgRsqrt (e1 % e1));
							e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
							e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));

							dgBigVector n1 (e0 * e1);
							projection = n1 % normal0;
							if (projection >= DG_MAX_EDGE_ANGLE) {
								dgAssert (&(*iter) != edge0);
								dgAssert (&(*iter) != edge0->m_twin);
								polyhedra.DeleteEdge(edge0);
								removeEdge = true;
							}
						}

					} else {
						dgEdge* next = edge0->m_next;
						dgEdge* prev = edge0->m_prev;
						polyhedra.DeleteEdge(edge0);
						for (edge0 = next; edge0->m_prev->m_twin == edge0; edge0 = next) {
							next = edge0->m_next;
							polyhedra.DeleteEdge(edge0);
						}

						for (edge0 = prev; edge0->m_next->m_twin == edge0; edge0 = prev) {
							prev = edge0->m_prev;
							polyhedra.DeleteEdge(edge0);
						}
						iter.Begin(); 
						removeEdge = true;
					}
				}
			}
		}
	}

	return removeEdge;
}

dgMeshEffect * dgMeshEffect::CreateVoronoiConvexDecomposition (dgMemoryAllocator * const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32 * const pointCloud, dgInt32 materialId, const dgMatrix & textureProjectionMatrix)
{
   dgFloat32 normalAngleInRadians = 30.0f * 3.1416f / 180.0f;
   dgStack<dgBigVector> buffer (pointCount + 16);
   dgBigVector * const pool = &buffer[0];
   dgInt32 count = 0;
   dgFloat64 quantizeFactor = dgFloat64 (16.0f);
   dgFloat64 invQuantizeFactor = dgFloat64 (1.0f) / quantizeFactor;
   dgInt32 stride = pointStrideInBytes / sizeof (dgFloat32);
   dgBigVector pMin (dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (0.0f));
   dgBigVector pMax (dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (0.0f));
   for (dgInt32 i = 0; i < pointCount; i ++)
   {
      dgFloat64 x = pointCloud[i * stride + 0];
      dgFloat64 y	= pointCloud[i * stride + 1];
      dgFloat64 z	= pointCloud[i * stride + 2];
      x = floor (x * quantizeFactor) * invQuantizeFactor;
      y = floor (y * quantizeFactor) * invQuantizeFactor;
      z = floor (z * quantizeFactor) * invQuantizeFactor;
      dgBigVector p (x, y, z, dgFloat64 (0.0f));
      pMin = dgBigVector (dgMin (x, pMin.m_x), dgMin (y, pMin.m_y), dgMin (z, pMin.m_z), dgFloat64 (0.0f));
      pMax = dgBigVector (dgMax (x, pMax.m_x), dgMax (y, pMax.m_y), dgMax (z, pMax.m_z), dgFloat64 (0.0f));
      pool[count] = p;
      count ++;
   }
   // add the bbox as a barrier
   pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
   pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
   pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
   pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
   count += 8;
   dgStack<dgInt32> indexList (count);
   count = dgVertexListToIndexList (&pool[0].m_x, sizeof (dgBigVector), 3, count, &indexList[0], dgFloat64 (5.0e-2f));
   dgAssert (count >= 8);
   dgFloat64 maxSize = dgMax (pMax.m_x - pMin.m_x, pMax.m_y - pMin.m_y, pMax.m_z - pMin.m_z);
   pMin -= dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f));
   pMax += dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f));
   // add the a guard zone, so that we do no have to clip
   dgInt32 guadVertexKey = count;
   pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
   pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
   pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
   pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
   pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
   count += 8;
   dgDelaunayTetrahedralization delaunayTetrahedras (allocator, &pool[0].m_x, count, sizeof (dgBigVector), dgFloat32 (0.0f));
   delaunayTetrahedras.RemoveUpperHull ();
   //	delaunayTetrahedras.Save("xxx0.txt");
   dgInt32 tetraCount = delaunayTetrahedras.GetCount();
   dgStack<dgBigVector> voronoiPoints (tetraCount + 32);
   dgStack<dgDelaunayTetrahedralization::dgListNode *> tetradrumNode (tetraCount);
   dgTree<dgList<dgInt32>, dgInt32> delanayNodes (allocator);
   dgInt32 index = 0;
   const dgHullVector * const delanayPoints = delaunayTetrahedras.GetHullVertexArray();
   for (dgDelaunayTetrahedralization::dgListNode * node = delaunayTetrahedras.GetFirst(); node; node = node->GetNext())
   {
      dgConvexHull4dTetraherum & tetra = node->GetInfo();
      voronoiPoints[index] = tetra.CircumSphereCenter (delanayPoints);
      tetradrumNode[index] = node;
      for (dgInt32 i = 0; i < 4; i ++)
      {
         dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * header = delanayNodes.Find (tetra.m_faces[0].m_index[i]);
         if (!header)
         {
            dgList<dgInt32> list (allocator);
            header = delanayNodes.Insert (list, tetra.m_faces[0].m_index[i]);
         }
         header->GetInfo().Append (index);
      }
      index ++;
   }
   dgMeshEffect * const voronoiPartition = new (allocator) dgMeshEffect (allocator);
   voronoiPartition->BeginPolygon();
   dgFloat64 layer = dgFloat64 (0.0f);
   dgTree<dgList<dgInt32>, dgInt32>::Iterator iter (delanayNodes);
   for (iter.Begin(); iter; iter ++)
   {
      dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * const nodeNode = iter.GetNode();
      const dgList<dgInt32> & list = nodeNode->GetInfo();
      dgInt32 key = nodeNode->GetKey();
      if (key < guadVertexKey)
      {
         dgBigVector pointArray[512];
         dgInt32 indexArray[512];
         dgInt32 count = 0;
         for (dgList<dgInt32>::dgListNode * ptr = list.GetFirst(); ptr; ptr = ptr->GetNext())
         {
            dgInt32 i = ptr->GetInfo();
            pointArray[count] = voronoiPoints[i];
            count ++;
            dgAssert (count < dgInt32 (sizeof (pointArray) / sizeof (pointArray[0])));
         }
         count = dgVertexListToIndexList (&pointArray[0].m_x, sizeof (dgBigVector), 3, count, &indexArray[0], dgFloat64 (1.0e-3f));
//.........这里部分代码省略.........

dgMeshEffect * dgMeshEffect::CreateDelaunayTetrahedralization (dgMemoryAllocator * const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32 * const pointCloud, dgInt32 materialId, const dgMatrix & textureProjectionMatrix)
{
   dgAssert (0);
   return NULL;
}

void dgCollisionConvexPolygon::BeamClipping (const dgVector& origin, dgFloat32 dist)
{
    dgPlane planes[4];
    dgVector points[sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];
    dgClippedFaceEdge clippedFace [2 * sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];

    dgVector dir (m_localPoly[1] - m_localPoly[0]);
    dgAssert (dir.m_w == dgFloat32 (0.0f));
    dgAssert ((dir % dir) > dgFloat32 (1.0e-8f));
    dir = dir.CompProduct4 (dir.InvMagSqrt());

    dgFloat32 distH = origin.DotProduct4(dir).GetScalar();
    planes[0] = dgPlane (dir, dist - distH);
    planes[2] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distH);

    dir = m_normal * dir;
    dgFloat32 distV = origin.DotProduct4(dir).GetScalar();
    planes[1] = dgPlane (dir, dist - distV);
    planes[3] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distV);

    for (dgInt32 i = 0; i < m_count; i ++) {
        dgInt32 j = i << 1;
        dgAssert (j < sizeof (clippedFace) / sizeof (clippedFace[0]));

        points[i] = m_localPoly[i];

        clippedFace[j + 0].m_twin = &clippedFace[j + 1];
        clippedFace[j + 0].m_next = &clippedFace[j + 2];
        clippedFace[j + 0].m_incidentVertex = i;
        clippedFace[j + 0].m_incidentNormal = m_adjacentFaceEdgeNormalIndex[i];

        clippedFace[j + 1].m_twin = &clippedFace[j + 0];
        clippedFace[j + 1].m_next = &clippedFace[j - 2];
        clippedFace[j + 1].m_incidentVertex = i + 1;
        clippedFace[j + 1].m_incidentNormal = -1;
    }

    clippedFace[1].m_next = &clippedFace[m_count * 2 - 2 + 1];
    dgAssert ((m_count * 2 - 2) >= 0);
    clippedFace[m_count * 2 - 2].m_next = &clippedFace[0];
    clippedFace[m_count * 2 - 2 + 1].m_incidentVertex = 0;

    dgInt32 edgeCount = m_count * 2;
    dgInt32 indexCount = m_count;
    dgClippedFaceEdge* first = &clippedFace[0];
    for (dgInt32 i = 0; i < 4; i ++) {
        const dgPlane& plane = planes[i];

        dgInt32 conectCount = 0;
        dgClippedFaceEdge* connect[2];
        dgClippedFaceEdge* ptr = first;
        dgClippedFaceEdge* newFirst = first;
        dgFloat32 test0 = plane.Evalue(points[ptr->m_incidentVertex]);
        do {
            dgFloat32 test1 = plane.Evalue(points[ptr->m_next->m_incidentVertex]);

            if (test0 > dgFloat32 (1.0e-2f)) {
                if (test1 <= dgFloat32 (-1.0e-2f)) {
                    const dgVector& p0 = points[ptr->m_incidentVertex];
                    const dgVector& p1 = points[ptr->m_next->m_incidentVertex];
                    dgVector dp (p1 - p0);
                    points[indexCount] = p0 - dp.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());

                    dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
                    newEdge->m_twin = newEdge + 1;
                    newEdge->m_twin->m_twin = newEdge;

                    newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;
                    newEdge->m_incidentNormal = ptr->m_incidentNormal;

                    newEdge->m_incidentVertex = indexCount;
                    newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex;
                    ptr->m_twin->m_incidentVertex = indexCount;

                    newEdge->m_next = ptr->m_next;
                    ptr->m_next->m_twin->m_next = newEdge->m_twin;
                    newEdge->m_twin->m_next = ptr->m_twin;
                    ptr->m_next = newEdge;

                    connect[conectCount] = ptr;
                    conectCount ++;
                    indexCount ++;
                    edgeCount += 2;
                    ptr = newEdge;
                }
            } else {
                if (test1 > dgFloat32 (1.0e-2f)) {
                    newFirst = ptr->m_next;

                    const dgVector& p0 = points[ptr->m_incidentVertex];
                    const dgVector& p1 = points[ptr->m_next->m_incidentVertex];
                    dgVector dp (p1 - p0);
                    points[indexCount] = p0 - dp.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());

                    dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
                    newEdge->m_twin = newEdge + 1;
                    newEdge->m_twin->m_twin = newEdge;

                    newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;;
                    newEdge->m_incidentNormal = ptr->m_incidentNormal;
//.........这里部分代码省略.........

dgBody::dgBody (dgWorld* const world, const dgTree<const dgCollision*, dgInt32>* const collisionCashe, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber)
	:m_invWorldInertiaMatrix(dgGetZeroMatrix())
	,m_matrix (dgGetIdentityMatrix())
	,m_rotation(dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
	,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f))
	,m_invMass(dgFloat32 (0.0))
	,m_veloc(dgFloat32 (0.0))
	,m_omega(dgFloat32 (0.0))
	,m_minAABB(dgFloat32 (0.0))
	,m_maxAABB(dgFloat32 (0.0))
	,m_netForce(dgFloat32 (0.0))
	,m_netTorque(dgFloat32 (0.0))
	,m_localCentreOfMass(dgFloat32 (0.0))	
	,m_globalCentreOfMass(dgFloat32 (0.0))	
	,m_aparentMass(dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS))
	,m_maxAngulaRotationPerSet2(DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP )
	,m_criticalSectionLock()
	,m_flags(0)
	,m_userData(NULL)
	,m_world(world)
	,m_collision(NULL)
	,m_broadPhaseNode(NULL)
	,m_masterNode(NULL)
	,m_broadPhaseaggregateNode(NULL)
	,m_destructor(NULL)
	,m_matrixUpdate(NULL)
	,m_index(0)
	,m_uniqueID(0)
	,m_bodyGroupId(0)
	,m_rtti(m_baseBodyRTTI)
	,m_type(0)
	,m_dynamicsLru(0)
	,m_genericLRUMark(0)
{
	m_autoSleep = true;
	m_collidable = true;
	m_collideWithLinkedBodies = true;
	m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);

	serializeCallback (userData, &m_rotation, sizeof (m_rotation));
	serializeCallback (userData, &m_matrix.m_posit, sizeof (m_matrix.m_posit));
	serializeCallback (userData, &m_veloc, sizeof (m_veloc));
	serializeCallback (userData, &m_omega, sizeof (m_omega));
	serializeCallback (userData, &m_localCentreOfMass, sizeof (m_localCentreOfMass));
	serializeCallback (userData, &m_aparentMass, sizeof (m_aparentMass));
	serializeCallback (userData, &m_flags, sizeof (m_flags));
	serializeCallback (userData, &m_maxAngulaRotationPerSet2, sizeof (m_maxAngulaRotationPerSet2));

	m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);

	dgInt32 id;
	serializeCallback (userData, &id, sizeof (id));

	dgTree<const dgCollision*, dgInt32>::dgTreeNode* const node = collisionCashe->Find(id);
	dgAssert (node);

	const dgCollision* const collision = node->GetInfo();
	collision->AddRef();

	dgCollisionInstance* const instance = new (world->GetAllocator()) dgCollisionInstance (world, serializeCallback, userData, revisionNumber);
	instance->m_childShape = collision;
	m_collision = instance;
}

dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
    dgVector normal(normalIn);
    dgInt32 count = 0;
    dgFloat32 maxDist = dgFloat32 (1.0f);
    dgFloat32 projectFactor = m_normal % normal;
    if (projectFactor < dgFloat32 (0.0f)) {
        projectFactor *= dgFloat32 (-1.0f);
        normal = normal.Scale3 (dgFloat32 (-1.0f));
    }

    if (projectFactor > dgFloat32 (0.9999f)) {
        for (dgInt32 i = 0; i < m_count; i ++) {
            contactsOut[count] = m_localPoly[i];
            count ++;
        }

#ifdef _DEBUG
        dgInt32 j = count - 1;
        for (dgInt32 i = 0; i < count; i ++) {
            dgVector error (contactsOut[i] - contactsOut[j]);
            dgAssert ((error % error) > dgFloat32 (1.0e-20f));
            j = i;
        }
#endif

    } else if (projectFactor > dgFloat32 (0.1736f)) {
        maxDist = dgFloat32 (0.0f);
        dgPlane plane (normal, - (normal % origin));

        dgVector p0 (m_localPoly[m_count - 1]);
        dgFloat32 side0 = plane.Evalue (p0);
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector p1 (m_localPoly[i]);
            dgFloat32 side1 = plane.Evalue (p1);

            if (side0 > dgFloat32 (0.0f)) {
                maxDist = dgMax (maxDist, side0);
                contactsOut[count] = p0 - plane.Scale3 (side0);
                count ++;
                if (count > 1) {
                    dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                    dgFloat32 error = edgeSegment % edgeSegment;
                    if (error < dgFloat32 (1.0e-8f)) {
                        count --;
                    }
                }

                if (side1 <= dgFloat32 (0.0f)) {
                    dgVector dp (p1 - p0);
                    dgFloat32 t = plane % dp;
                    dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
                    if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
                        t = dgSign(t) * dgFloat32 (1.0e-8f);
                    }
                    contactsOut[count] = p0 - dp.Scale3 (side0 / t);
                    count ++;
                    if (count > 1) {
                        dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                        dgFloat32 error = edgeSegment % edgeSegment;
                        if (error < dgFloat32 (1.0e-8f)) {
                            count --;
                        }
                    }
                }
            } else if (side1 > dgFloat32 (0.0f)) {
                dgVector dp (p1 - p0);
                dgFloat32 t = plane % dp;
                dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
                if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
                    t = dgSign(t) * dgFloat32 (1.0e-8f);
                }
                contactsOut[count] = p0 - dp.Scale3 (side0 / t);
                count ++;
                if (count > 1) {
                    dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                    dgFloat32 error = edgeSegment % edgeSegment;
                    if (error < dgFloat32 (1.0e-8f)) {
                        count --;
                    }
                }
            }

            side0 = side1;
            p0 = p1;
        }
    } else {
        maxDist = dgFloat32 (1.0e10f);
        dgPlane plane (normal, - (normal % origin));

        dgVector p0 (m_localPoly[m_count - 1]);
        dgFloat32 side0 = plane.Evalue (p0);
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector p1 (m_localPoly[i]);
            dgFloat32 side1 = plane.Evalue (p1);

            if ((side0 * side1) < dgFloat32 (0.0f)) {
                dgVector dp (p1 - p0);
                dgFloat32 t = plane % dp;
                dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
//.........这里部分代码省略.........

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);
				dgFloat