void MSNewton::Servo::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
	dMatrix matrix;
	dVector centre;
	NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
	NewtonBodyGetCentreOfMass(joint_data->child, &centre[0]);
	centre = matrix.TransformVector(centre);
	centre = pin_matrix.UntransformVector(centre);
	dVector point(0.0f, 0.0f, centre.m_z);
	pin_matrix.m_posit = pin_matrix.TransformVector(point);
}

dMatrix CustomDGRayCastCar::CalculateTireMatrix (int tireIndex) const
{
	const Tire& tire = m_tires[tireIndex];
	// calculate the rotation angle matrix
	dMatrix angleMatrix ( dPitchMatrix( tire.m_spinAngle ) );
	// get the tire body matrix
	dMatrix bodyMatrix;
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	return angleMatrix * CalculateSuspensionMatrix (tireIndex, tire.m_posit) * m_localFrame * bodyMatrix;
}

void CustomDGRayCastCar::ApplyChassisTorque (const dVector& vForce, const dVector& vPoint)
{
	dVector Torque;
	dMatrix M;
	dVector com;
	NewtonBodyGetCentreOfMass( m_body0, &com[0] );
	NewtonBodyGetMatrix( m_body0, &M[0][0] );
	Torque = ( vPoint - M.TransformVector( dVector( com.m_x, com.m_y, com.m_z, 1.0f ) ) ) * vForce;
	NewtonBodyAddTorque( m_body0, &Torque[0] );
}

VALUE MSNewton::Bodies::get_closest_points(VALUE self, VALUE v_body1, VALUE v_body2) {
	const NewtonBody* body1 = Util::value_to_body(v_body1);
	const NewtonBody* body2 = Util::value_to_body(v_body2);
	Util::validate_two_bodies(body1, body2);
	const NewtonWorld* world = NewtonBodyGetWorld(body1);
	WorldData* world_data = (WorldData*)NewtonWorldGetUserData(world);
	NewtonCollision* colA = NewtonBodyGetCollision(body1);
	NewtonCollision* colB = NewtonBodyGetCollision(body2);
	dMatrix matrixA;
	dMatrix matrixB;
	NewtonBodyGetMatrix(body1, &matrixA[0][0]);
	NewtonBodyGetMatrix(body2, &matrixB[0][0]);
	dVector pointA;
	dVector pointB;
	dVector normalAB;
	if (NewtonCollisionClosestPoint(world, colA, &matrixA[0][0], colB, &matrixB[0][0], &pointA[0], &pointB[0], &normalAB[0], 0) == 0)
		return Qnil;
	return rb_ary_new3(2, Util::point_to_value(pointA, world_data->inverse_scale), Util::point_to_value(pointB, world_data->inverse_scale));
}

	dVector BodyGetPointVelocity(const NewtonBody* const body, const dVector &point)
	{
		dVector v, w, c;
		NewtonBodyGetVelocity(body, &v[0]);
		NewtonBodyGetOmega(body, &w[0]);
		dMatrix matrix;
		NewtonBodyGetMatrix(body, &matrix[0][0]);
		c = matrix.m_posit; // TODO: Does not handle COM offset !!!
		return v + w * (point - c);
	}

static CustomCorkScrew* AddCylindricalWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
{
    NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);

    // the joint pin is the first row of the matrix
    dMatrix matrix;
    NewtonBodyGetMatrix (wheel, &matrix[0][0]);

    return new CustomCorkScrew (matrix, wheel, parent);
}

dVector CalculateToqueAtPoint (const NewtonBody* body, const dVector& point, const dVector& force)
{
	dVector com;
	dMatrix matrix;
 
	NewtonBodyGetMatrix (body, &matrix[0][0]);
	NewtonBodyGetCentreOfMass (body, &matrix[0][0]);
	com = matrix.TransformVector (com);
	return (point - com) *  force;
}

void AddPulley (DemoEntityManager* const scene, const dVector& origin)
{
    NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
	// this is just for show
    NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f));
	NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f);
	NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f);
    
	dMatrix matrix;
    CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
    CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);

    slider0->EnableLimits(true);
    slider0->SetLimits (-2.0f, 2.0f);

    NewtonBody* const body0 = slider0->GetBody0();
    NewtonBody* const body1 = slider1->GetBody0();

    dMatrix matrix0;
    dMatrix matrix1;
    NewtonBodyGetMatrix (body0, &matrix0[0][0]);
    NewtonBodyGetMatrix (body1, &matrix1[0][0]);

    dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
    dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
    new CustomPulley (4.0f, pin0, pin1, body0, body1);

#ifdef _USE_HARD_JOINTS
	NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
	NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0);
	NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel0);
	NewtonSkeletonContainerFinalize(skeleton);
