//////////////////////////////////////////////////////////////////////////
// Position the body precisely
//////////////////////////////////////////////////////////////////////////
void DropDownBox(NewtonBody *body)
{
float matrix[16];
NewtonBodyGetMatrix(body, matrix);
Matrix4 m = Matrix4(matrix);
Vector3 pos = m.GetPosition();
pos.y += 1.0f;
m.SetPosition(pos);
Vector3 p = pos;
p.y -= 20;
// cast collision shape within +20 -20 y range
NewtonWorld *world = NewtonBodyGetWorld(body);
NewtonCollision *collision = NewtonBodyGetCollision(body);
float param;
NewtonWorldConvexCastReturnInfo info[16];
m.FlattenToArray(matrix);
NewtonWorldConvexCast(world, matrix, &p[0], collision, ¶m, body, DropDownConvexCastCallback, info, 16, 0);
m = Matrix4(matrix);
pos = m.GetPosition();
m.SetPosition(pos + Vector3(0, (p.y - pos.y) * param, 0) );
m.FlattenToArray(matrix);
NewtonBodySetMatrix(body, matrix);
}
iPhysicsBody* iPhysics::createBody(iPhysicsCollision* collisionVolume)
{
con_assert(collisionVolume != nullptr, "zero pointer");
con_assert(collisionVolume->_collision != nullptr, "zero pointer");
iaMatrixf matrix;
NewtonWaitForUpdateToFinish(static_cast<const NewtonWorld*>(_defaultWorld));
NewtonBody* newtonBody = NewtonCreateDynamicBody(static_cast<const NewtonWorld*>(_defaultWorld), static_cast<const NewtonCollision*>(collisionVolume->_collision), matrix.getData());
// set callbacks
NewtonBodySetDestructorCallback(newtonBody, reinterpret_cast<NewtonBodyDestructor>(PhysicsNodeDestructor));
NewtonBodySetTransformCallback(newtonBody, reinterpret_cast<NewtonSetTransform>(PhysicsNodeSetTransform));
NewtonBodySetForceAndTorqueCallback(newtonBody, reinterpret_cast<NewtonApplyForceAndTorque>(PhysicsApplyForceAndTorque));
NewtonBodySetMassMatrix(newtonBody, 0, 0, 0, 0);
NewtonBodySetMatrix(newtonBody, matrix.getData());
start();
iPhysicsBody* result = new iPhysicsBody(newtonBody);
_bodyListMutex.lock();
_bodys[result->getID()] = result;
_bodyListMutex.unlock();
return result;
}
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 Roket_PhysicsBody::setPosition(irr::core::vector3df vec)
{
irr::core::matrix4 matrix;
NewtonBodyGetMatrix(body, matrix.pointer());
matrix.setTranslation(vec);
NewtonBodySetMatrix(body, matrix.pointer());
}
NewtonBody* Body::create(NewtonCollision* collision, float mass, int freezeState, const Vec4f& damping)
{
Vec4f minBox, maxBox;
Vec4f origin, inertia;
m_body = NewtonCreateBody(newton::world, collision, this->m_matrix[0]);
NewtonBodySetUserData(m_body, this);
NewtonBodySetMatrix(m_body, this->m_matrix[0]);
NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]);
if (mass < 0.0f)
mass = NewtonConvexCollisionCalculateVolume(collision) * 0.5f;
if (mass != 0.0f)
NewtonBodySetMassMatrix(m_body, mass, mass * inertia.x, mass * inertia.y, mass * inertia.z);
NewtonBodySetCentreOfMass(m_body, &origin[0]);
NewtonBodySetForceAndTorqueCallback(m_body, Body::__applyForceAndTorqueCallback);
NewtonBodySetTransformCallback(m_body, Body::__setTransformCallback);
NewtonBodySetDestructorCallback(m_body, Body::__destroyBodyCallback);
NewtonBodySetFreezeState(m_body, freezeState);
NewtonBodySetLinearDamping(m_body, damping.w);
NewtonBodySetAngularDamping(m_body, &damping[0]);
return m_body;
}
void RigidBodyData::Load(ILoad* const iload)
{
ULONG nwrit;
int revision;
dVector veloc;
dVector omega;
dMatrix matrix;
RigidBodyWorldDesc& me = *(RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor();
iload->Read((const char*)&revision, sizeof (revision), &nwrit);
iload->Read((const char*)&m_oldControlerID, sizeof (m_oldControlerID), &nwrit);
iload->Read((const char*)&m_collisionShape, sizeof (m_collisionShape), &nwrit);
iload->Read((const char*)&m_hideGizmos, sizeof (m_hideGizmos), &nwrit);
iload->Read((const char*)&m_mass, sizeof (m_mass), &nwrit);
iload->Read((const char*)&m_inertia, sizeof (m_inertia), &nwrit);
iload->Read((const char*)&m_origin, sizeof (m_origin), &nwrit);
iload->Read((const char*)&matrix, sizeof (matrix), &nwrit);
iload->Read((const char*)&veloc, sizeof (veloc), &nwrit);
iload->Read((const char*)&omega, sizeof (omega), &nwrit);
NewtonCollision* const collision = NewtonCreateCollisionFromSerialization (me.m_newton, LoadCollision, iload);
CreateBody(collision, veloc, omega);
NewtonBodySetMatrix(m_body, &matrix[0][0]);
NewtonDestroyCollision(collision);
}
void dNewtonBody::SetPosition(dFloat x, dFloat y, dFloat z)
{
dQuaternion rot;
NewtonBodyGetRotation(m_body, &rot.m_q0);
dMatrix mat(rot, dVector(x, y, z));
NewtonBodySetMatrix(m_body, &mat[0][0]);
}
void Roket_PhysicsBody::setRotation(irr::core::vector3df vec)
{
irr::core::matrix4 matrix;
NewtonBodyGetMatrix(body, matrix.pointer());
matrix.setRotationDegrees(vec);
NewtonBodySetMatrix(body, matrix.pointer());
}
void dNewtonBody::SetRotation(dFloat x, dFloat y, dFloat z, dFloat w)
{
dVector pos(0, 0, 0);
NewtonBodyGetPosition(m_body, &pos.m_x);
dMatrix mat(dQuaternion(x, y, z, w), pos);
NewtonBodySetMatrix(m_body, &mat[0][0]);
}
void CCamera::Dessiner()
{
calculcentre();
if (m_dPhi > 1.5)
m_dPhi=1.5;
if (m_dPhi < -1.0)
m_dPhi=-1.0;
// Calcul des coordonnées polaires
m_dPitch =m_dR*cos(m_dPhi)*cos(m_dTheta);
m_dRoll =m_dR*cos(m_dPhi)*sin(m_dTheta);
m_dHeading =m_dR*sin(m_dPhi);
//Empecher le retour Camera
if(m_dHeading <0)
m_dHeading = 0;
//Mise a jour de la matrice Newton de la camera
CMatrix matrice_Collision_Cam;
NewtonBodyGetMatrix(GetBody(), &matrice_Collision_Cam.m_Mat[0][0]);
matrice_Collision_Cam.m_Mat[3][0] = m_vCenter_Cam.x+m_dPitch;
matrice_Collision_Cam.m_Mat[3][1] = m_vCenter_Cam.y+m_dHeading;
matrice_Collision_Cam.m_Mat[3][2] = m_vCenter_Cam.z+m_dRoll;
NewtonBodySetMatrix(GetBody(), &matrice_Collision_Cam.m_Mat[0][0]);
//Placer la camera
gluLookAt(matrice_Collision_Cam.getPosition().x,matrice_Collision_Cam.getPosition().y,matrice_Collision_Cam.getPosition().z,m_vCenter_Cam.x,m_vCenter_Cam.y+1.5,m_vCenter_Cam.z, m_vHaut_Cam.x,m_vHaut_Cam.y,m_vHaut_Cam.z);
}
void CCamera::Initialiser(NewtonWorld *nWorld,CVector3 taille)
{
// On initialise le vecteur de dimensions
m_vTailleCollisionCam.x = taille.x;
m_vTailleCollisionCam.y = taille.y;
m_vTailleCollisionCam.z = taille.z;
CMatrix matrice; // On créé une matrice
matrice.setIdentite();
// On définit la matrice de manière à ce que l'objet soit placé aux positions
// spécifiées en utilisant la dernière colonne de la matrice
matrice.m_Mat[3][0] = 0;
matrice.m_Mat [3][1] = 0;
matrice.m_Mat [3][2] = 0;
// On initialise la boîte de collision
NewtonCollision * collision = NULL;
// On créé la boite de collision aux dimensions de l'objet
collision = NewtonCreateBox (nWorld, m_vTailleCollisionCam.x, m_vTailleCollisionCam.y, m_vTailleCollisionCam.z, NULL);
// On initialise le corps avec la boite de collision
m_pBody = NewtonCreateBody (nWorld, collision);
if (m_pBody == NULL)
std::cerr << "Impossible d'initialiser le corps.";
// On détruit la boite de collision, on n'en a plus besoin
NewtonReleaseCollision (nWorld, collision);
// Enfin, on affecte notre matrice (qui représente donc sa position dans l'espace)
// à notre corps grâce à la fonction NewtonBodySetMatrix
NewtonBodySetMatrix (m_pBody, &matrice.m_Mat [0][0]);
}
static void AddUniversal(DemoEntityManager* const scene, const dVector& origin)
{
dVector size(1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateBox(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), dVector(0.25f, 0.25f, 4.0f, 0.0f));
NewtonBody* const box1 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, 2.0f, 0.0f), 1.0f, 0.5f);
NewtonBody* const box2 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, -2.0f, 0.0f), 1.0f, 0.5f);
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// align the object so that is looks nice
dMatrix matrix;
NewtonBodyGetMatrix(box1, &matrix[0][0]);
matrix = dYawMatrix (3.1416f * 0.5f) * matrix;
NewtonBodySetMatrix(box1, &matrix[0][0]);
((DemoEntity*) NewtonBodyGetUserData(box1))->ResetMatrix (*scene, matrix);
NewtonBodyGetMatrix(box2, &matrix[0][0]);
matrix = dYawMatrix(3.1416f * 0.5f) * matrix;
NewtonBodySetMatrix(box2, &matrix[0][0]);
((DemoEntity*) NewtonBodyGetUserData(box2))->ResetMatrix (*scene, matrix);
// link the two boxes
NewtonBodyGetMatrix(box1, &matrix[0][0]);
CustomUniversal* const joint1 = new CustomUniversal(matrix, box1, box0);
joint1->EnableLimit_0(true);
joint1->SetLimis_0 (-5.0f * 3.141592f, 2.0f * 3.141592f);
joint1->EnableLimit_1(true);
joint1->SetLimis_1 (-3.0f * 3.141592f, 4.0f * 3.141592f);
// link the two boxes
NewtonBodyGetMatrix(box2, &matrix[0][0]);
CustomUniversal* const joint2 = new CustomUniversal(matrix, box2, box0);
joint2->EnableLimit_0(true);
joint2->SetLimis_0 (-3.0f * 3.141592f, 5.0f * 3.141592f);
joint2->EnableLimit_1(true);
joint2->SetLimis_1(-4.0f * 3.141592f, 2.0f * 3.141592f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
void cPhysicsBodyNewtonCallback::OnTransformUpdate(iEntity3D * apEntity)
{
if(cPhysicsBodyNewton::mbUseCallback==false) return;
cPhysicsBodyNewton *pRigidBody = static_cast<cPhysicsBodyNewton*>(apEntity);
NewtonBodySetMatrix(pRigidBody->mpNewtonBody, &apEntity->GetLocalMatrix().GetTranspose().m[0][0]);
if(pRigidBody->mpNode) pRigidBody->mpNode->SetMatrix(apEntity->GetLocalMatrix());
}
void NewtonEntity::SetRotation(float rx, float ry, float rz)
{
glm::mat4 mat = glm::gtx::euler_angles::yawPitchRoll(ry,rz,rx);
curRotation = glm::gtc::quaternion::quat_cast(mat);
prevRotation = curRotation;
matrix = createMat4(curRotation,curPosition);
if(body)
NewtonBodySetMatrix( body, &matrix[0][0]);
}
dFloat dCustomPlayerController::PredictTimestep(dFloat timestep)
{
dMatrix matrix;
dMatrix predicMatrix;
NewtonWorld* const world = m_manager->GetWorld();
NewtonWorldConvexCastReturnInfo info[16];
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);
NewtonBodyGetMatrix(m_kinematicBody, &predicMatrix[0][0]);
int contactCount = NewtonWorldCollide(world, &predicMatrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
if (contactCount) {
dFloat t0 = 0.0f;
dFloat t1 = timestep;
dFloat dt = (t1 + t0) * 0.5f;
timestep = dt;
for (int i = 0; i < D_MAX_COLLIONSION_STEPS; i++) {
NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);
NewtonBodyGetMatrix(m_kinematicBody, &predicMatrix[0][0]);
contactCount = NewtonWorldCollide(world, &predicMatrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
dt *= 0.5f;
if (contactCount) {
dFloat penetration = 0.0f;
for (int j = 0; j < contactCount; j++) {
penetration = dMax(penetration, info[j].m_penetration);
}
if (penetration < D_MAX_COLLISION_PENETRATION) {
break;
}
timestep -= dt;
} else {
timestep += dt;
}
}
}
return timestep;
}
void PhysicMap::moveEntity(int id, const Triplet_f& pos)
{
BodyMap::const_iterator it = m_bodyMap.find(id);
if (it != m_bodyMap.end() && it->second != 00)
{
Matrix_f matrix;
matrix.setPosition(pos);
NewtonBodySetMatrix(it->second, matrix.raw());
}
}
void OgreNewtonSceneBody::EndAddRemoveCollision()
{
dNewtonCollisionScene* const scene = (dNewtonCollisionScene*) GetCollision();
scene->EndAddRemoveCollision();
// need to update the aabb in the broad phase, for this we call set matrix
dMatrix matrix;
NewtonBody* const body = GetNewtonBody();
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodySetMatrix(body, &matrix[0][0]);
}
void CreateScene (NewtonWorld* world, SceneManager* sceneManager)
{
Entity* floor;
NewtonBody* floorBody;
NewtonCollision* shape;
// initialize the camera
InitCamera (dVector (-15.0f, 5.0f, 0.0f, 0.0f), dVector (1.0f, 0.0f, 0.0f));
// Create a large body to be the floor
floor = sceneManager->CreateEntity();
floor->LoadMesh ("FloorBox.dat");
// add static floor Physics
shape = CreateNewtonBox (world, floor, 0);
floorBody = CreateRigidBody (world, floor, shape, 0.0f);
NewtonDestroyCollision(shape);
// set the Transformation Matrix for this rigid body
dMatrix matrix (floor->m_curRotation, floor->m_curPosition);
NewtonBodySetMatrix (floorBody, &matrix[0][0]);
// find the floor base, and add some distance up;
dFloat floorY = FindFloor (world, 0.0f, 0.0f) + 2.0f;
// this is the width of the Box;
dFloat boxWidth = 0.42f;
// Make a small pyramid of Boxes
for (int i = 0; i < PYRAMID_BASE + 1; i ++) {
for (int j = 0; j < PYRAMID_BASE - i; j ++) {
Entity* smilly;
NewtonBody* smillyBody;
// crate a visual Box and place in a pyramid formation above the floor
smilly = sceneManager->CreateEntity();
smilly->LoadMesh ("Smilly.dat");
smilly->m_curPosition.m_x = 0.0f;
smilly->m_curPosition.m_z = dFloat (j) * boxWidth + dFloat (i) * (boxWidth * 0.5f);
smilly->m_curPosition.m_y = floorY + dFloat (i) * boxWidth;
smilly->m_prevPosition = smilly->m_curPosition;
// add a body with a box shape
shape = CreateNewtonBox (world, smilly, 0);
smillyBody = CreateRigidBody (world, smilly, shape, 10.0f);
NewtonDestroyCollision(shape);
// we want some nice object placement, with zero penetration and and zero jitter
// therefore we are going use use a Convex Cast function to snap the Box to the closest contact surface
ConvexCastPlacement (smillyBody);
}
}
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
void
PhysicsActor::setPosition(const Math::Point3& _pos)
{
Zen::Math::Matrix4 mat;
//mat.identity();
getOrientation(mat);
mat.setPosition(_pos);
setActivationState(true);
m_activationState = 1;
NewtonBodySetMatrix(m_pActor, mat.m_array);
}
/*
=============
CMod_PhysicsEndBSPCollisionTree
=============
*/
void CMod_PhysicsEndBSPCollisionTree() {
dVector worldMin, worldMax;
NewtonTreeCollisionEndBuild (g_collision, 1);
dMatrix worldMatrix (GetIdentityMatrix());
g_rigidBody = NewtonCreateBody (g_world, g_collision);
NewtonReleaseCollision (g_world, g_collision);
NewtonBodySetMatrix (g_rigidBody, &worldMatrix[0][0]);
NewtonCollisionCalculateAABB (g_collision, &worldMatrix[0][0], &worldMin[0], &worldMax[0]);
NewtonSetWorldSize (g_world, &worldMin[0], &worldMax[0]);
}
void Body::setPositionOrientation( Ogre::Quaternion orient, Ogre::Vector3 pos )
{
if (m_body)
{
float matrix[16];
OgreNewt::Converters::QuatPosToMatrix( orient, pos, &matrix[0] );
NewtonBodySetMatrix( m_body, &matrix[0] );
if (m_node)
{
m_node->setOrientation( orient );
m_node->setPosition( pos );
}
}
}
/*
=============
CMod_PhysicsAddEntity
=============
*/
void CMod_PhysicsAddEntity(sharedEntity_t * gEnt) {
NewtonCollision* collision = NULL;
NewtonBody* body = NULL;
std::map<int, bspCmodel>::iterator it = bspModels.find (gEnt->s.modelindex);
if ( it == bspModels.end() ) {
return;
}
vec3_t inertia, com;
dMatrix matrix (GetIdentityMatrix());
bspCmodel* bmodel = &it->second;
collision = NewtonCreateConvexHull (g_world, bmodel->vertices.size(), &bmodel->vertices[0].m_x, sizeof (dVector), 0.0f, &matrix[0][0]);
body = NewtonCreateBody (g_world, collision);
NewtonConvexCollisionCalculateVolume (collision);
NewtonReleaseCollision (g_world, collision);
bmodel->rigidBody = body;
NewtonBodySetMaterialGroupID (body, defaultMaterialGroup);
NewtonBodySetUserData (body, (void*)gEnt);
NewtonBodySetDestructorCallback (body, PhysicsEntityDie);
NewtonBodySetContinuousCollisionMode (body, 0);
NewtonBodySetForceAndTorqueCallback (body, PhysicsEntityThink);
NewtonBodySetTransformCallback (body, PhysicsEntitySetTransform);
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &com[0]);
NewtonBodySetCentreOfMass (body, &com[0]);
VectorScale (inertia, 10.0f, inertia); // The inertia needs to be scaled by the mass.
NewtonBodySetMassMatrix (body, 10.f, inertia[0], inertia[1], inertia[2]);
matrix.m_posit.m_x = gEnt->s.origin[0] * UNITS_PER_METRE;
matrix.m_posit.m_y = gEnt->s.origin[1] * UNITS_PER_METRE;
matrix.m_posit.m_z = gEnt->s.origin[2] * UNITS_PER_METRE;
NewtonBodySetMatrix (body, &matrix[0][0]);
gEnt->s.pos.trType = TR_INTERPOLATE;
VectorCopy (gEnt->s.origin, gEnt->s.pos.trBase);
VectorCopy (gEnt->s.origin, gEnt->r.currentOrigin);
gEnt->s.apos.trType = TR_INTERPOLATE;
VectorCopy (gEnt->s.angles, gEnt->s.apos.trBase);
VectorCopy (gEnt->s.angles, gEnt->r.currentAngles);
}
BasicPlayerEntity (DemoEntityManager* const scene, CustomPlayerControllerManager* const manager, dFloat radius, dFloat height, const dMatrix& location)
:DemoEntity (dGetIdentityMatrix(), NULL)
{
// add this entity to the scene for rendering
scene->Append(this);
// now make a simple player controller,
dMatrix playerAxis;
playerAxis[0] = dVector (0.0f, 1.0f, 0.0f, 0.0f); // the y axis is the character up vector
playerAxis[1] = dVector (1.0f, 0.0f, 0.0f, 0.0f); // the x axis is the character front direction
playerAxis[2] = playerAxis[0].CrossProduct(playerAxis[1]);
playerAxis[3] = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// make the player controller, this function makes a kinematic body
m_controller = manager->CreatePlayer(PLAYER_MASS, radius, radius * 0.5f, height, height * 0.33f, playerAxis);
//m_controller = manager->CreatePlayer(200.0f, 0.5f, 0.4f, 2.0f, 0.5f, playerAxis);
// players by default have the origin at the center of mass of the collision shape.
// you can change this by calling SetPlayerOrigin, for example if a player has its origin at the lower bottom of its AABB
m_controller->SetPlayerOrigin(height * 0.0f);
// set a restraining distance that the player can not get closet than
m_controller->SetRestrainingDistance(0.1f);
// players by default have the origin at the center of the lower bottom of the collision shape.
// you can change this by calling SetPlayerOrigin, for example if a player has it origin at the center of the AABB you can call
//m_controller->SetPlayerOrigin (height * 0.5f);
// get body from player, and set some parameter
NewtonBody* const body = m_controller->GetBody();
// set the user data
NewtonBodySetUserData(body, this);
// set the transform callback
NewtonBodySetTransformCallback (body, DemoEntity::TransformCallback);
// set the player matrix
NewtonBodySetMatrix(body, &location[0][0]);
// create the visual mesh from the player collision shape
NewtonCollision* const collision = NewtonBodyGetCollision(body);
DemoMesh* const geometry = new DemoMesh("player", collision, "smilli.tga", "smilli.tga", "smilli.tga");
SetMesh(geometry, dGetIdentityMatrix());
geometry->Release();
}
// TODO (#1#): use a "physicComponent" instead
void PhysicMap::addEntity(int id, const std::string& type)
{
PhysicEntityLoader loader(m_world);
std::string path("../media/" + type + ".def");
NewtonBody* body = loader.parseEntity(id, path, m_playerID);
if (body)
{
// TODO (Benjamin#1#): used as debug for uninitialised entity
Matrix_f matrix;
matrix.setPosition(Triplet_f(-5, -5, -5));
NewtonBodySetMatrix(body, matrix.raw());
NewtonBodySetForceAndTorqueCallback(body, playerForceAndTorque);
m_bodyMap.insert(BodyMap::value_type(id, body));
}
}
NewtonBody* CreateRigidBody (NewtonWorld* world, Entity* ent, NewtonCollision* collision, dFloat mass)
{
dVector minBox;
dVector maxBox;
dVector origin;
dVector inertia;
NewtonBody* body;
// Now with the collision Shape we can crate a rigid body
body = NewtonCreateBody (world, collision);
// bodies can have a destructor.
// this is a function callback that can be used to destroy any local data stored
// and that need to be destroyed before the body is destroyed.
NewtonBodySetDestructorCallback (body, DestroyBodyCallback);
// save the entity as the user data for this body
nEWTONbODySetUserData (body, ent);
// we need to set physics properties to this body
dMatrix matrix (ent->m_curRotation, ent->m_curPosition);
NewtonBodySetMatrix (body, &matrix[0][0]);
// we need to set the proper center of mass and inertia matrix for this body
// the inertia matrix calculated by this function does not include the mass.
// therefore it needs to be multiplied by the mass of the body before it is used.
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
// set the body mass matrix
NewtonBodySetMassMatrix (body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z);
// set the body origin
NewtonBodySetCentreOfMass (body, &origin[0]);
// set the function callback to apply the external forces and torque to the body
// the most common force is Gravity
NewtonBodySetForceAndTorqueCallback (body, ApplyForceAndTorqueCallback);
// set the function callback to set the transformation state of the graphic entity associated with this body
// each time the body change position and orientation in the physics world
NewtonBodySetTransformCallback (body, SetTransformCallback);
return body;
}
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
PhysicsActor::setOrientation(const Math::Matrix4& _orient)
{
// transfer values from input matrix to temporary matrix:
Math::Matrix4 matrix;
for (int i = 0; i < 16; i++)
{
matrix.m_array[i] = _orient.m_array[i];
}
// add offset to orientation matrix before setting body:
Math::Point3 pos;
pos = getPosition();
matrix.setPosition(pos);
setActivationState(true);
m_activationState = 1;
NewtonBodySetMatrix(m_pActor, matrix.m_array);
}
void PhysicMap::playerForceAndTorque(const NewtonBody* nBody)
{
int id = (int)(NewtonBodyGetUserData(nBody));
CompLocation* loc = CKernel::data()->getGameObjectLocation(id);
float fMasse, ixx, iyy, izz;
NewtonBodyGetMassMatrix(nBody, &fMasse, &ixx, &iyy, &izz);
// Apply gravity force
Triplet_f gravity(0, -fMasse * 9.81f, 0);
NewtonBodyAddForce(nBody, &gravity.x);
// Apply movment force
Triplet_f desiredVel = loc->desiredVel() * 20;
Triplet_f currentVel;
NewtonBodyGetVelocity(nBody,¤tVel.x);
Triplet_f forceApply = (desiredVel - currentVel) * fMasse * 10;
forceApply.y = 0;
NewtonBodyAddForce(nBody, &forceApply.x);
// Apply rotation
Matrix_f mat;
NewtonBodyGetMatrix(nBody, mat.raw());
mat.setRotationY(loc->desiredAngle());
NewtonBodySetMatrix(nBody, mat.raw());
// Apply shot recoil
if (loc->isImpact())
{
Triplet_f impact = loc->getNormal() * -3;
NewtonAddBodyImpulse(nBody, &impact.x, &mat.position().x);
}
// Update the game object
loc->setPosition(mat.position());
loc->setDirection(Triplet_f(loc->desiredAngle()));
loc->setVelocity(currentVel);
}
void ConvexCastPlacement (NewtonBody* body)
{
dFloat param;
dMatrix matrix;
NewtonWorld* world;
NewtonCollision* collision;
NewtonWorldConvexCastReturnInfo info[16];
NewtonBodyGetMatrix (body, &matrix[0][0]);
matrix.m_posit.m_y += 40.0f;
dVector p (matrix.m_posit);
p.m_y -= 80.0f;
world = NewtonBodyGetWorld(body);
collision = NewtonBodyGetCollision(body);
NewtonWorldConvexCast (world, &matrix[0][0], &p[0], collision, ¶m, body, ConvexCastCallback, info, 16, 0);
dAssert (param < 1.0f);
matrix.m_posit.m_y += (p.m_y - matrix.m_posit.m_y) * param;
NewtonBodySetMatrix(body, &matrix[0][0]);
}
void CreateScene (NewtonWorld* world, SceneManager* sceneManager)
{
Entity* floor;
NewtonCollision* shape;
NewtonBody* floorBody;
void* materialManager;
SoundManager* sndManager;
PhysicsMaterialInteration matInterations;
sndManager = sceneManager->GetSoundManager();
// Create the material for this scene, and attach it to the Newton World
materialManager = CreateMaterialManager (world, sndManager);
// add the Material table
matInterations.m_restitution = 0.6f;
matInterations.m_staticFriction = 0.6f;
matInterations.m_kineticFriction = 0.3f;
matInterations.m_scrapingSound = NULL;
matInterations.m_impactSound = sndManager->LoadSound ("metalMetal.wav");
AddMaterilInteraction (materialManager, m_metal, m_metal, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("boxBox.wav");
AddMaterilInteraction (materialManager, m_wood, m_wood, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("metalBox.wav");
AddMaterilInteraction (materialManager, m_metal, m_wood, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("grass0.wav");
AddMaterilInteraction (materialManager, m_wood, m_grass, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("boxHit.wav");
AddMaterilInteraction (materialManager, m_wood, m_bricks, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("grass1.wav");
AddMaterilInteraction (materialManager, m_metal, m_grass, &matInterations);
AddMaterilInteraction (materialManager, m_grass, m_bricks, &matInterations);
matInterations.m_impactSound = sndManager->LoadSound ("metal.wav");
AddMaterilInteraction (materialManager, m_metal, m_bricks, &matInterations);
AddMaterilInteraction (materialManager, m_grass, m_grass, &matInterations);
// Create a large body to be the floor
floor = sceneManager->CreateEntity();
floor->LoadMesh ("LevelMesh.dat");
// add static floor Physics
int materialMap[] = {m_bricks, m_grass, m_wood, m_metal};
shape = CreateMeshCollision (world, floor, materialMap);
floorBody = CreateRigidBody (world, floor, shape, 0.0f);
NewtonReleaseCollision (world, shape);
// set the Transformation Matrix for this rigid body
dMatrix matrix (floor->m_curRotation, floor->m_curPosition);
NewtonBodySetMatrix (floorBody, &matrix[0][0]);
// now we will use the properties of this body to set a proper world size.
dVector minBox;
dVector maxBox;
NewtonCollisionCalculateAABB (shape, &matrix[0][0], &minBox[0], &maxBox[0]);
// add some extra padding
minBox.m_x -= 50.0f;
minBox.m_y -= 500.0f;
minBox.m_z -= 50.0f;
maxBox.m_x += 50.0f;
maxBox.m_y += 500.0f;
maxBox.m_z += 50.0f;
// set the new world size
NewtonSetWorldSize (world, &minBox[0], &maxBox[0]);
// add some visual entities.
dFloat y0 = FindFloor (world, 0.0f, 0.0f) + 10.0f;
for (int i = 0; i < 5; i ++) {
Entity* smilly;
NewtonBody* smillyBody;
smilly = sceneManager->CreateEntity();
smilly->LoadMesh ("Smilly.dat");
smilly->m_curPosition.m_y = y0;
y0 += 2.0f;
smilly->m_prevPosition = smilly->m_curPosition;
// add a body with a box shape
shape = CreateNewtonBox (world, smilly, m_metal);
smillyBody = CreateRigidBody (world, smilly, shape, 10.0f);
NewtonReleaseCollision (world, shape);
}
// add some visual entities.
y0 = FindFloor (world, 0.0f, 0.4f) + 10.5f;
for (int i = 0; i < 5; i ++) {
Entity* frowny;
NewtonBody* frownyBody;
frowny = sceneManager->CreateEntity();
frowny->LoadMesh ("Frowny.dat");
frowny->m_curPosition.m_z = 0.4f;
frowny->m_curPosition.m_y = y0;
y0 += 2.0f;
frowny->m_prevPosition = frowny->m_curPosition;
// add a body with a Convex hull shape
shape = CreateNewtonConvex (world, frowny, m_wood);
frownyBody = CreateRigidBody (world, frowny, shape, 10.0f);
NewtonReleaseCollision (world, shape);
}
y0 = FindFloor (world, 0.0f, 2.0f) + 10.5f;
for (int i = 0; i < 5; i ++) {
Entity* frowny;
NewtonBody* frownyBody;
frowny = sceneManager->CreateEntity();
frowny->LoadMesh ("dumb.dat");
frowny->m_curPosition.m_z = 2.0f;
frowny->m_curPosition.m_y = y0;
y0 += 2.0f;
frowny->m_prevPosition = frowny->m_curPosition;
// add a body with a Convex hull shape
int materialMap[] = {m_grass, m_wood, m_metal, m_metal};
shape = CreateNewtonCompoundFromEntitySubmesh (world, frowny, materialMap);
frownyBody = CreateRigidBody (world, frowny, shape, 10.0f);
NewtonReleaseCollision (world, shape);
}
// set the Camera EyePoint close to the scene action
InitCamera (dVector (-15.0f, FindFloor (world, -15.0f, 0.0f) + 5.0f, 0.0f), dVector (1.0f, 0.0f, 0.0f));
}
