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); } }