// rolling friction works as follow: the idealization of the contact of a spherical object // with a another surface is a point that pass by the center of the sphere. // in most cases this is enough to model the collision but in insufficient for modeling // the rolling friction. In reality contact with the sphere with the other surface is not // a point but a contact patch. A contact patch has the property the it generates a fix // constant rolling torque that opposes the movement of the sphere. // we can model this torque by adding a clamped torque aligned to the instantaneously axis // of rotation of the ball. and with a magnitude of the stopping angular acceleration. void CustomDryRollingFriction::SubmitConstrainst (dFloat timestep, int threadIndex) { dVector omega; dFloat omegaMag; dFloat torqueFriction; // get the omega vector NewtonBodyGetOmega(m_body0, &omega[0]); omegaMag = dSqrt (omega % omega); if (omegaMag > 0.1f) { // tell newton to used this the friction of the omega vector to apply the rolling friction dVector pin (omega.Scale (1.0f / omegaMag)); NewtonUserJointAddAngularRow (m_joint, 0.0f, &pin[0]); // calculate the acceleration to stop the ball in one time step NewtonUserJointSetRowAcceleration (m_joint, -omegaMag / timestep); // set the friction limit proportional the sphere Inertia torqueFriction = m_frictionTorque * m_frictionCoef; NewtonUserJointSetRowMinimumFriction (m_joint, -torqueFriction); NewtonUserJointSetRowMaximumFriction (m_joint, torqueFriction); } else { // when omega is too low sheath a little bit and damp the omega directly omega = omega.Scale (0.2f); NewtonBodySetOmega(m_body0, &omega[0]); } }
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~ void PhysicsActor::setAngularVelocity(const Math::Vector3& _omega) { setActivationState(true); m_activationState = 1; NewtonBodySetOmega(m_pActor, _omega.m_array); }
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; } }
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); }
static void AddNonUniformScaledPrimitives (DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int xCount, int zCount, dFloat spacing, PrimitiveType type, int materialID, const dMatrix& shapeOffsetMatrix) { // create the shape and visual mesh as a common data to be re used NewtonWorld* const world = scene->GetNewton(); // NewtonCollision* const collision = CreateConvexCollision (world, &shapeOffsetMatrix[0][0], size, type, materialID); NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, type, materialID); dFloat startElevation = 1000.0f; dMatrix matrix (dGetIdentityMatrix()); //matrix = dPitchMatrix(-45.0f * 3.141592f/180.0f); for (int i = 0; i < xCount; i ++) { dFloat x = origin.m_x + (i - xCount / 2) * spacing; for (int j = 0; j < zCount; j ++) { dFloat scalex = 0.75f + 1.0f * (dFloat (dRand()) / dFloat(dRAND_MAX) - 0.5f); dFloat scaley = 0.75f + 1.0f * (dFloat (dRand()) / dFloat(dRAND_MAX) - 0.5f); dFloat scalez = 0.75f + 1.0f * (dFloat (dRand()) / dFloat(dRAND_MAX) - 0.5f); // scalex = 1.0f; // scaley = 1.0f; // scalez = 1.0f; NewtonCollisionSetScale(collision, scalex, scaley, scalez); DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "smilli.tga", "smilli.tga", "smilli.tga"); dFloat z = origin.m_z + (j - zCount / 2) * spacing; matrix.m_posit.m_x = x; matrix.m_posit.m_z = z; dVector floor (FindFloor (world, dVector (matrix.m_posit.m_x, startElevation, matrix.m_posit.m_z, 0.0f), 2.0f * startElevation)); matrix.m_posit.m_y = floor.m_y + 8.0f; // create a solid NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID); //NewtonBodySetCollisionScale (body, scalex, scaley, scalez); dVector omega (0, 0, 0); NewtonBodySetOmega(body, &omega[0]); // release the mesh geometry->Release(); } } // do not forget to delete the collision NewtonDestroyCollision (collision); }
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); } }
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]); }
void dNewtonBody::CalculateBuoyancyForces(const void* plane, void* force, void* torque, float bodyDensity) { dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz); if (mass > 0.0f) { dMatrix matrix; dVector cog(0.0f); dVector accelPerUnitMass(0.0f); dVector torquePerUnitMass(0.0f); const dVector gravity(0.0f, -9.8f, 0.0f, 0.0f); NewtonBodyGetMatrix(m_body, &matrix[0][0]); NewtonBodyGetCentreOfMass(m_body, &cog[0]); cog = matrix.TransformVector(cog); NewtonCollision* const collision = NewtonBodyGetCollision(m_body); dFloat shapeVolume = NewtonConvexCollisionCalculateVolume(collision); dFloat fluidDensity = 1.0f / (bodyDensity * shapeVolume); dFloat viscosity = 0.995f; NewtonConvexCollisionCalculateBuoyancyAcceleration(collision, &matrix[0][0], &cog[0], &gravity[0], (float*)plane, fluidDensity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]); dVector finalForce(accelPerUnitMass.Scale(mass)); dVector finalTorque(torquePerUnitMass.Scale(mass)); dVector omega(0.0f); NewtonBodyGetOmega(m_body, &omega[0]); omega = omega.Scale(viscosity); NewtonBodySetOmega(m_body, &omega[0]); ((float*)force)[0] = finalForce.m_x ; ((float*)force)[1] = finalForce.m_y ; ((float*)force)[2] = finalForce.m_z ; ((float*)torque)[0] = finalTorque.m_x; ((float*)torque)[1] = finalTorque.m_y; ((float*)torque)[2] = finalTorque.m_z; } }
void OnInside(NewtonBody* const visitor) { dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMass(visitor, &mass, &Ixx, &Iyy, &Izz); if (mass > 0.0f) { dMatrix matrix; dVector cog(0.0f); dVector accelPerUnitMass(0.0f); dVector torquePerUnitMass(0.0f); const dVector gravity (0.0f, DEMO_GRAVITY, 0.0f, 0.0f); NewtonBodyGetMatrix (visitor, &matrix[0][0]); NewtonBodyGetCentreOfMass(visitor, &cog[0]); cog = matrix.TransformVector (cog); NewtonCollision* const collision = NewtonBodyGetCollision(visitor); dFloat shapeVolume = NewtonConvexCollisionCalculateVolume (collision); dFloat fluidDentity = 1.0f / (m_waterToSolidVolumeRatio * shapeVolume); dFloat viscosity = 0.995f; NewtonConvexCollisionCalculateBuoyancyAcceleration (collision, &matrix[0][0], &cog[0], &gravity[0], &m_plane[0], fluidDentity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]); dVector force (accelPerUnitMass.Scale (mass)); dVector torque (torquePerUnitMass.Scale (mass)); dVector omega(0.0f); NewtonBodyGetOmega(visitor, &omega[0]); omega = omega.Scale (viscosity); NewtonBodySetOmega(visitor, &omega[0]); NewtonBodyAddForce (visitor, &force[0]); NewtonBodyAddTorque (visitor, &torque[0]); } }
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); }
static void ClampAngularVelocity(const NewtonBody* body, dFloat timestep, int threadIndex) { dVector omega; NewtonBodyGetOmega(body, &omega[0]); omega.m_w = 0.0f; dFloat mag2 = omega.DotProduct3(omega); if (mag2 > (100.0f * 100.0f)) { omega = omega.Normalize().Scale(100.0f); NewtonBodySetOmega(body, &omega[0]); } //PhysicsApplyGravityForce(body, timestep, threadIndex); dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; dFloat gravity = -0.0f; NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz); dVector dir(0.0f, gravity, 0.0f); dVector force(dir.Scale(mass)); NewtonBodySetForce(body, &force.m_x); }
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 dCustomTireSpringDG::TireMatrixProjection() { NewtonBody* tire; NewtonBody* chassis; // dMatrix tireMatrix; dMatrix chassisMatrix; dMatrix tireMatrixInGlobalSpace; dMatrix chassisMatrixInGlobalSpace; dMatrix projectedChildMatrixInGlobalSpace; // dVector projectDist; dVector projTireOmega; dVector chassisOmega; dVector tireOmega; dVector tireOmegaCorrection; // // D.G: Thanks Julio for the helps in the past for this part of code. // D.G: This code come from a pretty old vehicle implementation. tire = GetBody1(); chassis = GetBody0(); // NewtonBodyGetMatrix(tire, &tireMatrix[0][0]); NewtonBodyGetMatrix(chassis, &chassisMatrix[0][0]); // // project the tire matrix to the right space. tireMatrixInGlobalSpace = (m_localMatrix1 * tireMatrix); chassisMatrixInGlobalSpace = (m_localMatrix0 * chassisMatrix); // projectDist = (tireMatrixInGlobalSpace.m_posit - chassisMatrixInGlobalSpace.m_posit).DotProduct3(chassisMatrixInGlobalSpace.m_up); chassisMatrixInGlobalSpace.m_posit = chassisMatrixInGlobalSpace.m_posit + (chassisMatrixInGlobalSpace.m_up * projectDist); // chassisMatrixInGlobalSpace.m_up = tireMatrixInGlobalSpace.m_right.CrossProduct(chassisMatrixInGlobalSpace.m_front); chassisMatrixInGlobalSpace.m_up = chassisMatrixInGlobalSpace.m_up * (1.0f / dSqrt(chassisMatrixInGlobalSpace.m_up.DotProduct3(chassisMatrixInGlobalSpace.m_up))); chassisMatrixInGlobalSpace.m_right = chassisMatrixInGlobalSpace.m_front.CrossProduct(chassisMatrixInGlobalSpace.m_up); chassisMatrixInGlobalSpace.m_up.m_w = 0.0; chassisMatrixInGlobalSpace.m_right.m_w = 0.0; // projectedChildMatrixInGlobalSpace = (m_localMatrix1.Inverse() * chassisMatrixInGlobalSpace); NewtonBodySetMatrix(tire, &projectedChildMatrixInGlobalSpace[0][0]); // NewtonBodyGetOmega(chassis, &chassisOmega[0]); // D.G: Temporary disabled. // D.G: The result is a lot better without this part. // D.G: Causing problems, something is calculed wrong and the result give some bad bounce force on the wheels. // /* NewtonBodyGetCentreOfMass(chassis, @chassisCom.V[0]); NewtonBodyGetMatrix(chassis, @rlmat.V[0].V[0]); chassisCom: = OXTransformVector(chassisCom, rlmat); chassisVeloc: = VectorAdd(chassisVeloc, VectorCrossProduct(chassisOmega, VectorSubtract(chassisMatrixInGlobalSpace.m_posit, chassisCom))); projTireVeloc: = VectorSubtract{ VectorAdd }(chassisVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(chassisVeloc, chassisMatrixInGlobalSpace.m_up))); projTireVeloc: = VectorAdd{ VectorSubtract }(projTireVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(tireVeloc, chassisMatrixInGlobalSpace.m_up))); //NegateVector(projTireVeloc); NewtonBodySetVelocity(tire, @projTireVeloc.V[0]); */ // project angular velocity NewtonBodyGetOmega(tire, &tireOmega[0]); tireOmegaCorrection = tireOmega; // if (GetVecLength(tireOmegaCorrection) > mFpsRequest) { // I need to use this omega correction fix when NewtonBodySetLinearDamping,NewtonBodySetAngularDamping is set to zero. // Because the tire rotation overpass the physics rotation limit in time, and it can give bad result without this fix. // I prefered to have the body totally free rolling, because of this I need this fix. // If you don't use this fix, Don't set the NewtonBodySetLinearDamping,NewtonBodySetAngularDamping to zero value, just don't call this both functions on the body at all. // //printf("Omega tire correction, Fix a Rotation limitation. \n"); tireOmegaCorrection = (tireOmegaCorrection * mTireOmegaCorrection); NewtonBodySetOmega(tire, &tireOmegaCorrection[0]); } // projTireOmega = chassisOmega - chassisMatrixInGlobalSpace.m_front * (chassisOmega.DotProduct3(chassisMatrixInGlobalSpace.m_front)); projTireOmega = projTireOmega + chassisMatrixInGlobalSpace.m_front * (tireOmegaCorrection.DotProduct3(chassisMatrixInGlobalSpace.m_front)); NewtonBodySetOmega(tire, &projTireOmega[0]); }
static void PhysicsSpinBody (const NewtonBody* body, dFloat timestep, int threadIndex) { dVector omega (0.0f, 0.f, 1.0f, 0.0f); NewtonBodySetOmega (body, &omega[0]); }
void dCustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex) { // check if this is an impulsive time step dMatrix matrix0(GetBodyMatrix()); dVector omega(0.0f); dVector com(0.0f); dVector pointVeloc(0.0f); const dFloat damp = 0.3f; dAssert (timestep > 0.0f); const dFloat invTimestep = 1.0f / timestep; // we not longer cap excessive angular velocities, it is left to the client application. NewtonBodyGetOmega(m_body0, &omega[0]); //cap excessive angular velocities dFloat mag2 = omega.DotProduct3(omega); if (mag2 > (m_omegaCap * m_omegaCap)) { omega = omega.Normalize().Scale(m_omegaCap); NewtonBodySetOmega(m_body0, &omega[0]); } // calculate the position of the pivot point and the Jacobian direction vectors, in global space. dVector relPosit(m_targetMatrix.m_posit - matrix0.m_posit); NewtonBodyGetPointVelocity(m_body0, &m_targetMatrix.m_posit[0], &pointVeloc[0]); for (int i = 0; i < 3; i ++) { // Restrict the movement on the pivot point along all tree orthonormal direction dFloat speed = pointVeloc.DotProduct3(m_targetMatrix[i]); dFloat dist = relPosit.DotProduct3(m_targetMatrix[i]) * damp; dFloat relSpeed = dist * invTimestep - speed; dFloat relAccel = relSpeed * invTimestep; NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &m_targetMatrix[i][0]); NewtonUserJointSetRowAcceleration(m_joint, relAccel); NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_maxLinearFriction); } if (m_isSixdof) { dQuaternion rotation (matrix0.Inverse() * m_targetMatrix); if (dAbs (rotation.m_q0) < 0.99998f) { dMatrix rot (dGrammSchmidt(dVector (rotation.m_q1, rotation.m_q2, rotation.m_q3))); dFloat angle = 2.0f * dAcos(dClamp(rotation.m_q0, dFloat(-1.0f), dFloat(1.0f))); NewtonUserJointAddAngularRow (m_joint, angle, &rot.m_front[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_up[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_right[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } else { NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } } }
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 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 dNewtonBody::SetOmega(dFloat x, dFloat y, dFloat z) { dVector omg(x, y, z); NewtonBodySetOmega(m_body, &omg.m_x); }
void cPhysicsBodyNewton::SetAngularVelocity(const cVector3f &a_vVel) { NewtonBodySetOmega(m_pNewtonBody, a_vVel.v); }
void PrecessingTops (DemoEntityManager* const scene) { scene->CreateSkyBox(); // customize the scene after loading // set a user friction variable in the body for variable friction demos // later this will be done using LUA script dMatrix offsetMatrix (dGetIdentityMatrix()); CreateLevelMesh (scene, "flatPlane.ngd", 1); dVector location (0.0f, 0.0f, 0.0f, 0.0f); dVector size (3.0f, 2.0f, 0.0f, 0.0f); // create an array of cones const int count = 10; // all shapes use the x axis as the axis of symmetry, to make an upright cone we apply a 90 degree rotation local matrix dMatrix shapeOffsetMatrix (dRollMatrix(-3.141592f/2.0f)); AddPrimitiveArray(scene, 50.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, 0, shapeOffsetMatrix); // till the cont 30 degrees, and apply a local high angular velocity dMatrix matrix (dRollMatrix (-25.0f * 3.141592f / 180.0f)); dVector omega (0.0f, 50.0f, 0.0f); omega = matrix.RotateVector (omega); dVector damp (0.0f, 0.0f, 0.0f, 0.0f); int topscount = 0; NewtonBody* array[count * count]; NewtonWorld* const world = scene->GetNewton(); for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) { NewtonCollision* const collision = NewtonBodyGetCollision(body); dVector com; if (NewtonCollisionGetType (collision) == SERIALIZE_ID_CONE) { array[topscount] = body; topscount ++; } } for (int i = 0; i < topscount ; i ++) { dMatrix bodyMatrix; NewtonBody* const body = array[i]; NewtonBodyGetMatrix(body, &bodyMatrix[0][0]); matrix.m_posit = bodyMatrix.m_posit; matrix.m_posit.m_y += 1.0f; NewtonBodySetMatrix(body, &matrix[0][0]); dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz); NewtonBodySetMassMatrix(body, mass, Ixx, Iyy * 8.0f, Izz); NewtonBodySetOmega (body, &omega[0]); NewtonBodySetAutoSleep (body, 0); NewtonBodySetLinearDamping(body, 0.0f); NewtonBodySetAngularDamping (body, &damp[0]); } // place camera into position dMatrix camMatrix (dGetIdentityMatrix()); dQuaternion rot (camMatrix); dVector origin (-40.0f, 5.0f, 0.0f, 0.0f); scene->SetCameraMatrix(rot, origin); }
void 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]); }
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]); }
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 CustomDGRayCastCar::ApplyOmegaCorrection () { m_chassisOmega = m_chassisOmega.Scale( m_chassisRotationLimit ); NewtonBodySetOmega( m_body0, &m_chassisOmega[0] ); }
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); } };