NewtonBody* Body::create(NewtonCollision* collision, float mass, int freezeState, const Vec4f& damping)
{
Vec4f minBox, maxBox;
Vec4f origin, inertia;
m_body = NewtonCreateBody(newton::world, collision, this->m_matrix[0]);
NewtonBodySetUserData(m_body, this);
NewtonBodySetMatrix(m_body, this->m_matrix[0]);
NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]);
if (mass < 0.0f)
mass = NewtonConvexCollisionCalculateVolume(collision) * 0.5f;
if (mass != 0.0f)
NewtonBodySetMassMatrix(m_body, mass, mass * inertia.x, mass * inertia.y, mass * inertia.z);
NewtonBodySetCentreOfMass(m_body, &origin[0]);
NewtonBodySetForceAndTorqueCallback(m_body, Body::__applyForceAndTorqueCallback);
NewtonBodySetTransformCallback(m_body, Body::__setTransformCallback);
NewtonBodySetDestructorCallback(m_body, Body::__destroyBodyCallback);
NewtonBodySetFreezeState(m_body, freezeState);
NewtonBodySetLinearDamping(m_body, damping.w);
NewtonBodySetAngularDamping(m_body, &damping[0]);
return m_body;
}
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);
}
// create a rope of boxes
void AddRope (NewtonWorld* nWorld)
{
int i;
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBody* link0;
NewtonBody* link1;
NewtonCustomJoint* joint;
NewtonCollision* collision;
RenderPrimitive* visualObject;
dVector size (2.0f, 0.25f, 0.25f);
// calculate a acurate momenet of inertia
mass = 2.0f;
Ixx = 0.7f * mass * (size.m_y * size.m_y + size.m_z * size.m_z) / 12.0f;
Iyy = 0.7f * mass * (size.m_x * size.m_x + size.m_z * size.m_z) / 12.0f;
Izz = 0.7f * mass * (size.m_x * size.m_x + size.m_y * size.m_y) / 12.0f;
// create 100 tack of 10 boxes each
//dMatrix location (GetIdentityMatrix());
dMatrix location (dgRollMatrix(3.1426f * 0.5f));
location.m_posit.m_y = 11.5f;
location.m_posit.m_z = -5.0f;
// create a collision primitive to be shared by all links
collision = NewtonCreateCapsule (nWorld, size.m_y, size.m_x, NULL);
link0 = NULL;
// create a lon vertical rope with limits
for (i = 0; i < 7; i ++) {
// create the a graphic character (use a visualObject as our body
visualObject = new CapsulePrimitive (location, size.m_y, size.m_x);
//create the rigid body
link1 = NewtonCreateBody (nWorld, collision);
// add some damping to each link
NewtonBodySetLinearDamping (link1, 0.2f);
dVector angularDamp (0.2f, 0.2f, 0.2f);
NewtonBodySetAngularDamping (link1, &angularDamp.m_x);
// Set Material Id for this object
NewtonBodySetMaterialGroupID (link1, woodID);
// save the pointer to the graphic object with the body.
NewtonBodySetUserData (link1, visualObject);
// set a destrutor for this rigid body
NewtonBodySetDestructorCallback (link1, PhysicsBodyDestructor);
// set the tranform call back function
NewtonBodySetTransformCallback (link1, PhysicsSetTransform);
// set the force and torque call back funtion
NewtonBodySetForceAndTorqueCallback (link1,PhysicsApplyGravityForce);
// set the mass matrix
NewtonBodySetMassMatrix (link1, mass, Ixx, Iyy, Izz);
// set the matrix for tboth the rigid nody and the graphic body
NewtonBodySetMatrix (link1, &location[0][0]);
PhysicsSetTransform (link1, &location[0][0]);
dVector pivot (location.m_posit);
pivot.m_y += (size.m_x - size.m_y) * 0.5f;
dFloat coneAngle = 2.0 * 3.1416f / 180.0f;
dFloat twistAngle = 2.0 * 3.1416f / 180.0f;
dVector pin (location.m_front.Scale (-1.0f));
joint = new CustomConeLimitedBallAndSocket(twistAngle, coneAngle, pin, pivot, link1, link0);
link0 = link1;
location.m_posit.m_y -= (size.m_x - size.m_y);
}
// vrete a short horizontal rope with limits
location = GetIdentityMatrix();
location.m_posit.m_y = 2.5f;
location.m_posit.m_z = -7.0f;
link0 = NULL;
for (i = 0; i < 3; i ++) {
// create the a graphic character (use a visualObject as our body
visualObject = new CapsulePrimitive (location, size.m_y, size.m_x);
//create the rigid body
link1 = NewtonCreateBody (nWorld, collision);
// add some damping to each link
NewtonBodySetLinearDamping (link1, 0.2f);
dVector angularDamp (0.2f, 0.2f, 0.2f);
NewtonBodySetAngularDamping (link1, &angularDamp.m_x);
// Set Material Id for this object
NewtonBodySetMaterialGroupID (link1, woodID);
// save the pointer to the graphic object with the body.
NewtonBodySetUserData (link1, visualObject);
// make sure it is active
NewtonWorldUnfreezeBody (nWorld, link1);
//NewtonBodySetAutoFreeze (link1, 0);
// set a destrutor for this rigid body
NewtonBodySetDestructorCallback (link1, PhysicsBodyDestructor);
// set the tranform call back function
NewtonBodySetTransformCallback (link1, PhysicsSetTransform);
// set the force and torque call back funtion
NewtonBodySetForceAndTorqueCallback (link1,PhysicsApplyGravityForce);
// set the mass matrix
NewtonBodySetMassMatrix (link1, mass, Ixx, Iyy, Izz);
// set the matrix for tboth the rigid nody and the graphic body
NewtonBodySetMatrix (link1, &location[0][0]);
PhysicsSetTransform (link1, &location[0][0]);
dVector pivot (location.m_posit);
pivot.m_x += (size.m_x - size.m_y) * 0.5f;
dFloat coneAngle = 10.0 * 3.1416f / 180.0f;
dFloat twistAngle = 10.0 * 3.1416f / 180.0f;
dVector pin (location.m_front.Scale (-1.0f));
joint = new CustomConeLimitedBallAndSocket(twistAngle, coneAngle, pin, pivot, link1, link0);
link0 = link1;
location.m_posit.m_x -= (size.m_x - size.m_y);
}
// release the collision geometry when not need it
NewtonReleaseCollision (nWorld, collision);
}
void dVehicleChassis::Init(NewtonBody* const body, const dMatrix& localFrame, NewtonApplyForceAndTorque forceAndTorque, dFloat gravityMag)
{
m_body = body;
NewtonBodySetForceAndTorqueCallback(m_body, forceAndTorque);
m_vehicle = new dVehicleSingleBody(this);
m_localFrame = localFrame;
m_localFrame.m_posit = dVector(0.0f, 0.0f, 0.0f, 1.0f);
dAssert(m_localFrame.TestOrthogonal());
m_gravity = dVector (0.0f, -dAbs(gravityMag), 0.0f, 0.0f);
// set linear and angular drag to zero
dVector drag(0.0f);
NewtonBodySetLinearDamping(m_body, 0.0f);
NewtonBodySetAngularDamping(m_body, &drag[0]);
/*
m_speed = 0.0f;
m_sideSlip = 0.0f;
m_prevSideSlip = 0.0f;
m_finalized = false;
m_gravityMag = dAbs(gravityMag);
m_weightDistribution = 0.5f;
m_aerodynamicsDownForce0 = 0.0f;
m_aerodynamicsDownForce1 = 0.0f;
m_aerodynamicsDownSpeedCutOff = 0.0f;
m_aerodynamicsDownForceCoefficient = 0.0f;
m_forceAndTorqueCallback = forceAndTorque;
dCustomVehicleControllerManager* const manager = (dCustomVehicleControllerManager*)GetManager();
NewtonWorld* const world = manager->GetWorld();
// set the standard force and torque call back
NewtonBodySetForceAndTorqueCallback(body, m_forceAndTorqueCallback);
m_contactFilter = new dTireFrictionModel(this);
m_collisionAggregate = NewtonCollisionAggregateCreate(world);
NewtonCollisionAggregateSetSelfCollision(m_collisionAggregate, 0);
NewtonCollisionAggregateAddBody(m_collisionAggregate, m_body);
m_bodyList.Append(m_body);
SetAerodynamicsDownforceCoefficient(0.5f, 0.4f, 1.0f);
#ifdef D_PLOT_ENGINE_CURVE
file_xxx = fopen("vehiceLog.csv", "wb");
fprintf(file_xxx, "eng_rpm, eng_torque, eng_nominalTorque,\n");
#endif
*/
// m_engine = NULL;
// m_brakesControl = NULL;
// m_engineControl = NULL;
m_steeringControl = NULL;
// m_handBrakesControl = NULL;
}
void dNewtonBody::SetAngularDamping(dFloat x, dFloat y, dFloat z)
{
dVector damp(x, y, z);
NewtonBodySetAngularDamping(m_body, &damp.m_x);
}
void cPhysicsBodyNewton::SetAngularDamping(float a_fDamping)
{
float fDamp[3] = {a_fDamping, a_fDamping, a_fDamping};
NewtonBodySetAngularDamping(m_pNewtonBody, fDamp);
}
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);
}
void PrecessingTops (DemoEntityManager* const scene)
{
scene->CreateSkyBox();
// customize the scene after loading
// set a user friction variable in the body for variable friction demos
// later this will be done using LUA script
dMatrix offsetMatrix (dGetIdentityMatrix());
CreateLevelMesh (scene, "flatPlane.ngd", 1);
dVector location (0.0f, 0.0f, 0.0f, 0.0f);
dVector size (3.0f, 2.0f, 0.0f, 0.0f);
// create an array of cones
const int count = 10;
// all shapes use the x axis as the axis of symmetry, to make an upright cone we apply a 90 degree rotation local matrix
dMatrix shapeOffsetMatrix (dRollMatrix(-3.141592f/2.0f));
AddPrimitiveArray(scene, 50.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, 0, shapeOffsetMatrix);
// till the cont 30 degrees, and apply a local high angular velocity
dMatrix matrix (dRollMatrix (-25.0f * 3.141592f / 180.0f));
dVector omega (0.0f, 50.0f, 0.0f);
omega = matrix.RotateVector (omega);
dVector damp (0.0f, 0.0f, 0.0f, 0.0f);
int topscount = 0;
NewtonBody* array[count * count];
NewtonWorld* const world = scene->GetNewton();
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
NewtonCollision* const collision = NewtonBodyGetCollision(body);
dVector com;
if (NewtonCollisionGetType (collision) == SERIALIZE_ID_CONE) {
array[topscount] = body;
topscount ++;
}
}
for (int i = 0; i < topscount ; i ++) {
dMatrix bodyMatrix;
NewtonBody* const body = array[i];
NewtonBodyGetMatrix(body, &bodyMatrix[0][0]);
matrix.m_posit = bodyMatrix.m_posit;
matrix.m_posit.m_y += 1.0f;
NewtonBodySetMatrix(body, &matrix[0][0]);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodySetMassMatrix(body, mass, Ixx, Iyy * 8.0f, Izz);
NewtonBodySetOmega (body, &omega[0]);
NewtonBodySetAutoSleep (body, 0);
NewtonBodySetLinearDamping(body, 0.0f);
NewtonBodySetAngularDamping (body, &damp[0]);
}
// place camera into position
dMatrix camMatrix (dGetIdentityMatrix());
dQuaternion rot (camMatrix);
dVector origin (-40.0f, 5.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
void
PhysicsActor::setAngularDamping(const Math::Vector3& _damping)
{
NewtonBodySetAngularDamping(m_pActor, _damping.m_array);
}
void iPhysics::setAngularDamping(void* newtonBody, const iaVector3f& angularDamp)
{
NewtonBodySetAngularDamping(static_cast<const NewtonBody*>(newtonBody), angularDamp.getData());
}
