dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
	dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	dgFloat32 x = dir.GetScalar();
	if (dgAbsf (x) > dgFloat32 (0.9999f)) {
		return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	}

	dgVector sideDir (m_yzMask & dir);
	sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
	return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const
{
	dgAssert (dir.m_w == dgFloat32 (0.0f));
	dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	dgFloat32 x = dir.GetScalar();
	if (dgAbs (x) > dgFloat32 (0.99995f)) {
		return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	}

	dgVector sideDir (m_yzMask & dir);
	dgAssert (sideDir.DotProduct(sideDir).GetScalar() > dgFloat32 (0.0f));
	return sideDir.Normalize().Scale(m_radius);
}
dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgInt32* const vertexIndex) const
{
	*vertexIndex = -1;
	dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	dgFloat32 x = dir.GetScalar();
	if (dgAbsf (x) > dgFloat32 (0.99995f)) {
		return dgVector::m_zero; 
	}

	dgVector sideDir (m_yzMask & dir);
	dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f));
	return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius);
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir) const
{
	_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	if (dgAbsf (dir.m_x) > dgFloat32 (0.9998f)) {
		dgFloat32 x0;
		x0 = (dir.m_x >= dgFloat32 (0.0f)) ? m_height : -m_height;
		return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f)); 
	}

	_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
	sideDir = sideDir.Scale (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f)));
	return sideDir + dir.Scale (m_height);
}
dgVector dgCollisionChamferCylinder::ConvexConicSupporVertex (const dgVector& dir) const
{
	dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	dgFloat32 x = dir.GetScalar();
	//if (dgAbsf (dir.m_x) > dgFloat32 (0.99995f)) {
	if (dgAbsf (x) > dgFloat32 (0.99995f)) {
		return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	}

	//dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
	dgVector sideDir (m_yzMask & dir);
	dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f));
	//return sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir));
	return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius);
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
	dgAssert (dir.m_w == dgFloat32 (0.0f));
	dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	dgFloat32 x = dir.GetScalar();
	if (dgAbs (x) > dgFloat32 (0.9999f)) {
		//return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
		return dgVector (dgSign (x) * m_height, m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	}

	dgVector sideDir (m_yzMask & dir);
	//sideDir = sideDir * sideDir.InvMagSqrt();
	sideDir = sideDir.Normalize();
	return sideDir.Scale(m_radius) + dir.Scale (m_height);
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
	dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

	dgFloat32 x = dir.GetScalar();
	if (dgAbsf (x) > dgFloat32 (0.9999f)) {
		dgFloat32 x0 = (x >= dgFloat32 (0.0f)) ? m_height : -m_height;
		return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f)); 
	}

//	dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
	dgVector sideDir (m_yzMask & dir);
//	sideDir = sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f)));
	sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
//	return sideDir + dir.Scale3 (m_height);
	return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
void dCustomPlayerController::ResolveCollision()
{
	dMatrix matrix;
	NewtonWorldConvexCastReturnInfo info[D_MAX_ROWS];
	NewtonWorld* const world = m_manager->GetWorld();

	NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
	NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);

	int contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
	if (!contactCount) {
		return;
	}

	dFloat maxPenetration = 0.0f;
	for (int i = 0; i < contactCount; i ++) {
		maxPenetration = dMax (info[i].m_penetration, maxPenetration);
	}

	if (maxPenetration > D_MAX_COLLISION_PENETRATION) {
		ResolveInterpenetrations(contactCount, info);
		NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
	}
	
	int rowCount = 0;
	dVector zero(0.0f);

	dMatrix invInertia;
	dVector com(0.0f);
	dVector veloc(0.0f);
	dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
	dFloat rhs[D_MAX_ROWS];
	dFloat low[D_MAX_ROWS];
	dFloat high[D_MAX_ROWS];
	dFloat impulseMag[D_MAX_ROWS];
	int normalIndex[D_MAX_ROWS];
	
	NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);
	NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
	NewtonBodyGetInvInertiaMatrix(m_kinematicBody, &invInertia[0][0]);

