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);
}
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);
}
dCustomKinematicController::dCustomKinematicController(NewtonBody* const body, const dVector& handleInGlobalSpace)
:dCustomJoint(6, body, NULL)
{
dMatrix matrix;
dAssert (GetBody0() == body);
NewtonBodyGetMatrix (body, &matrix[0][0]);
matrix.m_posit = handleInGlobalSpace;
Init (body, matrix);
}
void MoveToTarget (NewtonBody* const targetBody)
{
dAssert ((targetBody == m_triggerPort0) || (targetBody == m_triggerPort1));
if (targetBody != m_currentPort) {
m_currentPort = targetBody;
// get the location of next target
dMatrix targetMatrix;
NewtonBodyGetMatrix(m_currentPort, &targetMatrix[0][0]);
m_target = targetMatrix.m_posit;
// the body might be sleep we need to activate the body by sett the sleep stet off
NewtonBodySetSleepState(GetBody0(), 0);
m_state = m_driving;
}
}
void dgConstraint::InitInfo (dgConstraintInfo* const info) const
{
info->m_attachBody_0 = GetBody0();
dgAssert (info->m_attachBody_0);
dgWorld* const world = info->m_attachBody_0->GetWorld();
if (info->m_attachBody_0 == (dgBody*)world->GetSentinelBody()) {
info->m_attachBody_0 = NULL;
}
info->m_attachBody_1 = GetBody1();
if (info->m_attachBody_1 == (dgBody*)world->GetSentinelBody()) {
info->m_attachBody_1 = NULL;
}
info->m_attachMatrix_0 = dgGetIdentityMatrix();
info->m_attachMatrix_1 = dgGetIdentityMatrix();
info->m_discriptionType[0] = 0;
}
void dCustomTireSpringDG::TireMatrixProjection()
{
NewtonBody* tire;
NewtonBody* chassis;
//
dMatrix tireMatrix;
dMatrix chassisMatrix;
dMatrix tireMatrixInGlobalSpace;
dMatrix chassisMatrixInGlobalSpace;
dMatrix projectedChildMatrixInGlobalSpace;
//
dVector projectDist;
dVector projTireOmega;
dVector chassisOmega;
dVector tireOmega;
dVector tireOmegaCorrection;
//
// D.G: Thanks Julio for the helps in the past for this part of code.
// D.G: This code come from a pretty old vehicle implementation.
tire = GetBody1();
chassis = GetBody0();
//
NewtonBodyGetMatrix(tire, &tireMatrix[0][0]);
NewtonBodyGetMatrix(chassis, &chassisMatrix[0][0]);
//
// project the tire matrix to the right space.
tireMatrixInGlobalSpace = (m_localMatrix1 * tireMatrix);
chassisMatrixInGlobalSpace = (m_localMatrix0 * chassisMatrix);
//
projectDist = (tireMatrixInGlobalSpace.m_posit - chassisMatrixInGlobalSpace.m_posit).DotProduct3(chassisMatrixInGlobalSpace.m_up);
chassisMatrixInGlobalSpace.m_posit = chassisMatrixInGlobalSpace.m_posit + (chassisMatrixInGlobalSpace.m_up * projectDist);
//
chassisMatrixInGlobalSpace.m_up = tireMatrixInGlobalSpace.m_right.CrossProduct(chassisMatrixInGlobalSpace.m_front);
chassisMatrixInGlobalSpace.m_up = chassisMatrixInGlobalSpace.m_up * (1.0f / dSqrt(chassisMatrixInGlobalSpace.m_up.DotProduct3(chassisMatrixInGlobalSpace.m_up)));
chassisMatrixInGlobalSpace.m_right = chassisMatrixInGlobalSpace.m_front.CrossProduct(chassisMatrixInGlobalSpace.m_up);
chassisMatrixInGlobalSpace.m_up.m_w = 0.0;
chassisMatrixInGlobalSpace.m_right.m_w = 0.0;
//
projectedChildMatrixInGlobalSpace = (m_localMatrix1.Inverse() * chassisMatrixInGlobalSpace);
NewtonBodySetMatrix(tire, &projectedChildMatrixInGlobalSpace[0][0]);
//
NewtonBodyGetOmega(chassis, &chassisOmega[0]);
// D.G: Temporary disabled.
// D.G: The result is a lot better without this part.
// D.G: Causing problems, something is calculed wrong and the result give some bad bounce force on the wheels.
//
/*
NewtonBodyGetCentreOfMass(chassis, @chassisCom.V[0]);
NewtonBodyGetMatrix(chassis, @rlmat.V[0].V[0]);
chassisCom: = OXTransformVector(chassisCom, rlmat);
chassisVeloc: = VectorAdd(chassisVeloc, VectorCrossProduct(chassisOmega, VectorSubtract(chassisMatrixInGlobalSpace.m_posit, chassisCom)));
projTireVeloc: = VectorSubtract{ VectorAdd }(chassisVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(chassisVeloc, chassisMatrixInGlobalSpace.m_up)));
projTireVeloc: = VectorAdd{ VectorSubtract }(projTireVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(tireVeloc, chassisMatrixInGlobalSpace.m_up)));
//NegateVector(projTireVeloc);
NewtonBodySetVelocity(tire, @projTireVeloc.V[0]);
*/
// project angular velocity
NewtonBodyGetOmega(tire, &tireOmega[0]);
tireOmegaCorrection = tireOmega;
//
if (GetVecLength(tireOmegaCorrection) > mFpsRequest) {
// I need to use this omega correction fix when NewtonBodySetLinearDamping,NewtonBodySetAngularDamping is set to zero.
// Because the tire rotation overpass the physics rotation limit in time, and it can give bad result without this fix.
// I prefered to have the body totally free rolling, because of this I need this fix.
// If you don't use this fix, Don't set the NewtonBodySetLinearDamping,NewtonBodySetAngularDamping to zero value, just don't call this both functions on the body at all.
//
//printf("Omega tire correction, Fix a Rotation limitation. \n");
tireOmegaCorrection = (tireOmegaCorrection * mTireOmegaCorrection);
NewtonBodySetOmega(tire, &tireOmegaCorrection[0]);
}
//
projTireOmega = chassisOmega - chassisMatrixInGlobalSpace.m_front * (chassisOmega.DotProduct3(chassisMatrixInGlobalSpace.m_front));
projTireOmega = projTireOmega + chassisMatrixInGlobalSpace.m_front * (tireOmegaCorrection.DotProduct3(chassisMatrixInGlobalSpace.m_front));
NewtonBodySetOmega(tire, &projTireOmega[0]);
}
void dCustomKinematicController::ResetAutoSleep ()
{
NewtonBodySetAutoSleep(GetBody0(), 0);
}
dCustomKinematicController::dCustomKinematicController(NewtonInverseDynamics* const invDynSolver, void* const invDynNode, const dMatrix& handleInGlobalSpace)
:dCustomJoint(invDynSolver, invDynNode)
{
Init(GetBody0(), handleInGlobalSpace);
}
int CustomMultiBodyVehicle::AddSingleSuspensionTire (
void* userData,
const dVector& localPosition,
dFloat mass,
dFloat radius,
dFloat width,
dFloat suspensionLength,
dFloat springConst,
dFloat springDamper)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dMatrix carMatrix;
NewtonBody* tire;
NewtonWorld* world;
NewtonCollision *collision;
world = NewtonBodyGetWorld(GetBody0());
// create the tire RogidBody
collision = NewtonCreateChamferCylinder(world, radius, width, 0, NULL);
//create the rigid body
tire = NewtonCreateBody (world, collision);
// release the collision
NewtonReleaseCollision (world, collision);
// save the user data
NewtonBodySetUserData (tire, userData);
// set the material group id for vehicle
NewtonBodySetMaterialGroupID (tire, 0);
// NewtonBodySetMaterialGroupID (tire, woodID);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback (tire, NewtonBodyGetForceAndTorqueCallback (GetBody0()));
// body part do not collision
NewtonBodySetJointRecursiveCollision (tire, 0);
// calculate the moment of inertia and the relative center of mass of the solid
dVector origin;
dVector inertia;
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
Ixx = mass * inertia[0];
Iyy = mass * inertia[1];
Izz = mass * inertia[2];
// set the mass matrix
NewtonBodySetMassMatrix (tire, mass, Ixx, Iyy, Izz);
// calculate the tire local base pose matrix
dMatrix tireMatrix;
tireMatrix.m_front = m_localFrame.m_right;
tireMatrix.m_up = m_localFrame.m_up;
tireMatrix.m_right = tireMatrix.m_front * tireMatrix.m_up;
tireMatrix.m_posit = localPosition;
NewtonBodyGetMatrix(GetBody0(), &carMatrix[0][0]);
tireMatrix = tireMatrix * carMatrix;
// set the matrix for both the rigid body and the graphic body
NewtonBodySetMatrix (tire, &tireMatrix[0][0]);
// add a single tire
m_tires[m_tiresCount] = new CustomMultiBodyVehicleTire (GetBody0(), tire, suspensionLength, springConst, springDamper, radius);
m_tiresCount ++;
return m_tiresCount - 1;
}
void SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix tirePivotMatrix;
dMatrix chassisPivotMatrix;
ProjectTireMatrix();
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_chassisLocalMatrix, m_tireLocalMatrix, chassisPivotMatrix, tirePivotMatrix);
// Restrict the movement on the pivot point along all two orthonormal direction
dVector centerInTire (tirePivotMatrix.m_posit);
dVector centerInChassis (chassisPivotMatrix.m_posit + chassisPivotMatrix.m_up.Scale ((centerInTire - chassisPivotMatrix.m_posit) % chassisPivotMatrix.m_up));
NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInTire[0], &chassisPivotMatrix.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInTire[0], &chassisPivotMatrix.m_right[0]);
// get a point along the pin axis at some reasonable large distance from the pivot
dVector pointInPinInTire (centerInChassis + chassisPivotMatrix.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector pointInPinInChassis (centerInTire + tirePivotMatrix.m_front.Scale(MIN_JOINT_PIN_LENGTH));
NewtonUserJointAddLinearRow (m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &chassisPivotMatrix.m_right[0]);
NewtonUserJointAddLinearRow (m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &chassisPivotMatrix.m_up[0]);
//calculate the suspension spring and damper force
dFloat dist;
dFloat speed;
dFloat force;
dVector tireVeloc;
dVector chassisVeloc;
dVector chassisOmega;
dVector chassisCom;
dMatrix chassisMatrix;
const NewtonBody* tire;
const NewtonBody* chassis;
// calculate the velocity of tire attachments point on the car chassis
tire = GetBody1();
chassis = GetBody0();
NewtonBodyGetVelocity (tire, &tireVeloc[0]);
NewtonBodyGetVelocity (chassis, &chassisVeloc[0]);
NewtonBodyGetOmega (chassis, &chassisOmega[0]);
NewtonBodyGetMatrix (chassis, &chassisMatrix[0][0]);
NewtonBodyGetCentreOfMass (chassis, &chassisCom[0]);
chassisCom = chassisMatrix.TransformVector(chassisCom);
chassisVeloc += chassisOmega * (centerInChassis - chassisCom);
// get the spring damper parameters
speed = (chassisVeloc - tireVeloc) % chassisPivotMatrix.m_up;
dist = (chassisPivotMatrix.m_posit - tirePivotMatrix.m_posit) % chassisPivotMatrix.m_up;
// check if the suspension pass the bumpers limits
if (-dist > m_suspenstionSpan* 0.5f) {
// if it hit the bumpers then speed is zero
speed = 0;
NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInChassis[0], &chassisPivotMatrix.m_up[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
} else if (dist > 0.0f) {
// if it hit the bumpers then speed is zero
speed = 0;
NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInChassis[0], &chassisPivotMatrix.m_up[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
// calculate magnitude of suspension force
force = NewtonCalculateSpringDamperAcceleration (timestep, m_spring, dist, m_damper, speed) * m_effectiveSpringMass;
dVector chassisForce (chassisMatrix.m_up.Scale (force));
dVector chassisTorque ((centerInChassis - chassisCom) * chassisForce);
NewtonBodyAddForce (chassis, &chassisForce[0]);
NewtonBodyAddTorque (chassis, &chassisTorque[0]);
dVector tireForce (chassisForce.Scale (-1.0f));
NewtonBodyAddForce(tire, &tireForce[0]);
// apply the engine torque to tire torque
dFloat relOmega;
dMatrix tireMatrix;
dVector tireOmega;
NewtonBodyGetOmega(tire, &tireOmega[0]);
NewtonBodyGetMatrix (tire, &tireMatrix[0][0]);
relOmega = ((tireOmega - chassisOmega) % tireMatrix.m_front);
// apply engine torque plus some tire angular drag
dVector tireTorque (tireMatrix.m_front.Scale (m_enginetorque - relOmega * m_Ixx * m_angularDragCoef));
NewtonBodyAddTorque (tire, &tireTorque[0]);
dVector chassisReationTorque (chassisMatrix.m_right.Scale (- m_enginetorque));
NewtonBodyAddTorque(chassis, &chassisTorque[0]);
m_enginetorque = 0.0f;
// add the brake torque row
if (dAbs(m_brakeToque) > 1.0e-3f) {
relOmega /= timestep;
NewtonUserJointAddAngularRow (m_joint, 0.0f, &tireMatrix.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, relOmega);
NewtonUserJointSetRowMaximumFriction(m_joint, m_brakeToque);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_brakeToque);
}
m_brakeToque = 0.0f;
}
