void Collider3D::ComputeInertialMatrix(Vector3f* inertia, Vector3f* center) const
{
float inertiaMatrix[3];
float origin[3];
// Si nous n'avons aucune instance, nous en créons une temporaire
if (m_handles.empty())
{
PhysWorld3D world;
NewtonCollision* collision = CreateHandle(&world);
{
NewtonConvexCollisionCalculateInertialMatrix(collision, inertiaMatrix, origin);
}
NewtonDestroyCollision(collision);
}
else // Sinon on utilise une instance au hasard (elles sont toutes identiques de toute façon)
NewtonConvexCollisionCalculateInertialMatrix(m_handles.begin()->second, inertiaMatrix, origin);
if (inertia)
inertia->Set(inertiaMatrix);
if (center)
center->Set(origin);
}
void Collider3D::ComputeInertialMatrix(Vector3f* inertia, Vector3f* center) const
{
float inertiaMatrix[3];
float origin[3];
// Check for existing collision handles, and create a temporary one if none is available
if (m_handles.empty())
{
PhysWorld3D world;
NewtonCollision* collision = CreateHandle(&world);
{
NewtonConvexCollisionCalculateInertialMatrix(collision, inertiaMatrix, origin);
}
NewtonDestroyCollision(collision);
}
else
NewtonConvexCollisionCalculateInertialMatrix(m_handles.begin()->second, inertiaMatrix, origin);
if (inertia)
inertia->Set(inertiaMatrix);
if (center)
center->Set(origin);
}
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;
}
NewtonBody* CPhysics::CreateRigidBody(NewtonWorld *world, CObject3D *object, NewtonCollision *collision, float mass)
{
object->UpdateMatrix();
float matrix[16];
object->matrixModel.FlattenToArray(matrix);
Vector3 angles = object->rotation * ToRad;
NewtonBody *body = NewtonCreateBody(world, collision, matrix);
if (mass > 0)
{
float inertia[3], origin[3];
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
NewtonBodySetMassMatrix(body, mass, mass * inertia[0], mass * inertia[1], mass * inertia[2]);
NewtonBodySetCentreOfMass(body, &origin[0]);
}
NewtonBodySetUserData(body, object);
NewtonBodySetTransformCallback(body, CPhysics::TransformCallback);
NewtonBodySetDestructorCallback(body, CPhysics::DestroyBodyCallback);
NewtonReleaseCollision(world, collision);
return body;
}
ShatterEffect(NewtonWorld* const world, NewtonMesh* const mesh, int interiorMaterial)
:dList<ShatterAtom>(), m_world (world)
{
// first we populate the bounding Box area with few random point to get some interior subdivisions.
// the subdivision are local to the point placement, by placing these points visual ally with a 3d tool
// and have precise control of how the debris are created.
// the number of pieces is equal to the number of point inside the Mesh plus the number of point on the mesh
dVector size;
dMatrix matrix(GetIdentityMatrix());
NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);
// pepper the inside of the BBox box of the mesh with random points
int count = 0;
dVector points[NUMBER_OF_ITERNAL_PARTS + 1];
while (count < NUMBER_OF_ITERNAL_PARTS) {
dFloat x = RandomVariable(size.m_x);
dFloat y = RandomVariable(size.m_y);
dFloat z = RandomVariable(size.m_z);
if ((x <= size.m_x) && (x >= -size.m_x) && (y <= size.m_y) && (y >= -size.m_y) && (z <= size.m_z) && (z >= -size.m_z)){
points[count] = dVector (x, y, z);
count ++;
}
}
// create a texture matrix, for applying the material's UV to all internal faces
dMatrix textureMatrix (GetIdentityMatrix());
textureMatrix[0][0] = 1.0f / size.m_x;
textureMatrix[1][1] = 1.0f / size.m_y;
// now we call create we decompose the mesh into several convex pieces
NewtonMesh* const debriMeshPieces = NewtonMeshVoronoiDecomposition (mesh, count, sizeof (dVector), &points[0].m_x, interiorMaterial, &textureMatrix[0][0]);
// Get the volume of the original mesh
NewtonCollision* const collision = NewtonCreateConvexHullFromMesh (m_world, mesh, 0.0f, 0);
dFloat volume = NewtonConvexCollisionCalculateVolume (collision);
NewtonReleaseCollision(m_world, collision);
// now we iterate over each pieces and for each one we create a visual entity and a rigid body
NewtonMesh* nextDebri;
for (NewtonMesh* debri = NewtonMeshCreateFirstLayer (debriMeshPieces); debri; debri = nextDebri) {
nextDebri = NewtonMeshCreateNextLayer (debriMeshPieces, debri);
NewtonCollision* const collision = NewtonCreateConvexHullFromMesh (m_world, debri, 0.0f, 0);
if (collision) {
ShatterAtom& atom = Append()->GetInfo();
atom.m_mesh = new DemoMesh(debri);
atom.m_collision = collision;
NewtonConvexCollisionCalculateInertialMatrix (atom.m_collision, &atom.m_momentOfInirtia[0], &atom.m_centerOfMass[0]);
dFloat debriVolume = NewtonConvexCollisionCalculateVolume (atom.m_collision);
atom.m_massFraction = debriVolume / volume;
}
NewtonMeshDestroy(debri);
}
NewtonMeshDestroy(debriMeshPieces);
}
static void AddShatterEntity (DemoEntityManager* const scene, DemoMesh* const visualMesh, NewtonCollision* const collision, const ShatterEffect& shatterEffect, dVector location)
{
dQuaternion rotation;
SimpleShatterEffectEntity* const entity = new SimpleShatterEffectEntity (visualMesh, shatterEffect);
entity->SetMatrix(*scene, rotation, location);
entity->InterpolateMatrix (*scene, 1.0f);
scene->Append(entity);
dVector origin;
dVector inertia;
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
float mass = 10.0f;
int materialId = 0;
dFloat Ixx = mass * inertia[0];
dFloat Iyy = mass * inertia[1];
dFloat Izz = mass * inertia[2];
//create the rigid body
dMatrix matrix (GetIdentityMatrix());
matrix.m_posit = location;
NewtonWorld* const world = scene->GetNewton();
NewtonBody* const rigidBody = NewtonCreateBody (world, collision, &matrix[0][0]);
entity->m_myBody = rigidBody;
entity->m_myweight = dAbs (mass * DEMO_GRAVITY);
// set the correct center of gravity for this body
NewtonBodySetCentreOfMass (rigidBody, &origin[0]);
// set the mass matrix
NewtonBodySetMassMatrix (rigidBody, mass, 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);
}
void NzBaseGeom::ComputeInertialMatrix(NzVector3f* inertia, NzVector3f* center) const
{
float inertiaMatrix[3];
float origin[3];
NewtonConvexCollisionCalculateInertialMatrix(m_collision, inertiaMatrix, origin);
if (inertia)
inertia->Set(inertiaMatrix);
if (center)
center->Set(origin);
}
static void AddFracturedEntity(DemoEntityManager* const scene, DemoMesh* const visualMesh, NewtonCollision* const collision, const FractureEffect& fractureEffect, const dVector& location)
{
dQuaternion rotation;
SimpleFracturedEffectEntity* const entity = new SimpleFracturedEffectEntity(visualMesh, fractureEffect);
entity->SetMatrix(*scene, rotation, location);
entity->InterpolateMatrix(*scene, 1.0f);
scene->Append(entity);
dVector origin(0.0f);
dVector inertia(0.0f);
NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]);
dFloat mass = 10.0f;
int materialId = 0;
//create the rigid body
dMatrix matrix(dGetIdentityMatrix());
matrix.m_posit = location;
NewtonWorld* const world = scene->GetNewton();
NewtonBody* const rigidBody = NewtonCreateDynamicBody(world, collision, &matrix[0][0]);
entity->m_myBody = rigidBody;
entity->m_myMassInverse = 1.0f / mass;
// set the correct center of gravity for this body
//NewtonBodySetCentreOfMass (rigidBody, &origin[0]);
// set the mass matrix
NewtonBodySetMassProperties(rigidBody, mass, 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);
}
void cPhysicsBodyNewton::SetMass(float afMass)
{
cCollideShapeNewton *pShapeNewton = static_cast<cCollideShapeNewton*>(mpShape);
cVector3f vInertia;// = pShapeNewton->GetInertia(afMass);
cVector3f vOffset;
NewtonConvexCollisionCalculateInertialMatrix(pShapeNewton->GetNewtonCollision(),
vInertia.v, vOffset.v);
vInertia = vInertia * afMass;
NewtonBodySetCentreOfMass(mpNewtonBody,vOffset.v);
NewtonBodySetMassMatrix(mpNewtonBody, afMass, vInertia.x, vInertia.y, vInertia.z);
mfMass = afMass;
}
DelaunayEffect(NewtonWorld* const world, NewtonMesh* const mesh, int interiorMaterial)
:FractureEffect(world)
{
// first we populate the bounding Box area with few random point to get some interior subdivisions.
// the subdivision are local to the point placement, by placing these points visual ally with a 3d tool
// and have precise control of how the debris are created.
// the number of pieces is equal to the number of point inside the Mesh plus the number of point on the mesh
dVector size(0.0f);
dMatrix matrix(dGetIdentityMatrix());
NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);
// create a texture matrix, for applying the material's UV to all internal faces
dMatrix textureMatrix(dGetIdentityMatrix());
textureMatrix[0][0] = 1.0f / size.m_x;
textureMatrix[1][1] = 1.0f / size.m_y;
// Get the volume of the original mesh
NewtonCollision* const collision1 = NewtonCreateConvexHullFromMesh(m_world, mesh, 0.0f, 0);
dFloat volume = NewtonConvexCollisionCalculateVolume(collision1);
NewtonDestroyCollision(collision1);
// now we call create we decompose the mesh into several convex pieces
NewtonMesh* const debriMeshPieces = NewtonMeshCreateTetrahedraIsoSurface(mesh);
dAssert(debriMeshPieces);
// now we iterate over each pieces and for each one we create a visual entity and a rigid body
NewtonMesh* nextDebri;
for (NewtonMesh* debri = NewtonMeshCreateFirstLayer(debriMeshPieces); debri; debri = nextDebri) {
// get next segment piece
nextDebri = NewtonMeshCreateNextLayer(debriMeshPieces, debri);
//clip the Delaunay convexes against the mesh, make a convex hull collision shape
NewtonCollision* const collision = NewtonCreateConvexHullFromMesh(m_world, debri, 0.0f, 0);
if (collision) {
// we have a piece which has a convex collision representation, add that to the list
FractureAtom& atom = Append()->GetInfo();
atom.m_mesh = new DemoMesh(debri);
atom.m_collision = collision;
NewtonConvexCollisionCalculateInertialMatrix(atom.m_collision, &atom.m_momentOfInirtia[0], &atom.m_centerOfMass[0]);
dFloat debriVolume = NewtonConvexCollisionCalculateVolume(atom.m_collision);
atom.m_massFraction = debriVolume / volume;
}
NewtonMeshDestroy(debri);
}
NewtonMeshDestroy(debriMeshPieces);
}
/*
=============
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);
}
NewtonUserJoint* CreateRayCastCast (NewtonBody* body, SceneManager* sceneManager, dFloat tirePosit[4][3])
{
dVector minBox;
dVector maxBox;
dVector origin(0, 0, 0, 1);
dVector inertia(0, 0, 0, 1);
NewtonUserJoint* carJoint;
NewtonCollision* chassisCollision;
// the first thing we need to do is to place the vehicle center of Mass on a stable position for a car
// calculate the moment of inertia and the relative center of mass of the solid
chassisCollision = NewtonBodyGetCollision(body);
NewtonConvexCollisionCalculateInertialMatrix (chassisCollision, &inertia[0], &origin[0]);
CalculateBoxdingBox (chassisCollision, minBox, maxBox);
//displace the center of mass by some value
origin.m_y -= 0.5f * (maxBox.m_y - minBox.m_y) * LOW_CENTER_OF_MASS_FACTOR;
// now we Set a new center of mass for this car
NewtonBodySetCentreOfMass (body, &origin[0]);
// Next we need to set the internal coordinate system of the car geometry
// this particular demo the car direction of motion is axis (1, 0, 0)
// the car up direction (0, 1, 0);
// the car lateral direction is the cross product of the front and vertical axis
dMatrix chassisMatrix;
chassisMatrix.m_front = dVector (1.0f, 0.0f, 0.0f, 0.0f); // this is the vehicle direction of travel
chassisMatrix.m_up = dVector (0.0f, 1.0f, 0.0f, 0.0f); // this is the downward vehicle direction
chassisMatrix.m_right = chassisMatrix.m_front * chassisMatrix.m_up; // this is in the side vehicle direction (the plane of the wheels)
chassisMatrix.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// create a vehicle joint with 4 tires
carJoint = DGRaycastVehicleCreate (4, &chassisMatrix[0][0], body);
// we need to set a transform call back to render the tire entities
DGRaycastVehicleSetTireTransformCallback (carJoint, TireTransformCallback);
// now we will add four tires to this vehicle,
AddTire (carJoint, "leftTire.dat", dVector (tirePosit[0][0], tirePosit[0][1], tirePosit[0][2], 0.0f), sceneManager);
AddTire (carJoint, "rightTire.dat", dVector (tirePosit[1][0], tirePosit[1][1], tirePosit[1][2], 0.0f), sceneManager);
AddTire (carJoint, "leftTire.dat", dVector (tirePosit[2][0], tirePosit[2][1], tirePosit[2][2], 0.0f), sceneManager);
AddTire (carJoint, "rightTire.dat", dVector (tirePosit[3][0], tirePosit[3][1], tirePosit[3][2], 0.0f), sceneManager);
return carJoint;
}
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;
}
void Car::initPhysics() {
NewtonWorld * nWorld = this->controller->getWorld();
NewtonCollision* collision;
float mass = 900.0f;
vector3df v1 = this->carNode->getBoundingBox().MinEdge;
vector3df v2 = this->carNode->getBoundingBox().MaxEdge;
dVector minBox(v1.X, v1.Y, v1.Z);
dVector maxBox(v2.X, v2.Y, v2.Z);
dVector size(maxBox - minBox);
dVector origin((maxBox + minBox).Scale(0.5f));
size.m_w = 1.0f;
origin.m_w = 1.0f;
dMatrix offset(GetIdentityMatrix());
offset.m_posit = origin;
collision = NewtonCreateBox(nWorld, size.m_x, size.m_y, size.m_z, 0, &offset[0][0]);
dVector inertia;
matrix4 m = this->carNode->getRelativeTransformation();
NewtonConvexHullModifierSetMatrix(collision, m.pointer());
NewtonBody * body = NewtonCreateBody(nWorld, collision, m.pointer());
NewtonBodySetUserData(body, this);
NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]);
NewtonBodySetMassMatrix(body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z);
NewtonBodySetCentreOfMass(body, &origin[0]);
NewtonBodySetForceAndTorqueCallback(body, applyCarMoveForce);
NewtonBodySetTransformCallback(body, applyCarTransform);
int matId = NewtonMaterialGetDefaultGroupID(nWorld);
NewtonMaterialSetCollisionCallback(nWorld, matId, matId, this, 0, applyCarCollisionForce);
NewtonReleaseCollision(nWorld, collision);
this->setCarBodyAndGravity(body, dVector(0,-10,0,0));
this->setLocalCoordinates(this->createChassisMatrix());
}
void RigidBodyUIPane::SetSelectionMass (dFloat mass)
{
RigidBodyWorldDesc* const plugin = (RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor();
Interface* const ip = GetCOREInterface();
int selectionCount = ip->GetSelNodeCount();
for (int i = 0; i < selectionCount; i ++) {
INode* const node = ip->GetSelNode(i);
RigidBodyController* const bodyInfo = (RigidBodyController*)plugin->GetRigidBodyControl(node);
if (bodyInfo) {
bodyInfo->m_mass = mass;
_ASSERTE (bodyInfo->m_body);
// dVector inertia (bodyInfo->m_inertia.Scale (mass));
// NewtonBodySetMassMatrix(bodyInfo->m_body, mass, inertia.m_x, inertia.m_y, inertia.m_z);
NewtonCollisionInfoRecord info;
NewtonCollision* const collision = NewtonBodyGetCollision(bodyInfo->m_body);
NewtonCollisionGetInfo (collision, &info);
switch (info.m_collisionType)
{
case SERIALIZE_ID_BOX:
case SERIALIZE_ID_CONE:
case SERIALIZE_ID_SPHERE:
case SERIALIZE_ID_CAPSULE:
case SERIALIZE_ID_CYLINDER:
case SERIALIZE_ID_COMPOUND:
case SERIALIZE_ID_CONVEXHULL:
case SERIALIZE_ID_CONVEXMODIFIER:
case SERIALIZE_ID_CHAMFERCYLINDER:
{
NewtonConvexCollisionCalculateInertialMatrix (collision, &bodyInfo->m_inertia[0], &bodyInfo->m_origin[0]);
NewtonBodySetCentreOfMass(bodyInfo->m_body, &bodyInfo->m_origin[0]);
NewtonBodySetMassMatrix(bodyInfo->m_body, bodyInfo->m_mass, bodyInfo->m_mass * bodyInfo->m_inertia.m_x, bodyInfo->m_mass * bodyInfo->m_inertia.m_y, bodyInfo->m_mass * bodyInfo->m_inertia.m_z);
}
}
}
}
}
Car::Car(const ion::base::String& identifier,NewtonWorld *pWorld,const ion::video::Mesh& srcmesh,const float mass,
const float Ixx,const float Iyy,const float Izz):Node(identifier),m_pNewtonChassisCollision(0),m_pBody(0),
m_pNewtonworld(pWorld),m_pNewtonJoint(0)
{
if (pWorld==0) {
ion::base::log("Car::Car()",ion::base::Error) << "No NewtonWorld pointer given\n";
return;
}
if (!srcmesh.isValid()) {
ion::base::log("Car::Car()",ion::base::Error) << "Source mesh is invalid\n";
return;
}
if (!srcmesh.vertexstream().isMapped()) {
ion::base::log("Car::Car()",ion::base::Error) << "source mesh \"" << srcmesh.objIdentifier() << " is not mapped!\n";
return;
}
if ((m_pNewtonChassisCollision!=0) && (m_pNewtonworld!=0))
NewtonReleaseCollision(m_pNewtonworld,m_pNewtonChassisCollision);
float *pPoints;
{
pPoints=new float[3*srcmesh.vertexstream().capacity()];
for (ion_uint32 v=0;v<srcmesh.vertexstream().capacity();++v) {
const ion::math::Vector3f &rV=srcmesh.vertexstream().position(v);
pPoints[v*3+0]=rV.x();
pPoints[v*3+1]=rV.y();
pPoints[v*3+2]=rV.z();
}
}
ion::math::Matrix4f c;
m_pNewtonworld=pWorld;
m_pNewtonChassisCollision=NewtonCreateConvexHull(m_pNewtonworld,srcmesh.vertexstream().capacity(),pPoints,12,c);
m_pBody=NewtonCreateBody(m_pNewtonworld,m_pNewtonChassisCollision);
NewtonBodySetUserData(m_pBody,this);
ion::math::Vector3f origin,inertia;
// calculate the moment of inertia and the relative center of mass of the solid
NewtonConvexCollisionCalculateInertialMatrix (m_pNewtonChassisCollision, &inertia[0], &origin[0]);
float ixx = mass * inertia[0];
float iyy = mass * inertia[1];
float izz = mass * inertia[2];
// set the mass matrix
NewtonBodySetMassMatrix (m_pBody, mass, ixx, iyy, izz);
origin.y()=-1;
NewtonBodySetCentreOfMass (m_pBody, &origin[0]);
NewtonBodySetMatrix(m_pBody,localTransform().matrix());
NewtonReleaseCollision(m_pNewtonworld,m_pNewtonChassisCollision);
NewtonBodySetTransformCallback (m_pBody, physicsSetTransform);
NewtonBodySetForceAndTorqueCallback (m_pBody, physicsApplyForceAndTorque);
float updir[3]={0,1,0};
m_pNewtonJoint=NewtonConstraintCreateVehicle(m_pNewtonworld,&updir[0],m_pBody);
NewtonVehicleSetTireCallback(m_pNewtonJoint,tireUpdate);
delete [] pPoints;
}
VoronoidEffect(NewtonWorld* const world, NewtonMesh* const mesh, int interiorMaterial)
:FractureEffect(world)
{
// first we populate the bounding Box area with few random point to get some interior subdivisions.
// the subdivision are local to the point placement, by placing these points visual ally with a 3d tool
// and have precise control of how the debris are created.
// the number of pieces is equal to the number of point inside the Mesh plus the number of point on the mesh
dVector size(0.0f);
dMatrix matrix(dGetIdentityMatrix());
NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);
dVector points[NUMBER_OF_INTERNAL_PARTS + 8];
int count = 0;
// pepper the inside of the BBox box of the mesh with random points
while (count < NUMBER_OF_INTERNAL_PARTS) {
dFloat x = dGaussianRandom (size.m_x);
dFloat y = dGaussianRandom (size.m_y);
dFloat z = dGaussianRandom (size.m_z);
if ((x <= size.m_x) && (x >= -size.m_x) && (y <= size.m_y) && (y >= -size.m_y) && (z <= size.m_z) && (z >= -size.m_z)) {
points[count] = dVector(x, y, z);
count++;
}
}
// add the bounding box as a safeguard area
points[count + 0] = dVector(size.m_x, size.m_y, size.m_z, 0.0f);
points[count + 1] = dVector(size.m_x, size.m_y, -size.m_z, 0.0f);
points[count + 2] = dVector(size.m_x, -size.m_y, size.m_z, 0.0f);
points[count + 3] = dVector(size.m_x, -size.m_y, -size.m_z, 0.0f);
points[count + 4] = dVector(-size.m_x, size.m_y, size.m_z, 0.0f);
points[count + 5] = dVector(-size.m_x, size.m_y, -size.m_z, 0.0f);
points[count + 6] = dVector(-size.m_x, -size.m_y, size.m_z, 0.0f);
points[count + 7] = dVector(-size.m_x, -size.m_y, -size.m_z, 0.0f);
count += 8;
// create a texture matrix, for applying the material's UV to all internal faces
dMatrix textureMatrix(dGetIdentityMatrix());
textureMatrix[0][0] = 1.0f / size.m_x;
textureMatrix[1][1] = 1.0f / size.m_y;
// Get the volume of the original mesh
NewtonCollision* const collision1 = NewtonCreateConvexHullFromMesh(m_world, mesh, 0.0f, 0);
dFloat volume = NewtonConvexCollisionCalculateVolume(collision1);
NewtonDestroyCollision(collision1);
// now we call create we decompose the mesh into several convex pieces
NewtonMesh* const debriMeshPieces = NewtonMeshCreateVoronoiConvexDecomposition(m_world, count, &points[0].m_x, sizeof (dVector), interiorMaterial, &textureMatrix[0][0]);
dAssert(debriMeshPieces);
// now we iterate over each pieces and for each one we create a visual entity and a rigid body
NewtonMesh* nextDebri;
for (NewtonMesh* debri = NewtonMeshCreateFirstLayer(debriMeshPieces); debri; debri = nextDebri) {
// get next segment piece
nextDebri = NewtonMeshCreateNextLayer(debriMeshPieces, debri);
//clip the voronoi convexes against the mesh
NewtonMesh* const fracturePiece = NewtonMeshConvexMeshIntersection(mesh, debri);
if (fracturePiece) {
// make a convex hull collision shape
NewtonCollision* const collision = NewtonCreateConvexHullFromMesh(m_world, fracturePiece, 0.0f, 0);
if (collision) {
// we have a piece which has a convex collision representation, add that to the list
FractureAtom& atom = Append()->GetInfo();
atom.m_mesh = new DemoMesh(fracturePiece);
atom.m_collision = collision;
NewtonConvexCollisionCalculateInertialMatrix(atom.m_collision, &atom.m_momentOfInirtia[0], &atom.m_centerOfMass[0]);
dFloat debriVolume = NewtonConvexCollisionCalculateVolume(atom.m_collision);
atom.m_massFraction = debriVolume / volume;
}
NewtonMeshDestroy(fracturePiece);
}
NewtonMeshDestroy(debri);
}
NewtonMeshDestroy(debriMeshPieces);
}
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);
}
static void AddStructuredFractured (DemoEntityManager* const scene, const dVector& origin, int materialID, const char* const assetName)
{
// create the shape and visual mesh as a common data to be re used
NewtonWorld* const world = scene->GetNewton();
#if 0
// load the mesh asset
DemoEntity entity(GetIdentityMatrix(), NULL);
entity.LoadNGD_mesh (assetName, world);
DemoMesh____* const mesh = entity.GetMesh();
dAssert (mesh);
// convert the mesh to a newtonMesh
NewtonMesh* const solidMesh = mesh->CreateNewtonMesh (world, entity.GetMeshMatrix() * entity.GetCurrentMatrix());
#else
int externalMaterial = LoadTexture("wood_0.tga");
NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), dVector (3.0f, 3.0f, 3.0f, 0.0), _BOX_PRIMITIVE, 0);
NewtonMesh* const solidMesh = NewtonMeshCreateFromCollision(collision);
NewtonDestroyCollision(collision);
//NewtonMeshTriangulate(solidMesh);
NewtonMeshApplyBoxMapping (solidMesh, externalMaterial, externalMaterial, externalMaterial);
#endif
// create a random point cloud
dVector points[MAX_POINT_CLOUD_SIZE];
int pointCount = MakeRandomPoisonPointCloud (solidMesh, points);
// int pointCount = MakeRandomGuassianPointCloud (solidMesh, points, MAX_POINT_CLOUD_SIZE);
// create and interiors material for texturing the fractured pieces
//int internalMaterial = LoadTexture("KAMEN-stup.tga");
int internalMaterial = LoadTexture("concreteBrick.tga");
// crate a texture matrix for uv mapping of fractured pieces
dMatrix textureMatrix (dGetIdentityMatrix());
textureMatrix[0][0] = 1.0f / 2.0f;
textureMatrix[1][1] = 1.0f / 2.0f;
/// create the fractured collision and mesh
int debreePhysMaterial = NewtonMaterialGetDefaultGroupID(world);
NewtonCollision* structuredFracturedCollision = NewtonCreateFracturedCompoundCollision (world, solidMesh, 0, debreePhysMaterial, pointCount, &points[0][0], sizeof (dVector), internalMaterial, &textureMatrix[0][0],
OnReconstructMainMeshCallBack, OnEmitFracturedCompound, OnEmitFracturedChunk);
// uncomment this to test serialization
#if 0
FILE* file = fopen ("serialize.bin", "wb");
NewtonCollisionSerialize (world, structuredFracturedCollision, DemoEntityManager::SerializeFile, file);
NewtonDestroyCollision (structuredFracturedCollision);
fclose (file);
file = fopen ("serialize.bin", "rb");
structuredFracturedCollision = NewtonCreateCollisionFromSerialization (world, DemoEntityManager::DeserializeFile, file);
NewtonFracturedCompoundSetCallbacks (structuredFracturedCollision, OnReconstructMainMeshCallBack, OnEmitFracturedCompound, OnEmitFracturedChunk);
fclose (file);
#endif
#if 0
// test the interface
dTree<void*, void*> detachableNodes;
NewtonCompoundCollisionBeginAddRemove(structuredFracturedCollision);
// remove all chunk that can be detached for the first layer
for (void* node = NewtonCompoundCollisionGetFirstNode(structuredFracturedCollision); node; node = NewtonCompoundCollisionGetNextNode(structuredFracturedCollision, node)) {
if (NewtonFracturedCompoundIsNodeFreeToDetach (structuredFracturedCollision, node)) {
detachableNodes.Insert(node, node);
}
// remove any node that can be deched fro the secund layer, this codul; be reusive
void* neighbors[32];
int count = NewtonFracturedCompoundNeighborNodeList (structuredFracturedCollision, node, neighbors, sizeof (neighbors) / sizeof (neighbors[0]));
for (int i = 0; i < count; i ++ ) {
if (NewtonFracturedCompoundIsNodeFreeToDetach (structuredFracturedCollision, neighbors[i])) {
detachableNodes.Insert(node, node);
}
}
}
// now delete the actual nodes
dTree<void*, void*>::Iterator iter (detachableNodes) ;
for (iter.Begin(); iter; iter ++) {
void* const node = iter.GetNode()->GetInfo();
NewtonCompoundCollisionRemoveSubCollision (structuredFracturedCollision, node);
}
NewtonCompoundCollisionEndAddRemove(structuredFracturedCollision);
#endif
#if 1
dVector plane (0.0f, 1.0f, 0.0f, 0.0f);
NewtonCollision* const crack = NewtonFracturedCompoundPlaneClip (structuredFracturedCollision, &plane[0]);
if (crack) {
NewtonDestroyCollision (structuredFracturedCollision);
}
#endif
dVector com(0.0f);
dVector inertia(0.0f);
NewtonConvexCollisionCalculateInertialMatrix (structuredFracturedCollision, &inertia[0], &com[0]);
//dFloat mass = 10.0f;
//int materialId = 0;
//create the rigid body
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = origin;
matrix.m_posit.m_y = 20.0;
matrix.m_posit.m_w = 1.0f;
NewtonBody* const rigidBody = NewtonCreateDynamicBody (world, structuredFracturedCollision, &matrix[0][0]);
// set the mass and center of mass
dFloat density = 1.0f;
dFloat mass = density * NewtonConvexCollisionCalculateVolume (structuredFracturedCollision);
NewtonBodySetMassProperties (rigidBody, mass, structuredFracturedCollision);
// set the transform call back function
NewtonBodySetTransformCallback (rigidBody, DemoEntity::TransformCallback);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce);
// create the entity and visual mesh and attach to the body as user data
CreateVisualEntity (scene, rigidBody);
// assign the wood id
// NewtonBodySetMaterialGroupID (rigidBody, materialId);
// set a destructor for this rigid body
// NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor);
// release the interior texture
// ReleaseTexture (internalMaterial);
// delete the solid mesh since it no longed needed
NewtonMeshDestroy (solidMesh);
// destroy the fracture collision
NewtonDestroyCollision (structuredFracturedCollision);
}
int CustomMultiBodyVehicle::AddSingleSuspensionTire (
void* userData,
const dVector& localPosition,
dFloat mass,
dFloat radius,
dFloat width,
dFloat suspensionLength,
dFloat springConst,
dFloat springDamper)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dMatrix carMatrix;
NewtonBody* tire;
NewtonWorld* world;
NewtonCollision *collision;
world = NewtonBodyGetWorld(GetBody0());
// create the tire RogidBody
collision = NewtonCreateChamferCylinder(world, radius, width, 0, NULL);
//create the rigid body
tire = NewtonCreateBody (world, collision);
// release the collision
NewtonReleaseCollision (world, collision);
// save the user data
NewtonBodySetUserData (tire, userData);
// set the material group id for vehicle
NewtonBodySetMaterialGroupID (tire, 0);
// NewtonBodySetMaterialGroupID (tire, woodID);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback (tire, NewtonBodyGetForceAndTorqueCallback (GetBody0()));
// body part do not collision
NewtonBodySetJointRecursiveCollision (tire, 0);
// calculate the moment of inertia and the relative center of mass of the solid
dVector origin;
dVector inertia;
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
Ixx = mass * inertia[0];
Iyy = mass * inertia[1];
Izz = mass * inertia[2];
// set the mass matrix
NewtonBodySetMassMatrix (tire, mass, Ixx, Iyy, Izz);
// calculate the tire local base pose matrix
dMatrix tireMatrix;
tireMatrix.m_front = m_localFrame.m_right;
tireMatrix.m_up = m_localFrame.m_up;
tireMatrix.m_right = tireMatrix.m_front * tireMatrix.m_up;
tireMatrix.m_posit = localPosition;
NewtonBodyGetMatrix(GetBody0(), &carMatrix[0][0]);
tireMatrix = tireMatrix * carMatrix;
// set the matrix for both the rigid body and the graphic body
NewtonBodySetMatrix (tire, &tireMatrix[0][0]);
// add a single tire
m_tires[m_tiresCount] = new CustomMultiBodyVehicleTire (GetBody0(), tire, suspensionLength, springConst, springDamper, radius);
m_tiresCount ++;
return m_tiresCount - 1;
}
