1498 lines
39 KiB
C++
Executable File
1498 lines
39 KiB
C++
Executable File
/*************************************************************************
|
|
* *
|
|
* 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. *
|
|
* *
|
|
*************************************************************************/
|
|
|
|
|
|
/*************************************************************************
|
|
* *
|
|
* Triangle-box collider by Alen Ladavac and Vedran Klanac. *
|
|
* Ported to ODE by Oskari Nyman. *
|
|
* *
|
|
*************************************************************************/
|
|
|
|
|
|
#include <ode/collision.h>
|
|
#include <ode/matrix.h>
|
|
#include <ode/rotation.h>
|
|
#include <ode/odemath.h>
|
|
#include "collision_util.h"
|
|
|
|
#define TRIMESH_INTERNAL
|
|
#include "collision_trimesh_internal.h"
|
|
|
|
#if dTRIMESH_ENABLED
|
|
|
|
|
|
static void
|
|
GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
|
|
dxGeom* in_g1, dxGeom* in_g2,
|
|
const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
|
|
int& OutTriCount);
|
|
|
|
|
|
// largest number, double or float
|
|
#if defined(dSINGLE)
|
|
#define MAXVALUE FLT_MAX
|
|
#else
|
|
#define MAXVALUE DBL_MAX
|
|
#endif
|
|
|
|
|
|
// dVector3
|
|
// r=a-b
|
|
#define SUBTRACT(a,b,r) do{ \
|
|
(r)[0]=(a)[0] - (b)[0]; \
|
|
(r)[1]=(a)[1] - (b)[1]; \
|
|
(r)[2]=(a)[2] - (b)[2]; }while(0)
|
|
|
|
|
|
// dVector3
|
|
// a=b
|
|
#define SET(a,b) do{ \
|
|
(a)[0]=(b)[0]; \
|
|
(a)[1]=(b)[1]; \
|
|
(a)[2]=(b)[2]; }while(0)
|
|
|
|
|
|
// dMatrix3
|
|
// a=b
|
|
#define SETM(a,b) do{ \
|
|
(a)[0]=(b)[0]; \
|
|
(a)[1]=(b)[1]; \
|
|
(a)[2]=(b)[2]; \
|
|
(a)[3]=(b)[3]; \
|
|
(a)[4]=(b)[4]; \
|
|
(a)[5]=(b)[5]; \
|
|
(a)[6]=(b)[6]; \
|
|
(a)[7]=(b)[7]; \
|
|
(a)[8]=(b)[8]; \
|
|
(a)[9]=(b)[9]; \
|
|
(a)[10]=(b)[10]; \
|
|
(a)[11]=(b)[11]; }while(0)
|
|
|
|
|
|
// dVector3
|
|
// r=a+b
|
|
#define ADD(a,b,r) do{ \
|
|
(r)[0]=(a)[0] + (b)[0]; \
|
|
(r)[1]=(a)[1] + (b)[1]; \
|
|
(r)[2]=(a)[2] + (b)[2]; }while(0)
|
|
|
|
|
|
// dMatrix3, int, dVector3
|
|
// v=column a from m
|
|
#define GETCOL(m,a,v) do{ \
|
|
(v)[0]=(m)[(a)+0]; \
|
|
(v)[1]=(m)[(a)+4]; \
|
|
(v)[2]=(m)[(a)+8]; }while(0)
|
|
|
|
|
|
// dVector4, dVector3
|
|
// distance between plane p and point v
|
|
#define POINTDISTANCE(p,v) \
|
|
( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] )
|
|
|
|
|
|
// dVector4, dVector3, dReal
|
|
// construct plane from normal and d
|
|
#define CONSTRUCTPLANE(plane,normal,d) do{ \
|
|
plane[0]=normal[0];\
|
|
plane[1]=normal[1];\
|
|
plane[2]=normal[2];\
|
|
plane[3]=d; }while(0)
|
|
|
|
|
|
// dVector3
|
|
// length of vector a
|
|
#define LENGTHOF(a) \
|
|
dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2])
|
|
|
|
|
|
// box data
|
|
static dMatrix3 mHullBoxRot;
|
|
static dVector3 vHullBoxPos;
|
|
static dVector3 vBoxHalfSize;
|
|
|
|
// mesh data
|
|
static dVector3 vHullDstPos;
|
|
|
|
// global collider data
|
|
static dVector3 vBestNormal;
|
|
static dReal fBestDepth;
|
|
static int iBestAxis = 0;
|
|
static int iExitAxis = 0;
|
|
static dVector3 vE0, vE1, vE2, vN;
|
|
|
|
// global info for contact creation
|
|
static int iFlags;
|
|
static dContactGeom *ContactGeoms;
|
|
static int iStride;
|
|
static dxGeom *Geom1;
|
|
static dxGeom *Geom2;
|
|
static int ctContacts = 0;
|
|
|
|
|
|
|
|
// Test normal of mesh face as separating axis for intersection
|
|
static bool _cldTestNormal( dReal fp0, dReal fR, dVector3 vNormal, int iAxis )
|
|
{
|
|
// calculate overlapping interval of box and triangle
|
|
dReal fDepth = fR+fp0;
|
|
|
|
// if we do not overlap
|
|
if ( fDepth<0 ) {
|
|
// do nothing
|
|
return false;
|
|
}
|
|
|
|
// calculate normal's length
|
|
dReal fLength = LENGTHOF(vNormal);
|
|
// if long enough
|
|
if ( fLength > 0.0f ) {
|
|
|
|
dReal fOneOverLength = 1.0f/fLength;
|
|
// normalize depth
|
|
fDepth = fDepth*fOneOverLength;
|
|
|
|
// get minimum depth
|
|
if (fDepth<fBestDepth) {
|
|
vBestNormal[0] = -vNormal[0]*fOneOverLength;
|
|
vBestNormal[1] = -vNormal[1]*fOneOverLength;
|
|
vBestNormal[2] = -vNormal[2]*fOneOverLength;
|
|
iBestAxis = iAxis;
|
|
//dAASSERT(fDepth>=0);
|
|
fBestDepth = fDepth;
|
|
}
|
|
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
|
|
// Test box axis as separating axis
|
|
static bool _cldTestFace( dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD,
|
|
dVector3 vNormal, int iAxis )
|
|
{
|
|
dReal fMin, fMax;
|
|
|
|
// find min of triangle interval
|
|
if ( fp0 < fp1 ) {
|
|
if ( fp0 < fp2 ) {
|
|
fMin = fp0;
|
|
} else {
|
|
fMin = fp2;
|
|
}
|
|
} else {
|
|
if( fp1 < fp2 ) {
|
|
fMin = fp1;
|
|
} else {
|
|
fMin = fp2;
|
|
}
|
|
}
|
|
|
|
// find max of triangle interval
|
|
if ( fp0 > fp1 ) {
|
|
if ( fp0 > fp2 ) {
|
|
fMax = fp0;
|
|
} else {
|
|
fMax = fp2;
|
|
}
|
|
} else {
|
|
if( fp1 > fp2 ) {
|
|
fMax = fp1;
|
|
} else {
|
|
fMax = fp2;
|
|
}
|
|
}
|
|
|
|
// calculate minimum and maximum depth
|
|
dReal fDepthMin = fR - fMin;
|
|
dReal fDepthMax = fMax + fR;
|
|
|
|
// if we dont't have overlapping interval
|
|
if ( fDepthMin < 0 || fDepthMax < 0 ) {
|
|
// do nothing
|
|
return false;
|
|
}
|
|
|
|
dReal fDepth = 0;
|
|
|
|
// if greater depth is on negative side
|
|
if ( fDepthMin > fDepthMax ) {
|
|
// use smaller depth (one from positive side)
|
|
fDepth = fDepthMax;
|
|
// flip normal direction
|
|
vNormal[0] = -vNormal[0];
|
|
vNormal[1] = -vNormal[1];
|
|
vNormal[2] = -vNormal[2];
|
|
fD = -fD;
|
|
// if greater depth is on positive side
|
|
} else {
|
|
// use smaller depth (one from negative side)
|
|
fDepth = fDepthMin;
|
|
}
|
|
|
|
|
|
// if lower depth than best found so far
|
|
if (fDepth<fBestDepth) {
|
|
// remember current axis as best axis
|
|
vBestNormal[0] = vNormal[0];
|
|
vBestNormal[1] = vNormal[1];
|
|
vBestNormal[2] = vNormal[2];
|
|
iBestAxis = iAxis;
|
|
//dAASSERT(fDepth>=0);
|
|
fBestDepth = fDepth;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// Test cross products of box axis and triangle edges as separating axis
|
|
static bool _cldTestEdge( dReal fp0, dReal fp1, dReal fR, dReal fD,
|
|
dVector3 vNormal, int iAxis )
|
|
{
|
|
dReal fMin, fMax;
|
|
|
|
|
|
// ===== Begin Patch by Francisco Leon, 2006/10/28 =====
|
|
|
|
// Fixed Null Normal. This prevents boxes passing
|
|
// through trimeshes at certain contact angles
|
|
|
|
fMin = vNormal[0] * vNormal[0] +
|
|
vNormal[1] * vNormal[1] +
|
|
vNormal[2] * vNormal[2];
|
|
|
|
if ( fMin <= dEpsilon ) /// THIS NORMAL WOULD BE DANGEROUS
|
|
return true;
|
|
|
|
// ===== Ending Patch by Francisco Leon =====
|
|
|
|
|
|
// calculate min and max interval values
|
|
if ( fp0 < fp1 ) {
|
|
fMin = fp0;
|
|
fMax = fp1;
|
|
} else {
|
|
fMin = fp1;
|
|
fMax = fp0;
|
|
}
|
|
|
|
// check if we overlapp
|
|
dReal fDepthMin = fR - fMin;
|
|
dReal fDepthMax = fMax + fR;
|
|
|
|
// if we don't overlapp
|
|
if ( fDepthMin < 0 || fDepthMax < 0 ) {
|
|
// do nothing
|
|
return false;
|
|
}
|
|
|
|
dReal fDepth;
|
|
|
|
|
|
// if greater depth is on negative side
|
|
if ( fDepthMin > fDepthMax ) {
|
|
// use smaller depth (one from positive side)
|
|
fDepth = fDepthMax;
|
|
// flip normal direction
|
|
vNormal[0] = -vNormal[0];
|
|
vNormal[1] = -vNormal[1];
|
|
vNormal[2] = -vNormal[2];
|
|
fD = -fD;
|
|
// if greater depth is on positive side
|
|
} else {
|
|
// use smaller depth (one from negative side)
|
|
fDepth = fDepthMin;
|
|
}
|
|
|
|
// calculate normal's length
|
|
dReal fLength = LENGTHOF(vNormal);
|
|
|
|
// if long enough
|
|
if ( fLength > 0.0f ) {
|
|
|
|
// normalize depth
|
|
dReal fOneOverLength = 1.0f/fLength;
|
|
fDepth = fDepth*fOneOverLength;
|
|
fD*=fOneOverLength;
|
|
|
|
|
|
// if lower depth than best found so far (favor face over edges)
|
|
if (fDepth*1.5f<fBestDepth) {
|
|
// remember current axis as best axis
|
|
vBestNormal[0] = vNormal[0]*fOneOverLength;
|
|
vBestNormal[1] = vNormal[1]*fOneOverLength;
|
|
vBestNormal[2] = vNormal[2]*fOneOverLength;
|
|
iBestAxis = iAxis;
|
|
//dAASSERT(fDepth>=0);
|
|
fBestDepth = fDepth;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// clip polygon with plane and generate new polygon points
|
|
static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn,
|
|
dVector3 avArrayOut[], int &ctOut,
|
|
const dVector4 &plPlane )
|
|
{
|
|
// start with no output points
|
|
ctOut = 0;
|
|
|
|
int i0 = ctIn-1;
|
|
|
|
// for each edge in input polygon
|
|
for (int i1=0; i1<ctIn; i0=i1, i1++) {
|
|
|
|
|
|
// calculate distance of edge points to plane
|
|
dReal fDistance0 = POINTDISTANCE( plPlane ,avArrayIn[i0] );
|
|
dReal fDistance1 = POINTDISTANCE( plPlane ,avArrayIn[i1] );
|
|
|
|
|
|
// if first point is in front of plane
|
|
if( fDistance0 >= 0 ) {
|
|
// emit point
|
|
avArrayOut[ctOut][0] = avArrayIn[i0][0];
|
|
avArrayOut[ctOut][1] = avArrayIn[i0][1];
|
|
avArrayOut[ctOut][2] = avArrayIn[i0][2];
|
|
ctOut++;
|
|
}
|
|
|
|
// if points are on different sides
|
|
if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
|
|
|
|
// find intersection point of edge and plane
|
|
dVector3 vIntersectionPoint;
|
|
vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
|
|
vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
|
|
vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
|
|
|
|
// emit intersection point
|
|
avArrayOut[ctOut][0] = vIntersectionPoint[0];
|
|
avArrayOut[ctOut][1] = vIntersectionPoint[1];
|
|
avArrayOut[ctOut][2] = vIntersectionPoint[2];
|
|
ctOut++;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
|
|
// reset best axis
|
|
iBestAxis = 0;
|
|
iExitAxis = -1;
|
|
fBestDepth = MAXVALUE;
|
|
|
|
// calculate edges
|
|
SUBTRACT(v1,v0,vE0);
|
|
SUBTRACT(v2,v0,vE1);
|
|
SUBTRACT(vE1,vE0,vE2);
|
|
|
|
// calculate poly normal
|
|
dCROSS(vN,=,vE0,vE1);
|
|
|
|
// extract box axes as vectors
|
|
dVector3 vA0,vA1,vA2;
|
|
GETCOL(mHullBoxRot,0,vA0);
|
|
GETCOL(mHullBoxRot,1,vA1);
|
|
GETCOL(mHullBoxRot,2,vA2);
|
|
|
|
// box halfsizes
|
|
dReal fa0 = vBoxHalfSize[0];
|
|
dReal fa1 = vBoxHalfSize[1];
|
|
dReal fa2 = vBoxHalfSize[2];
|
|
|
|
// calculate relative position between box and triangle
|
|
dVector3 vD;
|
|
SUBTRACT(v0,vHullBoxPos,vD);
|
|
|
|
// calculate length of face normal
|
|
dReal fNLen = LENGTHOF( vN );
|
|
|
|
dVector3 vL;
|
|
dReal fp0, fp1, fp2, fR, fD;
|
|
|
|
// Test separating axes for intersection
|
|
// ************************************************
|
|
// Axis 1 - Triangle Normal
|
|
SET(vL,vN);
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0;
|
|
fp2 = fp0;
|
|
fR=fa0*dFabs( dDOT(vN,vA0) ) + fa1 * dFabs( dDOT(vN,vA1) ) + fa2 * dFabs( dDOT(vN,vA2) );
|
|
|
|
|
|
if( !_cldTestNormal( fp0, fR, vL, 1) ) {
|
|
iExitAxis=1;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
|
|
// Test Faces
|
|
// ************************************************
|
|
// Axis 2 - Box X-Axis
|
|
SET(vL,vA0);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 + dDOT(vA0,vE0);
|
|
fp2 = fp0 + dDOT(vA0,vE1);
|
|
fR = fa0;
|
|
|
|
|
|
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 2) ) {
|
|
iExitAxis=2;
|
|
return false;
|
|
}
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 3 - Box Y-Axis
|
|
SET(vL,vA1);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 + dDOT(vA1,vE0);
|
|
fp2 = fp0 + dDOT(vA1,vE1);
|
|
fR = fa1;
|
|
|
|
|
|
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 3) ) {
|
|
iExitAxis=3;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 4 - Box Z-Axis
|
|
SET(vL,vA2);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 + dDOT(vA2,vE0);
|
|
fp2 = fp0 + dDOT(vA2,vE1);
|
|
fR = fa2;
|
|
|
|
|
|
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 4) ) {
|
|
iExitAxis=4;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
|
|
// Test Edges
|
|
// ************************************************
|
|
// Axis 5 - Box X-Axis cross Edge0
|
|
dCROSS(vL,=,vA0,vE0);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0;
|
|
fp2 = fp0 + dDOT(vA0,vN);
|
|
fR = fa1 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA1,vE0));
|
|
|
|
|
|
if( !_cldTestEdge( fp1, fp2, fR, fD, vL, 5) ) {
|
|
iExitAxis=5;
|
|
return false;
|
|
}
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 6 - Box X-Axis cross Edge1
|
|
dCROSS(vL,=,vA0,vE1);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 - dDOT(vA0,vN);
|
|
fp2 = fp0;
|
|
fR = fa1 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA1,vE1));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 6) ) {
|
|
iExitAxis=6;
|
|
return false;
|
|
}
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 7 - Box X-Axis cross Edge2
|
|
dCROSS(vL,=,vA0,vE2);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 - dDOT(vA0,vN);
|
|
fp2 = fp0 - dDOT(vA0,vN);
|
|
fR = fa1 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA1,vE2));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 7) ) {
|
|
iExitAxis=7;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 8 - Box Y-Axis cross Edge0
|
|
dCROSS(vL,=,vA1,vE0);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0;
|
|
fp2 = fp0 + dDOT(vA1,vN);
|
|
fR = fa0 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA0,vE0));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 8) ) {
|
|
iExitAxis=8;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 9 - Box Y-Axis cross Edge1
|
|
dCROSS(vL,=,vA1,vE1);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 - dDOT(vA1,vN);
|
|
fp2 = fp0;
|
|
fR = fa0 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA0,vE1));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 9) ) {
|
|
iExitAxis=9;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 10 - Box Y-Axis cross Edge2
|
|
dCROSS(vL,=,vA1,vE2);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 - dDOT(vA1,vN);
|
|
fp2 = fp0 - dDOT(vA1,vN);
|
|
fR = fa0 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA0,vE2));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 10) ) {
|
|
iExitAxis=10;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 11 - Box Z-Axis cross Edge0
|
|
dCROSS(vL,=,vA2,vE0);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0;
|
|
fp2 = fp0 + dDOT(vA2,vN);
|
|
fR = fa0 * dFabs(dDOT(vA1,vE0)) + fa1 * dFabs(dDOT(vA0,vE0));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 11) ) {
|
|
iExitAxis=11;
|
|
return false;
|
|
}
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 12 - Box Z-Axis cross Edge1
|
|
dCROSS(vL,=,vA2,vE1);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 - dDOT(vA2,vN);
|
|
fp2 = fp0;
|
|
fR = fa0 * dFabs(dDOT(vA1,vE1)) + fa1 * dFabs(dDOT(vA0,vE1));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 12) ) {
|
|
iExitAxis=12;
|
|
return false;
|
|
}
|
|
// ************************************************
|
|
|
|
// ************************************************
|
|
// Axis 13 - Box Z-Axis cross Edge2
|
|
dCROSS(vL,=,vA2,vE2);
|
|
fD = dDOT(vL,vN)/fNLen;
|
|
fp0 = dDOT(vL,vD);
|
|
fp1 = fp0 - dDOT(vA2,vN);
|
|
fp2 = fp0 - dDOT(vA2,vN);
|
|
fR = fa0 * dFabs(dDOT(vA1,vE2)) + fa1 * dFabs(dDOT(vA0,vE2));
|
|
|
|
|
|
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 13) ) {
|
|
iExitAxis=13;
|
|
return false;
|
|
}
|
|
|
|
// ************************************************
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// find two closest points on two lines
|
|
static bool _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1,
|
|
dVector3 vPoint2, dVector3 vLenVec2,
|
|
dReal &fvalue1, dReal &fvalue2)
|
|
{
|
|
// calulate denominator
|
|
dVector3 vp;
|
|
SUBTRACT(vPoint2,vPoint1,vp);
|
|
dReal fuaub = dDOT(vLenVec1,vLenVec2);
|
|
dReal fq1 = dDOT(vLenVec1,vp);
|
|
dReal fq2 = -dDOT(vLenVec2,vp);
|
|
dReal fd = 1.0f - fuaub * fuaub;
|
|
|
|
// if denominator is positive
|
|
if (fd > 0.0f) {
|
|
// calculate points of closest approach
|
|
fd = 1.0f/fd;
|
|
fvalue1 = (fq1 + fuaub*fq2)*fd;
|
|
fvalue2 = (fuaub*fq1 + fq2)*fd;
|
|
return true;
|
|
// otherwise
|
|
} else {
|
|
// lines are parallel
|
|
fvalue1 = 0.0f;
|
|
fvalue2 = 0.0f;
|
|
return false;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// clip and generate contacts
|
|
static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
|
|
dIASSERT( !(iFlags & CONTACTS_UNIMPORTANT) || ctContacts < (iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results
|
|
|
|
// if we have edge/edge intersection
|
|
if ( iBestAxis > 4 ) {
|
|
|
|
dVector3 vub,vPb,vPa;
|
|
|
|
SET(vPa,vHullBoxPos);
|
|
|
|
// calculate point on box edge
|
|
for( int i=0; i<3; i++) {
|
|
dVector3 vRotCol;
|
|
GETCOL(mHullBoxRot,i,vRotCol);
|
|
dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
|
|
|
|
vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
|
|
vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
|
|
vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
|
|
}
|
|
|
|
int iEdge = (iBestAxis-5)%3;
|
|
|
|
// decide which edge is on triangle
|
|
if ( iEdge == 0 ) {
|
|
SET(vPb,v0);
|
|
SET(vub,vE0);
|
|
} else if ( iEdge == 1) {
|
|
SET(vPb,v2);
|
|
SET(vub,vE1);
|
|
} else {
|
|
SET(vPb,v1);
|
|
SET(vub,vE2);
|
|
}
|
|
|
|
|
|
// setup direction parameter for face edge
|
|
dNormalize3(vub);
|
|
|
|
dReal fParam1, fParam2;
|
|
|
|
// setup direction parameter for box edge
|
|
dVector3 vua;
|
|
int col=(iBestAxis-5)/3;
|
|
GETCOL(mHullBoxRot,col,vua);
|
|
|
|
// find two closest points on both edges
|
|
_cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
|
|
vPa[0] += vua[0]*fParam1;
|
|
vPa[1] += vua[1]*fParam1;
|
|
vPa[2] += vua[2]*fParam1;
|
|
|
|
vPb[0] += vub[0]*fParam2;
|
|
vPb[1] += vub[1]*fParam2;
|
|
vPb[2] += vub[2]*fParam2;
|
|
|
|
// calculate collision point
|
|
dVector3 vPntTmp;
|
|
ADD(vPa,vPb,vPntTmp);
|
|
|
|
vPntTmp[0]*=0.5f;
|
|
vPntTmp[1]*=0.5f;
|
|
vPntTmp[2]*=0.5f;
|
|
|
|
// generate contact point between two closest points
|
|
#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
|
|
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
|
|
Contact->depth = fBestDepth;
|
|
SET(Contact->normal,vBestNormal);
|
|
SET(Contact->pos,vPntTmp);
|
|
Contact->g1 = Geom1;
|
|
Contact->g2 = Geom2;
|
|
ctContacts++;
|
|
#endif
|
|
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
|
|
vPntTmp, vBestNormal, fBestDepth, ctContacts);
|
|
|
|
|
|
// if triangle is the referent face then clip box to triangle face
|
|
} else if ( iBestAxis == 1 ) {
|
|
|
|
|
|
dVector3 vNormal2;
|
|
vNormal2[0]=-vBestNormal[0];
|
|
vNormal2[1]=-vBestNormal[1];
|
|
vNormal2[2]=-vBestNormal[2];
|
|
|
|
|
|
// vNr is normal in box frame, pointing from triangle to box
|
|
dMatrix3 mTransposed;
|
|
mTransposed[0*4+0]=mHullBoxRot[0*4+0];
|
|
mTransposed[0*4+1]=mHullBoxRot[1*4+0];
|
|
mTransposed[0*4+2]=mHullBoxRot[2*4+0];
|
|
|
|
mTransposed[1*4+0]=mHullBoxRot[0*4+1];
|
|
mTransposed[1*4+1]=mHullBoxRot[1*4+1];
|
|
mTransposed[1*4+2]=mHullBoxRot[2*4+1];
|
|
|
|
mTransposed[2*4+0]=mHullBoxRot[0*4+2];
|
|
mTransposed[2*4+1]=mHullBoxRot[1*4+2];
|
|
mTransposed[2*4+2]=mHullBoxRot[2*4+2];
|
|
|
|
dVector3 vNr;
|
|
vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2];
|
|
vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2];
|
|
vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2];
|
|
|
|
|
|
dVector3 vAbsNormal;
|
|
vAbsNormal[0] = dFabs( vNr[0] );
|
|
vAbsNormal[1] = dFabs( vNr[1] );
|
|
vAbsNormal[2] = dFabs( vNr[2] );
|
|
|
|
// get closest face from box
|
|
int iB0, iB1, iB2;
|
|
if (vAbsNormal[1] > vAbsNormal[0]) {
|
|
if (vAbsNormal[1] > vAbsNormal[2]) {
|
|
iB1 = 0; iB0 = 1; iB2 = 2;
|
|
} else {
|
|
iB1 = 0; iB2 = 1; iB0 = 2;
|
|
}
|
|
} else {
|
|
|
|
if (vAbsNormal[0] > vAbsNormal[2]) {
|
|
iB0 = 0; iB1 = 1; iB2 = 2;
|
|
} else {
|
|
iB1 = 0; iB2 = 1; iB0 = 2;
|
|
}
|
|
}
|
|
|
|
// Here find center of box face we are going to project
|
|
dVector3 vCenter;
|
|
dVector3 vRotCol;
|
|
GETCOL(mHullBoxRot,iB0,vRotCol);
|
|
|
|
if (vNr[iB0] > 0) {
|
|
vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
|
|
vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
|
|
vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
|
|
} else {
|
|
vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
|
|
vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
|
|
vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
|
|
}
|
|
|
|
// Here find 4 corner points of box
|
|
dVector3 avPoints[4];
|
|
|
|
dVector3 vRotCol2;
|
|
GETCOL(mHullBoxRot,iB1,vRotCol);
|
|
GETCOL(mHullBoxRot,iB2,vRotCol2);
|
|
|
|
for(int x=0;x<3;x++) {
|
|
avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
|
|
avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
|
|
avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
|
|
avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
|
|
}
|
|
|
|
|
|
// clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
|
|
dVector3 avTempArray1[9];
|
|
dVector3 avTempArray2[9];
|
|
dVector4 plPlane;
|
|
|
|
int iTempCnt1=0;
|
|
int iTempCnt2=0;
|
|
|
|
// zeroify vectors - necessary?
|
|
for(int i=0; i<9; i++) {
|
|
avTempArray1[i][0]=0;
|
|
avTempArray1[i][1]=0;
|
|
avTempArray1[i][2]=0;
|
|
|
|
avTempArray2[i][0]=0;
|
|
avTempArray2[i][1]=0;
|
|
avTempArray2[i][2]=0;
|
|
}
|
|
|
|
|
|
// Normal plane
|
|
dVector3 vTemp;
|
|
vTemp[0]=-vN[0];
|
|
vTemp[1]=-vN[1];
|
|
vTemp[2]=-vN[2];
|
|
dNormalize3(vTemp);
|
|
CONSTRUCTPLANE(plPlane,vTemp,0);
|
|
|
|
_cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane );
|
|
|
|
|
|
// Plane p0
|
|
dVector3 vTemp2;
|
|
SUBTRACT(v1,v0,vTemp2);
|
|
dCROSS(vTemp,=,vN,vTemp2);
|
|
dNormalize3(vTemp);
|
|
CONSTRUCTPLANE(plPlane,vTemp,0);
|
|
|
|
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
|
|
|
|
|
|
// Plane p1
|
|
SUBTRACT(v2,v1,vTemp2);
|
|
dCROSS(vTemp,=,vN,vTemp2);
|
|
dNormalize3(vTemp);
|
|
SUBTRACT(v0,v2,vTemp2);
|
|
CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));
|
|
|
|
_cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
|
|
|
|
|
|
// Plane p2
|
|
SUBTRACT(v0,v2,vTemp2);
|
|
dCROSS(vTemp,=,vN,vTemp2);
|
|
dNormalize3(vTemp);
|
|
CONSTRUCTPLANE(plPlane,vTemp,0);
|
|
|
|
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
|
|
|
|
|
|
// END of clipping polygons
|
|
|
|
|
|
|
|
// for each generated contact point
|
|
for ( int i=0; i<iTempCnt2; i++ ) {
|
|
// calculate depth
|
|
dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]);
|
|
|
|
// clamp depth to zero
|
|
if (fTempDepth > 0) {
|
|
fTempDepth = 0;
|
|
}
|
|
|
|
dVector3 vPntTmp;
|
|
ADD(avTempArray2[i],v0,vPntTmp);
|
|
|
|
#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
|
|
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
|
|
|
|
Contact->depth = -fTempDepth;
|
|
SET(Contact->normal,vBestNormal);
|
|
SET(Contact->pos,vPntTmp);
|
|
Contact->g1 = Geom1;
|
|
Contact->g2 = Geom2;
|
|
ctContacts++;
|
|
#endif
|
|
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
|
|
vPntTmp, vBestNormal, -fTempDepth, ctContacts);
|
|
|
|
if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
//dAASSERT(ctContacts>0);
|
|
|
|
// if box face is the referent face, then clip triangle on box face
|
|
} else { // 2 <= if iBestAxis <= 4
|
|
|
|
// get normal of box face
|
|
dVector3 vNormal2;
|
|
SET(vNormal2,vBestNormal);
|
|
|
|
// get indices of box axes in correct order
|
|
int iA0,iA1,iA2;
|
|
iA0 = iBestAxis-2;
|
|
if ( iA0 == 0 ) {
|
|
iA1 = 1; iA2 = 2;
|
|
} else if ( iA0 == 1 ) {
|
|
iA1 = 0; iA2 = 2;
|
|
} else {
|
|
iA1 = 0; iA2 = 1;
|
|
}
|
|
|
|
dVector3 avPoints[3];
|
|
// calculate triangle vertices in box frame
|
|
SUBTRACT(v0,vHullBoxPos,avPoints[0]);
|
|
SUBTRACT(v1,vHullBoxPos,avPoints[1]);
|
|
SUBTRACT(v2,vHullBoxPos,avPoints[2]);
|
|
|
|
// CLIP Polygons
|
|
// define temp data for clipping
|
|
dVector3 avTempArray1[9];
|
|
dVector3 avTempArray2[9];
|
|
|
|
int iTempCnt1, iTempCnt2;
|
|
|
|
// zeroify vectors - necessary?
|
|
for(int i=0; i<9; i++) {
|
|
avTempArray1[i][0]=0;
|
|
avTempArray1[i][1]=0;
|
|
avTempArray1[i][2]=0;
|
|
|
|
avTempArray2[i][0]=0;
|
|
avTempArray2[i][1]=0;
|
|
avTempArray2[i][2]=0;
|
|
}
|
|
|
|
// clip triangle with 5 box planes (1 face plane, 4 edge planes)
|
|
|
|
dVector4 plPlane;
|
|
|
|
// Normal plane
|
|
dVector3 vTemp;
|
|
vTemp[0]=-vNormal2[0];
|
|
vTemp[1]=-vNormal2[1];
|
|
vTemp[2]=-vNormal2[2];
|
|
CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA0]);
|
|
|
|
_cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane );
|
|
|
|
|
|
// Plane p0
|
|
GETCOL(mHullBoxRot,iA1,vTemp);
|
|
CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
|
|
|
|
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
|
|
|
|
|
|
// Plane p1
|
|
GETCOL(mHullBoxRot,iA1,vTemp);
|
|
vTemp[0]=-vTemp[0];
|
|
vTemp[1]=-vTemp[1];
|
|
vTemp[2]=-vTemp[2];
|
|
CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
|
|
|
|
_cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
|
|
|
|
|
|
// Plane p2
|
|
GETCOL(mHullBoxRot,iA2,vTemp);
|
|
CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
|
|
|
|
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
|
|
|
|
|
|
// Plane p3
|
|
GETCOL(mHullBoxRot,iA2,vTemp);
|
|
vTemp[0]=-vTemp[0];
|
|
vTemp[1]=-vTemp[1];
|
|
vTemp[2]=-vTemp[2];
|
|
CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
|
|
|
|
_cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
|
|
|
|
|
|
// for each generated contact point
|
|
for ( int i=0; i<iTempCnt1; i++ ) {
|
|
// calculate depth
|
|
dReal fTempDepth = dDOT(vNormal2,avTempArray1[i])-vBoxHalfSize[iA0];
|
|
|
|
// clamp depth to zero
|
|
if (fTempDepth > 0) {
|
|
fTempDepth = 0;
|
|
}
|
|
|
|
// generate contact data
|
|
dVector3 vPntTmp;
|
|
ADD(avTempArray1[i],vHullBoxPos,vPntTmp);
|
|
|
|
#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
|
|
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
|
|
|
|
Contact->depth = -fTempDepth;
|
|
SET(Contact->normal,vBestNormal);
|
|
SET(Contact->pos,vPntTmp);
|
|
Contact->g1 = Geom1;
|
|
Contact->g2 = Geom2;
|
|
ctContacts++;
|
|
#endif
|
|
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
|
|
vPntTmp, vBestNormal, -fTempDepth, ctContacts);
|
|
|
|
if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
//dAASSERT(ctContacts>0);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// test one mesh triangle on intersection with given box
|
|
static void _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)//, void *pvUser)
|
|
{
|
|
// do intersection test and find best separating axis
|
|
if(!_cldTestSeparatingAxes(v0, v1, v2) ) {
|
|
// if not found do nothing
|
|
return;
|
|
}
|
|
|
|
// if best separation axis is not found
|
|
if ( iBestAxis == 0 ) {
|
|
// this should not happen (we should already exit in that case)
|
|
//dMessage (0, "best separation axis not found");
|
|
// do nothing
|
|
return;
|
|
}
|
|
|
|
_cldClipping(v0, v1, v2);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// OPCODE version of box to mesh collider
|
|
#if dTRIMESH_OPCODE
|
|
int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
|
|
dIASSERT (Stride >= (int)sizeof(dContactGeom));
|
|
dIASSERT (g1->type == dTriMeshClass);
|
|
dIASSERT (BoxGeom->type == dBoxClass);
|
|
dIASSERT ((Flags & NUMC_MASK) >= 1);
|
|
|
|
|
|
dxTriMesh* TriMesh = (dxTriMesh*)g1;
|
|
|
|
|
|
// get source hull position, orientation and half size
|
|
const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
|
|
const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
|
|
|
|
// to global
|
|
SETM(mHullBoxRot,mRotBox);
|
|
SET(vHullBoxPos,vPosBox);
|
|
|
|
dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
|
|
vBoxHalfSize[0] *= 0.5f;
|
|
vBoxHalfSize[1] *= 0.5f;
|
|
vBoxHalfSize[2] *= 0.5f;
|
|
|
|
|
|
|
|
// get destination hull position and orientation
|
|
const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
|
|
const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
|
|
|
|
// to global
|
|
SET(vHullDstPos,vPosMesh);
|
|
|
|
|
|
|
|
// global info for contact creation
|
|
ctContacts = 0;
|
|
iStride=Stride;
|
|
iFlags=Flags;
|
|
ContactGeoms=Contacts;
|
|
Geom1=TriMesh;
|
|
Geom2=BoxGeom;
|
|
|
|
|
|
|
|
// reset stuff
|
|
fBestDepth = MAXVALUE;
|
|
vBestNormal[0]=0;
|
|
vBestNormal[1]=0;
|
|
vBestNormal[2]=0;
|
|
|
|
OBBCollider& Collider = TriMesh->_OBBCollider;
|
|
|
|
|
|
|
|
|
|
// Make OBB
|
|
OBB Box;
|
|
Box.mCenter.x = vPosBox[0];
|
|
Box.mCenter.y = vPosBox[1];
|
|
Box.mCenter.z = vPosBox[2];
|
|
|
|
// It is a potential issue to explicitly cast to float
|
|
// if custom width floating point type is introduced in OPCODE.
|
|
// It is necessary to make a typedef and cast to it
|
|
// (e.g. typedef float opc_float;)
|
|
// However I'm not sure in what header it should be added.
|
|
|
|
Box.mExtents.x = /*(float)*/vBoxHalfSize[0];
|
|
Box.mExtents.y = /*(float)*/vBoxHalfSize[1];
|
|
Box.mExtents.z = /*(float)*/vBoxHalfSize[2];
|
|
|
|
Box.mRot.m[0][0] = /*(float)*/mRotBox[0];
|
|
Box.mRot.m[1][0] = /*(float)*/mRotBox[1];
|
|
Box.mRot.m[2][0] = /*(float)*/mRotBox[2];
|
|
|
|
Box.mRot.m[0][1] = /*(float)*/mRotBox[4];
|
|
Box.mRot.m[1][1] = /*(float)*/mRotBox[5];
|
|
Box.mRot.m[2][1] = /*(float)*/mRotBox[6];
|
|
|
|
Box.mRot.m[0][2] = /*(float)*/mRotBox[8];
|
|
Box.mRot.m[1][2] = /*(float)*/mRotBox[9];
|
|
Box.mRot.m[2][2] = /*(float)*/mRotBox[10];
|
|
|
|
Matrix4x4 amatrix;
|
|
Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix);
|
|
|
|
Matrix4x4 InvBoxMatrix;
|
|
InvertPRMatrix(InvBoxMatrix, BoxMatrix);
|
|
|
|
// TC results
|
|
if (TriMesh->doBoxTC) {
|
|
dxTriMesh::BoxTC* BoxTC = 0;
|
|
for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
|
|
if (TriMesh->BoxTCCache[i].Geom == BoxGeom){
|
|
BoxTC = &TriMesh->BoxTCCache[i];
|
|
break;
|
|
}
|
|
}
|
|
if (!BoxTC){
|
|
TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
|
|
|
|
BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
|
|
BoxTC->Geom = BoxGeom;
|
|
BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0
|
|
}
|
|
|
|
// Intersect
|
|
Collider.SetTemporalCoherence(true);
|
|
Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix));
|
|
}
|
|
else {
|
|
Collider.SetTemporalCoherence(false);
|
|
Collider.Collide(dxTriMesh::defaultBoxCache, Box, TriMesh->Data->BVTree, null,
|
|
&MakeMatrix(vPosMesh, mRotMesh, amatrix));
|
|
}
|
|
|
|
if (! Collider.GetContactStatus()) {
|
|
// no collision occurred
|
|
return 0;
|
|
}
|
|
|
|
// Retrieve data
|
|
int TriCount = Collider.GetNbTouchedPrimitives();
|
|
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
|
|
|
|
if (TriCount != 0){
|
|
if (TriMesh->ArrayCallback != null){
|
|
TriMesh->ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount);
|
|
}
|
|
|
|
int ctContacts0 = 0;
|
|
|
|
// loop through all intersecting triangles
|
|
for (int i = 0; i < TriCount; i++){
|
|
|
|
|
|
const int Triint = Triangles[i];
|
|
if (!Callback(TriMesh, BoxGeom, Triint)) continue;
|
|
|
|
|
|
dVector3 dv[3];
|
|
FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv);
|
|
|
|
|
|
// test this triangle
|
|
_cldTestOneTriangle(dv[0],dv[1],dv[2]);
|
|
|
|
// fill-in tri index for generated contacts
|
|
for (; ctContacts0<ctContacts; ctContacts0++)
|
|
SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = Triint;
|
|
|
|
/*
|
|
NOTE by Oleh_Derevenko:
|
|
The function continues checking triangles after maximal number
|
|
of contacts is reached because it selects maximal penetration depths.
|
|
See also comments in GenerateContact()
|
|
*/
|
|
// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
|
|
if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
return ctContacts;
|
|
}
|
|
#endif
|
|
|
|
// GIMPACT version of box to mesh collider
|
|
#if dTRIMESH_GIMPACT
|
|
int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride)
|
|
{
|
|
dIASSERT (Stride >= (int)sizeof(dContactGeom));
|
|
dIASSERT (g1->type == dTriMeshClass);
|
|
dIASSERT (BoxGeom->type == dBoxClass);
|
|
dIASSERT ((Flags & NUMC_MASK) >= 1);
|
|
|
|
|
|
dxTriMesh* TriMesh = (dxTriMesh*)g1;
|
|
|
|
|
|
// get source hull position, orientation and half size
|
|
const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
|
|
const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
|
|
|
|
// to global
|
|
SETM(mHullBoxRot,mRotBox);
|
|
SET(vHullBoxPos,vPosBox);
|
|
|
|
dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
|
|
vBoxHalfSize[0] *= 0.5f;
|
|
vBoxHalfSize[1] *= 0.5f;
|
|
vBoxHalfSize[2] *= 0.5f;
|
|
|
|
// get destination hull position and orientation
|
|
/*const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
|
|
const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
|
|
|
|
// to global
|
|
SET(vHullDstPos,vPosMesh);*/
|
|
|
|
// global info for contact creation
|
|
ctContacts = 0;
|
|
iStride=Stride;
|
|
iFlags=Flags;
|
|
ContactGeoms=Contacts;
|
|
Geom1=TriMesh;
|
|
Geom2=BoxGeom;
|
|
|
|
|
|
// reset stuff
|
|
fBestDepth = MAXVALUE;
|
|
vBestNormal[0]=0;
|
|
vBestNormal[1]=0;
|
|
vBestNormal[2]=0;
|
|
|
|
|
|
//*****at first , collide box aabb******//
|
|
|
|
GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh;
|
|
aabb3f test_aabb;
|
|
|
|
test_aabb.minX = BoxGeom->aabb[0];
|
|
test_aabb.maxX = BoxGeom->aabb[1];
|
|
test_aabb.minY = BoxGeom->aabb[2];
|
|
test_aabb.maxY = BoxGeom->aabb[3];
|
|
test_aabb.minZ = BoxGeom->aabb[4];
|
|
test_aabb.maxZ = BoxGeom->aabb[5];
|
|
|
|
GDYNAMIC_ARRAY collision_result;
|
|
GIM_CREATE_BOXQUERY_LIST(collision_result);
|
|
|
|
gim_aabbset_box_collision(&test_aabb, &ptrimesh->m_aabbset , &collision_result);
|
|
|
|
if(collision_result.m_size==0)
|
|
{
|
|
GIM_DYNARRAY_DESTROY(collision_result);
|
|
return 0;
|
|
}
|
|
//*****Set globals for box collision******//
|
|
|
|
//collide triangles
|
|
|
|
GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
|
|
gim_trimesh_locks_work_data(ptrimesh);
|
|
|
|
int ctContacts0 = 0;
|
|
|
|
for(unsigned int i=0;i<collision_result.m_size;i++)
|
|
{
|
|
dVector3 dv[3];
|
|
|
|
int Triint = boxesresult[i];
|
|
gim_trimesh_get_triangle_vertices(ptrimesh, Triint,dv[0],dv[1],dv[2]);
|
|
// test this triangle
|
|
_cldTestOneTriangle(dv[0],dv[1],dv[2]);
|
|
|
|
// fill-in tri index for generated contacts
|
|
for (; ctContacts0<ctContacts; ctContacts0++)
|
|
SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = Triint;
|
|
|
|
/*
|
|
NOTE by Oleh_Derevenko:
|
|
The function continues checking triangles after maximal number
|
|
of contacts is reached because it selects maximal penetration depths.
|
|
See also comments in GenerateContact()
|
|
*/
|
|
// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
|
|
if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
|
|
break;
|
|
}
|
|
|
|
gim_trimesh_unlocks_work_data(ptrimesh);
|
|
GIM_DYNARRAY_DESTROY(collision_result);
|
|
|
|
return ctContacts;
|
|
}
|
|
#endif
|
|
|
|
|
|
// GenerateContact - Written by Jeff Smith (jeff@burri.to)
|
|
// Generate a "unique" contact. A unique contact has a unique
|
|
// position or normal. If the potential contact has the same
|
|
// position and normal as an existing contact, but a larger
|
|
// penetration depth, this new depth is used instead
|
|
//
|
|
static void
|
|
GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
|
|
dxGeom* in_g1, dxGeom* in_g2,
|
|
const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
|
|
int& OutTriCount)
|
|
{
|
|
/*
|
|
NOTE by Oleh_Derevenko:
|
|
This function is called after maximal number of contacts has already been
|
|
collected because it has a side effect of replacing penetration depth of
|
|
existing contact with larger penetration depth of another matching normal contact.
|
|
If this logic is not necessary any more, you can bail out on reach of contact
|
|
number maximum immediately in dCollideBTL(). You will also need to correct
|
|
conditional statements after invocations of GenerateContact() in _cldClipping().
|
|
*/
|
|
do
|
|
{
|
|
dContactGeom* Contact;
|
|
dVector3 diff;
|
|
|
|
if (!(in_Flags & CONTACTS_UNIMPORTANT))
|
|
{
|
|
bool duplicate = false;
|
|
for (int i=0; i<OutTriCount; i++)
|
|
{
|
|
Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
|
|
|
|
// same position?
|
|
for (int j=0; j<3; j++)
|
|
diff[j] = in_ContactPos[j] - Contact->pos[j];
|
|
if (dDOT(diff, diff) < dEpsilon)
|
|
{
|
|
// same normal?
|
|
if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon))
|
|
{
|
|
if (in_Depth > Contact->depth)
|
|
Contact->depth = in_Depth;
|
|
duplicate = true;
|
|
/*
|
|
NOTE by Oleh_Derevenko:
|
|
There may be a case when two normals are close to each other but not duplicate
|
|
while third normal is detected to be duplicate for both of them.
|
|
This is the only reason I can think of, there is no "break" statement.
|
|
Perhaps author considered it to be logical that the third normal would
|
|
replace the depth in both of initial contacts.
|
|
However, I consider it a questionable practice which should not
|
|
be applied without deep understanding of underlaying physics.
|
|
Even more, is this situation with close normal triplet acceptable at all?
|
|
Should not be two initial contacts reduced to one (replaced with the latter)?
|
|
If you know the answers for these questions, you may want to change this code.
|
|
See the same statement in GenerateContact() of collision_trimesh_trimesh.cpp
|
|
*/
|
|
}
|
|
}
|
|
}
|
|
if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dIASSERT(OutTriCount < (in_Flags & NUMC_MASK));
|
|
}
|
|
|
|
// Add a new contact
|
|
Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride);
|
|
|
|
Contact->pos[0] = in_ContactPos[0];
|
|
Contact->pos[1] = in_ContactPos[1];
|
|
Contact->pos[2] = in_ContactPos[2];
|
|
Contact->pos[3] = 0.0;
|
|
|
|
Contact->normal[0] = in_Normal[0];
|
|
Contact->normal[1] = in_Normal[1];
|
|
Contact->normal[2] = in_Normal[2];
|
|
Contact->normal[3] = 0.0;
|
|
|
|
Contact->depth = in_Depth;
|
|
|
|
Contact->g1 = in_g1;
|
|
Contact->g2 = in_g2;
|
|
|
|
OutTriCount++;
|
|
}
|
|
while (false);
|
|
}
|
|
|
|
#endif // dTRIMESH_ENABLED
|