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