static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
			       dVector3 p1, dVector3 p2, dVector3 p3)
{
  int k;
  dVector3 u1,u2,n,tmp;
  for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
  for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
  dReal d1 = dSqrt(dCalcVectorDot3(u1,u1));
  dReal d2 = dSqrt(dCalcVectorDot3(u2,u2));
  dNormalize3 (u1);
  dNormalize3 (u2);
  if (dFabs(dCalcVectorDot3(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
  dCalcVectorCross3(n,u1,u2);
  for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
  dReal d = -dCalcVectorDot3(n,p1);
  if (dFabs(dCalcVectorDot3(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
  if (dFabs(dCalcVectorDot3(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
  if (dFabs(dCalcVectorDot3(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
  dReal alpha = -(d+dCalcVectorDot3(n,v1))/dCalcVectorDot3(n,tmp);
  for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
  if (dFabs(dCalcVectorDot3(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
  if (alpha < 0) return 0;
  if (alpha > 1) return 0;
  for (k=0; k<3; k++) tmp[k] -= p1[k];
  dReal a1 = dCalcVectorDot3(u1,tmp);
  dReal a2 = dCalcVectorDot3(u2,tmp);
  if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
  return 1;
}

static int MakeRandomGuassianPointCloud (NewtonMesh* const mesh, dVector* const points, int count)
{
	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).Scale (0.5f);
	dVector origin = (maxBox + minBox).Scale (0.5f);

	dFloat biasExp = 10.0f;
	dFloat r = dSqrt (size.DotProduct3(size));
	r = powf(r, 1.0f/biasExp);
	for (int i = 0; i < count; i++) {
		dVector& p = points[i];
		bool test;
		do {
			p = dVector (2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 0.0f);
			dFloat len = dSqrt (p.DotProduct3(p));
			dFloat scale = powf(len, biasExp) / len;
			p = p.Scale (scale) + origin;
			test = (p.m_x > minBox.m_x) && (p.m_x < maxBox.m_x) && (p.m_y > minBox.m_y) && (p.m_y < maxBox.m_y) && (p.m_z > minBox.m_z) && (p.m_z < maxBox.m_z);
		} while (!test);
	}

	return count;
}

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

		dVector p0(matrix0.m_posit);
		dVector p1(matrix1.m_posit);

		dVector dir(p1 - p0);
		dFloat mag2 = dir % dir;
		dir = dir.Scale(1.0f / dSqrt(mag2));
		dMatrix matrix(dGrammSchmidt(dir));
		dFloat x = dSqrt(mag2) - m_distance;

		dVector com0;
		dVector com1;
		dVector veloc0;
		dVector veloc1;
		dMatrix body0Matrix;
		dMatrix body1Matrix;

		NewtonBody* const body0 = GetBody0();
		NewtonBody* const body1 = GetBody1();

		NewtonBodyGetCentreOfMass(body0, &com0[0]);
		NewtonBodyGetMatrix(body0, &body0Matrix[0][0]);
		NewtonBodyGetPointVelocity(body0, &p0[0], &veloc0[0]);

		NewtonBodyGetCentreOfMass(body1, &com1[0]);
		NewtonBodyGetMatrix(body1, &body1Matrix[0][0]);
		NewtonBodyGetPointVelocity(body1, &p1[0], &veloc1[0]);

		dFloat v((veloc0 - veloc1) % dir);
		dFloat a = (x - v * timestep) / (timestep * timestep);

		dVector r0((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front);
		dVector r1((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front);
		dFloat jacobian0[6];
		dFloat jacobian1[6];

		jacobian0[0] = matrix[0][0];
		jacobian0[1] = matrix[0][1];
		jacobian0[2] = matrix[0][2];
		jacobian0[3] = r0[0];
		jacobian0[4] = r0[1];
		jacobian0[5] = r0[2];

		jacobian1[0] = -matrix[0][0];
		jacobian1[1] = -matrix[0][1];
		jacobian1[2] = -matrix[0][2];
		jacobian1[3] = -r1[0];
		jacobian1[4] = -r1[1];
		jacobian1[5] = -r1[2];
		NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1);
		NewtonUserJointSetRowAcceleration(m_joint, a);
	}

void BodyCutForce(dBodyID body,float l_limit,float w_limit)
{
	const dReal wa_limit=w_limit/fixed_step;
	const dReal* force=	dBodyGetForce(body);
	dReal force_mag=dSqrt(dDOT(force,force));

	//body mass
	dMass m;
	dBodyGetMass(body,&m);

	dReal force_limit =l_limit/fixed_step*m.mass;

	if(force_mag>force_limit)
	{
		dBodySetForce(body,
			force[0]/force_mag*force_limit,
			force[1]/force_mag*force_limit,
			force[2]/force_mag*force_limit
			);
	}


	const dReal* torque=dBodyGetTorque(body);
	dReal torque_mag=dSqrt(dDOT(torque,torque));

	if(torque_mag<0.001f) return;

	dMatrix3 tmp,invI,I;

	// compute inertia tensor in global frame
	dMULTIPLY2_333 (tmp,m.I,body->R);
	dMULTIPLY0_333 (I,body->R,tmp);

	// compute inverse inertia tensor in global frame
	dMULTIPLY2_333 (tmp,body->invI,body->R);
	dMULTIPLY0_333 (invI,body->R,tmp);

	//angular accel
	dVector3 wa;
	dMULTIPLY0_331(wa,invI,torque);
	dReal wa_mag=dSqrt(dDOT(wa,wa));

	if(wa_mag>wa_limit)
	{
		//scale w 
		for(int i=0;i<3;++i)wa[i]*=wa_limit/wa_mag;
		dVector3 new_torqu;

		dMULTIPLY0_331(new_torqu,I,wa);

		dBodySetTorque 
			(
			body,
			new_torqu[0],
			new_torqu[1],
			new_torqu[2]
			);
	}
}

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

}

//void dMatrix::PolarDecomposition (dMatrix& orthogonal, dMatrix& symetric) const
void dMatrix::PolarDecomposition (dMatrix & transformMatrix, dVector & scale, dMatrix & stretchAxis, const dMatrix & initialStretchAxis) const
{
   // a polar decomposition decompose matrix A = O * S
   // where S = sqrt (transpose (L) * L)
   // calculate transpose (L) * L
   dMatrix LL ((*this) * Transpose());
   // check is this si a pure uniformScale * rotation * translation
   dFloat det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * (1.0f / 3.0f);
   dFloat invdet2 = 1.0f / det2;
   dMatrix pureRotation (LL);
   pureRotation[0] = pureRotation[0].Scale (invdet2);
   pureRotation[1] = pureRotation[1].Scale (invdet2);
   pureRotation[2] = pureRotation[2].Scale (invdet2);
   //dFloat soureSign = ((*this)[0] * (*this)[1]) % (*this)[2];
   dFloat sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
   dFloat det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
   if (dAbs (det - 1.0f) < 1.e-5f)
   {
      // this is a pure scale * rotation * translation
      det = sign * dSqrt (det2);
      scale[0] = det;
      scale[1] = det;
      scale[2] = det;
      scale[3] = 1.0f;
      det = 1.0f / det;
      transformMatrix.m_front = m_front.Scale (det);
      transformMatrix.m_up = m_up.Scale (det);
      transformMatrix.m_right = m_right.Scale (det);
      transformMatrix[0][3] = 0.0f;
      transformMatrix[1][3] = 0.0f;
      transformMatrix[2][3] = 0.0f;
      transformMatrix.m_posit = m_posit;
      stretchAxis = dGetIdentityMatrix();
   }
   else
   {
      stretchAxis = LL.JacobiDiagonalization (scale, initialStretchAxis);
      // I need to deal with buy seeing of some of the Scale are duplicated
      // do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
      // for now just us the matrix
      scale[0] = sign * dSqrt (scale[0]);
      scale[1] = sign * dSqrt (scale[1]);
      scale[2] = sign * dSqrt (scale[2]);
      scale[3] = 1.0f;
      dMatrix scaledAxis;
      scaledAxis[0] = stretchAxis[0].Scale (1.0f / scale[0]);
      scaledAxis[1] = stretchAxis[1].Scale (1.0f / scale[1]);
      scaledAxis[2] = stretchAxis[2].Scale (1.0f / scale[2]);
      scaledAxis[3] = stretchAxis[3];
      dMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
      transformMatrix = symetricInv * (*this);
      transformMatrix.m_posit = m_posit;
   }
}

void dQfromR (dQuaternion q, const dMatrix3 R)
{
  dAASSERT (q && R);
  dReal tr(0), s(0);
  tr = _R(0,0) + _R(1,1) + _R(2,2);
  if (tr >= 0)
  {
    s = dSqrt (tr + 1);
    q[0] = REAL(0.5) * s;
    s = REAL(0.5) * dRecip(s);
    q[1] = (_R(2,1) - _R(1,2)) * s;
    q[2] = (_R(0,2) - _R(2,0)) * s;
    q[3] = (_R(1,0) - _R(0,1)) * s;
  }
  else {
    // find the largest diagonal element and jump to the appropriate case
    if (_R(1,1) > _R(0,0)) {
      if (_R(2,2) > _R(1,1)) goto case_2;
      goto case_1;
    }
    if (_R(2,2) > _R(0,0)) goto case_2;
    goto case_0;

    case_0:
    s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
    q[1] = REAL(0.5) * s;
    s = REAL(0.5) * dRecip(s);
    q[2] = (_R(0,1) + _R(1,0)) * s;
    q[3] = (_R(2,0) + _R(0,2)) * s;
    q[0] = (_R(2,1) - _R(1,2)) * s;
    return;

    case_1:
    s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
    q[2] = REAL(0.5) * s;
    s = REAL(0.5) * dRecip(s);
    q[3] = (_R(1,2) + _R(2,1)) * s;
    q[1] = (_R(0,1) + _R(1,0)) * s;
    q[0] = (_R(0,2) - _R(2,0)) * s;
    return;

    case_2:
    s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
    q[3] = REAL(0.5) * s;
    s = REAL(0.5) * dRecip(s);
    q[1] = (_R(2,0) + _R(0,2)) * s;
    q[2] = (_R(1,2) + _R(2,1)) * s;
    q[0] = (_R(1,0) - _R(0,1)) * s;
    return;
  }
}

EXPORT_C void dQfromR (dQuaternion q, const dMatrix3 R)
{

  dReal tr,s;
  tr = _R(0,0) + _R(1,1) + _R(2,2);
  if (tr >= 0) {
    s = dSqrt (tr + REAL(1.0));
    q[0] = dMUL(REAL(0.5),s);
    s = dMUL(REAL(0.5),dRecip(s));
    q[1] = dMUL((_R(2,1) - _R(1,2)),s);
    q[2] = dMUL((_R(0,2) - _R(2,0)),s);
    q[3] = dMUL((_R(1,0) - _R(0,1)),s);
  }
  else {
    // find the largest diagonal element and jump to the appropriate case
    if (_R(1,1) > _R(0,0)) {
      if (_R(2,2) > _R(1,1)) goto case_2;
      goto case_1;
    }
    if (_R(2,2) > _R(0,0)) goto case_2;
    goto case_0;

    case_0:
    s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + REAL(1.0));
    q[1] = dMUL(REAL(0.5),s);
    s = dMUL(REAL(0.5),dRecip(s));
    q[2] = dMUL((_R(0,1) + _R(1,0)),s);
    q[3] = dMUL((_R(2,0) + _R(0,2)),s);
    q[0] = dMUL((_R(2,1) - _R(1,2)),s);
    return;

    case_1:
    s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + REAL(1.0));
    q[2] = dMUL(REAL(0.5),s);
    s = dMUL(REAL(0.5),dRecip(s));
    q[3] = dMUL((_R(1,2) + _R(2,1)),s);
    q[1] = dMUL((_R(0,1) + _R(1,0)),s);
    q[0] = dMUL((_R(0,2) - _R(2,0)),s);
    return;

    case_2:
    s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + REAL(1.0));
    q[3] = dMUL(REAL(0.5),s);
    s = dMUL(REAL(0.5),dRecip(s));
    q[1] = dMUL((_R(2,0) + _R(0,2)),s);
    q[2] = dMUL((_R(1,2) + _R(2,1)),s);
    q[0] = dMUL((_R(1,0) - _R(0,1)),s);
    return;
  }
}

	static void MagneticField (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
	{
		dFloat magnetStregnth;
		const NewtonBody* body0;
		const NewtonBody* body1; 
		const NewtonBody* magneticField;
		const NewtonBody* magneticPiece;

		body0 = NewtonJointGetBody0 (contactJoint);
		body1 = NewtonJointGetBody1 (contactJoint);

		// get the magnetic field body
		magneticPiece = body0;
		magneticField = body1;
		if (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(body0))) {
			magneticPiece = body1;
			magneticField = body0;
		}
		_ASSERTE (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(magneticField)));

		// calculate the magnetic force field

		dMatrix center;
		dMatrix location;

		NewtonBodyGetMatrix (magneticField, &center[0][0]);
		NewtonBodyGetMatrix (magneticPiece, &location[0][0]);

		Magnet* magnet;
		magnet = (Magnet*)NewtonBodyGetUserData(magneticField);

		magnetStregnth = magnet->m_magnetStregnth;

		// calculate the magnetic force;

		dFloat den;
		dVector force (center.m_posit - location.m_posit);

		den = force % force;
		den = magnetStregnth / (den * dSqrt (den) + 0.1f);
		force = force.Scale (den);

		// because we are modifiing one of the bodies membber in the call back, there uis a chace that 
		// another materail can be operations on the same object at the same time of aother thread
		// therfore we need to make the assigmnet in a critical section.
		NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (magneticPiece));
		
		// add the magner force
		NewtonBodyAddForce (magneticPiece, &force[0]);

		force = force.Scale (-1.0f);
		NewtonBodyAddForce (magnet->m_magneticCore, &force[0]);

		// also if the body is sleeping fore it to wake up for this frame
		NewtonBodySetFreezeState (magneticPiece, 0);
		NewtonBodySetFreezeState (magnet->m_magneticCore, 0);

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

		virtual void SubmitConstraints (dFloat timestep, int threadIndex)
		{
			dMatrix matrix;
			dVector com;
			const dFloat speed = 3.0f;
			NewtonBody* const body = GetBody0();

			NewtonBodyGetCentreOfMass(body, &com[0]);
			NewtonBodyGetMatrix(body, &matrix[0][0]);
			com = matrix.TransformVector(com);

			switch (m_state)
			{
				case m_stop:
				{
					SetTargetPosit (com);
					break;
				}

				case m_driving:
				{
					dVector veloc (m_target - com);
					veloc = veloc.Scale (speed / dSqrt (veloc % veloc)); 
					dVector target = com + veloc.Scale(timestep);
					SetTargetPosit (target);
					break;
				}

				default:;
				dAssert (0);
			}

			CustomKinematicController::SubmitConstraints (timestep, threadIndex);
		}

int dFactorCholesky (dReal *A, int n)
{
  int i,j,k,nskip;
  dReal sum,*a,*b,*aa,*bb,*cc,*recip;
  dAASSERT (n > 0 && A);
  nskip = dPAD (n);
  recip = (dReal*) ALLOCA (n * sizeof(dReal));
  aa = A;
  for (i=0; i<n; i++) {
    bb = A;
    cc = A + i*nskip;
    for (j=0; j<i; j++) {
      sum = *cc;
      a = aa;
      b = bb;
      for (k=j; k; k--) sum -= (*(a++))*(*(b++));
      *cc = sum * recip[j];
      bb += nskip;
      cc++;
    }
    sum = *cc;
    a = aa;
    for (k=i; k; k--, a++) sum -= (*a)*(*a);
    if (sum <= REAL(0.0)) return 0;
    *cc = dSqrt(sum);
    recip[i] = dRecip (*cc);
    aa += nskip;
  }
  return 1;
}

void CPHCapture::CapturedUpdate()
{
    m_island.Unmerge();
    if(m_character->CPHObject::is_active())
    {
        m_taget_element->Enable();
    }

    if(!m_taget_element->isActive()||dDOT(m_joint_feedback.f2,m_joint_feedback.f2)>m_capture_force*m_capture_force)
    {
        Release();
        return;
    }

    float mag=dSqrt(dDOT(m_joint_feedback.f1,m_joint_feedback.f1));

    //m_back_force=m_back_force*0.999f+ ((mag<m_capture_force/5.f) ? mag : (m_capture_force/5.f))*0.001f;
    //
    if(b_character_feedback&&mag>m_capture_force/2.2f)
    {
        float f=mag/(m_capture_force/15.f);
        m_character->ApplyForce(m_joint_feedback.f1[0]/f,m_joint_feedback.f1[1]/f,m_joint_feedback.f1[2]/f);
    }

    Fvector capture_bone_position;
    //CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject());
    capture_bone_position.set(m_capture_bone->mTransform.c);
    m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position);
    dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
}

void dCustomUpVector::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);
  
	// if the body ha rotated by some amount, the there will be a plane of rotation
	dVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front));
	dFloat mag = lateralDir.DotProduct3(lateralDir);
	if (mag > 1.0e-6f) {
		// if the side vector is not zero, it means the body has rotated
		mag = dSqrt (mag);
		lateralDir = lateralDir.Scale (1.0f / mag);
		dFloat angle = dAsin (mag);

		// add an angular constraint to correct the error angle
		NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);

		// in theory only one correction is needed, but this produces instability as the body may move sideway.
		// a lateral correction prevent this from happening.
		dVector frontDir (lateralDir.CrossProduct(matrix1.m_front));
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
 	} else {
		// if the angle error is very small then two angular correction along the plane axis do the trick
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
	}
}

// rolling friction works as follow: the idealization of the contact of a spherical object 
// with a another surface is a point that pass by the center of the sphere.
// in most cases this is enough to model the collision but in insufficient for modeling 
// the rolling friction. In reality contact with the sphere with the other surface is not 
// a point but a contact patch. A contact patch has the property the it generates a fix 
// constant rolling torque that opposes the movement of the sphere.
// we can model this torque by adding a clamped torque aligned to the instantaneously axis 
// of rotation of the ball. and with a magnitude of the stopping angular acceleration.
void CustomDryRollingFriction::SubmitConstrainst (dFloat timestep, int threadIndex)
{
	dVector omega;
	dFloat omegaMag;
	dFloat torqueFriction;

	// get the omega vector
	NewtonBodyGetOmega(m_body0, &omega[0]);

	omegaMag = dSqrt (omega % omega);
	if (omegaMag > 0.1f) {
		// tell newton to used this the friction of the omega vector to apply the rolling friction
		dVector pin (omega.Scale (1.0f / omegaMag));
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &pin[0]);

		// calculate the acceleration to stop the ball in one time step
		NewtonUserJointSetRowAcceleration (m_joint, -omegaMag / timestep);

		// set the friction limit proportional the sphere Inertia
		torqueFriction = m_frictionTorque * m_frictionCoef;
		NewtonUserJointSetRowMinimumFriction (m_joint, -torqueFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint, torqueFriction);

	} else {
		// when omega is too low sheath a little bit and damp the omega directly
		omega = omega.Scale (0.2f);
		NewtonBodySetOmega(m_body0, &omega[0]);
	}
}

void DebugShowGeometryCollision (void* userData, int vertexCount, const dFloat* const faceVertec, int id)
{
	//DEBUG_DRAW_MODE mode = (DEBUG_DRAW_MODE) ((int)userData); //NOTE error: cast from ‘void*’ to ‘int’ loses precision
	DEBUG_DRAW_MODE mode = (DEBUG_DRAW_MODE) ((intptr_t)userData);
	
	if (mode == m_lines) {
		int index = vertexCount - 1;
		dVector p0 (faceVertec[index * 3 + 0], faceVertec[index * 3 + 1], faceVertec[index * 3 + 2]);
		for (int i = 0; i < vertexCount; i ++) {
			dVector p1 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]);
			glVertex3f (p0.m_x, p0.m_y, p0.m_z);
			glVertex3f (p1.m_x, p1.m_y, p1.m_z);
			p0 = p1;
		}
	} else {
		dVector p0 (faceVertec[0 * 3 + 0], faceVertec[0 * 3 + 1], faceVertec[0 * 3 + 2]);
		dVector p1 (faceVertec[1 * 3 + 0], faceVertec[1 * 3 + 1], faceVertec[1 * 3 + 2]);
		dVector p2 (faceVertec[2 * 3 + 0], faceVertec[2 * 3 + 1], faceVertec[2 * 3 + 2]);

		dVector normal ((p1 - p0) * (p2 - p0));
		normal = normal.Scale (1.0f / dSqrt (normal % normal));
		glNormal3f(normal.m_x, normal.m_y, normal.m_z);
		for (int i = 2; i < vertexCount; i ++) {
			p2 = dVector (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]);
			glVertex3f (p0.m_x, p0.m_y, p0.m_z);
			glVertex3f (p1.m_x, p1.m_y, p1.m_z);
			glVertex3f (p2.m_x, p2.m_y, p2.m_z);
			p1 = p2;
		}
	}
}

