// calculate the closest point from the spline to point point
dMatrix CustomPathFollow::EvalueCurve (const dVector& posit)
{
	dMatrix matrix;


	// calculate distance for point to list
	matrix.m_posit = m_pointOnPath + m_pathTangent.Scale ((posit - m_pointOnPath) % m_pathTangent);
	matrix.m_posit.m_w = 1.0f;


	//the tangent of the path is the line slope, passes in the first matrix dir
	matrix.m_front = m_pathTangent;

	// the normal will be such that it is horizontal to the the floor and perpendicular to the path 
	dVector normal (dVector (0.0f, 1.0f, 0.0f, 0.0f) * matrix.m_front);
	matrix.m_up = normal.Scale (1.0f / dSqrt (normal % matrix.m_front));

	// the binormal is just the cross product;
	matrix.m_right = matrix.m_front * matrix.m_up;

	return matrix;
}

// add force and torque to rigid body
void  PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;

	dVector force (dVector (0.0f, 1.0f, 0.0f).Scale (mass * DEMO_GRAVITY));
	NewtonBodySetForce (body, &force.m_x);
/*
// check that angular momentum is conserved
dMatrix I;
dVector omega(0.0f);
NewtonBodyGetInertiaMatrix(body, &I[0][0]);
NewtonBodyGetOmega(body, &omega[0]);
dVector L (I.RotateVector(omega));
dTrace (("(%f %f %f) (%f %f %f)\n", omega[0], omega[1], omega[2], L[0], L[1], L[2]));
*/
}

static int MakeRandomPoisonPointCloud(NewtonMesh* const mesh, dVector* const points)
{
	dVector size(0.0f);
	dMatrix matrix(dGetIdentityMatrix()); 
	NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);

	dVector minBox (matrix.m_posit - matrix[0].Scale (size.m_x) - matrix[1].Scale (size.m_y) - matrix[2].Scale (size.m_z));
	dVector maxBox (matrix.m_posit + matrix[0].Scale (size.m_x) + matrix[1].Scale (size.m_y) + matrix[2].Scale (size.m_z));

	size = maxBox - minBox;
	int xCount = int (size.m_x / POINT_DENSITY_PER_METERS) + 1;
	int yCount = int (size.m_y / POINT_DENSITY_PER_METERS) + 1;
	int zCount = int (size.m_z / POINT_DENSITY_PER_METERS) + 1;

	int count = 0;
	dFloat z0 = minBox.m_z;
	for (int iz = 0; (iz < zCount) && (count < MAX_POINT_CLOUD_SIZE); iz ++) {
		dFloat y0 = minBox.m_y;
		for (int iy = 0; (iy < yCount) && (count < MAX_POINT_CLOUD_SIZE); iy ++) {
			dFloat x0 = minBox.m_x;
			for (int ix = 0; (ix < xCount) && (count < MAX_POINT_CLOUD_SIZE); ix ++) {

				dFloat x = x0;
				dFloat y = y0;
				dFloat z = z0;
				x += RandomVariable(POISON_VARIANCE);
				y += RandomVariable(POISON_VARIANCE);
				z += RandomVariable(POISON_VARIANCE);
				points[count] = dVector (x, y, z);
				count ++;
				x0 += POINT_DENSITY_PER_METERS;
			}
			y0 += POINT_DENSITY_PER_METERS;
		}
		z0 += POINT_DENSITY_PER_METERS;
	}

	return count;
}

	void UpdateCamera (dFloat timestep)
	{
		if (m_player) {
			DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(GetWorld());
			DemoCamera* const camera = scene->GetCamera();
			dMatrix camMatrix (camera->GetNextMatrix ());
			dMatrix playerMatrix (m_player->GetNextMatrix());

			dVector frontDir (camMatrix[0]);
			dVector camOrigin; 
			if (m_externalView) {
				camOrigin = playerMatrix.m_posit + dVector(0.0f, VEHICLE_THIRD_PERSON_VIEW_HIGHT, 0.0f, 0.0f);
				camOrigin -= frontDir.Scale (VEHICLE_THIRD_PERSON_VIEW_DIST);
			} else {
				dAssert (0);
				//            camMatrix = camMatrix * playerMatrix;
				//            camOrigin = playerMatrix.TransformVector(dVector(-0.8f, ARTICULATED_VEHICLE_CAMERA_EYEPOINT, 0.0f, 0.0f));
			}

			camera->SetNextMatrix (*scene, camMatrix, camOrigin);
		}
	}

