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