dQuaternion dQuaternion::Slerp (const dQuaternion &QB, dFloat t) const
{
    dFloat dot;
    dFloat ang;
    dFloat Sclp;
    dFloat Sclq;
    dFloat den;
    dFloat sinAng;
    dQuaternion Q;

    dot = DotProduct (QB);
    _ASSERTE (dot >= 0.0f);

    if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) {
        if (dot < dFloat(0.995f)) {

            ang = dAcos (dot);

            sinAng = dSin (ang);
            den = dFloat(1.0f) / sinAng;

            Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den;
            Sclq = dSin (t * ang) * den;

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

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

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

        Sclp = dSin ((dFloat(1.0f) - t) * dFloat (3.141592f *0.5f));
        Sclq = dSin (t * dFloat (3.141592f * 0.5f));

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

    dot = Q.DotProduct (Q);
    if ((dot) < (1.0f - 1.0e-4f) ) {
        dot = dFloat(1.0f) / dSqrt (dot);
        //dot = dgRsqrt (dot);
        Q.m_q0 *= dot;
        Q.m_q1 *= dot;
        Q.m_q2 *= dot;
        Q.m_q3 *= dot;
    }
    return Q;
}

NewtonMesh* DemoMesh::CreateNewtonMesh(NewtonWorld* const world, const dMatrix& meshMatrix)
{
	NewtonMesh* const mesh = NewtonMeshCreate(world);

	NewtonMeshBeginBuild (mesh);
	dMatrix rotation ((meshMatrix.Inverse4x4()).Transpose4X4());

	for (dListNode* node = GetFirst(); node; node = node->GetNext()) {
		DemoSubMesh& segment = node->GetInfo();
		for (int i = 0; i < segment.m_indexCount; i += 3) {
			NewtonMeshBeginFace(mesh);
			for (int j = 0; j < 3; j ++) {
				int index = segment.m_indexes[i + j];
				dVector p (meshMatrix.TransformVector(dVector (m_vertex[index * 3 + 0], m_vertex[index * 3 + 1], m_vertex[index * 3 + 2], 0.0f)));
				dVector n (rotation.RotateVector(dVector (m_normal[index * 3 + 0], m_normal[index * 3 + 1], m_normal[index * 3 + 2], 0.0f)));
				dAssert ((n.DotProduct3(n)) > 0.0f);
				n = n.Scale (1.0f / dSqrt (n.DotProduct3(n)));
				
				NewtonMeshAddPoint(mesh, p.m_x, p.m_y, p.m_z);
				NewtonMeshAddNormal(mesh, n.m_x, n.m_y, n.m_z);
				NewtonMeshAddMaterial(mesh, segment.m_textureHandle);
				NewtonMeshAddUV0(mesh, m_uv[index * 2 + 0], m_uv[index * 2 + 1]);
			}
			NewtonMeshEndFace(mesh);
//			NewtonMeshAddFace(mesh, 3, &point[0][0], sizeof (point) / 3, segment.m_textureHandle);
		}
	}

	NewtonMeshEndBuild(mesh);
	return mesh;
}

int CreateCone (dVector* const points, dVector* const normals, int segments, dFloat radius, dFloat height, int maxPoints)
{
	dMatrix rotation (dPitchMatrix((-360.0f / segments) * 3.141592f/180.0f));
	dVector p0 (0.0, 0.0f, 0.0f, 0.0f);
	dVector p1 (0.0, radius, 0.0f, 0.0f);
	dVector q1 (height, 0.0f, 0.0f, 0.0f);

	dVector n (radius, height, 0.0f, 0.0f);
	n = n.Scale (1.0f / dSqrt (n.DotProduct3(n)));

	int count = 0;
	for (int i = 0; (i < segments) && (count < maxPoints); i ++) {
		dVector p2 (rotation.RotateVector(p1));

		normals[count] = dVector (-1.0f, 0.0f, 0.0f, 0.0f);
		points[count * 3 + 0] = p0;
		points[count * 3 + 1] = p1;
		points[count * 3 + 2] = p2;
		count ++;

		normals[count] = dVector (n.m_x, n.m_y, n.m_z, 0.0f);
		points[count * 3 + 0] = p1;
		points[count * 3 + 1] = q1;
		points[count * 3 + 2] = p2;
		count ++;

		n = rotation.RotateVector(n);
		p1 = p2;
	}
	return count;
}

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

void dBezierSpline::CreateCubicKnotVector(int count, const dBigVector* const points)
{
	dFloat64 d = 0.0f;
	dAssert (count >= 2);

	dFloat64 u[D_BEZIER_LOCAL_BUFFER_SIZE];
	u[0] = 0.0f;
	for (int i = 1; i < count; i ++) {
		dBigVector step (points[i] - points[i - 1]);
		dFloat64 len = dSqrt (step.DotProduct3(step));
//		u[i] = len; 
		u[i] = dSqrt (len); 
		d += u[i];
	}

	for (int i = 1; i < count; i ++) {
		u[i] = u[i-1] + u[i] / d;
	}
	u[0] = 0.0f;
	u[count - 1] = 1.0f;

	m_degree = 3;
	if ((count + 2 * m_degree) != m_knotsCount) {
		if (m_knotVector) {
			Free (m_knotVector);
		}
		m_knotsCount = count + 2 * m_degree;
		m_knotVector = (dFloat64*) Alloc (m_knotsCount * sizeof (dFloat64));
	}

	for (int i = 0; i < (m_degree + 1); i ++) {
		m_knotVector[i] = 0.0f;
		m_knotVector[i + m_knotsCount - m_degree - 1] = 1.0f;
	}

	for (int i = 1; i < (count - 1); i ++) {
		dFloat64 acc = 0.0f;
		for (int j = 0; j < m_degree; j ++) {
			acc += u[j + i - 1];
		}
		m_knotVector[m_degree + i] = acc / 3.0f;
	}

//m_knotVector[4] = 1.0f/4.0f;
//m_knotVector[5] = 2.0f/4.0f;
//m_knotVector[6] = 3.0f/4.0f;
}