void dCustomJoint::dDebugDisplay::DrawFrame(const dMatrix& matrix)
{
	dVector o0(matrix.m_posit);

	dFloat size = 0.25f;
	dVector x(matrix.m_posit + matrix.RotateVector(dVector(size, 0.0f, 0.0f, 0.0f)));
	SetColor(dVector (1.0f, 0.0f, 0.0f));
	DrawLine (matrix.m_posit, x);

	dVector y(matrix.m_posit + matrix.RotateVector(dVector(0.0f, size, 0.0f, 0.0f)));
	SetColor(dVector (0.0f, 1.0f, 0.0f));
	DrawLine (matrix.m_posit, y);

	dVector z(matrix.m_posit + matrix.RotateVector(dVector(0.0f, 0.0f, size, 0.0f)));
	SetColor(dVector (0.0f, 0.0f, 1.0f));
	DrawLine (matrix.m_posit, z);
}

void UserPlaneCollision (DemoEntityManager* const scene)
{
	// load the sky box
	scene->CreateSkyBox();

	//	create a user define infinite plane collision, and use it as a floor
	CreatePlaneCollision (scene, dVector (0.0f, 1.0f, 0.0f, 0.0f));


	dMatrix camMatrix (dRollMatrix(-20.0f * dDegreeToRad) * dYawMatrix(-45.0f * dDegreeToRad));
	dQuaternion rot (camMatrix);
	dVector origin (0.0f, 0.0f, 0.0f, 0.0f);
//	dFloat hight = 1000.0f;
//	origin = FindFloor (scene->GetNewton(), dVector (origin.m_x, hight, origin .m_z, 0.0f), hight * 2);
	origin.m_y += 10.0f;

	scene->SetCameraMatrix(rot, origin);

	int defaultMaterialID = NewtonMaterialGetDefaultGroupID (scene->GetNewton());
	dVector location (origin);
	location.m_x += 20.0f;
	location.m_z += 20.0f;
//	dVector size (0.5f, 0.5f, 0.75f, 0.0f);
	dVector size (1.0f, 1.0f, 1.0f, 0.0f);

	int count = 1;
	dMatrix shapeOffsetMatrix (dGetIdentityMatrix());
	AddUniformScaledPrimitives(scene, 10.0f, location, size, count, count, 4.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
//	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
}

void SimpleMeshLevelCollision (NewtonFrame& system)
{
	NewtonWorld* world;

	world = system.m_world;

	// create the sky box and the floor,
//	LoadLevelAndSceneRoot (system, "flatplane.dae", 0);
//	LoadLevelAndSceneRoot (system, "dungeon.dae", 0);
//	LoadLevelAndSceneRoot (system, "xxx.dae", 1);
//	LoadLevelAndSceneRoot (system, "xxx1.dae", 1);
//	LoadLevelAndSceneRoot (system, "pitpool1.dae", 0);
	LoadLevelAndSceneRoot (system, "pitpool.dae", 0);
//	LoadLevelAndSceneRoot (system, "playground.dae", 0);
	

	// create a material to collide with this object
	int defaultMaterialID;
	defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);

	dVector posit (0.0f, 0.0f, 0.0f, 0.0f);
	posit.m_y = FindFloor (world, posit.m_x, posit.m_z) + 5.0f;


	InitEyePoint (dVector (1.0f, 0.0f, 0.0f), posit);

	dVector size (1.0f, 1.0f, 1.0f);
	dVector location (cameraEyepoint + cameraDir.Scale (10.0f));

	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _SPHERE_PRIMITIVE, defaultMaterialID);
	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _BOX_PRIMITIVE, defaultMaterialID);
	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CONE_PRIMITIVE, defaultMaterialID);
	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CYLINDER_PRIMITIVE, defaultMaterialID);
	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CAPSULE_PRIMITIVE, defaultMaterialID);
	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID);
	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
	AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
}

dVector dQuaternion::CalcAverageOmega (const dQuaternion &QB, dFloat dt) const
{
    dFloat dirMag;
    dFloat dirMag2;
    dFloat omegaMag;
    dFloat dirMagInv;


    dQuaternion dq (Inverse() * QB);
    dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);

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

    dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
    dirMag = dirMag2 * dirMagInv;

    omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) / dt;

    return omegaDir.Scale (dirMagInv * omegaMag);
}