//	const dMatrix localFrame (dPitchMatrix(m_headingAngle) * m_localFrame * matrix);
	const dMatrix localFrame (m_localFrame * matrix);

	com = matrix.TransformVector(com);
	com.m_w = 0.0f;
	for (int i = 0; i < contactCount; i ++) {
		NewtonWorldConvexCastReturnInfo& contact = info[i];

		dVector point (contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
		dVector normal (contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);


		jt[rowCount].m_linear = normal;
		jt[rowCount].m_angular = (point - com).CrossProduct(normal);

		low[rowCount] = 0.0f;
		high[rowCount] = 1.0e12f;
		normalIndex[rowCount] = 0;
		dVector tmp (veloc * jt[rowCount].m_linear.Scale (1.001f));
		rhs[rowCount] = - (tmp.m_x + tmp.m_y + tmp.m_z);
		rowCount ++;
		dAssert (rowCount < (D_MAX_ROWS - 3));

		//dFloat updir = localFrame.m_front.DotProduct3(normal);
		dFloat friction = m_manager->ContactFriction(this, point, normal, contact.m_hitBody);
		if (friction > 0.0f)
		{
			// add lateral traction friction
			dVector sideDir (localFrame.m_up.CrossProduct(normal).Normalize());

			jt[rowCount].m_linear = sideDir;
			jt[rowCount].m_angular = (point - com).CrossProduct(sideDir);

			low[rowCount] = -friction;
			high[rowCount] = friction;
			normalIndex[rowCount] = -1;

			dVector tmp1 (veloc * jt[rowCount].m_linear);
			rhs[rowCount] =  -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z);
			rowCount++;
			dAssert (rowCount < (D_MAX_ROWS - 3));

			// add longitudinal  traction friction
			dVector frontDir (normal.CrossProduct(sideDir));
			jt[rowCount].m_linear = frontDir;
			jt[rowCount].m_angular = (point - com).CrossProduct(frontDir);

			low[rowCount] = -friction;
			high[rowCount] = friction;
			normalIndex[rowCount] = -2;
			dVector tmp2 (veloc * jt[rowCount].m_linear);
			rhs[rowCount] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z);
			rowCount++;
			dAssert(rowCount < (D_MAX_ROWS - 3));
		}
	}

	for (int i = 0; i < 3; i++) {
		jt[rowCount].m_linear = zero;
		jt[rowCount].m_angular = zero;
		jt[rowCount].m_angular[i] = dFloat(1.0f);
		rhs[rowCount] = 0.0f;
		impulseMag[rowCount] = 0;
		low[rowCount] = -1.0e12f;
		high[rowCount] = 1.0e12f;
		normalIndex[rowCount] = 0;

		rowCount ++;
		dAssert (rowCount < D_MAX_ROWS);
	}

	dVector impulse (veloc.Scale (m_mass) + CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));
	veloc = impulse.Scale(m_invMass);
	NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
}
    void SubmitConstraints (dFloat timestep, int threadIndex)
    {
        dFloat den;
        dFloat relAccel;
        dFloat jacobian0[6];
        dFloat jacobian1[6];

        dMatrix leftMatrix;
        dMatrix rightMatrix;
        dMatrix chassisMatrix;

        NewtonBodyGetMatrix (m_chassis, &chassisMatrix[0][0]);
        NewtonBodyGetMatrix (m_leftTire->GetBody1(), &leftMatrix[0][0]);
        NewtonBodyGetMatrix (m_rightTire->GetBody1(), &rightMatrix[0][0]);

        // calculate the geometrical turn radius of for this axle

        dVector leftOrigin (chassisMatrix.UntransformVector(leftMatrix.m_posit));
        dVector rightOrigin (chassisMatrix.UntransformVector(rightMatrix.m_posit));
        dVector axleCenter ((rightOrigin + leftOrigin).Scale (0.5f));

        dVector tireAxisDir (chassisMatrix.UnrotateVector((leftMatrix.m_front + rightMatrix.m_front).Scale (0.5f)));
        axleCenter.m_y = 0.0f;
        tireAxisDir.m_y = 0.0f;

        dVector sideDir (0.0f, 0.0f, 1.0f, 0.0f);
        dVector deltaDir (tireAxisDir - sideDir);
        relAccel = 0.0f;
        den = deltaDir % deltaDir;
        if (den > 1.0e-6f) {
            dFloat R;
            dFloat num;
            dFloat ratio;
            dFloat wl;
            dFloat wr;
            dFloat rl;
            dFloat rr;
            dFloat relOmega;

            num = axleCenter % deltaDir;
            R = - num / den;

            rr = (rightOrigin % sideDir);
            rl = (leftOrigin % sideDir);
            ratio = (R + rr) / (R + rl);

            dVector omegaLeft;
            dVector omegaRight;
            // calculate the angular velocity for both bodies
            NewtonBodyGetOmega(m_leftTire->GetBody1(), &omegaLeft[0]);
            NewtonBodyGetOmega(m_rightTire->GetBody1(), &omegaRight[0]);

            // get angular velocity relative to the pin vector
            wl = -(omegaLeft % leftMatrix.m_front);
            wr = omegaRight % rightMatrix.m_front;

            // establish the gear equation.
            relOmega = wl + ratio * wr;
            relAccel = - 0.5f * relOmega / timestep;
        }

        jacobian0[0] = 0.0f;
        jacobian0[1] = 0.0f;
        jacobian0[2] = 0.0f;
        jacobian0[3] = leftMatrix.m_front.m_x * -1.0f;
        jacobian0[4] = leftMatrix.m_front.m_y * -1.0f;
        jacobian0[5] = leftMatrix.m_front.m_z * -1.0f;

        jacobian1[0] = 0.0f;
        jacobian1[1] = 0.0f;
        jacobian1[2] = 0.0f;
        jacobian1[3] = rightMatrix.m_front.m_x;
        jacobian1[4] = rightMatrix.m_front.m_y;
        jacobian1[5] = rightMatrix.m_front.m_z;

        NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
        NewtonUserJointSetRowAcceleration (m_joint, relAccel);
        NewtonUserJointSetRowMaximumFriction(m_joint, m_maxfriction);
        NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxfriction);

    }