diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp | 1367 |
1 files changed, 1367 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp new file mode 100644 index 0000000..27c90bc --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh.cpp @@ -0,0 +1,1367 @@ +/************************************************************************* + * * + * 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 <ode/collision.h> +#include <ode/rotation.h> +#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<sz; ++i) + hash_input ^= hash_v[i]; + + hash = (( hash << 4 ) + (hash_input >> 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;i<node->m_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<dxTriMesh *>(g1)->fetchMeshTriangle(v1, id1, TLPosition1, TLRotation1); + static_cast<dxTriMesh *>(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&&i<Contacts.Count) + { + clipped[clippedcount][0] = Contacts.Points[vi][0]; + clipped[clippedcount][1] = Contacts.Points[vi][1]; + clipped[clippedcount][2] = Contacts.Points[vi][2]; + clipped[clippedcount][3] = 1.0f; + clippedcount++; + } + } + else + { + + if(i>0) + { + 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;i<verticecount;i++) + { + dist = DOT(vertices[i],dir); + if(vmin>dist) vmin=dist; + else if(vmax<dist) vmax=dist; + } +} + +///returns the penetration depth +dReal MostDeepPoints( + LineContactSet & points, + const dVector3 plane_normal, + dReal plane_dist, + LineContactSet & deep_points) +{ + int i; + int max_candidates[8]; + dReal maxdeep=-dInfinity; + dReal dist; + + deep_points.Count = 0; + for(i=0;i<points.Count;i++) + { + dist = DOT(plane_normal,points.Points[i]) - plane_dist; + dist *= -1.0f; + if(dist>maxdeep) + { + 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;i<deep_points.Count;i++) + { + SET(deep_points.Points[i],points.Points[max_candidates[i]]); + } + return maxdeep; + +} + +void ClipPointsByTri( + const dVector3 * points, int pointcount, + const dVector3 tri[3], + const dVector3 triplanenormal, + dReal triplanedist, + LineContactSet & clipped_points, + bool triplane_clips) +{ + ///build edges planes + int i; + dVector4 plane; + + clipped_points.Count = pointcount; + memcpy(&clipped_points.Points[0],&points[0],pointcount*sizeof(dVector3)); + for(i=0;i<3;i++) + { + if (BuildEdgePlane( + tri[i],tri[(i+1)%3],triplanenormal, + plane,plane[3])) + { + ClipConvexPolygonAgainstPlane( + plane, + plane[3], + clipped_points); + } + } + + if(triplane_clips) + { + ClipConvexPolygonAgainstPlane( + triplanenormal, + triplanedist, + clipped_points); + } +} + + +///returns the penetration depth +dReal FindTriangleTriangleCollision( + const dVector3 tri1[3], + const dVector3 tri2[3], + dVector3 separating_normal, + LineContactSet & deep_points) +{ + dReal maxdeep=dInfinity; + dReal dist; + int mostdir=0, /*mostface=0,*/ currdir=0; + // dReal vmin1,vmax1,vmin2,vmax2; + // dVector3 crossdir, pt1,pt2; + dVector4 tri1plane,tri2plane; + separating_normal[3] = 0.0f; + bool bl; + LineContactSet clipped_points1,clipped_points2; + LineContactSet deep_points1,deep_points2; + // It is necessary to initialize the count because both conditional statements + // might be skipped leading to uninitialized count being used for memcpy in if(mostdir==0) + deep_points1.Count = 0; + + ////find interval face1 + + bl = BuildPlane(tri1[0],tri1[1],tri1[2], + tri1plane,tri1plane[3]); + clipped_points1.Count = 0; + + if(bl) + { + ClipPointsByTri( + tri2, 3, + tri1, + tri1plane, + tri1plane[3], + clipped_points1,false); + + + + maxdeep = MostDeepPoints( + clipped_points1, + tri1plane, + tri1plane[3], + deep_points1); + SET(separating_normal,tri1plane); + + } + currdir++; + + ////find interval face2 + + bl = BuildPlane(tri2[0],tri2[1],tri2[2], + tri2plane,tri2plane[3]); + + + clipped_points2.Count = 0; + if(bl) + { + ClipPointsByTri( + tri1, 3, + tri2, + tri2plane, + tri2plane[3], + clipped_points2,false); + + + + dist = MostDeepPoints( + clipped_points2, + tri2plane, + tri2plane[3], + deep_points2); + + + + if(dist<maxdeep) + { + maxdeep = dist; + mostdir = currdir; + //mostface = 1; + SET(separating_normal,tri2plane); + } + } + currdir++; + + + ///find edge edge distances + ///test each edge plane + + /*for(i=0;i<3;i++) + { + + + for(j=0;j<3;j++) + { + + + bl = BuildEdgesDir( + tri1[i],tri1[(i+1)%3], + tri2[j],tri2[(j+1)%3], + crossdir); + + ////find plane distance + + if(bl) + { + FindInterval(tri1,3,crossdir,vmin1,vmax1); + FindInterval(tri2,3,crossdir,vmin2,vmax2); + + dist = IntervalPenetration( + vmin1, + vmax1, + vmin2, + vmax2); + if(dist<maxdeep) + { + maxdeep = dist; + mostdir = currdir; + SET(separating_normal,crossdir); + } + } + currdir++; + } + }*/ + + + ////check most dir for contacts + if(mostdir==0) + { + ///find most deep points + deep_points.Count = deep_points1.Count; + memcpy( + &deep_points.Points[0], + &deep_points1.Points[0], + deep_points1.Count*sizeof(dVector3)); + + ///invert normal for point to tri1 + MULT(separating_normal,separating_normal,-1.0f); + } + else if(mostdir==1) + { + deep_points.Count = deep_points2.Count; + memcpy( + &deep_points.Points[0], + &deep_points2.Points[0], + deep_points2.Count*sizeof(dVector3)); + + } + /*else + {///edge separation + mostdir -= 2; + + //edge 2 + j = mostdir%3; + //edge 1 + i = mostdir/3; + + ///find edge closest points + dClosestLineSegmentPoints( + tri1[i],tri1[(i+1)%3], + tri2[j],tri2[(j+1)%3], + pt1,pt2); + ///find correct direction + + SUB(crossdir,pt2,pt1); + + vmin1 = LENGTH(crossdir); + if(vmin1<REAL(0.000001)) + { + + if(mostface==0) + { + 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); + } + + } + 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<contactpoints.Count) + { + PushNewContact( g1, g2, TriIndex1, TriIndex2, + contactpoints.Points[ccount], + normal, depth, Flags, hashcontactset, + Contacts,Stride,contactcount); + + // Continue loop even after contacts are full + // as existing contacts' normals/depths might be updated + // Break only if contacts are not important + if ((contactcount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) + { + break; + } + + ccount++; + } + return true; +} + + +#endif // !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER + + +#endif // dTRIMESH_OPCODE + + +////////////////////////////////////////////////////////////////////////// + +#if dTRIMESH_GIMPACT + +#include "gimpact_contact_export_helper.h" +#include "gimpact_gim_contact_accessor.h" + + +// +// GIMPACT TRIMESH-TRIMESH COLLIDER +// + +/*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); + + int result = 0; + + dxTriMesh *triMesh1 = static_cast<dxTriMesh *>(g1); + dxTriMesh *triMesh2 = static_cast<dxTriMesh *>(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 + |