#endif

	// make an aggregate for disabling collisions
	void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
	NewtonCollisionAggregateSetSelfCollision (aggregate, 0);
	NewtonCollisionAggregateAddBody (aggregate, reel0);
	NewtonCollisionAggregateAddBody (aggregate, reel1);
	NewtonCollisionAggregateAddBody (aggregate, body0);
	NewtonCollisionAggregateAddBody (aggregate, body1);
}

		FrictionTrankThreaSuspention(const dMatrix& pinsAndPivoFrame, const NewtonBody* child, const NewtonBody* parent)
			: CustomSlidingContact (pinsAndPivoFrame, child, parent)
		{
			dMatrix childMatrix;
			dMatrix parentMatrix;
			dVector maxPointFront;
			dVector minPointFront;

			NewtonCollision* collision;
			collision = NewtonBodyGetCollision(child);

			NewtonBodyGetMatrix(child, &childMatrix[0][0]);
			NewtonBodyGetMatrix(parent, &parentMatrix[0][0]);

			// find the the extreme front and rear point, this is used to calculate the position of the suspension points
			dVector frontDir (childMatrix.UnrotateVector(parentMatrix.m_front));
			NewtonCollisionSupportVertex(collision, &frontDir[0], &maxPointFront[0]);

			dVector rearDir (frontDir.Scale (-1.0f));
			NewtonCollisionSupportVertex(collision, &rearDir[0], &minPointFront[0]);


			// calculate the front suspension points
			dVector frontHardPoint (childMatrix.m_posit + childMatrix.RotateVector(frontDir.Scale (maxPointFront % frontDir)));
			m_frontHarpointOnParent = parentMatrix.UntransformVector(frontHardPoint);
			m_frontHarpointOnThread = childMatrix.UntransformVector(frontHardPoint);

			// calculate the front rear suspension points
			dVector rearHardPoint (childMatrix.m_posit + childMatrix.RotateVector(rearDir.Scale (minPointFront % rearDir)));
			m_rearHarpointOnParent = parentMatrix.UntransformVector(rearHardPoint);
			m_rearHarpointOnThread = childMatrix.UntransformVector(rearHardPoint);

		
			dFloat Ixx;
			dFloat Iyy;
			dFloat Izz;
			dFloat mass0;
			dFloat mass1;
			NewtonBodyGetMassMatrix(child, &mass0, &Ixx, &Iyy, &Izz);
			NewtonBodyGetMassMatrix(parent, &mass1, &Ixx, &Iyy, &Izz);
			m_massScale = (mass0 * mass1) / (mass0 + mass1);
		}

static void AddLimitedBallAndSocket (DemoEntityManager* const scene, const dVector& origin)
{
	dVector size(1.0f, 1.0f, 1.0f);
	NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 5.0f, 0.0f, 0.0f), size);
	NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
	NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);

	dMatrix pinMatrix(dGrammSchmidt(dVector(0.0f, -1.0f, 0.0f, 0.0f)));

	// connect first box to the world
	dMatrix matrix;
	NewtonBodyGetMatrix(box0, &matrix[0][0]);
	pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);

	CustomLimitBallAndSocket* const joint0 = new CustomLimitBallAndSocket(pinMatrix, box0, NULL);
	joint0->SetConeAngle (30.0f * 3.141592f / 180.0f);
	joint0->SetTwistAngle (-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);

	// connect first box1 to box0 the world
	NewtonBodyGetMatrix(box1, &matrix[0][0]);
	pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);

	CustomLimitBallAndSocket* const joint1 = new CustomLimitBallAndSocket(pinMatrix, box1, box0);
	joint1->SetConeAngle(30.0f * 3.141592f / 180.0f);
	joint1->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);

	// connect first box2 to box1 the world
	NewtonBodyGetMatrix(box2, &matrix[0][0]);
	pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);

	CustomLimitBallAndSocket* const joint2 = new CustomLimitBallAndSocket(pinMatrix, box2, box1);
	joint2->SetConeAngle(30.0f * 3.141592f / 180.0f);
	joint2->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);

#ifdef _USE_HARD_JOINTS
	NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
	NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
	NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
	NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
	NewtonSkeletonContainerFinalize(skeleton);
#endif
}

void OgreNewtonSceneBody::EndAddRemoveCollision()
{
	dNewtonCollisionScene* const scene = (dNewtonCollisionScene*) GetCollision();
	scene->EndAddRemoveCollision();

	// need to update the aabb in the broad phase, for this we call set matrix
	dMatrix matrix;
	NewtonBody* const body = GetNewtonBody();
	NewtonBodyGetMatrix(body, &matrix[0][0]);
	NewtonBodySetMatrix(body, &matrix[0][0]);
}

