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