static void ShowMeshCollidingFaces (
	const NewtonBody* bodyWithTreeCollision, 
	const NewtonBody* body, 
	int faceID, 
	int vertexCount, 
	const dFloat* vertex, 
	int vertexstrideInBytes)
{

	// we are coping data to and array of memory, another call back may be doing the same thing
	// here fore we need to avoid race conditions
	NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (bodyWithTreeCollision));

	dVector face[64];
	int stride = vertexstrideInBytes / sizeof (dFloat);
	for (int j = 0; j < vertexCount; j ++) {
		face [j] = dVector (vertex[j * stride + 0], vertex[j * stride + 1] , vertex[j * stride + 2]);
	}
	DebugDrawPolygon (vertexCount, face);

	// unlock the critical section
	NewtonWorldCriticalSectionUnlock (NewtonBodyGetWorld (bodyWithTreeCollision));
}

static void AddPathFollow (DemoEntityManager* const scene, const dVector& origin)
{
	// create a Bezier Spline path for AI car to drive
	DemoEntity* const rollerCosterPath = new DemoEntity(dGetIdentityMatrix(), NULL);
	scene->Append(rollerCosterPath);
	
	dBezierSpline spline;
	dFloat64 knots[] = {0.0f, 1.0f / 3.0f, 2.0f / 3.0f, 1.0f};

	dBigVector o (origin[0], origin[1],  origin[2],  0.0f);
	dBigVector control[] =
	{
		dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
		dBigVector(150.0f - 100.0f, 10.0f, 150.0f - 250.0f, 1.0f) + o,
		dBigVector(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f) + o,
		dBigVector(150.0f - 100.0f, 10.0f, 350.0f - 250.0f, 1.0f) + o,
		dBigVector( 50.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
		dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
	};

	spline.CreateFromKnotVectorAndControlPoints(3, sizeof (knots) / sizeof (knots[0]), knots, control);

	DemoBezierCurve* const mesh = new DemoBezierCurve (spline);
	rollerCosterPath->SetMesh(mesh, dGetIdentityMatrix());
	
	mesh->SetVisible(true);
	mesh->SetRenderResolution(500);
	mesh->Release();

	NewtonBody* const box0 = CreateWheel(scene, origin + dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f), 1.0f, 0.5f);

	dMatrix matrix;
    NewtonBodyGetMatrix (box0, &matrix[0][0]);
	matrix.m_posit.m_y += 0.5f;
	new MyPathFollow (matrix, box0, rollerCosterPath);

}

