void dCustomPlayerController::PreUpdate(dFloat timestep) { dFloat timeLeft = timestep; const dFloat timeEpsilon = timestep * (1.0f / 16.0f); m_impulse = dVector(0.0f); m_manager->ApplyMove(this, timestep); //SetForwardSpeed(1.0f); //SetLateralSpeed(0.0f); //SetHeadingAngle(45.0f*dDegreeToRad); // set player orientation dMatrix matrix(dYawMatrix(GetHeadingAngle())); NewtonBodyGetPosition(m_kinematicBody, &matrix.m_posit[0]); NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]); // set play desired velocity dVector veloc(GetVelocity() + m_impulse.Scale(m_invMass)); NewtonBodySetVelocity(m_kinematicBody, &veloc[0]); // determine if player has to step over obstacles lower than step hight ResolveStep(timestep); // advance player until it hit a collision point, until there is not more time left for (int i = 0; (i < D_DESCRETE_MOTION_STEPS) && (timeLeft > timeEpsilon); i++) { if (timeLeft > timeEpsilon) { ResolveCollision(); } dFloat predicetdTime = PredictTimestep(timestep); NewtonBodyIntegrateVelocity(m_kinematicBody, predicetdTime); timeLeft -= predicetdTime; } }
void SpawnRandomProp(const dMatrix& location) const { NewtonWorld* const world = GetWorld(); DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world); //scene->SetCurrent(); static PrimitiveType proSelection[] = {_SPHERE_PRIMITIVE, _BOX_PRIMITIVE, _CAPSULE_PRIMITIVE, _CYLINDER_PRIMITIVE, _CONE_PRIMITIVE, _CHAMFER_CYLINDER_PRIMITIVE, _RANDOM_CONVEX_HULL_PRIMITIVE, _REGULAR_CONVEX_HULL_PRIMITIVE}; PrimitiveType type = PrimitiveType (dRand() % (sizeof (proSelection) / sizeof (proSelection[0]))); dVector size (0.35f, 0.25f, 0.25f, 0.0f); NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, type, 0); DemoMesh* const geometry = new DemoMesh("prop", collision, "smilli.tga", "smilli.tga", "smilli.tga"); dMatrix matrix (location); matrix.m_posit.m_y += 0.5f; NewtonBody* const prop = CreateSimpleSolid (scene, geometry, 30.0f, matrix, collision, 0); NewtonDestroyCollision(collision); geometry->Release(); dFloat initialSpeed = 20.0f; dVector veloc (matrix.m_front.Scale (initialSpeed)); NewtonBodySetVelocity(prop, &veloc[0]); NewtonBodySetLinearDamping(prop, 0); }
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~ void PhysicsActor::setLinearVelocity(const Math::Vector3& _velocity) { setActivationState(true); m_activationState = 1; NewtonBodySetVelocity(m_pActor, _velocity.m_array); }
void LaunchPuck() { if (!m_launched) { m_launched = true; NewtonInvalidateCache (NewtonBodyGetWorld(m_puckBody)); dVector zeros(0.0f, 0.0f, 0.0f, 0.0f); NewtonBodySetVelocity(m_puckBody, &zeros.m_x); NewtonBodySetOmega(m_puckBody, &zeros.m_x); NewtonBodySetForce(m_puckBody, &zeros.m_x); NewtonBodySetTorque(m_puckBody, &zeros.m_x); dVector vel(171.299469f, 0.0f, 0.0f); NewtonBodySetVelocity(m_puckBody, &vel.m_x); } }
void CustomDGRayCastCar::ApplyDeceleration (Tire& tire) { if ( dAbs( tire.m_torque ) < 1.0e-3f ){ dVector cvel = m_chassisVelocity; cvel = cvel.Scale( m_fixDeceleration ); cvel.m_y = m_chassisVelocity.m_y; NewtonBodySetVelocity( m_body0, &cvel.m_x ); } }
void CustomPlayerController::SetPlayerVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, dFloat headingAngle, const dVector& gravity, dFloat timestep) { dVector omega (CalculateDesiredOmega (headingAngle, timestep)); dVector veloc (CalculateDesiredVelocity (forwardSpeed, lateralSpeed, verticalSpeed, gravity, timestep)); NewtonBodySetOmega(m_body, &omega[0]); NewtonBodySetVelocity(m_body, &veloc[0]); if ((verticalSpeed > 0.0f)) { m_isJumping = true; } }
// create a mesh using the NewtonMesh low lever interface static NewtonBody* CreateSimpleBox_NewtonMesh (DemoEntityManager* const scene, const dVector& origin, const dVector& scale, dFloat mass) { dBigVector array[8]; dBigVector scale1 (scale); for (int i = 0; i < 8; i ++) { dBigVector p(&BoxPoints[i * 4]); array[i] = scale1 * p; } NewtonMeshVertexFormat vertexFormat; NewtonMeshClearVertexFormat(&vertexFormat); vertexFormat.m_faceCount = 10; vertexFormat.m_faceIndexCount = faceIndexList; vertexFormat.m_faceMaterial = faceMateriaIndexList; vertexFormat.m_vertex.m_data = &array[0][0]; vertexFormat.m_vertex.m_indexList = BoxIndices; vertexFormat.m_vertex.m_strideInBytes = sizeof (dBigVector); vertexFormat.m_normal.m_data = normal; vertexFormat.m_normal.m_indexList = faceNormalIndex; vertexFormat.m_normal.m_strideInBytes = 3 * sizeof (dFloat); // all channel are now optionals so we not longer has to pass default values // vertexFormat.m_uv0.m_data = uv0; // vertexFormat.m_uv0.m_indexList = uv0_indexList; // vertexFormat.m_uv0.m_strideInBytes = 2 * sizeof (dFloat); // now we create and empty mesh NewtonMesh* const newtonMesh = NewtonMeshCreate(scene->GetNewton()); NewtonMeshBuildFromVertexListIndexList(newtonMesh, &vertexFormat); // now we can use this mesh for lot of stuff, we can apply UV, we can decompose into convex, NewtonCollision* const collision = NewtonCreateConvexHullFromMesh(scene->GetNewton(), newtonMesh, 0.001f, 0); // for now we will simple make simple Box, make a visual Mesh DemoMesh* const visualMesh = new DemoMesh (newtonMesh); dMatrix matrix (dGetIdentityMatrix()); matrix.m_posit = origin; matrix.m_posit.m_w = 1.0f; NewtonBody* const body = CreateSimpleSolid(scene, visualMesh, mass, matrix, collision, 0); dVector veloc(1, 0, 2, 0); NewtonBodySetVelocity(body, &veloc[0]); visualMesh->Release(); NewtonDestroyCollision(collision); NewtonMeshDestroy (newtonMesh); return body; }
void RigidBodyData::CreateBody(NewtonCollision* const collision, const dVector& veloc, const dVector& omega) { _ASSERTE (!m_body); RigidBodyWorldDesc& me = *(RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor(); dMatrix matrix (GetIdentityMatrix()); m_body = NewtonCreateDynamicBody(me.m_newton, collision, &matrix[0][0]); //NewtonBodySetMassMatrix(m_body, m_mass, m_mass * m_inertia.m_x, m_mass * m_inertia.m_y, m_mass * m_inertia.m_z); NewtonBodySetMassProperties(m_body, m_mass, collision); NewtonBodySetCentreOfMass(m_body, &m_origin[0]); NewtonBodySetVelocity(m_body, &veloc[0]); NewtonBodySetOmega(m_body, &omega[0]); NewtonBodySetForceAndTorqueCallback(m_body, RigidBodyController::ApplyGravityForce); }
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~ // set the transformation of a rigid body void PhysicsActor::TransformCallback(const NewtonBody* _body, const Zen::Math::Real* _matrix) { void* pBody = NewtonBodyGetUserData(_body); if (pBody != NULL) { PhysicsActor* pShape = static_cast<PhysicsActor*>(pBody); // Only use the transform callback if the state is active if (pShape->m_activationState != 0) { Math::Matrix4 transform; for(int x = 0; x < 16; x++) { transform.m_array[x] = _matrix[x]; } TransformEventData evenData(*pShape, transform); pShape->onTransformEvent(evenData); } } #if 0 // HACK Keep the object to a constant velocity Math::Vector3 velocity; NewtonBodyGetVelocity(_body, velocity.m_array); velocity.normalize(); velocity = velocity * 50; NewtonBodySetVelocity(_body, velocity.m_array); #endif //std::cout << "TransformCallback()" << std::endl; #if 0 RenderPrimitive* primitive; // get the graphic object form the rigid body primitive = (RenderPrimitive*) NewtonBodyGetUserData (body); // set the transformation matrix for this rigid body dMatrix& mat = *((dMatrix*)matrix); primitive->SetMatrix (mat); #endif }
static void MagneticFieldNoForce (const NewtonBody* body, dFloat timestep, int threadIndex) { dMatrix matrix; dVector zero (0.0f, 0.0f, 0.0f, 0.0f); NewtonBodySetForce (body, &zero[0]); NewtonBodySetTorque (body, &zero[0]); NewtonBodySetOmega (body, &zero[0]); NewtonBodySetVelocity (body, &zero[0]); // telepor the magnetic field trigger to the center of the core Magnet* magnet; magnet = (Magnet*)NewtonBodyGetUserData(body); NewtonBodyGetMatrix (magnet->m_magneticCore, &matrix[0][0]); dMatrix posit (GetIdentityMatrix()); posit.m_posit = matrix.m_posit; NewtonBodySetMatrix (body, &posit[0][0]); }
// it fires a box when a key is pressed. static void FireNewtonCcdBox(NewtonWorld* world, const dVector & postion, const dVector & velocity) { NewtonCollision* collision = NewtonCreateBox(world, 1.0f, 1.0f, 1.0f, 0, NULL); dMatrix matrix(dGetIdentityMatrix()); matrix.m_posit = postion; NewtonBody* const body = NewtonCreateDynamicBody(world, collision, &matrix[0][0]); // set the force callback for applying the force and torque NewtonBodySetForceAndTorqueCallback(body, ApplyGravity); // set the mass for this body dFloat mass = 1.0f; NewtonBodySetMassProperties(body, mass, collision); NewtonDestroyCollision(collision); NewtonBodySetVelocity(body, &velocity[0]); NewtonBodySetContinuousCollisionMode(body, 1); }
void SetKinematicPose(NewtonBody* const body, const dMatrix& matrix1, dFloat timestep) { dMatrix matrix0; const dFloat OneOverDt = 1.0f / timestep; NewtonBodyGetMatrix(body, &matrix0[0][0]); dQuaternion q0(matrix0); dQuaternion q1(matrix1); dVector omega(q0.CalcAverageOmega(q1, OneOverDt)); // const dFloat maxOmega = 10.0f; // dFloat mag2 = omega.DotProduct3(omega); // if (mag2 > maxOmega) { // omega = omega.Normalize().Scale(maxOmega); // } dVector veloc ((matrix1.m_posit - matrix0.m_posit).Scale (OneOverDt)); NewtonBodySetVelocity(body, &veloc[0]); NewtonBodySetOmega(body, &omega[0]); NewtonBodyIntegrateVelocity(body, timestep); }
NewtonBody* dRigidbodyNodeInfo::CreateNewtonBody (NewtonWorld* const world, dScene* const scene, dScene::dTreeNode* const myNode) const { // find the collision and crate a rigid body dAssert (IsType (dRigidbodyNodeInfo::GetRttiType())); // attach the parent node as user data dScene::dTreeNode* parentNode = scene->FindParentByType(myNode, dSceneNodeInfo::GetRttiType()); dSceneNodeInfo* sceneInfo = (dSceneNodeInfo*) scene->GetInfoFromNode(parentNode); dScene::dTreeNode* shapeNode = scene->FindChildByType (myNode, dCollisionNodeInfo::GetRttiType()); dAssert (shapeNode); dCollisionNodeInfo* collInfo = (dCollisionNodeInfo*) scene->GetInfoFromNode(shapeNode); dMatrix matrix (sceneInfo->CalculateOrthoMatrix()); NewtonCollision* const collision = collInfo->CreateNewtonCollision (world, scene, shapeNode); NewtonBody* const body = NewtonCreateDynamicBody(world, collision, &matrix[0][0]); NewtonDestroyCollision(collision); // NewtonBodySetMatrix(body, &matrix[0][0]); NewtonBodySetUserData(body, parentNode); NewtonBodySetCentreOfMass(body, &m_centerOfMass[0]); NewtonBodySetMassMatrix(body, m_massMatrix.m_w, m_massMatrix.m_x, m_massMatrix.m_y, m_massMatrix.m_z); NewtonBodySetVelocity(body, &m_velocity[0]); NewtonBodySetOmega(body, &m_omega[0]); //dVector internalDamp(rigidBody->GetI); //NewtonBodySetLinearDamping(body, internalDamp); //dVariable* bodyType = rigidBody->FindVariable("rigidBodyType"); //if (!bodyType || !strcmp (bodyType->GetString(), "default gravity")) { // NewtonBodySetTransformCallback(body, DemoEntity::TransformCallback); //} return body; }
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); } };
void dNewtonBody::SetVelocity(dFloat x, dFloat y, dFloat z) { dVector vel(x,y,z); NewtonBodySetVelocity(m_body, &vel.m_x); }
void SimulationPostListener(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep) { // see if the net force on the body comes fr a high impact collision dFloat breakImpact = 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 contactForce; NewtonMaterial* const material = NewtonContactGetMaterial(contact); dFloat impulseImpact = NewtonMaterialGetContactMaxNormalImpact(material); if (impulseImpact > breakImpact) { breakImpact = impulseImpact; } } } // if the force is bigger than N time Gravities, It is considered a collision force breakImpact *= m_myMassInverse; // breakImpact = 1000.0f; if (breakImpact > BREAK_IMPACT_IN_METERS_PER_SECONDS) { NewtonWorld* const world = NewtonBodyGetWorld(m_myBody); dMatrix bodyMatrix; dVector com(0.0f); dVector veloc(0.0f); dVector omega(0.0f); dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetVelocity(m_myBody, &veloc[0]); NewtonBodyGetOmega(m_myBody, &omega[0]); NewtonBodyGetCentreOfMass(m_myBody, &com[0]); NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]); NewtonBodyGetMass(m_myBody, &mass, &Ixx, &Iyy, &Izz); com = bodyMatrix.TransformVector(com); dMatrix matrix(GetCurrentMatrix()); dQuaternion rotation(matrix); // we need to lock the world before creation a bunch of bodies scene->Lock(m_lock); for (FractureEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) { FractureAtom& atom = node->GetInfo(); DemoEntity* const entity = new DemoEntity(dMatrix(rotation, matrix.m_posit), NULL); entity->SetMesh(atom.m_mesh, dGetIdentityMatrix()); scene->Append(entity); int materialId = 0; dFloat debriMass = mass * atom.m_massFraction; //create the rigid body NewtonBody* const rigidBody = NewtonCreateDynamicBody(world, atom.m_collision, &matrix[0][0]); // calculate debris initial velocity dVector center(matrix.TransformVector(atom.m_centerOfMass)); dVector v(veloc + omega.CrossProduct(center - com)); // set initial velocity NewtonBodySetVelocity(rigidBody, &v[0]); NewtonBodySetOmega(rigidBody, &omega[0]); // set the debris mass properties, mass, center of mass, and inertia NewtonBodySetMassProperties(rigidBody, debriMass, atom.m_collision); // save the pointer to the graphic object with the body. NewtonBodySetUserData(rigidBody, entity); // assign the wood id NewtonBodySetMaterialGroupID(rigidBody, materialId); // set continuous collision mode // NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode); // set a destructor for this rigid body NewtonBodySetDestructorCallback(rigidBody, PhysicsBodyDestructor); // set the transform call back function NewtonBodySetTransformCallback(rigidBody, DemoEntity::TransformCallback); // set the force and torque call back function NewtonBodySetForceAndTorqueCallback(rigidBody, PhysicsApplyGravityForce); } NewtonDestroyBody(m_myBody); scene->RemoveEntity(mynode); // unlock the work after done with the effect scene->Unlock(m_lock); } }
void dCustomPlayerController::ResolveCollision() { dMatrix matrix; NewtonWorldConvexCastReturnInfo info[D_MAX_ROWS]; NewtonWorld* const world = m_manager->GetWorld(); NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]); NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody); int contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, info, 4, 0); if (!contactCount) { return; } dFloat maxPenetration = 0.0f; for (int i = 0; i < contactCount; i ++) { maxPenetration = dMax (info[i].m_penetration, maxPenetration); } if (maxPenetration > D_MAX_COLLISION_PENETRATION) { ResolveInterpenetrations(contactCount, info); NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]); } int rowCount = 0; dVector zero(0.0f); dMatrix invInertia; dVector com(0.0f); dVector veloc(0.0f); dComplementaritySolver::dJacobian jt[D_MAX_ROWS]; dFloat rhs[D_MAX_ROWS]; dFloat low[D_MAX_ROWS]; dFloat high[D_MAX_ROWS]; dFloat impulseMag[D_MAX_ROWS]; int normalIndex[D_MAX_ROWS]; NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]); NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]); NewtonBodyGetInvInertiaMatrix(m_kinematicBody, &invInertia[0][0]); // const dMatrix localFrame (dPitchMatrix(m_headingAngle) * m_localFrame * matrix); const dMatrix localFrame (m_localFrame * matrix); com = matrix.TransformVector(com); com.m_w = 0.0f; for (int i = 0; i < contactCount; i ++) { NewtonWorldConvexCastReturnInfo& contact = info[i]; dVector point (contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f); dVector normal (contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f); jt[rowCount].m_linear = normal; jt[rowCount].m_angular = (point - com).CrossProduct(normal); low[rowCount] = 0.0f; high[rowCount] = 1.0e12f; normalIndex[rowCount] = 0; dVector tmp (veloc * jt[rowCount].m_linear.Scale (1.001f)); rhs[rowCount] = - (tmp.m_x + tmp.m_y + tmp.m_z); rowCount ++; dAssert (rowCount < (D_MAX_ROWS - 3)); //dFloat updir = localFrame.m_front.DotProduct3(normal); dFloat friction = m_manager->ContactFriction(this, point, normal, contact.m_hitBody); if (friction > 0.0f) { // add lateral traction friction dVector sideDir (localFrame.m_up.CrossProduct(normal).Normalize()); jt[rowCount].m_linear = sideDir; jt[rowCount].m_angular = (point - com).CrossProduct(sideDir); low[rowCount] = -friction; high[rowCount] = friction; normalIndex[rowCount] = -1; dVector tmp1 (veloc * jt[rowCount].m_linear); rhs[rowCount] = -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z); rowCount++; dAssert (rowCount < (D_MAX_ROWS - 3)); // add longitudinal traction friction dVector frontDir (normal.CrossProduct(sideDir)); jt[rowCount].m_linear = frontDir; jt[rowCount].m_angular = (point - com).CrossProduct(frontDir); low[rowCount] = -friction; high[rowCount] = friction; normalIndex[rowCount] = -2; dVector tmp2 (veloc * jt[rowCount].m_linear); rhs[rowCount] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z); rowCount++; dAssert(rowCount < (D_MAX_ROWS - 3)); } } for (int i = 0; i < 3; i++) { jt[rowCount].m_linear = zero; jt[rowCount].m_angular = zero; jt[rowCount].m_angular[i] = dFloat(1.0f); rhs[rowCount] = 0.0f; impulseMag[rowCount] = 0; low[rowCount] = -1.0e12f; high[rowCount] = 1.0e12f; normalIndex[rowCount] = 0; rowCount ++; dAssert (rowCount < D_MAX_ROWS); } dVector impulse (veloc.Scale (m_mass) + CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt)); veloc = impulse.Scale(m_invMass); NewtonBodySetVelocity(m_kinematicBody, &veloc[0]); }
int dCustomPlayerController::ResolveInterpenetrations(int contactCount, NewtonWorldConvexCastReturnInfo* const contactArray) { dVector zero (0.0f); dVector veloc (0.0f); dVector savedVeloc (0.0f); NewtonBodyGetVelocity(m_kinematicBody, &savedVeloc[0]); NewtonBodySetVelocity(m_kinematicBody, &veloc[0]); dFloat timestep = 0.1f; dFloat invTimestep = 1.0f / timestep; dComplementaritySolver::dJacobian jt[D_MAX_ROWS]; dFloat rhs[D_MAX_ROWS]; dFloat low[D_MAX_ROWS]; dFloat high[D_MAX_ROWS]; int normalIndex[D_MAX_ROWS]; NewtonWorld* const world = m_manager->GetWorld(); for (int i = 0; i < 3; i++) { jt[i].m_linear = zero; jt[i].m_angular = zero; jt[i].m_angular[i] = dFloat(1.0f); rhs[i] = 0.0f; low[i] = -1.0e12f; high[i] = 1.0e12f; normalIndex[i] = 0; } dFloat penetration = D_MAX_COLLISION_PENETRATION * 10.0f; for (int j = 0; (j < 8) && (penetration > D_MAX_COLLISION_PENETRATION) ; j ++) { dMatrix matrix; dVector com(0.0f); NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]); NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]); com = matrix.TransformVector(com); com.m_w = 0.0f; int rowCount = 3; for (int i = 0; i < contactCount; i++) { NewtonWorldConvexCastReturnInfo& contact = contactArray[i]; dVector point(contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f); dVector normal(contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f); jt[rowCount].m_linear = normal; jt[rowCount].m_angular = (point - com).CrossProduct(normal); low[rowCount] = 0.0f; high[rowCount] = 1.0e12f; normalIndex[rowCount] = 0; penetration = dClamp(contact.m_penetration - D_MAX_COLLISION_PENETRATION * 0.5f, dFloat(0.0f), dFloat(0.5f)); rhs[rowCount] = penetration * invTimestep; rowCount++; } dVector impulse (CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt)); impulse = impulse.Scale(m_invMass); NewtonBodySetVelocity(m_kinematicBody, &impulse[0]); NewtonBodyIntegrateVelocity(m_kinematicBody, timestep); NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]); NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody); contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, contactArray, 4, 0); penetration = 0.0f; for (int i = 0; i < contactCount; i++) { penetration = dMax(contactArray[i].m_penetration, penetration); } } NewtonBodySetVelocity(m_kinematicBody, &savedVeloc[0]); return contactCount; }
void dCustomPlayerController::ResolveStep(dFloat timestep) { dMatrix matrix; dMatrix stepMatrix; dVector veloc(0.0f); dVector zero(0.0f); NewtonWorldConvexCastReturnInfo info[16]; NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]); NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]); dMatrix coodinateMatrix (m_localFrame * matrix); dComplementaritySolver::dJacobian jt[3]; dFloat rhs[3]; dFloat low[3]; dFloat high[3]; int normalIndex[3]; jt[0].m_linear = coodinateMatrix[0]; jt[0].m_angular = zero; low[0] = 0.0f; high[0] = 1.0e12f; normalIndex[0] = 0; rhs[0] = -m_impulse.DotProduct3(jt[0].m_linear) * m_invMass; // add lateral traction friction jt[1].m_linear = coodinateMatrix[1]; jt[1].m_angular = zero; low[1] = -m_friction; high[1] = m_friction; normalIndex[1] = -1; dVector tmp1(veloc * jt[1].m_linear); rhs[1] = -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z); // add longitudinal traction friction jt[2].m_linear = coodinateMatrix[2]; jt[2].m_angular = zero; low[2] = -m_friction; high[2] = m_friction; normalIndex[2] = -2; dVector tmp2(veloc * jt[2].m_linear); rhs[2] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z); dVector impulse(veloc.Scale(m_mass) + CalculateImpulse(3, rhs, low, high, normalIndex, jt)); impulse = impulse.Scale(m_invMass); NewtonBodySetVelocity(m_kinematicBody, &impulse[0]); NewtonBodyIntegrateVelocity(m_kinematicBody, timestep); NewtonWorld* const world = m_manager->GetWorld(); NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody); NewtonBodyGetMatrix(m_kinematicBody, &stepMatrix[0][0]); int contactCount = NewtonWorldCollide(world, &stepMatrix[0][0], shape, this, PrefilterCallback, info, 4, 0); NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]); NewtonBodySetVelocity(m_kinematicBody, &veloc[0]); dFloat maxHigh = 0.0f; for (int i = 0; i < contactCount; i++) { NewtonWorldConvexCastReturnInfo& contact = info[i]; dVector point(contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f); point = m_localFrame.UntransformVector (stepMatrix.UntransformVector(point)); maxHigh = dMax (point.m_x, maxHigh); } if ((maxHigh < m_stepHeight) && (maxHigh > m_contactPatch)) { dVector step (stepMatrix.RotateVector(m_localFrame.RotateVector (dVector(maxHigh, dFloat(0.0f), dFloat(0.0f), dFloat(0.0f))))); matrix.m_posit += step; NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]); } }
void dCustomPlayerController::SetVelocity(const dVector& veloc) { NewtonBodySetVelocity(m_kinematicBody, &veloc[0]); }
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex) { dMatrix matrix; dQuaternion bodyRotation; dVector veloc(0.0f, 0.0f, 0.0f, 0.0f); dVector omega(0.0f, 0.0f, 0.0f, 0.0f); CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); // apply the player motion, by calculation the desired plane linear and angular velocity manager->ApplyPlayerMove (this, timestep); // get the body motion state NewtonBodyGetMatrix(m_body, &matrix[0][0]); NewtonBodyGetVelocity(m_body, &veloc[0]); NewtonBodyGetOmega(m_body, &omega[0]); // integrate body angular velocity NewtonBodyGetRotation (m_body, &bodyRotation.m_q0); bodyRotation = bodyRotation.IntegrateOmega(omega, timestep); matrix = dMatrix (bodyRotation, matrix.m_posit); // integrate linear velocity dFloat normalizedTimeLeft = 1.0f; dFloat step = timestep * dSqrt (veloc % veloc) ; dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS); int prevContactCount = 0; CustomControllerConvexCastPreFilter castFilterData (m_body); NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector updir (matrix.RotateVector(m_upVector)); dVector scale; NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z); //const dFloat radio = m_outerRadio * 4.0f; const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f; NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio); NewtonWorldConvexCastReturnInfo upConstratint; memset (&upConstratint, 0, sizeof (upConstratint)); upConstratint.m_normal[0] = m_upVector.m_x; upConstratint.m_normal[1] = m_upVector.m_y; upConstratint.m_normal[2] = m_upVector.m_z; upConstratint.m_normal[3] = m_upVector.m_w; for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) { if ((veloc % veloc) < 1.0e-6f) { break; } dFloat timetoImpact; NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector destPosit (matrix.m_posit + veloc.Scale (timestep)); int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex); if (contactCount) { contactCount = manager->ProcessContacts (this, info, contactCount); } if (contactCount) { matrix.m_posit += veloc.Scale (timetoImpact * timestep); if (timetoImpact > 0.0f) { matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ; } normalizedTimeLeft -= timetoImpact; dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; for (int i = 1; i < contactCount; i ++) { dVector n0 (info[i-1].m_normal); for (int j = 0; j < i; j ++) { dVector n1 (info[j].m_normal); if ((n0 % n1) > 0.9999f) { info[i] = info[contactCount - 1]; i --; contactCount --; break; } } } int count = 0; if (!m_isJumping) { upConstratint.m_point[0] = matrix.m_posit.m_x; upConstratint.m_point[1] = matrix.m_posit.m_y; upConstratint.m_point[2] = matrix.m_posit.m_z; upConstratint.m_point[3] = matrix.m_posit.m_w; speed[count] = 0.0f; bounceNormal[count] = dVector (upConstratint.m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint); count ++; } for (int i = 0; i < contactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (info[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]); count ++; } for (int i = 0; i < prevContactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (prevInfo[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]); count ++; } dFloat residual = 10.0f; dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) { residual = 0.0f; for (int k = 0; k < count; k ++) { dVector normal (bounceNormal[k]); dFloat v = bounceSpeed[k] - normal % auxBounceVeloc; dFloat x = speed[k] + v; if (x < 0.0f) { v = 0.0f; x = 0.0f; } if (dAbs (v) > residual) { residual = dAbs (v); } auxBounceVeloc += normal.Scale (x - speed[k]); speed[k] = x; } } dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; i < count; i ++) { dVector normal (bounceNormal[i]); velocStep += normal.Scale (speed[i]); } veloc += velocStep; dFloat velocMag2 = velocStep % velocStep; if (velocMag2 < 1.0e-6f) { dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep); matrix.m_posit += veloc.Scale (advanceTime); normalizedTimeLeft -= advanceTime / timestep; } prevContactCount = contactCount; memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo)); } else { matrix.m_posit = destPosit; matrix.m_posit.m_w = 1.0f; break; } } NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z); // determine if player is standing on some plane dMatrix supportMatrix (matrix); supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin); if (m_isJumping) { dVector dst (matrix.m_posit); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } else { step = dAbs (updir % veloc.Scale (timestep)); dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step; dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f)); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } // set player velocity, position and orientation NewtonBodySetVelocity(m_body, &veloc[0]); NewtonBodySetMatrix (m_body, &matrix[0][0]); }
void cPhysicsBodyNewton::SetLinearVelocity(const cVector3f &a_vVel) { NewtonBodySetVelocity(m_pNewtonBody, a_vVel.v); }
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 / 5.0f, 2.0f / 5.0f, 3.0f / 5.0f, 4.0f / 5.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(175.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o, dBigVector(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f) + o, dBigVector(215.0f - 100.0f, 20.0f, 250.0f - 250.0f, 1.0f) + o, dBigVector(150.0f - 100.0f, 50.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(); const int count = 32; NewtonBody* bodies[count]; dBigVector point0; dVector positions[count + 1]; dFloat64 knot = spline.FindClosestKnot(point0, origin + dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f), 4); positions[0] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0); dFloat average = 0.0f; for (int i = 0; i < count; i ++) { dBigVector point1; average += positions[i].m_y; dBigVector tangent(spline.CurveDerivative(knot)); tangent = tangent.Scale (1.0 / sqrt (tangent % tangent)); knot = spline.FindClosestKnot(point1, dBigVector (point0 + tangent.Scale (2.0f)), 4); point0 = point1; positions[i + 1] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0); } average /= count; for (int i = 0; i < count + 1; i ++) { positions[i].m_y = average; } dFloat attachmentOffset = 0.8f; for (int i = 0; i < count; i ++) { dMatrix matrix; dVector location0 (positions[i].m_x, positions[i].m_y, positions[i].m_z, 0.0); bodies[i] = CreateWheel(scene, location0, 1.0f, 0.5f); NewtonBodySetLinearDamping(bodies[i], 0.0f); NewtonBody* const box = bodies[i]; NewtonBodyGetMatrix(box, &matrix[0][0]); dVector location1 (positions[i + 1].m_x, positions[i + 1].m_y, positions[i + 1].m_z, 0.0); dVector dir (location1 - location0); matrix.m_front = dir.Scale (1.0f / dSqrt (dir % dir)); matrix.m_right = matrix.m_front * matrix.m_up; dMatrix matrix1 (dYawMatrix(0.5f * 3.141692f) * matrix); NewtonBodySetMatrix(box, &matrix1[0][0]); matrix.m_posit.m_y += attachmentOffset; new MyPathFollow(matrix, box, rollerCosterPath); dVector veloc (dir.Scale (20.0f)); NewtonBodySetVelocity(box, &veloc[0]); } for (int i = 1; i < count; i ++) { NewtonBody* const box0 = bodies[i - 1]; NewtonBody* const box1 = bodies[i]; dMatrix matrix0; dMatrix matrix1; NewtonBodyGetMatrix(box0, &matrix0[0][0]); NewtonBodyGetMatrix(box1, &matrix1[0][0]); matrix0.m_posit.m_y += attachmentOffset; matrix1.m_posit.m_y += attachmentOffset; new CustomDistanceRope (matrix1.m_posit, matrix0.m_posit, box1, box0); } void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton()); for (int i = 0; i < count; i ++) { NewtonCollisionAggregateAddBody(aggregate, bodies[i]); } NewtonCollisionAggregateSetSelfCollision (aggregate, false); #ifdef _USE_HARD_JOINTS NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), bodies[0], NULL); for (int i = 1; i < count; i++) { NewtonSkeletonContainerAttachBone(skeleton, bodies[i], bodies[i - 1]); } NewtonSkeletonContainerFinalize(skeleton); #endif }
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); }
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 ProjectTireMatrix() { const NewtonBody* tire; const NewtonBody* chassis; dMatrix tireMatrix; dMatrix chassisMatrix; tire = m_body1; chassis = m_body0; NewtonBodyGetMatrix(tire, &tireMatrix[0][0]); NewtonBodyGetMatrix(chassis, &chassisMatrix[0][0]); // project the tire matrix to the right space. dMatrix tireMatrixInGlobalSpace (m_tireLocalMatrix * tireMatrix); dMatrix chassisMatrixInGlobalSpace (m_chassisLocalMatrix * chassisMatrix); dFloat projectDist; projectDist = (tireMatrixInGlobalSpace.m_posit - chassisMatrixInGlobalSpace.m_posit) % chassisMatrixInGlobalSpace.m_up; chassisMatrixInGlobalSpace.m_posit += chassisMatrixInGlobalSpace.m_up.Scale (projectDist); chassisMatrixInGlobalSpace.m_up = tireMatrixInGlobalSpace.m_right * chassisMatrixInGlobalSpace.m_front; chassisMatrixInGlobalSpace.m_up = chassisMatrixInGlobalSpace.m_up.Scale (1.0f / dSqrt (chassisMatrixInGlobalSpace.m_up % chassisMatrixInGlobalSpace.m_up)); chassisMatrixInGlobalSpace.m_right = chassisMatrixInGlobalSpace.m_front * chassisMatrixInGlobalSpace.m_up; chassisMatrixInGlobalSpace.m_up.m_w = 0.0f; chassisMatrixInGlobalSpace.m_right.m_w = 0.0f; dMatrix projectedChildMatrixInGlobalSpace (m_tireLocalMatrix.Inverse() * chassisMatrixInGlobalSpace); NewtonBodySetMatrix(tire, &projectedChildMatrixInGlobalSpace[0][0]); dVector tireVeloc; dVector chassisCom; dVector tireOmega; dVector chassisOmega; dVector chassisVeloc; // project the tire velocity NewtonBodyGetVelocity (tire, &tireVeloc[0]); NewtonBodyGetVelocity (chassis, &chassisVeloc[0]); NewtonBodyGetOmega (chassis, &chassisOmega[0]); NewtonBodyGetCentreOfMass (chassis, &chassisCom[0]); chassisCom = chassisMatrix.TransformVector(chassisCom); chassisVeloc += chassisOmega * (chassisMatrixInGlobalSpace.m_posit - chassisCom); dVector projTireVeloc (chassisVeloc - chassisMatrixInGlobalSpace.m_up.Scale (chassisVeloc % chassisMatrixInGlobalSpace.m_up)); projTireVeloc += chassisMatrixInGlobalSpace.m_up.Scale(tireVeloc % chassisMatrixInGlobalSpace.m_up); NewtonBodySetVelocity(tire, &projTireVeloc[0]); // project angular velocity NewtonBodyGetOmega (tire, &tireOmega[0]); dVector projTireOmega (chassisOmega - chassisMatrixInGlobalSpace.m_front.Scale (chassisOmega % chassisMatrixInGlobalSpace.m_front)); projTireOmega += chassisMatrixInGlobalSpace.m_front.Scale (tireOmega % chassisMatrixInGlobalSpace.m_front); NewtonBodySetOmega (tire, &projTireOmega[0]); }
// create a mesh using the dNewtonMesh wrapper static NewtonBody* CreateSimpledBox_dNetwonMesh(DemoEntityManager* const scene, const dVector& origin, const dVector& scale, dFloat mass) { dVector array[8]; dVector scale1(scale); for (int i = 0; i < 8; i++) { dVector p(&BoxPoints[i * 4]); array[i] = scale1 * p; } dNewtonMesh buildMesh(scene->GetNewton()); // start build a mesh buildMesh.BeginBuild(); // add faces one at a time int index = 0; const int faceCount = sizeof (faceIndexList)/sizeof (faceIndexList[0]); for (int i = 0; i < faceCount; i ++) { const int indexCount = faceIndexList[i]; const int faceMaterail = faceMateriaIndexList[i]; buildMesh.BeginPolygon(); for (int j = 0; j < indexCount; j ++) { // add a mesh point int k = BoxIndices[index + j]; buildMesh.AddPoint(array[k].m_x, array[k].m_y, array[k].m_z); // add vertex normal int n = faceNormalIndex[index + j]; buildMesh.AddNormal (dFloat32 (normal[n * 3 + 0]), dFloat32 (normal[n * 3 + 1]), dFloat32 (normal[n * 3 + 2])); // add face material index buildMesh.AddMaterial(faceMaterail); // continue adding more vertex attributes of you want, uv, binormal, weights, etc } buildMesh.EndPolygon(); index += indexCount; } buildMesh.EndBuild(); // get the newtonMesh from the dNewtonMesh class and use it to build a render mesh, collision or anything NewtonMesh* const newtonMesh = buildMesh.GetMesh(); // test collision tree //NewtonCollision* const collisionTree = NewtonCreateTreeCollisionFromMesh (scene->GetNewton(), newtonMesh, 0); //NewtonDestroyCollision(collisionTree); // now we can use this mesh for lot of stuff, we can apply UV, we can decompose into convex, NewtonCollision* const collision = NewtonCreateConvexHullFromMesh(scene->GetNewton(), newtonMesh, 0.001f, 0); // for now we will simple make simple Box, make a visual Mesh DemoMesh* const visualMesh = new DemoMesh(newtonMesh); dMatrix matrix(dGetIdentityMatrix()); matrix.m_posit = origin; matrix.m_posit.m_w = 1.0f; NewtonBody* const body = CreateSimpleSolid(scene, visualMesh, mass, matrix, collision, 0); dVector veloc (1, 0, 2, 0); NewtonBodySetVelocity(body, &veloc[0]); visualMesh->Release(); NewtonDestroyCollision(collision); return body; }