void CustomBallAndSocketWithFriction::SubmitConstraints(dFloat timestep, int threadIndex)
{
	CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
	dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
	dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);

	// get the omega vector
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}

	dVector relOmega(omega0 - omega1);
	dFloat omegaMag = dSqrt(relOmega % relOmega);
	if (omegaMag > 0.1f) {
		// tell newton to used this the friction of the omega vector to apply the rolling friction
		dMatrix basis(dGrammSchmidt(relOmega));

		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);

		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);

		// calculate the acceleration to stop the ball in one time step
		dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[0][0]);
		NewtonUserJointSetRowAcceleration(m_joint, -omegaMag * invTimestep);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
	} else {
		// when omega is too low this is correct but the small angle approximation theorem.
		dMatrix basis(dGetIdentityMatrix());
		for (int i = 0; i < 3; i++) {
			NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[i][0]);
			NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
			NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
		}
	}
}
dgVector dgBallConstraint::GetJointOmega () const
{
	dgAssert (m_body0);
	dgAssert (m_body1);
	const dgMatrix& matrix = m_body0->GetMatrix();

	dgVector dir0 (matrix.RotateVector (m_localMatrix0[0]));
	dgVector dir1 (matrix.RotateVector (m_localMatrix0[1]));
	dgVector dir2 (matrix.RotateVector (m_localMatrix0[2]));

	const dgVector& omega0 = m_body0->GetOmega();
	const dgVector& omega1 = m_body1->GetOmega();

//	dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
//	if (m_body1) {
//		omega1 = m_body1->GetOmega();
//	}

	dgVector relOmega (omega0 - omega1);
	return dgVector (relOmega % dir0, relOmega % dir1, relOmega % dir2, dgFloat32 (0.0f));

}
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{

	// check if this is an impulsive time step
	
	if (timestep > 0.0f) {
		dMatrix matrix0;
		dVector v(0.0f);
		dVector w(0.0f);
		dVector cg(0.0f);

		dFloat invTimestep = 1.0f / timestep;

		// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
		NewtonBodyGetOmega (m_body0, &w[0]);
		NewtonBodyGetVelocity (m_body0, &v[0]);
		NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
		NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);

		dVector p0 (matrix0.TransformVector (m_localHandle));

		dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg));
		dVector relPosit (m_targetPosit - p0);
		dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
		dVector relAccel (relVeloc.Scale (invTimestep * 0.3f)); 
			
		// Restrict the movement on the pivot point along all tree orthonormal direction
		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		if (m_pickMode) {
			dQuaternion rotation;

			NewtonBodyGetRotation (m_body0, &rotation.m_q0);
			if (m_targetRot.DotProduct (rotation) < 0.0f) {
				rotation.m_q0 *= -1.0f; 
				rotation.m_q1 *= -1.0f; 
				rotation.m_q2 *= -1.0f; 
				rotation.m_q3 *= -1.0f; 
			}

			dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w);
			dFloat mag = relOmega % relOmega;
			if (mag > 1.0e-6f) {
				dVector pin (relOmega.Scale (1.0f / mag));
				dMatrix basis (dGrammSchmidt (pin)); 	
				dFloat relSpeed = dSqrt (relOmega % relOmega);
				dFloat relAlpha = relSpeed * invTimestep;

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
				NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
				NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

			} else {

				dVector relAlpha (w.Scale (-invTimestep));
				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);
			}

		} else {
			// this is the single handle pick mode, add some angular friction

			dVector relAlpha = w.Scale (-invTimestep);
			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);
		}
	}
}
void CustomUniversal::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);

	// Restrict the movement on the pivot point along all tree orthonormal direction
	NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
	NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
	NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);


	// construct an orthogonal coordinate system with these two vectors
	dMatrix matrix1_1;
	matrix1_1.m_up = matrix1.m_up;
	matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
	matrix1_1.m_right = matrix1_1.m_right.Scale (1.0f / dSqrt (matrix1_1.m_right % matrix1_1.m_right));
	matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
	NewtonUserJointAddAngularRow (m_joint, CalculateAngle (matrix0.m_front, matrix1_1.m_front, matrix1_1.m_right), &matrix1_1.m_right[0]);

	dFloat sinAngle_0;
	dFloat cosAngle_0;
	CalculateAngle (matrix1_1.m_up, matrix0.m_up, matrix1_1.m_front, sinAngle_0, cosAngle_0);
	dFloat angle0 = -m_curJointAngle_0.Update (cosAngle_0, sinAngle_0);

	dFloat sinAngle_1;
	dFloat cosAngle_1;
	CalculateAngle(matrix1.m_front, matrix1_1.m_front, matrix1_1.m_up, sinAngle_1, cosAngle_1);
	dFloat angle1 = -m_curJointAngle_1.Update (cosAngle_1, sinAngle_1);

	dVector omega0 (0.0f, 0.0f, 0.0f, 0.0f);
	dVector omega1 (0.0f, 0.0f, 0.0f, 0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}

	// calculate the desired acceleration
	dVector relOmega (omega0 - omega1);
	m_jointOmega_0 = relOmega % matrix0.m_front;
	m_jointOmega_1 = relOmega % matrix1.m_up;
	
	// check is the joint limit are enable
	if (m_limit_0_On) {
		if (angle0 < m_minAngle_0) {
			dFloat relAngle = angle0 - m_minAngle_0;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffeners here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else if (angle0 > m_maxAngle_0) {
			dFloat relAngle = angle0 - m_maxAngle_0;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
		}

		// check is the joint limit motor is enable
	} else if (m_angularMotor_0_On) {
		// calculate the desired acceleration
//		dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
		dFloat relAccel = m_angularAccel_0 - m_angularDamp_0 * m_jointOmega_0;

		// add and angular constraint row to that will set the relative acceleration to zero
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);

		// override the joint acceleration.
		NewtonUserJointSetRowAcceleration (m_joint, relAccel);
	}

	// if limit are enable ...
	if (m_limit_1_On) {
		if (angle1 < m_minAngle_1) {
			dFloat relAngle = angle1 - m_minAngle_1;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffeners here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else if (angle1 > m_maxAngle_1) {
			dFloat relAngle = angle1 - m_maxAngle_1;
			
			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
  		}
	} else if (m_angularMotor_1_On) {
		// calculate the desired acceleration
		dFloat relAccel = m_angularAccel_1 - m_angularDamp_1 * m_jointOmega_1;

		// add and angular constraint row to that will set the relative acceleration to zero
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
		
		// override the joint acceleration.
		NewtonUserJointSetRowAcceleration (m_joint, relAccel);
	}
}
void dCustomBallAndSocket::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);
	SubmitLinearRows(0x07, matrix0, matrix1);

	const dVector& coneDir0 = matrix0.m_front;
	const dVector& coneDir1 = matrix1.m_front;

	dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0);
	dMatrix coneRotation(dGetIdentityMatrix());
	dVector lateralDir(matrix0.m_up);

	if (cosAngleCos < 0.9999f) {
		lateralDir = coneDir1.CrossProduct(coneDir0);
		dFloat mag2 = lateralDir.DotProduct3(lateralDir);
		if (mag2 > 1.0e-4f) {
			lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
			coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
		} else {
			lateralDir = matrix0.m_up.Scale (-1.0f);
			coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
		}
	}

	dVector omega0(0.0f);
	dVector omega1(0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}
	dVector relOmega(omega0 - omega1);

	// do twist angle calculations
	dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse());
	dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1]));
	if (m_options.m_option0) {
		if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) {
			NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		} else {
			if (m_options.m_option1) {
				// TODO spring option
				dAssert (0);
			} else {
				SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep);
			}
		}
	} else if (m_options.m_option1) {
		// TODO spring option
		dAssert (0);
	} else if (m_twistFriction > 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

		NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
	}

	// do twist cone angle calculations
	if (m_options.m_option2) {
		if ((m_maxConeAngle == 0.0f)) {
			dMatrix localMatrix(matrix0 * matrix1.Inverse());
			dVector euler0;
			dVector euler1;
			localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
			NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
			NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		} else {
			if (m_options.m_option3) {
				// TODO spring option
				dAssert(0);
			} else {
				dFloat jointOmega = relOmega.DotProduct3(lateralDir);
				dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)));
				dFloat coneAngle = currentAngle + jointOmega * timestep;
				if (coneAngle >= m_maxConeAngle) {
					//dQuaternion rot(lateralDir, coneAngle);
					//dVector frontDir(rot.RotateVector(coneDir1));
					//dVector upDir(lateralDir.CrossProduct(frontDir));

					dVector upDir(lateralDir.CrossProduct(coneDir0));
					NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
					NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
					NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

					NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
					NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
					NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
					const dFloat invtimestep = 1.0f / timestep;
					const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep;
					const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
					NewtonUserJointSetRowAcceleration(m_joint, stopAccel);

				} else if (m_coneFriction != 0) {
					NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
					NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
					NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
					NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);

					dVector upDir(lateralDir.CrossProduct(coneDir0));
					NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
					NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
					NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
					NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
				}
			}
		}
	} else if (m_options.m_option3) {
		// TODO spring option
		dAssert(0);
	} else if (m_coneFriction > 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
		NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);

		dVector upDir(lateralDir.CrossProduct(coneDir0));
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
		NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
	}
}