void UserHeightFieldCollision (DemoEntityManager* const scene)
{
	// load the sky box
	scene->CreateSkyBox();

	CreateHeightFieldTerrain(scene, HEIGHTFIELD_DEFAULT_SIZE, HEIGHTFIELD_DEFAULT_CELLSIZE, 1.5f, 0.2f, 200.0f, -50.0f);
	

	dMatrix camMatrix (dRollMatrix(-20.0f * dDegreeToRad) * dYawMatrix(-45.0f * dDegreeToRad));
	dQuaternion rot (camMatrix);
	dVector origin (250.0f, 0.0f, 250.0f, 0.0f);
	dFloat height = 1000.0f;
	origin = FindFloor (scene->GetNewton(), dVector (origin.m_x, height, origin .m_z, 0.0f), height * 2);
	origin.m_y += 10.0f;

	scene->SetCameraMatrix(rot, origin);

	int defaultMaterialID = NewtonMaterialGetDefaultGroupID (scene->GetNewton());
	dVector location (origin);
	location.m_x += 20.0f;
	location.m_z += 20.0f;
	dVector size (0.5f, 0.5f, 0.75f, 0.0f);

	int count = 5;
//	int count = 1;
	dMatrix shapeOffsetMatrix (dGetIdentityMatrix());
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
}

	//dAnimTakeData* LoadAnimation(dAnimIKController* const controller, const char* const animName)
	dAnimationKeyframesSequence* GetAnimationSequence(const char* const animName)
	{
		dTree<dAnimationKeyframesSequence*, dString>::dTreeNode* cachedAnimNode = m_animCache.Find(animName);
		if (!cachedAnimNode) {
			dScene scene(GetWorld());
			char pathName[2048];
			dGetWorkingFileName(animName, pathName);
			scene.Deserialize(pathName);

			dScene::dTreeNode* const animTakeNode = scene.FindChildByType(scene.GetRootNode(), dAnimationTake::GetRttiType());
			if (animTakeNode) {
				//dTree<dAnimimationKeyFramesTrack*, dString> map;
				//const dAnimPose& basePose = controller->GetBasePose();
				//dAnimTakeData* const animdata = new dAnimTakeData(basePose.GetCount());
				dAnimationKeyframesSequence* const animdata = new dAnimationKeyframesSequence();
				dAnimationTake* const animTake = (dAnimationTake*)scene.GetInfoFromNode(animTakeNode);
				animdata->SetPeriod(animTake->GetPeriod());

				cachedAnimNode = m_animCache.Insert(animdata, animName);

				//dList<dAnimimationKeyFramesTrack>& tracks = animdata->GetTracks();
				//dList<dAnimimationKeyFramesTrack>::dListNode* ptr = tracks.GetFirst();
				//dList<dAnimimationKeyFramesTrack>::dListNode* ptr = tracks.Append();
				//for (dAnimPose::dListNode* ptrNode = basePose.GetFirst(); ptrNode; ptrNode = ptrNode->GetNext()) {
				//	DemoEntity* const entity = (DemoEntity*)ptrNode->GetInfo().m_userData;
				//	map.Insert(&ptr->GetInfo(), entity->GetName());
				//	ptr = ptr->GetNext();
				//}

				dList<dAnimimationKeyFramesTrack>& tracks = animdata->GetTracks();
				for (void* link = scene.GetFirstChildLink(animTakeNode); link; link = scene.GetNextChildLink(animTakeNode, link)) {
					dScene::dTreeNode* const node = scene.GetNodeFromLink(link);
					dAnimationTrack* const srcTrack = (dAnimationTrack*)scene.GetInfoFromNode(node);

					if (srcTrack->IsType(dAnimationTrack::GetRttiType())) {
						//dTree<dAnimimationKeyFramesTrack*, dString>::dTreeNode* const ptrNode = map.Find(srcTrack->GetName());
						//dAssert(ptrNode);
						//dAnimTakeData::dAnimTakeTrack* const dstTrack = ptrNode->GetInfo();
						dAnimimationKeyFramesTrack* const dstTrack = &tracks.Append()->GetInfo();
						dstTrack->SetName(srcTrack->GetName());

						const dList<dAnimationTrack::dCurveValue>& rotations = srcTrack->GetRotations();
						dstTrack->m_rotation.Resize(rotations.GetCount());
						int index = 0;
						for (dList<dAnimationTrack::dCurveValue>::dListNode* node = rotations.GetFirst(); node; node = node->GetNext()) {
							dAnimationTrack::dCurveValue keyFrame(node->GetInfo());

							dMatrix matrix(dPitchMatrix(keyFrame.m_x) * dYawMatrix(keyFrame.m_y) * dRollMatrix(keyFrame.m_z));
							dQuaternion rot(matrix);
							dstTrack->m_rotation[index].m_rotation = rot;
							dstTrack->m_rotation[index].m_time = keyFrame.m_time;
							index++;
						}

						for (int i = 0; i < rotations.GetCount() - 1; i++) {
							dFloat dot = dstTrack->m_rotation[i].m_rotation.DotProduct(dstTrack->m_rotation[i + 1].m_rotation);
							if (dot < 0.0f) {
								dstTrack->m_rotation[i + 1].m_rotation.m_x *= -1.0f;
								dstTrack->m_rotation[i + 1].m_rotation.m_y *= -1.0f;
								dstTrack->m_rotation[i + 1].m_rotation.m_z *= -1.0f;
								dstTrack->m_rotation[i + 1].m_rotation.m_w *= -1.0f;
							}
						}

						const dList<dAnimationTrack::dCurveValue>& positions = srcTrack->GetPositions();
						dstTrack->m_position.Resize(positions.GetCount());
						index = 0;
						for (dList<dAnimationTrack::dCurveValue>::dListNode* node = positions.GetFirst(); node; node = node->GetNext()) {
							dAnimationTrack::dCurveValue keyFrame(node->GetInfo());
							dstTrack->m_position[index].m_posit = dVector(keyFrame.m_x, keyFrame.m_y, keyFrame.m_z, dFloat(1.0f));
							dstTrack->m_position[index].m_time = keyFrame.m_time;
							index++;
						}
					}
				}
			}
		}
		dAssert(cachedAnimNode);
		return cachedAnimNode->GetInfo();
	}

