PlaformEntityEntity (DemoEntityManager* const scene, DemoEntity* const source, NewtonBody* const triggerPort0, NewtonBody* const triggerPort1)
		:DemoEntity (source->GetNextMatrix(), NULL)
	{
		scene->Append(this);

		DemoMesh* const mesh = (DemoMesh*)source->GetMesh();
		dAssert (mesh->IsType(DemoMesh::GetRttiType()));

		SetMesh(mesh, source->GetMeshMatrix());

		const dFloat mass = 100.0f;
		dMatrix matrix (source->GetNextMatrix()) ;
		NewtonWorld* const world = scene->GetNewton();

		// note: because the mesh matrix can have scale, for simplicity just apply the local mesh matrix to the vertex cloud
		dVector pool[128];
		const dMatrix& meshMatrix = GetMeshMatrix();
		meshMatrix.TransformTriplex(&pool[0].m_x, sizeof (dVector), mesh->m_vertex, 3 * sizeof (dFloat), mesh->m_vertexCount);
		NewtonCollision* const collision = NewtonCreateConvexHull(world, mesh->m_vertexCount, &pool[0].m_x, sizeof (dVector), 0, 0, NULL);

		NewtonBody* body = CreateSimpleBody (world, this, 100, matrix, collision, 0);
		NewtonDestroyCollision(collision);


		// attach a kinematic joint controller joint to move this body 
		dVector pivot;
		NewtonBodyGetCentreOfMass (body, &pivot[0]);
		pivot = matrix.TransformVector(pivot);
		m_driver = new FerryDriver (body, pivot, triggerPort0, triggerPort1);
		m_driver->SetMaxLinearFriction (50.0f * dAbs (mass * DEMO_GRAVITY)); 
		m_driver->SetMaxAngularFriction(50.0f * dAbs (mass * DEMO_GRAVITY)); 
	}

CustomVehicleControllerComponentEngine::dGearBox::dGearState* CustomVehicleControllerComponentEngine::dGearBox::dGearState::Update(CustomVehicleController* const vehicle)
{
    const CustomVehicleControllerComponentEngine* const engine = vehicle->GetEngine();

    dFloat param = engine->GetParam();
    dFloat speed = engine->GetSpeed();
    dFloat normalrpm = engine->GetRPM() / engine->GetRedLineRPM();

    if ((normalrpm > m_shiftUp) && (speed > 1.0f)) {
        return m_next;
    } else if (normalrpm < m_shiftDown) {
        if ((dAbs (speed) < 0.5f) && (param < dFloat (1.0e-3f))) {
            return m_neutral;
        }

        if ((dAbs (speed) < 2.0f) && (param < dFloat (-1.0e-3f))) {
            return m_reverse;
        }

        if (param > dFloat (-1.0e-3f)) 

        dAssert (m_prev != m_neutral);
        dAssert (m_prev != m_reverse);
        return m_prev;
    } else if (param < 0.0f) {
        dAssert (0);
/*
        if (speed < 1.0f) {
            return m_reverse;
        }
*/
    }

    return this;
}

CustomVehicleControllerComponentTrackSkidSteering::CustomVehicleControllerComponentTrackSkidSteering (CustomVehicleController* const controller, dFloat steeringRPM, dFloat teeringTorque)
	:CustomVehicleControllerComponentSteering (controller, 0.0f)
	,m_steeringRPM (dAbs(steeringRPM))
	,m_steeringTorque (dAbs(teeringTorque))
{
	m_differencialTurnRate = m_steeringTorque / (m_steeringRPM * m_steeringRPM);
}

