Fast User-mesh collision

From Newton Wiki
Revision as of 08:02, 10 June 2019 by WikiSysop (talk | contribs) (1 revision imported)
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

Template:Languages
Newton has efficient built-in collision tree support. My tests have shown, however, that speed of collision detection between moveable body and static mesh could be greatly improved by replacing collision tree with user-defined mesh.

No going into details, it turns out that colliison tree takes more polygons into account than needed - it causes more contacts being generated. Of course these unnecessary contacts are not used for simulation, but unnecesary calculations tak significant amount of time. [DETAILS:): Collision tree seems to not test if triangle really intersects convex hull bounding box(and only test AABB). It means that in extremal cases, whel you have dozen diagonal polygons nearby player and only is touching player ellipsoid, all will be asked to calculate contacts]

Here's a class that I wrote to replace collision tree with user-defined mesh. It takes collision tree, enumerate it's polygons along with their user-id and create 3d spatial grid. Note that I'm still using original collision tree for raycasting. For reasonalble collision tree size(in my case it's ~200kb), waste of memory could be justified with high performance of Newton's builtin raycasting. Of course specialized raycasting could be more efficient, but it wasn't bottleneck in my game to rewrite it on my own.

The most important line is 'triBoxOverlap' which I searched somewhere on the net - it's public domain(search for tribox3.h). Without this line my user-mesh code is comparably fast to collision tree. This single line gives a significant speed boost.


// ----------------------------------
// user-mesh.h
// ----------------------------------

class CUserMesh
{
     // ----------------------------
     // for collision tree enumeration
     // ----------------------------
     static std::vector<D3DXVECTOR3> g_verts;
     static std::vector<int> g_faceID;
     static void _cdecl enumBodyFaces(const NewtonBody* body, int vertexCount, const float *v, int faceId);
     static void _cdecl enumBody( const NewtonBody *body );
     // ----------------------------
     
     std::vector<D3DXVECTOR3> m_intesectFaces;
     std::vector<int> m_intesectAttr;
     
     int m_intesectIndexTable[ 3 * 2000];
     int m_intesectVertNum[ 2000 ];
     
     // ----------------------------
     
     struct SFace
     {
             float v[3][3];		// vertices
             D3DXVECTOR3 mins, maxs;  // bounding box
             int faceID;		
             int m_num; // index of this face on the list
     };
     std::vector<SFace> m_faces; 
     
     // ----------------------------
     
     struct SFaceVector
     {
             SFace **m_faces;
             int m_faceNum;
     };
     SFaceVector ***m_cells;  // 3D grid 
     int m_sx, m_sy, m_sz;  // 3D grid size
     // ----------------------------
     
     D3DXVECTOR3 m_mins, m_maxs; // bounding box of all faces
     
     NewtonCollision *m_col; // user-mesh collision
     const NewtonCollision *m_oldTreeCol; // tree-collision
     NewtonWorld *m_world;
     
     // ----------------------------
     
     void collideCallback(NewtonUserMeshCollisionCollideDesc* desc);
     static void _cdecl static_collideCallback(NewtonUserMeshCollisionCollideDesc* collideDescData);
     static float _cdecl static_rayHitCallback(NewtonUserMeshCollisionRayHitDesc* lineDescData);
     
     
public:
     CUserMesh( const NewtonBody *a_nwtBody, const NewtonCollision *a_oldTreeCol );
     ~CUserMesh();
     
     NewtonCollision *getUserMesh(void) {return m_col;}
};


// ----------------------------------
// user-mesh.cpp
// ----------------------------------

//---------------------------------------------------------------------------
// Do enumeracji Collision Tree
//---------------------------------------------------------------------------

std::vector<D3DXVECTOR3> CUserMesh::g_verts;
std::vector<int> CUserMesh::g_faceID;

void _cdecl CUserMesh::enumBodyFaces(const NewtonBody* body, int vertexCount, const float *v, int faceId)
{
        if( vertexCount != 3 )
                throw "CUserMesh::userMeshCallb:  vertexCount != 3!";

        g_verts.push_back( D3DXVECTOR3(v[0],v[1],v[2]) );
        g_verts.push_back( D3DXVECTOR3(v[3],v[4],v[5]) );
        g_verts.push_back( D3DXVECTOR3(v[6],v[7],v[8]) );
        g_faceID.push_back( faceId );

//  Log_Printf( format("%d ", faceId ) );
}

//---------------------------------------------------------------------------