void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// add the linear limits
	const dVector& p0 = matrix0.m_posit;
	const dVector& p1 = matrix1.m_posit;
	dVector dp (p0 - p1);

	for (int i = 0; i < 3; i ++) {
		if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
		} else {
			// it is a limited linear dof, check if it pass the limits
			dFloat dist = dp.DotProduct3(matrix1[i]);
			if (dist > m_maxLinearLimits[i]) {
				dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));

				// clamp the error, so the not too much energy is added when constraint violation occurs
				dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
				if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
					q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
				}

				NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
				NewtonUserJointSetRowStiffness (m_joint, 1.0f);
				// allow the object to return but not to kick going forward
				NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

			} else if (dist < m_minLinearLimits[i]) {
				dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));

				// clamp the error, so the not too much energy is added when constraint violation occurs
				dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
				if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
					q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
				}

				NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
				NewtonUserJointSetRowStiffness (m_joint, 1.0f);
				// allow the object to return but not to kick going forward
				NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
			}
		}
	}

	dVector euler0(0.0f);
	dVector euler1(0.0f);
	dMatrix localMatrix (matrix0 * matrix1.Inverse());
	localMatrix.GetEulerAngles(euler0, euler1);

	AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
	AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
	if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
		euler0 = euler1;
	}

	dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);

//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f,  euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));

	bool limitViolation = false;
	for (int i = 0; i < 3; i ++) {
		if (euler[i] < m_minAngularLimits[i]) {
			limitViolation = true;
			euler[i] = m_minAngularLimits[i];
		} else if (euler[i] > m_maxAngularLimits[i]) {
			limitViolation = true;
			euler[i] = m_maxAngularLimits[i];
		}
	}

	if (limitViolation) {
		//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
		dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
		dMatrix baseMatrix (p0y0r0 * matrix1);
        dMatrix rotation (matrix0.Inverse() * baseMatrix);

        dQuaternion quat (rotation);
        if (quat.m_q0 > dFloat (0.99995f)) {
			//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
			//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
			//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);

			//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);

        } else {
            dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));

			dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
			dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
			NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]);
//.........这里部分代码省略.........

