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));
}
int dNewton::OnBodiesAABBOverlap (const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, 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);
return world->OnBodiesAABBOverlap(dBody0, dBody1, threadIndex);
}
static void OnEmitFracturedChunk (NewtonBody* const chunkBody, NewtonFracturedCompoundMeshPart* const fractureChunkMesh, const NewtonCollision* const fracturedCompoundCollision)
{
NewtonWorld* const world = NewtonBodyGetWorld(chunkBody);
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world);
// set the force an torque call back
NewtonBodySetForceAndTorqueCallback (chunkBody, PhysicsApplyGravityForce);
// set the transform callback
NewtonBodySetTransformCallback (chunkBody, DemoEntity::TransformCallback);
// create the visual entity and mesh, and set the use data
dMatrix matrix;
NewtonBodyGetMatrix (chunkBody, &matrix[0][0]);
DemoEntity* const visualChunkEntity = new DemoEntity(matrix, NULL);
scene->Append(visualChunkEntity);
NewtonBodySetUserData (chunkBody, visualChunkEntity);
// create the mesh geometry and attach it to the entity
DemoMesh* const visualChunkMesh = new DemoMesh ("fracturedChuckMesh");
visualChunkEntity->SetMesh (visualChunkMesh, dGetIdentityMatrix());
visualChunkMesh->Release();
// add the vertex data
AddMeshVertexwData (visualChunkMesh, fractureChunkMesh, fracturedCompoundCollision);
// add the mesh indices
OnReconstructMainMeshCallBack (chunkBody, fractureChunkMesh, fracturedCompoundCollision);
}
VALUE MSNewton::Bodies::get_force_in_between(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);
dVector net_force(0.0f, 0.0f, 0.0f);
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body1); joint; joint = NewtonBodyGetNextContactJoint(body1, joint)) {
if (NewtonJointGetBody0(joint) == body2 || NewtonJointGetBody1(joint) == body2) {
for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) {
NewtonMaterial* material = NewtonContactGetMaterial(contact);
dVector force;
NewtonMaterialGetContactForce(material, body1, &force[0]);
net_force += force;
}
}
}
const NewtonWorld* world = NewtonBodyGetWorld(body1);
WorldData* world_data = (WorldData*)NewtonWorldGetUserData(world);
//BodyData* body1_data = (BodyData*)NewtonBodyGetUserData(body1);
//BodyData* body2_data = (BodyData*)NewtonBodyGetUserData(body2);
/*if (world_data->gravity_enabled && (body1_data->gravity_enabled || body2_data->gravity_enabled)) {
for (int i = 0; i < 3; ++i)
net_force[i] *= world_data->inverse_scale;
}*/
return Util::vector_to_value(net_force, world_data->inverse_scale4);
}
void* dNewtonBody::GetInterpolatedPosition()
{
const dNewtonWorld* const world = (dNewtonWorld*)NewtonWorldGetUserData(NewtonBodyGetWorld(m_body));
ScopeLock scopelock(&m_lock);
m_interpolatedPosit = m_posit0 + (m_posit1 - m_posit0).Scale(world->m_interpotationParam);
return &m_interpolatedPosit.m_x;
}
void* dNewtonBody::GetInterpolatedRotation()
{
const dNewtonWorld* const world = (dNewtonWorld*) NewtonWorldGetUserData(NewtonBodyGetWorld(m_body));
ScopeLock scopelock(&m_lock);
m_interpolatedRotation = m_rotation0.Slerp(m_rotation1, world->m_interpotationParam);
return &m_interpolatedRotation.m_q0;
}
CustomVehicleControllerBodyStateTire* AddTire (const dVector& offset, dFloat width, dFloat radius, dFloat mass, dFloat suspensionLength, dFloat suspensionSpring, dFloat suspensionDamper, dFloat lateralStiffness, dFloat longitudinalStiffness, dFloat aligningMomentTrail, const dMatrix& tireAligmentMatrix)
{
NewtonBody* const body = m_controller->GetBody();
// make the tire matrix from the offset and the body matrix
dMatrix tireMatrix (GetNextMatrix());
tireMatrix.m_posit = offset;
// add the visual representation of the is tire to as a child of the vehicle model
NewtonCollision* const tireMeshGenerator = NewtonCreateChamferCylinder (NewtonBodyGetWorld(body), 0.5f, 1.0f, 0, NULL);
NewtonCollisionSetScale (tireMeshGenerator, width, radius, radius);
DemoEntity* const tireEntity = new DemoEntity (tireMatrix, this);
DemoMesh* const visualMesh = new DemoMesh ("tireMesh", tireMeshGenerator, "smilli.tga", "smilli.tga", "smilli.tga");
tireEntity->SetMesh (visualMesh, dYawMatrix(3.141592f * 90.0f / 180.0f));
visualMesh->Release();
NewtonDestroyCollision (tireMeshGenerator);
// add the tire to the vehicle
CustomVehicleControllerBodyStateTire::TireCreationInfo tireInfo;
tireInfo.m_location = tireMatrix.m_posit;
tireInfo.m_mass = mass;
tireInfo.m_radio = radius;
tireInfo.m_width = width;
tireInfo.m_dampingRatio = suspensionDamper;
tireInfo.m_springStrength = suspensionSpring;
tireInfo.m_suspesionlenght = suspensionLength;
tireInfo.m_lateralStiffness = lateralStiffness;
tireInfo.m_longitudialStiffness = longitudinalStiffness;
tireInfo.m_aligningMomentTrail = aligningMomentTrail;
tireInfo.m_userData = tireEntity;
return m_controller->AddTire (tireInfo);
}
//////////////////////////////////////////////////////////////////////////
// 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);
}
static void OnReconstructMainMeshCallBack (NewtonBody* const body, NewtonFracturedCompoundMeshPart* const mainMesh, const NewtonCollision* const fracturedCompoundCollision)
{
DemoEntity* const entity = (DemoEntity*)NewtonBodyGetUserData(body);
DemoMesh* const visualMesh = (DemoMesh*)entity->GetMesh();
dAssert (visualMesh->IsType(DemoMesh::GetRttiType()));
dAssert (NewtonCollisionGetType(fracturedCompoundCollision) == SERIALIZE_ID_FRACTURED_COMPOUND);
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(NewtonBodyGetWorld(body));
dAssert (scene);
visualMesh->RemoveAll();
for (void* segment = NewtonFracturedCompoundMeshPartGetFirstSegment(mainMesh); segment; segment = NewtonFracturedCompoundMeshPartGetNextSegment (segment)) {
DemoSubMesh* const subMesh = visualMesh->AddSubMesh();
int material = NewtonFracturedCompoundMeshPartGetMaterial (segment);
int indexCount = NewtonFracturedCompoundMeshPartGetIndexCount (segment);
subMesh->m_textureHandle = AddTextureRef ((GLuint)material);
subMesh->m_shader = scene->GetShaderCache().m_diffuseEffect;
subMesh->AllocIndexData (indexCount);
subMesh->m_indexCount = NewtonFracturedCompoundMeshPartGetIndexStream (fracturedCompoundCollision, mainMesh, segment, (int*)subMesh->m_indexes);
}
visualMesh->OptimizeForRender();
}
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 CustomJoint::Init (int maxDOF, NewtonBody* const body0, NewtonBody* const body1)
{
m_autoDestroy = 0;
m_joint = NULL;
m_body0 = body0;
m_body1 = body1;
m_maxDof = maxDOF;
m_stiffness = 1.0f;
m_world = body0 ? NewtonBodyGetWorld (body0) : NewtonBodyGetWorld (body1);
m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstraints, GetInfo, m_body0, m_body1);
NewtonJointSetUserData (m_joint, this);
NewtonJointSetDestructor (m_joint, Destructor);
m_userData = NULL;
m_userDestructor = NULL;
m_userConstrationCallback = NULL;
}
static void PlaneCollisionCollideCallbackDescrete (NewtonUserMeshCollisionCollideDesc* const collideDesc)
{
dInfinitePlane* const me = (dInfinitePlane*) collideDesc->m_userData;
dVector p0 (collideDesc->m_boxP0[0], collideDesc->m_boxP0[1], collideDesc->m_boxP0[2], 0.0f);
dVector p1 (collideDesc->m_boxP1[0], collideDesc->m_boxP1[1], collideDesc->m_boxP1[2], 0.0f);
dVector suportVertex ((me->m_plane.m_x > 0.0f) ? p0.m_x : p1.m_x, (me->m_plane.m_y > 0.0f) ? p0.m_y : p1.m_y, (me->m_plane.m_z > 0.0f) ? p0.m_z : p1.m_z);
dFloat dist = me->m_plane.DotProduct3(suportVertex) + me->m_plane.m_w;
if (dist < 0.25f) {
// calculate the aabb center
dVector centre ((p1 + p0).Scale (0.5f));
//find the projection of center point over the plane
dFloat t = - (me->m_plane.DotProduct3(centre) + me->m_plane.m_w);
centre += me->m_plane.Scale (t);
//know calculate the scale factor
dVector size (p1 - p0);
dFloat s = dMax(size.m_x, dMax (size.m_y, size.m_z));
dInt32 threadNumber = collideDesc->m_threadNumber;
// initialize the callback data structure
#ifdef PASS_A_QUAD
collideDesc->m_faceCount = 1;
#else
collideDesc->m_faceCount = 2;
#endif
collideDesc->m_vertexStrideInBytes = sizeof (dVector);
collideDesc->m_faceIndexCount = &me->m_faceIndices[threadNumber][0];
collideDesc->m_faceVertexIndex = &me->m_indexArray[threadNumber][0];
collideDesc->m_vertex = &me->m_collisionVertex[threadNumber][0][0];
dVector* const polygon = &me->m_collisionVertex[threadNumber][0];
for (int i = 0; i < 4; i ++) {
polygon[i] = centre + me->m_unitSphape[i].Scale (s);
}
// save face normal
polygon[4] = me->m_plane;
// show debug display info
if (DebugDisplayOn()) {
dMatrix matrix;
dVector face[64];
NewtonBodyGetMatrix (collideDesc->m_polySoupBody, &matrix[0][0]);
matrix.TransformTriplex (&face[0].m_x, sizeof (dVector), &polygon[0].m_x, sizeof (dVector), 4);
NewtonWorld* const world = NewtonBodyGetWorld (collideDesc->m_polySoupBody);
// critical section lock
NewtonWorldCriticalSectionLock (world, threadNumber);
//DebugDrawPolygon (4, &face[0]);
// unlock the critical section
NewtonWorldCriticalSectionUnlock (world);
}
}
}
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 dNewtonBody::Destroy()
{
if (m_body) {
NewtonWaitForUpdateToFinish(NewtonBodyGetWorld(m_body));
NewtonBodySetDestructorCallback(m_body, NULL);
NewtonDestroyBody(m_body);
m_body = NULL;
}
}
virtual void OnUpdateTransform(const dAnimationJoint* const bone, const dMatrix& localMatrix) const
{
// calculate the local transform for this player body
NewtonBody* const body = bone->GetBody();
DemoEntity* const ent = (DemoEntity*)NewtonBodyGetUserData(body);
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(NewtonBodyGetWorld(body));
dQuaternion rot(localMatrix);
ent->SetMatrix(*scene, rot, localMatrix.m_posit);
}
virtual void OnUpdateTransform (const dCustomActiveCharacterController::dSkeletonBone* const bone, const dMatrix& localMatrix) const
{
DemoEntity* const ent = (DemoEntity*) NewtonBodyGetUserData(bone->m_body);
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(NewtonBodyGetWorld(bone->m_body));
dQuaternion rot (localMatrix);
ent->SetMatrix (*scene, rot, localMatrix.m_posit);
dCustomActiveCharacterControllerManager::OnUpdateTransform (bone, localMatrix);
}
void _CDECL World::newtonLeaveWorld( const NewtonBody* body, int threadIndex )
{
OgreNewt::World* me = (OgreNewt::World*)NewtonWorldGetUserData( NewtonBodyGetWorld( body ) );
if (me->m_leaveCallback)
{
OgreNewt::Body* b = (OgreNewt::Body*)NewtonBodyGetUserData( body );
me->m_leaveCallback( b, threadIndex );
}
}
static void PlaneCollisionCollideCallbackConstinue (NewtonUserMeshCollisionCollideDesc* const collideDesc, const void* const continueCollisionHandle)
{
dInfinitePlane* const me = (dInfinitePlane*) collideDesc->m_userData;
// build that aabb of each face and submit only the one that pass the test.
if (NewtonUserMeshCollisionContinuousOverlapTest (collideDesc, continueCollisionHandle, &me->m_minBox[0], &me->m_maxBox[0])) {
const dVector& p0 = me->m_minBox;
const dVector& p1 = me->m_maxBox;
dVector centre ((p1 + p0).Scale (0.5f));
//find the projection of center point over the plane
dFloat t = - (me->m_plane.DotProduct3(centre) + me->m_plane.m_w);
centre += me->m_plane.Scale (t);
//know calculate the scale factor
dVector size (p1 - p0);
dFloat s = dMax(size.m_x, dMax (size.m_y, size.m_z)) * 0.5f;
dInt32 threadNumber = collideDesc->m_threadNumber;
// initialize the callback data structure
#ifdef PASS_A_QUAD
collideDesc->m_faceCount = 1;
#else
collideDesc->m_faceCount = 2;
#endif
collideDesc->m_vertexStrideInBytes = sizeof (dVector);
collideDesc->m_faceIndexCount = &me->m_faceIndices[threadNumber][0];
collideDesc->m_faceVertexIndex = &me->m_indexArray[threadNumber][0];
collideDesc->m_vertex = &me->m_collisionVertex[threadNumber][0][0];
dVector* const polygon = &me->m_collisionVertex[threadNumber][0];
for (int i = 0; i < 4; i ++) {
polygon[i] = centre + me->m_unitSphape[i].Scale (s);
}
// save face normal
polygon[4] = me->m_plane;
// show debug display info
if (DebugDisplayOn()) {
dMatrix matrix;
dVector face[64];
NewtonBodyGetMatrix (collideDesc->m_polySoupBody, &matrix[0][0]);
matrix.TransformTriplex (&face[0].m_x, sizeof (dVector), &polygon[0].m_x, sizeof (dVector), 4);
NewtonWorld* const world = NewtonBodyGetWorld (collideDesc->m_polySoupBody);
// critical section lock
NewtonWorldCriticalSectionLock (world, threadNumber);
//DebugDrawPolygon (4, &face[0]);
// unlock the critical section
NewtonWorldCriticalSectionUnlock (world);
}
}
}
void DemoEntity::SetTransformCallback(const NewtonBody* body, const dFloat* matrix, int threadIndex)
{
DemoEntity* const ent = (DemoEntity*) NewtonBodyGetUserData(body);
dQuaternion rot;
NewtonBodyGetRotation(body, &rot.m_q0);
DemoEntityManager* world = (DemoEntityManager*) NewtonBodyGetWorld(body);
const dMatrix& transform = *((dMatrix*) matrix);
ent->SetMatrix (*world, rot, transform.m_posit);
}
CustomDGRayCastCar::~CustomDGRayCastCar ()
{
NewtonWorld *world;
world = NewtonBodyGetWorld (m_body0);
for ( int i = 0; i < m_tiresCount; i ++ ) {
NewtonReleaseCollision ( world, m_tires[i].m_shape );
}
if (m_tires) {
delete[] m_tires;
}
}
void dNewton::OnContactProcess (const NewtonJoint* const contactJoint, dFloat timestep, int threadIndex)
{
NewtonBody* const body = NewtonJointGetBody0 (contactJoint);
dNewton* const world = (dNewton*) NewtonWorldGetUserData(NewtonBodyGetWorld (body));
// for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
// NewtonMaterial* const material = NewtonContactGetMaterial (contact);
// NewtonMaterialSetContactFrictionCoef (material, 1.0f, 1.0f, 0);
// NewtonMaterialSetContactFrictionCoef (material, 1.0f, 1.0f, 1);
// }
dNewtonContactMaterial contactMaterial ((void*)contactJoint);
world->OnContactProcess (&contactMaterial, timestep, threadIndex);
}
NewtonCustomJoint::NewtonCustomJoint (int maxDOF, NewtonBody* body0, NewtonBody* body1)
{
m_joint = NULL;
m_body0 = body0;
m_body1 = body1;
m_maxDox = maxDOF;
m_world = NewtonBodyGetWorld (body0);
m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstrainst, m_body0, m_body1);
NewtonJointSetUserData (m_joint, this);
NewtonJointSetDestructor (m_joint, Destructor);
}
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);
}
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 dNewtonDynamicBody::InitForceAccumulators()
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
const dNewtonWorld* const world = (dNewtonWorld*)NewtonWorldGetUserData(NewtonBodyGetWorld(m_body));
m_externalForce = world->GetGravity().Scale(mass);
m_externalTorque = dVector(0.0f);
}
static void ShowMeshCollidingFaces (
const NewtonBody* bodyWithTreeCollision,
const NewtonBody* body,
int faceID,
int vertexCount,
const dFloat* vertex,
int vertexstrideInBytes)
{
// we are coping data to and array of memory, another call back may be doing the same thing
// here fore we need to avoid race conditions
NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (bodyWithTreeCollision));
dVector face[64];
int stride = vertexstrideInBytes / sizeof (dFloat);
for (int j = 0; j < vertexCount; j ++) {
face [j] = dVector (vertex[j * stride + 0], vertex[j * stride + 1] , vertex[j * stride + 2]);
}
DebugDrawPolygon (vertexCount, face);
// unlock the critical section
NewtonWorldCriticalSectionUnlock (NewtonBodyGetWorld (bodyWithTreeCollision));
}
void ShowMeshCollidingFaces (const NewtonBody* const staticCollisionBody, const NewtonBody* const body, int faceID, int vertexCount, const dFloat* const vertex, int vertexstrideInBytes)
{
#ifdef USE_STATIC_MESHES_DEBUG_COLLISION
if (g_debugMode) {
if ((g_debugDisplayCount + vertexCount * 2) < int (sizeof (g_debugDisplayCallback) / sizeof(g_debugDisplayCallback[0]))) {
// we are coping data to and array of memory, another call back may be doing the same thing
// here fore we need to avoid race conditions
NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (staticCollisionBody), 0);
int stride = vertexstrideInBytes / sizeof (dFloat);
dVector l0 (vertex[(vertexCount-1) * stride + 0], vertex[(vertexCount-1) * stride + 1], vertex[(vertexCount-1) * stride + 2], 0.0f);
for (int j = 0; j < vertexCount; j ++) {
dVector l1 (vertex[j * stride + 0], vertex[j * stride + 1] , vertex[j * stride + 2], 0.0f);
g_debugDisplayCallback[g_debugDisplayCount + 0] = l0;
g_debugDisplayCallback[g_debugDisplayCount + 1] = l1;
g_debugDisplayCount += 2;
l0 = l1;
}
// unlock the critical section
NewtonWorldCriticalSectionUnlock (NewtonBodyGetWorld (staticCollisionBody));
}
}
#endif
}
void LaunchPuck()
{
if (!m_launched) {
m_launched = true;
NewtonInvalidateCache (NewtonBodyGetWorld(m_puckBody));
dVector zeros(0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetVelocity(m_puckBody, &zeros.m_x);
NewtonBodySetOmega(m_puckBody, &zeros.m_x);
NewtonBodySetForce(m_puckBody, &zeros.m_x);
NewtonBodySetTorque(m_puckBody, &zeros.m_x);
dVector vel(171.299469f, 0.0f, 0.0f);
NewtonBodySetVelocity(m_puckBody, &vel.m_x);
}
}
void DemoEntityManager::BodyDeserialization (NewtonBody* const body, void* const bodyUserData, NewtonDeserializeCallback deserializecallback, void* const serializeHandle)
{
int size;
char bodyIndentification[256];
deserializecallback (serializeHandle, &size, sizeof (size));
deserializecallback (serializeHandle, bodyIndentification, size);
// get the world and the scene form the world user data
NewtonWorld* const world = NewtonBodyGetWorld(body);
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world);
// here we attach a visual object to the entity,
dMatrix matrix;
NewtonBodyGetMatrix(body, &matrix[0][0]);
DemoEntity* const entity = new DemoEntity(matrix, NULL);
scene->Append (entity);
NewtonBodySetUserData (body, entity);
NewtonBodySetTransformCallback(body, DemoEntity::TransformCallback);
NewtonBodySetForceAndTorqueCallback(body, PhysicsApplyGravityForce);
NewtonCollision* const collision = NewtonBodyGetCollision(body);
#ifdef USE_STATIC_MESHES_DEBUG_COLLISION
if (NewtonCollisionGetType(collision) == SERIALIZE_ID_TREE) {
NewtonStaticCollisionSetDebugCallback (collision, ShowMeshCollidingFaces);
}
#endif
//for visual mesh we will collision mesh and convert it to a visual mesh using NewtonMesh
dTree <DemoMeshInterface*, const void*>* const cache = (dTree <DemoMeshInterface*, const void*>*)bodyUserData;
dTree <DemoMeshInterface*, const void*>::dTreeNode* node = cache->Find(NewtonCollisionDataPointer (collision));
if (!node) {
DemoMeshInterface* mesh = new DemoMesh(bodyIndentification, collision, NULL, NULL, NULL);
node = cache->Insert(mesh, NewtonCollisionDataPointer (collision));
} else {
node->GetInfo()->AddRef();
}
DemoMeshInterface* const mesh = node->GetInfo();
entity->SetMesh(mesh, dGetIdentityMatrix());
mesh->Release();
}
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));
}