void DemoCameraListener::RenderPickedTarget () const
{
	if (m_targetPicked) {
		dMatrix matrix;
		NewtonBodyGetMatrix(m_targetPicked, &matrix[0][0]);

		dVector p0 (matrix.TransformVector(m_pickedBodyLocalAtachmentPoint));
		dVector p1 (p0 + matrix.RotateVector (m_pickedBodyLocalAtachmentNormal.Scale (0.5f)));
		ShowMousePicking (p0, p1);
	}
}

	static void PlaneCollisionCollideCallbackConstinue (NewtonUserMeshCollisionCollideDesc* const collideDesc, const void* const continueCollisionHandle)
	{
		dInfinitePlane* const me = (dInfinitePlane*) collideDesc->m_userData;

		// build that aabb of each face and submit only the one that pass the test.
		if (NewtonUserMeshCollisionContinuousOverlapTest (collideDesc, continueCollisionHandle, &me->m_minBox[0], &me->m_maxBox[0])) {
			const dVector& p0 = me->m_minBox;
			const dVector& p1 = me->m_maxBox;
			dVector centre ((p1 + p0).Scale (0.5f));

			//find the projection of center point over the plane
			dFloat t = - (me->m_plane.DotProduct3(centre) + me->m_plane.m_w);
			centre += me->m_plane.Scale (t);

			//know calculate the scale factor
			dVector size (p1 - p0);
			dFloat s = dMax(size.m_x, dMax (size.m_y, size.m_z)) * 0.5f;

			dInt32 threadNumber = collideDesc->m_threadNumber;

			// initialize the callback data structure
#ifdef PASS_A_QUAD
			collideDesc->m_faceCount = 1;
#else
			collideDesc->m_faceCount = 2;
#endif
			collideDesc->m_vertexStrideInBytes = sizeof (dVector);
			collideDesc->m_faceIndexCount = &me->m_faceIndices[threadNumber][0];
			collideDesc->m_faceVertexIndex = &me->m_indexArray[threadNumber][0];
			collideDesc->m_vertex = &me->m_collisionVertex[threadNumber][0][0];
			dVector* const polygon = &me->m_collisionVertex[threadNumber][0];
			for (int i = 0; i < 4; i ++) {
				polygon[i] = centre + me->m_unitSphape[i].Scale (s);
			}
			// save face normal
			polygon[4] =  me->m_plane;

			// show debug display info
			if (DebugDisplayOn()) {
				dMatrix matrix;
				dVector face[64];

				NewtonBodyGetMatrix (collideDesc->m_polySoupBody, &matrix[0][0]);
				matrix.TransformTriplex (&face[0].m_x, sizeof (dVector), &polygon[0].m_x, sizeof (dVector), 4);

				NewtonWorld* const world = NewtonBodyGetWorld (collideDesc->m_polySoupBody);
				// critical section lock
				NewtonWorldCriticalSectionLock (world, threadNumber);
				//DebugDrawPolygon (4, &face[0]);
				// unlock the critical section
				NewtonWorldCriticalSectionUnlock (world);
			}
		}
	}

		void SubmitConstraints (dFloat timestep, int threadIndex)
		{
			// calculate suspension bumpers and forces
			dMatrix threadMatrix;
			dMatrix parentMatrix;

			dVector threadCOM;
			dVector parentCOM;
			dVector threadVeloc;
			dVector parentVeloc;
			dVector threadOmega;
			dVector parentOmega;


			// get the physics body state;
			NewtonBodyGetOmega(m_body0, &threadOmega[0]);
			NewtonBodyGetOmega(m_body1, &parentOmega[0]);

			NewtonBodyGetVelocity(m_body0, &threadVeloc[0]);
			NewtonBodyGetVelocity(m_body1, &parentVeloc[0]);

			NewtonBodyGetCentreOfMass(m_body0, &threadCOM[0]);
			NewtonBodyGetCentreOfMass(m_body1, &parentCOM[0]);

			NewtonBodyGetMatrix(m_body0, &threadMatrix[0][0]);
			NewtonBodyGetMatrix(m_body1, &parentMatrix[0][0]);

			threadCOM = threadMatrix.TransformVector(threadCOM);
			parentCOM = parentMatrix.TransformVector(parentCOM);
			
			ApplySuspesionForce (timestep,
				m_body0, m_rearHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
				m_body1, m_rearHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);

			ApplySuspesionForce (timestep,
				m_body0, m_frontHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
				m_body1, m_frontHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);


			CustomSlidingContact::SubmitConstraints (timestep, threadIndex);
		}