void CreateScene (NewtonWorld* world, SceneManager* sceneManager)
{
	Entity* floor;
	NewtonCollision* shape;
	NewtonBody* floorBody;
	void* materialManager;
	SoundManager* sndManager;
	PhysicsMaterialInteration matInterations;
	
	sndManager = sceneManager->GetSoundManager();

	// Create the material for this scene, and attach it to the Newton World
	materialManager = CreateMaterialManager (world, sndManager);

	// add the Material table
	matInterations.m_restitution = 0.6f;
	matInterations.m_staticFriction = 0.6f;
	matInterations.m_kineticFriction = 0.3f;
	matInterations.m_scrapingSound = NULL;

	matInterations.m_impactSound = sndManager->LoadSound ("metalMetal.wav");
	AddMaterilInteraction (materialManager, m_metal, m_metal, &matInterations);

	matInterations.m_impactSound = sndManager->LoadSound ("boxBox.wav");
	AddMaterilInteraction (materialManager, m_wood, m_wood, &matInterations);

	matInterations.m_impactSound = sndManager->LoadSound ("metalBox.wav");
	AddMaterilInteraction (materialManager, m_metal, m_wood, &matInterations);
	
	matInterations.m_impactSound = sndManager->LoadSound ("grass0.wav");
	AddMaterilInteraction (materialManager, m_wood, m_grass, &matInterations);

	matInterations.m_impactSound = sndManager->LoadSound ("boxHit.wav");
	AddMaterilInteraction (materialManager, m_wood, m_bricks, &matInterations);

	matInterations.m_impactSound = sndManager->LoadSound ("grass1.wav");
	AddMaterilInteraction (materialManager, m_metal, m_grass, &matInterations);
	AddMaterilInteraction (materialManager, m_grass, m_bricks, &matInterations);

	matInterations.m_impactSound = sndManager->LoadSound ("metal.wav");
	AddMaterilInteraction (materialManager, m_metal, m_bricks, &matInterations);
	AddMaterilInteraction (materialManager, m_grass, m_grass, &matInterations);
	
	

	// Create a large body to be the floor
	floor = sceneManager->CreateEntity();
	floor->LoadMesh ("LevelMesh.dat");

	// add static floor Physics
	int materialMap[] = {m_bricks, m_grass, m_wood,	m_metal};
	shape = CreateMeshCollision (world, floor, materialMap);
	floorBody = CreateRigidBody (world, floor, shape, 0.0f);
	NewtonReleaseCollision (world, shape);


	// set the Transformation Matrix for this rigid body
	dMatrix matrix (floor->m_curRotation, floor->m_curPosition);
	NewtonBodySetMatrix (floorBody, &matrix[0][0]);

	// now we will use the properties of this body to set a proper world size.
	dVector minBox;
	dVector maxBox;
	NewtonCollisionCalculateAABB (shape, &matrix[0][0], &minBox[0], &maxBox[0]);

	// add some extra padding
	minBox.m_x -=  50.0f;
	minBox.m_y -= 500.0f;
	minBox.m_z -=  50.0f;

	maxBox.m_x +=  50.0f;
	maxBox.m_y += 500.0f;
	maxBox.m_z +=  50.0f;

	// set the new world size
	NewtonSetWorldSize (world, &minBox[0], &maxBox[0]);

	// add some visual entities.
	dFloat y0 = FindFloor (world, 0.0f, 0.0f) + 10.0f;
	for (int i = 0; i < 5; i ++) {
		Entity* smilly;
		NewtonBody* smillyBody;
		smilly = sceneManager->CreateEntity();
		smilly->LoadMesh ("Smilly.dat");
		smilly->m_curPosition.m_y = y0;
		y0 += 2.0f;
		smilly->m_prevPosition = smilly->m_curPosition;

		// add a body with a box shape
		shape = CreateNewtonBox (world, smilly, m_metal);
		smillyBody = CreateRigidBody (world, smilly, shape, 10.0f);
		NewtonReleaseCollision (world, shape);
	}

	// add some visual entities.
	y0 = FindFloor (world, 0.0f, 0.4f) + 10.5f;
	for (int i = 0; i < 5; i ++) {
		Entity* frowny;
		NewtonBody* frownyBody;
		frowny = sceneManager->CreateEntity();
//.........这里部分代码省略.........

int dRuntimeProfiler::RenderConcurrentPerformance (int lineNumber)
{
    struct GLViewPort
    {
        int x;
        int y;
        int width;
        int height;
    } viewport;
    //Retrieves the viewport and stores it in the variable
    glGetIntegerv(GL_VIEWPORT, (GLint*) &viewport.x);

    m_width = viewport.width;
    m_height = viewport.height;

    glColor3f(1.0, 1.0, 1.0);
    glDisable (GL_LIGHTING);
    //glDisable(GL_TEXTURE_2D);

    glMatrixMode(GL_TEXTURE);
    glPushMatrix();
    glLoadIdentity();

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    gluOrtho2D(0, viewport.width, 0, viewport.height );

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_BLEND);

    DrawLabel (10, m_height - m_nextLine, "Concurrent profiler,    red = physcis;      blue = graphics      green = idle");
    m_nextLine = lineNumber;

    dFloat times[10];
    dVector colors[10];

    colors[0] = dVector (0.0f, 1.0f, 0.0f, 0.5f);
    colors[1] = dVector (0.0f, 0.0f, 1.0f, 0.5f);
    times[0] = m_scene->m_mainThreadPhysicsTime;
    times[1] = m_scene->m_mainThreadGraphicsTime;
    DrawLabel (10, m_height - m_nextLine, "graphics thread:");
    m_nextLine += 30;
    DrawConcurrentChart(2, times, colors);
    m_nextLine += 30;

    colors[0] = dVector (1.0f, 0.0f, 0.0f, 0.5f);
    times[0] = m_scene->m_physThreadTime;
    DrawLabel (10, m_height - m_nextLine, "physics thread:");
    m_nextLine += 30;
    DrawConcurrentChart(1, times, colors);
    m_nextLine += 40;

    glMatrixMode(GL_TEXTURE);
    glPopMatrix();

    glMatrixMode(GL_PROJECTION);
    glPopMatrix();

    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();

    glDisable(GL_BLEND);
    glEnable (GL_DEPTH_TEST);
    glColor3f(1.0, 1.0, 1.0);

    return m_nextLine + 30;
}

void dRuntimeProfiler::RenderThreadPerformance ()
{
    NewtonWorld* const world = m_scene->GetNewton();
    int threadCount = NewtonGetThreadsCount(world);

    if (threadCount > 0) {
        struct GLViewPort
        {
            int x;
            int y;
            int width;
            int height;
        } viewport;

        //Retrieves the viewport and stores it in the variable
        glGetIntegerv(GL_VIEWPORT, (GLint*) &viewport.x);


        glColor3f(1.0, 1.0, 1.0);
        glDisable (GL_LIGHTING);
        //glDisable(GL_TEXTURE_2D);

        glMatrixMode(GL_TEXTURE);
        glPushMatrix();
        glLoadIdentity();

        glMatrixMode(GL_PROJECTION);
        glPushMatrix();
        glLoadIdentity();
        gluOrtho2D(0, viewport.width, 0, viewport.height );

        glMatrixMode(GL_MODELVIEW);
        glPushMatrix();
        glLoadIdentity();

        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        glDisable(GL_DEPTH_TEST);
        glDisable(GL_BLEND);

        m_width = viewport.width;
        m_height = viewport.height;

        float x0 = m_width - 600.0f;
        float x1 = x0 + 256.0f;
        float y0 = 50.0f;
        for (int i = 0; i < threadCount; i ++) {
            char label[32];
            sprintf (label, "thread %d", i);
            DrawLabel (x0 - 90, y0 + i * 22 - 10, label);
        }

        DrawLabel (x0, y0 - 30, "0.0 ms");
        DrawLabel ((x1 + x0) * 0.5f, y0 - 30, "8.33 ms");
        DrawLabel (x1, y0 - 30, "16.66 ms");

        y0 -= 15.0f;
        dVector color (1.0f, 0.0f, 0.0f, 1.0f/8.0f);
        DrawRectangle (x0, m_height - (y0 + 20.0f * threadCount), x1 - x0, 20 * threadCount, color);

        color = dVector (1.0f, 1.0f, 0.0f, 1.0f/2.0f);
        if (threadCount > 1)  {
            for (int i = 0; i < threadCount; i ++) {
                int tick = NewtonReadThreadPerformanceTicks (world, i);
                dFloat time = dFloat (tick) * (1.0e-3f * 256.0f / 16.666f);
                DrawRectangle(x0, m_height - (y0 + 15), time, 10, color);
                y0 += 20.0f;
            }
        } else {
            int tick =  NewtonReadPerformanceTicks (world, NEWTON_PROFILER_WORLD_UPDATE);
            dFloat time = dFloat (tick) * (1.0e-3f * 256.0f / 16.666f);
            DrawRectangle(x0, m_height - (y0 + 15), time, 10, color);
        }

        glMatrixMode(GL_TEXTURE);
        glPopMatrix();

        glMatrixMode(GL_PROJECTION);
        glPopMatrix();

        glMatrixMode(GL_MODELVIEW);
        glPopMatrix();

        glDisable(GL_BLEND);
        glEnable (GL_DEPTH_TEST);
        glColor3f(1.0, 1.0, 1.0);
    }
}

void ScaledMeshCollision (DemoEntityManager* const scene)
{
	// load the skybox
	scene->CreateSkyBox();

	// load the scene from a ngd file format
	CreateLevelMesh (scene, "flatPlane.ngd", 1);
	//CreateLevelMesh (scene, "flatPlaneDoubleFace.ngd", 1);
	//CreateLevelMesh (scene, "sponza.ngd", 0);
	//CreateLevelMesh (scene, "cattle.ngd", fileName);
	//CreateLevelMesh (scene, "playground.ngd", 0);

	//dMatrix camMatrix (dRollMatrix(-20.0f * 3.1416f /180.0f) * dYawMatrix(-45.0f * 3.1416f /180.0f));
	dMatrix camMatrix (dGetIdentityMatrix());
	dQuaternion rot (camMatrix);
	dVector origin (-15.0f, 5.0f, 0.0f, 0.0f);
	//origin = origin.Scale (0.25f);
	scene->SetCameraMatrix(rot, origin);


	NewtonWorld* const world = scene->GetNewton();
	int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
	dVector location (0.0f, 0.0f, 0.0f, 0.0f);

	dMatrix matrix (dGetIdentityMatrix());
	matrix.m_posit = location;
	matrix.m_posit.m_x = 0.0f;
	matrix.m_posit.m_y = 0.0f;
	matrix.m_posit.m_z = 0.0f;
	matrix.m_posit.m_w = 1.0f;

	DemoEntity teaPot (dGetIdentityMatrix(), NULL);
	teaPot.LoadNGD_mesh("teapot.ngd", world);
	//teaPot.LoadNGD_mesh("box.ngd", world);

	NewtonCollision* const staticCollision = CreateCollisionTree (world, &teaPot, 0, true);
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (1.0f, 1.0f, 1.0f, 0.0f));
//	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 2.0f, 0.0f));

	matrix.m_posit.m_z = -5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 2.0f, 0.0f));

	matrix.m_posit.m_z = 5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (3.0f, 3.0f, 1.5f, 0.0f));

	matrix.m_posit.m_z = 0.0f;
	matrix.m_posit.m_x = -5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 0.5f, 0.0f));

	matrix.m_posit.m_x = 5.0f;
	CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (2.0f, 2.0f, 2.0f, 0.0f));

	// do not forget to destroy the collision mesh helper
	NewtonDestroyCollision(staticCollision);

	dVector size0 (1.0f, 1.0f, 1.0f, 0.0f);
	dVector size1 (0.5f, 1.0f, 1.0f, 0.0f);
	dMatrix shapeOffsetMatrix (dRollMatrix(3.141592f/2.0f));

	int count = 3;
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size1, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size1, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
	AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);

	origin.m_x -= 4.0f;
	origin.m_y += 1.0f;
	scene->SetCameraMatrix(rot, origin);	
}

