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 RenderCenterOfMass (NewtonWorld* const world)
{
glDisable (GL_LIGHTING);
glDisable(GL_TEXTURE_2D);
glColor3f(0.0f, 0.0f, 1.0f);
glBegin(GL_LINES);
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
dMatrix matrix;
dVector com(0.0f);
NewtonBodyGetCentreOfMass (body, &com[0]);
NewtonBodyGetMatrix (body, &matrix[0][0]);
dVector o (matrix.TransformVector (com));
dVector x (o + matrix.RotateVector (dVector (1.0f, 0.0f, 0.0f, 0.0f)));
glColor3f (1.0f, 0.0f, 0.0f);
glVertex3f (o.m_x, o.m_y, o.m_z);
glVertex3f (x.m_x, x.m_y, x.m_z);
dVector y (o + matrix.RotateVector (dVector (0.0f, 1.0f, 0.0f, 0.0f)));
glColor3f (0.0f, 1.0f, 0.0f);
glVertex3f (o.m_x, o.m_y, o.m_z);
glVertex3f (y.m_x, y.m_y, y.m_z);
dVector z (o + matrix.RotateVector (dVector (0.0f, 0.0f, 1.0f, 0.0f)));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (o.m_x, o.m_y, o.m_z);
glVertex3f (z.m_x, z.m_y, z.m_z);
}
glEnd();
}
CustomMultiBodyVehicle::CustomMultiBodyVehicle(const dVector& frontDir, const dVector& upDir, const NewtonBody* carBody)
:NewtonCustomJoint(1, carBody, NULL)
{
dVector com;
dMatrix tmp;
dMatrix chassisMatrix;
m_tiresCount = 0;
m_diffencialCount = 0;
NewtonBodyGetMatrix(m_body0, &tmp[0][0]);
NewtonBodyGetCentreOfMass(m_body0, &com[0]);
com.m_w = 1.0f;
// set the joint reference point at the center of mass of the body
chassisMatrix.m_front = frontDir;
chassisMatrix.m_up = upDir;
chassisMatrix.m_right = frontDir * upDir;
chassisMatrix.m_posit = tmp.TransformVector(com);
chassisMatrix.m_front.m_w = 0.0f;
chassisMatrix.m_up.m_w = 0.0f;
chassisMatrix.m_right.m_w = 0.0f;
chassisMatrix.m_posit.m_w = 1.0f;
CalculateLocalMatrix (chassisMatrix, m_localFrame, tmp);
}
static void GetCollisionSubShape(const NewtonJoint* const contactJoint, NewtonBody* const body)
{
NewtonCollisionInfoRecord collisionInfo;
NewtonCollision* const collision = NewtonBodyGetCollision(body);
NewtonCollisionGetInfo (collision, &collisionInfo);
int count = 0;
NewtonCollision* collidingSubShapeArrar[32];
// see if this is a compound collision or any other collision with sub collision shapes
if (collisionInfo.m_collisionType == SERIALIZE_ID_COMPOUND) {
// to get the torque we need the center of gravity in global space
dVector origin;
dMatrix bodyMatrix;
NewtonBodyGetMatrix(body, &bodyMatrix[0][0]);
NewtonBodyGetCentreOfMass(body, &origin[0]);
origin = bodyMatrix.TransformVector(origin);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
// get the material of this contact,
// this part contain all contact information, the sub colliding shape,
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonCollision* const subShape = NewtonMaterialGetBodyCollidingShape (material, body);
int i = count - 1;
for (; i >= 0; i --) {
if (collidingSubShapeArrar[i] == subShape) {
break;
}
}
if (i < 0) {
collidingSubShapeArrar[count] = subShape;
count ++;
dAssert (count < int (sizeof (collidingSubShapeArrar) / sizeof (collidingSubShapeArrar[0])));
// you can also get the forces here, however when tho function is call form a contact material
// we can only get resting forces, impulsive forces can not be read here since they has no being calculated yet.
// whoever if this function function is call after the NetwonUpdate they the application can read the contact force, that was applied to each contact point
dVector force;
dVector posit;
dVector normal;
NewtonMaterialGetContactForce (material, body, &force[0]);
NewtonMaterialGetContactPositionAndNormal (material, body, &posit[0], &normal[0]);
// the torque on this contact is
dVector torque ((origin - posit) * force);
// do what ever you want wit this
}
}
}
// here we should have an array of all colling sub shapes
if (count) {
// do what you need with this sub shape list
}
}
PlaformEntityEntity (DemoEntityManager* const scene, DemoEntity* const source, NewtonBody* const triggerPort0, NewtonBody* const triggerPort1)
:DemoEntity (source->GetNextMatrix(), NULL)
{
scene->Append(this);
DemoMesh* const mesh = (DemoMesh*)source->GetMesh();
dAssert (mesh->IsType(DemoMesh::GetRttiType()));
SetMesh(mesh, source->GetMeshMatrix());
const dFloat mass = 100.0f;
dMatrix matrix (source->GetNextMatrix()) ;
NewtonWorld* const world = scene->GetNewton();
// note: because the mesh matrix can have scale, for simplicity just apply the local mesh matrix to the vertex cloud
dVector pool[128];
const dMatrix& meshMatrix = GetMeshMatrix();
meshMatrix.TransformTriplex(&pool[0].m_x, sizeof (dVector), mesh->m_vertex, 3 * sizeof (dFloat), mesh->m_vertexCount);
NewtonCollision* const collision = NewtonCreateConvexHull(world, mesh->m_vertexCount, &pool[0].m_x, sizeof (dVector), 0, 0, NULL);
NewtonBody* body = CreateSimpleBody (world, this, 100, matrix, collision, 0);
NewtonDestroyCollision(collision);
// attach a kinematic joint controller joint to move this body
dVector pivot;
NewtonBodyGetCentreOfMass (body, &pivot[0]);
pivot = matrix.TransformVector(pivot);
m_driver = new FerryDriver (body, pivot, triggerPort0, triggerPort1);
m_driver->SetMaxLinearFriction (50.0f * dAbs (mass * DEMO_GRAVITY));
m_driver->SetMaxAngularFriction(50.0f * dAbs (mass * DEMO_GRAVITY));
}
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 MSNewton::Slider::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
dMatrix matrix;
dVector centre;
NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
NewtonBodyGetCentreOfMass(joint_data->child, ¢re[0]);
pin_matrix.m_posit = matrix.TransformVector(centre);
}
void CustomDGRayCastCar::ApplyChassisTorque (const dVector& vForce, const dVector& vPoint)
{
dVector Torque;
dMatrix M;
dVector com;
NewtonBodyGetCentreOfMass( m_body0, &com[0] );
NewtonBodyGetMatrix( m_body0, &M[0][0] );
Torque = ( vPoint - M.TransformVector( dVector( com.m_x, com.m_y, com.m_z, 1.0f ) ) ) * vForce;
NewtonBodyAddTorque( m_body0, &Torque[0] );
}
void MSNewton::Servo::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
dMatrix matrix;
dVector centre;
NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
NewtonBodyGetCentreOfMass(joint_data->child, ¢re[0]);
centre = matrix.TransformVector(centre);
centre = pin_matrix.UntransformVector(centre);
dVector point(0.0f, 0.0f, centre.m_z);
pin_matrix.m_posit = pin_matrix.TransformVector(point);
}
dVector CalculateToqueAtPoint (const NewtonBody* body, const dVector& point, const dVector& force)
{
dVector com;
dMatrix matrix;
NewtonBodyGetMatrix (body, &matrix[0][0]);
NewtonBodyGetCentreOfMass (body, &matrix[0][0]);
com = matrix.TransformVector (com);
return (point - com) * force;
}
dMatrix Car::createChassisMatrix()
{
// set the vehicle local coordinate system
dMatrix chassisMatrix;
chassisMatrix.m_front = dVector(0.0f, 0.0f, 1.0f, 0.0);
chassisMatrix.m_up = dVector(0.0f, 1.0f, 0.0f, 0.0f);
chassisMatrix.m_right = chassisMatrix.m_up * chassisMatrix.m_front;
NewtonBodyGetCentreOfMass(this->getCarBody(), &chassisMatrix.m_posit[0]);
return chassisMatrix;
}
void SubmitConstraints (dFloat timestep, int threadIndex)
{
// calculate suspension bumpers and forces
dMatrix threadMatrix;
dMatrix parentMatrix;
dVector threadCOM;
dVector parentCOM;
dVector threadVeloc;
dVector parentVeloc;
dVector threadOmega;
dVector parentOmega;
// get the physics body state;
NewtonBodyGetOmega(m_body0, &threadOmega[0]);
NewtonBodyGetOmega(m_body1, &parentOmega[0]);
NewtonBodyGetVelocity(m_body0, &threadVeloc[0]);
NewtonBodyGetVelocity(m_body1, &parentVeloc[0]);
NewtonBodyGetCentreOfMass(m_body0, &threadCOM[0]);
NewtonBodyGetCentreOfMass(m_body1, &parentCOM[0]);
NewtonBodyGetMatrix(m_body0, &threadMatrix[0][0]);
NewtonBodyGetMatrix(m_body1, &parentMatrix[0][0]);
threadCOM = threadMatrix.TransformVector(threadCOM);
parentCOM = parentMatrix.TransformVector(parentCOM);
ApplySuspesionForce (timestep,
m_body0, m_rearHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
m_body1, m_rearHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);
ApplySuspesionForce (timestep,
m_body0, m_frontHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
m_body1, m_frontHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);
CustomSlidingContact::SubmitConstraints (timestep, threadIndex);
}
void dNewtonBody::SetCenterOfMass(float com_x, float com_y, float com_z)
{
dVector com;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
NewtonBodySetMassProperties(m_body, mass, NewtonBodyGetCollision(m_body));
NewtonBodyGetCentreOfMass (m_body, &com[0]);
com.m_x += com_x;
com.m_y += com_y;
com.m_z += com_z;
NewtonBodySetCentreOfMass(m_body, &com[0]);
}
NzVector3f NzPhysObject::GetMassCenter(nzCoordSys coordSys) const
{
NzVector3f center;
NewtonBodyGetCentreOfMass(m_body, center);
switch (coordSys)
{
case nzCoordSys_Global:
center = m_matrix.Transform(center);
break;
case nzCoordSys_Local:
break; // Aucune opération à effectuer sur le centre de rotation
}
return center;
}
void cPhysicsBodyNewton::SetMass(float a_fMass)
{
cCollideShapeNewton *pShapeNewton = static_cast<cCollideShapeNewton*>(m_pShape);
cVector3f vInertia;
cVector3f vOffset;
NewtonConvexCollisionCalculateInertialMatrix(pShapeNewton->GetNewtonCollision(),
vInertia.v, vOffset.v);
vInertia = vInertia * a_fMass;
NewtonBodyGetCentreOfMass(m_pNewtonBody, vOffset.v);
NewtonBodySetMassMatrix(m_pNewtonBody, a_fMass, vInertia.x, vInertia.y, vInertia.z);
m_FMass = a_fMass;
}
void dNewtonBody::CalculateBuoyancyForces(const void* plane, void* force, void* torque, float bodyDensity)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
if (mass > 0.0f) {
dMatrix matrix;
dVector cog(0.0f);
dVector accelPerUnitMass(0.0f);
dVector torquePerUnitMass(0.0f);
const dVector gravity(0.0f, -9.8f, 0.0f, 0.0f);
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
NewtonBodyGetCentreOfMass(m_body, &cog[0]);
cog = matrix.TransformVector(cog);
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
dFloat shapeVolume = NewtonConvexCollisionCalculateVolume(collision);
dFloat fluidDensity = 1.0f / (bodyDensity * shapeVolume);
dFloat viscosity = 0.995f;
NewtonConvexCollisionCalculateBuoyancyAcceleration(collision, &matrix[0][0], &cog[0], &gravity[0], (float*)plane, fluidDensity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);
dVector finalForce(accelPerUnitMass.Scale(mass));
dVector finalTorque(torquePerUnitMass.Scale(mass));
dVector omega(0.0f);
NewtonBodyGetOmega(m_body, &omega[0]);
omega = omega.Scale(viscosity);
NewtonBodySetOmega(m_body, &omega[0]);
((float*)force)[0] = finalForce.m_x ;
((float*)force)[1] = finalForce.m_y ;
((float*)force)[2] = finalForce.m_z ;
((float*)torque)[0] = finalTorque.m_x;
((float*)torque)[1] = finalTorque.m_y;
((float*)torque)[2] = finalTorque.m_z;
}
}
void MSNewton::Fixed::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
dMatrix matrix;
dVector ccentre;
NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
NewtonBodyGetCentreOfMass(joint_data->child, &ccentre[0]);
ccentre = matrix.TransformVector(ccentre);
if (joint_data->parent != nullptr) {
/*NewtonBodyGetMatrix(joint_data->parent, &matrix[0][0]);
dVector pcentre;
NewtonBodyGetCentreOfMass(joint_data->parent, &pcentre[0]);
pcentre = matrix.TransformVector(pcentre);
pin_matrix.m_posit.m_x = (ccentre.m_x + pcentre.m_x) * 0.5f;
pin_matrix.m_posit.m_y = (ccentre.m_y + pcentre.m_y) * 0.5f;
pin_matrix.m_posit.m_z = (ccentre.m_z + pcentre.m_z) * 0.5f;*/
}
else {
pin_matrix.m_posit = ccentre;
}
}
static void PhysicsApplyPickForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dVector com;
dVector veloc;
dVector omega;
dMatrix matrix;
// apply the thew body forces
if (chainForceCallback) {
chainForceCallback (body, timestep, threadIndex);
}
// add the mouse pick penalty force and torque
NewtonBodyGetVelocity(body, &veloc[0]);
NewtonBodyGetOmega(body, &omega[0]);
NewtonBodyGetVelocity(body, &veloc[0]);
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector force (pickedForce.Scale (mass * MOUSE_PICK_STIFFNESS));
dVector dampForce (veloc.Scale (MOUSE_PICK_DAMP * mass));
force -= dampForce;
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetCentreOfMass (body, &com[0]);
// calculate local point relative to center of mass
dVector point (matrix.RotateVector (attachmentPoint - com));
dVector torque (point * force);
dVector torqueDamp (omega.Scale (mass * 0.1f));
NewtonBodyAddForce (body, &force.m_x);
NewtonBodyAddTorque (body, &torque.m_x);
// make sure the body is unfrozen, if it is picked
NewtonBodySetFreezeState (body, 0);
}
void MSNewton::CurvySlider::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
CurvySliderData* cj_data = (CurvySliderData*)joint_data->cj_data;
// Recalculate pin matrix so its on curve rather than on the joint origin.
dMatrix matrix;
dVector centre, point, vector, min_pt, max_pt;
dFloat distance, min_len, max_len;
NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
NewtonBodyGetCentreOfMass(joint_data->child, ¢re[0]);
centre = matrix.TransformVector(centre);
MSNewton::Joint::c_get_pin_matrix(joint_data, matrix);
centre = matrix.UntransformVector(centre);
bool success = c_calc_curve_data_at_location(cj_data, centre, point, vector, distance, min_pt, max_pt, min_len, max_len);
if (success) {
point = matrix.TransformVector(point);
vector = matrix.RotateVector(vector);
Util::matrix_from_pin_dir(point, vector, pin_matrix);
cj_data->cur_pos = distance;
cj_data->last_dist = distance;
cj_data->last_dir = vector;
}
}
dVehicleSingleBody::dVehicleSingleBody(dVehicleChassis* const chassis)
:dVehicleInterface(chassis)
,m_gravity(0.0f)
,m_groundNode(NULL)
,m_newtonBody(chassis->GetBody())
{
dVector tmp;
dComplementaritySolver::dBodyState* const chassisBody = GetBody();
m_groundNode.SetWorld(m_world);
m_groundNode.SetLoopNode(true);
// set the inertia matrix;
NewtonBodyGetMass(m_newtonBody, &tmp.m_w, &tmp.m_x, &tmp.m_y, &tmp.m_z);
chassisBody->SetMass(tmp.m_w);
chassisBody->SetInertia(tmp.m_x, tmp.m_y, tmp.m_z);
dMatrix matrix (dGetIdentityMatrix());
NewtonBodyGetCentreOfMass(m_newtonBody, &matrix.m_posit[0]);
matrix.m_posit.m_w = 1.0f;
chassisBody->SetLocalMatrix(matrix);
}
void OnInside(NewtonBody* const visitor)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(visitor, &mass, &Ixx, &Iyy, &Izz);
if (mass > 0.0f) {
dMatrix matrix;
dVector cog(0.0f);
dVector accelPerUnitMass(0.0f);
dVector torquePerUnitMass(0.0f);
const dVector gravity (0.0f, DEMO_GRAVITY, 0.0f, 0.0f);
NewtonBodyGetMatrix (visitor, &matrix[0][0]);
NewtonBodyGetCentreOfMass(visitor, &cog[0]);
cog = matrix.TransformVector (cog);
NewtonCollision* const collision = NewtonBodyGetCollision(visitor);
dFloat shapeVolume = NewtonConvexCollisionCalculateVolume (collision);
dFloat fluidDentity = 1.0f / (m_waterToSolidVolumeRatio * shapeVolume);
dFloat viscosity = 0.995f;
NewtonConvexCollisionCalculateBuoyancyAcceleration (collision, &matrix[0][0], &cog[0], &gravity[0], &m_plane[0], fluidDentity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);
dVector force (accelPerUnitMass.Scale (mass));
dVector torque (torquePerUnitMass.Scale (mass));
dVector omega(0.0f);
NewtonBodyGetOmega(visitor, &omega[0]);
omega = omega.Scale (viscosity);
NewtonBodySetOmega(visitor, &omega[0]);
NewtonBodyAddForce (visitor, &force[0]);
NewtonBodyAddTorque (visitor, &torque[0]);
}
}
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 CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
int constraintIndex;
dFloat invTimestep;
dMatrix bodyMatrix;
// get the simulation time
invTimestep = 1.0f / timestep ;
// get the vehicle global matrix, and use it in several calculations
NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
dMatrix chassisMatrix (m_localFrame * bodyMatrix);
// get the chassis instantaneous linear and angular velocity in the local space of the chassis
dVector bodyOmega;
dVector bodyVelocity;
NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
NewtonBodyGetOmega (m_body0, &bodyOmega[0]);
// all tire is on air check
m_vehicleOnAir = 0;
constraintIndex = 0;
for ( int i = 0; i < m_tiresCount; i ++ ) {
Tire& tire = m_tires[i];
tire.m_tireIsOnAir = 1;
tire.m_tireIsConstrained = 0;
tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);
// calculate all suspension matrices in global space and tire collision
dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);
// calculate the tire collision
CalculateTireCollision (tire, suspensionMatrix, threadIndex);
// calculate the linear velocity of the tire at the ground contact
tire.m_tireAxelPosit = chassisMatrix.TransformVector( tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit));
tire.m_tireAxelVeloc = bodyVelocity + bodyOmega * (tire.m_tireAxelPosit - chassisMatrix.m_posit);
tire.m_lateralPin = ( chassisMatrix.RotateVector ( tire.m_localAxis ) );
tire.m_longitudinalPin = ( chassisMatrix.m_up * tire.m_lateralPin );
if (tire.m_posit < tire.m_suspensionLenght ) {
dFloat distance;
dFloat sideSlipCoef;
dFloat slipRatioCoef;
dFloat tireForceMag;
dFloat tireTorqueMag;
dFloat suspensionSpeed;
dFloat axelLinealSpeed;
dFloat tireRotationSpeed;
dFloat frictionCircleMag;
dFloat longitudinalForceMag;
dFloat lateralFrictionForceMag;
tire.m_tireIsOnAir = 0;
tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
if (tire.m_HitBody){
dMatrix matrix;
dVector com;
dVector omega;
NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
}
// calculate the relative velocity
dVector relVeloc (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
suspensionSpeed = - (relVeloc % chassisMatrix.m_up);
// now calculate the tire load at the contact point
// Tire suspension distance and hard limit.
distance = tire.m_suspensionLenght - tire.m_posit;
_ASSERTE (distance <= tire.m_suspensionLenght);
tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
if ( tire.m_tireLoad < 0.0f ) {
// since the tire is not a body with real mass it can only push the chassis.
tire.m_tireLoad = 0.0f;
}
//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
tire.m_tireLoad *= (m_mass * 0.5f);
tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);
// convert the tire load force magnitude to a torque and force.
// accumulate the force doe to the suspension spring and damper
tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);
// calculate relative velocity at the tire center
dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
// axle linear speed
axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;
// calculate tire rotation velocity at the tire radio
dVector tireAngularVelocity (tire.m_lateralPin.Scale (tire.m_angularVelocity));
dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);
longitudinalForceMag = 0.0f;
// if (!tire.m_tireIsConstrained) {
// calculate slip ratio and max longitudinal force
tireRotationSpeed = tireRotationalVelocityAtContact % tire.m_longitudinalPin;
slipRatioCoef = (dAbs (axelLinealSpeed) > 1.e-3f) ? ((-tireRotationSpeed - axelLinealSpeed) / dAbs (axelLinealSpeed)) : 0.0f;
// calculate the formal longitudinal force the tire apply to the chassis
longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;
// }
// now calculate relative velocity a velocity at contact point
dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact);
// calculate the sideslip as the angle between the tire lateral speed and longitudila speed
sideSlipCoef = dAtan2 (dAbs (tireContactRelativeVelocity % tire.m_lateralPin), dAbs (axelLinealSpeed));
lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;
// Apply brake, need some little fix here.
// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
if ( tire.m_breakForce > 1.0e-3f ) {
// row constrained force is save for later determine the dynamic state of this tire
tire.m_isBrakingForceIndex = constraintIndex;
constraintIndex ++;
frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
if (tire.m_breakForce > frictionCircleMag) {
tire.m_breakForce = frictionCircleMag;
}
//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x );
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);
// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
}
}
//project the longitudinal and lateral forces over the circle of friction for this tire;
frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
dFloat invMag2;
invMag2 = frictionCircleMag / dSqrt (tireForceMag);
longitudinalForceMag *= invMag2;
lateralFrictionForceMag *= invMag2;
}
// submit this constraint for calculation of side slip forces
lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
tire.m_lateralForceIndex = constraintIndex;
constraintIndex ++;
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_lateralPin[0]);
NewtonUserJointSetRowMaximumFriction (m_joint, lateralFrictionForceMag);
NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
// accumulate the longitudinal force
dVector tireForce (tire.m_longitudinalPin.Scale (longitudinalForceMag));
tire.m_tireForceAcc += tireForce;
// now we apply the combined tire force generated by this tire, to the car chassis
dVector torque ((tire.m_tireAxelPosit - chassisMatrix.m_posit) * tire.m_tireForceAcc);
NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
NewtonBodyAddTorque( m_body0, &torque[0] );
// calculate the net torque on the tire
tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPin);
if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
// the tire reaction force can no be larger than the applied engine torque
// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
tire.m_tireIsConstrained = 1;
tireTorqueMag = tire.m_torque;
}
tire.m_torque -= tireTorqueMag;
}
}
}
void CustomVehicleController::Init (NewtonCollision* const chassisShape, const dMatrix& vehicleFrame, dFloat mass, const dVector& gravityVector)
{
m_finalized = false;
m_externalContactStatesCount = 0;
m_sleepCounter = VEHICLE_SLEEP_COUNTER;
m_freeContactList = m_externalContactStatesPoll.GetFirst();
CustomVehicleControllerManager* const manager = (CustomVehicleControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
// create a compound collision
NewtonCollision* const vehShape = NewtonCreateCompoundCollision(world, 0);
NewtonCompoundCollisionBeginAddRemove(vehShape);
// if the shape is a compound collision ass all the pieces one at a time
int shapeType = NewtonCollisionGetType (chassisShape);
if (shapeType == SERIALIZE_ID_COMPOUND) {
for (void* node = NewtonCompoundCollisionGetFirstNode(chassisShape); node; node = NewtonCompoundCollisionGetNextNode (chassisShape, node)) {
NewtonCollision* const subCollision = NewtonCompoundCollisionGetCollisionFromNode(chassisShape, node);
NewtonCompoundCollisionAddSubCollision (vehShape, subCollision);
}
} else {
dAssert ((shapeType == SERIALIZE_ID_CONVEXHULL) || (shapeType == SERIALIZE_ID_BOX));
NewtonCompoundCollisionAddSubCollision (vehShape, chassisShape);
}
NewtonCompoundCollisionEndAddRemove (vehShape);
// create the rigid body for this vehicle
dMatrix locationMatrix (dGetIdentityMatrix());
m_body = NewtonCreateDynamicBody(world, vehShape, &locationMatrix[0][0]);
// set vehicle mass, inertia and center of mass
NewtonBodySetMassProperties (m_body, mass, vehShape);
// set linear and angular drag to zero
dVector drag(0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetLinearDamping(m_body, 0);
NewtonBodySetAngularDamping(m_body, &drag[0]);
// destroy the collision help shape
NewtonDestroyCollision (vehShape);
// initialize vehicle internal components
NewtonBodyGetCentreOfMass (m_body, &m_chassisState.m_com[0]);
m_chassisState.m_comOffset = dVector (0.0f, 0.0f, 0.0f, 0.0f);
m_chassisState.m_gravity = gravityVector;
m_chassisState.m_gravityMag = dSqrt (gravityVector % gravityVector);
m_chassisState.Init(this, vehicleFrame);
// m_stateList.Append(&m_staticWorld);
m_stateList.Append(&m_chassisState);
// create the normalized size tire shape
m_tireCastShape = NewtonCreateChamferCylinder(world, 0.5f, 1.0f, 0, NULL);
// initialize all components to empty
m_engine = NULL;
m_brakes = NULL;
m_steering = NULL;
m_handBrakes = NULL;
SetDryRollingFrictionTorque (100.0f/4.0f);
SetAerodynamicsDownforceCoefficient (0.5f * dSqrt (gravityVector % gravityVector), 60.0f * 0.447f);
}
CustomDGRayCastCar::CustomDGRayCastCar (int maxTireCount, const dMatrix& cordenateSytem, NewtonBody* carBody)
:NewtonCustomJoint(2 * maxTireCount, carBody, NULL),
m_normalizedLateralForce(),
m_normalizedLongitudinalForce ()
{
dVector com;
dMatrix tmp;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dMatrix chassisMatrix;
NewtonBodyGetMassMatrix( m_body0, &m_mass, &Ixx, &Iyy, &Izz );
m_curSpeed = 0.0f;
m_tiresCount = 0;
m_vehicleOnAir = 0;
m_steerAngle = 0.0f;
//set default break force as a function of the vehicle weight
//2 time the car weight assuming gravity is 10
m_maxBrakeForce = 2.0f * m_mass * 10.0f;
m_maxSteerAngle = 30.0f * DefPO;
m_maxSteerRate = 0.075f;
m_engineSteerDiv = 100.0f;
m_maxSteerForce = 6000.0f;
m_maxSteerForceRate = 0.03f;
m_maxSteerSpeedRestriction = 2.0f;
// m_engineTireTorque = 0.0f;
// m_maxEngineTorque = 6000.0f;
// m_maxEngineTorqueRate = 500.0f;
/*
m_fixDeceleration = 0.9975f;
m_engineTireTorque = 0.0f;
m_maxBrakeForce = 350.0f;
m_tiresRollSide = 0;
m_engineTorqueDiv = 200.0f;
// chassis rotation fix...
m_chassisRotationLimit = 0.98f;
*/
// set the chassis matrix at the center of mass
NewtonBodyGetCentreOfMass( m_body0, &com[0] );
com.m_w = 1.0f;
// set the joint reference point at the center of mass of the body
NewtonBodyGetMatrix (m_body0, &chassisMatrix[0][0]);
//make sure the system matrix do not have any translations on it
dMatrix cordenateSytemLocal (cordenateSytem);
cordenateSytemLocal.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
chassisMatrix.m_posit += chassisMatrix.RotateVector (com);
chassisMatrix = cordenateSytemLocal * chassisMatrix;
// set the car local coordinate system
CalculateLocalMatrix ( chassisMatrix, m_localFrame, tmp );
// allocate space for the tires;
m_tires = new Tire[maxTireCount];
// Create a simplified normalized Tire Curve
// we will use a simple piece wise curve at this time,
// but end application user can use advance cubers like the Pacejkas tire model
dFloat slips[] = {0.0f, 0.3f, 0.5f, 2.0f};
dFloat normalizedLongitudinalForce[] = {0.0f, 1.0f, 0.9f, 0.7f};
m_normalizedLongitudinalForce.InitalizeCurve (sizeof (slips) / sizeof (dFloat), slips, normalizedLongitudinalForce);
dFloat sideSlip[] = {0.1f, 0.4f, 0.5f, 2.0f};
dFloat normalizedLateralForce[] = {0.0f, 1.0f, 0.6f, 0.4f};
m_normalizedLateralForce.InitalizeCurve (sizeof (sideSlip) / sizeof (dFloat), sideSlip, normalizedLateralForce);
// m_aerodynamicDrag = 0.1f;
// m_aerodynamicDownForce = 0.1f;
// set linear and angular Drag to zero, this joint will handle this by using Aerodynamic Drag;
dVector drag (0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetLinearDamping (m_body0, 0.0f);
NewtonBodySetAngularDamping (m_body0, &drag[0]);
// register the callback for tire integration
NewtonUserJointSetFeedbackCollectorCallback (m_joint, IntegrateTires);
}
void SimulationLister(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep)
{
m_delay --;
if (m_delay > 0) {
return;
}
// see if the net force on the body comes fr a high impact collision
dFloat maxInternalForce = 0.0f;
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(m_myBody); joint; joint = NewtonBodyGetNextContactJoint(m_myBody, joint)) {
for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) {
//dVector point;
//dVector normal;
dVector contactForce;
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
//NewtonMaterialGetContactPositionAndNormal (material, &point.m_x, &normal.m_x);
NewtonMaterialGetContactForce(material, m_myBody, &contactForce[0]);
dFloat forceMag = contactForce % contactForce;
if (forceMag > maxInternalForce) {
maxInternalForce = forceMag;
}
}
}
// if the force is bigger than 4 Gravities, It is considered a collision force
dFloat maxForce = BREAK_FORCE_IN_GRAVITIES * m_myweight;
if (maxInternalForce > (maxForce * maxForce)) {
NewtonWorld* const world = NewtonBodyGetWorld(m_myBody);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix(m_myBody, &mass, &Ixx, &Iyy, &Izz);
dVector com;
dVector veloc;
dVector omega;
dMatrix bodyMatrix;
NewtonBodyGetVelocity(m_myBody, &veloc[0]);
NewtonBodyGetOmega(m_myBody, &omega[0]);
NewtonBodyGetCentreOfMass(m_myBody, &com[0]);
NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]);
com = bodyMatrix.TransformVector (com);
dMatrix matrix (GetCurrentMatrix());
dQuaternion rotation (matrix);
for (ShatterEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) {
ShatterAtom& atom = node->GetInfo();
DemoEntity* const entity = new DemoEntity (NULL);
entity->SetMesh (atom.m_mesh);
entity->SetMatrix(*scene, rotation, matrix.m_posit);
entity->InterpolateMatrix (*scene, 1.0f);
scene->Append(entity);
int materialId = 0;
dFloat debriMass = mass * atom.m_massFraction;
dFloat Ixx = debriMass * atom.m_momentOfInirtia.m_x;
dFloat Iyy = debriMass * atom.m_momentOfInirtia.m_y;
dFloat Izz = debriMass * atom.m_momentOfInirtia.m_z;
//create the rigid body
NewtonBody* const rigidBody = NewtonCreateBody (world, atom.m_collision, &matrix[0][0]);
// set the correct center of gravity for this body
NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]);
// calculate the center of mas of the debris
dVector center (matrix.TransformVector(atom.m_centerOfMass));
// calculate debris initial velocity
dVector v (veloc + omega * (center - com));
// set initial velocity
NewtonBodySetVelocity(rigidBody, &v[0]);
NewtonBodySetOmega(rigidBody, &omega[0]);
// set the debrie center of mass
NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]);
// set the mass matrix
NewtonBodySetMassMatrix (rigidBody, debriMass, Ixx, Iyy, Izz);
// activate
// NewtonBodyCoriolisForcesMode (blockBoxBody, 1);
// save the pointer to the graphic object with the body.
NewtonBodySetUserData (rigidBody, entity);
// assign the wood id
NewtonBodySetMaterialGroupID (rigidBody, materialId);
// set continue collision mode
// NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode);
// set a destructor for this rigid body
NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor);
// set the transform call back function
NewtonBodySetTransformCallback (rigidBody, DemoEntity::SetTransformCallback);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce);
}
NewtonDestroyBody(world, m_myBody);
scene->RemoveEntity (mynode);
}
};
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);
}
}
}
cVector3f cPhysicsBodyNewton::GetMassCentre() const
{
cVector3f vCentre;
NewtonBodyGetCentreOfMass(m_pNewtonBody, vCentre.v);
return vCentre;
}
void* dNewtonBody::GetCenterOfMass()
{
NewtonBodyGetCentreOfMass(m_body, &m_com.m_x);
return &m_com;
}
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
// get the simulation time
// dFloat invTimestep = 1.0f / timestep ;
// get the vehicle global matrix, and use it in several calculations
dMatrix bodyMatrix;
NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
dMatrix chassisMatrix (m_localFrame * bodyMatrix);
// get the chassis instantaneous linear and angular velocity in the local space of the chassis
dVector bodyForce;
dVector bodyOmega;
dVector bodyVelocity;
NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
NewtonBodyGetOmega (m_body0, &bodyOmega[0]);
//static int xxx;
//dTrace (("frame %d veloc(%f %f %f)\n", xxx, bodyVelocity[0], bodyVelocity[1], bodyVelocity[2]));
//xxx ++;
//if (xxx >= 210) {
//xxx *=1;
//bodyVelocity.m_x = 0;
//bodyVelocity.m_z = 10;
//NewtonBodySetVelocity (m_body0, &bodyVelocity[0]);
//}
// dVector normalForces (0.0f, 0.0f, 0.0f, 0.0f);
// all tire is on air check
m_vehicleOnAir = 0;
// int constraintIndex = 0;
for (int i = 0; i < m_tiresCount; i ++) {
// dTrace (("tire: %d ", i));
Tire& tire = m_tires[i];
tire.m_tireIsOnAir = 1;
// tire.m_tireIsConstrained = 0;
tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);
// calculate all suspension matrices in global space and tire collision
dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);
// calculate the tire collision
CalculateTireCollision (tire, suspensionMatrix, threadIndex);
// calculate the linear velocity of the tire at the ground contact
tire.m_tireAxelPositGlobal = chassisMatrix.TransformVector (tire.m_harpointInJointSpace - m_localFrame.m_up.Scale (tire.m_posit));
tire.m_tireAxelVelocGlobal = bodyVelocity + bodyOmega * (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit);
tire.m_lateralPinGlobal = chassisMatrix.RotateVector (tire.m_localAxisInJointSpace);
tire.m_longitudinalPinGlobal = chassisMatrix.m_up * tire.m_lateralPinGlobal;
if (tire.m_posit < tire.m_suspensionLenght ) {
tire.m_tireIsOnAir = 0;
tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
if (tire.m_HitBody){
dMatrix matrix;
dVector com;
dVector omega;
NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
}
// calculate the relative velocity
dVector tireHubVeloc (tire.m_tireAxelVelocGlobal - tire.m_hitBodyPointVelocity);
dFloat suspensionSpeed = - (tireHubVeloc % chassisMatrix.m_up);
// now calculate the tire load at the contact point
// Tire suspension distance and hard limit.
dFloat distance = tire.m_suspensionLenght - tire.m_posit;
_ASSERTE (distance <= tire.m_suspensionLenght);
tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
if ( tire.m_tireLoad < 0.0f ) {
// since the tire is not a body with real mass it can only push the chassis.
tire.m_tireLoad = 0.0f;
}
//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
tire.m_tireLoad *= (m_mass * 0.5f);
// dTrace (("(load = %f) ", tire.m_tireLoad));
//tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);
// convert the tire load force magnitude to a torque and force.
// accumulate the force doe to the suspension spring and damper
tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);
// calculate relative velocity at the tire center
//dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
// axle linear speed
//axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;
dFloat axelLinearSpeed = tireHubVeloc % chassisMatrix.m_front;
// calculate tire rotation velocity at the tire radio
//dVector tireAngularVelocity (tire.m_lateralPinGlobal.Scale (tire.m_angularVelocity));
//dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPositGlobal);
//dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);
// calculate slip ratio and max longitudinal force
//dFloat tireRotationSpeed = -(tireRotationalVelocityAtContact % tire.m_longitudinalPinGlobal);
//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
//dTrace (("(slipRatio = %f) ", slipRatioCoef));
// calculate the formal longitudinal force the tire apply to the chassis
//dFloat longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;
dFloat longitudinalForceMag = CalculateLongitudinalForce (i, axelLinearSpeed, tire.m_tireLoad * tire.m_groundFriction);
// dTrace (("(longForce = %f) ", longitudinalForceMag));
#if 0
// now calculate relative velocity a velocity at contact point
//dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact);
//dVector tireContactAbsoluteVelocity (tireHubVeloc + tireRotationalVelocityAtContact);
// calculate the side slip as the angle between the tire lateral speed and longitudinal speed
//dFloat lateralSpeed = tireContactRelativeVelocity % tire.m_lateralPin;
dFloat lateralSpeed = tireHubVeloc % tire.m_lateralPinGlobal;
dFloat sideSlipCoef = dAtan2 (dAbs (lateralSpeed), dAbs (axelLinearSpeed));
dFloat lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;
// Apply brake, need some little fix here.
// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
if ( tire.m_breakForce > 1.0e-3f ) {
_ASSERTE (0);
/*
// row constrained force is save for later determine the dynamic state of this tire
tire.m_isBrakingForceIndex = constraintIndex;
constraintIndex ++;
frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
if (tire.m_breakForce > frictionCircleMag) {
tire.m_breakForce = frictionCircleMag;
}
//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x );
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);
// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
}
*/
}
//project the longitudinal and lateral forces over the circle of friction for this tire;
dFloat frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
dFloat tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
dFloat invMag2;
invMag2 = frictionCircleMag / dSqrt (tireForceMag);
longitudinalForceMag *= invMag2;
lateralFrictionForceMag *= invMag2;
}
// submit this constraint for calculation of side slip forces
lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
tire.m_lateralForceIndex = constraintIndex;
constraintIndex ++;
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPositGlobal[0], &tire.m_tireAxelPositGlobal[0], &tire.m_lateralPinGlobal[0]);
NewtonUserJointSetRowMaximumFriction (m_joint, lateralFrictionForceMag);
NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
#endif
// accumulate the longitudinal force
dVector tireForce (tire.m_longitudinalPinGlobal.Scale (longitudinalForceMag));
tire.m_tireForceAcc += tireForce;
// now we apply the combined tire force generated by this tire, to the car chassis
dVector r (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit);
// add the toque the tire asserts on the car body (principle of action reaction)
dVector torque (r * tire.m_tireForceAcc - tire.m_lateralPinGlobal.Scale (tire.m_torque));
NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
NewtonBodyAddTorque( m_body0, &torque[0] );
/*
// calculate the net torque on the tire
dFloat tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPinGlobal);
if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
// the tire reaction force cannot be larger than the applied engine torque
// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
tire.m_tireIsConstrained = 1;
tireTorqueMag = tire.m_torque;
}
tire.m_torque -= tireTorqueMag;
*/
// normalForces += tire.m_tireForceAcc;
} else {
// there is a next torque on the tire
tire.m_torque -= tire.m_angularVelocity * tire.m_Ixx * DG_TIRE_VISCUOS_DAMP;
tire.m_angularVelocity += tire.m_torque * tire.m_IxxInv * timestep;
if (m_tires[i].m_breakForce > dFloat (0.1f)) {
tire.m_angularVelocity = 0.0f;
}
}
// dTrace (("(tireTorque = %f) ", tire.m_torque));
// spin the tire by the angular velocity
tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);
// reset the tire torque
tire.m_torque = 0.0f;
tire.m_breakForce = 0.0f;
// dTrace (("\n"));
}
// add a row to simulate the engine rolling resistance
// float bodyWeight = dAbs (normalForces % chassisMatrix.m_up) * m_rollingResistance;
// if (bodyWeight > (1.0e-3f) * m_mass) {
// NewtonUserJointAddLinearRow (m_joint, &chassisMatrix.m_posit[0], &chassisMatrix.m_posit[0], &chassisMatrix.m_front[0]);
// NewtonUserJointSetRowMaximumFriction( m_joint, bodyWeight);
// NewtonUserJointSetRowMinimumFriction( m_joint, -bodyWeight);
// }
}