dCustomUpVector::dCustomUpVector(const dVector& pin, NewtonBody* child)
	:dCustomJoint(2, child, NULL)
{
	dMatrix pivot;

	NewtonBodyGetMatrix(child, &pivot[0][0]);

	dMatrix matrix (dGrammSchmidt(pin));
	matrix.m_posit = pivot.m_posit;

	CalculateLocalMatrix (matrix, m_localMatrix0, m_localMatrix1);
}

void NewtonCustomJoint::CalculateLocalMatrix (const dVector& pivot, const dVector& dir, dMatrix& localMatrix0, dMatrix& localMatrix1) const
{
	dMatrix matrix0;

	// Get the global matrices of each rigid body.
	NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);

	dMatrix matrix1 (GetIdentityMatrix());
	if (m_body1) {
		NewtonBodyGetMatrix(m_body1, &matrix1[0][0]);
	}

	// create a global matrix at the pivot point with front vector aligned to the pin vector
	dMatrix pinAndPivotMatrix (dgGrammSchmidt(dir));
	pinAndPivotMatrix.m_posit = pivot;
	pinAndPivotMatrix.m_posit.m_w = 1.0f;

	// calculate the relative matrix of the pin and pivot on each body
 	localMatrix0 = pinAndPivotMatrix * matrix0.Inverse();
	localMatrix1 = pinAndPivotMatrix * matrix1.Inverse();
}

static void AddMerryGoRound(DemoEntityManager* const scene, const dVector& location)
{
	NewtonBody* const pole = CreateCylinder(scene, location, 0.0f, 0.2f, 3.0f);

	dVector platformPosit(location);
	platformPosit.m_y += 0.2f;
	NewtonBody* const platform = CreateCylinder(scene, platformPosit, 200.0f, 10.0f, 0.2f);

	dMatrix pivot;
	NewtonBodyGetMatrix(platform, &pivot[0][0]);
	dCustomHinge* const hinge = new dCustomHinge(pivot, platform, pole);
	hinge;
}

dNewtonJointDoubleHinge::dNewtonJointDoubleHinge(const dMatrix pintAndPivotMatrix, void* const body0, void* const body1)
	:dNewtonJoint()
{
	dMatrix bodyMatrix;

	NewtonBody* const netwonBody0 = (NewtonBody*)body0;
	NewtonBody* const netwonBody1 = (NewtonBody*)body1;
	NewtonBodyGetMatrix(netwonBody0, &bodyMatrix[0][0]);

	dMatrix matrix(pintAndPivotMatrix * bodyMatrix);
	dCustomDoubleHinge* const joint = new dCustomDoubleHinge(matrix, netwonBody0, netwonBody1);
	SetJoint(joint);
}

	void cPhysicsBodyNewton::RenderDebugGeometry(iLowLevelGraphics *a_pLowLevel, const cColor &a_Color)
	{
		//cPhysicsWorldNewton *pPhysicsWorld = static_cast<cPhysicsWorldNewton*>(NewtonWorldGetUserData(m_pNewtonWorld));
		//pPhysicsWorld->GetWorld3D()->gets

		NewtonCollision *pCollision = NewtonBodyGetCollision(m_pNewtonBody);
		float matrix[4][4];
		NewtonBodyGetMatrix(m_pNewtonBody, &matrix[0][0]);
		g_pLowLevelGraphics = a_pLowLevel;
		g_DebugColor = a_Color;
		NewtonCollision *pNewtonCollision = NewtonBodyGetCollision(m_pNewtonBody);
		NewtonCollisionForEachPolygonDo(pNewtonCollision,
			m_mtxLocalTransform.GetTranspose().m[0], RenderDebugPolygon, this);
	}

	dAnimationJoint* CreateChildNode(NewtonBody* const boneBody, dAnimationJoint* const parent, const dJointDefinition& definition) const
	{
		dMatrix matrix;
		NewtonBodyGetMatrix(boneBody, &matrix[0][0]);

		dJointDefinition::dFrameMatrix frameAngle(definition.m_frameBasics);
		dMatrix pinAndPivotInGlobalSpace(dPitchMatrix(frameAngle.m_pitch * dDegreeToRad) * dYawMatrix(frameAngle.m_yaw * dDegreeToRad) * dRollMatrix(frameAngle.m_roll * dDegreeToRad));
		pinAndPivotInGlobalSpace = pinAndPivotInGlobalSpace * matrix;

		//dMatrix bindMatrix(entity->GetParent()->CalculateGlobalMatrix((DemoEntity*)NewtonBodyGetUserData(parentBody)).Inverse());
		dMatrix bindMatrix(dGetIdentityMatrix());
		dAnimationJoint* const joint = new dAnimationRagdollJoint(definition.m_type, pinAndPivotInGlobalSpace, boneBody, bindMatrix, parent);
		return joint;
	}