void _cdecl CUserMesh::enumBody( const NewtonBody *body )
{
//  Log_Printf( format("**************** faceIDs [ \n" ) );
                NewtonBodyForEachPolygonDo( body, enumBodyFaces );
//  Log_Printf( format("]************************* \n " ) );
}

//---------------------------------------------------------------------------
// CUserMesh
//---------------------------------------------------------------------------


#define CELL_SIZE_X  20.0f //7.5f
#define CELL_SIZE_Y  20.0f //7.5f
#define CELL_SIZE_Z  20.0f //7.5f


void CUserMesh::collideCallback(NewtonUserMeshCollisionCollideDesc* desc)
{   
        // ze wstepnych obserwacji wynika ze to jest wywolywane dla kazdego obiektu
        // czyli 4 autka => 4*5 wywolan
        static D3DXVECTOR3 amin, amax;
        amin.x = desc->m_boxP0[0]-0.01f;
        amin.y = desc->m_boxP0[1]-0.01f;
        amin.z = desc->m_boxP0[2]-0.01f;

        amax.x = desc->m_boxP1[0]+0.01f;
        amax.y = desc->m_boxP1[1]+0.01f;
        amax.z = desc->m_boxP1[2]+0.01f;

        // do testow z fejsem
        static D3DXVECTOR3 boxCenter, boxHalfSize;
        boxCenter = (amin + amax) * 0.5f;
        boxHalfSize = amax - boxCenter;
        /////


        m_intesectFaces.resize(0); 
        m_intesectAttr.resize(0);

        desc->m_faceCount = 0;

        int start_cx = (int)((amin.x - m_mins.x) / CELL_SIZE_X );
        int start_cy = (int)((amin.y - m_mins.y) / CELL_SIZE_Y );
        int start_cz = (int)((amin.z - m_mins.z) / CELL_SIZE_Z );

        int end_cx = (int)((amax.x - m_mins.x) / CELL_SIZE_X );
        int end_cy = (int)((amax.y - m_mins.y) / CELL_SIZE_Y );
        int end_cz = (int)((amax.z - m_mins.z) / CELL_SIZE_Z );

        static int usedFaceCache[100];
        int usedFaceCacheNum = 0;

        for( int x=start_cx; x<=end_cx; x++)
                for( int y=start_cy; y<=end_cy; y++)
                        for( int z=start_cz; z<=end_cz; z++)
                        {
                                if( x<0 || x>=m_sx ||
                                        y<0 || y>=m_sy ||
                                        z<0 || z>=m_sz )
                                {
                                        continue;
                                }

                                for( int i=0; i<m_cells[x][y][z].m_faceNum; i++ )
                                {
                                        const SFace &face = *m_cells[x][y][z].m_faces[i];

                                        // badamy czy juz tescia nie dodawalismy
                                        int j;
                                        for( j=0; j<usedFaceCacheNum; j++ )
                                                if( usedFaceCache[j] == face.m_num )
                                                        break;

                                        if( j==usedFaceCacheNum && is_box_box_aligned( face.mins, face.maxs, amin, amax ) )
                                        {                               
                                                if( triBoxOverlap( boxCenter, boxHalfSize, face.v ) == 1 )
                                                {
                                                        usedFaceCache[ usedFaceCacheNum++ ] = face.m_num;

                                                        m_intesectFaces.push_back( face.v[0] );
                                                        m_intesectFaces.push_back( face.v[1] );
                                                        m_intesectFaces.push_back( face.v[2] );

                                                        m_intesectAttr.push_back( face.faceID );

                                                        desc->m_faceCount++;
                                                }
                                        }
                                }
                        }

        // ---------------------------
        desc->m_vertex = (float*)&m_intesectFaces[0];
        desc->m_vertexStrideInBytes = 12;

        desc->m_userAttribute = (int*)&m_intesectAttr[0];
        desc->m_faceIndexCount = m_intesectVertNum;
        desc->m_faceVertexIndex = m_intesectIndexTable;                     
}

//---------------------------------------------------------------------------

void _cdecl CUserMesh::static_collideCallback(NewtonUserMeshCollisionCollideDesc* collideDescData)
{
        ((CUserMesh*)(collideDescData->m_userData))->collideCallback(collideDescData);
}

//---------------------------------------------------------------------------

