// callback to apply external forces to body void ApplyForceAndTorqueCallback (const NewtonBody* body, float timestep, int threadIndex) { float Ixx; float Iyy; float Izz; float mass; // for this tutorial the only external force in the Gravity NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz); float y=0.0; float x=0.0; if(glfwGetKey(GLFW_KEY_UP)) y=1.0; if(glfwGetKey(GLFW_KEY_DOWN)) y=-1.0; if(glfwGetKey(GLFW_KEY_LEFT)) x=-1.0; if(glfwGetKey(GLFW_KEY_RIGHT)) x=1.0; glm::vec4 gravityForce (300.0f*x*mass, 300.0f*y*mass, mass * -100.0f, 1.0f); NewtonBodySetForce(body, &gravityForce[0]); }
CustomDGRayCastCar::CustomDGRayCastCar (int maxTireCount, const dMatrix& cordenateSytem, NewtonBody* carBody) :NewtonCustomJoint(2 * maxTireCount, carBody, NULL), m_normalizedLateralForce(), m_normalizedLongitudinalForce () { dVector com; dMatrix tmp; dFloat Ixx; dFloat Iyy; dFloat Izz; dMatrix chassisMatrix; NewtonBodyGetMassMatrix( m_body0, &m_mass, &Ixx, &Iyy, &Izz ); m_curSpeed = 0.0f; m_tiresCount = 0; m_vehicleOnAir = 0; m_steerAngle = 0.0f; //set default break force as a function of the vehicle weight //2 time the car weight assuming gravity is 10 m_maxBrakeForce = 2.0f * m_mass * 10.0f; m_maxSteerAngle = 30.0f * DefPO; m_maxSteerRate = 0.075f; m_engineSteerDiv = 100.0f; m_maxSteerForce = 6000.0f; m_maxSteerForceRate = 0.03f; m_maxSteerSpeedRestriction = 2.0f; // m_engineTireTorque = 0.0f; // m_maxEngineTorque = 6000.0f; // m_maxEngineTorqueRate = 500.0f; /* m_fixDeceleration = 0.9975f; m_engineTireTorque = 0.0f; m_maxBrakeForce = 350.0f; m_tiresRollSide = 0; m_engineTorqueDiv = 200.0f; // chassis rotation fix... m_chassisRotationLimit = 0.98f; */ // set the chassis matrix at the center of mass NewtonBodyGetCentreOfMass( m_body0, &com[0] ); com.m_w = 1.0f; // set the joint reference point at the center of mass of the body NewtonBodyGetMatrix (m_body0, &chassisMatrix[0][0]); //make sure the system matrix do not have any translations on it dMatrix cordenateSytemLocal (cordenateSytem); cordenateSytemLocal.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f); chassisMatrix.m_posit += chassisMatrix.RotateVector (com); chassisMatrix = cordenateSytemLocal * chassisMatrix; // set the car local coordinate system CalculateLocalMatrix ( chassisMatrix, m_localFrame, tmp ); // allocate space for the tires; m_tires = new Tire[maxTireCount]; // Create a simplified normalized Tire Curve // we will use a simple piece wise curve at this time, // but end application user can use advance cubers like the Pacejkas tire model dFloat slips[] = {0.0f, 0.3f, 0.5f, 2.0f}; dFloat normalizedLongitudinalForce[] = {0.0f, 1.0f, 0.9f, 0.7f}; m_normalizedLongitudinalForce.InitalizeCurve (sizeof (slips) / sizeof (dFloat), slips, normalizedLongitudinalForce); dFloat sideSlip[] = {0.1f, 0.4f, 0.5f, 2.0f}; dFloat normalizedLateralForce[] = {0.0f, 1.0f, 0.6f, 0.4f}; m_normalizedLateralForce.InitalizeCurve (sizeof (sideSlip) / sizeof (dFloat), sideSlip, normalizedLateralForce); // m_aerodynamicDrag = 0.1f; // m_aerodynamicDownForce = 0.1f; // set linear and angular Drag to zero, this joint will handle this by using Aerodynamic Drag; dVector drag (0.0f, 0.0f, 0.0f, 0.0f); NewtonBodySetLinearDamping (m_body0, 0.0f); NewtonBodySetAngularDamping (m_body0, &drag[0]); // register the callback for tire integration NewtonUserJointSetFeedbackCollectorCallback (m_joint, IntegrateTires); }
void CarBoxCallback(const NewtonBody* body, float timestep, int threadIndex) { float mass, ix, iy, iz; NewtonBodyGetMassMatrix(body, &mass, &ix, &iy, &iz); Vector4 gravityForce = Vector4(0.0f, mass * GRAVITY, 0.0f, 1.0f); // set gravity as a force //NewtonBodySetForce(body, (float*)&gravityForce); CMesh *object = (CMesh*)NewtonBodyGetUserData(body); Vector3 force; if (Keydown('w')) force = object->forward; if (Keydown('s')) force = -object->forward; //if (Keydown('a')) force -= object->right; //if (Keydown('d')) force += object->right; force *= cv_speed.GetFloat(); // speed //force.y = GRAVITY; force *= mass; /*float f[3]; NewtonBodyGetForce(body, &f[0]); Debug("BODY: %f %f %f", f[0], f[1], f[2]); Debug("FORCE: %f %f %f", force.x, force.y, force.z); */ NewtonBodySetForce(body, &force[0]); //NewtonBodyAddImpulse(body, &force[0], &object->GetWorldPosition()[0]); //NewtonBodySetVelocity(body, &force[0]); float torqueForce = cv_rspeed.GetFloat(); Vector3 torque; if (Keydown('a')) torque.y -= torqueForce; if (Keydown('d')) torque.y += torqueForce; // Damping has no effect at all /* static float damp = 0.5f; if (KeyPressed('p')) damp += 0.1f; if (KeyPressed('o')) damp -= 0.1f; Vector3 damping = Vector3(damp,damp,damp); damping *= damp; NewtonBodySetAngularDamping(body, &damping[0]); */ NewtonBodySetOmega(body, &torque[0]); //torque *= mass; //NewtonBodyAddTorque(body, &torque[0]); int sleep = NewtonBodyGetSleepState(body); object->color = sleep == 1 ? GREEN : RED; //Vector3 speed; //NewtonBodyGetVelocity(body, &speed[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 CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep) { dVector com; dMatrix matrix; dVector omega0; dVector veloc0; dVector omega1; dVector veloc1; dVector pointVeloc; const dFloat stiffness = 0.3f; const dFloat angularDamp = 0.95f; dFloat invTimeStep = 1.0f / timestep; NewtonWorld* const world = NewtonBodyGetWorld (body); NewtonWorldCriticalSectionLock (world, 0); // calculate the desired impulse NewtonBodyGetMatrix(body, &matrix[0][0]); NewtonBodyGetOmega (body, &omega0[0]); NewtonBodyGetVelocity (body, &veloc0[0]); NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]); dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace); deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc; for (int i = 0; i < 3; i ++) { dVector veloc (0.0f, 0.0f, 0.0f, 0.0f); veloc[i] = deltaVeloc[i]; NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]); } // damp angular velocity NewtonBodyGetOmega (body, &omega1[0]); NewtonBodyGetVelocity (body, &veloc1[0]); omega1 = omega1.Scale (angularDamp); // restore body velocity and angular velocity NewtonBodySetOmega (body, &omega0[0]); NewtonBodySetVelocity(body, &veloc0[0]); // convert the delta velocity change to a external force and torque dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz); dVector angularMomentum (Ixx, Iyy, Izz); angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0))); dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep)); dVector torque (angularMomentum.Scale(invTimeStep)); NewtonBodyAddForce(body, &force[0]); NewtonBodyAddTorque(body, &torque[0]); // make sure the body is unfrozen, if it is picked NewtonBodySetSleepState (body, 0); NewtonWorldCriticalSectionUnlock (world); }
void Body::getMassMatrix( Ogre::Real& mass, Ogre::Vector3& inertia ) { NewtonBodyGetMassMatrix( m_body, &mass, &inertia.x, &inertia.y, &inertia.z ); }
void cPhysicsBodyNewton::OnUpdateCallback(const NewtonBody* apBody, dFloat timestep, int threadIndex) { float fMass; float fX,fY,fZ; cPhysicsBodyNewton* pRigidBody = (cPhysicsBodyNewton*) NewtonBodyGetUserData(apBody); if(pRigidBody->IsActive()==false) return; cVector3f vGravity = pRigidBody->mpWorld->GetGravity(); //Create some gravity if (pRigidBody->mbGravity) { NewtonBodyGetMassMatrix(apBody, &fMass, &fX, &fY, &fZ); float fForce[3] = { fMass * vGravity.x, fMass * vGravity.y, fMass * vGravity.z}; NewtonBodyAddForce(apBody, &fForce[0]); } // Create Buoyancy if (pRigidBody->mBuoyancy.mbActive) { gSurfacePlane = pRigidBody->mBuoyancy.mSurface; NewtonBodyAddBuoyancyForce( apBody, pRigidBody->mBuoyancy.mfDensity, pRigidBody->mBuoyancy.mfLinearViscosity, pRigidBody->mBuoyancy.mfAngularViscosity, vGravity.v, BuoyancyPlaneCallback, pRigidBody); } // Add forces from calls to Addforce(..), etc NewtonBodyAddForce(apBody, pRigidBody->mvTotalForce.v); NewtonBodyAddTorque(apBody, pRigidBody->mvTotalTorque.v); // Check so that all speeds are within thresholds // Linear if (pRigidBody->mfMaxLinearSpeed > 0) { cVector3f vVel = pRigidBody->GetLinearVelocity(); float fSpeed = vVel.Length(); if (fSpeed > pRigidBody->mfMaxLinearSpeed) { vVel = cMath::Vector3Normalize(vVel) * pRigidBody->mfMaxLinearSpeed; pRigidBody->SetLinearVelocity(vVel); } } // Angular if (pRigidBody->mfMaxAngularSpeed > 0) { cVector3f vVel = pRigidBody->GetAngularVelocity(); float fSpeed = vVel.Length(); if (fSpeed > pRigidBody->mfMaxAngularSpeed) { vVel = cMath::Vector3Normalize(vVel) * pRigidBody->mfMaxAngularSpeed; pRigidBody->SetAngularVelocity(vVel); } } //cVector3f vForce; //NewtonBodyGetForce(apBody,vForce.v); //Log("Engine force %s\n",pRigidBody->mvTotalForce.ToString().c_str()); //Log("Engine force %s, body force: %s \n",pRigidBody->mvTotalForce.ToString().c_str(), // vForce.ToString().c_str()); }
void SimulationLister(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep) { m_delay --; if (m_delay > 0) { return; } // see if the net force on the body comes fr a high impact collision dFloat maxInternalForce = 0.0f; for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(m_myBody); joint; joint = NewtonBodyGetNextContactJoint(m_myBody, joint)) { for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) { //dVector point; //dVector normal; dVector contactForce; NewtonMaterial* const material = NewtonContactGetMaterial (contact); //NewtonMaterialGetContactPositionAndNormal (material, &point.m_x, &normal.m_x); NewtonMaterialGetContactForce(material, m_myBody, &contactForce[0]); dFloat forceMag = contactForce % contactForce; if (forceMag > maxInternalForce) { maxInternalForce = forceMag; } } } // if the force is bigger than 4 Gravities, It is considered a collision force dFloat maxForce = BREAK_FORCE_IN_GRAVITIES * m_myweight; if (maxInternalForce > (maxForce * maxForce)) { NewtonWorld* const world = NewtonBodyGetWorld(m_myBody); dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMassMatrix(m_myBody, &mass, &Ixx, &Iyy, &Izz); dVector com; dVector veloc; dVector omega; dMatrix bodyMatrix; NewtonBodyGetVelocity(m_myBody, &veloc[0]); NewtonBodyGetOmega(m_myBody, &omega[0]); NewtonBodyGetCentreOfMass(m_myBody, &com[0]); NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]); com = bodyMatrix.TransformVector (com); dMatrix matrix (GetCurrentMatrix()); dQuaternion rotation (matrix); for (ShatterEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) { ShatterAtom& atom = node->GetInfo(); DemoEntity* const entity = new DemoEntity (NULL); entity->SetMesh (atom.m_mesh); entity->SetMatrix(*scene, rotation, matrix.m_posit); entity->InterpolateMatrix (*scene, 1.0f); scene->Append(entity); int materialId = 0; dFloat debriMass = mass * atom.m_massFraction; dFloat Ixx = debriMass * atom.m_momentOfInirtia.m_x; dFloat Iyy = debriMass * atom.m_momentOfInirtia.m_y; dFloat Izz = debriMass * atom.m_momentOfInirtia.m_z; //create the rigid body NewtonBody* const rigidBody = NewtonCreateBody (world, atom.m_collision, &matrix[0][0]); // set the correct center of gravity for this body NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]); // calculate the center of mas of the debris dVector center (matrix.TransformVector(atom.m_centerOfMass)); // calculate debris initial velocity dVector v (veloc + omega * (center - com)); // set initial velocity NewtonBodySetVelocity(rigidBody, &v[0]); NewtonBodySetOmega(rigidBody, &omega[0]); // set the debrie center of mass NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]); // set the mass matrix NewtonBodySetMassMatrix (rigidBody, debriMass, Ixx, Iyy, Izz); // activate // NewtonBodyCoriolisForcesMode (blockBoxBody, 1); // save the pointer to the graphic object with the body. NewtonBodySetUserData (rigidBody, entity); // assign the wood id NewtonBodySetMaterialGroupID (rigidBody, materialId); // set continue collision mode // NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode); // set a destructor for this rigid body NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor); // set the transform call back function NewtonBodySetTransformCallback (rigidBody, DemoEntity::SetTransformCallback); // set the force and torque call back function NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce); } NewtonDestroyBody(world, m_myBody); scene->RemoveEntity (mynode); } };
static void CreateDebriPiece (const NewtonBody* sourceBody, NewtonMesh* mesh, dFloat volume) { dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; dFloat shapeVolume; NewtonWorld* world; NewtonBody* rigidBody; NewtonCollision* collision; OGLMesh* meshInstance; SceneManager* system; RenderPrimitive* primitive; dVector inertia; dVector origin; dVector veloc; dVector omega; dMatrix matrix; world = NewtonBodyGetWorld (sourceBody); NewtonBodyGetMatrix (sourceBody, &matrix[0][0]); NewtonBodyGetMassMatrix (sourceBody, &mass, &Ixx, &Iyy, &Izz); // make a visual object meshInstance = new OGLMesh(); meshInstance->BuildFromMesh (mesh); // create a visual geometry primitive = new RenderPrimitive (matrix, meshInstance); meshInstance->Release(); // save the graphics system system = (SceneManager*) NewtonWorldGetUserData(world); system->AddModel (primitive); collision = NewtonCreateConvexHullFromMesh (world, mesh, 0.1f, DEBRI_ID); // calculate the moment of inertia and the relative center of mass of the solid shapeVolume = NewtonConvexCollisionCalculateVolume (collision); NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]); mass = mass * shapeVolume / volume; Ixx = mass * inertia[0]; Iyy = mass * inertia[1]; Izz = mass * inertia[2]; //create the rigid body rigidBody = NewtonCreateBody (world, collision); // set the correct center of gravity for this body NewtonBodySetCentreOfMass (rigidBody, &origin[0]); // set the mass matrix NewtonBodySetMassMatrix (rigidBody, mass, Ixx, Iyy, Izz); // save the pointer to the graphic object with the body. NewtonBodySetUserData (rigidBody, primitive); // assign the wood id // NewtonBodySetMaterialGroupID (rigidBody, NewtonBodyGetMaterialGroupID(source)); // set continue collision mode NewtonBodySetContinuousCollisionMode (rigidBody, 1); // set a destructor for this rigid body NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor); // set the transform call back function NewtonBodySetTransformCallback (rigidBody, PhysicsSetTransform); // set the force and torque call back function NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce); // set the matrix for both the rigid body and the graphic body NewtonBodySetMatrix (rigidBody, &matrix[0][0]); PhysicsSetTransform (rigidBody, &matrix[0][0], 0); NewtonBodyGetVelocity(sourceBody, &veloc[0]); NewtonBodyGetOmega(sourceBody, &omega[0]); veloc += omega * matrix.RotateVector(origin); // for now so that I can see the body veloc = dVector (0, 0, 0, 0); // omega = dVector (0, 0, 0, 0); NewtonBodySetVelocity(rigidBody, &veloc[0]); NewtonBodySetOmega(rigidBody, &omega[0]); NewtonReleaseCollision(world, collision); }
void PrecessingTops (DemoEntityManager* const scene) { scene->CreateSkyBox(); // customize the scene after loading // set a user friction variable in the body for variable friction demos // later this will be done using LUA script dMatrix offsetMatrix (dGetIdentityMatrix()); CreateLevelMesh (scene, "flatPlane.ngd", 1); dVector location (0.0f, 0.0f, 0.0f, 0.0f); dVector size (3.0f, 2.0f, 0.0f, 0.0f); // create an array of cones const int count = 10; // all shapes use the x axis as the axis of symmetry, to make an upright cone we apply a 90 degree rotation local matrix dMatrix shapeOffsetMatrix (dRollMatrix(-3.141592f/2.0f)); AddPrimitiveArray(scene, 50.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, 0, shapeOffsetMatrix); // till the cont 30 degrees, and apply a local high angular velocity dMatrix matrix (dRollMatrix (-25.0f * 3.141592f / 180.0f)); dVector omega (0.0f, 50.0f, 0.0f); omega = matrix.RotateVector (omega); dVector damp (0.0f, 0.0f, 0.0f, 0.0f); int topscount = 0; NewtonBody* array[count * count]; NewtonWorld* const world = scene->GetNewton(); for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) { NewtonCollision* const collision = NewtonBodyGetCollision(body); dVector com; if (NewtonCollisionGetType (collision) == SERIALIZE_ID_CONE) { array[topscount] = body; topscount ++; } } for (int i = 0; i < topscount ; i ++) { dMatrix bodyMatrix; NewtonBody* const body = array[i]; NewtonBodyGetMatrix(body, &bodyMatrix[0][0]); matrix.m_posit = bodyMatrix.m_posit; matrix.m_posit.m_y += 1.0f; NewtonBodySetMatrix(body, &matrix[0][0]); dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz); NewtonBodySetMassMatrix(body, mass, Ixx, Iyy * 8.0f, Izz); NewtonBodySetOmega (body, &omega[0]); NewtonBodySetAutoSleep (body, 0); NewtonBodySetLinearDamping(body, 0.0f); NewtonBodySetAngularDamping (body, &damp[0]); } // place camera into position dMatrix camMatrix (dGetIdentityMatrix()); dQuaternion rot (camMatrix); dVector origin (-40.0f, 5.0f, 0.0f, 0.0f); scene->SetCameraMatrix(rot, origin); }