// the call back is call before any of the constraint rows and collumns to inforce the joint are summited.
static void EvaluatePath (const NewtonUserJoint* me, dFloat timestep, int threadIndex)
{
FollowPath* path;
path = (FollowPath*) CustomGetUserData(me);
path->m_angle = dMod (path->m_angle + (45.0f * 3.1416f / 180.0f) * timestep, 2.0f * 3.1416f);
dMatrix matrix (path->BuildMatrix (timestep));
CustomKinematicControllerSetTargetMatrix (me, &matrix[0][0]);
}
void CustomDGRayCastCar::ApplyTiresTorqueVisual (Tire& tire, dFloat timestep, int threadIndex)
{
dFloat timestepInv;
// get the simulation time
timestepInv = 1.0f / timestep;
dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
dFloat tireLinearSpeed;
dFloat tireContactSpeed;
tireLinearSpeed = tire.m_tireAxelVeloc % tire.m_longitudinalPin;
tireContactSpeed = (tire.m_lateralPin * tireRadius) % tire.m_longitudinalPin;
//check if any engine torque or brake torque is applied to the tire
if (dAbs(tire.m_torque) < 1.0e-3f){
//tire is coasting, calculate the tire zero slip angular velocity
// this is the velocity that satisfy the constraint equation
// V % dir + W * R % dir = 0
// where V is the tire Axel velocity
// W is the tire local angular velocity
// R is the tire radius
// dir is the longitudinal direction of of the tire.
// this checkup is suposed to fix a infinit division by zero...
if ( dAbs(tireContactSpeed) > 1.0e-3) {
tire.m_angularVelocity = - (tireLinearSpeed) / (tireContactSpeed);
}
tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);
} else {
// tire is under some power, need to do the free body integration to apply the net torque
dFloat nettorque = tire.m_angularVelocity;
// this checkup is suposed to fix a infinit division by zero...
if ( dAbs(tireContactSpeed) > 1.0e-3) {
nettorque = - (tireLinearSpeed) / (tireContactSpeed);
}
//tire.m_angularVelocity = - tireLinearSpeed / tireContactSpeed;
dFloat torque;
torque = tire.m_torque - nettorque - tire.m_angularVelocity * tire.m_Ixx * 0.1f;
tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);
}
}
void PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrixA;
NewtonBodyGetMatrix(m_body, &matrixA[0][0]);
dFloat speed = m_step * timestep * 60.0f;
m_pith = dMod (m_pith + speed, 3.1416f * 2.0f);
m_yaw = dMod (m_yaw + speed, 3.1416f * 2.0f);
m_roll = dMod (m_roll + speed, 3.1416f * 2.0f);
dMatrix matrixB(dPitchMatrix(m_pith) * dYawMatrix(m_yaw) * dRollMatrix(m_roll));
matrixB.m_posit = matrixA.m_posit;
matrixB.m_posit.m_y = 5.0f;
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData (world);
m_castingVisualEntity->ResetMatrix(*scene, matrixB);
NewtonCollision* const collisionA = NewtonBodyGetCollision(m_body);
NewtonCollisionClosestPoint(world, collisionA, &matrixA[0][0], m_castingVisualEntity->m_castingShape, &matrixB[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_contact1[0], &m_castingVisualEntity->m_normal[0], 0);
}
virtual void Evaluate(dEffectorPose& output, dFloat timestep)
{
dEffectorTreeFixPose::Evaluate(output, timestep);
dFloat param = m_acc / m_period;
dBigVector left (m_cycle.CurvePoint(param));
dBigVector right (m_cycle.CurvePoint(dMod (param + 0.5f, 1.0f)));
dFloat high[2];
dFloat stride[2];
high[0] = dFloat (left.m_y);
high[1] = dFloat (right.m_y);
stride[0] = dFloat(left.m_x);
stride[1] = dFloat(right.m_x);
int index = 0;
for (dEffectorPose::dListNode* node = output.GetFirst(); node; node = node->GetNext()) {
dEffectorTransform& transform = node->GetInfo();
transform.m_posit.m_y += high[m_sequence[index]];
transform.m_posit.m_x += stride[m_sequence[index]];
index ++;
}
m_acc = dMod(m_acc + timestep, m_period);
}
void PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrixA;
NewtonBodyGetMatrix(m_body, &matrixA[0][0]);
dFloat speed = m_step * timestep * 60.0f;
m_pith = dMod (m_pith + speed, dPi * 2.0f);
m_yaw = dMod (m_yaw + speed, dPi * 2.0f);
m_roll = dMod (m_roll + speed, dPi * 2.0f);
dMatrix matrixB(dPitchMatrix(m_pith) * dYawMatrix(m_yaw) * dRollMatrix(m_roll));
matrixB.m_posit = matrixA.m_posit;
matrixB.m_posit.m_y = 5.0f;
//matrixB.m_posit.m_y = 1.5f;
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData (world);
m_castingVisualEntity->ResetMatrix(*scene, matrixB);
NewtonCollision* const collisionA = NewtonBodyGetCollision(m_body);
int res = NewtonCollisionClosestPoint(world, collisionA, &matrixA[0][0], m_castingVisualEntity->m_castingShape, &matrixB[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_contact1[0], &m_castingVisualEntity->m_normal[0], 0);
//just test the center of collisionB against collisionA to see if the point is inside or not:
//int res = NewtonCollisionPointDistance(world, &matrixA.m_posit[0], collisionA, &matrixA[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_normal[0], 0);
if (res == 0) {
printf("Point Inside Body!\n");
//dTrace(("Point Inside Body!\n"));
}
else
{
//printf("Point point outside Body!\n");
}
}
void dPluginCamera::SetMatrix (dFloat yaw, dFloat roll, dFloat distance)
{
// dMatrix yawMatrix_ (dYawMatrix(yaw));
// dMatrix rollMatrix_ (dRollMatrix(roll));
// m_matrix = rollMatrix_ * yawMatrix_;
// m_matrix.m_posit = dVector (-10.0f, 5.0f, 10.0f, 1.0f);
m_cameraRoll = dClamp(roll, -85.0f * 3.141692f / 180.0f, 85.0f * 3.141692f / 180.0f);
m_cameraYaw = dMod (yaw, 2.0f * 3.141592f);
m_distance = (distance < 0.5f) ? 0.5f : distance;
dMatrix yawMatrix (dYawMatrix(m_cameraYaw));
dMatrix rollMatrix (dRollMatrix(m_cameraRoll));
m_matrix = rollMatrix * yawMatrix;
m_matrix.m_posit = m_matrix.RotateVector(dVector (-distance, 0.0f, 0.0f, 1.0f)) + m_pointOfInterest;
m_matrix.m_posit.m_w = 1.0f;
}
int dBezierSpline::CurveAllDerivatives (dFloat64 u, dBigVector* const derivatives) const
{
u = dMod (u, 1.0f);
dFloat64 basicsFuncDerivatives[D_BEZIER_LOCAL_BUFFER_SIZE];
int span = GetSpan(u);
BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
const int with = m_degree + 1;
dBigVector point (0.0f, 0.0f, 0.0f, 0.0f);
for (int j = 0; j <= m_degree; j ++) {
dBigVector ck (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; i <= m_degree; i ++) {
ck += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * j + i]);
}
derivatives[j] = ck;
}
return m_degree + 1;
}
void CustomDGRayCastCar::IntegrateTires (dFloat timestep, int threadIndex)
{
dMatrix bodyMatrix;
// 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]);
// set the current vehicle speed
m_curSpeed = bodyMatrix.m_front % bodyVelocity;
for (int i = 0; i < m_tiresCount; i ++ ) {
Tire& tire = m_tires[i];
/*
if (tire.m_tireIsConstrained) {
// the torqued generate by the tire can no be larger than the external torque on the tire
// when this happens ther tire is spinning unde contrained rotation
// V % dir + W * R % dir = 0
// where V is the tire Axel velocity
// W is the tire local angular velocity
// R is the tire radius
// dir is the longitudinal direction of of the tire.
dFloat contactRadius;
dFloat axelLinealSpeed;
dVector tireAxelPosit (chassisMatrix.TransformVector (tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit)));
dVector tireAxelVeloc = bodyVelocity + bodyOmega * (tireAxelPosit - chassisMatrix.m_posit) - tire.m_hitBodyPointVelocity;
axelLinealSpeed = tireAxelVeloc % chassisMatrix.m_front;
dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
contactRadius = (tire.m_lateralPin * tireRadius) % tire.m_longitudinalPin;
tire.m_angularVelocity = - axelLinealSpeed / contactRadius ;
} else if (tire.m_tireIsOnAir) {
if (tire.m_breakForce > 1.0e-3f) {
tire.m_angularVelocity = 0.0f;
} else {
//the tire is on air, need to be integrate net toque and apply a drag coneficenct
// dFloat nettorque = tire.m_angularVelocity;
// // this checkup is suposed to fix a infinit division by zero...
// if ( dAbs(tireContactSpeed) > 1.0e-3) {
// nettorque = - (tireLinearSpeed) / (tireContactSpeed);
// }
//tire.m_angularVelocity = - tireLinearSpeed / tireContactSpeed;
dFloat torque;
torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
}
} else {
// there is a next torque on the tire
dFloat torque;
torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
}
*/
if (tire.m_tireIsOnAir) {
if (tire.m_breakForce > 1.0e-3f) {
tire.m_angularVelocity = 0.0f;
} else {
//the tire is on air, need to be integrate net toque and apply a drag coeficient
dFloat torque;
torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
}
} else if (tire.m_tireIsConstrained) {
// the torqued generate by the tire can no be larger than the external torque on the tire
// when this happens there tire is spinning under constrained rotation
// V % dir + W * R % dir = 0
// where V is the tire Axel velocity
// W is the tire local angular velocity
// R is the tire radius
// dir is the longitudinal direction of of the tire.
dFloat contactRadius;
dFloat axelLinealSpeed;
dVector tireAxelPosit (chassisMatrix.TransformVector (tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit)));
dVector tireAxelVeloc = bodyVelocity + bodyOmega * (tireAxelPosit - chassisMatrix.m_posit) - tire.m_hitBodyPointVelocity;
axelLinealSpeed = tireAxelVeloc % chassisMatrix.m_front;
dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
contactRadius = (tire.m_lateralPin * tireRadius) % tire.m_longitudinalPin;
tire.m_angularVelocity = - axelLinealSpeed / contactRadius ;
} else {
// there is a next torque on the tire
dFloat torque;
torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
}
// 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;
}
}
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);
// }
}
void DemoCameraListener::PreUpdate (const NewtonWorld* const world, dFloat timestep)
{
// update the camera;
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
dMatrix targetMatrix (m_camera->GetNextMatrix());
int mouseX;
int mouseY;
scene->GetMousePosition (mouseX, mouseY);
// slow down the Camera if we have a Body
dFloat slowDownFactor = scene->IsShiftKeyDown() ? 0.5f/10.0f : 0.5f;
// do camera translation
if (scene->GetKeyState ('W')) {
targetMatrix.m_posit += targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('S')) {
targetMatrix.m_posit -= targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('A')) {
targetMatrix.m_posit -= targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('D')) {
targetMatrix.m_posit += targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('Q')) {
targetMatrix.m_posit -= targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('E')) {
targetMatrix.m_posit += targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
// do camera rotation, only if we do not have anything picked
bool buttonState = m_mouseLockState || scene->GetMouseKeyState(0);
if (!m_targetPicked && buttonState) {
int mouseSpeedX = mouseX - m_mousePosX;
int mouseSpeedY = mouseY - m_mousePosY;
if (mouseSpeedX > 0) {
m_yaw = dMod(m_yaw + m_yawRate, 2.0f * 3.1416f);
} else if (mouseSpeedX < 0){
m_yaw = dMod(m_yaw - m_yawRate, 2.0f * 3.1416f);
}
if (mouseSpeedY > 0) {
m_pitch += m_pitchRate;
} else if (mouseSpeedY < 0){
m_pitch -= m_pitchRate;
}
m_pitch = dClamp(m_pitch, dFloat (-80.0f * 3.1416f / 180.0f), dFloat (80.0f * 3.1416f / 180.0f));
}
m_mousePosX = mouseX;
m_mousePosY = mouseY;
dMatrix matrix (dRollMatrix(m_pitch) * dYawMatrix(m_yaw));
dQuaternion rot (matrix);
m_camera->SetMatrix (*scene, rot, targetMatrix.m_posit);
UpdatePickBody(scene, timestep);
}
