static void MagneticField (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
dFloat magnetStregnth;
const NewtonBody* body0;
const NewtonBody* body1;
const NewtonBody* magneticField;
const NewtonBody* magneticPiece;
body0 = NewtonJointGetBody0 (contactJoint);
body1 = NewtonJointGetBody1 (contactJoint);
// get the magnetic field body
magneticPiece = body0;
magneticField = body1;
if (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(body0))) {
magneticPiece = body1;
magneticField = body0;
}
_ASSERTE (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(magneticField)));
// calculate the magnetic force field
dMatrix center;
dMatrix location;
NewtonBodyGetMatrix (magneticField, ¢er[0][0]);
NewtonBodyGetMatrix (magneticPiece, &location[0][0]);
Magnet* magnet;
magnet = (Magnet*)NewtonBodyGetUserData(magneticField);
magnetStregnth = magnet->m_magnetStregnth;
// calculate the magnetic force;
dFloat den;
dVector force (center.m_posit - location.m_posit);
den = force % force;
den = magnetStregnth / (den * dSqrt (den) + 0.1f);
force = force.Scale (den);
// because we are modifiing one of the bodies membber in the call back, there uis a chace that
// another materail can be operations on the same object at the same time of aother thread
// therfore we need to make the assigmnet in a critical section.
NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (magneticPiece));
// add the magner force
NewtonBodyAddForce (magneticPiece, &force[0]);
force = force.Scale (-1.0f);
NewtonBodyAddForce (magnet->m_magneticCore, &force[0]);
// also if the body is sleeping fore it to wake up for this frame
NewtonBodySetFreezeState (magneticPiece, 0);
NewtonBodySetFreezeState (magnet->m_magneticCore, 0);
// unlock the critical section
NewtonWorldCriticalSectionUnlock (NewtonBodyGetWorld (magneticPiece));
}
void GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
int isHightField;
NewtonBody* body;
NewtonCollision* collision;
NewtonCollisionInfoRecord info;
isHightField = 1;
body = NewtonJointGetBody0 (contactJoint);
collision = NewtonBodyGetCollision(body);
NewtonCollisionGetInfo(collision, &info);
if (info.m_collisionType != SERIALIZE_ID_HEIGHTFIELD) {
body = NewtonJointGetBody1 (contactJoint);
collision = NewtonBodyGetCollision(body);
NewtonCollisionGetInfo(collision, &info);
isHightField = (info.m_collisionType == SERIALIZE_ID_HEIGHTFIELD);
}
#define HOLE_IN_TERRAIN 10
if (isHightField) {
void* nextContact;
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = nextContact) {
int faceID;
NewtonMaterial* material;
nextContact = NewtonContactJointGetNextContact (contactJoint, contact);
material = NewtonContactGetMaterial (contact);
faceID = NewtonMaterialGetContactFaceAttribute (material);
if (faceID == HOLE_INTERRAIN) {
NewtonContactJointRemoveContact (contactJoint, contact);
}
}
}
}
int NewtonBodyCollide(NewtonWorld* world, int maxsize, NewtonBody* body0, NewtonBody* body1, float* contacts, float* normals, float* penetration)
{
NewtonCollision* collision[2];
float mat0[16];
float mat1[16];
collision[0]=NewtonBodyGetCollision(body0);
collision[1]=NewtonBodyGetCollision(body1);
NewtonBodyGetMatrix(body0,mat0);
NewtonBodyGetMatrix(body1,mat1);
return NewtonCollisionCollide(world,maxsize,collision[0],mat0,collision[1],mat1,contacts,normals,penetration,0);
}
VALUE MSNewton::Bodies::touching(VALUE self, VALUE v_body1, VALUE v_body2) {
const NewtonBody* body1 = Util::value_to_body(v_body1);
const NewtonBody* body2 = Util::value_to_body(v_body2);
Util::validate_two_bodies(body1, body2);
const NewtonWorld* world = NewtonBodyGetWorld(body1);
NewtonCollision* colA = NewtonBodyGetCollision(body1);
NewtonCollision* colB = NewtonBodyGetCollision(body2);
dMatrix matrixA;
dMatrix matrixB;
NewtonBodyGetMatrix(body1, &matrixA[0][0]);
NewtonBodyGetMatrix(body2, &matrixB[0][0]);
return NewtonCollisionIntersectionTest(world, colA, &matrixA[0][0], colB, &matrixB[0][0], 0) == 1 ? Qtrue : Qfalse;
}
void cPhysicsBodyNewton::RenderDebugGeometry(iLowLevelGraphics *a_pLowLevel, const cColor &a_Color)
{
//cPhysicsWorldNewton *pPhysicsWorld = static_cast<cPhysicsWorldNewton*>(NewtonWorldGetUserData(m_pNewtonWorld));
//pPhysicsWorld->GetWorld3D()->gets
NewtonCollision *pCollision = NewtonBodyGetCollision(m_pNewtonBody);
float matrix[4][4];
NewtonBodyGetMatrix(m_pNewtonBody, &matrix[0][0]);
g_pLowLevelGraphics = a_pLowLevel;
g_DebugColor = a_Color;
NewtonCollision *pNewtonCollision = NewtonBodyGetCollision(m_pNewtonBody);
NewtonCollisionForEachPolygonDo(pNewtonCollision,
m_mtxLocalTransform.GetTranspose().m[0], RenderDebugPolygon, this);
}
static void UserContactFriction (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
// call the basic call back
GenericContactProcess (contactJoint, timestep, threadIndex);
const NewtonBody* const body0 = NewtonJointGetBody0(contactJoint);
const NewtonBody* const body1 = NewtonJointGetBody1(contactJoint);
const NewtonBody* body = body0;
NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
if (mass == 0.0f) {
body = body1;
}
//now core 300 can have per collision user data
NewtonCollision* const collision = NewtonBodyGetCollision(body);
void* userData = NewtonCollisionGetUserData (collision);
dFloat frictionValue = *((dFloat*)&userData);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 0);
NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 1);
}
}
NewtonBody* CreateBodyPart(DemoEntity* const bodyPart, const dArmRobotConfig& definition)
{
NewtonCollision* const shape = MakeConvexHull(bodyPart);
// calculate the bone matrix
dMatrix matrix(bodyPart->CalculateGlobalMatrix());
NewtonWorld* const world = GetWorld();
// create the rigid body that will make this bone
NewtonBody* const body = NewtonCreateDynamicBody(world, shape, &matrix[0][0]);
// destroy the collision helper shape
NewtonDestroyCollision(shape);
// get the collision from body
NewtonCollision* const collision = NewtonBodyGetCollision(body);
// calculate the moment of inertia and the relative center of mass of the solid
NewtonBodySetMassProperties(body, definition.m_mass, collision);
//NewtonBodySetMassProperties(body, 0.0f, collision);
// save the user lifterData with the bone body (usually the visual geometry)
NewtonBodySetUserData(body, bodyPart);
// assign a body part id
//NewtonCollisionSetUserID(collision, definition.m_bodyPartID);
// set the bod part force and torque call back to the gravity force, skip the transform callback
NewtonBodySetForceAndTorqueCallback(body, PhysicsApplyGravityForce);
return body;
}
void RenderAABB (NewtonWorld* const world)
{
glDisable (GL_LIGHTING);
glDisable(GL_TEXTURE_2D);
glColor3f(0.0f, 0.0f, 1.0f);
glBegin(GL_LINES);
//glVertex3f (-20.3125000f, 3.54991579f, 34.3441200f);
//glVertex3f (-19.6875000f, 3.54257250f, 35.2211456f);
for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
dVector p0;
dVector p1;
dMatrix matrix;
NewtonCollision* const collision = NewtonBodyGetCollision(body);
NewtonBodyGetMatrix (body, &matrix[0][0]);
NewtonCollisionCalculateAABB (collision, &matrix[0][0], &p0[0], &p1[0]);
// CalculateAABB (collision, matrix, p0, p1);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p0.m_y, p0.m_z);
glVertex3f (p0.m_x, p1.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p0.m_z);
glVertex3f (p0.m_x, p1.m_y, p1.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
glVertex3f (p0.m_x, p0.m_y, p1.m_z);
glVertex3f (p1.m_x, p0.m_y, p1.m_z);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p0.m_x, p1.m_y, p0.m_z);
glVertex3f (p1.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p0.m_z);
glVertex3f (p0.m_x, p0.m_y, p1.m_z);
glVertex3f (p0.m_x, p1.m_y, p1.m_z);
glVertex3f (p1.m_x, p0.m_y, p1.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p0.m_x, p0.m_y, p1.m_z);
glVertex3f (p1.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p0.m_y, p1.m_z);
glVertex3f (p0.m_x, p1.m_y, p0.m_z);
glVertex3f (p0.m_x, p1.m_y, p1.m_z);
glVertex3f (p1.m_x, p1.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
}
glEnd();
}
static void GetCollisionSubShape(const NewtonJoint* const contactJoint, NewtonBody* const body)
{
NewtonCollisionInfoRecord collisionInfo;
NewtonCollision* const collision = NewtonBodyGetCollision(body);
NewtonCollisionGetInfo (collision, &collisionInfo);
int count = 0;
NewtonCollision* collidingSubShapeArrar[32];
// see if this is a compound collision or any other collision with sub collision shapes
if (collisionInfo.m_collisionType == SERIALIZE_ID_COMPOUND) {
// to get the torque we need the center of gravity in global space
dVector origin;
dMatrix bodyMatrix;
NewtonBodyGetMatrix(body, &bodyMatrix[0][0]);
NewtonBodyGetCentreOfMass(body, &origin[0]);
origin = bodyMatrix.TransformVector(origin);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
// get the material of this contact,
// this part contain all contact information, the sub colliding shape,
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonCollision* const subShape = NewtonMaterialGetBodyCollidingShape (material, body);
int i = count - 1;
for (; i >= 0; i --) {
if (collidingSubShapeArrar[i] == subShape) {
break;
}
}
if (i < 0) {
collidingSubShapeArrar[count] = subShape;
count ++;
dAssert (count < int (sizeof (collidingSubShapeArrar) / sizeof (collidingSubShapeArrar[0])));
// you can also get the forces here, however when tho function is call form a contact material
// we can only get resting forces, impulsive forces can not be read here since they has no being calculated yet.
// whoever if this function function is call after the NetwonUpdate they the application can read the contact force, that was applied to each contact point
dVector force;
dVector posit;
dVector normal;
NewtonMaterialGetContactForce (material, body, &force[0]);
NewtonMaterialGetContactPositionAndNormal (material, body, &posit[0], &normal[0]);
// the torque on this contact is
dVector torque ((origin - posit) * force);
// do what ever you want wit this
}
}
}
// here we should have an array of all colling sub shapes
if (count) {
// do what you need with this sub shape list
}
}
//////////////////////////////////////////////////////////////////////////
// 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);
}
void RayCastCompoundsAllSubShapes(const dVector& origin, const dVector& end)
{
m_param = 1.0f;
m_body = NULL;
NewtonWorld* const world = GetWorld();
NewtonWorldRayCast(world, &origin[0], &end[0], PickCompound, this, NULL, 0);
if (m_body) {
// we found a compound, find all sub shape on teh path of the ray
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
dVector localP0(matrix.UntransformVector(origin));
dVector localP1(matrix.UntransformVector(end));
NewtonCollision* const compoundCollision = NewtonBodyGetCollision(m_body);
dAssert(NewtonCollisionGetType(compoundCollision) == SERIALIZE_ID_COMPOUND);
for (void* node = NewtonCompoundCollisionGetFirstNode(compoundCollision); node; node = NewtonCompoundCollisionGetNextNode(compoundCollision, node)) {
dVector normal;
dLong attribute;
NewtonCollision* const subShape = NewtonCompoundCollisionGetCollisionFromNode(compoundCollision, node);
dFloat xxx = NewtonCollisionRayCast(subShape, &localP0[0], &localP1[0], &normal[0], &attribute);
if (xxx < 1.0f) {
dTrace (("sub shape hit\n"))
}
}
}
}
static void DebugShowBodyCollision (const NewtonBody* const body, DEBUG_DRAW_MODE mode)
{
switch (NewtonBodyGetType(body))
{
case NEWTON_DYNAMIC_BODY:
{
int sleepState = NewtonBodyGetSleepState(body);
if (sleepState == 1) {
// indicate when body is sleeping
glColor3f(0.42f, 0.73f, 0.98f);
} else {
// body is active
glColor3f(1.0f, 1.0f, 1.0f);
}
break;
}
case NEWTON_KINEMATIC_BODY:
glColor3f(1.0f, 1.0f, 0.0f);
break;
case NEWTON_DEFORMABLE_BODY:
glColor3f (0.0f, 1.0f, 1.0f);
break;
}
dMatrix matrix;
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonCollisionForEachPolygonDo (NewtonBodyGetCollision(body), &matrix[0][0], DebugShowGeometryCollision, (void*) mode);
}
static void UserContactRestitution (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBody* body;
NewtonBody* body0;
NewtonBody* body1;
// call the basic call back
GenericContactProcess (contactJoint, timestep, threadIndex);
body0 = NewtonJointGetBody0(contactJoint);
body1 = NewtonJointGetBody1(contactJoint);
body = body0;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
if (mass == 0.0f) {
body = body1;
}
NewtonCollision* const collision = NewtonBodyGetCollision(body);
void* userData = NewtonCollisionGetUserData (collision);
dFloat restitution = *((float*)&userData);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialSetContactElasticity (material, restitution);
}
}
int PhysicsIslandUpdate (const NewtonWorld* world, const void* islandHandle, int bodyCount)
{
if (showIslans) {
dVector minAABB ( 1.0e10f, 1.0e10f, 1.0e10f, 0.0f);
dVector maxAABB (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
for (int i = 0; i < bodyCount; i ++) {
dVector p0;
dVector p1;
#if 0
// show the engine loose aabb
NewtonIslandGetBodyAABB (islandHandle, i, &p0[0], &p1[0]);
#else
// calculate the shape aabb
dMatrix matrix;
NewtonBody* body;
NewtonCollision *collision;
body = NewtonIslandGetBody (islandHandle, i);
collision = NewtonBodyGetCollision (body);
NewtonBodyGetMatrix (body, &matrix[0][0]);
CalculateAABB (collision, matrix, p0, p1);
#endif
for (int j = 0; j < 3; j ++ ) {
minAABB[j] = p0[j] < minAABB[j] ? p0[j] : minAABB[j];
maxAABB[j] = p1[j] > maxAABB[j] ? p1[j] : maxAABB[j];
}
}
DebugDrawAABB (minAABB, maxAABB);
}
// g_activeBodies += bodyCount;
return 1;
}
static void CreateVisualEntity (DemoEntityManager* const scene, NewtonBody* const body)
{
dMatrix matrix;
NewtonBodyGetMatrix(body, &matrix[0][0]);
DemoEntity* const visualEntity = new DemoEntity(matrix, NULL);
scene->Append(visualEntity);
// set the entiry as the user data;
NewtonBodySetUserData (body, visualEntity);
// create the mesh geometry and attach it to the entity
DemoMesh* const visualMesh = new DemoMesh ("fraturedMainMesh");
visualEntity->SetMesh (visualMesh, dGetIdentityMatrix());
visualMesh->Release();
// create the mesh and set the vertex array, but not the sub meshes
NewtonCollision* const fracturedCompoundCollision = NewtonBodyGetCollision(body);
dAssert (NewtonCollisionGetType(fracturedCompoundCollision) == SERIALIZE_ID_FRACTURED_COMPOUND);
// add the vertex data
NewtonFracturedCompoundMeshPart* const mainMesh = NewtonFracturedCompoundGetMainMesh (fracturedCompoundCollision);
AddMeshVertexwData (visualMesh, mainMesh, fracturedCompoundCollision);
// now add the sub mesh by calling the call back
OnReconstructMainMeshCallBack (body, mainMesh, fracturedCompoundCollision);
}
dNewtonDynamicBody::dNewtonDynamicBody(dNewtonWorld* const world, dNewtonCollision* const collision, dMatrix matrix, dFloat mass)
:dNewtonBody(matrix)
{
NewtonWorld* const newton = world->m_world;
NewtonWaitForUpdateToFinish(newton);
m_body = NewtonCreateDynamicBody(newton, collision->m_shape, &matrix[0][0]);
collision->DeleteShape();
collision->SetShape(NewtonBodyGetCollision(m_body));
NewtonBodySetMassProperties(m_body, mass, NewtonBodyGetCollision(m_body));
NewtonBodySetUserData(m_body, this);
NewtonBodySetTransformCallback(m_body, OnBodyTransformCallback);
NewtonBodySetForceAndTorqueCallback(m_body, OnForceAndTorqueCallback);
}
void SetShowMeshCollision (SceneManager& me, int mode)
{
NewtonTreeCollisionCallback showFaceCallback;
showFaceCallback = NULL;
if (mode) {
showFaceCallback = ShowMeshCollidingFaces;
}
// iterate the world
for (const NewtonBody* body = NewtonWorldGetFirstBody (me.m_world); body; body = NewtonWorldGetNextBody (me.m_world, body)) {
NewtonCollision* collision;
NewtonCollisionInfoRecord info;
collision = NewtonBodyGetCollision (body);
NewtonCollisionGetInfo (collision, &info);
switch (info.m_collisionType)
{
case SERIALIZE_ID_TREE:
case SERIALIZE_ID_SCENE:
case SERIALIZE_ID_USERMESH:
case SERIALIZE_ID_HEIGHTFIELD:
{
NewtonStaticCollisionSetDebugCallback (collision, showFaceCallback);
break;
}
default:
break;
}
}
}
virtual const void InitRigiBody(const NewtonBody* const body, const char* const bodyName) const
{
dMatrix matrix;
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(NewtonBodyGetWorld(body));
NewtonCollision* const collision = NewtonBodyGetCollision(body);
DemoMesh* const mesh = new DemoMesh("ragdoll", collision, "smilli.tga", "smilli.tga", "smilli.tga");
NewtonBodyGetMatrix(body, &matrix[0][0]);
DemoEntity* const entity = new DemoEntity(matrix, NULL);
entity->SetNameID (bodyName);
entity->SetMesh(mesh, dGetIdentityMatrix());
scene->Append(entity);
mesh->Release();
// save the pointer to the graphic object with the body.
NewtonBodySetUserData(body, entity);
// assign the wood id
NewtonBodySetMaterialGroupID(body, m_material);
//set continuous collision mode
//NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode);
// set a destructor for this rigid body
NewtonBodySetDestructorCallback(body, PhysicsBodyDestructor);
// set the transform call back function
NewtonBodySetTransformCallback(body, DemoEntity::TransformCallback);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback(body, PhysicsApplyGravityForce);
}
void CustomPlayerController::SetPlayerOrigin (dFloat originHigh)
{
dAssert (0);
NewtonCollision* const playerShape = NewtonBodyGetCollision(m_body);
NewtonCompoundCollisionBeginAddRemove(playerShape);
dMatrix supportShapeMatrix (dGetIdentityMatrix());
supportShapeMatrix[0] = m_upVector;
supportShapeMatrix[1] = m_frontVector;
supportShapeMatrix[2] = supportShapeMatrix[0] * supportShapeMatrix[1];
supportShapeMatrix.m_posit = supportShapeMatrix[0].Scale(m_height * 0.5f - originHigh);
supportShapeMatrix.m_posit.m_w = 1.0f;
NewtonCollisionSetMatrix (m_supportShape, &supportShapeMatrix[0][0]);
dMatrix collisionShapeMatrix (supportShapeMatrix);
dFloat cylinderHeight = m_height - m_stairStep;
dAssert (cylinderHeight > 0.0f);
collisionShapeMatrix.m_posit = collisionShapeMatrix[0].Scale(cylinderHeight * 0.5f + m_stairStep - originHigh);
collisionShapeMatrix.m_posit.m_w = 1.0f;
NewtonCollisionSetMatrix (m_upperBodyShape, &collisionShapeMatrix[0][0]);
NewtonCompoundCollisionEndAddRemove (playerShape);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix(m_body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodySetMassProperties(m_body, mass, playerShape);
}
void RigidBodyData::Save(ISave* const isave)
{
ULONG nwrit;
int revision = D_FILE_REVISION;
dVector mass;
dVector com;
dVector veloc;
dVector omega;
dMatrix matrix;
RigidBodyWorldDesc& me = *(RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor();
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
NewtonBodyGetVelocity(m_body, &veloc[0]);
NewtonBodyGetOmega(m_body, &omega[0]);
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
isave->Write((const char*)&revision, sizeof (revision), &nwrit);
isave->Write((const char*)&m_oldControlerID, sizeof (m_oldControlerID), &nwrit);
isave->Write((const char*)&m_collisionShape, sizeof (m_collisionShape), &nwrit);
isave->Write((const char*)&m_hideGizmos, sizeof (m_hideGizmos), &nwrit);
isave->Write((const char*)&m_mass, sizeof (m_mass), &nwrit);
isave->Write((const char*)&m_inertia, sizeof (m_inertia), &nwrit);
isave->Write((const char*)&m_origin, sizeof (m_origin), &nwrit);
isave->Write((const char*)&matrix, sizeof (matrix), &nwrit);
isave->Write((const char*)&veloc, sizeof (veloc), &nwrit);
isave->Write((const char*)&omega, sizeof (omega), &nwrit);
NewtonCollisionSerialize (me.m_newton, collision, SaveCollision, isave);
}
void dNewtonBody::SetCenterOfMass(float com_x, float com_y, float com_z)
{
dVector com;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
NewtonBodySetMassProperties(m_body, mass, NewtonBodyGetCollision(m_body));
NewtonBodyGetCentreOfMass (m_body, &com[0]);
com.m_x += com_x;
com.m_y += com_y;
com.m_z += com_z;
NewtonBodySetCentreOfMass(m_body, &com[0]);
}
NewtonMesh* MakeViualMesh::CreateVisualMesh (NewtonBody* const body, char* const name, int maxNameSize) const
{
// here the use should take the user data from the body create newtonMesh form it and return that back
NewtonCollision* const collision = NewtonBodyGetCollision(body);
NewtonMesh* const mesh = NewtonMeshCreateFromCollision(collision);
sprintf (name, "visual Mesh");
return mesh;
}
static int OnBoneAABBOverlap (const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex)
{
dAssert (0);
NewtonCollision* const collision0 = NewtonBodyGetCollision(body0);
NewtonCollision* const collision1 = NewtonBodyGetCollision(body1);
dCustomActiveCharacterController::dSkeletonBone* const bone0 = (dCustomActiveCharacterController::dSkeletonBone*)NewtonCollisionGetUserData (collision0);
dCustomActiveCharacterController::dSkeletonBone* const bone1 = (dCustomActiveCharacterController::dSkeletonBone*)NewtonCollisionGetUserData (collision1);
dAssert (bone0);
dAssert (bone1);
if (bone0->m_myController && bone1->m_myController) {
return bone0->m_myController->SelfCollisionTest (bone0, bone1) ? 1 : 0;
}
*/
return 1;
}
int dNewton::OnCompoundSubCollisionAABBOverlap (const NewtonMaterial* const material, const NewtonBody* const body0, const void* const collisionNode0, const NewtonBody* const body1, const void* const collisionNode1, int threadIndex)
{
dAssert (NewtonBodyGetWorld (body0) == NewtonBodyGetWorld (body1));
dNewton* const world = (dNewton*) NewtonWorldGetUserData(NewtonBodyGetWorld (body0));
dNewtonBody* const dBody0 = (dNewtonBody*) NewtonBodyGetUserData (body0);
dNewtonBody* const dBody1 = (dNewtonBody*) NewtonBodyGetUserData (body1);
NewtonCollision* const collision0 = NewtonBodyGetCollision(body0);
NewtonCollision* const collision1 = NewtonBodyGetCollision(body1);
NewtonCollision* const subCollision0 = collisionNode0 ? (NewtonCollision*) NewtonCompoundCollisionGetCollisionFromNode (collision0, (void*)collisionNode0) : collision0;
NewtonCollision* const subCollision1 = collisionNode1 ? (NewtonCollision*) NewtonCompoundCollisionGetCollisionFromNode (collision1, (void*)collisionNode1) : collision1;
dNewtonCollision* const dsubCollision0 = (dNewtonCollision*)NewtonCollisionGetUserData(subCollision0);
dNewtonCollision* const dsubCollision1 = (dNewtonCollision*)NewtonCollisionGetUserData(subCollision1);
dAssert (dsubCollision0);
dAssert (dsubCollision1);
return world->OnCompoundSubCollisionAABBOverlap (dBody0, dsubCollision0, dBody1, dsubCollision1, threadIndex);
}
void cPhysicsBodyNewton::RenderDebugGeometry(iLowLevelGraphics *apLowLevel,const cColor &aColor)
{
NewtonCollision *pCollision = NewtonBodyGetCollision(mpNewtonBody);
float matrix[4][4];
NewtonBodyGetMatrix(mpNewtonBody, &matrix[0][0]);
gpLowLevelGraphics = apLowLevel;
gDebugColor = aColor;
NewtonCollisionForEachPolygonDo(pCollision, &matrix[0][0], RenderDebugPolygon, (void*)NULL);
}
VALUE MSNewton::Bodies::get_closest_points(VALUE self, VALUE v_body1, VALUE v_body2) {
const NewtonBody* body1 = Util::value_to_body(v_body1);
const NewtonBody* body2 = Util::value_to_body(v_body2);
Util::validate_two_bodies(body1, body2);
const NewtonWorld* world = NewtonBodyGetWorld(body1);
WorldData* world_data = (WorldData*)NewtonWorldGetUserData(world);
NewtonCollision* colA = NewtonBodyGetCollision(body1);
NewtonCollision* colB = NewtonBodyGetCollision(body2);
dMatrix matrixA;
dMatrix matrixB;
NewtonBodyGetMatrix(body1, &matrixA[0][0]);
NewtonBodyGetMatrix(body2, &matrixB[0][0]);
dVector pointA;
dVector pointB;
dVector normalAB;
if (NewtonCollisionClosestPoint(world, colA, &matrixA[0][0], colB, &matrixB[0][0], &pointA[0], &pointB[0], &normalAB[0], 0) == 0)
return Qnil;
return rb_ary_new3(2, Util::point_to_value(pointA, world_data->inverse_scale), Util::point_to_value(pointB, world_data->inverse_scale));
}
static int CalculateContacts (const NewtonBody* const otherBody, void* const userData)
{
ShowCollisionCollide* const me = (ShowCollisionCollide*)userData;
if (me->m_body != otherBody) {
const int cMaxContacts = 15;
dFloat contacts[cMaxContacts][3];
dFloat normals[cMaxContacts][3];
dFloat penetrations[cMaxContacts];
dLong attributeA[cMaxContacts];
dLong attributeB[cMaxContacts];
NewtonWorld* const world = NewtonBodyGetWorld(otherBody);
//NewtonBodyGetMatrix(me->m_body, &matrixA[0][0]);
//NewtonBodyGetMatrix(otherBody, &matrixB[0][0]);
DemoEntity* const entityA = (DemoEntity*)NewtonBodyGetUserData(me->m_body);
DemoEntity* const entityB = (DemoEntity*)NewtonBodyGetUserData(otherBody);
const dMatrix& matrixA = entityA->GetRenderMatrix ();
const dMatrix& matrixB = entityB->GetRenderMatrix ();
NewtonCollision* const collisionA = NewtonBodyGetCollision(me->m_body);
NewtonCollision* const collisionB = NewtonBodyGetCollision(otherBody);
int count = NewtonCollisionCollide (world, cMaxContacts, collisionA, &matrixA[0][0], collisionB, &matrixB[0][0],
&contacts[0][0], &normals[0][0], penetrations, attributeA, attributeB, 0);
dVector originColor (1.0f, 0.0f, 0.0f, 0.0f);
dVector lineColor (0.0f, 0.0f, 1.0f, 0.0f);
for (int i = 0; i < count; i ++) {
dVector n (normals[i][0], normals[i][1], normals[i][2], 0.0f);
dVector p0 (contacts[i][0], contacts[i][1], contacts[i][2], 0.0f);
dVector p1 (p0 + n.Scale (0.5f));
p0 = matrixA.UntransformVector(p0);
p1 = matrixA.UntransformVector(p1);
ShowMousePicking (p0, p1, originColor, lineColor);
}
}
return 1;
}
PuckEntity (DemoEntityManager* const scene, int materialID)
:DemoEntity (dGetIdentityMatrix(), NULL)
,m_launched(false)
{
scene->Append(this);
NewtonWorld* const world = scene->GetNewton();
dVector puckSize(WEIGHT_DIAMETER, WEIGHT_HEIGHT, 0.0f, 0.0f);
// create the shape and visual mesh as a common data to be re used
NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), puckSize, _CYLINDER_PRIMITIVE, materialID);
// correction: make the puck an upright cylinder, this makes everything simpler
dMatrix collisionAligmentMatrix (dRollMatrix(3.141592f/2.0f));
NewtonCollisionSetMatrix(collision, &collisionAligmentMatrix[0][0]);
DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "smilli.tga", "smilli.tga", "smilli.tga");
//dMatrix matrix = dRollMatrix(3.141592f/2.0f);
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit.m_x = -TABLE_LENGTH*0.5f+WEIGHT_DIAMETER;
matrix.m_posit.m_z = -11.8f;
//matrix.m_posit.m_z += 4.0f;
matrix.m_posit.m_y = 5.0f;
m_puckBody = CreateSimpleSolid (scene, geometry, WEIGHT_MASS, matrix, collision, materialID);
// Set moment of inertia
// correction: this is deprecated, NewtonBodySetMassProperties produce the exact result
//dVector I;
//dFloat Mass = WEIGHT_MASS;
//dFloat Radius = WEIGHT_RADIUS;
//dFloat Height = WEIGHT_HEIGHT;
//I.m_x = I.m_z = Mass*(3.0f*Radius*Radius+Height*Height)/12.0f;
//I.m_y = Mass*Radius*Radius/2.0f;
//NewtonBodySetMassMatrix(gPuckBody,Mass, I.m_x, I.m_y, I.m_z);
NewtonBodySetMassProperties(m_puckBody, WEIGHT_MASS, NewtonBodyGetCollision(m_puckBody));
NewtonBodySetMaterialGroupID(m_puckBody, materialID);
// remember to make continuous collision work with auto sleep mode, right now this is no working
NewtonBodySetContinuousCollisionMode(m_puckBody, 1);
NewtonBodySetAutoSleep(m_puckBody, 1);
// Set callbacks
NewtonBodySetForceAndTorqueCallback(m_puckBody, NewtonRigidBodySetForceCB);
// do not forget to release the assets
geometry->Release();
NewtonDestroyCollision (collision);
}
virtual void OnRender (dFloat timestep) const
{
dVector p0(0.0f);
dVector p1(0.0f);
DemoEntity* const entity = (DemoEntity*)NewtonBodyGetUserData(m_body);
const dMatrix& matrix = entity->GetRenderMatrix();
NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
CalculateAABB (collision, matrix, p0, p1);
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
NewtonWorldForEachBodyInAABBDo(world, &p0[0], &p1[0], CalculateContacts, (void*)this);
}
static dFloat PickCompound(const NewtonBody* const body, const NewtonCollision* const shapeHit, const dFloat* const hitContact, const dFloat* const hitNormal, dLong collisionID, void* const userData, dFloat intersectParam)
{
NewtonCollision* const collision = NewtonBodyGetCollision(body);
if (NewtonCollisionGetType(collision) == SERIALIZE_ID_COMPOUND) {
dShowAllSubShapes* const me = (dShowAllSubShapes*)userData;
if (intersectParam < me->m_param) {
me->m_param = intersectParam;
me->m_body = body;
return intersectParam;
}
}
return 1.2f;
}
