// calculate the closest point from the spline to point point
dMatrix CustomPathFollow::EvalueCurve (const dVector& posit)
{
dMatrix matrix;
// calculate distance for point to list
matrix.m_posit = m_pointOnPath + m_pathTangent.Scale ((posit - m_pointOnPath) % m_pathTangent);
matrix.m_posit.m_w = 1.0f;
//the tangent of the path is the line slope, passes in the first matrix dir
matrix.m_front = m_pathTangent;
// the normal will be such that it is horizontal to the the floor and perpendicular to the path
dVector normal (dVector (0.0f, 1.0f, 0.0f, 0.0f) * matrix.m_front);
matrix.m_up = normal.Scale (1.0f / dSqrt (normal % matrix.m_front));
// the binormal is just the cross product;
matrix.m_right = matrix.m_front * matrix.m_up;
return matrix;
}
// add force and torque to rigid body
void PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;
dVector force (dVector (0.0f, 1.0f, 0.0f).Scale (mass * DEMO_GRAVITY));
NewtonBodySetForce (body, &force.m_x);
/*
// check that angular momentum is conserved
dMatrix I;
dVector omega(0.0f);
NewtonBodyGetInertiaMatrix(body, &I[0][0]);
NewtonBodyGetOmega(body, &omega[0]);
dVector L (I.RotateVector(omega));
dTrace (("(%f %f %f) (%f %f %f)\n", omega[0], omega[1], omega[2], L[0], L[1], L[2]));
*/
}
static int MakeRandomPoisonPointCloud(NewtonMesh* const mesh, dVector* const points)
{
dVector size(0.0f);
dMatrix matrix(dGetIdentityMatrix());
NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);
dVector minBox (matrix.m_posit - matrix[0].Scale (size.m_x) - matrix[1].Scale (size.m_y) - matrix[2].Scale (size.m_z));
dVector maxBox (matrix.m_posit + matrix[0].Scale (size.m_x) + matrix[1].Scale (size.m_y) + matrix[2].Scale (size.m_z));
size = maxBox - minBox;
int xCount = int (size.m_x / POINT_DENSITY_PER_METERS) + 1;
int yCount = int (size.m_y / POINT_DENSITY_PER_METERS) + 1;
int zCount = int (size.m_z / POINT_DENSITY_PER_METERS) + 1;
int count = 0;
dFloat z0 = minBox.m_z;
for (int iz = 0; (iz < zCount) && (count < MAX_POINT_CLOUD_SIZE); iz ++) {
dFloat y0 = minBox.m_y;
for (int iy = 0; (iy < yCount) && (count < MAX_POINT_CLOUD_SIZE); iy ++) {
dFloat x0 = minBox.m_x;
for (int ix = 0; (ix < xCount) && (count < MAX_POINT_CLOUD_SIZE); ix ++) {
dFloat x = x0;
dFloat y = y0;
dFloat z = z0;
x += RandomVariable(POISON_VARIANCE);
y += RandomVariable(POISON_VARIANCE);
z += RandomVariable(POISON_VARIANCE);
points[count] = dVector (x, y, z);
count ++;
x0 += POINT_DENSITY_PER_METERS;
}
y0 += POINT_DENSITY_PER_METERS;
}
z0 += POINT_DENSITY_PER_METERS;
}
return count;
}
void UpdateCamera (dFloat timestep)
{
if (m_player) {
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(GetWorld());
DemoCamera* const camera = scene->GetCamera();
dMatrix camMatrix (camera->GetNextMatrix ());
dMatrix playerMatrix (m_player->GetNextMatrix());
dVector frontDir (camMatrix[0]);
dVector camOrigin;
if (m_externalView) {
camOrigin = playerMatrix.m_posit + dVector(0.0f, VEHICLE_THIRD_PERSON_VIEW_HIGHT, 0.0f, 0.0f);
camOrigin -= frontDir.Scale (VEHICLE_THIRD_PERSON_VIEW_DIST);
} else {
dAssert (0);
// camMatrix = camMatrix * playerMatrix;
// camOrigin = playerMatrix.TransformVector(dVector(-0.8f, ARTICULATED_VEHICLE_CAMERA_EYEPOINT, 0.0f, 0.0f));
}
camera->SetNextMatrix (*scene, camMatrix, camOrigin);
}
}
void dCustomJoint::dDebugDisplay::DrawFrame(const dMatrix& matrix)
{
dVector o0(matrix.m_posit);
dFloat size = 0.25f;
dVector x(matrix.m_posit + matrix.RotateVector(dVector(size, 0.0f, 0.0f, 0.0f)));
SetColor(dVector (1.0f, 0.0f, 0.0f));
DrawLine (matrix.m_posit, x);
dVector y(matrix.m_posit + matrix.RotateVector(dVector(0.0f, size, 0.0f, 0.0f)));
SetColor(dVector (0.0f, 1.0f, 0.0f));
DrawLine (matrix.m_posit, y);
dVector z(matrix.m_posit + matrix.RotateVector(dVector(0.0f, 0.0f, size, 0.0f)));
SetColor(dVector (0.0f, 0.0f, 1.0f));
DrawLine (matrix.m_posit, z);
}
void UserPlaneCollision (DemoEntityManager* const scene)
{
// load the sky box
scene->CreateSkyBox();
// create a user define infinite plane collision, and use it as a floor
CreatePlaneCollision (scene, dVector (0.0f, 1.0f, 0.0f, 0.0f));
dMatrix camMatrix (dRollMatrix(-20.0f * dDegreeToRad) * dYawMatrix(-45.0f * dDegreeToRad));
dQuaternion rot (camMatrix);
dVector origin (0.0f, 0.0f, 0.0f, 0.0f);
// dFloat hight = 1000.0f;
// origin = FindFloor (scene->GetNewton(), dVector (origin.m_x, hight, origin .m_z, 0.0f), hight * 2);
origin.m_y += 10.0f;
scene->SetCameraMatrix(rot, origin);
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (scene->GetNewton());
dVector location (origin);
location.m_x += 20.0f;
location.m_z += 20.0f;
// dVector size (0.5f, 0.5f, 0.75f, 0.0f);
dVector size (1.0f, 1.0f, 1.0f, 0.0f);
int count = 1;
dMatrix shapeOffsetMatrix (dGetIdentityMatrix());
AddUniformScaledPrimitives(scene, 10.0f, location, size, count, count, 4.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
}
void SimpleMeshLevelCollision (NewtonFrame& system)
{
NewtonWorld* world;
world = system.m_world;
// create the sky box and the floor,
// LoadLevelAndSceneRoot (system, "flatplane.dae", 0);
// LoadLevelAndSceneRoot (system, "dungeon.dae", 0);
// LoadLevelAndSceneRoot (system, "xxx.dae", 1);
// LoadLevelAndSceneRoot (system, "xxx1.dae", 1);
// LoadLevelAndSceneRoot (system, "pitpool1.dae", 0);
LoadLevelAndSceneRoot (system, "pitpool.dae", 0);
// LoadLevelAndSceneRoot (system, "playground.dae", 0);
// create a material to collide with this object
int defaultMaterialID;
defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
dVector posit (0.0f, 0.0f, 0.0f, 0.0f);
posit.m_y = FindFloor (world, posit.m_x, posit.m_z) + 5.0f;
InitEyePoint (dVector (1.0f, 0.0f, 0.0f), posit);
dVector size (1.0f, 1.0f, 1.0f);
dVector location (cameraEyepoint + cameraDir.Scale (10.0f));
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _SPHERE_PRIMITIVE, defaultMaterialID);
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _BOX_PRIMITIVE, defaultMaterialID);
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CONE_PRIMITIVE, defaultMaterialID);
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CYLINDER_PRIMITIVE, defaultMaterialID);
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CAPSULE_PRIMITIVE, defaultMaterialID);
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID);
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
AddBoxes(&system, 10.0f, location, size, 3, 3, 10.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &QB, dFloat dt) const
{
dFloat dirMag;
dFloat dirMag2;
dFloat omegaMag;
dFloat dirMagInv;
dQuaternion dq (Inverse() * QB);
dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
}
dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dirMag = dirMag2 * dirMagInv;
omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) / dt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
static void ShowMeshCollidingFaces (
const NewtonBody* bodyWithTreeCollision,
const NewtonBody* body,
int faceID,
int vertexCount,
const dFloat* vertex,
int vertexstrideInBytes)
{
// we are coping data to and array of memory, another call back may be doing the same thing
// here fore we need to avoid race conditions
NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (bodyWithTreeCollision));
dVector face[64];
int stride = vertexstrideInBytes / sizeof (dFloat);
for (int j = 0; j < vertexCount; j ++) {
face [j] = dVector (vertex[j * stride + 0], vertex[j * stride + 1] , vertex[j * stride + 2]);
}
DebugDrawPolygon (vertexCount, face);
// unlock the critical section
NewtonWorldCriticalSectionUnlock (NewtonBodyGetWorld (bodyWithTreeCollision));
}
static void AddPathFollow (DemoEntityManager* const scene, const dVector& origin)
{
// create a Bezier Spline path for AI car to drive
DemoEntity* const rollerCosterPath = new DemoEntity(dGetIdentityMatrix(), NULL);
scene->Append(rollerCosterPath);
dBezierSpline spline;
dFloat64 knots[] = {0.0f, 1.0f / 3.0f, 2.0f / 3.0f, 1.0f};
dBigVector o (origin[0], origin[1], origin[2], 0.0f);
dBigVector control[] =
{
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 10.0f, 150.0f - 250.0f, 1.0f) + o,
dBigVector(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 10.0f, 350.0f - 250.0f, 1.0f) + o,
dBigVector( 50.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
};
spline.CreateFromKnotVectorAndControlPoints(3, sizeof (knots) / sizeof (knots[0]), knots, control);
DemoBezierCurve* const mesh = new DemoBezierCurve (spline);
rollerCosterPath->SetMesh(mesh, dGetIdentityMatrix());
mesh->SetVisible(true);
mesh->SetRenderResolution(500);
mesh->Release();
NewtonBody* const box0 = CreateWheel(scene, origin + dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f), 1.0f, 0.5f);
dMatrix matrix;
NewtonBodyGetMatrix (box0, &matrix[0][0]);
matrix.m_posit.m_y += 0.5f;
new MyPathFollow (matrix, box0, rollerCosterPath);
}
void UserHeightFieldCollision (DemoEntityManager* const scene)
{
// load the sky box
scene->CreateSkyBox();
CreateHeightFieldTerrain(scene, HEIGHTFIELD_DEFAULT_SIZE, HEIGHTFIELD_DEFAULT_CELLSIZE, 1.5f, 0.2f, 200.0f, -50.0f);
dMatrix camMatrix (dRollMatrix(-20.0f * dDegreeToRad) * dYawMatrix(-45.0f * dDegreeToRad));
dQuaternion rot (camMatrix);
dVector origin (250.0f, 0.0f, 250.0f, 0.0f);
dFloat height = 1000.0f;
origin = FindFloor (scene->GetNewton(), dVector (origin.m_x, height, origin .m_z, 0.0f), height * 2);
origin.m_y += 10.0f;
scene->SetCameraMatrix(rot, origin);
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (scene->GetNewton());
dVector location (origin);
location.m_x += 20.0f;
location.m_z += 20.0f;
dVector size (0.5f, 0.5f, 0.75f, 0.0f);
int count = 5;
// int count = 1;
dMatrix shapeOffsetMatrix (dGetIdentityMatrix());
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
}
//dAnimTakeData* LoadAnimation(dAnimIKController* const controller, const char* const animName)
dAnimationKeyframesSequence* GetAnimationSequence(const char* const animName)
{
dTree<dAnimationKeyframesSequence*, dString>::dTreeNode* cachedAnimNode = m_animCache.Find(animName);
if (!cachedAnimNode) {
dScene scene(GetWorld());
char pathName[2048];
dGetWorkingFileName(animName, pathName);
scene.Deserialize(pathName);
dScene::dTreeNode* const animTakeNode = scene.FindChildByType(scene.GetRootNode(), dAnimationTake::GetRttiType());
if (animTakeNode) {
//dTree<dAnimimationKeyFramesTrack*, dString> map;
//const dAnimPose& basePose = controller->GetBasePose();
//dAnimTakeData* const animdata = new dAnimTakeData(basePose.GetCount());
dAnimationKeyframesSequence* const animdata = new dAnimationKeyframesSequence();
dAnimationTake* const animTake = (dAnimationTake*)scene.GetInfoFromNode(animTakeNode);
animdata->SetPeriod(animTake->GetPeriod());
cachedAnimNode = m_animCache.Insert(animdata, animName);
//dList<dAnimimationKeyFramesTrack>& tracks = animdata->GetTracks();
//dList<dAnimimationKeyFramesTrack>::dListNode* ptr = tracks.GetFirst();
//dList<dAnimimationKeyFramesTrack>::dListNode* ptr = tracks.Append();
//for (dAnimPose::dListNode* ptrNode = basePose.GetFirst(); ptrNode; ptrNode = ptrNode->GetNext()) {
// DemoEntity* const entity = (DemoEntity*)ptrNode->GetInfo().m_userData;
// map.Insert(&ptr->GetInfo(), entity->GetName());
// ptr = ptr->GetNext();
//}
dList<dAnimimationKeyFramesTrack>& tracks = animdata->GetTracks();
for (void* link = scene.GetFirstChildLink(animTakeNode); link; link = scene.GetNextChildLink(animTakeNode, link)) {
dScene::dTreeNode* const node = scene.GetNodeFromLink(link);
dAnimationTrack* const srcTrack = (dAnimationTrack*)scene.GetInfoFromNode(node);
if (srcTrack->IsType(dAnimationTrack::GetRttiType())) {
//dTree<dAnimimationKeyFramesTrack*, dString>::dTreeNode* const ptrNode = map.Find(srcTrack->GetName());
//dAssert(ptrNode);
//dAnimTakeData::dAnimTakeTrack* const dstTrack = ptrNode->GetInfo();
dAnimimationKeyFramesTrack* const dstTrack = &tracks.Append()->GetInfo();
dstTrack->SetName(srcTrack->GetName());
const dList<dAnimationTrack::dCurveValue>& rotations = srcTrack->GetRotations();
dstTrack->m_rotation.Resize(rotations.GetCount());
int index = 0;
for (dList<dAnimationTrack::dCurveValue>::dListNode* node = rotations.GetFirst(); node; node = node->GetNext()) {
dAnimationTrack::dCurveValue keyFrame(node->GetInfo());
dMatrix matrix(dPitchMatrix(keyFrame.m_x) * dYawMatrix(keyFrame.m_y) * dRollMatrix(keyFrame.m_z));
dQuaternion rot(matrix);
dstTrack->m_rotation[index].m_rotation = rot;
dstTrack->m_rotation[index].m_time = keyFrame.m_time;
index++;
}
for (int i = 0; i < rotations.GetCount() - 1; i++) {
dFloat dot = dstTrack->m_rotation[i].m_rotation.DotProduct(dstTrack->m_rotation[i + 1].m_rotation);
if (dot < 0.0f) {
dstTrack->m_rotation[i + 1].m_rotation.m_x *= -1.0f;
dstTrack->m_rotation[i + 1].m_rotation.m_y *= -1.0f;
dstTrack->m_rotation[i + 1].m_rotation.m_z *= -1.0f;
dstTrack->m_rotation[i + 1].m_rotation.m_w *= -1.0f;
}
}
const dList<dAnimationTrack::dCurveValue>& positions = srcTrack->GetPositions();
dstTrack->m_position.Resize(positions.GetCount());
index = 0;
for (dList<dAnimationTrack::dCurveValue>::dListNode* node = positions.GetFirst(); node; node = node->GetNext()) {
dAnimationTrack::dCurveValue keyFrame(node->GetInfo());
dstTrack->m_position[index].m_posit = dVector(keyFrame.m_x, keyFrame.m_y, keyFrame.m_z, dFloat(1.0f));
dstTrack->m_position[index].m_time = keyFrame.m_time;
index++;
}
}
}
}
}
dAssert(cachedAnimNode);
return cachedAnimNode->GetInfo();
}
void Custom6DOF::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);
// add the linear limits
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
dVector dp (p0 - p1);
for (int i = 0; i < 3; i ++) {
if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
// it is a limited linear dof, check if it pass the limits
dFloat dist = dp.DotProduct3(matrix1[i]);
if (dist > m_maxLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (dist < m_minLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
}
dVector euler0(0.0f);
dVector euler1(0.0f);
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));
bool limitViolation = false;
for (int i = 0; i < 3; i ++) {
if (euler[i] < m_minAngularLimits[i]) {
limitViolation = true;
euler[i] = m_minAngularLimits[i];
} else if (euler[i] > m_maxAngularLimits[i]) {
limitViolation = true;
euler[i] = m_maxAngularLimits[i];
}
}
if (limitViolation) {
//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
}
}
}
void CreateScene (NewtonWorld* world, SceneManager* sceneManager)
{
Entity* floor;
NewtonCollision* shape;
NewtonBody* floorBody;
void* materialManager;
SoundManager* sndManager;
PhysicsMaterialInteration matInterations;
sndManager = sceneManager->GetSoundManager();
// Create the material for this scene, and attach it to the Newton World
materialManager = CreateMaterialManager (world, sndManager);
// add the Material table
matInterations.m_restitution = 0.6f;
matInterations.m_staticFriction = 0.6f;
matInterations.m_kineticFriction = 0.3f;
matInterations.m_scrapingSound = NULL;
matInterations.m_impactSound = sndManager->LoadSound ("metalMetal.wav");
AddMaterilInteraction (materialManager, m_metal, m_metal, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("boxBox.wav");
AddMaterilInteraction (materialManager, m_wood, m_wood, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("metalBox.wav");
AddMaterilInteraction (materialManager, m_metal, m_wood, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("grass0.wav");
AddMaterilInteraction (materialManager, m_wood, m_grass, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("boxHit.wav");
AddMaterilInteraction (materialManager, m_wood, m_bricks, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("grass1.wav");
AddMaterilInteraction (materialManager, m_metal, m_grass, &matInterations);
AddMaterilInteraction (materialManager, m_grass, m_bricks, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("metal.wav");
AddMaterilInteraction (materialManager, m_metal, m_bricks, &matInterations);
AddMaterilInteraction (materialManager, m_grass, m_grass, &matInterations);
// Create a large body to be the floor
floor = sceneManager->CreateEntity();
floor->LoadMesh ("LevelMesh.dat");
// add static floor Physics
int materialMap[] = {m_bricks, m_grass, m_wood, m_metal};
shape = CreateMeshCollision (world, floor, materialMap);
floorBody = CreateRigidBody (world, floor, shape, 0.0f);
NewtonReleaseCollision (world, shape);
// set the Transformation Matrix for this rigid body
dMatrix matrix (floor->m_curRotation, floor->m_curPosition);
NewtonBodySetMatrix (floorBody, &matrix[0][0]);
// now we will use the properties of this body to set a proper world size.
dVector minBox;
dVector maxBox;
NewtonCollisionCalculateAABB (shape, &matrix[0][0], &minBox[0], &maxBox[0]);
// add some extra padding
minBox.m_x -= 50.0f;
minBox.m_y -= 500.0f;
minBox.m_z -= 50.0f;
maxBox.m_x += 50.0f;
maxBox.m_y += 500.0f;
maxBox.m_z += 50.0f;
// set the new world size
NewtonSetWorldSize (world, &minBox[0], &maxBox[0]);
// add some visual entities.
dFloat y0 = FindFloor (world, 0.0f, 0.0f) + 10.0f;
for (int i = 0; i < 5; i ++) {
Entity* smilly;
NewtonBody* smillyBody;
smilly = sceneManager->CreateEntity();
smilly->LoadMesh ("Smilly.dat");
smilly->m_curPosition.m_y = y0;
y0 += 2.0f;
smilly->m_prevPosition = smilly->m_curPosition;
// add a body with a box shape
shape = CreateNewtonBox (world, smilly, m_metal);
smillyBody = CreateRigidBody (world, smilly, shape, 10.0f);
NewtonReleaseCollision (world, shape);
}
// add some visual entities.
y0 = FindFloor (world, 0.0f, 0.4f) + 10.5f;
for (int i = 0; i < 5; i ++) {
Entity* frowny;
NewtonBody* frownyBody;
frowny = sceneManager->CreateEntity();
frowny->LoadMesh ("Frowny.dat");
frowny->m_curPosition.m_z = 0.4f;
frowny->m_curPosition.m_y = y0;
y0 += 2.0f;
frowny->m_prevPosition = frowny->m_curPosition;
// add a body with a Convex hull shape
shape = CreateNewtonConvex (world, frowny, m_wood);
frownyBody = CreateRigidBody (world, frowny, shape, 10.0f);
NewtonReleaseCollision (world, shape);
}
y0 = FindFloor (world, 0.0f, 2.0f) + 10.5f;
for (int i = 0; i < 5; i ++) {
Entity* frowny;
NewtonBody* frownyBody;
frowny = sceneManager->CreateEntity();
frowny->LoadMesh ("dumb.dat");
frowny->m_curPosition.m_z = 2.0f;
frowny->m_curPosition.m_y = y0;
y0 += 2.0f;
frowny->m_prevPosition = frowny->m_curPosition;
// add a body with a Convex hull shape
int materialMap[] = {m_grass, m_wood, m_metal, m_metal};
shape = CreateNewtonCompoundFromEntitySubmesh (world, frowny, materialMap);
frownyBody = CreateRigidBody (world, frowny, shape, 10.0f);
NewtonReleaseCollision (world, shape);
}
// set the Camera EyePoint close to the scene action
InitCamera (dVector (-15.0f, FindFloor (world, -15.0f, 0.0f) + 5.0f, 0.0f), dVector (1.0f, 0.0f, 0.0f));
}
int dRuntimeProfiler::RenderConcurrentPerformance (int lineNumber)
{
struct GLViewPort
{
int x;
int y;
int width;
int height;
} viewport;
//Retrieves the viewport and stores it in the variable
glGetIntegerv(GL_VIEWPORT, (GLint*) &viewport.x);
m_width = viewport.width;
m_height = viewport.height;
glColor3f(1.0, 1.0, 1.0);
glDisable (GL_LIGHTING);
//glDisable(GL_TEXTURE_2D);
glMatrixMode(GL_TEXTURE);
glPushMatrix();
glLoadIdentity();
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluOrtho2D(0, viewport.width, 0, viewport.height );
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glDisable(GL_DEPTH_TEST);
glDisable(GL_BLEND);
DrawLabel (10, m_height - m_nextLine, "Concurrent profiler, red = physcis; blue = graphics green = idle");
m_nextLine = lineNumber;
dFloat times[10];
dVector colors[10];
colors[0] = dVector (0.0f, 1.0f, 0.0f, 0.5f);
colors[1] = dVector (0.0f, 0.0f, 1.0f, 0.5f);
times[0] = m_scene->m_mainThreadPhysicsTime;
times[1] = m_scene->m_mainThreadGraphicsTime;
DrawLabel (10, m_height - m_nextLine, "graphics thread:");
m_nextLine += 30;
DrawConcurrentChart(2, times, colors);
m_nextLine += 30;
colors[0] = dVector (1.0f, 0.0f, 0.0f, 0.5f);
times[0] = m_scene->m_physThreadTime;
DrawLabel (10, m_height - m_nextLine, "physics thread:");
m_nextLine += 30;
DrawConcurrentChart(1, times, colors);
m_nextLine += 40;
glMatrixMode(GL_TEXTURE);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glDisable(GL_BLEND);
glEnable (GL_DEPTH_TEST);
glColor3f(1.0, 1.0, 1.0);
return m_nextLine + 30;
}
void dRuntimeProfiler::RenderThreadPerformance ()
{
NewtonWorld* const world = m_scene->GetNewton();
int threadCount = NewtonGetThreadsCount(world);
if (threadCount > 0) {
struct GLViewPort
{
int x;
int y;
int width;
int height;
} viewport;
//Retrieves the viewport and stores it in the variable
glGetIntegerv(GL_VIEWPORT, (GLint*) &viewport.x);
glColor3f(1.0, 1.0, 1.0);
glDisable (GL_LIGHTING);
//glDisable(GL_TEXTURE_2D);
glMatrixMode(GL_TEXTURE);
glPushMatrix();
glLoadIdentity();
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluOrtho2D(0, viewport.width, 0, viewport.height );
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glDisable(GL_DEPTH_TEST);
glDisable(GL_BLEND);
m_width = viewport.width;
m_height = viewport.height;
float x0 = m_width - 600.0f;
float x1 = x0 + 256.0f;
float y0 = 50.0f;
for (int i = 0; i < threadCount; i ++) {
char label[32];
sprintf (label, "thread %d", i);
DrawLabel (x0 - 90, y0 + i * 22 - 10, label);
}
DrawLabel (x0, y0 - 30, "0.0 ms");
DrawLabel ((x1 + x0) * 0.5f, y0 - 30, "8.33 ms");
DrawLabel (x1, y0 - 30, "16.66 ms");
y0 -= 15.0f;
dVector color (1.0f, 0.0f, 0.0f, 1.0f/8.0f);
DrawRectangle (x0, m_height - (y0 + 20.0f * threadCount), x1 - x0, 20 * threadCount, color);
color = dVector (1.0f, 1.0f, 0.0f, 1.0f/2.0f);
if (threadCount > 1) {
for (int i = 0; i < threadCount; i ++) {
int tick = NewtonReadThreadPerformanceTicks (world, i);
dFloat time = dFloat (tick) * (1.0e-3f * 256.0f / 16.666f);
DrawRectangle(x0, m_height - (y0 + 15), time, 10, color);
y0 += 20.0f;
}
} else {
int tick = NewtonReadPerformanceTicks (world, NEWTON_PROFILER_WORLD_UPDATE);
dFloat time = dFloat (tick) * (1.0e-3f * 256.0f / 16.666f);
DrawRectangle(x0, m_height - (y0 + 15), time, 10, color);
}
glMatrixMode(GL_TEXTURE);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glDisable(GL_BLEND);
glEnable (GL_DEPTH_TEST);
glColor3f(1.0, 1.0, 1.0);
}
}
void ScaledMeshCollision (DemoEntityManager* const scene)
{
// load the skybox
scene->CreateSkyBox();
// load the scene from a ngd file format
CreateLevelMesh (scene, "flatPlane.ngd", 1);
//CreateLevelMesh (scene, "flatPlaneDoubleFace.ngd", 1);
//CreateLevelMesh (scene, "sponza.ngd", 0);
//CreateLevelMesh (scene, "cattle.ngd", fileName);
//CreateLevelMesh (scene, "playground.ngd", 0);
//dMatrix camMatrix (dRollMatrix(-20.0f * 3.1416f /180.0f) * dYawMatrix(-45.0f * 3.1416f /180.0f));
dMatrix camMatrix (dGetIdentityMatrix());
dQuaternion rot (camMatrix);
dVector origin (-15.0f, 5.0f, 0.0f, 0.0f);
//origin = origin.Scale (0.25f);
scene->SetCameraMatrix(rot, origin);
NewtonWorld* const world = scene->GetNewton();
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
dVector location (0.0f, 0.0f, 0.0f, 0.0f);
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = location;
matrix.m_posit.m_x = 0.0f;
matrix.m_posit.m_y = 0.0f;
matrix.m_posit.m_z = 0.0f;
matrix.m_posit.m_w = 1.0f;
DemoEntity teaPot (dGetIdentityMatrix(), NULL);
teaPot.LoadNGD_mesh("teapot.ngd", world);
//teaPot.LoadNGD_mesh("box.ngd", world);
NewtonCollision* const staticCollision = CreateCollisionTree (world, &teaPot, 0, true);
CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (1.0f, 1.0f, 1.0f, 0.0f));
// CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 2.0f, 0.0f));
matrix.m_posit.m_z = -5.0f;
CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 2.0f, 0.0f));
matrix.m_posit.m_z = 5.0f;
CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (3.0f, 3.0f, 1.5f, 0.0f));
matrix.m_posit.m_z = 0.0f;
matrix.m_posit.m_x = -5.0f;
CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (0.5f, 0.5f, 0.5f, 0.0f));
matrix.m_posit.m_x = 5.0f;
CreateScaleStaticMesh (&teaPot, staticCollision, scene, matrix, dVector (2.0f, 2.0f, 2.0f, 0.0f));
// do not forget to destroy the collision mesh helper
NewtonDestroyCollision(staticCollision);
dVector size0 (1.0f, 1.0f, 1.0f, 0.0f);
dVector size1 (0.5f, 1.0f, 1.0f, 0.0f);
dMatrix shapeOffsetMatrix (dRollMatrix(3.141592f/2.0f));
int count = 3;
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size1, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size1, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddNonUniformScaledPrimitives(scene, 10.0f, location, size0, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
origin.m_x -= 4.0f;
origin.m_y += 1.0f;
scene->SetCameraMatrix(rot, origin);
}
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;
}
}
}
dInfinitePlane (NewtonWorld* const world, const dVector& plane)
:m_minBox (-0.1f, -2000.0f, -2000.0f, 0.0f)
,m_maxBox ( 0.1f, 2000.0f, 2000.0f, 0.0f)
{
// get the transformation matrix that takes the plane to the world local space
m_rotation = dGrammSchmidt(plane);
// build a unit grid in local space (this will be the shadow at projection of the collision aabb)
m_unitSphape[0] = dVector (0.0f, 1.0f, 1.0f);
m_unitSphape[1] = dVector (0.0f, -1.0f, 1.0f);
m_unitSphape[2] = dVector (0.0f, -1.0f, -1.0f);
m_unitSphape[3] = dVector (0.0f, 1.0f, -1.0f);
// save the plane in local space
m_plane = m_rotation.UntransformPlane (plane);
#ifdef PASS_A_QUAD
// passing a single quad
for (int i = 0; i < MAX_THREAD_FACES; i ++) {
m_faceIndices[i][0] = 4;
// face attribute
m_indexArray[i][4] = 0;
// face normal
m_indexArray[i][4 + 1] = 4;
// face area (the plane is clipped around the box, the face size is always optimal)
m_indexArray[i][4 + 2 + 4] = 0;
for (int j = 0; j < 4; j ++) {
// face vertex index
m_indexArray[i][j] = j;
// face adjacent index (infinite plane does not have shared edge with other faces)
m_indexArray[i][j + 4 + 2] = 4;
}
}
#else
// passing two triangle
for (int i = 0; i < MAX_THREAD_FACES; i ++) {
// first triangle
{
// index count
m_faceIndices[i][0] = 3;
// face indices
m_indexArray[i][0] = 0;
m_indexArray[i][1] = 1;
m_indexArray[i][2] = 2;
// face attribute
m_indexArray[i][3] = 0;
// face normal
m_indexArray[i][4] = 4;
// face adjacent index (infinite plane does not have shared edge with other faces)
m_indexArray[i][5] = 4;
m_indexArray[i][6] = 4;
m_indexArray[i][7] = 4;
// face area (the plane is clipped around the box, the face size is always optimal)
m_indexArray[i][8] = 0;
}
// second triangle
{
// index count
m_faceIndices[i][1] = 3;
// face indices
m_indexArray[i][0 + 9] = 0;
m_indexArray[i][1 + 9] = 2;
m_indexArray[i][2 + 9] = 3;
// face attribute
m_indexArray[i][3 + 9] = 0;
// face normal
m_indexArray[i][4 + 9] = 4;
// face adjacent index (infinite plane does not have shared edge with other faces)
m_indexArray[i][5 + 9] = 4;
m_indexArray[i][6 + 9] = 4;
m_indexArray[i][7 + 9] = 4;
// face area (the plane is clipped around the box, the face size is always optimal)
m_indexArray[i][8 + 9] = 0;
}
}
#endif
// create a Newton user collision
m_collision = NewtonCreateUserMeshCollision (world, &m_minBox[0], &m_maxBox[0], this,
PlaneCollisionCollideCallback, PlaneMeshCollisionRayHitCallback,
PlaneCollisionDestroyCallback, PlaneCollisionGetCollisionInfo,
PlaneCollisionAABBOverlapTest, PlaneCollisionGetFacesInAABB,
UserCollisionSerializationCallback, 0);
// set a debug display call back
NewtonStaticCollisionSetDebugCallback (m_collision, ShowMeshCollidingFaces);
// set the collisoin offset Matrix;
NewtonCollisionSetMatrix(m_collision, &m_rotation[0][0]);
}
bool MousePick (NewtonWorld* nWorld, const dMOUSE_POINT& mouse1, dInt32 mouseLeftKey1, dFloat witdh, dFloat length)
{
static int mouseLeftKey0;
static dMOUSE_POINT mouse0;
static bool mousePickMode = false;
dMatrix matrix;
static NewtonUserJoint* bodyPickController;
if (mouseLeftKey1) {
if (!mouseLeftKey0) {
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
pickedBody = NULL;
pickedParam = 1.1f;
isPickedBodyDynamics = false;
NewtonWorldRayCast(nWorld, &p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
if (pickedBody) {
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
mousePickMode = true;
//NewtonBodySetFreezeState (pickedBody, 0);
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p (p0 + (p1 - p0).Scale (pickedParam));
attachmentPoint = matrix.UntransformVector (p);
// convert normal to local space
rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);
// Create PickBody Joint
NewtonBodyGetMassMatrix (pickedBody, &mass, &Ixx, &Iyy, &Izz);
if (mass) {
// bodyPickController = new CustomPickBody (pickedBody, p);
// bodyPickController->SetMaxLinearFriction (MAX_PICK_ACCEL);
// bodyPickController->SetMaxAngularFriction (MAX_PICK_ACCEL * 5.0f);
bodyPickController = CreateCustomKinematicController (pickedBody, &p[0]);
CustomKinematicControllerSetMaxLinearFriction (bodyPickController, MAX_PICK_ACCEL);
CustomKinematicControllerSetMaxAngularFriction (bodyPickController, MAX_PICK_ACCEL * 5.0f);
}
}
}
if (mousePickMode) {
// init pick mode
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
dVector p (p0 + (p1 - p0).Scale (pickedParam));
if (bodyPickController) {
//bodyPickController->SetTargetPosit (p);
CustomKinematicControllerSetTargetPosit (bodyPickController, &p[0]);
}
// rotate normal to global space
dMatrix matrix;
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
rayWorldNormal = matrix.RotateVector(rayLocalNormal);
dVector p2 (matrix.TransformVector (attachmentPoint));
ShowMousePicking (p, p2, witdh);
ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length), witdh);
}
} else {
mousePickMode = false;
if (pickedBody) {
if (bodyPickController) {
//delete bodyPickController;
CustomDestroyJoint (bodyPickController);
}
bodyPickController = NULL;
}
}
mouse0 = mouse1;
mouseLeftKey0 = mouseLeftKey1;
bool retState;
retState = isPickedBodyDynamics;
return retState;
}
void CreateScene (NewtonWorld* world, SceneManager* sceneManager)
{
Entity* floor;
Entity* smilly;
Entity* frowny;
NewtonBody* floorBody;
NewtonBody* smillyBody;
NewtonBody* frownyBody;
NewtonCollision* shape;
// Create the material for this scene
CreateMateials (world, sceneManager);
// Create a large body to be the floor
floor = sceneManager->CreateEntity();
// add scene collision from a level mesh
shape = CreateHeightFieldCollision (world, "h2.raw", 0);
floorBody = CreateRigidBody (world, floor, shape, 0.0f);
NewtonReleaseCollision (world, shape);
// make a visual mesh for the collision data
CreateHeightFieldMesh (shape, floor);
// set the matrix at the origin
dVector boxP0;
dVector boxP1;
dMatrix matrix (floor->m_curRotation, floor->m_curPosition);
NewtonCollisionCalculateAABB (shape, &matrix[0][0], &boxP0.m_x, &boxP1.m_x);
// place the origin of the visual mesh at the center of the height field
matrix.m_posit = (boxP0 + boxP1).Scale (-0.5f);
matrix.m_posit.m_w = 1.0f;
floor->m_curPosition = matrix.m_posit;
floor->m_prevPosition = matrix.m_posit;
// relocate the body;
NewtonBodySetMatrix (floorBody, &matrix[0][0]);
// now we will use the properties of this body to set a proper world size.
NewtonCollisionCalculateAABB (shape, &matrix[0][0], &boxP0.m_x, &boxP1.m_x);
// add some extra padding
boxP0.m_x -= 50.0f;
boxP0.m_y -= 500.0f;
boxP0.m_z -= 50.0f;
boxP1.m_x += 50.0f;
boxP1.m_y += 500.0f;
boxP1.m_z += 50.0f;
// set the new world size
NewtonSetWorldSize (world, &boxP0[0], &boxP1[0]);
// assign an Material ID to this body
NewtonBodySetMaterialGroupID (floorBody, g_floorMaterial);
// add some visual entities.
dFloat y0 = FindFloor (world, 0.0f, 0.0f) + 10.0f;
for (int i = 0; i < 5; i ++) {
smilly = sceneManager->CreateEntity();
smilly->LoadMesh ("Smilly.dat");
smilly->m_curPosition.m_y = y0;
y0 += 2.0f;
smilly->m_prevPosition = smilly->m_curPosition;
// add a body with a box shape
shape = CreateNewtonBox (world, smilly, 0);
smillyBody = CreateRigidBody (world, smilly, shape, 10.0f);
NewtonReleaseCollision (world, shape);
// assign an Material ID to this body
NewtonBodySetMaterialGroupID (smillyBody, g_metalMaterial);
}
// add some visual entities.
y0 = FindFloor (world, 0.0f, 0.4f) + 10.5f;
for (int i = 0; i < 5; i ++) {
frowny = sceneManager->CreateEntity();
frowny->LoadMesh ("Frowny.dat");
frowny->m_curPosition.m_z = 0.4f;
frowny->m_curPosition.m_y = y0;
y0 += 2.0f;
frowny->m_prevPosition = frowny->m_curPosition;
// add a body with a Convex hull shape
shape = CreateNewtonConvex (world, frowny, 0);
frownyBody = CreateRigidBody (world, frowny, shape, 10.0f);
NewtonReleaseCollision (world, shape);
// assign an Material ID to this body
NewtonBodySetMaterialGroupID (frownyBody, g_woodMaterial);
}
// set the Camera EyePoint close to the scene action
InitCamera (dVector (-15.0f, FindFloor (world, -15.0f, 0.0f) + 5.0f, 0.0f), dVector (1.0f, 0.0f, 0.0f));
}
// create a simple vehicle
void BuidlBasicCar (const BasciCarParameters& parameters)
{
// step one: find the location of each tire, in the visual mesh and add them one by one to the vehicle controller
dFloat width = 0.35f;
dFloat radius = 0.5f;
// Muscle cars have the front engine, we need to shift the center of mass to the front to represent that
m_controller->SetCenterOfGravity (dVector (0.0f, parameters.COM_Y_OFFSET, 0.0f, 0.0f));
// add left tires
CustomVehicleControllerBodyStateTire* leftTire[2];
dVector offset (1.5f, 0.0f, -1.0f, 1.0f);
leftTire[0] = AddTire (offset, width, radius, parameters.TIRE_MASS, parameters.SUSPENSION_LENGTH, parameters.SUSPENSION_SPRING, parameters.SUSPENSION_DAMPER, parameters.LATERAL_STIFFNESS, parameters.LONGITUDINAL_STIFFNESS, parameters.ALIGNING_MOMENT_TRAIL, parameters.m_tireaLigment);
offset = dVector (-1.7f, 0.0f, -1.0f, 1.0f);
leftTire[1] = AddTire (offset, width, radius, parameters.TIRE_MASS, parameters.SUSPENSION_LENGTH, parameters.SUSPENSION_SPRING, parameters.SUSPENSION_DAMPER, parameters.LATERAL_STIFFNESS, parameters.LONGITUDINAL_STIFFNESS, parameters.ALIGNING_MOMENT_TRAIL, parameters.m_tireaLigment);
// add right tires
CustomVehicleControllerBodyStateTire* rightTire[2];
offset = dVector (1.5f, 0.0f, 1.0f, 1.0f);
rightTire[0] = AddTire (offset, width, radius, parameters.TIRE_MASS, parameters.SUSPENSION_LENGTH, parameters.SUSPENSION_SPRING, parameters.SUSPENSION_DAMPER, parameters.LATERAL_STIFFNESS, parameters.LONGITUDINAL_STIFFNESS, parameters.ALIGNING_MOMENT_TRAIL, parameters.m_tireaLigment);
offset = dVector (-1.7f, 0.0f, 1.0f, 1.0f);
rightTire[1] = AddTire (offset, width, radius, parameters.TIRE_MASS, parameters.SUSPENSION_LENGTH, parameters.SUSPENSION_SPRING, parameters.SUSPENSION_DAMPER, parameters.LATERAL_STIFFNESS, parameters.LONGITUDINAL_STIFFNESS, parameters.ALIGNING_MOMENT_TRAIL, parameters.m_tireaLigment);
// add a steering Wheel
CustomVehicleControllerComponentSteering* const steering = new CustomVehicleControllerComponentSteering (m_controller, parameters.STEER_ANGLE * 3.141592f / 180.0f);
steering->AddSteeringTire (leftTire[0]);
steering->AddSteeringTire (rightTire[0]);
m_controller->SetSteering(steering);
// add all wheels brakes
CustomVehicleControllerComponentBrake* const brakes = new CustomVehicleControllerComponentBrake (m_controller, parameters.BRAKE_TORQUE);
for (int i = 0; i < 2; i ++) {
brakes->AddBrakeTire (leftTire[i]);
brakes->AddBrakeTire (rightTire[i]);
}
m_controller->SetBrakes(brakes);
// add hand brakes
CustomVehicleControllerComponentBrake* const handBrakes = new CustomVehicleControllerComponentBrake (m_controller, parameters.BRAKE_TORQUE);
handBrakes->AddBrakeTire (leftTire[1]);
handBrakes->AddBrakeTire (rightTire[1]);
m_controller->SetHandBrakes (handBrakes);
// add an engine
CustomVehicleControllerComponentEngine::dMultiAxelDifferential* differencial = NULL;
switch (parameters.m_differentialType)
{
case BasciCarParameters::m_4WD:
{
CustomVehicleControllerComponentEngine::dSingleAxelDifferential* axles[2];
for (int i = 0; i < 2; i ++) {
axles[i] = new CustomVehicleControllerComponentEngine::dSingleAxelDifferential (m_controller, leftTire[i], rightTire[i]);
}
differencial = new CustomVehicleControllerComponentEngine::dMultiAxelDifferential (m_controller, 2, axles);
break;
}
case BasciCarParameters::m_FWD:
{
CustomVehicleControllerComponentEngine::dSingleAxelDifferential* axles[1];
axles[0] = new CustomVehicleControllerComponentEngine::dSingleAxelDifferential (m_controller, leftTire[0], rightTire[0]);
differencial = new CustomVehicleControllerComponentEngine::dMultiAxelDifferential (m_controller, 1, axles);
break;
}
case BasciCarParameters::m_RWD:
default:
{
CustomVehicleControllerComponentEngine::dSingleAxelDifferential* axles[1];
axles[0] = new CustomVehicleControllerComponentEngine::dSingleAxelDifferential (m_controller, leftTire[1], rightTire[1]);
differencial = new CustomVehicleControllerComponentEngine::dMultiAxelDifferential (m_controller, 1, axles);
break;
}
}
dFloat fowardSpeedGearsBoxRatios[] = {parameters.GEAR_1, parameters.GEAR_1, parameters.GEAR_3};
CustomVehicleControllerComponentEngine::dGearBox* const gearBox = new CustomVehicleControllerComponentEngine::dGearBox(m_controller, parameters.REVERSE_GEAR, sizeof (fowardSpeedGearsBoxRatios) / sizeof (fowardSpeedGearsBoxRatios[0]), fowardSpeedGearsBoxRatios);
CustomVehicleControllerComponentEngine* const engine = new CustomVehicleControllerComponentEngine (m_controller, gearBox, differencial);
// the the default transmission type
engine->SetTransmissionMode(m_automaticTransmission.GetPushButtonState());
m_controller->SetEngine(engine);
dFloat viperIdleRPM = parameters.IDLE_TORQUE_RPM;
dFloat viperIdleTorquePoundPerFoot = parameters.IDLE_TORQUE;
dFloat viperPeakTorqueRPM = parameters.PEAK_TORQUE_RPM;
dFloat viperPeakTorquePoundPerFoot = parameters.PEAK_TORQUE;
dFloat viperPeakHorsePowerRPM = parameters.PEAK_HP_RPM;
dFloat viperPeakHorsePower = parameters.PEAK_HP;
dFloat viperRedLineRPM = parameters.REDLINE_TORQUE_RPM;
dFloat viperRedLineTorquePoundPerFoot = parameters.REDLINE_TORQUE;
dFloat vehicleTopSpeedKPH = parameters.TIRE_TOP_SPEED_KMH;
engine->InitEngineTorqueCurve (vehicleTopSpeedKPH, viperIdleTorquePoundPerFoot, viperIdleRPM, viperPeakTorquePoundPerFoot, viperPeakTorqueRPM, viperPeakHorsePower, viperPeakHorsePowerRPM, viperRedLineTorquePoundPerFoot, viperRedLineRPM);
// do not forget to call finalize after all components are added or after any change is made to the vehicle
m_controller->Finalize();
/*
// test tire update interface
for (CustomVehicleControllerBodyStateTire* node = m_controller->GetFirstTire(); node; node = m_controller->GetNextTire(node)) {
CustomVehicleControllerBodyStateTire& tire = *node;
CustomVehicleControllerBodyStateTire::TireCreationInfo tireInfo;
tire.GetInfo(tireInfo);
tireInfo.m_location.m_y += 0.1f;
tire.UpdateInfo(tireInfo);
tire.GetInfo(tireInfo);
tire.GetInfo(tireInfo);
}
m_controller->Finalize();
for (CustomVehicleControllerBodyStateTire* node = m_controller->GetFirstTire(); node; node = m_controller->GetNextTire(node)) {
CustomVehicleControllerBodyStateTire& tire = *node;
CustomVehicleControllerBodyStateTire::TireCreationInfo tireInfo;
tire.GetInfo(tireInfo);
tire.GetInfo(tireInfo);
}
*/
}
BasicCarEntity (DemoEntityManager* const scene, CustomVehicleControllerManager* const manager, const dMatrix& location, const BasciCarParameters& parameters)
:DemoEntity (dGetIdentityMatrix(), NULL)
,m_tireaLigmentMatrix (dYawMatrix(3.141592f * 90.0f / 180.0f))
,m_controller(NULL)
,m_helpKey (true)
,m_gearUpKey (false)
,m_gearDownKey (false)
,m_reverseGear (false)
,m_engineKeySwitch(false)
,m_automaticTransmission(true)
,m_engineKeySwitchCounter(0)
,m_engineOldKeyState(false)
,m_engineRPMOn(false)
{
// add this entity to the scene for rendering
scene->Append(this);
// place entity in the world
ResetMatrix (*scene, location);
NewtonWorld* const world = scene->GetNewton();
// create the vehicle collision shape
NewtonCollision* const chassisCollision = CreateChassisCollision (world);
// caret the visual mesh form the collision shape
DemoMesh* const visualMesh = new DemoMesh ("vehicle chassis", chassisCollision, "metal_30.tga", "metal_30.tga", "metal_30.tga");
SetMesh (visualMesh, dGetIdentityMatrix());
visualMesh->Release();
// create the coordinate system
dMatrix chassisMatrix;
chassisMatrix.m_front = dVector (1.0f, 0.0f, 0.0f, 0.0f); // this is the vehicle direction of travel
chassisMatrix.m_up = dVector (0.0f, 1.0f, 0.0f, 0.0f); // this is the downward vehicle direction
chassisMatrix.m_right = chassisMatrix.m_front * chassisMatrix.m_up; // this is in the side vehicle direction (the plane of the wheels)
chassisMatrix.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// create a default vehicle controller
m_controller = manager->CreateVehicle (chassisCollision, chassisMatrix, parameters.MASS, dVector (0.0f, DEMO_GRAVITY, 0.0f, 0.0f));
// get body the vehicle rigid body and set the Newton rigid body physics properties
NewtonBody* const body = m_controller->GetBody();
// set the user data
NewtonBodySetUserData(body, this);
// set the transform callback
NewtonBodySetTransformCallback (body, DemoEntity::TransformCallback);
// set the standard force and torque call back
NewtonBodySetForceAndTorqueCallback(body, PhysicsApplyGravityForce);
// set the player matrix
NewtonBodySetMatrix(body, &location[0][0]);
// destroy the collision helper shape
NewtonDestroyCollision(chassisCollision);
// map the gear to a look up table: gear 0 is reverse, gea 1 is neutral, gear 1 is first, gear 2 is second and so on
for (int i = 0; i < int ((sizeof (m_gearMap) / sizeof (m_gearMap[0]))); i ++) {
m_gearMap[i] = i;
}
m_gearMap[0] = 1;
m_gearMap[1] = 0;
}
void RenderJointsDebugInfo (NewtonWorld* const world, dFloat size)
{
glDisable(GL_TEXTURE_2D);
glDisable (GL_LIGHTING);
glBegin(GL_LINES);
// this will go over the joint list twice,
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) {
NewtonJointRecord info;
NewtonJointGetInfo (joint, &info);
if (strcmp (info.m_descriptionType, "customJointNotInfo")) {
// draw first frame
dMatrix matrix0;
NewtonBodyGetMatrix (info.m_attachBody_0, &matrix0[0][0]);
matrix0 = dMatrix (&info.m_attachmenMatrix_0[0][0]) * matrix0;
dVector o0 (matrix0.m_posit);
dVector x (o0 + matrix0.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f)));
glColor3f (1.0f, 0.0f, 0.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (x.m_x, x.m_y, x.m_z);
dVector y (o0 + matrix0.RotateVector (dVector (0.0f, size, 0.0f, 0.0f)));
glColor3f (0.0f, 1.0f, 0.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (y.m_x, y.m_y, y.m_z);
dVector z (o0 + matrix0.RotateVector (dVector (0.0f, 0.0f, size, 0.0f)));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (z.m_x, z.m_y, z.m_z);
// draw second frame
dMatrix matrix1 (dGetIdentityMatrix());
if (info.m_attachBody_1) {
NewtonBodyGetMatrix (info.m_attachBody_1, &matrix1[0][0]);
}
matrix1 = dMatrix (&info.m_attachmenMatrix_1[0][0]) * matrix1;
dVector o1 (matrix1.m_posit);
x = o1 + matrix1.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f));
glColor3f (1.0f, 0.0f, 0.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (x.m_x, x.m_y, x.m_z);
y = o1 + matrix1.RotateVector (dVector (0.0f, size, 0.0f, 0.0f));
glColor3f (0.0f, 1.0f, 0.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (y.m_x, y.m_y, y.m_z);
z = o1 + matrix1.RotateVector (dVector (0.0f, 0.0f, size, 0.0f));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (o1.m_x, o1.m_y, o1.m_z);
glVertex3f (z.m_x, z.m_y, z.m_z);
if (!strcmp (info.m_descriptionType, "limitballsocket")) {
// draw the cone limit of this joint
int steps = 12;
dMatrix coneMatrix (dRollMatrix(info.m_maxAngularDof[1]));
dMatrix ratationStep (dPitchMatrix(2.0f * 3.14151693f / steps));
dVector p0 (coneMatrix.RotateVector(dVector (size * 0.5f, 0.0f, 0.0f, 0.0f)));
dVector q0 (matrix1.TransformVector(p0));
glColor3f (1.0f, 1.0f, 0.0f);
for (int i = 0; i < (steps + 1); i ++) {
dVector p1 (ratationStep.RotateVector(p0));
dVector q1 (matrix1.TransformVector(p1));
glVertex3f (o0.m_x, o0.m_y, o0.m_z);
glVertex3f (q0.m_x, q0.m_y, q0.m_z);
glVertex3f (q0.m_x, q0.m_y, q0.m_z);
glVertex3f (q1.m_x, q1.m_y, q1.m_z);
p0 = p1;
q0 = q1;
}
}
}
}
}
glEnd();
}
void CustomDGRayCastCar::AddSingleSuspensionTire (
void *userData,
const dVector& localPosition,
dFloat mass,
dFloat radius,
dFloat width,
dFloat friction,
dFloat suspensionLenght,
dFloat springConst,
dFloat springDamper,
int castMode)
{
dVector relTirePos = localPosition;
suspensionLenght = dAbs ( suspensionLenght );
dMatrix chassisMatrix;
NewtonBodyGetMatrix (m_body0, &chassisMatrix[0][0]);
chassisMatrix = chassisMatrix * chassisMatrix;
relTirePos += chassisMatrix.m_up.Scale ( suspensionLenght );
m_tires[m_tiresCount].m_harpoint = m_localFrame.UntransformVector( relTirePos );
m_tires[m_tiresCount].m_localAxis = dVector (0.0f, 0.0f, 1.0f, 0.0f);
m_tires[m_tiresCount].m_tireAxelPosit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_tireAxelVeloc = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_lateralPin = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_longitudinalPin = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_HitBody = NULL;
m_tires[m_tiresCount].m_userData = userData;
m_tires[m_tiresCount].m_spinAngle = 0.0f;
m_tires[m_tiresCount].m_steerAngle = 0.0f;
m_tires[m_tiresCount].m_tireLoad = 0.0f;
m_tires[m_tiresCount].m_posit = suspensionLenght;
m_tires[m_tiresCount].m_tireIsOnAir = 1;
m_tires[m_tiresCount].m_tireUseConvexCastMode = castMode;
m_tires[m_tiresCount].m_springConst = springConst;
m_tires[m_tiresCount].m_springDamper = springDamper;
m_tires[m_tiresCount].m_suspensionLenght = suspensionLenght;
m_tires[m_tiresCount].m_angularVelocity = 0.0f;
m_tires[m_tiresCount].m_breakForce = 0.0f;
m_tires[m_tiresCount].m_torque = 0.0f;
m_tires[m_tiresCount].m_groundFriction = friction;
m_tires[m_tiresCount].m_tireIsConstrained = 0;
m_tires[m_tiresCount].m_mass = mass;
m_tires[m_tiresCount].m_width = width;
m_tires[m_tiresCount].m_radius = radius;
m_tires[m_tiresCount].m_Ixx = mass * radius * radius / 2.0f;
m_tires[m_tiresCount].m_IxxInv = 1.0f / m_tires[m_tiresCount].m_Ixx;
#define TIRE_SHAPE_SIZE 12
dVector shapePoints[TIRE_SHAPE_SIZE * 2];
for ( int i = 0; i < TIRE_SHAPE_SIZE; i ++ ) {
shapePoints[i].m_x = -width * 0.5f;
shapePoints[i].m_y = radius * dCos ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
shapePoints[i].m_z = radius * dSin ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
shapePoints[i + TIRE_SHAPE_SIZE].m_x = -shapePoints[i].m_x;
shapePoints[i + TIRE_SHAPE_SIZE].m_y = shapePoints[i].m_y;
shapePoints[i + TIRE_SHAPE_SIZE].m_z = shapePoints[i].m_z;
}
m_tires[m_tiresCount].m_shape = NewtonCreateConvexHull ( m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, 0, NULL );
// NewtonCreateChamferCylinder(m_world,radius,width,NULL);
// NewtonCreateSphere(m_world,radius,radius,radius,&offmat[0][0]);
// NewtonCreateCone(m_world,radius,width,NULL);
// NewtonCreateCapsule(m_world,radius,width,NULL);
// NewtonCreateChamferCylinder(m_world,radius,width,NULL);
// NewtonCreateCylinder(m_world,radius*2,width*2,NULL);
// NewtonCreateBox(m_world,radius*2,radius*2,radius*2,NULL);
// NewtonCreateConvexHull (m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, NULL);
m_tiresCount ++;
}
void Restitution (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
NewtonWorld* const world = scene->GetNewton();
dMatrix offsetMatrix (dGetIdentityMatrix());
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
NewtonMaterialSetCollisionCallback (world, defaultMaterialID, defaultMaterialID, NULL, UserContactRestitution);
CreateLevelMesh (scene, "flatPlane.ngd", 0);
dVector location (0.0f, 0.0f, 0.0f, 0.0f);
dVector size (0.5f, 0.5f, 1.0f, 0.0f);
// create some spheres
dVector sphSize (1.0f, 1.0f, 1.0f, 0.0f);
NewtonCollision* const sphereCollision = CreateConvexCollision (world, offsetMatrix, sphSize, _SPHERE_PRIMITIVE, 0);
DemoMesh* const sphereMesh = new DemoMesh("sphere", sphereCollision, "smilli.tga", "smilli.tga", "smilli.tga");
// create some boxes too
dVector boxSize (1.0f, 0.5f, 2.0f, 0.0f);
NewtonCollision* const boxCollision = CreateConvexCollision (world, offsetMatrix, boxSize, _BOX_PRIMITIVE, 0);
DemoMesh* const boxMesh = new DemoMesh("box", boxCollision, "smilli.tga", "smilli.tga", "smilli.tga");
int zCount = 10;
dFloat spacing = 4.0f;
dMatrix matrix (dGetIdentityMatrix());
dVector origin (matrix.m_posit);
origin.m_x -= 0.0f;
// create a simple scene
for (int i = 0; i < zCount; i ++) {
dFloat z;
dFloat x;
dFloat mass;
dVector size (1.0f, 0.5f, 2.0f, 0.0f);
x = origin.m_x;
z = origin.m_z + (i - zCount / 2) * spacing;
mass = 1.0f;
matrix.m_posit = FindFloor (world, dVector (x, 100.0f, z), 200.0f);
matrix.m_posit.m_w = 1.0f;
float restitution;
NewtonBody* body;
NewtonCollision* collision;
matrix.m_posit.m_y += 4.0f;
body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
matrix.m_posit.m_y += 4.0f;
//body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID, shapeOffsetMatrix);
body = CreateSimpleSolid (scene, boxMesh, mass, matrix, boxCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
matrix.m_posit.m_y += 4.0f;
body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
matrix.m_posit.m_y += 4.0f;
dVector boxSize (1.0f, 0.5f, 2.0f, 0.0f);
body = CreateSimpleSolid (scene, boxMesh, mass, matrix, boxCollision, defaultMaterialID);
NewtonBodySetLinearDamping (body, 0.0f);
collision = NewtonBodyGetCollision(body);
restitution = i * 0.1f + 0.083f;
NewtonCollisionSetUserData (collision, *((void**)&restitution));
}
boxMesh->Release();
sphereMesh->Release();
NewtonDestroyCollision(boxCollision);
NewtonDestroyCollision(sphereCollision);
dMatrix camMatrix (dGetIdentityMatrix());
dQuaternion rot (camMatrix);
origin = dVector (-25.0f, 5.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
}
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);
}
int dRuntimeProfiler::Render (int mask, int lineNumber)
{
struct GLViewPort
{
int x;
int y;
int width;
int height;
} viewport;
//Retrieves the viewport and stores it in the variable
glGetIntegerv(GL_VIEWPORT, (GLint*) &viewport.x);
m_width = viewport.width;
m_height = viewport.height;
NewtonWorld* const world = m_scene->GetNewton();
for (int i = 0; i < MAX_TRACKS; i ++) {
m_perfomanceTracks[i][m_frameIndex] = NewtonReadPerformanceTicks (world, i);
}
glColor3f(1.0, 1.0, 1.0);
glDisable (GL_LIGHTING);
glDisable(GL_TEXTURE_2D);
glMatrixMode(GL_TEXTURE);
glPushMatrix();
glLoadIdentity();
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluOrtho2D(0, viewport.width, 0, viewport.height );
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glDisable(GL_DEPTH_TEST);
glDisable(GL_BLEND);
dFloat x0 = dFloat (m_oringin_x);
dFloat y0 = dFloat (m_oringin_y);
dFloat x1 = x0 + MAX_FRAMES * MAX_FRAMES_STEP;
dFloat y1 = y0 + CHART_HIEGHT;
glBegin(GL_LINES);
glVertex3f (x0, y0, 0.0f);
glVertex3f (x0, y1, 0.0f);
glVertex3f (x0, y0, 0.0f);
glVertex3f (x1, y0, 0.0f);
for (int i = 1; i < 4; i ++) {
dFloat y = y0 + (y1 - y0) * i / 4;
glVertex3f (x0 - 5, y, 0.0f);
glVertex3f (x0 + 5, y, 0.0f);
}
for (int i = 1; i < MAX_FRAMES; i += 16) {
dFloat x = x0 + (x1 - x0) * i / MAX_FRAMES;
glVertex3f (x , y0 - 5, 0.0f);
glVertex3f (x , y0 + 5, 0.0f);
}
glEnd();
DrawLabel (10, m_height - m_nextLine, "Profiler legend");
m_nextLine = lineNumber;
// total engine time
if (mask & 1) {
DrawLabel (10, m_height - m_nextLine, "white chart: world global update");
DrawTrack (x0, y0, dVector (1.0f, 1.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_WORLD_UPDATE][0]);
m_nextLine += 20;
}
// draw collision performance
if (mask & 2) {
DrawLabel (10, m_height - m_nextLine, "red chart: collision global update");
DrawTrack (x0, y0, dVector (1.0f, 0.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_COLLISION_UPDATE][0]);
m_nextLine += 20;
}
if (mask & 4) {
DrawLabel (10, m_height - m_nextLine, "green chart: collision broad phase update");
DrawTrack (x0, y0, dVector (0.0f, 1.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_COLLISION_UPDATE_BROAD_PHASE][0]);
m_nextLine += 20;
}
if (mask & 8) {
DrawLabel (10, m_height - m_nextLine, "blue chart: collision narrow phase update");
DrawTrack (x0, y0, dVector (0.0f, 0.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_COLLISION_UPDATE_NARROW_PHASE][0]);
m_nextLine += 20;
}
// draw dynamics performance
if (mask & 16) {
DrawLabel (10, m_height - m_nextLine, "cyan chart: dynamics global update");
DrawTrack (x0, y0, dVector (0.0f, 1.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_DYNAMICS_UPDATE][0]);
m_nextLine += 20;
}
if (mask & 32) {
DrawLabel (10, m_height - m_nextLine, "black chart: dynamics solver update");
DrawTrack (x0, y0, dVector (0.0f, 0.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_DYNAMICS_CONSTRAINT_GRAPH][0]);
m_nextLine += 20;
}
if (mask & 64) {
DrawLabel (10, m_height - m_nextLine, "yellow chart: dynamics solver update");
DrawTrack (x0, y0, dVector (1.0f, 1.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_DYNAMICS_SOLVE_CONSTRAINT_GRAPH][0]);
m_nextLine += 20;
}
// draw force Update performance
if (mask & 128) {
DrawLabel (10, m_height - m_nextLine, "magenta chart: force and torque callback update");
DrawTrack (x0, y0, dVector (1.0f, 0.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_FORCE_CALLBACK_UPDATE][0]);
m_nextLine += 20;
}
if (mask & 256) {
DrawLabel (10, m_height - m_nextLine, "magenta chart: pre simulation listener");
DrawTrack (x0, y0, dVector (1.0f, 0.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_FORCE_CALLBACK_UPDATE][0]);
m_nextLine += 20;
}
if (mask & 256) {
DrawLabel (10, m_height - m_nextLine, "purple chart: pre simulation listener");
DrawTrack (x0, y0, dVector (0.64f, 0.29f, 0.64f), m_frameIndex, &m_perfomanceTracks[NEWTON_PRE_LISTERNER_CALLBACK_UPDATE][0]);
m_nextLine += 20;
}
if (mask & 512) {
DrawLabel (10, m_height - m_nextLine, "pink chart: post simulation listener");
DrawTrack (x0, y0, dVector (1.0f, 0.68f, 0.79f), m_frameIndex, &m_perfomanceTracks[NEWTON_POST_LISTERNER_CALLBACK_UPDATE][0]);
m_nextLine += 20;
}
{
int base_y = 0;
glColor3f(1.0, 1.0, 1.0);
DrawLabel (x0 - 30, y0 + (y1 - y0) * 0 / 4 + base_y, "0");
for (int i = 1; i < 5; i ++) {
char label[32];
sprintf (label, "%4.2f", (1000.0f / 60.0f) * (float)i / 4.0f );
DrawLabel (x0 - 55, y0 + (y1 - y0) * i / 4 + base_y, label);
}
}
glMatrixMode(GL_TEXTURE);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glEnable( GL_DEPTH_TEST );
m_frameIndex = (m_frameIndex + 1) % MAX_FRAMES;
glColor3f(1.0, 1.0, 1.0);
return m_nextLine + 20;
}
void ConvexApproximationObject::BuildMesh()
{
// since max does no provide the iNode that will own this mesh I have no choice bu to apply the root matrix to all vertex
ConvexApproximationClassDesc* const desc = (ConvexApproximationClassDesc*) ConvexApproximationClassDesc::GetDescriptor();
INode* const sourceNode = desc->m_sourceNode;
//dMatrix rootMatrix1 (GetMatrixFromMaxMatrix (sourceNode->GetNodeTM (0)));
dMatrix rootMatrix (GetMatrixFromMaxMatrix (sourceNode->GetObjectTM(0)));
dVector scale;
dMatrix stretchAxis;
dMatrix orthogonalRootTransform;
rootMatrix.PolarDecomposition (orthogonalRootTransform, scale, stretchAxis);
orthogonalRootTransform = orthogonalRootTransform.Inverse();
// create a Newton world, as a manager of everything Newton related stuff
NewtonWorld* const world = NewtonCreate ();
// create an empty mesh and load the max mesh to it
NewtonMesh* const sourceMesh = NewtonMeshCreate (world);
// load all faces
NewtonMeshBeginFace(sourceMesh);
LoadGeometries (sourceMesh, orthogonalRootTransform);
NewtonMeshEndFace(sourceMesh);
// make a convex approximation form this newton mesh effect
desc->m_progress = -1;
Interface* const inteface = desc->m_currentInterface;
inteface->ProgressStart("Creation Convex approx ...", TRUE, ConvexApproximationClassDesc::ReportMaxProgress, NULL);
NewtonMesh* approximationMesh = NewtonMeshApproximateConvexDecomposition (sourceMesh, m_currentConcavity, 0.2f, m_currentMaxCount, 1000, ConvexApproximationClassDesc::ReportProgress);
inteface->ProgressEnd();
NewtonMeshDestroy (sourceMesh);
// now convert the new mesh to a max poly Object
MNMesh& maxMesh = GetMesh();
maxMesh.ClearAndFree();
int faceCount = 0;
int vertexCount = NewtonMeshGetVertexCount(approximationMesh);
for (void* face = NewtonMeshGetFirstFace(approximationMesh); face; face = NewtonMeshGetNextFace(approximationMesh, face)) {
if (!NewtonMeshIsFaceOpen(approximationMesh, face)) {
faceCount ++;
}
}
//maxMesh.Clear();
maxMesh.setNumVerts(vertexCount);
maxMesh.setNumFaces(faceCount);
// add all vertex
int vertexStride = NewtonMeshGetVertexStrideInByte(approximationMesh) / sizeof (dFloat64);
dFloat64* const vertex = NewtonMeshGetVertexArray (approximationMesh);
for (int j = 0; j < vertexCount; j ++) {
dVector p (orthogonalRootTransform.TransformVector(dVector (float (vertex[vertexStride * j + 0]), float (vertex[vertexStride * j + 1]), float (vertex[vertexStride * j + 2]), float(1.0f))));
maxMesh.P(j) = Point3 (p.m_x, p.m_y, p.m_z);
}
// count the number of face and make a face map
int faceIndex = 0;
for (void* face = NewtonMeshGetFirstFace(approximationMesh); face; face = NewtonMeshGetNextFace(approximationMesh, face)) {
if (!NewtonMeshIsFaceOpen(approximationMesh, face)) {
int faceIndices[256];
int indexCount = NewtonMeshGetFaceIndexCount (approximationMesh, face);
NewtonMeshGetFaceIndices (approximationMesh, face, faceIndices);
MNFace* const face = maxMesh.F(faceIndex);
face->MakePoly(indexCount, faceIndices, NULL, NULL);
face->material = 0;
faceIndex ++;
}
}
maxMesh.InvalidateGeomCache();
maxMesh.InvalidateTopoCache();
maxMesh.FillInMesh();
maxMesh.AutoSmooth(45.0f * 3.1416f / 160.0f, false, false);
NewtonMeshDestroy (approximationMesh);
NewtonDestroy (world);
}
bool MousePick (NewtonWorld* nWorld, const dMOUSE_POINT& mouse1, dInt32 mouseLeftKey1, dFloat witdh, dFloat length)
{
static int mouseLeftKey0;
static dMOUSE_POINT mouse0;
static bool mousePickMode = false;
dMatrix matrix;
witdh;
if (mouseLeftKey1) {
if (!mouseLeftKey0) {
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
pickedBody = NULL;
pickedParam = 1.1f;
isPickedBodyDynamics = false;
NewtonWorldRayCast(nWorld, &p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
if (pickedBody) {
mousePickMode = true;
//NewtonBodySetFreezeState (pickedBody, 0);
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p (p0 + (p1 - p0).Scale (pickedParam));
// save point local to th body matrix
attachmentPoint = matrix.UntransformVector (p);
// convert normal to local space
rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);
// save the transform call back
chainForceCallback = NewtonBodyGetForceAndTorqueCallback (pickedBody);
dAssert (chainForceCallback != PhysicsApplyPickForce);
// set a new call back
NewtonBodySetForceAndTorqueCallback (pickedBody, PhysicsApplyPickForce);
}
}
if (mousePickMode) {
// init pick mode
dMatrix matrix;
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
dVector p2 (matrix.TransformVector (attachmentPoint));
dVector p (p0 + (p1 - p0).Scale (pickedParam));
pickedForce = p - p2;
dFloat mag2 = pickedForce % pickedForce;
if (mag2 > dFloat (20 * 20)) {
pickedForce = pickedForce.Scale (20.0f / dSqrt (pickedForce % pickedForce));
}
// rotate normal to global space
rayWorldNormal = matrix.RotateVector(rayLocalNormal);
// rayWorldOrigin = p2;
// show the pick points
//ShowMousePicking (p, p2, witdh);
ShowMousePicking (p, p2);
//ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length), witdh);
ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length));
}
} else {
mousePickMode = false;
if (pickedBody) {
//dAssert (chainForceCallback != NewtonBodyGetForceAndTorqueCallback (pickedBody));
dAssert (chainForceCallback != PhysicsApplyPickForce);
NewtonBodySetForceAndTorqueCallback (pickedBody, chainForceCallback);
pickedBody = NULL;
chainForceCallback = NULL;
}
}
mouse0 = mouse1;
mouseLeftKey0 = mouseLeftKey1;
bool retState;
retState = isPickedBodyDynamics;
return retState;
}