float _cdecl CUserMesh::static_rayHitCallback(NewtonUserMeshCollisionRayHitDesc* lineDescData)
{
        return NewtonCollisionRayCast( ((CUserMesh*)(lineDescData->m_userData))->m_oldTreeCol, lineDescData->m_p0, lineDescData->m_p1, lineDescData->m_normalOut, &lineDescData->m_userIdOut );
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------

CUserMesh::CUserMesh( const NewtonBody *a_nwtBody, const NewtonCollision *a_oldTreeCol )
{
        //--------------------------------------
        // Wyciagamy fejsy z przekazanego NewtonBody
        //--------------------------------------
        g_verts.clear();
        g_verts.reserve( 30000 ); // powinno starczyc
        g_faceID.reserve( 10000 ); // powinno starczyc
        enumBody( a_nwtBody );

        //--------------------------------------
        // Tworzymy liste fejsow
        //--------------------------------------
        m_mins = m_maxs = g_verts[0];
        m_faces.resize( g_verts.size()/3 );
        for( unsigned int i=0; i<g_verts.size()/3; i++)
        {
                D3DXVec3Minimize( &m_mins, &m_mins, &g_verts[3*i+0] );
                D3DXVec3Minimize( &m_mins, &m_mins, &g_verts[3*i+1] );
                D3DXVec3Minimize( &m_mins, &m_mins, &g_verts[3*i+2] );

                D3DXVec3Maximize( &m_maxs, &m_maxs, &g_verts[3*i+0] );
                D3DXVec3Maximize( &m_maxs, &m_maxs, &g_verts[3*i+1] );
                D3DXVec3Maximize( &m_maxs, &m_maxs, &g_verts[3*i+2] );

                m_faces[i].v[0][0] = g_verts[3*i+0].x;
                m_faces[i].v[0][1] = g_verts[3*i+0].y;
                m_faces[i].v[0][2] = g_verts[3*i+0].z;

                m_faces[i].v[1][0] = g_verts[3*i+1].x;
                m_faces[i].v[1][1] = g_verts[3*i+1].y;
                m_faces[i].v[1][2] = g_verts[3*i+1].z;

                m_faces[i].v[2][0] = g_verts[3*i+2].x;
                m_faces[i].v[2][1] = g_verts[3*i+2].y;
                m_faces[i].v[2][2] = g_verts[3*i+2].z;

                m_faces[i].faceID = g_faceID[i];

                m_faces[i].mins = m_faces[i].maxs = g_verts[3*i+0];
                D3DXVec3Minimize( &m_faces[i].mins, &m_faces[i].mins, &g_verts[3*i+1] );
                D3DXVec3Maximize( &m_faces[i].maxs, &m_faces[i].maxs, &g_verts[3*i+1] );
                D3DXVec3Minimize( &m_faces[i].mins, &m_faces[i].mins, &g_verts[3*i+2] );
                D3DXVec3Maximize( &m_faces[i].maxs, &m_faces[i].maxs, &g_verts[3*i+2] );

                m_faces[i].m_num = i; // do identyfikacji fejsow w callbacku
        }

        //--------------------------------------
        // Tworzymy grida 3D
        //--------------------------------------
        m_sx = 1 + (int)((m_maxs.x - m_mins.x) / CELL_SIZE_X );
        m_sy = 1 + (int)((m_maxs.y - m_mins.y) / CELL_SIZE_Y );
        m_sz = 1 + (int)((m_maxs.z - m_mins.z) / CELL_SIZE_Z );

        m_cells = new SFaceVector**[m_sx];
        for(int x=0; x<m_sx; x++)
        {
                m_cells[x] = new SFaceVector*[m_sy];
                for(int y=0; y<m_sy; y++)
                {
                        m_cells[x][y] = new SFaceVector[m_sz];
                        for(int z=0; z<m_sz; z++)
                        {
                                D3DXVECTOR3 cmins( m_mins.x+x*CELL_SIZE_X, m_mins.y+y*CELL_SIZE_Y , m_mins.z+z*CELL_SIZE_Z  );
                                D3DXVECTOR3 cmaxs( cmins.x+CELL_SIZE_X, cmins.y+CELL_SIZE_Y , cmins.z+CELL_SIZE_Z  );

                                int numFaces = 0;
                                static int faceCache[2048];
                                for(unsigned int i=0; i<(unsigned int)m_faces.size(); i++)
                                        if( is_box_box_aligned( cmins, cmaxs, m_faces[i].mins, 
                                                                m_faces[i].maxs ) )
                                        {
                                                faceCache[numFaces] = i;
                                                numFaces++;

                                                if( numFaces == 2048 )
                                                        throw "UM: numFaces == 2048!";
                                        }

                                m_cells[x][y][z].m_faceNum = numFaces;
                                if( numFaces > 0)
                                {
                                        m_cells[x][y][z].m_faces = new SFace*[numFaces];
                                        for(int i=0; i<numFaces; i++)
                                                m_cells[x][y][z].m_faces[i] = &m_faces[ faceCache[i] ];
                                }
                                else
                                        m_cells[x][y][z].m_faces = NULL;
                        }
                }
        }

        // wypelniamy tez struktury dla Newtona zeby nie tracic czasu w callbacku
        for( unsigned int i=0; i<2000; i++)
        {
                m_intesectIndexTable[ 3*i+0 ] = 3*i+0;
                m_intesectIndexTable[ 3*i+1 ] = 3*i+1;
                m_intesectIndexTable[ 3*i+2 ] = 3*i+2;
                m_intesectVertNum[ i ] = 3;
        }
        /////////////////////////////////////
        m_oldTreeCol = a_oldTreeCol;
        m_world = NewtonBodyGetWorld(a_nwtBody);
        m_col = NewtonCreateUserMeshCollision( m_world, m_mins, m_maxs, this, static_collideCallback, static_rayHitCallback, NULL );
        NewtonBodySetCollision( a_nwtBody, m_col );
        NewtonReleaseCollision( m_world, m_col );

        // Stan wyjsciowy: sa dwa meshe do kolizji 
        //  m_col:  usermesh do kontaktow 
        //  m_oldTreeCol: collision tree do raycastingu
        // oba prymitywy maja reference count==1, z tym ze m_col zginie automatycznie razem z cialem
        //  a m_oldTreeCol musimy recznie w destruktorze zwolnic
}

//---------------------------------------------------------------------------

CUserMesh::~CUserMesh()
{
        for(int x=0; x<m_sx; x++)
        {
                for(int y=0; y<m_sy; y++)
                {
                        for(int z=0; z<m_sz; z++)
                                delete [] m_cells[x][y][z].m_faces;
                        delete [] m_cells[x][y];
                }
                delete [] m_cells[x];
        }
        delete [] m_cells;

        // m_col automatycznie z cialem zginie, a ten jest samopas
        NewtonReleaseCollision( m_world, m_oldTreeCol );
}

//---------------------------------------------------------------------------


/********************************************************/
/* AABB-triangle overlap test code                      */
/* by Tomas Möller                                      */
/* Function: int triBoxOverlap(float boxcenter[3],      */
/*          float boxhalfsize[3],float triverts[3][3]); */
/* History:                                             */
/*   2001-03-05: released the code in its first version */
/*                                                      */
/* Acknowledgement: Many thanks to Pierre Terdiman for  */
/* suggestions and discussions on how to optimize code. */
/* Thanks to David Hunt for finding a ">="-bug!         */
/********************************************************/
#include <math.h>
#include <stdio.h>

#define X 0
#define Y 1
#define Z 2

#define CROSS(dest,v1,v2) \
dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; 

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2) \
dest[0]=v1[0]-v2[0]; \
dest[1]=v1[1]-v2[1]; \
dest[2]=v1[2]-v2[2]; 