void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
	int constraintIndex;
	dFloat invTimestep;
	dMatrix bodyMatrix;  

	// get the simulation time
	invTimestep = 1.0f / timestep ;

	// get the vehicle global matrix, and use it in several calculations
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	dMatrix chassisMatrix (m_localFrame * bodyMatrix);

	// get the chassis instantaneous linear and angular velocity in the local space of the chassis
	dVector bodyOmega;
	dVector bodyVelocity;
	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

	// all tire is on air check
	m_vehicleOnAir = 0;
	constraintIndex = 0;
	for ( int i = 0; i < m_tiresCount; i ++ ) {

		Tire& tire = m_tires[i];
		tire.m_tireIsOnAir = 1;
		tire.m_tireIsConstrained = 0;	
		tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);

		// calculate all suspension matrices in global space and tire collision
		dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);

		// calculate the tire collision
		CalculateTireCollision (tire, suspensionMatrix, threadIndex);

		// calculate the linear velocity of the tire at the ground contact
		tire.m_tireAxelPosit = chassisMatrix.TransformVector( tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit));
		tire.m_tireAxelVeloc = bodyVelocity + bodyOmega * (tire.m_tireAxelPosit - chassisMatrix.m_posit); 
		tire.m_lateralPin = ( chassisMatrix.RotateVector ( tire.m_localAxis ) );
		tire.m_longitudinalPin = ( chassisMatrix.m_up * tire.m_lateralPin );

		if (tire.m_posit < tire.m_suspensionLenght )  {
			dFloat distance;
			dFloat sideSlipCoef;
			dFloat slipRatioCoef;
			dFloat tireForceMag;
			dFloat tireTorqueMag;
			dFloat suspensionSpeed;
			dFloat axelLinealSpeed;
			dFloat tireRotationSpeed;
			dFloat frictionCircleMag;
			dFloat longitudinalForceMag;
			dFloat lateralFrictionForceMag;


			tire.m_tireIsOnAir = 0;
			tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			if (tire.m_HitBody){
				dMatrix matrix;
				dVector com;
				dVector omega;

				NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
				NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
				NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
				NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
				tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
			} 

			// calculate the relative velocity
			dVector relVeloc (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
			suspensionSpeed = - (relVeloc % chassisMatrix.m_up);

			// now calculate the tire load at the contact point
			// Tire suspension distance and hard limit.
			distance = tire.m_suspensionLenght - tire.m_posit;
			_ASSERTE (distance <= tire.m_suspensionLenght);
			tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
			if ( tire.m_tireLoad < 0.0f ) {
				// since the tire is not a body with real mass it can only push the chassis.
				tire.m_tireLoad = 0.0f;
			} 

			//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
			tire.m_tireLoad *= (m_mass * 0.5f);

			tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);

			// convert the tire load force magnitude to a torque and force.
			// accumulate the force doe to the suspension spring and damper
			tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);

			// calculate relative velocity at the tire center
			dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity); 

			// axle linear speed
			axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;

			// calculate tire rotation velocity at the tire radio
//.........这里部分代码省略.........