void Custom6DOF::SetAngularLimits (const dVector& minAngularLimits, const dVector& maxAngularLimits)
{
	for (int i = 0; i < 3; i ++) {
		m_minAngularLimits[i] =  (dAbs (minAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : minAngularLimits[i];
		m_maxAngularLimits[i] =  (dAbs (maxAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : maxAngularLimits[i];
	}
}

void dNewtonCollision::SetScale(dFloat scaleX, dFloat scaleY, dFloat scaleZ)
{
	scaleX = dMax(0.01f, dAbs(scaleX));
	scaleY = dMax(0.01f, dAbs(scaleY));
	scaleZ = dMax(0.01f, dAbs(scaleZ));
	NewtonCollisionSetScale(m_shape, scaleX, scaleY, scaleZ);
}

dQuaternion::dQuaternion (const dMatrix &matrix)
{
	enum QUAT_INDEX
	{
		X_INDEX=0,
		Y_INDEX=1,
		Z_INDEX=2
	};
	static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};

	dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
	dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);

	if (trace > dFloat(0.0f)) {
		trace = dSqrt (trace + dFloat(1.0f));
		m_q0 = dFloat (0.5f) * trace;
		trace = dFloat (0.5f) / trace;
		m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
		m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
		m_q3 = (matrix[0][1] - matrix[1][0]) * trace;

	} else {
		QUAT_INDEX i = X_INDEX;
		if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
			i = Y_INDEX;
		}
		if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
			i = Z_INDEX;
		}
		QUAT_INDEX j = QIndex [i];
		QUAT_INDEX k = QIndex [j];

		trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
		trace = dSqrt (trace);

		dFloat* const ptr = &m_q1;
		ptr[i] = dFloat (0.5f) * trace;
		trace = dFloat (0.5f) / trace;
		m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
		ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
		ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
	}

#if _DEBUG

	dMatrix tmp (*this, matrix.m_posit);
	dMatrix unitMatrix (tmp * matrix.Inverse());
	for (int i = 0; i < 4; i ++) {
		dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
		dAssert (err < dFloat (1.0e-3f));
	}

	dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
	dAssert (err < dFloat(1.0e-3f));
#endif

}

bool dMatrix::TestIdentity() const
{
   const dMatrix & matrix = *this;
   const dMatrix & identity = dGetIdentityMatrix();
   bool isIdentity = true;
   for (int i = 0; isIdentity && (i < 3); i ++)
   {
      isIdentity &= dAbs (matrix[3][i]) < 1.0e-4f;
      for (int j = i; isIdentity && (j < 3); j ++)
         isIdentity &= dAbs (matrix[i][j] - identity[i][j]) < 1.0e-4f;
   }
   return isIdentity;
}

dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist)
{
	// shot a vertical ray from a high altitude and collect the intersection parameter.
	dVector p0 (origin); 
	dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f)); 

	dFloat parameter = 1.2f;
	NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, &parameter, RayPrefilter, 0);
	if (parameter < 1.0f) {
		p0 -= dVector (0.0f, dAbs (dist) * parameter, 0.0f, 0.0f);
	}
	return p0;
}

dFloat CustomDGRayCastCar::CalculateLongitudinalForce (int tireIndex, dFloat hubSpeed, dFloat tireLoad) const
{
	dFloat force = 0.0f;
	Tire& tire = m_tires[tireIndex];

//hubSpeed = 10.0;
//tire.m_torque = 500;

	dFloat velocAbs = dAbs (hubSpeed);
	if (velocAbs > 1.0e-2f) {
		dFloat den = 1.0f / velocAbs;

		float omega0 = hubSpeed;

		float maxSlip = m_normalizedLongitudinalForce.GetMaxValue ();
		float omega1 = velocAbs * maxSlip + hubSpeed;
		if (tire.m_torque < 0.0f) {
			//maxSlip *= -1.0f;
			omega1 = velocAbs * maxSlip - hubSpeed;
		}
//		float omega1 = velocAbs * maxSlip + hubSpeed;
		//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
	
		dFloat omega = 0.0f;
		dFloat slipRatio = 0.0f;
		for (int i = 0; i < 32; i ++) {
			omega  = (omega0 + omega1) * 0.5f;
			slipRatio = den * (omega - hubSpeed);
			force = m_normalizedLongitudinalForce.GetValue (slipRatio) * tireLoad;
			dFloat torque = tire.m_torque - tire.m_radius * force;
			if (torque > 1.0e-1f) {
				omega0 = omega;
			} else if (torque < -1.0e-1f) {
				omega1 = omega;
			} else {
				break;
			}
		}
		tire.m_angularVelocity = -omega / tire.m_radius;


	} else {
		if (dAbs (tire.m_torque) > 0.1f) {
			_ASSERTE (0);
		} 

		//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
	}

	return force;
}

bool dMatrix::SanityCheck() const
{
   dVector right (m_front * m_up);
   if (dAbs (right % m_right) < 0.9999f)
      return false;
   if (dAbs (m_right.m_w) > 0.0f)
      return false;
   if (dAbs (m_up.m_w) > 0.0f)
      return false;
   if (dAbs (m_right.m_w) > 0.0f)
      return false;
   if (dAbs (m_posit.m_w) != 1.0f)
      return false;
   return true;
}