#define FINDMINMAX(x0,x1,x2,min,max) \
min = max = x0;   \
if(x1<min) min=x1;\
if(x1>max) max=x1;\
if(x2<min) min=x2;\
if(x2>max) max=x2;

int planeBoxOverlap(float normal[3],float d, float maxbox[3])
{
int q;
float vmin[3],vmax[3];
for(q=X;q<=Z;q++)
{
 if(normal[q]>0.0f)
 {
  vmin[q]=-maxbox[q];
  vmax[q]=maxbox[q];
 }
 else
 {
  vmin[q]=maxbox[q];
  vmax[q]=-maxbox[q];
 }
}
if(DOT(normal,vmin)+d>0.0f) return 0;
if(DOT(normal,vmax)+d>=0.0f) return 1;

return 0;
}


/*======================== X-tests ========================*/
#define AXISTEST_X01(a, b, fa, fb)			   \
p0 = a*v0[Y] - b*v0[Z];			       	   \
p2 = a*v2[Y] - b*v2[Z];			       	   \
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

#define AXISTEST_X2(a, b, fa, fb)			   \
p0 = a*v0[Y] - b*v0[Z];			           \
p1 = a*v1[Y] - b*v1[Z];			       	   \
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

/*======================== Y-tests ========================*/
#define AXISTEST_Y02(a, b, fa, fb)			   \
p0 = -a*v0[X] + b*v0[Z];		      	   \
p2 = -a*v2[X] + b*v2[Z];	       	       	   \
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

