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));
}
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);
}
}
}
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);
}
}
}
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 CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dVector com;
dMatrix matrix;
dVector omega0;
dVector veloc0;
dVector omega1;
dVector veloc1;
dVector pointVeloc;
const dFloat stiffness = 0.3f;
const dFloat angularDamp = 0.95f;
dFloat invTimeStep = 1.0f / timestep;
NewtonWorld* const world = NewtonBodyGetWorld (body);
NewtonWorldCriticalSectionLock (world, 0);
// calculate the desired impulse
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetOmega (body, &omega0[0]);
NewtonBodyGetVelocity (body, &veloc0[0]);
NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);
dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
for (int i = 0; i < 3; i ++) {
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
veloc[i] = deltaVeloc[i];
NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
}
// damp angular velocity
NewtonBodyGetOmega (body, &omega1[0]);
NewtonBodyGetVelocity (body, &veloc1[0]);
omega1 = omega1.Scale (angularDamp);
// restore body velocity and angular velocity
NewtonBodySetOmega (body, &omega0[0]);
NewtonBodySetVelocity(body, &veloc0[0]);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector angularMomentum (Ixx, Iyy, Izz);
angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));
dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
dVector torque (angularMomentum.Scale(invTimeStep));
NewtonBodyAddForce(body, &force[0]);
NewtonBodyAddTorque(body, &torque[0]);
// make sure the body is unfrozen, if it is picked
NewtonBodySetSleepState (body, 0);
NewtonWorldCriticalSectionUnlock (world);
}
void ApplyTracktionForce (dFloat timestep, const NewtonBody* track)
{
dVector veloc;
dVector omega;
dMatrix matrix;
NewtonBodyGetOmega(m_body0, &omega[0]);
NewtonBodyGetVelocity(m_body0, &veloc[0]);
NewtonBodyGetMatrix (m_body0, &matrix[0][0]);
// itetate over the contact list and condition each contact direction anc contact acclerations
for (NewtonJoint* contactJoint = NewtonBodyGetFirstContactJoint (track); contactJoint; contactJoint = NewtonBodyGetNextContactJoint (track, contactJoint)) {
_ASSERTE ((NewtonJointGetBody0 (contactJoint) == track) || (NewtonJointGetBody1 (contactJoint) == track));
#ifdef REMOVE_REDUNDAT_CONTACT
int contactCount;
contactCount = NewtonContactJointGetContactCount(contactJoint);
if (contactCount > 2) {
// project the contact to the bounday of the conve hull o fteh trhread foot ptint
dFloat maxDist;
dFloat minDist;
void* minContact;
void* maxContact;
dMatrix matrix;
minContact = NULL;
maxContact = NULL;
NewtonBodyGetMatrix (track, &matrix[0][0]);
maxDist = -1.0e10f;
minDist = -1.0e10f;
//find the best two contacts and remove all others
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dFloat dist;
dVector point;
dVector normal;
NewtonMaterial* material;
material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]);
dist = matrix.m_front % point;
if (dist > maxDist) {
maxDist = dist;
maxContact = contact;
}
if (-dist > minDist) {
minDist = -dist;
minContact = contact;
}
}
// now delete all reduntact contacts
void* nextContact;
NewtonWorld* world;
world = NewtonBodyGetWorld (track);
NewtonWorldCriticalSectionLock(world);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = nextContact) {
nextContact = NewtonContactJointGetNextContact (contactJoint, contact);
if (!((contact == minContact) || (contact == maxContact))) {
NewtonContactJointRemoveContact (contactJoint, contact);
}
}
NewtonWorldCriticalSectionUnlock(world);
}
#endif
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dFloat speed;
dFloat accel;
dVector point;
dVector normal;
dVector dir0;
dVector dir1;
NewtonMaterial* material;
material = NewtonContactGetMaterial (contact);
NewtonMaterialContactRotateTangentDirections (material, &matrix.m_front[0]);
NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]);
NewtonMaterialGetContactTangentDirections (material, &dir0[0], &dir1[0]);
dVector posit (point - matrix.m_posit);
veloc += omega * posit;
speed = veloc % dir0;
// accel = m_accel - 0.1f * speed + (((posit % m_matrix.m_right) > 0.0f) ? m_turnAccel : - m_turnAccel);
accel = m_veloc + (((posit % matrix.m_right) > 0.0f) ? m_turnVeloc : - m_turnVeloc);
accel = (accel - speed) * 0.5f / timestep;
// NewtonMaterialSetContactStaticFrictionCoef (material, 1.0f, 0);
// NewtonMaterialSetContactKineticFrictionCoef (material, 1.0f, 0);
NewtonMaterialSetContactFrictionCoef (material, 1.0f, 1.0f, 0);
// NewtonMaterialSetContactStaticFrictionCoef (material, 0.5f, 1);
// NewtonMaterialSetContactKineticFrictionCoef (material, 0.5f, 1);
NewtonMaterialSetContactFrictionCoef (material, 0.5f, 0.5f, 1);
NewtonMaterialSetContactTangentAcceleration (material, accel, 0);
}
// for debug purpose show the contact
ShowJointContacts (contactJoint);
}
}