dQuaternion::dQuaternion (const dVector &unitAxis, dFloat angle)
{
	angle *= dFloat (0.5f);
	m_q0 = dCos (angle);
	dFloat sinAng = dSin (angle);

#ifdef _DEBUG
	if (dAbs (angle) > dFloat(1.0e-6f)) {
		dAssert (dAbs (dFloat(1.0f) - unitAxis % unitAxis) < dFloat(1.0e-3f));
	} 
#endif
	m_q1 = unitAxis.m_x * sinAng;
	m_q2 = unitAxis.m_y * sinAng;
	m_q3 = unitAxis.m_z * sinAng;
}

dMatrix dMatrix::Inverse4x4 () const
{
   const dFloat tol = 1.0e-4f;
   dMatrix tmp (*this);
   dMatrix inv (dGetIdentityMatrix());
   for (int i = 0; i < 4; i ++)
   {
      dFloat diag = tmp[i][i];
      if (dAbs (diag) < tol)
      {
         int j = 0;
         for (j = i + 1; j < 4; j ++)
         {
            dFloat val = tmp[j][i];
            if (dAbs (val) > tol)
               break;
         }
         dAssert (j < 4);
         for (int k = 0; k < 4; k ++)
         {
            tmp[i][k] += tmp[j][k];
            inv[i][k] += inv[j][k];
         }
         diag = tmp[i][i];
      }
      dFloat invDiag = 1.0f / diag;
      for (int j = 0; j < 4; j ++)
      {
         tmp[i][j] *= invDiag;
         inv[i][j] *= invDiag;
      }
      tmp[i][i] = 1.0f;
      for (int j = 0; j < 4; j ++)
      {
         if (j != i)
         {
            dFloat pivot = tmp[j][i];
            for (int k = 0; k < 4; k ++)
            {
               tmp[j][k] -= pivot * tmp[i][k];
               inv[j][k] -= pivot * inv[i][k];
            }
            tmp[j][i] = 0.0f;
         }
      }
   }
   return inv;
}

void dCustomTireSpringDG::SubmitConstraints(dFloat timestep, int threadIndex)
{
	NewtonBody* BodyAttach;
	//NewtonBody* BodyFrame;
	//
	dVector tireOmega = dVector(0.0f, 0.0f, 0.0f);
	//BodyFrame = GetBody0();
	BodyAttach = GetBody1();
	//
	SteeringController(timestep);
	//
	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix(mChassisPivotMatrix, mTirePivotMatrix);
	//
	NewtonBodyGetOmega(BodyAttach, &tireOmega[0]);
	//
    mRealOmega = dAbs(tireOmega.DotProduct3(mChassisPivotMatrix.m_front));
	//
	TireCenterPin(timestep);
	//
	TireCenterBolt(timestep);
    //
	SuspenssionSpringLimits(timestep);
	//
	TireBreakAction(BodyAttach, timestep);
}

void CustomVehicleControllerComponentBrake::Update (dFloat timestep)
{
	for (dList<dList<CustomVehicleControllerBodyStateTire>::dListNode*>::dListNode* node = m_brakeTires.GetFirst(); node; node = node->GetNext()) {
		CustomVehicleControllerBodyStateTire& tire = node->GetInfo()->GetInfo();
		tire.m_brakeTorque = dMax (tire.m_brakeTorque, dAbs (m_maxBrakeTorque * m_param));
	}
}

dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist, dVector* const normal)
{
	// shot a vertical ray from a high altitude and collect the intersection parameter.
	dVector p0 (origin); 
	dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f)); 

	RayCastPlacementData parameter;
	NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, &parameter, RayPrefilter, 0);
	if (parameter.m_param < 1.0f) {
		p0 -= dVector (0.0f, dAbs (dist) * parameter.m_param, 0.0f, 0.0f);
		if (normal) {
			*normal = parameter.m_normal;
		}
	}
	return p0;
}

