// 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; }