[216] | 1 | /************************************************************************* |
---|
| 2 | * * |
---|
| 3 | * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * |
---|
| 4 | * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
---|
| 5 | * * |
---|
| 6 | * This library is free software; you can redistribute it and/or * |
---|
| 7 | * modify it under the terms of EITHER: * |
---|
| 8 | * (1) The GNU Lesser General Public License as published by the Free * |
---|
| 9 | * Software Foundation; either version 2.1 of the License, or (at * |
---|
| 10 | * your option) any later version. The text of the GNU Lesser * |
---|
| 11 | * General Public License is included with this library in the * |
---|
| 12 | * file LICENSE.TXT. * |
---|
| 13 | * (2) The BSD-style license that is included with this library in * |
---|
| 14 | * the file LICENSE-BSD.TXT. * |
---|
| 15 | * * |
---|
| 16 | * This library is distributed in the hope that it will be useful, * |
---|
| 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
---|
| 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
---|
| 19 | * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
---|
| 20 | * * |
---|
| 21 | *************************************************************************/ |
---|
| 22 | |
---|
| 23 | // TriMesh code by Erwin de Vries. |
---|
| 24 | |
---|
| 25 | #include <ode/collision.h> |
---|
| 26 | #include <ode/matrix.h> |
---|
| 27 | #include <ode/rotation.h> |
---|
| 28 | #include <ode/odemath.h> |
---|
| 29 | #include "collision_util.h" |
---|
| 30 | #define TRIMESH_INTERNAL |
---|
| 31 | #include "collision_trimesh_internal.h" |
---|
| 32 | |
---|
| 33 | #if dTRIMESH_ENABLED |
---|
| 34 | #if dTRIMESH_OPCODE |
---|
| 35 | |
---|
| 36 | // Trimesh data |
---|
| 37 | dxTriMeshData::dxTriMeshData() : UseFlags( NULL ) |
---|
| 38 | { |
---|
| 39 | #if !dTRIMESH_ENABLED |
---|
| 40 | dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work"); |
---|
| 41 | #endif |
---|
| 42 | } |
---|
| 43 | |
---|
| 44 | dxTriMeshData::~dxTriMeshData() |
---|
| 45 | { |
---|
| 46 | if ( UseFlags ) |
---|
| 47 | delete [] UseFlags; |
---|
| 48 | } |
---|
| 49 | |
---|
| 50 | void |
---|
| 51 | dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount, |
---|
| 52 | const void* Indices, int IndexCount, int TriStride, |
---|
| 53 | const void* in_Normals, |
---|
| 54 | bool Single) |
---|
| 55 | { |
---|
| 56 | #if dTRIMESH_ENABLED |
---|
| 57 | |
---|
| 58 | Mesh.SetNbTriangles(IndexCount / 3); |
---|
| 59 | Mesh.SetNbVertices(VertexCount); |
---|
| 60 | Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices); |
---|
| 61 | Mesh.SetStrides(TriStride, VertexStide); |
---|
| 62 | Mesh.Single = Single; |
---|
| 63 | |
---|
| 64 | // Build tree |
---|
| 65 | BuildSettings Settings; |
---|
| 66 | // recommended in Opcode User Manual |
---|
| 67 | //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER; |
---|
| 68 | // used in ODE, why? |
---|
| 69 | //Settings.mRules = SPLIT_BEST_AXIS; |
---|
| 70 | |
---|
| 71 | // best compromise? |
---|
| 72 | Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER; |
---|
| 73 | |
---|
| 74 | |
---|
| 75 | OPCODECREATE TreeBuilder; |
---|
| 76 | TreeBuilder.mIMesh = &Mesh; |
---|
| 77 | |
---|
| 78 | TreeBuilder.mSettings = Settings; |
---|
| 79 | TreeBuilder.mNoLeaf = true; |
---|
| 80 | TreeBuilder.mQuantized = false; |
---|
| 81 | |
---|
| 82 | TreeBuilder.mKeepOriginal = false; |
---|
| 83 | TreeBuilder.mCanRemap = false; |
---|
| 84 | |
---|
| 85 | |
---|
| 86 | |
---|
| 87 | BVTree.Build(TreeBuilder); |
---|
| 88 | |
---|
| 89 | // compute model space AABB |
---|
| 90 | dVector3 AABBMax, AABBMin; |
---|
| 91 | AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity; |
---|
| 92 | AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity; |
---|
| 93 | if( Single ) { |
---|
| 94 | const char* verts = (const char*)Vertices; |
---|
| 95 | for( int i = 0; i < VertexCount; ++i ) { |
---|
| 96 | const float* v = (const float*)verts; |
---|
| 97 | if( v[0] > AABBMax[0] ) AABBMax[0] = v[0]; |
---|
| 98 | if( v[1] > AABBMax[1] ) AABBMax[1] = v[1]; |
---|
| 99 | if( v[2] > AABBMax[2] ) AABBMax[2] = v[2]; |
---|
| 100 | if( v[0] < AABBMin[0] ) AABBMin[0] = v[0]; |
---|
| 101 | if( v[1] < AABBMin[1] ) AABBMin[1] = v[1]; |
---|
| 102 | if( v[2] < AABBMin[2] ) AABBMin[2] = v[2]; |
---|
| 103 | verts += VertexStide; |
---|
| 104 | } |
---|
| 105 | } else { |
---|
| 106 | const char* verts = (const char*)Vertices; |
---|
| 107 | for( int i = 0; i < VertexCount; ++i ) { |
---|
| 108 | const double* v = (const double*)verts; |
---|
| 109 | if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0]; |
---|
| 110 | if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1]; |
---|
| 111 | if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2]; |
---|
| 112 | if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0]; |
---|
| 113 | if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1]; |
---|
| 114 | if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2]; |
---|
| 115 | verts += VertexStide; |
---|
| 116 | } |
---|
| 117 | } |
---|
| 118 | AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5); |
---|
| 119 | AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5); |
---|
| 120 | AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5); |
---|
| 121 | AABBExtents[0] = AABBMax[0] - AABBCenter[0]; |
---|
| 122 | AABBExtents[1] = AABBMax[1] - AABBCenter[1]; |
---|
| 123 | AABBExtents[2] = AABBMax[2] - AABBCenter[2]; |
---|
| 124 | |
---|
| 125 | // user data (not used by OPCODE) |
---|
| 126 | Normals = (dReal *) in_Normals; |
---|
| 127 | |
---|
| 128 | UseFlags = 0; |
---|
| 129 | |
---|
| 130 | #endif // dTRIMESH_ENABLED |
---|
| 131 | } |
---|
| 132 | |
---|
| 133 | struct EdgeRecord |
---|
| 134 | { |
---|
| 135 | int VertIdx1; // Index into vertex array for this edges vertices |
---|
| 136 | int VertIdx2; |
---|
| 137 | int TriIdx; // Index into triangle array for triangle this edge belongs to |
---|
| 138 | |
---|
| 139 | uint8 EdgeFlags; |
---|
| 140 | uint8 Vert1Flags; |
---|
| 141 | uint8 Vert2Flags; |
---|
| 142 | bool Concave; |
---|
| 143 | }; |
---|
| 144 | |
---|
| 145 | // Edge comparison function for qsort |
---|
| 146 | static int EdgeCompare(const void* edge1, const void* edge2) |
---|
| 147 | { |
---|
| 148 | EdgeRecord* e1 = (EdgeRecord*)edge1; |
---|
| 149 | EdgeRecord* e2 = (EdgeRecord*)edge2; |
---|
| 150 | |
---|
| 151 | if (e1->VertIdx1 == e2->VertIdx1) |
---|
| 152 | return e1->VertIdx2 - e2->VertIdx2; |
---|
| 153 | else |
---|
| 154 | return e1->VertIdx1 - e2->VertIdx1; |
---|
| 155 | } |
---|
| 156 | |
---|
| 157 | void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const unsigned int* vertIdxs) |
---|
| 158 | { |
---|
| 159 | if (edgeIdx == 0) |
---|
| 160 | { |
---|
| 161 | edge->EdgeFlags = dxTriMeshData::kEdge0; |
---|
| 162 | edge->Vert1Flags = dxTriMeshData::kVert0; |
---|
| 163 | edge->Vert2Flags = dxTriMeshData::kVert1; |
---|
| 164 | edge->VertIdx1 = vertIdxs[0]; |
---|
| 165 | edge->VertIdx2 = vertIdxs[1]; |
---|
| 166 | } |
---|
| 167 | else if (edgeIdx == 1) |
---|
| 168 | { |
---|
| 169 | edge->EdgeFlags = dxTriMeshData::kEdge1; |
---|
| 170 | edge->Vert1Flags = dxTriMeshData::kVert1; |
---|
| 171 | edge->Vert2Flags = dxTriMeshData::kVert2; |
---|
| 172 | edge->VertIdx1 = vertIdxs[1]; |
---|
| 173 | edge->VertIdx2 = vertIdxs[2]; |
---|
| 174 | } |
---|
| 175 | else if (edgeIdx == 2) |
---|
| 176 | { |
---|
| 177 | edge->EdgeFlags = dxTriMeshData::kEdge2; |
---|
| 178 | edge->Vert1Flags = dxTriMeshData::kVert2; |
---|
| 179 | edge->Vert2Flags = dxTriMeshData::kVert0; |
---|
| 180 | edge->VertIdx1 = vertIdxs[2]; |
---|
| 181 | edge->VertIdx2 = vertIdxs[0]; |
---|
| 182 | } |
---|
| 183 | |
---|
| 184 | // Make sure vert index 1 is less than index 2 (for easier sorting) |
---|
| 185 | if (edge->VertIdx1 > edge->VertIdx2) |
---|
| 186 | { |
---|
| 187 | unsigned int tempIdx = edge->VertIdx1; |
---|
| 188 | edge->VertIdx1 = edge->VertIdx2; |
---|
| 189 | edge->VertIdx2 = tempIdx; |
---|
| 190 | |
---|
| 191 | uint8 tempFlags = edge->Vert1Flags; |
---|
| 192 | edge->Vert1Flags = edge->Vert2Flags; |
---|
| 193 | edge->Vert2Flags = tempFlags; |
---|
| 194 | } |
---|
| 195 | |
---|
| 196 | edge->TriIdx = triIdx; |
---|
| 197 | edge->Concave = false; |
---|
| 198 | } |
---|
| 199 | |
---|
| 200 | #if dTRIMESH_ENABLED |
---|
| 201 | |
---|
| 202 | // Get the vertex opposite this edge in the triangle |
---|
| 203 | inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[]) |
---|
| 204 | { |
---|
| 205 | if ((edge->Vert1Flags == dxTriMeshData::kVert0 && edge->Vert2Flags == dxTriMeshData::kVert1) || |
---|
| 206 | (edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert0)) |
---|
| 207 | { |
---|
| 208 | return *vertices[2]; |
---|
| 209 | } |
---|
| 210 | else if ((edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert2) || |
---|
| 211 | (edge->Vert1Flags == dxTriMeshData::kVert2 && edge->Vert2Flags == dxTriMeshData::kVert1)) |
---|
| 212 | { |
---|
| 213 | return *vertices[0]; |
---|
| 214 | } |
---|
| 215 | else |
---|
| 216 | return *vertices[1]; |
---|
| 217 | } |
---|
| 218 | |
---|
| 219 | #endif // dTRIMESH_ENABLED |
---|
| 220 | |
---|
| 221 | void dxTriMeshData::Preprocess() |
---|
| 222 | { |
---|
| 223 | |
---|
| 224 | #if dTRIMESH_ENABLED |
---|
| 225 | |
---|
| 226 | // If this mesh has already been preprocessed, exit |
---|
| 227 | if (UseFlags) |
---|
| 228 | return; |
---|
| 229 | |
---|
| 230 | udword numTris = Mesh.GetNbTriangles(); |
---|
| 231 | udword numEdges = numTris * 3; |
---|
| 232 | |
---|
| 233 | UseFlags = new uint8[numTris]; |
---|
| 234 | memset(UseFlags, 0, sizeof(uint8) * numTris); |
---|
| 235 | |
---|
| 236 | EdgeRecord* records = new EdgeRecord[numEdges]; |
---|
| 237 | |
---|
| 238 | // Make a list of every edge in the mesh |
---|
| 239 | const IndexedTriangle* tris = Mesh.GetTris(); |
---|
| 240 | for (unsigned int i = 0; i < numTris; i++) |
---|
| 241 | { |
---|
| 242 | SetupEdge(&records[i*3], 0, i, tris->mVRef); |
---|
| 243 | SetupEdge(&records[i*3+1], 1, i, tris->mVRef); |
---|
| 244 | SetupEdge(&records[i*3+2], 2, i, tris->mVRef); |
---|
| 245 | |
---|
| 246 | tris = (const IndexedTriangle*)(((uint8*)tris) + Mesh.GetTriStride()); |
---|
| 247 | } |
---|
| 248 | |
---|
| 249 | // Sort the edges, so the ones sharing the same verts are beside each other |
---|
| 250 | qsort(records, numEdges, sizeof(EdgeRecord), EdgeCompare); |
---|
| 251 | |
---|
| 252 | // Go through the sorted list of edges and flag all the edges and vertices that we need to use |
---|
| 253 | for (unsigned int i = 0; i < numEdges; i++) |
---|
| 254 | { |
---|
| 255 | EdgeRecord* rec1 = &records[i]; |
---|
| 256 | EdgeRecord* rec2 = 0; |
---|
| 257 | if (i < numEdges - 1) |
---|
| 258 | rec2 = &records[i+1]; |
---|
| 259 | |
---|
| 260 | if (rec2 && |
---|
| 261 | rec1->VertIdx1 == rec2->VertIdx1 && |
---|
| 262 | rec1->VertIdx2 == rec2->VertIdx2) |
---|
| 263 | { |
---|
| 264 | VertexPointers vp; |
---|
| 265 | Mesh.GetTriangle(vp, rec1->TriIdx); |
---|
| 266 | |
---|
| 267 | // Get the normal of the first triangle |
---|
| 268 | Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]); |
---|
| 269 | triNorm.Normalize(); |
---|
| 270 | |
---|
| 271 | // Get the vert opposite this edge in the first triangle |
---|
| 272 | Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex); |
---|
| 273 | |
---|
| 274 | // Get the vert opposite this edge in the second triangle |
---|
| 275 | Mesh.GetTriangle(vp, rec2->TriIdx); |
---|
| 276 | Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex); |
---|
| 277 | |
---|
| 278 | float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize()); |
---|
| 279 | |
---|
| 280 | // We let the dot threshold for concavity get slightly negative to allow for rounding errors |
---|
| 281 | static const float kConcaveThresh = -0.000001f; |
---|
| 282 | |
---|
| 283 | // This is a concave edge, leave it for the next pass |
---|
| 284 | if (dot >= kConcaveThresh) |
---|
| 285 | rec1->Concave = true; |
---|
| 286 | // If this is a convex edge, mark its vertices and edge as used |
---|
| 287 | else |
---|
| 288 | UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags; |
---|
| 289 | |
---|
| 290 | // Skip the second edge |
---|
| 291 | i++; |
---|
| 292 | } |
---|
| 293 | // This is a boundary edge |
---|
| 294 | else |
---|
| 295 | { |
---|
| 296 | UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags; |
---|
| 297 | } |
---|
| 298 | } |
---|
| 299 | |
---|
| 300 | // Go through the list once more, and take any edge we marked as concave and |
---|
| 301 | // clear it's vertices flags in any triangles they're used in |
---|
| 302 | for (unsigned int i = 0; i < numEdges; i++) |
---|
| 303 | { |
---|
| 304 | EdgeRecord& er = records[i]; |
---|
| 305 | |
---|
| 306 | if (er.Concave) |
---|
| 307 | { |
---|
| 308 | for (unsigned int j = 0; j < numEdges; j++) |
---|
| 309 | { |
---|
| 310 | EdgeRecord& curER = records[j]; |
---|
| 311 | |
---|
| 312 | if (curER.VertIdx1 == er.VertIdx1 || |
---|
| 313 | curER.VertIdx1 == er.VertIdx2) |
---|
| 314 | UseFlags[curER.TriIdx] &= ~curER.Vert1Flags; |
---|
| 315 | |
---|
| 316 | if (curER.VertIdx2 == er.VertIdx1 || |
---|
| 317 | curER.VertIdx2 == er.VertIdx2) |
---|
| 318 | UseFlags[curER.TriIdx] &= ~curER.Vert2Flags; |
---|
| 319 | } |
---|
| 320 | } |
---|
| 321 | } |
---|
| 322 | |
---|
| 323 | delete [] records; |
---|
| 324 | |
---|
| 325 | #endif // dTRIMESH_ENABLED |
---|
| 326 | |
---|
| 327 | } |
---|
| 328 | |
---|
| 329 | dTriMeshDataID dGeomTriMeshDataCreate(){ |
---|
| 330 | return new dxTriMeshData(); |
---|
| 331 | } |
---|
| 332 | |
---|
| 333 | void dGeomTriMeshDataDestroy(dTriMeshDataID g){ |
---|
| 334 | delete g; |
---|
| 335 | } |
---|
| 336 | |
---|
| 337 | |
---|
| 338 | |
---|
| 339 | |
---|
| 340 | void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans ) |
---|
| 341 | { |
---|
| 342 | dAASSERT(g) |
---|
| 343 | dUASSERT(g->type == dTriMeshClass, "geom not trimesh"); |
---|
| 344 | |
---|
| 345 | for (int i=0; i<16; i++) |
---|
| 346 | (((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ]; |
---|
| 347 | |
---|
| 348 | return; |
---|
| 349 | } |
---|
| 350 | |
---|
| 351 | |
---|
| 352 | dReal* dGeomTriMeshGetLastTransform( dxGeom* g ) |
---|
| 353 | { |
---|
| 354 | dAASSERT(g) |
---|
| 355 | dUASSERT(g->type == dTriMeshClass, "geom not trimesh"); |
---|
| 356 | |
---|
| 357 | return (dReal*)(((dxTriMesh*)g)->last_trans); |
---|
| 358 | } |
---|
| 359 | |
---|
| 360 | |
---|
| 361 | |
---|
| 362 | |
---|
| 363 | void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) |
---|
| 364 | { |
---|
| 365 | dUASSERT(g, "argument not trimesh data"); |
---|
| 366 | |
---|
| 367 | switch (data_id) |
---|
| 368 | { |
---|
| 369 | case TRIMESH_FACE_NORMALS: |
---|
| 370 | g->Normals = (dReal *) in_data; |
---|
| 371 | break; |
---|
| 372 | |
---|
| 373 | default: |
---|
| 374 | dUASSERT(data_id, "invalid data type"); |
---|
| 375 | break; |
---|
| 376 | } |
---|
| 377 | |
---|
| 378 | return; |
---|
| 379 | } |
---|
| 380 | |
---|
| 381 | |
---|
| 382 | |
---|
| 383 | void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) |
---|
| 384 | { |
---|
| 385 | dUASSERT(g, "argument not trimesh data"); |
---|
| 386 | |
---|
| 387 | switch (data_id) |
---|
| 388 | { |
---|
| 389 | case TRIMESH_FACE_NORMALS: |
---|
| 390 | return (void *) g->Normals; |
---|
| 391 | break; |
---|
| 392 | |
---|
| 393 | default: |
---|
| 394 | dUASSERT(data_id, "invalid data type"); |
---|
| 395 | break; |
---|
| 396 | } |
---|
| 397 | |
---|
| 398 | return NULL; |
---|
| 399 | } |
---|
| 400 | |
---|
| 401 | |
---|
| 402 | void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, |
---|
| 403 | const void* Vertices, int VertexStride, int VertexCount, |
---|
| 404 | const void* Indices, int IndexCount, int TriStride, |
---|
| 405 | const void* Normals) |
---|
| 406 | { |
---|
| 407 | dUASSERT(g, "argument not trimesh data"); |
---|
| 408 | |
---|
| 409 | g->Build(Vertices, VertexStride, VertexCount, |
---|
| 410 | Indices, IndexCount, TriStride, |
---|
| 411 | Normals, |
---|
| 412 | true); |
---|
| 413 | } |
---|
| 414 | |
---|
| 415 | |
---|
| 416 | void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, |
---|
| 417 | const void* Vertices, int VertexStride, int VertexCount, |
---|
| 418 | const void* Indices, int IndexCount, int TriStride) |
---|
| 419 | { |
---|
| 420 | dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount, |
---|
| 421 | Indices, IndexCount, TriStride, (void*)NULL); |
---|
| 422 | } |
---|
| 423 | |
---|
| 424 | |
---|
| 425 | void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, |
---|
| 426 | const void* Vertices, int VertexStride, int VertexCount, |
---|
| 427 | const void* Indices, int IndexCount, int TriStride, |
---|
| 428 | const void* Normals) |
---|
| 429 | { |
---|
| 430 | dUASSERT(g, "argument not trimesh data"); |
---|
| 431 | |
---|
| 432 | g->Build(Vertices, VertexStride, VertexCount, |
---|
| 433 | Indices, IndexCount, TriStride, |
---|
| 434 | Normals, |
---|
| 435 | false); |
---|
| 436 | } |
---|
| 437 | |
---|
| 438 | |
---|
| 439 | void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, |
---|
| 440 | const void* Vertices, int VertexStride, int VertexCount, |
---|
| 441 | const void* Indices, int IndexCount, int TriStride) { |
---|
| 442 | dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount, |
---|
| 443 | Indices, IndexCount, TriStride, NULL); |
---|
| 444 | } |
---|
| 445 | |
---|
| 446 | |
---|
| 447 | void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, |
---|
| 448 | const dReal* Vertices, int VertexCount, |
---|
| 449 | const int* Indices, int IndexCount, |
---|
| 450 | const int* Normals){ |
---|
| 451 | #ifdef dSINGLE |
---|
| 452 | dGeomTriMeshDataBuildSingle1(g, |
---|
| 453 | Vertices, 4 * sizeof(dReal), VertexCount, |
---|
| 454 | Indices, IndexCount, 3 * sizeof(unsigned int), |
---|
| 455 | Normals); |
---|
| 456 | #else |
---|
| 457 | dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, |
---|
| 458 | Indices, IndexCount, 3 * sizeof(unsigned int), |
---|
| 459 | Normals); |
---|
| 460 | #endif |
---|
| 461 | } |
---|
| 462 | |
---|
| 463 | |
---|
| 464 | void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, |
---|
| 465 | const dReal* Vertices, int VertexCount, |
---|
| 466 | const int* Indices, int IndexCount) { |
---|
| 467 | dGeomTriMeshDataBuildSimple1(g, |
---|
| 468 | Vertices, VertexCount, Indices, IndexCount, |
---|
| 469 | (const int*)NULL); |
---|
| 470 | } |
---|
| 471 | |
---|
| 472 | void dGeomTriMeshDataPreprocess(dTriMeshDataID g) |
---|
| 473 | { |
---|
| 474 | dUASSERT(g, "argument not trimesh data"); |
---|
| 475 | g->Preprocess(); |
---|
| 476 | } |
---|
| 477 | |
---|
| 478 | void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen) |
---|
| 479 | { |
---|
| 480 | dUASSERT(g, "argument not trimesh data"); |
---|
| 481 | #if dTRIMESH_ENABLED |
---|
| 482 | *buf = g->UseFlags; |
---|
| 483 | *bufLen = g->Mesh.GetNbTriangles(); |
---|
| 484 | #endif // dTRIMESH_ENABLED |
---|
| 485 | } |
---|
| 486 | |
---|
| 487 | void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf) |
---|
| 488 | { |
---|
| 489 | dUASSERT(g, "argument not trimesh data"); |
---|
| 490 | g->UseFlags = buf; |
---|
| 491 | } |
---|
| 492 | |
---|
| 493 | |
---|
| 494 | #if dTRIMESH_ENABLED |
---|
| 495 | |
---|
| 496 | // Trimesh Class Statics |
---|
| 497 | PlanesCollider dxTriMesh::_PlanesCollider; |
---|
| 498 | SphereCollider dxTriMesh::_SphereCollider; |
---|
| 499 | OBBCollider dxTriMesh::_OBBCollider; |
---|
| 500 | RayCollider dxTriMesh::_RayCollider; |
---|
| 501 | AABBTreeCollider dxTriMesh::_AABBTreeCollider; |
---|
| 502 | LSSCollider dxTriMesh::_LSSCollider; |
---|
| 503 | |
---|
| 504 | SphereCache dxTriMesh::defaultSphereCache; |
---|
| 505 | OBBCache dxTriMesh::defaultBoxCache; |
---|
| 506 | LSSCache dxTriMesh::defaultCapsuleCache; |
---|
| 507 | |
---|
| 508 | CollisionFaces dxTriMesh::Faces; |
---|
| 509 | |
---|
| 510 | #endif // dTRIMESH_ENABLED |
---|
| 511 | |
---|
| 512 | |
---|
| 513 | dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1) |
---|
| 514 | { |
---|
| 515 | type = dTriMeshClass; |
---|
| 516 | |
---|
| 517 | this->Data = Data; |
---|
| 518 | |
---|
| 519 | #if dTRIMESH_ENABLED |
---|
| 520 | |
---|
| 521 | _RayCollider.SetDestination(&Faces); |
---|
| 522 | |
---|
| 523 | _PlanesCollider.SetTemporalCoherence(true); |
---|
| 524 | |
---|
| 525 | _SphereCollider.SetTemporalCoherence(true); |
---|
| 526 | _SphereCollider.SetPrimitiveTests(false); |
---|
| 527 | |
---|
| 528 | _OBBCollider.SetTemporalCoherence(true); |
---|
| 529 | |
---|
| 530 | // no first-contact test (i.e. return full contact info) |
---|
| 531 | _AABBTreeCollider.SetFirstContact( false ); |
---|
| 532 | // temporal coherence only works with "first conact" tests |
---|
| 533 | _AABBTreeCollider.SetTemporalCoherence(false); |
---|
| 534 | // Perform full BV-BV tests (true) or SAT-lite tests (false) |
---|
| 535 | _AABBTreeCollider.SetFullBoxBoxTest( true ); |
---|
| 536 | // Perform full Primitive-BV tests (true) or SAT-lite tests (false) |
---|
| 537 | _AABBTreeCollider.SetFullPrimBoxTest( true ); |
---|
| 538 | _LSSCollider.SetTemporalCoherence(false); |
---|
| 539 | |
---|
| 540 | #endif // dTRIMESH_ENABLED |
---|
| 541 | |
---|
| 542 | /* TC has speed/space 'issues' that don't make it a clear |
---|
| 543 | win by default on spheres/boxes. */ |
---|
| 544 | this->doSphereTC = false; |
---|
| 545 | this->doBoxTC = false; |
---|
| 546 | this->doCapsuleTC = false; |
---|
| 547 | |
---|
| 548 | #if dTRIMESH_ENABLED |
---|
| 549 | |
---|
| 550 | const char* msg; |
---|
| 551 | if ((msg =_AABBTreeCollider.ValidateSettings())) |
---|
| 552 | dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__); |
---|
| 553 | _LSSCollider.SetPrimitiveTests(false); |
---|
| 554 | _LSSCollider.SetFirstContact(false); |
---|
| 555 | |
---|
| 556 | #endif // dTRIMESH_ENABLED |
---|
| 557 | |
---|
| 558 | for (int i=0; i<16; i++) |
---|
| 559 | last_trans[i] = REAL( 0.0 ); |
---|
| 560 | } |
---|
| 561 | |
---|
| 562 | dxTriMesh::~dxTriMesh(){ |
---|
| 563 | // |
---|
| 564 | } |
---|
| 565 | |
---|
| 566 | // Cleanup for allocations when shutting down ODE |
---|
| 567 | void opcode_collider_cleanup() |
---|
| 568 | { |
---|
| 569 | #if dTRIMESH_ENABLED |
---|
| 570 | |
---|
| 571 | // Clear TC caches |
---|
| 572 | dxTriMesh::Faces.Empty(); |
---|
| 573 | dxTriMesh::defaultSphereCache.TouchedPrimitives.Empty(); |
---|
| 574 | dxTriMesh::defaultBoxCache.TouchedPrimitives.Empty(); |
---|
| 575 | dxTriMesh::defaultCapsuleCache.TouchedPrimitives.Empty(); |
---|
| 576 | |
---|
| 577 | #endif // dTRIMESH_ENABLED |
---|
| 578 | } |
---|
| 579 | |
---|
| 580 | |
---|
| 581 | |
---|
| 582 | void dxTriMesh::ClearTCCache() |
---|
| 583 | { |
---|
| 584 | #if dTRIMESH_ENABLED |
---|
| 585 | /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches - |
---|
| 586 | but the destructor isn't called when doing this, so we would leak. |
---|
| 587 | So, call the previous caches' containers' destructors by hand first. */ |
---|
| 588 | int i, n; |
---|
| 589 | n = SphereTCCache.size(); |
---|
| 590 | for( i = 0; i < n; ++i ) { |
---|
| 591 | SphereTCCache[i].~SphereTC(); |
---|
| 592 | } |
---|
| 593 | SphereTCCache.setSize(0); |
---|
| 594 | n = BoxTCCache.size(); |
---|
| 595 | for( i = 0; i < n; ++i ) { |
---|
| 596 | BoxTCCache[i].~BoxTC(); |
---|
| 597 | } |
---|
| 598 | BoxTCCache.setSize(0); |
---|
| 599 | n = CapsuleTCCache.size(); |
---|
| 600 | for( i = 0; i < n; ++i ) { |
---|
| 601 | CapsuleTCCache[i].~CapsuleTC(); |
---|
| 602 | } |
---|
| 603 | CapsuleTCCache.setSize(0); |
---|
| 604 | #endif // dTRIMESH_ENABLED |
---|
| 605 | } |
---|
| 606 | |
---|
| 607 | |
---|
| 608 | int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){ |
---|
| 609 | return 1; |
---|
| 610 | } |
---|
| 611 | |
---|
| 612 | |
---|
| 613 | void dxTriMesh::computeAABB() { |
---|
| 614 | const dxTriMeshData* d = Data; |
---|
| 615 | dVector3 c; |
---|
| 616 | const dMatrix3& R = final_posr->R; |
---|
| 617 | const dVector3& pos = final_posr->pos; |
---|
| 618 | |
---|
| 619 | dMULTIPLY0_331( c, R, d->AABBCenter ); |
---|
| 620 | |
---|
| 621 | dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) + |
---|
| 622 | dFabs(R[1] * Data->AABBExtents[1]) + |
---|
| 623 | dFabs(R[2] * Data->AABBExtents[2]); |
---|
| 624 | dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) + |
---|
| 625 | dFabs(R[5] * Data->AABBExtents[1]) + |
---|
| 626 | dFabs(R[6] * Data->AABBExtents[2]); |
---|
| 627 | dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) + |
---|
| 628 | dFabs(R[9] * Data->AABBExtents[1]) + |
---|
| 629 | dFabs(R[10] * Data->AABBExtents[2]); |
---|
| 630 | |
---|
| 631 | aabb[0] = c[0] + pos[0] - xrange; |
---|
| 632 | aabb[1] = c[0] + pos[0] + xrange; |
---|
| 633 | aabb[2] = c[1] + pos[1] - yrange; |
---|
| 634 | aabb[3] = c[1] + pos[1] + yrange; |
---|
| 635 | aabb[4] = c[2] + pos[2] - zrange; |
---|
| 636 | aabb[5] = c[2] + pos[2] + zrange; |
---|
| 637 | } |
---|
| 638 | |
---|
| 639 | |
---|
| 640 | void dxTriMeshData::UpdateData() |
---|
| 641 | { |
---|
| 642 | #if dTRIMESH_ENABLED |
---|
| 643 | BVTree.Refit(); |
---|
| 644 | #endif // dTRIMESH_ENABLED |
---|
| 645 | } |
---|
| 646 | |
---|
| 647 | |
---|
| 648 | dGeomID dCreateTriMesh(dSpaceID space, |
---|
| 649 | dTriMeshDataID Data, |
---|
| 650 | dTriCallback* Callback, |
---|
| 651 | dTriArrayCallback* ArrayCallback, |
---|
| 652 | dTriRayCallback* RayCallback) |
---|
| 653 | { |
---|
| 654 | dxTriMesh* Geom = new dxTriMesh(space, Data); |
---|
| 655 | Geom->Callback = Callback; |
---|
| 656 | Geom->ArrayCallback = ArrayCallback; |
---|
| 657 | Geom->RayCallback = RayCallback; |
---|
| 658 | |
---|
| 659 | return Geom; |
---|
| 660 | } |
---|
| 661 | |
---|
| 662 | void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback) |
---|
| 663 | { |
---|
| 664 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 665 | ((dxTriMesh*)g)->Callback = Callback; |
---|
| 666 | } |
---|
| 667 | |
---|
| 668 | dTriCallback* dGeomTriMeshGetCallback(dGeomID g) |
---|
| 669 | { |
---|
| 670 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 671 | return ((dxTriMesh*)g)->Callback; |
---|
| 672 | } |
---|
| 673 | |
---|
| 674 | void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback) |
---|
| 675 | { |
---|
| 676 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 677 | ((dxTriMesh*)g)->ArrayCallback = ArrayCallback; |
---|
| 678 | } |
---|
| 679 | |
---|
| 680 | dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g) |
---|
| 681 | { |
---|
| 682 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 683 | return ((dxTriMesh*)g)->ArrayCallback; |
---|
| 684 | } |
---|
| 685 | |
---|
| 686 | void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback) |
---|
| 687 | { |
---|
| 688 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 689 | ((dxTriMesh*)g)->RayCallback = Callback; |
---|
| 690 | } |
---|
| 691 | |
---|
| 692 | dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g) |
---|
| 693 | { |
---|
| 694 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 695 | return ((dxTriMesh*)g)->RayCallback; |
---|
| 696 | } |
---|
| 697 | |
---|
| 698 | void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data) |
---|
| 699 | { |
---|
| 700 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 701 | ((dxTriMesh*)g)->Data = Data; |
---|
| 702 | // I changed my data -- I know nothing about my own AABB anymore. |
---|
| 703 | ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD); |
---|
| 704 | } |
---|
| 705 | |
---|
| 706 | dTriMeshDataID dGeomTriMeshGetData(dGeomID g) |
---|
| 707 | { |
---|
| 708 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 709 | return ((dxTriMesh*)g)->Data; |
---|
| 710 | } |
---|
| 711 | |
---|
| 712 | |
---|
| 713 | |
---|
| 714 | void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable) |
---|
| 715 | { |
---|
| 716 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 717 | |
---|
| 718 | switch (geomClass) |
---|
| 719 | { |
---|
| 720 | case dSphereClass: |
---|
| 721 | ((dxTriMesh*)g)->doSphereTC = (1 == enable); |
---|
| 722 | break; |
---|
| 723 | case dBoxClass: |
---|
| 724 | ((dxTriMesh*)g)->doBoxTC = (1 == enable); |
---|
| 725 | break; |
---|
| 726 | case dCapsuleClass: |
---|
| 727 | ((dxTriMesh*)g)->doCapsuleTC = (1 == enable); |
---|
| 728 | break; |
---|
| 729 | } |
---|
| 730 | } |
---|
| 731 | |
---|
| 732 | int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass) |
---|
| 733 | { |
---|
| 734 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 735 | |
---|
| 736 | switch (geomClass) |
---|
| 737 | { |
---|
| 738 | case dSphereClass: |
---|
| 739 | if (((dxTriMesh*)g)->doSphereTC) |
---|
| 740 | return 1; |
---|
| 741 | break; |
---|
| 742 | case dBoxClass: |
---|
| 743 | if (((dxTriMesh*)g)->doBoxTC) |
---|
| 744 | return 1; |
---|
| 745 | break; |
---|
| 746 | case dCapsuleClass: |
---|
| 747 | if (((dxTriMesh*)g)->doCapsuleTC) |
---|
| 748 | return 1; |
---|
| 749 | break; |
---|
| 750 | } |
---|
| 751 | return 0; |
---|
| 752 | } |
---|
| 753 | |
---|
| 754 | void dGeomTriMeshClearTCCache(dGeomID g){ |
---|
| 755 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 756 | |
---|
| 757 | dxTriMesh* Geom = (dxTriMesh*)g; |
---|
| 758 | Geom->ClearTCCache(); |
---|
| 759 | } |
---|
| 760 | |
---|
| 761 | /* |
---|
| 762 | * returns the TriMeshDataID |
---|
| 763 | */ |
---|
| 764 | dTriMeshDataID |
---|
| 765 | dGeomTriMeshGetTriMeshDataID(dGeomID g) |
---|
| 766 | { |
---|
| 767 | dxTriMesh* Geom = (dxTriMesh*) g; |
---|
| 768 | return Geom->Data; |
---|
| 769 | } |
---|
| 770 | |
---|
| 771 | // Getting data |
---|
| 772 | void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){ |
---|
| 773 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 774 | |
---|
| 775 | dxTriMesh* Geom = (dxTriMesh*)g; |
---|
| 776 | |
---|
| 777 | const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); |
---|
| 778 | const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); |
---|
| 779 | |
---|
| 780 | dVector3 v[3]; |
---|
| 781 | FetchTriangle(Geom, Index, Position, Rotation, v); |
---|
| 782 | |
---|
| 783 | if (v0){ |
---|
| 784 | (*v0)[0] = v[0][0]; |
---|
| 785 | (*v0)[1] = v[0][1]; |
---|
| 786 | (*v0)[2] = v[0][2]; |
---|
| 787 | (*v0)[3] = v[0][3]; |
---|
| 788 | } |
---|
| 789 | if (v1){ |
---|
| 790 | (*v1)[0] = v[1][0]; |
---|
| 791 | (*v1)[1] = v[1][1]; |
---|
| 792 | (*v1)[2] = v[1][2]; |
---|
| 793 | (*v1)[3] = v[1][3]; |
---|
| 794 | } |
---|
| 795 | if (v2){ |
---|
| 796 | (*v2)[0] = v[2][0]; |
---|
| 797 | (*v2)[1] = v[2][1]; |
---|
| 798 | (*v2)[2] = v[2][2]; |
---|
| 799 | (*v2)[3] = v[2][3]; |
---|
| 800 | } |
---|
| 801 | } |
---|
| 802 | |
---|
| 803 | void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){ |
---|
| 804 | dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); |
---|
| 805 | |
---|
| 806 | dxTriMesh* Geom = (dxTriMesh*)g; |
---|
| 807 | |
---|
| 808 | const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); |
---|
| 809 | const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); |
---|
| 810 | |
---|
| 811 | dVector3 dv[3]; |
---|
| 812 | FetchTriangle(Geom, Index, Position, Rotation, dv); |
---|
| 813 | |
---|
| 814 | GetPointFromBarycentric(dv, u, v, Out); |
---|
| 815 | } |
---|
| 816 | |
---|
| 817 | int dGeomTriMeshGetTriangleCount (dGeomID g) |
---|
| 818 | { |
---|
| 819 | #if dTRIMESH_ENABLED |
---|
| 820 | dxTriMesh* Geom = (dxTriMesh*)g; |
---|
| 821 | return Geom->Data->Mesh.GetNbTriangles(); |
---|
| 822 | #else |
---|
| 823 | return 0; |
---|
| 824 | #endif // dTRIMESH_ENABLED |
---|
| 825 | } |
---|
| 826 | |
---|
| 827 | void dGeomTriMeshDataUpdate(dTriMeshDataID g) { |
---|
| 828 | dUASSERT(g, "argument not trimesh data"); |
---|
| 829 | g->UpdateData(); |
---|
| 830 | } |
---|
| 831 | |
---|
| 832 | #endif // dTRIMESH_OPCODE |
---|
| 833 | #endif // dTRIMESH_ENABLED |
---|