/************************************************************************* * * * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * * All rights reserved. Email: russ@q12.org Web: www.q12.org * * * * This library is free software; you can redistribute it and/or * * modify it under the terms of EITHER: * * (1) The GNU Lesser General Public License as published by the Free * * Software Foundation; either version 2.1 of the License, or (at * * your option) any later version. The text of the GNU Lesser * * General Public License is included with this library in the * * file LICENSE.TXT. * * (2) The BSD-style license that is included with this library in * * the file LICENSE-BSD.TXT. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * * LICENSE.TXT and LICENSE-BSD.TXT for more details. * * * *************************************************************************/ // OPCODE TriMesh/TriMesh collision code // Written at 2006-10-28 by Francisco León (http://gimpact.sourceforge.net) #ifdef _MSC_VER #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints #endif #include #include #include "config.h" #include "matrix.h" #include "odemath.h" #if dTRIMESH_ENABLED #include "collision_util.h" #include "collision_trimesh_internal.h" #if !dTLS_ENABLED // Have collider cache instance unconditionally of OPCODE or GIMPACT selection /*extern */TrimeshCollidersCache g_ccTrimeshCollidersCache; #endif #if dTRIMESH_OPCODE // New Implementation #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER #define SMALL_ELT REAL(2.5e-4) #define EXPANDED_ELT_THRESH REAL(1.0e-3) #define DISTANCE_EPSILON REAL(1.0e-8) #define VELOCITY_EPSILON REAL(1.0e-5) #define TINY_PENETRATION REAL(5.0e-6) struct LineContactSet { enum { MAX_POINTS = 8 }; dVector3 Points[MAX_POINTS]; int Count; }; // static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); -- not used static inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B); //static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ); //static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3); static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& ); ///returns the penetration depth static dReal MostDeepPoints( LineContactSet & points, const dVector3 plane_normal, dReal plane_dist, LineContactSet & deep_points); static bool TriTriContacts(const dVector3 tr1[3], const dVector3 tr2[3], int TriIndex1, int TriIndex2, dxGeom* g1, dxGeom* g2, int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset, dContactGeom* Contacts, int Stride, int &contactcount); /* some math macros */ #define IS_ZERO(v) (!(v)[0] && !(v)[1] && !(v)[2]) #define CROSS(dest,v1,v2) dCalcVectorCross3(dest, v1, v2) #define DOT(v1,v2) dCalcVectorDot3(v1, v2) #define SUB(dest,v1,v2) dSubtractVectors3(dest, v1, v2) #define ADD(dest,v1,v2) dAddVectors3(dest, v1, v2) #define MULT(dest,v,factor) dCopyScaledVector3(dest, v, factor) #define SET(dest,src) dCopyVector3(dest, src) #define SMULT(p,q,s) dCopyScaledVector3(p, q, s) #define COMBO(combo,p,t,q) dAddVectorScaledVector3(combo, p, q, t) #define LENGTH(x) dCalcVectorLength3(x) #define DEPTH(d, p, q, n) d = dCalcPointDepth3(q, p, n) static inline void SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2, dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2, dVector3 n, dVector3 n1, dVector3 n2) { if (pen_v == v1) { pen_v = v2; pen_elt = elt_f2; col_v = v1; SET(n, n1); } else { pen_v = v1; pen_elt = elt_f1; col_v = v2; SET(n, n2); } } ///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE/////////////////////////////// ////* Written by Francisco León (http://gimpact.sourceforge.net) */// #define CONTACT_DIFF_EPSILON REAL(0.00001) #if defined(dDOUBLE) #define CONTACT_NORMAL_ZERO REAL(0.0000001) #else // if defined(dSINGLE) #define CONTACT_NORMAL_ZERO REAL(0.00001) #endif #define CONTACT_POS_HASH_QUOTIENT REAL(10000.0) #define dSQRT3 REAL(1.7320508075688773) static void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact) { key.m_contact = contact; unsigned int hash=0; int i = 0; while (true) { dReal coord = contact->pos[i]; coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT); const int sz = sizeof(coord) / sizeof(unsigned); dIASSERT(sizeof(coord) % sizeof(unsigned) == 0); unsigned hash_v[ sz ]; memcpy(hash_v, &coord, sizeof(coord)); unsigned int hash_input = hash_v[0]; for (int i=1; i> 24)) ^ ( hash >> 28 ); hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 ); hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 ); hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 ); if (++i == 3) { break; } hash = (hash << 11) | (hash >> 21); } key.m_key = hash; } static inline unsigned int MakeContactIndex(unsigned int key) { dIASSERT(CONTACTS_HASHSIZE == 256); unsigned int index = key ^ (key >> 16); index = (index ^ (index >> 8)) & 0xFF; return index; } static dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node) { for(int i=0;im_keycount;i++) { if(node->m_keyarray[i].m_key == contactkey->m_key) { dContactGeom *contactfound = node->m_keyarray[i].m_contact; if (dCalcPointsDistance3(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/) { return contactfound; } } } if (node->m_keycount < MAXCONTACT_X_NODE) { node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact; node->m_keyarray[node->m_keycount].m_key = contactkey->m_key; node->m_keycount++; } else { dDEBUGMSG("Trimesh-trimesh contach hash table bucket overflow - close contacts might not be culled"); } return contactkey->m_contact; } static void RemoveNewContactFromNode(const CONTACT_KEY * contactkey, CONTACT_KEY_HASH_NODE * node) { dIASSERT(node->m_keycount > 0); if (node->m_keyarray[node->m_keycount - 1].m_contact == contactkey->m_contact) { node->m_keycount -= 1; } else { dIASSERT(node->m_keycount == MAXCONTACT_X_NODE); } } static void RemoveArbitraryContactFromNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node) { dIASSERT(node->m_keycount > 0); int keyindex, lastkeyindex = node->m_keycount - 1; // Do not check the last contact for (keyindex = 0; keyindex < lastkeyindex; keyindex++) { if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact) { node->m_keyarray[keyindex] = node->m_keyarray[lastkeyindex]; break; } } dIASSERT(keyindex < lastkeyindex || node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match node->m_keycount = lastkeyindex; } static void UpdateArbitraryContactInNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node, dContactGeom *pwithcontact) { dIASSERT(node->m_keycount > 0); int keyindex, lastkeyindex = node->m_keycount - 1; // Do not check the last contact for (keyindex = 0; keyindex < lastkeyindex; keyindex++) { if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact) { break; } } dIASSERT(keyindex < lastkeyindex || node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match node->m_keyarray[keyindex].m_contact = pwithcontact; } static void ClearContactSet(CONTACT_KEY_HASH_TABLE &hashcontactset) { memset(&hashcontactset, 0, sizeof(CONTACT_KEY_HASH_TABLE)); } //return true if found static dContactGeom *InsertContactInSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &newkey) { unsigned int index = MakeContactIndex(newkey.m_key); return AddContactToNode(&newkey, &hashcontactset[index]); } static void RemoveNewContactFromSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey) { unsigned int index = MakeContactIndex(contactkey.m_key); RemoveNewContactFromNode(&contactkey, &hashcontactset[index]); } static void RemoveArbitraryContactFromSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey) { unsigned int index = MakeContactIndex(contactkey.m_key); RemoveArbitraryContactFromNode(&contactkey, &hashcontactset[index]); } static void UpdateArbitraryContactInSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey, dContactGeom *pwithcontact) { unsigned int index = MakeContactIndex(contactkey.m_key); UpdateArbitraryContactInNode(&contactkey, &hashcontactset[index], pwithcontact); } static bool AllocNewContact( const dVector3 newpoint, dContactGeom *& out_pcontact, int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset, dContactGeom* Contacts, int Stride, int &contactcount) { bool allocated_new = false; dContactGeom dLocalContact; dContactGeom * pcontact = contactcount != (Flags & NUMC_MASK) ? SAFECONTACT(Flags, Contacts, contactcount, Stride) : &dLocalContact; pcontact->pos[0] = newpoint[0]; pcontact->pos[1] = newpoint[1]; pcontact->pos[2] = newpoint[2]; pcontact->pos[3] = 1.0f; CONTACT_KEY newkey; UpdateContactKey(newkey, pcontact); dContactGeom *pcontactfound = InsertContactInSet(hashcontactset, newkey); if (pcontactfound == pcontact) { if (pcontactfound != &dLocalContact) { contactcount++; } else { RemoveNewContactFromSet(hashcontactset, newkey); pcontactfound = NULL; } allocated_new = true; } out_pcontact = pcontactfound; return allocated_new; } static void FreeExistingContact(dContactGeom *pcontact, int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset, dContactGeom *Contacts, int Stride, int &contactcount) { CONTACT_KEY contactKey; UpdateContactKey(contactKey, pcontact); RemoveArbitraryContactFromSet(hashcontactset, contactKey); int lastContactIndex = contactcount - 1; dContactGeom *plastContact = SAFECONTACT(Flags, Contacts, lastContactIndex, Stride); if (pcontact != plastContact) { *pcontact = *plastContact; CONTACT_KEY lastContactKey; UpdateContactKey(lastContactKey, plastContact); UpdateArbitraryContactInSet(hashcontactset, lastContactKey, pcontact); } contactcount = lastContactIndex; } static dContactGeom * PushNewContact( dxGeom* g1, dxGeom* g2, int TriIndex1, int TriIndex2, const dVector3 point, dVector3 normal, dReal depth, int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset, dContactGeom* Contacts, int Stride, int &contactcount) { dIASSERT(dFabs(dVector3Length((const dVector3 &)(*normal)) - REAL(1.0)) < REAL(1e-6)); // This assumption is used in the code dContactGeom * pcontact; if (!AllocNewContact(point, pcontact, Flags, hashcontactset, Contacts, Stride, contactcount)) { const dReal depthDifference = depth - pcontact->depth; if (depthDifference > CONTACT_DIFF_EPSILON) { pcontact->normal[0] = normal[0]; pcontact->normal[1] = normal[1]; pcontact->normal[2] = normal[2]; pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging pcontact->depth = depth; pcontact->g1 = g1; pcontact->g2 = g2; pcontact->side1 = TriIndex1; pcontact->side2 = TriIndex2; } else if (depthDifference >= -CONTACT_DIFF_EPSILON) ///average { if(pcontact->g1 == g2) { MULT(normal,normal, REAL(-1.0)); int tempInt = TriIndex1; TriIndex1 = TriIndex2; TriIndex2 = tempInt; // This should be discarded by optimizer as g1 and g2 are // not used any more but it's preferable to keep this line for // the sake of consistency in variable values. dxGeom *tempGeom = g1; g1 = g2; g2 = tempGeom; } const dReal oldLen = pcontact->normal[3]; COMBO(pcontact->normal, normal, oldLen, pcontact->normal); const dReal len = LENGTH(pcontact->normal); if (len > CONTACT_NORMAL_ZERO) { MULT(pcontact->normal, pcontact->normal, REAL(1.0) / len); pcontact->normal[3] = len; pcontact->side1 = ((dxTriMesh *)pcontact->g1)->m_TriMergeCallback ? ((dxTriMesh *)pcontact->g1)->m_TriMergeCallback((dxTriMesh *)pcontact->g1, pcontact->side1, TriIndex1) : -1; pcontact->side2 = ((dxTriMesh *)pcontact->g2)->m_TriMergeCallback ? ((dxTriMesh *)pcontact->g2)->m_TriMergeCallback((dxTriMesh *)pcontact->g2, pcontact->side2, TriIndex2) : -1; } else { FreeExistingContact(pcontact, Flags, hashcontactset, Contacts, Stride, contactcount); } } } // Contact can be not available if buffer is full else if (pcontact) { pcontact->normal[0] = normal[0]; pcontact->normal[1] = normal[1]; pcontact->normal[2] = normal[2]; pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging pcontact->depth = depth; pcontact->g1 = g1; pcontact->g2 = g2; pcontact->side1 = TriIndex1; pcontact->side2 = TriIndex2; } return pcontact; } //////////////////////////////////////////////////////////////////////////////////////////// /*extern */ int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) { dIASSERT (Stride >= (int)sizeof(dContactGeom)); dIASSERT (g1->type == dTriMeshClass); dIASSERT (g2->type == dTriMeshClass); dIASSERT ((Flags & NUMC_MASK) >= 1); dxTriMesh* TriMesh1 = (dxTriMesh*) g1; dxTriMesh* TriMesh2 = (dxTriMesh*) g2; //dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals; //dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals; const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1); // TLRotation1 = column-major order const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1); const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2); // TLRotation2 = column-major order const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2); const unsigned uiTLSKind = TriMesh1->getParentSpaceTLSKind(); dIASSERT(uiTLSKind == TriMesh2->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); AABBTreeCollider& Collider = pccColliderCache->m_AABBTreeCollider; BVTCache &ColCache = pccColliderCache->ColCache; CONTACT_KEY_HASH_TABLE &hashcontactset = pccColliderCache->m_hashcontactset; ColCache.Model0 = &TriMesh1->retrieveMeshBVTreeRef(); ColCache.Model1 = &TriMesh2->retrieveMeshBVTreeRef(); ////Prepare contact list ClearContactSet(hashcontactset); // Collision query Matrix4x4 amatrix, bmatrix; dVector3 TLOffsetPosition1 = { REAL(0.0), }; dVector3 TLOffsetPosition2; dSubtractVectors3(TLOffsetPosition2, TLPosition2, TLPosition1); MakeMatrix(TLOffsetPosition1, TLRotation1, amatrix); MakeMatrix(TLOffsetPosition2, TLRotation2, bmatrix); bool IsOk = Collider.Collide(ColCache, &amatrix, &bmatrix); if (IsOk) { // Get collision status => if true, objects overlap if ( Collider.GetContactStatus() ) { // Number of colliding pairs and list of pairs int TriCount = Collider.GetNbPairs(); const Pair* CollidingPairs = Collider.GetPairs(); if (TriCount > 0) { // step through the pairs, adding contacts int id1, id2; int OutTriCount = 0; dVector3 v1[3], v2[3]; for (int i = 0; i < TriCount; i++) { id1 = CollidingPairs[i].id0; id2 = CollidingPairs[i].id1; // grab the colliding triangles static_cast(g1)->fetchMeshTriangle(v1, id1, TLPosition1, TLRotation1); static_cast(g2)->fetchMeshTriangle(v2, id2, TLPosition2, TLRotation2); // Since we'll be doing matrix transformations, we need to // make sure that all vertices have four elements for (int j=0; j<3; j++) { v1[j][3] = 1.0; v2[j][3] = 1.0; } TriTriContacts(v1,v2, id1,id2, g1, g2, Flags, hashcontactset, Contacts,Stride,OutTriCount); // Continue loop even after contacts are full // as existing contacts' normals/depths might be updated // Break only if contacts are not important if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } // Return the number of contacts return OutTriCount; } } } // There was some kind of failure during the Collide call or // there are no faces overlapping return 0; } /* -- not used static void GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data) { dVector3 Out[3]; FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out); for (int i = 0; i < 3; i++) triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]); } */ // // // #define B11 B[0] #define B12 B[1] #define B13 B[2] #define B14 B[3] #define B21 B[4] #define B22 B[5] #define B23 B[6] #define B24 B[7] #define B31 B[8] #define B32 B[9] #define B33 B[10] #define B34 B[11] #define B41 B[12] #define B42 B[13] #define B43 B[14] #define B44 B[15] #define Binv11 Binv[0] #define Binv12 Binv[1] #define Binv13 Binv[2] #define Binv14 Binv[3] #define Binv21 Binv[4] #define Binv22 Binv[5] #define Binv23 Binv[6] #define Binv24 Binv[7] #define Binv31 Binv[8] #define Binv32 Binv[9] #define Binv33 Binv[10] #define Binv34 Binv[11] #define Binv41 Binv[12] #define Binv42 Binv[13] #define Binv43 Binv[14] #define Binv44 Binv[15] static inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B) { B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0]; B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1]; B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2]; B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0; } #if 0 static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ) { dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43) -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42) +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42) +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41) -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41) +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41); dAASSERT (det != 0.0); det = 1.0 / det; Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32))); Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12))); Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22))); Binv14 = 0.0f; Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33))); Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13))); Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23))); Binv24 = 0.0f; Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31))); Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11))); Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21))); Binv34 = 0.0f; Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42))); Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42))); Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22))); Binv44 = 1.0f; } #endif // Find the intersectiojn point between a coplanar line segement, // defined by X1 and X2, and a ray defined by X3 and direction N. // // This forumla for this calculation is: // (c x b) . (a x b) // Q = x1 + a ------------------- // | a x b | ^2 // // where a = x2 - x1 // b = x4 - x3 // c = x3 - x1 // x1 and x2 are the edges of the triangle, and x3 is CoplanarPt // and x4 is (CoplanarPt - n) #if 0 static int IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n, dVector3 out_pt) { dVector3 a, b, c, x4; ADD(x4, x3, n); // x4 = x3 + n SUB(a, x2, x1); // a = x2 - x1 SUB(b, x4, x3); SUB(c, x3, x1); dVector3 tmp1, tmp2; CROSS(tmp1, c, b); CROSS(tmp2, a, b); dReal num, denom; num = dCalcVectorDot3(tmp1, tmp2); denom = LENGTH( tmp2 ); dReal s; s = num /(denom*denom); for (int i=0; i<3; i++) out_pt[i] = x1[i] + a[i]*s; // Test if this intersection is "behind" x3, w.r.t. n SUB(a, x3, out_pt); if (dCalcVectorDot3(a, n) > 0.0) return 0; // Test if this intersection point is outside the edge limits, // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside // else outside SUB(a, out_pt, x1); SUB(b, out_pt, x2); if (dCalcVectorDot3(a,b) < 0.0) return 1; else return 0; } #endif void PlaneClipSegment( const dVector3 s1, const dVector3 s2, const dVector3 N, dReal C, dVector3 clipped) { dReal dis1,dis2; dis1 = DOT(s1,N)-C; SUB(clipped,s2,s1); dis2 = DOT(clipped,N); MULT(clipped,clipped,-dis1/dis2); ADD(clipped,clipped,s1); clipped[3] = 1.0f; } /* ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by CONTACTS, with a plane (described by N and C distance from origin). Note: the input vertices are assumed to be in invcounterclockwise order. changed by Francisco Leon (http://gimpact.sourceforge.net) */ static void ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C, LineContactSet& Contacts ) { int i, vi, prevclassif=32000, classif; /* classif 0 : back, 1 : front */ dReal d; dVector3 clipped[8]; int clippedcount =0; if(Contacts.Count==0) { return; } for(i=0;i<=Contacts.Count;i++) { vi = i%Contacts.Count; d = DOT(N,Contacts.Points[vi]) - C; ////classify point if(d>REAL(1.0e-8)) classif = 1; else classif = 0; if(classif == 0)//back { if(i>0) { if(prevclassif==1)///in front { ///add clipped point if(clippedcount<8) { PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi], N,C,clipped[clippedcount]); clippedcount++; } } } ///add point if(clippedcount<8&&i0) { if(prevclassif==0) { ///add point if(clippedcount<8) { PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi], N,C,clipped[clippedcount]); clippedcount++; } } } } prevclassif = classif; } if(clippedcount==0) { Contacts.Count = 0; return; } Contacts.Count = clippedcount; memcpy( Contacts.Points, clipped, clippedcount * sizeof(dVector3) ); return; } bool BuildPlane(const dVector3 s0, const dVector3 s1,const dVector3 s2, dVector3 Normal, dReal & Dist) { dVector3 e0,e1; SUB(e0,s1,s0); SUB(e1,s2,s0); CROSS(Normal,e0,e1); if (!dSafeNormalize3(Normal)) { return false; } Dist = DOT(Normal,s0); return true; } // bool BuildEdgesDir(const dVector3 s0, const dVector3 s1, // const dVector3 t0, const dVector3 t1, // dVector3 crossdir) // { // dVector3 e0,e1; // // SUB(e0,s1,s0); // SUB(e1,t1,t0); // CROSS(crossdir,e0,e1); // // if (!dSafeNormalize3(crossdir)) // { // return false; // } // return true; // } bool BuildEdgePlane( const dVector3 s0, const dVector3 s1, const dVector3 normal, dVector3 plane_normal, dReal & plane_dist) { dVector3 e0; SUB(e0,s1,s0); CROSS(plane_normal,e0,normal); if (!dSafeNormalize3(plane_normal)) { return false; } plane_dist = DOT(plane_normal,s0); return true; } /* Positive penetration Negative number: they are separated */ dReal IntervalPenetration(dReal &vmin1,dReal &vmax1, dReal &vmin2,dReal &vmax2) { if(vmax1<=vmin2) { return -(vmin2-vmax1); } else { if(vmax2<=vmin1) { return -(vmin1-vmax2); } else { if(vmax1<=vmax2) { return vmax1-vmin2; } return vmax2-vmin1; } } return 0; } void FindInterval( const dVector3 * vertices, int verticecount, dVector3 dir,dReal &vmin,dReal &vmax) { dReal dist; int i; vmin = DOT(vertices[0],dir); vmax = vmin; for(i=1;idist) vmin=dist; else if(vmaxmaxdeep) { maxdeep = dist; deep_points.Count=1; max_candidates[deep_points.Count-1] = i; } else if(dist+REAL(0.000001)>=maxdeep) { deep_points.Count++; max_candidates[deep_points.Count-1] = i; } } for(i=0;i0.0) { MULT(separating_normal,separating_normal,-1.0f); deep_points.Count = 1; SET(deep_points.Points[0],pt2); } else { deep_points.Count = 1; SET(deep_points.Points[0],pt2); } } else { vmin1 = DOT(separating_normal,tri2plane); if(vmin1<0.0) { MULT(separating_normal,separating_normal,-1.0f); deep_points.Count = 1; SET(deep_points.Points[0],pt2); } else { deep_points.Count = 1; SET(deep_points.Points[0],pt2); } } } else { MULT(separating_normal,crossdir,1.0f/vmin1); vmin1 = DOT(separating_normal,tri1plane); if(vmin1>0.0) { MULT(separating_normal,separating_normal,-1.0f); deep_points.Count = 1; SET(deep_points.Points[0],pt2); } else { deep_points.Count = 1; SET(deep_points.Points[0],pt2); } } }*/ return maxdeep; } ///SUPPORT UP TO 8 CONTACTS bool TriTriContacts(const dVector3 tr1[3], const dVector3 tr2[3], int TriIndex1, int TriIndex2, dxGeom* g1, dxGeom* g2, int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset, dContactGeom* Contacts, int Stride, int &contactcount) { dVector4 normal; dReal depth; ///Test Tri Vs Tri // dContactGeom* pcontact; int ccount = 0; LineContactSet contactpoints; contactpoints.Count = 0; ///find best direction depth = FindTriangleTriangleCollision( tr1, tr2, normal, contactpoints); if(depth<0.0f) return false; ccount = 0; while (ccount= (int)sizeof(dContactGeom)); dIASSERT (g1->type == dTriMeshClass); dIASSERT (g2->type == dTriMeshClass); dIASSERT ((Flags & NUMC_MASK) >= 1); int result = 0; dxTriMesh *triMesh1 = static_cast(g1); dxTriMesh *triMesh2 = static_cast(g2); //Create contact list GDYNAMIC_ARRAY trimeshContacts; GIM_CREATE_CONTACT_LIST(trimeshContacts); triMesh1->recomputeAABB(); triMesh2->recomputeAABB(); //Collide trimeshes gim_trimesh_trimesh_collision(&triMesh1->m_collision_trimesh, &triMesh2->m_collision_trimesh, &trimeshContacts); unsigned contactCount = trimeshContacts.m_size; if (contactCount != 0) { GIM_CONTACT *pTriMeshContacts = GIM_DYNARRAY_POINTER(GIM_CONTACT, trimeshContacts); dxGIMCContactAccessor contactAccessor(pTriMeshContacts, g1, g2); unsigned culledContactCount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactAccessor, contactCount, Flags, Contacts, Stride); result = culledContactCount; } GIM_DYNARRAY_DESTROY(trimeshContacts); return result; } #endif // dTRIMESH_GIMPACT #endif // dTRIMESH_ENABLED