static dErr JakoSIAVelocity(VHTCase scase,dReal b,dReal h,dReal dh[2],dReal z,dScalar u[3])
{
  struct VHTRheology *rheo = &scase->rheo;
  const dReal
    p = rheo->pe,
    n = 1 ? 1 : 1./(p-1),
    B = rheo->B0 * pow(2*rheo->gamma0,(n-1)/(2*n)), // reduce to Stress = B |Du|^{1/n}
    A = pow(B,-n);
  const dScalar
    slope = dSqrt(dSqr(dh[0]) + dSqr(dh[1])),
    sliding = 0,
    hmz = z > h ? 0 : (z < b ? h-b : h-z),
    // The strain rate is: Du = A tau^n
    // where: tau = rho*grav*(h-z)*dh
    int_bz = -1. / (n+1) * (pow(hmz,n+1) - pow(h-b,n+1)), // \int_b^z (h-z)^n
    //bstress = rheo->rhoi * dAbs(rheo->gravity[2]) * (h-b) * slope, // diagnostic
    //rstress = dUnitNonDimensionalizeSI(scase->utable.Pressure,1e5),
    siaspeed = sliding + A * pow(rheo->rhoi * dAbs(rheo->gravity[2]) * slope, n) * int_bz, // Integrate strain rate from the bottom
    speed = siaspeed * (5 + 0 * (1 + tanh((z-b)/100))/2);
  u[0] = -speed / slope * dh[0];
  u[1] = -speed / slope * dh[1];
  u[2] = 0; // Would need another derivative to evaluate this and it should not be a big deal for this stationary computation
  //printf("u0 %g  u1 %g  rho %g  grav %g  stress %g  1bar %g  dh[2] %g %g  A %g  B %g;\n",u[0],u[1],rheo->rhoi,rheo->gravity[2],bstress,rstress,dh[0],dh[1],A,B);
  return 0;
}

bool dMatrix::TestOrthogonal() const
{
   dVector n (m_front * m_up);
   dFloat a = m_right % m_right;
   dFloat b = m_up % m_up;
   dFloat c = m_front % m_front;
   dFloat d = n % m_right;
   return (m_front[3] == dFloat (0.0f)) &
          (m_up[3] == dFloat (0.0f)) &
          (m_right[3] == dFloat (0.0f)) &
          (m_posit[3] == dFloat (1.0f)) &
          (dAbs (a - dFloat (1.0f)) < dFloat (1.0e-4f)) &
          (dAbs (b - dFloat (1.0f)) < dFloat (1.0e-4f)) &
          (dAbs (c - dFloat (1.0f)) < dFloat (1.0e-4f)) &
          (dAbs (d - dFloat (1.0f)) < dFloat (1.0e-4f));
}

dFloat dPolygonRayCast (const dVector& l0, const dVector& l1, int indexCount, const dFloat* const vertex, int strideInBytes, const int* const indices)
{
	int stride = strideInBytes / sizeof (dFloat);

	dBigVector line0 (l0);
	dBigVector line1(l1);
	dBigVector segment (line1 - line0);
	dBigVector normal (dPolygonNormal (indexCount, vertex, strideInBytes, indices));
	double den = normal % segment;
	if (dAbs(den) < 1.0e-6) {
		return 1.2f;
	}
	
	double sign = (den < 0.0f) ? 1.0f : -1.0f;

	int index = indices[indexCount - 1] * stride;
	dBigVector v0 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
	dBigVector p0v0 (v0 - line0);
	for (int i = 0; i < indexCount; i ++) {
		index = indices[i] * stride;
		dBigVector v1 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
		dBigVector p0v1 (v1 - line0);
		double alpha = sign * ((p0v1 * p0v0) % segment);
		if (alpha < 1.0e-3f) {
			return 1.2f;
		}
		p0v0 = p0v1;
	}

	double t = - ((line0 - v0) % normal) / den;
	if ((t < 0.0f) || (t > 1.0f)) {
		return 1.2f;
	}
	return dFloat (t);
}

void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	const dFloat angleError = GetMaxAngleError();
	dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up);
	NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	if (dAbs(angle0) > angleError) {
		const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep);
		NewtonUserJointSetRowAcceleration(m_joint, alpha);
	}

	dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right);
	NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	if (dAbs(angle1) > angleError) {
		const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep);
		NewtonUserJointSetRowAcceleration(m_joint, alpha);
	}

	// the joint angle can be determined by getting the angle between any two non parallel vectors
	m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front));

	// save the current joint Omega
	dVector omega0(0.0f);
	dVector omega1(0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}
	m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);

	if (m_options.m_option2) {
		if (m_options.m_option3) {
			dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
		} else {
			dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
		}
	} else if (m_options.m_option3) {
		dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
	} else if (m_angularFriction != 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
	}
}

 void SetSteerAngle (dFloat angle)
 {
     if (dAbs (angle - m_steeAngle) > 1.0e-4f) {
         m_steeAngle = angle;
         dMatrix rotation (dYawMatrix(m_steeAngle));
         m_chassisLocalMatrix = rotation * m_refChassisLocalMatrix;
     }
 }