#define AXISTEST_Y1(a, b, fa, fb)			   \
p0 = -a*v0[X] + b*v0[Z];		      	   \
p1 = -a*v1[X] + b*v1[Z];	     	       	   \
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
if(min>rad || max<-rad) return 0;

/*======================== Z-tests ========================*/

#define AXISTEST_Z12(a, b, fa, fb)			   \
p1 = a*v1[X] - b*v1[Y];			           \
p2 = a*v2[X] - b*v2[Y];			       	   \
if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
if(min>rad || max<-rad) return 0;

#define AXISTEST_Z0(a, b, fa, fb)			   \
p0 = a*v0[X] - b*v0[Y];				   \
p1 = a*v1[X] - b*v1[Y];			           \
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
if(min>rad || max<-rad) return 0;

int triBoxOverlap(float boxcenter[3],float boxhalfsize[3],float triverts[3][3])
{

/*    use separating axis theorem to test overlap between triangle and box */
/*    need to test for overlap in these directions: */
/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
/*       we do not even need to test these) */
/*    2) normal of the triangle */
/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
/*       this gives 3x3=9 more tests */
float v0[3],v1[3],v2[3];
float axis[3];
float min,max,d,p0,p1,p2,rad,fex,fey,fez;  
float normal[3],e0[3],e1[3],e2[3];

/* 1) first test overlap in the {x,y,z}-directions */
/*    find min, max of the triangle each direction, and test for overlap in */
/*    that direction -- this is equivalent to testing a minimal AABB around */
/*    the triangle against the AABB */
#if 1
/* This is the fastest branch on Sun */
/* move everything so that the boxcenter is in (0,0,0) */
SUB(v0,triverts[0],boxcenter);
SUB(v1,triverts[1],boxcenter);
SUB(v2,triverts[2],boxcenter);

/* test in X-direction */
FINDMINMAX(v0[X],v1[X],v2[X],min,max);
if(min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0;

/* test in Y-direction */
FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
if(min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0;

/* test in Z-direction */
FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
if(min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0;
#else
/*    another implementation */
/*    test in X */
v0[X]=triverts[0][X]-boxcenter[X];
v1[X]=triverts[1][X]-boxcenter[X];
v2[X]=triverts[2][X]-boxcenter[X];
FINDMINMAX(v0[X],v1[X],v2[X],min,max);
if(min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0;

/*    test in Y */
v0[Y]=triverts[0][Y]-boxcenter[Y];
v1[Y]=triverts[1][Y]-boxcenter[Y];
v2[Y]=triverts[2][Y]-boxcenter[Y];
FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
if(min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0;

/*    test in Z */
v0[Z]=triverts[0][Z]-boxcenter[Z];
v1[Z]=triverts[1][Z]-boxcenter[Z];
v2[Z]=triverts[2][Z]-boxcenter[Z];
FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
if(min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0;
#endif

/*    2) */
/*    test if the box intersects the plane of the triangle */
/*    compute plane equation of triangle: normal*x+d=0 */
SUB(e0,v1,v0);      /* tri edge 0 */
SUB(e1,v2,v1);      /* tri edge 1 */
CROSS(normal,e0,e1);
d=-DOT(normal,v0);  /* plane eq: normal.x+d=0 */

if(!planeBoxOverlap(normal,d,boxhalfsize)) return 0;

/*    compute the last triangle edge */
SUB(e2,v0,v2);

/*    3) */
fex = fabs(e0[X]);
fey = fabs(e0[Y]);
fez = fabs(e0[Z]);
AXISTEST_X01(e0[Z], e0[Y], fez, fey);
AXISTEST_Y02(e0[Z], e0[X], fez, fex);
AXISTEST_Z12(e0[Y], e0[X], fey, fex);

fex = fabs(e1[X]);
fey = fabs(e1[Y]);
fez = fabs(e1[Z]);
AXISTEST_X01(e1[Z], e1[Y], fez, fey);
AXISTEST_Y02(e1[Z], e1[X], fez, fex);
AXISTEST_Z0(e1[Y], e1[X], fey, fex);


fex = fabs(e2[X]);
fey = fabs(e2[Y]);
fez = fabs(e2[Z]);
AXISTEST_X2(e2[Z], e2[Y], fez, fey);
AXISTEST_Y1(e2[Z], e2[X], fez, fex);
AXISTEST_Z12(e2[Y], e2[X], fey, fex);

return 1;
}