diff options
Diffstat (limited to 'libraries/ode-0.9/OPCODE')
101 files changed, 23212 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceAABB.cpp b/libraries/ode-0.9/OPCODE/Ice/IceAABB.cpp new file mode 100644 index 0000000..d62b0ed --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceAABB.cpp | |||
| @@ -0,0 +1,405 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains AABB-related code. | ||
| 4 | * \file IceAABB.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 29, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * AABB class. | ||
| 13 | * \class AABB | ||
| 14 | * \author Pierre Terdiman | ||
| 15 | * \version 1.0 | ||
| 16 | */ | ||
| 17 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 18 | |||
| 19 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 20 | // Precompiled Header | ||
| 21 | #include "Stdafx.h" | ||
| 22 | |||
| 23 | using namespace IceMaths; | ||
| 24 | |||
| 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 26 | /** | ||
| 27 | * Computes the sum of two AABBs. | ||
| 28 | * \param aabb [in] the other AABB | ||
| 29 | * \return Self-Reference | ||
| 30 | */ | ||
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 32 | AABB& AABB::Add(const AABB& aabb) | ||
| 33 | { | ||
| 34 | // Compute new min & max values | ||
| 35 | Point Min; GetMin(Min); | ||
| 36 | Point Tmp; aabb.GetMin(Tmp); | ||
| 37 | Min.Min(Tmp); | ||
| 38 | |||
| 39 | Point Max; GetMax(Max); | ||
| 40 | aabb.GetMax(Tmp); | ||
| 41 | Max.Max(Tmp); | ||
| 42 | |||
| 43 | // Update this | ||
| 44 | SetMinMax(Min, Max); | ||
| 45 | return *this; | ||
| 46 | } | ||
| 47 | |||
| 48 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 49 | /** | ||
| 50 | * Makes a cube from the AABB. | ||
| 51 | * \param cube [out] the cube AABB | ||
| 52 | * \return cube edge length | ||
| 53 | */ | ||
| 54 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 55 | float AABB::MakeCube(AABB& cube) const | ||
| 56 | { | ||
| 57 | Point Ext; GetExtents(Ext); | ||
| 58 | float Max = Ext.Max(); | ||
| 59 | |||
| 60 | Point Cnt; GetCenter(Cnt); | ||
| 61 | cube.SetCenterExtents(Cnt, Point(Max, Max, Max)); | ||
| 62 | return Max; | ||
| 63 | } | ||
| 64 | |||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | /** | ||
| 67 | * Makes a sphere from the AABB. | ||
| 68 | * \param sphere [out] sphere containing the AABB | ||
| 69 | */ | ||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | void AABB::MakeSphere(Sphere& sphere) const | ||
| 72 | { | ||
| 73 | GetExtents(sphere.mCenter); | ||
| 74 | sphere.mRadius = sphere.mCenter.Magnitude() * 1.00001f; // To make sure sphere::Contains(*this) succeeds | ||
| 75 | GetCenter(sphere.mCenter); | ||
| 76 | } | ||
| 77 | |||
| 78 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 79 | /** | ||
| 80 | * Checks a box is inside another box. | ||
| 81 | * \param box [in] the other AABB | ||
| 82 | * \return true if current box is inside input box | ||
| 83 | */ | ||
| 84 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 85 | bool AABB::IsInside(const AABB& box) const | ||
| 86 | { | ||
| 87 | if(box.GetMin(0)>GetMin(0)) return false; | ||
| 88 | if(box.GetMin(1)>GetMin(1)) return false; | ||
| 89 | if(box.GetMin(2)>GetMin(2)) return false; | ||
| 90 | if(box.GetMax(0)<GetMax(0)) return false; | ||
| 91 | if(box.GetMax(1)<GetMax(1)) return false; | ||
| 92 | if(box.GetMax(2)<GetMax(2)) return false; | ||
| 93 | return true; | ||
| 94 | } | ||
| 95 | |||
| 96 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 97 | /** | ||
| 98 | * Computes the AABB planes. | ||
| 99 | * \param planes [out] 6 planes surrounding the box | ||
| 100 | * \return true if success | ||
| 101 | */ | ||
| 102 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 103 | bool AABB::ComputePlanes(Plane* planes) const | ||
| 104 | { | ||
| 105 | // Checkings | ||
| 106 | if(!planes) return false; | ||
| 107 | |||
| 108 | Point Center, Extents; | ||
| 109 | GetCenter(Center); | ||
| 110 | GetExtents(Extents); | ||
| 111 | |||
| 112 | // Writes normals | ||
| 113 | planes[0].n = Point(1.0f, 0.0f, 0.0f); | ||
| 114 | planes[1].n = Point(-1.0f, 0.0f, 0.0f); | ||
| 115 | planes[2].n = Point(0.0f, 1.0f, 0.0f); | ||
| 116 | planes[3].n = Point(0.0f, -1.0f, 0.0f); | ||
| 117 | planes[4].n = Point(0.0f, 0.0f, 1.0f); | ||
| 118 | planes[5].n = Point(0.0f, 0.0f, -1.0f); | ||
| 119 | |||
| 120 | // Compute a point on each plane | ||
| 121 | Point p0 = Point(Center.x+Extents.x, Center.y, Center.z); | ||
| 122 | Point p1 = Point(Center.x-Extents.x, Center.y, Center.z); | ||
| 123 | Point p2 = Point(Center.x, Center.y+Extents.y, Center.z); | ||
| 124 | Point p3 = Point(Center.x, Center.y-Extents.y, Center.z); | ||
| 125 | Point p4 = Point(Center.x, Center.y, Center.z+Extents.z); | ||
| 126 | Point p5 = Point(Center.x, Center.y, Center.z-Extents.z); | ||
| 127 | |||
| 128 | // Compute d | ||
| 129 | planes[0].d = -(planes[0].n|p0); | ||
| 130 | planes[1].d = -(planes[1].n|p1); | ||
| 131 | planes[2].d = -(planes[2].n|p2); | ||
| 132 | planes[3].d = -(planes[3].n|p3); | ||
| 133 | planes[4].d = -(planes[4].n|p4); | ||
| 134 | planes[5].d = -(planes[5].n|p5); | ||
| 135 | |||
| 136 | return true; | ||
| 137 | } | ||
| 138 | |||
| 139 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 140 | /** | ||
| 141 | * Computes the aabb points. | ||
| 142 | * \param pts [out] 8 box points | ||
| 143 | * \return true if success | ||
| 144 | */ | ||
| 145 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 146 | bool AABB::ComputePoints(Point* pts) const | ||
| 147 | { | ||
| 148 | // Checkings | ||
| 149 | if(!pts) return false; | ||
| 150 | |||
| 151 | // Get box corners | ||
| 152 | Point min; GetMin(min); | ||
| 153 | Point max; GetMax(max); | ||
| 154 | |||
| 155 | // 7+------+6 0 = --- | ||
| 156 | // /| /| 1 = +-- | ||
| 157 | // / | / | 2 = ++- | ||
| 158 | // / 4+---/--+5 3 = -+- | ||
| 159 | // 3+------+2 / y z 4 = --+ | ||
| 160 | // | / | / | / 5 = +-+ | ||
| 161 | // |/ |/ |/ 6 = +++ | ||
| 162 | // 0+------+1 *---x 7 = -++ | ||
| 163 | |||
| 164 | // Generate 8 corners of the bbox | ||
| 165 | pts[0] = Point(min.x, min.y, min.z); | ||
| 166 | pts[1] = Point(max.x, min.y, min.z); | ||
| 167 | pts[2] = Point(max.x, max.y, min.z); | ||
| 168 | pts[3] = Point(min.x, max.y, min.z); | ||
| 169 | pts[4] = Point(min.x, min.y, max.z); | ||
| 170 | pts[5] = Point(max.x, min.y, max.z); | ||
| 171 | pts[6] = Point(max.x, max.y, max.z); | ||
| 172 | pts[7] = Point(min.x, max.y, max.z); | ||
| 173 | |||
| 174 | return true; | ||
| 175 | } | ||
| 176 | |||
| 177 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 178 | /** | ||
| 179 | * Gets vertex normals. | ||
| 180 | * \param pts [out] 8 box points | ||
| 181 | * \return true if success | ||
| 182 | */ | ||
| 183 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 184 | const Point* AABB::GetVertexNormals() const | ||
| 185 | { | ||
| 186 | static float VertexNormals[] = | ||
| 187 | { | ||
| 188 | -INVSQRT3, -INVSQRT3, -INVSQRT3, | ||
| 189 | INVSQRT3, -INVSQRT3, -INVSQRT3, | ||
| 190 | INVSQRT3, INVSQRT3, -INVSQRT3, | ||
| 191 | -INVSQRT3, INVSQRT3, -INVSQRT3, | ||
| 192 | -INVSQRT3, -INVSQRT3, INVSQRT3, | ||
| 193 | INVSQRT3, -INVSQRT3, INVSQRT3, | ||
| 194 | INVSQRT3, INVSQRT3, INVSQRT3, | ||
| 195 | -INVSQRT3, INVSQRT3, INVSQRT3 | ||
| 196 | }; | ||
| 197 | return (const Point*)VertexNormals; | ||
| 198 | } | ||
| 199 | |||
| 200 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 201 | /** | ||
| 202 | * Returns edges. | ||
| 203 | * \return 24 indices (12 edges) indexing the list returned by ComputePoints() | ||
| 204 | */ | ||
| 205 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 206 | const udword* AABB::GetEdges() const | ||
| 207 | { | ||
| 208 | static udword Indices[] = { | ||
| 209 | 0, 1, 1, 2, 2, 3, 3, 0, | ||
| 210 | 7, 6, 6, 5, 5, 4, 4, 7, | ||
| 211 | 1, 5, 6, 2, | ||
| 212 | 3, 7, 4, 0 | ||
| 213 | }; | ||
| 214 | return Indices; | ||
| 215 | } | ||
| 216 | |||
| 217 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 218 | /** | ||
| 219 | * Returns edge normals. | ||
| 220 | * \return edge normals in local space | ||
| 221 | */ | ||
| 222 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 223 | const Point* AABB::GetEdgeNormals() const | ||
| 224 | { | ||
| 225 | static float EdgeNormals[] = | ||
| 226 | { | ||
| 227 | 0, -INVSQRT2, -INVSQRT2, // 0-1 | ||
| 228 | INVSQRT2, 0, -INVSQRT2, // 1-2 | ||
| 229 | 0, INVSQRT2, -INVSQRT2, // 2-3 | ||
| 230 | -INVSQRT2, 0, -INVSQRT2, // 3-0 | ||
| 231 | |||
| 232 | 0, INVSQRT2, INVSQRT2, // 7-6 | ||
| 233 | INVSQRT2, 0, INVSQRT2, // 6-5 | ||
| 234 | 0, -INVSQRT2, INVSQRT2, // 5-4 | ||
| 235 | -INVSQRT2, 0, INVSQRT2, // 4-7 | ||
| 236 | |||
| 237 | INVSQRT2, -INVSQRT2, 0, // 1-5 | ||
| 238 | INVSQRT2, INVSQRT2, 0, // 6-2 | ||
| 239 | -INVSQRT2, INVSQRT2, 0, // 3-7 | ||
| 240 | -INVSQRT2, -INVSQRT2, 0 // 4-0 | ||
| 241 | }; | ||
| 242 | return (const Point*)EdgeNormals; | ||
| 243 | } | ||
| 244 | |||
| 245 | // =========================================================================== | ||
| 246 | // (C) 1996-98 Vienna University of Technology | ||
| 247 | // =========================================================================== | ||
| 248 | // NAME: bboxarea | ||
| 249 | // TYPE: c++ code | ||
| 250 | // PROJECT: Bounding Box Area | ||
| 251 | // CONTENT: Computes area of 2D projection of 3D oriented bounding box | ||
| 252 | // VERSION: 1.0 | ||
| 253 | // =========================================================================== | ||
| 254 | // AUTHORS: ds Dieter Schmalstieg | ||
| 255 | // ep Erik Pojar | ||
| 256 | // =========================================================================== | ||
| 257 | // HISTORY: | ||
| 258 | // | ||
| 259 | // 19-sep-99 15:23:03 ds last modification | ||
| 260 | // 01-dec-98 15:23:03 ep created | ||
| 261 | // =========================================================================== | ||
| 262 | |||
| 263 | //---------------------------------------------------------------------------- | ||
| 264 | // SAMPLE CODE STARTS HERE | ||
| 265 | //---------------------------------------------------------------------------- | ||
| 266 | |||
| 267 | // NOTE: This sample program requires OPEN INVENTOR! | ||
| 268 | |||
| 269 | //indexlist: this table stores the 64 possible cases of classification of | ||
| 270 | //the eyepoint with respect to the 6 defining planes of the bbox (2^6=64) | ||
| 271 | //only 26 (3^3-1, where 1 is "inside" cube) of these cases are valid. | ||
| 272 | //the first 6 numbers in each row are the indices of the bbox vertices that | ||
| 273 | //form the outline of which we want to compute the area (counterclockwise | ||
| 274 | //ordering), the 7th entry means the number of vertices in the outline. | ||
| 275 | //there are 6 cases with a single face and and a 4-vertex outline, and | ||
| 276 | //20 cases with 2 or 3 faces and a 6-vertex outline. a value of 0 indicates | ||
| 277 | //an invalid case. | ||
| 278 | |||
| 279 | |||
| 280 | // Original list was made of 7 items, I added an 8th element: | ||
| 281 | // - to padd on a cache line | ||
| 282 | // - to repeat the first entry to avoid modulos | ||
| 283 | // | ||
| 284 | // I also replaced original ints with sbytes. | ||
| 285 | |||
| 286 | static const sbyte gIndexList[64][8] = | ||
| 287 | { | ||
| 288 | {-1,-1,-1,-1,-1,-1,-1, 0}, // 0 inside | ||
| 289 | { 0, 4, 7, 3, 0,-1,-1, 4}, // 1 left | ||
| 290 | { 1, 2, 6, 5, 1,-1,-1, 4}, // 2 right | ||
| 291 | {-1,-1,-1,-1,-1,-1,-1, 0}, // 3 - | ||
| 292 | { 0, 1, 5, 4, 0,-1,-1, 4}, // 4 bottom | ||
| 293 | { 0, 1, 5, 4, 7, 3, 0, 6}, // 5 bottom, left | ||
| 294 | { 0, 1, 2, 6, 5, 4, 0, 6}, // 6 bottom, right | ||
| 295 | {-1,-1,-1,-1,-1,-1,-1, 0}, // 7 - | ||
| 296 | { 2, 3, 7, 6, 2,-1,-1, 4}, // 8 top | ||
| 297 | { 0, 4, 7, 6, 2, 3, 0, 6}, // 9 top, left | ||
| 298 | { 1, 2, 3, 7, 6, 5, 1, 6}, //10 top, right | ||
| 299 | {-1,-1,-1,-1,-1,-1,-1, 0}, //11 - | ||
| 300 | {-1,-1,-1,-1,-1,-1,-1, 0}, //12 - | ||
| 301 | {-1,-1,-1,-1,-1,-1,-1, 0}, //13 - | ||
| 302 | {-1,-1,-1,-1,-1,-1,-1, 0}, //14 - | ||
| 303 | {-1,-1,-1,-1,-1,-1,-1, 0}, //15 - | ||
| 304 | { 0, 3, 2, 1, 0,-1,-1, 4}, //16 front | ||
| 305 | { 0, 4, 7, 3, 2, 1, 0, 6}, //17 front, left | ||
| 306 | { 0, 3, 2, 6, 5, 1, 0, 6}, //18 front, right | ||
| 307 | {-1,-1,-1,-1,-1,-1,-1, 0}, //19 - | ||
| 308 | { 0, 3, 2, 1, 5, 4, 0, 6}, //20 front, bottom | ||
| 309 | { 1, 5, 4, 7, 3, 2, 1, 6}, //21 front, bottom, left | ||
| 310 | { 0, 3, 2, 6, 5, 4, 0, 6}, //22 front, bottom, right | ||
| 311 | {-1,-1,-1,-1,-1,-1,-1, 0}, //23 - | ||
| 312 | { 0, 3, 7, 6, 2, 1, 0, 6}, //24 front, top | ||
| 313 | { 0, 4, 7, 6, 2, 1, 0, 6}, //25 front, top, left | ||
| 314 | { 0, 3, 7, 6, 5, 1, 0, 6}, //26 front, top, right | ||
| 315 | {-1,-1,-1,-1,-1,-1,-1, 0}, //27 - | ||
| 316 | {-1,-1,-1,-1,-1,-1,-1, 0}, //28 - | ||
| 317 | {-1,-1,-1,-1,-1,-1,-1, 0}, //29 - | ||
| 318 | {-1,-1,-1,-1,-1,-1,-1, 0}, //30 - | ||
| 319 | {-1,-1,-1,-1,-1,-1,-1, 0}, //31 - | ||
| 320 | { 4, 5, 6, 7, 4,-1,-1, 4}, //32 back | ||
| 321 | { 0, 4, 5, 6, 7, 3, 0, 6}, //33 back, left | ||
| 322 | { 1, 2, 6, 7, 4, 5, 1, 6}, //34 back, right | ||
| 323 | {-1,-1,-1,-1,-1,-1,-1, 0}, //35 - | ||
| 324 | { 0, 1, 5, 6, 7, 4, 0, 6}, //36 back, bottom | ||
| 325 | { 0, 1, 5, 6, 7, 3, 0, 6}, //37 back, bottom, left | ||
| 326 | { 0, 1, 2, 6, 7, 4, 0, 6}, //38 back, bottom, right | ||
| 327 | {-1,-1,-1,-1,-1,-1,-1, 0}, //39 - | ||
| 328 | { 2, 3, 7, 4, 5, 6, 2, 6}, //40 back, top | ||
| 329 | { 0, 4, 5, 6, 2, 3, 0, 6}, //41 back, top, left | ||
| 330 | { 1, 2, 3, 7, 4, 5, 1, 6}, //42 back, top, right | ||
| 331 | {-1,-1,-1,-1,-1,-1,-1, 0}, //43 invalid | ||
| 332 | {-1,-1,-1,-1,-1,-1,-1, 0}, //44 invalid | ||
| 333 | {-1,-1,-1,-1,-1,-1,-1, 0}, //45 invalid | ||
| 334 | {-1,-1,-1,-1,-1,-1,-1, 0}, //46 invalid | ||
| 335 | {-1,-1,-1,-1,-1,-1,-1, 0}, //47 invalid | ||
| 336 | {-1,-1,-1,-1,-1,-1,-1, 0}, //48 invalid | ||
| 337 | {-1,-1,-1,-1,-1,-1,-1, 0}, //49 invalid | ||
| 338 | {-1,-1,-1,-1,-1,-1,-1, 0}, //50 invalid | ||
| 339 | {-1,-1,-1,-1,-1,-1,-1, 0}, //51 invalid | ||
| 340 | {-1,-1,-1,-1,-1,-1,-1, 0}, //52 invalid | ||
| 341 | {-1,-1,-1,-1,-1,-1,-1, 0}, //53 invalid | ||
| 342 | {-1,-1,-1,-1,-1,-1,-1, 0}, //54 invalid | ||
| 343 | {-1,-1,-1,-1,-1,-1,-1, 0}, //55 invalid | ||
| 344 | {-1,-1,-1,-1,-1,-1,-1, 0}, //56 invalid | ||
| 345 | {-1,-1,-1,-1,-1,-1,-1, 0}, //57 invalid | ||
| 346 | {-1,-1,-1,-1,-1,-1,-1, 0}, //58 invalid | ||
| 347 | {-1,-1,-1,-1,-1,-1,-1, 0}, //59 invalid | ||
| 348 | {-1,-1,-1,-1,-1,-1,-1, 0}, //60 invalid | ||
| 349 | {-1,-1,-1,-1,-1,-1,-1, 0}, //61 invalid | ||
| 350 | {-1,-1,-1,-1,-1,-1,-1, 0}, //62 invalid | ||
| 351 | {-1,-1,-1,-1,-1,-1,-1, 0} //63 invalid | ||
| 352 | }; | ||
| 353 | |||
| 354 | const sbyte* AABB::ComputeOutline(const Point& local_eye, sdword& num) const | ||
| 355 | { | ||
| 356 | // Get box corners | ||
| 357 | Point min; GetMin(min); | ||
| 358 | Point max; GetMax(max); | ||
| 359 | |||
| 360 | // Compute 6-bit code to classify eye with respect to the 6 defining planes of the bbox | ||
| 361 | int pos = ((local_eye.x < min.x) ? 1 : 0) // 1 = left | ||
| 362 | + ((local_eye.x > max.x) ? 2 : 0) // 2 = right | ||
| 363 | + ((local_eye.y < min.y) ? 4 : 0) // 4 = bottom | ||
| 364 | + ((local_eye.y > max.y) ? 8 : 0) // 8 = top | ||
| 365 | + ((local_eye.z < min.z) ? 16 : 0) // 16 = front | ||
| 366 | + ((local_eye.z > max.z) ? 32 : 0); // 32 = back | ||
| 367 | |||
| 368 | // Look up number of vertices in outline | ||
| 369 | num = (sdword)gIndexList[pos][7]; | ||
| 370 | // Zero indicates invalid case | ||
| 371 | if(!num) return null; | ||
| 372 | |||
| 373 | return &gIndexList[pos][0]; | ||
| 374 | } | ||
| 375 | |||
| 376 | // calculateBoxArea: computes the screen-projected 2D area of an oriented 3D bounding box | ||
| 377 | |||
| 378 | //const Point& eye, //eye point (in bbox object coordinates) | ||
| 379 | //const AABB& box, //3d bbox | ||
| 380 | //const Matrix4x4& mat, //free transformation for bbox | ||
| 381 | //float width, float height, int& num) | ||
| 382 | float AABB::ComputeBoxArea(const Point& eye, const Matrix4x4& mat, float width, float height, sdword& num) const | ||
| 383 | { | ||
| 384 | const sbyte* Outline = ComputeOutline(eye, num); | ||
| 385 | if(!Outline) return -1.0f; | ||
| 386 | |||
| 387 | // Compute box vertices | ||
| 388 | Point vertexBox[8], dst[8]; | ||
| 389 | ComputePoints(vertexBox); | ||
| 390 | |||
| 391 | // Transform all outline corners into 2D screen space | ||
| 392 | for(sdword i=0;i<num;i++) | ||
| 393 | { | ||
| 394 | HPoint Projected; | ||
| 395 | vertexBox[Outline[i]].ProjectToScreen(width, height, mat, Projected); | ||
| 396 | dst[i] = Projected; | ||
| 397 | } | ||
| 398 | |||
| 399 | float Sum = (dst[num-1][0] - dst[0][0]) * (dst[num-1][1] + dst[0][1]); | ||
| 400 | |||
| 401 | for(int i=0; i<num-1; i++) | ||
| 402 | Sum += (dst[i][0] - dst[i+1][0]) * (dst[i][1] + dst[i+1][1]); | ||
| 403 | |||
| 404 | return Sum * 0.5f; //return computed value corrected by 0.5 | ||
| 405 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceAABB.h b/libraries/ode-0.9/OPCODE/Ice/IceAABB.h new file mode 100644 index 0000000..f71d7e9 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceAABB.h | |||
| @@ -0,0 +1,505 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains AABB-related code. (axis-aligned bounding box) | ||
| 4 | * \file IceAABB.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 13, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEAABB_H__ | ||
| 13 | #define __ICEAABB_H__ | ||
| 14 | |||
| 15 | // Forward declarations | ||
| 16 | class Sphere; | ||
| 17 | |||
| 18 | //! Declarations of type-independent methods (most of them implemented in the .cpp) | ||
| 19 | #define AABB_COMMON_METHODS \ | ||
| 20 | AABB& Add(const AABB& aabb); \ | ||
| 21 | float MakeCube(AABB& cube) const; \ | ||
| 22 | void MakeSphere(Sphere& sphere) const; \ | ||
| 23 | const sbyte* ComputeOutline(const Point& local_eye, sdword& num) const; \ | ||
| 24 | float ComputeBoxArea(const Point& eye, const Matrix4x4& mat, float width, float height, sdword& num) const; \ | ||
| 25 | bool IsInside(const AABB& box) const; \ | ||
| 26 | bool ComputePlanes(Plane* planes) const; \ | ||
| 27 | bool ComputePoints(Point* pts) const; \ | ||
| 28 | const Point* GetVertexNormals() const; \ | ||
| 29 | const udword* GetEdges() const; \ | ||
| 30 | const Point* GetEdgeNormals() const; \ | ||
| 31 | inline_ BOOL ContainsPoint(const Point& p) const \ | ||
| 32 | { \ | ||
| 33 | if(p.x > GetMax(0) || p.x < GetMin(0)) return FALSE; \ | ||
| 34 | if(p.y > GetMax(1) || p.y < GetMin(1)) return FALSE; \ | ||
| 35 | if(p.z > GetMax(2) || p.z < GetMin(2)) return FALSE; \ | ||
| 36 | return TRUE; \ | ||
| 37 | } | ||
| 38 | |||
| 39 | enum AABBType | ||
| 40 | { | ||
| 41 | AABB_RENDER = 0, //!< AABB used for rendering. Not visible == not rendered. | ||
| 42 | AABB_UPDATE = 1, //!< AABB used for dynamic updates. Not visible == not updated. | ||
| 43 | |||
| 44 | AABB_FORCE_DWORD = 0x7fffffff, | ||
| 45 | }; | ||
| 46 | |||
| 47 | #ifdef USE_MINMAX | ||
| 48 | |||
| 49 | struct ICEMATHS_API ShadowAABB | ||
| 50 | { | ||
| 51 | Point mMin; | ||
| 52 | Point mMax; | ||
| 53 | }; | ||
| 54 | |||
| 55 | class ICEMATHS_API AABB | ||
| 56 | { | ||
| 57 | public: | ||
| 58 | //! Constructor | ||
| 59 | inline_ AABB() {} | ||
| 60 | //! Destructor | ||
| 61 | inline_ ~AABB() {} | ||
| 62 | |||
| 63 | //! Type-independent methods | ||
| 64 | AABB_COMMON_METHODS; | ||
| 65 | |||
| 66 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 67 | /** | ||
| 68 | * Setups an AABB from min & max vectors. | ||
| 69 | * \param min [in] the min point | ||
| 70 | * \param max [in] the max point | ||
| 71 | */ | ||
| 72 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 73 | void SetMinMax(const Point& min, const Point& max) { mMin = min; mMax = max; } | ||
| 74 | |||
| 75 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 76 | /** | ||
| 77 | * Setups an AABB from center & extents vectors. | ||
| 78 | * \param c [in] the center point | ||
| 79 | * \param e [in] the extents vector | ||
| 80 | */ | ||
| 81 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 82 | void SetCenterExtents(const Point& c, const Point& e) { mMin = c - e; mMax = c + e; } | ||
| 83 | |||
| 84 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 85 | /** | ||
| 86 | * Setups an empty AABB. | ||
| 87 | */ | ||
| 88 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 89 | void SetEmpty() { Point p(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); mMin = -p; mMax = p;} | ||
| 90 | |||
| 91 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 92 | /** | ||
| 93 | * Setups a point AABB. | ||
| 94 | */ | ||
| 95 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 96 | void SetPoint(const Point& pt) { mMin = mMax = pt; } | ||
| 97 | |||
| 98 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 99 | /** | ||
| 100 | * Gets the size of the AABB. The size is defined as the longest extent. | ||
| 101 | * \return the size of the AABB | ||
| 102 | */ | ||
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 104 | float GetSize() const { Point e; GetExtents(e); return e.Max(); } | ||
| 105 | |||
| 106 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 107 | /** | ||
| 108 | * Extends the AABB. | ||
| 109 | * \param p [in] the next point | ||
| 110 | */ | ||
| 111 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 112 | void Extend(const Point& p) | ||
| 113 | { | ||
| 114 | if(p.x > mMax.x) mMax.x = p.x; | ||
| 115 | if(p.x < mMin.x) mMin.x = p.x; | ||
| 116 | |||
| 117 | if(p.y > mMax.y) mMax.y = p.y; | ||
| 118 | if(p.y < mMin.y) mMin.y = p.y; | ||
| 119 | |||
| 120 | if(p.z > mMax.z) mMax.z = p.z; | ||
| 121 | if(p.z < mMin.z) mMin.z = p.z; | ||
| 122 | } | ||
| 123 | // Data access | ||
| 124 | |||
| 125 | //! Get min point of the box | ||
| 126 | inline_ void GetMin(Point& min) const { min = mMin; } | ||
| 127 | //! Get max point of the box | ||
| 128 | inline_ void GetMax(Point& max) const { max = mMax; } | ||
| 129 | |||
| 130 | //! Get component of the box's min point along a given axis | ||
| 131 | inline_ float GetMin(udword axis) const { return mMin[axis]; } | ||
| 132 | //! Get component of the box's max point along a given axis | ||
| 133 | inline_ float GetMax(udword axis) const { return mMax[axis]; } | ||
| 134 | |||
| 135 | //! Get box center | ||
| 136 | inline_ void GetCenter(Point& center) const { center = (mMax + mMin)*0.5f; } | ||
| 137 | //! Get box extents | ||
| 138 | inline_ void GetExtents(Point& extents) const { extents = (mMax - mMin)*0.5f; } | ||
| 139 | |||
| 140 | //! Get component of the box's center along a given axis | ||
| 141 | inline_ float GetCenter(udword axis) const { return (mMax[axis] + mMin[axis])*0.5f; } | ||
| 142 | //! Get component of the box's extents along a given axis | ||
| 143 | inline_ float GetExtents(udword axis) const { return (mMax[axis] - mMin[axis])*0.5f; } | ||
| 144 | |||
| 145 | //! Get box diagonal | ||
| 146 | inline_ void GetDiagonal(Point& diagonal) const { diagonal = mMax - mMin; } | ||
| 147 | inline_ float GetWidth() const { return mMax.x - mMin.x; } | ||
| 148 | inline_ float GetHeight() const { return mMax.y - mMin.y; } | ||
| 149 | inline_ float GetDepth() const { return mMax.z - mMin.z; } | ||
| 150 | |||
| 151 | //! Volume | ||
| 152 | inline_ float GetVolume() const { return GetWidth() * GetHeight() * GetDepth(); } | ||
| 153 | |||
| 154 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 155 | /** | ||
| 156 | * Computes the intersection between two AABBs. | ||
| 157 | * \param a [in] the other AABB | ||
| 158 | * \return true on intersection | ||
| 159 | */ | ||
| 160 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 161 | inline_ BOOL Intersect(const AABB& a) const | ||
| 162 | { | ||
| 163 | if(mMax.x < a.mMin.x | ||
| 164 | || a.mMax.x < mMin.x | ||
| 165 | || mMax.y < a.mMin.y | ||
| 166 | || a.mMax.y < mMin.y | ||
| 167 | || mMax.z < a.mMin.z | ||
| 168 | || a.mMax.z < mMin.z) return FALSE; | ||
| 169 | |||
| 170 | return TRUE; | ||
| 171 | } | ||
| 172 | |||
| 173 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 174 | /** | ||
| 175 | * Computes the 1D-intersection between two AABBs, on a given axis. | ||
| 176 | * \param a [in] the other AABB | ||
| 177 | * \param axis [in] the axis (0, 1, 2) | ||
| 178 | * \return true on intersection | ||
| 179 | */ | ||
| 180 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 181 | inline_ BOOL Intersect(const AABB& a, udword axis) const | ||
| 182 | { | ||
| 183 | if(mMax[axis] < a.mMin[axis] || a.mMax[axis] < mMin[axis]) return FALSE; | ||
| 184 | return TRUE; | ||
| 185 | } | ||
| 186 | |||
| 187 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 188 | /** | ||
| 189 | * Recomputes the AABB after an arbitrary transform by a 4x4 matrix. | ||
| 190 | * Original code by Charles Bloom on the GD-Algorithm list. (I slightly modified it) | ||
| 191 | * \param mtx [in] the transform matrix | ||
| 192 | * \param aabb [out] the transformed AABB [can be *this] | ||
| 193 | */ | ||
| 194 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 195 | inline_ void Rotate(const Matrix4x4& mtx, AABB& aabb) const | ||
| 196 | { | ||
| 197 | // The three edges transformed: you can efficiently transform an X-only vector | ||
| 198 | // by just getting the "X" column of the matrix | ||
| 199 | Point vx,vy,vz; | ||
| 200 | mtx.GetRow(0, vx); vx *= (mMax.x - mMin.x); | ||
| 201 | mtx.GetRow(1, vy); vy *= (mMax.y - mMin.y); | ||
| 202 | mtx.GetRow(2, vz); vz *= (mMax.z - mMin.z); | ||
| 203 | |||
| 204 | // Transform the min point | ||
| 205 | aabb.mMin = aabb.mMax = mMin * mtx; | ||
| 206 | |||
| 207 | // Take the transformed min & axes and find new extents | ||
| 208 | // Using CPU code in the right place is faster... | ||
| 209 | if(IS_NEGATIVE_FLOAT(vx.x)) aabb.mMin.x += vx.x; else aabb.mMax.x += vx.x; | ||
| 210 | if(IS_NEGATIVE_FLOAT(vx.y)) aabb.mMin.y += vx.y; else aabb.mMax.y += vx.y; | ||
| 211 | if(IS_NEGATIVE_FLOAT(vx.z)) aabb.mMin.z += vx.z; else aabb.mMax.z += vx.z; | ||
| 212 | if(IS_NEGATIVE_FLOAT(vy.x)) aabb.mMin.x += vy.x; else aabb.mMax.x += vy.x; | ||
| 213 | if(IS_NEGATIVE_FLOAT(vy.y)) aabb.mMin.y += vy.y; else aabb.mMax.y += vy.y; | ||
| 214 | if(IS_NEGATIVE_FLOAT(vy.z)) aabb.mMin.z += vy.z; else aabb.mMax.z += vy.z; | ||
| 215 | if(IS_NEGATIVE_FLOAT(vz.x)) aabb.mMin.x += vz.x; else aabb.mMax.x += vz.x; | ||
| 216 | if(IS_NEGATIVE_FLOAT(vz.y)) aabb.mMin.y += vz.y; else aabb.mMax.y += vz.y; | ||
| 217 | if(IS_NEGATIVE_FLOAT(vz.z)) aabb.mMin.z += vz.z; else aabb.mMax.z += vz.z; | ||
| 218 | } | ||
| 219 | |||
| 220 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 221 | /** | ||
| 222 | * Checks the AABB is valid. | ||
| 223 | * \return true if the box is valid | ||
| 224 | */ | ||
| 225 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 226 | inline_ BOOL IsValid() const | ||
| 227 | { | ||
| 228 | // Consistency condition for (Min, Max) boxes: min < max | ||
| 229 | if(mMin.x > mMax.x) return FALSE; | ||
| 230 | if(mMin.y > mMax.y) return FALSE; | ||
| 231 | if(mMin.z > mMax.z) return FALSE; | ||
| 232 | return TRUE; | ||
| 233 | } | ||
| 234 | |||
| 235 | //! Operator for AABB *= float. Scales the extents, keeps same center. | ||
| 236 | inline_ AABB& operator*=(float s) | ||
| 237 | { | ||
| 238 | Point Center; GetCenter(Center); | ||
| 239 | Point Extents; GetExtents(Extents); | ||
| 240 | SetCenterExtents(Center, Extents * s); | ||
| 241 | return *this; | ||
| 242 | } | ||
| 243 | |||
| 244 | //! Operator for AABB /= float. Scales the extents, keeps same center. | ||
| 245 | inline_ AABB& operator/=(float s) | ||
| 246 | { | ||
| 247 | Point Center; GetCenter(Center); | ||
| 248 | Point Extents; GetExtents(Extents); | ||
| 249 | SetCenterExtents(Center, Extents / s); | ||
| 250 | return *this; | ||
| 251 | } | ||
| 252 | |||
| 253 | //! Operator for AABB += Point. Translates the box. | ||
| 254 | inline_ AABB& operator+=(const Point& trans) | ||
| 255 | { | ||
| 256 | mMin+=trans; | ||
| 257 | mMax+=trans; | ||
| 258 | return *this; | ||
| 259 | } | ||
| 260 | private: | ||
| 261 | Point mMin; //!< Min point | ||
| 262 | Point mMax; //!< Max point | ||
| 263 | }; | ||
| 264 | |||
| 265 | #else | ||
| 266 | |||
| 267 | class ICEMATHS_API AABB | ||
| 268 | { | ||
| 269 | public: | ||
| 270 | //! Constructor | ||
| 271 | inline_ AABB() {} | ||
| 272 | //! Destructor | ||
| 273 | inline_ ~AABB() {} | ||
| 274 | |||
| 275 | //! Type-independent methods | ||
| 276 | AABB_COMMON_METHODS; | ||
| 277 | |||
| 278 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 279 | /** | ||
| 280 | * Setups an AABB from min & max vectors. | ||
| 281 | * \param min [in] the min point | ||
| 282 | * \param max [in] the max point | ||
| 283 | */ | ||
| 284 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 285 | void SetMinMax(const Point& min, const Point& max) { mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f; } | ||
| 286 | |||
| 287 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 288 | /** | ||
| 289 | * Setups an AABB from center & extents vectors. | ||
| 290 | * \param c [in] the center point | ||
| 291 | * \param e [in] the extents vector | ||
| 292 | */ | ||
| 293 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 294 | void SetCenterExtents(const Point& c, const Point& e) { mCenter = c; mExtents = e; } | ||
| 295 | |||
| 296 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 297 | /** | ||
| 298 | * Setups an empty AABB. | ||
| 299 | */ | ||
| 300 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 301 | void SetEmpty() { mCenter.Zero(); mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);} | ||
| 302 | |||
| 303 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 304 | /** | ||
| 305 | * Setups a point AABB. | ||
| 306 | */ | ||
| 307 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 308 | void SetPoint(const Point& pt) { mCenter = pt; mExtents.Zero(); } | ||
| 309 | |||
| 310 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 311 | /** | ||
| 312 | * Gets the size of the AABB. The size is defined as the longest extent. | ||
| 313 | * \return the size of the AABB | ||
| 314 | */ | ||
| 315 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 316 | float GetSize() const { return mExtents.Max(); } | ||
| 317 | |||
| 318 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 319 | /** | ||
| 320 | * Extends the AABB. | ||
| 321 | * \param p [in] the next point | ||
| 322 | */ | ||
| 323 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 324 | void Extend(const Point& p) | ||
| 325 | { | ||
| 326 | Point Max = mCenter + mExtents; | ||
| 327 | Point Min = mCenter - mExtents; | ||
| 328 | |||
| 329 | if(p.x > Max.x) Max.x = p.x; | ||
| 330 | if(p.x < Min.x) Min.x = p.x; | ||
| 331 | |||
| 332 | if(p.y > Max.y) Max.y = p.y; | ||
| 333 | if(p.y < Min.y) Min.y = p.y; | ||
| 334 | |||
| 335 | if(p.z > Max.z) Max.z = p.z; | ||
| 336 | if(p.z < Min.z) Min.z = p.z; | ||
| 337 | |||
| 338 | SetMinMax(Min, Max); | ||
| 339 | } | ||
| 340 | // Data access | ||
| 341 | |||
| 342 | //! Get min point of the box | ||
| 343 | inline_ void GetMin(Point& min) const { min = mCenter - mExtents; } | ||
| 344 | //! Get max point of the box | ||
| 345 | inline_ void GetMax(Point& max) const { max = mCenter + mExtents; } | ||
| 346 | |||
| 347 | //! Get component of the box's min point along a given axis | ||
| 348 | inline_ float GetMin(udword axis) const { return mCenter[axis] - mExtents[axis]; } | ||
| 349 | //! Get component of the box's max point along a given axis | ||
| 350 | inline_ float GetMax(udword axis) const { return mCenter[axis] + mExtents[axis]; } | ||
| 351 | |||
| 352 | //! Get box center | ||
| 353 | inline_ void GetCenter(Point& center) const { center = mCenter; } | ||
| 354 | //! Get box extents | ||
| 355 | inline_ void GetExtents(Point& extents) const { extents = mExtents; } | ||
| 356 | |||
| 357 | //! Get component of the box's center along a given axis | ||
| 358 | inline_ float GetCenter(udword axis) const { return mCenter[axis]; } | ||
| 359 | //! Get component of the box's extents along a given axis | ||
| 360 | inline_ float GetExtents(udword axis) const { return mExtents[axis]; } | ||
| 361 | |||
| 362 | //! Get box diagonal | ||
| 363 | inline_ void GetDiagonal(Point& diagonal) const { diagonal = mExtents * 2.0f; } | ||
| 364 | inline_ float GetWidth() const { return mExtents.x * 2.0f; } | ||
| 365 | inline_ float GetHeight() const { return mExtents.y * 2.0f; } | ||
| 366 | inline_ float GetDepth() const { return mExtents.z * 2.0f; } | ||
| 367 | |||
| 368 | //! Volume | ||
| 369 | inline_ float GetVolume() const { return mExtents.x * mExtents.y * mExtents.z * 8.0f; } | ||
| 370 | |||
| 371 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 372 | /** | ||
| 373 | * Computes the intersection between two AABBs. | ||
| 374 | * \param a [in] the other AABB | ||
| 375 | * \return true on intersection | ||
| 376 | */ | ||
| 377 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 378 | inline_ BOOL Intersect(const AABB& a) const | ||
| 379 | { | ||
| 380 | float tx = mCenter.x - a.mCenter.x; float ex = a.mExtents.x + mExtents.x; if(AIR(tx) > IR(ex)) return FALSE; | ||
| 381 | float ty = mCenter.y - a.mCenter.y; float ey = a.mExtents.y + mExtents.y; if(AIR(ty) > IR(ey)) return FALSE; | ||
| 382 | float tz = mCenter.z - a.mCenter.z; float ez = a.mExtents.z + mExtents.z; if(AIR(tz) > IR(ez)) return FALSE; | ||
| 383 | return TRUE; | ||
| 384 | } | ||
| 385 | |||
| 386 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 387 | /** | ||
| 388 | * The standard intersection method from Gamasutra. Just here to check its speed against the one above. | ||
| 389 | * \param a [in] the other AABB | ||
| 390 | * \return true on intersection | ||
| 391 | */ | ||
| 392 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 393 | inline_ bool GomezIntersect(const AABB& a) | ||
| 394 | { | ||
| 395 | Point T = mCenter - a.mCenter; // Vector from A to B | ||
| 396 | return ((fabsf(T.x) <= (a.mExtents.x + mExtents.x)) | ||
| 397 | && (fabsf(T.y) <= (a.mExtents.y + mExtents.y)) | ||
| 398 | && (fabsf(T.z) <= (a.mExtents.z + mExtents.z))); | ||
| 399 | } | ||
| 400 | |||
| 401 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 402 | /** | ||
| 403 | * Computes the 1D-intersection between two AABBs, on a given axis. | ||
| 404 | * \param a [in] the other AABB | ||
| 405 | * \param axis [in] the axis (0, 1, 2) | ||
| 406 | * \return true on intersection | ||
| 407 | */ | ||
| 408 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 409 | inline_ BOOL Intersect(const AABB& a, udword axis) const | ||
| 410 | { | ||
| 411 | float t = mCenter[axis] - a.mCenter[axis]; | ||
| 412 | float e = a.mExtents[axis] + mExtents[axis]; | ||
| 413 | if(AIR(t) > IR(e)) return FALSE; | ||
| 414 | return TRUE; | ||
| 415 | } | ||
| 416 | |||
| 417 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 418 | /** | ||
| 419 | * Recomputes the AABB after an arbitrary transform by a 4x4 matrix. | ||
| 420 | * \param mtx [in] the transform matrix | ||
| 421 | * \param aabb [out] the transformed AABB [can be *this] | ||
| 422 | */ | ||
| 423 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 424 | inline_ void Rotate(const Matrix4x4& mtx, AABB& aabb) const | ||
| 425 | { | ||
| 426 | // Compute new center | ||
| 427 | aabb.mCenter = mCenter * mtx; | ||
| 428 | |||
| 429 | // Compute new extents. FPU code & CPU code have been interleaved for improved performance. | ||
| 430 | Point Ex(mtx.m[0][0] * mExtents.x, mtx.m[0][1] * mExtents.x, mtx.m[0][2] * mExtents.x); | ||
| 431 | IR(Ex.x)&=0x7fffffff; IR(Ex.y)&=0x7fffffff; IR(Ex.z)&=0x7fffffff; | ||
| 432 | |||
| 433 | Point Ey(mtx.m[1][0] * mExtents.y, mtx.m[1][1] * mExtents.y, mtx.m[1][2] * mExtents.y); | ||
| 434 | IR(Ey.x)&=0x7fffffff; IR(Ey.y)&=0x7fffffff; IR(Ey.z)&=0x7fffffff; | ||
| 435 | |||
| 436 | Point Ez(mtx.m[2][0] * mExtents.z, mtx.m[2][1] * mExtents.z, mtx.m[2][2] * mExtents.z); | ||
| 437 | IR(Ez.x)&=0x7fffffff; IR(Ez.y)&=0x7fffffff; IR(Ez.z)&=0x7fffffff; | ||
| 438 | |||
| 439 | aabb.mExtents.x = Ex.x + Ey.x + Ez.x; | ||
| 440 | aabb.mExtents.y = Ex.y + Ey.y + Ez.y; | ||
| 441 | aabb.mExtents.z = Ex.z + Ey.z + Ez.z; | ||
| 442 | } | ||
| 443 | |||
| 444 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 445 | /** | ||
| 446 | * Checks the AABB is valid. | ||
| 447 | * \return true if the box is valid | ||
| 448 | */ | ||
| 449 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 450 | inline_ BOOL IsValid() const | ||
| 451 | { | ||
| 452 | // Consistency condition for (Center, Extents) boxes: Extents >= 0 | ||
| 453 | if(IS_NEGATIVE_FLOAT(mExtents.x)) return FALSE; | ||
| 454 | if(IS_NEGATIVE_FLOAT(mExtents.y)) return FALSE; | ||
| 455 | if(IS_NEGATIVE_FLOAT(mExtents.z)) return FALSE; | ||
| 456 | return TRUE; | ||
| 457 | } | ||
| 458 | |||
| 459 | //! Operator for AABB *= float. Scales the extents, keeps same center. | ||
| 460 | inline_ AABB& operator*=(float s) { mExtents*=s; return *this; } | ||
| 461 | |||
| 462 | //! Operator for AABB /= float. Scales the extents, keeps same center. | ||
| 463 | inline_ AABB& operator/=(float s) { mExtents/=s; return *this; } | ||
| 464 | |||
| 465 | //! Operator for AABB += Point. Translates the box. | ||
| 466 | inline_ AABB& operator+=(const Point& trans) | ||
| 467 | { | ||
| 468 | mCenter+=trans; | ||
| 469 | return *this; | ||
| 470 | } | ||
| 471 | private: | ||
| 472 | Point mCenter; //!< AABB Center | ||
| 473 | Point mExtents; //!< x, y and z extents | ||
| 474 | }; | ||
| 475 | |||
| 476 | #endif | ||
| 477 | |||
| 478 | inline_ void ComputeMinMax(const Point& p, Point& min, Point& max) | ||
| 479 | { | ||
| 480 | if(p.x > max.x) max.x = p.x; | ||
| 481 | if(p.x < min.x) min.x = p.x; | ||
| 482 | |||
| 483 | if(p.y > max.y) max.y = p.y; | ||
| 484 | if(p.y < min.y) min.y = p.y; | ||
| 485 | |||
| 486 | if(p.z > max.z) max.z = p.z; | ||
| 487 | if(p.z < min.z) min.z = p.z; | ||
| 488 | } | ||
| 489 | |||
| 490 | inline_ void ComputeAABB(AABB& aabb, const Point* list, udword nb_pts) | ||
| 491 | { | ||
| 492 | if(list) | ||
| 493 | { | ||
| 494 | Point Maxi(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); | ||
| 495 | Point Mini(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT); | ||
| 496 | while(nb_pts--) | ||
| 497 | { | ||
| 498 | // _prefetch(list+1); // off by one ? | ||
| 499 | ComputeMinMax(*list++, Mini, Maxi); | ||
| 500 | } | ||
| 501 | aabb.SetMinMax(Mini, Maxi); | ||
| 502 | } | ||
| 503 | } | ||
| 504 | |||
| 505 | #endif // __ICEAABB_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceAxes.h b/libraries/ode-0.9/OPCODE/Ice/IceAxes.h new file mode 100644 index 0000000..8af57e1 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceAxes.h | |||
| @@ -0,0 +1,54 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains axes definition. | ||
| 4 | * \file IceAxes.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 29, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEAXES_H__ | ||
| 13 | #define __ICEAXES_H__ | ||
| 14 | |||
| 15 | enum PointComponent | ||
| 16 | { | ||
| 17 | X = 0, | ||
| 18 | Y = 1, | ||
| 19 | Z = 2, | ||
| 20 | W = 3, | ||
| 21 | |||
| 22 | FORCE_DWORD = 0x7fffffff | ||
| 23 | }; | ||
| 24 | |||
| 25 | enum AxisOrder | ||
| 26 | { | ||
| 27 | AXES_XYZ = (X)|(Y<<2)|(Z<<4), | ||
| 28 | AXES_XZY = (X)|(Z<<2)|(Y<<4), | ||
| 29 | AXES_YXZ = (Y)|(X<<2)|(Z<<4), | ||
| 30 | AXES_YZX = (Y)|(Z<<2)|(X<<4), | ||
| 31 | AXES_ZXY = (Z)|(X<<2)|(Y<<4), | ||
| 32 | AXES_ZYX = (Z)|(Y<<2)|(X<<4), | ||
| 33 | |||
| 34 | AXES_FORCE_DWORD = 0x7fffffff | ||
| 35 | }; | ||
| 36 | |||
| 37 | class ICEMATHS_API Axes | ||
| 38 | { | ||
| 39 | public: | ||
| 40 | |||
| 41 | inline_ Axes(AxisOrder order) | ||
| 42 | { | ||
| 43 | mAxis0 = (order ) & 3; | ||
| 44 | mAxis1 = (order>>2) & 3; | ||
| 45 | mAxis2 = (order>>4) & 3; | ||
| 46 | } | ||
| 47 | inline_ ~Axes() {} | ||
| 48 | |||
| 49 | udword mAxis0; | ||
| 50 | udword mAxis1; | ||
| 51 | udword mAxis2; | ||
| 52 | }; | ||
| 53 | |||
| 54 | #endif // __ICEAXES_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceBoundingSphere.h b/libraries/ode-0.9/OPCODE/Ice/IceBoundingSphere.h new file mode 100644 index 0000000..945d38c --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceBoundingSphere.h | |||
| @@ -0,0 +1,142 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code to compute the minimal bounding sphere. | ||
| 4 | * \file IceBoundingSphere.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 29, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEBOUNDINGSPHERE_H__ | ||
| 13 | #define __ICEBOUNDINGSPHERE_H__ | ||
| 14 | |||
| 15 | enum BSphereMethod | ||
| 16 | { | ||
| 17 | BS_NONE, | ||
| 18 | BS_GEMS, | ||
| 19 | BS_MINIBALL, | ||
| 20 | |||
| 21 | BS_FORCE_DWORD = 0x7fffffff | ||
| 22 | }; | ||
| 23 | |||
| 24 | class ICEMATHS_API Sphere | ||
| 25 | { | ||
| 26 | public: | ||
| 27 | //! Constructor | ||
| 28 | inline_ Sphere() {} | ||
| 29 | //! Constructor | ||
| 30 | inline_ Sphere(const Point& center, float radius) : mCenter(center), mRadius(radius) {} | ||
| 31 | //! Constructor | ||
| 32 | Sphere(udword nb_verts, const Point* verts); | ||
| 33 | //! Copy constructor | ||
| 34 | inline_ Sphere(const Sphere& sphere) : mCenter(sphere.mCenter), mRadius(sphere.mRadius) {} | ||
| 35 | //! Destructor | ||
| 36 | inline_ ~Sphere() {} | ||
| 37 | |||
| 38 | BSphereMethod Compute(udword nb_verts, const Point* verts); | ||
| 39 | bool FastCompute(udword nb_verts, const Point* verts); | ||
| 40 | |||
| 41 | // Access methods | ||
| 42 | inline_ const Point& GetCenter() const { return mCenter; } | ||
| 43 | inline_ float GetRadius() const { return mRadius; } | ||
| 44 | |||
| 45 | inline_ const Point& Center() const { return mCenter; } | ||
| 46 | inline_ float Radius() const { return mRadius; } | ||
| 47 | |||
| 48 | inline_ Sphere& Set(const Point& center, float radius) { mCenter = center; mRadius = radius; return *this; } | ||
| 49 | inline_ Sphere& SetCenter(const Point& center) { mCenter = center; return *this; } | ||
| 50 | inline_ Sphere& SetRadius(float radius) { mRadius = radius; return *this; } | ||
| 51 | |||
| 52 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 53 | /** | ||
| 54 | * Tests if a point is contained within the sphere. | ||
| 55 | * \param p [in] the point to test | ||
| 56 | * \return true if inside the sphere | ||
| 57 | */ | ||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | inline_ bool Contains(const Point& p) const | ||
| 60 | { | ||
| 61 | return mCenter.SquareDistance(p) <= mRadius*mRadius; | ||
| 62 | } | ||
| 63 | |||
| 64 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 65 | /** | ||
| 66 | * Tests if a sphere is contained within the sphere. | ||
| 67 | * \param sphere [in] the sphere to test | ||
| 68 | * \return true if inside the sphere | ||
| 69 | */ | ||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | inline_ bool Contains(const Sphere& sphere) const | ||
| 72 | { | ||
| 73 | // If our radius is the smallest, we can't possibly contain the other sphere | ||
| 74 | if(mRadius < sphere.mRadius) return false; | ||
| 75 | // So r is always positive or null now | ||
| 76 | float r = mRadius - sphere.mRadius; | ||
| 77 | return mCenter.SquareDistance(sphere.mCenter) <= r*r; | ||
| 78 | } | ||
| 79 | |||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | /** | ||
| 82 | * Tests if a box is contained within the sphere. | ||
| 83 | * \param aabb [in] the box to test | ||
| 84 | * \return true if inside the sphere | ||
| 85 | */ | ||
| 86 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 87 | inline_ BOOL Contains(const AABB& aabb) const | ||
| 88 | { | ||
| 89 | // I assume if all 8 box vertices are inside the sphere, so does the whole box. | ||
| 90 | // Sounds ok but maybe there's a better way? | ||
| 91 | float R2 = mRadius * mRadius; | ||
| 92 | #ifdef USE_MIN_MAX | ||
| 93 | const Point& Max = ((ShadowAABB&)&aabb).mMax; | ||
| 94 | const Point& Min = ((ShadowAABB&)&aabb).mMin; | ||
| 95 | #else | ||
| 96 | Point Max; aabb.GetMax(Max); | ||
| 97 | Point Min; aabb.GetMin(Min); | ||
| 98 | #endif | ||
| 99 | Point p; | ||
| 100 | p.x=Max.x; p.y=Max.y; p.z=Max.z; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 101 | p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 102 | p.x=Max.x; p.y=Min.y; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 103 | p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 104 | p.x=Max.x; p.y=Max.y; p.z=Min.z; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 105 | p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 106 | p.x=Max.x; p.y=Min.y; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 107 | p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE; | ||
| 108 | |||
| 109 | return TRUE; | ||
| 110 | } | ||
| 111 | |||
| 112 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 113 | /** | ||
| 114 | * Tests if the sphere intersects another sphere | ||
| 115 | * \param sphere [in] the other sphere | ||
| 116 | * \return true if spheres overlap | ||
| 117 | */ | ||
| 118 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 119 | inline_ bool Intersect(const Sphere& sphere) const | ||
| 120 | { | ||
| 121 | float r = mRadius + sphere.mRadius; | ||
| 122 | return mCenter.SquareDistance(sphere.mCenter) <= r*r; | ||
| 123 | } | ||
| 124 | |||
| 125 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 126 | /** | ||
| 127 | * Checks the sphere is valid. | ||
| 128 | * \return true if the box is valid | ||
| 129 | */ | ||
| 130 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 131 | inline_ BOOL IsValid() const | ||
| 132 | { | ||
| 133 | // Consistency condition for spheres: Radius >= 0.0f | ||
| 134 | if(mRadius < 0.0f) return FALSE; | ||
| 135 | return TRUE; | ||
| 136 | } | ||
| 137 | public: | ||
| 138 | Point mCenter; //!< Sphere center | ||
| 139 | float mRadius; //!< Sphere radius | ||
| 140 | }; | ||
| 141 | |||
| 142 | #endif // __ICEBOUNDINGSPHERE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceContainer.cpp b/libraries/ode-0.9/OPCODE/Ice/IceContainer.cpp new file mode 100644 index 0000000..8a4a570 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceContainer.cpp | |||
| @@ -0,0 +1,345 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains a simple container class. | ||
| 4 | * \file IceContainer.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date February, 5, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * Contains a list of 32-bits values. | ||
| 13 | * Use this class when you need to store an unknown number of values. The list is automatically | ||
| 14 | * resized and can contains 32-bits entities (dwords or floats) | ||
| 15 | * | ||
| 16 | * \class Container | ||
| 17 | * \author Pierre Terdiman | ||
| 18 | * \version 1.0 | ||
| 19 | * \date 08.15.98 | ||
| 20 | */ | ||
| 21 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 22 | |||
| 23 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 24 | // Precompiled Header | ||
| 25 | #include "Stdafx.h" | ||
| 26 | |||
| 27 | using namespace IceCore; | ||
| 28 | |||
| 29 | // Static members | ||
| 30 | #ifdef CONTAINER_STATS | ||
| 31 | udword Container::mNbContainers = 0; | ||
| 32 | udword Container::mUsedRam = 0; | ||
| 33 | #endif | ||
| 34 | |||
| 35 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 36 | /** | ||
| 37 | * Constructor. No entries allocated there. | ||
| 38 | */ | ||
| 39 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 40 | Container::Container() : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f) | ||
| 41 | { | ||
| 42 | #ifdef CONTAINER_STATS | ||
| 43 | mNbContainers++; | ||
| 44 | mUsedRam+=sizeof(Container); | ||
| 45 | #endif | ||
| 46 | } | ||
| 47 | |||
| 48 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 49 | /** | ||
| 50 | * Constructor. Also allocates a given number of entries. | ||
| 51 | */ | ||
| 52 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 53 | Container::Container(udword size, float growth_factor) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(growth_factor) | ||
| 54 | { | ||
| 55 | #ifdef CONTAINER_STATS | ||
| 56 | mNbContainers++; | ||
| 57 | mUsedRam+=sizeof(Container); | ||
| 58 | #endif | ||
| 59 | SetSize(size); | ||
| 60 | } | ||
| 61 | |||
| 62 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 63 | /** | ||
| 64 | * Copy constructor. | ||
| 65 | */ | ||
| 66 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 67 | Container::Container(const Container& object) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f) | ||
| 68 | { | ||
| 69 | #ifdef CONTAINER_STATS | ||
| 70 | mNbContainers++; | ||
| 71 | mUsedRam+=sizeof(Container); | ||
| 72 | #endif | ||
| 73 | *this = object; | ||
| 74 | } | ||
| 75 | |||
| 76 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 77 | /** | ||
| 78 | * Destructor. Frees everything and leaves. | ||
| 79 | */ | ||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | Container::~Container() | ||
| 82 | { | ||
| 83 | Empty(); | ||
| 84 | #ifdef CONTAINER_STATS | ||
| 85 | mNbContainers--; | ||
| 86 | mUsedRam-=GetUsedRam(); | ||
| 87 | #endif | ||
| 88 | } | ||
| 89 | |||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | /** | ||
| 92 | * Clears the container. All stored values are deleted, and it frees used ram. | ||
| 93 | * \see Reset() | ||
| 94 | * \return Self-Reference | ||
| 95 | */ | ||
| 96 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 97 | Container& Container::Empty() | ||
| 98 | { | ||
| 99 | #ifdef CONTAINER_STATS | ||
| 100 | mUsedRam-=mMaxNbEntries*sizeof(udword); | ||
| 101 | #endif | ||
| 102 | DELETEARRAY(mEntries); | ||
| 103 | mCurNbEntries = mMaxNbEntries = 0; | ||
| 104 | return *this; | ||
| 105 | } | ||
| 106 | |||
| 107 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 108 | /** | ||
| 109 | * Resizes the container. | ||
| 110 | * \param needed [in] assume the container can be added at least "needed" values | ||
| 111 | * \return true if success. | ||
| 112 | */ | ||
| 113 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 114 | bool Container::Resize(udword needed) | ||
| 115 | { | ||
| 116 | #ifdef CONTAINER_STATS | ||
| 117 | // Subtract previous amount of bytes | ||
| 118 | mUsedRam-=mMaxNbEntries*sizeof(udword); | ||
| 119 | #endif | ||
| 120 | |||
| 121 | // Get more entries | ||
| 122 | mMaxNbEntries = mMaxNbEntries ? udword(float(mMaxNbEntries)*mGrowthFactor) : 2; // Default nb Entries = 2 | ||
| 123 | if(mMaxNbEntries<mCurNbEntries + needed) mMaxNbEntries = mCurNbEntries + needed; | ||
| 124 | |||
| 125 | // Get some bytes for new entries | ||
| 126 | udword* NewEntries = new udword[mMaxNbEntries]; | ||
| 127 | CHECKALLOC(NewEntries); | ||
| 128 | |||
| 129 | #ifdef CONTAINER_STATS | ||
| 130 | // Add current amount of bytes | ||
| 131 | mUsedRam+=mMaxNbEntries*sizeof(udword); | ||
| 132 | #endif | ||
| 133 | |||
| 134 | // Copy old data if needed | ||
| 135 | if(mCurNbEntries) CopyMemory(NewEntries, mEntries, mCurNbEntries*sizeof(udword)); | ||
| 136 | |||
| 137 | // Delete old data | ||
| 138 | DELETEARRAY(mEntries); | ||
| 139 | |||
| 140 | // Assign new pointer | ||
| 141 | mEntries = NewEntries; | ||
| 142 | |||
| 143 | return true; | ||
| 144 | } | ||
| 145 | |||
| 146 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 147 | /** | ||
| 148 | * Sets the initial size of the container. If it already contains something, it's discarded. | ||
| 149 | * \param nb [in] Number of entries | ||
| 150 | * \return true if success | ||
| 151 | */ | ||
| 152 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 153 | bool Container::SetSize(udword nb) | ||
| 154 | { | ||
| 155 | // Make sure it's empty | ||
| 156 | Empty(); | ||
| 157 | |||
| 158 | // Checkings | ||
| 159 | if(!nb) return false; | ||
| 160 | |||
| 161 | // Initialize for nb entries | ||
| 162 | mMaxNbEntries = nb; | ||
| 163 | |||
| 164 | // Get some bytes for new entries | ||
| 165 | mEntries = new udword[mMaxNbEntries]; | ||
| 166 | CHECKALLOC(mEntries); | ||
| 167 | |||
| 168 | #ifdef CONTAINER_STATS | ||
| 169 | // Add current amount of bytes | ||
| 170 | mUsedRam+=mMaxNbEntries*sizeof(udword); | ||
| 171 | #endif | ||
| 172 | return true; | ||
| 173 | } | ||
| 174 | |||
| 175 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 176 | /** | ||
| 177 | * Refits the container and get rid of unused bytes. | ||
| 178 | * \return true if success | ||
| 179 | */ | ||
| 180 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 181 | bool Container::Refit() | ||
| 182 | { | ||
| 183 | #ifdef CONTAINER_STATS | ||
| 184 | // Subtract previous amount of bytes | ||
| 185 | mUsedRam-=mMaxNbEntries*sizeof(udword); | ||
| 186 | #endif | ||
| 187 | |||
| 188 | // Get just enough entries | ||
| 189 | mMaxNbEntries = mCurNbEntries; | ||
| 190 | if(!mMaxNbEntries) return false; | ||
| 191 | |||
| 192 | // Get just enough bytes | ||
| 193 | udword* NewEntries = new udword[mMaxNbEntries]; | ||
| 194 | CHECKALLOC(NewEntries); | ||
| 195 | |||
| 196 | #ifdef CONTAINER_STATS | ||
| 197 | // Add current amount of bytes | ||
| 198 | mUsedRam+=mMaxNbEntries*sizeof(udword); | ||
| 199 | #endif | ||
| 200 | |||
| 201 | // Copy old data | ||
| 202 | CopyMemory(NewEntries, mEntries, mCurNbEntries*sizeof(udword)); | ||
| 203 | |||
| 204 | // Delete old data | ||
| 205 | DELETEARRAY(mEntries); | ||
| 206 | |||
| 207 | // Assign new pointer | ||
| 208 | mEntries = NewEntries; | ||
| 209 | |||
| 210 | return true; | ||
| 211 | } | ||
| 212 | |||
| 213 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 214 | /** | ||
| 215 | * Checks whether the container already contains a given value. | ||
| 216 | * \param entry [in] the value to look for in the container | ||
| 217 | * \param location [out] a possible pointer to store the entry location | ||
| 218 | * \see Add(udword entry) | ||
| 219 | * \see Add(float entry) | ||
| 220 | * \see Empty() | ||
| 221 | * \return true if the value has been found in the container, else false. | ||
| 222 | */ | ||
| 223 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 224 | bool Container::Contains(udword entry, udword* location) const | ||
| 225 | { | ||
| 226 | // Look for the entry | ||
| 227 | for(udword i=0;i<mCurNbEntries;i++) | ||
| 228 | { | ||
| 229 | if(mEntries[i]==entry) | ||
| 230 | { | ||
| 231 | if(location) *location = i; | ||
| 232 | return true; | ||
| 233 | } | ||
| 234 | } | ||
| 235 | return false; | ||
| 236 | } | ||
| 237 | |||
| 238 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 239 | /** | ||
| 240 | * Deletes an entry. If the container contains such an entry, it's removed. | ||
| 241 | * \param entry [in] the value to delete. | ||
| 242 | * \return true if the value has been found in the container, else false. | ||
| 243 | * \warning This method is arbitrary slow (O(n)) and should be used carefully. Insertion order is not preserved. | ||
| 244 | */ | ||
| 245 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 246 | bool Container::Delete(udword entry) | ||
| 247 | { | ||
| 248 | // Look for the entry | ||
| 249 | for(udword i=0;i<mCurNbEntries;i++) | ||
| 250 | { | ||
| 251 | if(mEntries[i]==entry) | ||
| 252 | { | ||
| 253 | // Entry has been found at index i. The strategy is to copy the last current entry at index i, and decrement the current number of entries. | ||
| 254 | DeleteIndex(i); | ||
| 255 | return true; | ||
| 256 | } | ||
| 257 | } | ||
| 258 | return false; | ||
| 259 | } | ||
| 260 | |||
| 261 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 262 | /** | ||
| 263 | * Deletes an entry, preserving the insertion order. If the container contains such an entry, it's removed. | ||
| 264 | * \param entry [in] the value to delete. | ||
| 265 | * \return true if the value has been found in the container, else false. | ||
| 266 | * \warning This method is arbitrary slow (O(n)) and should be used carefully. | ||
| 267 | */ | ||
| 268 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 269 | bool Container::DeleteKeepingOrder(udword entry) | ||
| 270 | { | ||
| 271 | // Look for the entry | ||
| 272 | for(udword i=0;i<mCurNbEntries;i++) | ||
| 273 | { | ||
| 274 | if(mEntries[i]==entry) | ||
| 275 | { | ||
| 276 | // Entry has been found at index i. | ||
| 277 | // Shift entries to preserve order. You really should use a linked list instead. | ||
| 278 | mCurNbEntries--; | ||
| 279 | for(udword j=i;j<mCurNbEntries;j++) | ||
| 280 | { | ||
| 281 | mEntries[j] = mEntries[j+1]; | ||
| 282 | } | ||
| 283 | return true; | ||
| 284 | } | ||
| 285 | } | ||
| 286 | return false; | ||
| 287 | } | ||
| 288 | |||
| 289 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 290 | /** | ||
| 291 | * Gets the next entry, starting from input one. | ||
| 292 | * \param entry [in/out] On input, the entry to look for. On output, the next entry | ||
| 293 | * \param find_mode [in] wrap/clamp | ||
| 294 | * \return Self-Reference | ||
| 295 | */ | ||
| 296 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 297 | Container& Container::FindNext(udword& entry, FindMode find_mode) | ||
| 298 | { | ||
| 299 | udword Location; | ||
| 300 | if(Contains(entry, &Location)) | ||
| 301 | { | ||
| 302 | Location++; | ||
| 303 | if(Location==mCurNbEntries) Location = find_mode==FIND_WRAP ? 0 : mCurNbEntries-1; | ||
| 304 | entry = mEntries[Location]; | ||
| 305 | } | ||
| 306 | return *this; | ||
| 307 | } | ||
| 308 | |||
| 309 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 310 | /** | ||
| 311 | * Gets the previous entry, starting from input one. | ||
| 312 | * \param entry [in/out] On input, the entry to look for. On output, the previous entry | ||
| 313 | * \param find_mode [in] wrap/clamp | ||
| 314 | * \return Self-Reference | ||
| 315 | */ | ||
| 316 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 317 | Container& Container::FindPrev(udword& entry, FindMode find_mode) | ||
| 318 | { | ||
| 319 | udword Location; | ||
| 320 | if(Contains(entry, &Location)) | ||
| 321 | { | ||
| 322 | Location--; | ||
| 323 | if(Location==0xffffffff) Location = find_mode==FIND_WRAP ? mCurNbEntries-1 : 0; | ||
| 324 | entry = mEntries[Location]; | ||
| 325 | } | ||
| 326 | return *this; | ||
| 327 | } | ||
| 328 | |||
| 329 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 330 | /** | ||
| 331 | * Gets the ram used by the container. | ||
| 332 | * \return the ram used in bytes. | ||
| 333 | */ | ||
| 334 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 335 | udword Container::GetUsedRam() const | ||
| 336 | { | ||
| 337 | return sizeof(Container) + mMaxNbEntries * sizeof(udword); | ||
| 338 | } | ||
| 339 | |||
| 340 | /*void Container::operator=(const Container& object) | ||
| 341 | { | ||
| 342 | SetSize(object.GetNbEntries()); | ||
| 343 | CopyMemory(mEntries, object.GetEntries(), mMaxNbEntries*sizeof(udword)); | ||
| 344 | mCurNbEntries = mMaxNbEntries; | ||
| 345 | }*/ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceContainer.h b/libraries/ode-0.9/OPCODE/Ice/IceContainer.h new file mode 100644 index 0000000..d726c2a --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceContainer.h | |||
| @@ -0,0 +1,212 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains a simple container class. | ||
| 4 | * \file IceContainer.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date February, 5, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICECONTAINER_H__ | ||
| 13 | #define __ICECONTAINER_H__ | ||
| 14 | |||
| 15 | #define CONTAINER_STATS | ||
| 16 | |||
| 17 | enum FindMode | ||
| 18 | { | ||
| 19 | FIND_CLAMP, | ||
| 20 | FIND_WRAP, | ||
| 21 | |||
| 22 | FIND_FORCE_DWORD = 0x7fffffff | ||
| 23 | }; | ||
| 24 | |||
| 25 | class ICECORE_API Container | ||
| 26 | { | ||
| 27 | public: | ||
| 28 | // Constructor / Destructor | ||
| 29 | Container(); | ||
| 30 | Container(const Container& object); | ||
| 31 | Container(udword size, float growth_factor); | ||
| 32 | ~Container(); | ||
| 33 | // Management | ||
| 34 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 35 | /** | ||
| 36 | * A O(1) method to add a value in the container. The container is automatically resized if needed. | ||
| 37 | * The method is inline, not the resize. The call overhead happens on resizes only, which is not a problem since the resizing operation | ||
| 38 | * costs a lot more than the call overhead... | ||
| 39 | * | ||
| 40 | * \param entry [in] a udword to store in the container | ||
| 41 | * \see Add(float entry) | ||
| 42 | * \see Empty() | ||
| 43 | * \see Contains(udword entry) | ||
| 44 | * \return Self-Reference | ||
| 45 | */ | ||
| 46 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 47 | inline_ Container& Add(udword entry) | ||
| 48 | { | ||
| 49 | // Resize if needed | ||
| 50 | if(mCurNbEntries==mMaxNbEntries) Resize(); | ||
| 51 | |||
| 52 | // Add new entry | ||
| 53 | mEntries[mCurNbEntries++] = entry; | ||
| 54 | return *this; | ||
| 55 | } | ||
| 56 | |||
| 57 | inline_ Container& Add(const udword* entries, udword nb) | ||
| 58 | { | ||
| 59 | // Resize if needed | ||
| 60 | if(mCurNbEntries+nb>mMaxNbEntries) Resize(nb); | ||
| 61 | |||
| 62 | // Add new entry | ||
| 63 | CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(udword)); | ||
| 64 | mCurNbEntries+=nb; | ||
| 65 | return *this; | ||
| 66 | } | ||
| 67 | |||
| 68 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 69 | /** | ||
| 70 | * A O(1) method to add a value in the container. The container is automatically resized if needed. | ||
| 71 | * The method is inline, not the resize. The call overhead happens on resizes only, which is not a problem since the resizing operation | ||
| 72 | * costs a lot more than the call overhead... | ||
| 73 | * | ||
| 74 | * \param entry [in] a float to store in the container | ||
| 75 | * \see Add(udword entry) | ||
| 76 | * \see Empty() | ||
| 77 | * \see Contains(udword entry) | ||
| 78 | * \return Self-Reference | ||
| 79 | */ | ||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | inline_ Container& Add(float entry) | ||
| 82 | { | ||
| 83 | // Resize if needed | ||
| 84 | if(mCurNbEntries==mMaxNbEntries) Resize(); | ||
| 85 | |||
| 86 | // Add new entry | ||
| 87 | mEntries[mCurNbEntries++] = IR(entry); | ||
| 88 | return *this; | ||
| 89 | } | ||
| 90 | |||
| 91 | inline_ Container& Add(const float* entries, udword nb) | ||
| 92 | { | ||
| 93 | // Resize if needed | ||
| 94 | if(mCurNbEntries+nb>mMaxNbEntries) Resize(nb); | ||
| 95 | |||
| 96 | // Add new entry | ||
| 97 | CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(float)); | ||
| 98 | mCurNbEntries+=nb; | ||
| 99 | return *this; | ||
| 100 | } | ||
| 101 | |||
| 102 | //! Add unique [slow] | ||
| 103 | inline_ Container& AddUnique(udword entry) | ||
| 104 | { | ||
| 105 | if(!Contains(entry)) Add(entry); | ||
| 106 | return *this; | ||
| 107 | } | ||
| 108 | |||
| 109 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 110 | /** | ||
| 111 | * Clears the container. All stored values are deleted, and it frees used ram. | ||
| 112 | * \see Reset() | ||
| 113 | * \return Self-Reference | ||
| 114 | */ | ||
| 115 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 116 | Container& Empty(); | ||
| 117 | |||
| 118 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 119 | /** | ||
| 120 | * Resets the container. Stored values are discarded but the buffer is kept so that further calls don't need resizing again. | ||
| 121 | * That's a kind of temporal coherence. | ||
| 122 | * \see Empty() | ||
| 123 | */ | ||
| 124 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 125 | inline_ void Reset() | ||
| 126 | { | ||
| 127 | // Avoid the write if possible | ||
| 128 | // ### CMOV | ||
| 129 | if(mCurNbEntries) mCurNbEntries = 0; | ||
| 130 | } | ||
| 131 | |||
| 132 | // HANDLE WITH CARE | ||
| 133 | inline_ void ForceSize(udword size) | ||
| 134 | { | ||
| 135 | mCurNbEntries = size; | ||
| 136 | } | ||
| 137 | |||
| 138 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 139 | /** | ||
| 140 | * Sets the initial size of the container. If it already contains something, it's discarded. | ||
| 141 | * \param nb [in] Number of entries | ||
| 142 | * \return true if success | ||
| 143 | */ | ||
| 144 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 145 | bool SetSize(udword nb); | ||
| 146 | |||
| 147 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 148 | /** | ||
| 149 | * Refits the container and get rid of unused bytes. | ||
| 150 | * \return true if success | ||
| 151 | */ | ||
| 152 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 153 | bool Refit(); | ||
| 154 | |||
| 155 | // Checks whether the container already contains a given value. | ||
| 156 | bool Contains(udword entry, udword* location=null) const; | ||
| 157 | // Deletes an entry - doesn't preserve insertion order. | ||
| 158 | bool Delete(udword entry); | ||
| 159 | // Deletes an entry - does preserve insertion order. | ||
| 160 | bool DeleteKeepingOrder(udword entry); | ||
| 161 | //! Deletes the very last entry. | ||
| 162 | inline_ void DeleteLastEntry() { if(mCurNbEntries) mCurNbEntries--; } | ||
| 163 | //! Deletes the entry whose index is given | ||
| 164 | inline_ void DeleteIndex(udword index) { mEntries[index] = mEntries[--mCurNbEntries]; } | ||
| 165 | |||
| 166 | // Helpers | ||
| 167 | Container& FindNext(udword& entry, FindMode find_mode=FIND_CLAMP); | ||
| 168 | Container& FindPrev(udword& entry, FindMode find_mode=FIND_CLAMP); | ||
| 169 | // Data access. | ||
| 170 | inline_ udword GetNbEntries() const { return mCurNbEntries; } //!< Returns the current number of entries. | ||
| 171 | inline_ udword GetEntry(udword i) const { return mEntries[i]; } //!< Returns ith entry | ||
| 172 | inline_ udword* GetEntries() const { return mEntries; } //!< Returns the list of entries. | ||
| 173 | |||
| 174 | inline_ udword GetFirst() const { return mEntries[0]; } | ||
| 175 | inline_ udword GetLast() const { return mEntries[mCurNbEntries-1]; } | ||
| 176 | |||
| 177 | // Growth control | ||
| 178 | inline_ float GetGrowthFactor() const { return mGrowthFactor; } //!< Returns the growth factor | ||
| 179 | inline_ void SetGrowthFactor(float growth) { mGrowthFactor = growth; } //!< Sets the growth factor | ||
| 180 | inline_ bool IsFull() const { return mCurNbEntries==mMaxNbEntries; } //!< Checks the container is full | ||
| 181 | inline_ BOOL IsNotEmpty() const { return mCurNbEntries; } //!< Checks the container is empty | ||
| 182 | |||
| 183 | //! Read-access as an array | ||
| 184 | inline_ udword operator[](udword i) const { ASSERT(i>=0 && i<mCurNbEntries); return mEntries[i]; } | ||
| 185 | //! Write-access as an array | ||
| 186 | inline_ udword& operator[](udword i) { ASSERT(i>=0 && i<mCurNbEntries); return mEntries[i]; } | ||
| 187 | |||
| 188 | // Stats | ||
| 189 | udword GetUsedRam() const; | ||
| 190 | |||
| 191 | //! Operator for "Container A = Container B" | ||
| 192 | //void operator = (const Container& object); | ||
| 193 | |||
| 194 | #ifdef CONTAINER_STATS | ||
| 195 | inline_ udword GetNbContainers() const { return mNbContainers; } | ||
| 196 | inline_ udword GetTotalBytes() const { return mUsedRam; } | ||
| 197 | private: | ||
| 198 | |||
| 199 | static udword mNbContainers; //!< Number of containers around | ||
| 200 | static udword mUsedRam; //!< Amount of bytes used by containers in the system | ||
| 201 | #endif | ||
| 202 | private: | ||
| 203 | // Resizing | ||
| 204 | bool Resize(udword needed=1); | ||
| 205 | // Data | ||
| 206 | udword mMaxNbEntries; //!< Maximum possible number of entries | ||
| 207 | udword mCurNbEntries; //!< Current number of entries | ||
| 208 | udword* mEntries; //!< List of entries | ||
| 209 | float mGrowthFactor; //!< Resize: new number of entries = old number * mGrowthFactor | ||
| 210 | }; | ||
| 211 | |||
| 212 | #endif // __ICECONTAINER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceFPU.h b/libraries/ode-0.9/OPCODE/Ice/IceFPU.h new file mode 100644 index 0000000..0cae4cb --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceFPU.h | |||
| @@ -0,0 +1,337 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains FPU related code. | ||
| 4 | * \file IceFPU.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEFPU_H__ | ||
| 13 | #define __ICEFPU_H__ | ||
| 14 | |||
| 15 | #define SIGN_BITMASK 0x80000000 | ||
| 16 | |||
| 17 | //! Integer representation of a floating-point value. | ||
| 18 | #define IR(x) ((udword&)(x)) | ||
| 19 | |||
| 20 | //! Signed integer representation of a floating-point value. | ||
| 21 | #define SIR(x) ((sdword&)(x)) | ||
| 22 | |||
| 23 | //! Absolute integer representation of a floating-point value | ||
| 24 | #define AIR(x) (IR(x)&0x7fffffff) | ||
| 25 | |||
| 26 | //! Floating-point representation of an integer value. | ||
| 27 | #define FR(x) ((float&)(x)) | ||
| 28 | |||
| 29 | //! Integer-based comparison of a floating point value. | ||
| 30 | //! Don't use it blindly, it can be faster or slower than the FPU comparison, depends on the context. | ||
| 31 | #define IS_NEGATIVE_FLOAT(x) (IR(x)&0x80000000) | ||
| 32 | |||
| 33 | //! Fast fabs for floating-point values. It just clears the sign bit. | ||
| 34 | //! Don't use it blindy, it can be faster or slower than the FPU comparison, depends on the context. | ||
| 35 | inline_ float FastFabs(float x) | ||
| 36 | { | ||
| 37 | udword FloatBits = IR(x)&0x7fffffff; | ||
| 38 | return FR(FloatBits); | ||
| 39 | } | ||
| 40 | |||
| 41 | //! Fast square root for floating-point values. | ||
| 42 | inline_ float FastSqrt(float square) | ||
| 43 | { | ||
| 44 | #ifdef _MSC_VER | ||
| 45 | float retval; | ||
| 46 | |||
| 47 | __asm { | ||
| 48 | mov eax, square | ||
| 49 | sub eax, 0x3F800000 | ||
| 50 | sar eax, 1 | ||
| 51 | add eax, 0x3F800000 | ||
| 52 | mov [retval], eax | ||
| 53 | } | ||
| 54 | return retval; | ||
| 55 | #else | ||
| 56 | return sqrt(square); | ||
| 57 | #endif | ||
| 58 | } | ||
| 59 | |||
| 60 | //! Saturates positive to zero. | ||
| 61 | inline_ float fsat(float f) | ||
| 62 | { | ||
| 63 | udword y = (udword&)f & ~((sdword&)f >>31); | ||
| 64 | return (float&)y; | ||
| 65 | } | ||
| 66 | |||
| 67 | //! Computes 1.0f / sqrtf(x). | ||
| 68 | inline_ float frsqrt(float f) | ||
| 69 | { | ||
| 70 | float x = f * 0.5f; | ||
| 71 | udword y = 0x5f3759df - ((udword&)f >> 1); | ||
| 72 | // Iteration... | ||
| 73 | (float&)y = (float&)y * ( 1.5f - ( x * (float&)y * (float&)y ) ); | ||
| 74 | // Result | ||
| 75 | return (float&)y; | ||
| 76 | } | ||
| 77 | |||
| 78 | //! Computes 1.0f / sqrtf(x). Comes from NVIDIA. | ||
| 79 | inline_ float InvSqrt(const float& x) | ||
| 80 | { | ||
| 81 | udword tmp = (udword(IEEE_1_0 << 1) + IEEE_1_0 - *(udword*)&x) >> 1; | ||
| 82 | float y = *(float*)&tmp; | ||
| 83 | return y * (1.47f - 0.47f * x * y * y); | ||
| 84 | } | ||
| 85 | |||
| 86 | //! Computes 1.0f / sqrtf(x). Comes from Quake3. Looks like the first one I had above. | ||
| 87 | //! See http://www.magic-software.com/3DGEDInvSqrt.html | ||
| 88 | inline_ float RSqrt(float number) | ||
| 89 | { | ||
| 90 | long i; | ||
| 91 | float x2, y; | ||
| 92 | const float threehalfs = 1.5f; | ||
| 93 | |||
| 94 | x2 = number * 0.5f; | ||
| 95 | y = number; | ||
| 96 | i = * (long *) &y; | ||
| 97 | i = 0x5f3759df - (i >> 1); | ||
| 98 | y = * (float *) &i; | ||
| 99 | y = y * (threehalfs - (x2 * y * y)); | ||
| 100 | |||
| 101 | return y; | ||
| 102 | } | ||
| 103 | |||
| 104 | //! TO BE DOCUMENTED | ||
| 105 | inline_ float fsqrt(float f) | ||
| 106 | { | ||
| 107 | udword y = ( ( (sdword&)f - 0x3f800000 ) >> 1 ) + 0x3f800000; | ||
| 108 | // Iteration...? | ||
| 109 | // (float&)y = (3.0f - ((float&)y * (float&)y) / f) * (float&)y * 0.5f; | ||
| 110 | // Result | ||
| 111 | return (float&)y; | ||
| 112 | } | ||
| 113 | |||
| 114 | //! Returns the float ranged espilon value. | ||
| 115 | inline_ float fepsilon(float f) | ||
| 116 | { | ||
| 117 | udword b = (udword&)f & 0xff800000; | ||
| 118 | udword a = b | 0x00000001; | ||
| 119 | (float&)a -= (float&)b; | ||
| 120 | // Result | ||
| 121 | return (float&)a; | ||
| 122 | } | ||
| 123 | |||
| 124 | //! Is the float valid ? | ||
| 125 | inline_ bool IsNAN(float value) { return (IR(value)&0x7f800000) == 0x7f800000; } | ||
| 126 | inline_ bool IsIndeterminate(float value) { return IR(value) == 0xffc00000; } | ||
| 127 | inline_ bool IsPlusInf(float value) { return IR(value) == 0x7f800000; } | ||
| 128 | inline_ bool IsMinusInf(float value) { return IR(value) == 0xff800000; } | ||
| 129 | |||
| 130 | inline_ bool IsValidFloat(float value) | ||
| 131 | { | ||
| 132 | if(IsNAN(value)) return false; | ||
| 133 | if(IsIndeterminate(value)) return false; | ||
| 134 | if(IsPlusInf(value)) return false; | ||
| 135 | if(IsMinusInf(value)) return false; | ||
| 136 | return true; | ||
| 137 | } | ||
| 138 | |||
| 139 | #define CHECK_VALID_FLOAT(x) ASSERT(IsValidFloat(x)); | ||
| 140 | |||
| 141 | /* | ||
| 142 | //! FPU precision setting function. | ||
| 143 | inline_ void SetFPU() | ||
| 144 | { | ||
| 145 | // This function evaluates whether the floating-point | ||
| 146 | // control word is set to single precision/round to nearest/ | ||
| 147 | // exceptions disabled. If these conditions don't hold, the | ||
| 148 | // function changes the control word to set them and returns | ||
| 149 | // TRUE, putting the old control word value in the passback | ||
| 150 | // location pointed to by pwOldCW. | ||
| 151 | { | ||
| 152 | uword wTemp, wSave; | ||
| 153 | |||
| 154 | __asm fstcw wSave | ||
| 155 | if (wSave & 0x300 || // Not single mode | ||
| 156 | 0x3f != (wSave & 0x3f) || // Exceptions enabled | ||
| 157 | wSave & 0xC00) // Not round to nearest mode | ||
| 158 | { | ||
| 159 | __asm | ||
| 160 | { | ||
| 161 | mov ax, wSave | ||
| 162 | and ax, not 300h ;; single mode | ||
| 163 | or ax, 3fh ;; disable all exceptions | ||
| 164 | and ax, not 0xC00 ;; round to nearest mode | ||
| 165 | mov wTemp, ax | ||
| 166 | fldcw wTemp | ||
| 167 | } | ||
| 168 | } | ||
| 169 | } | ||
| 170 | } | ||
| 171 | */ | ||
| 172 | //! This function computes the slowest possible floating-point value (you can also directly use FLT_EPSILON) | ||
| 173 | inline_ float ComputeFloatEpsilon() | ||
| 174 | { | ||
| 175 | float f = 1.0f; | ||
| 176 | ((udword&)f)^=1; | ||
| 177 | return f - 1.0f; // You can check it's the same as FLT_EPSILON | ||
| 178 | } | ||
| 179 | |||
| 180 | inline_ bool IsFloatZero(float x, float epsilon=1e-6f) | ||
| 181 | { | ||
| 182 | return x*x < epsilon; | ||
| 183 | } | ||
| 184 | |||
| 185 | #define FCOMI_ST0 _asm _emit 0xdb _asm _emit 0xf0 | ||
| 186 | #define FCOMIP_ST0 _asm _emit 0xdf _asm _emit 0xf0 | ||
| 187 | #define FCMOVB_ST0 _asm _emit 0xda _asm _emit 0xc0 | ||
| 188 | #define FCMOVNB_ST0 _asm _emit 0xdb _asm _emit 0xc0 | ||
| 189 | |||
| 190 | #define FCOMI_ST1 _asm _emit 0xdb _asm _emit 0xf1 | ||
| 191 | #define FCOMIP_ST1 _asm _emit 0xdf _asm _emit 0xf1 | ||
| 192 | #define FCMOVB_ST1 _asm _emit 0xda _asm _emit 0xc1 | ||
| 193 | #define FCMOVNB_ST1 _asm _emit 0xdb _asm _emit 0xc1 | ||
| 194 | |||
| 195 | #define FCOMI_ST2 _asm _emit 0xdb _asm _emit 0xf2 | ||
| 196 | #define FCOMIP_ST2 _asm _emit 0xdf _asm _emit 0xf2 | ||
| 197 | #define FCMOVB_ST2 _asm _emit 0xda _asm _emit 0xc2 | ||
| 198 | #define FCMOVNB_ST2 _asm _emit 0xdb _asm _emit 0xc2 | ||
| 199 | |||
| 200 | #define FCOMI_ST3 _asm _emit 0xdb _asm _emit 0xf3 | ||
| 201 | #define FCOMIP_ST3 _asm _emit 0xdf _asm _emit 0xf3 | ||
| 202 | #define FCMOVB_ST3 _asm _emit 0xda _asm _emit 0xc3 | ||
| 203 | #define FCMOVNB_ST3 _asm _emit 0xdb _asm _emit 0xc3 | ||
| 204 | |||
| 205 | #define FCOMI_ST4 _asm _emit 0xdb _asm _emit 0xf4 | ||
| 206 | #define FCOMIP_ST4 _asm _emit 0xdf _asm _emit 0xf4 | ||
| 207 | #define FCMOVB_ST4 _asm _emit 0xda _asm _emit 0xc4 | ||
| 208 | #define FCMOVNB_ST4 _asm _emit 0xdb _asm _emit 0xc4 | ||
| 209 | |||
| 210 | #define FCOMI_ST5 _asm _emit 0xdb _asm _emit 0xf5 | ||
| 211 | #define FCOMIP_ST5 _asm _emit 0xdf _asm _emit 0xf5 | ||
| 212 | #define FCMOVB_ST5 _asm _emit 0xda _asm _emit 0xc5 | ||
| 213 | #define FCMOVNB_ST5 _asm _emit 0xdb _asm _emit 0xc5 | ||
| 214 | |||
| 215 | #define FCOMI_ST6 _asm _emit 0xdb _asm _emit 0xf6 | ||
| 216 | #define FCOMIP_ST6 _asm _emit 0xdf _asm _emit 0xf6 | ||
| 217 | #define FCMOVB_ST6 _asm _emit 0xda _asm _emit 0xc6 | ||
| 218 | #define FCMOVNB_ST6 _asm _emit 0xdb _asm _emit 0xc6 | ||
| 219 | |||
| 220 | #define FCOMI_ST7 _asm _emit 0xdb _asm _emit 0xf7 | ||
| 221 | #define FCOMIP_ST7 _asm _emit 0xdf _asm _emit 0xf7 | ||
| 222 | #define FCMOVB_ST7 _asm _emit 0xda _asm _emit 0xc7 | ||
| 223 | #define FCMOVNB_ST7 _asm _emit 0xdb _asm _emit 0xc7 | ||
| 224 | |||
| 225 | //! A global function to find MAX(a,b) using FCOMI/FCMOV | ||
| 226 | inline_ float FCMax2(float a, float b) | ||
| 227 | { | ||
| 228 | #ifdef _MSC_VER | ||
| 229 | float Res; | ||
| 230 | _asm fld [a] | ||
| 231 | _asm fld [b] | ||
| 232 | FCOMI_ST1 | ||
| 233 | FCMOVB_ST1 | ||
| 234 | _asm fstp [Res] | ||
| 235 | _asm fcomp | ||
| 236 | return Res; | ||
| 237 | #else | ||
| 238 | return (a > b) ? a : b; | ||
| 239 | #endif | ||
| 240 | } | ||
| 241 | |||
| 242 | //! A global function to find MIN(a,b) using FCOMI/FCMOV | ||
| 243 | inline_ float FCMin2(float a, float b) | ||
| 244 | { | ||
| 245 | #ifdef _MSC_VER | ||
| 246 | float Res; | ||
| 247 | _asm fld [a] | ||
| 248 | _asm fld [b] | ||
| 249 | FCOMI_ST1 | ||
| 250 | FCMOVNB_ST1 | ||
| 251 | _asm fstp [Res] | ||
| 252 | _asm fcomp | ||
| 253 | return Res; | ||
| 254 | #else | ||
| 255 | return (a < b) ? a : b; | ||
| 256 | #endif | ||
| 257 | } | ||
| 258 | |||
| 259 | //! A global function to find MAX(a,b,c) using FCOMI/FCMOV | ||
| 260 | inline_ float FCMax3(float a, float b, float c) | ||
| 261 | { | ||
| 262 | #ifdef _MSC_VER | ||
| 263 | float Res; | ||
| 264 | _asm fld [a] | ||
| 265 | _asm fld [b] | ||
| 266 | _asm fld [c] | ||
| 267 | FCOMI_ST1 | ||
| 268 | FCMOVB_ST1 | ||
| 269 | FCOMI_ST2 | ||
| 270 | FCMOVB_ST2 | ||
| 271 | _asm fstp [Res] | ||
| 272 | _asm fcompp | ||
| 273 | return Res; | ||
| 274 | #else | ||
| 275 | return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c); | ||
| 276 | #endif | ||
| 277 | } | ||
| 278 | |||
| 279 | //! A global function to find MIN(a,b,c) using FCOMI/FCMOV | ||
| 280 | inline_ float FCMin3(float a, float b, float c) | ||
| 281 | { | ||
| 282 | #ifdef _MSC_VER | ||
| 283 | float Res; | ||
| 284 | _asm fld [a] | ||
| 285 | _asm fld [b] | ||
| 286 | _asm fld [c] | ||
| 287 | FCOMI_ST1 | ||
| 288 | FCMOVNB_ST1 | ||
| 289 | FCOMI_ST2 | ||
| 290 | FCMOVNB_ST2 | ||
| 291 | _asm fstp [Res] | ||
| 292 | _asm fcompp | ||
| 293 | return Res; | ||
| 294 | #else | ||
| 295 | return (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c); | ||
| 296 | #endif | ||
| 297 | } | ||
| 298 | |||
| 299 | inline_ int ConvertToSortable(float f) | ||
| 300 | { | ||
| 301 | int& Fi = (int&)f; | ||
| 302 | int Fmask = (Fi>>31); | ||
| 303 | Fi ^= Fmask; | ||
| 304 | Fmask &= ~(1<<31); | ||
| 305 | Fi -= Fmask; | ||
| 306 | return Fi; | ||
| 307 | } | ||
| 308 | |||
| 309 | enum FPUMode | ||
| 310 | { | ||
| 311 | FPU_FLOOR = 0, | ||
| 312 | FPU_CEIL = 1, | ||
| 313 | FPU_BEST = 2, | ||
| 314 | |||
| 315 | FPU_FORCE_DWORD = 0x7fffffff | ||
| 316 | }; | ||
| 317 | |||
| 318 | FUNCTION ICECORE_API FPUMode GetFPUMode(); | ||
| 319 | FUNCTION ICECORE_API void SaveFPU(); | ||
| 320 | FUNCTION ICECORE_API void RestoreFPU(); | ||
| 321 | FUNCTION ICECORE_API void SetFPUFloorMode(); | ||
| 322 | FUNCTION ICECORE_API void SetFPUCeilMode(); | ||
| 323 | FUNCTION ICECORE_API void SetFPUBestMode(); | ||
| 324 | |||
| 325 | FUNCTION ICECORE_API void SetFPUPrecision24(); | ||
| 326 | FUNCTION ICECORE_API void SetFPUPrecision53(); | ||
| 327 | FUNCTION ICECORE_API void SetFPUPrecision64(); | ||
| 328 | FUNCTION ICECORE_API void SetFPURoundingChop(); | ||
| 329 | FUNCTION ICECORE_API void SetFPURoundingUp(); | ||
| 330 | FUNCTION ICECORE_API void SetFPURoundingDown(); | ||
| 331 | FUNCTION ICECORE_API void SetFPURoundingNear(); | ||
| 332 | |||
| 333 | FUNCTION ICECORE_API int intChop(const float& f); | ||
| 334 | FUNCTION ICECORE_API int intFloor(const float& f); | ||
| 335 | FUNCTION ICECORE_API int intCeil(const float& f); | ||
| 336 | |||
| 337 | #endif // __ICEFPU_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceHPoint.cpp b/libraries/ode-0.9/OPCODE/Ice/IceHPoint.cpp new file mode 100644 index 0000000..f806a0c --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceHPoint.cpp | |||
| @@ -0,0 +1,70 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for homogeneous points. | ||
| 4 | * \file IceHPoint.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * Homogeneous point. | ||
| 13 | * | ||
| 14 | * Use it: | ||
| 15 | * - for clipping in homogeneous space (standard way) | ||
| 16 | * - to differentiate between points (w=1) and vectors (w=0). | ||
| 17 | * - in some cases you can also use it instead of Point for padding reasons. | ||
| 18 | * | ||
| 19 | * \class HPoint | ||
| 20 | * \author Pierre Terdiman | ||
| 21 | * \version 1.0 | ||
| 22 | * \warning No cross-product in 4D. | ||
| 23 | * \warning HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4 | ||
| 24 | */ | ||
| 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 26 | |||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | // Precompiled Header | ||
| 29 | #include "Stdafx.h" | ||
| 30 | |||
| 31 | using namespace IceMaths; | ||
| 32 | |||
| 33 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 34 | // Point Mul = HPoint * Matrix3x3; | ||
| 35 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 36 | Point HPoint::operator*(const Matrix3x3& mat) const | ||
| 37 | { | ||
| 38 | return Point( | ||
| 39 | x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0], | ||
| 40 | x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1], | ||
| 41 | x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] ); | ||
| 42 | } | ||
| 43 | |||
| 44 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 45 | // HPoint Mul = HPoint * Matrix4x4; | ||
| 46 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 47 | HPoint HPoint::operator*(const Matrix4x4& mat) const | ||
| 48 | { | ||
| 49 | return HPoint( | ||
| 50 | x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0], | ||
| 51 | x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1], | ||
| 52 | x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2], | ||
| 53 | x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3]); | ||
| 54 | } | ||
| 55 | |||
| 56 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 57 | // HPoint *= Matrix4x4 | ||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | HPoint& HPoint::operator*=(const Matrix4x4& mat) | ||
| 60 | { | ||
| 61 | float xp = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0]; | ||
| 62 | float yp = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1]; | ||
| 63 | float zp = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2]; | ||
| 64 | float wp = x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3]; | ||
| 65 | |||
| 66 | x = xp; y = yp; z = zp; w = wp; | ||
| 67 | |||
| 68 | return *this; | ||
| 69 | } | ||
| 70 | |||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceHPoint.h b/libraries/ode-0.9/OPCODE/Ice/IceHPoint.h new file mode 100644 index 0000000..a3770cd --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceHPoint.h | |||
| @@ -0,0 +1,160 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for homogeneous points. | ||
| 4 | * \file IceHPoint.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEHPOINT_H__ | ||
| 13 | #define __ICEHPOINT_H__ | ||
| 14 | |||
| 15 | class ICEMATHS_API HPoint : public Point | ||
| 16 | { | ||
| 17 | public: | ||
| 18 | |||
| 19 | //! Empty constructor | ||
| 20 | inline_ HPoint() {} | ||
| 21 | //! Constructor from floats | ||
| 22 | inline_ HPoint(float xx, float yy, float zz, float ww=0.0f) : Point(xx, yy, zz), w(ww) {} | ||
| 23 | //! Constructor from array | ||
| 24 | inline_ HPoint(const float f[4]) : Point(f), w(f[3]) {} | ||
| 25 | //! Constructor from a Point | ||
| 26 | inline_ HPoint(const Point& p, float ww=0.0f) : Point(p), w(ww) {} | ||
| 27 | //! Destructor | ||
| 28 | inline_ ~HPoint() {} | ||
| 29 | |||
| 30 | //! Clear the point | ||
| 31 | inline_ HPoint& Zero() { x = y = z = w = 0.0f; return *this; } | ||
| 32 | |||
| 33 | //! Assignment from values | ||
| 34 | inline_ HPoint& Set(float xx, float yy, float zz, float ww ) { x = xx; y = yy; z = zz; w = ww; return *this; } | ||
| 35 | //! Assignment from array | ||
| 36 | inline_ HPoint& Set(const float f[4]) { x = f[X]; y = f[Y]; z = f[Z]; w = f[W]; return *this; } | ||
| 37 | //! Assignment from another h-point | ||
| 38 | inline_ HPoint& Set(const HPoint& src) { x = src.x; y = src.y; z = src.z; w = src.w; return *this; } | ||
| 39 | |||
| 40 | //! Add a vector | ||
| 41 | inline_ HPoint& Add(float xx, float yy, float zz, float ww ) { x += xx; y += yy; z += zz; w += ww; return *this; } | ||
| 42 | //! Add a vector | ||
| 43 | inline_ HPoint& Add(const float f[4]) { x += f[X]; y += f[Y]; z += f[Z]; w += f[W]; return *this; } | ||
| 44 | |||
| 45 | //! Subtract a vector | ||
| 46 | inline_ HPoint& Sub(float xx, float yy, float zz, float ww ) { x -= xx; y -= yy; z -= zz; w -= ww; return *this; } | ||
| 47 | //! Subtract a vector | ||
| 48 | inline_ HPoint& Sub(const float f[4]) { x -= f[X]; y -= f[Y]; z -= f[Z]; w -= f[W]; return *this; } | ||
| 49 | |||
| 50 | //! Multiplies by a scalar | ||
| 51 | inline_ HPoint& Mul(float s) { x *= s; y *= s; z *= s; w *= s; return *this; } | ||
| 52 | |||
| 53 | //! Returns MIN(x, y, z, w); | ||
| 54 | float Min() const { return MIN(x, MIN(y, MIN(z, w))); } | ||
| 55 | //! Returns MAX(x, y, z, w); | ||
| 56 | float Max() const { return MAX(x, MAX(y, MAX(z, w))); } | ||
| 57 | //! Sets each element to be componentwise minimum | ||
| 58 | HPoint& Min(const HPoint& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); w = MIN(w, p.w); return *this; } | ||
| 59 | //! Sets each element to be componentwise maximum | ||
| 60 | HPoint& Max(const HPoint& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); w = MAX(w, p.w); return *this; } | ||
| 61 | |||
| 62 | //! Computes square magnitude | ||
| 63 | inline_ float SquareMagnitude() const { return x*x + y*y + z*z + w*w; } | ||
| 64 | //! Computes magnitude | ||
| 65 | inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z + w*w); } | ||
| 66 | |||
| 67 | //! Normalize the vector | ||
| 68 | inline_ HPoint& Normalize() | ||
| 69 | { | ||
| 70 | float M = Magnitude(); | ||
| 71 | if(M) | ||
| 72 | { | ||
| 73 | M = 1.0f / M; | ||
| 74 | x *= M; | ||
| 75 | y *= M; | ||
| 76 | z *= M; | ||
| 77 | w *= M; | ||
| 78 | } | ||
| 79 | return *this; | ||
| 80 | } | ||
| 81 | |||
| 82 | // Arithmetic operators | ||
| 83 | //! Operator for HPoint Negate = - HPoint; | ||
| 84 | inline_ HPoint operator-() const { return HPoint(-x, -y, -z, -w); } | ||
| 85 | |||
| 86 | //! Operator for HPoint Plus = HPoint + HPoint; | ||
| 87 | inline_ HPoint operator+(const HPoint& p) const { return HPoint(x + p.x, y + p.y, z + p.z, w + p.w); } | ||
| 88 | //! Operator for HPoint Minus = HPoint - HPoint; | ||
| 89 | inline_ HPoint operator-(const HPoint& p) const { return HPoint(x - p.x, y - p.y, z - p.z, w - p.w); } | ||
| 90 | |||
| 91 | //! Operator for HPoint Mul = HPoint * HPoint; | ||
| 92 | inline_ HPoint operator*(const HPoint& p) const { return HPoint(x * p.x, y * p.y, z * p.z, w * p.w); } | ||
| 93 | //! Operator for HPoint Scale = HPoint * float; | ||
| 94 | inline_ HPoint operator*(float s) const { return HPoint(x * s, y * s, z * s, w * s); } | ||
| 95 | //! Operator for HPoint Scale = float * HPoint; | ||
| 96 | inline_ friend HPoint operator*(float s, const HPoint& p) { return HPoint(s * p.x, s * p.y, s * p.z, s * p.w); } | ||
| 97 | |||
| 98 | //! Operator for HPoint Div = HPoint / HPoint; | ||
| 99 | inline_ HPoint operator/(const HPoint& p) const { return HPoint(x / p.x, y / p.y, z / p.z, w / p.w); } | ||
| 100 | //! Operator for HPoint Scale = HPoint / float; | ||
| 101 | inline_ HPoint operator/(float s) const { s = 1.0f / s; return HPoint(x * s, y * s, z * s, w * s); } | ||
| 102 | //! Operator for HPoint Scale = float / HPoint; | ||
| 103 | inline_ friend HPoint operator/(float s, const HPoint& p) { return HPoint(s / p.x, s / p.y, s / p.z, s / p.w); } | ||
| 104 | |||
| 105 | //! Operator for float DotProd = HPoint | HPoint; | ||
| 106 | inline_ float operator|(const HPoint& p) const { return x*p.x + y*p.y + z*p.z + w*p.w; } | ||
| 107 | // No cross-product in 4D | ||
| 108 | |||
| 109 | //! Operator for HPoint += HPoint; | ||
| 110 | inline_ HPoint& operator+=(const HPoint& p) { x += p.x; y += p.y; z += p.z; w += p.w; return *this; } | ||
| 111 | //! Operator for HPoint += float; | ||
| 112 | inline_ HPoint& operator+=(float s) { x += s; y += s; z += s; w += s; return *this; } | ||
| 113 | |||
| 114 | //! Operator for HPoint -= HPoint; | ||
| 115 | inline_ HPoint& operator-=(const HPoint& p) { x -= p.x; y -= p.y; z -= p.z; w -= p.w; return *this; } | ||
| 116 | //! Operator for HPoint -= float; | ||
| 117 | inline_ HPoint& operator-=(float s) { x -= s; y -= s; z -= s; w -= s; return *this; } | ||
| 118 | |||
| 119 | //! Operator for HPoint *= HPoint; | ||
| 120 | inline_ HPoint& operator*=(const HPoint& p) { x *= p.x; y *= p.y; z *= p.z; w *= p.w; return *this; } | ||
| 121 | //! Operator for HPoint *= float; | ||
| 122 | inline_ HPoint& operator*=(float s) { x*=s; y*=s; z*=s; w*=s; return *this; } | ||
| 123 | |||
| 124 | //! Operator for HPoint /= HPoint; | ||
| 125 | inline_ HPoint& operator/=(const HPoint& p) { x /= p.x; y /= p.y; z /= p.z; w /= p.w; return *this; } | ||
| 126 | //! Operator for HPoint /= float; | ||
| 127 | inline_ HPoint& operator/=(float s) { s = 1.0f / s; x*=s; y*=s; z*=s; w*=s; return *this; } | ||
| 128 | |||
| 129 | // Arithmetic operators | ||
| 130 | |||
| 131 | //! Operator for Point Mul = HPoint * Matrix3x3; | ||
| 132 | Point operator*(const Matrix3x3& mat) const; | ||
| 133 | //! Operator for HPoint Mul = HPoint * Matrix4x4; | ||
| 134 | HPoint operator*(const Matrix4x4& mat) const; | ||
| 135 | |||
| 136 | // HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4 | ||
| 137 | //! Operator for HPoint *= Matrix4x4 | ||
| 138 | HPoint& operator*=(const Matrix4x4& mat); | ||
| 139 | |||
| 140 | // Logical operators | ||
| 141 | |||
| 142 | //! Operator for "if(HPoint==HPoint)" | ||
| 143 | inline_ bool operator==(const HPoint& p) const { return ( (x==p.x)&&(y==p.y)&&(z==p.z)&&(w==p.w)); } | ||
| 144 | //! Operator for "if(HPoint!=HPoint)" | ||
| 145 | inline_ bool operator!=(const HPoint& p) const { return ( (x!=p.x)||(y!=p.y)||(z!=p.z)||(w!=p.w)); } | ||
| 146 | |||
| 147 | // Cast operators | ||
| 148 | |||
| 149 | //! Cast a HPoint to a Point. w is discarded. | ||
| 150 | #ifdef _MSC_VER | ||
| 151 | inline_ operator Point() const { return Point(x, y, z); } | ||
| 152 | // gcc complains that conversion to a base class will never use a type conversion operator | ||
| 153 | #endif | ||
| 154 | |||
| 155 | public: | ||
| 156 | float w; | ||
| 157 | }; | ||
| 158 | |||
| 159 | #endif // __ICEHPOINT_H__ | ||
| 160 | |||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceIndexedTriangle.cpp b/libraries/ode-0.9/OPCODE/Ice/IceIndexedTriangle.cpp new file mode 100644 index 0000000..db279c4 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceIndexedTriangle.cpp | |||
| @@ -0,0 +1,548 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains a handy indexed triangle class. | ||
| 4 | * \file IceIndexedTriangle.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 17, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Precompiled Header | ||
| 12 | #include "Stdafx.h" | ||
| 13 | |||
| 14 | using namespace IceMaths; | ||
| 15 | |||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | /** | ||
| 18 | * Contains an indexed triangle class. | ||
| 19 | * | ||
| 20 | * \class Triangle | ||
| 21 | * \author Pierre Terdiman | ||
| 22 | * \version 1.0 | ||
| 23 | * \date 08.15.98 | ||
| 24 | */ | ||
| 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 26 | |||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | /** | ||
| 29 | * Flips the winding order. | ||
| 30 | */ | ||
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 32 | void IndexedTriangle::Flip() | ||
| 33 | { | ||
| 34 | Swap(mVRef[1], mVRef[2]); | ||
| 35 | } | ||
| 36 | |||
| 37 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 38 | /** | ||
| 39 | * Computes the triangle area. | ||
| 40 | * \param verts [in] the list of indexed vertices | ||
| 41 | * \return the area | ||
| 42 | */ | ||
| 43 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 44 | float IndexedTriangle::Area(const Point* verts) const | ||
| 45 | { | ||
| 46 | if(!verts) return 0.0f; | ||
| 47 | const Point& p0 = verts[0]; | ||
| 48 | const Point& p1 = verts[1]; | ||
| 49 | const Point& p2 = verts[2]; | ||
| 50 | return ((p0-p1)^(p0-p2)).Magnitude() * 0.5f; | ||
| 51 | } | ||
| 52 | |||
| 53 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 54 | /** | ||
| 55 | * Computes the triangle perimeter. | ||
| 56 | * \param verts [in] the list of indexed vertices | ||
| 57 | * \return the perimeter | ||
| 58 | */ | ||
| 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 60 | float IndexedTriangle::Perimeter(const Point* verts) const | ||
| 61 | { | ||
| 62 | if(!verts) return 0.0f; | ||
| 63 | const Point& p0 = verts[0]; | ||
| 64 | const Point& p1 = verts[1]; | ||
| 65 | const Point& p2 = verts[2]; | ||
| 66 | return p0.Distance(p1) | ||
| 67 | + p0.Distance(p2) | ||
| 68 | + p1.Distance(p2); | ||
| 69 | } | ||
| 70 | |||
| 71 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 72 | /** | ||
| 73 | * Computes the triangle compacity. | ||
| 74 | * \param verts [in] the list of indexed vertices | ||
| 75 | * \return the compacity | ||
| 76 | */ | ||
| 77 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 78 | float IndexedTriangle::Compacity(const Point* verts) const | ||
| 79 | { | ||
| 80 | if(!verts) return 0.0f; | ||
| 81 | float P = Perimeter(verts); | ||
| 82 | if(P==0.0f) return 0.0f; | ||
| 83 | return (4.0f*PI*Area(verts)/(P*P)); | ||
| 84 | } | ||
| 85 | |||
| 86 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 87 | /** | ||
| 88 | * Computes the triangle normal. | ||
| 89 | * \param verts [in] the list of indexed vertices | ||
| 90 | * \param normal [out] the computed normal | ||
| 91 | */ | ||
| 92 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 93 | void IndexedTriangle::Normal(const Point* verts, Point& normal) const | ||
| 94 | { | ||
| 95 | if(!verts) return; | ||
| 96 | |||
| 97 | const Point& p0 = verts[mVRef[0]]; | ||
| 98 | const Point& p1 = verts[mVRef[1]]; | ||
| 99 | const Point& p2 = verts[mVRef[2]]; | ||
| 100 | normal = ((p2-p1)^(p0-p1)).Normalize(); | ||
| 101 | } | ||
| 102 | |||
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 104 | /** | ||
| 105 | * Computes the triangle denormalized normal. | ||
| 106 | * \param verts [in] the list of indexed vertices | ||
| 107 | * \param normal [out] the computed normal | ||
| 108 | */ | ||
| 109 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 110 | void IndexedTriangle::DenormalizedNormal(const Point* verts, Point& normal) const | ||
| 111 | { | ||
| 112 | if(!verts) return; | ||
| 113 | |||
| 114 | const Point& p0 = verts[mVRef[0]]; | ||
| 115 | const Point& p1 = verts[mVRef[1]]; | ||
| 116 | const Point& p2 = verts[mVRef[2]]; | ||
| 117 | normal = ((p2-p1)^(p0-p1)); | ||
| 118 | } | ||
| 119 | |||
| 120 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 121 | /** | ||
| 122 | * Computes the triangle center. | ||
| 123 | * \param verts [in] the list of indexed vertices | ||
| 124 | * \param center [out] the computed center | ||
| 125 | */ | ||
| 126 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 127 | void IndexedTriangle::Center(const Point* verts, Point& center) const | ||
| 128 | { | ||
| 129 | if(!verts) return; | ||
| 130 | |||
| 131 | const Point& p0 = verts[mVRef[0]]; | ||
| 132 | const Point& p1 = verts[mVRef[1]]; | ||
| 133 | const Point& p2 = verts[mVRef[2]]; | ||
| 134 | center = (p0+p1+p2)*INV3; | ||
| 135 | } | ||
| 136 | |||
| 137 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 138 | /** | ||
| 139 | * Computes the centered normal | ||
| 140 | * \param verts [in] the list of indexed vertices | ||
| 141 | * \param normal [out] the computed centered normal | ||
| 142 | */ | ||
| 143 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 144 | void IndexedTriangle::CenteredNormal(const Point* verts, Point& normal) const | ||
| 145 | { | ||
| 146 | if(!verts) return; | ||
| 147 | |||
| 148 | const Point& p0 = verts[mVRef[0]]; | ||
| 149 | const Point& p1 = verts[mVRef[1]]; | ||
| 150 | const Point& p2 = verts[mVRef[2]]; | ||
| 151 | Point Center = (p0+p1+p2)*INV3; | ||
| 152 | normal = Center + ((p2-p1)^(p0-p1)).Normalize(); | ||
| 153 | } | ||
| 154 | |||
| 155 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 156 | /** | ||
| 157 | * Computes a random point within the triangle. | ||
| 158 | * \param verts [in] the list of indexed vertices | ||
| 159 | * \param normal [out] the computed centered normal | ||
| 160 | */ | ||
| 161 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 162 | void IndexedTriangle::RandomPoint(const Point* verts, Point& random) const | ||
| 163 | { | ||
| 164 | if(!verts) return; | ||
| 165 | |||
| 166 | // Random barycentric coords | ||
| 167 | float Alpha = UnitRandomFloat(); | ||
| 168 | float Beta = UnitRandomFloat(); | ||
| 169 | float Gamma = UnitRandomFloat(); | ||
| 170 | float OneOverTotal = 1.0f / (Alpha + Beta + Gamma); | ||
| 171 | Alpha *= OneOverTotal; | ||
| 172 | Beta *= OneOverTotal; | ||
| 173 | Gamma *= OneOverTotal; | ||
| 174 | |||
| 175 | const Point& p0 = verts[mVRef[0]]; | ||
| 176 | const Point& p1 = verts[mVRef[1]]; | ||
| 177 | const Point& p2 = verts[mVRef[2]]; | ||
| 178 | random = Alpha*p0 + Beta*p1 + Gamma*p2; | ||
| 179 | } | ||
| 180 | |||
| 181 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 182 | /** | ||
| 183 | * Computes backface culling. | ||
| 184 | * \param verts [in] the list of indexed vertices | ||
| 185 | * \param source [in] source point (in local space) from which culling must be computed | ||
| 186 | * \return true if the triangle is visible from the source point | ||
| 187 | */ | ||
| 188 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 189 | bool IndexedTriangle::IsVisible(const Point* verts, const Point& source) const | ||
| 190 | { | ||
| 191 | // Checkings | ||
| 192 | if(!verts) return false; | ||
| 193 | |||
| 194 | const Point& p0 = verts[mVRef[0]]; | ||
| 195 | const Point& p1 = verts[mVRef[1]]; | ||
| 196 | const Point& p2 = verts[mVRef[2]]; | ||
| 197 | |||
| 198 | // Compute denormalized normal | ||
| 199 | Point Normal = (p2 - p1)^(p0 - p1); | ||
| 200 | |||
| 201 | // Backface culling | ||
| 202 | return (Normal | source) >= 0.0f; | ||
| 203 | |||
| 204 | // Same as: | ||
| 205 | // Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]); | ||
| 206 | // return PL.Distance(source) > PL.d; | ||
| 207 | } | ||
| 208 | |||
| 209 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 210 | /** | ||
| 211 | * Computes backface culling. | ||
| 212 | * \param verts [in] the list of indexed vertices | ||
| 213 | * \param source [in] source point (in local space) from which culling must be computed | ||
| 214 | * \return true if the triangle is visible from the source point | ||
| 215 | */ | ||
| 216 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 217 | bool IndexedTriangle::BackfaceCulling(const Point* verts, const Point& source) const | ||
| 218 | { | ||
| 219 | // Checkings | ||
| 220 | if(!verts) return false; | ||
| 221 | |||
| 222 | const Point& p0 = verts[mVRef[0]]; | ||
| 223 | const Point& p1 = verts[mVRef[1]]; | ||
| 224 | const Point& p2 = verts[mVRef[2]]; | ||
| 225 | |||
| 226 | // Compute base | ||
| 227 | // Point Base = (p0 + p1 + p2)*INV3; | ||
| 228 | |||
| 229 | // Compute denormalized normal | ||
| 230 | Point Normal = (p2 - p1)^(p0 - p1); | ||
| 231 | |||
| 232 | // Backface culling | ||
| 233 | // return (Normal | (source - Base)) >= 0.0f; | ||
| 234 | return (Normal | (source - p0)) >= 0.0f; | ||
| 235 | |||
| 236 | // Same as: (but a bit faster) | ||
| 237 | // Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]); | ||
| 238 | // return PL.Distance(source)>0.0f; | ||
| 239 | } | ||
| 240 | |||
| 241 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 242 | /** | ||
| 243 | * Computes the occlusion potential of the triangle. | ||
| 244 | * \param verts [in] the list of indexed vertices | ||
| 245 | * \param source [in] source point (in local space) from which occlusion potential must be computed | ||
| 246 | * \return the occlusion potential | ||
| 247 | */ | ||
| 248 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 249 | float IndexedTriangle::ComputeOcclusionPotential(const Point* verts, const Point& view) const | ||
| 250 | { | ||
| 251 | if(!verts) return 0.0f; | ||
| 252 | // Occlusion potential: -(A * (N|V) / d^2) | ||
| 253 | // A = polygon area | ||
| 254 | // N = polygon normal | ||
| 255 | // V = view vector | ||
| 256 | // d = distance viewpoint-center of polygon | ||
| 257 | |||
| 258 | float A = Area(verts); | ||
| 259 | Point N; Normal(verts, N); | ||
| 260 | Point C; Center(verts, C); | ||
| 261 | float d = view.Distance(C); | ||
| 262 | return -(A*(N|view))/(d*d); | ||
| 263 | } | ||
| 264 | |||
| 265 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 266 | /** | ||
| 267 | * Replaces a vertex reference with another one. | ||
| 268 | * \param oldref [in] the vertex reference to replace | ||
| 269 | * \param newref [in] the new vertex reference | ||
| 270 | * \return true if success, else false if the input vertex reference doesn't belong to the triangle | ||
| 271 | */ | ||
| 272 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 273 | bool IndexedTriangle::ReplaceVertex(udword oldref, udword newref) | ||
| 274 | { | ||
| 275 | if(mVRef[0]==oldref) { mVRef[0] = newref; return true; } | ||
| 276 | else if(mVRef[1]==oldref) { mVRef[1] = newref; return true; } | ||
| 277 | else if(mVRef[2]==oldref) { mVRef[2] = newref; return true; } | ||
| 278 | return false; | ||
| 279 | } | ||
| 280 | |||
| 281 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 282 | /** | ||
| 283 | * Checks whether the triangle is degenerate or not. A degenerate triangle has two common vertex references. This is a zero-area triangle. | ||
| 284 | * \return true if the triangle is degenerate | ||
| 285 | */ | ||
| 286 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 287 | bool IndexedTriangle::IsDegenerate() const | ||
| 288 | { | ||
| 289 | if(mVRef[0]==mVRef[1]) return true; | ||
| 290 | if(mVRef[1]==mVRef[2]) return true; | ||
| 291 | if(mVRef[2]==mVRef[0]) return true; | ||
| 292 | return false; | ||
| 293 | } | ||
| 294 | |||
| 295 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 296 | /** | ||
| 297 | * Checks whether the input vertex reference belongs to the triangle or not. | ||
| 298 | * \param ref [in] the vertex reference to look for | ||
| 299 | * \return true if the triangle contains the vertex reference | ||
| 300 | */ | ||
| 301 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 302 | bool IndexedTriangle::HasVertex(udword ref) const | ||
| 303 | { | ||
| 304 | if(mVRef[0]==ref) return true; | ||
| 305 | if(mVRef[1]==ref) return true; | ||
| 306 | if(mVRef[2]==ref) return true; | ||
| 307 | return false; | ||
| 308 | } | ||
| 309 | |||
| 310 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 311 | /** | ||
| 312 | * Checks whether the input vertex reference belongs to the triangle or not. | ||
| 313 | * \param ref [in] the vertex reference to look for | ||
| 314 | * \param index [out] the corresponding index in the triangle | ||
| 315 | * \return true if the triangle contains the vertex reference | ||
| 316 | */ | ||
| 317 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 318 | bool IndexedTriangle::HasVertex(udword ref, udword* index) const | ||
| 319 | { | ||
| 320 | if(mVRef[0]==ref) { *index = 0; return true; } | ||
| 321 | if(mVRef[1]==ref) { *index = 1; return true; } | ||
| 322 | if(mVRef[2]==ref) { *index = 2; return true; } | ||
| 323 | return false; | ||
| 324 | } | ||
| 325 | |||
| 326 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 327 | /** | ||
| 328 | * Finds an edge in a tri, given two vertex references. | ||
| 329 | * \param vref0 [in] the edge's first vertex reference | ||
| 330 | * \param vref1 [in] the edge's second vertex reference | ||
| 331 | * \return the edge number between 0 and 2, or 0xff if input refs are wrong. | ||
| 332 | */ | ||
| 333 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 334 | ubyte IndexedTriangle::FindEdge(udword vref0, udword vref1) const | ||
| 335 | { | ||
| 336 | if(mVRef[0]==vref0 && mVRef[1]==vref1) return 0; | ||
| 337 | else if(mVRef[0]==vref1 && mVRef[1]==vref0) return 0; | ||
| 338 | else if(mVRef[0]==vref0 && mVRef[2]==vref1) return 1; | ||
| 339 | else if(mVRef[0]==vref1 && mVRef[2]==vref0) return 1; | ||
| 340 | else if(mVRef[1]==vref0 && mVRef[2]==vref1) return 2; | ||
| 341 | else if(mVRef[1]==vref1 && mVRef[2]==vref0) return 2; | ||
| 342 | return 0xff; | ||
| 343 | } | ||
| 344 | |||
| 345 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 346 | /** | ||
| 347 | * Gets the last reference given the first two. | ||
| 348 | * \param vref0 [in] the first vertex reference | ||
| 349 | * \param vref1 [in] the second vertex reference | ||
| 350 | * \return the last reference, or INVALID_ID if input refs are wrong. | ||
| 351 | */ | ||
| 352 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 353 | udword IndexedTriangle::OppositeVertex(udword vref0, udword vref1) const | ||
| 354 | { | ||
| 355 | if(mVRef[0]==vref0 && mVRef[1]==vref1) return mVRef[2]; | ||
| 356 | else if(mVRef[0]==vref1 && mVRef[1]==vref0) return mVRef[2]; | ||
| 357 | else if(mVRef[0]==vref0 && mVRef[2]==vref1) return mVRef[1]; | ||
| 358 | else if(mVRef[0]==vref1 && mVRef[2]==vref0) return mVRef[1]; | ||
| 359 | else if(mVRef[1]==vref0 && mVRef[2]==vref1) return mVRef[0]; | ||
| 360 | else if(mVRef[1]==vref1 && mVRef[2]==vref0) return mVRef[0]; | ||
| 361 | return INVALID_ID; | ||
| 362 | } | ||
| 363 | |||
| 364 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 365 | /** | ||
| 366 | * Gets the three sorted vertex references according to an edge number. | ||
| 367 | * edgenb = 0 => edge 0-1, returns references 0, 1, 2 | ||
| 368 | * edgenb = 1 => edge 0-2, returns references 0, 2, 1 | ||
| 369 | * edgenb = 2 => edge 1-2, returns references 1, 2, 0 | ||
| 370 | * | ||
| 371 | * \param edgenb [in] the edge number, 0, 1 or 2 | ||
| 372 | * \param vref0 [out] the returned first vertex reference | ||
| 373 | * \param vref1 [out] the returned second vertex reference | ||
| 374 | * \param vref2 [out] the returned third vertex reference | ||
| 375 | */ | ||
| 376 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 377 | void IndexedTriangle::GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2) const | ||
| 378 | { | ||
| 379 | if(edgenb==0) | ||
| 380 | { | ||
| 381 | vref0 = mVRef[0]; | ||
| 382 | vref1 = mVRef[1]; | ||
| 383 | vref2 = mVRef[2]; | ||
| 384 | } | ||
| 385 | else if(edgenb==1) | ||
| 386 | { | ||
| 387 | vref0 = mVRef[0]; | ||
| 388 | vref1 = mVRef[2]; | ||
| 389 | vref2 = mVRef[1]; | ||
| 390 | } | ||
| 391 | else if(edgenb==2) | ||
| 392 | { | ||
| 393 | vref0 = mVRef[1]; | ||
| 394 | vref1 = mVRef[2]; | ||
| 395 | vref2 = mVRef[0]; | ||
| 396 | } | ||
| 397 | } | ||
| 398 | |||
| 399 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 400 | /** | ||
| 401 | * Computes the triangle's smallest edge length. | ||
| 402 | * \param verts [in] the list of indexed vertices | ||
| 403 | * \return the smallest edge length | ||
| 404 | */ | ||
| 405 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 406 | float IndexedTriangle::MinEdgeLength(const Point* verts) const | ||
| 407 | { | ||
| 408 | if(!verts) return 0.0f; | ||
| 409 | |||
| 410 | float Min = MAX_FLOAT; | ||
| 411 | float Length01 = verts[0].Distance(verts[1]); | ||
| 412 | float Length02 = verts[0].Distance(verts[2]); | ||
| 413 | float Length12 = verts[1].Distance(verts[2]); | ||
| 414 | if(Length01 < Min) Min = Length01; | ||
| 415 | if(Length02 < Min) Min = Length02; | ||
| 416 | if(Length12 < Min) Min = Length12; | ||
| 417 | return Min; | ||
| 418 | } | ||
| 419 | |||
| 420 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 421 | /** | ||
| 422 | * Computes the triangle's largest edge length. | ||
| 423 | * \param verts [in] the list of indexed vertices | ||
| 424 | * \return the largest edge length | ||
| 425 | */ | ||
| 426 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 427 | float IndexedTriangle::MaxEdgeLength(const Point* verts) const | ||
| 428 | { | ||
| 429 | if(!verts) return 0.0f; | ||
| 430 | |||
| 431 | float Max = MIN_FLOAT; | ||
| 432 | float Length01 = verts[0].Distance(verts[1]); | ||
| 433 | float Length02 = verts[0].Distance(verts[2]); | ||
| 434 | float Length12 = verts[1].Distance(verts[2]); | ||
| 435 | if(Length01 > Max) Max = Length01; | ||
| 436 | if(Length02 > Max) Max = Length02; | ||
| 437 | if(Length12 > Max) Max = Length12; | ||
| 438 | return Max; | ||
| 439 | } | ||
| 440 | |||
| 441 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 442 | /** | ||
| 443 | * Computes a point on the triangle according to the stabbing information. | ||
| 444 | * \param verts [in] the list of indexed vertices | ||
| 445 | * \param u,v [in] point's barycentric coordinates | ||
| 446 | * \param pt [out] point on triangle | ||
| 447 | * \param nearvtx [out] index of nearest vertex | ||
| 448 | */ | ||
| 449 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 450 | void IndexedTriangle::ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx) const | ||
| 451 | { | ||
| 452 | // Checkings | ||
| 453 | if(!verts) return; | ||
| 454 | |||
| 455 | // Get face in local or global space | ||
| 456 | const Point& p0 = verts[mVRef[0]]; | ||
| 457 | const Point& p1 = verts[mVRef[1]]; | ||
| 458 | const Point& p2 = verts[mVRef[2]]; | ||
| 459 | |||
| 460 | // Compute point coordinates | ||
| 461 | pt = (1.0f - u - v)*p0 + u*p1 + v*p2; | ||
| 462 | |||
| 463 | // Compute nearest vertex if needed | ||
| 464 | if(nearvtx) | ||
| 465 | { | ||
| 466 | // Compute distance vector | ||
| 467 | Point d(p0.SquareDistance(pt), // Distance^2 from vertex 0 to point on the face | ||
| 468 | p1.SquareDistance(pt), // Distance^2 from vertex 1 to point on the face | ||
| 469 | p2.SquareDistance(pt)); // Distance^2 from vertex 2 to point on the face | ||
| 470 | |||
| 471 | // Get smallest distance | ||
| 472 | *nearvtx = mVRef[d.SmallestAxis()]; | ||
| 473 | } | ||
| 474 | } | ||
| 475 | |||
| 476 | //************************************** | ||
| 477 | // Angle between two vectors (in radians) | ||
| 478 | // we use this formula | ||
| 479 | // uv = |u||v| cos(u,v) | ||
| 480 | // u ^ v = w | ||
| 481 | // |w| = |u||v| |sin(u,v)| | ||
| 482 | //************************************** | ||
| 483 | float Angle(const Point& u, const Point& v) | ||
| 484 | { | ||
| 485 | float NormU = u.Magnitude(); // |u| | ||
| 486 | float NormV = v.Magnitude(); // |v| | ||
| 487 | float Product = NormU*NormV; // |u||v| | ||
| 488 | if(Product==0.0f) return 0.0f; | ||
| 489 | float OneOverProduct = 1.0f / Product; | ||
| 490 | |||
| 491 | // Cosinus | ||
| 492 | float Cosinus = (u|v) * OneOverProduct; | ||
| 493 | |||
| 494 | // Sinus | ||
| 495 | Point w = u^v; | ||
| 496 | float NormW = w.Magnitude(); | ||
| 497 | |||
| 498 | float AbsSinus = NormW * OneOverProduct; | ||
| 499 | |||
| 500 | // Remove degeneracy | ||
| 501 | if(AbsSinus > 1.0f) AbsSinus = 1.0f; | ||
| 502 | if(AbsSinus < -1.0f) AbsSinus = -1.0f; | ||
| 503 | |||
| 504 | if(Cosinus>=0.0f) return asinf(AbsSinus); | ||
| 505 | else return (PI-asinf(AbsSinus)); | ||
| 506 | } | ||
| 507 | |||
| 508 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 509 | /** | ||
| 510 | * Computes the angle between two triangles. | ||
| 511 | * \param tri [in] the other triangle | ||
| 512 | * \param verts [in] the list of indexed vertices | ||
| 513 | * \return the angle in radians | ||
| 514 | */ | ||
| 515 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 516 | float IndexedTriangle::Angle(const IndexedTriangle& tri, const Point* verts) const | ||
| 517 | { | ||
| 518 | // Checkings | ||
| 519 | if(!verts) return 0.0f; | ||
| 520 | |||
| 521 | // Compute face normals | ||
| 522 | Point n0, n1; | ||
| 523 | Normal(verts, n0); | ||
| 524 | tri.Normal(verts, n1); | ||
| 525 | |||
| 526 | // Compute angle | ||
| 527 | float dp = n0|n1; | ||
| 528 | if(dp>1.0f) return 0.0f; | ||
| 529 | if(dp<-1.0f) return PI; | ||
| 530 | return acosf(dp); | ||
| 531 | |||
| 532 | // return ::Angle(n0,n1); | ||
| 533 | } | ||
| 534 | |||
| 535 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 536 | /** | ||
| 537 | * Checks a triangle is the same as another one. | ||
| 538 | * \param tri [in] the other triangle | ||
| 539 | * \return true if same triangle | ||
| 540 | */ | ||
| 541 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 542 | bool IndexedTriangle::Equal(const IndexedTriangle& tri) const | ||
| 543 | { | ||
| 544 | // Test all vertex references | ||
| 545 | return (HasVertex(tri.mVRef[0]) && | ||
| 546 | HasVertex(tri.mVRef[1]) && | ||
| 547 | HasVertex(tri.mVRef[2])); | ||
| 548 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceIndexedTriangle.h b/libraries/ode-0.9/OPCODE/Ice/IceIndexedTriangle.h new file mode 100644 index 0000000..b34c485 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceIndexedTriangle.h | |||
| @@ -0,0 +1,72 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains a handy indexed triangle class. | ||
| 4 | * \file IceIndexedTriangle.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 17, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEINDEXEDTRIANGLE_H__ | ||
| 13 | #define __ICEINDEXEDTRIANGLE_H__ | ||
| 14 | |||
| 15 | // Forward declarations | ||
| 16 | #ifdef _MSC_VER | ||
| 17 | enum CubeIndex; | ||
| 18 | #else | ||
| 19 | typedef int CubeIndex; | ||
| 20 | #endif | ||
| 21 | |||
| 22 | // An indexed triangle class. | ||
| 23 | class ICEMATHS_API IndexedTriangle | ||
| 24 | { | ||
| 25 | public: | ||
| 26 | //! Constructor | ||
| 27 | inline_ IndexedTriangle() {} | ||
| 28 | //! Constructor | ||
| 29 | inline_ IndexedTriangle(udword r0, udword r1, udword r2) { mVRef[0]=r0; mVRef[1]=r1; mVRef[2]=r2; } | ||
| 30 | //! Copy constructor | ||
| 31 | inline_ IndexedTriangle(const IndexedTriangle& triangle) | ||
| 32 | { | ||
| 33 | mVRef[0] = triangle.mVRef[0]; | ||
| 34 | mVRef[1] = triangle.mVRef[1]; | ||
| 35 | mVRef[2] = triangle.mVRef[2]; | ||
| 36 | } | ||
| 37 | //! Destructor | ||
| 38 | inline_ ~IndexedTriangle() {} | ||
| 39 | //! Vertex-references | ||
| 40 | udword mVRef[3]; | ||
| 41 | |||
| 42 | // Methods | ||
| 43 | void Flip(); | ||
| 44 | float Area(const Point* verts) const; | ||
| 45 | float Perimeter(const Point* verts) const; | ||
| 46 | float Compacity(const Point* verts) const; | ||
| 47 | void Normal(const Point* verts, Point& normal) const; | ||
| 48 | void DenormalizedNormal(const Point* verts, Point& normal) const; | ||
| 49 | void Center(const Point* verts, Point& center) const; | ||
| 50 | void CenteredNormal(const Point* verts, Point& normal) const; | ||
| 51 | void RandomPoint(const Point* verts, Point& random) const; | ||
| 52 | bool IsVisible(const Point* verts, const Point& source) const; | ||
| 53 | bool BackfaceCulling(const Point* verts, const Point& source) const; | ||
| 54 | float ComputeOcclusionPotential(const Point* verts, const Point& view) const; | ||
| 55 | bool ReplaceVertex(udword oldref, udword newref); | ||
| 56 | bool IsDegenerate() const; | ||
| 57 | bool HasVertex(udword ref) const; | ||
| 58 | bool HasVertex(udword ref, udword* index) const; | ||
| 59 | ubyte FindEdge(udword vref0, udword vref1) const; | ||
| 60 | udword OppositeVertex(udword vref0, udword vref1) const; | ||
| 61 | inline_ udword OppositeVertex(ubyte edgenb) const { return mVRef[2-edgenb]; } | ||
| 62 | void GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2) const; | ||
| 63 | float MinEdgeLength(const Point* verts) const; | ||
| 64 | float MaxEdgeLength(const Point* verts) const; | ||
| 65 | void ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx=null) const; | ||
| 66 | float Angle(const IndexedTriangle& tri, const Point* verts) const; | ||
| 67 | inline_ Plane PlaneEquation(const Point* verts) const { return Plane(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]); } | ||
| 68 | bool Equal(const IndexedTriangle& tri) const; | ||
| 69 | CubeIndex ComputeCubeIndex(const Point* verts) const; | ||
| 70 | }; | ||
| 71 | |||
| 72 | #endif // __ICEINDEXEDTRIANGLE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceLSS.h b/libraries/ode-0.9/OPCODE/Ice/IceLSS.h new file mode 100644 index 0000000..bd260c1 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceLSS.h | |||
| @@ -0,0 +1,75 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for line-swept spheres. | ||
| 4 | * \file IceLSS.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICELSS_H__ | ||
| 13 | #define __ICELSS_H__ | ||
| 14 | |||
| 15 | class ICEMATHS_API LSS : public Segment | ||
| 16 | { | ||
| 17 | public: | ||
| 18 | //! Constructor | ||
| 19 | inline_ LSS() {} | ||
| 20 | //! Constructor | ||
| 21 | inline_ LSS(const Segment& seg, float radius) : Segment(seg), mRadius(radius) {} | ||
| 22 | //! Destructor | ||
| 23 | inline_ ~LSS() {} | ||
| 24 | |||
| 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 26 | /** | ||
| 27 | * Computes an OBB surrounding the LSS. | ||
| 28 | * \param box [out] the OBB | ||
| 29 | */ | ||
| 30 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 31 | void ComputeOBB(OBB& box); | ||
| 32 | |||
| 33 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 34 | /** | ||
| 35 | * Tests if a point is contained within the LSS. | ||
| 36 | * \param pt [in] the point to test | ||
| 37 | * \return true if inside the LSS | ||
| 38 | * \warning point and LSS must be in same space | ||
| 39 | */ | ||
| 40 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 41 | inline_ bool Contains(const Point& pt) const { return SquareDistance(pt) <= mRadius*mRadius; } | ||
| 42 | |||
| 43 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 44 | /** | ||
| 45 | * Tests if a sphere is contained within the LSS. | ||
| 46 | * \param sphere [in] the sphere to test | ||
| 47 | * \return true if inside the LSS | ||
| 48 | * \warning sphere and LSS must be in same space | ||
| 49 | */ | ||
| 50 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 51 | inline_ bool Contains(const Sphere& sphere) | ||
| 52 | { | ||
| 53 | float d = mRadius - sphere.mRadius; | ||
| 54 | if(d>=0.0f) return SquareDistance(sphere.mCenter) <= d*d; | ||
| 55 | else return false; | ||
| 56 | } | ||
| 57 | |||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | /** | ||
| 60 | * Tests if an LSS is contained within the LSS. | ||
| 61 | * \param lss [in] the LSS to test | ||
| 62 | * \return true if inside the LSS | ||
| 63 | * \warning both LSS must be in same space | ||
| 64 | */ | ||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | inline_ bool Contains(const LSS& lss) | ||
| 67 | { | ||
| 68 | // We check the LSS contains the two spheres at the start and end of the sweep | ||
| 69 | return Contains(Sphere(lss.mP0, lss.mRadius)) && Contains(Sphere(lss.mP0, lss.mRadius)); | ||
| 70 | } | ||
| 71 | |||
| 72 | float mRadius; //!< Sphere radius | ||
| 73 | }; | ||
| 74 | |||
| 75 | #endif // __ICELSS_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceMatrix3x3.cpp b/libraries/ode-0.9/OPCODE/Ice/IceMatrix3x3.cpp new file mode 100644 index 0000000..af56d3e --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceMatrix3x3.cpp | |||
| @@ -0,0 +1,48 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for 3x3 matrices. | ||
| 4 | * \file IceMatrix3x3.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * 3x3 matrix. | ||
| 13 | * DirectX-compliant, ie row-column order, ie m[Row][Col]. | ||
| 14 | * Same as: | ||
| 15 | * m11 m12 m13 first row. | ||
| 16 | * m21 m22 m23 second row. | ||
| 17 | * m31 m32 m33 third row. | ||
| 18 | * Stored in memory as m11 m12 m13 m21... | ||
| 19 | * | ||
| 20 | * Multiplication rules: | ||
| 21 | * | ||
| 22 | * [x'y'z'] = [xyz][M] | ||
| 23 | * | ||
| 24 | * x' = x*m11 + y*m21 + z*m31 | ||
| 25 | * y' = x*m12 + y*m22 + z*m32 | ||
| 26 | * z' = x*m13 + y*m23 + z*m33 | ||
| 27 | * | ||
| 28 | * \class Matrix3x3 | ||
| 29 | * \author Pierre Terdiman | ||
| 30 | * \version 1.0 | ||
| 31 | */ | ||
| 32 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 33 | |||
| 34 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 35 | // Precompiled Header | ||
| 36 | #include "Stdafx.h" | ||
| 37 | |||
| 38 | using namespace IceMaths; | ||
| 39 | |||
| 40 | // Cast operator | ||
| 41 | Matrix3x3::operator Matrix4x4() const | ||
| 42 | { | ||
| 43 | return Matrix4x4( | ||
| 44 | m[0][0], m[0][1], m[0][2], 0.0f, | ||
| 45 | m[1][0], m[1][1], m[1][2], 0.0f, | ||
| 46 | m[2][0], m[2][1], m[2][2], 0.0f, | ||
| 47 | 0.0f, 0.0f, 0.0f, 1.0f); | ||
| 48 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceMatrix3x3.h b/libraries/ode-0.9/OPCODE/Ice/IceMatrix3x3.h new file mode 100644 index 0000000..a30680d --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceMatrix3x3.h | |||
| @@ -0,0 +1,496 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for 3x3 matrices. | ||
| 4 | * \file IceMatrix3x3.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEMATRIX3X3_H__ | ||
| 13 | #define __ICEMATRIX3X3_H__ | ||
| 14 | |||
| 15 | // Forward declarations | ||
| 16 | class Quat; | ||
| 17 | |||
| 18 | #define MATRIX3X3_EPSILON (1.0e-7f) | ||
| 19 | |||
| 20 | class ICEMATHS_API Matrix3x3 | ||
| 21 | { | ||
| 22 | public: | ||
| 23 | //! Empty constructor | ||
| 24 | inline_ Matrix3x3() {} | ||
| 25 | //! Constructor from 9 values | ||
| 26 | inline_ Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) | ||
| 27 | { | ||
| 28 | m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; | ||
| 29 | m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; | ||
| 30 | m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; | ||
| 31 | } | ||
| 32 | //! Copy constructor | ||
| 33 | inline_ Matrix3x3(const Matrix3x3& mat) { CopyMemory(m, &mat.m, 9*sizeof(float)); } | ||
| 34 | //! Destructor | ||
| 35 | inline_ ~Matrix3x3() {} | ||
| 36 | |||
| 37 | //! Assign values | ||
| 38 | inline_ void Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) | ||
| 39 | { | ||
| 40 | m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; | ||
| 41 | m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; | ||
| 42 | m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; | ||
| 43 | } | ||
| 44 | |||
| 45 | //! Sets the scale from a Point. The point is put on the diagonal. | ||
| 46 | inline_ void SetScale(const Point& p) { m[0][0] = p.x; m[1][1] = p.y; m[2][2] = p.z; } | ||
| 47 | |||
| 48 | //! Sets the scale from floats. Values are put on the diagonal. | ||
| 49 | inline_ void SetScale(float sx, float sy, float sz) { m[0][0] = sx; m[1][1] = sy; m[2][2] = sz; } | ||
| 50 | |||
| 51 | //! Scales from a Point. Each row is multiplied by a component. | ||
| 52 | inline_ void Scale(const Point& p) | ||
| 53 | { | ||
| 54 | m[0][0] *= p.x; m[0][1] *= p.x; m[0][2] *= p.x; | ||
| 55 | m[1][0] *= p.y; m[1][1] *= p.y; m[1][2] *= p.y; | ||
| 56 | m[2][0] *= p.z; m[2][1] *= p.z; m[2][2] *= p.z; | ||
| 57 | } | ||
| 58 | |||
| 59 | //! Scales from floats. Each row is multiplied by a value. | ||
| 60 | inline_ void Scale(float sx, float sy, float sz) | ||
| 61 | { | ||
| 62 | m[0][0] *= sx; m[0][1] *= sx; m[0][2] *= sx; | ||
| 63 | m[1][0] *= sy; m[1][1] *= sy; m[1][2] *= sy; | ||
| 64 | m[2][0] *= sz; m[2][1] *= sz; m[2][2] *= sz; | ||
| 65 | } | ||
| 66 | |||
| 67 | //! Copy from a Matrix3x3 | ||
| 68 | inline_ void Copy(const Matrix3x3& source) { CopyMemory(m, source.m, 9*sizeof(float)); } | ||
| 69 | |||
| 70 | // Row-column access | ||
| 71 | //! Returns a row. | ||
| 72 | inline_ void GetRow(const udword r, Point& p) const { p.x = m[r][0]; p.y = m[r][1]; p.z = m[r][2]; } | ||
| 73 | //! Returns a row. | ||
| 74 | inline_ const Point& GetRow(const udword r) const { return *(const Point*)&m[r][0]; } | ||
| 75 | //! Returns a row. | ||
| 76 | inline_ Point& GetRow(const udword r) { return *(Point*)&m[r][0]; } | ||
| 77 | //! Sets a row. | ||
| 78 | inline_ void SetRow(const udword r, const Point& p) { m[r][0] = p.x; m[r][1] = p.y; m[r][2] = p.z; } | ||
| 79 | //! Returns a column. | ||
| 80 | inline_ void GetCol(const udword c, Point& p) const { p.x = m[0][c]; p.y = m[1][c]; p.z = m[2][c]; } | ||
| 81 | //! Sets a column. | ||
| 82 | inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; } | ||
| 83 | |||
| 84 | //! Computes the trace. The trace is the sum of the 3 diagonal components. | ||
| 85 | inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2]; } | ||
| 86 | //! Clears the matrix. | ||
| 87 | inline_ void Zero() { ZeroMemory(&m, sizeof(m)); } | ||
| 88 | //! Sets the identity matrix. | ||
| 89 | inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; } | ||
| 90 | //! Checks for identity | ||
| 91 | inline_ bool IsIdentity() const | ||
| 92 | { | ||
| 93 | if(IR(m[0][0])!=IEEE_1_0) return false; | ||
| 94 | if(IR(m[0][1])!=0) return false; | ||
| 95 | if(IR(m[0][2])!=0) return false; | ||
| 96 | |||
| 97 | if(IR(m[1][0])!=0) return false; | ||
| 98 | if(IR(m[1][1])!=IEEE_1_0) return false; | ||
| 99 | if(IR(m[1][2])!=0) return false; | ||
| 100 | |||
| 101 | if(IR(m[2][0])!=0) return false; | ||
| 102 | if(IR(m[2][1])!=0) return false; | ||
| 103 | if(IR(m[2][2])!=IEEE_1_0) return false; | ||
| 104 | |||
| 105 | return true; | ||
| 106 | } | ||
| 107 | |||
| 108 | //! Checks matrix validity | ||
| 109 | inline_ BOOL IsValid() const | ||
| 110 | { | ||
| 111 | for(udword j=0;j<3;j++) | ||
| 112 | { | ||
| 113 | for(udword i=0;i<3;i++) | ||
| 114 | { | ||
| 115 | if(!IsValidFloat(m[j][i])) return FALSE; | ||
| 116 | } | ||
| 117 | } | ||
| 118 | return TRUE; | ||
| 119 | } | ||
| 120 | |||
| 121 | //! Makes a skew-symmetric matrix (a.k.a. Star(*) Matrix) | ||
| 122 | //! [ 0.0 -a.z a.y ] | ||
| 123 | //! [ a.z 0.0 -a.x ] | ||
| 124 | //! [ -a.y a.x 0.0 ] | ||
| 125 | //! This is also called a "cross matrix" since for any vectors A and B, | ||
| 126 | //! A^B = Skew(A) * B = - B * Skew(A); | ||
| 127 | inline_ void SkewSymmetric(const Point& a) | ||
| 128 | { | ||
| 129 | m[0][0] = 0.0f; | ||
| 130 | m[0][1] = -a.z; | ||
| 131 | m[0][2] = a.y; | ||
| 132 | |||
| 133 | m[1][0] = a.z; | ||
| 134 | m[1][1] = 0.0f; | ||
| 135 | m[1][2] = -a.x; | ||
| 136 | |||
| 137 | m[2][0] = -a.y; | ||
| 138 | m[2][1] = a.x; | ||
| 139 | m[2][2] = 0.0f; | ||
| 140 | } | ||
| 141 | |||
| 142 | //! Negates the matrix | ||
| 143 | inline_ void Neg() | ||
| 144 | { | ||
| 145 | m[0][0] = -m[0][0]; m[0][1] = -m[0][1]; m[0][2] = -m[0][2]; | ||
| 146 | m[1][0] = -m[1][0]; m[1][1] = -m[1][1]; m[1][2] = -m[1][2]; | ||
| 147 | m[2][0] = -m[2][0]; m[2][1] = -m[2][1]; m[2][2] = -m[2][2]; | ||
| 148 | } | ||
| 149 | |||
| 150 | //! Neg from another matrix | ||
| 151 | inline_ void Neg(const Matrix3x3& mat) | ||
| 152 | { | ||
| 153 | m[0][0] = -mat.m[0][0]; m[0][1] = -mat.m[0][1]; m[0][2] = -mat.m[0][2]; | ||
| 154 | m[1][0] = -mat.m[1][0]; m[1][1] = -mat.m[1][1]; m[1][2] = -mat.m[1][2]; | ||
| 155 | m[2][0] = -mat.m[2][0]; m[2][1] = -mat.m[2][1]; m[2][2] = -mat.m[2][2]; | ||
| 156 | } | ||
| 157 | |||
| 158 | //! Add another matrix | ||
| 159 | inline_ void Add(const Matrix3x3& mat) | ||
| 160 | { | ||
| 161 | m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2]; | ||
| 162 | m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2]; | ||
| 163 | m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2]; | ||
| 164 | } | ||
| 165 | |||
| 166 | //! Sub another matrix | ||
| 167 | inline_ void Sub(const Matrix3x3& mat) | ||
| 168 | { | ||
| 169 | m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2]; | ||
| 170 | m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2]; | ||
| 171 | m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2]; | ||
| 172 | } | ||
| 173 | //! Mac | ||
| 174 | inline_ void Mac(const Matrix3x3& a, const Matrix3x3& b, float s) | ||
| 175 | { | ||
| 176 | m[0][0] = a.m[0][0] + b.m[0][0] * s; | ||
| 177 | m[0][1] = a.m[0][1] + b.m[0][1] * s; | ||
| 178 | m[0][2] = a.m[0][2] + b.m[0][2] * s; | ||
| 179 | |||
| 180 | m[1][0] = a.m[1][0] + b.m[1][0] * s; | ||
| 181 | m[1][1] = a.m[1][1] + b.m[1][1] * s; | ||
| 182 | m[1][2] = a.m[1][2] + b.m[1][2] * s; | ||
| 183 | |||
| 184 | m[2][0] = a.m[2][0] + b.m[2][0] * s; | ||
| 185 | m[2][1] = a.m[2][1] + b.m[2][1] * s; | ||
| 186 | m[2][2] = a.m[2][2] + b.m[2][2] * s; | ||
| 187 | } | ||
| 188 | //! Mac | ||
| 189 | inline_ void Mac(const Matrix3x3& a, float s) | ||
| 190 | { | ||
| 191 | m[0][0] += a.m[0][0] * s; m[0][1] += a.m[0][1] * s; m[0][2] += a.m[0][2] * s; | ||
| 192 | m[1][0] += a.m[1][0] * s; m[1][1] += a.m[1][1] * s; m[1][2] += a.m[1][2] * s; | ||
| 193 | m[2][0] += a.m[2][0] * s; m[2][1] += a.m[2][1] * s; m[2][2] += a.m[2][2] * s; | ||
| 194 | } | ||
| 195 | |||
| 196 | //! this = A * s | ||
| 197 | inline_ void Mult(const Matrix3x3& a, float s) | ||
| 198 | { | ||
| 199 | m[0][0] = a.m[0][0] * s; m[0][1] = a.m[0][1] * s; m[0][2] = a.m[0][2] * s; | ||
| 200 | m[1][0] = a.m[1][0] * s; m[1][1] = a.m[1][1] * s; m[1][2] = a.m[1][2] * s; | ||
| 201 | m[2][0] = a.m[2][0] * s; m[2][1] = a.m[2][1] * s; m[2][2] = a.m[2][2] * s; | ||
| 202 | } | ||
| 203 | |||
| 204 | inline_ void Add(const Matrix3x3& a, const Matrix3x3& b) | ||
| 205 | { | ||
| 206 | m[0][0] = a.m[0][0] + b.m[0][0]; m[0][1] = a.m[0][1] + b.m[0][1]; m[0][2] = a.m[0][2] + b.m[0][2]; | ||
| 207 | m[1][0] = a.m[1][0] + b.m[1][0]; m[1][1] = a.m[1][1] + b.m[1][1]; m[1][2] = a.m[1][2] + b.m[1][2]; | ||
| 208 | m[2][0] = a.m[2][0] + b.m[2][0]; m[2][1] = a.m[2][1] + b.m[2][1]; m[2][2] = a.m[2][2] + b.m[2][2]; | ||
| 209 | } | ||
| 210 | |||
| 211 | inline_ void Sub(const Matrix3x3& a, const Matrix3x3& b) | ||
| 212 | { | ||
| 213 | m[0][0] = a.m[0][0] - b.m[0][0]; m[0][1] = a.m[0][1] - b.m[0][1]; m[0][2] = a.m[0][2] - b.m[0][2]; | ||
| 214 | m[1][0] = a.m[1][0] - b.m[1][0]; m[1][1] = a.m[1][1] - b.m[1][1]; m[1][2] = a.m[1][2] - b.m[1][2]; | ||
| 215 | m[2][0] = a.m[2][0] - b.m[2][0]; m[2][1] = a.m[2][1] - b.m[2][1]; m[2][2] = a.m[2][2] - b.m[2][2]; | ||
| 216 | } | ||
| 217 | |||
| 218 | //! this = a * b | ||
| 219 | inline_ void Mult(const Matrix3x3& a, const Matrix3x3& b) | ||
| 220 | { | ||
| 221 | m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0]; | ||
| 222 | m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1]; | ||
| 223 | m[0][2] = a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2]; | ||
| 224 | m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0]; | ||
| 225 | m[1][1] = a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1]; | ||
| 226 | m[1][2] = a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2]; | ||
| 227 | m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0]; | ||
| 228 | m[2][1] = a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1]; | ||
| 229 | m[2][2] = a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2]; | ||
| 230 | } | ||
| 231 | |||
| 232 | //! this = transpose(a) * b | ||
| 233 | inline_ void MultAtB(const Matrix3x3& a, const Matrix3x3& b) | ||
| 234 | { | ||
| 235 | m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0]; | ||
| 236 | m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1]; | ||
| 237 | m[0][2] = a.m[0][0] * b.m[0][2] + a.m[1][0] * b.m[1][2] + a.m[2][0] * b.m[2][2]; | ||
| 238 | m[1][0] = a.m[0][1] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[2][1] * b.m[2][0]; | ||
| 239 | m[1][1] = a.m[0][1] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[2][1] * b.m[2][1]; | ||
| 240 | m[1][2] = a.m[0][1] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[2][1] * b.m[2][2]; | ||
| 241 | m[2][0] = a.m[0][2] * b.m[0][0] + a.m[1][2] * b.m[1][0] + a.m[2][2] * b.m[2][0]; | ||
| 242 | m[2][1] = a.m[0][2] * b.m[0][1] + a.m[1][2] * b.m[1][1] + a.m[2][2] * b.m[2][1]; | ||
| 243 | m[2][2] = a.m[0][2] * b.m[0][2] + a.m[1][2] * b.m[1][2] + a.m[2][2] * b.m[2][2]; | ||
| 244 | } | ||
| 245 | |||
| 246 | //! this = a * transpose(b) | ||
| 247 | inline_ void MultABt(const Matrix3x3& a, const Matrix3x3& b) | ||
| 248 | { | ||
| 249 | m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2]; | ||
| 250 | m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2]; | ||
| 251 | m[0][2] = a.m[0][0] * b.m[2][0] + a.m[0][1] * b.m[2][1] + a.m[0][2] * b.m[2][2]; | ||
| 252 | m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[0][1] + a.m[1][2] * b.m[0][2]; | ||
| 253 | m[1][1] = a.m[1][0] * b.m[1][0] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[1][2]; | ||
| 254 | m[1][2] = a.m[1][0] * b.m[2][0] + a.m[1][1] * b.m[2][1] + a.m[1][2] * b.m[2][2]; | ||
| 255 | m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[0][1] + a.m[2][2] * b.m[0][2]; | ||
| 256 | m[2][1] = a.m[2][0] * b.m[1][0] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[1][2]; | ||
| 257 | m[2][2] = a.m[2][0] * b.m[2][0] + a.m[2][1] * b.m[2][1] + a.m[2][2] * b.m[2][2]; | ||
| 258 | } | ||
| 259 | |||
| 260 | //! Makes a rotation matrix mapping vector "from" to vector "to". | ||
| 261 | Matrix3x3& FromTo(const Point& from, const Point& to); | ||
| 262 | |||
| 263 | //! Set a rotation matrix around the X axis. | ||
| 264 | //! 1 0 0 | ||
| 265 | //! RX = 0 cx sx | ||
| 266 | //! 0 -sx cx | ||
| 267 | void RotX(float angle); | ||
| 268 | //! Set a rotation matrix around the Y axis. | ||
| 269 | //! cy 0 -sy | ||
| 270 | //! RY = 0 1 0 | ||
| 271 | //! sy 0 cy | ||
| 272 | void RotY(float angle); | ||
| 273 | //! Set a rotation matrix around the Z axis. | ||
| 274 | //! cz sz 0 | ||
| 275 | //! RZ = -sz cz 0 | ||
| 276 | //! 0 0 1 | ||
| 277 | void RotZ(float angle); | ||
| 278 | //! cy sx.sy -sy.cx | ||
| 279 | //! RY.RX 0 cx sx | ||
| 280 | //! sy -sx.cy cx.cy | ||
| 281 | void RotYX(float y, float x); | ||
| 282 | |||
| 283 | //! Make a rotation matrix about an arbitrary axis | ||
| 284 | Matrix3x3& Rot(float angle, const Point& axis); | ||
| 285 | |||
| 286 | //! Transpose the matrix. | ||
| 287 | void Transpose() | ||
| 288 | { | ||
| 289 | IR(m[1][0]) ^= IR(m[0][1]); IR(m[0][1]) ^= IR(m[1][0]); IR(m[1][0]) ^= IR(m[0][1]); | ||
| 290 | IR(m[2][0]) ^= IR(m[0][2]); IR(m[0][2]) ^= IR(m[2][0]); IR(m[2][0]) ^= IR(m[0][2]); | ||
| 291 | IR(m[2][1]) ^= IR(m[1][2]); IR(m[1][2]) ^= IR(m[2][1]); IR(m[2][1]) ^= IR(m[1][2]); | ||
| 292 | } | ||
| 293 | |||
| 294 | //! this = Transpose(a) | ||
| 295 | void Transpose(const Matrix3x3& a) | ||
| 296 | { | ||
| 297 | m[0][0] = a.m[0][0]; m[0][1] = a.m[1][0]; m[0][2] = a.m[2][0]; | ||
| 298 | m[1][0] = a.m[0][1]; m[1][1] = a.m[1][1]; m[1][2] = a.m[2][1]; | ||
| 299 | m[2][0] = a.m[0][2]; m[2][1] = a.m[1][2]; m[2][2] = a.m[2][2]; | ||
| 300 | } | ||
| 301 | |||
| 302 | //! Compute the determinant of the matrix. We use the rule of Sarrus. | ||
| 303 | float Determinant() const | ||
| 304 | { | ||
| 305 | return (m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1]) | ||
| 306 | - (m[2][0]*m[1][1]*m[0][2] + m[2][1]*m[1][2]*m[0][0] + m[2][2]*m[1][0]*m[0][1]); | ||
| 307 | } | ||
| 308 | /* | ||
| 309 | //! Compute a cofactor. Used for matrix inversion. | ||
| 310 | float CoFactor(ubyte row, ubyte column) const | ||
| 311 | { | ||
| 312 | static sdword gIndex[3+2] = { 0, 1, 2, 0, 1 }; | ||
| 313 | return (m[gIndex[row+1]][gIndex[column+1]]*m[gIndex[row+2]][gIndex[column+2]] - m[gIndex[row+2]][gIndex[column+1]]*m[gIndex[row+1]][gIndex[column+2]]); | ||
| 314 | } | ||
| 315 | */ | ||
| 316 | //! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted. | ||
| 317 | Matrix3x3& Invert() | ||
| 318 | { | ||
| 319 | float Det = Determinant(); // Must be !=0 | ||
| 320 | float OneOverDet = 1.0f / Det; | ||
| 321 | |||
| 322 | Matrix3x3 Temp; | ||
| 323 | Temp.m[0][0] = +(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * OneOverDet; | ||
| 324 | Temp.m[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * OneOverDet; | ||
| 325 | Temp.m[2][0] = +(m[1][0] * m[2][1] - m[2][0] * m[1][1]) * OneOverDet; | ||
| 326 | Temp.m[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * OneOverDet; | ||
| 327 | Temp.m[1][1] = +(m[0][0] * m[2][2] - m[2][0] * m[0][2]) * OneOverDet; | ||
| 328 | Temp.m[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * OneOverDet; | ||
| 329 | Temp.m[0][2] = +(m[0][1] * m[1][2] - m[1][1] * m[0][2]) * OneOverDet; | ||
| 330 | Temp.m[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * OneOverDet; | ||
| 331 | Temp.m[2][2] = +(m[0][0] * m[1][1] - m[1][0] * m[0][1]) * OneOverDet; | ||
| 332 | |||
| 333 | *this = Temp; | ||
| 334 | |||
| 335 | return *this; | ||
| 336 | } | ||
| 337 | |||
| 338 | Matrix3x3& Normalize(); | ||
| 339 | |||
| 340 | //! this = exp(a) | ||
| 341 | Matrix3x3& Exp(const Matrix3x3& a); | ||
| 342 | |||
| 343 | void FromQuat(const Quat &q); | ||
| 344 | void FromQuatL2(const Quat &q, float l2); | ||
| 345 | |||
| 346 | // Arithmetic operators | ||
| 347 | //! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3; | ||
| 348 | inline_ Matrix3x3 operator+(const Matrix3x3& mat) const | ||
| 349 | { | ||
| 350 | return Matrix3x3( | ||
| 351 | m[0][0] + mat.m[0][0], m[0][1] + mat.m[0][1], m[0][2] + mat.m[0][2], | ||
| 352 | m[1][0] + mat.m[1][0], m[1][1] + mat.m[1][1], m[1][2] + mat.m[1][2], | ||
| 353 | m[2][0] + mat.m[2][0], m[2][1] + mat.m[2][1], m[2][2] + mat.m[2][2]); | ||
| 354 | } | ||
| 355 | |||
| 356 | //! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3; | ||
| 357 | inline_ Matrix3x3 operator-(const Matrix3x3& mat) const | ||
| 358 | { | ||
| 359 | return Matrix3x3( | ||
| 360 | m[0][0] - mat.m[0][0], m[0][1] - mat.m[0][1], m[0][2] - mat.m[0][2], | ||
| 361 | m[1][0] - mat.m[1][0], m[1][1] - mat.m[1][1], m[1][2] - mat.m[1][2], | ||
| 362 | m[2][0] - mat.m[2][0], m[2][1] - mat.m[2][1], m[2][2] - mat.m[2][2]); | ||
| 363 | } | ||
| 364 | |||
| 365 | //! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3; | ||
| 366 | inline_ Matrix3x3 operator*(const Matrix3x3& mat) const | ||
| 367 | { | ||
| 368 | return Matrix3x3( | ||
| 369 | m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0], | ||
| 370 | m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1], | ||
| 371 | m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2], | ||
| 372 | |||
| 373 | m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0], | ||
| 374 | m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1], | ||
| 375 | m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2], | ||
| 376 | |||
| 377 | m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0], | ||
| 378 | m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1], | ||
| 379 | m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2]); | ||
| 380 | } | ||
| 381 | |||
| 382 | //! Operator for Point Mul = Matrix3x3 * Point; | ||
| 383 | inline_ Point operator*(const Point& v) const { return Point(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v); } | ||
| 384 | |||
| 385 | //! Operator for Matrix3x3 Mul = Matrix3x3 * float; | ||
| 386 | inline_ Matrix3x3 operator*(float s) const | ||
| 387 | { | ||
| 388 | return Matrix3x3( | ||
| 389 | m[0][0]*s, m[0][1]*s, m[0][2]*s, | ||
| 390 | m[1][0]*s, m[1][1]*s, m[1][2]*s, | ||
| 391 | m[2][0]*s, m[2][1]*s, m[2][2]*s); | ||
| 392 | } | ||
| 393 | |||
| 394 | //! Operator for Matrix3x3 Mul = float * Matrix3x3; | ||
| 395 | inline_ friend Matrix3x3 operator*(float s, const Matrix3x3& mat) | ||
| 396 | { | ||
| 397 | return Matrix3x3( | ||
| 398 | s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2], | ||
| 399 | s*mat.m[1][0], s*mat.m[1][1], s*mat.m[1][2], | ||
| 400 | s*mat.m[2][0], s*mat.m[2][1], s*mat.m[2][2]); | ||
| 401 | } | ||
| 402 | |||
| 403 | //! Operator for Matrix3x3 Div = Matrix3x3 / float; | ||
| 404 | inline_ Matrix3x3 operator/(float s) const | ||
| 405 | { | ||
| 406 | if (s) s = 1.0f / s; | ||
| 407 | return Matrix3x3( | ||
| 408 | m[0][0]*s, m[0][1]*s, m[0][2]*s, | ||
| 409 | m[1][0]*s, m[1][1]*s, m[1][2]*s, | ||
| 410 | m[2][0]*s, m[2][1]*s, m[2][2]*s); | ||
| 411 | } | ||
| 412 | |||
| 413 | //! Operator for Matrix3x3 Div = float / Matrix3x3; | ||
| 414 | inline_ friend Matrix3x3 operator/(float s, const Matrix3x3& mat) | ||
| 415 | { | ||
| 416 | return Matrix3x3( | ||
| 417 | s/mat.m[0][0], s/mat.m[0][1], s/mat.m[0][2], | ||
| 418 | s/mat.m[1][0], s/mat.m[1][1], s/mat.m[1][2], | ||
| 419 | s/mat.m[2][0], s/mat.m[2][1], s/mat.m[2][2]); | ||
| 420 | } | ||
| 421 | |||
| 422 | //! Operator for Matrix3x3 += Matrix3x3 | ||
| 423 | inline_ Matrix3x3& operator+=(const Matrix3x3& mat) | ||
| 424 | { | ||
| 425 | m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2]; | ||
| 426 | m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2]; | ||
| 427 | m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2]; | ||
| 428 | return *this; | ||
| 429 | } | ||
| 430 | |||
| 431 | //! Operator for Matrix3x3 -= Matrix3x3 | ||
| 432 | inline_ Matrix3x3& operator-=(const Matrix3x3& mat) | ||
| 433 | { | ||
| 434 | m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2]; | ||
| 435 | m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2]; | ||
| 436 | m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2]; | ||
| 437 | return *this; | ||
| 438 | } | ||
| 439 | |||
| 440 | //! Operator for Matrix3x3 *= Matrix3x3 | ||
| 441 | inline_ Matrix3x3& operator*=(const Matrix3x3& mat) | ||
| 442 | { | ||
| 443 | Point TempRow; | ||
| 444 | |||
| 445 | GetRow(0, TempRow); | ||
| 446 | m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; | ||
| 447 | m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; | ||
| 448 | m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; | ||
| 449 | |||
| 450 | GetRow(1, TempRow); | ||
| 451 | m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; | ||
| 452 | m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; | ||
| 453 | m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; | ||
| 454 | |||
| 455 | GetRow(2, TempRow); | ||
| 456 | m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0]; | ||
| 457 | m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1]; | ||
| 458 | m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2]; | ||
| 459 | return *this; | ||
| 460 | } | ||
| 461 | |||
| 462 | //! Operator for Matrix3x3 *= float | ||
| 463 | inline_ Matrix3x3& operator*=(float s) | ||
| 464 | { | ||
| 465 | m[0][0] *= s; m[0][1] *= s; m[0][2] *= s; | ||
| 466 | m[1][0] *= s; m[1][1] *= s; m[1][2] *= s; | ||
| 467 | m[2][0] *= s; m[2][1] *= s; m[2][2] *= s; | ||
| 468 | return *this; | ||
| 469 | } | ||
| 470 | |||
| 471 | //! Operator for Matrix3x3 /= float | ||
| 472 | inline_ Matrix3x3& operator/=(float s) | ||
| 473 | { | ||
| 474 | if (s) s = 1.0f / s; | ||
| 475 | m[0][0] *= s; m[0][1] *= s; m[0][2] *= s; | ||
| 476 | m[1][0] *= s; m[1][1] *= s; m[1][2] *= s; | ||
| 477 | m[2][0] *= s; m[2][1] *= s; m[2][2] *= s; | ||
| 478 | return *this; | ||
| 479 | } | ||
| 480 | |||
| 481 | // Cast operators | ||
| 482 | //! Cast a Matrix3x3 to a Matrix4x4. | ||
| 483 | operator Matrix4x4() const; | ||
| 484 | //! Cast a Matrix3x3 to a Quat. | ||
| 485 | operator Quat() const; | ||
| 486 | |||
| 487 | inline_ const Point& operator[](int row) const { return *(const Point*)&m[row][0]; } | ||
| 488 | inline_ Point& operator[](int row) { return *(Point*)&m[row][0]; } | ||
| 489 | |||
| 490 | public: | ||
| 491 | |||
| 492 | float m[3][3]; | ||
| 493 | }; | ||
| 494 | |||
| 495 | #endif // __ICEMATRIX3X3_H__ | ||
| 496 | |||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceMatrix4x4.cpp b/libraries/ode-0.9/OPCODE/Ice/IceMatrix4x4.cpp new file mode 100644 index 0000000..0b258f0 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceMatrix4x4.cpp | |||
| @@ -0,0 +1,135 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for 4x4 matrices. | ||
| 4 | * \file IceMatrix4x4.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * 4x4 matrix. | ||
| 13 | * DirectX-compliant, ie row-column order, ie m[Row][Col]. | ||
| 14 | * Same as: | ||
| 15 | * m11 m12 m13 m14 first row. | ||
| 16 | * m21 m22 m23 m24 second row. | ||
| 17 | * m31 m32 m33 m34 third row. | ||
| 18 | * m41 m42 m43 m44 fourth row. | ||
| 19 | * Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1). | ||
| 20 | * Stored in memory as m11 m12 m13 m14 m21... | ||
| 21 | * | ||
| 22 | * Multiplication rules: | ||
| 23 | * | ||
| 24 | * [x'y'z'1] = [xyz1][M] | ||
| 25 | * | ||
| 26 | * x' = x*m11 + y*m21 + z*m31 + m41 | ||
| 27 | * y' = x*m12 + y*m22 + z*m32 + m42 | ||
| 28 | * z' = x*m13 + y*m23 + z*m33 + m43 | ||
| 29 | * 1' = 0 + 0 + 0 + m44 | ||
| 30 | * | ||
| 31 | * \class Matrix4x4 | ||
| 32 | * \author Pierre Terdiman | ||
| 33 | * \version 1.0 | ||
| 34 | */ | ||
| 35 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 36 | |||
| 37 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 38 | // Precompiled Header | ||
| 39 | #include "Stdafx.h" | ||
| 40 | |||
| 41 | using namespace IceMaths; | ||
| 42 | |||
| 43 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 44 | /** | ||
| 45 | * Inverts a PR matrix. (which only contains a rotation and a translation) | ||
| 46 | * This is faster and less subject to FPU errors than the generic inversion code. | ||
| 47 | * | ||
| 48 | * \relates Matrix4x4 | ||
| 49 | * \fn InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src) | ||
| 50 | * \param dest [out] destination matrix | ||
| 51 | * \param src [in] source matrix | ||
| 52 | */ | ||
| 53 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 54 | ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src) | ||
| 55 | { | ||
| 56 | dest.m[0][0] = src.m[0][0]; | ||
| 57 | dest.m[1][0] = src.m[0][1]; | ||
| 58 | dest.m[2][0] = src.m[0][2]; | ||
| 59 | dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]); | ||
| 60 | |||
| 61 | dest.m[0][1] = src.m[1][0]; | ||
| 62 | dest.m[1][1] = src.m[1][1]; | ||
| 63 | dest.m[2][1] = src.m[1][2]; | ||
| 64 | dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]); | ||
| 65 | |||
| 66 | dest.m[0][2] = src.m[2][0]; | ||
| 67 | dest.m[1][2] = src.m[2][1]; | ||
| 68 | dest.m[2][2] = src.m[2][2]; | ||
| 69 | dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]); | ||
| 70 | |||
| 71 | dest.m[0][3] = 0.0f; | ||
| 72 | dest.m[1][3] = 0.0f; | ||
| 73 | dest.m[2][3] = 0.0f; | ||
| 74 | dest.m[3][3] = 1.0f; | ||
| 75 | } | ||
| 76 | |||
| 77 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 78 | // Compute the cofactor of the Matrix at a specified location | ||
| 79 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 80 | float Matrix4x4::CoFactor(udword row, udword col) const | ||
| 81 | { | ||
| 82 | return (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] + | ||
| 83 | m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] + | ||
| 84 | m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3]) | ||
| 85 | - (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] + | ||
| 86 | m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] + | ||
| 87 | m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f); | ||
| 88 | } | ||
| 89 | |||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | // Compute the determinant of the Matrix | ||
| 92 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 93 | float Matrix4x4::Determinant() const | ||
| 94 | { | ||
| 95 | return m[0][0] * CoFactor(0, 0) + | ||
| 96 | m[0][1] * CoFactor(0, 1) + | ||
| 97 | m[0][2] * CoFactor(0, 2) + | ||
| 98 | m[0][3] * CoFactor(0, 3); | ||
| 99 | } | ||
| 100 | |||
| 101 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 102 | // Compute the inverse of the matrix | ||
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 104 | Matrix4x4& Matrix4x4::Invert() | ||
| 105 | { | ||
| 106 | float Det = Determinant(); | ||
| 107 | Matrix4x4 Temp; | ||
| 108 | |||
| 109 | if(fabsf(Det) < MATRIX4X4_EPSILON) | ||
| 110 | return *this; // The matrix is not invertible! Singular case! | ||
| 111 | |||
| 112 | float IDet = 1.0f / Det; | ||
| 113 | |||
| 114 | Temp.m[0][0] = CoFactor(0,0) * IDet; | ||
| 115 | Temp.m[1][0] = CoFactor(0,1) * IDet; | ||
| 116 | Temp.m[2][0] = CoFactor(0,2) * IDet; | ||
| 117 | Temp.m[3][0] = CoFactor(0,3) * IDet; | ||
| 118 | Temp.m[0][1] = CoFactor(1,0) * IDet; | ||
| 119 | Temp.m[1][1] = CoFactor(1,1) * IDet; | ||
| 120 | Temp.m[2][1] = CoFactor(1,2) * IDet; | ||
| 121 | Temp.m[3][1] = CoFactor(1,3) * IDet; | ||
| 122 | Temp.m[0][2] = CoFactor(2,0) * IDet; | ||
| 123 | Temp.m[1][2] = CoFactor(2,1) * IDet; | ||
| 124 | Temp.m[2][2] = CoFactor(2,2) * IDet; | ||
| 125 | Temp.m[3][2] = CoFactor(2,3) * IDet; | ||
| 126 | Temp.m[0][3] = CoFactor(3,0) * IDet; | ||
| 127 | Temp.m[1][3] = CoFactor(3,1) * IDet; | ||
| 128 | Temp.m[2][3] = CoFactor(3,2) * IDet; | ||
| 129 | Temp.m[3][3] = CoFactor(3,3) * IDet; | ||
| 130 | |||
| 131 | *this = Temp; | ||
| 132 | |||
| 133 | return *this; | ||
| 134 | } | ||
| 135 | |||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceMatrix4x4.h b/libraries/ode-0.9/OPCODE/Ice/IceMatrix4x4.h new file mode 100644 index 0000000..45919be --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceMatrix4x4.h | |||
| @@ -0,0 +1,455 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for 4x4 matrices. | ||
| 4 | * \file IceMatrix4x4.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEMATRIX4X4_H__ | ||
| 13 | #define __ICEMATRIX4X4_H__ | ||
| 14 | |||
| 15 | // Forward declarations | ||
| 16 | class PRS; | ||
| 17 | class PR; | ||
| 18 | |||
| 19 | #define MATRIX4X4_EPSILON (1.0e-7f) | ||
| 20 | |||
| 21 | class ICEMATHS_API Matrix4x4 | ||
| 22 | { | ||
| 23 | // void LUBackwardSubstitution( sdword *indx, float* b ); | ||
| 24 | // void LUDecomposition( sdword* indx, float* d ); | ||
| 25 | |||
| 26 | public: | ||
| 27 | //! Empty constructor. | ||
| 28 | inline_ Matrix4x4() {} | ||
| 29 | //! Constructor from 16 values | ||
| 30 | inline_ Matrix4x4( float m00, float m01, float m02, float m03, | ||
| 31 | float m10, float m11, float m12, float m13, | ||
| 32 | float m20, float m21, float m22, float m23, | ||
| 33 | float m30, float m31, float m32, float m33) | ||
| 34 | { | ||
| 35 | m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03; | ||
| 36 | m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13; | ||
| 37 | m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23; | ||
| 38 | m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33; | ||
| 39 | } | ||
| 40 | //! Copy constructor | ||
| 41 | inline_ Matrix4x4(const Matrix4x4& mat) { CopyMemory(m, &mat.m, 16*sizeof(float)); } | ||
| 42 | //! Destructor. | ||
| 43 | inline_ ~Matrix4x4() {} | ||
| 44 | |||
| 45 | //! Assign values (rotation only) | ||
| 46 | inline_ Matrix4x4& Set( float m00, float m01, float m02, | ||
| 47 | float m10, float m11, float m12, | ||
| 48 | float m20, float m21, float m22) | ||
| 49 | { | ||
| 50 | m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; | ||
| 51 | m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; | ||
| 52 | m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; | ||
| 53 | return *this; | ||
| 54 | } | ||
| 55 | //! Assign values | ||
| 56 | inline_ Matrix4x4& Set( float m00, float m01, float m02, float m03, | ||
| 57 | float m10, float m11, float m12, float m13, | ||
| 58 | float m20, float m21, float m22, float m23, | ||
| 59 | float m30, float m31, float m32, float m33) | ||
| 60 | { | ||
| 61 | m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03; | ||
| 62 | m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13; | ||
| 63 | m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23; | ||
| 64 | m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33; | ||
| 65 | return *this; | ||
| 66 | } | ||
| 67 | |||
| 68 | //! Copy from a Matrix4x4 | ||
| 69 | inline_ void Copy(const Matrix4x4& source) { CopyMemory(m, source.m, 16*sizeof(float)); } | ||
| 70 | |||
| 71 | // Row-column access | ||
| 72 | //! Returns a row. | ||
| 73 | inline_ void GetRow(const udword r, HPoint& p) const { p.x=m[r][0]; p.y=m[r][1]; p.z=m[r][2]; p.w=m[r][3]; } | ||
| 74 | //! Returns a row. | ||
| 75 | inline_ void GetRow(const udword r, Point& p) const { p.x=m[r][0]; p.y=m[r][1]; p.z=m[r][2]; } | ||
| 76 | //! Returns a row. | ||
| 77 | inline_ const HPoint& GetRow(const udword r) const { return *(const HPoint*)&m[r][0]; } | ||
| 78 | //! Returns a row. | ||
| 79 | inline_ HPoint& GetRow(const udword r) { return *(HPoint*)&m[r][0]; } | ||
| 80 | //! Sets a row. | ||
| 81 | inline_ void SetRow(const udword r, const HPoint& p) { m[r][0]=p.x; m[r][1]=p.y; m[r][2]=p.z; m[r][3]=p.w; } | ||
| 82 | //! Sets a row. | ||
| 83 | inline_ void SetRow(const udword r, const Point& p) { m[r][0]=p.x; m[r][1]=p.y; m[r][2]=p.z; m[r][3]= (r!=3) ? 0.0f : 1.0f; } | ||
| 84 | //! Returns a column. | ||
| 85 | inline_ void GetCol(const udword c, HPoint& p) const { p.x=m[0][c]; p.y=m[1][c]; p.z=m[2][c]; p.w=m[3][c]; } | ||
| 86 | //! Returns a column. | ||
| 87 | inline_ void GetCol(const udword c, Point& p) const { p.x=m[0][c]; p.y=m[1][c]; p.z=m[2][c]; } | ||
| 88 | //! Sets a column. | ||
| 89 | inline_ void SetCol(const udword c, const HPoint& p) { m[0][c]=p.x; m[1][c]=p.y; m[2][c]=p.z; m[3][c]=p.w; } | ||
| 90 | //! Sets a column. | ||
| 91 | inline_ void SetCol(const udword c, const Point& p) { m[0][c]=p.x; m[1][c]=p.y; m[2][c]=p.z; m[3][c]= (c!=3) ? 0.0f : 1.0f; } | ||
| 92 | |||
| 93 | // Translation | ||
| 94 | //! Returns the translation part of the matrix. | ||
| 95 | inline_ const HPoint& GetTrans() const { return GetRow(3); } | ||
| 96 | //! Gets the translation part of the matrix | ||
| 97 | inline_ void GetTrans(Point& p) const { p.x=m[3][0]; p.y=m[3][1]; p.z=m[3][2]; } | ||
| 98 | //! Sets the translation part of the matrix, from a Point. | ||
| 99 | inline_ void SetTrans(const Point& p) { m[3][0]=p.x; m[3][1]=p.y; m[3][2]=p.z; } | ||
| 100 | //! Sets the translation part of the matrix, from a HPoint. | ||
| 101 | inline_ void SetTrans(const HPoint& p) { m[3][0]=p.x; m[3][1]=p.y; m[3][2]=p.z; m[3][3]=p.w; } | ||
| 102 | //! Sets the translation part of the matrix, from floats. | ||
| 103 | inline_ void SetTrans(float tx, float ty, float tz) { m[3][0]=tx; m[3][1]=ty; m[3][2]=tz; } | ||
| 104 | |||
| 105 | // Scale | ||
| 106 | //! Sets the scale from a Point. The point is put on the diagonal. | ||
| 107 | inline_ void SetScale(const Point& p) { m[0][0]=p.x; m[1][1]=p.y; m[2][2]=p.z; } | ||
| 108 | //! Sets the scale from floats. Values are put on the diagonal. | ||
| 109 | inline_ void SetScale(float sx, float sy, float sz) { m[0][0]=sx; m[1][1]=sy; m[2][2]=sz; } | ||
| 110 | //! Scales from a Point. Each row is multiplied by a component. | ||
| 111 | void Scale(const Point& p) | ||
| 112 | { | ||
| 113 | m[0][0] *= p.x; m[1][0] *= p.y; m[2][0] *= p.z; | ||
| 114 | m[0][1] *= p.x; m[1][1] *= p.y; m[2][1] *= p.z; | ||
| 115 | m[0][2] *= p.x; m[1][2] *= p.y; m[2][2] *= p.z; | ||
| 116 | } | ||
| 117 | //! Scales from floats. Each row is multiplied by a value. | ||
| 118 | void Scale(float sx, float sy, float sz) | ||
| 119 | { | ||
| 120 | m[0][0] *= sx; m[1][0] *= sy; m[2][0] *= sz; | ||
| 121 | m[0][1] *= sx; m[1][1] *= sy; m[2][1] *= sz; | ||
| 122 | m[0][2] *= sx; m[1][2] *= sy; m[2][2] *= sz; | ||
| 123 | } | ||
| 124 | /* | ||
| 125 | //! Returns a row. | ||
| 126 | inline_ HPoint GetRow(const udword row) const { return mRow[row]; } | ||
| 127 | //! Sets a row. | ||
| 128 | inline_ Matrix4x4& SetRow(const udword row, const HPoint& p) { mRow[row] = p; return *this; } | ||
| 129 | //! Sets a row. | ||
| 130 | Matrix4x4& SetRow(const udword row, const Point& p) | ||
| 131 | { | ||
| 132 | m[row][0] = p.x; | ||
| 133 | m[row][1] = p.y; | ||
| 134 | m[row][2] = p.z; | ||
| 135 | m[row][3] = (row != 3) ? 0.0f : 1.0f; | ||
| 136 | return *this; | ||
| 137 | } | ||
| 138 | //! Returns a column. | ||
| 139 | HPoint GetCol(const udword col) const | ||
| 140 | { | ||
| 141 | HPoint Res; | ||
| 142 | Res.x = m[0][col]; | ||
| 143 | Res.y = m[1][col]; | ||
| 144 | Res.z = m[2][col]; | ||
| 145 | Res.w = m[3][col]; | ||
| 146 | return Res; | ||
| 147 | } | ||
| 148 | //! Sets a column. | ||
| 149 | Matrix4x4& SetCol(const udword col, const HPoint& p) | ||
| 150 | { | ||
| 151 | m[0][col] = p.x; | ||
| 152 | m[1][col] = p.y; | ||
| 153 | m[2][col] = p.z; | ||
| 154 | m[3][col] = p.w; | ||
| 155 | return *this; | ||
| 156 | } | ||
| 157 | //! Sets a column. | ||
| 158 | Matrix4x4& SetCol(const udword col, const Point& p) | ||
| 159 | { | ||
| 160 | m[0][col] = p.x; | ||
| 161 | m[1][col] = p.y; | ||
| 162 | m[2][col] = p.z; | ||
| 163 | m[3][col] = (col != 3) ? 0.0f : 1.0f; | ||
| 164 | return *this; | ||
| 165 | } | ||
| 166 | */ | ||
| 167 | //! Computes the trace. The trace is the sum of the 4 diagonal components. | ||
| 168 | inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2] + m[3][3]; } | ||
| 169 | //! Computes the trace of the upper 3x3 matrix. | ||
| 170 | inline_ float Trace3x3() const { return m[0][0] + m[1][1] + m[2][2]; } | ||
| 171 | //! Clears the matrix. | ||
| 172 | inline_ void Zero() { ZeroMemory(&m, sizeof(m)); } | ||
| 173 | //! Sets the identity matrix. | ||
| 174 | inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f; } | ||
| 175 | //! Checks for identity | ||
| 176 | inline_ bool IsIdentity() const | ||
| 177 | { | ||
| 178 | if(IR(m[0][0])!=IEEE_1_0) return false; | ||
| 179 | if(IR(m[0][1])!=0) return false; | ||
| 180 | if(IR(m[0][2])!=0) return false; | ||
| 181 | if(IR(m[0][3])!=0) return false; | ||
| 182 | |||
| 183 | if(IR(m[1][0])!=0) return false; | ||
| 184 | if(IR(m[1][1])!=IEEE_1_0) return false; | ||
| 185 | if(IR(m[1][2])!=0) return false; | ||
| 186 | if(IR(m[1][3])!=0) return false; | ||
| 187 | |||
| 188 | if(IR(m[2][0])!=0) return false; | ||
| 189 | if(IR(m[2][1])!=0) return false; | ||
| 190 | if(IR(m[2][2])!=IEEE_1_0) return false; | ||
| 191 | if(IR(m[2][3])!=0) return false; | ||
| 192 | |||
| 193 | if(IR(m[3][0])!=0) return false; | ||
| 194 | if(IR(m[3][1])!=0) return false; | ||
| 195 | if(IR(m[3][2])!=0) return false; | ||
| 196 | if(IR(m[3][3])!=IEEE_1_0) return false; | ||
| 197 | return true; | ||
| 198 | } | ||
| 199 | |||
| 200 | //! Checks matrix validity | ||
| 201 | inline_ BOOL IsValid() const | ||
| 202 | { | ||
| 203 | for(udword j=0;j<4;j++) | ||
| 204 | { | ||
| 205 | for(udword i=0;i<4;i++) | ||
| 206 | { | ||
| 207 | if(!IsValidFloat(m[j][i])) return FALSE; | ||
| 208 | } | ||
| 209 | } | ||
| 210 | return TRUE; | ||
| 211 | } | ||
| 212 | |||
| 213 | //! Sets a rotation matrix around the X axis. | ||
| 214 | void RotX(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[1][1] = m[2][2] = Cos; m[2][1] = -Sin; m[1][2] = Sin; } | ||
| 215 | //! Sets a rotation matrix around the Y axis. | ||
| 216 | void RotY(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[2][2] = Cos; m[2][0] = Sin; m[0][2] = -Sin; } | ||
| 217 | //! Sets a rotation matrix around the Z axis. | ||
| 218 | void RotZ(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[1][1] = Cos; m[1][0] = -Sin; m[0][1] = Sin; } | ||
| 219 | |||
| 220 | //! Makes a rotation matrix about an arbitrary axis | ||
| 221 | Matrix4x4& Rot(float angle, Point& p1, Point& p2); | ||
| 222 | |||
| 223 | //! Transposes the matrix. | ||
| 224 | void Transpose() | ||
| 225 | { | ||
| 226 | IR(m[1][0]) ^= IR(m[0][1]); IR(m[0][1]) ^= IR(m[1][0]); IR(m[1][0]) ^= IR(m[0][1]); | ||
| 227 | IR(m[2][0]) ^= IR(m[0][2]); IR(m[0][2]) ^= IR(m[2][0]); IR(m[2][0]) ^= IR(m[0][2]); | ||
| 228 | IR(m[3][0]) ^= IR(m[0][3]); IR(m[0][3]) ^= IR(m[3][0]); IR(m[3][0]) ^= IR(m[0][3]); | ||
| 229 | IR(m[1][2]) ^= IR(m[2][1]); IR(m[2][1]) ^= IR(m[1][2]); IR(m[1][2]) ^= IR(m[2][1]); | ||
| 230 | IR(m[1][3]) ^= IR(m[3][1]); IR(m[3][1]) ^= IR(m[1][3]); IR(m[1][3]) ^= IR(m[3][1]); | ||
| 231 | IR(m[2][3]) ^= IR(m[3][2]); IR(m[3][2]) ^= IR(m[2][3]); IR(m[2][3]) ^= IR(m[3][2]); | ||
| 232 | } | ||
| 233 | |||
| 234 | //! Computes a cofactor. Used for matrix inversion. | ||
| 235 | float CoFactor(udword row, udword col) const; | ||
| 236 | //! Computes the determinant of the matrix. | ||
| 237 | float Determinant() const; | ||
| 238 | //! Inverts the matrix. Determinant must be different from zero, else matrix can't be inverted. | ||
| 239 | Matrix4x4& Invert(); | ||
| 240 | // Matrix& ComputeAxisMatrix(Point& axis, float angle); | ||
| 241 | |||
| 242 | // Cast operators | ||
| 243 | //! Casts a Matrix4x4 to a Matrix3x3. | ||
| 244 | inline_ operator Matrix3x3() const | ||
| 245 | { | ||
| 246 | return Matrix3x3( | ||
| 247 | m[0][0], m[0][1], m[0][2], | ||
| 248 | m[1][0], m[1][1], m[1][2], | ||
| 249 | m[2][0], m[2][1], m[2][2]); | ||
| 250 | } | ||
| 251 | //! Casts a Matrix4x4 to a Quat. | ||
| 252 | operator Quat() const; | ||
| 253 | //! Casts a Matrix4x4 to a PR. | ||
| 254 | operator PR() const; | ||
| 255 | |||
| 256 | // Arithmetic operators | ||
| 257 | //! Operator for Matrix4x4 Plus = Matrix4x4 + Matrix4x4; | ||
| 258 | inline_ Matrix4x4 operator+(const Matrix4x4& mat) const | ||
| 259 | { | ||
| 260 | return Matrix4x4( | ||
| 261 | m[0][0]+mat.m[0][0], m[0][1]+mat.m[0][1], m[0][2]+mat.m[0][2], m[0][3]+mat.m[0][3], | ||
| 262 | m[1][0]+mat.m[1][0], m[1][1]+mat.m[1][1], m[1][2]+mat.m[1][2], m[1][3]+mat.m[1][3], | ||
| 263 | m[2][0]+mat.m[2][0], m[2][1]+mat.m[2][1], m[2][2]+mat.m[2][2], m[2][3]+mat.m[2][3], | ||
| 264 | m[3][0]+mat.m[3][0], m[3][1]+mat.m[3][1], m[3][2]+mat.m[3][2], m[3][3]+mat.m[3][3]); | ||
| 265 | } | ||
| 266 | |||
| 267 | //! Operator for Matrix4x4 Minus = Matrix4x4 - Matrix4x4; | ||
| 268 | inline_ Matrix4x4 operator-(const Matrix4x4& mat) const | ||
| 269 | { | ||
| 270 | return Matrix4x4( | ||
| 271 | m[0][0]-mat.m[0][0], m[0][1]-mat.m[0][1], m[0][2]-mat.m[0][2], m[0][3]-mat.m[0][3], | ||
| 272 | m[1][0]-mat.m[1][0], m[1][1]-mat.m[1][1], m[1][2]-mat.m[1][2], m[1][3]-mat.m[1][3], | ||
| 273 | m[2][0]-mat.m[2][0], m[2][1]-mat.m[2][1], m[2][2]-mat.m[2][2], m[2][3]-mat.m[2][3], | ||
| 274 | m[3][0]-mat.m[3][0], m[3][1]-mat.m[3][1], m[3][2]-mat.m[3][2], m[3][3]-mat.m[3][3]); | ||
| 275 | } | ||
| 276 | |||
| 277 | //! Operator for Matrix4x4 Mul = Matrix4x4 * Matrix4x4; | ||
| 278 | inline_ Matrix4x4 operator*(const Matrix4x4& mat) const | ||
| 279 | { | ||
| 280 | return Matrix4x4( | ||
| 281 | m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0] + m[0][3]*mat.m[3][0], | ||
| 282 | m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1] + m[0][3]*mat.m[3][1], | ||
| 283 | m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2] + m[0][3]*mat.m[3][2], | ||
| 284 | m[0][0]*mat.m[0][3] + m[0][1]*mat.m[1][3] + m[0][2]*mat.m[2][3] + m[0][3]*mat.m[3][3], | ||
| 285 | |||
| 286 | m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0] + m[1][3]*mat.m[3][0], | ||
| 287 | m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1] + m[1][3]*mat.m[3][1], | ||
| 288 | m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2] + m[1][3]*mat.m[3][2], | ||
| 289 | m[1][0]*mat.m[0][3] + m[1][1]*mat.m[1][3] + m[1][2]*mat.m[2][3] + m[1][3]*mat.m[3][3], | ||
| 290 | |||
| 291 | m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0] + m[2][3]*mat.m[3][0], | ||
| 292 | m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1] + m[2][3]*mat.m[3][1], | ||
| 293 | m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2] + m[2][3]*mat.m[3][2], | ||
| 294 | m[2][0]*mat.m[0][3] + m[2][1]*mat.m[1][3] + m[2][2]*mat.m[2][3] + m[2][3]*mat.m[3][3], | ||
| 295 | |||
| 296 | m[3][0]*mat.m[0][0] + m[3][1]*mat.m[1][0] + m[3][2]*mat.m[2][0] + m[3][3]*mat.m[3][0], | ||
| 297 | m[3][0]*mat.m[0][1] + m[3][1]*mat.m[1][1] + m[3][2]*mat.m[2][1] + m[3][3]*mat.m[3][1], | ||
| 298 | m[3][0]*mat.m[0][2] + m[3][1]*mat.m[1][2] + m[3][2]*mat.m[2][2] + m[3][3]*mat.m[3][2], | ||
| 299 | m[3][0]*mat.m[0][3] + m[3][1]*mat.m[1][3] + m[3][2]*mat.m[2][3] + m[3][3]*mat.m[3][3]); | ||
| 300 | } | ||
| 301 | |||
| 302 | //! Operator for HPoint Mul = Matrix4x4 * HPoint; | ||
| 303 | inline_ HPoint operator*(const HPoint& v) const { return HPoint(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v, GetRow(3)|v); } | ||
| 304 | |||
| 305 | //! Operator for Point Mul = Matrix4x4 * Point; | ||
| 306 | inline_ Point operator*(const Point& v) const | ||
| 307 | { | ||
| 308 | return Point( m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3], | ||
| 309 | m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3], | ||
| 310 | m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3] ); | ||
| 311 | } | ||
| 312 | |||
| 313 | //! Operator for Matrix4x4 Scale = Matrix4x4 * float; | ||
| 314 | inline_ Matrix4x4 operator*(float s) const | ||
| 315 | { | ||
| 316 | return Matrix4x4( | ||
| 317 | m[0][0]*s, m[0][1]*s, m[0][2]*s, m[0][3]*s, | ||
| 318 | m[1][0]*s, m[1][1]*s, m[1][2]*s, m[1][3]*s, | ||
| 319 | m[2][0]*s, m[2][1]*s, m[2][2]*s, m[2][3]*s, | ||
| 320 | m[3][0]*s, m[3][1]*s, m[3][2]*s, m[3][3]*s); | ||
| 321 | } | ||
| 322 | |||
| 323 | //! Operator for Matrix4x4 Scale = float * Matrix4x4; | ||
| 324 | inline_ friend Matrix4x4 operator*(float s, const Matrix4x4& mat) | ||
| 325 | { | ||
| 326 | return Matrix4x4( | ||
| 327 | s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2], s*mat.m[0][3], | ||
| 328 | s*mat.m[1][0], s*mat.m[1][1], s*mat.m[1][2], s*mat.m[1][3], | ||
| 329 | s*mat.m[2][0], s*mat.m[2][1], s*mat.m[2][2], s*mat.m[2][3], | ||
| 330 | s*mat.m[3][0], s*mat.m[3][1], s*mat.m[3][2], s*mat.m[3][3]); | ||
| 331 | } | ||
| 332 | |||
| 333 | //! Operator for Matrix4x4 Div = Matrix4x4 / float; | ||
| 334 | inline_ Matrix4x4 operator/(float s) const | ||
| 335 | { | ||
| 336 | if(s) s = 1.0f / s; | ||
| 337 | |||
| 338 | return Matrix4x4( | ||
| 339 | m[0][0]*s, m[0][1]*s, m[0][2]*s, m[0][3]*s, | ||
| 340 | m[1][0]*s, m[1][1]*s, m[1][2]*s, m[1][3]*s, | ||
| 341 | m[2][0]*s, m[2][1]*s, m[2][2]*s, m[2][3]*s, | ||
| 342 | m[3][0]*s, m[3][1]*s, m[3][2]*s, m[3][3]*s); | ||
| 343 | } | ||
| 344 | |||
| 345 | //! Operator for Matrix4x4 Div = float / Matrix4x4; | ||
| 346 | inline_ friend Matrix4x4 operator/(float s, const Matrix4x4& mat) | ||
| 347 | { | ||
| 348 | return Matrix4x4( | ||
| 349 | s/mat.m[0][0], s/mat.m[0][1], s/mat.m[0][2], s/mat.m[0][3], | ||
| 350 | s/mat.m[1][0], s/mat.m[1][1], s/mat.m[1][2], s/mat.m[1][3], | ||
| 351 | s/mat.m[2][0], s/mat.m[2][1], s/mat.m[2][2], s/mat.m[2][3], | ||
| 352 | s/mat.m[3][0], s/mat.m[3][1], s/mat.m[3][2], s/mat.m[3][3]); | ||
| 353 | } | ||
| 354 | |||
| 355 | //! Operator for Matrix4x4 += Matrix4x4; | ||
| 356 | inline_ Matrix4x4& operator+=(const Matrix4x4& mat) | ||
| 357 | { | ||
| 358 | m[0][0]+=mat.m[0][0]; m[0][1]+=mat.m[0][1]; m[0][2]+=mat.m[0][2]; m[0][3]+=mat.m[0][3]; | ||
| 359 | m[1][0]+=mat.m[1][0]; m[1][1]+=mat.m[1][1]; m[1][2]+=mat.m[1][2]; m[1][3]+=mat.m[1][3]; | ||
| 360 | m[2][0]+=mat.m[2][0]; m[2][1]+=mat.m[2][1]; m[2][2]+=mat.m[2][2]; m[2][3]+=mat.m[2][3]; | ||
| 361 | m[3][0]+=mat.m[3][0]; m[3][1]+=mat.m[3][1]; m[3][2]+=mat.m[3][2]; m[3][3]+=mat.m[3][3]; | ||
| 362 | return *this; | ||
| 363 | } | ||
| 364 | |||
| 365 | //! Operator for Matrix4x4 -= Matrix4x4; | ||
| 366 | inline_ Matrix4x4& operator-=(const Matrix4x4& mat) | ||
| 367 | { | ||
| 368 | m[0][0]-=mat.m[0][0]; m[0][1]-=mat.m[0][1]; m[0][2]-=mat.m[0][2]; m[0][3]-=mat.m[0][3]; | ||
| 369 | m[1][0]-=mat.m[1][0]; m[1][1]-=mat.m[1][1]; m[1][2]-=mat.m[1][2]; m[1][3]-=mat.m[1][3]; | ||
| 370 | m[2][0]-=mat.m[2][0]; m[2][1]-=mat.m[2][1]; m[2][2]-=mat.m[2][2]; m[2][3]-=mat.m[2][3]; | ||
| 371 | m[3][0]-=mat.m[3][0]; m[3][1]-=mat.m[3][1]; m[3][2]-=mat.m[3][2]; m[3][3]-=mat.m[3][3]; | ||
| 372 | return *this; | ||
| 373 | } | ||
| 374 | |||
| 375 | //! Operator for Matrix4x4 *= Matrix4x4; | ||
| 376 | Matrix4x4& operator*=(const Matrix4x4& mat) | ||
| 377 | { | ||
| 378 | HPoint TempRow; | ||
| 379 | |||
| 380 | GetRow(0, TempRow); | ||
| 381 | m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; | ||
| 382 | m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; | ||
| 383 | m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; | ||
| 384 | m[0][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; | ||
| 385 | |||
| 386 | GetRow(1, TempRow); | ||
| 387 | m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; | ||
| 388 | m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; | ||
| 389 | m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; | ||
| 390 | m[1][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; | ||
| 391 | |||
| 392 | GetRow(2, TempRow); | ||
| 393 | m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; | ||
| 394 | m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; | ||
| 395 | m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; | ||
| 396 | m[2][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; | ||
| 397 | |||
| 398 | GetRow(3, TempRow); | ||
| 399 | m[3][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0]; | ||
| 400 | m[3][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1]; | ||
| 401 | m[3][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2]; | ||
| 402 | m[3][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3]; | ||
| 403 | |||
| 404 | return *this; | ||
| 405 | } | ||
| 406 | |||
| 407 | //! Operator for Matrix4x4 *= float; | ||
| 408 | inline_ Matrix4x4& operator*=(float s) | ||
| 409 | { | ||
| 410 | m[0][0]*=s; m[0][1]*=s; m[0][2]*=s; m[0][3]*=s; | ||
| 411 | m[1][0]*=s; m[1][1]*=s; m[1][2]*=s; m[1][3]*=s; | ||
| 412 | m[2][0]*=s; m[2][1]*=s; m[2][2]*=s; m[2][3]*=s; | ||
| 413 | m[3][0]*=s; m[3][1]*=s; m[3][2]*=s; m[3][3]*=s; | ||
| 414 | return *this; | ||
| 415 | } | ||
| 416 | |||
| 417 | //! Operator for Matrix4x4 /= float; | ||
| 418 | inline_ Matrix4x4& operator/=(float s) | ||
| 419 | { | ||
| 420 | if(s) s = 1.0f / s; | ||
| 421 | m[0][0]*=s; m[0][1]*=s; m[0][2]*=s; m[0][3]*=s; | ||
| 422 | m[1][0]*=s; m[1][1]*=s; m[1][2]*=s; m[1][3]*=s; | ||
| 423 | m[2][0]*=s; m[2][1]*=s; m[2][2]*=s; m[2][3]*=s; | ||
| 424 | m[3][0]*=s; m[3][1]*=s; m[3][2]*=s; m[3][3]*=s; | ||
| 425 | return *this; | ||
| 426 | } | ||
| 427 | |||
| 428 | inline_ const HPoint& operator[](int row) const { return *(const HPoint*)&m[row][0]; } | ||
| 429 | inline_ HPoint& operator[](int row) { return *(HPoint*)&m[row][0]; } | ||
| 430 | |||
| 431 | public: | ||
| 432 | |||
| 433 | float m[4][4]; | ||
| 434 | }; | ||
| 435 | |||
| 436 | //! Quickly rotates & translates a vector, using the 4x3 part of a 4x4 matrix | ||
| 437 | inline_ void TransformPoint4x3(Point& dest, const Point& source, const Matrix4x4& rot) | ||
| 438 | { | ||
| 439 | dest.x = rot.m[3][0] + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0]; | ||
| 440 | dest.y = rot.m[3][1] + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1]; | ||
| 441 | dest.z = rot.m[3][2] + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2]; | ||
| 442 | } | ||
| 443 | |||
| 444 | //! Quickly rotates a vector, using the 3x3 part of a 4x4 matrix | ||
| 445 | inline_ void TransformPoint3x3(Point& dest, const Point& source, const Matrix4x4& rot) | ||
| 446 | { | ||
| 447 | dest.x = source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0]; | ||
| 448 | dest.y = source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1]; | ||
| 449 | dest.z = source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2]; | ||
| 450 | } | ||
| 451 | |||
| 452 | ICEMATHS_API void InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src); | ||
| 453 | |||
| 454 | #endif // __ICEMATRIX4X4_H__ | ||
| 455 | |||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceMemoryMacros.h b/libraries/ode-0.9/OPCODE/Ice/IceMemoryMacros.h new file mode 100644 index 0000000..6386333 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceMemoryMacros.h | |||
| @@ -0,0 +1,109 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains all memory macros. | ||
| 4 | * \file IceMemoryMacros.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEMEMORYMACROS_H__ | ||
| 13 | #define __ICEMEMORYMACROS_H__ | ||
| 14 | |||
| 15 | #undef ZeroMemory | ||
| 16 | #undef CopyMemory | ||
| 17 | #undef MoveMemory | ||
| 18 | #undef FillMemory | ||
| 19 | |||
| 20 | //! Clears a buffer. | ||
| 21 | //! \param addr [in] buffer address | ||
| 22 | //! \param size [in] buffer length | ||
| 23 | //! \see FillMemory | ||
| 24 | //! \see StoreDwords | ||
| 25 | //! \see CopyMemory | ||
| 26 | //! \see MoveMemory | ||
| 27 | inline_ void ZeroMemory(void* addr, udword size) { memset(addr, 0, size); } | ||
| 28 | |||
| 29 | //! Fills a buffer with a given byte. | ||
| 30 | //! \param addr [in] buffer address | ||
| 31 | //! \param size [in] buffer length | ||
| 32 | //! \param val [in] the byte value | ||
| 33 | //! \see StoreDwords | ||
| 34 | //! \see ZeroMemory | ||
| 35 | //! \see CopyMemory | ||
| 36 | //! \see MoveMemory | ||
| 37 | inline_ void FillMemory(void* dest, udword size, ubyte val) { memset(dest, val, size); } | ||
| 38 | |||
| 39 | //! Fills a buffer with a given dword. | ||
| 40 | //! \param addr [in] buffer address | ||
| 41 | //! \param nb [in] number of dwords to write | ||
| 42 | //! \param value [in] the dword value | ||
| 43 | //! \see FillMemory | ||
| 44 | //! \see ZeroMemory | ||
| 45 | //! \see CopyMemory | ||
| 46 | //! \see MoveMemory | ||
| 47 | //! \warning writes nb*4 bytes ! | ||
| 48 | inline_ void StoreDwords(udword* dest, udword nb, udword value) | ||
| 49 | { | ||
| 50 | // The asm code below **SHOULD** be equivalent to one of those C versions | ||
| 51 | // or the other if your compiled is good: (checked on VC++ 6.0) | ||
| 52 | // | ||
| 53 | // 1) while(nb--) *dest++ = value; | ||
| 54 | // | ||
| 55 | // 2) for(udword i=0;i<nb;i++) dest[i] = value; | ||
| 56 | // | ||
| 57 | #ifdef _MSC_VER | ||
| 58 | _asm push eax | ||
| 59 | _asm push ecx | ||
| 60 | _asm push edi | ||
| 61 | _asm mov edi, dest | ||
| 62 | _asm mov ecx, nb | ||
| 63 | _asm mov eax, value | ||
| 64 | _asm rep stosd | ||
| 65 | _asm pop edi | ||
| 66 | _asm pop ecx | ||
| 67 | _asm pop eax | ||
| 68 | #else | ||
| 69 | while(nb--) *dest++ = value; | ||
| 70 | #endif | ||
| 71 | } | ||
| 72 | |||
| 73 | //! Copies a buffer. | ||
| 74 | //! \param addr [in] destination buffer address | ||
| 75 | //! \param addr [in] source buffer address | ||
| 76 | //! \param size [in] buffer length | ||
| 77 | //! \see ZeroMemory | ||
| 78 | //! \see FillMemory | ||
| 79 | //! \see StoreDwords | ||
| 80 | //! \see MoveMemory | ||
| 81 | inline_ void CopyMemory(void* dest, const void* src, udword size) { memcpy(dest, src, size); } | ||
| 82 | |||
| 83 | //! Moves a buffer. | ||
| 84 | //! \param addr [in] destination buffer address | ||
| 85 | //! \param addr [in] source buffer address | ||
| 86 | //! \param size [in] buffer length | ||
| 87 | //! \see ZeroMemory | ||
| 88 | //! \see FillMemory | ||
| 89 | //! \see StoreDwords | ||
| 90 | //! \see CopyMemory | ||
| 91 | inline_ void MoveMemory(void* dest, const void* src, udword size) { memmove(dest, src, size); } | ||
| 92 | |||
| 93 | #define SIZEOFOBJECT sizeof(*this) //!< Gives the size of current object. Avoid some mistakes (e.g. "sizeof(this)"). | ||
| 94 | //#define CLEAROBJECT { memset(this, 0, SIZEOFOBJECT); } //!< Clears current object. Laziness is my business. HANDLE WITH CARE. | ||
| 95 | #define DELETESINGLE(x) if (x) { delete x; x = null; } //!< Deletes an instance of a class. | ||
| 96 | #define DELETEARRAY(x) if (x) { delete []x; x = null; } //!< Deletes an array. | ||
| 97 | #define SAFE_RELEASE(x) if (x) { (x)->Release(); (x) = null; } //!< Safe D3D-style release | ||
| 98 | #define SAFE_DESTRUCT(x) if (x) { (x)->SelfDestruct(); (x) = null; } //!< Safe ICE-style release | ||
| 99 | |||
| 100 | #ifdef __ICEERROR_H__ | ||
| 101 | #define CHECKALLOC(x) if(!x) return SetIceError("Out of memory.", EC_OUT_OF_MEMORY); //!< Standard alloc checking. HANDLE WITH CARE. | ||
| 102 | #else | ||
| 103 | #define CHECKALLOC(x) if(!x) return false; | ||
| 104 | #endif | ||
| 105 | |||
| 106 | //! Standard allocation cycle | ||
| 107 | #define SAFE_ALLOC(ptr, type, count) DELETEARRAY(ptr); ptr = new type[count]; CHECKALLOC(ptr); | ||
| 108 | |||
| 109 | #endif // __ICEMEMORYMACROS_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp b/libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp new file mode 100644 index 0000000..0b1b6f7 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceOBB.cpp | |||
| @@ -0,0 +1,323 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains OBB-related code. | ||
| 4 | * \file IceOBB.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 29, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * An Oriented Bounding Box (OBB). | ||
| 13 | * \class OBB | ||
| 14 | * \author Pierre Terdiman | ||
| 15 | * \version 1.0 | ||
| 16 | */ | ||
| 17 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 18 | |||
| 19 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 20 | // Precompiled Header | ||
| 21 | #include "Stdafx.h" | ||
| 22 | |||
| 23 | using namespace IceMaths; | ||
| 24 | |||
| 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 26 | /** | ||
| 27 | * Tests if a point is contained within the OBB. | ||
| 28 | * \param p [in] the world point to test | ||
| 29 | * \return true if inside the OBB | ||
| 30 | */ | ||
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 32 | bool OBB::ContainsPoint(const Point& p) const | ||
| 33 | { | ||
| 34 | // Point in OBB test using lazy evaluation and early exits | ||
| 35 | |||
| 36 | // Translate to box space | ||
| 37 | Point RelPoint = p - mCenter; | ||
| 38 | |||
| 39 | // Point * mRot maps from box space to world space | ||
| 40 | // mRot * Point maps from world space to box space (what we need here) | ||
| 41 | |||
| 42 | float f = mRot.m[0][0] * RelPoint.x + mRot.m[0][1] * RelPoint.y + mRot.m[0][2] * RelPoint.z; | ||
| 43 | if(f >= mExtents.x || f <= -mExtents.x) return false; | ||
| 44 | |||
| 45 | f = mRot.m[1][0] * RelPoint.x + mRot.m[1][1] * RelPoint.y + mRot.m[1][2] * RelPoint.z; | ||
| 46 | if(f >= mExtents.y || f <= -mExtents.y) return false; | ||
| 47 | |||
| 48 | f = mRot.m[2][0] * RelPoint.x + mRot.m[2][1] * RelPoint.y + mRot.m[2][2] * RelPoint.z; | ||
| 49 | if(f >= mExtents.z || f <= -mExtents.z) return false; | ||
| 50 | return true; | ||
| 51 | } | ||
| 52 | |||
| 53 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 54 | /** | ||
| 55 | * Builds an OBB from an AABB and a world transform. | ||
| 56 | * \param aabb [in] the aabb | ||
| 57 | * \param mat [in] the world transform | ||
| 58 | */ | ||
| 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 60 | void OBB::Create(const AABB& aabb, const Matrix4x4& mat) | ||
| 61 | { | ||
| 62 | // Note: must be coherent with Rotate() | ||
| 63 | |||
| 64 | aabb.GetCenter(mCenter); | ||
| 65 | aabb.GetExtents(mExtents); | ||
| 66 | // Here we have the same as OBB::Rotate(mat) where the obb is (mCenter, mExtents, Identity). | ||
| 67 | |||
| 68 | // So following what's done in Rotate: | ||
| 69 | // - x-form the center | ||
| 70 | mCenter *= mat; | ||
| 71 | // - combine rotation with identity, i.e. just use given matrix | ||
| 72 | mRot = mat; | ||
| 73 | } | ||
| 74 | |||
| 75 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 76 | /** | ||
| 77 | * Computes the obb planes. | ||
| 78 | * \param planes [out] 6 box planes | ||
| 79 | * \return true if success | ||
| 80 | */ | ||
| 81 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 82 | bool OBB::ComputePlanes(Plane* planes) const | ||
| 83 | { | ||
| 84 | // Checkings | ||
| 85 | if(!planes) return false; | ||
| 86 | |||
| 87 | Point Axis0 = mRot[0]; | ||
| 88 | Point Axis1 = mRot[1]; | ||
| 89 | Point Axis2 = mRot[2]; | ||
| 90 | |||
| 91 | // Writes normals | ||
| 92 | planes[0].n = Axis0; | ||
| 93 | planes[1].n = -Axis0; | ||
| 94 | planes[2].n = Axis1; | ||
| 95 | planes[3].n = -Axis1; | ||
| 96 | planes[4].n = Axis2; | ||
| 97 | planes[5].n = -Axis2; | ||
| 98 | |||
| 99 | // Compute a point on each plane | ||
| 100 | Point p0 = mCenter + Axis0 * mExtents.x; | ||
| 101 | Point p1 = mCenter - Axis0 * mExtents.x; | ||
| 102 | Point p2 = mCenter + Axis1 * mExtents.y; | ||
| 103 | Point p3 = mCenter - Axis1 * mExtents.y; | ||
| 104 | Point p4 = mCenter + Axis2 * mExtents.z; | ||
| 105 | Point p5 = mCenter - Axis2 * mExtents.z; | ||
| 106 | |||
| 107 | // Compute d | ||
| 108 | planes[0].d = -(planes[0].n|p0); | ||
| 109 | planes[1].d = -(planes[1].n|p1); | ||
| 110 | planes[2].d = -(planes[2].n|p2); | ||
| 111 | planes[3].d = -(planes[3].n|p3); | ||
| 112 | planes[4].d = -(planes[4].n|p4); | ||
| 113 | planes[5].d = -(planes[5].n|p5); | ||
| 114 | |||
| 115 | return true; | ||
| 116 | } | ||
| 117 | |||
| 118 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 119 | /** | ||
| 120 | * Computes the obb points. | ||
| 121 | * \param pts [out] 8 box points | ||
| 122 | * \return true if success | ||
| 123 | */ | ||
| 124 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 125 | bool OBB::ComputePoints(Point* pts) const | ||
| 126 | { | ||
| 127 | // Checkings | ||
| 128 | if(!pts) return false; | ||
| 129 | |||
| 130 | Point Axis0 = mRot[0]; | ||
| 131 | Point Axis1 = mRot[1]; | ||
| 132 | Point Axis2 = mRot[2]; | ||
| 133 | |||
| 134 | Axis0 *= mExtents.x; | ||
| 135 | Axis1 *= mExtents.y; | ||
| 136 | Axis2 *= mExtents.z; | ||
| 137 | |||
| 138 | // 7+------+6 0 = --- | ||
| 139 | // /| /| 1 = +-- | ||
| 140 | // / | / | 2 = ++- | ||
| 141 | // / 4+---/--+5 3 = -+- | ||
| 142 | // 3+------+2 / y z 4 = --+ | ||
| 143 | // | / | / | / 5 = +-+ | ||
| 144 | // |/ |/ |/ 6 = +++ | ||
| 145 | // 0+------+1 *---x 7 = -++ | ||
| 146 | |||
| 147 | pts[0] = mCenter - Axis0 - Axis1 - Axis2; | ||
| 148 | pts[1] = mCenter + Axis0 - Axis1 - Axis2; | ||
| 149 | pts[2] = mCenter + Axis0 + Axis1 - Axis2; | ||
| 150 | pts[3] = mCenter - Axis0 + Axis1 - Axis2; | ||
| 151 | pts[4] = mCenter - Axis0 - Axis1 + Axis2; | ||
| 152 | pts[5] = mCenter + Axis0 - Axis1 + Axis2; | ||
| 153 | pts[6] = mCenter + Axis0 + Axis1 + Axis2; | ||
| 154 | pts[7] = mCenter - Axis0 + Axis1 + Axis2; | ||
| 155 | |||
| 156 | return true; | ||
| 157 | } | ||
| 158 | |||
| 159 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 160 | /** | ||
| 161 | * Computes vertex normals. | ||
| 162 | * \param pts [out] 8 box points | ||
| 163 | * \return true if success | ||
| 164 | */ | ||
| 165 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 166 | bool OBB::ComputeVertexNormals(Point* pts) const | ||
| 167 | { | ||
| 168 | static float VertexNormals[] = | ||
| 169 | { | ||
| 170 | -INVSQRT3, -INVSQRT3, -INVSQRT3, | ||
| 171 | INVSQRT3, -INVSQRT3, -INVSQRT3, | ||
| 172 | INVSQRT3, INVSQRT3, -INVSQRT3, | ||
| 173 | -INVSQRT3, INVSQRT3, -INVSQRT3, | ||
| 174 | -INVSQRT3, -INVSQRT3, INVSQRT3, | ||
| 175 | INVSQRT3, -INVSQRT3, INVSQRT3, | ||
| 176 | INVSQRT3, INVSQRT3, INVSQRT3, | ||
| 177 | -INVSQRT3, INVSQRT3, INVSQRT3 | ||
| 178 | }; | ||
| 179 | |||
| 180 | if(!pts) return false; | ||
| 181 | |||
| 182 | const Point* VN = (const Point*)VertexNormals; | ||
| 183 | for(udword i=0;i<8;i++) | ||
| 184 | { | ||
| 185 | pts[i] = VN[i] * mRot; | ||
| 186 | } | ||
| 187 | |||
| 188 | return true; | ||
| 189 | } | ||
| 190 | |||
| 191 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 192 | /** | ||
| 193 | * Returns edges. | ||
| 194 | * \return 24 indices (12 edges) indexing the list returned by ComputePoints() | ||
| 195 | */ | ||
| 196 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 197 | const udword* OBB::GetEdges() const | ||
| 198 | { | ||
| 199 | static udword Indices[] = { | ||
| 200 | 0, 1, 1, 2, 2, 3, 3, 0, | ||
| 201 | 7, 6, 6, 5, 5, 4, 4, 7, | ||
| 202 | 1, 5, 6, 2, | ||
| 203 | 3, 7, 4, 0 | ||
| 204 | }; | ||
| 205 | return Indices; | ||
| 206 | } | ||
| 207 | |||
| 208 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 209 | /** | ||
| 210 | * Returns local edge normals. | ||
| 211 | * \return edge normals in local space | ||
| 212 | */ | ||
| 213 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 214 | const Point* OBB::GetLocalEdgeNormals() const | ||
| 215 | { | ||
| 216 | static float EdgeNormals[] = | ||
| 217 | { | ||
| 218 | 0, -INVSQRT2, -INVSQRT2, // 0-1 | ||
| 219 | INVSQRT2, 0, -INVSQRT2, // 1-2 | ||
| 220 | 0, INVSQRT2, -INVSQRT2, // 2-3 | ||
| 221 | -INVSQRT2, 0, -INVSQRT2, // 3-0 | ||
| 222 | |||
| 223 | 0, INVSQRT2, INVSQRT2, // 7-6 | ||
| 224 | INVSQRT2, 0, INVSQRT2, // 6-5 | ||
| 225 | 0, -INVSQRT2, INVSQRT2, // 5-4 | ||
| 226 | -INVSQRT2, 0, INVSQRT2, // 4-7 | ||
| 227 | |||
| 228 | INVSQRT2, -INVSQRT2, 0, // 1-5 | ||
| 229 | INVSQRT2, INVSQRT2, 0, // 6-2 | ||
| 230 | -INVSQRT2, INVSQRT2, 0, // 3-7 | ||
| 231 | -INVSQRT2, -INVSQRT2, 0 // 4-0 | ||
| 232 | }; | ||
| 233 | return (const Point*)EdgeNormals; | ||
| 234 | } | ||
| 235 | |||
| 236 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 237 | /** | ||
| 238 | * Returns world edge normal | ||
| 239 | * \param edge_index [in] 0 <= edge index < 12 | ||
| 240 | * \param world_normal [out] edge normal in world space | ||
| 241 | */ | ||
| 242 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 243 | void OBB::ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const | ||
| 244 | { | ||
| 245 | ASSERT(edge_index<12); | ||
| 246 | world_normal = GetLocalEdgeNormals()[edge_index] * mRot; | ||
| 247 | } | ||
| 248 | |||
| 249 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 250 | /** | ||
| 251 | * Computes an LSS surrounding the OBB. | ||
| 252 | * \param lss [out] the LSS | ||
| 253 | */ | ||
| 254 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 255 | void OBB::ComputeLSS(LSS& lss) const | ||
| 256 | { | ||
| 257 | Point Axis0 = mRot[0]; | ||
| 258 | Point Axis1 = mRot[1]; | ||
| 259 | Point Axis2 = mRot[2]; | ||
| 260 | |||
| 261 | switch(mExtents.LargestAxis()) | ||
| 262 | { | ||
| 263 | case 0: | ||
| 264 | lss.mRadius = (mExtents.y + mExtents.z)*0.5f; | ||
| 265 | lss.mP0 = mCenter + Axis0 * (mExtents.x - lss.mRadius); | ||
| 266 | lss.mP1 = mCenter - Axis0 * (mExtents.x - lss.mRadius); | ||
| 267 | break; | ||
| 268 | case 1: | ||
| 269 | lss.mRadius = (mExtents.x + mExtents.z)*0.5f; | ||
| 270 | lss.mP0 = mCenter + Axis1 * (mExtents.y - lss.mRadius); | ||
| 271 | lss.mP1 = mCenter - Axis1 * (mExtents.y - lss.mRadius); | ||
| 272 | break; | ||
| 273 | case 2: | ||
| 274 | lss.mRadius = (mExtents.x + mExtents.y)*0.5f; | ||
| 275 | lss.mP0 = mCenter + Axis2 * (mExtents.z - lss.mRadius); | ||
| 276 | lss.mP1 = mCenter - Axis2 * (mExtents.z - lss.mRadius); | ||
| 277 | break; | ||
| 278 | } | ||
| 279 | } | ||
| 280 | |||
| 281 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 282 | /** | ||
| 283 | * Checks the OBB is inside another OBB. | ||
| 284 | * \param box [in] the other OBB | ||
| 285 | * \return TRUE if we're inside the other box | ||
| 286 | */ | ||
| 287 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 288 | BOOL OBB::IsInside(const OBB& box) const | ||
| 289 | { | ||
| 290 | // Make a 4x4 from the box & inverse it | ||
| 291 | Matrix4x4 M0Inv; | ||
| 292 | { | ||
| 293 | Matrix4x4 M0 = box.mRot; | ||
| 294 | M0.SetTrans(box.mCenter); | ||
| 295 | InvertPRMatrix(M0Inv, M0); | ||
| 296 | } | ||
| 297 | |||
| 298 | // With our inversed 4x4, create box1 in space of box0 | ||
| 299 | OBB _1in0; | ||
| 300 | Rotate(M0Inv, _1in0); | ||
| 301 | |||
| 302 | // This should cancel out box0's rotation, i.e. it's now an AABB. | ||
| 303 | // => Center(0,0,0), Rot(identity) | ||
| 304 | |||
| 305 | // The two boxes are in the same space so now we can compare them. | ||
| 306 | |||
| 307 | // Create the AABB of (box1 in space of box0) | ||
| 308 | const Matrix3x3& mtx = _1in0.mRot; | ||
| 309 | |||
| 310 | float f = fabsf(mtx.m[0][0] * mExtents.x) + fabsf(mtx.m[1][0] * mExtents.y) + fabsf(mtx.m[2][0] * mExtents.z) - box.mExtents.x; | ||
| 311 | if(f > _1in0.mCenter.x) return FALSE; | ||
| 312 | if(-f < _1in0.mCenter.x) return FALSE; | ||
| 313 | |||
| 314 | f = fabsf(mtx.m[0][1] * mExtents.x) + fabsf(mtx.m[1][1] * mExtents.y) + fabsf(mtx.m[2][1] * mExtents.z) - box.mExtents.y; | ||
| 315 | if(f > _1in0.mCenter.y) return FALSE; | ||
| 316 | if(-f < _1in0.mCenter.y) return FALSE; | ||
| 317 | |||
| 318 | f = fabsf(mtx.m[0][2] * mExtents.x) + fabsf(mtx.m[1][2] * mExtents.y) + fabsf(mtx.m[2][2] * mExtents.z) - box.mExtents.z; | ||
| 319 | if(f > _1in0.mCenter.z) return FALSE; | ||
| 320 | if(-f < _1in0.mCenter.z) return FALSE; | ||
| 321 | |||
| 322 | return TRUE; | ||
| 323 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceOBB.h b/libraries/ode-0.9/OPCODE/Ice/IceOBB.h new file mode 100644 index 0000000..d6cf43e --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceOBB.h | |||
| @@ -0,0 +1,177 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains OBB-related code. (oriented bounding box) | ||
| 4 | * \file IceOBB.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 13, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEOBB_H__ | ||
| 13 | #define __ICEOBB_H__ | ||
| 14 | |||
| 15 | // Forward declarations | ||
| 16 | class LSS; | ||
| 17 | |||
| 18 | class ICEMATHS_API OBB | ||
| 19 | { | ||
| 20 | public: | ||
| 21 | //! Constructor | ||
| 22 | inline_ OBB() {} | ||
| 23 | //! Constructor | ||
| 24 | inline_ OBB(const Point& center, const Point& extents, const Matrix3x3& rot) : mCenter(center), mExtents(extents), mRot(rot) {} | ||
| 25 | //! Destructor | ||
| 26 | inline_ ~OBB() {} | ||
| 27 | |||
| 28 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 29 | /** | ||
| 30 | * Setups an empty OBB. | ||
| 31 | */ | ||
| 32 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 33 | void SetEmpty() | ||
| 34 | { | ||
| 35 | mCenter.Zero(); | ||
| 36 | mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); | ||
| 37 | mRot.Identity(); | ||
| 38 | } | ||
| 39 | |||
| 40 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 41 | /** | ||
| 42 | * Tests if a point is contained within the OBB. | ||
| 43 | * \param p [in] the world point to test | ||
| 44 | * \return true if inside the OBB | ||
| 45 | */ | ||
| 46 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 47 | bool ContainsPoint(const Point& p) const; | ||
| 48 | |||
| 49 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 50 | /** | ||
| 51 | * Builds an OBB from an AABB and a world transform. | ||
| 52 | * \param aabb [in] the aabb | ||
| 53 | * \param mat [in] the world transform | ||
| 54 | */ | ||
| 55 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 56 | void Create(const AABB& aabb, const Matrix4x4& mat); | ||
| 57 | |||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | /** | ||
| 60 | * Recomputes the OBB after an arbitrary transform by a 4x4 matrix. | ||
| 61 | * \param mtx [in] the transform matrix | ||
| 62 | * \param obb [out] the transformed OBB | ||
| 63 | */ | ||
| 64 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 65 | inline_ void Rotate(const Matrix4x4& mtx, OBB& obb) const | ||
| 66 | { | ||
| 67 | // The extents remain constant | ||
| 68 | obb.mExtents = mExtents; | ||
| 69 | // The center gets x-formed | ||
| 70 | obb.mCenter = mCenter * mtx; | ||
| 71 | // Combine rotations | ||
| 72 | obb.mRot = mRot * Matrix3x3(mtx); | ||
| 73 | } | ||
| 74 | |||
| 75 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 76 | /** | ||
| 77 | * Checks the OBB is valid. | ||
| 78 | * \return true if the box is valid | ||
| 79 | */ | ||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | inline_ BOOL IsValid() const | ||
| 82 | { | ||
| 83 | // Consistency condition for (Center, Extents) boxes: Extents >= 0.0f | ||
| 84 | if(mExtents.x < 0.0f) return FALSE; | ||
| 85 | if(mExtents.y < 0.0f) return FALSE; | ||
| 86 | if(mExtents.z < 0.0f) return FALSE; | ||
| 87 | return TRUE; | ||
| 88 | } | ||
| 89 | |||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | /** | ||
| 92 | * Computes the obb planes. | ||
| 93 | * \param planes [out] 6 box planes | ||
| 94 | * \return true if success | ||
| 95 | */ | ||
| 96 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 97 | bool ComputePlanes(Plane* planes) const; | ||
| 98 | |||
| 99 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 100 | /** | ||
| 101 | * Computes the obb points. | ||
| 102 | * \param pts [out] 8 box points | ||
| 103 | * \return true if success | ||
| 104 | */ | ||
| 105 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 106 | bool ComputePoints(Point* pts) const; | ||
| 107 | |||
| 108 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 109 | /** | ||
| 110 | * Computes vertex normals. | ||
| 111 | * \param pts [out] 8 box points | ||
| 112 | * \return true if success | ||
| 113 | */ | ||
| 114 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 115 | bool ComputeVertexNormals(Point* pts) const; | ||
| 116 | |||
| 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 118 | /** | ||
| 119 | * Returns edges. | ||
| 120 | * \return 24 indices (12 edges) indexing the list returned by ComputePoints() | ||
| 121 | */ | ||
| 122 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 123 | const udword* GetEdges() const; | ||
| 124 | |||
| 125 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 126 | /** | ||
| 127 | * Returns local edge normals. | ||
| 128 | * \return edge normals in local space | ||
| 129 | */ | ||
| 130 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 131 | const Point* GetLocalEdgeNormals() const; | ||
| 132 | |||
| 133 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 134 | /** | ||
| 135 | * Returns world edge normal | ||
| 136 | * \param edge_index [in] 0 <= edge index < 12 | ||
| 137 | * \param world_normal [out] edge normal in world space | ||
| 138 | */ | ||
| 139 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 140 | void ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const; | ||
| 141 | |||
| 142 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 143 | /** | ||
| 144 | * Computes an LSS surrounding the OBB. | ||
| 145 | * \param lss [out] the LSS | ||
| 146 | */ | ||
| 147 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 148 | void ComputeLSS(LSS& lss) const; | ||
| 149 | |||
| 150 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 151 | /** | ||
| 152 | * Checks the OBB is inside another OBB. | ||
| 153 | * \param box [in] the other OBB | ||
| 154 | * \return TRUE if we're inside the other box | ||
| 155 | */ | ||
| 156 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 157 | BOOL IsInside(const OBB& box) const; | ||
| 158 | |||
| 159 | inline_ const Point& GetCenter() const { return mCenter; } | ||
| 160 | inline_ const Point& GetExtents() const { return mExtents; } | ||
| 161 | inline_ const Matrix3x3& GetRot() const { return mRot; } | ||
| 162 | |||
| 163 | inline_ void GetRotatedExtents(Matrix3x3& extents) const | ||
| 164 | { | ||
| 165 | extents = mRot; | ||
| 166 | extents.Scale(mExtents); | ||
| 167 | } | ||
| 168 | |||
| 169 | Point mCenter; //!< B for Box | ||
| 170 | Point mExtents; //!< B for Bounding | ||
| 171 | Matrix3x3 mRot; //!< O for Oriented | ||
| 172 | |||
| 173 | // Orientation is stored in row-major format, | ||
| 174 | // i.e. rows = eigen vectors of the covariance matrix | ||
| 175 | }; | ||
| 176 | |||
| 177 | #endif // __ICEOBB_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IcePairs.h b/libraries/ode-0.9/OPCODE/Ice/IcePairs.h new file mode 100644 index 0000000..2c09b92 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IcePairs.h | |||
| @@ -0,0 +1,45 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains a simple pair class. | ||
| 4 | * \file IcePairs.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 13, 2003 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEPAIRS_H__ | ||
| 13 | #define __ICEPAIRS_H__ | ||
| 14 | |||
| 15 | //! A generic couple structure | ||
| 16 | struct ICECORE_API Pair | ||
| 17 | { | ||
| 18 | inline_ Pair() {} | ||
| 19 | inline_ Pair(udword i0, udword i1) : id0(i0), id1(i1) {} | ||
| 20 | |||
| 21 | udword id0; //!< First index of the pair | ||
| 22 | udword id1; //!< Second index of the pair | ||
| 23 | }; | ||
| 24 | |||
| 25 | class ICECORE_API Pairs : private Container | ||
| 26 | { | ||
| 27 | public: | ||
| 28 | // Constructor / Destructor | ||
| 29 | Pairs() {} | ||
| 30 | ~Pairs() {} | ||
| 31 | |||
| 32 | inline_ udword GetNbPairs() const { return GetNbEntries()>>1; } | ||
| 33 | inline_ const Pair* GetPairs() const { return (const Pair*)GetEntries(); } | ||
| 34 | inline_ const Pair* GetPair(udword i) const { return (const Pair*)&GetEntries()[i+i]; } | ||
| 35 | |||
| 36 | inline_ BOOL HasPairs() const { return IsNotEmpty(); } | ||
| 37 | |||
| 38 | inline_ void ResetPairs() { Reset(); } | ||
| 39 | inline_ void DeleteLastPair() { DeleteLastEntry(); DeleteLastEntry(); } | ||
| 40 | |||
| 41 | inline_ void AddPair(const Pair& p) { Add(p.id0).Add(p.id1); } | ||
| 42 | inline_ void AddPair(udword id0, udword id1) { Add(id0).Add(id1); } | ||
| 43 | }; | ||
| 44 | |||
| 45 | #endif // __ICEPAIRS_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IcePlane.cpp b/libraries/ode-0.9/OPCODE/Ice/IcePlane.cpp new file mode 100644 index 0000000..394b31b --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IcePlane.cpp | |||
| @@ -0,0 +1,45 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for planes. | ||
| 4 | * \file IcePlane.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * Plane class. | ||
| 13 | * \class Plane | ||
| 14 | * \author Pierre Terdiman | ||
| 15 | * \version 1.0 | ||
| 16 | */ | ||
| 17 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 18 | |||
| 19 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 20 | // Precompiled Header | ||
| 21 | #include "Stdafx.h" | ||
| 22 | |||
| 23 | using namespace IceMaths; | ||
| 24 | |||
| 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 26 | /** | ||
| 27 | * Computes the plane equation from 3 points. | ||
| 28 | * \param p0 [in] first point | ||
| 29 | * \param p1 [in] second point | ||
| 30 | * \param p2 [in] third point | ||
| 31 | * \return Self-reference | ||
| 32 | */ | ||
| 33 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 34 | Plane& Plane::Set(const Point& p0, const Point& p1, const Point& p2) | ||
| 35 | { | ||
| 36 | Point Edge0 = p1 - p0; | ||
| 37 | Point Edge1 = p2 - p0; | ||
| 38 | |||
| 39 | n = Edge0 ^ Edge1; | ||
| 40 | n.Normalize(); | ||
| 41 | |||
| 42 | d = -(p0 | n); | ||
| 43 | |||
| 44 | return *this; | ||
| 45 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IcePlane.h b/libraries/ode-0.9/OPCODE/Ice/IcePlane.h new file mode 100644 index 0000000..4d47081 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IcePlane.h | |||
| @@ -0,0 +1,113 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for planes. | ||
| 4 | * \file IcePlane.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEPLANE_H__ | ||
| 13 | #define __ICEPLANE_H__ | ||
| 14 | |||
| 15 | #define PLANE_EPSILON (1.0e-7f) | ||
| 16 | |||
| 17 | class ICEMATHS_API Plane | ||
| 18 | { | ||
| 19 | public: | ||
| 20 | //! Constructor | ||
| 21 | inline_ Plane() { } | ||
| 22 | //! Constructor from a normal and a distance | ||
| 23 | inline_ Plane(float nx, float ny, float nz, float d) { Set(nx, ny, nz, d); } | ||
| 24 | //! Constructor from a point on the plane and a normal | ||
| 25 | inline_ Plane(const Point& p, const Point& n) { Set(p, n); } | ||
| 26 | //! Constructor from three points | ||
| 27 | inline_ Plane(const Point& p0, const Point& p1, const Point& p2) { Set(p0, p1, p2); } | ||
| 28 | //! Constructor from a normal and a distance | ||
| 29 | inline_ Plane(const Point& _n, float _d) { n = _n; d = _d; } | ||
| 30 | //! Copy constructor | ||
| 31 | inline_ Plane(const Plane& plane) : n(plane.n), d(plane.d) { } | ||
| 32 | //! Destructor | ||
| 33 | inline_ ~Plane() { } | ||
| 34 | |||
| 35 | inline_ Plane& Zero() { n.Zero(); d = 0.0f; return *this; } | ||
| 36 | inline_ Plane& Set(float nx, float ny, float nz, float _d) { n.Set(nx, ny, nz); d = _d; return *this; } | ||
| 37 | inline_ Plane& Set(const Point& p, const Point& _n) { n = _n; d = - p | _n; return *this; } | ||
| 38 | Plane& Set(const Point& p0, const Point& p1, const Point& p2); | ||
| 39 | |||
| 40 | inline_ float Distance(const Point& p) const { return (p | n) + d; } | ||
| 41 | inline_ bool Belongs(const Point& p) const { return fabsf(Distance(p)) < PLANE_EPSILON; } | ||
| 42 | |||
| 43 | inline_ void Normalize() | ||
| 44 | { | ||
| 45 | float Denom = 1.0f / n.Magnitude(); | ||
| 46 | n.x *= Denom; | ||
| 47 | n.y *= Denom; | ||
| 48 | n.z *= Denom; | ||
| 49 | d *= Denom; | ||
| 50 | } | ||
| 51 | public: | ||
| 52 | // Members | ||
| 53 | Point n; //!< The normal to the plane | ||
| 54 | float d; //!< The distance from the origin | ||
| 55 | |||
| 56 | // Cast operators | ||
| 57 | inline_ operator Point() const { return n; } | ||
| 58 | inline_ operator HPoint() const { return HPoint(n, d); } | ||
| 59 | |||
| 60 | // Arithmetic operators | ||
| 61 | inline_ Plane operator*(const Matrix4x4& m) const | ||
| 62 | { | ||
| 63 | // Old code from Irion. Kept for reference. | ||
| 64 | Plane Ret(*this); | ||
| 65 | return Ret *= m; | ||
| 66 | } | ||
| 67 | |||
| 68 | inline_ Plane& operator*=(const Matrix4x4& m) | ||
| 69 | { | ||
| 70 | // Old code from Irion. Kept for reference. | ||
| 71 | Point n2 = HPoint(n, 0.0f) * m; | ||
| 72 | d = -((Point) (HPoint( -d*n, 1.0f ) * m) | n2); | ||
| 73 | n = n2; | ||
| 74 | return *this; | ||
| 75 | } | ||
| 76 | }; | ||
| 77 | |||
| 78 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 79 | /** | ||
| 80 | * Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster. | ||
| 81 | * \param transformed [out] transformed plane | ||
| 82 | * \param plane [in] source plane | ||
| 83 | * \param transform [in] transform matrix | ||
| 84 | * \warning the plane normal must be unit-length | ||
| 85 | */ | ||
| 86 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 87 | inline_ void TransformPlane(Plane& transformed, const Plane& plane, const Matrix4x4& transform) | ||
| 88 | { | ||
| 89 | // Rotate the normal using the rotation part of the 4x4 matrix | ||
| 90 | transformed.n = plane.n * Matrix3x3(transform); | ||
| 91 | |||
| 92 | // Compute new d | ||
| 93 | transformed.d = plane.d - (Point(transform.GetTrans())|transformed.n); | ||
| 94 | } | ||
| 95 | |||
| 96 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 97 | /** | ||
| 98 | * Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster. | ||
| 99 | * \param plane [in/out] source plane (transformed on return) | ||
| 100 | * \param transform [in] transform matrix | ||
| 101 | * \warning the plane normal must be unit-length | ||
| 102 | */ | ||
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 104 | inline_ void TransformPlane(Plane& plane, const Matrix4x4& transform) | ||
| 105 | { | ||
| 106 | // Rotate the normal using the rotation part of the 4x4 matrix | ||
| 107 | plane.n *= Matrix3x3(transform); | ||
| 108 | |||
| 109 | // Compute new d | ||
| 110 | plane.d -= Point(transform.GetTrans())|plane.n; | ||
| 111 | } | ||
| 112 | |||
| 113 | #endif // __ICEPLANE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IcePoint.cpp b/libraries/ode-0.9/OPCODE/Ice/IcePoint.cpp new file mode 100644 index 0000000..e715055 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IcePoint.cpp | |||
| @@ -0,0 +1,193 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for 3D vectors. | ||
| 4 | * \file IcePoint.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * 3D point. | ||
| 13 | * | ||
| 14 | * The name is "Point" instead of "Vector" since a vector is N-dimensional, whereas a point is an implicit "vector of dimension 3". | ||
| 15 | * So the choice was between "Point" and "Vector3", the first one looked better (IMHO). | ||
| 16 | * | ||
| 17 | * Some people, then, use a typedef to handle both points & vectors using the same class: typedef Point Vector3; | ||
| 18 | * This is bad since it opens the door to a lot of confusion while reading the code. I know it may sounds weird but check this out: | ||
| 19 | * | ||
| 20 | * \code | ||
| 21 | * Point P0,P1 = some 3D points; | ||
| 22 | * Point Delta = P1 - P0; | ||
| 23 | * \endcode | ||
| 24 | * | ||
| 25 | * This compiles fine, although you should have written: | ||
| 26 | * | ||
| 27 | * \code | ||
| 28 | * Point P0,P1 = some 3D points; | ||
| 29 | * Vector3 Delta = P1 - P0; | ||
| 30 | * \endcode | ||
| 31 | * | ||
| 32 | * Subtle things like this are not caught at compile-time, and when you find one in the code, you never know whether it's a mistake | ||
| 33 | * from the author or something you don't get. | ||
| 34 | * | ||
| 35 | * One way to handle it at compile-time would be to use different classes for Point & Vector3, only overloading operator "-" for vectors. | ||
| 36 | * But then, you get a lot of redundant code in thoses classes, and basically it's really a lot of useless work. | ||
| 37 | * | ||
| 38 | * Another way would be to use homogeneous points: w=1 for points, w=0 for vectors. That's why the HPoint class exists. Now, to store | ||
| 39 | * your model's vertices and in most cases, you really want to use Points to save ram. | ||
| 40 | * | ||
| 41 | * \class Point | ||
| 42 | * \author Pierre Terdiman | ||
| 43 | * \version 1.0 | ||
| 44 | */ | ||
| 45 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 46 | |||
| 47 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 48 | // Precompiled Header | ||
| 49 | #include "Stdafx.h" | ||
| 50 | |||
| 51 | using namespace IceMaths; | ||
| 52 | |||
| 53 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 54 | /** | ||
| 55 | * Creates a positive unit random vector. | ||
| 56 | * \return Self-reference | ||
| 57 | */ | ||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | Point& Point::PositiveUnitRandomVector() | ||
| 60 | { | ||
| 61 | x = UnitRandomFloat(); | ||
| 62 | y = UnitRandomFloat(); | ||
| 63 | z = UnitRandomFloat(); | ||
| 64 | Normalize(); | ||
| 65 | return *this; | ||
| 66 | } | ||
| 67 | |||
| 68 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 69 | /** | ||
| 70 | * Creates a unit random vector. | ||
| 71 | * \return Self-reference | ||
| 72 | */ | ||
| 73 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 74 | Point& Point::UnitRandomVector() | ||
| 75 | { | ||
| 76 | x = UnitRandomFloat() - 0.5f; | ||
| 77 | y = UnitRandomFloat() - 0.5f; | ||
| 78 | z = UnitRandomFloat() - 0.5f; | ||
| 79 | Normalize(); | ||
| 80 | return *this; | ||
| 81 | } | ||
| 82 | |||
| 83 | // Cast operator | ||
| 84 | // WARNING: not inlined | ||
| 85 | Point::operator HPoint() const { return HPoint(x, y, z, 0.0f); } | ||
| 86 | |||
| 87 | Point& Point::Refract(const Point& eye, const Point& n, float refractindex, Point& refracted) | ||
| 88 | { | ||
| 89 | // Point EyePt = eye position | ||
| 90 | // Point p = current vertex | ||
| 91 | // Point n = vertex normal | ||
| 92 | // Point rv = refracted vector | ||
| 93 | // Eye vector - doesn't need to be normalized | ||
| 94 | Point Env; | ||
| 95 | Env.x = eye.x - x; | ||
| 96 | Env.y = eye.y - y; | ||
| 97 | Env.z = eye.z - z; | ||
| 98 | |||
| 99 | float NDotE = n|Env; | ||
| 100 | float NDotN = n|n; | ||
| 101 | NDotE /= refractindex; | ||
| 102 | |||
| 103 | // Refracted vector | ||
| 104 | refracted = n*NDotE - Env*NDotN; | ||
| 105 | |||
| 106 | return *this; | ||
| 107 | } | ||
| 108 | |||
| 109 | Point& Point::ProjectToPlane(const Plane& p) | ||
| 110 | { | ||
| 111 | *this-= (p.d + (*this|p.n))*p.n; | ||
| 112 | return *this; | ||
| 113 | } | ||
| 114 | |||
| 115 | void Point::ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const | ||
| 116 | { | ||
| 117 | projected = HPoint(x, y, z, 1.0f) * mat; | ||
| 118 | projected.w = 1.0f / projected.w; | ||
| 119 | |||
| 120 | projected.x*=projected.w; | ||
| 121 | projected.y*=projected.w; | ||
| 122 | projected.z*=projected.w; | ||
| 123 | |||
| 124 | projected.x *= halfrenderwidth; projected.x += halfrenderwidth; | ||
| 125 | projected.y *= -halfrenderheight; projected.y += halfrenderheight; | ||
| 126 | } | ||
| 127 | |||
| 128 | void Point::SetNotUsed() | ||
| 129 | { | ||
| 130 | // We use a particular integer pattern : 0xffffffff everywhere. This is a NAN. | ||
| 131 | IR(x) = 0xffffffff; | ||
| 132 | IR(y) = 0xffffffff; | ||
| 133 | IR(z) = 0xffffffff; | ||
| 134 | } | ||
| 135 | |||
| 136 | BOOL Point::IsNotUsed() const | ||
| 137 | { | ||
| 138 | if(IR(x)!=0xffffffff) return FALSE; | ||
| 139 | if(IR(y)!=0xffffffff) return FALSE; | ||
| 140 | if(IR(z)!=0xffffffff) return FALSE; | ||
| 141 | return TRUE; | ||
| 142 | } | ||
| 143 | |||
| 144 | Point& Point::Mult(const Matrix3x3& mat, const Point& a) | ||
| 145 | { | ||
| 146 | x = a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2]; | ||
| 147 | y = a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2]; | ||
| 148 | z = a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2]; | ||
| 149 | return *this; | ||
| 150 | } | ||
| 151 | |||
| 152 | Point& Point::Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2) | ||
| 153 | { | ||
| 154 | x = a1.x * mat1.m[0][0] + a1.y * mat1.m[0][1] + a1.z * mat1.m[0][2] + a2.x * mat2.m[0][0] + a2.y * mat2.m[0][1] + a2.z * mat2.m[0][2]; | ||
| 155 | y = a1.x * mat1.m[1][0] + a1.y * mat1.m[1][1] + a1.z * mat1.m[1][2] + a2.x * mat2.m[1][0] + a2.y * mat2.m[1][1] + a2.z * mat2.m[1][2]; | ||
| 156 | z = a1.x * mat1.m[2][0] + a1.y * mat1.m[2][1] + a1.z * mat1.m[2][2] + a2.x * mat2.m[2][0] + a2.y * mat2.m[2][1] + a2.z * mat2.m[2][2]; | ||
| 157 | return *this; | ||
| 158 | } | ||
| 159 | |||
| 160 | Point& Point::Mac(const Matrix3x3& mat, const Point& a) | ||
| 161 | { | ||
| 162 | x += a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2]; | ||
| 163 | y += a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2]; | ||
| 164 | z += a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2]; | ||
| 165 | return *this; | ||
| 166 | } | ||
| 167 | |||
| 168 | Point& Point::TransMult(const Matrix3x3& mat, const Point& a) | ||
| 169 | { | ||
| 170 | x = a.x * mat.m[0][0] + a.y * mat.m[1][0] + a.z * mat.m[2][0]; | ||
| 171 | y = a.x * mat.m[0][1] + a.y * mat.m[1][1] + a.z * mat.m[2][1]; | ||
| 172 | z = a.x * mat.m[0][2] + a.y * mat.m[1][2] + a.z * mat.m[2][2]; | ||
| 173 | return *this; | ||
| 174 | } | ||
| 175 | |||
| 176 | Point& Point::Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos) | ||
| 177 | { | ||
| 178 | x = r.x * rotpos.m[0][0] + r.y * rotpos.m[0][1] + r.z * rotpos.m[0][2] + linpos.x; | ||
| 179 | y = r.x * rotpos.m[1][0] + r.y * rotpos.m[1][1] + r.z * rotpos.m[1][2] + linpos.y; | ||
| 180 | z = r.x * rotpos.m[2][0] + r.y * rotpos.m[2][1] + r.z * rotpos.m[2][2] + linpos.z; | ||
| 181 | return *this; | ||
| 182 | } | ||
| 183 | |||
| 184 | Point& Point::InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos) | ||
| 185 | { | ||
| 186 | float sx = r.x - linpos.x; | ||
| 187 | float sy = r.y - linpos.y; | ||
| 188 | float sz = r.z - linpos.z; | ||
| 189 | x = sx * rotpos.m[0][0] + sy * rotpos.m[1][0] + sz * rotpos.m[2][0]; | ||
| 190 | y = sx * rotpos.m[0][1] + sy * rotpos.m[1][1] + sz * rotpos.m[2][1]; | ||
| 191 | z = sx * rotpos.m[0][2] + sy * rotpos.m[1][2] + sz * rotpos.m[2][2]; | ||
| 192 | return *this; | ||
| 193 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IcePoint.h b/libraries/ode-0.9/OPCODE/Ice/IcePoint.h new file mode 100644 index 0000000..a97fbe6 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IcePoint.h | |||
| @@ -0,0 +1,528 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for 3D vectors. | ||
| 4 | * \file IcePoint.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEPOINT_H__ | ||
| 13 | #define __ICEPOINT_H__ | ||
| 14 | |||
| 15 | // Forward declarations | ||
| 16 | class HPoint; | ||
| 17 | class Plane; | ||
| 18 | class Matrix3x3; | ||
| 19 | class Matrix4x4; | ||
| 20 | |||
| 21 | #define CROSS2D(a, b) (a.x*b.y - b.x*a.y) | ||
| 22 | |||
| 23 | const float EPSILON2 = 1.0e-20f; | ||
| 24 | |||
| 25 | class ICEMATHS_API Point | ||
| 26 | { | ||
| 27 | public: | ||
| 28 | |||
| 29 | //! Empty constructor | ||
| 30 | inline_ Point() {} | ||
| 31 | //! Constructor from a single float | ||
| 32 | // inline_ Point(float val) : x(val), y(val), z(val) {} | ||
| 33 | // Removed since it introduced the nasty "Point T = *Matrix4x4.GetTrans();" bug....... | ||
| 34 | //! Constructor from floats | ||
| 35 | inline_ Point(float xx, float yy, float zz) : x(xx), y(yy), z(zz) {} | ||
| 36 | //! Constructor from array | ||
| 37 | inline_ Point(const float f[3]) : x(f[X]), y(f[Y]), z(f[Z]) {} | ||
| 38 | //! Copy constructor | ||
| 39 | inline_ Point(const Point& p) : x(p.x), y(p.y), z(p.z) {} | ||
| 40 | //! Destructor | ||
| 41 | inline_ ~Point() {} | ||
| 42 | |||
| 43 | //! Clears the vector | ||
| 44 | inline_ Point& Zero() { x = y = z = 0.0f; return *this; } | ||
| 45 | |||
| 46 | //! + infinity | ||
| 47 | inline_ Point& SetPlusInfinity() { x = y = z = MAX_FLOAT; return *this; } | ||
| 48 | //! - infinity | ||
| 49 | inline_ Point& SetMinusInfinity() { x = y = z = MIN_FLOAT; return *this; } | ||
| 50 | |||
| 51 | //! Sets positive unit random vector | ||
| 52 | Point& PositiveUnitRandomVector(); | ||
| 53 | //! Sets unit random vector | ||
| 54 | Point& UnitRandomVector(); | ||
| 55 | |||
| 56 | //! Assignment from values | ||
| 57 | inline_ Point& Set(float xx, float yy, float zz) { x = xx; y = yy; z = zz; return *this; } | ||
| 58 | //! Assignment from array | ||
| 59 | inline_ Point& Set(const float f[3]) { x = f[X]; y = f[Y]; z = f[Z]; return *this; } | ||
| 60 | //! Assignment from another point | ||
| 61 | inline_ Point& Set(const Point& src) { x = src.x; y = src.y; z = src.z; return *this; } | ||
| 62 | |||
| 63 | //! Adds a vector | ||
| 64 | inline_ Point& Add(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; } | ||
| 65 | //! Adds a vector | ||
| 66 | inline_ Point& Add(float xx, float yy, float zz) { x += xx; y += yy; z += zz; return *this; } | ||
| 67 | //! Adds a vector | ||
| 68 | inline_ Point& Add(const float f[3]) { x += f[X]; y += f[Y]; z += f[Z]; return *this; } | ||
| 69 | //! Adds vectors | ||
| 70 | inline_ Point& Add(const Point& p, const Point& q) { x = p.x+q.x; y = p.y+q.y; z = p.z+q.z; return *this; } | ||
| 71 | |||
| 72 | //! Subtracts a vector | ||
| 73 | inline_ Point& Sub(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; } | ||
| 74 | //! Subtracts a vector | ||
| 75 | inline_ Point& Sub(float xx, float yy, float zz) { x -= xx; y -= yy; z -= zz; return *this; } | ||
| 76 | //! Subtracts a vector | ||
| 77 | inline_ Point& Sub(const float f[3]) { x -= f[X]; y -= f[Y]; z -= f[Z]; return *this; } | ||
| 78 | //! Subtracts vectors | ||
| 79 | inline_ Point& Sub(const Point& p, const Point& q) { x = p.x-q.x; y = p.y-q.y; z = p.z-q.z; return *this; } | ||
| 80 | |||
| 81 | //! this = -this | ||
| 82 | inline_ Point& Neg() { x = -x; y = -y; z = -z; return *this; } | ||
| 83 | //! this = -a | ||
| 84 | inline_ Point& Neg(const Point& a) { x = -a.x; y = -a.y; z = -a.z; return *this; } | ||
| 85 | |||
| 86 | //! Multiplies by a scalar | ||
| 87 | inline_ Point& Mult(float s) { x *= s; y *= s; z *= s; return *this; } | ||
| 88 | |||
| 89 | //! this = a * scalar | ||
| 90 | inline_ Point& Mult(const Point& a, float scalar) | ||
| 91 | { | ||
| 92 | x = a.x * scalar; | ||
| 93 | y = a.y * scalar; | ||
| 94 | z = a.z * scalar; | ||
| 95 | return *this; | ||
| 96 | } | ||
| 97 | |||
| 98 | //! this = a + b * scalar | ||
| 99 | inline_ Point& Mac(const Point& a, const Point& b, float scalar) | ||
| 100 | { | ||
| 101 | x = a.x + b.x * scalar; | ||
| 102 | y = a.y + b.y * scalar; | ||
| 103 | z = a.z + b.z * scalar; | ||
| 104 | return *this; | ||
| 105 | } | ||
| 106 | |||
| 107 | //! this = this + a * scalar | ||
| 108 | inline_ Point& Mac(const Point& a, float scalar) | ||
| 109 | { | ||
| 110 | x += a.x * scalar; | ||
| 111 | y += a.y * scalar; | ||
| 112 | z += a.z * scalar; | ||
| 113 | return *this; | ||
| 114 | } | ||
| 115 | |||
| 116 | //! this = a - b * scalar | ||
| 117 | inline_ Point& Msc(const Point& a, const Point& b, float scalar) | ||
| 118 | { | ||
| 119 | x = a.x - b.x * scalar; | ||
| 120 | y = a.y - b.y * scalar; | ||
| 121 | z = a.z - b.z * scalar; | ||
| 122 | return *this; | ||
| 123 | } | ||
| 124 | |||
| 125 | //! this = this - a * scalar | ||
| 126 | inline_ Point& Msc(const Point& a, float scalar) | ||
| 127 | { | ||
| 128 | x -= a.x * scalar; | ||
| 129 | y -= a.y * scalar; | ||
| 130 | z -= a.z * scalar; | ||
| 131 | return *this; | ||
| 132 | } | ||
| 133 | |||
| 134 | //! this = a + b * scalarb + c * scalarc | ||
| 135 | inline_ Point& Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc) | ||
| 136 | { | ||
| 137 | x = a.x + b.x * scalarb + c.x * scalarc; | ||
| 138 | y = a.y + b.y * scalarb + c.y * scalarc; | ||
| 139 | z = a.z + b.z * scalarb + c.z * scalarc; | ||
| 140 | return *this; | ||
| 141 | } | ||
| 142 | |||
| 143 | //! this = a - b * scalarb - c * scalarc | ||
| 144 | inline_ Point& Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc) | ||
| 145 | { | ||
| 146 | x = a.x - b.x * scalarb - c.x * scalarc; | ||
| 147 | y = a.y - b.y * scalarb - c.y * scalarc; | ||
| 148 | z = a.z - b.z * scalarb - c.z * scalarc; | ||
| 149 | return *this; | ||
| 150 | } | ||
| 151 | |||
| 152 | //! this = mat * a | ||
| 153 | inline_ Point& Mult(const Matrix3x3& mat, const Point& a); | ||
| 154 | |||
| 155 | //! this = mat1 * a1 + mat2 * a2 | ||
| 156 | inline_ Point& Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2); | ||
| 157 | |||
| 158 | //! this = this + mat * a | ||
| 159 | inline_ Point& Mac(const Matrix3x3& mat, const Point& a); | ||
| 160 | |||
| 161 | //! this = transpose(mat) * a | ||
| 162 | inline_ Point& TransMult(const Matrix3x3& mat, const Point& a); | ||
| 163 | |||
| 164 | //! Linear interpolate between two vectors: this = a + t * (b - a) | ||
| 165 | inline_ Point& Lerp(const Point& a, const Point& b, float t) | ||
| 166 | { | ||
| 167 | x = a.x + t * (b.x - a.x); | ||
| 168 | y = a.y + t * (b.y - a.y); | ||
| 169 | z = a.z + t * (b.z - a.z); | ||
| 170 | return *this; | ||
| 171 | } | ||
| 172 | |||
| 173 | //! Hermite interpolate between p1 and p2. p0 and p3 are used for finding gradient at p1 and p2. | ||
| 174 | //! this = p0 * (2t^2 - t^3 - t)/2 | ||
| 175 | //! + p1 * (3t^3 - 5t^2 + 2)/2 | ||
| 176 | //! + p2 * (4t^2 - 3t^3 + t)/2 | ||
| 177 | //! + p3 * (t^3 - t^2)/2 | ||
| 178 | inline_ Point& Herp(const Point& p0, const Point& p1, const Point& p2, const Point& p3, float t) | ||
| 179 | { | ||
| 180 | float t2 = t * t; | ||
| 181 | float t3 = t2 * t; | ||
| 182 | float kp0 = (2.0f * t2 - t3 - t) * 0.5f; | ||
| 183 | float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f; | ||
| 184 | float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f; | ||
| 185 | float kp3 = (t3 - t2) * 0.5f; | ||
| 186 | x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3; | ||
| 187 | y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3; | ||
| 188 | z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3; | ||
| 189 | return *this; | ||
| 190 | } | ||
| 191 | |||
| 192 | //! this = rotpos * r + linpos | ||
| 193 | inline_ Point& Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos); | ||
| 194 | |||
| 195 | //! this = trans(rotpos) * (r - linpos) | ||
| 196 | inline_ Point& InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos); | ||
| 197 | |||
| 198 | //! Returns MIN(x, y, z); | ||
| 199 | inline_ float Min() const { return MIN(x, MIN(y, z)); } | ||
| 200 | //! Returns MAX(x, y, z); | ||
| 201 | inline_ float Max() const { return MAX(x, MAX(y, z)); } | ||
| 202 | //! Sets each element to be componentwise minimum | ||
| 203 | inline_ Point& Min(const Point& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); return *this; } | ||
| 204 | //! Sets each element to be componentwise maximum | ||
| 205 | inline_ Point& Max(const Point& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); return *this; } | ||
| 206 | |||
| 207 | //! Clamps each element | ||
| 208 | inline_ Point& Clamp(float min, float max) | ||
| 209 | { | ||
| 210 | if(x<min) x=min; if(x>max) x=max; | ||
| 211 | if(y<min) y=min; if(y>max) y=max; | ||
| 212 | if(z<min) z=min; if(z>max) z=max; | ||
| 213 | return *this; | ||
| 214 | } | ||
| 215 | |||
| 216 | //! Computes square magnitude | ||
| 217 | inline_ float SquareMagnitude() const { return x*x + y*y + z*z; } | ||
| 218 | //! Computes magnitude | ||
| 219 | inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z); } | ||
| 220 | //! Computes volume | ||
| 221 | inline_ float Volume() const { return x * y * z; } | ||
| 222 | |||
| 223 | //! Checks the point is near zero | ||
| 224 | inline_ bool ApproxZero() const { return SquareMagnitude() < EPSILON2; } | ||
| 225 | |||
| 226 | //! Tests for exact zero vector | ||
| 227 | inline_ BOOL IsZero() const | ||
| 228 | { | ||
| 229 | if(IR(x) || IR(y) || IR(z)) return FALSE; | ||
| 230 | return TRUE; | ||
| 231 | } | ||
| 232 | |||
| 233 | //! Checks point validity | ||
| 234 | inline_ BOOL IsValid() const | ||
| 235 | { | ||
| 236 | if(!IsValidFloat(x)) return FALSE; | ||
| 237 | if(!IsValidFloat(y)) return FALSE; | ||
| 238 | if(!IsValidFloat(z)) return FALSE; | ||
| 239 | return TRUE; | ||
| 240 | } | ||
| 241 | |||
| 242 | //! Slighty moves the point | ||
| 243 | void Tweak(udword coord_mask, udword tweak_mask) | ||
| 244 | { | ||
| 245 | if(coord_mask&1) { udword Dummy = IR(x); Dummy^=tweak_mask; x = FR(Dummy); } | ||
| 246 | if(coord_mask&2) { udword Dummy = IR(y); Dummy^=tweak_mask; y = FR(Dummy); } | ||
| 247 | if(coord_mask&4) { udword Dummy = IR(z); Dummy^=tweak_mask; z = FR(Dummy); } | ||
| 248 | } | ||
| 249 | |||
| 250 | #define TWEAKMASK 0x3fffff | ||
| 251 | #define TWEAKNOTMASK ~TWEAKMASK | ||
| 252 | //! Slighty moves the point out | ||
| 253 | inline_ void TweakBigger() | ||
| 254 | { | ||
| 255 | udword Dummy = (IR(x)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy); | ||
| 256 | Dummy = (IR(y)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy); | ||
| 257 | Dummy = (IR(z)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy); | ||
| 258 | } | ||
| 259 | |||
| 260 | //! Slighty moves the point in | ||
| 261 | inline_ void TweakSmaller() | ||
| 262 | { | ||
| 263 | udword Dummy = (IR(x)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy); | ||
| 264 | Dummy = (IR(y)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy); | ||
| 265 | Dummy = (IR(z)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy); | ||
| 266 | } | ||
| 267 | |||
| 268 | //! Normalizes the vector | ||
| 269 | inline_ Point& Normalize() | ||
| 270 | { | ||
| 271 | float M = x*x + y*y + z*z; | ||
| 272 | if(M) | ||
| 273 | { | ||
| 274 | M = 1.0f / sqrtf(M); | ||
| 275 | x *= M; | ||
| 276 | y *= M; | ||
| 277 | z *= M; | ||
| 278 | } | ||
| 279 | return *this; | ||
| 280 | } | ||
| 281 | |||
| 282 | //! Sets vector length | ||
| 283 | inline_ Point& SetLength(float length) | ||
| 284 | { | ||
| 285 | float NewLength = length / Magnitude(); | ||
| 286 | x *= NewLength; | ||
| 287 | y *= NewLength; | ||
| 288 | z *= NewLength; | ||
| 289 | return *this; | ||
| 290 | } | ||
| 291 | |||
| 292 | //! Clamps vector length | ||
| 293 | inline_ Point& ClampLength(float limit_length) | ||
| 294 | { | ||
| 295 | if(limit_length>=0.0f) // Magnitude must be positive | ||
| 296 | { | ||
| 297 | float CurrentSquareLength = SquareMagnitude(); | ||
| 298 | |||
| 299 | if(CurrentSquareLength > limit_length * limit_length) | ||
| 300 | { | ||
| 301 | float Coeff = limit_length / sqrtf(CurrentSquareLength); | ||
| 302 | x *= Coeff; | ||
| 303 | y *= Coeff; | ||
| 304 | z *= Coeff; | ||
| 305 | } | ||
| 306 | } | ||
| 307 | return *this; | ||
| 308 | } | ||
| 309 | |||
| 310 | //! Computes distance to another point | ||
| 311 | inline_ float Distance(const Point& b) const | ||
| 312 | { | ||
| 313 | return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z)); | ||
| 314 | } | ||
| 315 | |||
| 316 | //! Computes square distance to another point | ||
| 317 | inline_ float SquareDistance(const Point& b) const | ||
| 318 | { | ||
| 319 | return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z)); | ||
| 320 | } | ||
| 321 | |||
| 322 | //! Dot product dp = this|a | ||
| 323 | inline_ float Dot(const Point& p) const { return p.x * x + p.y * y + p.z * z; } | ||
| 324 | |||
| 325 | //! Cross product this = a x b | ||
| 326 | inline_ Point& Cross(const Point& a, const Point& b) | ||
| 327 | { | ||
| 328 | x = a.y * b.z - a.z * b.y; | ||
| 329 | y = a.z * b.x - a.x * b.z; | ||
| 330 | z = a.x * b.y - a.y * b.x; | ||
| 331 | return *this; | ||
| 332 | } | ||
| 333 | |||
| 334 | //! Vector code ( bitmask = sign(z) | sign(y) | sign(x) ) | ||
| 335 | inline_ udword VectorCode() const | ||
| 336 | { | ||
| 337 | return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29); | ||
| 338 | } | ||
| 339 | |||
| 340 | //! Returns largest axis | ||
| 341 | inline_ PointComponent LargestAxis() const | ||
| 342 | { | ||
| 343 | const float* Vals = &x; | ||
| 344 | PointComponent m = X; | ||
| 345 | if(Vals[Y] > Vals[m]) m = Y; | ||
| 346 | if(Vals[Z] > Vals[m]) m = Z; | ||
| 347 | return m; | ||
| 348 | } | ||
| 349 | |||
| 350 | //! Returns closest axis | ||
| 351 | inline_ PointComponent ClosestAxis() const | ||
| 352 | { | ||
| 353 | const float* Vals = &x; | ||
| 354 | PointComponent m = X; | ||
| 355 | if(AIR(Vals[Y]) > AIR(Vals[m])) m = Y; | ||
| 356 | if(AIR(Vals[Z]) > AIR(Vals[m])) m = Z; | ||
| 357 | return m; | ||
| 358 | } | ||
| 359 | |||
| 360 | //! Returns smallest axis | ||
| 361 | inline_ PointComponent SmallestAxis() const | ||
| 362 | { | ||
| 363 | const float* Vals = &x; | ||
| 364 | PointComponent m = X; | ||
| 365 | if(Vals[Y] < Vals[m]) m = Y; | ||
| 366 | if(Vals[Z] < Vals[m]) m = Z; | ||
| 367 | return m; | ||
| 368 | } | ||
| 369 | |||
| 370 | //! Refracts the point | ||
| 371 | Point& Refract(const Point& eye, const Point& n, float refractindex, Point& refracted); | ||
| 372 | |||
| 373 | //! Projects the point onto a plane | ||
| 374 | Point& ProjectToPlane(const Plane& p); | ||
| 375 | |||
| 376 | //! Projects the point onto the screen | ||
| 377 | void ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const; | ||
| 378 | |||
| 379 | //! Unfolds the point onto a plane according to edge(a,b) | ||
| 380 | Point& Unfold(Plane& p, Point& a, Point& b); | ||
| 381 | |||
| 382 | //! Hash function from Ville Miettinen | ||
| 383 | inline_ udword GetHashValue() const | ||
| 384 | { | ||
| 385 | const udword* h = (const udword*)(this); | ||
| 386 | udword f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff; // avoid problems with +-0 | ||
| 387 | return (f>>22)^(f>>12)^(f); | ||
| 388 | } | ||
| 389 | |||
| 390 | //! Stuff magic values in the point, marking it as explicitely not used. | ||
| 391 | void SetNotUsed(); | ||
| 392 | //! Checks the point is marked as not used | ||
| 393 | BOOL IsNotUsed() const; | ||
| 394 | |||
| 395 | // Arithmetic operators | ||
| 396 | |||
| 397 | //! Unary operator for Point Negate = - Point | ||
| 398 | inline_ Point operator-() const { return Point(-x, -y, -z); } | ||
| 399 | |||
| 400 | //! Operator for Point Plus = Point + Point. | ||
| 401 | inline_ Point operator+(const Point& p) const { return Point(x + p.x, y + p.y, z + p.z); } | ||
| 402 | //! Operator for Point Minus = Point - Point. | ||
| 403 | inline_ Point operator-(const Point& p) const { return Point(x - p.x, y - p.y, z - p.z); } | ||
| 404 | |||
| 405 | //! Operator for Point Mul = Point * Point. | ||
| 406 | inline_ Point operator*(const Point& p) const { return Point(x * p.x, y * p.y, z * p.z); } | ||
| 407 | //! Operator for Point Scale = Point * float. | ||
| 408 | inline_ Point operator*(float s) const { return Point(x * s, y * s, z * s ); } | ||
| 409 | //! Operator for Point Scale = float * Point. | ||
| 410 | inline_ friend Point operator*(float s, const Point& p) { return Point(s * p.x, s * p.y, s * p.z); } | ||
| 411 | |||
| 412 | //! Operator for Point Div = Point / Point. | ||
| 413 | inline_ Point operator/(const Point& p) const { return Point(x / p.x, y / p.y, z / p.z); } | ||
| 414 | //! Operator for Point Scale = Point / float. | ||
| 415 | inline_ Point operator/(float s) const { s = 1.0f / s; return Point(x * s, y * s, z * s); } | ||
| 416 | //! Operator for Point Scale = float / Point. | ||
| 417 | inline_ friend Point operator/(float s, const Point& p) { return Point(s / p.x, s / p.y, s / p.z); } | ||
| 418 | |||
| 419 | //! Operator for float DotProd = Point | Point. | ||
| 420 | inline_ float operator|(const Point& p) const { return x*p.x + y*p.y + z*p.z; } | ||
| 421 | //! Operator for Point VecProd = Point ^ Point. | ||
| 422 | inline_ Point operator^(const Point& p) const | ||
| 423 | { | ||
| 424 | return Point( | ||
| 425 | y * p.z - z * p.y, | ||
| 426 | z * p.x - x * p.z, | ||
| 427 | x * p.y - y * p.x ); | ||
| 428 | } | ||
| 429 | |||
| 430 | //! Operator for Point += Point. | ||
| 431 | inline_ Point& operator+=(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; } | ||
| 432 | //! Operator for Point += float. | ||
| 433 | inline_ Point& operator+=(float s) { x += s; y += s; z += s; return *this; } | ||
| 434 | |||
| 435 | //! Operator for Point -= Point. | ||
| 436 | inline_ Point& operator-=(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; } | ||
| 437 | //! Operator for Point -= float. | ||
| 438 | inline_ Point& operator-=(float s) { x -= s; y -= s; z -= s; return *this; } | ||
| 439 | |||
| 440 | //! Operator for Point *= Point. | ||
| 441 | inline_ Point& operator*=(const Point& p) { x *= p.x; y *= p.y; z *= p.z; return *this; } | ||
| 442 | //! Operator for Point *= float. | ||
| 443 | inline_ Point& operator*=(float s) { x *= s; y *= s; z *= s; return *this; } | ||
| 444 | |||
| 445 | //! Operator for Point /= Point. | ||
| 446 | inline_ Point& operator/=(const Point& p) { x /= p.x; y /= p.y; z /= p.z; return *this; } | ||
| 447 | //! Operator for Point /= float. | ||
| 448 | inline_ Point& operator/=(float s) { s = 1.0f/s; x *= s; y *= s; z *= s; return *this; } | ||
| 449 | |||
| 450 | // Logical operators | ||
| 451 | |||
| 452 | //! Operator for "if(Point==Point)" | ||
| 453 | inline_ bool operator==(const Point& p) const { return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z))); } | ||
| 454 | //! Operator for "if(Point!=Point)" | ||
| 455 | inline_ bool operator!=(const Point& p) const { return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z))); } | ||
| 456 | |||
| 457 | // Arithmetic operators | ||
| 458 | |||
| 459 | //! Operator for Point Mul = Point * Matrix3x3. | ||
| 460 | inline_ Point operator*(const Matrix3x3& mat) const | ||
| 461 | { | ||
| 462 | class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining | ||
| 463 | const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat; | ||
| 464 | |||
| 465 | return Point( | ||
| 466 | x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0], | ||
| 467 | x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1], | ||
| 468 | x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] ); | ||
| 469 | } | ||
| 470 | |||
| 471 | //! Operator for Point Mul = Point * Matrix4x4. | ||
| 472 | inline_ Point operator*(const Matrix4x4& mat) const | ||
| 473 | { | ||
| 474 | class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining | ||
| 475 | const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat; | ||
| 476 | |||
| 477 | return Point( | ||
| 478 | x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0], | ||
| 479 | x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1], | ||
| 480 | x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]); | ||
| 481 | } | ||
| 482 | |||
| 483 | //! Operator for Point *= Matrix3x3. | ||
| 484 | inline_ Point& operator*=(const Matrix3x3& mat) | ||
| 485 | { | ||
| 486 | class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining | ||
| 487 | const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat; | ||
| 488 | |||
| 489 | float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0]; | ||
| 490 | float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1]; | ||
| 491 | float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2]; | ||
| 492 | |||
| 493 | x = xp; y = yp; z = zp; | ||
| 494 | |||
| 495 | return *this; | ||
| 496 | } | ||
| 497 | |||
| 498 | //! Operator for Point *= Matrix4x4. | ||
| 499 | inline_ Point& operator*=(const Matrix4x4& mat) | ||
| 500 | { | ||
| 501 | class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining | ||
| 502 | const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat; | ||
| 503 | |||
| 504 | float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0]; | ||
| 505 | float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1]; | ||
| 506 | float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]; | ||
| 507 | |||
| 508 | x = xp; y = yp; z = zp; | ||
| 509 | |||
| 510 | return *this; | ||
| 511 | } | ||
| 512 | |||
| 513 | // Cast operators | ||
| 514 | |||
| 515 | //! Cast a Point to a HPoint. w is set to zero. | ||
| 516 | operator HPoint() const; | ||
| 517 | |||
| 518 | inline_ operator const float*() const { return &x; } | ||
| 519 | inline_ operator float*() { return &x; } | ||
| 520 | |||
| 521 | public: | ||
| 522 | float x, y, z; | ||
| 523 | }; | ||
| 524 | |||
| 525 | FUNCTION ICEMATHS_API void Normalize1(Point& a); | ||
| 526 | FUNCTION ICEMATHS_API void Normalize2(Point& a); | ||
| 527 | |||
| 528 | #endif //__ICEPOINT_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IcePreprocessor.h b/libraries/ode-0.9/OPCODE/Ice/IcePreprocessor.h new file mode 100644 index 0000000..dbeca38 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IcePreprocessor.h | |||
| @@ -0,0 +1,132 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains preprocessor stuff. This should be the first included header. | ||
| 4 | * \file IcePreprocessor.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEPREPROCESSOR_H__ | ||
| 13 | #define __ICEPREPROCESSOR_H__ | ||
| 14 | |||
| 15 | // Check platform | ||
| 16 | #if defined( _WIN32 ) || defined( WIN32 ) | ||
| 17 | // #pragma message("Compiling on Windows...") | ||
| 18 | #define PLATFORM_WINDOWS | ||
| 19 | #else | ||
| 20 | // don't issue pragmas on unknown platforms | ||
| 21 | // #pragma message("Compiling on unknown platform...") | ||
| 22 | #endif | ||
| 23 | |||
| 24 | // Check compiler | ||
| 25 | #if defined(_MSC_VER) | ||
| 26 | // #pragma message("Compiling with VC++...") | ||
| 27 | #define COMPILER_VISUAL_CPP | ||
| 28 | #else | ||
| 29 | // don't issue pragmas on unknown platforms | ||
| 30 | // #pragma message("Compiling with unknown compiler...") | ||
| 31 | #endif | ||
| 32 | |||
| 33 | // Check compiler options. If this file is included in user-apps, this | ||
| 34 | // shouldn't be needed, so that they can use what they like best. | ||
| 35 | #ifndef ICE_DONT_CHECK_COMPILER_OPTIONS | ||
| 36 | #ifdef COMPILER_VISUAL_CPP | ||
| 37 | #if defined(_CHAR_UNSIGNED) | ||
| 38 | #endif | ||
| 39 | |||
| 40 | #if defined(_CPPRTTI) | ||
| 41 | #error Please disable RTTI... | ||
| 42 | #endif | ||
| 43 | |||
| 44 | #if defined(_CPPUNWIND) | ||
| 45 | #error Please disable exceptions... | ||
| 46 | #endif | ||
| 47 | |||
| 48 | #if defined(_MT) | ||
| 49 | // Multithreading | ||
| 50 | #endif | ||
| 51 | #endif | ||
| 52 | #endif | ||
| 53 | |||
| 54 | // Check debug mode | ||
| 55 | #ifdef DEBUG // May be defined instead of _DEBUG. Let's fix it. | ||
| 56 | #ifndef _DEBUG | ||
| 57 | #define _DEBUG | ||
| 58 | #endif | ||
| 59 | #endif | ||
| 60 | |||
| 61 | #ifdef _DEBUG | ||
| 62 | // Here you may define items for debug builds | ||
| 63 | #endif | ||
| 64 | |||
| 65 | #ifndef THIS_FILE | ||
| 66 | #define THIS_FILE __FILE__ | ||
| 67 | #endif | ||
| 68 | |||
| 69 | #ifndef ICE_NO_DLL | ||
| 70 | #ifdef ICECORE_EXPORTS | ||
| 71 | #define ICECORE_API __declspec(dllexport) | ||
| 72 | #else | ||
| 73 | #define ICECORE_API __declspec(dllimport) | ||
| 74 | #endif | ||
| 75 | #else | ||
| 76 | #define ICECORE_API | ||
| 77 | #endif | ||
| 78 | |||
| 79 | // Don't override new/delete | ||
| 80 | // #define DEFAULT_NEWDELETE | ||
| 81 | #define DONT_TRACK_MEMORY_LEAKS | ||
| 82 | |||
| 83 | #define FUNCTION extern "C" | ||
| 84 | |||
| 85 | // Cosmetic stuff [mainly useful with multiple inheritance] | ||
| 86 | #define override(base_class) virtual | ||
| 87 | |||
| 88 | // Our own inline keyword, so that: | ||
| 89 | // - we can switch to __forceinline to check it's really better or not | ||
| 90 | // - we can remove __forceinline if the compiler doesn't support it | ||
| 91 | // #define inline_ __forceinline | ||
| 92 | // #define inline_ inline | ||
| 93 | |||
| 94 | // Contributed by Bruce Mitchener | ||
| 95 | #if defined(COMPILER_VISUAL_CPP) | ||
| 96 | #define inline_ __forceinline | ||
| 97 | // #define inline_ inline | ||
| 98 | #elif defined(__GNUC__) && __GNUC__ < 3 | ||
| 99 | #define inline_ inline | ||
| 100 | #elif defined(__GNUC__) | ||
| 101 | #define inline_ inline __attribute__ ((always_inline)) | ||
| 102 | #else | ||
| 103 | #define inline_ inline | ||
| 104 | #endif | ||
| 105 | |||
| 106 | // Down the hatch | ||
| 107 | #ifdef _MSC_VER | ||
| 108 | #pragma inline_depth( 255 ) | ||
| 109 | #endif | ||
| 110 | |||
| 111 | #ifdef COMPILER_VISUAL_CPP | ||
| 112 | #pragma intrinsic(memcmp) | ||
| 113 | #pragma intrinsic(memcpy) | ||
| 114 | #pragma intrinsic(memset) | ||
| 115 | #pragma intrinsic(strcat) | ||
| 116 | #pragma intrinsic(strcmp) | ||
| 117 | #pragma intrinsic(strcpy) | ||
| 118 | #pragma intrinsic(strlen) | ||
| 119 | #pragma intrinsic(abs) | ||
| 120 | #pragma intrinsic(labs) | ||
| 121 | #endif | ||
| 122 | |||
| 123 | // ANSI compliance | ||
| 124 | #ifdef _DEBUG | ||
| 125 | // Remove painful warning in debug | ||
| 126 | inline_ bool __False__(){ return false; } | ||
| 127 | #define for if(__False__()){} else for | ||
| 128 | #else | ||
| 129 | #define for if(0){} else for | ||
| 130 | #endif | ||
| 131 | |||
| 132 | #endif // __ICEPREPROCESSOR_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceRandom.cpp b/libraries/ode-0.9/OPCODE/Ice/IceRandom.cpp new file mode 100644 index 0000000..cc63a04 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceRandom.cpp | |||
| @@ -0,0 +1,35 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for random generators. | ||
| 4 | * \file IceRandom.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date August, 9, 2001 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Precompiled Header | ||
| 12 | #include "Stdafx.h" | ||
| 13 | |||
| 14 | using namespace IceCore; | ||
| 15 | |||
| 16 | void IceCore:: SRand(udword seed) | ||
| 17 | { | ||
| 18 | srand(seed); | ||
| 19 | } | ||
| 20 | |||
| 21 | udword IceCore::Rand() | ||
| 22 | { | ||
| 23 | return rand(); | ||
| 24 | } | ||
| 25 | |||
| 26 | |||
| 27 | static BasicRandom gRandomGenerator(42); | ||
| 28 | |||
| 29 | udword IceCore::GetRandomIndex(udword max_index) | ||
| 30 | { | ||
| 31 | // We don't use rand() since it's limited to RAND_MAX | ||
| 32 | udword Index = gRandomGenerator.Randomize(); | ||
| 33 | return Index % max_index; | ||
| 34 | } | ||
| 35 | |||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceRandom.h b/libraries/ode-0.9/OPCODE/Ice/IceRandom.h new file mode 100644 index 0000000..3170b33 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceRandom.h | |||
| @@ -0,0 +1,42 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for random generators. | ||
| 4 | * \file IceRandom.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date August, 9, 2001 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICERANDOM_H__ | ||
| 13 | #define __ICERANDOM_H__ | ||
| 14 | |||
| 15 | FUNCTION ICECORE_API void SRand(udword seed); | ||
| 16 | FUNCTION ICECORE_API udword Rand(); | ||
| 17 | |||
| 18 | //! Returns a unit random floating-point value | ||
| 19 | inline_ float UnitRandomFloat() { return float(Rand()) * ONE_OVER_RAND_MAX; } | ||
| 20 | |||
| 21 | //! Returns a random index so that 0<= index < max_index | ||
| 22 | ICECORE_API udword GetRandomIndex(udword max_index); | ||
| 23 | |||
| 24 | class ICECORE_API BasicRandom | ||
| 25 | { | ||
| 26 | public: | ||
| 27 | |||
| 28 | //! Constructor | ||
| 29 | inline_ BasicRandom(udword seed=0) : mRnd(seed) {} | ||
| 30 | //! Destructor | ||
| 31 | inline_ ~BasicRandom() {} | ||
| 32 | |||
| 33 | inline_ void SetSeed(udword seed) { mRnd = seed; } | ||
| 34 | inline_ udword GetCurrentValue() const { return mRnd; } | ||
| 35 | inline_ udword Randomize() { mRnd = mRnd * 2147001325 + 715136305; return mRnd; } | ||
| 36 | |||
| 37 | private: | ||
| 38 | udword mRnd; | ||
| 39 | }; | ||
| 40 | |||
| 41 | #endif // __ICERANDOM_H__ | ||
| 42 | |||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceRay.cpp b/libraries/ode-0.9/OPCODE/Ice/IceRay.cpp new file mode 100644 index 0000000..6cf0330 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceRay.cpp | |||
| @@ -0,0 +1,84 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for rays. | ||
| 4 | * \file IceRay.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * Ray class. | ||
| 13 | * A ray is a half-line P(t) = mOrig + mDir * t, with 0 <= t <= +infinity | ||
| 14 | * \class Ray | ||
| 15 | * \author Pierre Terdiman | ||
| 16 | * \version 1.0 | ||
| 17 | */ | ||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | |||
| 20 | /* | ||
| 21 | O = Origin = impact point | ||
| 22 | i = normalized vector along the x axis | ||
| 23 | j = normalized vector along the y axis = actually the normal vector in O | ||
| 24 | D = Direction vector, norm |D| = 1 | ||
| 25 | N = Projection of D on y axis, norm |N| = normal reaction | ||
| 26 | T = Projection of D on x axis, norm |T| = tangential reaction | ||
| 27 | R = Reflexion vector | ||
| 28 | |||
| 29 | ^y | ||
| 30 | | | ||
| 31 | | | ||
| 32 | | | ||
| 33 | _ _ _| _ _ _ | ||
| 34 | * * *| | ||
| 35 | \ | / | ||
| 36 | \ |N / | | ||
| 37 | R\ | /D | ||
| 38 | \ | / | | ||
| 39 | \ | / | ||
| 40 | _________\|/______*_______>x | ||
| 41 | O T | ||
| 42 | |||
| 43 | Let define theta = angle between D and N. Then cos(theta) = |N| / |D| = |N| since D is normalized. | ||
| 44 | |||
| 45 | j|D = |j|*|D|*cos(theta) => |N| = j|D | ||
| 46 | |||
| 47 | Then we simply have: | ||
| 48 | |||
| 49 | D = N + T | ||
| 50 | |||
| 51 | To compute tangential reaction : | ||
| 52 | |||
| 53 | T = D - N | ||
| 54 | |||
| 55 | To compute reflexion vector : | ||
| 56 | |||
| 57 | R = N - T = N - (D-N) = 2*N - D | ||
| 58 | */ | ||
| 59 | |||
| 60 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 61 | // Precompiled Header | ||
| 62 | #include "Stdafx.h" | ||
| 63 | |||
| 64 | using namespace IceMaths; | ||
| 65 | |||
| 66 | float Ray::SquareDistance(const Point& point, float* t) const | ||
| 67 | { | ||
| 68 | Point Diff = point - mOrig; | ||
| 69 | float fT = Diff | mDir; | ||
| 70 | |||
| 71 | if(fT<=0.0f) | ||
| 72 | { | ||
| 73 | fT = 0.0f; | ||
| 74 | } | ||
| 75 | else | ||
| 76 | { | ||
| 77 | fT /= mDir.SquareMagnitude(); | ||
| 78 | Diff -= fT*mDir; | ||
| 79 | } | ||
| 80 | |||
| 81 | if(t) *t = fT; | ||
| 82 | |||
| 83 | return Diff.SquareMagnitude(); | ||
| 84 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceRay.h b/libraries/ode-0.9/OPCODE/Ice/IceRay.h new file mode 100644 index 0000000..0268287 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceRay.h | |||
| @@ -0,0 +1,98 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for rays. | ||
| 4 | * \file IceRay.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICERAY_H__ | ||
| 13 | #define __ICERAY_H__ | ||
| 14 | |||
| 15 | class ICEMATHS_API Ray | ||
| 16 | { | ||
| 17 | public: | ||
| 18 | //! Constructor | ||
| 19 | inline_ Ray() {} | ||
| 20 | //! Constructor | ||
| 21 | inline_ Ray(const Point& orig, const Point& dir) : mOrig(orig), mDir(dir) {} | ||
| 22 | //! Copy constructor | ||
| 23 | inline_ Ray(const Ray& ray) : mOrig(ray.mOrig), mDir(ray.mDir) {} | ||
| 24 | //! Destructor | ||
| 25 | inline_ ~Ray() {} | ||
| 26 | |||
| 27 | float SquareDistance(const Point& point, float* t=null) const; | ||
| 28 | inline_ float Distance(const Point& point, float* t=null) const { return sqrtf(SquareDistance(point, t)); } | ||
| 29 | |||
| 30 | Point mOrig; //!< Ray origin | ||
| 31 | Point mDir; //!< Normalized direction | ||
| 32 | }; | ||
| 33 | |||
| 34 | inline_ void ComputeReflexionVector(Point& reflected, const Point& incoming_dir, const Point& outward_normal) | ||
| 35 | { | ||
| 36 | reflected = incoming_dir - outward_normal * 2.0f * (incoming_dir|outward_normal); | ||
| 37 | } | ||
| 38 | |||
| 39 | inline_ void ComputeReflexionVector(Point& reflected, const Point& source, const Point& impact, const Point& normal) | ||
| 40 | { | ||
| 41 | Point V = impact - source; | ||
| 42 | reflected = V - normal * 2.0f * (V|normal); | ||
| 43 | } | ||
| 44 | |||
| 45 | inline_ void DecomposeVector(Point& normal_compo, Point& tangent_compo, const Point& outward_dir, const Point& outward_normal) | ||
| 46 | { | ||
| 47 | normal_compo = outward_normal * (outward_dir|outward_normal); | ||
| 48 | tangent_compo = outward_dir - normal_compo; | ||
| 49 | } | ||
| 50 | |||
| 51 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 52 | /** | ||
| 53 | * Transforms a direction vector from world space to local space | ||
| 54 | * \param local_dir [out] direction vector in local space | ||
| 55 | * \param world_dir [in] direction vector in world space | ||
| 56 | * \param world [in] world transform | ||
| 57 | */ | ||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | inline_ void ComputeLocalDirection(Point& local_dir, const Point& world_dir, const Matrix4x4& world) | ||
| 60 | { | ||
| 61 | // Get world direction back in local space | ||
| 62 | // Matrix3x3 InvWorld = world; | ||
| 63 | // local_dir = InvWorld * world_dir; | ||
| 64 | local_dir = Matrix3x3(world) * world_dir; | ||
| 65 | } | ||
| 66 | |||
| 67 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 68 | /** | ||
| 69 | * Transforms a position vector from world space to local space | ||
| 70 | * \param local_pt [out] position vector in local space | ||
| 71 | * \param world_pt [in] position vector in world space | ||
| 72 | * \param world [in] world transform | ||
| 73 | */ | ||
| 74 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 75 | inline_ void ComputeLocalPoint(Point& local_pt, const Point& world_pt, const Matrix4x4& world) | ||
| 76 | { | ||
| 77 | // Get world vertex back in local space | ||
| 78 | Matrix4x4 InvWorld = world; | ||
| 79 | InvWorld.Invert(); | ||
| 80 | local_pt = world_pt * InvWorld; | ||
| 81 | } | ||
| 82 | |||
| 83 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 84 | /** | ||
| 85 | * Transforms a ray from world space to local space | ||
| 86 | * \param local_ray [out] ray in local space | ||
| 87 | * \param world_ray [in] ray in world space | ||
| 88 | * \param world [in] world transform | ||
| 89 | */ | ||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | inline_ void ComputeLocalRay(Ray& local_ray, const Ray& world_ray, const Matrix4x4& world) | ||
| 92 | { | ||
| 93 | // Get world ray back in local space | ||
| 94 | ComputeLocalDirection(local_ray.mDir, world_ray.mDir, world); | ||
| 95 | ComputeLocalPoint(local_ray.mOrig, world_ray.mOrig, world); | ||
| 96 | } | ||
| 97 | |||
| 98 | #endif // __ICERAY_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceRevisitedRadix.cpp b/libraries/ode-0.9/OPCODE/Ice/IceRevisitedRadix.cpp new file mode 100644 index 0000000..3e351da --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceRevisitedRadix.cpp | |||
| @@ -0,0 +1,520 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains source code from the article "Radix Sort Revisited". | ||
| 4 | * \file IceRevisitedRadix.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * Revisited Radix Sort. | ||
| 13 | * This is my new radix routine: | ||
| 14 | * - it uses indices and doesn't recopy the values anymore, hence wasting less ram | ||
| 15 | * - it creates all the histograms in one run instead of four | ||
| 16 | * - it sorts words faster than dwords and bytes faster than words | ||
| 17 | * - it correctly sorts negative floating-point values by patching the offsets | ||
| 18 | * - it automatically takes advantage of temporal coherence | ||
| 19 | * - multiple keys support is a side effect of temporal coherence | ||
| 20 | * - it may be worth recoding in asm... (mainly to use FCOMI, FCMOV, etc) [it's probably memory-bound anyway] | ||
| 21 | * | ||
| 22 | * History: | ||
| 23 | * - 08.15.98: very first version | ||
| 24 | * - 04.04.00: recoded for the radix article | ||
| 25 | * - 12.xx.00: code lifting | ||
| 26 | * - 09.18.01: faster CHECK_PASS_VALIDITY thanks to Mark D. Shattuck (who provided other tips, not included here) | ||
| 27 | * - 10.11.01: added local ram support | ||
| 28 | * - 01.20.02: bugfix! In very particular cases the last pass was skipped in the float code-path, leading to incorrect sorting...... | ||
| 29 | * - 01.02.02: - "mIndices" renamed => "mRanks". That's a rank sorter after all. | ||
| 30 | * - ranks are not "reset" anymore, but implicit on first calls | ||
| 31 | * - 07.05.02: - offsets rewritten with one less indirection. | ||
| 32 | * - 11.03.02: - "bool" replaced with RadixHint enum | ||
| 33 | * | ||
| 34 | * \class RadixSort | ||
| 35 | * \author Pierre Terdiman | ||
| 36 | * \version 1.4 | ||
| 37 | * \date August, 15, 1998 | ||
| 38 | */ | ||
| 39 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 40 | |||
| 41 | /* | ||
| 42 | To do: | ||
| 43 | - add an offset parameter between two input values (avoid some data recopy sometimes) | ||
| 44 | - unroll ? asm ? | ||
| 45 | - 11 bits trick & 3 passes as Michael did | ||
| 46 | - prefetch stuff the day I have a P3 | ||
| 47 | - make a version with 16-bits indices ? | ||
| 48 | */ | ||
| 49 | |||
| 50 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 51 | // Precompiled Header | ||
| 52 | #include "Stdafx.h" | ||
| 53 | |||
| 54 | using namespace IceCore; | ||
| 55 | |||
| 56 | #define INVALIDATE_RANKS mCurrentSize|=0x80000000 | ||
| 57 | #define VALIDATE_RANKS mCurrentSize&=0x7fffffff | ||
| 58 | #define CURRENT_SIZE (mCurrentSize&0x7fffffff) | ||
| 59 | #define INVALID_RANKS (mCurrentSize&0x80000000) | ||
| 60 | |||
| 61 | #define CHECK_RESIZE(n) \ | ||
| 62 | if(n!=mPreviousSize) \ | ||
| 63 | { \ | ||
| 64 | if(n>mCurrentSize) Resize(n); \ | ||
| 65 | else ResetRanks(); \ | ||
| 66 | mPreviousSize = n; \ | ||
| 67 | } | ||
| 68 | |||
| 69 | #define CREATE_HISTOGRAMS(type, buffer) \ | ||
| 70 | /* Clear counters/histograms */ \ | ||
| 71 | ZeroMemory(mHistogram, 256*4*sizeof(udword)); \ | ||
| 72 | \ | ||
| 73 | /* Prepare to count */ \ | ||
| 74 | ubyte* p = (ubyte*)input; \ | ||
| 75 | ubyte* pe = &p[nb*4]; \ | ||
| 76 | udword* h0= &mHistogram[0]; /* Histogram for first pass (LSB) */ \ | ||
| 77 | udword* h1= &mHistogram[256]; /* Histogram for second pass */ \ | ||
| 78 | udword* h2= &mHistogram[512]; /* Histogram for third pass */ \ | ||
| 79 | udword* h3= &mHistogram[768]; /* Histogram for last pass (MSB) */ \ | ||
| 80 | \ | ||
| 81 | bool AlreadySorted = true; /* Optimism... */ \ | ||
| 82 | \ | ||
| 83 | if(INVALID_RANKS) \ | ||
| 84 | { \ | ||
| 85 | /* Prepare for temporal coherence */ \ | ||
| 86 | type* Running = (type*)buffer; \ | ||
| 87 | type PrevVal = *Running; \ | ||
| 88 | \ | ||
| 89 | while(p!=pe) \ | ||
| 90 | { \ | ||
| 91 | /* Read input buffer in previous sorted order */ \ | ||
| 92 | type Val = *Running++; \ | ||
| 93 | /* Check whether already sorted or not */ \ | ||
| 94 | if(Val<PrevVal) { AlreadySorted = false; break; } /* Early out */ \ | ||
| 95 | /* Update for next iteration */ \ | ||
| 96 | PrevVal = Val; \ | ||
| 97 | \ | ||
| 98 | /* Create histograms */ \ | ||
| 99 | h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; \ | ||
| 100 | } \ | ||
| 101 | \ | ||
| 102 | /* If all input values are already sorted, we just have to return and leave the */ \ | ||
| 103 | /* previous list unchanged. That way the routine may take advantage of temporal */ \ | ||
| 104 | /* coherence, for example when used to sort transparent faces. */ \ | ||
| 105 | if(AlreadySorted) \ | ||
| 106 | { \ | ||
| 107 | mNbHits++; \ | ||
| 108 | for(udword i=0;i<nb;i++) mRanks[i] = i; \ | ||
| 109 | return *this; \ | ||
| 110 | } \ | ||
| 111 | } \ | ||
| 112 | else \ | ||
| 113 | { \ | ||
| 114 | /* Prepare for temporal coherence */ \ | ||
| 115 | udword* Indices = mRanks; \ | ||
| 116 | type PrevVal = (type)buffer[*Indices]; \ | ||
| 117 | \ | ||
| 118 | while(p!=pe) \ | ||
| 119 | { \ | ||
| 120 | /* Read input buffer in previous sorted order */ \ | ||
| 121 | type Val = (type)buffer[*Indices++]; \ | ||
| 122 | /* Check whether already sorted or not */ \ | ||
| 123 | if(Val<PrevVal) { AlreadySorted = false; break; } /* Early out */ \ | ||
| 124 | /* Update for next iteration */ \ | ||
| 125 | PrevVal = Val; \ | ||
| 126 | \ | ||
| 127 | /* Create histograms */ \ | ||
| 128 | h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; \ | ||
| 129 | } \ | ||
| 130 | \ | ||
| 131 | /* If all input values are already sorted, we just have to return and leave the */ \ | ||
| 132 | /* previous list unchanged. That way the routine may take advantage of temporal */ \ | ||
| 133 | /* coherence, for example when used to sort transparent faces. */ \ | ||
| 134 | if(AlreadySorted) { mNbHits++; return *this; } \ | ||
| 135 | } \ | ||
| 136 | \ | ||
| 137 | /* Else there has been an early out and we must finish computing the histograms */ \ | ||
| 138 | while(p!=pe) \ | ||
| 139 | { \ | ||
| 140 | /* Create histograms without the previous overhead */ \ | ||
| 141 | h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; \ | ||
| 142 | } | ||
| 143 | |||
| 144 | #define CHECK_PASS_VALIDITY(pass) \ | ||
| 145 | /* Shortcut to current counters */ \ | ||
| 146 | udword* CurCount = &mHistogram[pass<<8]; \ | ||
| 147 | \ | ||
| 148 | /* Reset flag. The sorting pass is supposed to be performed. (default) */ \ | ||
| 149 | bool PerformPass = true; \ | ||
| 150 | \ | ||
| 151 | /* Check pass validity */ \ | ||
| 152 | \ | ||
| 153 | /* If all values have the same byte, sorting is useless. */ \ | ||
| 154 | /* It may happen when sorting bytes or words instead of dwords. */ \ | ||
| 155 | /* This routine actually sorts words faster than dwords, and bytes */ \ | ||
| 156 | /* faster than words. Standard running time (O(4*n))is reduced to O(2*n) */ \ | ||
| 157 | /* for words and O(n) for bytes. Running time for floats depends on actual values... */ \ | ||
| 158 | \ | ||
| 159 | /* Get first byte */ \ | ||
| 160 | ubyte UniqueVal = *(((ubyte*)input)+pass); \ | ||
| 161 | \ | ||
| 162 | /* Check that byte's counter */ \ | ||
| 163 | if(CurCount[UniqueVal]==nb) PerformPass=false; | ||
| 164 | |||
| 165 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 166 | /** | ||
| 167 | * Constructor. | ||
| 168 | */ | ||
| 169 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 170 | RadixSort::RadixSort() : mRanks(null), mRanks2(null), mCurrentSize(0), mTotalCalls(0), mNbHits(0) | ||
| 171 | { | ||
| 172 | #ifndef RADIX_LOCAL_RAM | ||
| 173 | // Allocate input-independent ram | ||
| 174 | mHistogram = new udword[256*4]; | ||
| 175 | mOffset = new udword[256]; | ||
| 176 | #endif | ||
| 177 | // Initialize indices | ||
| 178 | INVALIDATE_RANKS; | ||
| 179 | } | ||
| 180 | |||
| 181 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 182 | /** | ||
| 183 | * Destructor. | ||
| 184 | */ | ||
| 185 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 186 | RadixSort::~RadixSort() | ||
| 187 | { | ||
| 188 | // Release everything | ||
| 189 | #ifndef RADIX_LOCAL_RAM | ||
| 190 | DELETEARRAY(mOffset); | ||
| 191 | DELETEARRAY(mHistogram); | ||
| 192 | #endif | ||
| 193 | DELETEARRAY(mRanks2); | ||
| 194 | DELETEARRAY(mRanks); | ||
| 195 | } | ||
| 196 | |||
| 197 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 198 | /** | ||
| 199 | * Resizes the inner lists. | ||
| 200 | * \param nb [in] new size (number of dwords) | ||
| 201 | * \return true if success | ||
| 202 | */ | ||
| 203 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 204 | bool RadixSort::Resize(udword nb) | ||
| 205 | { | ||
| 206 | // Free previously used ram | ||
| 207 | DELETEARRAY(mRanks2); | ||
| 208 | DELETEARRAY(mRanks); | ||
| 209 | |||
| 210 | // Get some fresh one | ||
| 211 | mRanks = new udword[nb]; CHECKALLOC(mRanks); | ||
| 212 | mRanks2 = new udword[nb]; CHECKALLOC(mRanks2); | ||
| 213 | |||
| 214 | return true; | ||
| 215 | } | ||
| 216 | |||
| 217 | inline_ void RadixSort::CheckResize(udword nb) | ||
| 218 | { | ||
| 219 | udword CurSize = CURRENT_SIZE; | ||
| 220 | if(nb!=CurSize) | ||
| 221 | { | ||
| 222 | if(nb>CurSize) Resize(nb); | ||
| 223 | mCurrentSize = nb; | ||
| 224 | INVALIDATE_RANKS; | ||
| 225 | } | ||
| 226 | } | ||
| 227 | |||
| 228 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 229 | /** | ||
| 230 | * Main sort routine. | ||
| 231 | * This one is for integer values. After the call, mRanks contains a list of indices in sorted order, i.e. in the order you may process your data. | ||
| 232 | * \param input [in] a list of integer values to sort | ||
| 233 | * \param nb [in] number of values to sort, must be < 2^31 | ||
| 234 | * \param hint [in] RADIX_SIGNED to handle negative values, RADIX_UNSIGNED if you know your input buffer only contains positive values | ||
| 235 | * \return Self-Reference | ||
| 236 | */ | ||
| 237 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 238 | RadixSort& RadixSort::Sort(const udword* input, udword nb, RadixHint hint) | ||
| 239 | { | ||
| 240 | // Checkings | ||
| 241 | if(!input || !nb || nb&0x80000000) return *this; | ||
| 242 | |||
| 243 | // Stats | ||
| 244 | mTotalCalls++; | ||
| 245 | |||
| 246 | // Resize lists if needed | ||
| 247 | CheckResize(nb); | ||
| 248 | |||
| 249 | #ifdef RADIX_LOCAL_RAM | ||
| 250 | // Allocate histograms & offsets on the stack | ||
| 251 | udword mHistogram[256*4]; | ||
| 252 | // udword mOffset[256]; | ||
| 253 | udword* mLink[256]; | ||
| 254 | #endif | ||
| 255 | |||
| 256 | // Create histograms (counters). Counters for all passes are created in one run. | ||
| 257 | // Pros: read input buffer once instead of four times | ||
| 258 | // Cons: mHistogram is 4Kb instead of 1Kb | ||
| 259 | // We must take care of signed/unsigned values for temporal coherence.... I just | ||
| 260 | // have 2 code paths even if just a single opcode changes. Self-modifying code, someone? | ||
| 261 | if(hint==RADIX_UNSIGNED) { CREATE_HISTOGRAMS(udword, input); } | ||
| 262 | else { CREATE_HISTOGRAMS(sdword, input); } | ||
| 263 | |||
| 264 | // Compute #negative values involved if needed | ||
| 265 | udword NbNegativeValues = 0; | ||
| 266 | if(hint==RADIX_SIGNED) | ||
| 267 | { | ||
| 268 | // An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128 | ||
| 269 | // last values of the last histogram. Last histogram because that's the one for the Most Significant Byte, | ||
| 270 | // responsible for the sign. 128 last values because the 128 first ones are related to positive numbers. | ||
| 271 | udword* h3= &mHistogram[768]; | ||
| 272 | for(udword i=128;i<256;i++) NbNegativeValues += h3[i]; // 768 for last histogram, 128 for negative part | ||
| 273 | } | ||
| 274 | |||
| 275 | // Radix sort, j is the pass number (0=LSB, 3=MSB) | ||
| 276 | for(udword j=0;j<4;j++) | ||
| 277 | { | ||
| 278 | CHECK_PASS_VALIDITY(j); | ||
| 279 | |||
| 280 | // Sometimes the fourth (negative) pass is skipped because all numbers are negative and the MSB is 0xFF (for example). This is | ||
| 281 | // not a problem, numbers are correctly sorted anyway. | ||
| 282 | if(PerformPass) | ||
| 283 | { | ||
| 284 | // Should we care about negative values? | ||
| 285 | if(j!=3 || hint==RADIX_UNSIGNED) | ||
| 286 | { | ||
| 287 | // Here we deal with positive values only | ||
| 288 | |||
| 289 | // Create offsets | ||
| 290 | // mOffset[0] = 0; | ||
| 291 | // for(udword i=1;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; | ||
| 292 | mLink[0] = mRanks2; | ||
| 293 | for(udword i=1;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; | ||
| 294 | } | ||
| 295 | else | ||
| 296 | { | ||
| 297 | // This is a special case to correctly handle negative integers. They're sorted in the right order but at the wrong place. | ||
| 298 | |||
| 299 | // Create biased offsets, in order for negative numbers to be sorted as well | ||
| 300 | // mOffset[0] = NbNegativeValues; // First positive number takes place after the negative ones | ||
| 301 | mLink[0] = &mRanks2[NbNegativeValues]; // First positive number takes place after the negative ones | ||
| 302 | // for(udword i=1;i<128;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers | ||
| 303 | for(udword i=1;i<128;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers | ||
| 304 | |||
| 305 | // Fixing the wrong place for negative values | ||
| 306 | // mOffset[128] = 0; | ||
| 307 | mLink[128] = mRanks2; | ||
| 308 | // for(i=129;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; | ||
| 309 | for(udword i=129;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; | ||
| 310 | } | ||
| 311 | |||
| 312 | // Perform Radix Sort | ||
| 313 | ubyte* InputBytes = (ubyte*)input; | ||
| 314 | InputBytes += j; | ||
| 315 | if(INVALID_RANKS) | ||
| 316 | { | ||
| 317 | // for(udword i=0;i<nb;i++) mRanks2[mOffset[InputBytes[i<<2]]++] = i; | ||
| 318 | for(udword i=0;i<nb;i++) *mLink[InputBytes[i<<2]]++ = i; | ||
| 319 | VALIDATE_RANKS; | ||
| 320 | } | ||
| 321 | else | ||
| 322 | { | ||
| 323 | udword* Indices = mRanks; | ||
| 324 | udword* IndicesEnd = &mRanks[nb]; | ||
| 325 | while(Indices!=IndicesEnd) | ||
| 326 | { | ||
| 327 | udword id = *Indices++; | ||
| 328 | // mRanks2[mOffset[InputBytes[id<<2]]++] = id; | ||
| 329 | *mLink[InputBytes[id<<2]]++ = id; | ||
| 330 | } | ||
| 331 | } | ||
| 332 | |||
| 333 | // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. | ||
| 334 | udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp; | ||
| 335 | } | ||
| 336 | } | ||
| 337 | return *this; | ||
| 338 | } | ||
| 339 | |||
| 340 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 341 | /** | ||
| 342 | * Main sort routine. | ||
| 343 | * This one is for floating-point values. After the call, mRanks contains a list of indices in sorted order, i.e. in the order you may process your data. | ||
| 344 | * \param input [in] a list of floating-point values to sort | ||
| 345 | * \param nb [in] number of values to sort, must be < 2^31 | ||
| 346 | * \return Self-Reference | ||
| 347 | * \warning only sorts IEEE floating-point values | ||
| 348 | */ | ||
| 349 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 350 | RadixSort& RadixSort::Sort(const float* input2, udword nb) | ||
| 351 | { | ||
| 352 | // Checkings | ||
| 353 | if(!input2 || !nb || nb&0x80000000) return *this; | ||
| 354 | |||
| 355 | // Stats | ||
| 356 | mTotalCalls++; | ||
| 357 | |||
| 358 | udword* input = (udword*)input2; | ||
| 359 | |||
| 360 | // Resize lists if needed | ||
| 361 | CheckResize(nb); | ||
| 362 | |||
| 363 | #ifdef RADIX_LOCAL_RAM | ||
| 364 | // Allocate histograms & offsets on the stack | ||
| 365 | udword mHistogram[256*4]; | ||
| 366 | // udword mOffset[256]; | ||
| 367 | udword* mLink[256]; | ||
| 368 | #endif | ||
| 369 | |||
| 370 | // Create histograms (counters). Counters for all passes are created in one run. | ||
| 371 | // Pros: read input buffer once instead of four times | ||
| 372 | // Cons: mHistogram is 4Kb instead of 1Kb | ||
| 373 | // Floating-point values are always supposed to be signed values, so there's only one code path there. | ||
| 374 | // Please note the floating point comparison needed for temporal coherence! Although the resulting asm code | ||
| 375 | // is dreadful, this is surprisingly not such a performance hit - well, I suppose that's a big one on first | ||
| 376 | // generation Pentiums....We can't make comparison on integer representations because, as Chris said, it just | ||
| 377 | // wouldn't work with mixed positive/negative values.... | ||
| 378 | { CREATE_HISTOGRAMS(float, input2); } | ||
| 379 | |||
| 380 | // Compute #negative values involved if needed | ||
| 381 | udword NbNegativeValues = 0; | ||
| 382 | // An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128 | ||
| 383 | // last values of the last histogram. Last histogram because that's the one for the Most Significant Byte, | ||
| 384 | // responsible for the sign. 128 last values because the 128 first ones are related to positive numbers. | ||
| 385 | udword* h3= &mHistogram[768]; | ||
| 386 | for(udword i=128;i<256;i++) NbNegativeValues += h3[i]; // 768 for last histogram, 128 for negative part | ||
| 387 | |||
| 388 | // Radix sort, j is the pass number (0=LSB, 3=MSB) | ||
| 389 | for(udword j=0;j<4;j++) | ||
| 390 | { | ||
| 391 | // Should we care about negative values? | ||
| 392 | if(j!=3) | ||
| 393 | { | ||
| 394 | // Here we deal with positive values only | ||
| 395 | CHECK_PASS_VALIDITY(j); | ||
| 396 | |||
| 397 | if(PerformPass) | ||
| 398 | { | ||
| 399 | // Create offsets | ||
| 400 | // mOffset[0] = 0; | ||
| 401 | mLink[0] = mRanks2; | ||
| 402 | // for(udword i=1;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; | ||
| 403 | for(udword i=1;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; | ||
| 404 | |||
| 405 | // Perform Radix Sort | ||
| 406 | ubyte* InputBytes = (ubyte*)input; | ||
| 407 | InputBytes += j; | ||
| 408 | if(INVALID_RANKS) | ||
| 409 | { | ||
| 410 | // for(i=0;i<nb;i++) mRanks2[mOffset[InputBytes[i<<2]]++] = i; | ||
| 411 | for(udword i=0;i<nb;i++) *mLink[InputBytes[i<<2]]++ = i; | ||
| 412 | VALIDATE_RANKS; | ||
| 413 | } | ||
| 414 | else | ||
| 415 | { | ||
| 416 | udword* Indices = mRanks; | ||
| 417 | udword* IndicesEnd = &mRanks[nb]; | ||
| 418 | while(Indices!=IndicesEnd) | ||
| 419 | { | ||
| 420 | udword id = *Indices++; | ||
| 421 | // mRanks2[mOffset[InputBytes[id<<2]]++] = id; | ||
| 422 | *mLink[InputBytes[id<<2]]++ = id; | ||
| 423 | } | ||
| 424 | } | ||
| 425 | |||
| 426 | // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. | ||
| 427 | udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp; | ||
| 428 | } | ||
| 429 | } | ||
| 430 | else | ||
| 431 | { | ||
| 432 | // This is a special case to correctly handle negative values | ||
| 433 | CHECK_PASS_VALIDITY(j); | ||
| 434 | |||
| 435 | if(PerformPass) | ||
| 436 | { | ||
| 437 | // Create biased offsets, in order for negative numbers to be sorted as well | ||
| 438 | // mOffset[0] = NbNegativeValues; // First positive number takes place after the negative ones | ||
| 439 | mLink[0] = &mRanks2[NbNegativeValues]; // First positive number takes place after the negative ones | ||
| 440 | // for(udword i=1;i<128;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers | ||
| 441 | for(udword i=1;i<128;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers | ||
| 442 | |||
| 443 | // We must reverse the sorting order for negative numbers! | ||
| 444 | // mOffset[255] = 0; | ||
| 445 | mLink[255] = mRanks2; | ||
| 446 | // for(i=0;i<127;i++) mOffset[254-i] = mOffset[255-i] + CurCount[255-i]; // Fixing the wrong order for negative values | ||
| 447 | for(udword i=0;i<127;i++) mLink[254-i] = mLink[255-i] + CurCount[255-i]; // Fixing the wrong order for negative values | ||
| 448 | // for(i=128;i<256;i++) mOffset[i] += CurCount[i]; // Fixing the wrong place for negative values | ||
| 449 | for(udword i=128;i<256;i++) mLink[i] += CurCount[i]; // Fixing the wrong place for negative values | ||
| 450 | |||
| 451 | // Perform Radix Sort | ||
| 452 | if(INVALID_RANKS) | ||
| 453 | { | ||
| 454 | for(udword i=0;i<nb;i++) | ||
| 455 | { | ||
| 456 | udword Radix = input[i]>>24; // Radix byte, same as above. AND is useless here (udword). | ||
| 457 | // ### cmp to be killed. Not good. Later. | ||
| 458 | // if(Radix<128) mRanks2[mOffset[Radix]++] = i; // Number is positive, same as above | ||
| 459 | // else mRanks2[--mOffset[Radix]] = i; // Number is negative, flip the sorting order | ||
| 460 | if(Radix<128) *mLink[Radix]++ = i; // Number is positive, same as above | ||
| 461 | else *(--mLink[Radix]) = i; // Number is negative, flip the sorting order | ||
| 462 | } | ||
| 463 | VALIDATE_RANKS; | ||
| 464 | } | ||
| 465 | else | ||
| 466 | { | ||
| 467 | for(udword i=0;i<nb;i++) | ||
| 468 | { | ||
| 469 | udword Radix = input[mRanks[i]]>>24; // Radix byte, same as above. AND is useless here (udword). | ||
| 470 | // ### cmp to be killed. Not good. Later. | ||
| 471 | // if(Radix<128) mRanks2[mOffset[Radix]++] = mRanks[i]; // Number is positive, same as above | ||
| 472 | // else mRanks2[--mOffset[Radix]] = mRanks[i]; // Number is negative, flip the sorting order | ||
| 473 | if(Radix<128) *mLink[Radix]++ = mRanks[i]; // Number is positive, same as above | ||
| 474 | else *(--mLink[Radix]) = mRanks[i]; // Number is negative, flip the sorting order | ||
| 475 | } | ||
| 476 | } | ||
| 477 | // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. | ||
| 478 | udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp; | ||
| 479 | } | ||
| 480 | else | ||
| 481 | { | ||
| 482 | // The pass is useless, yet we still have to reverse the order of current list if all values are negative. | ||
| 483 | if(UniqueVal>=128) | ||
| 484 | { | ||
| 485 | if(INVALID_RANKS) | ||
| 486 | { | ||
| 487 | // ###Possible? | ||
| 488 | for(udword i=0;i<nb;i++) mRanks2[i] = nb-i-1; | ||
| 489 | VALIDATE_RANKS; | ||
| 490 | } | ||
| 491 | else | ||
| 492 | { | ||
| 493 | for(udword i=0;i<nb;i++) mRanks2[i] = mRanks[nb-i-1]; | ||
| 494 | } | ||
| 495 | |||
| 496 | // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap. | ||
| 497 | udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp; | ||
| 498 | } | ||
| 499 | } | ||
| 500 | } | ||
| 501 | } | ||
| 502 | return *this; | ||
| 503 | } | ||
| 504 | |||
| 505 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 506 | /** | ||
| 507 | * Gets the ram used. | ||
| 508 | * \return memory used in bytes | ||
| 509 | */ | ||
| 510 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 511 | udword RadixSort::GetUsedRam() const | ||
| 512 | { | ||
| 513 | udword UsedRam = sizeof(RadixSort); | ||
| 514 | #ifndef RADIX_LOCAL_RAM | ||
| 515 | UsedRam += 256*4*sizeof(udword); // Histograms | ||
| 516 | UsedRam += 256*sizeof(udword); // Offsets | ||
| 517 | #endif | ||
| 518 | UsedRam += 2*CURRENT_SIZE*sizeof(udword); // 2 lists of indices | ||
| 519 | return UsedRam; | ||
| 520 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceRevisitedRadix.h b/libraries/ode-0.9/OPCODE/Ice/IceRevisitedRadix.h new file mode 100644 index 0000000..3bdfc22 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceRevisitedRadix.h | |||
| @@ -0,0 +1,65 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains source code from the article "Radix Sort Revisited". | ||
| 4 | * \file IceRevisitedRadix.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICERADIXSORT_H__ | ||
| 13 | #define __ICERADIXSORT_H__ | ||
| 14 | |||
| 15 | //! Allocate histograms & offsets locally | ||
| 16 | #define RADIX_LOCAL_RAM | ||
| 17 | |||
| 18 | enum RadixHint | ||
| 19 | { | ||
| 20 | RADIX_SIGNED, //!< Input values are signed | ||
| 21 | RADIX_UNSIGNED, //!< Input values are unsigned | ||
| 22 | |||
| 23 | RADIX_FORCE_DWORD = 0x7fffffff | ||
| 24 | }; | ||
| 25 | |||
| 26 | class ICECORE_API RadixSort | ||
| 27 | { | ||
| 28 | public: | ||
| 29 | // Constructor/Destructor | ||
| 30 | RadixSort(); | ||
| 31 | ~RadixSort(); | ||
| 32 | // Sorting methods | ||
| 33 | RadixSort& Sort(const udword* input, udword nb, RadixHint hint=RADIX_SIGNED); | ||
| 34 | RadixSort& Sort(const float* input, udword nb); | ||
| 35 | |||
| 36 | //! Access to results. mRanks is a list of indices in sorted order, i.e. in the order you may further process your data | ||
| 37 | inline_ const udword* GetRanks() const { return mRanks; } | ||
| 38 | |||
| 39 | //! mIndices2 gets trashed on calling the sort routine, but otherwise you can recycle it the way you want. | ||
| 40 | inline_ udword* GetRecyclable() const { return mRanks2; } | ||
| 41 | |||
| 42 | // Stats | ||
| 43 | udword GetUsedRam() const; | ||
| 44 | //! Returns the total number of calls to the radix sorter. | ||
| 45 | inline_ udword GetNbTotalCalls() const { return mTotalCalls; } | ||
| 46 | //! Returns the number of eraly exits due to temporal coherence. | ||
| 47 | inline_ udword GetNbHits() const { return mNbHits; } | ||
| 48 | |||
| 49 | private: | ||
| 50 | #ifndef RADIX_LOCAL_RAM | ||
| 51 | udword* mHistogram; //!< Counters for each byte | ||
| 52 | udword* mOffset; //!< Offsets (nearly a cumulative distribution function) | ||
| 53 | #endif | ||
| 54 | udword mCurrentSize; //!< Current size of the indices list | ||
| 55 | udword* mRanks; //!< Two lists, swapped each pass | ||
| 56 | udword* mRanks2; | ||
| 57 | // Stats | ||
| 58 | udword mTotalCalls; //!< Total number of calls to the sort routine | ||
| 59 | udword mNbHits; //!< Number of early exits due to coherence | ||
| 60 | // Internal methods | ||
| 61 | void CheckResize(udword nb); | ||
| 62 | bool Resize(udword nb); | ||
| 63 | }; | ||
| 64 | |||
| 65 | #endif // __ICERADIXSORT_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceSegment.cpp b/libraries/ode-0.9/OPCODE/Ice/IceSegment.cpp new file mode 100644 index 0000000..cd9ceb7 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceSegment.cpp | |||
| @@ -0,0 +1,57 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for segments. | ||
| 4 | * \file IceSegment.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | /** | ||
| 12 | * Segment class. | ||
| 13 | * A segment is defined by S(t) = mP0 * (1 - t) + mP1 * t, with 0 <= t <= 1 | ||
| 14 | * Alternatively, a segment is S(t) = Origin + t * Direction for 0 <= t <= 1. | ||
| 15 | * Direction is not necessarily unit length. The end points are Origin = mP0 and Origin + Direction = mP1. | ||
| 16 | * | ||
| 17 | * \class Segment | ||
| 18 | * \author Pierre Terdiman | ||
| 19 | * \version 1.0 | ||
| 20 | */ | ||
| 21 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 22 | |||
| 23 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 24 | // Precompiled Header | ||
| 25 | #include "Stdafx.h" | ||
| 26 | |||
| 27 | using namespace IceMaths; | ||
| 28 | |||
| 29 | float Segment::SquareDistance(const Point& point, float* t) const | ||
| 30 | { | ||
| 31 | Point Diff = point - mP0; | ||
| 32 | Point Dir = mP1 - mP0; | ||
| 33 | float fT = Diff | Dir; | ||
| 34 | |||
| 35 | if(fT<=0.0f) | ||
| 36 | { | ||
| 37 | fT = 0.0f; | ||
| 38 | } | ||
| 39 | else | ||
| 40 | { | ||
| 41 | float SqrLen= Dir.SquareMagnitude(); | ||
| 42 | if(fT>=SqrLen) | ||
| 43 | { | ||
| 44 | fT = 1.0f; | ||
| 45 | Diff -= Dir; | ||
| 46 | } | ||
| 47 | else | ||
| 48 | { | ||
| 49 | fT /= SqrLen; | ||
| 50 | Diff -= fT*Dir; | ||
| 51 | } | ||
| 52 | } | ||
| 53 | |||
| 54 | if(t) *t = fT; | ||
| 55 | |||
| 56 | return Diff.SquareMagnitude(); | ||
| 57 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceSegment.h b/libraries/ode-0.9/OPCODE/Ice/IceSegment.h new file mode 100644 index 0000000..8d66322 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceSegment.h | |||
| @@ -0,0 +1,55 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for segments. | ||
| 4 | * \file IceSegment.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICESEGMENT_H__ | ||
| 13 | #define __ICESEGMENT_H__ | ||
| 14 | |||
| 15 | class ICEMATHS_API Segment | ||
| 16 | { | ||
| 17 | public: | ||
| 18 | //! Constructor | ||
| 19 | inline_ Segment() {} | ||
| 20 | //! Constructor | ||
| 21 | inline_ Segment(const Point& p0, const Point& p1) : mP0(p0), mP1(p1) {} | ||
| 22 | //! Copy constructor | ||
| 23 | inline_ Segment(const Segment& seg) : mP0(seg.mP0), mP1(seg.mP1) {} | ||
| 24 | //! Destructor | ||
| 25 | inline_ ~Segment() {} | ||
| 26 | |||
| 27 | inline_ const Point& GetOrigin() const { return mP0; } | ||
| 28 | inline_ Point ComputeDirection() const { return mP1 - mP0; } | ||
| 29 | inline_ void ComputeDirection(Point& dir) const { dir = mP1 - mP0; } | ||
| 30 | inline_ float ComputeLength() const { return mP1.Distance(mP0); } | ||
| 31 | inline_ float ComputeSquareLength() const { return mP1.SquareDistance(mP0); } | ||
| 32 | |||
| 33 | inline_ void SetOriginDirection(const Point& origin, const Point& direction) | ||
| 34 | { | ||
| 35 | mP0 = mP1 = origin; | ||
| 36 | mP1 += direction; | ||
| 37 | } | ||
| 38 | |||
| 39 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 40 | /** | ||
| 41 | * Computes a point on the segment | ||
| 42 | * \param pt [out] point on segment | ||
| 43 | * \param t [in] point's parameter [t=0 => pt = mP0, t=1 => pt = mP1] | ||
| 44 | */ | ||
| 45 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 46 | inline_ void ComputePoint(Point& pt, float t) const { pt = mP0 + t * (mP1 - mP0); } | ||
| 47 | |||
| 48 | float SquareDistance(const Point& point, float* t=null) const; | ||
| 49 | inline_ float Distance(const Point& point, float* t=null) const { return sqrtf(SquareDistance(point, t)); } | ||
| 50 | |||
| 51 | Point mP0; //!< Start of segment | ||
| 52 | Point mP1; //!< End of segment | ||
| 53 | }; | ||
| 54 | |||
| 55 | #endif // __ICESEGMENT_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceTriList.h b/libraries/ode-0.9/OPCODE/Ice/IceTriList.h new file mode 100644 index 0000000..b2b6ecf --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceTriList.h | |||
| @@ -0,0 +1,61 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains code for a triangle container. | ||
| 4 | * \file IceTrilist.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICETRILIST_H__ | ||
| 13 | #define __ICETRILIST_H__ | ||
| 14 | |||
| 15 | class ICEMATHS_API TriList : public Container | ||
| 16 | { | ||
| 17 | public: | ||
| 18 | // Constructor / Destructor | ||
| 19 | TriList() {} | ||
| 20 | ~TriList() {} | ||
| 21 | |||
| 22 | inline_ udword GetNbTriangles() const { return GetNbEntries()/9; } | ||
| 23 | inline_ Triangle* GetTriangles() const { return (Triangle*)GetEntries(); } | ||
| 24 | |||
| 25 | void AddTri(const Triangle& tri) | ||
| 26 | { | ||
| 27 | Add(tri.mVerts[0].x).Add(tri.mVerts[0].y).Add(tri.mVerts[0].z); | ||
| 28 | Add(tri.mVerts[1].x).Add(tri.mVerts[1].y).Add(tri.mVerts[1].z); | ||
| 29 | Add(tri.mVerts[2].x).Add(tri.mVerts[2].y).Add(tri.mVerts[2].z); | ||
| 30 | } | ||
| 31 | |||
| 32 | void AddTri(const Point& p0, const Point& p1, const Point& p2) | ||
| 33 | { | ||
| 34 | Add(p0.x).Add(p0.y).Add(p0.z); | ||
| 35 | Add(p1.x).Add(p1.y).Add(p1.z); | ||
| 36 | Add(p2.x).Add(p2.y).Add(p2.z); | ||
| 37 | } | ||
| 38 | }; | ||
| 39 | |||
| 40 | class ICEMATHS_API TriangleList : public Container | ||
| 41 | { | ||
| 42 | public: | ||
| 43 | // Constructor / Destructor | ||
| 44 | TriangleList() {} | ||
| 45 | ~TriangleList() {} | ||
| 46 | |||
| 47 | inline_ udword GetNbTriangles() const { return GetNbEntries()/3; } | ||
| 48 | inline_ IndexedTriangle* GetTriangles() const { return (IndexedTriangle*)GetEntries();} | ||
| 49 | |||
| 50 | void AddTriangle(const IndexedTriangle& tri) | ||
| 51 | { | ||
| 52 | Add(tri.mVRef[0]).Add(tri.mVRef[1]).Add(tri.mVRef[2]); | ||
| 53 | } | ||
| 54 | |||
| 55 | void AddTriangle(udword vref0, udword vref1, udword vref2) | ||
| 56 | { | ||
| 57 | Add(vref0).Add(vref1).Add(vref2); | ||
| 58 | } | ||
| 59 | }; | ||
| 60 | |||
| 61 | #endif //__ICETRILIST_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceTriangle.cpp b/libraries/ode-0.9/OPCODE/Ice/IceTriangle.cpp new file mode 100644 index 0000000..4268ff4 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceTriangle.cpp | |||
| @@ -0,0 +1,286 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains a handy triangle class. | ||
| 4 | * \file IceTriangle.cpp | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 17, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Precompiled Header | ||
| 12 | #include "Stdafx.h" | ||
| 13 | |||
| 14 | using namespace IceMaths; | ||
| 15 | |||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | /** | ||
| 18 | * Contains a triangle class. | ||
| 19 | * | ||
| 20 | * \class Tri | ||
| 21 | * \author Pierre Terdiman | ||
| 22 | * \version 1.0 | ||
| 23 | * \date 08.15.98 | ||
| 24 | */ | ||
| 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 26 | |||
| 27 | static sdword VPlaneSideEps(const Point& v, const Plane& plane, float epsilon) | ||
| 28 | { | ||
| 29 | // Compute distance from current vertex to the plane | ||
| 30 | float Dist = plane.Distance(v); | ||
| 31 | // Compute side: | ||
| 32 | // 1 = the vertex is on the positive side of the plane | ||
| 33 | // -1 = the vertex is on the negative side of the plane | ||
| 34 | // 0 = the vertex is on the plane (within epsilon) | ||
| 35 | return Dist > epsilon ? 1 : Dist < -epsilon ? -1 : 0; | ||
| 36 | } | ||
| 37 | |||
| 38 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 39 | /** | ||
| 40 | * Flips the winding order. | ||
| 41 | */ | ||
| 42 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 43 | void Triangle::Flip() | ||
| 44 | { | ||
| 45 | Point Tmp = mVerts[1]; | ||
| 46 | mVerts[1] = mVerts[2]; | ||
| 47 | mVerts[2] = Tmp; | ||
| 48 | } | ||
| 49 | |||
| 50 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 51 | /** | ||
| 52 | * Computes the triangle area. | ||
| 53 | * \return the area | ||
| 54 | */ | ||
| 55 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 56 | float Triangle::Area() const | ||
| 57 | { | ||
| 58 | const Point& p0 = mVerts[0]; | ||
| 59 | const Point& p1 = mVerts[1]; | ||
| 60 | const Point& p2 = mVerts[2]; | ||
| 61 | return ((p0 - p1)^(p0 - p2)).Magnitude() * 0.5f; | ||
| 62 | } | ||
| 63 | |||
| 64 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 65 | /** | ||
| 66 | * Computes the triangle perimeter. | ||
| 67 | * \return the perimeter | ||
| 68 | */ | ||
| 69 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 70 | float Triangle::Perimeter() const | ||
| 71 | { | ||
| 72 | const Point& p0 = mVerts[0]; | ||
| 73 | const Point& p1 = mVerts[1]; | ||
| 74 | const Point& p2 = mVerts[2]; | ||
| 75 | return p0.Distance(p1) | ||
| 76 | + p0.Distance(p2) | ||
| 77 | + p1.Distance(p2); | ||
| 78 | } | ||
| 79 | |||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | /** | ||
| 82 | * Computes the triangle compacity. | ||
| 83 | * \return the compacity | ||
| 84 | */ | ||
| 85 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 86 | float Triangle::Compacity() const | ||
| 87 | { | ||
| 88 | float P = Perimeter(); | ||
| 89 | if(P==0.0f) return 0.0f; | ||
| 90 | return (4.0f*PI*Area()/(P*P)); | ||
| 91 | } | ||
| 92 | |||
| 93 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 94 | /** | ||
| 95 | * Computes the triangle normal. | ||
| 96 | * \param normal [out] the computed normal | ||
| 97 | */ | ||
| 98 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 99 | void Triangle::Normal(Point& normal) const | ||
| 100 | { | ||
| 101 | const Point& p0 = mVerts[0]; | ||
| 102 | const Point& p1 = mVerts[1]; | ||
| 103 | const Point& p2 = mVerts[2]; | ||
| 104 | normal = ((p0 - p1)^(p0 - p2)).Normalize(); | ||
| 105 | } | ||
| 106 | |||
| 107 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 108 | /** | ||
| 109 | * Computes the triangle denormalized normal. | ||
| 110 | * \param normal [out] the computed normal | ||
| 111 | */ | ||
| 112 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 113 | void Triangle::DenormalizedNormal(Point& normal) const | ||
| 114 | { | ||
| 115 | const Point& p0 = mVerts[0]; | ||
| 116 | const Point& p1 = mVerts[1]; | ||
| 117 | const Point& p2 = mVerts[2]; | ||
| 118 | normal = ((p0 - p1)^(p0 - p2)); | ||
| 119 | } | ||
| 120 | |||
| 121 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 122 | /** | ||
| 123 | * Computes the triangle center. | ||
| 124 | * \param center [out] the computed center | ||
| 125 | */ | ||
| 126 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 127 | void Triangle::Center(Point& center) const | ||
| 128 | { | ||
| 129 | const Point& p0 = mVerts[0]; | ||
| 130 | const Point& p1 = mVerts[1]; | ||
| 131 | const Point& p2 = mVerts[2]; | ||
| 132 | center = (p0 + p1 + p2)*INV3; | ||
| 133 | } | ||
| 134 | |||
| 135 | PartVal Triangle::TestAgainstPlane(const Plane& plane, float epsilon) const | ||
| 136 | { | ||
| 137 | bool Pos = false, Neg = false; | ||
| 138 | |||
| 139 | // Loop through all vertices | ||
| 140 | for(udword i=0;i<3;i++) | ||
| 141 | { | ||
| 142 | // Compute side: | ||
| 143 | sdword Side = VPlaneSideEps(mVerts[i], plane, epsilon); | ||
| 144 | |||
| 145 | if (Side < 0) Neg = true; | ||
| 146 | else if (Side > 0) Pos = true; | ||
| 147 | } | ||
| 148 | |||
| 149 | if (!Pos && !Neg) return TRI_ON_PLANE; | ||
| 150 | else if (Pos && Neg) return TRI_INTERSECT; | ||
| 151 | else if (Pos && !Neg) return TRI_PLUS_SPACE; | ||
| 152 | else if (!Pos && Neg) return TRI_MINUS_SPACE; | ||
| 153 | |||
| 154 | // What?! | ||
| 155 | return TRI_FORCEDWORD; | ||
| 156 | } | ||
| 157 | |||
| 158 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 159 | /** | ||
| 160 | * Computes the triangle moment. | ||
| 161 | * \param m [out] the moment | ||
| 162 | */ | ||
| 163 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 164 | /* | ||
| 165 | void Triangle::ComputeMoment(Moment& m) | ||
| 166 | { | ||
| 167 | // Compute the area of the triangle | ||
| 168 | m.mArea = Area(); | ||
| 169 | |||
| 170 | // Compute the centroid | ||
| 171 | Center(m.mCentroid); | ||
| 172 | |||
| 173 | // Second-order components. Handle zero-area faces. | ||
| 174 | Point& p = mVerts[0]; | ||
| 175 | Point& q = mVerts[1]; | ||
| 176 | Point& r = mVerts[2]; | ||
| 177 | if(m.mArea==0.0f) | ||
| 178 | { | ||
| 179 | // This triangle has zero area. The second order components would be eliminated with the usual formula, so, for the | ||
| 180 | // sake of robustness we use an alternative form. These are the centroid and second-order components of the triangle's vertices. | ||
| 181 | m.mCovariance.m[0][0] = (p.x*p.x + q.x*q.x + r.x*r.x); | ||
| 182 | m.mCovariance.m[0][1] = (p.x*p.y + q.x*q.y + r.x*r.y); | ||
| 183 | m.mCovariance.m[0][2] = (p.x*p.z + q.x*q.z + r.x*r.z); | ||
| 184 | m.mCovariance.m[1][1] = (p.y*p.y + q.y*q.y + r.y*r.y); | ||
| 185 | m.mCovariance.m[1][2] = (p.y*p.z + q.y*q.z + r.y*r.z); | ||
| 186 | m.mCovariance.m[2][2] = (p.z*p.z + q.z*q.z + r.z*r.z); | ||
| 187 | m.mCovariance.m[2][1] = m.mCovariance.m[1][2]; | ||
| 188 | m.mCovariance.m[1][0] = m.mCovariance.m[0][1]; | ||
| 189 | m.mCovariance.m[2][0] = m.mCovariance.m[0][2]; | ||
| 190 | } | ||
| 191 | else | ||
| 192 | { | ||
| 193 | const float OneOverTwelve = 1.0f / 12.0f; | ||
| 194 | m.mCovariance.m[0][0] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.x + p.x*p.x + q.x*q.x + r.x*r.x) * OneOverTwelve; | ||
| 195 | m.mCovariance.m[0][1] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.y + p.x*p.y + q.x*q.y + r.x*r.y) * OneOverTwelve; | ||
| 196 | m.mCovariance.m[1][1] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.y + p.y*p.y + q.y*q.y + r.y*r.y) * OneOverTwelve; | ||
| 197 | m.mCovariance.m[0][2] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.z + p.x*p.z + q.x*q.z + r.x*r.z) * OneOverTwelve; | ||
| 198 | m.mCovariance.m[1][2] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.z + p.y*p.z + q.y*q.z + r.y*r.z) * OneOverTwelve; | ||
| 199 | m.mCovariance.m[2][2] = m.mArea * (9.0f * m.mCentroid.z*m.mCentroid.z + p.z*p.z + q.z*q.z + r.z*r.z) * OneOverTwelve; | ||
| 200 | m.mCovariance.m[2][1] = m.mCovariance.m[1][2]; | ||
| 201 | m.mCovariance.m[1][0] = m.mCovariance.m[0][1]; | ||
| 202 | m.mCovariance.m[2][0] = m.mCovariance.m[0][2]; | ||
| 203 | } | ||
| 204 | } | ||
| 205 | */ | ||
| 206 | |||
| 207 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 208 | /** | ||
| 209 | * Computes the triangle's smallest edge length. | ||
| 210 | * \return the smallest edge length | ||
| 211 | */ | ||
| 212 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 213 | float Triangle::MinEdgeLength() const | ||
| 214 | { | ||
| 215 | float Min = MAX_FLOAT; | ||
| 216 | float Length01 = mVerts[0].Distance(mVerts[1]); | ||
| 217 | float Length02 = mVerts[0].Distance(mVerts[2]); | ||
| 218 | float Length12 = mVerts[1].Distance(mVerts[2]); | ||
| 219 | if(Length01 < Min) Min = Length01; | ||
| 220 | if(Length02 < Min) Min = Length02; | ||
| 221 | if(Length12 < Min) Min = Length12; | ||
| 222 | return Min; | ||
| 223 | } | ||
| 224 | |||
| 225 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 226 | /** | ||
| 227 | * Computes the triangle's largest edge length. | ||
| 228 | * \return the largest edge length | ||
| 229 | */ | ||
| 230 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 231 | float Triangle::MaxEdgeLength() const | ||
| 232 | { | ||
| 233 | float Max = MIN_FLOAT; | ||
| 234 | float Length01 = mVerts[0].Distance(mVerts[1]); | ||
| 235 | float Length02 = mVerts[0].Distance(mVerts[2]); | ||
| 236 | float Length12 = mVerts[1].Distance(mVerts[2]); | ||
| 237 | if(Length01 > Max) Max = Length01; | ||
| 238 | if(Length02 > Max) Max = Length02; | ||
| 239 | if(Length12 > Max) Max = Length12; | ||
| 240 | return Max; | ||
| 241 | } | ||
| 242 | |||
| 243 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 244 | /** | ||
| 245 | * Computes a point on the triangle according to the stabbing information. | ||
| 246 | * \param u,v [in] point's barycentric coordinates | ||
| 247 | * \param pt [out] point on triangle | ||
| 248 | * \param nearvtx [out] index of nearest vertex | ||
| 249 | */ | ||
| 250 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 251 | void Triangle::ComputePoint(float u, float v, Point& pt, udword* nearvtx) const | ||
| 252 | { | ||
| 253 | // Compute point coordinates | ||
| 254 | pt = (1.0f - u - v)*mVerts[0] + u*mVerts[1] + v*mVerts[2]; | ||
| 255 | |||
| 256 | // Compute nearest vertex if needed | ||
| 257 | if(nearvtx) | ||
| 258 | { | ||
| 259 | // Compute distance vector | ||
| 260 | Point d(mVerts[0].SquareDistance(pt), // Distance^2 from vertex 0 to point on the face | ||
| 261 | mVerts[1].SquareDistance(pt), // Distance^2 from vertex 1 to point on the face | ||
| 262 | mVerts[2].SquareDistance(pt)); // Distance^2 from vertex 2 to point on the face | ||
| 263 | |||
| 264 | // Get smallest distance | ||
| 265 | *nearvtx = d.SmallestAxis(); | ||
| 266 | } | ||
| 267 | } | ||
| 268 | |||
| 269 | void Triangle::Inflate(float fat_coeff, bool constant_border) | ||
| 270 | { | ||
| 271 | // Compute triangle center | ||
| 272 | Point TriangleCenter; | ||
| 273 | Center(TriangleCenter); | ||
| 274 | |||
| 275 | // Don't normalize? | ||
| 276 | // Normalize => add a constant border, regardless of triangle size | ||
| 277 | // Don't => add more to big triangles | ||
| 278 | for(udword i=0;i<3;i++) | ||
| 279 | { | ||
| 280 | Point v = mVerts[i] - TriangleCenter; | ||
| 281 | |||
| 282 | if(constant_border) v.Normalize(); | ||
| 283 | |||
| 284 | mVerts[i] += v * fat_coeff; | ||
| 285 | } | ||
| 286 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceTriangle.h b/libraries/ode-0.9/OPCODE/Ice/IceTriangle.h new file mode 100644 index 0000000..a984db8 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceTriangle.h | |||
| @@ -0,0 +1,68 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains a handy triangle class. | ||
| 4 | * \file IceTriangle.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date January, 17, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICETRIANGLE_H__ | ||
| 13 | #define __ICETRIANGLE_H__ | ||
| 14 | |||
| 15 | // Forward declarations | ||
| 16 | class Moment; | ||
| 17 | |||
| 18 | // Partitioning values | ||
| 19 | enum PartVal | ||
| 20 | { | ||
| 21 | TRI_MINUS_SPACE = 0, //!< Triangle is in the negative space | ||
| 22 | TRI_PLUS_SPACE = 1, //!< Triangle is in the positive space | ||
| 23 | TRI_INTERSECT = 2, //!< Triangle intersects plane | ||
| 24 | TRI_ON_PLANE = 3, //!< Triangle and plane are coplanar | ||
| 25 | |||
| 26 | TRI_FORCEDWORD = 0x7fffffff | ||
| 27 | }; | ||
| 28 | |||
| 29 | // A triangle class. | ||
| 30 | class ICEMATHS_API Triangle | ||
| 31 | { | ||
| 32 | public: | ||
| 33 | //! Constructor | ||
| 34 | inline_ Triangle() {} | ||
| 35 | //! Constructor | ||
| 36 | inline_ Triangle(const Point& p0, const Point& p1, const Point& p2) { mVerts[0]=p0; mVerts[1]=p1; mVerts[2]=p2; } | ||
| 37 | //! Copy constructor | ||
| 38 | inline_ Triangle(const Triangle& triangle) | ||
| 39 | { | ||
| 40 | mVerts[0] = triangle.mVerts[0]; | ||
| 41 | mVerts[1] = triangle.mVerts[1]; | ||
| 42 | mVerts[2] = triangle.mVerts[2]; | ||
| 43 | } | ||
| 44 | //! Destructor | ||
| 45 | inline_ ~Triangle() {} | ||
| 46 | //! Vertices | ||
| 47 | Point mVerts[3]; | ||
| 48 | |||
| 49 | // Methods | ||
| 50 | void Flip(); | ||
| 51 | float Area() const; | ||
| 52 | float Perimeter() const; | ||
| 53 | float Compacity() const; | ||
| 54 | void Normal(Point& normal) const; | ||
| 55 | void DenormalizedNormal(Point& normal) const; | ||
| 56 | void Center(Point& center) const; | ||
| 57 | inline_ Plane PlaneEquation() const { return Plane(mVerts[0], mVerts[1], mVerts[2]); } | ||
| 58 | |||
| 59 | PartVal TestAgainstPlane(const Plane& plane, float epsilon) const; | ||
| 60 | // float Distance(Point& cp, Point& cq, Tri& tri); | ||
| 61 | void ComputeMoment(Moment& m); | ||
| 62 | float MinEdgeLength() const; | ||
| 63 | float MaxEdgeLength() const; | ||
| 64 | void ComputePoint(float u, float v, Point& pt, udword* nearvtx=null) const; | ||
| 65 | void Inflate(float fat_coeff, bool constant_border); | ||
| 66 | }; | ||
| 67 | |||
| 68 | #endif // __ICETRIANGLE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceTypes.h b/libraries/ode-0.9/OPCODE/Ice/IceTypes.h new file mode 100644 index 0000000..4ff71b5 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceTypes.h | |||
| @@ -0,0 +1,171 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains custom types. | ||
| 4 | * \file IceTypes.h | ||
| 5 | * \author Pierre Terdiman | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICETYPES_H__ | ||
| 13 | #define __ICETYPES_H__ | ||
| 14 | |||
| 15 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 16 | // Things to help us compile on non-windows platforms | ||
| 17 | |||
| 18 | #if defined(__MACOSX__) || defined(__APPLE__) | ||
| 19 | #undef bool | ||
| 20 | #define bool char | ||
| 21 | #undef true | ||
| 22 | #define true ((bool)-1) | ||
| 23 | #undef false | ||
| 24 | #define false ((bool)0) | ||
| 25 | #endif // mac stuff | ||
| 26 | |||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | #define USE_HANDLE_MANAGER | ||
| 30 | |||
| 31 | // Constants | ||
| 32 | #define PI 3.1415926535897932384626433832795028841971693993751f //!< PI | ||
| 33 | #define HALFPI 1.57079632679489661923f //!< 0.5 * PI | ||
| 34 | #define TWOPI 6.28318530717958647692f //!< 2.0 * PI | ||
| 35 | #define INVPI 0.31830988618379067154f //!< 1.0 / PI | ||
| 36 | |||
| 37 | #define RADTODEG 57.2957795130823208768f //!< 180.0 / PI, convert radians to degrees | ||
| 38 | #define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians | ||
| 39 | |||
| 40 | #define EXP 2.71828182845904523536f //!< e | ||
| 41 | #define INVLOG2 3.32192809488736234787f //!< 1.0 / log10(2) | ||
| 42 | #define LN2 0.693147180559945f //!< ln(2) | ||
| 43 | #define INVLN2 1.44269504089f //!< 1.0f / ln(2) | ||
| 44 | |||
| 45 | #define INV3 0.33333333333333333333f //!< 1/3 | ||
| 46 | #define INV6 0.16666666666666666666f //!< 1/6 | ||
| 47 | #define INV7 0.14285714285714285714f //!< 1/7 | ||
| 48 | #define INV9 0.11111111111111111111f //!< 1/9 | ||
| 49 | #define INV255 0.00392156862745098039f //!< 1/255 | ||
| 50 | |||
| 51 | #define SQRT2 1.41421356237f //!< sqrt(2) | ||
| 52 | #define INVSQRT2 0.707106781188f //!< 1 / sqrt(2) | ||
| 53 | |||
| 54 | #define SQRT3 1.73205080757f //!< sqrt(3) | ||
| 55 | #define INVSQRT3 0.577350269189f //!< 1 / sqrt(3) | ||
| 56 | |||
| 57 | #define null 0 //!< our own NULL pointer | ||
| 58 | |||
| 59 | // Custom types used in ICE | ||
| 60 | typedef signed char sbyte; //!< sizeof(sbyte) must be 1 | ||
| 61 | typedef unsigned char ubyte; //!< sizeof(ubyte) must be 1 | ||
| 62 | typedef signed short sword; //!< sizeof(sword) must be 2 | ||
| 63 | typedef unsigned short uword; //!< sizeof(uword) must be 2 | ||
| 64 | typedef signed int sdword; //!< sizeof(sdword) must be 4 | ||
| 65 | typedef unsigned int udword; //!< sizeof(udword) must be 4 | ||
| 66 | typedef signed __int64 sqword; //!< sizeof(sqword) must be 8 | ||
| 67 | typedef unsigned __int64 uqword; //!< sizeof(uqword) must be 8 | ||
| 68 | typedef float float32; //!< sizeof(float32) must be 4 | ||
| 69 | typedef double float64; //!< sizeof(float64) must be 4 | ||
| 70 | |||
| 71 | ICE_COMPILE_TIME_ASSERT(sizeof(bool)==1); // ...otherwise things might fail with VC++ 4.2 ! | ||
| 72 | ICE_COMPILE_TIME_ASSERT(sizeof(ubyte)==1); | ||
| 73 | ICE_COMPILE_TIME_ASSERT(sizeof(sbyte)==1); | ||
| 74 | ICE_COMPILE_TIME_ASSERT(sizeof(sword)==2); | ||
| 75 | ICE_COMPILE_TIME_ASSERT(sizeof(uword)==2); | ||
| 76 | ICE_COMPILE_TIME_ASSERT(sizeof(udword)==4); | ||
| 77 | ICE_COMPILE_TIME_ASSERT(sizeof(sdword)==4); | ||
| 78 | ICE_COMPILE_TIME_ASSERT(sizeof(uqword)==8); | ||
| 79 | ICE_COMPILE_TIME_ASSERT(sizeof(sqword)==8); | ||
| 80 | |||
| 81 | //! TO BE DOCUMENTED | ||
| 82 | #define DECLARE_ICE_HANDLE(name) struct name##__ { int unused; }; typedef struct name##__ *name | ||
| 83 | |||
| 84 | typedef udword DynID; //!< Dynamic identifier | ||
| 85 | #ifdef USE_HANDLE_MANAGER | ||
| 86 | typedef udword KID; //!< Kernel ID | ||
| 87 | // DECLARE_ICE_HANDLE(KID); | ||
| 88 | #else | ||
| 89 | typedef uword KID; //!< Kernel ID | ||
| 90 | #endif | ||
| 91 | typedef udword RTYPE; //!< Relationship-type (!) between owners and references | ||
| 92 | #define INVALID_ID 0xffffffff //!< Invalid dword ID (counterpart of null pointers) | ||
| 93 | #ifdef USE_HANDLE_MANAGER | ||
| 94 | #define INVALID_KID 0xffffffff //!< Invalid Kernel ID | ||
| 95 | #else | ||
| 96 | #define INVALID_KID 0xffff //!< Invalid Kernel ID | ||
| 97 | #endif | ||
| 98 | #define INVALID_NUMBER 0xDEADBEEF //!< Standard junk value | ||
| 99 | |||
| 100 | // Define BOOL if needed | ||
| 101 | #ifndef BOOL | ||
| 102 | typedef int BOOL; //!< Another boolean type. | ||
| 103 | #endif | ||
| 104 | |||
| 105 | //! Union of a float and a sdword | ||
| 106 | typedef union { | ||
| 107 | float f; //!< The float | ||
| 108 | sdword d; //!< The integer | ||
| 109 | }scell; | ||
| 110 | |||
| 111 | //! Union of a float and a udword | ||
| 112 | typedef union { | ||
| 113 | float f; //!< The float | ||
| 114 | udword d; //!< The integer | ||
| 115 | }ucell; | ||
| 116 | |||
| 117 | // Type ranges | ||
| 118 | #define MAX_SBYTE 0x7f //!< max possible sbyte value | ||
| 119 | #define MIN_SBYTE 0x80 //!< min possible sbyte value | ||
| 120 | #define MAX_UBYTE 0xff //!< max possible ubyte value | ||
| 121 | #define MIN_UBYTE 0x00 //!< min possible ubyte value | ||
| 122 | #define MAX_SWORD 0x7fff //!< max possible sword value | ||
| 123 | #define MIN_SWORD 0x8000 //!< min possible sword value | ||
| 124 | #define MAX_UWORD 0xffff //!< max possible uword value | ||
| 125 | #define MIN_UWORD 0x0000 //!< min possible uword value | ||
| 126 | #define MAX_SDWORD 0x7fffffff //!< max possible sdword value | ||
| 127 | #define MIN_SDWORD 0x80000000 //!< min possible sdword value | ||
| 128 | #define MAX_UDWORD 0xffffffff //!< max possible udword value | ||
| 129 | #define MIN_UDWORD 0x00000000 //!< min possible udword value | ||
| 130 | #define MAX_FLOAT FLT_MAX //!< max possible float value | ||
| 131 | #define MIN_FLOAT (-FLT_MAX) //!< min possible loat value | ||
| 132 | #define IEEE_1_0 0x3f800000 //!< integer representation of 1.0 | ||
| 133 | #define IEEE_255_0 0x437f0000 //!< integer representation of 255.0 | ||
| 134 | #define IEEE_MAX_FLOAT 0x7f7fffff //!< integer representation of MAX_FLOAT | ||
| 135 | #define IEEE_MIN_FLOAT 0xff7fffff //!< integer representation of MIN_FLOAT | ||
| 136 | #define IEEE_UNDERFLOW_LIMIT 0x1a000000 | ||
| 137 | |||
| 138 | #define ONE_OVER_RAND_MAX (1.0f / float(RAND_MAX)) //!< Inverse of the max possible value returned by rand() | ||
| 139 | |||
| 140 | typedef int (__stdcall* PROC)(); //!< A standard procedure call. | ||
| 141 | typedef bool (*ENUMERATION)(udword value, udword param, udword context); //!< ICE standard enumeration call | ||
| 142 | typedef void** VTABLE; //!< A V-Table. | ||
| 143 | |||
| 144 | #undef MIN | ||
| 145 | #undef MAX | ||
| 146 | #define MIN(a, b) ((a) < (b) ? (a) : (b)) //!< Returns the min value between a and b | ||
| 147 | #define MAX(a, b) ((a) > (b) ? (a) : (b)) //!< Returns the max value between a and b | ||
| 148 | #define MAXMAX(a,b,c) ((a) > (b) ? MAX (a,c) : MAX (b,c)) //!< Returns the max value between a, b and c | ||
| 149 | |||
| 150 | template<class T> inline_ const T& TMin (const T& a, const T& b) { return b < a ? b : a; } | ||
| 151 | template<class T> inline_ const T& TMax (const T& a, const T& b) { return a < b ? b : a; } | ||
| 152 | template<class T> inline_ void TSetMin (T& a, const T& b) { if(a>b) a = b; } | ||
| 153 | template<class T> inline_ void TSetMax (T& a, const T& b) { if(a<b) a = b; } | ||
| 154 | |||
| 155 | #define SQR(x) ((x)*(x)) //!< Returns x square | ||
| 156 | #define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube | ||
| 157 | |||
| 158 | #define AND & //!< ... | ||
| 159 | #define OR | //!< ... | ||
| 160 | #define XOR ^ //!< ... | ||
| 161 | |||
| 162 | #define QUADRAT(x) ((x)*(x)) //!< Returns x square | ||
| 163 | |||
| 164 | #ifdef _WIN32 | ||
| 165 | # define srand48(x) srand((unsigned int) (x)) | ||
| 166 | # define srandom(x) srand((unsigned int) (x)) | ||
| 167 | # define random() ((double) rand()) | ||
| 168 | # define drand48() ((double) (((double) rand()) / ((double) RAND_MAX))) | ||
| 169 | #endif | ||
| 170 | |||
| 171 | #endif // __ICETYPES_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceUtils.cpp b/libraries/ode-0.9/OPCODE/Ice/IceUtils.cpp new file mode 100644 index 0000000..29b6c57 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceUtils.cpp | |||
| @@ -0,0 +1,39 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains misc. useful macros & defines. | ||
| 4 | * \file IceUtils.cpp | ||
| 5 | * \author Pierre Terdiman (collected from various sources) | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Precompiled Header | ||
| 12 | #include "Stdafx.h" | ||
| 13 | |||
| 14 | using namespace IceCore; | ||
| 15 | |||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | /** | ||
| 18 | * Returns the alignment of the input address. | ||
| 19 | * \fn Alignment() | ||
| 20 | * \param address [in] address to check | ||
| 21 | * \return the best alignment (e.g. 1 for odd addresses, etc) | ||
| 22 | */ | ||
| 23 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 24 | udword IceCore::Alignment(udword address) | ||
| 25 | { | ||
| 26 | // Returns 0 for null addresses | ||
| 27 | if(!address) return 0; | ||
| 28 | |||
| 29 | // Test all bits | ||
| 30 | udword Align = 1; | ||
| 31 | for(udword i=1;i<32;i++) | ||
| 32 | { | ||
| 33 | // Returns as soon as the alignment is broken | ||
| 34 | if(address&Align) return Align; | ||
| 35 | Align<<=1; | ||
| 36 | } | ||
| 37 | // Here all bits are null, except the highest one (else the address would be null) | ||
| 38 | return Align; | ||
| 39 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/Ice/IceUtils.h b/libraries/ode-0.9/OPCODE/Ice/IceUtils.h new file mode 100644 index 0000000..f4552ec --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Ice/IceUtils.h | |||
| @@ -0,0 +1,256 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Contains misc. useful macros & defines. | ||
| 4 | * \file IceUtils.h | ||
| 5 | * \author Pierre Terdiman (collected from various sources) | ||
| 6 | * \date April, 4, 2000 | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 11 | // Include Guard | ||
| 12 | #ifndef __ICEUTILS_H__ | ||
| 13 | #define __ICEUTILS_H__ | ||
| 14 | |||
| 15 | #define START_RUNONCE { static bool __RunOnce__ = false; if(!__RunOnce__){ | ||
| 16 | #define END_RUNONCE __RunOnce__ = true;}} | ||
| 17 | |||
| 18 | //! Reverse all the bits in a 32 bit word (from Steve Baker's Cute Code Collection) | ||
| 19 | //! (each line can be done in any order. | ||
| 20 | inline_ void ReverseBits(udword& n) | ||
| 21 | { | ||
| 22 | n = ((n >> 1) & 0x55555555) | ((n << 1) & 0xaaaaaaaa); | ||
| 23 | n = ((n >> 2) & 0x33333333) | ((n << 2) & 0xcccccccc); | ||
| 24 | n = ((n >> 4) & 0x0f0f0f0f) | ((n << 4) & 0xf0f0f0f0); | ||
| 25 | n = ((n >> 8) & 0x00ff00ff) | ((n << 8) & 0xff00ff00); | ||
| 26 | n = ((n >> 16) & 0x0000ffff) | ((n << 16) & 0xffff0000); | ||
| 27 | // Etc for larger intergers (64 bits in Java) | ||
| 28 | // NOTE: the >> operation must be unsigned! (>>> in java) | ||
| 29 | } | ||
| 30 | |||
| 31 | //! Count the number of '1' bits in a 32 bit word (from Steve Baker's Cute Code Collection) | ||
| 32 | inline_ udword CountBits(udword n) | ||
| 33 | { | ||
| 34 | // This relies of the fact that the count of n bits can NOT overflow | ||
| 35 | // an n bit interger. EG: 1 bit count takes a 1 bit interger, 2 bit counts | ||
| 36 | // 2 bit interger, 3 bit count requires only a 2 bit interger. | ||
| 37 | // So we add all bit pairs, then each nible, then each byte etc... | ||
| 38 | n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1); | ||
| 39 | n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2); | ||
| 40 | n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4); | ||
| 41 | n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8); | ||
| 42 | n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16); | ||
| 43 | // Etc for larger intergers (64 bits in Java) | ||
| 44 | // NOTE: the >> operation must be unsigned! (>>> in java) | ||
| 45 | return n; | ||
| 46 | } | ||
| 47 | |||
| 48 | //! Even faster? | ||
| 49 | inline_ udword CountBits2(udword bits) | ||
| 50 | { | ||
| 51 | bits = bits - ((bits >> 1) & 0x55555555); | ||
| 52 | bits = ((bits >> 2) & 0x33333333) + (bits & 0x33333333); | ||
| 53 | bits = ((bits >> 4) + bits) & 0x0F0F0F0F; | ||
| 54 | return (bits * 0x01010101) >> 24; | ||
| 55 | } | ||
| 56 | |||
| 57 | //! Spread out bits. EG 00001111 -> 0101010101 | ||
| 58 | //! 00001010 -> 0100010000 | ||
| 59 | //! This is used to interleve to intergers to produce a `Morten Key' | ||
| 60 | //! used in Space Filling Curves (See DrDobbs Journal, July 1999) | ||
| 61 | //! Order is important. | ||
| 62 | inline_ void SpreadBits(udword& n) | ||
| 63 | { | ||
| 64 | n = ( n & 0x0000ffff) | (( n & 0xffff0000) << 16); | ||
| 65 | n = ( n & 0x000000ff) | (( n & 0x0000ff00) << 8); | ||
| 66 | n = ( n & 0x000f000f) | (( n & 0x00f000f0) << 4); | ||
| 67 | n = ( n & 0x03030303) | (( n & 0x0c0c0c0c) << 2); | ||
| 68 | n = ( n & 0x11111111) | (( n & 0x22222222) << 1); | ||
| 69 | } | ||
| 70 | |||
| 71 | // Next Largest Power of 2 | ||
| 72 | // Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm | ||
| 73 | // that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with | ||
| 74 | // the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next | ||
| 75 | // largest power of 2. For a 32-bit value: | ||
| 76 | inline_ udword nlpo2(udword x) | ||
| 77 | { | ||
| 78 | x |= (x >> 1); | ||
| 79 | x |= (x >> 2); | ||
| 80 | x |= (x >> 4); | ||
| 81 | x |= (x >> 8); | ||
| 82 | x |= (x >> 16); | ||
| 83 | return x+1; | ||
| 84 | } | ||
| 85 | |||
| 86 | //! Test to see if a number is an exact power of two (from Steve Baker's Cute Code Collection) | ||
| 87 | inline_ bool IsPowerOfTwo(udword n) { return ((n&(n-1))==0); } | ||
| 88 | |||
| 89 | //! Zero the least significant '1' bit in a word. (from Steve Baker's Cute Code Collection) | ||
| 90 | inline_ void ZeroLeastSetBit(udword& n) { n&=(n-1); } | ||
| 91 | |||
| 92 | //! Set the least significant N bits in a word. (from Steve Baker's Cute Code Collection) | ||
| 93 | inline_ void SetLeastNBits(udword& x, udword n) { x|=~(~0<<n); } | ||
| 94 | |||
| 95 | //! Classic XOR swap (from Steve Baker's Cute Code Collection) | ||
| 96 | //! x ^= y; /* x' = (x^y) */ | ||
| 97 | //! y ^= x; /* y' = (y^(x^y)) = x */ | ||
| 98 | //! x ^= y; /* x' = (x^y)^x = y */ | ||
| 99 | inline_ void Swap(udword& x, udword& y) { x ^= y; y ^= x; x ^= y; } | ||
| 100 | |||
| 101 | //! Little/Big endian (from Steve Baker's Cute Code Collection) | ||
| 102 | //! | ||
| 103 | //! Extra comments by Kenny Hoff: | ||
| 104 | //! Determines the byte-ordering of the current machine (little or big endian) | ||
| 105 | //! by setting an integer value to 1 (so least significant bit is now 1); take | ||
| 106 | //! the address of the int and cast to a byte pointer (treat integer as an | ||
| 107 | //! array of four bytes); check the value of the first byte (must be 0 or 1). | ||
| 108 | //! If the value is 1, then the first byte least significant byte and this | ||
| 109 | //! implies LITTLE endian. If the value is 0, the first byte is the most | ||
| 110 | //! significant byte, BIG endian. Examples: | ||
| 111 | //! integer 1 on BIG endian: 00000000 00000000 00000000 00000001 | ||
| 112 | //! integer 1 on LITTLE endian: 00000001 00000000 00000000 00000000 | ||
| 113 | //!--------------------------------------------------------------------------- | ||
| 114 | //! int IsLittleEndian() { int x=1; return ( ((char*)(&x))[0] ); } | ||
| 115 | inline_ char LittleEndian() { int i = 1; return *((char*)&i); } | ||
| 116 | |||
| 117 | //!< Alternative abs function | ||
| 118 | inline_ udword abs_(sdword x) { sdword y= x >> 31; return (x^y)-y; } | ||
| 119 | |||
| 120 | //!< Alternative min function | ||
| 121 | inline_ sdword min_(sdword a, sdword b) { sdword delta = b-a; return a + (delta&(delta>>31)); } | ||
| 122 | |||
| 123 | // Determine if one of the bytes in a 4 byte word is zero | ||
| 124 | inline_ BOOL HasNullByte(udword x) { return ((x + 0xfefefeff) & (~x) & 0x80808080); } | ||
| 125 | |||
| 126 | // To find the smallest 1 bit in a word EG: ~~~~~~10---0 => 0----010---0 | ||
| 127 | inline_ udword LowestOneBit(udword w) { return ((w) & (~(w)+1)); } | ||
| 128 | // inline_ udword LowestOneBit_(udword w) { return ((w) & (-(w))); } | ||
| 129 | |||
| 130 | // Most Significant 1 Bit | ||
| 131 | // Given a binary integer value x, the most significant 1 bit (highest numbered element of a bit set) | ||
| 132 | // can be computed using a SWAR algorithm that recursively "folds" the upper bits into the lower bits. | ||
| 133 | // This process yields a bit vector with the same most significant 1 as x, but all 1's below it. | ||
| 134 | // Bitwise AND of the original value with the complement of the "folded" value shifted down by one | ||
| 135 | // yields the most significant bit. For a 32-bit value: | ||
| 136 | inline_ udword msb32(udword x) | ||
| 137 | { | ||
| 138 | x |= (x >> 1); | ||
| 139 | x |= (x >> 2); | ||
| 140 | x |= (x >> 4); | ||
| 141 | x |= (x >> 8); | ||
| 142 | x |= (x >> 16); | ||
| 143 | return (x & ~(x >> 1)); | ||
| 144 | } | ||
| 145 | |||
| 146 | /* | ||
| 147 | "Just call it repeatedly with various input values and always with the same variable as "memory". | ||
| 148 | The sharpness determines the degree of filtering, where 0 completely filters out the input, and 1 | ||
| 149 | does no filtering at all. | ||
| 150 | |||
| 151 | I seem to recall from college that this is called an IIR (Infinite Impulse Response) filter. As opposed | ||
| 152 | to the more typical FIR (Finite Impulse Response). | ||
| 153 | |||
| 154 | Also, I'd say that you can make more intelligent and interesting filters than this, for example filters | ||
| 155 | that remove wrong responses from the mouse because it's being moved too fast. You'd want such a filter | ||
| 156 | to be applied before this one, of course." | ||
| 157 | |||
| 158 | (JCAB on Flipcode) | ||
| 159 | */ | ||
| 160 | inline_ float FeedbackFilter(float val, float& memory, float sharpness) | ||
| 161 | { | ||
| 162 | ASSERT(sharpness>=0.0f && sharpness<=1.0f && "Invalid sharpness value in feedback filter"); | ||
| 163 | if(sharpness<0.0f) sharpness = 0.0f; | ||
| 164 | else if(sharpness>1.0f) sharpness = 1.0f; | ||
| 165 | return memory = val * sharpness + memory * (1.0f - sharpness); | ||
| 166 | } | ||
| 167 | |||
| 168 | //! If you can guarantee that your input domain (i.e. value of x) is slightly | ||
| 169 | //! limited (abs(x) must be < ((1<<31u)-32767)), then you can use the | ||
| 170 | //! following code to clamp the resulting value into [-32768,+32767] range: | ||
| 171 | inline_ int ClampToInt16(int x) | ||
| 172 | { | ||
| 173 | // ASSERT(abs(x) < (int)((1<<31u)-32767)); | ||
| 174 | |||
| 175 | int delta = 32767 - x; | ||
| 176 | x += (delta>>31) & delta; | ||
| 177 | delta = x + 32768; | ||
| 178 | x -= (delta>>31) & delta; | ||
| 179 | return x; | ||
| 180 | } | ||
| 181 | |||
| 182 | // Generic functions | ||
| 183 | template<class Type> inline_ void TSwap(Type& a, Type& b) { const Type c = a; a = b; b = c; } | ||
| 184 | template<class Type> inline_ Type TClamp(const Type& x, const Type& lo, const Type& hi) { return ((x<lo) ? lo : (x>hi) ? hi : x); } | ||
| 185 | |||
| 186 | template<class Type> inline_ void TSort(Type& a, Type& b) | ||
| 187 | { | ||
| 188 | if(a>b) TSwap(a, b); | ||
| 189 | } | ||
| 190 | |||
| 191 | template<class Type> inline_ void TSort(Type& a, Type& b, Type& c) | ||
| 192 | { | ||
| 193 | if(a>b) TSwap(a, b); | ||
| 194 | if(b>c) TSwap(b, c); | ||
| 195 | if(a>b) TSwap(a, b); | ||
| 196 | if(b>c) TSwap(b, c); | ||
| 197 | } | ||
| 198 | |||
| 199 | // Prevent nasty user-manipulations (strategy borrowed from Charles Bloom) | ||
| 200 | // #define PREVENT_COPY(curclass) void operator = (const curclass& object) { ASSERT(!"Bad use of operator ="); } | ||
| 201 | // ... actually this is better ! | ||
| 202 | #define PREVENT_COPY(cur_class) private: cur_class(const cur_class& object); cur_class& operator=(const cur_class& object); | ||
| 203 | |||
| 204 | //! TO BE DOCUMENTED | ||
| 205 | #define OFFSET_OF(Class, Member) (size_t)&(((Class*)0)->Member) | ||
| 206 | //! TO BE DOCUMENTED | ||
| 207 | #define ARRAYSIZE(p) (sizeof(p)/sizeof(p[0])) | ||
| 208 | |||
| 209 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 210 | /** | ||
| 211 | * Returns the alignment of the input address. | ||
| 212 | * \fn Alignment() | ||
| 213 | * \param address [in] address to check | ||
| 214 | * \return the best alignment (e.g. 1 for odd addresses, etc) | ||
| 215 | */ | ||
| 216 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 217 | FUNCTION ICECORE_API udword Alignment(udword address); | ||
| 218 | |||
| 219 | #define IS_ALIGNED_2(x) ((x&1)==0) | ||
| 220 | #define IS_ALIGNED_4(x) ((x&3)==0) | ||
| 221 | #define IS_ALIGNED_8(x) ((x&7)==0) | ||
| 222 | |||
| 223 | inline_ void _prefetch(void const* ptr) { (void)*(char const volatile *)ptr; } | ||
| 224 | |||
| 225 | // Compute implicit coords from an index: | ||
| 226 | // The idea is to get back 2D coords from a 1D index. | ||
| 227 | // For example: | ||
| 228 | // | ||
| 229 | // 0 1 2 ... nbu-1 | ||
| 230 | // nbu nbu+1 i ... | ||
| 231 | // | ||
| 232 | // We have i, we're looking for the equivalent (u=2, v=1) location. | ||
| 233 | // i = u + v*nbu | ||
| 234 | // <=> i/nbu = u/nbu + v | ||
| 235 | // Since 0 <= u < nbu, u/nbu = 0 (integer) | ||
| 236 | // Hence: v = i/nbu | ||
| 237 | // Then we simply put it back in the original equation to compute u = i - v*nbu | ||
| 238 | inline_ void Compute2DCoords(udword& u, udword& v, udword i, udword nbu) | ||
| 239 | { | ||
| 240 | v = i / nbu; | ||
| 241 | u = i - (v * nbu); | ||
| 242 | } | ||
| 243 | |||
| 244 | // In 3D: i = u + v*nbu + w*nbu*nbv | ||
| 245 | // <=> i/(nbu*nbv) = u/(nbu*nbv) + v/nbv + w | ||
| 246 | // u/(nbu*nbv) is null since u/nbu was null already. | ||
| 247 | // v/nbv is null as well for the same reason. | ||
| 248 | // Hence w = i/(nbu*nbv) | ||
| 249 | // Then we're left with a 2D problem: i' = i - w*nbu*nbv = u + v*nbu | ||
| 250 | inline_ void Compute3DCoords(udword& u, udword& v, udword& w, udword i, udword nbu, udword nbu_nbv) | ||
| 251 | { | ||
| 252 | w = i / (nbu_nbv); | ||
| 253 | Compute2DCoords(u, v, i - (w * nbu_nbv), nbu); | ||
| 254 | } | ||
| 255 | |||
| 256 | #endif // __ICEUTILS_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_AABBCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_AABBCollider.cpp new file mode 100644 index 0000000..eaaadf1 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_AABBCollider.cpp | |||
| @@ -0,0 +1,696 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for an AABB collider. | ||
| 12 | * \file OPC_AABBCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 1st, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains an AABB-vs-tree collider. | ||
| 21 | * | ||
| 22 | * \class AABBCollider | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date January, 1st, 2002 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | // Precompiled Header | ||
| 31 | #include "Stdafx.h" | ||
| 32 | |||
| 33 | using namespace Opcode; | ||
| 34 | |||
| 35 | #include "OPC_BoxBoxOverlap.h" | ||
| 36 | #include "OPC_TriBoxOverlap.h" | ||
| 37 | |||
| 38 | #define SET_CONTACT(prim_index, flag) \ | ||
| 39 | /* Set contact status */ \ | ||
| 40 | mFlags |= flag; \ | ||
| 41 | mTouchedPrimitives->Add(udword(prim_index)); | ||
| 42 | |||
| 43 | //! AABB-triangle test | ||
| 44 | #define AABB_PRIM(prim_index, flag) \ | ||
| 45 | /* Request vertices from the app */ \ | ||
| 46 | VertexPointers VP; mIMesh->GetTriangle(VP, prim_index);\ | ||
| 47 | mLeafVerts[0] = *VP.Vertex[0]; \ | ||
| 48 | mLeafVerts[1] = *VP.Vertex[1]; \ | ||
| 49 | mLeafVerts[2] = *VP.Vertex[2]; \ | ||
| 50 | /* Perform triangle-box overlap test */ \ | ||
| 51 | if(TriBoxOverlap()) \ | ||
| 52 | { \ | ||
| 53 | SET_CONTACT(prim_index, flag) \ | ||
| 54 | } | ||
| 55 | |||
| 56 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 57 | /** | ||
| 58 | * Constructor. | ||
| 59 | */ | ||
| 60 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 61 | AABBCollider::AABBCollider() | ||
| 62 | { | ||
| 63 | } | ||
| 64 | |||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | /** | ||
| 67 | * Destructor. | ||
| 68 | */ | ||
| 69 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 70 | AABBCollider::~AABBCollider() | ||
| 71 | { | ||
| 72 | } | ||
| 73 | |||
| 74 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 75 | /** | ||
| 76 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 77 | * - with GetContactStatus() | ||
| 78 | * - with GetNbTouchedPrimitives() | ||
| 79 | * - with GetTouchedPrimitives() | ||
| 80 | * | ||
| 81 | * \param cache [in/out] a box cache | ||
| 82 | * \param box [in] collision AABB in world space | ||
| 83 | * \param model [in] Opcode model to collide with | ||
| 84 | * \return true if success | ||
| 85 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 86 | */ | ||
| 87 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 88 | bool AABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const Model& model) | ||
| 89 | { | ||
| 90 | // Checkings | ||
| 91 | if(!Setup(&model)) return false; | ||
| 92 | |||
| 93 | // Init collision query | ||
| 94 | if(InitQuery(cache, box)) return true; | ||
| 95 | |||
| 96 | if(!model.HasLeafNodes()) | ||
| 97 | { | ||
| 98 | if(model.IsQuantized()) | ||
| 99 | { | ||
| 100 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 101 | |||
| 102 | // Setup dequantization coeffs | ||
| 103 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 104 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 105 | |||
| 106 | // Perform collision query | ||
| 107 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 108 | else _Collide(Tree->GetNodes()); | ||
| 109 | } | ||
| 110 | else | ||
| 111 | { | ||
| 112 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 113 | |||
| 114 | // Perform collision query | ||
| 115 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 116 | else _Collide(Tree->GetNodes()); | ||
| 117 | } | ||
| 118 | } | ||
| 119 | else | ||
| 120 | { | ||
| 121 | if(model.IsQuantized()) | ||
| 122 | { | ||
| 123 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 124 | |||
| 125 | // Setup dequantization coeffs | ||
| 126 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 127 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 128 | |||
| 129 | // Perform collision query | ||
| 130 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 131 | else _Collide(Tree->GetNodes()); | ||
| 132 | } | ||
| 133 | else | ||
| 134 | { | ||
| 135 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 136 | |||
| 137 | // Perform collision query | ||
| 138 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 139 | else _Collide(Tree->GetNodes()); | ||
| 140 | } | ||
| 141 | } | ||
| 142 | return true; | ||
| 143 | } | ||
| 144 | |||
| 145 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 146 | /** | ||
| 147 | * Initializes a collision query : | ||
| 148 | * - reset stats & contact status | ||
| 149 | * - check temporal coherence | ||
| 150 | * | ||
| 151 | * \param cache [in/out] a box cache | ||
| 152 | * \param box [in] AABB in world space | ||
| 153 | * \return TRUE if we can return immediately | ||
| 154 | */ | ||
| 155 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 156 | BOOL AABBCollider::InitQuery(AABBCache& cache, const CollisionAABB& box) | ||
| 157 | { | ||
| 158 | // 1) Call the base method | ||
| 159 | VolumeCollider::InitQuery(); | ||
| 160 | |||
| 161 | // 2) Keep track of the query box | ||
| 162 | mBox = box; | ||
| 163 | |||
| 164 | // 3) Setup destination pointer | ||
| 165 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 166 | |||
| 167 | // 4) Special case: 1-triangle meshes [Opcode 1.3] | ||
| 168 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 169 | { | ||
| 170 | if(!SkipPrimitiveTests()) | ||
| 171 | { | ||
| 172 | // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. | ||
| 173 | mTouchedPrimitives->Reset(); | ||
| 174 | |||
| 175 | // Perform overlap test between the unique triangle and the box (and set contact status if needed) | ||
| 176 | AABB_PRIM(udword(0), OPC_CONTACT) | ||
| 177 | |||
| 178 | // Return immediately regardless of status | ||
| 179 | return TRUE; | ||
| 180 | } | ||
| 181 | } | ||
| 182 | |||
| 183 | // 5) Check temporal coherence : | ||
| 184 | if(TemporalCoherenceEnabled()) | ||
| 185 | { | ||
| 186 | // Here we use temporal coherence | ||
| 187 | // => check results from previous frame before performing the collision query | ||
| 188 | if(FirstContactEnabled()) | ||
| 189 | { | ||
| 190 | // We're only interested in the first contact found => test the unique previously touched face | ||
| 191 | if(mTouchedPrimitives->GetNbEntries()) | ||
| 192 | { | ||
| 193 | // Get index of previously touched face = the first entry in the array | ||
| 194 | udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0); | ||
| 195 | |||
| 196 | // Then reset the array: | ||
| 197 | // - if the overlap test below is successful, the index we'll get added back anyway | ||
| 198 | // - if it isn't, then the array should be reset anyway for the normal query | ||
| 199 | mTouchedPrimitives->Reset(); | ||
| 200 | |||
| 201 | // Perform overlap test between the cached triangle and the box (and set contact status if needed) | ||
| 202 | AABB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT) | ||
| 203 | |||
| 204 | // Return immediately if possible | ||
| 205 | if(GetContactStatus()) return TRUE; | ||
| 206 | } | ||
| 207 | // else no face has been touched during previous query | ||
| 208 | // => we'll have to perform a normal query | ||
| 209 | } | ||
| 210 | else | ||
| 211 | { | ||
| 212 | // We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious): | ||
| 213 | if(IsCacheValid(cache) && mBox.IsInside(cache.FatBox)) | ||
| 214 | { | ||
| 215 | // - if N is included in P, return previous list | ||
| 216 | // => we simply leave the list (mTouchedFaces) unchanged | ||
| 217 | |||
| 218 | // Set contact status if needed | ||
| 219 | if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT; | ||
| 220 | |||
| 221 | // In any case we don't need to do a query | ||
| 222 | return TRUE; | ||
| 223 | } | ||
| 224 | else | ||
| 225 | { | ||
| 226 | // - else do the query using a fat N | ||
| 227 | |||
| 228 | // Reset cache since we'll about to perform a real query | ||
| 229 | mTouchedPrimitives->Reset(); | ||
| 230 | |||
| 231 | // Make a fat box so that coherence will work for subsequent frames | ||
| 232 | mBox.mExtents *= cache.FatCoeff; | ||
| 233 | |||
| 234 | // Update cache with query data (signature for cached faces) | ||
| 235 | cache.FatBox = mBox; | ||
| 236 | } | ||
| 237 | } | ||
| 238 | } | ||
| 239 | else | ||
| 240 | { | ||
| 241 | // Here we don't use temporal coherence => do a normal query | ||
| 242 | mTouchedPrimitives->Reset(); | ||
| 243 | } | ||
| 244 | |||
| 245 | // 5) Precompute min & max bounds if needed | ||
| 246 | mMin = box.mCenter - box.mExtents; | ||
| 247 | mMax = box.mCenter + box.mExtents; | ||
| 248 | |||
| 249 | return FALSE; | ||
| 250 | } | ||
| 251 | |||
| 252 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 253 | /** | ||
| 254 | * Collision query for vanilla AABB trees. | ||
| 255 | * \param cache [in/out] a box cache | ||
| 256 | * \param box [in] collision AABB in world space | ||
| 257 | * \param tree [in] AABB tree | ||
| 258 | * \return true if success | ||
| 259 | */ | ||
| 260 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 261 | bool AABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const AABBTree* tree) | ||
| 262 | { | ||
| 263 | // This is typically called for a scene tree, full of -AABBs-, not full of triangles. | ||
| 264 | // So we don't really have "primitives" to deal with. Hence it doesn't work with | ||
| 265 | // "FirstContact" + "TemporalCoherence". | ||
| 266 | ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) ); | ||
| 267 | |||
| 268 | // Checkings | ||
| 269 | if(!tree) return false; | ||
| 270 | |||
| 271 | // Init collision query | ||
| 272 | if(InitQuery(cache, box)) return true; | ||
| 273 | |||
| 274 | // Perform collision query | ||
| 275 | _Collide(tree); | ||
| 276 | |||
| 277 | return true; | ||
| 278 | } | ||
| 279 | |||
| 280 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 281 | /** | ||
| 282 | * Checks the AABB completely contains the box. In which case we can end the query sooner. | ||
| 283 | * \param bc [in] box center | ||
| 284 | * \param be [in] box extents | ||
| 285 | * \return true if the AABB contains the whole box | ||
| 286 | */ | ||
| 287 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 288 | inline_ BOOL AABBCollider::AABBContainsBox(const Point& bc, const Point& be) | ||
| 289 | { | ||
| 290 | if(mMin.x > bc.x - be.x) return FALSE; | ||
| 291 | if(mMin.y > bc.y - be.y) return FALSE; | ||
| 292 | if(mMin.z > bc.z - be.z) return FALSE; | ||
| 293 | |||
| 294 | if(mMax.x < bc.x + be.x) return FALSE; | ||
| 295 | if(mMax.y < bc.y + be.y) return FALSE; | ||
| 296 | if(mMax.z < bc.z + be.z) return FALSE; | ||
| 297 | |||
| 298 | return TRUE; | ||
| 299 | } | ||
| 300 | |||
| 301 | #define TEST_BOX_IN_AABB(center, extents) \ | ||
| 302 | if(AABBContainsBox(center, extents)) \ | ||
| 303 | { \ | ||
| 304 | /* Set contact status */ \ | ||
| 305 | mFlags |= OPC_CONTACT; \ | ||
| 306 | _Dump(node); \ | ||
| 307 | return; \ | ||
| 308 | } | ||
| 309 | |||
| 310 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 311 | /** | ||
| 312 | * Recursive collision query for normal AABB trees. | ||
| 313 | * \param node [in] current collision node | ||
| 314 | */ | ||
| 315 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 316 | void AABBCollider::_Collide(const AABBCollisionNode* node) | ||
| 317 | { | ||
| 318 | // Perform AABB-AABB overlap test | ||
| 319 | if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 320 | |||
| 321 | TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 322 | |||
| 323 | if(node->IsLeaf()) | ||
| 324 | { | ||
| 325 | AABB_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 326 | } | ||
| 327 | else | ||
| 328 | { | ||
| 329 | _Collide(node->GetPos()); | ||
| 330 | |||
| 331 | if(ContactFound()) return; | ||
| 332 | |||
| 333 | _Collide(node->GetNeg()); | ||
| 334 | } | ||
| 335 | } | ||
| 336 | |||
| 337 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 338 | /** | ||
| 339 | * Recursive collision query for normal AABB trees, without primitive tests. | ||
| 340 | * \param node [in] current collision node | ||
| 341 | */ | ||
| 342 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 343 | void AABBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node) | ||
| 344 | { | ||
| 345 | // Perform AABB-AABB overlap test | ||
| 346 | if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 347 | |||
| 348 | TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 349 | |||
| 350 | if(node->IsLeaf()) | ||
| 351 | { | ||
| 352 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 353 | } | ||
| 354 | else | ||
| 355 | { | ||
| 356 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 357 | |||
| 358 | if(ContactFound()) return; | ||
| 359 | |||
| 360 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 361 | } | ||
| 362 | } | ||
| 363 | |||
| 364 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 365 | /** | ||
| 366 | * Recursive collision query for quantized AABB trees. | ||
| 367 | * \param node [in] current collision node | ||
| 368 | */ | ||
| 369 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 370 | void AABBCollider::_Collide(const AABBQuantizedNode* node) | ||
| 371 | { | ||
| 372 | // Dequantize box | ||
| 373 | const QuantizedAABB& Box = node->mAABB; | ||
| 374 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 375 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 376 | |||
| 377 | // Perform AABB-AABB overlap test | ||
| 378 | if(!AABBAABBOverlap(Extents, Center)) return; | ||
| 379 | |||
| 380 | TEST_BOX_IN_AABB(Center, Extents) | ||
| 381 | |||
| 382 | if(node->IsLeaf()) | ||
| 383 | { | ||
| 384 | AABB_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 385 | } | ||
| 386 | else | ||
| 387 | { | ||
| 388 | _Collide(node->GetPos()); | ||
| 389 | |||
| 390 | if(ContactFound()) return; | ||
| 391 | |||
| 392 | _Collide(node->GetNeg()); | ||
| 393 | } | ||
| 394 | } | ||
| 395 | |||
| 396 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 397 | /** | ||
| 398 | * Recursive collision query for quantized AABB trees, without primitive tests. | ||
| 399 | * \param node [in] current collision node | ||
| 400 | */ | ||
| 401 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 402 | void AABBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node) | ||
| 403 | { | ||
| 404 | // Dequantize box | ||
| 405 | const QuantizedAABB& Box = node->mAABB; | ||
| 406 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 407 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 408 | |||
| 409 | // Perform AABB-AABB overlap test | ||
| 410 | if(!AABBAABBOverlap(Extents, Center)) return; | ||
| 411 | |||
| 412 | TEST_BOX_IN_AABB(Center, Extents) | ||
| 413 | |||
| 414 | if(node->IsLeaf()) | ||
| 415 | { | ||
| 416 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 417 | } | ||
| 418 | else | ||
| 419 | { | ||
| 420 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 421 | |||
| 422 | if(ContactFound()) return; | ||
| 423 | |||
| 424 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 425 | } | ||
| 426 | } | ||
| 427 | |||
| 428 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 429 | /** | ||
| 430 | * Recursive collision query for no-leaf AABB trees. | ||
| 431 | * \param node [in] current collision node | ||
| 432 | */ | ||
| 433 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 434 | void AABBCollider::_Collide(const AABBNoLeafNode* node) | ||
| 435 | { | ||
| 436 | // Perform AABB-AABB overlap test | ||
| 437 | if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 438 | |||
| 439 | TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 440 | |||
| 441 | if(node->HasPosLeaf()) { AABB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 442 | else _Collide(node->GetPos()); | ||
| 443 | |||
| 444 | if(ContactFound()) return; | ||
| 445 | |||
| 446 | if(node->HasNegLeaf()) { AABB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 447 | else _Collide(node->GetNeg()); | ||
| 448 | } | ||
| 449 | |||
| 450 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 451 | /** | ||
| 452 | * Recursive collision query for no-leaf AABB trees, without primitive tests. | ||
| 453 | * \param node [in] current collision node | ||
| 454 | */ | ||
| 455 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 456 | void AABBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node) | ||
| 457 | { | ||
| 458 | // Perform AABB-AABB overlap test | ||
| 459 | if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 460 | |||
| 461 | TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 462 | |||
| 463 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 464 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 465 | |||
| 466 | if(ContactFound()) return; | ||
| 467 | |||
| 468 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 469 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 470 | } | ||
| 471 | |||
| 472 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 473 | /** | ||
| 474 | * Recursive collision query for quantized no-leaf AABB trees. | ||
| 475 | * \param node [in] current collision node | ||
| 476 | */ | ||
| 477 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 478 | void AABBCollider::_Collide(const AABBQuantizedNoLeafNode* node) | ||
| 479 | { | ||
| 480 | // Dequantize box | ||
| 481 | const QuantizedAABB& Box = node->mAABB; | ||
| 482 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 483 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 484 | |||
| 485 | // Perform AABB-AABB overlap test | ||
| 486 | if(!AABBAABBOverlap(Extents, Center)) return; | ||
| 487 | |||
| 488 | TEST_BOX_IN_AABB(Center, Extents) | ||
| 489 | |||
| 490 | if(node->HasPosLeaf()) { AABB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 491 | else _Collide(node->GetPos()); | ||
| 492 | |||
| 493 | if(ContactFound()) return; | ||
| 494 | |||
| 495 | if(node->HasNegLeaf()) { AABB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 496 | else _Collide(node->GetNeg()); | ||
| 497 | } | ||
| 498 | |||
| 499 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 500 | /** | ||
| 501 | * Recursive collision query for quantized no-leaf AABB trees, without primitive tests. | ||
| 502 | * \param node [in] current collision node | ||
| 503 | */ | ||
| 504 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 505 | void AABBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node) | ||
| 506 | { | ||
| 507 | // Dequantize box | ||
| 508 | const QuantizedAABB& Box = node->mAABB; | ||
| 509 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 510 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 511 | |||
| 512 | // Perform AABB-AABB overlap test | ||
| 513 | if(!AABBAABBOverlap(Extents, Center)) return; | ||
| 514 | |||
| 515 | TEST_BOX_IN_AABB(Center, Extents) | ||
| 516 | |||
| 517 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 518 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 519 | |||
| 520 | if(ContactFound()) return; | ||
| 521 | |||
| 522 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 523 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 524 | } | ||
| 525 | |||
| 526 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 527 | /** | ||
| 528 | * Recursive collision query for vanilla AABB trees. | ||
| 529 | * \param node [in] current collision node | ||
| 530 | */ | ||
| 531 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 532 | void AABBCollider::_Collide(const AABBTreeNode* node) | ||
| 533 | { | ||
| 534 | // Perform AABB-AABB overlap test | ||
| 535 | Point Center, Extents; | ||
| 536 | node->GetAABB()->GetCenter(Center); | ||
| 537 | node->GetAABB()->GetExtents(Extents); | ||
| 538 | if(!AABBAABBOverlap(Center, Extents)) return; | ||
| 539 | |||
| 540 | if(node->IsLeaf() || AABBContainsBox(Center, Extents)) | ||
| 541 | { | ||
| 542 | mFlags |= OPC_CONTACT; | ||
| 543 | mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives()); | ||
| 544 | } | ||
| 545 | else | ||
| 546 | { | ||
| 547 | _Collide(node->GetPos()); | ||
| 548 | _Collide(node->GetNeg()); | ||
| 549 | } | ||
| 550 | } | ||
| 551 | |||
| 552 | |||
| 553 | |||
| 554 | |||
| 555 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 556 | /** | ||
| 557 | * Constructor. | ||
| 558 | */ | ||
| 559 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 560 | HybridAABBCollider::HybridAABBCollider() | ||
| 561 | { | ||
| 562 | } | ||
| 563 | |||
| 564 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 565 | /** | ||
| 566 | * Destructor. | ||
| 567 | */ | ||
| 568 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 569 | HybridAABBCollider::~HybridAABBCollider() | ||
| 570 | { | ||
| 571 | } | ||
| 572 | |||
| 573 | bool HybridAABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const HybridModel& model) | ||
| 574 | { | ||
| 575 | // We don't want primitive tests here! | ||
| 576 | mFlags |= OPC_NO_PRIMITIVE_TESTS; | ||
| 577 | |||
| 578 | // Checkings | ||
| 579 | if(!Setup(&model)) return false; | ||
| 580 | |||
| 581 | // Init collision query | ||
| 582 | if(InitQuery(cache, box)) return true; | ||
| 583 | |||
| 584 | // Special case for 1-leaf trees | ||
| 585 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 586 | { | ||
| 587 | // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles | ||
| 588 | udword Nb = mIMesh->GetNbTriangles(); | ||
| 589 | |||
| 590 | // Loop through all triangles | ||
| 591 | for(udword i=0;i<Nb;i++) | ||
| 592 | { | ||
| 593 | AABB_PRIM(i, OPC_CONTACT) | ||
| 594 | } | ||
| 595 | return true; | ||
| 596 | } | ||
| 597 | |||
| 598 | // Override destination array since we're only going to get leaf boxes here | ||
| 599 | mTouchedBoxes.Reset(); | ||
| 600 | mTouchedPrimitives = &mTouchedBoxes; | ||
| 601 | |||
| 602 | // Now, do the actual query against leaf boxes | ||
| 603 | if(!model.HasLeafNodes()) | ||
| 604 | { | ||
| 605 | if(model.IsQuantized()) | ||
| 606 | { | ||
| 607 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 608 | |||
| 609 | // Setup dequantization coeffs | ||
| 610 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 611 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 612 | |||
| 613 | // Perform collision query - we don't want primitive tests here! | ||
| 614 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 615 | } | ||
| 616 | else | ||
| 617 | { | ||
| 618 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 619 | |||
| 620 | // Perform collision query - we don't want primitive tests here! | ||
| 621 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 622 | } | ||
| 623 | } | ||
| 624 | else | ||
| 625 | { | ||
| 626 | if(model.IsQuantized()) | ||
| 627 | { | ||
| 628 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 629 | |||
| 630 | // Setup dequantization coeffs | ||
| 631 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 632 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 633 | |||
| 634 | // Perform collision query - we don't want primitive tests here! | ||
| 635 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 636 | } | ||
| 637 | else | ||
| 638 | { | ||
| 639 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 640 | |||
| 641 | // Perform collision query - we don't want primitive tests here! | ||
| 642 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 643 | } | ||
| 644 | } | ||
| 645 | |||
| 646 | // We only have a list of boxes so far | ||
| 647 | if(GetContactStatus()) | ||
| 648 | { | ||
| 649 | // Reset contact status, since it currently only reflects collisions with leaf boxes | ||
| 650 | Collider::InitQuery(); | ||
| 651 | |||
| 652 | // Change dest container so that we can use built-in overlap tests and get collided primitives | ||
| 653 | cache.TouchedPrimitives.Reset(); | ||
| 654 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 655 | |||
| 656 | // Read touched leaf boxes | ||
| 657 | udword Nb = mTouchedBoxes.GetNbEntries(); | ||
| 658 | const udword* Touched = mTouchedBoxes.GetEntries(); | ||
| 659 | |||
| 660 | const LeafTriangles* LT = model.GetLeafTriangles(); | ||
| 661 | const udword* Indices = model.GetIndices(); | ||
| 662 | |||
| 663 | // Loop through touched leaves | ||
| 664 | while(Nb--) | ||
| 665 | { | ||
| 666 | const LeafTriangles& CurrentLeaf = LT[*Touched++]; | ||
| 667 | |||
| 668 | // Each leaf box has a set of triangles | ||
| 669 | udword NbTris = CurrentLeaf.GetNbTriangles(); | ||
| 670 | if(Indices) | ||
| 671 | { | ||
| 672 | const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; | ||
| 673 | |||
| 674 | // Loop through triangles and test each of them | ||
| 675 | while(NbTris--) | ||
| 676 | { | ||
| 677 | udword TriangleIndex = *T++; | ||
| 678 | AABB_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 679 | } | ||
| 680 | } | ||
| 681 | else | ||
| 682 | { | ||
| 683 | udword BaseIndex = CurrentLeaf.GetTriangleIndex(); | ||
| 684 | |||
| 685 | // Loop through triangles and test each of them | ||
| 686 | while(NbTris--) | ||
| 687 | { | ||
| 688 | udword TriangleIndex = BaseIndex++; | ||
| 689 | AABB_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 690 | } | ||
| 691 | } | ||
| 692 | } | ||
| 693 | } | ||
| 694 | |||
| 695 | return true; | ||
| 696 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_AABBCollider.h b/libraries/ode-0.9/OPCODE/OPC_AABBCollider.h new file mode 100644 index 0000000..315d2d3 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_AABBCollider.h | |||
| @@ -0,0 +1,97 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for an AABB collider. | ||
| 12 | * \file OPC_AABBCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 1st, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_AABBCOLLIDER_H__ | ||
| 21 | #define __OPC_AABBCOLLIDER_H__ | ||
| 22 | |||
| 23 | struct OPCODE_API AABBCache : VolumeCache | ||
| 24 | { | ||
| 25 | AABBCache() : FatCoeff(1.1f) | ||
| 26 | { | ||
| 27 | FatBox.mCenter.Zero(); | ||
| 28 | FatBox.mExtents.Zero(); | ||
| 29 | } | ||
| 30 | |||
| 31 | // Cached faces signature | ||
| 32 | CollisionAABB FatBox; //!< Box used when performing the query resulting in cached faces | ||
| 33 | // User settings | ||
| 34 | float FatCoeff; //!< mRadius2 multiplier used to create a fat sphere | ||
| 35 | }; | ||
| 36 | |||
| 37 | class OPCODE_API AABBCollider : public VolumeCollider | ||
| 38 | { | ||
| 39 | public: | ||
| 40 | // Constructor / Destructor | ||
| 41 | AABBCollider(); | ||
| 42 | virtual ~AABBCollider(); | ||
| 43 | |||
| 44 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 45 | /** | ||
| 46 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 47 | * - with GetContactStatus() | ||
| 48 | * - with GetNbTouchedPrimitives() | ||
| 49 | * - with GetTouchedPrimitives() | ||
| 50 | * | ||
| 51 | * \param cache [in/out] a box cache | ||
| 52 | * \param box [in] collision AABB in world space | ||
| 53 | * \param model [in] Opcode model to collide with | ||
| 54 | * \return true if success | ||
| 55 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 56 | */ | ||
| 57 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 58 | bool Collide(AABBCache& cache, const CollisionAABB& box, const Model& model); | ||
| 59 | // | ||
| 60 | bool Collide(AABBCache& cache, const CollisionAABB& box, const AABBTree* tree); | ||
| 61 | protected: | ||
| 62 | CollisionAABB mBox; //!< Query box in (center, extents) form | ||
| 63 | Point mMin; //!< Query box min point | ||
| 64 | Point mMax; //!< Query box max point | ||
| 65 | // Leaf description | ||
| 66 | Point mLeafVerts[3]; //!< Triangle vertices | ||
| 67 | // Internal methods | ||
| 68 | void _Collide(const AABBCollisionNode* node); | ||
| 69 | void _Collide(const AABBNoLeafNode* node); | ||
| 70 | void _Collide(const AABBQuantizedNode* node); | ||
| 71 | void _Collide(const AABBQuantizedNoLeafNode* node); | ||
| 72 | void _Collide(const AABBTreeNode* node); | ||
| 73 | void _CollideNoPrimitiveTest(const AABBCollisionNode* node); | ||
| 74 | void _CollideNoPrimitiveTest(const AABBNoLeafNode* node); | ||
| 75 | void _CollideNoPrimitiveTest(const AABBQuantizedNode* node); | ||
| 76 | void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node); | ||
| 77 | // Overlap tests | ||
| 78 | inline_ BOOL AABBContainsBox(const Point& bc, const Point& be); | ||
| 79 | inline_ BOOL AABBAABBOverlap(const Point& b, const Point& Pb); | ||
| 80 | inline_ BOOL TriBoxOverlap(); | ||
| 81 | // Init methods | ||
| 82 | BOOL InitQuery(AABBCache& cache, const CollisionAABB& box); | ||
| 83 | }; | ||
| 84 | |||
| 85 | class OPCODE_API HybridAABBCollider : public AABBCollider | ||
| 86 | { | ||
| 87 | public: | ||
| 88 | // Constructor / Destructor | ||
| 89 | HybridAABBCollider(); | ||
| 90 | virtual ~HybridAABBCollider(); | ||
| 91 | |||
| 92 | bool Collide(AABBCache& cache, const CollisionAABB& box, const HybridModel& model); | ||
| 93 | protected: | ||
| 94 | Container mTouchedBoxes; | ||
| 95 | }; | ||
| 96 | |||
| 97 | #endif // __OPC_AABBCOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_AABBTree.cpp b/libraries/ode-0.9/OPCODE/OPC_AABBTree.cpp new file mode 100644 index 0000000..32214f4 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_AABBTree.cpp | |||
| @@ -0,0 +1,573 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a versatile AABB tree. | ||
| 12 | * \file OPC_AABBTree.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains a generic AABB tree node. | ||
| 21 | * | ||
| 22 | * \class AABBTreeNode | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date March, 20, 2001 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | /** | ||
| 31 | * Contains a generic AABB tree. | ||
| 32 | * This is a vanilla AABB tree, without any particular optimization. It contains anonymous references to | ||
| 33 | * user-provided primitives, which can theoretically be anything - triangles, boxes, etc. Each primitive | ||
| 34 | * is surrounded by an AABB, regardless of the primitive's nature. When the primitive is a triangle, the | ||
| 35 | * resulting tree can be converted into an optimized tree. If the primitive is a box, the resulting tree | ||
| 36 | * can be used for culling - VFC or occlusion -, assuming you cull on a mesh-by-mesh basis (modern way). | ||
| 37 | * | ||
| 38 | * \class AABBTree | ||
| 39 | * \author Pierre Terdiman | ||
| 40 | * \version 1.3 | ||
| 41 | * \date March, 20, 2001 | ||
| 42 | */ | ||
| 43 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 44 | |||
| 45 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 46 | // Precompiled Header | ||
| 47 | #include "Stdafx.h" | ||
| 48 | |||
| 49 | using namespace Opcode; | ||
| 50 | |||
| 51 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 52 | /** | ||
| 53 | * Constructor. | ||
| 54 | */ | ||
| 55 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 56 | AABBTreeNode::AABBTreeNode() : | ||
| 57 | mPos (null), | ||
| 58 | #ifndef OPC_NO_NEG_VANILLA_TREE | ||
| 59 | mNeg (null), | ||
| 60 | #endif | ||
| 61 | mNbPrimitives (0), | ||
| 62 | mNodePrimitives (null) | ||
| 63 | { | ||
| 64 | #ifdef OPC_USE_TREE_COHERENCE | ||
| 65 | mBitmask = 0; | ||
| 66 | #endif | ||
| 67 | } | ||
| 68 | |||
| 69 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 70 | /** | ||
| 71 | * Destructor. | ||
| 72 | */ | ||
| 73 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 74 | AABBTreeNode::~AABBTreeNode() | ||
| 75 | { | ||
| 76 | // Opcode 1.3: | ||
| 77 | const AABBTreeNode* Pos = GetPos(); | ||
| 78 | const AABBTreeNode* Neg = GetNeg(); | ||
| 79 | #ifndef OPC_NO_NEG_VANILLA_TREE | ||
| 80 | if(!(mPos&1)) DELETESINGLE(Pos); | ||
| 81 | if(!(mNeg&1)) DELETESINGLE(Neg); | ||
| 82 | #else | ||
| 83 | if(!(mPos&1)) DELETEARRAY(Pos); | ||
| 84 | #endif | ||
| 85 | mNodePrimitives = null; // This was just a shortcut to the global list => no release | ||
| 86 | mNbPrimitives = 0; | ||
| 87 | } | ||
| 88 | |||
| 89 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 90 | /** | ||
| 91 | * Splits the node along a given axis. | ||
| 92 | * The list of indices is reorganized according to the split values. | ||
| 93 | * \param axis [in] splitting axis index | ||
| 94 | * \param builder [in] the tree builder | ||
| 95 | * \return the number of primitives assigned to the first child | ||
| 96 | * \warning this method reorganizes the internal list of primitives | ||
| 97 | */ | ||
| 98 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 99 | udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder) | ||
| 100 | { | ||
| 101 | // Get node split value | ||
| 102 | float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis); | ||
| 103 | |||
| 104 | udword NbPos = 0; | ||
| 105 | // Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1]. | ||
| 106 | // Those indices map the global list in the tree builder. | ||
| 107 | for(udword i=0;i<mNbPrimitives;i++) | ||
| 108 | { | ||
| 109 | // Get index in global list | ||
| 110 | udword Index = mNodePrimitives[i]; | ||
| 111 | |||
| 112 | // Test against the splitting value. The primitive value is tested against the enclosing-box center. | ||
| 113 | // [We only need an approximate partition of the enclosing box here.] | ||
| 114 | float PrimitiveValue = builder->GetSplittingValue(Index, axis); | ||
| 115 | |||
| 116 | // Reorganize the list of indices in this order: positive - negative. | ||
| 117 | if(PrimitiveValue > SplitValue) | ||
| 118 | { | ||
| 119 | // Swap entries | ||
| 120 | udword Tmp = mNodePrimitives[i]; | ||
| 121 | mNodePrimitives[i] = mNodePrimitives[NbPos]; | ||
| 122 | mNodePrimitives[NbPos] = Tmp; | ||
| 123 | // Count primitives assigned to positive space | ||
| 124 | NbPos++; | ||
| 125 | } | ||
| 126 | } | ||
| 127 | return NbPos; | ||
| 128 | } | ||
| 129 | |||
| 130 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 131 | /** | ||
| 132 | * Subdivides the node. | ||
| 133 | * | ||
| 134 | * N | ||
| 135 | * / \ | ||
| 136 | * / \ | ||
| 137 | * N/2 N/2 | ||
| 138 | * / \ / \ | ||
| 139 | * N/4 N/4 N/4 N/4 | ||
| 140 | * (etc) | ||
| 141 | * | ||
| 142 | * A well-balanced tree should have a O(log n) depth. | ||
| 143 | * A degenerate tree would have a O(n) depth. | ||
| 144 | * Note a perfectly-balanced tree is not well-suited to collision detection anyway. | ||
| 145 | * | ||
| 146 | * \param builder [in] the tree builder | ||
| 147 | * \return true if success | ||
| 148 | */ | ||
| 149 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 150 | bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder) | ||
| 151 | { | ||
| 152 | // Checkings | ||
| 153 | if(!builder) return false; | ||
| 154 | |||
| 155 | // Stop subdividing if we reach a leaf node. This is always performed here, | ||
| 156 | // else we could end in trouble if user overrides this. | ||
| 157 | if(mNbPrimitives==1) return true; | ||
| 158 | |||
| 159 | // Let the user validate the subdivision | ||
| 160 | if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV)) return true; | ||
| 161 | |||
| 162 | bool ValidSplit = true; // Optimism... | ||
| 163 | udword NbPos; | ||
| 164 | if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS) | ||
| 165 | { | ||
| 166 | // Find the largest axis to split along | ||
| 167 | Point Extents; mBV.GetExtents(Extents); // Box extents | ||
| 168 | udword Axis = Extents.LargestAxis(); // Index of largest axis | ||
| 169 | |||
| 170 | // Split along the axis | ||
| 171 | NbPos = Split(Axis, builder); | ||
| 172 | |||
| 173 | // Check split validity | ||
| 174 | if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false; | ||
| 175 | } | ||
| 176 | else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS) | ||
| 177 | { | ||
| 178 | // Compute the means | ||
| 179 | Point Means(0.0f, 0.0f, 0.0f); | ||
| 180 | for(udword i=0;i<mNbPrimitives;i++) | ||
| 181 | { | ||
| 182 | udword Index = mNodePrimitives[i]; | ||
| 183 | Means.x+=builder->GetSplittingValue(Index, 0); | ||
| 184 | Means.y+=builder->GetSplittingValue(Index, 1); | ||
| 185 | Means.z+=builder->GetSplittingValue(Index, 2); | ||
| 186 | } | ||
| 187 | Means/=float(mNbPrimitives); | ||
| 188 | |||
| 189 | // Compute variances | ||
| 190 | Point Vars(0.0f, 0.0f, 0.0f); | ||
| 191 | for(udword i=0;i<mNbPrimitives;i++) | ||
| 192 | { | ||
| 193 | udword Index = mNodePrimitives[i]; | ||
| 194 | float Cx = builder->GetSplittingValue(Index, 0); | ||
| 195 | float Cy = builder->GetSplittingValue(Index, 1); | ||
| 196 | float Cz = builder->GetSplittingValue(Index, 2); | ||
| 197 | Vars.x += (Cx - Means.x)*(Cx - Means.x); | ||
| 198 | Vars.y += (Cy - Means.y)*(Cy - Means.y); | ||
| 199 | Vars.z += (Cz - Means.z)*(Cz - Means.z); | ||
| 200 | } | ||
| 201 | Vars/=float(mNbPrimitives-1); | ||
| 202 | |||
| 203 | // Choose axis with greatest variance | ||
| 204 | udword Axis = Vars.LargestAxis(); | ||
| 205 | |||
| 206 | // Split along the axis | ||
| 207 | NbPos = Split(Axis, builder); | ||
| 208 | |||
| 209 | // Check split validity | ||
| 210 | if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false; | ||
| 211 | } | ||
| 212 | else if(builder->mSettings.mRules & SPLIT_BALANCED) | ||
| 213 | { | ||
| 214 | // Test 3 axis, take the best | ||
| 215 | float Results[3]; | ||
| 216 | NbPos = Split(0, builder); Results[0] = float(NbPos)/float(mNbPrimitives); | ||
| 217 | NbPos = Split(1, builder); Results[1] = float(NbPos)/float(mNbPrimitives); | ||
| 218 | NbPos = Split(2, builder); Results[2] = float(NbPos)/float(mNbPrimitives); | ||
| 219 | Results[0]-=0.5f; Results[0]*=Results[0]; | ||
| 220 | Results[1]-=0.5f; Results[1]*=Results[1]; | ||
| 221 | Results[2]-=0.5f; Results[2]*=Results[2]; | ||
| 222 | udword Min=0; | ||
| 223 | if(Results[1]<Results[Min]) Min = 1; | ||
| 224 | if(Results[2]<Results[Min]) Min = 2; | ||
| 225 | |||
| 226 | // Split along the axis | ||
| 227 | NbPos = Split(Min, builder); | ||
| 228 | |||
| 229 | // Check split validity | ||
| 230 | if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false; | ||
| 231 | } | ||
| 232 | else if(builder->mSettings.mRules & SPLIT_BEST_AXIS) | ||
| 233 | { | ||
| 234 | // Test largest, then middle, then smallest axis... | ||
| 235 | |||
| 236 | // Sort axis | ||
| 237 | Point Extents; mBV.GetExtents(Extents); // Box extents | ||
| 238 | udword SortedAxis[] = { 0, 1, 2 }; | ||
| 239 | float* Keys = (float*)&Extents.x; | ||
| 240 | for(udword j=0;j<3;j++) | ||
| 241 | { | ||
| 242 | for(udword i=0;i<2;i++) | ||
| 243 | { | ||
| 244 | if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]]) | ||
| 245 | { | ||
| 246 | udword Tmp = SortedAxis[i]; | ||
| 247 | SortedAxis[i] = SortedAxis[i+1]; | ||
| 248 | SortedAxis[i+1] = Tmp; | ||
| 249 | } | ||
| 250 | } | ||
| 251 | } | ||
| 252 | |||
| 253 | // Find the largest axis to split along | ||
| 254 | udword CurAxis = 0; | ||
| 255 | ValidSplit = false; | ||
| 256 | while(!ValidSplit && CurAxis!=3) | ||
| 257 | { | ||
| 258 | NbPos = Split(SortedAxis[CurAxis], builder); | ||
| 259 | // Check the subdivision has been successful | ||
| 260 | if(!NbPos || NbPos==mNbPrimitives) CurAxis++; | ||
| 261 | else ValidSplit = true; | ||
| 262 | } | ||
| 263 | } | ||
| 264 | else if(builder->mSettings.mRules & SPLIT_FIFTY) | ||
| 265 | { | ||
| 266 | // Don't even bother splitting (mainly a performance test) | ||
| 267 | NbPos = mNbPrimitives>>1; | ||
| 268 | } | ||
| 269 | else return false; // Unknown splitting rules | ||
| 270 | |||
| 271 | // Check the subdivision has been successful | ||
| 272 | if(!ValidSplit) | ||
| 273 | { | ||
| 274 | // Here, all boxes lie in the same sub-space. Two strategies: | ||
| 275 | // - if the tree *must* be complete, make an arbitrary 50-50 split | ||
| 276 | // - else stop subdividing | ||
| 277 | // if(builder->mSettings.mRules&SPLIT_COMPLETE) | ||
| 278 | if(builder->mSettings.mLimit==1) | ||
| 279 | { | ||
| 280 | builder->IncreaseNbInvalidSplits(); | ||
| 281 | NbPos = mNbPrimitives>>1; | ||
| 282 | } | ||
| 283 | else return true; | ||
| 284 | } | ||
| 285 | |||
| 286 | // Now create children and assign their pointers. | ||
| 287 | if(builder->mNodeBase) | ||
| 288 | { | ||
| 289 | // We use a pre-allocated linear pool for complete trees [Opcode 1.3] | ||
| 290 | AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase; | ||
| 291 | udword Count = builder->GetCount() - 1; // Count begins to 1... | ||
| 292 | // Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives | ||
| 293 | ASSERT(!(udword(&Pool[Count+0])&1)); | ||
| 294 | ASSERT(!(udword(&Pool[Count+1])&1)); | ||
| 295 | mPos = size_t(&Pool[Count+0])|1; | ||
| 296 | #ifndef OPC_NO_NEG_VANILLA_TREE | ||
| 297 | mNeg = size_t(&Pool[Count+1])|1; | ||
| 298 | #endif | ||
| 299 | } | ||
| 300 | else | ||
| 301 | { | ||
| 302 | // Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly | ||
| 303 | #ifndef OPC_NO_NEG_VANILLA_TREE | ||
| 304 | mPos = (size_t)new AABBTreeNode; CHECKALLOC(mPos); | ||
| 305 | mNeg = (size_t)new AABBTreeNode; CHECKALLOC(mNeg); | ||
| 306 | #else | ||
| 307 | AABBTreeNode* PosNeg = new AABBTreeNode[2]; | ||
| 308 | CHECKALLOC(PosNeg); | ||
| 309 | mPos = (size_t)PosNeg; | ||
| 310 | #endif | ||
| 311 | } | ||
| 312 | |||
| 313 | // Update stats | ||
| 314 | builder->IncreaseCount(2); | ||
| 315 | |||
| 316 | // Assign children | ||
| 317 | AABBTreeNode* Pos = (AABBTreeNode*)GetPos(); | ||
| 318 | AABBTreeNode* Neg = (AABBTreeNode*)GetNeg(); | ||
| 319 | Pos->mNodePrimitives = &mNodePrimitives[0]; | ||
| 320 | Pos->mNbPrimitives = NbPos; | ||
| 321 | Neg->mNodePrimitives = &mNodePrimitives[NbPos]; | ||
| 322 | Neg->mNbPrimitives = mNbPrimitives - NbPos; | ||
| 323 | |||
| 324 | return true; | ||
| 325 | } | ||
| 326 | |||
| 327 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 328 | /** | ||
| 329 | * Recursive hierarchy building in a top-down fashion. | ||
| 330 | * \param builder [in] the tree builder | ||
| 331 | */ | ||
| 332 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 333 | void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder) | ||
| 334 | { | ||
| 335 | // 1) Compute the global box for current node. The box is stored in mBV. | ||
| 336 | builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV); | ||
| 337 | |||
| 338 | // 2) Subdivide current node | ||
| 339 | Subdivide(builder); | ||
| 340 | |||
| 341 | // 3) Recurse | ||
| 342 | AABBTreeNode* Pos = (AABBTreeNode*)GetPos(); | ||
| 343 | AABBTreeNode* Neg = (AABBTreeNode*)GetNeg(); | ||
| 344 | if(Pos) Pos->_BuildHierarchy(builder); | ||
| 345 | if(Neg) Neg->_BuildHierarchy(builder); | ||
| 346 | } | ||
| 347 | |||
| 348 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 349 | /** | ||
| 350 | * Refits the tree (top-down). | ||
| 351 | * \param builder [in] the tree builder | ||
| 352 | */ | ||
| 353 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 354 | void AABBTreeNode::_Refit(AABBTreeBuilder* builder) | ||
| 355 | { | ||
| 356 | // 1) Recompute the new global box for current node | ||
| 357 | builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV); | ||
| 358 | |||
| 359 | // 2) Recurse | ||
| 360 | AABBTreeNode* Pos = (AABBTreeNode*)GetPos(); | ||
| 361 | AABBTreeNode* Neg = (AABBTreeNode*)GetNeg(); | ||
| 362 | if(Pos) Pos->_Refit(builder); | ||
| 363 | if(Neg) Neg->_Refit(builder); | ||
| 364 | } | ||
| 365 | |||
| 366 | |||
| 367 | |||
| 368 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 369 | /** | ||
| 370 | * Constructor. | ||
| 371 | */ | ||
| 372 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 373 | AABBTree::AABBTree() : mIndices(null), mTotalNbNodes(0), mPool(null) | ||
| 374 | { | ||
| 375 | } | ||
| 376 | |||
| 377 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 378 | /** | ||
| 379 | * Destructor. | ||
| 380 | */ | ||
| 381 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 382 | AABBTree::~AABBTree() | ||
| 383 | { | ||
| 384 | Release(); | ||
| 385 | } | ||
| 386 | |||
| 387 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 388 | /** | ||
| 389 | * Releases the tree. | ||
| 390 | */ | ||
| 391 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 392 | void AABBTree::Release() | ||
| 393 | { | ||
| 394 | DELETEARRAY(mPool); | ||
| 395 | DELETEARRAY(mIndices); | ||
| 396 | } | ||
| 397 | |||
| 398 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 399 | /** | ||
| 400 | * Builds a generic AABB tree from a tree builder. | ||
| 401 | * \param builder [in] the tree builder | ||
| 402 | * \return true if success | ||
| 403 | */ | ||
| 404 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 405 | bool AABBTree::Build(AABBTreeBuilder* builder) | ||
| 406 | { | ||
| 407 | // Checkings | ||
| 408 | if(!builder || !builder->mNbPrimitives) return false; | ||
| 409 | |||
| 410 | // Release previous tree | ||
| 411 | Release(); | ||
| 412 | |||
| 413 | // Init stats | ||
| 414 | builder->SetCount(1); | ||
| 415 | builder->SetNbInvalidSplits(0); | ||
| 416 | |||
| 417 | // Initialize indices. This list will be modified during build. | ||
| 418 | mIndices = new udword[builder->mNbPrimitives]; | ||
| 419 | CHECKALLOC(mIndices); | ||
| 420 | // Identity permutation | ||
| 421 | for(udword i=0;i<builder->mNbPrimitives;i++) mIndices[i] = i; | ||
| 422 | |||
| 423 | // Setup initial node. Here we have a complete permutation of the app's primitives. | ||
| 424 | mNodePrimitives = mIndices; | ||
| 425 | mNbPrimitives = builder->mNbPrimitives; | ||
| 426 | |||
| 427 | // Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3] | ||
| 428 | // if(builder->mRules&SPLIT_COMPLETE) | ||
| 429 | if(builder->mSettings.mLimit==1) | ||
| 430 | { | ||
| 431 | // Allocate a pool of nodes | ||
| 432 | mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1]; | ||
| 433 | |||
| 434 | builder->mNodeBase = mPool; // ### ugly ! | ||
| 435 | } | ||
| 436 | |||
| 437 | // Build the hierarchy | ||
| 438 | _BuildHierarchy(builder); | ||
| 439 | |||
| 440 | // Get back total number of nodes | ||
| 441 | mTotalNbNodes = builder->GetCount(); | ||
| 442 | |||
| 443 | // For complete trees, check the correct number of nodes has been created [Opcode 1.3] | ||
| 444 | if(mPool) ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1); | ||
| 445 | |||
| 446 | return true; | ||
| 447 | } | ||
| 448 | |||
| 449 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 450 | /** | ||
| 451 | * Computes the depth of the tree. | ||
| 452 | * A well-balanced tree should have a log(n) depth. A degenerate tree O(n) depth. | ||
| 453 | * \return depth of the tree | ||
| 454 | */ | ||
| 455 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 456 | udword AABBTree::ComputeDepth() const | ||
| 457 | { | ||
| 458 | return Walk(null, null); // Use the walking code without callback | ||
| 459 | } | ||
| 460 | |||
| 461 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 462 | /** | ||
| 463 | * Walks the tree, calling the user back for each node. | ||
| 464 | */ | ||
| 465 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 466 | udword AABBTree::Walk(WalkingCallback callback, void* user_data) const | ||
| 467 | { | ||
| 468 | // Call it without callback to compute max depth | ||
| 469 | udword MaxDepth = 0; | ||
| 470 | udword CurrentDepth = 0; | ||
| 471 | |||
| 472 | struct Local | ||
| 473 | { | ||
| 474 | static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data) | ||
| 475 | { | ||
| 476 | // Checkings | ||
| 477 | if(!current_node) return; | ||
| 478 | // Entering a new node => increase depth | ||
| 479 | current_depth++; | ||
| 480 | // Keep track of max depth | ||
| 481 | if(current_depth>max_depth) max_depth = current_depth; | ||
| 482 | |||
| 483 | // Callback | ||
| 484 | if(callback && !(callback)(current_node, current_depth, user_data)) return; | ||
| 485 | |||
| 486 | // Recurse | ||
| 487 | if(current_node->GetPos()) { _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data); current_depth--; } | ||
| 488 | if(current_node->GetNeg()) { _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data); current_depth--; } | ||
| 489 | } | ||
| 490 | }; | ||
| 491 | Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data); | ||
| 492 | return MaxDepth; | ||
| 493 | } | ||
| 494 | |||
| 495 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 496 | /** | ||
| 497 | * Refits the tree in a top-down way. | ||
| 498 | * \param builder [in] the tree builder | ||
| 499 | */ | ||
| 500 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 501 | bool AABBTree::Refit(AABBTreeBuilder* builder) | ||
| 502 | { | ||
| 503 | if(!builder) return false; | ||
| 504 | _Refit(builder); | ||
| 505 | return true; | ||
| 506 | } | ||
| 507 | |||
| 508 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 509 | /** | ||
| 510 | * Refits the tree in a bottom-up way. | ||
| 511 | * \param builder [in] the tree builder | ||
| 512 | */ | ||
| 513 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 514 | bool AABBTree::Refit2(AABBTreeBuilder* builder) | ||
| 515 | { | ||
| 516 | // Checkings | ||
| 517 | if(!builder) return false; | ||
| 518 | |||
| 519 | ASSERT(mPool); | ||
| 520 | |||
| 521 | // Bottom-up update | ||
| 522 | Point Min,Max; | ||
| 523 | Point Min_,Max_; | ||
| 524 | udword Index = mTotalNbNodes; | ||
| 525 | while(Index--) | ||
| 526 | { | ||
| 527 | AABBTreeNode& Current = mPool[Index]; | ||
| 528 | |||
| 529 | if(Current.IsLeaf()) | ||
| 530 | { | ||
| 531 | builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *(AABB*)Current.GetAABB()); | ||
| 532 | } | ||
| 533 | else | ||
| 534 | { | ||
| 535 | Current.GetPos()->GetAABB()->GetMin(Min); | ||
| 536 | Current.GetPos()->GetAABB()->GetMax(Max); | ||
| 537 | |||
| 538 | Current.GetNeg()->GetAABB()->GetMin(Min_); | ||
| 539 | Current.GetNeg()->GetAABB()->GetMax(Max_); | ||
| 540 | |||
| 541 | Min.Min(Min_); | ||
| 542 | Max.Max(Max_); | ||
| 543 | |||
| 544 | ((AABB*)Current.GetAABB())->SetMinMax(Min, Max); | ||
| 545 | } | ||
| 546 | } | ||
| 547 | return true; | ||
| 548 | } | ||
| 549 | |||
| 550 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 551 | /** | ||
| 552 | * Computes the number of bytes used by the tree. | ||
| 553 | * \return number of bytes used | ||
| 554 | */ | ||
| 555 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 556 | udword AABBTree::GetUsedBytes() const | ||
| 557 | { | ||
| 558 | udword TotalSize = mTotalNbNodes*GetNodeSize(); | ||
| 559 | if(mIndices) TotalSize+=mNbPrimitives*sizeof(udword); | ||
| 560 | return TotalSize; | ||
| 561 | } | ||
| 562 | |||
| 563 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 564 | /** | ||
| 565 | * Checks the tree is a complete tree or not. | ||
| 566 | * A complete tree is made of 2*N-1 nodes, where N is the number of primitives in the tree. | ||
| 567 | * \return true for complete trees | ||
| 568 | */ | ||
| 569 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 570 | bool AABBTree::IsComplete() const | ||
| 571 | { | ||
| 572 | return (GetNbNodes()==GetNbPrimitives()*2-1); | ||
| 573 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_AABBTree.h b/libraries/ode-0.9/OPCODE/OPC_AABBTree.h new file mode 100644 index 0000000..ee2533d --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_AABBTree.h | |||
| @@ -0,0 +1,137 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a versatile AABB tree. | ||
| 12 | * \file OPC_AABBTree.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_AABBTREE_H__ | ||
| 21 | #define __OPC_AABBTREE_H__ | ||
| 22 | |||
| 23 | #ifdef OPC_NO_NEG_VANILLA_TREE | ||
| 24 | //! TO BE DOCUMENTED | ||
| 25 | #define IMPLEMENT_TREE(base_class, volume) \ | ||
| 26 | public: \ | ||
| 27 | /* Constructor / Destructor */ \ | ||
| 28 | base_class(); \ | ||
| 29 | ~base_class(); \ | ||
| 30 | /* Data access */ \ | ||
| 31 | inline_ const volume* Get##volume() const { return &mBV; } \ | ||
| 32 | /* Clear the last bit */ \ | ||
| 33 | inline_ const base_class* GetPos() const { return (const base_class*)(mPos&~1); } \ | ||
| 34 | inline_ const base_class* GetNeg() const { const base_class* P = GetPos(); return P ? P+1 : null;} \ | ||
| 35 | \ | ||
| 36 | /* We don't need to test both nodes since we can't have one without the other */ \ | ||
| 37 | inline_ bool IsLeaf() const { return !GetPos(); } \ | ||
| 38 | \ | ||
| 39 | /* Stats */ \ | ||
| 40 | inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ | ||
| 41 | protected: \ | ||
| 42 | /* Tree-independent data */ \ | ||
| 43 | /* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/ \ | ||
| 44 | /* Whatever happens we need the two children and the enclosing volume.*/ \ | ||
| 45 | volume mBV; /* Global bounding-volume enclosing all the node-related primitives */ \ | ||
| 46 | size_t mPos; /* "Positive" & "Negative" children */ | ||
| 47 | #else | ||
| 48 | //! TO BE DOCUMENTED | ||
| 49 | #define IMPLEMENT_TREE(base_class, volume) \ | ||
| 50 | public: \ | ||
| 51 | /* Constructor / Destructor */ \ | ||
| 52 | base_class(); \ | ||
| 53 | ~base_class(); \ | ||
| 54 | /* Data access */ \ | ||
| 55 | inline_ const volume* Get##volume() const { return &mBV; } \ | ||
| 56 | /* Clear the last bit */ \ | ||
| 57 | inline_ const base_class* GetPos() const { return (const base_class*)(mPos&~1); } \ | ||
| 58 | inline_ const base_class* GetNeg() const { return (const base_class*)(mNeg&~1); } \ | ||
| 59 | \ | ||
| 60 | /* inline_ bool IsLeaf() const { return (!GetPos() && !GetNeg()); } */ \ | ||
| 61 | /* We don't need to test both nodes since we can't have one without the other */ \ | ||
| 62 | inline_ bool IsLeaf() const { return !GetPos(); } \ | ||
| 63 | \ | ||
| 64 | /* Stats */ \ | ||
| 65 | inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ | ||
| 66 | protected: \ | ||
| 67 | /* Tree-independent data */ \ | ||
| 68 | /* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/ \ | ||
| 69 | /* Whatever happens we need the two children and the enclosing volume.*/ \ | ||
| 70 | volume mBV; /* Global bounding-volume enclosing all the node-related primitives */ \ | ||
| 71 | size_t mPos; /* "Positive" child */ \ | ||
| 72 | size_t mNeg; /* "Negative" child */ | ||
| 73 | #endif | ||
| 74 | |||
| 75 | typedef void (*CullingCallback) (udword nb_primitives, udword* node_primitives, BOOL need_clipping, void* user_data); | ||
| 76 | |||
| 77 | class OPCODE_API AABBTreeNode | ||
| 78 | { | ||
| 79 | IMPLEMENT_TREE(AABBTreeNode, AABB) | ||
| 80 | public: | ||
| 81 | // Data access | ||
| 82 | inline_ const udword* GetPrimitives() const { return mNodePrimitives; } | ||
| 83 | inline_ udword GetNbPrimitives() const { return mNbPrimitives; } | ||
| 84 | |||
| 85 | protected: | ||
| 86 | // Tree-dependent data | ||
| 87 | udword* mNodePrimitives; //!< Node-related primitives (shortcut to a position in mIndices below) | ||
| 88 | udword mNbPrimitives; //!< Number of primitives for this node | ||
| 89 | // Internal methods | ||
| 90 | udword Split(udword axis, AABBTreeBuilder* builder); | ||
| 91 | bool Subdivide(AABBTreeBuilder* builder); | ||
| 92 | void _BuildHierarchy(AABBTreeBuilder* builder); | ||
| 93 | void _Refit(AABBTreeBuilder* builder); | ||
| 94 | }; | ||
| 95 | |||
| 96 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 97 | /** | ||
| 98 | * User-callback, called for each node by the walking code. | ||
| 99 | * \param current [in] current node | ||
| 100 | * \param depth [in] current node's depth | ||
| 101 | * \param user_data [in] user-defined data | ||
| 102 | * \return true to recurse through children, else false to bypass them | ||
| 103 | */ | ||
| 104 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 105 | typedef bool (*WalkingCallback) (const AABBTreeNode* current, udword depth, void* user_data); | ||
| 106 | |||
| 107 | class OPCODE_API AABBTree : public AABBTreeNode | ||
| 108 | { | ||
| 109 | public: | ||
| 110 | // Constructor / Destructor | ||
| 111 | AABBTree(); | ||
| 112 | ~AABBTree(); | ||
| 113 | // Build | ||
| 114 | bool Build(AABBTreeBuilder* builder); | ||
| 115 | void Release(); | ||
| 116 | |||
| 117 | // Data access | ||
| 118 | inline_ const udword* GetIndices() const { return mIndices; } //!< Catch the indices | ||
| 119 | inline_ udword GetNbNodes() const { return mTotalNbNodes; } //!< Catch the number of nodes | ||
| 120 | |||
| 121 | // Infos | ||
| 122 | bool IsComplete() const; | ||
| 123 | // Stats | ||
| 124 | udword ComputeDepth() const; | ||
| 125 | udword GetUsedBytes() const; | ||
| 126 | udword Walk(WalkingCallback callback, void* user_data) const; | ||
| 127 | |||
| 128 | bool Refit(AABBTreeBuilder* builder); | ||
| 129 | bool Refit2(AABBTreeBuilder* builder); | ||
| 130 | private: | ||
| 131 | udword* mIndices; //!< Indices in the app list. Indices are reorganized during build (permutation). | ||
| 132 | AABBTreeNode* mPool; //!< Linear pool of nodes for complete trees. Null otherwise. [Opcode 1.3] | ||
| 133 | // Stats | ||
| 134 | udword mTotalNbNodes; //!< Number of nodes in the tree. | ||
| 135 | }; | ||
| 136 | |||
| 137 | #endif // __OPC_AABBTREE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_BaseModel.cpp b/libraries/ode-0.9/OPCODE/OPC_BaseModel.cpp new file mode 100644 index 0000000..9520d9e --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_BaseModel.cpp | |||
| @@ -0,0 +1,138 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains base model interface. | ||
| 12 | * \file OPC_BaseModel.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date May, 18, 2003 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * The base class for collision models. | ||
| 21 | * | ||
| 22 | * \class BaseModel | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date May, 18, 2003 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | // Precompiled Header | ||
| 31 | #include "Stdafx.h" | ||
| 32 | |||
| 33 | using namespace Opcode; | ||
| 34 | |||
| 35 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 36 | /** | ||
| 37 | * Constructor. | ||
| 38 | */ | ||
| 39 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 40 | OPCODECREATE::OPCODECREATE() | ||
| 41 | { | ||
| 42 | mIMesh = null; | ||
| 43 | mSettings.mRules = SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER; | ||
| 44 | mSettings.mLimit = 1; // Mandatory for complete trees | ||
| 45 | mNoLeaf = true; | ||
| 46 | mQuantized = true; | ||
| 47 | #ifdef __MESHMERIZER_H__ | ||
| 48 | mCollisionHull = false; | ||
| 49 | #endif // __MESHMERIZER_H__ | ||
| 50 | mKeepOriginal = false; | ||
| 51 | mCanRemap = false; | ||
| 52 | } | ||
| 53 | |||
| 54 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 55 | /** | ||
| 56 | * Constructor. | ||
| 57 | */ | ||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | BaseModel::BaseModel() : mIMesh(null), mModelCode(0), mSource(null), mTree(null) | ||
| 60 | { | ||
| 61 | } | ||
| 62 | |||
| 63 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 64 | /** | ||
| 65 | * Destructor. | ||
| 66 | */ | ||
| 67 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 68 | BaseModel::~BaseModel() | ||
| 69 | { | ||
| 70 | ReleaseBase(); | ||
| 71 | } | ||
| 72 | |||
| 73 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 74 | /** | ||
| 75 | * Releases everything. | ||
| 76 | */ | ||
| 77 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 78 | void BaseModel::ReleaseBase() | ||
| 79 | { | ||
| 80 | DELETESINGLE(mSource); | ||
| 81 | DELETESINGLE(mTree); | ||
| 82 | } | ||
| 83 | |||
| 84 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 85 | /** | ||
| 86 | * Creates an optimized tree according to user-settings, and setups mModelCode. | ||
| 87 | * \param no_leaf [in] true for "no leaf" tree | ||
| 88 | * \param quantized [in] true for quantized tree | ||
| 89 | * \return true if success | ||
| 90 | */ | ||
| 91 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 92 | bool BaseModel::CreateTree(bool no_leaf, bool quantized) | ||
| 93 | { | ||
| 94 | DELETESINGLE(mTree); | ||
| 95 | |||
| 96 | // Setup model code | ||
| 97 | if(no_leaf) mModelCode |= OPC_NO_LEAF; | ||
| 98 | else mModelCode &= ~OPC_NO_LEAF; | ||
| 99 | |||
| 100 | if(quantized) mModelCode |= OPC_QUANTIZED; | ||
| 101 | else mModelCode &= ~OPC_QUANTIZED; | ||
| 102 | |||
| 103 | // Create the correct class | ||
| 104 | if(mModelCode & OPC_NO_LEAF) | ||
| 105 | { | ||
| 106 | if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedNoLeafTree; | ||
| 107 | else mTree = new AABBNoLeafTree; | ||
| 108 | } | ||
| 109 | else | ||
| 110 | { | ||
| 111 | if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedTree; | ||
| 112 | else mTree = new AABBCollisionTree; | ||
| 113 | } | ||
| 114 | CHECKALLOC(mTree); | ||
| 115 | |||
| 116 | return true; | ||
| 117 | } | ||
| 118 | |||
| 119 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 120 | /** | ||
| 121 | * Refits the collision model. This can be used to handle dynamic meshes. Usage is: | ||
| 122 | * 1. modify your mesh vertices (keep the topology constant!) | ||
| 123 | * 2. refit the tree (call this method) | ||
| 124 | * \return true if success | ||
| 125 | */ | ||
| 126 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 127 | bool BaseModel::Refit() | ||
| 128 | { | ||
| 129 | // Refit the optimized tree | ||
| 130 | return mTree->Refit(mIMesh); | ||
| 131 | |||
| 132 | // Old code kept for reference : refit the source tree then rebuild ! | ||
| 133 | // if(!mSource) return false; | ||
| 134 | // // Ouch... | ||
| 135 | // mSource->Refit(&mTB); | ||
| 136 | // // Ouch... | ||
| 137 | // return mTree->Build(mSource); | ||
| 138 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_BaseModel.h b/libraries/ode-0.9/OPCODE/OPC_BaseModel.h new file mode 100644 index 0000000..c6072db --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_BaseModel.h | |||
| @@ -0,0 +1,175 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains base model interface. | ||
| 12 | * \file OPC_BaseModel.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date May, 18, 2003 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_BASEMODEL_H__ | ||
| 21 | #define __OPC_BASEMODEL_H__ | ||
| 22 | |||
| 23 | //! Model creation structure | ||
| 24 | struct OPCODE_API OPCODECREATE | ||
| 25 | { | ||
| 26 | //! Constructor | ||
| 27 | OPCODECREATE(); | ||
| 28 | |||
| 29 | MeshInterface* mIMesh; //!< Mesh interface (access to triangles & vertices) (*) | ||
| 30 | BuildSettings mSettings; //!< Builder's settings | ||
| 31 | bool mNoLeaf; //!< true => discard leaf nodes (else use a normal tree) | ||
| 32 | bool mQuantized; //!< true => quantize the tree (else use a normal tree) | ||
| 33 | #ifdef __MESHMERIZER_H__ | ||
| 34 | bool mCollisionHull; //!< true => use convex hull + GJK | ||
| 35 | #endif // __MESHMERIZER_H__ | ||
| 36 | bool mKeepOriginal; //!< true => keep a copy of the original tree (debug purpose) | ||
| 37 | bool mCanRemap; //!< true => allows OPCODE to reorganize client arrays | ||
| 38 | |||
| 39 | // (*) This pointer is saved internally and used by OPCODE until collision structures are released, | ||
| 40 | // so beware of the object's lifetime. | ||
| 41 | }; | ||
| 42 | |||
| 43 | enum ModelFlag | ||
| 44 | { | ||
| 45 | OPC_QUANTIZED = (1<<0), //!< Compressed/uncompressed tree | ||
| 46 | OPC_NO_LEAF = (1<<1), //!< Leaf/NoLeaf tree | ||
| 47 | OPC_SINGLE_NODE = (1<<2) //!< Special case for 1-node models | ||
| 48 | }; | ||
| 49 | |||
| 50 | class OPCODE_API BaseModel | ||
| 51 | { | ||
| 52 | public: | ||
| 53 | // Constructor/Destructor | ||
| 54 | BaseModel(); | ||
| 55 | virtual ~BaseModel(); | ||
| 56 | |||
| 57 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 58 | /** | ||
| 59 | * Builds a collision model. | ||
| 60 | * \param create [in] model creation structure | ||
| 61 | * \return true if success | ||
| 62 | */ | ||
| 63 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 64 | virtual bool Build(const OPCODECREATE& create) = 0; | ||
| 65 | |||
| 66 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 67 | /** | ||
| 68 | * Gets the number of bytes used by the tree. | ||
| 69 | * \return amount of bytes used | ||
| 70 | */ | ||
| 71 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 72 | virtual udword GetUsedBytes() const = 0; | ||
| 73 | |||
| 74 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 75 | /** | ||
| 76 | * Refits the collision model. This can be used to handle dynamic meshes. Usage is: | ||
| 77 | * 1. modify your mesh vertices (keep the topology constant!) | ||
| 78 | * 2. refit the tree (call this method) | ||
| 79 | * \return true if success | ||
| 80 | */ | ||
| 81 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 82 | virtual bool Refit(); | ||
| 83 | |||
| 84 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 85 | /** | ||
| 86 | * Gets the source tree. | ||
| 87 | * \return generic tree | ||
| 88 | */ | ||
| 89 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 90 | inline_ const AABBTree* GetSourceTree() const { return mSource; } | ||
| 91 | |||
| 92 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 93 | /** | ||
| 94 | * Gets the tree. | ||
| 95 | * \return the collision tree | ||
| 96 | */ | ||
| 97 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 98 | inline_ const AABBOptimizedTree* GetTree() const { return mTree; } | ||
| 99 | |||
| 100 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 101 | /** | ||
| 102 | * Gets the tree. | ||
| 103 | * \return the collision tree | ||
| 104 | */ | ||
| 105 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 106 | inline_ AABBOptimizedTree* GetTree() { return mTree; } | ||
| 107 | |||
| 108 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 109 | /** | ||
| 110 | * Gets the number of nodes in the tree. | ||
| 111 | * Should be 2*N-1 for normal trees and N-1 for optimized ones. | ||
| 112 | * \return number of nodes | ||
| 113 | */ | ||
| 114 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 115 | inline_ udword GetNbNodes() const { return mTree->GetNbNodes(); } | ||
| 116 | |||
| 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 118 | /** | ||
| 119 | * Checks whether the tree has leaf nodes or not. | ||
| 120 | * \return true if the tree has leaf nodes (normal tree), else false (optimized tree) | ||
| 121 | */ | ||
| 122 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 123 | inline_ BOOL HasLeafNodes() const { return !(mModelCode & OPC_NO_LEAF); } | ||
| 124 | |||
| 125 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 126 | /** | ||
| 127 | * Checks whether the tree is quantized or not. | ||
| 128 | * \return true if the tree is quantized | ||
| 129 | */ | ||
| 130 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 131 | inline_ BOOL IsQuantized() const { return mModelCode & OPC_QUANTIZED; } | ||
| 132 | |||
| 133 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 134 | /** | ||
| 135 | * Checks whether the model has a single node or not. This special case must be handled separately. | ||
| 136 | * \return true if the model has only 1 node | ||
| 137 | */ | ||
| 138 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 139 | inline_ BOOL HasSingleNode() const { return mModelCode & OPC_SINGLE_NODE; } | ||
| 140 | |||
| 141 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 142 | /** | ||
| 143 | * Gets the model's code. | ||
| 144 | * \return model's code | ||
| 145 | */ | ||
| 146 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 147 | inline_ udword GetModelCode() const { return mModelCode; } | ||
| 148 | |||
| 149 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 150 | /** | ||
| 151 | * Gets the mesh interface. | ||
| 152 | * \return mesh interface | ||
| 153 | */ | ||
| 154 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 155 | inline_ const MeshInterface* GetMeshInterface() const { return mIMesh; } | ||
| 156 | |||
| 157 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 158 | /** | ||
| 159 | * Sets the mesh interface. | ||
| 160 | * \param imesh [in] mesh interface | ||
| 161 | */ | ||
| 162 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 163 | inline_ void SetMeshInterface(const MeshInterface* imesh) { mIMesh = imesh; } | ||
| 164 | |||
| 165 | protected: | ||
| 166 | const MeshInterface* mIMesh; //!< User-defined mesh interface | ||
| 167 | udword mModelCode; //!< Model code = combination of ModelFlag(s) | ||
| 168 | AABBTree* mSource; //!< Original source tree | ||
| 169 | AABBOptimizedTree* mTree; //!< Optimized tree owned by the model | ||
| 170 | // Internal methods | ||
| 171 | void ReleaseBase(); | ||
| 172 | bool CreateTree(bool no_leaf, bool quantized); | ||
| 173 | }; | ||
| 174 | |||
| 175 | #endif //__OPC_BASEMODEL_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_BoxBoxOverlap.h b/libraries/ode-0.9/OPCODE/OPC_BoxBoxOverlap.h new file mode 100644 index 0000000..757a17d --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_BoxBoxOverlap.h | |||
| @@ -0,0 +1,122 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * OBB-OBB overlap test using the separating axis theorem. | ||
| 4 | * - original code by Gomez / Gamasutra (similar to Gottschalk's one in RAPID) | ||
| 5 | * - optimized for AABB trees by computing the rotation matrix once (SOLID-fashion) | ||
| 6 | * - the fabs matrix is precomputed as well and epsilon-tweaked (RAPID-style, we found this almost mandatory) | ||
| 7 | * - Class III axes can be disabled... (SOLID & Intel fashion) | ||
| 8 | * - ...or enabled to perform some profiling | ||
| 9 | * - CPU comparisons used when appropriate | ||
| 10 | * - lazy evaluation sometimes saves some work in case of early exits (unlike SOLID) | ||
| 11 | * | ||
| 12 | * \param ea [in] extents from box A | ||
| 13 | * \param ca [in] center from box A | ||
| 14 | * \param eb [in] extents from box B | ||
| 15 | * \param cb [in] center from box B | ||
| 16 | * \return true if boxes overlap | ||
| 17 | */ | ||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb) | ||
| 20 | { | ||
| 21 | // Stats | ||
| 22 | mNbBVBVTests++; | ||
| 23 | |||
| 24 | float t,t2; | ||
| 25 | |||
| 26 | // Class I : A's basis vectors | ||
| 27 | float Tx = (mR1to0.m[0][0]*cb.x + mR1to0.m[1][0]*cb.y + mR1to0.m[2][0]*cb.z) + mT1to0.x - ca.x; | ||
| 28 | t = ea.x + eb.x*mAR.m[0][0] + eb.y*mAR.m[1][0] + eb.z*mAR.m[2][0]; | ||
| 29 | if(GREATER(Tx, t)) return FALSE; | ||
| 30 | |||
| 31 | float Ty = (mR1to0.m[0][1]*cb.x + mR1to0.m[1][1]*cb.y + mR1to0.m[2][1]*cb.z) + mT1to0.y - ca.y; | ||
| 32 | t = ea.y + eb.x*mAR.m[0][1] + eb.y*mAR.m[1][1] + eb.z*mAR.m[2][1]; | ||
| 33 | if(GREATER(Ty, t)) return FALSE; | ||
| 34 | |||
| 35 | float Tz = (mR1to0.m[0][2]*cb.x + mR1to0.m[1][2]*cb.y + mR1to0.m[2][2]*cb.z) + mT1to0.z - ca.z; | ||
| 36 | t = ea.z + eb.x*mAR.m[0][2] + eb.y*mAR.m[1][2] + eb.z*mAR.m[2][2]; | ||
| 37 | if(GREATER(Tz, t)) return FALSE; | ||
| 38 | |||
| 39 | // Class II : B's basis vectors | ||
| 40 | t = Tx*mR1to0.m[0][0] + Ty*mR1to0.m[0][1] + Tz*mR1to0.m[0][2]; t2 = ea.x*mAR.m[0][0] + ea.y*mAR.m[0][1] + ea.z*mAR.m[0][2] + eb.x; | ||
| 41 | if(GREATER(t, t2)) return FALSE; | ||
| 42 | |||
| 43 | t = Tx*mR1to0.m[1][0] + Ty*mR1to0.m[1][1] + Tz*mR1to0.m[1][2]; t2 = ea.x*mAR.m[1][0] + ea.y*mAR.m[1][1] + ea.z*mAR.m[1][2] + eb.y; | ||
| 44 | if(GREATER(t, t2)) return FALSE; | ||
| 45 | |||
| 46 | t = Tx*mR1to0.m[2][0] + Ty*mR1to0.m[2][1] + Tz*mR1to0.m[2][2]; t2 = ea.x*mAR.m[2][0] + ea.y*mAR.m[2][1] + ea.z*mAR.m[2][2] + eb.z; | ||
| 47 | if(GREATER(t, t2)) return FALSE; | ||
| 48 | |||
| 49 | // Class III : 9 cross products | ||
| 50 | // Cool trick: always perform the full test for first level, regardless of settings. | ||
| 51 | // That way pathological cases (such as the pencils scene) are quickly rejected anyway ! | ||
| 52 | if(mFullBoxBoxTest || mNbBVBVTests==1) | ||
| 53 | { | ||
| 54 | t = Tz*mR1to0.m[0][1] - Ty*mR1to0.m[0][2]; t2 = ea.y*mAR.m[0][2] + ea.z*mAR.m[0][1] + eb.y*mAR.m[2][0] + eb.z*mAR.m[1][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B0 | ||
| 55 | t = Tz*mR1to0.m[1][1] - Ty*mR1to0.m[1][2]; t2 = ea.y*mAR.m[1][2] + ea.z*mAR.m[1][1] + eb.x*mAR.m[2][0] + eb.z*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B1 | ||
| 56 | t = Tz*mR1to0.m[2][1] - Ty*mR1to0.m[2][2]; t2 = ea.y*mAR.m[2][2] + ea.z*mAR.m[2][1] + eb.x*mAR.m[1][0] + eb.y*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B2 | ||
| 57 | t = Tx*mR1to0.m[0][2] - Tz*mR1to0.m[0][0]; t2 = ea.x*mAR.m[0][2] + ea.z*mAR.m[0][0] + eb.y*mAR.m[2][1] + eb.z*mAR.m[1][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B0 | ||
| 58 | t = Tx*mR1to0.m[1][2] - Tz*mR1to0.m[1][0]; t2 = ea.x*mAR.m[1][2] + ea.z*mAR.m[1][0] + eb.x*mAR.m[2][1] + eb.z*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B1 | ||
| 59 | t = Tx*mR1to0.m[2][2] - Tz*mR1to0.m[2][0]; t2 = ea.x*mAR.m[2][2] + ea.z*mAR.m[2][0] + eb.x*mAR.m[1][1] + eb.y*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B2 | ||
| 60 | t = Ty*mR1to0.m[0][0] - Tx*mR1to0.m[0][1]; t2 = ea.x*mAR.m[0][1] + ea.y*mAR.m[0][0] + eb.y*mAR.m[2][2] + eb.z*mAR.m[1][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B0 | ||
| 61 | t = Ty*mR1to0.m[1][0] - Tx*mR1to0.m[1][1]; t2 = ea.x*mAR.m[1][1] + ea.y*mAR.m[1][0] + eb.x*mAR.m[2][2] + eb.z*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B1 | ||
| 62 | t = Ty*mR1to0.m[2][0] - Tx*mR1to0.m[2][1]; t2 = ea.x*mAR.m[2][1] + ea.y*mAR.m[2][0] + eb.x*mAR.m[1][2] + eb.y*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B2 | ||
| 63 | } | ||
| 64 | return TRUE; | ||
| 65 | } | ||
| 66 | |||
| 67 | //! A dedicated version when one box is constant | ||
| 68 | inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& center) | ||
| 69 | { | ||
| 70 | // Stats | ||
| 71 | mNbVolumeBVTests++; | ||
| 72 | |||
| 73 | float t,t2; | ||
| 74 | |||
| 75 | // Class I : A's basis vectors | ||
| 76 | float Tx = mTBoxToModel.x - center.x; t = extents.x + mBBx1; if(GREATER(Tx, t)) return FALSE; | ||
| 77 | float Ty = mTBoxToModel.y - center.y; t = extents.y + mBBy1; if(GREATER(Ty, t)) return FALSE; | ||
| 78 | float Tz = mTBoxToModel.z - center.z; t = extents.z + mBBz1; if(GREATER(Tz, t)) return FALSE; | ||
| 79 | |||
| 80 | // Class II : B's basis vectors | ||
| 81 | t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2]; | ||
| 82 | t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x; | ||
| 83 | if(GREATER(t, t2)) return FALSE; | ||
| 84 | |||
| 85 | t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2]; | ||
| 86 | t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y; | ||
| 87 | if(GREATER(t, t2)) return FALSE; | ||
| 88 | |||
| 89 | t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2]; | ||
| 90 | t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z; | ||
| 91 | if(GREATER(t, t2)) return FALSE; | ||
| 92 | |||
| 93 | // Class III : 9 cross products | ||
| 94 | // Cool trick: always perform the full test for first level, regardless of settings. | ||
| 95 | // That way pathological cases (such as the pencils scene) are quickly rejected anyway ! | ||
| 96 | if(mFullBoxBoxTest || mNbVolumeBVTests==1) | ||
| 97 | { | ||
| 98 | t = Tz*mRBoxToModel.m[0][1] - Ty*mRBoxToModel.m[0][2]; t2 = extents.y*mAR.m[0][2] + extents.z*mAR.m[0][1] + mBB_1; if(GREATER(t, t2)) return FALSE; // L = A0 x B0 | ||
| 99 | t = Tz*mRBoxToModel.m[1][1] - Ty*mRBoxToModel.m[1][2]; t2 = extents.y*mAR.m[1][2] + extents.z*mAR.m[1][1] + mBB_2; if(GREATER(t, t2)) return FALSE; // L = A0 x B1 | ||
| 100 | t = Tz*mRBoxToModel.m[2][1] - Ty*mRBoxToModel.m[2][2]; t2 = extents.y*mAR.m[2][2] + extents.z*mAR.m[2][1] + mBB_3; if(GREATER(t, t2)) return FALSE; // L = A0 x B2 | ||
| 101 | t = Tx*mRBoxToModel.m[0][2] - Tz*mRBoxToModel.m[0][0]; t2 = extents.x*mAR.m[0][2] + extents.z*mAR.m[0][0] + mBB_4; if(GREATER(t, t2)) return FALSE; // L = A1 x B0 | ||
| 102 | t = Tx*mRBoxToModel.m[1][2] - Tz*mRBoxToModel.m[1][0]; t2 = extents.x*mAR.m[1][2] + extents.z*mAR.m[1][0] + mBB_5; if(GREATER(t, t2)) return FALSE; // L = A1 x B1 | ||
| 103 | t = Tx*mRBoxToModel.m[2][2] - Tz*mRBoxToModel.m[2][0]; t2 = extents.x*mAR.m[2][2] + extents.z*mAR.m[2][0] + mBB_6; if(GREATER(t, t2)) return FALSE; // L = A1 x B2 | ||
| 104 | t = Ty*mRBoxToModel.m[0][0] - Tx*mRBoxToModel.m[0][1]; t2 = extents.x*mAR.m[0][1] + extents.y*mAR.m[0][0] + mBB_7; if(GREATER(t, t2)) return FALSE; // L = A2 x B0 | ||
| 105 | t = Ty*mRBoxToModel.m[1][0] - Tx*mRBoxToModel.m[1][1]; t2 = extents.x*mAR.m[1][1] + extents.y*mAR.m[1][0] + mBB_8; if(GREATER(t, t2)) return FALSE; // L = A2 x B1 | ||
| 106 | t = Ty*mRBoxToModel.m[2][0] - Tx*mRBoxToModel.m[2][1]; t2 = extents.x*mAR.m[2][1] + extents.y*mAR.m[2][0] + mBB_9; if(GREATER(t, t2)) return FALSE; // L = A2 x B2 | ||
| 107 | } | ||
| 108 | return TRUE; | ||
| 109 | } | ||
| 110 | |||
| 111 | //! A special version for 2 axis-aligned boxes | ||
| 112 | inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& center) | ||
| 113 | { | ||
| 114 | // Stats | ||
| 115 | mNbVolumeBVTests++; | ||
| 116 | |||
| 117 | float tx = mBox.mCenter.x - center.x; float ex = extents.x + mBox.mExtents.x; if(GREATER(tx, ex)) return FALSE; | ||
| 118 | float ty = mBox.mCenter.y - center.y; float ey = extents.y + mBox.mExtents.y; if(GREATER(ty, ey)) return FALSE; | ||
| 119 | float tz = mBox.mCenter.z - center.z; float ez = extents.z + mBox.mExtents.z; if(GREATER(tz, ez)) return FALSE; | ||
| 120 | |||
| 121 | return TRUE; | ||
| 122 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp new file mode 100644 index 0000000..6906160 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp | |||
| @@ -0,0 +1,367 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for box pruning. | ||
| 12 | * \file IceBoxPruning.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 29, 2000 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /* | ||
| 19 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 20 | You could use a complex sweep-and-prune as implemented in I-Collide. | ||
| 21 | You could use a complex hashing scheme as implemented in V-Clip or recently in ODE it seems. | ||
| 22 | You could use a "Recursive Dimensional Clustering" algorithm as implemented in GPG2. | ||
| 23 | |||
| 24 | Or you could use this. | ||
| 25 | Faster ? I don't know. Probably not. It would be a shame. But who knows ? | ||
| 26 | Easier ? Definitely. Enjoy the sheer simplicity. | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | */ | ||
| 29 | |||
| 30 | |||
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 32 | // Precompiled Header | ||
| 33 | #include "Stdafx.h" | ||
| 34 | |||
| 35 | using namespace Opcode; | ||
| 36 | |||
| 37 | inline_ void FindRunningIndex(udword& index, float* array, udword* sorted, int last, float max) | ||
| 38 | { | ||
| 39 | int First=index; | ||
| 40 | while(First<=last) | ||
| 41 | { | ||
| 42 | index = (First+last)>>1; | ||
| 43 | |||
| 44 | if(max>array[sorted[index]]) First = index+1; | ||
| 45 | else last = index-1; | ||
| 46 | } | ||
| 47 | } | ||
| 48 | // ### could be log(n) ! | ||
| 49 | // and maybe use cmp integers | ||
| 50 | |||
| 51 | // InsertionSort has better coherence, RadixSort is better for one-shot queries. | ||
| 52 | #define PRUNING_SORTER RadixSort | ||
| 53 | //#define PRUNING_SORTER InsertionSort | ||
| 54 | |||
| 55 | // Static for coherence | ||
| 56 | static PRUNING_SORTER* gCompletePruningSorter = null; | ||
| 57 | static PRUNING_SORTER* gBipartitePruningSorter0 = null; | ||
| 58 | static PRUNING_SORTER* gBipartitePruningSorter1 = null; | ||
| 59 | inline_ PRUNING_SORTER* GetCompletePruningSorter() | ||
| 60 | { | ||
| 61 | if(!gCompletePruningSorter) gCompletePruningSorter = new PRUNING_SORTER; | ||
| 62 | return gCompletePruningSorter; | ||
| 63 | } | ||
| 64 | inline_ PRUNING_SORTER* GetBipartitePruningSorter0() | ||
| 65 | { | ||
| 66 | if(!gBipartitePruningSorter0) gBipartitePruningSorter0 = new PRUNING_SORTER; | ||
| 67 | return gBipartitePruningSorter0; | ||
| 68 | } | ||
| 69 | inline_ PRUNING_SORTER* GetBipartitePruningSorter1() | ||
| 70 | { | ||
| 71 | if(!gBipartitePruningSorter1) gBipartitePruningSorter1 = new PRUNING_SORTER; | ||
| 72 | return gBipartitePruningSorter1; | ||
| 73 | } | ||
| 74 | void ReleasePruningSorters() | ||
| 75 | { | ||
| 76 | DELETESINGLE(gBipartitePruningSorter1); | ||
| 77 | DELETESINGLE(gBipartitePruningSorter0); | ||
| 78 | DELETESINGLE(gCompletePruningSorter); | ||
| 79 | } | ||
| 80 | |||
| 81 | |||
| 82 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 83 | /** | ||
| 84 | * Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set. | ||
| 85 | * \param nb0 [in] number of boxes in the first set | ||
| 86 | * \param array0 [in] array of boxes for the first set | ||
| 87 | * \param nb1 [in] number of boxes in the second set | ||
| 88 | * \param array1 [in] array of boxes for the second set | ||
| 89 | * \param pairs [out] array of overlapping pairs | ||
| 90 | * \param axes [in] projection order (0,2,1 is often best) | ||
| 91 | * \return true if success. | ||
| 92 | */ | ||
| 93 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 94 | bool Opcode::BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes) | ||
| 95 | { | ||
| 96 | // Checkings | ||
| 97 | if(!nb0 || !array0 || !nb1 || !array1) return false; | ||
| 98 | |||
| 99 | // Catch axes | ||
| 100 | udword Axis0 = axes.mAxis0; | ||
| 101 | udword Axis1 = axes.mAxis1; | ||
| 102 | udword Axis2 = axes.mAxis2; | ||
| 103 | |||
| 104 | // Allocate some temporary data | ||
| 105 | float* MinPosList0 = new float[nb0]; | ||
| 106 | float* MinPosList1 = new float[nb1]; | ||
| 107 | |||
| 108 | // 1) Build main lists using the primary axis | ||
| 109 | for(udword i=0;i<nb0;i++) MinPosList0[i] = array0[i]->GetMin(Axis0); | ||
| 110 | for(udword i=0;i<nb1;i++) MinPosList1[i] = array1[i]->GetMin(Axis0); | ||
| 111 | |||
| 112 | // 2) Sort the lists | ||
| 113 | PRUNING_SORTER* RS0 = GetBipartitePruningSorter0(); | ||
| 114 | PRUNING_SORTER* RS1 = GetBipartitePruningSorter1(); | ||
| 115 | const udword* Sorted0 = RS0->Sort(MinPosList0, nb0).GetRanks(); | ||
| 116 | const udword* Sorted1 = RS1->Sort(MinPosList1, nb1).GetRanks(); | ||
| 117 | |||
| 118 | // 3) Prune the lists | ||
| 119 | udword Index0, Index1; | ||
| 120 | |||
| 121 | const udword* const LastSorted0 = &Sorted0[nb0]; | ||
| 122 | const udword* const LastSorted1 = &Sorted1[nb1]; | ||
| 123 | const udword* RunningAddress0 = Sorted0; | ||
| 124 | const udword* RunningAddress1 = Sorted1; | ||
| 125 | |||
| 126 | while(RunningAddress1<LastSorted1 && Sorted0<LastSorted0) | ||
| 127 | { | ||
| 128 | Index0 = *Sorted0++; | ||
| 129 | |||
| 130 | while(RunningAddress1<LastSorted1 && MinPosList1[*RunningAddress1]<MinPosList0[Index0]) RunningAddress1++; | ||
| 131 | |||
| 132 | const udword* RunningAddress2_1 = RunningAddress1; | ||
| 133 | |||
| 134 | while(RunningAddress2_1<LastSorted1 && MinPosList1[Index1 = *RunningAddress2_1++]<=array0[Index0]->GetMax(Axis0)) | ||
| 135 | { | ||
| 136 | if(array0[Index0]->Intersect(*array1[Index1], Axis1)) | ||
| 137 | { | ||
| 138 | if(array0[Index0]->Intersect(*array1[Index1], Axis2)) | ||
| 139 | { | ||
| 140 | pairs.AddPair(Index0, Index1); | ||
| 141 | } | ||
| 142 | } | ||
| 143 | } | ||
| 144 | } | ||
| 145 | |||
| 146 | //// | ||
| 147 | |||
| 148 | while(RunningAddress0<LastSorted0 && Sorted1<LastSorted1) | ||
| 149 | { | ||
| 150 | Index0 = *Sorted1++; | ||
| 151 | |||
| 152 | while(RunningAddress0<LastSorted0 && MinPosList0[*RunningAddress0]<=MinPosList1[Index0]) RunningAddress0++; | ||
| 153 | |||
| 154 | const udword* RunningAddress2_0 = RunningAddress0; | ||
| 155 | |||
| 156 | while(RunningAddress2_0<LastSorted0 && MinPosList0[Index1 = *RunningAddress2_0++]<=array1[Index0]->GetMax(Axis0)) | ||
| 157 | { | ||
| 158 | if(array0[Index1]->Intersect(*array1[Index0], Axis1)) | ||
| 159 | { | ||
| 160 | if(array0[Index1]->Intersect(*array1[Index0], Axis2)) | ||
| 161 | { | ||
| 162 | pairs.AddPair(Index1, Index0); | ||
| 163 | } | ||
| 164 | } | ||
| 165 | |||
| 166 | } | ||
| 167 | } | ||
| 168 | |||
| 169 | DELETEARRAY(MinPosList1); | ||
| 170 | DELETEARRAY(MinPosList0); | ||
| 171 | |||
| 172 | return true; | ||
| 173 | } | ||
| 174 | |||
| 175 | #define ORIGINAL_VERSION | ||
| 176 | //#define JOAKIM | ||
| 177 | |||
| 178 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 179 | /** | ||
| 180 | * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set. | ||
| 181 | * \param nb [in] number of boxes | ||
| 182 | * \param array [in] array of boxes | ||
| 183 | * \param pairs [out] array of overlapping pairs | ||
| 184 | * \param axes [in] projection order (0,2,1 is often best) | ||
| 185 | * \return true if success. | ||
| 186 | */ | ||
| 187 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 188 | bool Opcode::CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes) | ||
| 189 | { | ||
| 190 | // Checkings | ||
| 191 | if(!nb || !array) return false; | ||
| 192 | |||
| 193 | // Catch axes | ||
| 194 | udword Axis0 = axes.mAxis0; | ||
| 195 | udword Axis1 = axes.mAxis1; | ||
| 196 | udword Axis2 = axes.mAxis2; | ||
| 197 | |||
| 198 | #ifdef ORIGINAL_VERSION | ||
| 199 | // Allocate some temporary data | ||
| 200 | // float* PosList = new float[nb]; | ||
| 201 | float* PosList = new float[nb+1]; | ||
| 202 | |||
| 203 | // 1) Build main list using the primary axis | ||
| 204 | for(udword i=0;i<nb;i++) PosList[i] = array[i]->GetMin(Axis0); | ||
| 205 | PosList[nb++] = MAX_FLOAT; | ||
| 206 | |||
| 207 | // 2) Sort the list | ||
| 208 | PRUNING_SORTER* RS = GetCompletePruningSorter(); | ||
| 209 | const udword* Sorted = RS->Sort(PosList, nb).GetRanks(); | ||
| 210 | |||
| 211 | // 3) Prune the list | ||
| 212 | const udword* const LastSorted = &Sorted[nb]; | ||
| 213 | const udword* RunningAddress = Sorted; | ||
| 214 | udword Index0, Index1; | ||
| 215 | while(RunningAddress<LastSorted && Sorted<LastSorted) | ||
| 216 | { | ||
| 217 | Index0 = *Sorted++; | ||
| 218 | |||
| 219 | // while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]); | ||
| 220 | while(PosList[*RunningAddress++]<PosList[Index0]); | ||
| 221 | |||
| 222 | if(RunningAddress<LastSorted) | ||
| 223 | { | ||
| 224 | const udword* RunningAddress2 = RunningAddress; | ||
| 225 | |||
| 226 | // while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0)) | ||
| 227 | while(PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0)) | ||
| 228 | { | ||
| 229 | // if(Index0!=Index1) | ||
| 230 | // { | ||
| 231 | if(array[Index0]->Intersect(*array[Index1], Axis1)) | ||
| 232 | { | ||
| 233 | if(array[Index0]->Intersect(*array[Index1], Axis2)) | ||
| 234 | { | ||
| 235 | pairs.AddPair(Index0, Index1); | ||
| 236 | } | ||
| 237 | } | ||
| 238 | // } | ||
| 239 | } | ||
| 240 | } | ||
| 241 | } | ||
| 242 | |||
| 243 | DELETEARRAY(PosList); | ||
| 244 | #endif | ||
| 245 | |||
| 246 | #ifdef JOAKIM | ||
| 247 | // Allocate some temporary data | ||
| 248 | // float* PosList = new float[nb]; | ||
| 249 | float* MinList = new float[nb+1]; | ||
| 250 | |||
| 251 | // 1) Build main list using the primary axis | ||
| 252 | for(udword i=0;i<nb;i++) MinList[i] = array[i]->GetMin(Axis0); | ||
| 253 | MinList[nb] = MAX_FLOAT; | ||
| 254 | |||
| 255 | // 2) Sort the list | ||
| 256 | PRUNING_SORTER* RS = GetCompletePruningSorter(); | ||
| 257 | udword* Sorted = RS->Sort(MinList, nb+1).GetRanks(); | ||
| 258 | |||
| 259 | // 3) Prune the list | ||
| 260 | // const udword* const LastSorted = &Sorted[nb]; | ||
| 261 | // const udword* const LastSorted = &Sorted[nb-1]; | ||
| 262 | const udword* RunningAddress = Sorted; | ||
| 263 | udword Index0, Index1; | ||
| 264 | |||
| 265 | // while(RunningAddress<LastSorted && Sorted<LastSorted) | ||
| 266 | // while(RunningAddress<LastSorted) | ||
| 267 | while(RunningAddress<&Sorted[nb]) | ||
| 268 | // while(Sorted<LastSorted) | ||
| 269 | { | ||
| 270 | // Index0 = *Sorted++; | ||
| 271 | Index0 = *RunningAddress++; | ||
| 272 | |||
| 273 | // while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]); | ||
| 274 | // while(PosList[*RunningAddress++]<PosList[Index0]); | ||
| 275 | //RunningAddress = Sorted; | ||
| 276 | // if(RunningAddress<LastSorted) | ||
| 277 | { | ||
| 278 | const udword* RunningAddress2 = RunningAddress; | ||
| 279 | |||
| 280 | // while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0)) | ||
| 281 | |||
| 282 | // float CurrentMin = array[Index0]->GetMin(Axis0); | ||
| 283 | float CurrentMax = array[Index0]->GetMax(Axis0); | ||
| 284 | |||
| 285 | while(MinList[Index1 = *RunningAddress2] <= CurrentMax) | ||
| 286 | // while(PosList[Index1 = *RunningAddress] <= CurrentMax) | ||
| 287 | { | ||
| 288 | // if(Index0!=Index1) | ||
| 289 | // { | ||
| 290 | if(array[Index0]->Intersect(*array[Index1], Axis1)) | ||
| 291 | { | ||
| 292 | if(array[Index0]->Intersect(*array[Index1], Axis2)) | ||
| 293 | { | ||
| 294 | pairs.AddPair(Index0, Index1); | ||
| 295 | } | ||
| 296 | } | ||
| 297 | // } | ||
| 298 | |||
| 299 | RunningAddress2++; | ||
| 300 | // RunningAddress++; | ||
| 301 | } | ||
| 302 | } | ||
| 303 | } | ||
| 304 | |||
| 305 | DELETEARRAY(MinList); | ||
| 306 | #endif | ||
| 307 | |||
| 308 | return true; | ||
| 309 | } | ||
| 310 | |||
| 311 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 312 | // Brute-force versions are kept: | ||
| 313 | // - to check the optimized versions return the correct list of intersections | ||
| 314 | // - to check the speed of the optimized code against the brute-force one | ||
| 315 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 316 | |||
| 317 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 318 | /** | ||
| 319 | * Brute-force bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set. | ||
| 320 | * \param nb0 [in] number of boxes in the first set | ||
| 321 | * \param array0 [in] array of boxes for the first set | ||
| 322 | * \param nb1 [in] number of boxes in the second set | ||
| 323 | * \param array1 [in] array of boxes for the second set | ||
| 324 | * \param pairs [out] array of overlapping pairs | ||
| 325 | * \return true if success. | ||
| 326 | */ | ||
| 327 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 328 | bool Opcode::BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs) | ||
| 329 | { | ||
| 330 | // Checkings | ||
| 331 | if(!nb0 || !array0 || !nb1 || !array1) return false; | ||
| 332 | |||
| 333 | // Brute-force nb0*nb1 overlap tests | ||
| 334 | for(udword i=0;i<nb0;i++) | ||
| 335 | { | ||
| 336 | for(udword j=0;j<nb1;j++) | ||
| 337 | { | ||
| 338 | if(array0[i]->Intersect(*array1[j])) pairs.AddPair(i, j); | ||
| 339 | } | ||
| 340 | } | ||
| 341 | return true; | ||
| 342 | } | ||
| 343 | |||
| 344 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 345 | /** | ||
| 346 | * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set. | ||
| 347 | * \param nb [in] number of boxes | ||
| 348 | * \param array [in] array of boxes | ||
| 349 | * \param pairs [out] array of overlapping pairs | ||
| 350 | * \return true if success. | ||
| 351 | */ | ||
| 352 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 353 | bool Opcode::BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs) | ||
| 354 | { | ||
| 355 | // Checkings | ||
| 356 | if(!nb || !array) return false; | ||
| 357 | |||
| 358 | // Brute-force n(n-1)/2 overlap tests | ||
| 359 | for(udword i=0;i<nb;i++) | ||
| 360 | { | ||
| 361 | for(udword j=i+1;j<nb;j++) | ||
| 362 | { | ||
| 363 | if(array[i]->Intersect(*array[j])) pairs.AddPair(i, j); | ||
| 364 | } | ||
| 365 | } | ||
| 366 | return true; | ||
| 367 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_BoxPruning.h b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.h new file mode 100644 index 0000000..2f77cd2 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.h | |||
| @@ -0,0 +1,31 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for box pruning. | ||
| 12 | * \file IceBoxPruning.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 29, 2000 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_BOXPRUNING_H__ | ||
| 21 | #define __OPC_BOXPRUNING_H__ | ||
| 22 | |||
| 23 | // Optimized versions | ||
| 24 | FUNCTION OPCODE_API bool CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes); | ||
| 25 | FUNCTION OPCODE_API bool BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes); | ||
| 26 | |||
| 27 | // Brute-force versions | ||
| 28 | FUNCTION OPCODE_API bool BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs); | ||
| 29 | FUNCTION OPCODE_API bool BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs); | ||
| 30 | |||
| 31 | #endif //__OPC_BOXPRUNING_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Collider.cpp b/libraries/ode-0.9/OPCODE/OPC_Collider.cpp new file mode 100644 index 0000000..f41fc36 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Collider.cpp | |||
| @@ -0,0 +1,54 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains base collider class. | ||
| 12 | * \file OPC_Collider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains the abstract class for colliders. | ||
| 21 | * | ||
| 22 | * \class Collider | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date June, 2, 2001 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | // Precompiled Header | ||
| 31 | #include "Stdafx.h" | ||
| 32 | |||
| 33 | using namespace Opcode; | ||
| 34 | |||
| 35 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 36 | /** | ||
| 37 | * Constructor. | ||
| 38 | */ | ||
| 39 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 40 | Collider::Collider() : | ||
| 41 | mFlags (0), | ||
| 42 | mCurrentModel (null), | ||
| 43 | mIMesh (null) | ||
| 44 | { | ||
| 45 | } | ||
| 46 | |||
| 47 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 48 | /** | ||
| 49 | * Destructor. | ||
| 50 | */ | ||
| 51 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 52 | Collider::~Collider() | ||
| 53 | { | ||
| 54 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Collider.h b/libraries/ode-0.9/OPCODE/OPC_Collider.h new file mode 100644 index 0000000..d718e02 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Collider.h | |||
| @@ -0,0 +1,176 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains base collider class. | ||
| 12 | * \file OPC_Collider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_COLLIDER_H__ | ||
| 21 | #define __OPC_COLLIDER_H__ | ||
| 22 | |||
| 23 | enum CollisionFlag | ||
| 24 | { | ||
| 25 | OPC_FIRST_CONTACT = (1<<0), //!< Report all contacts (false) or only first one (true) | ||
| 26 | OPC_TEMPORAL_COHERENCE = (1<<1), //!< Use temporal coherence or not | ||
| 27 | OPC_CONTACT = (1<<2), //!< Final contact status after a collision query | ||
| 28 | OPC_TEMPORAL_HIT = (1<<3), //!< There has been an early exit due to temporal coherence | ||
| 29 | OPC_NO_PRIMITIVE_TESTS = (1<<4), //!< Keep or discard primitive-bv tests in leaf nodes (volume-mesh queries) | ||
| 30 | |||
| 31 | OPC_CONTACT_FOUND = OPC_FIRST_CONTACT | OPC_CONTACT, | ||
| 32 | OPC_TEMPORAL_CONTACT = OPC_TEMPORAL_HIT | OPC_CONTACT, | ||
| 33 | |||
| 34 | OPC_FORCE_DWORD = 0x7fffffff | ||
| 35 | }; | ||
| 36 | |||
| 37 | class OPCODE_API Collider | ||
| 38 | { | ||
| 39 | public: | ||
| 40 | // Constructor / Destructor | ||
| 41 | Collider(); | ||
| 42 | virtual ~Collider(); | ||
| 43 | |||
| 44 | // Collision report | ||
| 45 | |||
| 46 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 47 | /** | ||
| 48 | * Gets the last collision status after a collision query. | ||
| 49 | * \return true if a collision occured | ||
| 50 | */ | ||
| 51 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 52 | inline_ BOOL GetContactStatus() const { return mFlags & OPC_CONTACT; } | ||
| 53 | |||
| 54 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 55 | /** | ||
| 56 | * Gets the "first contact" mode. | ||
| 57 | * \return true if "first contact" mode is on | ||
| 58 | */ | ||
| 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 60 | inline_ BOOL FirstContactEnabled() const { return mFlags & OPC_FIRST_CONTACT; } | ||
| 61 | |||
| 62 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 63 | /** | ||
| 64 | * Gets the temporal coherence mode. | ||
| 65 | * \return true if temporal coherence is on | ||
| 66 | */ | ||
| 67 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 68 | inline_ BOOL TemporalCoherenceEnabled() const { return mFlags & OPC_TEMPORAL_COHERENCE; } | ||
| 69 | |||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | /** | ||
| 72 | * Checks a first contact has already been found. | ||
| 73 | * \return true if a first contact has been found and we can stop a query | ||
| 74 | */ | ||
| 75 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 76 | inline_ BOOL ContactFound() const { return (mFlags&OPC_CONTACT_FOUND)==OPC_CONTACT_FOUND; } | ||
| 77 | |||
| 78 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 79 | /** | ||
| 80 | * Checks there's been an early exit due to temporal coherence; | ||
| 81 | * \return true if a temporal hit has occured | ||
| 82 | */ | ||
| 83 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 84 | inline_ BOOL TemporalHit() const { return mFlags & OPC_TEMPORAL_HIT; } | ||
| 85 | |||
| 86 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 87 | /** | ||
| 88 | * Checks primitive tests are enabled; | ||
| 89 | * \return true if primitive tests must be skipped | ||
| 90 | */ | ||
| 91 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 92 | inline_ BOOL SkipPrimitiveTests() const { return mFlags & OPC_NO_PRIMITIVE_TESTS; } | ||
| 93 | |||
| 94 | // Settings | ||
| 95 | |||
| 96 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 97 | /** | ||
| 98 | * Reports all contacts (false) or first contact only (true) | ||
| 99 | * \param flag [in] true for first contact, false for all contacts | ||
| 100 | * \see SetTemporalCoherence(bool flag) | ||
| 101 | * \see ValidateSettings() | ||
| 102 | */ | ||
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 104 | inline_ void SetFirstContact(bool flag) | ||
| 105 | { | ||
| 106 | if(flag) mFlags |= OPC_FIRST_CONTACT; | ||
| 107 | else mFlags &= ~OPC_FIRST_CONTACT; | ||
| 108 | } | ||
| 109 | |||
| 110 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 111 | /** | ||
| 112 | * Enable/disable temporal coherence. | ||
| 113 | * \param flag [in] true to enable temporal coherence, false to discard it | ||
| 114 | * \see SetFirstContact(bool flag) | ||
| 115 | * \see ValidateSettings() | ||
| 116 | */ | ||
| 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 118 | inline_ void SetTemporalCoherence(bool flag) | ||
| 119 | { | ||
| 120 | if(flag) mFlags |= OPC_TEMPORAL_COHERENCE; | ||
| 121 | else mFlags &= ~OPC_TEMPORAL_COHERENCE; | ||
| 122 | } | ||
| 123 | |||
| 124 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 125 | /** | ||
| 126 | * Enable/disable primitive tests. | ||
| 127 | * \param flag [in] true to enable primitive tests, false to discard them | ||
| 128 | */ | ||
| 129 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 130 | inline_ void SetPrimitiveTests(bool flag) | ||
| 131 | { | ||
| 132 | if(!flag) mFlags |= OPC_NO_PRIMITIVE_TESTS; | ||
| 133 | else mFlags &= ~OPC_NO_PRIMITIVE_TESTS; | ||
| 134 | } | ||
| 135 | |||
| 136 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 137 | /** | ||
| 138 | * Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider. | ||
| 139 | * \return null if everything is ok, else a string describing the problem | ||
| 140 | */ | ||
| 141 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 142 | virtual const char* ValidateSettings() = 0; | ||
| 143 | |||
| 144 | protected: | ||
| 145 | udword mFlags; //!< Bit flags | ||
| 146 | const BaseModel* mCurrentModel; //!< Current model for collision query (owner of touched faces) | ||
| 147 | // User mesh interface | ||
| 148 | const MeshInterface* mIMesh; //!< User-defined mesh interface | ||
| 149 | |||
| 150 | // Internal methods | ||
| 151 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 152 | /** | ||
| 153 | * Setups current collision model | ||
| 154 | * \param model [in] current collision model | ||
| 155 | * \return TRUE if success | ||
| 156 | */ | ||
| 157 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 158 | inline_ BOOL Setup(const BaseModel* model) | ||
| 159 | { | ||
| 160 | // Keep track of current model | ||
| 161 | mCurrentModel = model; | ||
| 162 | if(!mCurrentModel) return FALSE; | ||
| 163 | |||
| 164 | mIMesh = model->GetMeshInterface(); | ||
| 165 | return mIMesh!=null; | ||
| 166 | } | ||
| 167 | |||
| 168 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 169 | /** | ||
| 170 | * Initializes a query | ||
| 171 | */ | ||
| 172 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 173 | virtual inline_ void InitQuery() { mFlags &= ~OPC_TEMPORAL_CONTACT; } | ||
| 174 | }; | ||
| 175 | |||
| 176 | #endif // __OPC_COLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Common.cpp b/libraries/ode-0.9/OPCODE/OPC_Common.cpp new file mode 100644 index 0000000..5b9a9c8 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Common.cpp | |||
| @@ -0,0 +1,48 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains common classes & defs used in OPCODE. | ||
| 12 | * \file OPC_Common.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * An AABB dedicated to collision detection. | ||
| 21 | * We don't use the generic AABB class included in ICE, since it can be a Min/Max or a Center/Extents one (depends | ||
| 22 | * on compilation flags). Since the Center/Extents model is more efficient in collision detection, it was worth | ||
| 23 | * using an extra special class. | ||
| 24 | * | ||
| 25 | * \class CollisionAABB | ||
| 26 | * \author Pierre Terdiman | ||
| 27 | * \version 1.3 | ||
| 28 | * \date March, 20, 2001 | ||
| 29 | */ | ||
| 30 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 31 | |||
| 32 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 33 | /** | ||
| 34 | * A quantized AABB. | ||
| 35 | * Center/Extent model, using 16-bits integers. | ||
| 36 | * | ||
| 37 | * \class QuantizedAABB | ||
| 38 | * \author Pierre Terdiman | ||
| 39 | * \version 1.3 | ||
| 40 | * \date March, 20, 2001 | ||
| 41 | */ | ||
| 42 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 43 | |||
| 44 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 45 | // Precompiled Header | ||
| 46 | #include "Stdafx.h" | ||
| 47 | |||
| 48 | using namespace Opcode; | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Common.h b/libraries/ode-0.9/OPCODE/OPC_Common.h new file mode 100644 index 0000000..f134990 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Common.h | |||
| @@ -0,0 +1,101 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains common classes & defs used in OPCODE. | ||
| 12 | * \file OPC_Common.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_COMMON_H__ | ||
| 21 | #define __OPC_COMMON_H__ | ||
| 22 | |||
| 23 | // [GOTTFRIED]: Just a small change for readability. | ||
| 24 | #ifdef OPC_CPU_COMPARE | ||
| 25 | #define GREATER(x, y) AIR(x) > IR(y) | ||
| 26 | #else | ||
| 27 | #define GREATER(x, y) fabsf(x) > (y) | ||
| 28 | #endif | ||
| 29 | |||
| 30 | class OPCODE_API CollisionAABB | ||
| 31 | { | ||
| 32 | public: | ||
| 33 | //! Constructor | ||
| 34 | inline_ CollisionAABB() {} | ||
| 35 | //! Constructor | ||
| 36 | inline_ CollisionAABB(const AABB& b) { b.GetCenter(mCenter); b.GetExtents(mExtents); } | ||
| 37 | //! Destructor | ||
| 38 | inline_ ~CollisionAABB() {} | ||
| 39 | |||
| 40 | //! Get min point of the box | ||
| 41 | inline_ void GetMin(Point& min) const { min = mCenter - mExtents; } | ||
| 42 | //! Get max point of the box | ||
| 43 | inline_ void GetMax(Point& max) const { max = mCenter + mExtents; } | ||
| 44 | |||
| 45 | //! Get component of the box's min point along a given axis | ||
| 46 | inline_ float GetMin(udword axis) const { return mCenter[axis] - mExtents[axis]; } | ||
| 47 | //! Get component of the box's max point along a given axis | ||
| 48 | inline_ float GetMax(udword axis) const { return mCenter[axis] + mExtents[axis]; } | ||
| 49 | |||
| 50 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 51 | /** | ||
| 52 | * Setups an AABB from min & max vectors. | ||
| 53 | * \param min [in] the min point | ||
| 54 | * \param max [in] the max point | ||
| 55 | */ | ||
| 56 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 57 | inline_ void SetMinMax(const Point& min, const Point& max) { mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f; } | ||
| 58 | |||
| 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 60 | /** | ||
| 61 | * Checks a box is inside another box. | ||
| 62 | * \param box [in] the other box | ||
| 63 | * \return true if current box is inside input box | ||
| 64 | */ | ||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | inline_ BOOL IsInside(const CollisionAABB& box) const | ||
| 67 | { | ||
| 68 | if(box.GetMin(0)>GetMin(0)) return FALSE; | ||
| 69 | if(box.GetMin(1)>GetMin(1)) return FALSE; | ||
| 70 | if(box.GetMin(2)>GetMin(2)) return FALSE; | ||
| 71 | if(box.GetMax(0)<GetMax(0)) return FALSE; | ||
| 72 | if(box.GetMax(1)<GetMax(1)) return FALSE; | ||
| 73 | if(box.GetMax(2)<GetMax(2)) return FALSE; | ||
| 74 | return TRUE; | ||
| 75 | } | ||
| 76 | |||
| 77 | Point mCenter; //!< Box center | ||
| 78 | Point mExtents; //!< Box extents | ||
| 79 | }; | ||
| 80 | |||
| 81 | class OPCODE_API QuantizedAABB | ||
| 82 | { | ||
| 83 | public: | ||
| 84 | //! Constructor | ||
| 85 | inline_ QuantizedAABB() {} | ||
| 86 | //! Destructor | ||
| 87 | inline_ ~QuantizedAABB() {} | ||
| 88 | |||
| 89 | sword mCenter[3]; //!< Quantized center | ||
| 90 | uword mExtents[3]; //!< Quantized extents | ||
| 91 | }; | ||
| 92 | |||
| 93 | //! Quickly rotates & translates a vector | ||
| 94 | inline_ void TransformPoint(Point& dest, const Point& source, const Matrix3x3& rot, const Point& trans) | ||
| 95 | { | ||
| 96 | dest.x = trans.x + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0]; | ||
| 97 | dest.y = trans.y + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1]; | ||
| 98 | dest.z = trans.z + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2]; | ||
| 99 | } | ||
| 100 | |||
| 101 | #endif //__OPC_COMMON_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_HybridModel.cpp b/libraries/ode-0.9/OPCODE/OPC_HybridModel.cpp new file mode 100644 index 0000000..7016219 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_HybridModel.cpp | |||
| @@ -0,0 +1,466 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for hybrid models. | ||
| 12 | * \file OPC_HybridModel.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date May, 18, 2003 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * An hybrid collision model. | ||
| 21 | * | ||
| 22 | * The problem : | ||
| 23 | * | ||
| 24 | * Opcode really shines for mesh-mesh collision, especially when meshes are deeply overlapping | ||
| 25 | * (it typically outperforms RAPID in those cases). | ||
| 26 | * | ||
| 27 | * Unfortunately this is not the typical scenario in games. | ||
| 28 | * | ||
| 29 | * For close-proximity cases, especially for volume-mesh queries, it's relatively easy to run faster | ||
| 30 | * than Opcode, that suffers from a relatively high setup time. | ||
| 31 | * | ||
| 32 | * In particular, Opcode's "vanilla" trees in those cases -can- run faster. They can also use -less- | ||
| 33 | * memory than the optimized ones, when you let the system stop at ~10 triangles / leaf for example | ||
| 34 | * (i.e. when you don't use "complete" trees). However, those trees tend to fragment memory quite a | ||
| 35 | * lot, increasing cache misses : since they're not "complete", we can't predict the final number of | ||
| 36 | * nodes and we have to allocate nodes on-the-fly. For the same reasons we can't use Opcode's "optimized" | ||
| 37 | * trees here, since they rely on a known layout to perform the "optimization". | ||
| 38 | * | ||
| 39 | * Hybrid trees : | ||
| 40 | * | ||
| 41 | * Hybrid trees try to combine best of both worlds : | ||
| 42 | * | ||
| 43 | * - they use a maximum limit of 16 triangles/leaf. "16" is used so that we'll be able to save the | ||
| 44 | * number of triangles using 4 bits only. | ||
| 45 | * | ||
| 46 | * - they're still "complete" trees thanks to a two-passes building phase. First we create a "vanilla" | ||
| 47 | * AABB-tree with Opcode, limited to 16 triangles/leaf. Then we create a *second* vanilla tree, this | ||
| 48 | * time using the leaves of the first one. The trick is : this second tree is now "complete"... so we | ||
| 49 | * can further transform it into an Opcode's optimized tree. | ||
| 50 | * | ||
| 51 | * - then we run the collision queries on that standard Opcode tree. The only difference is that leaf | ||
| 52 | * nodes contain indices to leaf nodes of another tree. Also, we have to skip all primitive tests in | ||
| 53 | * Opcode optimized trees, since our leaves don't contain triangles anymore. | ||
| 54 | * | ||
| 55 | * - finally, for each collided leaf, we simply loop through 16 triangles max, and collide them with | ||
| 56 | * the bounding volume used in the query (we only support volume-vs-mesh queries here, not mesh-vs-mesh) | ||
| 57 | * | ||
| 58 | * All of that is wrapped in this "hybrid model" that contains the minimal data required for this to work. | ||
| 59 | * It's a mix between old "vanilla" trees, and old "optimized" trees. | ||
| 60 | * | ||
| 61 | * Extra advantages: | ||
| 62 | * | ||
| 63 | * - If we use them for dynamic models, we're left with a very small number of leaf nodes to refit. It | ||
| 64 | * might be a bit faster since we have less nodes to write back. | ||
| 65 | * | ||
| 66 | * - In rigid body simulation, using temporal coherence and sleeping objects greatly reduce the actual | ||
| 67 | * influence of one tree over another (i.e. the speed difference is often invisible). So memory is really | ||
| 68 | * the key element to consider, and in this regard hybrid trees are just better. | ||
| 69 | * | ||
| 70 | * Information to take home: | ||
| 71 | * - they use less ram | ||
| 72 | * - they're not slower (they're faster or slower depending on cases, overall there's no significant | ||
| 73 | * difference *as long as objects don't interpenetrate too much* - in which case Opcode's optimized trees | ||
| 74 | * are still notably faster) | ||
| 75 | * | ||
| 76 | * \class HybridModel | ||
| 77 | * \author Pierre Terdiman | ||
| 78 | * \version 1.3 | ||
| 79 | * \date May, 18, 2003 | ||
| 80 | */ | ||
| 81 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 82 | |||
| 83 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 84 | // Precompiled Header | ||
| 85 | #include "Stdafx.h" | ||
| 86 | |||
| 87 | using namespace Opcode; | ||
| 88 | |||
| 89 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 90 | /** | ||
| 91 | * Constructor. | ||
| 92 | */ | ||
| 93 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 94 | HybridModel::HybridModel() : | ||
| 95 | mNbLeaves (0), | ||
| 96 | mNbPrimitives (0), | ||
| 97 | mTriangles (null), | ||
| 98 | mIndices (null) | ||
| 99 | { | ||
| 100 | } | ||
| 101 | |||
| 102 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 103 | /** | ||
| 104 | * Destructor. | ||
| 105 | */ | ||
| 106 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 107 | HybridModel::~HybridModel() | ||
| 108 | { | ||
| 109 | Release(); | ||
| 110 | } | ||
| 111 | |||
| 112 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 113 | /** | ||
| 114 | * Releases everything. | ||
| 115 | */ | ||
| 116 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 117 | void HybridModel::Release() | ||
| 118 | { | ||
| 119 | ReleaseBase(); | ||
| 120 | DELETEARRAY(mIndices); | ||
| 121 | DELETEARRAY(mTriangles); | ||
| 122 | mNbLeaves = 0; | ||
| 123 | mNbPrimitives = 0; | ||
| 124 | } | ||
| 125 | |||
| 126 | struct Internal | ||
| 127 | { | ||
| 128 | Internal() | ||
| 129 | { | ||
| 130 | mNbLeaves = 0; | ||
| 131 | mLeaves = null; | ||
| 132 | mTriangles = null; | ||
| 133 | mBase = null; | ||
| 134 | } | ||
| 135 | ~Internal() | ||
| 136 | { | ||
| 137 | DELETEARRAY(mLeaves); | ||
| 138 | } | ||
| 139 | |||
| 140 | udword mNbLeaves; | ||
| 141 | AABB* mLeaves; | ||
| 142 | LeafTriangles* mTriangles; | ||
| 143 | const udword* mBase; | ||
| 144 | }; | ||
| 145 | |||
| 146 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 147 | /** | ||
| 148 | * Builds a collision model. | ||
| 149 | * \param create [in] model creation structure | ||
| 150 | * \return true if success | ||
| 151 | */ | ||
| 152 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 153 | bool HybridModel::Build(const OPCODECREATE& create) | ||
| 154 | { | ||
| 155 | // 1) Checkings | ||
| 156 | if(!create.mIMesh || !create.mIMesh->IsValid()) return false; | ||
| 157 | |||
| 158 | // Look for degenerate faces. | ||
| 159 | udword NbDegenerate = create.mIMesh->CheckTopology(); | ||
| 160 | if(NbDegenerate) Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate); | ||
| 161 | // We continue nonetheless.... | ||
| 162 | |||
| 163 | Release(); // Make sure previous tree has been discarded | ||
| 164 | |||
| 165 | // 1-1) Setup mesh interface automatically | ||
| 166 | SetMeshInterface(create.mIMesh); | ||
| 167 | |||
| 168 | bool Status = false; | ||
| 169 | AABBTree* LeafTree = null; | ||
| 170 | Internal Data; | ||
| 171 | |||
| 172 | // 2) Build a generic AABB Tree. | ||
| 173 | mSource = new AABBTree; | ||
| 174 | CHECKALLOC(mSource); | ||
| 175 | |||
| 176 | // 2-1) Setup a builder. Our primitives here are triangles from input mesh, | ||
| 177 | // so we use an AABBTreeOfTrianglesBuilder..... | ||
| 178 | { | ||
| 179 | AABBTreeOfTrianglesBuilder TB; | ||
| 180 | TB.mIMesh = create.mIMesh; | ||
| 181 | TB.mNbPrimitives = create.mIMesh->GetNbTriangles(); | ||
| 182 | TB.mSettings = create.mSettings; | ||
| 183 | TB.mSettings.mLimit = 16; // ### Hardcoded, but maybe we could let the user choose 8 / 16 / 32 ... | ||
| 184 | if(!mSource->Build(&TB)) goto FreeAndExit; | ||
| 185 | } | ||
| 186 | |||
| 187 | // 2-2) Here's the trick : create *another* AABB tree using the leaves of the first one (which are boxes, this time) | ||
| 188 | struct Local | ||
| 189 | { | ||
| 190 | // A callback to count leaf nodes | ||
| 191 | static bool CountLeaves(const AABBTreeNode* current, udword depth, void* user_data) | ||
| 192 | { | ||
| 193 | if(current->IsLeaf()) | ||
| 194 | { | ||
| 195 | Internal* Data = (Internal*)user_data; | ||
| 196 | Data->mNbLeaves++; | ||
| 197 | } | ||
| 198 | return true; | ||
| 199 | } | ||
| 200 | |||
| 201 | // A callback to setup leaf nodes in our internal structures | ||
| 202 | static bool SetupLeafData(const AABBTreeNode* current, udword depth, void* user_data) | ||
| 203 | { | ||
| 204 | if(current->IsLeaf()) | ||
| 205 | { | ||
| 206 | Internal* Data = (Internal*)user_data; | ||
| 207 | |||
| 208 | // Get current leaf's box | ||
| 209 | Data->mLeaves[Data->mNbLeaves] = *current->GetAABB(); | ||
| 210 | |||
| 211 | // Setup leaf data | ||
| 212 | udword Index = udword((size_t(current->GetPrimitives()) - size_t(Data->mBase)) / sizeof(udword)); | ||
| 213 | Data->mTriangles[Data->mNbLeaves].SetData(current->GetNbPrimitives(), Index); | ||
| 214 | |||
| 215 | Data->mNbLeaves++; | ||
| 216 | } | ||
| 217 | return true; | ||
| 218 | } | ||
| 219 | }; | ||
| 220 | |||
| 221 | // Walk the tree & count number of leaves | ||
| 222 | Data.mNbLeaves = 0; | ||
| 223 | mSource->Walk(Local::CountLeaves, &Data); | ||
| 224 | mNbLeaves = Data.mNbLeaves; // Keep track of it | ||
| 225 | |||
| 226 | // Special case for 1-leaf meshes | ||
| 227 | if(mNbLeaves==1) | ||
| 228 | { | ||
| 229 | mModelCode |= OPC_SINGLE_NODE; | ||
| 230 | Status = true; | ||
| 231 | goto FreeAndExit; | ||
| 232 | } | ||
| 233 | |||
| 234 | // Allocate our structures | ||
| 235 | Data.mLeaves = new AABB[Data.mNbLeaves]; CHECKALLOC(Data.mLeaves); | ||
| 236 | mTriangles = new LeafTriangles[Data.mNbLeaves]; CHECKALLOC(mTriangles); | ||
| 237 | |||
| 238 | // Walk the tree again & setup leaf data | ||
| 239 | Data.mTriangles = mTriangles; | ||
| 240 | Data.mBase = mSource->GetIndices(); | ||
| 241 | Data.mNbLeaves = 0; // Reset for incoming walk | ||
| 242 | mSource->Walk(Local::SetupLeafData, &Data); | ||
| 243 | |||
| 244 | // Handle source indices | ||
| 245 | { | ||
| 246 | bool MustKeepIndices = true; | ||
| 247 | if(create.mCanRemap) | ||
| 248 | { | ||
| 249 | // We try to get rid of source indices (saving more ram!) by reorganizing triangle arrays... | ||
| 250 | // Remap can fail when we use callbacks => keep track of indices in that case (it still | ||
| 251 | // works, only using more memory) | ||
| 252 | if(create.mIMesh->RemapClient(mSource->GetNbPrimitives(), mSource->GetIndices())) | ||
| 253 | { | ||
| 254 | MustKeepIndices = false; | ||
| 255 | } | ||
| 256 | } | ||
| 257 | |||
| 258 | if(MustKeepIndices) | ||
| 259 | { | ||
| 260 | // Keep track of source indices (from vanilla tree) | ||
| 261 | mNbPrimitives = mSource->GetNbPrimitives(); | ||
| 262 | mIndices = new udword[mNbPrimitives]; | ||
| 263 | CopyMemory(mIndices, mSource->GetIndices(), mNbPrimitives*sizeof(udword)); | ||
| 264 | } | ||
| 265 | } | ||
| 266 | |||
| 267 | // Now, create our optimized tree using previous leaf nodes | ||
| 268 | LeafTree = new AABBTree; | ||
| 269 | CHECKALLOC(LeafTree); | ||
| 270 | { | ||
| 271 | AABBTreeOfAABBsBuilder TB; // Now using boxes ! | ||
| 272 | TB.mSettings = create.mSettings; | ||
| 273 | TB.mSettings.mLimit = 1; // We now want a complete tree so that we can "optimize" it | ||
| 274 | TB.mNbPrimitives = Data.mNbLeaves; | ||
| 275 | TB.mAABBArray = Data.mLeaves; | ||
| 276 | if(!LeafTree->Build(&TB)) goto FreeAndExit; | ||
| 277 | } | ||
| 278 | |||
| 279 | // 3) Create an optimized tree according to user-settings | ||
| 280 | if(!CreateTree(create.mNoLeaf, create.mQuantized)) goto FreeAndExit; | ||
| 281 | |||
| 282 | // 3-2) Create optimized tree | ||
| 283 | if(!mTree->Build(LeafTree)) goto FreeAndExit; | ||
| 284 | |||
| 285 | // Finally ok... | ||
| 286 | Status = true; | ||
| 287 | |||
| 288 | FreeAndExit: // Allow me this one... | ||
| 289 | DELETESINGLE(LeafTree); | ||
| 290 | |||
| 291 | // 3-3) Delete generic tree if needed | ||
| 292 | if(!create.mKeepOriginal) DELETESINGLE(mSource); | ||
| 293 | |||
| 294 | return Status; | ||
| 295 | } | ||
| 296 | |||
| 297 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 298 | /** | ||
| 299 | * Gets the number of bytes used by the tree. | ||
| 300 | * \return amount of bytes used | ||
| 301 | */ | ||
| 302 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 303 | udword HybridModel::GetUsedBytes() const | ||
| 304 | { | ||
| 305 | udword UsedBytes = 0; | ||
| 306 | if(mTree) UsedBytes += mTree->GetUsedBytes(); | ||
| 307 | if(mIndices) UsedBytes += mNbPrimitives * sizeof(udword); // mIndices | ||
| 308 | if(mTriangles) UsedBytes += mNbLeaves * sizeof(LeafTriangles); // mTriangles | ||
| 309 | return UsedBytes; | ||
| 310 | } | ||
| 311 | |||
| 312 | inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp) | ||
| 313 | { | ||
| 314 | // Compute triangle's AABB = a leaf box | ||
| 315 | #ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much | ||
| 316 | min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); | ||
| 317 | max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); | ||
| 318 | |||
| 319 | min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); | ||
| 320 | max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); | ||
| 321 | |||
| 322 | min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); | ||
| 323 | max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); | ||
| 324 | #else | ||
| 325 | min = *vp.Vertex[0]; | ||
| 326 | max = *vp.Vertex[0]; | ||
| 327 | min.Min(*vp.Vertex[1]); | ||
| 328 | max.Max(*vp.Vertex[1]); | ||
| 329 | min.Min(*vp.Vertex[2]); | ||
| 330 | max.Max(*vp.Vertex[2]); | ||
| 331 | #endif | ||
| 332 | } | ||
| 333 | |||
| 334 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 335 | /** | ||
| 336 | * Refits the collision model. This can be used to handle dynamic meshes. Usage is: | ||
| 337 | * 1. modify your mesh vertices (keep the topology constant!) | ||
| 338 | * 2. refit the tree (call this method) | ||
| 339 | * \return true if success | ||
| 340 | */ | ||
| 341 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 342 | bool HybridModel::Refit() | ||
| 343 | { | ||
| 344 | if(!mIMesh) return false; | ||
| 345 | if(!mTree) return false; | ||
| 346 | |||
| 347 | if(IsQuantized()) return false; | ||
| 348 | if(HasLeafNodes()) return false; | ||
| 349 | |||
| 350 | const LeafTriangles* LT = GetLeafTriangles(); | ||
| 351 | const udword* Indices = GetIndices(); | ||
| 352 | |||
| 353 | // Bottom-up update | ||
| 354 | VertexPointers VP; | ||
| 355 | Point Min,Max; | ||
| 356 | Point Min_,Max_; | ||
| 357 | udword Index = mTree->GetNbNodes(); | ||
| 358 | AABBNoLeafNode* Nodes = (AABBNoLeafNode*)((AABBNoLeafTree*)mTree)->GetNodes(); | ||
| 359 | while(Index--) | ||
| 360 | { | ||
| 361 | AABBNoLeafNode& Current = Nodes[Index]; | ||
| 362 | |||
| 363 | if(Current.HasPosLeaf()) | ||
| 364 | { | ||
| 365 | const LeafTriangles& CurrentLeaf = LT[Current.GetPosPrimitive()]; | ||
| 366 | |||
| 367 | Min.SetPlusInfinity(); | ||
| 368 | Max.SetMinusInfinity(); | ||
| 369 | |||
| 370 | Point TmpMin, TmpMax; | ||
| 371 | |||
| 372 | // Each leaf box has a set of triangles | ||
| 373 | udword NbTris = CurrentLeaf.GetNbTriangles(); | ||
| 374 | if(Indices) | ||
| 375 | { | ||
| 376 | const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; | ||
| 377 | |||
| 378 | // Loop through triangles and test each of them | ||
| 379 | while(NbTris--) | ||
| 380 | { | ||
| 381 | mIMesh->GetTriangle(VP, *T++); | ||
| 382 | ComputeMinMax(TmpMin, TmpMax, VP); | ||
| 383 | Min.Min(TmpMin); | ||
| 384 | Max.Max(TmpMax); | ||
| 385 | } | ||
| 386 | } | ||
| 387 | else | ||
| 388 | { | ||
| 389 | udword BaseIndex = CurrentLeaf.GetTriangleIndex(); | ||
| 390 | |||
| 391 | // Loop through triangles and test each of them | ||
| 392 | while(NbTris--) | ||
| 393 | { | ||
| 394 | mIMesh->GetTriangle(VP, BaseIndex++); | ||
| 395 | ComputeMinMax(TmpMin, TmpMax, VP); | ||
| 396 | Min.Min(TmpMin); | ||
| 397 | Max.Max(TmpMax); | ||
| 398 | } | ||
| 399 | } | ||
| 400 | } | ||
| 401 | else | ||
| 402 | { | ||
| 403 | const CollisionAABB& CurrentBox = Current.GetPos()->mAABB; | ||
| 404 | CurrentBox.GetMin(Min); | ||
| 405 | CurrentBox.GetMax(Max); | ||
| 406 | } | ||
| 407 | |||
| 408 | if(Current.HasNegLeaf()) | ||
| 409 | { | ||
| 410 | const LeafTriangles& CurrentLeaf = LT[Current.GetNegPrimitive()]; | ||
| 411 | |||
| 412 | Min_.SetPlusInfinity(); | ||
| 413 | Max_.SetMinusInfinity(); | ||
| 414 | |||
| 415 | Point TmpMin, TmpMax; | ||
| 416 | |||
| 417 | // Each leaf box has a set of triangles | ||
| 418 | udword NbTris = CurrentLeaf.GetNbTriangles(); | ||
| 419 | if(Indices) | ||
| 420 | { | ||
| 421 | const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; | ||
| 422 | |||
| 423 | // Loop through triangles and test each of them | ||
| 424 | while(NbTris--) | ||
| 425 | { | ||
| 426 | mIMesh->GetTriangle(VP, *T++); | ||
| 427 | ComputeMinMax(TmpMin, TmpMax, VP); | ||
| 428 | Min_.Min(TmpMin); | ||
| 429 | Max_.Max(TmpMax); | ||
| 430 | } | ||
| 431 | } | ||
| 432 | else | ||
| 433 | { | ||
| 434 | udword BaseIndex = CurrentLeaf.GetTriangleIndex(); | ||
| 435 | |||
| 436 | // Loop through triangles and test each of them | ||
| 437 | while(NbTris--) | ||
| 438 | { | ||
| 439 | mIMesh->GetTriangle(VP, BaseIndex++); | ||
| 440 | ComputeMinMax(TmpMin, TmpMax, VP); | ||
| 441 | Min_.Min(TmpMin); | ||
| 442 | Max_.Max(TmpMax); | ||
| 443 | } | ||
| 444 | } | ||
| 445 | } | ||
| 446 | else | ||
| 447 | { | ||
| 448 | const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB; | ||
| 449 | CurrentBox.GetMin(Min_); | ||
| 450 | CurrentBox.GetMax(Max_); | ||
| 451 | } | ||
| 452 | #ifdef OPC_USE_FCOMI | ||
| 453 | Min.x = FCMin2(Min.x, Min_.x); | ||
| 454 | Max.x = FCMax2(Max.x, Max_.x); | ||
| 455 | Min.y = FCMin2(Min.y, Min_.y); | ||
| 456 | Max.y = FCMax2(Max.y, Max_.y); | ||
| 457 | Min.z = FCMin2(Min.z, Min_.z); | ||
| 458 | Max.z = FCMax2(Max.z, Max_.z); | ||
| 459 | #else | ||
| 460 | Min.Min(Min_); | ||
| 461 | Max.Max(Max_); | ||
| 462 | #endif | ||
| 463 | Current.mAABB.SetMinMax(Min, Max); | ||
| 464 | } | ||
| 465 | return true; | ||
| 466 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_HybridModel.h b/libraries/ode-0.9/OPCODE/OPC_HybridModel.h new file mode 100644 index 0000000..c7eb59d --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_HybridModel.h | |||
| @@ -0,0 +1,106 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for hybrid models. | ||
| 12 | * \file OPC_HybridModel.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date May, 18, 2003 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_HYBRIDMODEL_H__ | ||
| 21 | #define __OPC_HYBRIDMODEL_H__ | ||
| 22 | |||
| 23 | //! Leaf descriptor | ||
| 24 | struct LeafTriangles | ||
| 25 | { | ||
| 26 | udword Data; //!< Packed data | ||
| 27 | |||
| 28 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 29 | /** | ||
| 30 | * Gets number of triangles in the leaf. | ||
| 31 | * \return number of triangles N, with 0 < N <= 16 | ||
| 32 | */ | ||
| 33 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 34 | inline_ udword GetNbTriangles() const { return (Data & 15)+1; } | ||
| 35 | |||
| 36 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 37 | /** | ||
| 38 | * Gets triangle index for this leaf. Indexed model's array of indices retrieved with HybridModel::GetIndices() | ||
| 39 | * \return triangle index | ||
| 40 | */ | ||
| 41 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 42 | inline_ udword GetTriangleIndex() const { return Data>>4; } | ||
| 43 | inline_ void SetData(udword nb, udword index) { ASSERT(nb>0 && nb<=16); nb--; Data = (index<<4)|(nb&15); } | ||
| 44 | }; | ||
| 45 | |||
| 46 | class OPCODE_API HybridModel : public BaseModel | ||
| 47 | { | ||
| 48 | public: | ||
| 49 | // Constructor/Destructor | ||
| 50 | HybridModel(); | ||
| 51 | virtual ~HybridModel(); | ||
| 52 | |||
| 53 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 54 | /** | ||
| 55 | * Builds a collision model. | ||
| 56 | * \param create [in] model creation structure | ||
| 57 | * \return true if success | ||
| 58 | */ | ||
| 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 60 | override(BaseModel) bool Build(const OPCODECREATE& create); | ||
| 61 | |||
| 62 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 63 | /** | ||
| 64 | * Gets the number of bytes used by the tree. | ||
| 65 | * \return amount of bytes used | ||
| 66 | */ | ||
| 67 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 68 | override(BaseModel) udword GetUsedBytes() const; | ||
| 69 | |||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | /** | ||
| 72 | * Refits the collision model. This can be used to handle dynamic meshes. Usage is: | ||
| 73 | * 1. modify your mesh vertices (keep the topology constant!) | ||
| 74 | * 2. refit the tree (call this method) | ||
| 75 | * \return true if success | ||
| 76 | */ | ||
| 77 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 78 | override(BaseModel) bool Refit(); | ||
| 79 | |||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | /** | ||
| 82 | * Gets array of triangles. | ||
| 83 | * \return array of triangles | ||
| 84 | */ | ||
| 85 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 86 | inline_ const LeafTriangles* GetLeafTriangles() const { return mTriangles; } | ||
| 87 | |||
| 88 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 89 | /** | ||
| 90 | * Gets array of indices. | ||
| 91 | * \return array of indices | ||
| 92 | */ | ||
| 93 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 94 | inline_ const udword* GetIndices() const { return mIndices; } | ||
| 95 | |||
| 96 | private: | ||
| 97 | udword mNbLeaves; //!< Number of leaf nodes in the model | ||
| 98 | LeafTriangles* mTriangles; //!< Array of mNbLeaves leaf descriptors | ||
| 99 | udword mNbPrimitives; //!< Number of primitives in the model | ||
| 100 | udword* mIndices; //!< Array of primitive indices | ||
| 101 | |||
| 102 | // Internal methods | ||
| 103 | void Release(); | ||
| 104 | }; | ||
| 105 | |||
| 106 | #endif // __OPC_HYBRIDMODEL_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_IceHook.h b/libraries/ode-0.9/OPCODE/OPC_IceHook.h new file mode 100644 index 0000000..62343b8 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_IceHook.h | |||
| @@ -0,0 +1,70 @@ | |||
| 1 | |||
| 2 | // Should be included by Opcode.h if needed | ||
| 3 | |||
| 4 | #define ICE_DONT_CHECK_COMPILER_OPTIONS | ||
| 5 | |||
| 6 | // From Windows... | ||
| 7 | typedef int BOOL; | ||
| 8 | #ifndef FALSE | ||
| 9 | #define FALSE 0 | ||
| 10 | #endif | ||
| 11 | |||
| 12 | #ifndef TRUE | ||
| 13 | #define TRUE 1 | ||
| 14 | #endif | ||
| 15 | |||
| 16 | #include <stdio.h> | ||
| 17 | #include <stdlib.h> | ||
| 18 | #include <assert.h> | ||
| 19 | #include <string.h> | ||
| 20 | #include <float.h> | ||
| 21 | #include <math.h> | ||
| 22 | |||
| 23 | #ifndef ASSERT | ||
| 24 | #define ASSERT(exp) {} | ||
| 25 | #endif | ||
| 26 | #define ICE_COMPILE_TIME_ASSERT(exp) extern char ICE_Dummy[ (exp) ? 1 : -1 ] | ||
| 27 | |||
| 28 | #define Log {} | ||
| 29 | #define SetIceError(a,b) false | ||
| 30 | #define EC_OUTOFMEMORY "Out of memory" | ||
| 31 | |||
| 32 | #include "Ice/IcePreprocessor.h" | ||
| 33 | |||
| 34 | #undef ICECORE_API | ||
| 35 | #define ICECORE_API OPCODE_API | ||
| 36 | |||
| 37 | #include "Ice/IceTypes.h" | ||
| 38 | #include "Ice/IceFPU.h" | ||
| 39 | #include "Ice/IceMemoryMacros.h" | ||
| 40 | |||
| 41 | namespace IceCore | ||
| 42 | { | ||
| 43 | #include "Ice/IceUtils.h" | ||
| 44 | #include "Ice/IceContainer.h" | ||
| 45 | #include "Ice/IcePairs.h" | ||
| 46 | #include "Ice/IceRevisitedRadix.h" | ||
| 47 | #include "Ice/IceRandom.h" | ||
| 48 | } | ||
| 49 | using namespace IceCore; | ||
| 50 | |||
| 51 | #define ICEMATHS_API OPCODE_API | ||
| 52 | namespace IceMaths | ||
| 53 | { | ||
| 54 | #include "Ice/IceAxes.h" | ||
| 55 | #include "Ice/IcePoint.h" | ||
| 56 | #include "Ice/IceHPoint.h" | ||
| 57 | #include "Ice/IceMatrix3x3.h" | ||
| 58 | #include "Ice/IceMatrix4x4.h" | ||
| 59 | #include "Ice/IcePlane.h" | ||
| 60 | #include "Ice/IceRay.h" | ||
| 61 | #include "Ice/IceIndexedTriangle.h" | ||
| 62 | #include "Ice/IceTriangle.h" | ||
| 63 | #include "Ice/IceTriList.h" | ||
| 64 | #include "Ice/IceAABB.h" | ||
| 65 | #include "Ice/IceOBB.h" | ||
| 66 | #include "Ice/IceBoundingSphere.h" | ||
| 67 | #include "Ice/IceSegment.h" | ||
| 68 | #include "Ice/IceLSS.h" | ||
| 69 | } | ||
| 70 | using namespace IceMaths; | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h b/libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h new file mode 100644 index 0000000..5cc50b5 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_LSSAABBOverlap.h | |||
| @@ -0,0 +1,523 @@ | |||
| 1 | |||
| 2 | // Following code from Magic-Software (http://www.magic-software.com/) | ||
| 3 | // A bit modified for Opcode | ||
| 4 | |||
| 5 | inline_ float OPC_PointAABBSqrDist(const Point& point, const Point& center, const Point& extents) | ||
| 6 | { | ||
| 7 | // Compute coordinates of point in box coordinate system | ||
| 8 | Point Closest = point - center; | ||
| 9 | |||
| 10 | float SqrDistance = 0.0f; | ||
| 11 | |||
| 12 | if(Closest.x < -extents.x) | ||
| 13 | { | ||
| 14 | float Delta = Closest.x + extents.x; | ||
| 15 | SqrDistance += Delta*Delta; | ||
| 16 | } | ||
| 17 | else if(Closest.x > extents.x) | ||
| 18 | { | ||
| 19 | float Delta = Closest.x - extents.x; | ||
| 20 | SqrDistance += Delta*Delta; | ||
| 21 | } | ||
| 22 | |||
| 23 | if(Closest.y < -extents.y) | ||
| 24 | { | ||
| 25 | float Delta = Closest.y + extents.y; | ||
| 26 | SqrDistance += Delta*Delta; | ||
| 27 | } | ||
| 28 | else if(Closest.y > extents.y) | ||
| 29 | { | ||
| 30 | float Delta = Closest.y - extents.y; | ||
| 31 | SqrDistance += Delta*Delta; | ||
| 32 | } | ||
| 33 | |||
| 34 | if(Closest.z < -extents.z) | ||
| 35 | { | ||
| 36 | float Delta = Closest.z + extents.z; | ||
| 37 | SqrDistance += Delta*Delta; | ||
| 38 | } | ||
| 39 | else if(Closest.z > extents.z) | ||
| 40 | { | ||
| 41 | float Delta = Closest.z - extents.z; | ||
| 42 | SqrDistance += Delta*Delta; | ||
| 43 | } | ||
| 44 | return SqrDistance; | ||
| 45 | } | ||
| 46 | |||
| 47 | static void Face(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, const Point& rkPmE, float* pfLParam, float& rfSqrDistance) | ||
| 48 | { | ||
| 49 | Point kPpE; | ||
| 50 | float fLSqr, fInv, fTmp, fParam, fT, fDelta; | ||
| 51 | |||
| 52 | kPpE[i1] = rkPnt[i1] + extents[i1]; | ||
| 53 | kPpE[i2] = rkPnt[i2] + extents[i2]; | ||
| 54 | if(rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0]) | ||
| 55 | { | ||
| 56 | if(rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0]) | ||
| 57 | { | ||
| 58 | // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0) | ||
| 59 | if(pfLParam) | ||
| 60 | { | ||
| 61 | rkPnt[i0] = extents[i0]; | ||
| 62 | fInv = 1.0f/rkDir[i0]; | ||
| 63 | rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv; | ||
| 64 | rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv; | ||
| 65 | *pfLParam = -rkPmE[i0]*fInv; | ||
| 66 | } | ||
| 67 | } | ||
| 68 | else | ||
| 69 | { | ||
| 70 | // v[i1] >= -e[i1], v[i2] < -e[i2] | ||
| 71 | fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2]; | ||
| 72 | fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]); | ||
| 73 | if(fTmp <= 2.0f*fLSqr*extents[i1]) | ||
| 74 | { | ||
| 75 | fT = fTmp/fLSqr; | ||
| 76 | fLSqr += rkDir[i1]*rkDir[i1]; | ||
| 77 | fTmp = kPpE[i1] - fT; | ||
| 78 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2]; | ||
| 79 | fParam = -fDelta/fLSqr; | ||
| 80 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam; | ||
| 81 | |||
| 82 | if(pfLParam) | ||
| 83 | { | ||
| 84 | *pfLParam = fParam; | ||
| 85 | rkPnt[i0] = extents[i0]; | ||
| 86 | rkPnt[i1] = fT - extents[i1]; | ||
| 87 | rkPnt[i2] = -extents[i2]; | ||
| 88 | } | ||
| 89 | } | ||
| 90 | else | ||
| 91 | { | ||
| 92 | fLSqr += rkDir[i1]*rkDir[i1]; | ||
| 93 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2]; | ||
| 94 | fParam = -fDelta/fLSqr; | ||
| 95 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam; | ||
| 96 | |||
| 97 | if(pfLParam) | ||
| 98 | { | ||
| 99 | *pfLParam = fParam; | ||
| 100 | rkPnt[i0] = extents[i0]; | ||
| 101 | rkPnt[i1] = extents[i1]; | ||
| 102 | rkPnt[i2] = -extents[i2]; | ||
| 103 | } | ||
| 104 | } | ||
| 105 | } | ||
| 106 | } | ||
| 107 | else | ||
| 108 | { | ||
| 109 | if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] ) | ||
| 110 | { | ||
| 111 | // v[i1] < -e[i1], v[i2] >= -e[i2] | ||
| 112 | fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]; | ||
| 113 | fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]); | ||
| 114 | if(fTmp <= 2.0f*fLSqr*extents[i2]) | ||
| 115 | { | ||
| 116 | fT = fTmp/fLSqr; | ||
| 117 | fLSqr += rkDir[i2]*rkDir[i2]; | ||
| 118 | fTmp = kPpE[i2] - fT; | ||
| 119 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp; | ||
| 120 | fParam = -fDelta/fLSqr; | ||
| 121 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam; | ||
| 122 | |||
| 123 | if(pfLParam) | ||
| 124 | { | ||
| 125 | *pfLParam = fParam; | ||
| 126 | rkPnt[i0] = extents[i0]; | ||
| 127 | rkPnt[i1] = -extents[i1]; | ||
| 128 | rkPnt[i2] = fT - extents[i2]; | ||
| 129 | } | ||
| 130 | } | ||
| 131 | else | ||
| 132 | { | ||
| 133 | fLSqr += rkDir[i2]*rkDir[i2]; | ||
| 134 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2]; | ||
| 135 | fParam = -fDelta/fLSqr; | ||
| 136 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam; | ||
| 137 | |||
| 138 | if(pfLParam) | ||
| 139 | { | ||
| 140 | *pfLParam = fParam; | ||
| 141 | rkPnt[i0] = extents[i0]; | ||
| 142 | rkPnt[i1] = -extents[i1]; | ||
| 143 | rkPnt[i2] = extents[i2]; | ||
| 144 | } | ||
| 145 | } | ||
| 146 | } | ||
| 147 | else | ||
| 148 | { | ||
| 149 | // v[i1] < -e[i1], v[i2] < -e[i2] | ||
| 150 | fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2]; | ||
| 151 | fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]); | ||
| 152 | if(fTmp >= 0.0f) | ||
| 153 | { | ||
| 154 | // v[i1]-edge is closest | ||
| 155 | if ( fTmp <= 2.0f*fLSqr*extents[i1] ) | ||
| 156 | { | ||
| 157 | fT = fTmp/fLSqr; | ||
| 158 | fLSqr += rkDir[i1]*rkDir[i1]; | ||
| 159 | fTmp = kPpE[i1] - fT; | ||
| 160 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2]; | ||
| 161 | fParam = -fDelta/fLSqr; | ||
| 162 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam; | ||
| 163 | |||
| 164 | if(pfLParam) | ||
| 165 | { | ||
| 166 | *pfLParam = fParam; | ||
| 167 | rkPnt[i0] = extents[i0]; | ||
| 168 | rkPnt[i1] = fT - extents[i1]; | ||
| 169 | rkPnt[i2] = -extents[i2]; | ||
| 170 | } | ||
| 171 | } | ||
| 172 | else | ||
| 173 | { | ||
| 174 | fLSqr += rkDir[i1]*rkDir[i1]; | ||
| 175 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2]; | ||
| 176 | fParam = -fDelta/fLSqr; | ||
| 177 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam; | ||
| 178 | |||
| 179 | if(pfLParam) | ||
| 180 | { | ||
| 181 | *pfLParam = fParam; | ||
| 182 | rkPnt[i0] = extents[i0]; | ||
| 183 | rkPnt[i1] = extents[i1]; | ||
| 184 | rkPnt[i2] = -extents[i2]; | ||
| 185 | } | ||
| 186 | } | ||
| 187 | return; | ||
| 188 | } | ||
| 189 | |||
| 190 | fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]; | ||
| 191 | fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]); | ||
| 192 | if(fTmp >= 0.0f) | ||
| 193 | { | ||
| 194 | // v[i2]-edge is closest | ||
| 195 | if(fTmp <= 2.0f*fLSqr*extents[i2]) | ||
| 196 | { | ||
| 197 | fT = fTmp/fLSqr; | ||
| 198 | fLSqr += rkDir[i2]*rkDir[i2]; | ||
| 199 | fTmp = kPpE[i2] - fT; | ||
| 200 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp; | ||
| 201 | fParam = -fDelta/fLSqr; | ||
| 202 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam; | ||
| 203 | |||
| 204 | if(pfLParam) | ||
| 205 | { | ||
| 206 | *pfLParam = fParam; | ||
| 207 | rkPnt[i0] = extents[i0]; | ||
| 208 | rkPnt[i1] = -extents[i1]; | ||
| 209 | rkPnt[i2] = fT - extents[i2]; | ||
| 210 | } | ||
| 211 | } | ||
| 212 | else | ||
| 213 | { | ||
| 214 | fLSqr += rkDir[i2]*rkDir[i2]; | ||
| 215 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2]; | ||
| 216 | fParam = -fDelta/fLSqr; | ||
| 217 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam; | ||
| 218 | |||
| 219 | if(pfLParam) | ||
| 220 | { | ||
| 221 | *pfLParam = fParam; | ||
| 222 | rkPnt[i0] = extents[i0]; | ||
| 223 | rkPnt[i1] = -extents[i1]; | ||
| 224 | rkPnt[i2] = extents[i2]; | ||
| 225 | } | ||
| 226 | } | ||
| 227 | return; | ||
| 228 | } | ||
| 229 | |||
| 230 | // (v[i1],v[i2])-corner is closest | ||
| 231 | fLSqr += rkDir[i2]*rkDir[i2]; | ||
| 232 | fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2]; | ||
| 233 | fParam = -fDelta/fLSqr; | ||
| 234 | rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam; | ||
| 235 | |||
| 236 | if(pfLParam) | ||
| 237 | { | ||
| 238 | *pfLParam = fParam; | ||
| 239 | rkPnt[i0] = extents[i0]; | ||
| 240 | rkPnt[i1] = -extents[i1]; | ||
| 241 | rkPnt[i2] = -extents[i2]; | ||
| 242 | } | ||
| 243 | } | ||
| 244 | } | ||
| 245 | } | ||
| 246 | |||
| 247 | static void CaseNoZeros(Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance) | ||
| 248 | { | ||
| 249 | Point kPmE(rkPnt.x - extents.x, rkPnt.y - extents.y, rkPnt.z - extents.z); | ||
| 250 | |||
| 251 | float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz; | ||
| 252 | |||
| 253 | fProdDxPy = rkDir.x*kPmE.y; | ||
| 254 | fProdDyPx = rkDir.y*kPmE.x; | ||
| 255 | if(fProdDyPx >= fProdDxPy) | ||
| 256 | { | ||
| 257 | fProdDzPx = rkDir.z*kPmE.x; | ||
| 258 | fProdDxPz = rkDir.x*kPmE.z; | ||
| 259 | if(fProdDzPx >= fProdDxPz) | ||
| 260 | { | ||
| 261 | // line intersects x = e0 | ||
| 262 | Face(0, 1, 2, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance); | ||
| 263 | } | ||
| 264 | else | ||
| 265 | { | ||
| 266 | // line intersects z = e2 | ||
| 267 | Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance); | ||
| 268 | } | ||
| 269 | } | ||
| 270 | else | ||
| 271 | { | ||
| 272 | fProdDzPy = rkDir.z*kPmE.y; | ||
| 273 | fProdDyPz = rkDir.y*kPmE.z; | ||
| 274 | if(fProdDzPy >= fProdDyPz) | ||
| 275 | { | ||
| 276 | // line intersects y = e1 | ||
| 277 | Face(1, 2, 0, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance); | ||
| 278 | } | ||
| 279 | else | ||
| 280 | { | ||
| 281 | // line intersects z = e2 | ||
| 282 | Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance); | ||
| 283 | } | ||
| 284 | } | ||
| 285 | } | ||
| 286 | |||
| 287 | static void Case0(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance) | ||
| 288 | { | ||
| 289 | float fPmE0 = rkPnt[i0] - extents[i0]; | ||
| 290 | float fPmE1 = rkPnt[i1] - extents[i1]; | ||
| 291 | float fProd0 = rkDir[i1]*fPmE0; | ||
| 292 | float fProd1 = rkDir[i0]*fPmE1; | ||
| 293 | float fDelta, fInvLSqr, fInv; | ||
| 294 | |||
| 295 | if(fProd0 >= fProd1) | ||
| 296 | { | ||
| 297 | // line intersects P[i0] = e[i0] | ||
| 298 | rkPnt[i0] = extents[i0]; | ||
| 299 | |||
| 300 | float fPpE1 = rkPnt[i1] + extents[i1]; | ||
| 301 | fDelta = fProd0 - rkDir[i0]*fPpE1; | ||
| 302 | if(fDelta >= 0.0f) | ||
| 303 | { | ||
| 304 | fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]); | ||
| 305 | rfSqrDistance += fDelta*fDelta*fInvLSqr; | ||
| 306 | if(pfLParam) | ||
| 307 | { | ||
| 308 | rkPnt[i1] = -extents[i1]; | ||
| 309 | *pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr; | ||
| 310 | } | ||
| 311 | } | ||
| 312 | else | ||
| 313 | { | ||
| 314 | if(pfLParam) | ||
| 315 | { | ||
| 316 | fInv = 1.0f/rkDir[i0]; | ||
| 317 | rkPnt[i1] -= fProd0*fInv; | ||
| 318 | *pfLParam = -fPmE0*fInv; | ||
| 319 | } | ||
| 320 | } | ||
| 321 | } | ||
| 322 | else | ||
| 323 | { | ||
| 324 | // line intersects P[i1] = e[i1] | ||
| 325 | rkPnt[i1] = extents[i1]; | ||
| 326 | |||
| 327 | float fPpE0 = rkPnt[i0] + extents[i0]; | ||
| 328 | fDelta = fProd1 - rkDir[i1]*fPpE0; | ||
| 329 | if(fDelta >= 0.0f) | ||
| 330 | { | ||
| 331 | fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]); | ||
| 332 | rfSqrDistance += fDelta*fDelta*fInvLSqr; | ||
| 333 | if(pfLParam) | ||
| 334 | { | ||
| 335 | rkPnt[i0] = -extents[i0]; | ||
| 336 | *pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr; | ||
| 337 | } | ||
| 338 | } | ||
| 339 | else | ||
| 340 | { | ||
| 341 | if(pfLParam) | ||
| 342 | { | ||
| 343 | fInv = 1.0f/rkDir[i1]; | ||
| 344 | rkPnt[i0] -= fProd1*fInv; | ||
| 345 | *pfLParam = -fPmE1*fInv; | ||
| 346 | } | ||
| 347 | } | ||
| 348 | } | ||
| 349 | |||
| 350 | if(rkPnt[i2] < -extents[i2]) | ||
| 351 | { | ||
| 352 | fDelta = rkPnt[i2] + extents[i2]; | ||
| 353 | rfSqrDistance += fDelta*fDelta; | ||
| 354 | rkPnt[i2] = -extents[i2]; | ||
| 355 | } | ||
| 356 | else if ( rkPnt[i2] > extents[i2] ) | ||
| 357 | { | ||
| 358 | fDelta = rkPnt[i2] - extents[i2]; | ||
| 359 | rfSqrDistance += fDelta*fDelta; | ||
| 360 | rkPnt[i2] = extents[i2]; | ||
| 361 | } | ||
| 362 | } | ||
| 363 | |||
| 364 | static void Case00(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance) | ||
| 365 | { | ||
| 366 | float fDelta; | ||
| 367 | |||
| 368 | if(pfLParam) | ||
| 369 | *pfLParam = (extents[i0] - rkPnt[i0])/rkDir[i0]; | ||
| 370 | |||
| 371 | rkPnt[i0] = extents[i0]; | ||
| 372 | |||
| 373 | if(rkPnt[i1] < -extents[i1]) | ||
| 374 | { | ||
| 375 | fDelta = rkPnt[i1] + extents[i1]; | ||
| 376 | rfSqrDistance += fDelta*fDelta; | ||
| 377 | rkPnt[i1] = -extents[i1]; | ||
| 378 | } | ||
| 379 | else if(rkPnt[i1] > extents[i1]) | ||
| 380 | { | ||
| 381 | fDelta = rkPnt[i1] - extents[i1]; | ||
| 382 | rfSqrDistance += fDelta*fDelta; | ||
| 383 | rkPnt[i1] = extents[i1]; | ||
| 384 | } | ||
| 385 | |||
| 386 | if(rkPnt[i2] < -extents[i2]) | ||
| 387 | { | ||
| 388 | fDelta = rkPnt[i2] + extents[i2]; | ||
| 389 | rfSqrDistance += fDelta*fDelta; | ||
| 390 | rkPnt[i1] = -extents[i2]; | ||
| 391 | } | ||
| 392 | else if(rkPnt[i2] > extents[i2]) | ||
| 393 | { | ||
| 394 | fDelta = rkPnt[i2] - extents[i2]; | ||
| 395 | rfSqrDistance += fDelta*fDelta; | ||
| 396 | rkPnt[i2] = extents[i2]; | ||
| 397 | } | ||
| 398 | } | ||
| 399 | |||
| 400 | static void Case000(Point& rkPnt, const Point& extents, float& rfSqrDistance) | ||
| 401 | { | ||
| 402 | float fDelta; | ||
| 403 | |||
| 404 | if(rkPnt.x < -extents.x) | ||
| 405 | { | ||
| 406 | fDelta = rkPnt.x + extents.x; | ||
| 407 | rfSqrDistance += fDelta*fDelta; | ||
| 408 | rkPnt.x = -extents.x; | ||
| 409 | } | ||
| 410 | else if(rkPnt.x > extents.x) | ||
| 411 | { | ||
| 412 | fDelta = rkPnt.x - extents.x; | ||
| 413 | rfSqrDistance += fDelta*fDelta; | ||
| 414 | rkPnt.x = extents.x; | ||
| 415 | } | ||
| 416 | |||
| 417 | if(rkPnt.y < -extents.y) | ||
| 418 | { | ||
| 419 | fDelta = rkPnt.y + extents.y; | ||
| 420 | rfSqrDistance += fDelta*fDelta; | ||
| 421 | rkPnt.y = -extents.y; | ||
| 422 | } | ||
| 423 | else if(rkPnt.y > extents.y) | ||
| 424 | { | ||
| 425 | fDelta = rkPnt.y - extents.y; | ||
| 426 | rfSqrDistance += fDelta*fDelta; | ||
| 427 | rkPnt.y = extents.y; | ||
| 428 | } | ||
| 429 | |||
| 430 | if(rkPnt.z < -extents.z) | ||
| 431 | { | ||
| 432 | fDelta = rkPnt.z + extents.z; | ||
| 433 | rfSqrDistance += fDelta*fDelta; | ||
| 434 | rkPnt.z = -extents.z; | ||
| 435 | } | ||
| 436 | else if(rkPnt.z > extents.z) | ||
| 437 | { | ||
| 438 | fDelta = rkPnt.z - extents.z; | ||
| 439 | rfSqrDistance += fDelta*fDelta; | ||
| 440 | rkPnt.z = extents.z; | ||
| 441 | } | ||
| 442 | } | ||
| 443 | |||
| 444 | static float SqrDistance(const Ray& rkLine, const Point& center, const Point& extents, float* pfLParam) | ||
| 445 | { | ||
| 446 | // compute coordinates of line in box coordinate system | ||
| 447 | Point kDiff = rkLine.mOrig - center; | ||
| 448 | Point kPnt = kDiff; | ||
| 449 | Point kDir = rkLine.mDir; | ||
| 450 | |||
| 451 | // Apply reflections so that direction vector has nonnegative components. | ||
| 452 | bool bReflect[3]; | ||
| 453 | for(int i=0;i<3;i++) | ||
| 454 | { | ||
| 455 | if(kDir[i]<0.0f) | ||
| 456 | { | ||
| 457 | kPnt[i] = -kPnt[i]; | ||
| 458 | kDir[i] = -kDir[i]; | ||
| 459 | bReflect[i] = true; | ||
| 460 | } | ||
| 461 | else | ||
| 462 | { | ||
| 463 | bReflect[i] = false; | ||
| 464 | } | ||
| 465 | } | ||
| 466 | |||
| 467 | float fSqrDistance = 0.0f; | ||
| 468 | |||
| 469 | if(kDir.x>0.0f) | ||
| 470 | { | ||
| 471 | if(kDir.y>0.0f) | ||
| 472 | { | ||
| 473 | if(kDir.z>0.0f) CaseNoZeros(kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,+) | ||
| 474 | else Case0(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,0) | ||
| 475 | } | ||
| 476 | else | ||
| 477 | { | ||
| 478 | if(kDir.z>0.0f) Case0(0, 2, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,+) | ||
| 479 | else Case00(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,0) | ||
| 480 | } | ||
| 481 | } | ||
| 482 | else | ||
| 483 | { | ||
| 484 | if(kDir.y>0.0f) | ||
| 485 | { | ||
| 486 | if(kDir.z>0.0f) Case0(1, 2, 0, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,+) | ||
| 487 | else Case00(1, 0, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,0) | ||
| 488 | } | ||
| 489 | else | ||
| 490 | { | ||
| 491 | if(kDir.z>0.0f) Case00(2, 0, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,0,+) | ||
| 492 | else | ||
| 493 | { | ||
| 494 | Case000(kPnt, extents, fSqrDistance); // (0,0,0) | ||
| 495 | if(pfLParam) *pfLParam = 0.0f; | ||
| 496 | } | ||
| 497 | } | ||
| 498 | } | ||
| 499 | return fSqrDistance; | ||
| 500 | } | ||
| 501 | |||
| 502 | inline_ float OPC_SegmentOBBSqrDist(const Segment& segment, const Point& c0, const Point& e0) | ||
| 503 | { | ||
| 504 | float fLP; | ||
| 505 | float fSqrDistance = SqrDistance(Ray(segment.GetOrigin(), segment.ComputeDirection()), c0, e0, &fLP); | ||
| 506 | if(fLP>=0.0f) | ||
| 507 | { | ||
| 508 | if(fLP<=1.0f) return fSqrDistance; | ||
| 509 | else return OPC_PointAABBSqrDist(segment.mP1, c0, e0); | ||
| 510 | } | ||
| 511 | else return OPC_PointAABBSqrDist(segment.mP0, c0, e0); | ||
| 512 | } | ||
| 513 | |||
| 514 | inline_ BOOL LSSCollider::LSSAABBOverlap(const Point& center, const Point& extents) | ||
| 515 | { | ||
| 516 | // Stats | ||
| 517 | mNbVolumeBVTests++; | ||
| 518 | |||
| 519 | float s2 = OPC_SegmentOBBSqrDist(mSeg, center, extents); | ||
| 520 | if(s2<mRadius2) return TRUE; | ||
| 521 | |||
| 522 | return FALSE; | ||
| 523 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_LSSCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_LSSCollider.cpp new file mode 100644 index 0000000..458c0a2 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_LSSCollider.cpp | |||
| @@ -0,0 +1,725 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for an LSS collider. | ||
| 12 | * \file OPC_LSSCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date December, 28, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains a lss-vs-tree collider. | ||
| 21 | * | ||
| 22 | * \class LSSCollider | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date December, 28, 2002 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | // Precompiled Header | ||
| 31 | #include "Stdafx.h" | ||
| 32 | |||
| 33 | using namespace Opcode; | ||
| 34 | |||
| 35 | #include "OPC_LSSAABBOverlap.h" | ||
| 36 | #include "OPC_LSSTriOverlap.h" | ||
| 37 | |||
| 38 | #define SET_CONTACT(prim_index, flag) \ | ||
| 39 | /* Set contact status */ \ | ||
| 40 | mFlags |= flag; \ | ||
| 41 | mTouchedPrimitives->Add(udword(prim_index)); | ||
| 42 | |||
| 43 | //! LSS-triangle overlap test | ||
| 44 | #define LSS_PRIM(prim_index, flag) \ | ||
| 45 | /* Request vertices from the app */ \ | ||
| 46 | VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ | ||
| 47 | \ | ||
| 48 | /* Perform LSS-tri overlap test */ \ | ||
| 49 | if(LSSTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ | ||
| 50 | { \ | ||
| 51 | SET_CONTACT(prim_index, flag) \ | ||
| 52 | } | ||
| 53 | |||
| 54 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 55 | /** | ||
| 56 | * Constructor. | ||
| 57 | */ | ||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | LSSCollider::LSSCollider() | ||
| 60 | { | ||
| 61 | // mCenter.Zero(); | ||
| 62 | // mRadius2 = 0.0f; | ||
| 63 | } | ||
| 64 | |||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | /** | ||
| 67 | * Destructor. | ||
| 68 | */ | ||
| 69 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 70 | LSSCollider::~LSSCollider() | ||
| 71 | { | ||
| 72 | } | ||
| 73 | |||
| 74 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 75 | /** | ||
| 76 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 77 | * - with GetContactStatus() | ||
| 78 | * - with GetNbTouchedPrimitives() | ||
| 79 | * - with GetTouchedPrimitives() | ||
| 80 | * | ||
| 81 | * \param cache [in/out] an lss cache | ||
| 82 | * \param lss [in] collision lss in local space | ||
| 83 | * \param model [in] Opcode model to collide with | ||
| 84 | * \param worldl [in] lss world matrix, or null | ||
| 85 | * \param worldm [in] model's world matrix, or null | ||
| 86 | * \return true if success | ||
| 87 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 88 | */ | ||
| 89 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 90 | bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, const Model& model, const Matrix4x4* worldl, const Matrix4x4* worldm) | ||
| 91 | { | ||
| 92 | // Checkings | ||
| 93 | if(!Setup(&model)) return false; | ||
| 94 | |||
| 95 | // Init collision query | ||
| 96 | if(InitQuery(cache, lss, worldl, worldm)) return true; | ||
| 97 | |||
| 98 | if(!model.HasLeafNodes()) | ||
| 99 | { | ||
| 100 | if(model.IsQuantized()) | ||
| 101 | { | ||
| 102 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 103 | |||
| 104 | // Setup dequantization coeffs | ||
| 105 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 106 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 107 | |||
| 108 | // Perform collision query | ||
| 109 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 110 | else _Collide(Tree->GetNodes()); | ||
| 111 | } | ||
| 112 | else | ||
| 113 | { | ||
| 114 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 115 | |||
| 116 | // Perform collision query | ||
| 117 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 118 | else _Collide(Tree->GetNodes()); | ||
| 119 | } | ||
| 120 | } | ||
| 121 | else | ||
| 122 | { | ||
| 123 | if(model.IsQuantized()) | ||
| 124 | { | ||
| 125 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 126 | |||
| 127 | // Setup dequantization coeffs | ||
| 128 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 129 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 130 | |||
| 131 | // Perform collision query | ||
| 132 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 133 | else _Collide(Tree->GetNodes()); | ||
| 134 | } | ||
| 135 | else | ||
| 136 | { | ||
| 137 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 138 | |||
| 139 | // Perform collision query | ||
| 140 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 141 | else _Collide(Tree->GetNodes()); | ||
| 142 | } | ||
| 143 | } | ||
| 144 | |||
| 145 | return true; | ||
| 146 | } | ||
| 147 | |||
| 148 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 149 | /** | ||
| 150 | * Initializes a collision query : | ||
| 151 | * - reset stats & contact status | ||
| 152 | * - setup matrices | ||
| 153 | * - check temporal coherence | ||
| 154 | * | ||
| 155 | * \param cache [in/out] an lss cache | ||
| 156 | * \param lss [in] lss in local space | ||
| 157 | * \param worldl [in] lss world matrix, or null | ||
| 158 | * \param worldm [in] model's world matrix, or null | ||
| 159 | * \return TRUE if we can return immediately | ||
| 160 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 161 | */ | ||
| 162 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 163 | BOOL LSSCollider::InitQuery(LSSCache& cache, const LSS& lss, const Matrix4x4* worldl, const Matrix4x4* worldm) | ||
| 164 | { | ||
| 165 | // 1) Call the base method | ||
| 166 | VolumeCollider::InitQuery(); | ||
| 167 | |||
| 168 | // 2) Compute LSS in model space: | ||
| 169 | // - Precompute R^2 | ||
| 170 | mRadius2 = lss.mRadius * lss.mRadius; | ||
| 171 | // - Compute segment | ||
| 172 | mSeg.mP0 = lss.mP0; | ||
| 173 | mSeg.mP1 = lss.mP1; | ||
| 174 | // -> to world space | ||
| 175 | if(worldl) | ||
| 176 | { | ||
| 177 | mSeg.mP0 *= *worldl; | ||
| 178 | mSeg.mP1 *= *worldl; | ||
| 179 | } | ||
| 180 | // -> to model space | ||
| 181 | if(worldm) | ||
| 182 | { | ||
| 183 | // Invert model matrix | ||
| 184 | Matrix4x4 InvWorldM; | ||
| 185 | InvertPRMatrix(InvWorldM, *worldm); | ||
| 186 | |||
| 187 | mSeg.mP0 *= InvWorldM; | ||
| 188 | mSeg.mP1 *= InvWorldM; | ||
| 189 | } | ||
| 190 | |||
| 191 | // 3) Setup destination pointer | ||
| 192 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 193 | |||
| 194 | // 4) Special case: 1-triangle meshes [Opcode 1.3] | ||
| 195 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 196 | { | ||
| 197 | if(!SkipPrimitiveTests()) | ||
| 198 | { | ||
| 199 | // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. | ||
| 200 | mTouchedPrimitives->Reset(); | ||
| 201 | |||
| 202 | // Perform overlap test between the unique triangle and the LSS (and set contact status if needed) | ||
| 203 | LSS_PRIM(udword(0), OPC_CONTACT) | ||
| 204 | |||
| 205 | // Return immediately regardless of status | ||
| 206 | return TRUE; | ||
| 207 | } | ||
| 208 | } | ||
| 209 | |||
| 210 | // 5) Check temporal coherence : | ||
| 211 | if(TemporalCoherenceEnabled()) | ||
| 212 | { | ||
| 213 | // Here we use temporal coherence | ||
| 214 | // => check results from previous frame before performing the collision query | ||
| 215 | if(FirstContactEnabled()) | ||
| 216 | { | ||
| 217 | // We're only interested in the first contact found => test the unique previously touched face | ||
| 218 | if(mTouchedPrimitives->GetNbEntries()) | ||
| 219 | { | ||
| 220 | // Get index of previously touched face = the first entry in the array | ||
| 221 | udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0); | ||
| 222 | |||
| 223 | // Then reset the array: | ||
| 224 | // - if the overlap test below is successful, the index we'll get added back anyway | ||
| 225 | // - if it isn't, then the array should be reset anyway for the normal query | ||
| 226 | mTouchedPrimitives->Reset(); | ||
| 227 | |||
| 228 | // Perform overlap test between the cached triangle and the LSS (and set contact status if needed) | ||
| 229 | LSS_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT) | ||
| 230 | |||
| 231 | // Return immediately if possible | ||
| 232 | if(GetContactStatus()) return TRUE; | ||
| 233 | } | ||
| 234 | // else no face has been touched during previous query | ||
| 235 | // => we'll have to perform a normal query | ||
| 236 | } | ||
| 237 | else | ||
| 238 | { | ||
| 239 | // We're interested in all contacts =>test the new real LSS N(ew) against the previous fat LSS P(revious): | ||
| 240 | |||
| 241 | // ### rewrite this | ||
| 242 | |||
| 243 | LSS Test(mSeg, lss.mRadius); // in model space | ||
| 244 | LSS Previous(cache.Previous, sqrtf(cache.Previous.mRadius)); | ||
| 245 | |||
| 246 | // if(cache.Previous.Contains(Test)) | ||
| 247 | if(IsCacheValid(cache) && Previous.Contains(Test)) | ||
| 248 | { | ||
| 249 | // - if N is included in P, return previous list | ||
| 250 | // => we simply leave the list (mTouchedFaces) unchanged | ||
| 251 | |||
| 252 | // Set contact status if needed | ||
| 253 | if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT; | ||
| 254 | |||
| 255 | // In any case we don't need to do a query | ||
| 256 | return TRUE; | ||
| 257 | } | ||
| 258 | else | ||
| 259 | { | ||
| 260 | // - else do the query using a fat N | ||
| 261 | |||
| 262 | // Reset cache since we'll about to perform a real query | ||
| 263 | mTouchedPrimitives->Reset(); | ||
| 264 | |||
| 265 | // Make a fat sphere so that coherence will work for subsequent frames | ||
| 266 | mRadius2 *= cache.FatCoeff; | ||
| 267 | // mRadius2 = (lss.mRadius * cache.FatCoeff)*(lss.mRadius * cache.FatCoeff); | ||
| 268 | |||
| 269 | |||
| 270 | // Update cache with query data (signature for cached faces) | ||
| 271 | cache.Previous.mP0 = mSeg.mP0; | ||
| 272 | cache.Previous.mP1 = mSeg.mP1; | ||
| 273 | cache.Previous.mRadius = mRadius2; | ||
| 274 | } | ||
| 275 | } | ||
| 276 | } | ||
| 277 | else | ||
| 278 | { | ||
| 279 | // Here we don't use temporal coherence => do a normal query | ||
| 280 | mTouchedPrimitives->Reset(); | ||
| 281 | } | ||
| 282 | |||
| 283 | return FALSE; | ||
| 284 | } | ||
| 285 | |||
| 286 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 287 | /** | ||
| 288 | * Collision query for vanilla AABB trees. | ||
| 289 | * \param cache [in/out] an lss cache | ||
| 290 | * \param lss [in] collision lss in world space | ||
| 291 | * \param tree [in] AABB tree | ||
| 292 | * \return true if success | ||
| 293 | */ | ||
| 294 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 295 | bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, const AABBTree* tree) | ||
| 296 | { | ||
| 297 | // This is typically called for a scene tree, full of -AABBs-, not full of triangles. | ||
| 298 | // So we don't really have "primitives" to deal with. Hence it doesn't work with | ||
| 299 | // "FirstContact" + "TemporalCoherence". | ||
| 300 | ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) ); | ||
| 301 | |||
| 302 | // Checkings | ||
| 303 | if(!tree) return false; | ||
| 304 | |||
| 305 | // Init collision query | ||
| 306 | if(InitQuery(cache, lss)) return true; | ||
| 307 | |||
| 308 | // Perform collision query | ||
| 309 | _Collide(tree); | ||
| 310 | |||
| 311 | return true; | ||
| 312 | } | ||
| 313 | |||
| 314 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 315 | /** | ||
| 316 | * Checks the LSS completely contains the box. In which case we can end the query sooner. | ||
| 317 | * \param bc [in] box center | ||
| 318 | * \param be [in] box extents | ||
| 319 | * \return true if the LSS contains the whole box | ||
| 320 | */ | ||
| 321 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 322 | inline_ BOOL LSSCollider::LSSContainsBox(const Point& bc, const Point& be) | ||
| 323 | { | ||
| 324 | // Not implemented | ||
| 325 | return FALSE; | ||
| 326 | } | ||
| 327 | |||
| 328 | #define TEST_BOX_IN_LSS(center, extents) \ | ||
| 329 | if(LSSContainsBox(center, extents)) \ | ||
| 330 | { \ | ||
| 331 | /* Set contact status */ \ | ||
| 332 | mFlags |= OPC_CONTACT; \ | ||
| 333 | _Dump(node); \ | ||
| 334 | return; \ | ||
| 335 | } | ||
| 336 | |||
| 337 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 338 | /** | ||
| 339 | * Recursive collision query for normal AABB trees. | ||
| 340 | * \param node [in] current collision node | ||
| 341 | */ | ||
| 342 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 343 | void LSSCollider::_Collide(const AABBCollisionNode* node) | ||
| 344 | { | ||
| 345 | // Perform LSS-AABB overlap test | ||
| 346 | if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 347 | |||
| 348 | TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 349 | |||
| 350 | if(node->IsLeaf()) | ||
| 351 | { | ||
| 352 | LSS_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 353 | } | ||
| 354 | else | ||
| 355 | { | ||
| 356 | _Collide(node->GetPos()); | ||
| 357 | |||
| 358 | if(ContactFound()) return; | ||
| 359 | |||
| 360 | _Collide(node->GetNeg()); | ||
| 361 | } | ||
| 362 | } | ||
| 363 | |||
| 364 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 365 | /** | ||
| 366 | * Recursive collision query for normal AABB trees, without primitive tests. | ||
| 367 | * \param node [in] current collision node | ||
| 368 | */ | ||
| 369 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 370 | void LSSCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node) | ||
| 371 | { | ||
| 372 | // Perform LSS-AABB overlap test | ||
| 373 | if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 374 | |||
| 375 | TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 376 | |||
| 377 | if(node->IsLeaf()) | ||
| 378 | { | ||
| 379 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 380 | } | ||
| 381 | else | ||
| 382 | { | ||
| 383 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 384 | |||
| 385 | if(ContactFound()) return; | ||
| 386 | |||
| 387 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 388 | } | ||
| 389 | } | ||
| 390 | |||
| 391 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 392 | /** | ||
| 393 | * Recursive collision query for quantized AABB trees. | ||
| 394 | * \param node [in] current collision node | ||
| 395 | */ | ||
| 396 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 397 | void LSSCollider::_Collide(const AABBQuantizedNode* node) | ||
| 398 | { | ||
| 399 | // Dequantize box | ||
| 400 | const QuantizedAABB& Box = node->mAABB; | ||
| 401 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 402 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 403 | |||
| 404 | // Perform LSS-AABB overlap test | ||
| 405 | if(!LSSAABBOverlap(Center, Extents)) return; | ||
| 406 | |||
| 407 | TEST_BOX_IN_LSS(Center, Extents) | ||
| 408 | |||
| 409 | if(node->IsLeaf()) | ||
| 410 | { | ||
| 411 | LSS_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 412 | } | ||
| 413 | else | ||
| 414 | { | ||
| 415 | _Collide(node->GetPos()); | ||
| 416 | |||
| 417 | if(ContactFound()) return; | ||
| 418 | |||
| 419 | _Collide(node->GetNeg()); | ||
| 420 | } | ||
| 421 | } | ||
| 422 | |||
| 423 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 424 | /** | ||
| 425 | * Recursive collision query for quantized AABB trees, without primitive tests. | ||
| 426 | * \param node [in] current collision node | ||
| 427 | */ | ||
| 428 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 429 | void LSSCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node) | ||
| 430 | { | ||
| 431 | // Dequantize box | ||
| 432 | const QuantizedAABB& Box = node->mAABB; | ||
| 433 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 434 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 435 | |||
| 436 | // Perform LSS-AABB overlap test | ||
| 437 | if(!LSSAABBOverlap(Center, Extents)) return; | ||
| 438 | |||
| 439 | TEST_BOX_IN_LSS(Center, Extents) | ||
| 440 | |||
| 441 | if(node->IsLeaf()) | ||
| 442 | { | ||
| 443 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 444 | } | ||
| 445 | else | ||
| 446 | { | ||
| 447 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 448 | |||
| 449 | if(ContactFound()) return; | ||
| 450 | |||
| 451 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 452 | } | ||
| 453 | } | ||
| 454 | |||
| 455 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 456 | /** | ||
| 457 | * Recursive collision query for no-leaf AABB trees. | ||
| 458 | * \param node [in] current collision node | ||
| 459 | */ | ||
| 460 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 461 | void LSSCollider::_Collide(const AABBNoLeafNode* node) | ||
| 462 | { | ||
| 463 | // Perform LSS-AABB overlap test | ||
| 464 | if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 465 | |||
| 466 | TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 467 | |||
| 468 | if(node->HasPosLeaf()) { LSS_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 469 | else _Collide(node->GetPos()); | ||
| 470 | |||
| 471 | if(ContactFound()) return; | ||
| 472 | |||
| 473 | if(node->HasNegLeaf()) { LSS_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 474 | else _Collide(node->GetNeg()); | ||
| 475 | } | ||
| 476 | |||
| 477 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 478 | /** | ||
| 479 | * Recursive collision query for no-leaf AABB trees, without primitive tests. | ||
| 480 | * \param node [in] current collision node | ||
| 481 | */ | ||
| 482 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 483 | void LSSCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node) | ||
| 484 | { | ||
| 485 | // Perform LSS-AABB overlap test | ||
| 486 | if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 487 | |||
| 488 | TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 489 | |||
| 490 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 491 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 492 | |||
| 493 | if(ContactFound()) return; | ||
| 494 | |||
| 495 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 496 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 497 | } | ||
| 498 | |||
| 499 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 500 | /** | ||
| 501 | * Recursive collision query for quantized no-leaf AABB trees. | ||
| 502 | * \param node [in] current collision node | ||
| 503 | */ | ||
| 504 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 505 | void LSSCollider::_Collide(const AABBQuantizedNoLeafNode* node) | ||
| 506 | { | ||
| 507 | // Dequantize box | ||
| 508 | const QuantizedAABB& Box = node->mAABB; | ||
| 509 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 510 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 511 | |||
| 512 | // Perform LSS-AABB overlap test | ||
| 513 | if(!LSSAABBOverlap(Center, Extents)) return; | ||
| 514 | |||
| 515 | TEST_BOX_IN_LSS(Center, Extents) | ||
| 516 | |||
| 517 | if(node->HasPosLeaf()) { LSS_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 518 | else _Collide(node->GetPos()); | ||
| 519 | |||
| 520 | if(ContactFound()) return; | ||
| 521 | |||
| 522 | if(node->HasNegLeaf()) { LSS_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 523 | else _Collide(node->GetNeg()); | ||
| 524 | } | ||
| 525 | |||
| 526 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 527 | /** | ||
| 528 | * Recursive collision query for quantized no-leaf AABB trees, without primitive tests. | ||
| 529 | * \param node [in] current collision node | ||
| 530 | */ | ||
| 531 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 532 | void LSSCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node) | ||
| 533 | { | ||
| 534 | // Dequantize box | ||
| 535 | const QuantizedAABB& Box = node->mAABB; | ||
| 536 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 537 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 538 | |||
| 539 | // Perform LSS-AABB overlap test | ||
| 540 | if(!LSSAABBOverlap(Center, Extents)) return; | ||
| 541 | |||
| 542 | TEST_BOX_IN_LSS(Center, Extents) | ||
| 543 | |||
| 544 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 545 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 546 | |||
| 547 | if(ContactFound()) return; | ||
| 548 | |||
| 549 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 550 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 551 | } | ||
| 552 | |||
| 553 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 554 | /** | ||
| 555 | * Recursive collision query for vanilla AABB trees. | ||
| 556 | * \param node [in] current collision node | ||
| 557 | */ | ||
| 558 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 559 | void LSSCollider::_Collide(const AABBTreeNode* node) | ||
| 560 | { | ||
| 561 | // Perform LSS-AABB overlap test | ||
| 562 | Point Center, Extents; | ||
| 563 | node->GetAABB()->GetCenter(Center); | ||
| 564 | node->GetAABB()->GetExtents(Extents); | ||
| 565 | if(!LSSAABBOverlap(Center, Extents)) return; | ||
| 566 | |||
| 567 | if(node->IsLeaf() || LSSContainsBox(Center, Extents)) | ||
| 568 | { | ||
| 569 | mFlags |= OPC_CONTACT; | ||
| 570 | mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives()); | ||
| 571 | } | ||
| 572 | else | ||
| 573 | { | ||
| 574 | _Collide(node->GetPos()); | ||
| 575 | _Collide(node->GetNeg()); | ||
| 576 | } | ||
| 577 | } | ||
| 578 | |||
| 579 | |||
| 580 | |||
| 581 | |||
| 582 | |||
| 583 | |||
| 584 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 585 | /** | ||
| 586 | * Constructor. | ||
| 587 | */ | ||
| 588 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 589 | HybridLSSCollider::HybridLSSCollider() | ||
| 590 | { | ||
| 591 | } | ||
| 592 | |||
| 593 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 594 | /** | ||
| 595 | * Destructor. | ||
| 596 | */ | ||
| 597 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 598 | HybridLSSCollider::~HybridLSSCollider() | ||
| 599 | { | ||
| 600 | } | ||
| 601 | |||
| 602 | bool HybridLSSCollider::Collide(LSSCache& cache, const LSS& lss, const HybridModel& model, const Matrix4x4* worldl, const Matrix4x4* worldm) | ||
| 603 | { | ||
| 604 | // We don't want primitive tests here! | ||
| 605 | mFlags |= OPC_NO_PRIMITIVE_TESTS; | ||
| 606 | |||
| 607 | // Checkings | ||
| 608 | if(!Setup(&model)) return false; | ||
| 609 | |||
| 610 | // Init collision query | ||
| 611 | if(InitQuery(cache, lss, worldl, worldm)) return true; | ||
| 612 | |||
| 613 | // Special case for 1-leaf trees | ||
| 614 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 615 | { | ||
| 616 | // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles | ||
| 617 | udword Nb = mIMesh->GetNbTriangles(); | ||
| 618 | |||
| 619 | // Loop through all triangles | ||
| 620 | for(udword i=0;i<Nb;i++) | ||
| 621 | { | ||
| 622 | LSS_PRIM(i, OPC_CONTACT) | ||
| 623 | } | ||
| 624 | return true; | ||
| 625 | } | ||
| 626 | |||
| 627 | // Override destination array since we're only going to get leaf boxes here | ||
| 628 | mTouchedBoxes.Reset(); | ||
| 629 | mTouchedPrimitives = &mTouchedBoxes; | ||
| 630 | |||
| 631 | // Now, do the actual query against leaf boxes | ||
| 632 | if(!model.HasLeafNodes()) | ||
| 633 | { | ||
| 634 | if(model.IsQuantized()) | ||
| 635 | { | ||
| 636 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 637 | |||
| 638 | // Setup dequantization coeffs | ||
| 639 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 640 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 641 | |||
| 642 | // Perform collision query - we don't want primitive tests here! | ||
| 643 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 644 | } | ||
| 645 | else | ||
| 646 | { | ||
| 647 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 648 | |||
| 649 | // Perform collision query - we don't want primitive tests here! | ||
| 650 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 651 | } | ||
| 652 | } | ||
| 653 | else | ||
| 654 | { | ||
| 655 | if(model.IsQuantized()) | ||
| 656 | { | ||
| 657 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 658 | |||
| 659 | // Setup dequantization coeffs | ||
| 660 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 661 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 662 | |||
| 663 | // Perform collision query - we don't want primitive tests here! | ||
| 664 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 665 | } | ||
| 666 | else | ||
| 667 | { | ||
| 668 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 669 | |||
| 670 | // Perform collision query - we don't want primitive tests here! | ||
| 671 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 672 | } | ||
| 673 | } | ||
| 674 | |||
| 675 | // We only have a list of boxes so far | ||
| 676 | if(GetContactStatus()) | ||
| 677 | { | ||
| 678 | // Reset contact status, since it currently only reflects collisions with leaf boxes | ||
| 679 | Collider::InitQuery(); | ||
| 680 | |||
| 681 | // Change dest container so that we can use built-in overlap tests and get collided primitives | ||
| 682 | cache.TouchedPrimitives.Reset(); | ||
| 683 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 684 | |||
| 685 | // Read touched leaf boxes | ||
| 686 | udword Nb = mTouchedBoxes.GetNbEntries(); | ||
| 687 | const udword* Touched = mTouchedBoxes.GetEntries(); | ||
| 688 | |||
| 689 | const LeafTriangles* LT = model.GetLeafTriangles(); | ||
| 690 | const udword* Indices = model.GetIndices(); | ||
| 691 | |||
| 692 | // Loop through touched leaves | ||
| 693 | while(Nb--) | ||
| 694 | { | ||
| 695 | const LeafTriangles& CurrentLeaf = LT[*Touched++]; | ||
| 696 | |||
| 697 | // Each leaf box has a set of triangles | ||
| 698 | udword NbTris = CurrentLeaf.GetNbTriangles(); | ||
| 699 | if(Indices) | ||
| 700 | { | ||
| 701 | const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; | ||
| 702 | |||
| 703 | // Loop through triangles and test each of them | ||
| 704 | while(NbTris--) | ||
| 705 | { | ||
| 706 | udword TriangleIndex = *T++; | ||
| 707 | LSS_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 708 | } | ||
| 709 | } | ||
| 710 | else | ||
| 711 | { | ||
| 712 | udword BaseIndex = CurrentLeaf.GetTriangleIndex(); | ||
| 713 | |||
| 714 | // Loop through triangles and test each of them | ||
| 715 | while(NbTris--) | ||
| 716 | { | ||
| 717 | udword TriangleIndex = BaseIndex++; | ||
| 718 | LSS_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 719 | } | ||
| 720 | } | ||
| 721 | } | ||
| 722 | } | ||
| 723 | |||
| 724 | return true; | ||
| 725 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_LSSCollider.h b/libraries/ode-0.9/OPCODE/OPC_LSSCollider.h new file mode 100644 index 0000000..b4d0893 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_LSSCollider.h | |||
| @@ -0,0 +1,99 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for an LSS collider. | ||
| 12 | * \file OPC_LSSCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date December, 28, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_LSSCOLLIDER_H__ | ||
| 21 | #define __OPC_LSSCOLLIDER_H__ | ||
| 22 | |||
| 23 | struct OPCODE_API LSSCache : VolumeCache | ||
| 24 | { | ||
| 25 | LSSCache() | ||
| 26 | { | ||
| 27 | Previous.mP0 = Point(0.0f, 0.0f, 0.0f); | ||
| 28 | Previous.mP1 = Point(0.0f, 0.0f, 0.0f); | ||
| 29 | Previous.mRadius = 0.0f; | ||
| 30 | FatCoeff = 1.1f; | ||
| 31 | } | ||
| 32 | |||
| 33 | // Cached faces signature | ||
| 34 | LSS Previous; //!< LSS used when performing the query resulting in cached faces | ||
| 35 | // User settings | ||
| 36 | float FatCoeff; //!< mRadius2 multiplier used to create a fat LSS | ||
| 37 | }; | ||
| 38 | |||
| 39 | class OPCODE_API LSSCollider : public VolumeCollider | ||
| 40 | { | ||
| 41 | public: | ||
| 42 | // Constructor / Destructor | ||
| 43 | LSSCollider(); | ||
| 44 | virtual ~LSSCollider(); | ||
| 45 | |||
| 46 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 47 | /** | ||
| 48 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 49 | * - with GetContactStatus() | ||
| 50 | * - with GetNbTouchedPrimitives() | ||
| 51 | * - with GetTouchedPrimitives() | ||
| 52 | * | ||
| 53 | * \param cache [in/out] an lss cache | ||
| 54 | * \param lss [in] collision lss in local space | ||
| 55 | * \param model [in] Opcode model to collide with | ||
| 56 | * \param worldl [in] lss world matrix, or null | ||
| 57 | * \param worldm [in] model's world matrix, or null | ||
| 58 | * \return true if success | ||
| 59 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 60 | */ | ||
| 61 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 62 | bool Collide(LSSCache& cache, const LSS& lss, const Model& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null); | ||
| 63 | // | ||
| 64 | bool Collide(LSSCache& cache, const LSS& lss, const AABBTree* tree); | ||
| 65 | protected: | ||
| 66 | // LSS in model space | ||
| 67 | Segment mSeg; //!< Segment | ||
| 68 | float mRadius2; //!< LSS radius squared | ||
| 69 | // Internal methods | ||
| 70 | void _Collide(const AABBCollisionNode* node); | ||
| 71 | void _Collide(const AABBNoLeafNode* node); | ||
| 72 | void _Collide(const AABBQuantizedNode* node); | ||
| 73 | void _Collide(const AABBQuantizedNoLeafNode* node); | ||
| 74 | void _Collide(const AABBTreeNode* node); | ||
| 75 | void _CollideNoPrimitiveTest(const AABBCollisionNode* node); | ||
| 76 | void _CollideNoPrimitiveTest(const AABBNoLeafNode* node); | ||
| 77 | void _CollideNoPrimitiveTest(const AABBQuantizedNode* node); | ||
| 78 | void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node); | ||
| 79 | // Overlap tests | ||
| 80 | inline_ BOOL LSSContainsBox(const Point& bc, const Point& be); | ||
| 81 | inline_ BOOL LSSAABBOverlap(const Point& center, const Point& extents); | ||
| 82 | inline_ BOOL LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2); | ||
| 83 | // Init methods | ||
| 84 | BOOL InitQuery(LSSCache& cache, const LSS& lss, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null); | ||
| 85 | }; | ||
| 86 | |||
| 87 | class OPCODE_API HybridLSSCollider : public LSSCollider | ||
| 88 | { | ||
| 89 | public: | ||
| 90 | // Constructor / Destructor | ||
| 91 | HybridLSSCollider(); | ||
| 92 | virtual ~HybridLSSCollider(); | ||
| 93 | |||
| 94 | bool Collide(LSSCache& cache, const LSS& lss, const HybridModel& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null); | ||
| 95 | protected: | ||
| 96 | Container mTouchedBoxes; | ||
| 97 | }; | ||
| 98 | |||
| 99 | #endif // __OPC_LSSCOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_LSSTriOverlap.h b/libraries/ode-0.9/OPCODE/OPC_LSSTriOverlap.h new file mode 100644 index 0000000..f1d17e4 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_LSSTriOverlap.h | |||
| @@ -0,0 +1,679 @@ | |||
| 1 | // Following code from Magic-Software (http://www.magic-software.com/) | ||
| 2 | // A bit modified for Opcode | ||
| 3 | |||
| 4 | static const float gs_fTolerance = 1e-05f; | ||
| 5 | |||
| 6 | static float OPC_PointTriangleSqrDist(const Point& point, const Point& p0, const Point& p1, const Point& p2) | ||
| 7 | { | ||
| 8 | // Hook | ||
| 9 | Point TriEdge0 = p1 - p0; | ||
| 10 | Point TriEdge1 = p2 - p0; | ||
| 11 | |||
| 12 | Point kDiff = p0 - point; | ||
| 13 | float fA00 = TriEdge0.SquareMagnitude(); | ||
| 14 | float fA01 = TriEdge0 | TriEdge1; | ||
| 15 | float fA11 = TriEdge1.SquareMagnitude(); | ||
| 16 | float fB0 = kDiff | TriEdge0; | ||
| 17 | float fB1 = kDiff | TriEdge1; | ||
| 18 | float fC = kDiff.SquareMagnitude(); | ||
| 19 | float fDet = fabsf(fA00*fA11 - fA01*fA01); | ||
| 20 | float fS = fA01*fB1-fA11*fB0; | ||
| 21 | float fT = fA01*fB0-fA00*fB1; | ||
| 22 | float fSqrDist; | ||
| 23 | |||
| 24 | if(fS + fT <= fDet) | ||
| 25 | { | ||
| 26 | if(fS < 0.0f) | ||
| 27 | { | ||
| 28 | if(fT < 0.0f) // region 4 | ||
| 29 | { | ||
| 30 | if(fB0 < 0.0f) | ||
| 31 | { | ||
| 32 | if(-fB0 >= fA00) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 33 | else fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 34 | } | ||
| 35 | else | ||
| 36 | { | ||
| 37 | if(fB1 >= 0.0f) fSqrDist = fC; | ||
| 38 | else if(-fB1 >= fA11) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 39 | else fSqrDist = fB1*(-fB1/fA11)+fC; | ||
| 40 | } | ||
| 41 | } | ||
| 42 | else // region 3 | ||
| 43 | { | ||
| 44 | if(fB1 >= 0.0f) fSqrDist = fC; | ||
| 45 | else if(-fB1 >= fA11) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 46 | else fSqrDist = fB1*(-fB1/fA11)+fC; | ||
| 47 | } | ||
| 48 | } | ||
| 49 | else if(fT < 0.0f) // region 5 | ||
| 50 | { | ||
| 51 | if(fB0 >= 0.0f) fSqrDist = fC; | ||
| 52 | else if(-fB0 >= fA00) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 53 | else fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 54 | } | ||
| 55 | else // region 0 | ||
| 56 | { | ||
| 57 | // minimum at interior point | ||
| 58 | if(fDet==0.0f) | ||
| 59 | { | ||
| 60 | fSqrDist = MAX_FLOAT; | ||
| 61 | } | ||
| 62 | else | ||
| 63 | { | ||
| 64 | float fInvDet = 1.0f/fDet; | ||
| 65 | fS *= fInvDet; | ||
| 66 | fT *= fInvDet; | ||
| 67 | fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC; | ||
| 68 | } | ||
| 69 | } | ||
| 70 | } | ||
| 71 | else | ||
| 72 | { | ||
| 73 | float fTmp0, fTmp1, fNumer, fDenom; | ||
| 74 | |||
| 75 | if(fS < 0.0f) // region 2 | ||
| 76 | { | ||
| 77 | fTmp0 = fA01 + fB0; | ||
| 78 | fTmp1 = fA11 + fB1; | ||
| 79 | if(fTmp1 > fTmp0) | ||
| 80 | { | ||
| 81 | fNumer = fTmp1 - fTmp0; | ||
| 82 | fDenom = fA00-2.0f*fA01+fA11; | ||
| 83 | if(fNumer >= fDenom) | ||
| 84 | { | ||
| 85 | fSqrDist = fA00+2.0f*fB0+fC; | ||
| 86 | } | ||
| 87 | else | ||
| 88 | { | ||
| 89 | fS = fNumer/fDenom; | ||
| 90 | fT = 1.0f - fS; | ||
| 91 | fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC; | ||
| 92 | } | ||
| 93 | } | ||
| 94 | else | ||
| 95 | { | ||
| 96 | if(fTmp1 <= 0.0f) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 97 | else if(fB1 >= 0.0f) fSqrDist = fC; | ||
| 98 | else fSqrDist = fB1*(-fB1/fA11)+fC; | ||
| 99 | } | ||
| 100 | } | ||
| 101 | else if(fT < 0.0f) // region 6 | ||
| 102 | { | ||
| 103 | fTmp0 = fA01 + fB1; | ||
| 104 | fTmp1 = fA00 + fB0; | ||
| 105 | if(fTmp1 > fTmp0) | ||
| 106 | { | ||
| 107 | fNumer = fTmp1 - fTmp0; | ||
| 108 | fDenom = fA00-2.0f*fA01+fA11; | ||
| 109 | if(fNumer >= fDenom) | ||
| 110 | { | ||
| 111 | fSqrDist = fA11+2.0f*fB1+fC; | ||
| 112 | } | ||
| 113 | else | ||
| 114 | { | ||
| 115 | fT = fNumer/fDenom; | ||
| 116 | fS = 1.0f - fT; | ||
| 117 | fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC; | ||
| 118 | } | ||
| 119 | } | ||
| 120 | else | ||
| 121 | { | ||
| 122 | if(fTmp1 <= 0.0f) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 123 | else if(fB0 >= 0.0f) fSqrDist = fC; | ||
| 124 | else fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 125 | } | ||
| 126 | } | ||
| 127 | else // region 1 | ||
| 128 | { | ||
| 129 | fNumer = fA11 + fB1 - fA01 - fB0; | ||
| 130 | if(fNumer <= 0.0f) | ||
| 131 | { | ||
| 132 | fSqrDist = fA11+2.0f*fB1+fC; | ||
| 133 | } | ||
| 134 | else | ||
| 135 | { | ||
| 136 | fDenom = fA00-2.0f*fA01+fA11; | ||
| 137 | if(fNumer >= fDenom) | ||
| 138 | { | ||
| 139 | fSqrDist = fA00+2.0f*fB0+fC; | ||
| 140 | } | ||
| 141 | else | ||
| 142 | { | ||
| 143 | fS = fNumer/fDenom; | ||
| 144 | fT = 1.0f - fS; | ||
| 145 | fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC; | ||
| 146 | } | ||
| 147 | } | ||
| 148 | } | ||
| 149 | } | ||
| 150 | return fabsf(fSqrDist); | ||
| 151 | } | ||
| 152 | |||
| 153 | static float OPC_SegmentSegmentSqrDist(const Segment& rkSeg0, const Segment& rkSeg1) | ||
| 154 | { | ||
| 155 | // Hook | ||
| 156 | Point rkSeg0Direction = rkSeg0.ComputeDirection(); | ||
| 157 | Point rkSeg1Direction = rkSeg1.ComputeDirection(); | ||
| 158 | |||
| 159 | Point kDiff = rkSeg0.mP0 - rkSeg1.mP0; | ||
| 160 | float fA00 = rkSeg0Direction.SquareMagnitude(); | ||
| 161 | float fA01 = -rkSeg0Direction.Dot(rkSeg1Direction); | ||
| 162 | float fA11 = rkSeg1Direction.SquareMagnitude(); | ||
| 163 | float fB0 = kDiff.Dot(rkSeg0Direction); | ||
| 164 | float fC = kDiff.SquareMagnitude(); | ||
| 165 | float fDet = fabsf(fA00*fA11-fA01*fA01); | ||
| 166 | |||
| 167 | float fB1, fS, fT, fSqrDist, fTmp; | ||
| 168 | |||
| 169 | if(fDet>=gs_fTolerance) | ||
| 170 | { | ||
| 171 | // line segments are not parallel | ||
| 172 | fB1 = -kDiff.Dot(rkSeg1Direction); | ||
| 173 | fS = fA01*fB1-fA11*fB0; | ||
| 174 | fT = fA01*fB0-fA00*fB1; | ||
| 175 | |||
| 176 | if(fS >= 0.0f) | ||
| 177 | { | ||
| 178 | if(fS <= fDet) | ||
| 179 | { | ||
| 180 | if(fT >= 0.0f) | ||
| 181 | { | ||
| 182 | if(fT <= fDet) // region 0 (interior) | ||
| 183 | { | ||
| 184 | // minimum at two interior points of 3D lines | ||
| 185 | float fInvDet = 1.0f/fDet; | ||
| 186 | fS *= fInvDet; | ||
| 187 | fT *= fInvDet; | ||
| 188 | fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC; | ||
| 189 | } | ||
| 190 | else // region 3 (side) | ||
| 191 | { | ||
| 192 | fTmp = fA01+fB0; | ||
| 193 | if(fTmp>=0.0f) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 194 | else if(-fTmp>=fA00) fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp); | ||
| 195 | else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC; | ||
| 196 | } | ||
| 197 | } | ||
| 198 | else // region 7 (side) | ||
| 199 | { | ||
| 200 | if(fB0>=0.0f) fSqrDist = fC; | ||
| 201 | else if(-fB0>=fA00) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 202 | else fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 203 | } | ||
| 204 | } | ||
| 205 | else | ||
| 206 | { | ||
| 207 | if ( fT >= 0.0 ) | ||
| 208 | { | ||
| 209 | if ( fT <= fDet ) // region 1 (side) | ||
| 210 | { | ||
| 211 | fTmp = fA01+fB1; | ||
| 212 | if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 213 | else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp); | ||
| 214 | else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC; | ||
| 215 | } | ||
| 216 | else // region 2 (corner) | ||
| 217 | { | ||
| 218 | fTmp = fA01+fB0; | ||
| 219 | if ( -fTmp <= fA00 ) | ||
| 220 | { | ||
| 221 | if(fTmp>=0.0f) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 222 | else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC; | ||
| 223 | } | ||
| 224 | else | ||
| 225 | { | ||
| 226 | fTmp = fA01+fB1; | ||
| 227 | if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 228 | else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp); | ||
| 229 | else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC; | ||
| 230 | } | ||
| 231 | } | ||
| 232 | } | ||
| 233 | else // region 8 (corner) | ||
| 234 | { | ||
| 235 | if ( -fB0 < fA00 ) | ||
| 236 | { | ||
| 237 | if(fB0>=0.0f) fSqrDist = fC; | ||
| 238 | else fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 239 | } | ||
| 240 | else | ||
| 241 | { | ||
| 242 | fTmp = fA01+fB1; | ||
| 243 | if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 244 | else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp); | ||
| 245 | else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC; | ||
| 246 | } | ||
| 247 | } | ||
| 248 | } | ||
| 249 | } | ||
| 250 | else | ||
| 251 | { | ||
| 252 | if ( fT >= 0.0f ) | ||
| 253 | { | ||
| 254 | if ( fT <= fDet ) // region 5 (side) | ||
| 255 | { | ||
| 256 | if(fB1>=0.0f) fSqrDist = fC; | ||
| 257 | else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 258 | else fSqrDist = fB1*(-fB1/fA11)+fC; | ||
| 259 | } | ||
| 260 | else // region 4 (corner) | ||
| 261 | { | ||
| 262 | fTmp = fA01+fB0; | ||
| 263 | if ( fTmp < 0.0f ) | ||
| 264 | { | ||
| 265 | if(-fTmp>=fA00) fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp); | ||
| 266 | else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC; | ||
| 267 | } | ||
| 268 | else | ||
| 269 | { | ||
| 270 | if(fB1>=0.0f) fSqrDist = fC; | ||
| 271 | else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 272 | else fSqrDist = fB1*(-fB1/fA11)+fC; | ||
| 273 | } | ||
| 274 | } | ||
| 275 | } | ||
| 276 | else // region 6 (corner) | ||
| 277 | { | ||
| 278 | if ( fB0 < 0.0f ) | ||
| 279 | { | ||
| 280 | if(-fB0>=fA00) fSqrDist = fA00+2.0f*fB0+fC; | ||
| 281 | else fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 282 | } | ||
| 283 | else | ||
| 284 | { | ||
| 285 | if(fB1>=0.0f) fSqrDist = fC; | ||
| 286 | else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC; | ||
| 287 | else fSqrDist = fB1*(-fB1/fA11)+fC; | ||
| 288 | } | ||
| 289 | } | ||
| 290 | } | ||
| 291 | } | ||
| 292 | else | ||
| 293 | { | ||
| 294 | // line segments are parallel | ||
| 295 | if ( fA01 > 0.0f ) | ||
| 296 | { | ||
| 297 | // direction vectors form an obtuse angle | ||
| 298 | if ( fB0 >= 0.0f ) | ||
| 299 | { | ||
| 300 | fSqrDist = fC; | ||
| 301 | } | ||
| 302 | else if ( -fB0 <= fA00 ) | ||
| 303 | { | ||
| 304 | fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 305 | } | ||
| 306 | else | ||
| 307 | { | ||
| 308 | fB1 = -kDiff.Dot(rkSeg1Direction); | ||
| 309 | fTmp = fA00+fB0; | ||
| 310 | if ( -fTmp >= fA01 ) | ||
| 311 | { | ||
| 312 | fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1); | ||
| 313 | } | ||
| 314 | else | ||
| 315 | { | ||
| 316 | fT = -fTmp/fA01; | ||
| 317 | fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1)); | ||
| 318 | } | ||
| 319 | } | ||
| 320 | } | ||
| 321 | else | ||
| 322 | { | ||
| 323 | // direction vectors form an acute angle | ||
| 324 | if ( -fB0 >= fA00 ) | ||
| 325 | { | ||
| 326 | fSqrDist = fA00+2.0f*fB0+fC; | ||
| 327 | } | ||
| 328 | else if ( fB0 <= 0.0f ) | ||
| 329 | { | ||
| 330 | fSqrDist = fB0*(-fB0/fA00)+fC; | ||
| 331 | } | ||
| 332 | else | ||
| 333 | { | ||
| 334 | fB1 = -kDiff.Dot(rkSeg1Direction); | ||
| 335 | if ( fB0 >= -fA01 ) | ||
| 336 | { | ||
| 337 | fSqrDist = fA11+2.0f*fB1+fC; | ||
| 338 | } | ||
| 339 | else | ||
| 340 | { | ||
| 341 | fT = -fB0/fA01; | ||
| 342 | fSqrDist = fC+fT*(2.0f*fB1+fA11*fT); | ||
| 343 | } | ||
| 344 | } | ||
| 345 | } | ||
| 346 | } | ||
| 347 | return fabsf(fSqrDist); | ||
| 348 | } | ||
| 349 | |||
| 350 | inline_ float OPC_SegmentRaySqrDist(const Segment& rkSeg0, const Ray& rkSeg1) | ||
| 351 | { | ||
| 352 | return OPC_SegmentSegmentSqrDist(rkSeg0, Segment(rkSeg1.mOrig, rkSeg1.mOrig + rkSeg1.mDir)); | ||
| 353 | } | ||
| 354 | |||
| 355 | static float OPC_SegmentTriangleSqrDist(const Segment& segment, const Point& p0, const Point& p1, const Point& p2) | ||
| 356 | { | ||
| 357 | // Hook | ||
| 358 | const Point TriEdge0 = p1 - p0; | ||
| 359 | const Point TriEdge1 = p2 - p0; | ||
| 360 | |||
| 361 | const Point& rkSegOrigin = segment.GetOrigin(); | ||
| 362 | Point rkSegDirection = segment.ComputeDirection(); | ||
| 363 | |||
| 364 | Point kDiff = p0 - rkSegOrigin; | ||
| 365 | float fA00 = rkSegDirection.SquareMagnitude(); | ||
| 366 | float fA01 = -rkSegDirection.Dot(TriEdge0); | ||
| 367 | float fA02 = -rkSegDirection.Dot(TriEdge1); | ||
| 368 | float fA11 = TriEdge0.SquareMagnitude(); | ||
| 369 | float fA12 = TriEdge0.Dot(TriEdge1); | ||
| 370 | float fA22 = TriEdge1.Dot(TriEdge1); | ||
| 371 | float fB0 = -kDiff.Dot(rkSegDirection); | ||
| 372 | float fB1 = kDiff.Dot(TriEdge0); | ||
| 373 | float fB2 = kDiff.Dot(TriEdge1); | ||
| 374 | float fCof00 = fA11*fA22-fA12*fA12; | ||
| 375 | float fCof01 = fA02*fA12-fA01*fA22; | ||
| 376 | float fCof02 = fA01*fA12-fA02*fA11; | ||
| 377 | float fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02; | ||
| 378 | |||
| 379 | Ray kTriSeg; | ||
| 380 | Point kPt; | ||
| 381 | float fSqrDist, fSqrDist0; | ||
| 382 | |||
| 383 | if(fabsf(fDet)>=gs_fTolerance) | ||
| 384 | { | ||
| 385 | float fCof11 = fA00*fA22-fA02*fA02; | ||
| 386 | float fCof12 = fA02*fA01-fA00*fA12; | ||
| 387 | float fCof22 = fA00*fA11-fA01*fA01; | ||
| 388 | float fInvDet = 1.0f/fDet; | ||
| 389 | float fRhs0 = -fB0*fInvDet; | ||
| 390 | float fRhs1 = -fB1*fInvDet; | ||
| 391 | float fRhs2 = -fB2*fInvDet; | ||
| 392 | |||
| 393 | float fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2; | ||
| 394 | float fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2; | ||
| 395 | float fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2; | ||
| 396 | |||
| 397 | if ( fR < 0.0f ) | ||
| 398 | { | ||
| 399 | if ( fS+fT <= 1.0f ) | ||
| 400 | { | ||
| 401 | if ( fS < 0.0f ) | ||
| 402 | { | ||
| 403 | if ( fT < 0.0f ) // region 4m | ||
| 404 | { | ||
| 405 | // min on face s=0 or t=0 or r=0 | ||
| 406 | kTriSeg.mOrig = p0; | ||
| 407 | kTriSeg.mDir = TriEdge1; | ||
| 408 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 409 | kTriSeg.mOrig = p0; | ||
| 410 | kTriSeg.mDir = TriEdge0; | ||
| 411 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 412 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 413 | fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 414 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 415 | } | ||
| 416 | else // region 3m | ||
| 417 | { | ||
| 418 | // min on face s=0 or r=0 | ||
| 419 | kTriSeg.mOrig = p0; | ||
| 420 | kTriSeg.mDir = TriEdge1; | ||
| 421 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 422 | fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 423 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 424 | } | ||
| 425 | } | ||
| 426 | else if ( fT < 0.0f ) // region 5m | ||
| 427 | { | ||
| 428 | // min on face t=0 or r=0 | ||
| 429 | kTriSeg.mOrig = p0; | ||
| 430 | kTriSeg.mDir = TriEdge0; | ||
| 431 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 432 | fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 433 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 434 | } | ||
| 435 | else // region 0m | ||
| 436 | { | ||
| 437 | // min on face r=0 | ||
| 438 | fSqrDist = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 439 | } | ||
| 440 | } | ||
| 441 | else | ||
| 442 | { | ||
| 443 | if ( fS < 0.0f ) // region 2m | ||
| 444 | { | ||
| 445 | // min on face s=0 or s+t=1 or r=0 | ||
| 446 | kTriSeg.mOrig = p0; | ||
| 447 | kTriSeg.mDir = TriEdge1; | ||
| 448 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 449 | kTriSeg.mOrig = p1; | ||
| 450 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 451 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 452 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 453 | fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 454 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 455 | } | ||
| 456 | else if ( fT < 0.0f ) // region 6m | ||
| 457 | { | ||
| 458 | // min on face t=0 or s+t=1 or r=0 | ||
| 459 | kTriSeg.mOrig = p0; | ||
| 460 | kTriSeg.mDir = TriEdge0; | ||
| 461 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 462 | kTriSeg.mOrig = p1; | ||
| 463 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 464 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 465 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 466 | fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 467 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 468 | } | ||
| 469 | else // region 1m | ||
| 470 | { | ||
| 471 | // min on face s+t=1 or r=0 | ||
| 472 | kTriSeg.mOrig = p1; | ||
| 473 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 474 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 475 | fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 476 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 477 | } | ||
| 478 | } | ||
| 479 | } | ||
| 480 | else if ( fR <= 1.0f ) | ||
| 481 | { | ||
| 482 | if ( fS+fT <= 1.0f ) | ||
| 483 | { | ||
| 484 | if ( fS < 0.0f ) | ||
| 485 | { | ||
| 486 | if ( fT < 0.0f ) // region 4 | ||
| 487 | { | ||
| 488 | // min on face s=0 or t=0 | ||
| 489 | kTriSeg.mOrig = p0; | ||
| 490 | kTriSeg.mDir = TriEdge1; | ||
| 491 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 492 | kTriSeg.mOrig = p0; | ||
| 493 | kTriSeg.mDir = TriEdge0; | ||
| 494 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 495 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 496 | } | ||
| 497 | else // region 3 | ||
| 498 | { | ||
| 499 | // min on face s=0 | ||
| 500 | kTriSeg.mOrig = p0; | ||
| 501 | kTriSeg.mDir = TriEdge1; | ||
| 502 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 503 | } | ||
| 504 | } | ||
| 505 | else if ( fT < 0.0f ) // region 5 | ||
| 506 | { | ||
| 507 | // min on face t=0 | ||
| 508 | kTriSeg.mOrig = p0; | ||
| 509 | kTriSeg.mDir = TriEdge0; | ||
| 510 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 511 | } | ||
| 512 | else // region 0 | ||
| 513 | { | ||
| 514 | // global minimum is interior, done | ||
| 515 | fSqrDist = fR*(fA00*fR+fA01*fS+fA02*fT+2.0f*fB0) | ||
| 516 | +fS*(fA01*fR+fA11*fS+fA12*fT+2.0f*fB1) | ||
| 517 | +fT*(fA02*fR+fA12*fS+fA22*fT+2.0f*fB2) | ||
| 518 | +kDiff.SquareMagnitude(); | ||
| 519 | } | ||
| 520 | } | ||
| 521 | else | ||
| 522 | { | ||
| 523 | if ( fS < 0.0f ) // region 2 | ||
| 524 | { | ||
| 525 | // min on face s=0 or s+t=1 | ||
| 526 | kTriSeg.mOrig = p0; | ||
| 527 | kTriSeg.mDir = TriEdge1; | ||
| 528 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 529 | kTriSeg.mOrig = p1; | ||
| 530 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 531 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 532 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 533 | } | ||
| 534 | else if ( fT < 0.0f ) // region 6 | ||
| 535 | { | ||
| 536 | // min on face t=0 or s+t=1 | ||
| 537 | kTriSeg.mOrig = p0; | ||
| 538 | kTriSeg.mDir = TriEdge0; | ||
| 539 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 540 | kTriSeg.mOrig = p1; | ||
| 541 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 542 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 543 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 544 | } | ||
| 545 | else // region 1 | ||
| 546 | { | ||
| 547 | // min on face s+t=1 | ||
| 548 | kTriSeg.mOrig = p1; | ||
| 549 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 550 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 551 | } | ||
| 552 | } | ||
| 553 | } | ||
| 554 | else // fR > 1 | ||
| 555 | { | ||
| 556 | if ( fS+fT <= 1.0f ) | ||
| 557 | { | ||
| 558 | if ( fS < 0.0f ) | ||
| 559 | { | ||
| 560 | if ( fT < 0.0f ) // region 4p | ||
| 561 | { | ||
| 562 | // min on face s=0 or t=0 or r=1 | ||
| 563 | kTriSeg.mOrig = p0; | ||
| 564 | kTriSeg.mDir = TriEdge1; | ||
| 565 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 566 | kTriSeg.mOrig = p0; | ||
| 567 | kTriSeg.mDir = TriEdge0; | ||
| 568 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 569 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 570 | kPt = rkSegOrigin+rkSegDirection; | ||
| 571 | fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 572 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 573 | } | ||
| 574 | else // region 3p | ||
| 575 | { | ||
| 576 | // min on face s=0 or r=1 | ||
| 577 | kTriSeg.mOrig = p0; | ||
| 578 | kTriSeg.mDir = TriEdge1; | ||
| 579 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 580 | kPt = rkSegOrigin+rkSegDirection; | ||
| 581 | fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 582 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 583 | } | ||
| 584 | } | ||
| 585 | else if ( fT < 0.0f ) // region 5p | ||
| 586 | { | ||
| 587 | // min on face t=0 or r=1 | ||
| 588 | kTriSeg.mOrig = p0; | ||
| 589 | kTriSeg.mDir = TriEdge0; | ||
| 590 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 591 | kPt = rkSegOrigin+rkSegDirection; | ||
| 592 | fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 593 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 594 | } | ||
| 595 | else // region 0p | ||
| 596 | { | ||
| 597 | // min face on r=1 | ||
| 598 | kPt = rkSegOrigin+rkSegDirection; | ||
| 599 | fSqrDist = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 600 | } | ||
| 601 | } | ||
| 602 | else | ||
| 603 | { | ||
| 604 | if ( fS < 0.0f ) // region 2p | ||
| 605 | { | ||
| 606 | // min on face s=0 or s+t=1 or r=1 | ||
| 607 | kTriSeg.mOrig = p0; | ||
| 608 | kTriSeg.mDir = TriEdge1; | ||
| 609 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 610 | kTriSeg.mOrig = p1; | ||
| 611 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 612 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 613 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 614 | kPt = rkSegOrigin+rkSegDirection; | ||
| 615 | fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 616 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 617 | } | ||
| 618 | else if ( fT < 0.0f ) // region 6p | ||
| 619 | { | ||
| 620 | // min on face t=0 or s+t=1 or r=1 | ||
| 621 | kTriSeg.mOrig = p0; | ||
| 622 | kTriSeg.mDir = TriEdge0; | ||
| 623 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 624 | kTriSeg.mOrig = p1; | ||
| 625 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 626 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 627 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 628 | kPt = rkSegOrigin+rkSegDirection; | ||
| 629 | fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 630 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 631 | } | ||
| 632 | else // region 1p | ||
| 633 | { | ||
| 634 | // min on face s+t=1 or r=1 | ||
| 635 | kTriSeg.mOrig = p1; | ||
| 636 | kTriSeg.mDir = TriEdge1-TriEdge0; | ||
| 637 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 638 | kPt = rkSegOrigin+rkSegDirection; | ||
| 639 | fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 640 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 641 | } | ||
| 642 | } | ||
| 643 | } | ||
| 644 | } | ||
| 645 | else | ||
| 646 | { | ||
| 647 | // segment and triangle are parallel | ||
| 648 | kTriSeg.mOrig = p0; | ||
| 649 | kTriSeg.mDir = TriEdge0; | ||
| 650 | fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 651 | |||
| 652 | kTriSeg.mDir = TriEdge1; | ||
| 653 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 654 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 655 | |||
| 656 | kTriSeg.mOrig = p1; | ||
| 657 | kTriSeg.mDir = TriEdge1 - TriEdge0; | ||
| 658 | fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg); | ||
| 659 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 660 | |||
| 661 | fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2); | ||
| 662 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 663 | |||
| 664 | kPt = rkSegOrigin+rkSegDirection; | ||
| 665 | fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2); | ||
| 666 | if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0; | ||
| 667 | } | ||
| 668 | return fabsf(fSqrDist); | ||
| 669 | } | ||
| 670 | |||
| 671 | inline_ BOOL LSSCollider::LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2) | ||
| 672 | { | ||
| 673 | // Stats | ||
| 674 | mNbVolumePrimTests++; | ||
| 675 | |||
| 676 | float s2 = OPC_SegmentTriangleSqrDist(mSeg, vert0, vert1, vert2); | ||
| 677 | if(s2<mRadius2) return TRUE; | ||
| 678 | return FALSE; | ||
| 679 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_MeshInterface.cpp b/libraries/ode-0.9/OPCODE/OPC_MeshInterface.cpp new file mode 100644 index 0000000..b962764 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_MeshInterface.cpp | |||
| @@ -0,0 +1,303 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains a mesh interface. | ||
| 12 | * \file OPC_MeshInterface.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date November, 27, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * This structure holds 3 vertex-pointers. It's mainly used by collision callbacks so that the app doesn't have | ||
| 21 | * to return 3 vertices to OPCODE (36 bytes) but only 3 pointers (12 bytes). It seems better but I never profiled | ||
| 22 | * the alternative. | ||
| 23 | * | ||
| 24 | * \class VertexPointers | ||
| 25 | * \author Pierre Terdiman | ||
| 26 | * \version 1.3 | ||
| 27 | * \date March, 20, 2001 | ||
| 28 | */ | ||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | |||
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 32 | /** | ||
| 33 | * This class is an interface between us and user-defined meshes. Meshes can be defined in a lot of ways, and here we | ||
| 34 | * try to support most of them. | ||
| 35 | * | ||
| 36 | * Basically you have two options: | ||
| 37 | * - callbacks, if OPC_USE_CALLBACKS is defined in OPC_Settings.h. | ||
| 38 | * - else pointers. | ||
| 39 | * | ||
| 40 | * If using pointers, you can also use strides or not. Strides are used when OPC_USE_STRIDE is defined. | ||
| 41 | * | ||
| 42 | * | ||
| 43 | * CALLBACKS: | ||
| 44 | * | ||
| 45 | * Using callbacks is the most generic way to feed OPCODE with your meshes. Indeed, you just have to give | ||
| 46 | * access to three vertices at the end of the day. It's up to you to fetch them from your database, using | ||
| 47 | * whatever method you want. Hence your meshes can lie in system memory or AGP, be indexed or not, use 16 | ||
| 48 | * or 32-bits indices, you can decompress them on-the-fly if needed, etc. On the other hand, a callback is | ||
| 49 | * called each time OPCODE needs access to a particular triangle, so there might be a slight overhead. | ||
| 50 | * | ||
| 51 | * To make things clear: geometry & topology are NOT stored in the collision system, | ||
| 52 | * in order to save some ram. So, when the system needs them to perform accurate intersection | ||
| 53 | * tests, you're requested to provide the triangle-vertices corresponding to a given face index. | ||
| 54 | * | ||
| 55 | * Ex: | ||
| 56 | * | ||
| 57 | * \code | ||
| 58 | * static void ColCallback(udword triangle_index, VertexPointers& triangle, udword user_data) | ||
| 59 | * { | ||
| 60 | * // Get back Mesh0 or Mesh1 (you also can use 2 different callbacks) | ||
| 61 | * Mesh* MyMesh = (Mesh*)user_data; | ||
| 62 | * // Get correct triangle in the app-controlled database | ||
| 63 | * const Triangle* Tri = MyMesh->GetTriangle(triangle_index); | ||
| 64 | * // Setup pointers to vertices for the collision system | ||
| 65 | * triangle.Vertex[0] = MyMesh->GetVertex(Tri->mVRef[0]); | ||
| 66 | * triangle.Vertex[1] = MyMesh->GetVertex(Tri->mVRef[1]); | ||
| 67 | * triangle.Vertex[2] = MyMesh->GetVertex(Tri->mVRef[2]); | ||
| 68 | * } | ||
| 69 | * | ||
| 70 | * // Setup callbacks | ||
| 71 | * MeshInterface0->SetCallback(ColCallback, udword(Mesh0)); | ||
| 72 | * MeshInterface1->SetCallback(ColCallback, udword(Mesh1)); | ||
| 73 | * \endcode | ||
| 74 | * | ||
| 75 | * Of course, you should make this callback as fast as possible. And you're also not supposed | ||
| 76 | * to modify the geometry *after* the collision trees have been built. The alternative was to | ||
| 77 | * store the geometry & topology in the collision system as well (as in RAPID) but we have found | ||
| 78 | * this approach to waste a lot of ram in many cases. | ||
| 79 | * | ||
| 80 | * | ||
| 81 | * POINTERS: | ||
| 82 | * | ||
| 83 | * If you're internally using the following canonical structures: | ||
| 84 | * - a vertex made of three 32-bits floating point values | ||
| 85 | * - a triangle made of three 32-bits integer vertex references | ||
| 86 | * ...then you may want to use pointers instead of callbacks. This is the same, except OPCODE will directly | ||
| 87 | * use provided pointers to access the topology and geometry, without using a callback. It might be faster, | ||
| 88 | * but probably not as safe. Pointers have been introduced in OPCODE 1.2. | ||
| 89 | * | ||
| 90 | * Ex: | ||
| 91 | * | ||
| 92 | * \code | ||
| 93 | * // Setup pointers | ||
| 94 | * MeshInterface0->SetPointers(Mesh0->GetFaces(), Mesh0->GetVerts()); | ||
| 95 | * MeshInterface1->SetPointers(Mesh1->GetFaces(), Mesh1->GetVerts()); | ||
| 96 | * \endcode | ||
| 97 | * | ||
| 98 | * | ||
| 99 | * STRIDES: | ||
| 100 | * | ||
| 101 | * If your vertices are D3D-like entities interleaving a position, a normal and/or texture coordinates | ||
| 102 | * (i.e. if your vertices are FVFs), you might want to use a vertex stride to skip extra data OPCODE | ||
| 103 | * doesn't need. Using a stride shouldn't be notably slower than not using it, but it might increase | ||
| 104 | * cache misses. Please also note that you *shouldn't* read from AGP or video-memory buffers ! | ||
| 105 | * | ||
| 106 | * | ||
| 107 | * In any case, compilation flags are here to select callbacks/pointers/strides at compile time, so | ||
| 108 | * choose what's best for your application. All of this has been wrapped into this MeshInterface. | ||
| 109 | * | ||
| 110 | * \class MeshInterface | ||
| 111 | * \author Pierre Terdiman | ||
| 112 | * \version 1.3 | ||
| 113 | * \date November, 27, 2002 | ||
| 114 | */ | ||
| 115 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 116 | |||
| 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 118 | // Precompiled Header | ||
| 119 | #include "Stdafx.h" | ||
| 120 | |||
| 121 | using namespace Opcode; | ||
| 122 | |||
| 123 | Point MeshInterface::VertexCache[3]; | ||
| 124 | |||
| 125 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 126 | /** | ||
| 127 | * Constructor. | ||
| 128 | */ | ||
| 129 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 130 | MeshInterface::MeshInterface() : | ||
| 131 | #ifdef OPC_USE_CALLBACKS | ||
| 132 | mUserData (null), | ||
| 133 | mObjCallback (null), | ||
| 134 | #else | ||
| 135 | mTris (null), | ||
| 136 | mVerts (null), | ||
| 137 | #ifdef OPC_USE_STRIDE | ||
| 138 | mTriStride (sizeof(IndexedTriangle)), | ||
| 139 | mVertexStride (sizeof(Point)), | ||
| 140 | #endif | ||
| 141 | #endif | ||
| 142 | mNbTris (0), | ||
| 143 | mNbVerts (0), | ||
| 144 | |||
| 145 | Single(true) | ||
| 146 | { | ||
| 147 | } | ||
| 148 | |||
| 149 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 150 | /** | ||
| 151 | * Destructor. | ||
| 152 | */ | ||
| 153 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 154 | MeshInterface::~MeshInterface() | ||
| 155 | { | ||
| 156 | } | ||
| 157 | |||
| 158 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 159 | /** | ||
| 160 | * Checks the mesh interface is valid, i.e. things have been setup correctly. | ||
| 161 | * \return true if valid | ||
| 162 | */ | ||
| 163 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 164 | bool MeshInterface::IsValid() const | ||
| 165 | { | ||
| 166 | if(!mNbTris || !mNbVerts) return false; | ||
| 167 | #ifdef OPC_USE_CALLBACKS | ||
| 168 | if(!mObjCallback) return false; | ||
| 169 | #else | ||
| 170 | if(!mTris || !mVerts) return false; | ||
| 171 | #endif | ||
| 172 | return true; | ||
| 173 | } | ||
| 174 | |||
| 175 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 176 | /** | ||
| 177 | * Checks the mesh itself is valid. | ||
| 178 | * Currently we only look for degenerate faces. | ||
| 179 | * \return number of degenerate faces | ||
| 180 | */ | ||
| 181 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 182 | udword MeshInterface::CheckTopology() const | ||
| 183 | { | ||
| 184 | // Check topology. If the model contains degenerate faces, collision report can be wrong in some cases. | ||
| 185 | // e.g. it happens with the standard MAX teapot. So clean your meshes first... If you don't have a mesh cleaner | ||
| 186 | // you can try this: www.codercorner.com/Consolidation.zip | ||
| 187 | |||
| 188 | udword NbDegenerate = 0; | ||
| 189 | |||
| 190 | VertexPointers VP; | ||
| 191 | |||
| 192 | // Using callbacks, we don't have access to vertex indices. Nevertheless we still can check for | ||
| 193 | // redundant vertex pointers, which cover all possibilities (callbacks/pointers/strides). | ||
| 194 | for(udword i=0;i<mNbTris;i++) | ||
| 195 | { | ||
| 196 | GetTriangle(VP, i); | ||
| 197 | |||
| 198 | if( (VP.Vertex[0]==VP.Vertex[1]) | ||
| 199 | || (VP.Vertex[1]==VP.Vertex[2]) | ||
| 200 | || (VP.Vertex[2]==VP.Vertex[0])) NbDegenerate++; | ||
| 201 | } | ||
| 202 | |||
| 203 | return NbDegenerate; | ||
| 204 | } | ||
| 205 | |||
| 206 | #ifdef OPC_USE_CALLBACKS | ||
| 207 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 208 | /** | ||
| 209 | * Callback control: setups object callback. Must provide triangle-vertices for a given triangle index. | ||
| 210 | * \param callback [in] user-defined callback | ||
| 211 | * \param user_data [in] user-defined data | ||
| 212 | * \return true if success | ||
| 213 | */ | ||
| 214 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 215 | bool MeshInterface::SetCallback(RequestCallback callback, void* user_data) | ||
| 216 | { | ||
| 217 | if(!callback) return SetIceError("MeshInterface::SetCallback: callback pointer is null"); | ||
| 218 | |||
| 219 | mObjCallback = callback; | ||
| 220 | mUserData = user_data; | ||
| 221 | return true; | ||
| 222 | } | ||
| 223 | #else | ||
| 224 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 225 | /** | ||
| 226 | * Pointers control: setups object pointers. Must provide access to faces and vertices for a given object. | ||
| 227 | * \param tris [in] pointer to triangles | ||
| 228 | * \param verts [in] pointer to vertices | ||
| 229 | * \return true if success | ||
| 230 | */ | ||
| 231 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 232 | bool MeshInterface::SetPointers(const IndexedTriangle* tris, const Point* verts) | ||
| 233 | { | ||
| 234 | if(!tris || !verts) return SetIceError("MeshInterface::SetPointers: pointer is null", null); | ||
| 235 | |||
| 236 | mTris = tris; | ||
| 237 | mVerts = verts; | ||
| 238 | return true; | ||
| 239 | } | ||
| 240 | #ifdef OPC_USE_STRIDE | ||
| 241 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 242 | /** | ||
| 243 | * Strides control | ||
| 244 | * \param tri_stride [in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices. | ||
| 245 | * \param vertex_stride [in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position. | ||
| 246 | * \return true if success | ||
| 247 | */ | ||
| 248 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 249 | bool MeshInterface::SetStrides(udword tri_stride, udword vertex_stride) | ||
| 250 | { | ||
| 251 | if(tri_stride<sizeof(IndexedTriangle)) return SetIceError("MeshInterface::SetStrides: invalid triangle stride", null); | ||
| 252 | if(vertex_stride<sizeof(Point)) return SetIceError("MeshInterface::SetStrides: invalid vertex stride", null); | ||
| 253 | |||
| 254 | mTriStride = tri_stride; | ||
| 255 | mVertexStride = vertex_stride; | ||
| 256 | return true; | ||
| 257 | } | ||
| 258 | #endif | ||
| 259 | #endif | ||
| 260 | |||
| 261 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 262 | /** | ||
| 263 | * Remaps client's mesh according to a permutation. | ||
| 264 | * \param nb_indices [in] number of indices in the permutation (will be checked against number of triangles) | ||
| 265 | * \param permutation [in] list of triangle indices | ||
| 266 | * \return true if success | ||
| 267 | */ | ||
| 268 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 269 | bool MeshInterface::RemapClient(udword nb_indices, const udword* permutation) const | ||
| 270 | { | ||
| 271 | // Checkings | ||
| 272 | if(!nb_indices || !permutation) return false; | ||
| 273 | if(nb_indices!=mNbTris) return false; | ||
| 274 | |||
| 275 | #ifdef OPC_USE_CALLBACKS | ||
| 276 | // We can't really do that using callbacks | ||
| 277 | return false; | ||
| 278 | #else | ||
| 279 | IndexedTriangle* Tmp = new IndexedTriangle[mNbTris]; | ||
| 280 | CHECKALLOC(Tmp); | ||
| 281 | |||
| 282 | #ifdef OPC_USE_STRIDE | ||
| 283 | udword Stride = mTriStride; | ||
| 284 | #else | ||
| 285 | udword Stride = sizeof(IndexedTriangle); | ||
| 286 | #endif | ||
| 287 | |||
| 288 | for(udword i=0;i<mNbTris;i++) | ||
| 289 | { | ||
| 290 | const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + i * Stride); | ||
| 291 | Tmp[i] = *T; | ||
| 292 | } | ||
| 293 | |||
| 294 | for(udword i=0;i<mNbTris;i++) | ||
| 295 | { | ||
| 296 | IndexedTriangle* T = (IndexedTriangle*)(((ubyte*)mTris) + i * Stride); | ||
| 297 | *T = Tmp[permutation[i]]; | ||
| 298 | } | ||
| 299 | |||
| 300 | DELETEARRAY(Tmp); | ||
| 301 | #endif | ||
| 302 | return true; | ||
| 303 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_MeshInterface.h b/libraries/ode-0.9/OPCODE/OPC_MeshInterface.h new file mode 100644 index 0000000..61142cf --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_MeshInterface.h | |||
| @@ -0,0 +1,199 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains a mesh interface. | ||
| 12 | * \file OPC_MeshInterface.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date November, 27, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_MESHINTERFACE_H__ | ||
| 21 | #define __OPC_MESHINTERFACE_H__ | ||
| 22 | |||
| 23 | struct VertexPointers | ||
| 24 | { | ||
| 25 | const Point* Vertex[3]; | ||
| 26 | |||
| 27 | bool BackfaceCulling(const Point& source) | ||
| 28 | { | ||
| 29 | const Point& p0 = *Vertex[0]; | ||
| 30 | const Point& p1 = *Vertex[1]; | ||
| 31 | const Point& p2 = *Vertex[2]; | ||
| 32 | |||
| 33 | // Compute normal direction | ||
| 34 | Point Normal = (p2 - p1)^(p0 - p1); | ||
| 35 | |||
| 36 | // Backface culling | ||
| 37 | return (Normal | (source - p0)) >= 0.0f; | ||
| 38 | } | ||
| 39 | }; | ||
| 40 | |||
| 41 | #ifdef OPC_USE_CALLBACKS | ||
| 42 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 43 | /** | ||
| 44 | * User-callback, called by OPCODE to request vertices from the app. | ||
| 45 | * \param triangle_index [in] face index for which the system is requesting the vertices | ||
| 46 | * \param triangle [out] triangle's vertices (must be provided by the user) | ||
| 47 | * \param user_data [in] user-defined data from SetCallback() | ||
| 48 | */ | ||
| 49 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 50 | typedef void (*RequestCallback) (udword triangle_index, VertexPointers& triangle, void* user_data); | ||
| 51 | #endif | ||
| 52 | |||
| 53 | class OPCODE_API MeshInterface | ||
| 54 | { | ||
| 55 | public: | ||
| 56 | // Constructor / Destructor | ||
| 57 | MeshInterface(); | ||
| 58 | ~MeshInterface(); | ||
| 59 | // Common settings | ||
| 60 | inline_ udword GetNbTriangles() const { return mNbTris; } | ||
| 61 | inline_ udword GetNbVertices() const { return mNbVerts; } | ||
| 62 | inline_ void SetNbTriangles(udword nb) { mNbTris = nb; } | ||
| 63 | inline_ void SetNbVertices(udword nb) { mNbVerts = nb; } | ||
| 64 | |||
| 65 | #ifdef OPC_USE_CALLBACKS | ||
| 66 | // Callback settings | ||
| 67 | |||
| 68 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 69 | /** | ||
| 70 | * Callback control: setups object callback. Must provide triangle-vertices for a given triangle index. | ||
| 71 | * \param callback [in] user-defined callback | ||
| 72 | * \param user_data [in] user-defined data | ||
| 73 | * \return true if success | ||
| 74 | */ | ||
| 75 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 76 | bool SetCallback(RequestCallback callback, void* user_data); | ||
| 77 | inline_ void* GetUserData() const { return mUserData; } | ||
| 78 | inline_ RequestCallback GetCallback() const { return mObjCallback; } | ||
| 79 | #else | ||
| 80 | // Pointers settings | ||
| 81 | |||
| 82 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 83 | /** | ||
| 84 | * Pointers control: setups object pointers. Must provide access to faces and vertices for a given object. | ||
| 85 | * \param tris [in] pointer to triangles | ||
| 86 | * \param verts [in] pointer to vertices | ||
| 87 | * \return true if success | ||
| 88 | */ | ||
| 89 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 90 | bool SetPointers(const IndexedTriangle* tris, const Point* verts); | ||
| 91 | inline_ const IndexedTriangle* GetTris() const { return mTris; } | ||
| 92 | inline_ const Point* GetVerts() const { return mVerts; } | ||
| 93 | |||
| 94 | #ifdef OPC_USE_STRIDE | ||
| 95 | // Strides settings | ||
| 96 | |||
| 97 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 98 | /** | ||
| 99 | * Strides control | ||
| 100 | * \param tri_stride [in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices. | ||
| 101 | * \param vertex_stride [in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position. | ||
| 102 | * \return true if success | ||
| 103 | */ | ||
| 104 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 105 | bool SetStrides(udword tri_stride=sizeof(IndexedTriangle), udword vertex_stride=sizeof(Point)); | ||
| 106 | inline_ udword GetTriStride() const { return mTriStride; } | ||
| 107 | inline_ udword GetVertexStride() const { return mVertexStride; } | ||
| 108 | #endif | ||
| 109 | #endif | ||
| 110 | |||
| 111 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 112 | /** | ||
| 113 | * Fetches a triangle given a triangle index. | ||
| 114 | * \param vp [out] required triangle's vertex pointers | ||
| 115 | * \param index [in] triangle index | ||
| 116 | */ | ||
| 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 118 | inline_ void GetTriangle(VertexPointers& vp, udword index) const | ||
| 119 | { | ||
| 120 | #ifdef OPC_USE_CALLBACKS | ||
| 121 | (mObjCallback)(index, vp, mUserData); | ||
| 122 | #else | ||
| 123 | #ifdef OPC_USE_STRIDE | ||
| 124 | const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride); | ||
| 125 | |||
| 126 | if (Single){ | ||
| 127 | vp.Vertex[0] = (const Point*)(((ubyte*)mVerts) + T->mVRef[0] * mVertexStride); | ||
| 128 | vp.Vertex[1] = (const Point*)(((ubyte*)mVerts) + T->mVRef[1] * mVertexStride); | ||
| 129 | vp.Vertex[2] = (const Point*)(((ubyte*)mVerts) + T->mVRef[2] * mVertexStride); | ||
| 130 | } | ||
| 131 | else{ | ||
| 132 | for (int i = 0; i < 3; i++){ | ||
| 133 | const double* v = (const double*)(((ubyte*)mVerts) + T->mVRef[i] * mVertexStride); | ||
| 134 | |||
| 135 | VertexCache[i].x = (float)v[0]; | ||
| 136 | VertexCache[i].y = (float)v[1]; | ||
| 137 | VertexCache[i].z = (float)v[2]; | ||
| 138 | vp.Vertex[i] = &VertexCache[i]; | ||
| 139 | } | ||
| 140 | } | ||
| 141 | #else | ||
| 142 | const IndexedTriangle* T = &mTris[index]; | ||
| 143 | vp.Vertex[0] = &mVerts[T->mVRef[0]]; | ||
| 144 | vp.Vertex[1] = &mVerts[T->mVRef[1]]; | ||
| 145 | vp.Vertex[2] = &mVerts[T->mVRef[2]]; | ||
| 146 | #endif | ||
| 147 | #endif | ||
| 148 | } | ||
| 149 | |||
| 150 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 151 | /** | ||
| 152 | * Remaps client's mesh according to a permutation. | ||
| 153 | * \param nb_indices [in] number of indices in the permutation (will be checked against number of triangles) | ||
| 154 | * \param permutation [in] list of triangle indices | ||
| 155 | * \return true if success | ||
| 156 | */ | ||
| 157 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 158 | bool RemapClient(udword nb_indices, const udword* permutation) const; | ||
| 159 | |||
| 160 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 161 | /** | ||
| 162 | * Checks the mesh interface is valid, i.e. things have been setup correctly. | ||
| 163 | * \return true if valid | ||
| 164 | */ | ||
| 165 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 166 | bool IsValid() const; | ||
| 167 | |||
| 168 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 169 | /** | ||
| 170 | * Checks the mesh itself is valid. | ||
| 171 | * Currently we only look for degenerate faces. | ||
| 172 | * \return number of degenerate faces | ||
| 173 | */ | ||
| 174 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 175 | udword CheckTopology() const; | ||
| 176 | private: | ||
| 177 | |||
| 178 | udword mNbTris; //!< Number of triangles in the input model | ||
| 179 | udword mNbVerts; //!< Number of vertices in the input model | ||
| 180 | #ifdef OPC_USE_CALLBACKS | ||
| 181 | // User callback | ||
| 182 | void* mUserData; //!< User-defined data sent to callback | ||
| 183 | RequestCallback mObjCallback; //!< Object callback | ||
| 184 | #else | ||
| 185 | // User pointers | ||
| 186 | const IndexedTriangle* mTris; //!< Array of indexed triangles | ||
| 187 | const Point* mVerts; //!< Array of vertices | ||
| 188 | #ifdef OPC_USE_STRIDE | ||
| 189 | udword mTriStride; //!< Possible triangle stride in bytes [Opcode 1.3] | ||
| 190 | udword mVertexStride; //!< Possible vertex stride in bytes [Opcode 1.3] | ||
| 191 | #endif | ||
| 192 | public: | ||
| 193 | bool Single; //!< Use single or double precision vertices | ||
| 194 | private: | ||
| 195 | static Point VertexCache[3]; | ||
| 196 | #endif | ||
| 197 | }; | ||
| 198 | |||
| 199 | #endif //__OPC_MESHINTERFACE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Model.cpp b/libraries/ode-0.9/OPCODE/OPC_Model.cpp new file mode 100644 index 0000000..8b71fb1 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Model.cpp | |||
| @@ -0,0 +1,222 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for OPCODE models. | ||
| 12 | * \file OPC_Model.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * The main collision wrapper, for all trees. Supported trees are: | ||
| 21 | * - Normal trees (2*N-1 nodes, full size) | ||
| 22 | * - No-leaf trees (N-1 nodes, full size) | ||
| 23 | * - Quantized trees (2*N-1 nodes, half size) | ||
| 24 | * - Quantized no-leaf trees (N-1 nodes, half size) | ||
| 25 | * | ||
| 26 | * Usage: | ||
| 27 | * | ||
| 28 | * 1) Create a static mesh interface using callbacks or pointers. (see OPC_MeshInterface.cpp). | ||
| 29 | * Keep it around in your app, since a pointer to this interface is saved internally and | ||
| 30 | * used until you release the collision structures. | ||
| 31 | * | ||
| 32 | * 2) Build a Model using a creation structure: | ||
| 33 | * | ||
| 34 | * \code | ||
| 35 | * Model Sample; | ||
| 36 | * | ||
| 37 | * OPCODECREATE OPCC; | ||
| 38 | * OPCC.IMesh = ...; | ||
| 39 | * OPCC.Rules = ...; | ||
| 40 | * OPCC.NoLeaf = ...; | ||
| 41 | * OPCC.Quantized = ...; | ||
| 42 | * OPCC.KeepOriginal = ...; | ||
| 43 | * bool Status = Sample.Build(OPCC); | ||
| 44 | * \endcode | ||
| 45 | * | ||
| 46 | * 3) Create a tree collider and set it up: | ||
| 47 | * | ||
| 48 | * \code | ||
| 49 | * AABBTreeCollider TC; | ||
| 50 | * TC.SetFirstContact(...); | ||
| 51 | * TC.SetFullBoxBoxTest(...); | ||
| 52 | * TC.SetFullPrimBoxTest(...); | ||
| 53 | * TC.SetTemporalCoherence(...); | ||
| 54 | * \endcode | ||
| 55 | * | ||
| 56 | * 4) Perform a collision query | ||
| 57 | * | ||
| 58 | * \code | ||
| 59 | * // Setup cache | ||
| 60 | * static BVTCache ColCache; | ||
| 61 | * ColCache.Model0 = &Model0; | ||
| 62 | * ColCache.Model1 = &Model1; | ||
| 63 | * | ||
| 64 | * // Collision query | ||
| 65 | * bool IsOk = TC.Collide(ColCache, World0, World1); | ||
| 66 | * | ||
| 67 | * // Get collision status => if true, objects overlap | ||
| 68 | * BOOL Status = TC.GetContactStatus(); | ||
| 69 | * | ||
| 70 | * // Number of colliding pairs and list of pairs | ||
| 71 | * udword NbPairs = TC.GetNbPairs(); | ||
| 72 | * const Pair* p = TC.GetPairs() | ||
| 73 | * \endcode | ||
| 74 | * | ||
| 75 | * 5) Stats | ||
| 76 | * | ||
| 77 | * \code | ||
| 78 | * Model0.GetUsedBytes() = number of bytes used for this collision tree | ||
| 79 | * TC.GetNbBVBVTests() = number of BV-BV overlap tests performed during last query | ||
| 80 | * TC.GetNbPrimPrimTests() = number of Triangle-Triangle overlap tests performed during last query | ||
| 81 | * TC.GetNbBVPrimTests() = number of Triangle-BV overlap tests performed during last query | ||
| 82 | * \endcode | ||
| 83 | * | ||
| 84 | * \class Model | ||
| 85 | * \author Pierre Terdiman | ||
| 86 | * \version 1.3 | ||
| 87 | * \date March, 20, 2001 | ||
| 88 | */ | ||
| 89 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 90 | |||
| 91 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 92 | // Precompiled Header | ||
| 93 | #include "Stdafx.h" | ||
| 94 | |||
| 95 | using namespace Opcode; | ||
| 96 | |||
| 97 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 98 | /** | ||
| 99 | * Constructor. | ||
| 100 | */ | ||
| 101 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 102 | Model::Model() | ||
| 103 | { | ||
| 104 | #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! | ||
| 105 | mHull = null; | ||
| 106 | #endif // __MESHMERIZER_H__ | ||
| 107 | } | ||
| 108 | |||
| 109 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 110 | /** | ||
| 111 | * Destructor. | ||
| 112 | */ | ||
| 113 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 114 | Model::~Model() | ||
| 115 | { | ||
| 116 | Release(); | ||
| 117 | } | ||
| 118 | |||
| 119 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 120 | /** | ||
| 121 | * Releases the model. | ||
| 122 | */ | ||
| 123 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 124 | void Model::Release() | ||
| 125 | { | ||
| 126 | ReleaseBase(); | ||
| 127 | #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! | ||
| 128 | DELETESINGLE(mHull); | ||
| 129 | #endif // __MESHMERIZER_H__ | ||
| 130 | } | ||
| 131 | |||
| 132 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 133 | /** | ||
| 134 | * Builds a collision model. | ||
| 135 | * \param create [in] model creation structure | ||
| 136 | * \return true if success | ||
| 137 | */ | ||
| 138 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 139 | bool Model::Build(const OPCODECREATE& create) | ||
| 140 | { | ||
| 141 | // 1) Checkings | ||
| 142 | if(!create.mIMesh || !create.mIMesh->IsValid()) return false; | ||
| 143 | |||
| 144 | // For this model, we only support complete trees | ||
| 145 | if(create.mSettings.mLimit!=1) return SetIceError("OPCODE WARNING: supports complete trees only! Use mLimit = 1.\n", null); | ||
| 146 | |||
| 147 | // Look for degenerate faces. | ||
| 148 | udword NbDegenerate = create.mIMesh->CheckTopology(); | ||
| 149 | if(NbDegenerate) Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate); | ||
| 150 | // We continue nonetheless.... | ||
| 151 | |||
| 152 | Release(); // Make sure previous tree has been discarded [Opcode 1.3, thanks Adam] | ||
| 153 | |||
| 154 | // 1-1) Setup mesh interface automatically [Opcode 1.3] | ||
| 155 | SetMeshInterface(create.mIMesh); | ||
| 156 | |||
| 157 | // Special case for 1-triangle meshes [Opcode 1.3] | ||
| 158 | udword NbTris = create.mIMesh->GetNbTriangles(); | ||
| 159 | if(NbTris==1) | ||
| 160 | { | ||
| 161 | // We don't need to actually create a tree here, since we'll only have a single triangle to deal with anyway. | ||
| 162 | // It's a waste to use a "model" for this but at least it will work. | ||
| 163 | mModelCode |= OPC_SINGLE_NODE; | ||
| 164 | return true; | ||
| 165 | } | ||
| 166 | |||
| 167 | // 2) Build a generic AABB Tree. | ||
| 168 | mSource = new AABBTree; | ||
| 169 | CHECKALLOC(mSource); | ||
| 170 | |||
| 171 | // 2-1) Setup a builder. Our primitives here are triangles from input mesh, | ||
| 172 | // so we use an AABBTreeOfTrianglesBuilder..... | ||
| 173 | { | ||
| 174 | AABBTreeOfTrianglesBuilder TB; | ||
| 175 | TB.mIMesh = create.mIMesh; | ||
| 176 | TB.mSettings = create.mSettings; | ||
| 177 | TB.mNbPrimitives = NbTris; | ||
| 178 | if(!mSource->Build(&TB)) return false; | ||
| 179 | } | ||
| 180 | |||
| 181 | // 3) Create an optimized tree according to user-settings | ||
| 182 | if(!CreateTree(create.mNoLeaf, create.mQuantized)) return false; | ||
| 183 | |||
| 184 | // 3-2) Create optimized tree | ||
| 185 | if(!mTree->Build(mSource)) return false; | ||
| 186 | |||
| 187 | // 3-3) Delete generic tree if needed | ||
| 188 | if(!create.mKeepOriginal) DELETESINGLE(mSource); | ||
| 189 | |||
| 190 | #ifdef __MESHMERIZER_H__ | ||
| 191 | // 4) Convex hull | ||
| 192 | if(create.mCollisionHull) | ||
| 193 | { | ||
| 194 | // Create hull | ||
| 195 | mHull = new CollisionHull; | ||
| 196 | CHECKALLOC(mHull); | ||
| 197 | |||
| 198 | CONVEXHULLCREATE CHC; | ||
| 199 | // ### doesn't work with strides | ||
| 200 | CHC.NbVerts = create.mIMesh->GetNbVertices(); | ||
| 201 | CHC.Vertices = create.mIMesh->GetVerts(); | ||
| 202 | CHC.UnifyNormals = true; | ||
| 203 | CHC.ReduceVertices = true; | ||
| 204 | CHC.WordFaces = false; | ||
| 205 | mHull->Compute(CHC); | ||
| 206 | } | ||
| 207 | #endif // __MESHMERIZER_H__ | ||
| 208 | |||
| 209 | return true; | ||
| 210 | } | ||
| 211 | |||
| 212 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 213 | /** | ||
| 214 | * Gets the number of bytes used by the tree. | ||
| 215 | * \return amount of bytes used | ||
| 216 | */ | ||
| 217 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 218 | udword Model::GetUsedBytes() const | ||
| 219 | { | ||
| 220 | if(!mTree) return 0; | ||
| 221 | return mTree->GetUsedBytes(); | ||
| 222 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Model.h b/libraries/ode-0.9/OPCODE/OPC_Model.h new file mode 100644 index 0000000..98dee56 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Model.h | |||
| @@ -0,0 +1,65 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for OPCODE models. | ||
| 12 | * \file OPC_Model.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_MODEL_H__ | ||
| 21 | #define __OPC_MODEL_H__ | ||
| 22 | |||
| 23 | class OPCODE_API Model : public BaseModel | ||
| 24 | { | ||
| 25 | public: | ||
| 26 | // Constructor/Destructor | ||
| 27 | Model(); | ||
| 28 | virtual ~Model(); | ||
| 29 | |||
| 30 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 31 | /** | ||
| 32 | * Builds a collision model. | ||
| 33 | * \param create [in] model creation structure | ||
| 34 | * \return true if success | ||
| 35 | */ | ||
| 36 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 37 | override(BaseModel) bool Build(const OPCODECREATE& create); | ||
| 38 | |||
| 39 | #ifdef __MESHMERIZER_H__ | ||
| 40 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 41 | /** | ||
| 42 | * Gets the collision hull. | ||
| 43 | * \return the collision hull if it exists | ||
| 44 | */ | ||
| 45 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 46 | inline_ const CollisionHull* GetHull() const { return mHull; } | ||
| 47 | #endif // __MESHMERIZER_H__ | ||
| 48 | |||
| 49 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 50 | /** | ||
| 51 | * Gets the number of bytes used by the tree. | ||
| 52 | * \return amount of bytes used | ||
| 53 | */ | ||
| 54 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 55 | override(BaseModel) udword GetUsedBytes() const; | ||
| 56 | |||
| 57 | private: | ||
| 58 | #ifdef __MESHMERIZER_H__ | ||
| 59 | CollisionHull* mHull; //!< Possible convex hull | ||
| 60 | #endif // __MESHMERIZER_H__ | ||
| 61 | // Internal methods | ||
| 62 | void Release(); | ||
| 63 | }; | ||
| 64 | |||
| 65 | #endif //__OPC_MODEL_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp new file mode 100644 index 0000000..028d000 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp | |||
| @@ -0,0 +1,767 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for an OBB collider. | ||
| 12 | * \file OPC_OBBCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 1st, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains an OBB-vs-tree collider. | ||
| 21 | * | ||
| 22 | * \class OBBCollider | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date January, 1st, 2002 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | // Precompiled Header | ||
| 31 | #include "Stdafx.h" | ||
| 32 | |||
| 33 | using namespace Opcode; | ||
| 34 | |||
| 35 | #include "OPC_BoxBoxOverlap.h" | ||
| 36 | #include "OPC_TriBoxOverlap.h" | ||
| 37 | |||
| 38 | #define SET_CONTACT(prim_index, flag) \ | ||
| 39 | /* Set contact status */ \ | ||
| 40 | mFlags |= flag; \ | ||
| 41 | mTouchedPrimitives->Add(udword(prim_index)); | ||
| 42 | |||
| 43 | //! OBB-triangle test | ||
| 44 | #define OBB_PRIM(prim_index, flag) \ | ||
| 45 | /* Request vertices from the app */ \ | ||
| 46 | VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ | ||
| 47 | /* Transform them in a common space */ \ | ||
| 48 | TransformPoint(mLeafVerts[0], *VP.Vertex[0], mRModelToBox, mTModelToBox); \ | ||
| 49 | TransformPoint(mLeafVerts[1], *VP.Vertex[1], mRModelToBox, mTModelToBox); \ | ||
| 50 | TransformPoint(mLeafVerts[2], *VP.Vertex[2], mRModelToBox, mTModelToBox); \ | ||
| 51 | /* Perform triangle-box overlap test */ \ | ||
| 52 | if(TriBoxOverlap()) \ | ||
| 53 | { \ | ||
| 54 | SET_CONTACT(prim_index, flag) \ | ||
| 55 | } | ||
| 56 | |||
| 57 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 58 | /** | ||
| 59 | * Constructor. | ||
| 60 | */ | ||
| 61 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 62 | OBBCollider::OBBCollider() : mFullBoxBoxTest(true) | ||
| 63 | { | ||
| 64 | } | ||
| 65 | |||
| 66 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 67 | /** | ||
| 68 | * Destructor. | ||
| 69 | */ | ||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | OBBCollider::~OBBCollider() | ||
| 72 | { | ||
| 73 | } | ||
| 74 | |||
| 75 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 76 | /** | ||
| 77 | * Validates current settings. You should call this method after all the settings and callbacks have been defined. | ||
| 78 | * \return null if everything is ok, else a string describing the problem | ||
| 79 | */ | ||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | const char* OBBCollider::ValidateSettings() | ||
| 82 | { | ||
| 83 | if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; | ||
| 84 | |||
| 85 | return VolumeCollider::ValidateSettings(); | ||
| 86 | } | ||
| 87 | |||
| 88 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 89 | /** | ||
| 90 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 91 | * - with GetContactStatus() | ||
| 92 | * - with GetNbTouchedPrimitives() | ||
| 93 | * - with GetTouchedPrimitives() | ||
| 94 | * | ||
| 95 | * \param cache [in/out] a box cache | ||
| 96 | * \param box [in] collision OBB in local space | ||
| 97 | * \param model [in] Opcode model to collide with | ||
| 98 | * \param worldb [in] OBB's world matrix, or null | ||
| 99 | * \param worldm [in] model's world matrix, or null | ||
| 100 | * \return true if success | ||
| 101 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 102 | */ | ||
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 104 | bool OBBCollider::Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb, const Matrix4x4* worldm) | ||
| 105 | { | ||
| 106 | // Checkings | ||
| 107 | if(!Setup(&model)) return false; | ||
| 108 | |||
| 109 | // Init collision query | ||
| 110 | if(InitQuery(cache, box, worldb, worldm)) return true; | ||
| 111 | |||
| 112 | if(!model.HasLeafNodes()) | ||
| 113 | { | ||
| 114 | if(model.IsQuantized()) | ||
| 115 | { | ||
| 116 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 117 | |||
| 118 | // Setup dequantization coeffs | ||
| 119 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 120 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 121 | |||
| 122 | // Perform collision query | ||
| 123 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 124 | else _Collide(Tree->GetNodes()); | ||
| 125 | } | ||
| 126 | else | ||
| 127 | { | ||
| 128 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 129 | |||
| 130 | // Perform collision query | ||
| 131 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 132 | else _Collide(Tree->GetNodes()); | ||
| 133 | } | ||
| 134 | } | ||
| 135 | else | ||
| 136 | { | ||
| 137 | if(model.IsQuantized()) | ||
| 138 | { | ||
| 139 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 140 | |||
| 141 | // Setup dequantization coeffs | ||
| 142 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 143 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 144 | |||
| 145 | // Perform collision query | ||
| 146 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 147 | else _Collide(Tree->GetNodes()); | ||
| 148 | } | ||
| 149 | else | ||
| 150 | { | ||
| 151 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 152 | |||
| 153 | // Perform collision query | ||
| 154 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 155 | else _Collide(Tree->GetNodes()); | ||
| 156 | } | ||
| 157 | } | ||
| 158 | |||
| 159 | return true; | ||
| 160 | } | ||
| 161 | |||
| 162 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 163 | /** | ||
| 164 | * Initializes a collision query : | ||
| 165 | * - reset stats & contact status | ||
| 166 | * - setup matrices | ||
| 167 | * - check temporal coherence | ||
| 168 | * | ||
| 169 | * \param cache [in/out] a box cache | ||
| 170 | * \param box [in] obb in local space | ||
| 171 | * \param worldb [in] obb's world matrix, or null | ||
| 172 | * \param worldm [in] model's world matrix, or null | ||
| 173 | * \return TRUE if we can return immediately | ||
| 174 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 175 | */ | ||
| 176 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 177 | BOOL OBBCollider::InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb, const Matrix4x4* worldm) | ||
| 178 | { | ||
| 179 | // 1) Call the base method | ||
| 180 | VolumeCollider::InitQuery(); | ||
| 181 | |||
| 182 | // 2) Compute obb in world space | ||
| 183 | mBoxExtents = box.mExtents; | ||
| 184 | |||
| 185 | Matrix4x4 WorldB; | ||
| 186 | |||
| 187 | if(worldb) | ||
| 188 | { | ||
| 189 | WorldB = Matrix4x4( box.mRot * Matrix3x3(*worldb) ); | ||
| 190 | WorldB.SetTrans(box.mCenter * *worldb); | ||
| 191 | } | ||
| 192 | else | ||
| 193 | { | ||
| 194 | WorldB = box.mRot; | ||
| 195 | WorldB.SetTrans(box.mCenter); | ||
| 196 | } | ||
| 197 | |||
| 198 | // Setup matrices | ||
| 199 | Matrix4x4 InvWorldB; | ||
| 200 | InvertPRMatrix(InvWorldB, WorldB); | ||
| 201 | |||
| 202 | if(worldm) | ||
| 203 | { | ||
| 204 | Matrix4x4 InvWorldM; | ||
| 205 | InvertPRMatrix(InvWorldM, *worldm); | ||
| 206 | |||
| 207 | Matrix4x4 WorldBtoM = WorldB * InvWorldM; | ||
| 208 | Matrix4x4 WorldMtoB = *worldm * InvWorldB; | ||
| 209 | |||
| 210 | mRModelToBox = WorldMtoB; WorldMtoB.GetTrans(mTModelToBox); | ||
| 211 | mRBoxToModel = WorldBtoM; WorldBtoM.GetTrans(mTBoxToModel); | ||
| 212 | } | ||
| 213 | else | ||
| 214 | { | ||
| 215 | mRModelToBox = InvWorldB; InvWorldB.GetTrans(mTModelToBox); | ||
| 216 | mRBoxToModel = WorldB; WorldB.GetTrans(mTBoxToModel); | ||
| 217 | } | ||
| 218 | |||
| 219 | // 3) Setup destination pointer | ||
| 220 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 221 | |||
| 222 | // 4) Special case: 1-triangle meshes [Opcode 1.3] | ||
| 223 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 224 | { | ||
| 225 | if(!SkipPrimitiveTests()) | ||
| 226 | { | ||
| 227 | // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. | ||
| 228 | mTouchedPrimitives->Reset(); | ||
| 229 | |||
| 230 | // Perform overlap test between the unique triangle and the box (and set contact status if needed) | ||
| 231 | OBB_PRIM(udword(0), OPC_CONTACT) | ||
| 232 | |||
| 233 | // Return immediately regardless of status | ||
| 234 | return TRUE; | ||
| 235 | } | ||
| 236 | } | ||
| 237 | |||
| 238 | // 5) Check temporal coherence: | ||
| 239 | if(TemporalCoherenceEnabled()) | ||
| 240 | { | ||
| 241 | // Here we use temporal coherence | ||
| 242 | // => check results from previous frame before performing the collision query | ||
| 243 | if(FirstContactEnabled()) | ||
| 244 | { | ||
| 245 | // We're only interested in the first contact found => test the unique previously touched face | ||
| 246 | if(mTouchedPrimitives->GetNbEntries()) | ||
| 247 | { | ||
| 248 | // Get index of previously touched face = the first entry in the array | ||
| 249 | udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0); | ||
| 250 | |||
| 251 | // Then reset the array: | ||
| 252 | // - if the overlap test below is successful, the index we'll get added back anyway | ||
| 253 | // - if it isn't, then the array should be reset anyway for the normal query | ||
| 254 | mTouchedPrimitives->Reset(); | ||
| 255 | |||
| 256 | // Perform overlap test between the cached triangle and the box (and set contact status if needed) | ||
| 257 | OBB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT) | ||
| 258 | |||
| 259 | // Return immediately if possible | ||
| 260 | if(GetContactStatus()) return TRUE; | ||
| 261 | } | ||
| 262 | // else no face has been touched during previous query | ||
| 263 | // => we'll have to perform a normal query | ||
| 264 | } | ||
| 265 | else | ||
| 266 | { | ||
| 267 | // ### rewrite this | ||
| 268 | OBB TestBox(mTBoxToModel, mBoxExtents, mRBoxToModel); | ||
| 269 | |||
| 270 | // We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious): | ||
| 271 | if(IsCacheValid(cache) && TestBox.IsInside(cache.FatBox)) | ||
| 272 | { | ||
| 273 | // - if N is included in P, return previous list | ||
| 274 | // => we simply leave the list (mTouchedFaces) unchanged | ||
| 275 | |||
| 276 | // Set contact status if needed | ||
| 277 | if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT; | ||
| 278 | |||
| 279 | // In any case we don't need to do a query | ||
| 280 | return TRUE; | ||
| 281 | } | ||
| 282 | else | ||
| 283 | { | ||
| 284 | // - else do the query using a fat N | ||
| 285 | |||
| 286 | // Reset cache since we'll about to perform a real query | ||
| 287 | mTouchedPrimitives->Reset(); | ||
| 288 | |||
| 289 | // Make a fat box so that coherence will work for subsequent frames | ||
| 290 | TestBox.mExtents *= cache.FatCoeff; | ||
| 291 | mBoxExtents *= cache.FatCoeff; | ||
| 292 | |||
| 293 | // Update cache with query data (signature for cached faces) | ||
| 294 | cache.FatBox = TestBox; | ||
| 295 | } | ||
| 296 | } | ||
| 297 | } | ||
| 298 | else | ||
| 299 | { | ||
| 300 | // Here we don't use temporal coherence => do a normal query | ||
| 301 | mTouchedPrimitives->Reset(); | ||
| 302 | } | ||
| 303 | |||
| 304 | // Now we can precompute box-box data | ||
| 305 | |||
| 306 | // Precompute absolute box-to-model rotation matrix | ||
| 307 | for(udword i=0;i<3;i++) | ||
| 308 | { | ||
| 309 | for(udword j=0;j<3;j++) | ||
| 310 | { | ||
| 311 | // Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID) | ||
| 312 | mAR.m[i][j] = 1e-6f + fabsf(mRBoxToModel.m[i][j]); | ||
| 313 | } | ||
| 314 | } | ||
| 315 | |||
| 316 | // Precompute bounds for box-in-box test | ||
| 317 | mB0 = mBoxExtents - mTModelToBox; | ||
| 318 | mB1 = - mBoxExtents - mTModelToBox; | ||
| 319 | |||
| 320 | // Precompute box-box data - Courtesy of Erwin de Vries | ||
| 321 | mBBx1 = mBoxExtents.x*mAR.m[0][0] + mBoxExtents.y*mAR.m[1][0] + mBoxExtents.z*mAR.m[2][0]; | ||
| 322 | mBBy1 = mBoxExtents.x*mAR.m[0][1] + mBoxExtents.y*mAR.m[1][1] + mBoxExtents.z*mAR.m[2][1]; | ||
| 323 | mBBz1 = mBoxExtents.x*mAR.m[0][2] + mBoxExtents.y*mAR.m[1][2] + mBoxExtents.z*mAR.m[2][2]; | ||
| 324 | |||
| 325 | mBB_1 = mBoxExtents.y*mAR.m[2][0] + mBoxExtents.z*mAR.m[1][0]; | ||
| 326 | mBB_2 = mBoxExtents.x*mAR.m[2][0] + mBoxExtents.z*mAR.m[0][0]; | ||
| 327 | mBB_3 = mBoxExtents.x*mAR.m[1][0] + mBoxExtents.y*mAR.m[0][0]; | ||
| 328 | mBB_4 = mBoxExtents.y*mAR.m[2][1] + mBoxExtents.z*mAR.m[1][1]; | ||
| 329 | mBB_5 = mBoxExtents.x*mAR.m[2][1] + mBoxExtents.z*mAR.m[0][1]; | ||
| 330 | mBB_6 = mBoxExtents.x*mAR.m[1][1] + mBoxExtents.y*mAR.m[0][1]; | ||
| 331 | mBB_7 = mBoxExtents.y*mAR.m[2][2] + mBoxExtents.z*mAR.m[1][2]; | ||
| 332 | mBB_8 = mBoxExtents.x*mAR.m[2][2] + mBoxExtents.z*mAR.m[0][2]; | ||
| 333 | mBB_9 = mBoxExtents.x*mAR.m[1][2] + mBoxExtents.y*mAR.m[0][2]; | ||
| 334 | |||
| 335 | return FALSE; | ||
| 336 | } | ||
| 337 | |||
| 338 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 339 | /** | ||
| 340 | * Checks the OBB completely contains the box. In which case we can end the query sooner. | ||
| 341 | * \param bc [in] box center | ||
| 342 | * \param be [in] box extents | ||
| 343 | * \return true if the OBB contains the whole box | ||
| 344 | */ | ||
| 345 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 346 | inline_ BOOL OBBCollider::OBBContainsBox(const Point& bc, const Point& be) | ||
| 347 | { | ||
| 348 | // I assume if all 8 box vertices are inside the OBB, so does the whole box. | ||
| 349 | // Sounds ok but maybe there's a better way? | ||
| 350 | /* | ||
| 351 | #define TEST_PT(a,b,c) \ | ||
| 352 | p.x=a; p.y=b; p.z=c; p+=bc; \ | ||
| 353 | f = p.x * mRModelToBox.m[0][0] + p.y * mRModelToBox.m[1][0] + p.z * mRModelToBox.m[2][0]; if(f>mB0.x || f<mB1.x) return FALSE;\ | ||
| 354 | f = p.x * mRModelToBox.m[0][1] + p.y * mRModelToBox.m[1][1] + p.z * mRModelToBox.m[2][1]; if(f>mB0.y || f<mB1.y) return FALSE;\ | ||
| 355 | f = p.x * mRModelToBox.m[0][2] + p.y * mRModelToBox.m[1][2] + p.z * mRModelToBox.m[2][2]; if(f>mB0.z || f<mB1.z) return FALSE; | ||
| 356 | |||
| 357 | Point p; | ||
| 358 | float f; | ||
| 359 | |||
| 360 | TEST_PT(be.x, be.y, be.z) | ||
| 361 | TEST_PT(-be.x, be.y, be.z) | ||
| 362 | TEST_PT(be.x, -be.y, be.z) | ||
| 363 | TEST_PT(-be.x, -be.y, be.z) | ||
| 364 | TEST_PT(be.x, be.y, -be.z) | ||
| 365 | TEST_PT(-be.x, be.y, -be.z) | ||
| 366 | TEST_PT(be.x, -be.y, -be.z) | ||
| 367 | TEST_PT(-be.x, -be.y, -be.z) | ||
| 368 | |||
| 369 | return TRUE; | ||
| 370 | */ | ||
| 371 | |||
| 372 | // Yes there is: | ||
| 373 | // - compute model-box's AABB in OBB space | ||
| 374 | // - test AABB-in-AABB | ||
| 375 | float NCx = bc.x * mRModelToBox.m[0][0] + bc.y * mRModelToBox.m[1][0] + bc.z * mRModelToBox.m[2][0]; | ||
| 376 | float NEx = fabsf(mRModelToBox.m[0][0] * be.x) + fabsf(mRModelToBox.m[1][0] * be.y) + fabsf(mRModelToBox.m[2][0] * be.z); | ||
| 377 | |||
| 378 | if(mB0.x < NCx+NEx) return FALSE; | ||
| 379 | if(mB1.x > NCx-NEx) return FALSE; | ||
| 380 | |||
| 381 | float NCy = bc.x * mRModelToBox.m[0][1] + bc.y * mRModelToBox.m[1][1] + bc.z * mRModelToBox.m[2][1]; | ||
| 382 | float NEy = fabsf(mRModelToBox.m[0][1] * be.x) + fabsf(mRModelToBox.m[1][1] * be.y) + fabsf(mRModelToBox.m[2][1] * be.z); | ||
| 383 | |||
| 384 | if(mB0.y < NCy+NEy) return FALSE; | ||
| 385 | if(mB1.y > NCy-NEy) return FALSE; | ||
| 386 | |||
| 387 | float NCz = bc.x * mRModelToBox.m[0][2] + bc.y * mRModelToBox.m[1][2] + bc.z * mRModelToBox.m[2][2]; | ||
| 388 | float NEz = fabsf(mRModelToBox.m[0][2] * be.x) + fabsf(mRModelToBox.m[1][2] * be.y) + fabsf(mRModelToBox.m[2][2] * be.z); | ||
| 389 | |||
| 390 | if(mB0.z < NCz+NEz) return FALSE; | ||
| 391 | if(mB1.z > NCz-NEz) return FALSE; | ||
| 392 | |||
| 393 | return TRUE; | ||
| 394 | } | ||
| 395 | |||
| 396 | #define TEST_BOX_IN_OBB(center, extents) \ | ||
| 397 | if(OBBContainsBox(center, extents)) \ | ||
| 398 | { \ | ||
| 399 | /* Set contact status */ \ | ||
| 400 | mFlags |= OPC_CONTACT; \ | ||
| 401 | _Dump(node); \ | ||
| 402 | return; \ | ||
| 403 | } | ||
| 404 | |||
| 405 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 406 | /** | ||
| 407 | * Recursive collision query for normal AABB trees. | ||
| 408 | * \param node [in] current collision node | ||
| 409 | */ | ||
| 410 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 411 | void OBBCollider::_Collide(const AABBCollisionNode* node) | ||
| 412 | { | ||
| 413 | // Perform OBB-AABB overlap test | ||
| 414 | if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 415 | |||
| 416 | TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 417 | |||
| 418 | if(node->IsLeaf()) | ||
| 419 | { | ||
| 420 | OBB_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 421 | } | ||
| 422 | else | ||
| 423 | { | ||
| 424 | _Collide(node->GetPos()); | ||
| 425 | |||
| 426 | if(ContactFound()) return; | ||
| 427 | |||
| 428 | _Collide(node->GetNeg()); | ||
| 429 | } | ||
| 430 | } | ||
| 431 | |||
| 432 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 433 | /** | ||
| 434 | * Recursive collision query for normal AABB trees, without primitive tests. | ||
| 435 | * \param node [in] current collision node | ||
| 436 | */ | ||
| 437 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 438 | void OBBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node) | ||
| 439 | { | ||
| 440 | // Perform OBB-AABB overlap test | ||
| 441 | if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 442 | |||
| 443 | TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 444 | |||
| 445 | if(node->IsLeaf()) | ||
| 446 | { | ||
| 447 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 448 | } | ||
| 449 | else | ||
| 450 | { | ||
| 451 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 452 | |||
| 453 | if(ContactFound()) return; | ||
| 454 | |||
| 455 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 456 | } | ||
| 457 | } | ||
| 458 | |||
| 459 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 460 | /** | ||
| 461 | * Recursive collision query for quantized AABB trees. | ||
| 462 | * \param node [in] current collision node | ||
| 463 | */ | ||
| 464 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 465 | void OBBCollider::_Collide(const AABBQuantizedNode* node) | ||
| 466 | { | ||
| 467 | // Dequantize box | ||
| 468 | const QuantizedAABB& Box = node->mAABB; | ||
| 469 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 470 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 471 | |||
| 472 | // Perform OBB-AABB overlap test | ||
| 473 | if(!BoxBoxOverlap(Extents, Center)) return; | ||
| 474 | |||
| 475 | TEST_BOX_IN_OBB(Center, Extents) | ||
| 476 | |||
| 477 | if(node->IsLeaf()) | ||
| 478 | { | ||
| 479 | OBB_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 480 | } | ||
| 481 | else | ||
| 482 | { | ||
| 483 | _Collide(node->GetPos()); | ||
| 484 | |||
| 485 | if(ContactFound()) return; | ||
| 486 | |||
| 487 | _Collide(node->GetNeg()); | ||
| 488 | } | ||
| 489 | } | ||
| 490 | |||
| 491 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 492 | /** | ||
| 493 | * Recursive collision query for quantized AABB trees, without primitive tests. | ||
| 494 | * \param node [in] current collision node | ||
| 495 | */ | ||
| 496 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 497 | void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node) | ||
| 498 | { | ||
| 499 | // Dequantize box | ||
| 500 | const QuantizedAABB& Box = node->mAABB; | ||
| 501 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 502 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 503 | |||
| 504 | // Perform OBB-AABB overlap test | ||
| 505 | if(!BoxBoxOverlap(Extents, Center)) return; | ||
| 506 | |||
| 507 | TEST_BOX_IN_OBB(Center, Extents) | ||
| 508 | |||
| 509 | if(node->IsLeaf()) | ||
| 510 | { | ||
| 511 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 512 | } | ||
| 513 | else | ||
| 514 | { | ||
| 515 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 516 | |||
| 517 | if(ContactFound()) return; | ||
| 518 | |||
| 519 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 520 | } | ||
| 521 | } | ||
| 522 | |||
| 523 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 524 | /** | ||
| 525 | * Recursive collision query for no-leaf AABB trees. | ||
| 526 | * \param node [in] current collision node | ||
| 527 | */ | ||
| 528 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 529 | void OBBCollider::_Collide(const AABBNoLeafNode* node) | ||
| 530 | { | ||
| 531 | // Perform OBB-AABB overlap test | ||
| 532 | if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 533 | |||
| 534 | TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 535 | |||
| 536 | if(node->HasPosLeaf()) { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 537 | else _Collide(node->GetPos()); | ||
| 538 | |||
| 539 | if(ContactFound()) return; | ||
| 540 | |||
| 541 | if(node->HasNegLeaf()) { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 542 | else _Collide(node->GetNeg()); | ||
| 543 | } | ||
| 544 | |||
| 545 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 546 | /** | ||
| 547 | * Recursive collision query for no-leaf AABB trees, without primitive tests. | ||
| 548 | * \param node [in] current collision node | ||
| 549 | */ | ||
| 550 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 551 | void OBBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node) | ||
| 552 | { | ||
| 553 | // Perform OBB-AABB overlap test | ||
| 554 | if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; | ||
| 555 | |||
| 556 | TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 557 | |||
| 558 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 559 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 560 | |||
| 561 | if(ContactFound()) return; | ||
| 562 | |||
| 563 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 564 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 565 | } | ||
| 566 | |||
| 567 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 568 | /** | ||
| 569 | * Recursive collision query for quantized no-leaf AABB trees. | ||
| 570 | * \param node [in] current collision node | ||
| 571 | */ | ||
| 572 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 573 | void OBBCollider::_Collide(const AABBQuantizedNoLeafNode* node) | ||
| 574 | { | ||
| 575 | // Dequantize box | ||
| 576 | const QuantizedAABB& Box = node->mAABB; | ||
| 577 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 578 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 579 | |||
| 580 | // Perform OBB-AABB overlap test | ||
| 581 | if(!BoxBoxOverlap(Extents, Center)) return; | ||
| 582 | |||
| 583 | TEST_BOX_IN_OBB(Center, Extents) | ||
| 584 | |||
| 585 | if(node->HasPosLeaf()) { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 586 | else _Collide(node->GetPos()); | ||
| 587 | |||
| 588 | if(ContactFound()) return; | ||
| 589 | |||
| 590 | if(node->HasNegLeaf()) { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 591 | else _Collide(node->GetNeg()); | ||
| 592 | } | ||
| 593 | |||
| 594 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 595 | /** | ||
| 596 | * Recursive collision query for quantized no-leaf AABB trees, without primitive tests. | ||
| 597 | * \param node [in] current collision node | ||
| 598 | */ | ||
| 599 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 600 | void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node) | ||
| 601 | { | ||
| 602 | // Dequantize box | ||
| 603 | const QuantizedAABB& Box = node->mAABB; | ||
| 604 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 605 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 606 | |||
| 607 | // Perform OBB-AABB overlap test | ||
| 608 | if(!BoxBoxOverlap(Extents, Center)) return; | ||
| 609 | |||
| 610 | TEST_BOX_IN_OBB(Center, Extents) | ||
| 611 | |||
| 612 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 613 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 614 | |||
| 615 | if(ContactFound()) return; | ||
| 616 | |||
| 617 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 618 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 619 | } | ||
| 620 | |||
| 621 | |||
| 622 | |||
| 623 | |||
| 624 | |||
| 625 | |||
| 626 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 627 | /** | ||
| 628 | * Constructor. | ||
| 629 | */ | ||
| 630 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 631 | HybridOBBCollider::HybridOBBCollider() | ||
| 632 | { | ||
| 633 | } | ||
| 634 | |||
| 635 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 636 | /** | ||
| 637 | * Destructor. | ||
| 638 | */ | ||
| 639 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 640 | HybridOBBCollider::~HybridOBBCollider() | ||
| 641 | { | ||
| 642 | } | ||
| 643 | |||
| 644 | bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb, const Matrix4x4* worldm) | ||
| 645 | { | ||
| 646 | // We don't want primitive tests here! | ||
| 647 | mFlags |= OPC_NO_PRIMITIVE_TESTS; | ||
| 648 | |||
| 649 | // Checkings | ||
| 650 | if(!Setup(&model)) return false; | ||
| 651 | |||
| 652 | // Init collision query | ||
| 653 | if(InitQuery(cache, box, worldb, worldm)) return true; | ||
| 654 | |||
| 655 | // Special case for 1-leaf trees | ||
| 656 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 657 | { | ||
| 658 | // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles | ||
| 659 | udword Nb = mIMesh->GetNbTriangles(); | ||
| 660 | |||
| 661 | // Loop through all triangles | ||
| 662 | for(udword i=0;i<Nb;i++) | ||
| 663 | { | ||
| 664 | OBB_PRIM(i, OPC_CONTACT) | ||
| 665 | } | ||
| 666 | return true; | ||
| 667 | } | ||
| 668 | |||
| 669 | // Override destination array since we're only going to get leaf boxes here | ||
| 670 | mTouchedBoxes.Reset(); | ||
| 671 | mTouchedPrimitives = &mTouchedBoxes; | ||
| 672 | |||
| 673 | // Now, do the actual query against leaf boxes | ||
| 674 | if(!model.HasLeafNodes()) | ||
| 675 | { | ||
| 676 | if(model.IsQuantized()) | ||
| 677 | { | ||
| 678 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 679 | |||
| 680 | // Setup dequantization coeffs | ||
| 681 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 682 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 683 | |||
| 684 | // Perform collision query - we don't want primitive tests here! | ||
| 685 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 686 | } | ||
| 687 | else | ||
| 688 | { | ||
| 689 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 690 | |||
| 691 | // Perform collision query - we don't want primitive tests here! | ||
| 692 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 693 | } | ||
| 694 | } | ||
| 695 | else | ||
| 696 | { | ||
| 697 | if(model.IsQuantized()) | ||
| 698 | { | ||
| 699 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 700 | |||
| 701 | // Setup dequantization coeffs | ||
| 702 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 703 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 704 | |||
| 705 | // Perform collision query - we don't want primitive tests here! | ||
| 706 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 707 | } | ||
| 708 | else | ||
| 709 | { | ||
| 710 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 711 | |||
| 712 | // Perform collision query - we don't want primitive tests here! | ||
| 713 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 714 | } | ||
| 715 | } | ||
| 716 | |||
| 717 | // We only have a list of boxes so far | ||
| 718 | if(GetContactStatus()) | ||
| 719 | { | ||
| 720 | // Reset contact status, since it currently only reflects collisions with leaf boxes | ||
| 721 | Collider::InitQuery(); | ||
| 722 | |||
| 723 | // Change dest container so that we can use built-in overlap tests and get collided primitives | ||
| 724 | cache.TouchedPrimitives.Reset(); | ||
| 725 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 726 | |||
| 727 | // Read touched leaf boxes | ||
| 728 | udword Nb = mTouchedBoxes.GetNbEntries(); | ||
| 729 | const udword* Touched = mTouchedBoxes.GetEntries(); | ||
| 730 | |||
| 731 | const LeafTriangles* LT = model.GetLeafTriangles(); | ||
| 732 | const udword* Indices = model.GetIndices(); | ||
| 733 | |||
| 734 | // Loop through touched leaves | ||
| 735 | while(Nb--) | ||
| 736 | { | ||
| 737 | const LeafTriangles& CurrentLeaf = LT[*Touched++]; | ||
| 738 | |||
| 739 | // Each leaf box has a set of triangles | ||
| 740 | udword NbTris = CurrentLeaf.GetNbTriangles(); | ||
| 741 | if(Indices) | ||
| 742 | { | ||
| 743 | const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; | ||
| 744 | |||
| 745 | // Loop through triangles and test each of them | ||
| 746 | while(NbTris--) | ||
| 747 | { | ||
| 748 | udword TriangleIndex = *T++; | ||
| 749 | OBB_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 750 | } | ||
| 751 | } | ||
| 752 | else | ||
| 753 | { | ||
| 754 | udword BaseIndex = CurrentLeaf.GetTriangleIndex(); | ||
| 755 | |||
| 756 | // Loop through triangles and test each of them | ||
| 757 | while(NbTris--) | ||
| 758 | { | ||
| 759 | udword TriangleIndex = BaseIndex++; | ||
| 760 | OBB_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 761 | } | ||
| 762 | } | ||
| 763 | } | ||
| 764 | } | ||
| 765 | |||
| 766 | return true; | ||
| 767 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_OBBCollider.h b/libraries/ode-0.9/OPCODE/OPC_OBBCollider.h new file mode 100644 index 0000000..a050118 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_OBBCollider.h | |||
| @@ -0,0 +1,142 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for an OBB collider. | ||
| 12 | * \file OPC_OBBCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 1st, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_OBBCOLLIDER_H__ | ||
| 21 | #define __OPC_OBBCOLLIDER_H__ | ||
| 22 | |||
| 23 | struct OPCODE_API OBBCache : VolumeCache | ||
| 24 | { | ||
| 25 | OBBCache() : FatCoeff(1.1f) | ||
| 26 | { | ||
| 27 | FatBox.mCenter.Zero(); | ||
| 28 | FatBox.mExtents.Zero(); | ||
| 29 | FatBox.mRot.Identity(); | ||
| 30 | } | ||
| 31 | |||
| 32 | // Cached faces signature | ||
| 33 | OBB FatBox; //!< Box used when performing the query resulting in cached faces | ||
| 34 | // User settings | ||
| 35 | float FatCoeff; //!< extents multiplier used to create a fat box | ||
| 36 | }; | ||
| 37 | |||
| 38 | class OPCODE_API OBBCollider : public VolumeCollider | ||
| 39 | { | ||
| 40 | public: | ||
| 41 | // Constructor / Destructor | ||
| 42 | OBBCollider(); | ||
| 43 | virtual ~OBBCollider(); | ||
| 44 | |||
| 45 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 46 | /** | ||
| 47 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 48 | * - with GetContactStatus() | ||
| 49 | * - with GetNbTouchedPrimitives() | ||
| 50 | * - with GetTouchedPrimitives() | ||
| 51 | * | ||
| 52 | * \param cache [in/out] a box cache | ||
| 53 | * \param box [in] collision OBB in local space | ||
| 54 | * \param model [in] Opcode model to collide with | ||
| 55 | * \param worldb [in] OBB's world matrix, or null | ||
| 56 | * \param worldm [in] model's world matrix, or null | ||
| 57 | * \return true if success | ||
| 58 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 59 | */ | ||
| 60 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 61 | bool Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null); | ||
| 62 | |||
| 63 | // Settings | ||
| 64 | |||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | /** | ||
| 67 | * Settings: select between full box-box tests or "SAT-lite" tests (where Class III axes are discarded) | ||
| 68 | * \param flag [in] true for full tests, false for coarse tests | ||
| 69 | */ | ||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | inline_ void SetFullBoxBoxTest(bool flag) { mFullBoxBoxTest = flag; } | ||
| 72 | |||
| 73 | // Settings | ||
| 74 | |||
| 75 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 76 | /** | ||
| 77 | * Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider. | ||
| 78 | * \return null if everything is ok, else a string describing the problem | ||
| 79 | */ | ||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | override(Collider) const char* ValidateSettings(); | ||
| 82 | |||
| 83 | protected: | ||
| 84 | // Precomputed data | ||
| 85 | Matrix3x3 mAR; //!< Absolute rotation matrix | ||
| 86 | Matrix3x3 mRModelToBox; //!< Rotation from model space to obb space | ||
| 87 | Matrix3x3 mRBoxToModel; //!< Rotation from obb space to model space | ||
| 88 | Point mTModelToBox; //!< Translation from model space to obb space | ||
| 89 | Point mTBoxToModel; //!< Translation from obb space to model space | ||
| 90 | |||
| 91 | Point mBoxExtents; | ||
| 92 | Point mB0; //!< - mTModelToBox + mBoxExtents | ||
| 93 | Point mB1; //!< - mTModelToBox - mBoxExtents | ||
| 94 | |||
| 95 | float mBBx1; | ||
| 96 | float mBBy1; | ||
| 97 | float mBBz1; | ||
| 98 | |||
| 99 | float mBB_1; | ||
| 100 | float mBB_2; | ||
| 101 | float mBB_3; | ||
| 102 | float mBB_4; | ||
| 103 | float mBB_5; | ||
| 104 | float mBB_6; | ||
| 105 | float mBB_7; | ||
| 106 | float mBB_8; | ||
| 107 | float mBB_9; | ||
| 108 | |||
| 109 | // Leaf description | ||
| 110 | Point mLeafVerts[3]; //!< Triangle vertices | ||
| 111 | // Settings | ||
| 112 | bool mFullBoxBoxTest; //!< Perform full BV-BV tests (true) or SAT-lite tests (false) | ||
| 113 | // Internal methods | ||
| 114 | void _Collide(const AABBCollisionNode* node); | ||
| 115 | void _Collide(const AABBNoLeafNode* node); | ||
| 116 | void _Collide(const AABBQuantizedNode* node); | ||
| 117 | void _Collide(const AABBQuantizedNoLeafNode* node); | ||
| 118 | void _CollideNoPrimitiveTest(const AABBCollisionNode* node); | ||
| 119 | void _CollideNoPrimitiveTest(const AABBNoLeafNode* node); | ||
| 120 | void _CollideNoPrimitiveTest(const AABBQuantizedNode* node); | ||
| 121 | void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node); | ||
| 122 | // Overlap tests | ||
| 123 | inline_ BOOL OBBContainsBox(const Point& bc, const Point& be); | ||
| 124 | inline_ BOOL BoxBoxOverlap(const Point& extents, const Point& center); | ||
| 125 | inline_ BOOL TriBoxOverlap(); | ||
| 126 | // Init methods | ||
| 127 | BOOL InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null); | ||
| 128 | }; | ||
| 129 | |||
| 130 | class OPCODE_API HybridOBBCollider : public OBBCollider | ||
| 131 | { | ||
| 132 | public: | ||
| 133 | // Constructor / Destructor | ||
| 134 | HybridOBBCollider(); | ||
| 135 | virtual ~HybridOBBCollider(); | ||
| 136 | |||
| 137 | bool Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null); | ||
| 138 | protected: | ||
| 139 | Container mTouchedBoxes; | ||
| 140 | }; | ||
| 141 | |||
| 142 | #endif // __OPC_OBBCOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_OptimizedTree.cpp b/libraries/ode-0.9/OPCODE/OPC_OptimizedTree.cpp new file mode 100644 index 0000000..11b92f9 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_OptimizedTree.cpp | |||
| @@ -0,0 +1,782 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for optimized trees. Implements 4 trees: | ||
| 12 | * - normal | ||
| 13 | * - no leaf | ||
| 14 | * - quantized | ||
| 15 | * - no leaf / quantized | ||
| 16 | * | ||
| 17 | * \file OPC_OptimizedTree.cpp | ||
| 18 | * \author Pierre Terdiman | ||
| 19 | * \date March, 20, 2001 | ||
| 20 | */ | ||
| 21 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 22 | |||
| 23 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 24 | /** | ||
| 25 | * A standard AABB tree. | ||
| 26 | * | ||
| 27 | * \class AABBCollisionTree | ||
| 28 | * \author Pierre Terdiman | ||
| 29 | * \version 1.3 | ||
| 30 | * \date March, 20, 2001 | ||
| 31 | */ | ||
| 32 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 33 | |||
| 34 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 35 | /** | ||
| 36 | * A no-leaf AABB tree. | ||
| 37 | * | ||
| 38 | * \class AABBNoLeafTree | ||
| 39 | * \author Pierre Terdiman | ||
| 40 | * \version 1.3 | ||
| 41 | * \date March, 20, 2001 | ||
| 42 | */ | ||
| 43 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 44 | |||
| 45 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 46 | /** | ||
| 47 | * A quantized AABB tree. | ||
| 48 | * | ||
| 49 | * \class AABBQuantizedTree | ||
| 50 | * \author Pierre Terdiman | ||
| 51 | * \version 1.3 | ||
| 52 | * \date March, 20, 2001 | ||
| 53 | */ | ||
| 54 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 55 | |||
| 56 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 57 | /** | ||
| 58 | * A quantized no-leaf AABB tree. | ||
| 59 | * | ||
| 60 | * \class AABBQuantizedNoLeafTree | ||
| 61 | * \author Pierre Terdiman | ||
| 62 | * \version 1.3 | ||
| 63 | * \date March, 20, 2001 | ||
| 64 | */ | ||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | |||
| 67 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 68 | // Precompiled Header | ||
| 69 | #include "Stdafx.h" | ||
| 70 | |||
| 71 | using namespace Opcode; | ||
| 72 | |||
| 73 | //! Compilation flag: | ||
| 74 | //! - true to fix quantized boxes (i.e. make sure they enclose the original ones) | ||
| 75 | //! - false to see the effects of quantization errors (faster, but wrong results in some cases) | ||
| 76 | static bool gFixQuantized = true; | ||
| 77 | |||
| 78 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 79 | /** | ||
| 80 | * Builds an implicit tree from a standard one. An implicit tree is a complete tree (2*N-1 nodes) whose negative | ||
| 81 | * box pointers and primitive pointers have been made implicit, hence packing 3 pointers in one. | ||
| 82 | * | ||
| 83 | * Layout for implicit trees: | ||
| 84 | * Node: | ||
| 85 | * - box | ||
| 86 | * - data (32-bits value) | ||
| 87 | * | ||
| 88 | * if data's LSB = 1 => remaining bits are a primitive pointer | ||
| 89 | * else remaining bits are a P-node pointer, and N = P + 1 | ||
| 90 | * | ||
| 91 | * \relates AABBCollisionNode | ||
| 92 | * \fn _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) | ||
| 93 | * \param linear [in] base address of destination nodes | ||
| 94 | * \param box_id [in] index of destination node | ||
| 95 | * \param current_id [in] current running index | ||
| 96 | * \param current_node [in] current node from input tree | ||
| 97 | */ | ||
| 98 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 99 | static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) | ||
| 100 | { | ||
| 101 | // Current node from input tree is "current_node". Must be flattened into "linear[boxid]". | ||
| 102 | |||
| 103 | // Store the AABB | ||
| 104 | current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter); | ||
| 105 | current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents); | ||
| 106 | // Store remaining info | ||
| 107 | if(current_node->IsLeaf()) | ||
| 108 | { | ||
| 109 | // The input tree must be complete => i.e. one primitive/leaf | ||
| 110 | ASSERT(current_node->GetNbPrimitives()==1); | ||
| 111 | // Get the primitive index from the input tree | ||
| 112 | udword PrimitiveIndex = current_node->GetPrimitives()[0]; | ||
| 113 | // Setup box data as the primitive index, marked as leaf | ||
| 114 | linear[box_id].mData = (PrimitiveIndex<<1)|1; | ||
| 115 | } | ||
| 116 | else | ||
| 117 | { | ||
| 118 | // To make the negative one implicit, we must store P and N in successive order | ||
| 119 | udword PosID = current_id++; // Get a new id for positive child | ||
| 120 | udword NegID = current_id++; // Get a new id for negative child | ||
| 121 | // Setup box data as the forthcoming new P pointer | ||
| 122 | linear[box_id].mData = (size_t)&linear[PosID]; | ||
| 123 | // Make sure it's not marked as leaf | ||
| 124 | ASSERT(!(linear[box_id].mData&1)); | ||
| 125 | // Recurse with new IDs | ||
| 126 | _BuildCollisionTree(linear, PosID, current_id, current_node->GetPos()); | ||
| 127 | _BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg()); | ||
| 128 | } | ||
| 129 | } | ||
| 130 | |||
| 131 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 132 | /** | ||
| 133 | * Builds a "no-leaf" tree from a standard one. This is a tree whose leaf nodes have been removed. | ||
| 134 | * | ||
| 135 | * Layout for no-leaf trees: | ||
| 136 | * | ||
| 137 | * Node: | ||
| 138 | * - box | ||
| 139 | * - P pointer => a node (LSB=0) or a primitive (LSB=1) | ||
| 140 | * - N pointer => a node (LSB=0) or a primitive (LSB=1) | ||
| 141 | * | ||
| 142 | * \relates AABBNoLeafNode | ||
| 143 | * \fn _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) | ||
| 144 | * \param linear [in] base address of destination nodes | ||
| 145 | * \param box_id [in] index of destination node | ||
| 146 | * \param current_id [in] current running index | ||
| 147 | * \param current_node [in] current node from input tree | ||
| 148 | */ | ||
| 149 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 150 | static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) | ||
| 151 | { | ||
| 152 | const AABBTreeNode* P = current_node->GetPos(); | ||
| 153 | const AABBTreeNode* N = current_node->GetNeg(); | ||
| 154 | // Leaf nodes here?! | ||
| 155 | ASSERT(P); | ||
| 156 | ASSERT(N); | ||
| 157 | // Internal node => keep the box | ||
| 158 | current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter); | ||
| 159 | current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents); | ||
| 160 | |||
| 161 | if(P->IsLeaf()) | ||
| 162 | { | ||
| 163 | // The input tree must be complete => i.e. one primitive/leaf | ||
| 164 | ASSERT(P->GetNbPrimitives()==1); | ||
| 165 | // Get the primitive index from the input tree | ||
| 166 | udword PrimitiveIndex = P->GetPrimitives()[0]; | ||
| 167 | // Setup prev box data as the primitive index, marked as leaf | ||
| 168 | linear[box_id].mPosData = (PrimitiveIndex<<1)|1; | ||
| 169 | } | ||
| 170 | else | ||
| 171 | { | ||
| 172 | // Get a new id for positive child | ||
| 173 | udword PosID = current_id++; | ||
| 174 | // Setup box data | ||
| 175 | linear[box_id].mPosData = (size_t)&linear[PosID]; | ||
| 176 | // Make sure it's not marked as leaf | ||
| 177 | ASSERT(!(linear[box_id].mPosData&1)); | ||
| 178 | // Recurse | ||
| 179 | _BuildNoLeafTree(linear, PosID, current_id, P); | ||
| 180 | } | ||
| 181 | |||
| 182 | if(N->IsLeaf()) | ||
| 183 | { | ||
| 184 | // The input tree must be complete => i.e. one primitive/leaf | ||
| 185 | ASSERT(N->GetNbPrimitives()==1); | ||
| 186 | // Get the primitive index from the input tree | ||
| 187 | udword PrimitiveIndex = N->GetPrimitives()[0]; | ||
| 188 | // Setup prev box data as the primitive index, marked as leaf | ||
| 189 | linear[box_id].mNegData = (PrimitiveIndex<<1)|1; | ||
| 190 | } | ||
| 191 | else | ||
| 192 | { | ||
| 193 | // Get a new id for negative child | ||
| 194 | udword NegID = current_id++; | ||
| 195 | // Setup box data | ||
| 196 | linear[box_id].mNegData = (size_t)&linear[NegID]; | ||
| 197 | // Make sure it's not marked as leaf | ||
| 198 | ASSERT(!(linear[box_id].mNegData&1)); | ||
| 199 | // Recurse | ||
| 200 | _BuildNoLeafTree(linear, NegID, current_id, N); | ||
| 201 | } | ||
| 202 | } | ||
| 203 | |||
| 204 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 205 | /** | ||
| 206 | * Constructor. | ||
| 207 | */ | ||
| 208 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 209 | AABBCollisionTree::AABBCollisionTree() : mNodes(null) | ||
| 210 | { | ||
| 211 | } | ||
| 212 | |||
| 213 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 214 | /** | ||
| 215 | * Destructor. | ||
| 216 | */ | ||
| 217 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 218 | AABBCollisionTree::~AABBCollisionTree() | ||
| 219 | { | ||
| 220 | DELETEARRAY(mNodes); | ||
| 221 | } | ||
| 222 | |||
| 223 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 224 | /** | ||
| 225 | * Builds the collision tree from a generic AABB tree. | ||
| 226 | * \param tree [in] generic AABB tree | ||
| 227 | * \return true if success | ||
| 228 | */ | ||
| 229 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 230 | bool AABBCollisionTree::Build(AABBTree* tree) | ||
| 231 | { | ||
| 232 | // Checkings | ||
| 233 | if(!tree) return false; | ||
| 234 | // Check the input tree is complete | ||
| 235 | udword NbTriangles = tree->GetNbPrimitives(); | ||
| 236 | udword NbNodes = tree->GetNbNodes(); | ||
| 237 | if(NbNodes!=NbTriangles*2-1) return false; | ||
| 238 | |||
| 239 | // Get nodes | ||
| 240 | if(mNbNodes!=NbNodes) // Same number of nodes => keep moving | ||
| 241 | { | ||
| 242 | mNbNodes = NbNodes; | ||
| 243 | DELETEARRAY(mNodes); | ||
| 244 | mNodes = new AABBCollisionNode[mNbNodes]; | ||
| 245 | CHECKALLOC(mNodes); | ||
| 246 | } | ||
| 247 | |||
| 248 | // Build the tree | ||
| 249 | udword CurID = 1; | ||
| 250 | _BuildCollisionTree(mNodes, 0, CurID, tree); | ||
| 251 | ASSERT(CurID==mNbNodes); | ||
| 252 | |||
| 253 | return true; | ||
| 254 | } | ||
| 255 | |||
| 256 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 257 | /** | ||
| 258 | * Refits the collision tree after vertices have been modified. | ||
| 259 | * \param mesh_interface [in] mesh interface for current model | ||
| 260 | * \return true if success | ||
| 261 | */ | ||
| 262 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 263 | bool AABBCollisionTree::Refit(const MeshInterface* mesh_interface) | ||
| 264 | { | ||
| 265 | ASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!"); | ||
| 266 | return false; | ||
| 267 | } | ||
| 268 | |||
| 269 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 270 | /** | ||
| 271 | * Walks the tree and call the user back for each node. | ||
| 272 | * \param callback [in] walking callback | ||
| 273 | * \param user_data [in] callback's user data | ||
| 274 | * \return true if success | ||
| 275 | */ | ||
| 276 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 277 | bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const | ||
| 278 | { | ||
| 279 | if(!callback) return false; | ||
| 280 | |||
| 281 | struct Local | ||
| 282 | { | ||
| 283 | static void _Walk(const AABBCollisionNode* current_node, GenericWalkingCallback callback, void* user_data) | ||
| 284 | { | ||
| 285 | if(!current_node || !(callback)(current_node, user_data)) return; | ||
| 286 | |||
| 287 | if(!current_node->IsLeaf()) | ||
| 288 | { | ||
| 289 | _Walk(current_node->GetPos(), callback, user_data); | ||
| 290 | _Walk(current_node->GetNeg(), callback, user_data); | ||
| 291 | } | ||
| 292 | } | ||
| 293 | }; | ||
| 294 | Local::_Walk(mNodes, callback, user_data); | ||
| 295 | return true; | ||
| 296 | } | ||
| 297 | |||
| 298 | |||
| 299 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 300 | /** | ||
| 301 | * Constructor. | ||
| 302 | */ | ||
| 303 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 304 | AABBNoLeafTree::AABBNoLeafTree() : mNodes(null) | ||
| 305 | { | ||
| 306 | } | ||
| 307 | |||
| 308 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 309 | /** | ||
| 310 | * Destructor. | ||
| 311 | */ | ||
| 312 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 313 | AABBNoLeafTree::~AABBNoLeafTree() | ||
| 314 | { | ||
| 315 | DELETEARRAY(mNodes); | ||
| 316 | } | ||
| 317 | |||
| 318 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 319 | /** | ||
| 320 | * Builds the collision tree from a generic AABB tree. | ||
| 321 | * \param tree [in] generic AABB tree | ||
| 322 | * \return true if success | ||
| 323 | */ | ||
| 324 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 325 | bool AABBNoLeafTree::Build(AABBTree* tree) | ||
| 326 | { | ||
| 327 | // Checkings | ||
| 328 | if(!tree) return false; | ||
| 329 | // Check the input tree is complete | ||
| 330 | udword NbTriangles = tree->GetNbPrimitives(); | ||
| 331 | udword NbNodes = tree->GetNbNodes(); | ||
| 332 | if(NbNodes!=NbTriangles*2-1) return false; | ||
| 333 | |||
| 334 | // Get nodes | ||
| 335 | if(mNbNodes!=NbTriangles-1) // Same number of nodes => keep moving | ||
| 336 | { | ||
| 337 | mNbNodes = NbTriangles-1; | ||
| 338 | DELETEARRAY(mNodes); | ||
| 339 | mNodes = new AABBNoLeafNode[mNbNodes]; | ||
| 340 | CHECKALLOC(mNodes); | ||
| 341 | } | ||
| 342 | |||
| 343 | // Build the tree | ||
| 344 | udword CurID = 1; | ||
| 345 | _BuildNoLeafTree(mNodes, 0, CurID, tree); | ||
| 346 | ASSERT(CurID==mNbNodes); | ||
| 347 | |||
| 348 | return true; | ||
| 349 | } | ||
| 350 | |||
| 351 | inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp) | ||
| 352 | { | ||
| 353 | // Compute triangle's AABB = a leaf box | ||
| 354 | #ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much | ||
| 355 | min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); | ||
| 356 | max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); | ||
| 357 | |||
| 358 | min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); | ||
| 359 | max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); | ||
| 360 | |||
| 361 | min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); | ||
| 362 | max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); | ||
| 363 | #else | ||
| 364 | min = *vp.Vertex[0]; | ||
| 365 | max = *vp.Vertex[0]; | ||
| 366 | min.Min(*vp.Vertex[1]); | ||
| 367 | max.Max(*vp.Vertex[1]); | ||
| 368 | min.Min(*vp.Vertex[2]); | ||
| 369 | max.Max(*vp.Vertex[2]); | ||
| 370 | #endif | ||
| 371 | } | ||
| 372 | |||
| 373 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 374 | /** | ||
| 375 | * Refits the collision tree after vertices have been modified. | ||
| 376 | * \param mesh_interface [in] mesh interface for current model | ||
| 377 | * \return true if success | ||
| 378 | */ | ||
| 379 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 380 | bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface) | ||
| 381 | { | ||
| 382 | // Checkings | ||
| 383 | if(!mesh_interface) return false; | ||
| 384 | |||
| 385 | // Bottom-up update | ||
| 386 | VertexPointers VP; | ||
| 387 | Point Min,Max; | ||
| 388 | Point Min_,Max_; | ||
| 389 | udword Index = mNbNodes; | ||
| 390 | while(Index--) | ||
| 391 | { | ||
| 392 | AABBNoLeafNode& Current = mNodes[Index]; | ||
| 393 | |||
| 394 | if(Current.HasPosLeaf()) | ||
| 395 | { | ||
| 396 | mesh_interface->GetTriangle(VP, Current.GetPosPrimitive()); | ||
| 397 | ComputeMinMax(Min, Max, VP); | ||
| 398 | } | ||
| 399 | else | ||
| 400 | { | ||
| 401 | const CollisionAABB& CurrentBox = Current.GetPos()->mAABB; | ||
| 402 | CurrentBox.GetMin(Min); | ||
| 403 | CurrentBox.GetMax(Max); | ||
| 404 | } | ||
| 405 | |||
| 406 | if(Current.HasNegLeaf()) | ||
| 407 | { | ||
| 408 | mesh_interface->GetTriangle(VP, Current.GetNegPrimitive()); | ||
| 409 | ComputeMinMax(Min_, Max_, VP); | ||
| 410 | } | ||
| 411 | else | ||
| 412 | { | ||
| 413 | const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB; | ||
| 414 | CurrentBox.GetMin(Min_); | ||
| 415 | CurrentBox.GetMax(Max_); | ||
| 416 | } | ||
| 417 | #ifdef OPC_USE_FCOMI | ||
| 418 | Min.x = FCMin2(Min.x, Min_.x); | ||
| 419 | Max.x = FCMax2(Max.x, Max_.x); | ||
| 420 | Min.y = FCMin2(Min.y, Min_.y); | ||
| 421 | Max.y = FCMax2(Max.y, Max_.y); | ||
| 422 | Min.z = FCMin2(Min.z, Min_.z); | ||
| 423 | Max.z = FCMax2(Max.z, Max_.z); | ||
| 424 | #else | ||
| 425 | Min.Min(Min_); | ||
| 426 | Max.Max(Max_); | ||
| 427 | #endif | ||
| 428 | Current.mAABB.SetMinMax(Min, Max); | ||
| 429 | } | ||
| 430 | return true; | ||
| 431 | } | ||
| 432 | |||
| 433 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 434 | /** | ||
| 435 | * Walks the tree and call the user back for each node. | ||
| 436 | * \param callback [in] walking callback | ||
| 437 | * \param user_data [in] callback's user data | ||
| 438 | * \return true if success | ||
| 439 | */ | ||
| 440 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 441 | bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const | ||
| 442 | { | ||
| 443 | if(!callback) return false; | ||
| 444 | |||
| 445 | struct Local | ||
| 446 | { | ||
| 447 | static void _Walk(const AABBNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data) | ||
| 448 | { | ||
| 449 | if(!current_node || !(callback)(current_node, user_data)) return; | ||
| 450 | |||
| 451 | if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data); | ||
| 452 | if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data); | ||
| 453 | } | ||
| 454 | }; | ||
| 455 | Local::_Walk(mNodes, callback, user_data); | ||
| 456 | return true; | ||
| 457 | } | ||
| 458 | |||
| 459 | // Quantization notes: | ||
| 460 | // - We could use the highest bits of mData to store some more quantized bits. Dequantization code | ||
| 461 | // would be slightly more complex, but number of overlap tests would be reduced (and anyhow those | ||
| 462 | // bits are currently wasted). Of course it's not possible if we move to 16 bits mData. | ||
| 463 | // - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion. | ||
| 464 | // - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some | ||
| 465 | // lazy-dequantization which may save some work in case of early exits). At the very least some | ||
| 466 | // muls could be saved by precomputing several more matrices. But maybe not worth the pain. | ||
| 467 | // - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has | ||
| 468 | // been scaled, for example. | ||
| 469 | // - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed | ||
| 470 | // number of quantization bits. Even better, could probably be best delta-encoded. | ||
| 471 | |||
| 472 | |||
| 473 | // Find max values. Some people asked why I wasn't simply using the first node. Well, I can't. | ||
| 474 | // I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal | ||
| 475 | // centers/extents in order to quantize them. The first node would only give a single center and | ||
| 476 | // a single extents. While extents would be the biggest, the center wouldn't. | ||
| 477 | #define FIND_MAX_VALUES \ | ||
| 478 | /* Get max values */ \ | ||
| 479 | Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \ | ||
| 480 | Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \ | ||
| 481 | for(udword i=0;i<mNbNodes;i++) \ | ||
| 482 | { \ | ||
| 483 | if(fabsf(Nodes[i].mAABB.mCenter.x)>CMax.x) CMax.x = fabsf(Nodes[i].mAABB.mCenter.x); \ | ||
| 484 | if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y) CMax.y = fabsf(Nodes[i].mAABB.mCenter.y); \ | ||
| 485 | if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z) CMax.z = fabsf(Nodes[i].mAABB.mCenter.z); \ | ||
| 486 | if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x) EMax.x = fabsf(Nodes[i].mAABB.mExtents.x); \ | ||
| 487 | if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y) EMax.y = fabsf(Nodes[i].mAABB.mExtents.y); \ | ||
| 488 | if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z) EMax.z = fabsf(Nodes[i].mAABB.mExtents.z); \ | ||
| 489 | } | ||
| 490 | |||
| 491 | #define INIT_QUANTIZATION \ | ||
| 492 | udword nbc=15; /* Keep one bit for sign */ \ | ||
| 493 | udword nbe=15; /* Keep one bit for fix */ \ | ||
| 494 | if(!gFixQuantized) nbe++; \ | ||
| 495 | \ | ||
| 496 | /* Compute quantization coeffs */ \ | ||
| 497 | Point CQuantCoeff, EQuantCoeff; \ | ||
| 498 | CQuantCoeff.x = CMax.x!=0.0f ? float((1<<nbc)-1)/CMax.x : 0.0f; \ | ||
| 499 | CQuantCoeff.y = CMax.y!=0.0f ? float((1<<nbc)-1)/CMax.y : 0.0f; \ | ||
| 500 | CQuantCoeff.z = CMax.z!=0.0f ? float((1<<nbc)-1)/CMax.z : 0.0f; \ | ||
| 501 | EQuantCoeff.x = EMax.x!=0.0f ? float((1<<nbe)-1)/EMax.x : 0.0f; \ | ||
| 502 | EQuantCoeff.y = EMax.y!=0.0f ? float((1<<nbe)-1)/EMax.y : 0.0f; \ | ||
| 503 | EQuantCoeff.z = EMax.z!=0.0f ? float((1<<nbe)-1)/EMax.z : 0.0f; \ | ||
| 504 | /* Compute and save dequantization coeffs */ \ | ||
| 505 | mCenterCoeff.x = CQuantCoeff.x!=0.0f ? 1.0f / CQuantCoeff.x : 0.0f; \ | ||
| 506 | mCenterCoeff.y = CQuantCoeff.y!=0.0f ? 1.0f / CQuantCoeff.y : 0.0f; \ | ||
| 507 | mCenterCoeff.z = CQuantCoeff.z!=0.0f ? 1.0f / CQuantCoeff.z : 0.0f; \ | ||
| 508 | mExtentsCoeff.x = EQuantCoeff.x!=0.0f ? 1.0f / EQuantCoeff.x : 0.0f; \ | ||
| 509 | mExtentsCoeff.y = EQuantCoeff.y!=0.0f ? 1.0f / EQuantCoeff.y : 0.0f; \ | ||
| 510 | mExtentsCoeff.z = EQuantCoeff.z!=0.0f ? 1.0f / EQuantCoeff.z : 0.0f; \ | ||
| 511 | |||
| 512 | #define PERFORM_QUANTIZATION \ | ||
| 513 | /* Quantize */ \ | ||
| 514 | mNodes[i].mAABB.mCenter[0] = sword(Nodes[i].mAABB.mCenter.x * CQuantCoeff.x); \ | ||
| 515 | mNodes[i].mAABB.mCenter[1] = sword(Nodes[i].mAABB.mCenter.y * CQuantCoeff.y); \ | ||
| 516 | mNodes[i].mAABB.mCenter[2] = sword(Nodes[i].mAABB.mCenter.z * CQuantCoeff.z); \ | ||
| 517 | mNodes[i].mAABB.mExtents[0] = uword(Nodes[i].mAABB.mExtents.x * EQuantCoeff.x); \ | ||
| 518 | mNodes[i].mAABB.mExtents[1] = uword(Nodes[i].mAABB.mExtents.y * EQuantCoeff.y); \ | ||
| 519 | mNodes[i].mAABB.mExtents[2] = uword(Nodes[i].mAABB.mExtents.z * EQuantCoeff.z); \ | ||
| 520 | /* Fix quantized boxes */ \ | ||
| 521 | if(gFixQuantized) \ | ||
| 522 | { \ | ||
| 523 | /* Make sure the quantized box is still valid */ \ | ||
| 524 | Point Max = Nodes[i].mAABB.mCenter + Nodes[i].mAABB.mExtents; \ | ||
| 525 | Point Min = Nodes[i].mAABB.mCenter - Nodes[i].mAABB.mExtents; \ | ||
| 526 | /* For each axis */ \ | ||
| 527 | for(udword j=0;j<3;j++) \ | ||
| 528 | { /* Dequantize the box center */ \ | ||
| 529 | float qc = float(mNodes[i].mAABB.mCenter[j]) * mCenterCoeff[j]; \ | ||
| 530 | bool FixMe=true; \ | ||
| 531 | do \ | ||
| 532 | { /* Dequantize the box extent */ \ | ||
| 533 | float qe = float(mNodes[i].mAABB.mExtents[j]) * mExtentsCoeff[j]; \ | ||
| 534 | /* Compare real & dequantized values */ \ | ||
| 535 | if(qc+qe<Max[j] || qc-qe>Min[j]) mNodes[i].mAABB.mExtents[j]++; \ | ||
| 536 | else FixMe=false; \ | ||
| 537 | /* Prevent wrapping */ \ | ||
| 538 | if(!mNodes[i].mAABB.mExtents[j]) \ | ||
| 539 | { \ | ||
| 540 | mNodes[i].mAABB.mExtents[j]=0xffff; \ | ||
| 541 | FixMe=false; \ | ||
| 542 | } \ | ||
| 543 | }while(FixMe); \ | ||
| 544 | } \ | ||
| 545 | } | ||
| 546 | |||
| 547 | #define REMAP_DATA(member) \ | ||
| 548 | /* Fix data */ \ | ||
| 549 | Data = Nodes[i].member; \ | ||
| 550 | if(!(Data&1)) \ | ||
| 551 | { \ | ||
| 552 | /* Compute box number */ \ | ||
| 553 | size_t Nb = (Data - size_t(Nodes))/Nodes[i].GetNodeSize(); \ | ||
| 554 | Data = (size_t) &mNodes[Nb]; \ | ||
| 555 | } \ | ||
| 556 | /* ...remapped */ \ | ||
| 557 | mNodes[i].member = Data; | ||
| 558 | |||
| 559 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 560 | /** | ||
| 561 | * Constructor. | ||
| 562 | */ | ||
| 563 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 564 | AABBQuantizedTree::AABBQuantizedTree() : mNodes(null) | ||
| 565 | { | ||
| 566 | } | ||
| 567 | |||
| 568 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 569 | /** | ||
| 570 | * Destructor. | ||
| 571 | */ | ||
| 572 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 573 | AABBQuantizedTree::~AABBQuantizedTree() | ||
| 574 | { | ||
| 575 | DELETEARRAY(mNodes); | ||
| 576 | } | ||
| 577 | |||
| 578 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 579 | /** | ||
| 580 | * Builds the collision tree from a generic AABB tree. | ||
| 581 | * \param tree [in] generic AABB tree | ||
| 582 | * \return true if success | ||
| 583 | */ | ||
| 584 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 585 | bool AABBQuantizedTree::Build(AABBTree* tree) | ||
| 586 | { | ||
| 587 | // Checkings | ||
| 588 | if(!tree) return false; | ||
| 589 | // Check the input tree is complete | ||
| 590 | udword NbTriangles = tree->GetNbPrimitives(); | ||
| 591 | udword NbNodes = tree->GetNbNodes(); | ||
| 592 | if(NbNodes!=NbTriangles*2-1) return false; | ||
| 593 | |||
| 594 | // Get nodes | ||
| 595 | mNbNodes = NbNodes; | ||
| 596 | DELETEARRAY(mNodes); | ||
| 597 | AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes]; | ||
| 598 | CHECKALLOC(Nodes); | ||
| 599 | |||
| 600 | // Build the tree | ||
| 601 | udword CurID = 1; | ||
| 602 | _BuildCollisionTree(Nodes, 0, CurID, tree); | ||
| 603 | |||
| 604 | // Quantize | ||
| 605 | { | ||
| 606 | mNodes = new AABBQuantizedNode[mNbNodes]; | ||
| 607 | CHECKALLOC(mNodes); | ||
| 608 | |||
| 609 | // Get max values | ||
| 610 | FIND_MAX_VALUES | ||
| 611 | |||
| 612 | // Quantization | ||
| 613 | INIT_QUANTIZATION | ||
| 614 | |||
| 615 | // Quantize | ||
| 616 | size_t Data; | ||
| 617 | for(udword i=0;i<mNbNodes;i++) | ||
| 618 | { | ||
| 619 | PERFORM_QUANTIZATION | ||
| 620 | REMAP_DATA(mData) | ||
| 621 | } | ||
| 622 | |||
| 623 | DELETEARRAY(Nodes); | ||
| 624 | } | ||
| 625 | |||
| 626 | return true; | ||
| 627 | } | ||
| 628 | |||
| 629 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 630 | /** | ||
| 631 | * Refits the collision tree after vertices have been modified. | ||
| 632 | * \param mesh_interface [in] mesh interface for current model | ||
| 633 | * \return true if success | ||
| 634 | */ | ||
| 635 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 636 | bool AABBQuantizedTree::Refit(const MeshInterface* mesh_interface) | ||
| 637 | { | ||
| 638 | ASSERT(!"Not implemented since requantizing is painful !"); | ||
| 639 | return false; | ||
| 640 | } | ||
| 641 | |||
| 642 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 643 | /** | ||
| 644 | * Walks the tree and call the user back for each node. | ||
| 645 | * \param callback [in] walking callback | ||
| 646 | * \param user_data [in] callback's user data | ||
| 647 | * \return true if success | ||
| 648 | */ | ||
| 649 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 650 | bool AABBQuantizedTree::Walk(GenericWalkingCallback callback, void* user_data) const | ||
| 651 | { | ||
| 652 | if(!callback) return false; | ||
| 653 | |||
| 654 | struct Local | ||
| 655 | { | ||
| 656 | static void _Walk(const AABBQuantizedNode* current_node, GenericWalkingCallback callback, void* user_data) | ||
| 657 | { | ||
| 658 | if(!current_node || !(callback)(current_node, user_data)) return; | ||
| 659 | |||
| 660 | if(!current_node->IsLeaf()) | ||
| 661 | { | ||
| 662 | _Walk(current_node->GetPos(), callback, user_data); | ||
| 663 | _Walk(current_node->GetNeg(), callback, user_data); | ||
| 664 | } | ||
| 665 | } | ||
| 666 | }; | ||
| 667 | Local::_Walk(mNodes, callback, user_data); | ||
| 668 | return true; | ||
| 669 | } | ||
| 670 | |||
| 671 | |||
| 672 | |||
| 673 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 674 | /** | ||
| 675 | * Constructor. | ||
| 676 | */ | ||
| 677 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 678 | AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null) | ||
| 679 | { | ||
| 680 | } | ||
| 681 | |||
| 682 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 683 | /** | ||
| 684 | * Destructor. | ||
| 685 | */ | ||
| 686 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 687 | AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree() | ||
| 688 | { | ||
| 689 | DELETEARRAY(mNodes); | ||
| 690 | } | ||
| 691 | |||
| 692 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 693 | /** | ||
| 694 | * Builds the collision tree from a generic AABB tree. | ||
| 695 | * \param tree [in] generic AABB tree | ||
| 696 | * \return true if success | ||
| 697 | */ | ||
| 698 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 699 | bool AABBQuantizedNoLeafTree::Build(AABBTree* tree) | ||
| 700 | { | ||
| 701 | // Checkings | ||
| 702 | if(!tree) return false; | ||
| 703 | // Check the input tree is complete | ||
| 704 | udword NbTriangles = tree->GetNbPrimitives(); | ||
| 705 | udword NbNodes = tree->GetNbNodes(); | ||
| 706 | if(NbNodes!=NbTriangles*2-1) return false; | ||
| 707 | |||
| 708 | // Get nodes | ||
| 709 | mNbNodes = NbTriangles-1; | ||
| 710 | DELETEARRAY(mNodes); | ||
| 711 | AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes]; | ||
| 712 | CHECKALLOC(Nodes); | ||
| 713 | |||
| 714 | // Build the tree | ||
| 715 | udword CurID = 1; | ||
| 716 | _BuildNoLeafTree(Nodes, 0, CurID, tree); | ||
| 717 | ASSERT(CurID==mNbNodes); | ||
| 718 | |||
| 719 | // Quantize | ||
| 720 | { | ||
| 721 | mNodes = new AABBQuantizedNoLeafNode[mNbNodes]; | ||
| 722 | CHECKALLOC(mNodes); | ||
| 723 | |||
| 724 | // Get max values | ||
| 725 | FIND_MAX_VALUES | ||
| 726 | |||
| 727 | // Quantization | ||
| 728 | INIT_QUANTIZATION | ||
| 729 | |||
| 730 | // Quantize | ||
| 731 | size_t Data; | ||
| 732 | for(udword i=0;i<mNbNodes;i++) | ||
| 733 | { | ||
| 734 | PERFORM_QUANTIZATION | ||
| 735 | REMAP_DATA(mPosData) | ||
| 736 | REMAP_DATA(mNegData) | ||
| 737 | } | ||
| 738 | |||
| 739 | DELETEARRAY(Nodes); | ||
| 740 | } | ||
| 741 | |||
| 742 | return true; | ||
| 743 | } | ||
| 744 | |||
| 745 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 746 | /** | ||
| 747 | * Refits the collision tree after vertices have been modified. | ||
| 748 | * \param mesh_interface [in] mesh interface for current model | ||
| 749 | * \return true if success | ||
| 750 | */ | ||
| 751 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 752 | bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* mesh_interface) | ||
| 753 | { | ||
| 754 | ASSERT(!"Not implemented since requantizing is painful !"); | ||
| 755 | return false; | ||
| 756 | } | ||
| 757 | |||
| 758 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 759 | /** | ||
| 760 | * Walks the tree and call the user back for each node. | ||
| 761 | * \param callback [in] walking callback | ||
| 762 | * \param user_data [in] callback's user data | ||
| 763 | * \return true if success | ||
| 764 | */ | ||
| 765 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 766 | bool AABBQuantizedNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const | ||
| 767 | { | ||
| 768 | if(!callback) return false; | ||
| 769 | |||
| 770 | struct Local | ||
| 771 | { | ||
| 772 | static void _Walk(const AABBQuantizedNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data) | ||
| 773 | { | ||
| 774 | if(!current_node || !(callback)(current_node, user_data)) return; | ||
| 775 | |||
| 776 | if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data); | ||
| 777 | if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data); | ||
| 778 | } | ||
| 779 | }; | ||
| 780 | Local::_Walk(mNodes, callback, user_data); | ||
| 781 | return true; | ||
| 782 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_OptimizedTree.h b/libraries/ode-0.9/OPCODE/OPC_OptimizedTree.h new file mode 100644 index 0000000..36aea07 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_OptimizedTree.h | |||
| @@ -0,0 +1,206 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for optimized trees. | ||
| 12 | * \file OPC_OptimizedTree.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_OPTIMIZEDTREE_H__ | ||
| 21 | #define __OPC_OPTIMIZEDTREE_H__ | ||
| 22 | |||
| 23 | //! Common interface for a node of an implicit tree | ||
| 24 | #define IMPLEMENT_IMPLICIT_NODE(base_class, volume) \ | ||
| 25 | public: \ | ||
| 26 | /* Constructor / Destructor */ \ | ||
| 27 | inline_ base_class() : mData(0) {} \ | ||
| 28 | inline_ ~base_class() {} \ | ||
| 29 | /* Leaf test */ \ | ||
| 30 | inline_ BOOL IsLeaf() const { return (mData&1)!=0; } \ | ||
| 31 | /* Data access */ \ | ||
| 32 | inline_ const base_class* GetPos() const { return (base_class*)mData; } \ | ||
| 33 | inline_ const base_class* GetNeg() const { return ((base_class*)mData)+1; } \ | ||
| 34 | inline_ size_t GetPrimitive() const { return (mData>>1); } \ | ||
| 35 | /* Stats */ \ | ||
| 36 | inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ | ||
| 37 | \ | ||
| 38 | volume mAABB; \ | ||
| 39 | size_t mData; | ||
| 40 | |||
| 41 | //! Common interface for a node of a no-leaf tree | ||
| 42 | #define IMPLEMENT_NOLEAF_NODE(base_class, volume) \ | ||
| 43 | public: \ | ||
| 44 | /* Constructor / Destructor */ \ | ||
| 45 | inline_ base_class() : mPosData(0), mNegData(0) {} \ | ||
| 46 | inline_ ~base_class() {} \ | ||
| 47 | /* Leaf tests */ \ | ||
| 48 | inline_ BOOL HasPosLeaf() const { return (mPosData&1)!=0; } \ | ||
| 49 | inline_ BOOL HasNegLeaf() const { return (mNegData&1)!=0; } \ | ||
| 50 | /* Data access */ \ | ||
| 51 | inline_ const base_class* GetPos() const { return (base_class*)mPosData; } \ | ||
| 52 | inline_ const base_class* GetNeg() const { return (base_class*)mNegData; } \ | ||
| 53 | inline_ size_t GetPosPrimitive() const { return (mPosData>>1); } \ | ||
| 54 | inline_ size_t GetNegPrimitive() const { return (mNegData>>1); } \ | ||
| 55 | /* Stats */ \ | ||
| 56 | inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \ | ||
| 57 | \ | ||
| 58 | volume mAABB; \ | ||
| 59 | size_t mPosData; \ | ||
| 60 | size_t mNegData; | ||
| 61 | |||
| 62 | class OPCODE_API AABBCollisionNode | ||
| 63 | { | ||
| 64 | IMPLEMENT_IMPLICIT_NODE(AABBCollisionNode, CollisionAABB) | ||
| 65 | |||
| 66 | inline_ float GetVolume() const { return mAABB.mExtents.x * mAABB.mExtents.y * mAABB.mExtents.z; } | ||
| 67 | inline_ float GetSize() const { return mAABB.mExtents.SquareMagnitude(); } | ||
| 68 | inline_ udword GetRadius() const | ||
| 69 | { | ||
| 70 | udword* Bits = (udword*)&mAABB.mExtents.x; | ||
| 71 | udword Max = Bits[0]; | ||
| 72 | if(Bits[1]>Max) Max = Bits[1]; | ||
| 73 | if(Bits[2]>Max) Max = Bits[2]; | ||
| 74 | return Max; | ||
| 75 | } | ||
| 76 | |||
| 77 | // NB: using the square-magnitude or the true volume of the box, seems to yield better results | ||
| 78 | // (assuming UNC-like informed traversal methods). I borrowed this idea from PQP. The usual "size" | ||
| 79 | // otherwise, is the largest box extent. In SOLID that extent is computed on-the-fly each time it's | ||
| 80 | // needed (the best approach IMHO). In RAPID the rotation matrix is permuted so that Extent[0] is | ||
| 81 | // always the greatest, which saves looking for it at runtime. On the other hand, it yields matrices | ||
| 82 | // whose determinant is not 1, i.e. you can't encode them anymore as unit quaternions. Not a very | ||
| 83 | // good strategy. | ||
| 84 | }; | ||
| 85 | |||
| 86 | class OPCODE_API AABBQuantizedNode | ||
| 87 | { | ||
| 88 | IMPLEMENT_IMPLICIT_NODE(AABBQuantizedNode, QuantizedAABB) | ||
| 89 | |||
| 90 | inline_ uword GetSize() const | ||
| 91 | { | ||
| 92 | const uword* Bits = mAABB.mExtents; | ||
| 93 | uword Max = Bits[0]; | ||
| 94 | if(Bits[1]>Max) Max = Bits[1]; | ||
| 95 | if(Bits[2]>Max) Max = Bits[2]; | ||
| 96 | return Max; | ||
| 97 | } | ||
| 98 | // NB: for quantized nodes I don't feel like computing a square-magnitude with integers all | ||
| 99 | // over the place.......! | ||
| 100 | }; | ||
| 101 | |||
| 102 | class OPCODE_API AABBNoLeafNode | ||
| 103 | { | ||
| 104 | IMPLEMENT_NOLEAF_NODE(AABBNoLeafNode, CollisionAABB) | ||
| 105 | }; | ||
| 106 | |||
| 107 | class OPCODE_API AABBQuantizedNoLeafNode | ||
| 108 | { | ||
| 109 | IMPLEMENT_NOLEAF_NODE(AABBQuantizedNoLeafNode, QuantizedAABB) | ||
| 110 | }; | ||
| 111 | |||
| 112 | //! Common interface for a collision tree | ||
| 113 | #define IMPLEMENT_COLLISION_TREE(base_class, node) \ | ||
| 114 | public: \ | ||
| 115 | /* Constructor / Destructor */ \ | ||
| 116 | base_class(); \ | ||
| 117 | virtual ~base_class(); \ | ||
| 118 | /* Builds from a standard tree */ \ | ||
| 119 | override(AABBOptimizedTree) bool Build(AABBTree* tree); \ | ||
| 120 | /* Refits the tree */ \ | ||
| 121 | override(AABBOptimizedTree) bool Refit(const MeshInterface* mesh_interface); \ | ||
| 122 | /* Walks the tree */ \ | ||
| 123 | override(AABBOptimizedTree) bool Walk(GenericWalkingCallback callback, void* user_data) const; \ | ||
| 124 | /* Data access */ \ | ||
| 125 | inline_ const node* GetNodes() const { return mNodes; } \ | ||
| 126 | /* Stats */ \ | ||
| 127 | override(AABBOptimizedTree) udword GetUsedBytes() const { return mNbNodes*sizeof(node); } \ | ||
| 128 | private: \ | ||
| 129 | node* mNodes; | ||
| 130 | |||
| 131 | typedef bool (*GenericWalkingCallback) (const void* current, void* user_data); | ||
| 132 | |||
| 133 | class OPCODE_API AABBOptimizedTree | ||
| 134 | { | ||
| 135 | public: | ||
| 136 | // Constructor / Destructor | ||
| 137 | AABBOptimizedTree() : | ||
| 138 | mNbNodes (0) | ||
| 139 | {} | ||
| 140 | virtual ~AABBOptimizedTree() {} | ||
| 141 | |||
| 142 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 143 | /** | ||
| 144 | * Builds the collision tree from a generic AABB tree. | ||
| 145 | * \param tree [in] generic AABB tree | ||
| 146 | * \return true if success | ||
| 147 | */ | ||
| 148 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 149 | virtual bool Build(AABBTree* tree) = 0; | ||
| 150 | |||
| 151 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 152 | /** | ||
| 153 | * Refits the collision tree after vertices have been modified. | ||
| 154 | * \param mesh_interface [in] mesh interface for current model | ||
| 155 | * \return true if success | ||
| 156 | */ | ||
| 157 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 158 | virtual bool Refit(const MeshInterface* mesh_interface) = 0; | ||
| 159 | |||
| 160 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 161 | /** | ||
| 162 | * Walks the tree and call the user back for each node. | ||
| 163 | * \param callback [in] walking callback | ||
| 164 | * \param user_data [in] callback's user data | ||
| 165 | * \return true if success | ||
| 166 | */ | ||
| 167 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 168 | virtual bool Walk(GenericWalkingCallback callback, void* user_data) const = 0; | ||
| 169 | |||
| 170 | // Data access | ||
| 171 | virtual udword GetUsedBytes() const = 0; | ||
| 172 | inline_ udword GetNbNodes() const { return mNbNodes; } | ||
| 173 | |||
| 174 | protected: | ||
| 175 | udword mNbNodes; | ||
| 176 | }; | ||
| 177 | |||
| 178 | class OPCODE_API AABBCollisionTree : public AABBOptimizedTree | ||
| 179 | { | ||
| 180 | IMPLEMENT_COLLISION_TREE(AABBCollisionTree, AABBCollisionNode) | ||
| 181 | }; | ||
| 182 | |||
| 183 | class OPCODE_API AABBNoLeafTree : public AABBOptimizedTree | ||
| 184 | { | ||
| 185 | IMPLEMENT_COLLISION_TREE(AABBNoLeafTree, AABBNoLeafNode) | ||
| 186 | }; | ||
| 187 | |||
| 188 | class OPCODE_API AABBQuantizedTree : public AABBOptimizedTree | ||
| 189 | { | ||
| 190 | IMPLEMENT_COLLISION_TREE(AABBQuantizedTree, AABBQuantizedNode) | ||
| 191 | |||
| 192 | public: | ||
| 193 | Point mCenterCoeff; | ||
| 194 | Point mExtentsCoeff; | ||
| 195 | }; | ||
| 196 | |||
| 197 | class OPCODE_API AABBQuantizedNoLeafTree : public AABBOptimizedTree | ||
| 198 | { | ||
| 199 | IMPLEMENT_COLLISION_TREE(AABBQuantizedNoLeafTree, AABBQuantizedNoLeafNode) | ||
| 200 | |||
| 201 | public: | ||
| 202 | Point mCenterCoeff; | ||
| 203 | Point mExtentsCoeff; | ||
| 204 | }; | ||
| 205 | |||
| 206 | #endif // __OPC_OPTIMIZEDTREE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Picking.cpp b/libraries/ode-0.9/OPCODE/OPC_Picking.cpp new file mode 100644 index 0000000..3b53774 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Picking.cpp | |||
| @@ -0,0 +1,182 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code to perform "picking". | ||
| 12 | * \file OPC_Picking.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Precompiled Header | ||
| 20 | #include "Stdafx.h" | ||
| 21 | |||
| 22 | using namespace Opcode; | ||
| 23 | |||
| 24 | #ifdef OPC_RAYHIT_CALLBACK | ||
| 25 | |||
| 26 | /* | ||
| 27 | Possible RayCollider usages: | ||
| 28 | - boolean query (shadow feeler) | ||
| 29 | - closest hit | ||
| 30 | - all hits | ||
| 31 | - number of intersection (boolean) | ||
| 32 | |||
| 33 | */ | ||
| 34 | |||
| 35 | bool Opcode::SetupAllHits(RayCollider& collider, CollisionFaces& contacts) | ||
| 36 | { | ||
| 37 | struct Local | ||
| 38 | { | ||
| 39 | static void AllContacts(const CollisionFace& hit, void* user_data) | ||
| 40 | { | ||
| 41 | CollisionFaces* CF = (CollisionFaces*)user_data; | ||
| 42 | CF->AddFace(hit); | ||
| 43 | } | ||
| 44 | }; | ||
| 45 | |||
| 46 | collider.SetFirstContact(false); | ||
| 47 | collider.SetHitCallback(Local::AllContacts); | ||
| 48 | collider.SetUserData(&contacts); | ||
| 49 | return true; | ||
| 50 | } | ||
| 51 | |||
| 52 | bool Opcode::SetupClosestHit(RayCollider& collider, CollisionFace& closest_contact) | ||
| 53 | { | ||
| 54 | struct Local | ||
| 55 | { | ||
| 56 | static void ClosestContact(const CollisionFace& hit, void* user_data) | ||
| 57 | { | ||
| 58 | CollisionFace* CF = (CollisionFace*)user_data; | ||
| 59 | if(hit.mDistance<CF->mDistance) *CF = hit; | ||
| 60 | } | ||
| 61 | }; | ||
| 62 | |||
| 63 | collider.SetFirstContact(false); | ||
| 64 | collider.SetHitCallback(Local::ClosestContact); | ||
| 65 | collider.SetUserData(&closest_contact); | ||
| 66 | closest_contact.mDistance = MAX_FLOAT; | ||
| 67 | return true; | ||
| 68 | } | ||
| 69 | |||
| 70 | bool Opcode::SetupShadowFeeler(RayCollider& collider) | ||
| 71 | { | ||
| 72 | collider.SetFirstContact(true); | ||
| 73 | collider.SetHitCallback(null); | ||
| 74 | return true; | ||
| 75 | } | ||
| 76 | |||
| 77 | bool Opcode::SetupInOutTest(RayCollider& collider) | ||
| 78 | { | ||
| 79 | collider.SetFirstContact(false); | ||
| 80 | collider.SetHitCallback(null); | ||
| 81 | // Results with collider.GetNbIntersections() | ||
| 82 | return true; | ||
| 83 | } | ||
| 84 | |||
| 85 | bool Opcode::Picking( | ||
| 86 | CollisionFace& picked_face, | ||
| 87 | const Ray& world_ray, const Model& model, const Matrix4x4* world, | ||
| 88 | float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data) | ||
| 89 | { | ||
| 90 | struct Local | ||
| 91 | { | ||
| 92 | struct CullData | ||
| 93 | { | ||
| 94 | CollisionFace* Closest; | ||
| 95 | float MinLimit; | ||
| 96 | CullModeCallback Callback; | ||
| 97 | void* UserData; | ||
| 98 | Point ViewPoint; | ||
| 99 | const MeshInterface* IMesh; | ||
| 100 | }; | ||
| 101 | |||
| 102 | // Called for each stabbed face | ||
| 103 | static void RenderCullingCallback(const CollisionFace& hit, void* user_data) | ||
| 104 | { | ||
| 105 | CullData* Data = (CullData*)user_data; | ||
| 106 | |||
| 107 | // Discard face if we already have a closer hit | ||
| 108 | if(hit.mDistance>=Data->Closest->mDistance) return; | ||
| 109 | |||
| 110 | // Discard face if hit point is smaller than min limit. This mainly happens when the face is in front | ||
| 111 | // of the near clip plane (or straddles it). If we keep the face nonetheless, the user can select an | ||
| 112 | // object that he may not even be able to see, which is very annoying. | ||
| 113 | if(hit.mDistance<=Data->MinLimit) return; | ||
| 114 | |||
| 115 | // This is the index of currently stabbed triangle. | ||
| 116 | udword StabbedFaceIndex = hit.mFaceID; | ||
| 117 | |||
| 118 | // We may keep it or not, depending on backface culling | ||
| 119 | bool KeepIt = true; | ||
| 120 | |||
| 121 | // Catch *render* cull mode for this face | ||
| 122 | CullMode CM = (Data->Callback)(StabbedFaceIndex, Data->UserData); | ||
| 123 | |||
| 124 | if(CM!=CULLMODE_NONE) // Don't even compute culling for double-sided triangles | ||
| 125 | { | ||
| 126 | // Compute backface culling for current face | ||
| 127 | |||
| 128 | VertexPointers VP; | ||
| 129 | Data->IMesh->GetTriangle(VP, StabbedFaceIndex); | ||
| 130 | if(VP.BackfaceCulling(Data->ViewPoint)) | ||
| 131 | { | ||
| 132 | if(CM==CULLMODE_CW) KeepIt = false; | ||
| 133 | } | ||
| 134 | else | ||
| 135 | { | ||
| 136 | if(CM==CULLMODE_CCW) KeepIt = false; | ||
| 137 | } | ||
| 138 | } | ||
| 139 | |||
| 140 | if(KeepIt) *Data->Closest = hit; | ||
| 141 | } | ||
| 142 | }; | ||
| 143 | |||
| 144 | RayCollider RC; | ||
| 145 | RC.SetMaxDist(max_dist); | ||
| 146 | RC.SetTemporalCoherence(false); | ||
| 147 | RC.SetCulling(false); // We need all faces since some of them can be double-sided | ||
| 148 | RC.SetFirstContact(false); | ||
| 149 | RC.SetHitCallback(Local::RenderCullingCallback); | ||
| 150 | |||
| 151 | picked_face.mFaceID = INVALID_ID; | ||
| 152 | picked_face.mDistance = MAX_FLOAT; | ||
| 153 | picked_face.mU = 0.0f; | ||
| 154 | picked_face.mV = 0.0f; | ||
| 155 | |||
| 156 | Local::CullData Data; | ||
| 157 | Data.Closest = &picked_face; | ||
| 158 | Data.MinLimit = min_dist; | ||
| 159 | Data.Callback = callback; | ||
| 160 | Data.UserData = user_data; | ||
| 161 | Data.ViewPoint = view_point; | ||
| 162 | Data.IMesh = model.GetMeshInterface(); | ||
| 163 | |||
| 164 | if(world) | ||
| 165 | { | ||
| 166 | // Get matrices | ||
| 167 | Matrix4x4 InvWorld; | ||
| 168 | InvertPRMatrix(InvWorld, *world); | ||
| 169 | |||
| 170 | // Compute camera position in mesh space | ||
| 171 | Data.ViewPoint *= InvWorld; | ||
| 172 | } | ||
| 173 | |||
| 174 | RC.SetUserData(&Data); | ||
| 175 | if(RC.Collide(world_ray, model, world)) | ||
| 176 | { | ||
| 177 | return picked_face.mFaceID!=INVALID_ID; | ||
| 178 | } | ||
| 179 | return false; | ||
| 180 | } | ||
| 181 | |||
| 182 | #endif | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Picking.h b/libraries/ode-0.9/OPCODE/OPC_Picking.h new file mode 100644 index 0000000..d22fa38 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Picking.h | |||
| @@ -0,0 +1,45 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code to perform "picking". | ||
| 12 | * \file OPC_Picking.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_PICKING_H__ | ||
| 21 | #define __OPC_PICKING_H__ | ||
| 22 | |||
| 23 | #ifdef OPC_RAYHIT_CALLBACK | ||
| 24 | |||
| 25 | enum CullMode | ||
| 26 | { | ||
| 27 | CULLMODE_NONE = 0, | ||
| 28 | CULLMODE_CW = 1, | ||
| 29 | CULLMODE_CCW = 2 | ||
| 30 | }; | ||
| 31 | |||
| 32 | typedef CullMode (*CullModeCallback)(udword triangle_index, void* user_data); | ||
| 33 | |||
| 34 | OPCODE_API bool SetupAllHits (RayCollider& collider, CollisionFaces& contacts); | ||
| 35 | OPCODE_API bool SetupClosestHit (RayCollider& collider, CollisionFace& closest_contact); | ||
| 36 | OPCODE_API bool SetupShadowFeeler (RayCollider& collider); | ||
| 37 | OPCODE_API bool SetupInOutTest (RayCollider& collider); | ||
| 38 | |||
| 39 | OPCODE_API bool Picking( | ||
| 40 | CollisionFace& picked_face, | ||
| 41 | const Ray& world_ray, const Model& model, const Matrix4x4* world, | ||
| 42 | float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data); | ||
| 43 | #endif | ||
| 44 | |||
| 45 | #endif //__OPC_PICKING_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_PlanesAABBOverlap.h b/libraries/ode-0.9/OPCODE/OPC_PlanesAABBOverlap.h new file mode 100644 index 0000000..5d7576e --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_PlanesAABBOverlap.h | |||
| @@ -0,0 +1,50 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Planes-AABB overlap test. | ||
| 4 | * - original code by Ville Miettinen, from Umbra/dPVS (released on the GD-Algorithms mailing list) | ||
| 5 | * - almost used "as-is", I even left the comments (hence the frustum-related notes) | ||
| 6 | * | ||
| 7 | * \param center [in] box center | ||
| 8 | * \param extents [in] box extents | ||
| 9 | * \param out_clip_mask [out] bitmask for active planes | ||
| 10 | * \param in_clip_mask [in] bitmask for active planes | ||
| 11 | * \return TRUE if boxes overlap planes | ||
| 12 | */ | ||
| 13 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 14 | inline_ BOOL PlanesCollider::PlanesAABBOverlap(const Point& center, const Point& extents, udword& out_clip_mask, udword in_clip_mask) | ||
| 15 | { | ||
| 16 | // Stats | ||
| 17 | mNbVolumeBVTests++; | ||
| 18 | |||
| 19 | const Plane* p = mPlanes; | ||
| 20 | |||
| 21 | // Evaluate through all active frustum planes. We determine the relation | ||
| 22 | // between the AABB and a plane by using the concept of "near" and "far" | ||
| 23 | // vertices originally described by Zhang (and later by Möller). Our | ||
| 24 | // variant here uses 3 fabs ops, 6 muls, 7 adds and two floating point | ||
| 25 | // comparisons per plane. The routine early-exits if the AABB is found | ||
| 26 | // to be outside any of the planes. The loop also constructs a new output | ||
| 27 | // clip mask. Most FPUs have a native single-cycle fabsf() operation. | ||
| 28 | |||
| 29 | udword Mask = 1; // current mask index (1,2,4,8,..) | ||
| 30 | udword TmpOutClipMask = 0; // initialize output clip mask into empty. | ||
| 31 | |||
| 32 | while(Mask<=in_clip_mask) // keep looping while we have active planes left... | ||
| 33 | { | ||
| 34 | if(in_clip_mask & Mask) // if clip plane is active, process it.. | ||
| 35 | { | ||
| 36 | float NP = extents.x*fabsf(p->n.x) + extents.y*fabsf(p->n.y) + extents.z*fabsf(p->n.z); // ### fabsf could be precomputed | ||
| 37 | float MP = center.x*p->n.x + center.y*p->n.y + center.z*p->n.z + p->d; | ||
| 38 | |||
| 39 | if(NP < MP) // near vertex behind the clip plane... | ||
| 40 | return FALSE; // .. so there is no intersection.. | ||
| 41 | if((-NP) < MP) // near and far vertices on different sides of plane.. | ||
| 42 | TmpOutClipMask |= Mask; // .. so update the clip mask... | ||
| 43 | } | ||
| 44 | Mask+=Mask; // mk = (1<<plane) | ||
| 45 | p++; // advance to next plane | ||
| 46 | } | ||
| 47 | |||
| 48 | out_clip_mask = TmpOutClipMask; // copy output value (temp used to resolve aliasing!) | ||
| 49 | return TRUE; // indicate that AABB intersects frustum | ||
| 50 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_PlanesCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_PlanesCollider.cpp new file mode 100644 index 0000000..60d7f9c --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_PlanesCollider.cpp | |||
| @@ -0,0 +1,653 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a planes collider. | ||
| 12 | * \file OPC_PlanesCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 1st, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains a Planes-vs-tree collider. | ||
| 21 | * | ||
| 22 | * \class PlanesCollider | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date January, 1st, 2002 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | // Precompiled Header | ||
| 31 | #include "Stdafx.h" | ||
| 32 | |||
| 33 | using namespace Opcode; | ||
| 34 | |||
| 35 | #include "OPC_PlanesAABBOverlap.h" | ||
| 36 | #include "OPC_PlanesTriOverlap.h" | ||
| 37 | |||
| 38 | #define SET_CONTACT(prim_index, flag) \ | ||
| 39 | /* Set contact status */ \ | ||
| 40 | mFlags |= flag; \ | ||
| 41 | mTouchedPrimitives->Add(udword(prim_index)); | ||
| 42 | |||
| 43 | //! Planes-triangle test | ||
| 44 | #define PLANES_PRIM(prim_index, flag) \ | ||
| 45 | /* Request vertices from the app */ \ | ||
| 46 | mIMesh->GetTriangle(mVP, prim_index); \ | ||
| 47 | /* Perform triangle-box overlap test */ \ | ||
| 48 | if(PlanesTriOverlap(clip_mask)) \ | ||
| 49 | { \ | ||
| 50 | SET_CONTACT(prim_index, flag) \ | ||
| 51 | } | ||
| 52 | |||
| 53 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 54 | /** | ||
| 55 | * Constructor. | ||
| 56 | */ | ||
| 57 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 58 | PlanesCollider::PlanesCollider() : | ||
| 59 | mPlanes (null), | ||
| 60 | mNbPlanes (0) | ||
| 61 | { | ||
| 62 | } | ||
| 63 | |||
| 64 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 65 | /** | ||
| 66 | * Destructor. | ||
| 67 | */ | ||
| 68 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 69 | PlanesCollider::~PlanesCollider() | ||
| 70 | { | ||
| 71 | DELETEARRAY(mPlanes); | ||
| 72 | } | ||
| 73 | |||
| 74 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 75 | /** | ||
| 76 | * Validates current settings. You should call this method after all the settings and callbacks have been defined. | ||
| 77 | * \return null if everything is ok, else a string describing the problem | ||
| 78 | */ | ||
| 79 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 80 | const char* PlanesCollider::ValidateSettings() | ||
| 81 | { | ||
| 82 | if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; | ||
| 83 | |||
| 84 | return VolumeCollider::ValidateSettings(); | ||
| 85 | } | ||
| 86 | |||
| 87 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 88 | /** | ||
| 89 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 90 | * - with GetContactStatus() | ||
| 91 | * - with GetNbTouchedPrimitives() | ||
| 92 | * - with GetTouchedPrimitives() | ||
| 93 | * | ||
| 94 | * \param cache [in/out] a planes cache | ||
| 95 | * \param planes [in] list of planes in world space | ||
| 96 | * \param nb_planes [in] number of planes | ||
| 97 | * \param model [in] Opcode model to collide with | ||
| 98 | * \param worldm [in] model's world matrix, or null | ||
| 99 | * \return true if success | ||
| 100 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 101 | */ | ||
| 102 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 103 | bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm) | ||
| 104 | { | ||
| 105 | // Checkings | ||
| 106 | if(!Setup(&model)) return false; | ||
| 107 | |||
| 108 | // Init collision query | ||
| 109 | if(InitQuery(cache, planes, nb_planes, worldm)) return true; | ||
| 110 | |||
| 111 | udword PlaneMask = (1<<nb_planes)-1; | ||
| 112 | |||
| 113 | if(!model.HasLeafNodes()) | ||
| 114 | { | ||
| 115 | if(model.IsQuantized()) | ||
| 116 | { | ||
| 117 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 118 | |||
| 119 | // Setup dequantization coeffs | ||
| 120 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 121 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 122 | |||
| 123 | // Perform collision query | ||
| 124 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 125 | else _Collide(Tree->GetNodes(), PlaneMask); | ||
| 126 | } | ||
| 127 | else | ||
| 128 | { | ||
| 129 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 130 | |||
| 131 | // Perform collision query | ||
| 132 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 133 | else _Collide(Tree->GetNodes(), PlaneMask); | ||
| 134 | } | ||
| 135 | } | ||
| 136 | else | ||
| 137 | { | ||
| 138 | if(model.IsQuantized()) | ||
| 139 | { | ||
| 140 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 141 | |||
| 142 | // Setup dequantization coeffs | ||
| 143 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 144 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 145 | |||
| 146 | // Perform collision query | ||
| 147 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 148 | else _Collide(Tree->GetNodes(), PlaneMask); | ||
| 149 | } | ||
| 150 | else | ||
| 151 | { | ||
| 152 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 153 | |||
| 154 | // Perform collision query | ||
| 155 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 156 | else _Collide(Tree->GetNodes(), PlaneMask); | ||
| 157 | } | ||
| 158 | } | ||
| 159 | return true; | ||
| 160 | } | ||
| 161 | |||
| 162 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 163 | /** | ||
| 164 | * Initializes a collision query : | ||
| 165 | * - reset stats & contact status | ||
| 166 | * - compute planes in model space | ||
| 167 | * - check temporal coherence | ||
| 168 | * | ||
| 169 | * \param cache [in/out] a planes cache | ||
| 170 | * \param planes [in] list of planes | ||
| 171 | * \param nb_planes [in] number of planes | ||
| 172 | * \param worldm [in] model's world matrix, or null | ||
| 173 | * \return TRUE if we can return immediately | ||
| 174 | * \warning SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only. | ||
| 175 | */ | ||
| 176 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 177 | BOOL PlanesCollider::InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm) | ||
| 178 | { | ||
| 179 | // 1) Call the base method | ||
| 180 | VolumeCollider::InitQuery(); | ||
| 181 | |||
| 182 | // 2) Compute planes in model space | ||
| 183 | if(nb_planes>mNbPlanes) | ||
| 184 | { | ||
| 185 | DELETEARRAY(mPlanes); | ||
| 186 | mPlanes = new Plane[nb_planes]; | ||
| 187 | } | ||
| 188 | mNbPlanes = nb_planes; | ||
| 189 | |||
| 190 | if(worldm) | ||
| 191 | { | ||
| 192 | Matrix4x4 InvWorldM; | ||
| 193 | InvertPRMatrix(InvWorldM, *worldm); | ||
| 194 | |||
| 195 | // for(udword i=0;i<nb_planes;i++) mPlanes[i] = planes[i] * InvWorldM; | ||
| 196 | for(udword i=0;i<nb_planes;i++) TransformPlane(mPlanes[i], planes[i], InvWorldM); | ||
| 197 | } | ||
| 198 | else CopyMemory(mPlanes, planes, nb_planes*sizeof(Plane)); | ||
| 199 | |||
| 200 | // 3) Setup destination pointer | ||
| 201 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 202 | |||
| 203 | // 4) Special case: 1-triangle meshes [Opcode 1.3] | ||
| 204 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 205 | { | ||
| 206 | if(!SkipPrimitiveTests()) | ||
| 207 | { | ||
| 208 | // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. | ||
| 209 | mTouchedPrimitives->Reset(); | ||
| 210 | |||
| 211 | // Perform overlap test between the unique triangle and the planes (and set contact status if needed) | ||
| 212 | udword clip_mask = (1<<mNbPlanes)-1; | ||
| 213 | PLANES_PRIM(udword(0), OPC_CONTACT) | ||
| 214 | |||
| 215 | // Return immediately regardless of status | ||
| 216 | return TRUE; | ||
| 217 | } | ||
| 218 | } | ||
| 219 | |||
| 220 | // 4) Check temporal coherence: | ||
| 221 | if(TemporalCoherenceEnabled()) | ||
| 222 | { | ||
| 223 | // Here we use temporal coherence | ||
| 224 | // => check results from previous frame before performing the collision query | ||
| 225 | if(FirstContactEnabled()) | ||
| 226 | { | ||
| 227 | // We're only interested in the first contact found => test the unique previously touched face | ||
| 228 | if(mTouchedPrimitives->GetNbEntries()) | ||
| 229 | { | ||
| 230 | // Get index of previously touched face = the first entry in the array | ||
| 231 | udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0); | ||
| 232 | |||
| 233 | // Then reset the array: | ||
| 234 | // - if the overlap test below is successful, the index we'll get added back anyway | ||
| 235 | // - if it isn't, then the array should be reset anyway for the normal query | ||
| 236 | mTouchedPrimitives->Reset(); | ||
| 237 | |||
| 238 | // Perform overlap test between the cached triangle and the planes (and set contact status if needed) | ||
| 239 | udword clip_mask = (1<<mNbPlanes)-1; | ||
| 240 | PLANES_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT) | ||
| 241 | |||
| 242 | // Return immediately if possible | ||
| 243 | if(GetContactStatus()) return TRUE; | ||
| 244 | } | ||
| 245 | // else no face has been touched during previous query | ||
| 246 | // => we'll have to perform a normal query | ||
| 247 | } | ||
| 248 | else mTouchedPrimitives->Reset(); | ||
| 249 | } | ||
| 250 | else | ||
| 251 | { | ||
| 252 | // Here we don't use temporal coherence => do a normal query | ||
| 253 | mTouchedPrimitives->Reset(); | ||
| 254 | } | ||
| 255 | |||
| 256 | return FALSE; | ||
| 257 | } | ||
| 258 | |||
| 259 | #define TEST_CLIP_MASK \ | ||
| 260 | /* If the box is completely included, so are its children. We don't need to do extra tests, we */ \ | ||
| 261 | /* can immediately output a list of visible children. Those ones won't need to be clipped. */ \ | ||
| 262 | if(!OutClipMask) \ | ||
| 263 | { \ | ||
| 264 | /* Set contact status */ \ | ||
| 265 | mFlags |= OPC_CONTACT; \ | ||
| 266 | _Dump(node); \ | ||
| 267 | return; \ | ||
| 268 | } | ||
| 269 | |||
| 270 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 271 | /** | ||
| 272 | * Recursive collision query for normal AABB trees. | ||
| 273 | * \param node [in] current collision node | ||
| 274 | */ | ||
| 275 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 276 | void PlanesCollider::_Collide(const AABBCollisionNode* node, udword clip_mask) | ||
| 277 | { | ||
| 278 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 279 | udword OutClipMask; | ||
| 280 | if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return; | ||
| 281 | |||
| 282 | TEST_CLIP_MASK | ||
| 283 | |||
| 284 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 285 | if(node->IsLeaf()) | ||
| 286 | { | ||
| 287 | PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 288 | } | ||
| 289 | else | ||
| 290 | { | ||
| 291 | _Collide(node->GetPos(), OutClipMask); | ||
| 292 | |||
| 293 | if(ContactFound()) return; | ||
| 294 | |||
| 295 | _Collide(node->GetNeg(), OutClipMask); | ||
| 296 | } | ||
| 297 | } | ||
| 298 | |||
| 299 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 300 | /** | ||
| 301 | * Recursive collision query for normal AABB trees. | ||
| 302 | * \param node [in] current collision node | ||
| 303 | */ | ||
| 304 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 305 | void PlanesCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask) | ||
| 306 | { | ||
| 307 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 308 | udword OutClipMask; | ||
| 309 | if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return; | ||
| 310 | |||
| 311 | TEST_CLIP_MASK | ||
| 312 | |||
| 313 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 314 | if(node->IsLeaf()) | ||
| 315 | { | ||
| 316 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 317 | } | ||
| 318 | else | ||
| 319 | { | ||
| 320 | _CollideNoPrimitiveTest(node->GetPos(), OutClipMask); | ||
| 321 | |||
| 322 | if(ContactFound()) return; | ||
| 323 | |||
| 324 | _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask); | ||
| 325 | } | ||
| 326 | } | ||
| 327 | |||
| 328 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 329 | /** | ||
| 330 | * Recursive collision query for quantized AABB trees. | ||
| 331 | * \param node [in] current collision node | ||
| 332 | */ | ||
| 333 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 334 | void PlanesCollider::_Collide(const AABBQuantizedNode* node, udword clip_mask) | ||
| 335 | { | ||
| 336 | // Dequantize box | ||
| 337 | const QuantizedAABB& Box = node->mAABB; | ||
| 338 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 339 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 340 | |||
| 341 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 342 | udword OutClipMask; | ||
| 343 | if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return; | ||
| 344 | |||
| 345 | TEST_CLIP_MASK | ||
| 346 | |||
| 347 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 348 | if(node->IsLeaf()) | ||
| 349 | { | ||
| 350 | PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 351 | } | ||
| 352 | else | ||
| 353 | { | ||
| 354 | _Collide(node->GetPos(), OutClipMask); | ||
| 355 | |||
| 356 | if(ContactFound()) return; | ||
| 357 | |||
| 358 | _Collide(node->GetNeg(), OutClipMask); | ||
| 359 | } | ||
| 360 | } | ||
| 361 | |||
| 362 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 363 | /** | ||
| 364 | * Recursive collision query for quantized AABB trees. | ||
| 365 | * \param node [in] current collision node | ||
| 366 | */ | ||
| 367 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 368 | void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask) | ||
| 369 | { | ||
| 370 | // Dequantize box | ||
| 371 | const QuantizedAABB& Box = node->mAABB; | ||
| 372 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 373 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 374 | |||
| 375 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 376 | udword OutClipMask; | ||
| 377 | if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return; | ||
| 378 | |||
| 379 | TEST_CLIP_MASK | ||
| 380 | |||
| 381 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 382 | if(node->IsLeaf()) | ||
| 383 | { | ||
| 384 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 385 | } | ||
| 386 | else | ||
| 387 | { | ||
| 388 | _CollideNoPrimitiveTest(node->GetPos(), OutClipMask); | ||
| 389 | |||
| 390 | if(ContactFound()) return; | ||
| 391 | |||
| 392 | _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask); | ||
| 393 | } | ||
| 394 | } | ||
| 395 | |||
| 396 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 397 | /** | ||
| 398 | * Recursive collision query for no-leaf AABB trees. | ||
| 399 | * \param node [in] current collision node | ||
| 400 | */ | ||
| 401 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 402 | void PlanesCollider::_Collide(const AABBNoLeafNode* node, udword clip_mask) | ||
| 403 | { | ||
| 404 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 405 | udword OutClipMask; | ||
| 406 | if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return; | ||
| 407 | |||
| 408 | TEST_CLIP_MASK | ||
| 409 | |||
| 410 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 411 | if(node->HasPosLeaf()) { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 412 | else _Collide(node->GetPos(), OutClipMask); | ||
| 413 | |||
| 414 | if(ContactFound()) return; | ||
| 415 | |||
| 416 | if(node->HasNegLeaf()) { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 417 | else _Collide(node->GetNeg(), OutClipMask); | ||
| 418 | } | ||
| 419 | |||
| 420 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 421 | /** | ||
| 422 | * Recursive collision query for no-leaf AABB trees. | ||
| 423 | * \param node [in] current collision node | ||
| 424 | */ | ||
| 425 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 426 | void PlanesCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask) | ||
| 427 | { | ||
| 428 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 429 | udword OutClipMask; | ||
| 430 | if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return; | ||
| 431 | |||
| 432 | TEST_CLIP_MASK | ||
| 433 | |||
| 434 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 435 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 436 | else _CollideNoPrimitiveTest(node->GetPos(), OutClipMask); | ||
| 437 | |||
| 438 | if(ContactFound()) return; | ||
| 439 | |||
| 440 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 441 | else _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask); | ||
| 442 | } | ||
| 443 | |||
| 444 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 445 | /** | ||
| 446 | * Recursive collision query for quantized no-leaf AABB trees. | ||
| 447 | * \param node [in] current collision node | ||
| 448 | */ | ||
| 449 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 450 | void PlanesCollider::_Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask) | ||
| 451 | { | ||
| 452 | // Dequantize box | ||
| 453 | const QuantizedAABB& Box = node->mAABB; | ||
| 454 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 455 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 456 | |||
| 457 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 458 | udword OutClipMask; | ||
| 459 | if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return; | ||
| 460 | |||
| 461 | TEST_CLIP_MASK | ||
| 462 | |||
| 463 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 464 | if(node->HasPosLeaf()) { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 465 | else _Collide(node->GetPos(), OutClipMask); | ||
| 466 | |||
| 467 | if(ContactFound()) return; | ||
| 468 | |||
| 469 | if(node->HasNegLeaf()) { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 470 | else _Collide(node->GetNeg(), OutClipMask); | ||
| 471 | } | ||
| 472 | |||
| 473 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 474 | /** | ||
| 475 | * Recursive collision query for quantized no-leaf AABB trees. | ||
| 476 | * \param node [in] current collision node | ||
| 477 | */ | ||
| 478 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 479 | void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask) | ||
| 480 | { | ||
| 481 | // Dequantize box | ||
| 482 | const QuantizedAABB& Box = node->mAABB; | ||
| 483 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 484 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 485 | |||
| 486 | // Test the box against the planes. If the box is completely culled, so are its children, hence we exit. | ||
| 487 | udword OutClipMask; | ||
| 488 | if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return; | ||
| 489 | |||
| 490 | TEST_CLIP_MASK | ||
| 491 | |||
| 492 | // Else the box straddles one or several planes, so we need to recurse down the tree. | ||
| 493 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 494 | else _CollideNoPrimitiveTest(node->GetPos(), OutClipMask); | ||
| 495 | |||
| 496 | if(ContactFound()) return; | ||
| 497 | |||
| 498 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 499 | else _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask); | ||
| 500 | } | ||
| 501 | |||
| 502 | |||
| 503 | |||
| 504 | |||
| 505 | |||
| 506 | |||
| 507 | |||
| 508 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 509 | /** | ||
| 510 | * Constructor. | ||
| 511 | */ | ||
| 512 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 513 | HybridPlanesCollider::HybridPlanesCollider() | ||
| 514 | { | ||
| 515 | } | ||
| 516 | |||
| 517 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 518 | /** | ||
| 519 | * Destructor. | ||
| 520 | */ | ||
| 521 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 522 | HybridPlanesCollider::~HybridPlanesCollider() | ||
| 523 | { | ||
| 524 | } | ||
| 525 | |||
| 526 | bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm) | ||
| 527 | { | ||
| 528 | // We don't want primitive tests here! | ||
| 529 | mFlags |= OPC_NO_PRIMITIVE_TESTS; | ||
| 530 | |||
| 531 | // Checkings | ||
| 532 | if(!Setup(&model)) return false; | ||
| 533 | |||
| 534 | // Init collision query | ||
| 535 | if(InitQuery(cache, planes, nb_planes, worldm)) return true; | ||
| 536 | |||
| 537 | // Special case for 1-leaf trees | ||
| 538 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 539 | { | ||
| 540 | // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles | ||
| 541 | udword Nb = mIMesh->GetNbTriangles(); | ||
| 542 | |||
| 543 | // Loop through all triangles | ||
| 544 | udword clip_mask = (1<<mNbPlanes)-1; | ||
| 545 | for(udword i=0;i<Nb;i++) | ||
| 546 | { | ||
| 547 | PLANES_PRIM(i, OPC_CONTACT) | ||
| 548 | } | ||
| 549 | return true; | ||
| 550 | } | ||
| 551 | |||
| 552 | // Override destination array since we're only going to get leaf boxes here | ||
| 553 | mTouchedBoxes.Reset(); | ||
| 554 | mTouchedPrimitives = &mTouchedBoxes; | ||
| 555 | |||
| 556 | udword PlaneMask = (1<<nb_planes)-1; | ||
| 557 | |||
| 558 | // Now, do the actual query against leaf boxes | ||
| 559 | if(!model.HasLeafNodes()) | ||
| 560 | { | ||
| 561 | if(model.IsQuantized()) | ||
| 562 | { | ||
| 563 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 564 | |||
| 565 | // Setup dequantization coeffs | ||
| 566 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 567 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 568 | |||
| 569 | // Perform collision query - we don't want primitive tests here! | ||
| 570 | _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 571 | } | ||
| 572 | else | ||
| 573 | { | ||
| 574 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 575 | |||
| 576 | // Perform collision query - we don't want primitive tests here! | ||
| 577 | _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 578 | } | ||
| 579 | } | ||
| 580 | else | ||
| 581 | { | ||
| 582 | if(model.IsQuantized()) | ||
| 583 | { | ||
| 584 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 585 | |||
| 586 | // Setup dequantization coeffs | ||
| 587 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 588 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 589 | |||
| 590 | // Perform collision query - we don't want primitive tests here! | ||
| 591 | _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 592 | } | ||
| 593 | else | ||
| 594 | { | ||
| 595 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 596 | |||
| 597 | // Perform collision query - we don't want primitive tests here! | ||
| 598 | _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); | ||
| 599 | } | ||
| 600 | } | ||
| 601 | |||
| 602 | // We only have a list of boxes so far | ||
| 603 | if(GetContactStatus()) | ||
| 604 | { | ||
| 605 | // Reset contact status, since it currently only reflects collisions with leaf boxes | ||
| 606 | Collider::InitQuery(); | ||
| 607 | |||
| 608 | // Change dest container so that we can use built-in overlap tests and get collided primitives | ||
| 609 | cache.TouchedPrimitives.Reset(); | ||
| 610 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 611 | |||
| 612 | // Read touched leaf boxes | ||
| 613 | udword Nb = mTouchedBoxes.GetNbEntries(); | ||
| 614 | const udword* Touched = mTouchedBoxes.GetEntries(); | ||
| 615 | |||
| 616 | const LeafTriangles* LT = model.GetLeafTriangles(); | ||
| 617 | const udword* Indices = model.GetIndices(); | ||
| 618 | |||
| 619 | // Loop through touched leaves | ||
| 620 | udword clip_mask = (1<<mNbPlanes)-1; | ||
| 621 | while(Nb--) | ||
| 622 | { | ||
| 623 | const LeafTriangles& CurrentLeaf = LT[*Touched++]; | ||
| 624 | |||
| 625 | // Each leaf box has a set of triangles | ||
| 626 | udword NbTris = CurrentLeaf.GetNbTriangles(); | ||
| 627 | if(Indices) | ||
| 628 | { | ||
| 629 | const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; | ||
| 630 | |||
| 631 | // Loop through triangles and test each of them | ||
| 632 | while(NbTris--) | ||
| 633 | { | ||
| 634 | udword TriangleIndex = *T++; | ||
| 635 | PLANES_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 636 | } | ||
| 637 | } | ||
| 638 | else | ||
| 639 | { | ||
| 640 | udword BaseIndex = CurrentLeaf.GetTriangleIndex(); | ||
| 641 | |||
| 642 | // Loop through triangles and test each of them | ||
| 643 | while(NbTris--) | ||
| 644 | { | ||
| 645 | udword TriangleIndex = BaseIndex++; | ||
| 646 | PLANES_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 647 | } | ||
| 648 | } | ||
| 649 | } | ||
| 650 | } | ||
| 651 | |||
| 652 | return true; | ||
| 653 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_PlanesCollider.h b/libraries/ode-0.9/OPCODE/OPC_PlanesCollider.h new file mode 100644 index 0000000..35de28a --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_PlanesCollider.h | |||
| @@ -0,0 +1,121 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a planes collider. | ||
| 12 | * \file OPC_PlanesCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 1st, 2002 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_PLANESCOLLIDER_H__ | ||
| 21 | #define __OPC_PLANESCOLLIDER_H__ | ||
| 22 | |||
| 23 | struct OPCODE_API PlanesCache : VolumeCache | ||
| 24 | { | ||
| 25 | PlanesCache() | ||
| 26 | { | ||
| 27 | } | ||
| 28 | }; | ||
| 29 | |||
| 30 | class OPCODE_API PlanesCollider : public VolumeCollider | ||
| 31 | { | ||
| 32 | public: | ||
| 33 | // Constructor / Destructor | ||
| 34 | PlanesCollider(); | ||
| 35 | virtual ~PlanesCollider(); | ||
| 36 | |||
| 37 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 38 | /** | ||
| 39 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 40 | * - with GetContactStatus() | ||
| 41 | * - with GetNbTouchedPrimitives() | ||
| 42 | * - with GetTouchedPrimitives() | ||
| 43 | * | ||
| 44 | * \param cache [in/out] a planes cache | ||
| 45 | * \param planes [in] list of planes in world space | ||
| 46 | * \param nb_planes [in] number of planes | ||
| 47 | * \param model [in] Opcode model to collide with | ||
| 48 | * \param worldm [in] model's world matrix, or null | ||
| 49 | * \return true if success | ||
| 50 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 51 | */ | ||
| 52 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 53 | bool Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm=null); | ||
| 54 | |||
| 55 | // Mutant box-with-planes collision queries | ||
| 56 | inline_ bool Collide(PlanesCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null) | ||
| 57 | { | ||
| 58 | Plane PL[6]; | ||
| 59 | |||
| 60 | if(worldb) | ||
| 61 | { | ||
| 62 | // Create a new OBB in world space | ||
| 63 | OBB WorldBox; | ||
| 64 | box.Rotate(*worldb, WorldBox); | ||
| 65 | // Compute planes from the sides of the box | ||
| 66 | WorldBox.ComputePlanes(PL); | ||
| 67 | } | ||
| 68 | else | ||
| 69 | { | ||
| 70 | // Compute planes from the sides of the box | ||
| 71 | box.ComputePlanes(PL); | ||
| 72 | } | ||
| 73 | |||
| 74 | // Collide with box planes | ||
| 75 | return Collide(cache, PL, 6, model, worldm); | ||
| 76 | } | ||
| 77 | // Settings | ||
| 78 | |||
| 79 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 80 | /** | ||
| 81 | * Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider. | ||
| 82 | * \return null if everything is ok, else a string describing the problem | ||
| 83 | */ | ||
| 84 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 85 | override(Collider) const char* ValidateSettings(); | ||
| 86 | |||
| 87 | protected: | ||
| 88 | // Planes in model space | ||
| 89 | udword mNbPlanes; | ||
| 90 | Plane* mPlanes; | ||
| 91 | // Leaf description | ||
| 92 | VertexPointers mVP; | ||
| 93 | // Internal methods | ||
| 94 | void _Collide(const AABBCollisionNode* node, udword clip_mask); | ||
| 95 | void _Collide(const AABBNoLeafNode* node, udword clip_mask); | ||
| 96 | void _Collide(const AABBQuantizedNode* node, udword clip_mask); | ||
| 97 | void _Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask); | ||
| 98 | void _CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask); | ||
| 99 | void _CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask); | ||
| 100 | void _CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask); | ||
| 101 | void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask); | ||
| 102 | // Overlap tests | ||
| 103 | inline_ BOOL PlanesAABBOverlap(const Point& center, const Point& extents, udword& out_clip_mask, udword in_clip_mask); | ||
| 104 | inline_ BOOL PlanesTriOverlap(udword in_clip_mask); | ||
| 105 | // Init methods | ||
| 106 | BOOL InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm=null); | ||
| 107 | }; | ||
| 108 | |||
| 109 | class OPCODE_API HybridPlanesCollider : public PlanesCollider | ||
| 110 | { | ||
| 111 | public: | ||
| 112 | // Constructor / Destructor | ||
| 113 | HybridPlanesCollider(); | ||
| 114 | virtual ~HybridPlanesCollider(); | ||
| 115 | |||
| 116 | bool Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm=null); | ||
| 117 | protected: | ||
| 118 | Container mTouchedBoxes; | ||
| 119 | }; | ||
| 120 | |||
| 121 | #endif // __OPC_PLANESCOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_PlanesTriOverlap.h b/libraries/ode-0.9/OPCODE/OPC_PlanesTriOverlap.h new file mode 100644 index 0000000..3f9b39f --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_PlanesTriOverlap.h | |||
| @@ -0,0 +1,40 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Planes-triangle overlap test. | ||
| 4 | * \param in_clip_mask [in] bitmask for active planes | ||
| 5 | * \return TRUE if triangle overlap planes | ||
| 6 | * \warning THIS IS A CONSERVATIVE TEST !! Some triangles will be returned as intersecting, while they're not! | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | inline_ BOOL PlanesCollider::PlanesTriOverlap(udword in_clip_mask) | ||
| 10 | { | ||
| 11 | // Stats | ||
| 12 | mNbVolumePrimTests++; | ||
| 13 | |||
| 14 | const Plane* p = mPlanes; | ||
| 15 | udword Mask = 1; | ||
| 16 | |||
| 17 | while(Mask<=in_clip_mask) | ||
| 18 | { | ||
| 19 | if(in_clip_mask & Mask) | ||
| 20 | { | ||
| 21 | float d0 = p->Distance(*mVP.Vertex[0]); | ||
| 22 | float d1 = p->Distance(*mVP.Vertex[1]); | ||
| 23 | float d2 = p->Distance(*mVP.Vertex[2]); | ||
| 24 | if(d0>0.0f && d1>0.0f && d2>0.0f) return FALSE; | ||
| 25 | // if(!(IR(d0)&SIGN_BITMASK) && !(IR(d1)&SIGN_BITMASK) && !(IR(d2)&SIGN_BITMASK)) return FALSE; | ||
| 26 | } | ||
| 27 | Mask+=Mask; | ||
| 28 | p++; | ||
| 29 | } | ||
| 30 | /* | ||
| 31 | for(udword i=0;i<6;i++) | ||
| 32 | { | ||
| 33 | float d0 = p[i].Distance(mLeafVerts[0]); | ||
| 34 | float d1 = p[i].Distance(mLeafVerts[1]); | ||
| 35 | float d2 = p[i].Distance(mLeafVerts[2]); | ||
| 36 | if(d0>0.0f && d1>0.0f && d2>0.0f) return false; | ||
| 37 | } | ||
| 38 | */ | ||
| 39 | return TRUE; | ||
| 40 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_RayAABBOverlap.h b/libraries/ode-0.9/OPCODE/OPC_RayAABBOverlap.h new file mode 100644 index 0000000..a8162bf --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_RayAABBOverlap.h | |||
| @@ -0,0 +1,63 @@ | |||
| 1 | // Opcode 1.1: ray-AABB overlap tests based on Woo's code | ||
| 2 | // Opcode 1.2: ray-AABB overlap tests based on the separating axis theorem | ||
| 3 | // | ||
| 4 | // The point of intersection is not computed anymore. The distance to impact is not needed anymore | ||
| 5 | // since we now have two different queries for segments or rays. | ||
| 6 | |||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | /** | ||
| 9 | * Computes a segment-AABB overlap test using the separating axis theorem. Segment is cached within the class. | ||
| 10 | * \param center [in] AABB center | ||
| 11 | * \param extents [in] AABB extents | ||
| 12 | * \return true on overlap | ||
| 13 | */ | ||
| 14 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 15 | inline_ BOOL RayCollider::SegmentAABBOverlap(const Point& center, const Point& extents) | ||
| 16 | { | ||
| 17 | // Stats | ||
| 18 | mNbRayBVTests++; | ||
| 19 | |||
| 20 | float Dx = mData2.x - center.x; if(fabsf(Dx) > extents.x + mFDir.x) return FALSE; | ||
| 21 | float Dy = mData2.y - center.y; if(fabsf(Dy) > extents.y + mFDir.y) return FALSE; | ||
| 22 | float Dz = mData2.z - center.z; if(fabsf(Dz) > extents.z + mFDir.z) return FALSE; | ||
| 23 | |||
| 24 | float f; | ||
| 25 | f = mData.y * Dz - mData.z * Dy; if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) return FALSE; | ||
| 26 | f = mData.z * Dx - mData.x * Dz; if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) return FALSE; | ||
| 27 | f = mData.x * Dy - mData.y * Dx; if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) return FALSE; | ||
| 28 | |||
| 29 | return TRUE; | ||
| 30 | } | ||
| 31 | |||
| 32 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 33 | /** | ||
| 34 | * Computes a ray-AABB overlap test using the separating axis theorem. Ray is cached within the class. | ||
| 35 | * \param center [in] AABB center | ||
| 36 | * \param extents [in] AABB extents | ||
| 37 | * \return true on overlap | ||
| 38 | */ | ||
| 39 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 40 | inline_ BOOL RayCollider::RayAABBOverlap(const Point& center, const Point& extents) | ||
| 41 | { | ||
| 42 | // Stats | ||
| 43 | mNbRayBVTests++; | ||
| 44 | |||
| 45 | // float Dx = mOrigin.x - center.x; if(fabsf(Dx) > extents.x && Dx*mDir.x>=0.0f) return FALSE; | ||
| 46 | // float Dy = mOrigin.y - center.y; if(fabsf(Dy) > extents.y && Dy*mDir.y>=0.0f) return FALSE; | ||
| 47 | // float Dz = mOrigin.z - center.z; if(fabsf(Dz) > extents.z && Dz*mDir.z>=0.0f) return FALSE; | ||
| 48 | |||
| 49 | float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && Dx*mDir.x>=0.0f) return FALSE; | ||
| 50 | float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && Dy*mDir.y>=0.0f) return FALSE; | ||
| 51 | float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && Dz*mDir.z>=0.0f) return FALSE; | ||
| 52 | |||
| 53 | // float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && ((SIR(Dx)-1)^SIR(mDir.x))>=0.0f) return FALSE; | ||
| 54 | // float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && ((SIR(Dy)-1)^SIR(mDir.y))>=0.0f) return FALSE; | ||
| 55 | // float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && ((SIR(Dz)-1)^SIR(mDir.z))>=0.0f) return FALSE; | ||
| 56 | |||
| 57 | float f; | ||
| 58 | f = mDir.y * Dz - mDir.z * Dy; if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) return FALSE; | ||
| 59 | f = mDir.z * Dx - mDir.x * Dz; if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) return FALSE; | ||
| 60 | f = mDir.x * Dy - mDir.y * Dx; if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) return FALSE; | ||
| 61 | |||
| 62 | return TRUE; | ||
| 63 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_RayCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_RayCollider.cpp new file mode 100644 index 0000000..5828ce4 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_RayCollider.cpp | |||
| @@ -0,0 +1,762 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a ray collider. | ||
| 12 | * \file OPC_RayCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains a ray-vs-tree collider. | ||
| 21 | * This class performs a stabbing query on an AABB tree, i.e. does a ray-mesh collision. | ||
| 22 | * | ||
| 23 | * HIGHER DISTANCE BOUND: | ||
| 24 | * | ||
| 25 | * If P0 and P1 are two 3D points, let's define: | ||
| 26 | * - d = distance between P0 and P1 | ||
| 27 | * - Origin = P0 | ||
| 28 | * - Direction = (P1 - P0) / d = normalized direction vector | ||
| 29 | * - A parameter t such as a point P on the line (P0,P1) is P = Origin + t * Direction | ||
| 30 | * - t = 0 --> P = P0 | ||
| 31 | * - t = d --> P = P1 | ||
| 32 | * | ||
| 33 | * Then we can define a general "ray" as: | ||
| 34 | * | ||
| 35 | * struct Ray | ||
| 36 | * { | ||
| 37 | * Point Origin; | ||
| 38 | * Point Direction; | ||
| 39 | * }; | ||
| 40 | * | ||
| 41 | * But it actually maps three different things: | ||
| 42 | * - a segment, when 0 <= t <= d | ||
| 43 | * - a half-line, when 0 <= t < +infinity, or -infinity < t <= d | ||
| 44 | * - a line, when -infinity < t < +infinity | ||
| 45 | * | ||
| 46 | * In Opcode, we support segment queries, which yield half-line queries by setting d = +infinity. | ||
| 47 | * We don't support line-queries. If you need them, shift the origin along the ray by an appropriate margin. | ||
| 48 | * | ||
| 49 | * In short, the lower bound is always 0, and you can setup the higher bound "d" with RayCollider::SetMaxDist(). | ||
| 50 | * | ||
| 51 | * Query |segment |half-line |line | ||
| 52 | * --------|-------------------|---------------|---------------- | ||
| 53 | * Usages |-shadow feelers |-raytracing |- | ||
| 54 | * |-sweep tests |-in/out tests | | ||
| 55 | * | ||
| 56 | * FIRST CONTACT: | ||
| 57 | * | ||
| 58 | * - You can setup "first contact" mode or "all contacts" mode with RayCollider::SetFirstContact(). | ||
| 59 | * - In "first contact" mode we return as soon as the ray hits one face. If can be useful e.g. for shadow feelers, where | ||
| 60 | * you want to know whether the path to the light is free or not (a boolean answer is enough). | ||
| 61 | * - In "all contacts" mode we return all faces hit by the ray. | ||
| 62 | * | ||
| 63 | * TEMPORAL COHERENCE: | ||
| 64 | * | ||
| 65 | * - You can enable or disable temporal coherence with RayCollider::SetTemporalCoherence(). | ||
| 66 | * - It currently only works in "first contact" mode. | ||
| 67 | * - If temporal coherence is enabled, the previously hit triangle is cached during the first query. Then, next queries | ||
| 68 | * start by colliding the ray against the cached triangle. If they still collide, we return immediately. | ||
| 69 | * | ||
| 70 | * CLOSEST HIT: | ||
| 71 | * | ||
| 72 | * - You can enable or disable "closest hit" with RayCollider::SetClosestHit(). | ||
| 73 | * - It currently only works in "all contacts" mode. | ||
| 74 | * - If closest hit is enabled, faces are sorted by distance on-the-fly and the closest one only is reported. | ||
| 75 | * | ||
| 76 | * BACKFACE CULLING: | ||
| 77 | * | ||
| 78 | * - You can enable or disable backface culling with RayCollider::SetCulling(). | ||
| 79 | * - If culling is enabled, ray will not hit back faces (only front faces). | ||
| 80 | * | ||
| 81 | * | ||
| 82 | * | ||
| 83 | * \class RayCollider | ||
| 84 | * \author Pierre Terdiman | ||
| 85 | * \version 1.3 | ||
| 86 | * \date June, 2, 2001 | ||
| 87 | */ | ||
| 88 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 89 | |||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | /** | ||
| 92 | * This class describes a face hit by a ray or segment. | ||
| 93 | * This is a particular class dedicated to stabbing queries. | ||
| 94 | * | ||
| 95 | * \class CollisionFace | ||
| 96 | * \author Pierre Terdiman | ||
| 97 | * \version 1.3 | ||
| 98 | * \date March, 20, 2001 | ||
| 99 | */ | ||
| 100 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 101 | |||
| 102 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 103 | /** | ||
| 104 | * This class is a dedicated collection of CollisionFace. | ||
| 105 | * | ||
| 106 | * \class CollisionFaces | ||
| 107 | * \author Pierre Terdiman | ||
| 108 | * \version 1.3 | ||
| 109 | * \date March, 20, 2001 | ||
| 110 | */ | ||
| 111 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 112 | |||
| 113 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 114 | // Precompiled Header | ||
| 115 | #include "Stdafx.h" | ||
| 116 | |||
| 117 | using namespace Opcode; | ||
| 118 | |||
| 119 | #include "OPC_RayAABBOverlap.h" | ||
| 120 | #include "OPC_RayTriOverlap.h" | ||
| 121 | |||
| 122 | #define SET_CONTACT(prim_index, flag) \ | ||
| 123 | mNbIntersections++; \ | ||
| 124 | /* Set contact status */ \ | ||
| 125 | mFlags |= flag; \ | ||
| 126 | /* In any case the contact has been found and recorded in mStabbedFace */ \ | ||
| 127 | mStabbedFace.mFaceID = prim_index; | ||
| 128 | |||
| 129 | #ifdef OPC_RAYHIT_CALLBACK | ||
| 130 | |||
| 131 | #define HANDLE_CONTACT(prim_index, flag) \ | ||
| 132 | SET_CONTACT(prim_index, flag) \ | ||
| 133 | \ | ||
| 134 | if(mHitCallback) (mHitCallback)(mStabbedFace, mUserData); | ||
| 135 | |||
| 136 | #define UPDATE_CACHE \ | ||
| 137 | if(cache && GetContactStatus()) \ | ||
| 138 | { \ | ||
| 139 | *cache = mStabbedFace.mFaceID; \ | ||
| 140 | } | ||
| 141 | #else | ||
| 142 | |||
| 143 | #define HANDLE_CONTACT(prim_index, flag) \ | ||
| 144 | SET_CONTACT(prim_index, flag) \ | ||
| 145 | \ | ||
| 146 | /* Now we can also record it in mStabbedFaces if available */ \ | ||
| 147 | if(mStabbedFaces) \ | ||
| 148 | { \ | ||
| 149 | /* If we want all faces or if that's the first one we hit */ \ | ||
| 150 | if(!mClosestHit || !mStabbedFaces->GetNbFaces()) \ | ||
| 151 | { \ | ||
| 152 | mStabbedFaces->AddFace(mStabbedFace); \ | ||
| 153 | } \ | ||
| 154 | else \ | ||
| 155 | { \ | ||
| 156 | /* We only keep closest hit */ \ | ||
| 157 | CollisionFace* Current = const_cast<CollisionFace*>(mStabbedFaces->GetFaces()); \ | ||
| 158 | if(Current && mStabbedFace.mDistance<Current->mDistance) \ | ||
| 159 | { \ | ||
| 160 | *Current = mStabbedFace; \ | ||
| 161 | } \ | ||
| 162 | } \ | ||
| 163 | } | ||
| 164 | |||
| 165 | #define UPDATE_CACHE \ | ||
| 166 | if(cache && GetContactStatus() && mStabbedFaces) \ | ||
| 167 | { \ | ||
| 168 | const CollisionFace* Current = mStabbedFaces->GetFaces(); \ | ||
| 169 | if(Current) *cache = Current->mFaceID; \ | ||
| 170 | else *cache = INVALID_ID; \ | ||
| 171 | } | ||
| 172 | #endif | ||
| 173 | |||
| 174 | #define SEGMENT_PRIM(prim_index, flag) \ | ||
| 175 | /* Request vertices from the app */ \ | ||
| 176 | VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ | ||
| 177 | \ | ||
| 178 | /* Perform ray-tri overlap test and return */ \ | ||
| 179 | if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ | ||
| 180 | { \ | ||
| 181 | /* Intersection point is valid if dist < segment's length */ \ | ||
| 182 | /* We know dist>0 so we can use integers */ \ | ||
| 183 | if(IR(mStabbedFace.mDistance)<IR(mMaxDist)) \ | ||
| 184 | { \ | ||
| 185 | HANDLE_CONTACT(prim_index, flag) \ | ||
| 186 | } \ | ||
| 187 | } | ||
| 188 | |||
| 189 | #define RAY_PRIM(prim_index, flag) \ | ||
| 190 | /* Request vertices from the app */ \ | ||
| 191 | VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ | ||
| 192 | \ | ||
| 193 | /* Perform ray-tri overlap test and return */ \ | ||
| 194 | if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ | ||
| 195 | { \ | ||
| 196 | HANDLE_CONTACT(prim_index, flag) \ | ||
| 197 | } | ||
| 198 | |||
| 199 | |||
| 200 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 201 | /** | ||
| 202 | * Constructor. | ||
| 203 | */ | ||
| 204 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 205 | RayCollider::RayCollider() : | ||
| 206 | mNbRayBVTests (0), | ||
| 207 | mNbRayPrimTests (0), | ||
| 208 | mNbIntersections (0), | ||
| 209 | mCulling (true), | ||
| 210 | #ifdef OPC_RAYHIT_CALLBACK | ||
| 211 | mHitCallback (null), | ||
| 212 | mUserData (0), | ||
| 213 | #else | ||
| 214 | mClosestHit (false), | ||
| 215 | mStabbedFaces (null), | ||
| 216 | #endif | ||
| 217 | mMaxDist (MAX_FLOAT) | ||
| 218 | { | ||
| 219 | } | ||
| 220 | |||
| 221 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 222 | /** | ||
| 223 | * Destructor. | ||
| 224 | */ | ||
| 225 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 226 | RayCollider::~RayCollider() | ||
| 227 | { | ||
| 228 | } | ||
| 229 | |||
| 230 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 231 | /** | ||
| 232 | * Validates current settings. You should call this method after all the settings and callbacks have been defined. | ||
| 233 | * \return null if everything is ok, else a string describing the problem | ||
| 234 | */ | ||
| 235 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 236 | const char* RayCollider::ValidateSettings() | ||
| 237 | { | ||
| 238 | if(mMaxDist<0.0f) return "Higher distance bound must be positive!"; | ||
| 239 | if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; | ||
| 240 | #ifndef OPC_RAYHIT_CALLBACK | ||
| 241 | if(mClosestHit && FirstContactEnabled()) return "Closest hit doesn't work with ""First contact"" mode!"; | ||
| 242 | if(TemporalCoherenceEnabled() && mClosestHit) return "Temporal coherence can't guarantee to report closest hit!"; | ||
| 243 | #endif | ||
| 244 | if(SkipPrimitiveTests()) return "SkipPrimitiveTests not possible for RayCollider ! (not implemented)"; | ||
| 245 | return null; | ||
| 246 | } | ||
| 247 | |||
| 248 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 249 | /** | ||
| 250 | * Generic stabbing query for generic OPCODE models. After the call, access the results: | ||
| 251 | * - with GetContactStatus() | ||
| 252 | * - in the user-provided destination array | ||
| 253 | * | ||
| 254 | * \param world_ray [in] stabbing ray in world space | ||
| 255 | * \param model [in] Opcode model to collide with | ||
| 256 | * \param world [in] model's world matrix, or null | ||
| 257 | * \param cache [in] a possibly cached face index, or null | ||
| 258 | * \return true if success | ||
| 259 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 260 | */ | ||
| 261 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 262 | bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world, udword* cache) | ||
| 263 | { | ||
| 264 | // Checkings | ||
| 265 | if(!Setup(&model)) return false; | ||
| 266 | |||
| 267 | // Init collision query | ||
| 268 | if(InitQuery(world_ray, world, cache)) return true; | ||
| 269 | |||
| 270 | if(!model.HasLeafNodes()) | ||
| 271 | { | ||
| 272 | if(model.IsQuantized()) | ||
| 273 | { | ||
| 274 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 275 | |||
| 276 | // Setup dequantization coeffs | ||
| 277 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 278 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 279 | |||
| 280 | // Perform stabbing query | ||
| 281 | if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); | ||
| 282 | else _RayStab(Tree->GetNodes()); | ||
| 283 | } | ||
| 284 | else | ||
| 285 | { | ||
| 286 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 287 | |||
| 288 | // Perform stabbing query | ||
| 289 | if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); | ||
| 290 | else _RayStab(Tree->GetNodes()); | ||
| 291 | } | ||
| 292 | } | ||
| 293 | else | ||
| 294 | { | ||
| 295 | if(model.IsQuantized()) | ||
| 296 | { | ||
| 297 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 298 | |||
| 299 | // Setup dequantization coeffs | ||
| 300 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 301 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 302 | |||
| 303 | // Perform stabbing query | ||
| 304 | if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); | ||
| 305 | else _RayStab(Tree->GetNodes()); | ||
| 306 | } | ||
| 307 | else | ||
| 308 | { | ||
| 309 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 310 | |||
| 311 | // Perform stabbing query | ||
| 312 | if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); | ||
| 313 | else _RayStab(Tree->GetNodes()); | ||
| 314 | } | ||
| 315 | } | ||
| 316 | |||
| 317 | // Update cache if needed | ||
| 318 | UPDATE_CACHE | ||
| 319 | return true; | ||
| 320 | } | ||
| 321 | |||
| 322 | |||
| 323 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 324 | /** | ||
| 325 | * Initializes a stabbing query : | ||
| 326 | * - reset stats & contact status | ||
| 327 | * - compute ray in local space | ||
| 328 | * - check temporal coherence | ||
| 329 | * | ||
| 330 | * \param world_ray [in] stabbing ray in world space | ||
| 331 | * \param world [in] object's world matrix, or null | ||
| 332 | * \param face_id [in] index of previously stabbed triangle | ||
| 333 | * \return TRUE if we can return immediately | ||
| 334 | * \warning SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only. | ||
| 335 | */ | ||
| 336 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 337 | BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword* face_id) | ||
| 338 | { | ||
| 339 | // Reset stats & contact status | ||
| 340 | Collider::InitQuery(); | ||
| 341 | mNbRayBVTests = 0; | ||
| 342 | mNbRayPrimTests = 0; | ||
| 343 | mNbIntersections = 0; | ||
| 344 | #ifndef OPC_RAYHIT_CALLBACK | ||
| 345 | if(mStabbedFaces) mStabbedFaces->Reset(); | ||
| 346 | #endif | ||
| 347 | |||
| 348 | // Compute ray in local space | ||
| 349 | // The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests) | ||
| 350 | if(world) | ||
| 351 | { | ||
| 352 | Matrix3x3 InvWorld = *world; | ||
| 353 | mDir = InvWorld * world_ray.mDir; | ||
| 354 | |||
| 355 | Matrix4x4 World; | ||
| 356 | InvertPRMatrix(World, *world); | ||
| 357 | mOrigin = world_ray.mOrig * World; | ||
| 358 | } | ||
| 359 | else | ||
| 360 | { | ||
| 361 | mDir = world_ray.mDir; | ||
| 362 | mOrigin = world_ray.mOrig; | ||
| 363 | } | ||
| 364 | |||
| 365 | // 4) Special case: 1-triangle meshes [Opcode 1.3] | ||
| 366 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 367 | { | ||
| 368 | // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. | ||
| 369 | if(!SkipPrimitiveTests()) | ||
| 370 | { | ||
| 371 | // Perform overlap test between the unique triangle and the ray (and set contact status if needed) | ||
| 372 | SEGMENT_PRIM(udword(0), OPC_CONTACT) | ||
| 373 | |||
| 374 | // Return immediately regardless of status | ||
| 375 | return TRUE; | ||
| 376 | } | ||
| 377 | } | ||
| 378 | |||
| 379 | // Check temporal coherence : | ||
| 380 | |||
| 381 | // Test previously colliding primitives first | ||
| 382 | if(TemporalCoherenceEnabled() && FirstContactEnabled() && face_id && *face_id!=INVALID_ID) | ||
| 383 | { | ||
| 384 | #ifdef OLD_CODE | ||
| 385 | #ifndef OPC_RAYHIT_CALLBACK | ||
| 386 | if(!mClosestHit) | ||
| 387 | #endif | ||
| 388 | { | ||
| 389 | // Request vertices from the app | ||
| 390 | VertexPointers VP; | ||
| 391 | mIMesh->GetTriangle(VP, *face_id); | ||
| 392 | // Perform ray-cached tri overlap test | ||
| 393 | if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) | ||
| 394 | { | ||
| 395 | // Intersection point is valid if: | ||
| 396 | // - distance is positive (else it can just be a face behind the orig point) | ||
| 397 | // - distance is smaller than a given max distance (useful for shadow feelers) | ||
| 398 | // if(mStabbedFace.mDistance>0.0f && mStabbedFace.mDistance<mMaxDist) | ||
| 399 | if(IR(mStabbedFace.mDistance)<IR(mMaxDist)) // The other test is already performed in RayTriOverlap | ||
| 400 | { | ||
| 401 | // Set contact status | ||
| 402 | mFlags |= OPC_TEMPORAL_CONTACT; | ||
| 403 | |||
| 404 | mStabbedFace.mFaceID = *face_id; | ||
| 405 | |||
| 406 | #ifndef OPC_RAYHIT_CALLBACK | ||
| 407 | if(mStabbedFaces) mStabbedFaces->AddFace(mStabbedFace); | ||
| 408 | #endif | ||
| 409 | return TRUE; | ||
| 410 | } | ||
| 411 | } | ||
| 412 | } | ||
| 413 | #else | ||
| 414 | // New code | ||
| 415 | // We handle both Segment/ray queries with the same segment code, and a possible infinite limit | ||
| 416 | SEGMENT_PRIM(*face_id, OPC_TEMPORAL_CONTACT) | ||
| 417 | |||
| 418 | // Return immediately if possible | ||
| 419 | if(GetContactStatus()) return TRUE; | ||
| 420 | #endif | ||
| 421 | } | ||
| 422 | |||
| 423 | // Precompute data (moved after temporal coherence since only needed for ray-AABB) | ||
| 424 | if(IR(mMaxDist)!=IEEE_MAX_FLOAT) | ||
| 425 | { | ||
| 426 | // For Segment-AABB overlap | ||
| 427 | mData = 0.5f * mDir * mMaxDist; | ||
| 428 | mData2 = mOrigin + mData; | ||
| 429 | |||
| 430 | // Precompute mFDir; | ||
| 431 | mFDir.x = fabsf(mData.x); | ||
| 432 | mFDir.y = fabsf(mData.y); | ||
| 433 | mFDir.z = fabsf(mData.z); | ||
| 434 | } | ||
| 435 | else | ||
| 436 | { | ||
| 437 | // For Ray-AABB overlap | ||
| 438 | // udword x = SIR(mDir.x)-1; | ||
| 439 | // udword y = SIR(mDir.y)-1; | ||
| 440 | // udword z = SIR(mDir.z)-1; | ||
| 441 | // mData.x = FR(x); | ||
| 442 | // mData.y = FR(y); | ||
| 443 | // mData.z = FR(z); | ||
| 444 | |||
| 445 | // Precompute mFDir; | ||
| 446 | mFDir.x = fabsf(mDir.x); | ||
| 447 | mFDir.y = fabsf(mDir.y); | ||
| 448 | mFDir.z = fabsf(mDir.z); | ||
| 449 | } | ||
| 450 | |||
| 451 | return FALSE; | ||
| 452 | } | ||
| 453 | |||
| 454 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 455 | /** | ||
| 456 | * Stabbing query for vanilla AABB trees. | ||
| 457 | * \param world_ray [in] stabbing ray in world space | ||
| 458 | * \param tree [in] AABB tree | ||
| 459 | * \param box_indices [out] indices of stabbed boxes | ||
| 460 | * \return true if success | ||
| 461 | */ | ||
| 462 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 463 | bool RayCollider::Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices) | ||
| 464 | { | ||
| 465 | // ### bad design here | ||
| 466 | |||
| 467 | // This is typically called for a scene tree, full of -AABBs-, not full of triangles. | ||
| 468 | // So we don't really have "primitives" to deal with. Hence it doesn't work with | ||
| 469 | // "FirstContact" + "TemporalCoherence". | ||
| 470 | ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) ); | ||
| 471 | |||
| 472 | // Checkings | ||
| 473 | if(!tree) return false; | ||
| 474 | |||
| 475 | // Init collision query | ||
| 476 | // Basically this is only called to initialize precomputed data | ||
| 477 | if(InitQuery(world_ray)) return true; | ||
| 478 | |||
| 479 | // Perform stabbing query | ||
| 480 | if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(tree, box_indices); | ||
| 481 | else _RayStab(tree, box_indices); | ||
| 482 | |||
| 483 | return true; | ||
| 484 | } | ||
| 485 | |||
| 486 | |||
| 487 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 488 | /** | ||
| 489 | * Recursive stabbing query for normal AABB trees. | ||
| 490 | * \param node [in] current collision node | ||
| 491 | */ | ||
| 492 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 493 | void RayCollider::_SegmentStab(const AABBCollisionNode* node) | ||
| 494 | { | ||
| 495 | // Perform Segment-AABB overlap test | ||
| 496 | if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 497 | |||
| 498 | if(node->IsLeaf()) | ||
| 499 | { | ||
| 500 | SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 501 | } | ||
| 502 | else | ||
| 503 | { | ||
| 504 | _SegmentStab(node->GetPos()); | ||
| 505 | |||
| 506 | if(ContactFound()) return; | ||
| 507 | |||
| 508 | _SegmentStab(node->GetNeg()); | ||
| 509 | } | ||
| 510 | } | ||
| 511 | |||
| 512 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 513 | /** | ||
| 514 | * Recursive stabbing query for quantized AABB trees. | ||
| 515 | * \param node [in] current collision node | ||
| 516 | */ | ||
| 517 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 518 | void RayCollider::_SegmentStab(const AABBQuantizedNode* node) | ||
| 519 | { | ||
| 520 | // Dequantize box | ||
| 521 | const QuantizedAABB& Box = node->mAABB; | ||
| 522 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 523 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 524 | |||
| 525 | // Perform Segment-AABB overlap test | ||
| 526 | if(!SegmentAABBOverlap(Center, Extents)) return; | ||
| 527 | |||
| 528 | if(node->IsLeaf()) | ||
| 529 | { | ||
| 530 | SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 531 | } | ||
| 532 | else | ||
| 533 | { | ||
| 534 | _SegmentStab(node->GetPos()); | ||
| 535 | |||
| 536 | if(ContactFound()) return; | ||
| 537 | |||
| 538 | _SegmentStab(node->GetNeg()); | ||
| 539 | } | ||
| 540 | } | ||
| 541 | |||
| 542 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 543 | /** | ||
| 544 | * Recursive stabbing query for no-leaf AABB trees. | ||
| 545 | * \param node [in] current collision node | ||
| 546 | */ | ||
| 547 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 548 | void RayCollider::_SegmentStab(const AABBNoLeafNode* node) | ||
| 549 | { | ||
| 550 | // Perform Segment-AABB overlap test | ||
| 551 | if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 552 | |||
| 553 | if(node->HasPosLeaf()) | ||
| 554 | { | ||
| 555 | SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT) | ||
| 556 | } | ||
| 557 | else _SegmentStab(node->GetPos()); | ||
| 558 | |||
| 559 | if(ContactFound()) return; | ||
| 560 | |||
| 561 | if(node->HasNegLeaf()) | ||
| 562 | { | ||
| 563 | SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT) | ||
| 564 | } | ||
| 565 | else _SegmentStab(node->GetNeg()); | ||
| 566 | } | ||
| 567 | |||
| 568 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 569 | /** | ||
| 570 | * Recursive stabbing query for quantized no-leaf AABB trees. | ||
| 571 | * \param node [in] current collision node | ||
| 572 | */ | ||
| 573 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 574 | void RayCollider::_SegmentStab(const AABBQuantizedNoLeafNode* node) | ||
| 575 | { | ||
| 576 | // Dequantize box | ||
| 577 | const QuantizedAABB& Box = node->mAABB; | ||
| 578 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 579 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 580 | |||
| 581 | // Perform Segment-AABB overlap test | ||
| 582 | if(!SegmentAABBOverlap(Center, Extents)) return; | ||
| 583 | |||
| 584 | if(node->HasPosLeaf()) | ||
| 585 | { | ||
| 586 | SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT) | ||
| 587 | } | ||
| 588 | else _SegmentStab(node->GetPos()); | ||
| 589 | |||
| 590 | if(ContactFound()) return; | ||
| 591 | |||
| 592 | if(node->HasNegLeaf()) | ||
| 593 | { | ||
| 594 | SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT) | ||
| 595 | } | ||
| 596 | else _SegmentStab(node->GetNeg()); | ||
| 597 | } | ||
| 598 | |||
| 599 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 600 | /** | ||
| 601 | * Recursive stabbing query for vanilla AABB trees. | ||
| 602 | * \param node [in] current collision node | ||
| 603 | * \param box_indices [out] indices of stabbed boxes | ||
| 604 | */ | ||
| 605 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 606 | void RayCollider::_SegmentStab(const AABBTreeNode* node, Container& box_indices) | ||
| 607 | { | ||
| 608 | // Test the box against the segment | ||
| 609 | Point Center, Extents; | ||
| 610 | node->GetAABB()->GetCenter(Center); | ||
| 611 | node->GetAABB()->GetExtents(Extents); | ||
| 612 | if(!SegmentAABBOverlap(Center, Extents)) return; | ||
| 613 | |||
| 614 | if(node->IsLeaf()) | ||
| 615 | { | ||
| 616 | box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives()); | ||
| 617 | } | ||
| 618 | else | ||
| 619 | { | ||
| 620 | _SegmentStab(node->GetPos(), box_indices); | ||
| 621 | _SegmentStab(node->GetNeg(), box_indices); | ||
| 622 | } | ||
| 623 | } | ||
| 624 | |||
| 625 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 626 | /** | ||
| 627 | * Recursive stabbing query for normal AABB trees. | ||
| 628 | * \param node [in] current collision node | ||
| 629 | */ | ||
| 630 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 631 | void RayCollider::_RayStab(const AABBCollisionNode* node) | ||
| 632 | { | ||
| 633 | // Perform Ray-AABB overlap test | ||
| 634 | if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 635 | |||
| 636 | if(node->IsLeaf()) | ||
| 637 | { | ||
| 638 | RAY_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 639 | } | ||
| 640 | else | ||
| 641 | { | ||
| 642 | _RayStab(node->GetPos()); | ||
| 643 | |||
| 644 | if(ContactFound()) return; | ||
| 645 | |||
| 646 | _RayStab(node->GetNeg()); | ||
| 647 | } | ||
| 648 | } | ||
| 649 | |||
| 650 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 651 | /** | ||
| 652 | * Recursive stabbing query for quantized AABB trees. | ||
| 653 | * \param node [in] current collision node | ||
| 654 | */ | ||
| 655 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 656 | void RayCollider::_RayStab(const AABBQuantizedNode* node) | ||
| 657 | { | ||
| 658 | // Dequantize box | ||
| 659 | const QuantizedAABB& Box = node->mAABB; | ||
| 660 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 661 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 662 | |||
| 663 | // Perform Ray-AABB overlap test | ||
| 664 | if(!RayAABBOverlap(Center, Extents)) return; | ||
| 665 | |||
| 666 | if(node->IsLeaf()) | ||
| 667 | { | ||
| 668 | RAY_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 669 | } | ||
| 670 | else | ||
| 671 | { | ||
| 672 | _RayStab(node->GetPos()); | ||
| 673 | |||
| 674 | if(ContactFound()) return; | ||
| 675 | |||
| 676 | _RayStab(node->GetNeg()); | ||
| 677 | } | ||
| 678 | } | ||
| 679 | |||
| 680 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 681 | /** | ||
| 682 | * Recursive stabbing query for no-leaf AABB trees. | ||
| 683 | * \param node [in] current collision node | ||
| 684 | */ | ||
| 685 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 686 | void RayCollider::_RayStab(const AABBNoLeafNode* node) | ||
| 687 | { | ||
| 688 | // Perform Ray-AABB overlap test | ||
| 689 | if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 690 | |||
| 691 | if(node->HasPosLeaf()) | ||
| 692 | { | ||
| 693 | RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT) | ||
| 694 | } | ||
| 695 | else _RayStab(node->GetPos()); | ||
| 696 | |||
| 697 | if(ContactFound()) return; | ||
| 698 | |||
| 699 | if(node->HasNegLeaf()) | ||
| 700 | { | ||
| 701 | RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT) | ||
| 702 | } | ||
| 703 | else _RayStab(node->GetNeg()); | ||
| 704 | } | ||
| 705 | |||
| 706 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 707 | /** | ||
| 708 | * Recursive stabbing query for quantized no-leaf AABB trees. | ||
| 709 | * \param node [in] current collision node | ||
| 710 | */ | ||
| 711 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 712 | void RayCollider::_RayStab(const AABBQuantizedNoLeafNode* node) | ||
| 713 | { | ||
| 714 | // Dequantize box | ||
| 715 | const QuantizedAABB& Box = node->mAABB; | ||
| 716 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 717 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 718 | |||
| 719 | // Perform Ray-AABB overlap test | ||
| 720 | if(!RayAABBOverlap(Center, Extents)) return; | ||
| 721 | |||
| 722 | if(node->HasPosLeaf()) | ||
| 723 | { | ||
| 724 | RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT) | ||
| 725 | } | ||
| 726 | else _RayStab(node->GetPos()); | ||
| 727 | |||
| 728 | if(ContactFound()) return; | ||
| 729 | |||
| 730 | if(node->HasNegLeaf()) | ||
| 731 | { | ||
| 732 | RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT) | ||
| 733 | } | ||
| 734 | else _RayStab(node->GetNeg()); | ||
| 735 | } | ||
| 736 | |||
| 737 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 738 | /** | ||
| 739 | * Recursive stabbing query for vanilla AABB trees. | ||
| 740 | * \param node [in] current collision node | ||
| 741 | * \param box_indices [out] indices of stabbed boxes | ||
| 742 | */ | ||
| 743 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 744 | void RayCollider::_RayStab(const AABBTreeNode* node, Container& box_indices) | ||
| 745 | { | ||
| 746 | // Test the box against the ray | ||
| 747 | Point Center, Extents; | ||
| 748 | node->GetAABB()->GetCenter(Center); | ||
| 749 | node->GetAABB()->GetExtents(Extents); | ||
| 750 | if(!RayAABBOverlap(Center, Extents)) return; | ||
| 751 | |||
| 752 | if(node->IsLeaf()) | ||
| 753 | { | ||
| 754 | mFlags |= OPC_CONTACT; | ||
| 755 | box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives()); | ||
| 756 | } | ||
| 757 | else | ||
| 758 | { | ||
| 759 | _RayStab(node->GetPos(), box_indices); | ||
| 760 | _RayStab(node->GetNeg(), box_indices); | ||
| 761 | } | ||
| 762 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_RayCollider.h b/libraries/ode-0.9/OPCODE/OPC_RayCollider.h new file mode 100644 index 0000000..3fd8381 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_RayCollider.h | |||
| @@ -0,0 +1,225 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a ray collider. | ||
| 12 | * \file OPC_RayCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_RAYCOLLIDER_H__ | ||
| 21 | #define __OPC_RAYCOLLIDER_H__ | ||
| 22 | |||
| 23 | class OPCODE_API CollisionFace | ||
| 24 | { | ||
| 25 | public: | ||
| 26 | //! Constructor | ||
| 27 | inline_ CollisionFace() {} | ||
| 28 | //! Destructor | ||
| 29 | inline_ ~CollisionFace() {} | ||
| 30 | |||
| 31 | udword mFaceID; //!< Index of touched face | ||
| 32 | float mDistance; //!< Distance from collider to hitpoint | ||
| 33 | float mU, mV; //!< Impact barycentric coordinates | ||
| 34 | }; | ||
| 35 | |||
| 36 | class OPCODE_API CollisionFaces : public Container | ||
| 37 | { | ||
| 38 | public: | ||
| 39 | //! Constructor | ||
| 40 | CollisionFaces() {} | ||
| 41 | //! Destructor | ||
| 42 | ~CollisionFaces() {} | ||
| 43 | |||
| 44 | inline_ udword GetNbFaces() const { return GetNbEntries()>>2; } | ||
| 45 | inline_ const CollisionFace* GetFaces() const { return (const CollisionFace*)GetEntries(); } | ||
| 46 | |||
| 47 | inline_ void Reset() { Container::Reset(); } | ||
| 48 | |||
| 49 | inline_ void AddFace(const CollisionFace& face) { Add(face.mFaceID).Add(face.mDistance).Add(face.mU).Add(face.mV); } | ||
| 50 | }; | ||
| 51 | |||
| 52 | #ifdef OPC_RAYHIT_CALLBACK | ||
| 53 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 54 | /** | ||
| 55 | * User-callback, called by OPCODE to record a hit. | ||
| 56 | * \param hit [in] current hit | ||
| 57 | * \param user_data [in] user-defined data from SetCallback() | ||
| 58 | */ | ||
| 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 60 | typedef void (*HitCallback) (const CollisionFace& hit, void* user_data); | ||
| 61 | #endif | ||
| 62 | |||
| 63 | class OPCODE_API RayCollider : public Collider | ||
| 64 | { | ||
| 65 | public: | ||
| 66 | // Constructor / Destructor | ||
| 67 | RayCollider(); | ||
| 68 | virtual ~RayCollider(); | ||
| 69 | |||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | /** | ||
| 72 | * Generic stabbing query for generic OPCODE models. After the call, access the results: | ||
| 73 | * - with GetContactStatus() | ||
| 74 | * - in the user-provided destination array | ||
| 75 | * | ||
| 76 | * \param world_ray [in] stabbing ray in world space | ||
| 77 | * \param model [in] Opcode model to collide with | ||
| 78 | * \param world [in] model's world matrix, or null | ||
| 79 | * \param cache [in] a possibly cached face index, or null | ||
| 80 | * \return true if success | ||
| 81 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 82 | */ | ||
| 83 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 84 | bool Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world=null, udword* cache=null); | ||
| 85 | // | ||
| 86 | bool Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices); | ||
| 87 | // Settings | ||
| 88 | |||
| 89 | #ifndef OPC_RAYHIT_CALLBACK | ||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | /** | ||
| 92 | * Settings: enable or disable "closest hit" mode. | ||
| 93 | * \param flag [in] true to report closest hit only | ||
| 94 | * \see SetCulling(bool flag) | ||
| 95 | * \see SetMaxDist(float max_dist) | ||
| 96 | * \see SetDestination(StabbedFaces* sf) | ||
| 97 | */ | ||
| 98 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 99 | inline_ void SetClosestHit(bool flag) { mClosestHit = flag; } | ||
| 100 | #endif | ||
| 101 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 102 | /** | ||
| 103 | * Settings: enable or disable backface culling. | ||
| 104 | * \param flag [in] true to enable backface culling | ||
| 105 | * \see SetClosestHit(bool flag) | ||
| 106 | * \see SetMaxDist(float max_dist) | ||
| 107 | * \see SetDestination(StabbedFaces* sf) | ||
| 108 | */ | ||
| 109 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 110 | inline_ void SetCulling(bool flag) { mCulling = flag; } | ||
| 111 | |||
| 112 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 113 | /** | ||
| 114 | * Settings: sets the higher distance bound. | ||
| 115 | * \param max_dist [in] higher distance bound. Default = maximal value, for ray queries (else segment) | ||
| 116 | * \see SetClosestHit(bool flag) | ||
| 117 | * \see SetCulling(bool flag) | ||
| 118 | * \see SetDestination(StabbedFaces* sf) | ||
| 119 | */ | ||
| 120 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 121 | inline_ void SetMaxDist(float max_dist=MAX_FLOAT) { mMaxDist = max_dist; } | ||
| 122 | |||
| 123 | #ifdef OPC_RAYHIT_CALLBACK | ||
| 124 | inline_ void SetHitCallback(HitCallback cb) { mHitCallback = cb; } | ||
| 125 | inline_ void SetUserData(void* user_data) { mUserData = user_data; } | ||
| 126 | #else | ||
| 127 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 128 | /** | ||
| 129 | * Settings: sets the destination array for stabbed faces. | ||
| 130 | * \param cf [in] destination array, filled during queries | ||
| 131 | * \see SetClosestHit(bool flag) | ||
| 132 | * \see SetCulling(bool flag) | ||
| 133 | * \see SetMaxDist(float max_dist) | ||
| 134 | */ | ||
| 135 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 136 | inline_ void SetDestination(CollisionFaces* cf) { mStabbedFaces = cf; } | ||
| 137 | #endif | ||
| 138 | // Stats | ||
| 139 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 140 | /** | ||
| 141 | * Stats: gets the number of Ray-BV overlap tests after a collision query. | ||
| 142 | * \see GetNbRayPrimTests() | ||
| 143 | * \see GetNbIntersections() | ||
| 144 | * \return the number of Ray-BV tests performed during last query | ||
| 145 | */ | ||
| 146 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 147 | inline_ udword GetNbRayBVTests() const { return mNbRayBVTests; } | ||
| 148 | |||
| 149 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 150 | /** | ||
| 151 | * Stats: gets the number of Ray-Triangle overlap tests after a collision query. | ||
| 152 | * \see GetNbRayBVTests() | ||
| 153 | * \see GetNbIntersections() | ||
| 154 | * \return the number of Ray-Triangle tests performed during last query | ||
| 155 | */ | ||
| 156 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 157 | inline_ udword GetNbRayPrimTests() const { return mNbRayPrimTests; } | ||
| 158 | |||
| 159 | // In-out test | ||
| 160 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 161 | /** | ||
| 162 | * Stats: gets the number of intersection found after a collision query. Can be used for in/out tests. | ||
| 163 | * \see GetNbRayBVTests() | ||
| 164 | * \see GetNbRayPrimTests() | ||
| 165 | * \return the number of valid intersections during last query | ||
| 166 | */ | ||
| 167 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 168 | inline_ udword GetNbIntersections() const { return mNbIntersections; } | ||
| 169 | |||
| 170 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 171 | /** | ||
| 172 | * Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider. | ||
| 173 | * \return null if everything is ok, else a string describing the problem | ||
| 174 | */ | ||
| 175 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 176 | override(Collider) const char* ValidateSettings(); | ||
| 177 | |||
| 178 | protected: | ||
| 179 | // Ray in local space | ||
| 180 | Point mOrigin; //!< Ray origin | ||
| 181 | Point mDir; //!< Ray direction (normalized) | ||
| 182 | Point mFDir; //!< fabsf(mDir) | ||
| 183 | Point mData, mData2; | ||
| 184 | // Stabbed faces | ||
| 185 | CollisionFace mStabbedFace; //!< Current stabbed face | ||
| 186 | #ifdef OPC_RAYHIT_CALLBACK | ||
| 187 | HitCallback mHitCallback; //!< Callback used to record a hit | ||
| 188 | void* mUserData; //!< User-defined data | ||
| 189 | #else | ||
| 190 | CollisionFaces* mStabbedFaces; //!< List of stabbed faces | ||
| 191 | #endif | ||
| 192 | // Stats | ||
| 193 | udword mNbRayBVTests; //!< Number of Ray-BV tests | ||
| 194 | udword mNbRayPrimTests; //!< Number of Ray-Primitive tests | ||
| 195 | // In-out test | ||
| 196 | udword mNbIntersections; //!< Number of valid intersections | ||
| 197 | // Dequantization coeffs | ||
| 198 | Point mCenterCoeff; | ||
| 199 | Point mExtentsCoeff; | ||
| 200 | // Settings | ||
| 201 | float mMaxDist; //!< Valid segment on the ray | ||
| 202 | #ifndef OPC_RAYHIT_CALLBACK | ||
| 203 | bool mClosestHit; //!< Report closest hit only | ||
| 204 | #endif | ||
| 205 | bool mCulling; //!< Stab culled faces or not | ||
| 206 | // Internal methods | ||
| 207 | void _SegmentStab(const AABBCollisionNode* node); | ||
| 208 | void _SegmentStab(const AABBNoLeafNode* node); | ||
| 209 | void _SegmentStab(const AABBQuantizedNode* node); | ||
| 210 | void _SegmentStab(const AABBQuantizedNoLeafNode* node); | ||
| 211 | void _SegmentStab(const AABBTreeNode* node, Container& box_indices); | ||
| 212 | void _RayStab(const AABBCollisionNode* node); | ||
| 213 | void _RayStab(const AABBNoLeafNode* node); | ||
| 214 | void _RayStab(const AABBQuantizedNode* node); | ||
| 215 | void _RayStab(const AABBQuantizedNoLeafNode* node); | ||
| 216 | void _RayStab(const AABBTreeNode* node, Container& box_indices); | ||
| 217 | // Overlap tests | ||
| 218 | inline_ BOOL RayAABBOverlap(const Point& center, const Point& extents); | ||
| 219 | inline_ BOOL SegmentAABBOverlap(const Point& center, const Point& extents); | ||
| 220 | inline_ BOOL RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2); | ||
| 221 | // Init methods | ||
| 222 | BOOL InitQuery(const Ray& world_ray, const Matrix4x4* world=null, udword* face_id=null); | ||
| 223 | }; | ||
| 224 | |||
| 225 | #endif // __OPC_RAYCOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_RayTriOverlap.h b/libraries/ode-0.9/OPCODE/OPC_RayTriOverlap.h new file mode 100644 index 0000000..7fe37c9 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_RayTriOverlap.h | |||
| @@ -0,0 +1,89 @@ | |||
| 1 | #define LOCAL_EPSILON 0.000001f | ||
| 2 | |||
| 3 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 4 | /** | ||
| 5 | * Computes a ray-triangle intersection test. | ||
| 6 | * Original code from Tomas Möller's "Fast Minimum Storage Ray-Triangle Intersection". | ||
| 7 | * It's been optimized a bit with integer code, and modified to return a non-intersection if distance from | ||
| 8 | * ray origin to triangle is negative. | ||
| 9 | * | ||
| 10 | * \param vert0 [in] triangle vertex | ||
| 11 | * \param vert1 [in] triangle vertex | ||
| 12 | * \param vert2 [in] triangle vertex | ||
| 13 | * \return true on overlap. mStabbedFace is filled with relevant info. | ||
| 14 | */ | ||
| 15 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 16 | inline_ BOOL RayCollider::RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2) | ||
| 17 | { | ||
| 18 | // Stats | ||
| 19 | mNbRayPrimTests++; | ||
| 20 | |||
| 21 | // Find vectors for two edges sharing vert0 | ||
| 22 | Point edge1 = vert1 - vert0; | ||
| 23 | Point edge2 = vert2 - vert0; | ||
| 24 | |||
| 25 | // Begin calculating determinant - also used to calculate U parameter | ||
| 26 | Point pvec = mDir^edge2; | ||
| 27 | |||
| 28 | // If determinant is near zero, ray lies in plane of triangle | ||
| 29 | float det = edge1|pvec; | ||
| 30 | |||
| 31 | if(mCulling) | ||
| 32 | { | ||
| 33 | if(det<LOCAL_EPSILON) return FALSE; | ||
| 34 | // From here, det is > 0. So we can use integer cmp. | ||
| 35 | |||
| 36 | // Calculate distance from vert0 to ray origin | ||
| 37 | Point tvec = mOrigin - vert0; | ||
| 38 | |||
| 39 | // Calculate U parameter and test bounds | ||
| 40 | mStabbedFace.mU = tvec|pvec; | ||
| 41 | // if(IR(u)&0x80000000 || u>det) return FALSE; | ||
| 42 | if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IR(det)) return FALSE; | ||
| 43 | |||
| 44 | // Prepare to test V parameter | ||
| 45 | Point qvec = tvec^edge1; | ||
| 46 | |||
| 47 | // Calculate V parameter and test bounds | ||
| 48 | mStabbedFace.mV = mDir|qvec; | ||
| 49 | if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>det) return FALSE; | ||
| 50 | |||
| 51 | // Calculate t, scale parameters, ray intersects triangle | ||
| 52 | mStabbedFace.mDistance = edge2|qvec; | ||
| 53 | // Det > 0 so we can early exit here | ||
| 54 | // Intersection point is valid if distance is positive (else it can just be a face behind the orig point) | ||
| 55 | if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance)) return FALSE; | ||
| 56 | // Else go on | ||
| 57 | float OneOverDet = 1.0f / det; | ||
| 58 | mStabbedFace.mDistance *= OneOverDet; | ||
| 59 | mStabbedFace.mU *= OneOverDet; | ||
| 60 | mStabbedFace.mV *= OneOverDet; | ||
| 61 | } | ||
| 62 | else | ||
| 63 | { | ||
| 64 | // the non-culling branch | ||
| 65 | if(det>-LOCAL_EPSILON && det<LOCAL_EPSILON) return FALSE; | ||
| 66 | float OneOverDet = 1.0f / det; | ||
| 67 | |||
| 68 | // Calculate distance from vert0 to ray origin | ||
| 69 | Point tvec = mOrigin - vert0; | ||
| 70 | |||
| 71 | // Calculate U parameter and test bounds | ||
| 72 | mStabbedFace.mU = (tvec|pvec) * OneOverDet; | ||
| 73 | // if(IR(u)&0x80000000 || u>1.0f) return FALSE; | ||
| 74 | if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IEEE_1_0) return FALSE; | ||
| 75 | |||
| 76 | // prepare to test V parameter | ||
| 77 | Point qvec = tvec^edge1; | ||
| 78 | |||
| 79 | // Calculate V parameter and test bounds | ||
| 80 | mStabbedFace.mV = (mDir|qvec) * OneOverDet; | ||
| 81 | if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>1.0f) return FALSE; | ||
| 82 | |||
| 83 | // Calculate t, ray intersects triangle | ||
| 84 | mStabbedFace.mDistance = (edge2|qvec) * OneOverDet; | ||
| 85 | // Intersection point is valid if distance is positive (else it can just be a face behind the orig point) | ||
| 86 | if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance)) return FALSE; | ||
| 87 | } | ||
| 88 | return TRUE; | ||
| 89 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_Settings.h b/libraries/ode-0.9/OPCODE/OPC_Settings.h new file mode 100644 index 0000000..1841a2b --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_Settings.h | |||
| @@ -0,0 +1,49 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains compilation flags. | ||
| 12 | * \file OPC_Settings.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date May, 12, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_SETTINGS_H__ | ||
| 21 | #define __OPC_SETTINGS_H__ | ||
| 22 | |||
| 23 | //! Use CPU comparisons (comment that line to use standard FPU compares) | ||
| 24 | #define OPC_CPU_COMPARE | ||
| 25 | |||
| 26 | //! Use FCOMI / FCMOV on Pentium-Pro based processors (comment that line to use plain C++) | ||
| 27 | #define OPC_USE_FCOMI | ||
| 28 | |||
| 29 | //! Use epsilon value in tri-tri overlap test | ||
| 30 | #define OPC_TRITRI_EPSILON_TEST | ||
| 31 | |||
| 32 | //! Use tree-coherence or not [not implemented yet] | ||
| 33 | // #define OPC_USE_TREE_COHERENCE | ||
| 34 | |||
| 35 | //! Use callbacks or direct pointers. Using callbacks might be a bit slower (but probably not much) | ||
| 36 | // #define OPC_USE_CALLBACKS | ||
| 37 | |||
| 38 | //! Support triangle and vertex strides or not. Using strides might be a bit slower (but probably not much) | ||
| 39 | #define OPC_USE_STRIDE | ||
| 40 | |||
| 41 | //! Discard negative pointer in vanilla trees | ||
| 42 | #define OPC_NO_NEG_VANILLA_TREE | ||
| 43 | |||
| 44 | //! Use a callback in the ray collider | ||
| 45 | //#define OPC_RAYHIT_CALLBACK | ||
| 46 | |||
| 47 | // NB: no compilation flag to enable/disable stats since they're actually needed in the box/box overlap test | ||
| 48 | |||
| 49 | #endif //__OPC_SETTINGS_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_SphereAABBOverlap.h b/libraries/ode-0.9/OPCODE/OPC_SphereAABBOverlap.h new file mode 100644 index 0000000..2278bc0 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_SphereAABBOverlap.h | |||
| @@ -0,0 +1,128 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /** | ||
| 3 | * Sphere-AABB overlap test, based on Jim Arvo's code. | ||
| 4 | * \param center [in] box center | ||
| 5 | * \param extents [in] box extents | ||
| 6 | * \return TRUE on overlap | ||
| 7 | */ | ||
| 8 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 9 | inline_ BOOL SphereCollider::SphereAABBOverlap(const Point& center, const Point& extents) | ||
| 10 | { | ||
| 11 | // Stats | ||
| 12 | mNbVolumeBVTests++; | ||
| 13 | |||
| 14 | float d = 0.0f; | ||
| 15 | |||
| 16 | //find the square of the distance | ||
| 17 | //from the sphere to the box | ||
| 18 | #ifdef OLDIES | ||
| 19 | for(udword i=0;i<3;i++) | ||
| 20 | { | ||
| 21 | float tmp = mCenter[i] - center[i]; | ||
| 22 | float s = tmp + extents[i]; | ||
| 23 | |||
| 24 | if(s<0.0f) d += s*s; | ||
| 25 | else | ||
| 26 | { | ||
| 27 | s = tmp - extents[i]; | ||
| 28 | if(s>0.0f) d += s*s; | ||
| 29 | } | ||
| 30 | } | ||
| 31 | #endif | ||
| 32 | |||
| 33 | //#ifdef NEW_TEST | ||
| 34 | |||
| 35 | // float tmp = mCenter.x - center.x; | ||
| 36 | // float s = tmp + extents.x; | ||
| 37 | |||
| 38 | float tmp,s; | ||
| 39 | |||
| 40 | tmp = mCenter.x - center.x; | ||
| 41 | s = tmp + extents.x; | ||
| 42 | |||
| 43 | if(s<0.0f) | ||
| 44 | { | ||
| 45 | d += s*s; | ||
| 46 | if(d>mRadius2) return FALSE; | ||
| 47 | } | ||
| 48 | else | ||
| 49 | { | ||
| 50 | s = tmp - extents.x; | ||
| 51 | if(s>0.0f) | ||
| 52 | { | ||
| 53 | d += s*s; | ||
| 54 | if(d>mRadius2) return FALSE; | ||
| 55 | } | ||
| 56 | } | ||
| 57 | |||
| 58 | tmp = mCenter.y - center.y; | ||
| 59 | s = tmp + extents.y; | ||
| 60 | |||
| 61 | if(s<0.0f) | ||
| 62 | { | ||
| 63 | d += s*s; | ||
| 64 | if(d>mRadius2) return FALSE; | ||
| 65 | } | ||
| 66 | else | ||
| 67 | { | ||
| 68 | s = tmp - extents.y; | ||
| 69 | if(s>0.0f) | ||
| 70 | { | ||
| 71 | d += s*s; | ||
| 72 | if(d>mRadius2) return FALSE; | ||
| 73 | } | ||
| 74 | } | ||
| 75 | |||
| 76 | tmp = mCenter.z - center.z; | ||
| 77 | s = tmp + extents.z; | ||
| 78 | |||
| 79 | if(s<0.0f) | ||
| 80 | { | ||
| 81 | d += s*s; | ||
| 82 | if(d>mRadius2) return FALSE; | ||
| 83 | } | ||
| 84 | else | ||
| 85 | { | ||
| 86 | s = tmp - extents.z; | ||
| 87 | if(s>0.0f) | ||
| 88 | { | ||
| 89 | d += s*s; | ||
| 90 | if(d>mRadius2) return FALSE; | ||
| 91 | } | ||
| 92 | } | ||
| 93 | //#endif | ||
| 94 | |||
| 95 | #ifdef OLDIES | ||
| 96 | // Point Min = center - extents; | ||
| 97 | // Point Max = center + extents; | ||
| 98 | |||
| 99 | float d = 0.0f; | ||
| 100 | |||
| 101 | //find the square of the distance | ||
| 102 | //from the sphere to the box | ||
| 103 | for(udword i=0;i<3;i++) | ||
| 104 | { | ||
| 105 | float Min = center[i] - extents[i]; | ||
| 106 | |||
| 107 | // if(mCenter[i]<Min[i]) | ||
| 108 | if(mCenter[i]<Min) | ||
| 109 | { | ||
| 110 | // float s = mCenter[i] - Min[i]; | ||
| 111 | float s = mCenter[i] - Min; | ||
| 112 | d += s*s; | ||
| 113 | } | ||
| 114 | else | ||
| 115 | { | ||
| 116 | float Max = center[i] + extents[i]; | ||
| 117 | |||
| 118 | // if(mCenter[i]>Max[i]) | ||
| 119 | if(mCenter[i]>Max) | ||
| 120 | { | ||
| 121 | float s = mCenter[i] - Max; | ||
| 122 | d += s*s; | ||
| 123 | } | ||
| 124 | } | ||
| 125 | } | ||
| 126 | #endif | ||
| 127 | return d <= mRadius2; | ||
| 128 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp new file mode 100644 index 0000000..65350b7 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp | |||
| @@ -0,0 +1,739 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a sphere collider. | ||
| 12 | * \file OPC_SphereCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains a sphere-vs-tree collider. | ||
| 21 | * This class performs a collision test between a sphere and an AABB tree. You can use this to do a standard player vs world collision, | ||
| 22 | * in a Nettle/Telemachos way. It doesn't suffer from all reported bugs in those two classic codes - the "new" one by Paul Nettle is a | ||
| 23 | * debuggued version I think. Collision response can be driven by reported collision data - it works extremely well for me. In sake of | ||
| 24 | * efficiency, all meshes (that is, all AABB trees) should of course also be kept in an extra hierarchical structure (octree, whatever). | ||
| 25 | * | ||
| 26 | * \class SphereCollider | ||
| 27 | * \author Pierre Terdiman | ||
| 28 | * \version 1.3 | ||
| 29 | * \date June, 2, 2001 | ||
| 30 | */ | ||
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 32 | |||
| 33 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 34 | // Precompiled Header | ||
| 35 | #include "Stdafx.h" | ||
| 36 | |||
| 37 | using namespace Opcode; | ||
| 38 | |||
| 39 | #include "OPC_SphereAABBOverlap.h" | ||
| 40 | #include "OPC_SphereTriOverlap.h" | ||
| 41 | |||
| 42 | #define SET_CONTACT(prim_index, flag) \ | ||
| 43 | /* Set contact status */ \ | ||
| 44 | mFlags |= flag; \ | ||
| 45 | mTouchedPrimitives->Add(udword(prim_index)); | ||
| 46 | |||
| 47 | //! Sphere-triangle overlap test | ||
| 48 | #define SPHERE_PRIM(prim_index, flag) \ | ||
| 49 | /* Request vertices from the app */ \ | ||
| 50 | VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ | ||
| 51 | \ | ||
| 52 | /* Perform sphere-tri overlap test */ \ | ||
| 53 | if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ | ||
| 54 | { \ | ||
| 55 | SET_CONTACT(prim_index, flag) \ | ||
| 56 | } | ||
| 57 | |||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | /** | ||
| 60 | * Constructor. | ||
| 61 | */ | ||
| 62 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 63 | SphereCollider::SphereCollider() | ||
| 64 | { | ||
| 65 | mCenter.Zero(); | ||
| 66 | mRadius2 = 0.0f; | ||
| 67 | } | ||
| 68 | |||
| 69 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 70 | /** | ||
| 71 | * Destructor. | ||
| 72 | */ | ||
| 73 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 74 | SphereCollider::~SphereCollider() | ||
| 75 | { | ||
| 76 | } | ||
| 77 | |||
| 78 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 79 | /** | ||
| 80 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 81 | * - with GetContactStatus() | ||
| 82 | * - with GetNbTouchedPrimitives() | ||
| 83 | * - with GetTouchedPrimitives() | ||
| 84 | * | ||
| 85 | * \param cache [in/out] a sphere cache | ||
| 86 | * \param sphere [in] collision sphere in local space | ||
| 87 | * \param model [in] Opcode model to collide with | ||
| 88 | * \param worlds [in] sphere's world matrix, or null | ||
| 89 | * \param worldm [in] model's world matrix, or null | ||
| 90 | * \return true if success | ||
| 91 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 92 | */ | ||
| 93 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 94 | bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm) | ||
| 95 | { | ||
| 96 | // Checkings | ||
| 97 | if(!Setup(&model)) return false; | ||
| 98 | |||
| 99 | // Init collision query | ||
| 100 | if(InitQuery(cache, sphere, worlds, worldm)) return true; | ||
| 101 | |||
| 102 | // Special case for 1-leaf trees | ||
| 103 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 104 | { | ||
| 105 | // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles | ||
| 106 | udword Nb = mIMesh->GetNbTriangles(); | ||
| 107 | // Loop through all triangles | ||
| 108 | for(udword i=0;i<Nb;i++) | ||
| 109 | { | ||
| 110 | SPHERE_PRIM(i, OPC_CONTACT) | ||
| 111 | } | ||
| 112 | return true; | ||
| 113 | } | ||
| 114 | |||
| 115 | if(!model.HasLeafNodes()) | ||
| 116 | { | ||
| 117 | if(model.IsQuantized()) | ||
| 118 | { | ||
| 119 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 120 | |||
| 121 | // Setup dequantization coeffs | ||
| 122 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 123 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 124 | |||
| 125 | // Perform collision query | ||
| 126 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 127 | else _Collide(Tree->GetNodes()); | ||
| 128 | } | ||
| 129 | else | ||
| 130 | { | ||
| 131 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 132 | |||
| 133 | // Perform collision query | ||
| 134 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 135 | else _Collide(Tree->GetNodes()); | ||
| 136 | } | ||
| 137 | } | ||
| 138 | else | ||
| 139 | { | ||
| 140 | if(model.IsQuantized()) | ||
| 141 | { | ||
| 142 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 143 | |||
| 144 | // Setup dequantization coeffs | ||
| 145 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 146 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 147 | |||
| 148 | // Perform collision query | ||
| 149 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 150 | else _Collide(Tree->GetNodes()); | ||
| 151 | } | ||
| 152 | else | ||
| 153 | { | ||
| 154 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 155 | |||
| 156 | // Perform collision query | ||
| 157 | if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 158 | else _Collide(Tree->GetNodes()); | ||
| 159 | } | ||
| 160 | } | ||
| 161 | return true; | ||
| 162 | } | ||
| 163 | |||
| 164 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 165 | /** | ||
| 166 | * Initializes a collision query : | ||
| 167 | * - reset stats & contact status | ||
| 168 | * - setup matrices | ||
| 169 | * - check temporal coherence | ||
| 170 | * | ||
| 171 | * \param cache [in/out] a sphere cache | ||
| 172 | * \param sphere [in] sphere in local space | ||
| 173 | * \param worlds [in] sphere's world matrix, or null | ||
| 174 | * \param worldm [in] model's world matrix, or null | ||
| 175 | * \return TRUE if we can return immediately | ||
| 176 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 177 | */ | ||
| 178 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 179 | BOOL SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm) | ||
| 180 | { | ||
| 181 | // 1) Call the base method | ||
| 182 | VolumeCollider::InitQuery(); | ||
| 183 | |||
| 184 | // 2) Compute sphere in model space: | ||
| 185 | // - Precompute R^2 | ||
| 186 | mRadius2 = sphere.mRadius * sphere.mRadius; | ||
| 187 | // - Compute center position | ||
| 188 | mCenter = sphere.mCenter; | ||
| 189 | // -> to world space | ||
| 190 | if(worlds) mCenter *= *worlds; | ||
| 191 | // -> to model space | ||
| 192 | if(worldm) | ||
| 193 | { | ||
| 194 | // Invert model matrix | ||
| 195 | Matrix4x4 InvWorldM; | ||
| 196 | InvertPRMatrix(InvWorldM, *worldm); | ||
| 197 | |||
| 198 | mCenter *= InvWorldM; | ||
| 199 | } | ||
| 200 | |||
| 201 | // 3) Setup destination pointer | ||
| 202 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 203 | |||
| 204 | // 4) Special case: 1-triangle meshes [Opcode 1.3] | ||
| 205 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 206 | { | ||
| 207 | if(!SkipPrimitiveTests()) | ||
| 208 | { | ||
| 209 | // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. | ||
| 210 | mTouchedPrimitives->Reset(); | ||
| 211 | |||
| 212 | // Perform overlap test between the unique triangle and the sphere (and set contact status if needed) | ||
| 213 | SPHERE_PRIM(udword(0), OPC_CONTACT) | ||
| 214 | |||
| 215 | // Return immediately regardless of status | ||
| 216 | return TRUE; | ||
| 217 | } | ||
| 218 | } | ||
| 219 | |||
| 220 | // 5) Check temporal coherence : | ||
| 221 | if(TemporalCoherenceEnabled()) | ||
| 222 | { | ||
| 223 | // Here we use temporal coherence | ||
| 224 | // => check results from previous frame before performing the collision query | ||
| 225 | if(FirstContactEnabled()) | ||
| 226 | { | ||
| 227 | // We're only interested in the first contact found => test the unique previously touched face | ||
| 228 | if(mTouchedPrimitives->GetNbEntries()) | ||
| 229 | { | ||
| 230 | // Get index of previously touched face = the first entry in the array | ||
| 231 | udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0); | ||
| 232 | |||
| 233 | // Then reset the array: | ||
| 234 | // - if the overlap test below is successful, the index we'll get added back anyway | ||
| 235 | // - if it isn't, then the array should be reset anyway for the normal query | ||
| 236 | mTouchedPrimitives->Reset(); | ||
| 237 | |||
| 238 | // Perform overlap test between the cached triangle and the sphere (and set contact status if needed) | ||
| 239 | SPHERE_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT) | ||
| 240 | |||
| 241 | // Return immediately if possible | ||
| 242 | if(GetContactStatus()) return TRUE; | ||
| 243 | } | ||
| 244 | // else no face has been touched during previous query | ||
| 245 | // => we'll have to perform a normal query | ||
| 246 | } | ||
| 247 | else | ||
| 248 | { | ||
| 249 | // We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious): | ||
| 250 | float r = sqrtf(cache.FatRadius2) - sphere.mRadius; | ||
| 251 | if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r) | ||
| 252 | { | ||
| 253 | // - if N is included in P, return previous list | ||
| 254 | // => we simply leave the list (mTouchedFaces) unchanged | ||
| 255 | |||
| 256 | // Set contact status if needed | ||
| 257 | if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT; | ||
| 258 | |||
| 259 | // In any case we don't need to do a query | ||
| 260 | return TRUE; | ||
| 261 | } | ||
| 262 | else | ||
| 263 | { | ||
| 264 | // - else do the query using a fat N | ||
| 265 | |||
| 266 | // Reset cache since we'll about to perform a real query | ||
| 267 | mTouchedPrimitives->Reset(); | ||
| 268 | |||
| 269 | // Make a fat sphere so that coherence will work for subsequent frames | ||
| 270 | mRadius2 *= cache.FatCoeff; | ||
| 271 | // mRadius2 = (sphere.mRadius * cache.FatCoeff)*(sphere.mRadius * cache.FatCoeff); | ||
| 272 | |||
| 273 | // Update cache with query data (signature for cached faces) | ||
| 274 | cache.Center = mCenter; | ||
| 275 | cache.FatRadius2 = mRadius2; | ||
| 276 | } | ||
| 277 | } | ||
| 278 | } | ||
| 279 | else | ||
| 280 | { | ||
| 281 | // Here we don't use temporal coherence => do a normal query | ||
| 282 | mTouchedPrimitives->Reset(); | ||
| 283 | } | ||
| 284 | |||
| 285 | return FALSE; | ||
| 286 | } | ||
| 287 | |||
| 288 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 289 | /** | ||
| 290 | * Collision query for vanilla AABB trees. | ||
| 291 | * \param cache [in/out] a sphere cache | ||
| 292 | * \param sphere [in] collision sphere in world space | ||
| 293 | * \param tree [in] AABB tree | ||
| 294 | * \return true if success | ||
| 295 | */ | ||
| 296 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 297 | bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree) | ||
| 298 | { | ||
| 299 | // This is typically called for a scene tree, full of -AABBs-, not full of triangles. | ||
| 300 | // So we don't really have "primitives" to deal with. Hence it doesn't work with | ||
| 301 | // "FirstContact" + "TemporalCoherence". | ||
| 302 | ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) ); | ||
| 303 | |||
| 304 | // Checkings | ||
| 305 | if(!tree) return false; | ||
| 306 | |||
| 307 | // Init collision query | ||
| 308 | if(InitQuery(cache, sphere)) return true; | ||
| 309 | |||
| 310 | // Perform collision query | ||
| 311 | _Collide(tree); | ||
| 312 | |||
| 313 | return true; | ||
| 314 | } | ||
| 315 | |||
| 316 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 317 | /** | ||
| 318 | * Checks the sphere completely contains the box. In which case we can end the query sooner. | ||
| 319 | * \param bc [in] box center | ||
| 320 | * \param be [in] box extents | ||
| 321 | * \return true if the sphere contains the whole box | ||
| 322 | */ | ||
| 323 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 324 | inline_ BOOL SphereCollider::SphereContainsBox(const Point& bc, const Point& be) | ||
| 325 | { | ||
| 326 | // I assume if all 8 box vertices are inside the sphere, so does the whole box. | ||
| 327 | // Sounds ok but maybe there's a better way? | ||
| 328 | Point p; | ||
| 329 | p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 330 | p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 331 | p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 332 | p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 333 | p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 334 | p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 335 | p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 336 | p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE; | ||
| 337 | |||
| 338 | return TRUE; | ||
| 339 | } | ||
| 340 | |||
| 341 | #define TEST_BOX_IN_SPHERE(center, extents) \ | ||
| 342 | if(SphereContainsBox(center, extents)) \ | ||
| 343 | { \ | ||
| 344 | /* Set contact status */ \ | ||
| 345 | mFlags |= OPC_CONTACT; \ | ||
| 346 | _Dump(node); \ | ||
| 347 | return; \ | ||
| 348 | } | ||
| 349 | |||
| 350 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 351 | /** | ||
| 352 | * Recursive collision query for normal AABB trees. | ||
| 353 | * \param node [in] current collision node | ||
| 354 | */ | ||
| 355 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 356 | void SphereCollider::_Collide(const AABBCollisionNode* node) | ||
| 357 | { | ||
| 358 | // Perform Sphere-AABB overlap test | ||
| 359 | if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 360 | |||
| 361 | TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 362 | |||
| 363 | if(node->IsLeaf()) | ||
| 364 | { | ||
| 365 | SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 366 | } | ||
| 367 | else | ||
| 368 | { | ||
| 369 | _Collide(node->GetPos()); | ||
| 370 | |||
| 371 | if(ContactFound()) return; | ||
| 372 | |||
| 373 | _Collide(node->GetNeg()); | ||
| 374 | } | ||
| 375 | } | ||
| 376 | |||
| 377 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 378 | /** | ||
| 379 | * Recursive collision query for normal AABB trees, without primitive tests. | ||
| 380 | * \param node [in] current collision node | ||
| 381 | */ | ||
| 382 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 383 | void SphereCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node) | ||
| 384 | { | ||
| 385 | // Perform Sphere-AABB overlap test | ||
| 386 | if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 387 | |||
| 388 | TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 389 | |||
| 390 | if(node->IsLeaf()) | ||
| 391 | { | ||
| 392 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 393 | } | ||
| 394 | else | ||
| 395 | { | ||
| 396 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 397 | |||
| 398 | if(ContactFound()) return; | ||
| 399 | |||
| 400 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 401 | } | ||
| 402 | } | ||
| 403 | |||
| 404 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 405 | /** | ||
| 406 | * Recursive collision query for quantized AABB trees. | ||
| 407 | * \param node [in] current collision node | ||
| 408 | */ | ||
| 409 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 410 | void SphereCollider::_Collide(const AABBQuantizedNode* node) | ||
| 411 | { | ||
| 412 | // Dequantize box | ||
| 413 | const QuantizedAABB& Box = node->mAABB; | ||
| 414 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 415 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 416 | |||
| 417 | // Perform Sphere-AABB overlap test | ||
| 418 | if(!SphereAABBOverlap(Center, Extents)) return; | ||
| 419 | |||
| 420 | TEST_BOX_IN_SPHERE(Center, Extents) | ||
| 421 | |||
| 422 | if(node->IsLeaf()) | ||
| 423 | { | ||
| 424 | SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT) | ||
| 425 | } | ||
| 426 | else | ||
| 427 | { | ||
| 428 | _Collide(node->GetPos()); | ||
| 429 | |||
| 430 | if(ContactFound()) return; | ||
| 431 | |||
| 432 | _Collide(node->GetNeg()); | ||
| 433 | } | ||
| 434 | } | ||
| 435 | |||
| 436 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 437 | /** | ||
| 438 | * Recursive collision query for quantized AABB trees, without primitive tests. | ||
| 439 | * \param node [in] current collision node | ||
| 440 | */ | ||
| 441 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 442 | void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node) | ||
| 443 | { | ||
| 444 | // Dequantize box | ||
| 445 | const QuantizedAABB& Box = node->mAABB; | ||
| 446 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 447 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 448 | |||
| 449 | // Perform Sphere-AABB overlap test | ||
| 450 | if(!SphereAABBOverlap(Center, Extents)) return; | ||
| 451 | |||
| 452 | TEST_BOX_IN_SPHERE(Center, Extents) | ||
| 453 | |||
| 454 | if(node->IsLeaf()) | ||
| 455 | { | ||
| 456 | SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) | ||
| 457 | } | ||
| 458 | else | ||
| 459 | { | ||
| 460 | _CollideNoPrimitiveTest(node->GetPos()); | ||
| 461 | |||
| 462 | if(ContactFound()) return; | ||
| 463 | |||
| 464 | _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 465 | } | ||
| 466 | } | ||
| 467 | |||
| 468 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 469 | /** | ||
| 470 | * Recursive collision query for no-leaf AABB trees. | ||
| 471 | * \param node [in] current collision node | ||
| 472 | */ | ||
| 473 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 474 | void SphereCollider::_Collide(const AABBNoLeafNode* node) | ||
| 475 | { | ||
| 476 | // Perform Sphere-AABB overlap test | ||
| 477 | if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 478 | |||
| 479 | TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 480 | |||
| 481 | if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 482 | else _Collide(node->GetPos()); | ||
| 483 | |||
| 484 | if(ContactFound()) return; | ||
| 485 | |||
| 486 | if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 487 | else _Collide(node->GetNeg()); | ||
| 488 | } | ||
| 489 | |||
| 490 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 491 | /** | ||
| 492 | * Recursive collision query for no-leaf AABB trees, without primitive tests. | ||
| 493 | * \param node [in] current collision node | ||
| 494 | */ | ||
| 495 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 496 | void SphereCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node) | ||
| 497 | { | ||
| 498 | // Perform Sphere-AABB overlap test | ||
| 499 | if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return; | ||
| 500 | |||
| 501 | TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents) | ||
| 502 | |||
| 503 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 504 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 505 | |||
| 506 | if(ContactFound()) return; | ||
| 507 | |||
| 508 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 509 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 510 | } | ||
| 511 | |||
| 512 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 513 | /** | ||
| 514 | * Recursive collision query for quantized no-leaf AABB trees. | ||
| 515 | * \param node [in] current collision node | ||
| 516 | */ | ||
| 517 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 518 | void SphereCollider::_Collide(const AABBQuantizedNoLeafNode* node) | ||
| 519 | { | ||
| 520 | // Dequantize box | ||
| 521 | const QuantizedAABB& Box = node->mAABB; | ||
| 522 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 523 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 524 | |||
| 525 | // Perform Sphere-AABB overlap test | ||
| 526 | if(!SphereAABBOverlap(Center, Extents)) return; | ||
| 527 | |||
| 528 | TEST_BOX_IN_SPHERE(Center, Extents) | ||
| 529 | |||
| 530 | if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 531 | else _Collide(node->GetPos()); | ||
| 532 | |||
| 533 | if(ContactFound()) return; | ||
| 534 | |||
| 535 | if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 536 | else _Collide(node->GetNeg()); | ||
| 537 | } | ||
| 538 | |||
| 539 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 540 | /** | ||
| 541 | * Recursive collision query for quantized no-leaf AABB trees, without primitive tests. | ||
| 542 | * \param node [in] current collision node | ||
| 543 | */ | ||
| 544 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 545 | void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node) | ||
| 546 | { | ||
| 547 | // Dequantize box | ||
| 548 | const QuantizedAABB& Box = node->mAABB; | ||
| 549 | const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); | ||
| 550 | const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); | ||
| 551 | |||
| 552 | // Perform Sphere-AABB overlap test | ||
| 553 | if(!SphereAABBOverlap(Center, Extents)) return; | ||
| 554 | |||
| 555 | TEST_BOX_IN_SPHERE(Center, Extents) | ||
| 556 | |||
| 557 | if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } | ||
| 558 | else _CollideNoPrimitiveTest(node->GetPos()); | ||
| 559 | |||
| 560 | if(ContactFound()) return; | ||
| 561 | |||
| 562 | if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } | ||
| 563 | else _CollideNoPrimitiveTest(node->GetNeg()); | ||
| 564 | } | ||
| 565 | |||
| 566 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 567 | /** | ||
| 568 | * Recursive collision query for vanilla AABB trees. | ||
| 569 | * \param node [in] current collision node | ||
| 570 | */ | ||
| 571 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 572 | void SphereCollider::_Collide(const AABBTreeNode* node) | ||
| 573 | { | ||
| 574 | // Perform Sphere-AABB overlap test | ||
| 575 | Point Center, Extents; | ||
| 576 | node->GetAABB()->GetCenter(Center); | ||
| 577 | node->GetAABB()->GetExtents(Extents); | ||
| 578 | if(!SphereAABBOverlap(Center, Extents)) return; | ||
| 579 | |||
| 580 | if(node->IsLeaf() || SphereContainsBox(Center, Extents)) | ||
| 581 | { | ||
| 582 | mFlags |= OPC_CONTACT; | ||
| 583 | mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives()); | ||
| 584 | } | ||
| 585 | else | ||
| 586 | { | ||
| 587 | _Collide(node->GetPos()); | ||
| 588 | _Collide(node->GetNeg()); | ||
| 589 | } | ||
| 590 | } | ||
| 591 | |||
| 592 | |||
| 593 | |||
| 594 | |||
| 595 | |||
| 596 | |||
| 597 | |||
| 598 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 599 | /** | ||
| 600 | * Constructor. | ||
| 601 | */ | ||
| 602 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 603 | HybridSphereCollider::HybridSphereCollider() | ||
| 604 | { | ||
| 605 | } | ||
| 606 | |||
| 607 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 608 | /** | ||
| 609 | * Destructor. | ||
| 610 | */ | ||
| 611 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 612 | HybridSphereCollider::~HybridSphereCollider() | ||
| 613 | { | ||
| 614 | } | ||
| 615 | |||
| 616 | bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, const Matrix4x4* worldm) | ||
| 617 | { | ||
| 618 | // We don't want primitive tests here! | ||
| 619 | mFlags |= OPC_NO_PRIMITIVE_TESTS; | ||
| 620 | |||
| 621 | // Checkings | ||
| 622 | if(!Setup(&model)) return false; | ||
| 623 | |||
| 624 | // Init collision query | ||
| 625 | if(InitQuery(cache, sphere, worlds, worldm)) return true; | ||
| 626 | |||
| 627 | // Special case for 1-leaf trees | ||
| 628 | if(mCurrentModel && mCurrentModel->HasSingleNode()) | ||
| 629 | { | ||
| 630 | // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles | ||
| 631 | udword Nb = mIMesh->GetNbTriangles(); | ||
| 632 | |||
| 633 | // Loop through all triangles | ||
| 634 | for(udword i=0;i<Nb;i++) | ||
| 635 | { | ||
| 636 | SPHERE_PRIM(i, OPC_CONTACT) | ||
| 637 | } | ||
| 638 | return true; | ||
| 639 | } | ||
| 640 | |||
| 641 | // Override destination array since we're only going to get leaf boxes here | ||
| 642 | mTouchedBoxes.Reset(); | ||
| 643 | mTouchedPrimitives = &mTouchedBoxes; | ||
| 644 | |||
| 645 | // Now, do the actual query against leaf boxes | ||
| 646 | if(!model.HasLeafNodes()) | ||
| 647 | { | ||
| 648 | if(model.IsQuantized()) | ||
| 649 | { | ||
| 650 | const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); | ||
| 651 | |||
| 652 | // Setup dequantization coeffs | ||
| 653 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 654 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 655 | |||
| 656 | // Perform collision query - we don't want primitive tests here! | ||
| 657 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 658 | } | ||
| 659 | else | ||
| 660 | { | ||
| 661 | const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); | ||
| 662 | |||
| 663 | // Perform collision query - we don't want primitive tests here! | ||
| 664 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 665 | } | ||
| 666 | } | ||
| 667 | else | ||
| 668 | { | ||
| 669 | if(model.IsQuantized()) | ||
| 670 | { | ||
| 671 | const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); | ||
| 672 | |||
| 673 | // Setup dequantization coeffs | ||
| 674 | mCenterCoeff = Tree->mCenterCoeff; | ||
| 675 | mExtentsCoeff = Tree->mExtentsCoeff; | ||
| 676 | |||
| 677 | // Perform collision query - we don't want primitive tests here! | ||
| 678 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 679 | } | ||
| 680 | else | ||
| 681 | { | ||
| 682 | const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); | ||
| 683 | |||
| 684 | // Perform collision query - we don't want primitive tests here! | ||
| 685 | _CollideNoPrimitiveTest(Tree->GetNodes()); | ||
| 686 | } | ||
| 687 | } | ||
| 688 | |||
| 689 | // We only have a list of boxes so far | ||
| 690 | if(GetContactStatus()) | ||
| 691 | { | ||
| 692 | // Reset contact status, since it currently only reflects collisions with leaf boxes | ||
| 693 | Collider::InitQuery(); | ||
| 694 | |||
| 695 | // Change dest container so that we can use built-in overlap tests and get collided primitives | ||
| 696 | cache.TouchedPrimitives.Reset(); | ||
| 697 | mTouchedPrimitives = &cache.TouchedPrimitives; | ||
| 698 | |||
| 699 | // Read touched leaf boxes | ||
| 700 | udword Nb = mTouchedBoxes.GetNbEntries(); | ||
| 701 | const udword* Touched = mTouchedBoxes.GetEntries(); | ||
| 702 | |||
| 703 | const LeafTriangles* LT = model.GetLeafTriangles(); | ||
| 704 | const udword* Indices = model.GetIndices(); | ||
| 705 | |||
| 706 | // Loop through touched leaves | ||
| 707 | while(Nb--) | ||
| 708 | { | ||
| 709 | const LeafTriangles& CurrentLeaf = LT[*Touched++]; | ||
| 710 | |||
| 711 | // Each leaf box has a set of triangles | ||
| 712 | udword NbTris = CurrentLeaf.GetNbTriangles(); | ||
| 713 | if(Indices) | ||
| 714 | { | ||
| 715 | const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; | ||
| 716 | |||
| 717 | // Loop through triangles and test each of them | ||
| 718 | while(NbTris--) | ||
| 719 | { | ||
| 720 | udword TriangleIndex = *T++; | ||
| 721 | SPHERE_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 722 | } | ||
| 723 | } | ||
| 724 | else | ||
| 725 | { | ||
| 726 | udword BaseIndex = CurrentLeaf.GetTriangleIndex(); | ||
| 727 | |||
| 728 | // Loop through triangles and test each of them | ||
| 729 | while(NbTris--) | ||
| 730 | { | ||
| 731 | udword TriangleIndex = BaseIndex++; | ||
| 732 | SPHERE_PRIM(TriangleIndex, OPC_CONTACT) | ||
| 733 | } | ||
| 734 | } | ||
| 735 | } | ||
| 736 | } | ||
| 737 | |||
| 738 | return true; | ||
| 739 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_SphereCollider.h b/libraries/ode-0.9/OPCODE/OPC_SphereCollider.h new file mode 100644 index 0000000..ac6495e --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_SphereCollider.h | |||
| @@ -0,0 +1,96 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a sphere collider. | ||
| 12 | * \file OPC_SphereCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_SPHERECOLLIDER_H__ | ||
| 21 | #define __OPC_SPHERECOLLIDER_H__ | ||
| 22 | |||
| 23 | struct OPCODE_API SphereCache : VolumeCache | ||
| 24 | { | ||
| 25 | SphereCache() : Center(0.0f,0.0f,0.0f), FatRadius2(0.0f), FatCoeff(1.1f) {} | ||
| 26 | ~SphereCache() {} | ||
| 27 | |||
| 28 | // Cached faces signature | ||
| 29 | Point Center; //!< Sphere used when performing the query resulting in cached faces | ||
| 30 | float FatRadius2; //!< Sphere used when performing the query resulting in cached faces | ||
| 31 | // User settings | ||
| 32 | float FatCoeff; //!< mRadius2 multiplier used to create a fat sphere | ||
| 33 | }; | ||
| 34 | |||
| 35 | class OPCODE_API SphereCollider : public VolumeCollider | ||
| 36 | { | ||
| 37 | public: | ||
| 38 | // Constructor / Destructor | ||
| 39 | SphereCollider(); | ||
| 40 | virtual ~SphereCollider(); | ||
| 41 | |||
| 42 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 43 | /** | ||
| 44 | * Generic collision query for generic OPCODE models. After the call, access the results: | ||
| 45 | * - with GetContactStatus() | ||
| 46 | * - with GetNbTouchedPrimitives() | ||
| 47 | * - with GetTouchedPrimitives() | ||
| 48 | * | ||
| 49 | * \param cache [in/out] a sphere cache | ||
| 50 | * \param sphere [in] collision sphere in local space | ||
| 51 | * \param model [in] Opcode model to collide with | ||
| 52 | * \param worlds [in] sphere's world matrix, or null | ||
| 53 | * \param worldm [in] model's world matrix, or null | ||
| 54 | * \return true if success | ||
| 55 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 56 | */ | ||
| 57 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 58 | bool Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null); | ||
| 59 | |||
| 60 | // | ||
| 61 | bool Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree); | ||
| 62 | protected: | ||
| 63 | // Sphere in model space | ||
| 64 | Point mCenter; //!< Sphere center | ||
| 65 | float mRadius2; //!< Sphere radius squared | ||
| 66 | // Internal methods | ||
| 67 | void _Collide(const AABBCollisionNode* node); | ||
| 68 | void _Collide(const AABBNoLeafNode* node); | ||
| 69 | void _Collide(const AABBQuantizedNode* node); | ||
| 70 | void _Collide(const AABBQuantizedNoLeafNode* node); | ||
| 71 | void _Collide(const AABBTreeNode* node); | ||
| 72 | void _CollideNoPrimitiveTest(const AABBCollisionNode* node); | ||
| 73 | void _CollideNoPrimitiveTest(const AABBNoLeafNode* node); | ||
| 74 | void _CollideNoPrimitiveTest(const AABBQuantizedNode* node); | ||
| 75 | void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node); | ||
| 76 | // Overlap tests | ||
| 77 | inline_ BOOL SphereContainsBox(const Point& bc, const Point& be); | ||
| 78 | inline_ BOOL SphereAABBOverlap(const Point& center, const Point& extents); | ||
| 79 | BOOL SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2); | ||
| 80 | // Init methods | ||
| 81 | BOOL InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null); | ||
| 82 | }; | ||
| 83 | |||
| 84 | class OPCODE_API HybridSphereCollider : public SphereCollider | ||
| 85 | { | ||
| 86 | public: | ||
| 87 | // Constructor / Destructor | ||
| 88 | HybridSphereCollider(); | ||
| 89 | virtual ~HybridSphereCollider(); | ||
| 90 | |||
| 91 | bool Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null); | ||
| 92 | protected: | ||
| 93 | Container mTouchedBoxes; | ||
| 94 | }; | ||
| 95 | |||
| 96 | #endif // __OPC_SPHERECOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_SphereTriOverlap.h b/libraries/ode-0.9/OPCODE/OPC_SphereTriOverlap.h new file mode 100644 index 0000000..77e59f3 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_SphereTriOverlap.h | |||
| @@ -0,0 +1,187 @@ | |||
| 1 | |||
| 2 | // This is collision detection. If you do another distance test for collision *response*, | ||
| 3 | // if might be useful to simply *skip* the test below completely, and report a collision. | ||
| 4 | // - if sphere-triangle overlap, result is ok | ||
| 5 | // - if they don't, we'll discard them during collision response with a similar test anyway | ||
| 6 | // Overall this approach should run faster. | ||
| 7 | |||
| 8 | // Original code by David Eberly in Magic. | ||
| 9 | BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2) | ||
| 10 | { | ||
| 11 | // Stats | ||
| 12 | mNbVolumePrimTests++; | ||
| 13 | |||
| 14 | // Early exit if one of the vertices is inside the sphere | ||
| 15 | Point kDiff = vert2 - mCenter; | ||
| 16 | float fC = kDiff.SquareMagnitude(); | ||
| 17 | if(fC <= mRadius2) return TRUE; | ||
| 18 | |||
| 19 | kDiff = vert1 - mCenter; | ||
| 20 | fC = kDiff.SquareMagnitude(); | ||
| 21 | if(fC <= mRadius2) return TRUE; | ||
| 22 | |||
| 23 | kDiff = vert0 - mCenter; | ||
| 24 | fC = kDiff.SquareMagnitude(); | ||
| 25 | if(fC <= mRadius2) return TRUE; | ||
| 26 | |||
| 27 | // Else do the full distance test | ||
| 28 | Point TriEdge0 = vert1 - vert0; | ||
| 29 | Point TriEdge1 = vert2 - vert0; | ||
| 30 | |||
| 31 | //Point kDiff = vert0 - mCenter; | ||
| 32 | float fA00 = TriEdge0.SquareMagnitude(); | ||
| 33 | float fA01 = TriEdge0 | TriEdge1; | ||
| 34 | float fA11 = TriEdge1.SquareMagnitude(); | ||
| 35 | float fB0 = kDiff | TriEdge0; | ||
| 36 | float fB1 = kDiff | TriEdge1; | ||
| 37 | //float fC = kDiff.SquareMagnitude(); | ||
| 38 | float fDet = fabsf(fA00*fA11 - fA01*fA01); | ||
| 39 | float u = fA01*fB1-fA11*fB0; | ||
| 40 | float v = fA01*fB0-fA00*fB1; | ||
| 41 | float SqrDist; | ||
| 42 | |||
| 43 | if(u + v <= fDet) | ||
| 44 | { | ||
| 45 | if(u < 0.0f) | ||
| 46 | { | ||
| 47 | if(v < 0.0f) // region 4 | ||
| 48 | { | ||
| 49 | if(fB0 < 0.0f) | ||
| 50 | { | ||
| 51 | // v = 0.0f; | ||
| 52 | if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; } | ||
| 53 | else { u = -fB0/fA00; SqrDist = fB0*u+fC; } | ||
| 54 | } | ||
| 55 | else | ||
| 56 | { | ||
| 57 | // u = 0.0f; | ||
| 58 | if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; } | ||
| 59 | else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; } | ||
| 60 | else { v = -fB1/fA11; SqrDist = fB1*v+fC; } | ||
| 61 | } | ||
| 62 | } | ||
| 63 | else // region 3 | ||
| 64 | { | ||
| 65 | // u = 0.0f; | ||
| 66 | if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; } | ||
| 67 | else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; } | ||
| 68 | else { v = -fB1/fA11; SqrDist = fB1*v+fC; } | ||
| 69 | } | ||
| 70 | } | ||
| 71 | else if(v < 0.0f) // region 5 | ||
| 72 | { | ||
| 73 | // v = 0.0f; | ||
| 74 | if(fB0>=0.0f) { /*u = 0.0f;*/ SqrDist = fC; } | ||
| 75 | else if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; } | ||
| 76 | else { u = -fB0/fA00; SqrDist = fB0*u+fC; } | ||
| 77 | } | ||
| 78 | else // region 0 | ||
| 79 | { | ||
| 80 | // minimum at interior point | ||
| 81 | if(fDet==0.0f) | ||
| 82 | { | ||
| 83 | // u = 0.0f; | ||
| 84 | // v = 0.0f; | ||
| 85 | SqrDist = MAX_FLOAT; | ||
| 86 | } | ||
| 87 | else | ||
| 88 | { | ||
| 89 | float fInvDet = 1.0f/fDet; | ||
| 90 | u *= fInvDet; | ||
| 91 | v *= fInvDet; | ||
| 92 | SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; | ||
| 93 | } | ||
| 94 | } | ||
| 95 | } | ||
| 96 | else | ||
| 97 | { | ||
| 98 | float fTmp0, fTmp1, fNumer, fDenom; | ||
| 99 | |||
| 100 | if(u < 0.0f) // region 2 | ||
| 101 | { | ||
| 102 | fTmp0 = fA01 + fB0; | ||
| 103 | fTmp1 = fA11 + fB1; | ||
| 104 | if(fTmp1 > fTmp0) | ||
| 105 | { | ||
| 106 | fNumer = fTmp1 - fTmp0; | ||
| 107 | fDenom = fA00-2.0f*fA01+fA11; | ||
| 108 | if(fNumer >= fDenom) | ||
| 109 | { | ||
| 110 | // u = 1.0f; | ||
| 111 | // v = 0.0f; | ||
| 112 | SqrDist = fA00+2.0f*fB0+fC; | ||
| 113 | } | ||
| 114 | else | ||
| 115 | { | ||
| 116 | u = fNumer/fDenom; | ||
| 117 | v = 1.0f - u; | ||
| 118 | SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; | ||
| 119 | } | ||
| 120 | } | ||
| 121 | else | ||
| 122 | { | ||
| 123 | // u = 0.0f; | ||
| 124 | if(fTmp1 <= 0.0f) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; } | ||
| 125 | else if(fB1 >= 0.0f) { /*v = 0.0f;*/ SqrDist = fC; } | ||
| 126 | else { v = -fB1/fA11; SqrDist = fB1*v+fC; } | ||
| 127 | } | ||
| 128 | } | ||
| 129 | else if(v < 0.0f) // region 6 | ||
| 130 | { | ||
| 131 | fTmp0 = fA01 + fB1; | ||
| 132 | fTmp1 = fA00 + fB0; | ||
| 133 | if(fTmp1 > fTmp0) | ||
| 134 | { | ||
| 135 | fNumer = fTmp1 - fTmp0; | ||
| 136 | fDenom = fA00-2.0f*fA01+fA11; | ||
| 137 | if(fNumer >= fDenom) | ||
| 138 | { | ||
| 139 | // v = 1.0f; | ||
| 140 | // u = 0.0f; | ||
| 141 | SqrDist = fA11+2.0f*fB1+fC; | ||
| 142 | } | ||
| 143 | else | ||
| 144 | { | ||
| 145 | v = fNumer/fDenom; | ||
| 146 | u = 1.0f - v; | ||
| 147 | SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; | ||
| 148 | } | ||
| 149 | } | ||
| 150 | else | ||
| 151 | { | ||
| 152 | // v = 0.0f; | ||
| 153 | if(fTmp1 <= 0.0f) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; } | ||
| 154 | else if(fB0 >= 0.0f) { /*u = 0.0f;*/ SqrDist = fC; } | ||
| 155 | else { u = -fB0/fA00; SqrDist = fB0*u+fC; } | ||
| 156 | } | ||
| 157 | } | ||
| 158 | else // region 1 | ||
| 159 | { | ||
| 160 | fNumer = fA11 + fB1 - fA01 - fB0; | ||
| 161 | if(fNumer <= 0.0f) | ||
| 162 | { | ||
| 163 | // u = 0.0f; | ||
| 164 | // v = 1.0f; | ||
| 165 | SqrDist = fA11+2.0f*fB1+fC; | ||
| 166 | } | ||
| 167 | else | ||
| 168 | { | ||
| 169 | fDenom = fA00-2.0f*fA01+fA11; | ||
| 170 | if(fNumer >= fDenom) | ||
| 171 | { | ||
| 172 | // u = 1.0f; | ||
| 173 | // v = 0.0f; | ||
| 174 | SqrDist = fA00+2.0f*fB0+fC; | ||
| 175 | } | ||
| 176 | else | ||
| 177 | { | ||
| 178 | u = fNumer/fDenom; | ||
| 179 | v = 1.0f - u; | ||
| 180 | SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC; | ||
| 181 | } | ||
| 182 | } | ||
| 183 | } | ||
| 184 | } | ||
| 185 | |||
| 186 | return fabsf(SqrDist) < mRadius2; | ||
| 187 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp new file mode 100644 index 0000000..7f1a8f3 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp | |||
| @@ -0,0 +1,664 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide) | ||
| 12 | * \file OPC_SweepAndPrune.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 29, 2000 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Precompiled Header | ||
| 20 | #include "Stdafx.h" | ||
| 21 | |||
| 22 | using namespace Opcode; | ||
| 23 | |||
| 24 | inline_ void Sort(udword& id0, udword& id1) | ||
| 25 | { | ||
| 26 | if(id0>id1) Swap(id0, id1); | ||
| 27 | } | ||
| 28 | |||
| 29 | class Opcode::SAP_Element | ||
| 30 | { | ||
| 31 | public: | ||
| 32 | inline_ SAP_Element() {} | ||
| 33 | inline_ SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next) {} | ||
| 34 | inline_ ~SAP_Element() {} | ||
| 35 | |||
| 36 | udword mID; | ||
| 37 | SAP_Element* mNext; | ||
| 38 | }; | ||
| 39 | |||
| 40 | class Opcode::SAP_Box | ||
| 41 | { | ||
| 42 | public: | ||
| 43 | SAP_EndPoint* Min[3]; | ||
| 44 | SAP_EndPoint* Max[3]; | ||
| 45 | }; | ||
| 46 | |||
| 47 | class Opcode::SAP_EndPoint | ||
| 48 | { | ||
| 49 | public: | ||
| 50 | float Value; // Min or Max value | ||
| 51 | SAP_EndPoint* Previous; // Previous EndPoint whose Value is smaller than ours (or null) | ||
| 52 | SAP_EndPoint* Next; // Next EndPoint whose Value is greater than ours (or null) | ||
| 53 | udword Data; // Parent box ID *2 | MinMax flag | ||
| 54 | |||
| 55 | inline_ void SetData(udword box_id, BOOL is_max) { Data = (box_id<<1)|is_max; } | ||
| 56 | inline_ BOOL IsMax() const { return Data & 1; } | ||
| 57 | inline_ udword GetBoxID() const { return Data>>1; } | ||
| 58 | |||
| 59 | inline_ void InsertAfter(SAP_EndPoint* element) | ||
| 60 | { | ||
| 61 | if(this!=element && this!=element->Next) | ||
| 62 | { | ||
| 63 | // Remove | ||
| 64 | if(Previous) Previous->Next = Next; | ||
| 65 | if(Next) Next->Previous = Previous; | ||
| 66 | |||
| 67 | // Insert | ||
| 68 | Next = element->Next; | ||
| 69 | if(Next) Next->Previous = this; | ||
| 70 | |||
| 71 | element->Next = this; | ||
| 72 | Previous = element; | ||
| 73 | } | ||
| 74 | } | ||
| 75 | |||
| 76 | inline_ void InsertBefore(SAP_EndPoint* element) | ||
| 77 | { | ||
| 78 | if(this!=element && this!=element->Previous) | ||
| 79 | { | ||
| 80 | // Remove | ||
| 81 | if(Previous) Previous->Next = Next; | ||
| 82 | if(Next) Next->Previous = Previous; | ||
| 83 | |||
| 84 | // Insert | ||
| 85 | Previous = element->Previous; | ||
| 86 | element->Previous = this; | ||
| 87 | |||
| 88 | Next = element; | ||
| 89 | if(Previous) Previous->Next = this; | ||
| 90 | } | ||
| 91 | } | ||
| 92 | }; | ||
| 93 | |||
| 94 | |||
| 95 | |||
| 96 | |||
| 97 | |||
| 98 | |||
| 99 | |||
| 100 | |||
| 101 | |||
| 102 | |||
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 104 | /** | ||
| 105 | * Constructor. | ||
| 106 | */ | ||
| 107 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 108 | SAP_PairData::SAP_PairData() : | ||
| 109 | mNbElements (0), | ||
| 110 | mNbUsedElements (0), | ||
| 111 | mElementPool (null), | ||
| 112 | mFirstFree (null), | ||
| 113 | mNbObjects (0), | ||
| 114 | mArray (null) | ||
| 115 | { | ||
| 116 | } | ||
| 117 | |||
| 118 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 119 | /** | ||
| 120 | * Destructor. | ||
| 121 | */ | ||
| 122 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 123 | SAP_PairData::~SAP_PairData() | ||
| 124 | { | ||
| 125 | Release(); | ||
| 126 | } | ||
| 127 | |||
| 128 | void SAP_PairData::Release() | ||
| 129 | { | ||
| 130 | mNbElements = 0; | ||
| 131 | mNbUsedElements = 0; | ||
| 132 | mNbObjects = 0; | ||
| 133 | DELETEARRAY(mElementPool); | ||
| 134 | DELETEARRAY(mArray); | ||
| 135 | } | ||
| 136 | |||
| 137 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 138 | /** | ||
| 139 | * Initializes. | ||
| 140 | * \param nb_objects [in] | ||
| 141 | * \return true if success | ||
| 142 | */ | ||
| 143 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 144 | bool SAP_PairData::Init(udword nb_objects) | ||
| 145 | { | ||
| 146 | // Make sure everything has been released | ||
| 147 | Release(); | ||
| 148 | if(!nb_objects) return false; | ||
| 149 | |||
| 150 | mArray = new SAP_Element*[nb_objects]; | ||
| 151 | CHECKALLOC(mArray); | ||
| 152 | ZeroMemory(mArray, nb_objects*sizeof(SAP_Element*)); | ||
| 153 | mNbObjects = nb_objects; | ||
| 154 | |||
| 155 | return true; | ||
| 156 | } | ||
| 157 | |||
| 158 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 159 | /** | ||
| 160 | * Remaps a pointer when pool gets resized. | ||
| 161 | * \param element [in/out] remapped element | ||
| 162 | * \param delta [in] offset in bytes | ||
| 163 | */ | ||
| 164 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 165 | inline_ void Remap(SAP_Element*& element, size_t delta) | ||
| 166 | { | ||
| 167 | if(element) element = (SAP_Element*)(size_t(element) + delta); | ||
| 168 | } | ||
| 169 | |||
| 170 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 171 | /** | ||
| 172 | * Gets a free element in the pool. | ||
| 173 | * \param id [in] element id | ||
| 174 | * \param next [in] next element | ||
| 175 | * \param remap [out] possible remapping offset | ||
| 176 | * \return the new element | ||
| 177 | */ | ||
| 178 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 179 | SAP_Element* SAP_PairData::GetFreeElem(udword id, SAP_Element* next, udword* remap) | ||
| 180 | { | ||
| 181 | if(remap) *remap = 0; | ||
| 182 | |||
| 183 | SAP_Element* FreeElem; | ||
| 184 | if(mFirstFree) | ||
| 185 | { | ||
| 186 | // Recycle | ||
| 187 | FreeElem = mFirstFree; | ||
| 188 | mFirstFree = mFirstFree->mNext; // First free = next free (or null) | ||
| 189 | } | ||
| 190 | else | ||
| 191 | { | ||
| 192 | if(mNbUsedElements==mNbElements) | ||
| 193 | { | ||
| 194 | // Resize | ||
| 195 | mNbElements = mNbElements ? (mNbElements<<1) : 2; | ||
| 196 | |||
| 197 | SAP_Element* NewElems = new SAP_Element[mNbElements]; | ||
| 198 | |||
| 199 | if(mNbUsedElements) CopyMemory(NewElems, mElementPool, mNbUsedElements*sizeof(SAP_Element)); | ||
| 200 | |||
| 201 | // Remap everything | ||
| 202 | { | ||
| 203 | size_t Delta = size_t(NewElems) - size_t(mElementPool); | ||
| 204 | |||
| 205 | for(udword i=0;i<mNbUsedElements;i++) Remap(NewElems[i].mNext, Delta); | ||
| 206 | for(udword i=0;i<mNbObjects;i++) Remap(mArray[i], Delta); | ||
| 207 | |||
| 208 | Remap(mFirstFree, Delta); | ||
| 209 | Remap(next, Delta); | ||
| 210 | |||
| 211 | if(remap) *remap = Delta; | ||
| 212 | } | ||
| 213 | |||
| 214 | DELETEARRAY(mElementPool); | ||
| 215 | mElementPool = NewElems; | ||
| 216 | } | ||
| 217 | |||
| 218 | FreeElem = &mElementPool[mNbUsedElements++]; | ||
| 219 | } | ||
| 220 | |||
| 221 | FreeElem->mID = id; | ||
| 222 | FreeElem->mNext = next; | ||
| 223 | |||
| 224 | return FreeElem; | ||
| 225 | } | ||
| 226 | |||
| 227 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 228 | /** | ||
| 229 | * Frees an element of the pool. | ||
| 230 | * \param elem [in] element to free/recycle | ||
| 231 | */ | ||
| 232 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 233 | inline_ void SAP_PairData::FreeElem(SAP_Element* elem) | ||
| 234 | { | ||
| 235 | elem->mNext = mFirstFree; // Next free | ||
| 236 | mFirstFree = elem; | ||
| 237 | } | ||
| 238 | |||
| 239 | // Add a pair to the set. | ||
| 240 | void SAP_PairData::AddPair(udword id1, udword id2) | ||
| 241 | { | ||
| 242 | // Order the ids | ||
| 243 | Sort(id1, id2); | ||
| 244 | |||
| 245 | ASSERT(id1<mNbObjects); | ||
| 246 | if(id1>=mNbObjects) return; | ||
| 247 | |||
| 248 | // Select the right list from "mArray". | ||
| 249 | SAP_Element* Current = mArray[id1]; | ||
| 250 | |||
| 251 | if(!Current) | ||
| 252 | { | ||
| 253 | // Empty slot => create new element | ||
| 254 | mArray[id1] = GetFreeElem(id2, null); | ||
| 255 | } | ||
| 256 | else if(Current->mID>id2) | ||
| 257 | { | ||
| 258 | // The list is not empty but all elements are greater than id2 => insert id2 in the front. | ||
| 259 | mArray[id1] = GetFreeElem(id2, mArray[id1]); | ||
| 260 | } | ||
| 261 | else | ||
| 262 | { | ||
| 263 | // Else find the correct location in the sorted list (ascending order) and insert id2 there. | ||
| 264 | while(Current->mNext) | ||
| 265 | { | ||
| 266 | if(Current->mNext->mID > id2) break; | ||
| 267 | |||
| 268 | Current = Current->mNext; | ||
| 269 | } | ||
| 270 | |||
| 271 | if(Current->mID==id2) return; // The pair already exists | ||
| 272 | |||
| 273 | // Current->mNext = GetFreeElem(id2, Current->mNext); | ||
| 274 | udword Delta; | ||
| 275 | SAP_Element* E = GetFreeElem(id2, Current->mNext, &Delta); | ||
| 276 | if(Delta) Remap(Current, Delta); | ||
| 277 | Current->mNext = E; | ||
| 278 | } | ||
| 279 | } | ||
| 280 | |||
| 281 | // Delete a pair from the set. | ||
| 282 | void SAP_PairData::RemovePair(udword id1, udword id2) | ||
| 283 | { | ||
| 284 | // Order the ids. | ||
| 285 | Sort(id1, id2); | ||
| 286 | |||
| 287 | // Exit if the pair doesn't exist in the set | ||
| 288 | if(id1>=mNbObjects) return; | ||
| 289 | |||
| 290 | // Otherwise, select the correct list. | ||
| 291 | SAP_Element* Current = mArray[id1]; | ||
| 292 | |||
| 293 | // If this list is empty, the pair doesn't exist. | ||
| 294 | if(!Current) return; | ||
| 295 | |||
| 296 | // Otherwise, if id2 is the first element, delete it. | ||
| 297 | if(Current->mID==id2) | ||
| 298 | { | ||
| 299 | mArray[id1] = Current->mNext; | ||
| 300 | FreeElem(Current); | ||
| 301 | } | ||
| 302 | else | ||
| 303 | { | ||
| 304 | // If id2 is not the first element, start traversing the sorted list. | ||
| 305 | while(Current->mNext) | ||
| 306 | { | ||
| 307 | // If we have moved too far away without hitting id2, then the pair doesn't exist | ||
| 308 | if(Current->mNext->mID > id2) return; | ||
| 309 | |||
| 310 | // Otherwise, delete id2. | ||
| 311 | if(Current->mNext->mID == id2) | ||
| 312 | { | ||
| 313 | SAP_Element* Temp = Current->mNext; | ||
| 314 | Current->mNext = Temp->mNext; | ||
| 315 | FreeElem(Temp); | ||
| 316 | return; | ||
| 317 | } | ||
| 318 | Current = Current->mNext; | ||
| 319 | } | ||
| 320 | } | ||
| 321 | } | ||
| 322 | |||
| 323 | void SAP_PairData::DumpPairs(Pairs& pairs) const | ||
| 324 | { | ||
| 325 | // ### Ugly and slow | ||
| 326 | for(udword i=0;i<mNbObjects;i++) | ||
| 327 | { | ||
| 328 | SAP_Element* Current = mArray[i]; | ||
| 329 | while(Current) | ||
| 330 | { | ||
| 331 | ASSERT(Current->mID<mNbObjects); | ||
| 332 | |||
| 333 | pairs.AddPair(i, Current->mID); | ||
| 334 | Current = Current->mNext; | ||
| 335 | } | ||
| 336 | } | ||
| 337 | } | ||
| 338 | |||
| 339 | void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const | ||
| 340 | { | ||
| 341 | if(!callback) return; | ||
| 342 | |||
| 343 | // ### Ugly and slow | ||
| 344 | for(udword i=0;i<mNbObjects;i++) | ||
| 345 | { | ||
| 346 | SAP_Element* Current = mArray[i]; | ||
| 347 | while(Current) | ||
| 348 | { | ||
| 349 | ASSERT(Current->mID<mNbObjects); | ||
| 350 | |||
| 351 | if(!(callback)(i, Current->mID, user_data)) return; | ||
| 352 | Current = Current->mNext; | ||
| 353 | } | ||
| 354 | } | ||
| 355 | } | ||
| 356 | |||
| 357 | |||
| 358 | |||
| 359 | |||
| 360 | |||
| 361 | |||
| 362 | |||
| 363 | |||
| 364 | |||
| 365 | |||
| 366 | |||
| 367 | |||
| 368 | |||
| 369 | |||
| 370 | |||
| 371 | |||
| 372 | |||
| 373 | |||
| 374 | |||
| 375 | |||
| 376 | |||
| 377 | |||
| 378 | |||
| 379 | |||
| 380 | |||
| 381 | |||
| 382 | |||
| 383 | |||
| 384 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 385 | /** | ||
| 386 | * Constructor. | ||
| 387 | */ | ||
| 388 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 389 | SweepAndPrune::SweepAndPrune() | ||
| 390 | { | ||
| 391 | } | ||
| 392 | |||
| 393 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 394 | /** | ||
| 395 | * Destructor. | ||
| 396 | */ | ||
| 397 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 398 | SweepAndPrune::~SweepAndPrune() | ||
| 399 | { | ||
| 400 | } | ||
| 401 | |||
| 402 | void SweepAndPrune::GetPairs(Pairs& pairs) const | ||
| 403 | { | ||
| 404 | mPairs.DumpPairs(pairs); | ||
| 405 | } | ||
| 406 | |||
| 407 | void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const | ||
| 408 | { | ||
| 409 | mPairs.DumpPairs(callback, user_data); | ||
| 410 | } | ||
| 411 | |||
| 412 | bool SweepAndPrune::Init(udword nb_objects, const AABB** boxes) | ||
| 413 | { | ||
| 414 | // 1) Create sorted lists | ||
| 415 | mNbObjects = nb_objects; | ||
| 416 | |||
| 417 | mBoxes = new SAP_Box[nb_objects]; | ||
| 418 | // for(udword i=0;i<nb_objects;i++) mBoxes[i].Box = *boxes[i]; | ||
| 419 | |||
| 420 | float* Data = new float[nb_objects*2]; | ||
| 421 | |||
| 422 | for(udword Axis=0;Axis<3;Axis++) | ||
| 423 | { | ||
| 424 | mList[Axis] = new SAP_EndPoint[nb_objects*2]; | ||
| 425 | |||
| 426 | for(udword i=0;i<nb_objects;i++) | ||
| 427 | { | ||
| 428 | Data[i*2+0] = boxes[i]->GetMin(Axis); | ||
| 429 | Data[i*2+1] = boxes[i]->GetMax(Axis); | ||
| 430 | } | ||
| 431 | RadixSort RS; | ||
| 432 | const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks(); | ||
| 433 | |||
| 434 | SAP_EndPoint* PreviousEndPoint = null; | ||
| 435 | |||
| 436 | for(udword i=0;i<nb_objects*2;i++) | ||
| 437 | { | ||
| 438 | udword SortedIndex = *Sorted++; | ||
| 439 | float SortedCoord = Data[SortedIndex]; | ||
| 440 | udword BoxIndex = SortedIndex>>1; | ||
| 441 | |||
| 442 | ASSERT(BoxIndex<nb_objects); | ||
| 443 | |||
| 444 | SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex]; | ||
| 445 | CurrentEndPoint->Value = SortedCoord; | ||
| 446 | // CurrentEndPoint->IsMax = SortedIndex&1; // ### could be implicit ? | ||
| 447 | // CurrentEndPoint->ID = BoxIndex; // ### could be implicit ? | ||
| 448 | CurrentEndPoint->SetData(BoxIndex, SortedIndex&1); // ### could be implicit ? | ||
| 449 | CurrentEndPoint->Previous = PreviousEndPoint; | ||
| 450 | CurrentEndPoint->Next = null; | ||
| 451 | if(PreviousEndPoint) PreviousEndPoint->Next = CurrentEndPoint; | ||
| 452 | |||
| 453 | if(CurrentEndPoint->IsMax()) mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint; | ||
| 454 | else mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint; | ||
| 455 | |||
| 456 | PreviousEndPoint = CurrentEndPoint; | ||
| 457 | } | ||
| 458 | } | ||
| 459 | |||
| 460 | DELETEARRAY(Data); | ||
| 461 | |||
| 462 | CheckListsIntegrity(); | ||
| 463 | |||
| 464 | // 2) Quickly find starting pairs | ||
| 465 | |||
| 466 | mPairs.Init(nb_objects); | ||
| 467 | |||
| 468 | { | ||
| 469 | Pairs P; | ||
| 470 | CompleteBoxPruning(nb_objects, boxes, P, Axes(AXES_XZY)); | ||
| 471 | for(udword i=0;i<P.GetNbPairs();i++) | ||
| 472 | { | ||
| 473 | const Pair* PP = P.GetPair(i); | ||
| 474 | |||
| 475 | udword id0 = PP->id0; | ||
| 476 | udword id1 = PP->id1; | ||
| 477 | |||
| 478 | if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1])) | ||
| 479 | { | ||
| 480 | mPairs.AddPair(id0, id1); | ||
| 481 | } | ||
| 482 | else ASSERT(0); | ||
| 483 | } | ||
| 484 | } | ||
| 485 | |||
| 486 | return true; | ||
| 487 | } | ||
| 488 | |||
| 489 | bool SweepAndPrune::CheckListsIntegrity() | ||
| 490 | { | ||
| 491 | for(udword Axis=0;Axis<3;Axis++) | ||
| 492 | { | ||
| 493 | // Find list head | ||
| 494 | SAP_EndPoint* Current = mList[Axis]; | ||
| 495 | while(Current->Previous) Current = Current->Previous; | ||
| 496 | |||
| 497 | udword Nb = 0; | ||
| 498 | |||
| 499 | SAP_EndPoint* Previous = null; | ||
| 500 | while(Current) | ||
| 501 | { | ||
| 502 | Nb++; | ||
| 503 | |||
| 504 | if(Previous) | ||
| 505 | { | ||
| 506 | ASSERT(Previous->Value <= Current->Value); | ||
| 507 | if(Previous->Value > Current->Value) return false; | ||
| 508 | } | ||
| 509 | |||
| 510 | ASSERT(Current->Previous==Previous); | ||
| 511 | if(Current->Previous!=Previous) return false; | ||
| 512 | |||
| 513 | Previous = Current; | ||
| 514 | Current = Current->Next; | ||
| 515 | } | ||
| 516 | |||
| 517 | ASSERT(Nb==mNbObjects*2); | ||
| 518 | } | ||
| 519 | return true; | ||
| 520 | } | ||
| 521 | |||
| 522 | inline_ BOOL Intersect(const AABB& a, const SAP_Box& b) | ||
| 523 | { | ||
| 524 | if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value | ||
| 525 | || b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value | ||
| 526 | || b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value) return FALSE; | ||
| 527 | |||
| 528 | return TRUE; | ||
| 529 | } | ||
| 530 | |||
| 531 | |||
| 532 | |||
| 533 | bool SweepAndPrune::UpdateObject(udword i, const AABB& box) | ||
| 534 | { | ||
| 535 | for(udword Axis=0;Axis<3;Axis++) | ||
| 536 | { | ||
| 537 | // udword Base = (udword)&mList[Axis][0]; | ||
| 538 | |||
| 539 | // Update min | ||
| 540 | { | ||
| 541 | SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis]; | ||
| 542 | ASSERT(!CurrentMin->IsMax()); | ||
| 543 | |||
| 544 | const float Limit = box.GetMin(Axis); | ||
| 545 | if(Limit == CurrentMin->Value) | ||
| 546 | { | ||
| 547 | } | ||
| 548 | else if(Limit < CurrentMin->Value) | ||
| 549 | { | ||
| 550 | CurrentMin->Value = Limit; | ||
| 551 | |||
| 552 | // Min is moving left: | ||
| 553 | SAP_EndPoint* NewPos = CurrentMin; | ||
| 554 | ASSERT(NewPos); | ||
| 555 | |||
| 556 | SAP_EndPoint* tmp; | ||
| 557 | while((tmp = NewPos->Previous) && tmp->Value > Limit) | ||
| 558 | { | ||
| 559 | NewPos = tmp; | ||
| 560 | |||
| 561 | if(NewPos->IsMax()) | ||
| 562 | { | ||
| 563 | // Our min passed a max => start overlap | ||
| 564 | //udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint); | ||
| 565 | const udword id0 = CurrentMin->GetBoxID(); | ||
| 566 | const udword id1 = NewPos->GetBoxID(); | ||
| 567 | |||
| 568 | if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1); | ||
| 569 | } | ||
| 570 | } | ||
| 571 | |||
| 572 | CurrentMin->InsertBefore(NewPos); | ||
| 573 | } | ||
| 574 | else// if(Limit > CurrentMin->Value) | ||
| 575 | { | ||
| 576 | CurrentMin->Value = Limit; | ||
| 577 | |||
| 578 | // Min is moving right: | ||
| 579 | SAP_EndPoint* NewPos = CurrentMin; | ||
| 580 | ASSERT(NewPos); | ||
| 581 | |||
| 582 | SAP_EndPoint* tmp; | ||
| 583 | while((tmp = NewPos->Next) && tmp->Value < Limit) | ||
| 584 | { | ||
| 585 | NewPos = tmp; | ||
| 586 | |||
| 587 | if(NewPos->IsMax()) | ||
| 588 | { | ||
| 589 | // Our min passed a max => stop overlap | ||
| 590 | const udword id0 = CurrentMin->GetBoxID(); | ||
| 591 | const udword id1 = NewPos->GetBoxID(); | ||
| 592 | |||
| 593 | if(id0!=id1) mPairs.RemovePair(id0, id1); | ||
| 594 | } | ||
| 595 | } | ||
| 596 | |||
| 597 | CurrentMin->InsertAfter(NewPos); | ||
| 598 | } | ||
| 599 | } | ||
| 600 | |||
| 601 | // Update max | ||
| 602 | { | ||
| 603 | SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis]; | ||
| 604 | ASSERT(CurrentMax->IsMax()); | ||
| 605 | |||
| 606 | const float Limit = box.GetMax(Axis); | ||
| 607 | if(Limit == CurrentMax->Value) | ||
| 608 | { | ||
| 609 | } | ||
| 610 | else if(Limit > CurrentMax->Value) | ||
| 611 | { | ||
| 612 | CurrentMax->Value = Limit; | ||
| 613 | |||
| 614 | // Max is moving right: | ||
| 615 | SAP_EndPoint* NewPos = CurrentMax; | ||
| 616 | ASSERT(NewPos); | ||
| 617 | |||
| 618 | SAP_EndPoint* tmp; | ||
| 619 | while((tmp = NewPos->Next) && tmp->Value < Limit) | ||
| 620 | { | ||
| 621 | NewPos = tmp; | ||
| 622 | |||
| 623 | if(!NewPos->IsMax()) | ||
| 624 | { | ||
| 625 | // Our max passed a min => start overlap | ||
| 626 | const udword id0 = CurrentMax->GetBoxID(); | ||
| 627 | const udword id1 = NewPos->GetBoxID(); | ||
| 628 | |||
| 629 | if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1); | ||
| 630 | } | ||
| 631 | } | ||
| 632 | |||
| 633 | CurrentMax->InsertAfter(NewPos); | ||
| 634 | } | ||
| 635 | else// if(Limit < CurrentMax->Value) | ||
| 636 | { | ||
| 637 | CurrentMax->Value = Limit; | ||
| 638 | |||
| 639 | // Max is moving left: | ||
| 640 | SAP_EndPoint* NewPos = CurrentMax; | ||
| 641 | ASSERT(NewPos); | ||
| 642 | |||
| 643 | SAP_EndPoint* tmp; | ||
| 644 | while((tmp = NewPos->Previous) && tmp->Value > Limit) | ||
| 645 | { | ||
| 646 | NewPos = tmp; | ||
| 647 | |||
| 648 | if(!NewPos->IsMax()) | ||
| 649 | { | ||
| 650 | // Our max passed a min => stop overlap | ||
| 651 | const udword id0 = CurrentMax->GetBoxID(); | ||
| 652 | const udword id1 = NewPos->GetBoxID(); | ||
| 653 | |||
| 654 | if(id0!=id1) mPairs.RemovePair(id0, id1); | ||
| 655 | } | ||
| 656 | } | ||
| 657 | |||
| 658 | CurrentMax->InsertBefore(NewPos); | ||
| 659 | } | ||
| 660 | } | ||
| 661 | } | ||
| 662 | |||
| 663 | return true; | ||
| 664 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.h b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.h new file mode 100644 index 0000000..2cbbb7e --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.h | |||
| @@ -0,0 +1,86 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide) | ||
| 12 | * \file OPC_SweepAndPrune.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date January, 29, 2000 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_SWEEPANDPRUNE_H__ | ||
| 21 | #define __OPC_SWEEPANDPRUNE_H__ | ||
| 22 | |||
| 23 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 24 | /** | ||
| 25 | * User-callback, called by OPCODE for each colliding pairs. | ||
| 26 | * \param id0 [in] id of colliding object | ||
| 27 | * \param id1 [in] id of colliding object | ||
| 28 | * \param user_data [in] user-defined data | ||
| 29 | * \return TRUE to continue enumeration | ||
| 30 | */ | ||
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 32 | typedef BOOL (*PairCallback) (udword id0, udword id1, void* user_data); | ||
| 33 | |||
| 34 | class SAP_Element; | ||
| 35 | class SAP_EndPoint; | ||
| 36 | class SAP_Box; | ||
| 37 | |||
| 38 | class OPCODE_API SAP_PairData | ||
| 39 | { | ||
| 40 | public: | ||
| 41 | SAP_PairData(); | ||
| 42 | ~SAP_PairData(); | ||
| 43 | |||
| 44 | bool Init(udword nb_objects); | ||
| 45 | |||
| 46 | void AddPair(udword id1, udword id2); | ||
| 47 | void RemovePair(udword id1, udword id2); | ||
| 48 | |||
| 49 | void DumpPairs(Pairs& pairs) const; | ||
| 50 | void DumpPairs(PairCallback callback, void* user_data) const; | ||
| 51 | private: | ||
| 52 | udword mNbElements; //!< Total number of elements in the pool | ||
| 53 | udword mNbUsedElements; //!< Number of used elements | ||
| 54 | SAP_Element* mElementPool; //!< Array of mNbElements elements | ||
| 55 | SAP_Element* mFirstFree; //!< First free element in the pool | ||
| 56 | |||
| 57 | udword mNbObjects; //!< Max number of objects we can handle | ||
| 58 | SAP_Element** mArray; //!< Pointers to pool | ||
| 59 | // Internal methods | ||
| 60 | SAP_Element* GetFreeElem(udword id, SAP_Element* next, udword* remap=null); | ||
| 61 | inline_ void FreeElem(SAP_Element* elem); | ||
| 62 | void Release(); | ||
| 63 | }; | ||
| 64 | |||
| 65 | class OPCODE_API SweepAndPrune | ||
| 66 | { | ||
| 67 | public: | ||
| 68 | SweepAndPrune(); | ||
| 69 | ~SweepAndPrune(); | ||
| 70 | |||
| 71 | bool Init(udword nb_objects, const AABB** boxes); | ||
| 72 | bool UpdateObject(udword i, const AABB& box); | ||
| 73 | |||
| 74 | void GetPairs(Pairs& pairs) const; | ||
| 75 | void GetPairs(PairCallback callback, void* user_data) const; | ||
| 76 | private: | ||
| 77 | SAP_PairData mPairs; | ||
| 78 | |||
| 79 | udword mNbObjects; | ||
| 80 | SAP_Box* mBoxes; | ||
| 81 | SAP_EndPoint* mList[3]; | ||
| 82 | // Internal methods | ||
| 83 | bool CheckListsIntegrity(); | ||
| 84 | }; | ||
| 85 | |||
| 86 | #endif //__OPC_SWEEPANDPRUNE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_TreeBuilders.cpp b/libraries/ode-0.9/OPCODE/OPC_TreeBuilders.cpp new file mode 100644 index 0000000..d2359a0 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_TreeBuilders.cpp | |||
| @@ -0,0 +1,255 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for tree builders. | ||
| 12 | * \file OPC_TreeBuilders.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * A builder for AABB-trees of vertices. | ||
| 21 | * | ||
| 22 | * \class AABBTreeOfVerticesBuilder | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date March, 20, 2001 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | /** | ||
| 31 | * A builder for AABB-trees of AABBs. | ||
| 32 | * | ||
| 33 | * \class AABBTreeOfAABBsBuilder | ||
| 34 | * \author Pierre Terdiman | ||
| 35 | * \version 1.3 | ||
| 36 | * \date March, 20, 2001 | ||
| 37 | */ | ||
| 38 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 39 | |||
| 40 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 41 | /** | ||
| 42 | * A builder for AABB-trees of triangles. | ||
| 43 | * | ||
| 44 | * \class AABBTreeOfTrianglesBuilder | ||
| 45 | * \author Pierre Terdiman | ||
| 46 | * \version 1.3 | ||
| 47 | * \date March, 20, 2001 | ||
| 48 | */ | ||
| 49 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 50 | |||
| 51 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 52 | // Precompiled Header | ||
| 53 | #include "Stdafx.h" | ||
| 54 | |||
| 55 | using namespace Opcode; | ||
| 56 | |||
| 57 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 58 | /** | ||
| 59 | * Computes the AABB of a set of primitives. | ||
| 60 | * \param primitives [in] list of indices of primitives | ||
| 61 | * \param nb_prims [in] number of indices | ||
| 62 | * \param global_box [out] global AABB enclosing the set of input primitives | ||
| 63 | * \return true if success | ||
| 64 | */ | ||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | bool AABBTreeOfAABBsBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const | ||
| 67 | { | ||
| 68 | // Checkings | ||
| 69 | if(!primitives || !nb_prims) return false; | ||
| 70 | |||
| 71 | // Initialize global box | ||
| 72 | global_box = mAABBArray[primitives[0]]; | ||
| 73 | |||
| 74 | // Loop through boxes | ||
| 75 | for(udword i=1;i<nb_prims;i++) | ||
| 76 | { | ||
| 77 | // Update global box | ||
| 78 | global_box.Add(mAABBArray[primitives[i]]); | ||
| 79 | } | ||
| 80 | return true; | ||
| 81 | } | ||
| 82 | |||
| 83 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 84 | /** | ||
| 85 | * Computes the splitting value along a given axis for a given primitive. | ||
| 86 | * \param index [in] index of the primitive to split | ||
| 87 | * \param axis [in] axis index (0,1,2) | ||
| 88 | * \return splitting value | ||
| 89 | */ | ||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | float AABBTreeOfAABBsBuilder::GetSplittingValue(udword index, udword axis) const | ||
| 92 | { | ||
| 93 | // For an AABB, the splitting value is the middle of the given axis, | ||
| 94 | // i.e. the corresponding component of the center point | ||
| 95 | return mAABBArray[index].GetCenter(axis); | ||
| 96 | } | ||
| 97 | |||
| 98 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 99 | /** | ||
| 100 | * Computes the AABB of a set of primitives. | ||
| 101 | * \param primitives [in] list of indices of primitives | ||
| 102 | * \param nb_prims [in] number of indices | ||
| 103 | * \param global_box [out] global AABB enclosing the set of input primitives | ||
| 104 | * \return true if success | ||
| 105 | */ | ||
| 106 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 107 | bool AABBTreeOfTrianglesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const | ||
| 108 | { | ||
| 109 | // Checkings | ||
| 110 | if(!primitives || !nb_prims) return false; | ||
| 111 | |||
| 112 | // Initialize global box | ||
| 113 | Point Min(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT); | ||
| 114 | Point Max(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); | ||
| 115 | |||
| 116 | // Loop through triangles | ||
| 117 | VertexPointers VP; | ||
| 118 | while(nb_prims--) | ||
| 119 | { | ||
| 120 | // Get current triangle-vertices | ||
| 121 | mIMesh->GetTriangle(VP, *primitives++); | ||
| 122 | // Update global box | ||
| 123 | Min.Min(*VP.Vertex[0]).Min(*VP.Vertex[1]).Min(*VP.Vertex[2]); | ||
| 124 | Max.Max(*VP.Vertex[0]).Max(*VP.Vertex[1]).Max(*VP.Vertex[2]); | ||
| 125 | } | ||
| 126 | global_box.SetMinMax(Min, Max); | ||
| 127 | return true; | ||
| 128 | } | ||
| 129 | |||
| 130 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 131 | /** | ||
| 132 | * Computes the splitting value along a given axis for a given primitive. | ||
| 133 | * \param index [in] index of the primitive to split | ||
| 134 | * \param axis [in] axis index (0,1,2) | ||
| 135 | * \return splitting value | ||
| 136 | */ | ||
| 137 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 138 | float AABBTreeOfTrianglesBuilder::GetSplittingValue(udword index, udword axis) const | ||
| 139 | { | ||
| 140 | /* // Compute center of triangle | ||
| 141 | Point Center; | ||
| 142 | mTriList[index].Center(mVerts, Center); | ||
| 143 | // Return value | ||
| 144 | return Center[axis];*/ | ||
| 145 | |||
| 146 | // Compute correct component from center of triangle | ||
| 147 | // return (mVerts[mTriList[index].mVRef[0]][axis] | ||
| 148 | // +mVerts[mTriList[index].mVRef[1]][axis] | ||
| 149 | // +mVerts[mTriList[index].mVRef[2]][axis])*INV3; | ||
| 150 | |||
| 151 | VertexPointers VP; | ||
| 152 | mIMesh->GetTriangle(VP, index); | ||
| 153 | |||
| 154 | // Compute correct component from center of triangle | ||
| 155 | return ((*VP.Vertex[0])[axis] | ||
| 156 | +(*VP.Vertex[1])[axis] | ||
| 157 | +(*VP.Vertex[2])[axis])*INV3; | ||
| 158 | } | ||
| 159 | |||
| 160 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 161 | /** | ||
| 162 | * Computes the splitting value along a given axis for a given node. | ||
| 163 | * \param primitives [in] list of indices of primitives | ||
| 164 | * \param nb_prims [in] number of indices | ||
| 165 | * \param global_box [in] global AABB enclosing the set of input primitives | ||
| 166 | * \param axis [in] axis index (0,1,2) | ||
| 167 | * \return splitting value | ||
| 168 | */ | ||
| 169 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 170 | float AABBTreeOfTrianglesBuilder::GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const | ||
| 171 | { | ||
| 172 | if(mSettings.mRules&SPLIT_GEOM_CENTER) | ||
| 173 | { | ||
| 174 | // Loop through triangles | ||
| 175 | float SplitValue = 0.0f; | ||
| 176 | VertexPointers VP; | ||
| 177 | for(udword i=0;i<nb_prims;i++) | ||
| 178 | { | ||
| 179 | // Get current triangle-vertices | ||
| 180 | mIMesh->GetTriangle(VP, primitives[i]); | ||
| 181 | // Update split value | ||
| 182 | SplitValue += (*VP.Vertex[0])[axis]; | ||
| 183 | SplitValue += (*VP.Vertex[1])[axis]; | ||
| 184 | SplitValue += (*VP.Vertex[2])[axis]; | ||
| 185 | } | ||
| 186 | return SplitValue / float(nb_prims*3); | ||
| 187 | } | ||
| 188 | else return AABBTreeBuilder::GetSplittingValue(primitives, nb_prims, global_box, axis); | ||
| 189 | } | ||
| 190 | |||
| 191 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 192 | /** | ||
| 193 | * Computes the AABB of a set of primitives. | ||
| 194 | * \param primitives [in] list of indices of primitives | ||
| 195 | * \param nb_prims [in] number of indices | ||
| 196 | * \param global_box [out] global AABB enclosing the set of input primitives | ||
| 197 | * \return true if success | ||
| 198 | */ | ||
| 199 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 200 | bool AABBTreeOfVerticesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const | ||
| 201 | { | ||
| 202 | // Checkings | ||
| 203 | if(!primitives || !nb_prims) return false; | ||
| 204 | |||
| 205 | // Initialize global box | ||
| 206 | global_box.SetEmpty(); | ||
| 207 | |||
| 208 | // Loop through vertices | ||
| 209 | for(udword i=0;i<nb_prims;i++) | ||
| 210 | { | ||
| 211 | // Update global box | ||
| 212 | global_box.Extend(mVertexArray[primitives[i]]); | ||
| 213 | } | ||
| 214 | return true; | ||
| 215 | } | ||
| 216 | |||
| 217 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 218 | /** | ||
| 219 | * Computes the splitting value along a given axis for a given primitive. | ||
| 220 | * \param index [in] index of the primitive to split | ||
| 221 | * \param axis [in] axis index (0,1,2) | ||
| 222 | * \return splitting value | ||
| 223 | */ | ||
| 224 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 225 | float AABBTreeOfVerticesBuilder::GetSplittingValue(udword index, udword axis) const | ||
| 226 | { | ||
| 227 | // For a vertex, the splitting value is simply the vertex coordinate. | ||
| 228 | return mVertexArray[index][axis]; | ||
| 229 | } | ||
| 230 | |||
| 231 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 232 | /** | ||
| 233 | * Computes the splitting value along a given axis for a given node. | ||
| 234 | * \param primitives [in] list of indices of primitives | ||
| 235 | * \param nb_prims [in] number of indices | ||
| 236 | * \param global_box [in] global AABB enclosing the set of input primitives | ||
| 237 | * \param axis [in] axis index (0,1,2) | ||
| 238 | * \return splitting value | ||
| 239 | */ | ||
| 240 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 241 | float AABBTreeOfVerticesBuilder::GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const | ||
| 242 | { | ||
| 243 | if(mSettings.mRules&SPLIT_GEOM_CENTER) | ||
| 244 | { | ||
| 245 | // Loop through vertices | ||
| 246 | float SplitValue = 0.0f; | ||
| 247 | for(udword i=0;i<nb_prims;i++) | ||
| 248 | { | ||
| 249 | // Update split value | ||
| 250 | SplitValue += mVertexArray[primitives[i]][axis]; | ||
| 251 | } | ||
| 252 | return SplitValue / float(nb_prims); | ||
| 253 | } | ||
| 254 | else return AABBTreeBuilder::GetSplittingValue(primitives, nb_prims, global_box, axis); | ||
| 255 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_TreeBuilders.h b/libraries/ode-0.9/OPCODE/OPC_TreeBuilders.h new file mode 100644 index 0000000..b9a2a98 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_TreeBuilders.h | |||
| @@ -0,0 +1,173 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for tree builders. | ||
| 12 | * \file OPC_TreeBuilders.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_TREEBUILDERS_H__ | ||
| 21 | #define __OPC_TREEBUILDERS_H__ | ||
| 22 | |||
| 23 | //! Tree splitting rules | ||
| 24 | enum SplittingRules | ||
| 25 | { | ||
| 26 | // Primitive split | ||
| 27 | SPLIT_LARGEST_AXIS = (1<<0), //!< Split along the largest axis | ||
| 28 | SPLIT_SPLATTER_POINTS = (1<<1), //!< Splatter primitive centers (QuickCD-style) | ||
| 29 | SPLIT_BEST_AXIS = (1<<2), //!< Try largest axis, then second, then last | ||
| 30 | SPLIT_BALANCED = (1<<3), //!< Try to keep a well-balanced tree | ||
| 31 | SPLIT_FIFTY = (1<<4), //!< Arbitrary 50-50 split | ||
| 32 | // Node split | ||
| 33 | SPLIT_GEOM_CENTER = (1<<5), //!< Split at geometric center (else split in the middle) | ||
| 34 | // | ||
| 35 | SPLIT_FORCE_DWORD = 0x7fffffff | ||
| 36 | }; | ||
| 37 | |||
| 38 | //! Simple wrapper around build-related settings [Opcode 1.3] | ||
| 39 | struct OPCODE_API BuildSettings | ||
| 40 | { | ||
| 41 | inline_ BuildSettings() : mLimit(1), mRules(SPLIT_FORCE_DWORD) {} | ||
| 42 | |||
| 43 | udword mLimit; //!< Limit number of primitives / node. If limit is 1, build a complete tree (2*N-1 nodes) | ||
| 44 | udword mRules; //!< Building/Splitting rules (a combination of SplittingRules flags) | ||
| 45 | }; | ||
| 46 | |||
| 47 | class OPCODE_API AABBTreeBuilder | ||
| 48 | { | ||
| 49 | public: | ||
| 50 | //! Constructor | ||
| 51 | AABBTreeBuilder() : | ||
| 52 | mNbPrimitives(0), | ||
| 53 | mNodeBase(null), | ||
| 54 | mCount(0), | ||
| 55 | mNbInvalidSplits(0) {} | ||
| 56 | //! Destructor | ||
| 57 | virtual ~AABBTreeBuilder() {} | ||
| 58 | |||
| 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 60 | /** | ||
| 61 | * Computes the AABB of a set of primitives. | ||
| 62 | * \param primitives [in] list of indices of primitives | ||
| 63 | * \param nb_prims [in] number of indices | ||
| 64 | * \param global_box [out] global AABB enclosing the set of input primitives | ||
| 65 | * \return true if success | ||
| 66 | */ | ||
| 67 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 68 | virtual bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const = 0; | ||
| 69 | |||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | /** | ||
| 72 | * Computes the splitting value along a given axis for a given primitive. | ||
| 73 | * \param index [in] index of the primitive to split | ||
| 74 | * \param axis [in] axis index (0,1,2) | ||
| 75 | * \return splitting value | ||
| 76 | */ | ||
| 77 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 78 | virtual float GetSplittingValue(udword index, udword axis) const = 0; | ||
| 79 | |||
| 80 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 81 | /** | ||
| 82 | * Computes the splitting value along a given axis for a given node. | ||
| 83 | * \param primitives [in] list of indices of primitives | ||
| 84 | * \param nb_prims [in] number of indices | ||
| 85 | * \param global_box [in] global AABB enclosing the set of input primitives | ||
| 86 | * \param axis [in] axis index (0,1,2) | ||
| 87 | * \return splitting value | ||
| 88 | */ | ||
| 89 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 90 | virtual float GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const | ||
| 91 | { | ||
| 92 | // Default split value = middle of the axis (using only the box) | ||
| 93 | return global_box.GetCenter(axis); | ||
| 94 | } | ||
| 95 | |||
| 96 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 97 | /** | ||
| 98 | * Validates node subdivision. This is called each time a node is considered for subdivision, during tree building. | ||
| 99 | * \param primitives [in] list of indices of primitives | ||
| 100 | * \param nb_prims [in] number of indices | ||
| 101 | * \param global_box [in] global AABB enclosing the set of input primitives | ||
| 102 | * \return TRUE if the node should be subdivised | ||
| 103 | */ | ||
| 104 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 105 | virtual BOOL ValidateSubdivision(const udword* primitives, udword nb_prims, const AABB& global_box) | ||
| 106 | { | ||
| 107 | // Check the user-defined limit | ||
| 108 | if(nb_prims<=mSettings.mLimit) return FALSE; | ||
| 109 | |||
| 110 | return TRUE; | ||
| 111 | } | ||
| 112 | |||
| 113 | BuildSettings mSettings; //!< Splitting rules & split limit [Opcode 1.3] | ||
| 114 | udword mNbPrimitives; //!< Total number of primitives. | ||
| 115 | void* mNodeBase; //!< Address of node pool [Opcode 1.3] | ||
| 116 | // Stats | ||
| 117 | inline_ void SetCount(udword nb) { mCount=nb; } | ||
| 118 | inline_ void IncreaseCount(udword nb) { mCount+=nb; } | ||
| 119 | inline_ udword GetCount() const { return mCount; } | ||
| 120 | inline_ void SetNbInvalidSplits(udword nb) { mNbInvalidSplits=nb; } | ||
| 121 | inline_ void IncreaseNbInvalidSplits() { mNbInvalidSplits++; } | ||
| 122 | inline_ udword GetNbInvalidSplits() const { return mNbInvalidSplits; } | ||
| 123 | |||
| 124 | private: | ||
| 125 | udword mCount; //!< Stats: number of nodes created | ||
| 126 | udword mNbInvalidSplits; //!< Stats: number of invalid splits | ||
| 127 | }; | ||
| 128 | |||
| 129 | class OPCODE_API AABBTreeOfVerticesBuilder : public AABBTreeBuilder | ||
| 130 | { | ||
| 131 | public: | ||
| 132 | //! Constructor | ||
| 133 | AABBTreeOfVerticesBuilder() : mVertexArray(null) {} | ||
| 134 | //! Destructor | ||
| 135 | virtual ~AABBTreeOfVerticesBuilder() {} | ||
| 136 | |||
| 137 | override(AABBTreeBuilder) bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const; | ||
| 138 | override(AABBTreeBuilder) float GetSplittingValue(udword index, udword axis) const; | ||
| 139 | override(AABBTreeBuilder) float GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const; | ||
| 140 | |||
| 141 | const Point* mVertexArray; //!< Shortcut to an app-controlled array of vertices. | ||
| 142 | }; | ||
| 143 | |||
| 144 | class OPCODE_API AABBTreeOfAABBsBuilder : public AABBTreeBuilder | ||
| 145 | { | ||
| 146 | public: | ||
| 147 | //! Constructor | ||
| 148 | AABBTreeOfAABBsBuilder() : mAABBArray(null) {} | ||
| 149 | //! Destructor | ||
| 150 | virtual ~AABBTreeOfAABBsBuilder() {} | ||
| 151 | |||
| 152 | override(AABBTreeBuilder) bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const; | ||
| 153 | override(AABBTreeBuilder) float GetSplittingValue(udword index, udword axis) const; | ||
| 154 | |||
| 155 | const AABB* mAABBArray; //!< Shortcut to an app-controlled array of AABBs. | ||
| 156 | }; | ||
| 157 | |||
| 158 | class OPCODE_API AABBTreeOfTrianglesBuilder : public AABBTreeBuilder | ||
| 159 | { | ||
| 160 | public: | ||
| 161 | //! Constructor | ||
| 162 | AABBTreeOfTrianglesBuilder() : mIMesh(null) {} | ||
| 163 | //! Destructor | ||
| 164 | virtual ~AABBTreeOfTrianglesBuilder() {} | ||
| 165 | |||
| 166 | override(AABBTreeBuilder) bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const; | ||
| 167 | override(AABBTreeBuilder) float GetSplittingValue(udword index, udword axis) const; | ||
| 168 | override(AABBTreeBuilder) float GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const; | ||
| 169 | |||
| 170 | const MeshInterface* mIMesh; //!< Shortcut to an app-controlled mesh interface | ||
| 171 | }; | ||
| 172 | |||
| 173 | #endif // __OPC_TREEBUILDERS_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp new file mode 100644 index 0000000..f21b39b --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp | |||
| @@ -0,0 +1,943 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a tree collider. | ||
| 12 | * \file OPC_TreeCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains an AABB tree collider. | ||
| 21 | * This class performs a collision test between two AABB trees. | ||
| 22 | * | ||
| 23 | * \class AABBTreeCollider | ||
| 24 | * \author Pierre Terdiman | ||
| 25 | * \version 1.3 | ||
| 26 | * \date March, 20, 2001 | ||
| 27 | */ | ||
| 28 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 29 | |||
| 30 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 31 | // Precompiled Header | ||
| 32 | #include "Stdafx.h" | ||
| 33 | |||
| 34 | using namespace Opcode; | ||
| 35 | |||
| 36 | #include "OPC_BoxBoxOverlap.h" | ||
| 37 | #include "OPC_TriBoxOverlap.h" | ||
| 38 | #include "OPC_TriTriOverlap.h" | ||
| 39 | |||
| 40 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 41 | /** | ||
| 42 | * Constructor. | ||
| 43 | */ | ||
| 44 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 45 | AABBTreeCollider::AABBTreeCollider() : | ||
| 46 | mNbBVBVTests (0), | ||
| 47 | mNbPrimPrimTests (0), | ||
| 48 | mNbBVPrimTests (0), | ||
| 49 | mFullBoxBoxTest (true), | ||
| 50 | mFullPrimBoxTest (true), | ||
| 51 | mIMesh0 (null), | ||
| 52 | mIMesh1 (null) | ||
| 53 | { | ||
| 54 | } | ||
| 55 | |||
| 56 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 57 | /** | ||
| 58 | * Destructor. | ||
| 59 | */ | ||
| 60 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 61 | AABBTreeCollider::~AABBTreeCollider() | ||
| 62 | { | ||
| 63 | } | ||
| 64 | |||
| 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 66 | /** | ||
| 67 | * Validates current settings. You should call this method after all the settings and callbacks have been defined. | ||
| 68 | * \return null if everything is ok, else a string describing the problem | ||
| 69 | */ | ||
| 70 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 71 | const char* AABBTreeCollider::ValidateSettings() | ||
| 72 | { | ||
| 73 | if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; | ||
| 74 | return null; | ||
| 75 | } | ||
| 76 | |||
| 77 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 78 | /** | ||
| 79 | * Generic collision query for generic OPCODE models. After the call, access the results with: | ||
| 80 | * - GetContactStatus() | ||
| 81 | * - GetNbPairs() | ||
| 82 | * - GetPairs() | ||
| 83 | * | ||
| 84 | * \param cache [in] collision cache for model pointers and a colliding pair of primitives | ||
| 85 | * \param world0 [in] world matrix for first object | ||
| 86 | * \param world1 [in] world matrix for second object | ||
| 87 | * \return true if success | ||
| 88 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 89 | */ | ||
| 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 91 | bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1) | ||
| 92 | { | ||
| 93 | // Checkings | ||
| 94 | if(!cache.Model0 || !cache.Model1) return false; | ||
| 95 | if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false; | ||
| 96 | if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false; | ||
| 97 | |||
| 98 | /* | ||
| 99 | |||
| 100 | Rules: | ||
| 101 | - perform hull test | ||
| 102 | - when hulls collide, disable hull test | ||
| 103 | - if meshes overlap, reset countdown | ||
| 104 | - if countdown reaches 0, enable hull test | ||
| 105 | |||
| 106 | */ | ||
| 107 | |||
| 108 | #ifdef __MESHMERIZER_H__ | ||
| 109 | // Handle hulls | ||
| 110 | if(cache.HullTest) | ||
| 111 | { | ||
| 112 | if(cache.Model0->GetHull() && cache.Model1->GetHull()) | ||
| 113 | { | ||
| 114 | struct Local | ||
| 115 | { | ||
| 116 | static Point* SVCallback(const Point& sv, udword& previndex, udword user_data) | ||
| 117 | { | ||
| 118 | CollisionHull* Hull = (CollisionHull*)user_data; | ||
| 119 | previndex = Hull->ComputeSupportingVertex(sv, previndex); | ||
| 120 | return (Point*)&Hull->GetVerts()[previndex]; | ||
| 121 | } | ||
| 122 | }; | ||
| 123 | |||
| 124 | bool Collide; | ||
| 125 | |||
| 126 | if(0) | ||
| 127 | { | ||
| 128 | static GJKEngine GJK; | ||
| 129 | static bool GJKInitDone=false; | ||
| 130 | if(!GJKInitDone) | ||
| 131 | { | ||
| 132 | GJK.Enable(GJK_BACKUP_PROCEDURE); | ||
| 133 | GJK.Enable(GJK_DEGENERATE); | ||
| 134 | GJK.Enable(GJK_HILLCLIMBING); | ||
| 135 | GJKInitDone = true; | ||
| 136 | } | ||
| 137 | GJK.SetCallbackObj0(Local::SVCallback); | ||
| 138 | GJK.SetCallbackObj1(Local::SVCallback); | ||
| 139 | GJK.SetUserData0(udword(cache.Model0->GetHull())); | ||
| 140 | GJK.SetUserData1(udword(cache.Model1->GetHull())); | ||
| 141 | Collide = GJK.Collide(*world0, *world1, &cache.SepVector); | ||
| 142 | } | ||
| 143 | else | ||
| 144 | { | ||
| 145 | static SVEngine SVE; | ||
| 146 | SVE.SetCallbackObj0(Local::SVCallback); | ||
| 147 | SVE.SetCallbackObj1(Local::SVCallback); | ||
| 148 | SVE.SetUserData0(udword(cache.Model0->GetHull())); | ||
| 149 | SVE.SetUserData1(udword(cache.Model1->GetHull())); | ||
| 150 | Collide = SVE.Collide(*world0, *world1, &cache.SepVector); | ||
| 151 | } | ||
| 152 | |||
| 153 | if(!Collide) | ||
| 154 | { | ||
| 155 | // Reset stats & contact status | ||
| 156 | mFlags &= ~OPC_CONTACT; | ||
| 157 | mNbBVBVTests = 0; | ||
| 158 | mNbPrimPrimTests = 0; | ||
| 159 | mNbBVPrimTests = 0; | ||
| 160 | mPairs.Reset(); | ||
| 161 | return true; | ||
| 162 | } | ||
| 163 | } | ||
| 164 | } | ||
| 165 | |||
| 166 | // Here, hulls collide | ||
| 167 | cache.HullTest = false; | ||
| 168 | #endif // __MESHMERIZER_H__ | ||
| 169 | |||
| 170 | // Checkings | ||
| 171 | if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false; | ||
| 172 | |||
| 173 | // Simple double-dispatch | ||
| 174 | bool Status; | ||
| 175 | if(!cache.Model0->HasLeafNodes()) | ||
| 176 | { | ||
| 177 | if(cache.Model0->IsQuantized()) | ||
| 178 | { | ||
| 179 | const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree(); | ||
| 180 | const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree(); | ||
| 181 | Status = Collide(T0, T1, world0, world1, &cache); | ||
| 182 | } | ||
| 183 | else | ||
| 184 | { | ||
| 185 | const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree(); | ||
| 186 | const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree(); | ||
| 187 | Status = Collide(T0, T1, world0, world1, &cache); | ||
| 188 | } | ||
| 189 | } | ||
| 190 | else | ||
| 191 | { | ||
| 192 | if(cache.Model0->IsQuantized()) | ||
| 193 | { | ||
| 194 | const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree(); | ||
| 195 | const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree(); | ||
| 196 | Status = Collide(T0, T1, world0, world1, &cache); | ||
| 197 | } | ||
| 198 | else | ||
| 199 | { | ||
| 200 | const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree(); | ||
| 201 | const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree(); | ||
| 202 | Status = Collide(T0, T1, world0, world1, &cache); | ||
| 203 | } | ||
| 204 | } | ||
| 205 | |||
| 206 | #ifdef __MESHMERIZER_H__ | ||
| 207 | if(Status) | ||
| 208 | { | ||
| 209 | // Reset counter as long as overlap occurs | ||
| 210 | if(GetContactStatus()) cache.ResetCountDown(); | ||
| 211 | |||
| 212 | // Enable hull test again when counter reaches zero | ||
| 213 | cache.CountDown--; | ||
| 214 | if(!cache.CountDown) | ||
| 215 | { | ||
| 216 | cache.ResetCountDown(); | ||
| 217 | cache.HullTest = true; | ||
| 218 | } | ||
| 219 | } | ||
| 220 | #endif | ||
| 221 | return Status; | ||
| 222 | } | ||
| 223 | |||
| 224 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 225 | /** | ||
| 226 | * Initializes a collision query : | ||
| 227 | * - reset stats & contact status | ||
| 228 | * - setup matrices | ||
| 229 | * | ||
| 230 | * \param world0 [in] world matrix for first object | ||
| 231 | * \param world1 [in] world matrix for second object | ||
| 232 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 233 | */ | ||
| 234 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 235 | void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1) | ||
| 236 | { | ||
| 237 | // Reset stats & contact status | ||
| 238 | Collider::InitQuery(); | ||
| 239 | mNbBVBVTests = 0; | ||
| 240 | mNbPrimPrimTests = 0; | ||
| 241 | mNbBVPrimTests = 0; | ||
| 242 | mPairs.Reset(); | ||
| 243 | |||
| 244 | // Setup matrices | ||
| 245 | Matrix4x4 InvWorld0, InvWorld1; | ||
| 246 | if(world0) InvertPRMatrix(InvWorld0, *world0); | ||
| 247 | else InvWorld0.Identity(); | ||
| 248 | |||
| 249 | if(world1) InvertPRMatrix(InvWorld1, *world1); | ||
| 250 | else InvWorld1.Identity(); | ||
| 251 | |||
| 252 | Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1; | ||
| 253 | Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0; | ||
| 254 | |||
| 255 | mR0to1 = World0to1; World0to1.GetTrans(mT0to1); | ||
| 256 | mR1to0 = World1to0; World1to0.GetTrans(mT1to0); | ||
| 257 | |||
| 258 | // Precompute absolute 1-to-0 rotation matrix | ||
| 259 | for(udword i=0;i<3;i++) | ||
| 260 | { | ||
| 261 | for(udword j=0;j<3;j++) | ||
| 262 | { | ||
| 263 | // Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID) | ||
| 264 | mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]); | ||
| 265 | } | ||
| 266 | } | ||
| 267 | } | ||
| 268 | |||
| 269 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 270 | /** | ||
| 271 | * Takes advantage of temporal coherence. | ||
| 272 | * \param cache [in] cache for a pair of previously colliding primitives | ||
| 273 | * \return true if we can return immediately | ||
| 274 | * \warning only works for "First Contact" mode | ||
| 275 | */ | ||
| 276 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 277 | bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache) | ||
| 278 | { | ||
| 279 | // Checkings | ||
| 280 | if(!cache) return false; | ||
| 281 | |||
| 282 | // Test previously colliding primitives first | ||
| 283 | if(TemporalCoherenceEnabled() && FirstContactEnabled()) | ||
| 284 | { | ||
| 285 | PrimTest(cache->id0, cache->id1); | ||
| 286 | if(GetContactStatus()) return true; | ||
| 287 | } | ||
| 288 | return false; | ||
| 289 | } | ||
| 290 | |||
| 291 | #define UPDATE_CACHE \ | ||
| 292 | if(cache && GetContactStatus()) \ | ||
| 293 | { \ | ||
| 294 | cache->id0 = mPairs.GetEntry(0); \ | ||
| 295 | cache->id1 = mPairs.GetEntry(1); \ | ||
| 296 | } | ||
| 297 | |||
| 298 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 299 | /** | ||
| 300 | * Collision query for normal AABB trees. | ||
| 301 | * \param tree0 [in] AABB tree from first object | ||
| 302 | * \param tree1 [in] AABB tree from second object | ||
| 303 | * \param world0 [in] world matrix for first object | ||
| 304 | * \param world1 [in] world matrix for second object | ||
| 305 | * \param cache [in/out] cache for a pair of previously colliding primitives | ||
| 306 | * \return true if success | ||
| 307 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 308 | */ | ||
| 309 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 310 | bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) | ||
| 311 | { | ||
| 312 | // Init collision query | ||
| 313 | InitQuery(world0, world1); | ||
| 314 | |||
| 315 | // Check previous state | ||
| 316 | if(CheckTemporalCoherence(cache)) return true; | ||
| 317 | |||
| 318 | // Perform collision query | ||
| 319 | _Collide(tree0->GetNodes(), tree1->GetNodes()); | ||
| 320 | |||
| 321 | UPDATE_CACHE | ||
| 322 | |||
| 323 | return true; | ||
| 324 | } | ||
| 325 | |||
| 326 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 327 | /** | ||
| 328 | * Collision query for no-leaf AABB trees. | ||
| 329 | * \param tree0 [in] AABB tree from first object | ||
| 330 | * \param tree1 [in] AABB tree from second object | ||
| 331 | * \param world0 [in] world matrix for first object | ||
| 332 | * \param world1 [in] world matrix for second object | ||
| 333 | * \param cache [in/out] cache for a pair of previously colliding primitives | ||
| 334 | * \return true if success | ||
| 335 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 336 | */ | ||
| 337 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 338 | bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) | ||
| 339 | { | ||
| 340 | // Init collision query | ||
| 341 | InitQuery(world0, world1); | ||
| 342 | |||
| 343 | // Check previous state | ||
| 344 | if(CheckTemporalCoherence(cache)) return true; | ||
| 345 | |||
| 346 | // Perform collision query | ||
| 347 | _Collide(tree0->GetNodes(), tree1->GetNodes()); | ||
| 348 | |||
| 349 | UPDATE_CACHE | ||
| 350 | |||
| 351 | return true; | ||
| 352 | } | ||
| 353 | |||
| 354 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 355 | /** | ||
| 356 | * Collision query for quantized AABB trees. | ||
| 357 | * \param tree0 [in] AABB tree from first object | ||
| 358 | * \param tree1 [in] AABB tree from second object | ||
| 359 | * \param world0 [in] world matrix for first object | ||
| 360 | * \param world1 [in] world matrix for second object | ||
| 361 | * \param cache [in/out] cache for a pair of previously colliding primitives | ||
| 362 | * \return true if success | ||
| 363 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 364 | */ | ||
| 365 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 366 | bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) | ||
| 367 | { | ||
| 368 | // Init collision query | ||
| 369 | InitQuery(world0, world1); | ||
| 370 | |||
| 371 | // Check previous state | ||
| 372 | if(CheckTemporalCoherence(cache)) return true; | ||
| 373 | |||
| 374 | // Setup dequantization coeffs | ||
| 375 | mCenterCoeff0 = tree0->mCenterCoeff; | ||
| 376 | mExtentsCoeff0 = tree0->mExtentsCoeff; | ||
| 377 | mCenterCoeff1 = tree1->mCenterCoeff; | ||
| 378 | mExtentsCoeff1 = tree1->mExtentsCoeff; | ||
| 379 | |||
| 380 | // Dequantize box A | ||
| 381 | const AABBQuantizedNode* N0 = tree0->GetNodes(); | ||
| 382 | const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z); | ||
| 383 | const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z); | ||
| 384 | // Dequantize box B | ||
| 385 | const AABBQuantizedNode* N1 = tree1->GetNodes(); | ||
| 386 | const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z); | ||
| 387 | const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z); | ||
| 388 | |||
| 389 | // Perform collision query | ||
| 390 | _Collide(N0, N1, a, Pa, b, Pb); | ||
| 391 | |||
| 392 | UPDATE_CACHE | ||
| 393 | |||
| 394 | return true; | ||
| 395 | } | ||
| 396 | |||
| 397 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 398 | /** | ||
| 399 | * Collision query for quantized no-leaf AABB trees. | ||
| 400 | * \param tree0 [in] AABB tree from first object | ||
| 401 | * \param tree1 [in] AABB tree from second object | ||
| 402 | * \param world0 [in] world matrix for first object | ||
| 403 | * \param world1 [in] world matrix for second object | ||
| 404 | * \param cache [in/out] cache for a pair of previously colliding primitives | ||
| 405 | * \return true if success | ||
| 406 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 407 | */ | ||
| 408 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 409 | bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) | ||
| 410 | { | ||
| 411 | // Init collision query | ||
| 412 | InitQuery(world0, world1); | ||
| 413 | |||
| 414 | // Check previous state | ||
| 415 | if(CheckTemporalCoherence(cache)) return true; | ||
| 416 | |||
| 417 | // Setup dequantization coeffs | ||
| 418 | mCenterCoeff0 = tree0->mCenterCoeff; | ||
| 419 | mExtentsCoeff0 = tree0->mExtentsCoeff; | ||
| 420 | mCenterCoeff1 = tree1->mCenterCoeff; | ||
| 421 | mExtentsCoeff1 = tree1->mExtentsCoeff; | ||
| 422 | |||
| 423 | // Perform collision query | ||
| 424 | _Collide(tree0->GetNodes(), tree1->GetNodes()); | ||
| 425 | |||
| 426 | UPDATE_CACHE | ||
| 427 | |||
| 428 | return true; | ||
| 429 | } | ||
| 430 | |||
| 431 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 432 | // Standard trees | ||
| 433 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 434 | |||
| 435 | // The normal AABB tree can use 2 different descent rules (with different performances) | ||
| 436 | //#define ORIGINAL_CODE //!< UNC-like descent rules | ||
| 437 | #define ALTERNATIVE_CODE //!< Alternative descent rules | ||
| 438 | |||
| 439 | #ifdef ORIGINAL_CODE | ||
| 440 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 441 | /** | ||
| 442 | * Recursive collision query for normal AABB trees. | ||
| 443 | * \param b0 [in] collision node from first tree | ||
| 444 | * \param b1 [in] collision node from second tree | ||
| 445 | */ | ||
| 446 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 447 | void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) | ||
| 448 | { | ||
| 449 | // Perform BV-BV overlap test | ||
| 450 | if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return; | ||
| 451 | |||
| 452 | if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } | ||
| 453 | |||
| 454 | if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) | ||
| 455 | { | ||
| 456 | _Collide(b0->GetNeg(), b1); | ||
| 457 | if(ContactFound()) return; | ||
| 458 | _Collide(b0->GetPos(), b1); | ||
| 459 | } | ||
| 460 | else | ||
| 461 | { | ||
| 462 | _Collide(b0, b1->GetNeg()); | ||
| 463 | if(ContactFound()) return; | ||
| 464 | _Collide(b0, b1->GetPos()); | ||
| 465 | } | ||
| 466 | } | ||
| 467 | #endif | ||
| 468 | |||
| 469 | #ifdef ALTERNATIVE_CODE | ||
| 470 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 471 | /** | ||
| 472 | * Recursive collision query for normal AABB trees. | ||
| 473 | * \param b0 [in] collision node from first tree | ||
| 474 | * \param b1 [in] collision node from second tree | ||
| 475 | */ | ||
| 476 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 477 | void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) | ||
| 478 | { | ||
| 479 | // Perform BV-BV overlap test | ||
| 480 | if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) | ||
| 481 | { | ||
| 482 | return; | ||
| 483 | } | ||
| 484 | |||
| 485 | if(b0->IsLeaf()) | ||
| 486 | { | ||
| 487 | if(b1->IsLeaf()) | ||
| 488 | { | ||
| 489 | PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); | ||
| 490 | } | ||
| 491 | else | ||
| 492 | { | ||
| 493 | _Collide(b0, b1->GetNeg()); | ||
| 494 | if(ContactFound()) return; | ||
| 495 | _Collide(b0, b1->GetPos()); | ||
| 496 | } | ||
| 497 | } | ||
| 498 | else if(b1->IsLeaf()) | ||
| 499 | { | ||
| 500 | _Collide(b0->GetNeg(), b1); | ||
| 501 | if(ContactFound()) return; | ||
| 502 | _Collide(b0->GetPos(), b1); | ||
| 503 | } | ||
| 504 | else | ||
| 505 | { | ||
| 506 | _Collide(b0->GetNeg(), b1->GetNeg()); | ||
| 507 | if(ContactFound()) return; | ||
| 508 | _Collide(b0->GetNeg(), b1->GetPos()); | ||
| 509 | if(ContactFound()) return; | ||
| 510 | _Collide(b0->GetPos(), b1->GetNeg()); | ||
| 511 | if(ContactFound()) return; | ||
| 512 | _Collide(b0->GetPos(), b1->GetPos()); | ||
| 513 | } | ||
| 514 | } | ||
| 515 | #endif | ||
| 516 | |||
| 517 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 518 | // No-leaf trees | ||
| 519 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 520 | |||
| 521 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 522 | /** | ||
| 523 | * Leaf-leaf test for two primitive indices. | ||
| 524 | * \param id0 [in] index from first leaf-triangle | ||
| 525 | * \param id1 [in] index from second leaf-triangle | ||
| 526 | */ | ||
| 527 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 528 | void AABBTreeCollider::PrimTest(udword id0, udword id1) | ||
| 529 | { | ||
| 530 | // Request vertices from the app | ||
| 531 | VertexPointers VP0; | ||
| 532 | VertexPointers VP1; | ||
| 533 | mIMesh0->GetTriangle(VP0, id0); | ||
| 534 | mIMesh1->GetTriangle(VP1, id1); | ||
| 535 | |||
| 536 | // Transform from space 1 to space 0 | ||
| 537 | Point u0,u1,u2; | ||
| 538 | TransformPoint(u0, *VP1.Vertex[0], mR1to0, mT1to0); | ||
| 539 | TransformPoint(u1, *VP1.Vertex[1], mR1to0, mT1to0); | ||
| 540 | TransformPoint(u2, *VP1.Vertex[2], mR1to0, mT1to0); | ||
| 541 | |||
| 542 | // Perform triangle-triangle overlap test | ||
| 543 | if(TriTriOverlap(*VP0.Vertex[0], *VP0.Vertex[1], *VP0.Vertex[2], u0, u1, u2)) | ||
| 544 | { | ||
| 545 | // Keep track of colliding pairs | ||
| 546 | mPairs.Add(id0).Add(id1); | ||
| 547 | // Set contact status | ||
| 548 | mFlags |= OPC_CONTACT; | ||
| 549 | } | ||
| 550 | } | ||
| 551 | |||
| 552 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 553 | /** | ||
| 554 | * Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B. | ||
| 555 | * \param id1 [in] leaf-triangle index from tree B | ||
| 556 | */ | ||
| 557 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 558 | inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1) | ||
| 559 | { | ||
| 560 | // Request vertices from the app | ||
| 561 | VertexPointers VP; | ||
| 562 | mIMesh1->GetTriangle(VP, id1); | ||
| 563 | |||
| 564 | // Perform triangle-triangle overlap test | ||
| 565 | if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) | ||
| 566 | { | ||
| 567 | // Keep track of colliding pairs | ||
| 568 | mPairs.Add(mLeafIndex).Add(id1); | ||
| 569 | // Set contact status | ||
| 570 | mFlags |= OPC_CONTACT; | ||
| 571 | } | ||
| 572 | } | ||
| 573 | |||
| 574 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 575 | /** | ||
| 576 | * Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A. | ||
| 577 | * \param id0 [in] leaf-triangle index from tree A | ||
| 578 | */ | ||
| 579 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 580 | inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0) | ||
| 581 | { | ||
| 582 | // Request vertices from the app | ||
| 583 | VertexPointers VP; | ||
| 584 | mIMesh0->GetTriangle(VP, id0); | ||
| 585 | |||
| 586 | // Perform triangle-triangle overlap test | ||
| 587 | if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) | ||
| 588 | { | ||
| 589 | // Keep track of colliding pairs | ||
| 590 | mPairs.Add(id0).Add(mLeafIndex); | ||
| 591 | // Set contact status | ||
| 592 | mFlags |= OPC_CONTACT; | ||
| 593 | } | ||
| 594 | } | ||
| 595 | |||
| 596 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 597 | /** | ||
| 598 | * Recursive collision of a leaf node from A and a branch from B. | ||
| 599 | * \param b [in] collision node from second tree | ||
| 600 | */ | ||
| 601 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 602 | void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b) | ||
| 603 | { | ||
| 604 | // Perform triangle-box overlap test | ||
| 605 | if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; | ||
| 606 | |||
| 607 | // Keep same triangle, deal with first child | ||
| 608 | if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); | ||
| 609 | else _CollideTriBox(b->GetPos()); | ||
| 610 | |||
| 611 | if(ContactFound()) return; | ||
| 612 | |||
| 613 | // Keep same triangle, deal with second child | ||
| 614 | if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); | ||
| 615 | else _CollideTriBox(b->GetNeg()); | ||
| 616 | } | ||
| 617 | |||
| 618 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 619 | /** | ||
| 620 | * Recursive collision of a leaf node from B and a branch from A. | ||
| 621 | * \param b [in] collision node from first tree | ||
| 622 | */ | ||
| 623 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 624 | void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b) | ||
| 625 | { | ||
| 626 | // Perform triangle-box overlap test | ||
| 627 | if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; | ||
| 628 | |||
| 629 | // Keep same triangle, deal with first child | ||
| 630 | if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); | ||
| 631 | else _CollideBoxTri(b->GetPos()); | ||
| 632 | |||
| 633 | if(ContactFound()) return; | ||
| 634 | |||
| 635 | // Keep same triangle, deal with second child | ||
| 636 | if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); | ||
| 637 | else _CollideBoxTri(b->GetNeg()); | ||
| 638 | } | ||
| 639 | |||
| 640 | //! Request triangle vertices from the app and transform them | ||
| 641 | #define FETCH_LEAF(prim_index, imesh, rot, trans) \ | ||
| 642 | mLeafIndex = prim_index; \ | ||
| 643 | /* Request vertices from the app */ \ | ||
| 644 | VertexPointers VP; imesh->GetTriangle(VP, prim_index); \ | ||
| 645 | /* Transform them in a common space */ \ | ||
| 646 | TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \ | ||
| 647 | TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \ | ||
| 648 | TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans); | ||
| 649 | |||
| 650 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 651 | /** | ||
| 652 | * Recursive collision query for no-leaf AABB trees. | ||
| 653 | * \param a [in] collision node from first tree | ||
| 654 | * \param b [in] collision node from second tree | ||
| 655 | */ | ||
| 656 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 657 | void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b) | ||
| 658 | { | ||
| 659 | // Perform BV-BV overlap test | ||
| 660 | if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return; | ||
| 661 | |||
| 662 | // Catch leaf status | ||
| 663 | BOOL BHasPosLeaf = b->HasPosLeaf(); | ||
| 664 | BOOL BHasNegLeaf = b->HasNegLeaf(); | ||
| 665 | |||
| 666 | if(a->HasPosLeaf()) | ||
| 667 | { | ||
| 668 | FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) | ||
| 669 | |||
| 670 | if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); | ||
| 671 | else _CollideTriBox(b->GetPos()); | ||
| 672 | |||
| 673 | if(ContactFound()) return; | ||
| 674 | |||
| 675 | if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); | ||
| 676 | else _CollideTriBox(b->GetNeg()); | ||
| 677 | } | ||
| 678 | else | ||
| 679 | { | ||
| 680 | if(BHasPosLeaf) | ||
| 681 | { | ||
| 682 | FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 683 | |||
| 684 | _CollideBoxTri(a->GetPos()); | ||
| 685 | } | ||
| 686 | else _Collide(a->GetPos(), b->GetPos()); | ||
| 687 | |||
| 688 | if(ContactFound()) return; | ||
| 689 | |||
| 690 | if(BHasNegLeaf) | ||
| 691 | { | ||
| 692 | FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 693 | |||
| 694 | _CollideBoxTri(a->GetPos()); | ||
| 695 | } | ||
| 696 | else _Collide(a->GetPos(), b->GetNeg()); | ||
| 697 | } | ||
| 698 | |||
| 699 | if(ContactFound()) return; | ||
| 700 | |||
| 701 | if(a->HasNegLeaf()) | ||
| 702 | { | ||
| 703 | FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) | ||
| 704 | |||
| 705 | if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); | ||
| 706 | else _CollideTriBox(b->GetPos()); | ||
| 707 | |||
| 708 | if(ContactFound()) return; | ||
| 709 | |||
| 710 | if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); | ||
| 711 | else _CollideTriBox(b->GetNeg()); | ||
| 712 | } | ||
| 713 | else | ||
| 714 | { | ||
| 715 | if(BHasPosLeaf) | ||
| 716 | { | ||
| 717 | // ### That leaf has possibly already been fetched | ||
| 718 | FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 719 | |||
| 720 | _CollideBoxTri(a->GetNeg()); | ||
| 721 | } | ||
| 722 | else _Collide(a->GetNeg(), b->GetPos()); | ||
| 723 | |||
| 724 | if(ContactFound()) return; | ||
| 725 | |||
| 726 | if(BHasNegLeaf) | ||
| 727 | { | ||
| 728 | // ### That leaf has possibly already been fetched | ||
| 729 | FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 730 | |||
| 731 | _CollideBoxTri(a->GetNeg()); | ||
| 732 | } | ||
| 733 | else _Collide(a->GetNeg(), b->GetNeg()); | ||
| 734 | } | ||
| 735 | } | ||
| 736 | |||
| 737 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 738 | // Quantized trees | ||
| 739 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 740 | |||
| 741 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 742 | /** | ||
| 743 | * Recursive collision query for quantized AABB trees. | ||
| 744 | * \param b0 [in] collision node from first tree | ||
| 745 | * \param b1 [in] collision node from second tree | ||
| 746 | * \param a [in] extent from box A | ||
| 747 | * \param Pa [in] center from box A | ||
| 748 | * \param b [in] extent from box B | ||
| 749 | * \param Pb [in] center from box B | ||
| 750 | */ | ||
| 751 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 752 | void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb) | ||
| 753 | { | ||
| 754 | // Perform BV-BV overlap test | ||
| 755 | if(!BoxBoxOverlap(a, Pa, b, Pb)) return; | ||
| 756 | |||
| 757 | if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } | ||
| 758 | |||
| 759 | if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) | ||
| 760 | { | ||
| 761 | // Dequantize box | ||
| 762 | const QuantizedAABB* Box = &b0->GetNeg()->mAABB; | ||
| 763 | const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); | ||
| 764 | const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); | ||
| 765 | _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb); | ||
| 766 | |||
| 767 | if(ContactFound()) return; | ||
| 768 | |||
| 769 | // Dequantize box | ||
| 770 | Box = &b0->GetPos()->mAABB; | ||
| 771 | const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); | ||
| 772 | const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); | ||
| 773 | _Collide(b0->GetPos(), b1, posa, posPa, b, Pb); | ||
| 774 | } | ||
| 775 | else | ||
| 776 | { | ||
| 777 | // Dequantize box | ||
| 778 | const QuantizedAABB* Box = &b1->GetNeg()->mAABB; | ||
| 779 | const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); | ||
| 780 | const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); | ||
| 781 | _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb); | ||
| 782 | |||
| 783 | if(ContactFound()) return; | ||
| 784 | |||
| 785 | // Dequantize box | ||
| 786 | Box = &b1->GetPos()->mAABB; | ||
| 787 | const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); | ||
| 788 | const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); | ||
| 789 | _Collide(b0, b1->GetPos(), a, Pa, posb, posPb); | ||
| 790 | } | ||
| 791 | } | ||
| 792 | |||
| 793 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 794 | // Quantized no-leaf trees | ||
| 795 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 796 | |||
| 797 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 798 | /** | ||
| 799 | * Recursive collision of a leaf node from A and a quantized branch from B. | ||
| 800 | * \param leaf [in] leaf triangle from first tree | ||
| 801 | * \param b [in] collision node from second tree | ||
| 802 | */ | ||
| 803 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 804 | void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b) | ||
| 805 | { | ||
| 806 | // Dequantize box | ||
| 807 | const QuantizedAABB* bb = &b->mAABB; | ||
| 808 | const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); | ||
| 809 | const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); | ||
| 810 | |||
| 811 | // Perform triangle-box overlap test | ||
| 812 | if(!TriBoxOverlap(Pb, eb)) return; | ||
| 813 | |||
| 814 | if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); | ||
| 815 | else _CollideTriBox(b->GetPos()); | ||
| 816 | |||
| 817 | if(ContactFound()) return; | ||
| 818 | |||
| 819 | if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); | ||
| 820 | else _CollideTriBox(b->GetNeg()); | ||
| 821 | } | ||
| 822 | |||
| 823 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 824 | /** | ||
| 825 | * Recursive collision of a leaf node from B and a quantized branch from A. | ||
| 826 | * \param b [in] collision node from first tree | ||
| 827 | * \param leaf [in] leaf triangle from second tree | ||
| 828 | */ | ||
| 829 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 830 | void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b) | ||
| 831 | { | ||
| 832 | // Dequantize box | ||
| 833 | const QuantizedAABB* bb = &b->mAABB; | ||
| 834 | const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z); | ||
| 835 | const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z); | ||
| 836 | |||
| 837 | // Perform triangle-box overlap test | ||
| 838 | if(!TriBoxOverlap(Pa, ea)) return; | ||
| 839 | |||
| 840 | if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); | ||
| 841 | else _CollideBoxTri(b->GetPos()); | ||
| 842 | |||
| 843 | if(ContactFound()) return; | ||
| 844 | |||
| 845 | if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); | ||
| 846 | else _CollideBoxTri(b->GetNeg()); | ||
| 847 | } | ||
| 848 | |||
| 849 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 850 | /** | ||
| 851 | * Recursive collision query for quantized no-leaf AABB trees. | ||
| 852 | * \param a [in] collision node from first tree | ||
| 853 | * \param b [in] collision node from second tree | ||
| 854 | */ | ||
| 855 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 856 | void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b) | ||
| 857 | { | ||
| 858 | // Dequantize box A | ||
| 859 | const QuantizedAABB* ab = &a->mAABB; | ||
| 860 | const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z); | ||
| 861 | const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z); | ||
| 862 | // Dequantize box B | ||
| 863 | const QuantizedAABB* bb = &b->mAABB; | ||
| 864 | const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); | ||
| 865 | const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); | ||
| 866 | |||
| 867 | // Perform BV-BV overlap test | ||
| 868 | if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return; | ||
| 869 | |||
| 870 | // Catch leaf status | ||
| 871 | BOOL BHasPosLeaf = b->HasPosLeaf(); | ||
| 872 | BOOL BHasNegLeaf = b->HasNegLeaf(); | ||
| 873 | |||
| 874 | if(a->HasPosLeaf()) | ||
| 875 | { | ||
| 876 | FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) | ||
| 877 | |||
| 878 | if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); | ||
| 879 | else _CollideTriBox(b->GetPos()); | ||
| 880 | |||
| 881 | if(ContactFound()) return; | ||
| 882 | |||
| 883 | if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); | ||
| 884 | else _CollideTriBox(b->GetNeg()); | ||
| 885 | } | ||
| 886 | else | ||
| 887 | { | ||
| 888 | if(BHasPosLeaf) | ||
| 889 | { | ||
| 890 | FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 891 | |||
| 892 | _CollideBoxTri(a->GetPos()); | ||
| 893 | } | ||
| 894 | else _Collide(a->GetPos(), b->GetPos()); | ||
| 895 | |||
| 896 | if(ContactFound()) return; | ||
| 897 | |||
| 898 | if(BHasNegLeaf) | ||
| 899 | { | ||
| 900 | FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 901 | |||
| 902 | _CollideBoxTri(a->GetPos()); | ||
| 903 | } | ||
| 904 | else _Collide(a->GetPos(), b->GetNeg()); | ||
| 905 | } | ||
| 906 | |||
| 907 | if(ContactFound()) return; | ||
| 908 | |||
| 909 | if(a->HasNegLeaf()) | ||
| 910 | { | ||
| 911 | FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) | ||
| 912 | |||
| 913 | if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); | ||
| 914 | else _CollideTriBox(b->GetPos()); | ||
| 915 | |||
| 916 | if(ContactFound()) return; | ||
| 917 | |||
| 918 | if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); | ||
| 919 | else _CollideTriBox(b->GetNeg()); | ||
| 920 | } | ||
| 921 | else | ||
| 922 | { | ||
| 923 | if(BHasPosLeaf) | ||
| 924 | { | ||
| 925 | // ### That leaf has possibly already been fetched | ||
| 926 | FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 927 | |||
| 928 | _CollideBoxTri(a->GetNeg()); | ||
| 929 | } | ||
| 930 | else _Collide(a->GetNeg(), b->GetPos()); | ||
| 931 | |||
| 932 | if(ContactFound()) return; | ||
| 933 | |||
| 934 | if(BHasNegLeaf) | ||
| 935 | { | ||
| 936 | // ### That leaf has possibly already been fetched | ||
| 937 | FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) | ||
| 938 | |||
| 939 | _CollideBoxTri(a->GetNeg()); | ||
| 940 | } | ||
| 941 | else _Collide(a->GetNeg(), b->GetNeg()); | ||
| 942 | } | ||
| 943 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_TreeCollider.h b/libraries/ode-0.9/OPCODE/OPC_TreeCollider.h new file mode 100644 index 0000000..1e943a4 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_TreeCollider.h | |||
| @@ -0,0 +1,246 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains code for a tree collider. | ||
| 12 | * \file OPC_TreeCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_TREECOLLIDER_H__ | ||
| 21 | #define __OPC_TREECOLLIDER_H__ | ||
| 22 | |||
| 23 | //! This structure holds cached information used by the algorithm. | ||
| 24 | //! Two model pointers and two colliding primitives are cached. Model pointers are assigned | ||
| 25 | //! to their respective meshes, and the pair of colliding primitives is used for temporal | ||
| 26 | //! coherence. That is, in case temporal coherence is enabled, those two primitives are | ||
| 27 | //! tested for overlap before everything else. If they still collide, we're done before | ||
| 28 | //! even entering the recursive collision code. | ||
| 29 | struct OPCODE_API BVTCache : Pair | ||
| 30 | { | ||
| 31 | //! Constructor | ||
| 32 | inline_ BVTCache() | ||
| 33 | { | ||
| 34 | ResetCache(); | ||
| 35 | ResetCountDown(); | ||
| 36 | } | ||
| 37 | |||
| 38 | void ResetCache() | ||
| 39 | { | ||
| 40 | Model0 = null; | ||
| 41 | Model1 = null; | ||
| 42 | id0 = 0; | ||
| 43 | id1 = 1; | ||
| 44 | #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! | ||
| 45 | HullTest = true; | ||
| 46 | SepVector.pid = 0; | ||
| 47 | SepVector.qid = 0; | ||
| 48 | SepVector.SV = Point(1.0f, 0.0f, 0.0f); | ||
| 49 | #endif // __MESHMERIZER_H__ | ||
| 50 | } | ||
| 51 | |||
| 52 | #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! | ||
| 53 | inline_ void ResetCountDown() | ||
| 54 | { | ||
| 55 | CountDown = 50; | ||
| 56 | } | ||
| 57 | #else | ||
| 58 | void ResetCountDown(){}; | ||
| 59 | #endif // __MESHMERIZER_H__ | ||
| 60 | |||
| 61 | const Model* Model0; //!< Model for first object | ||
| 62 | const Model* Model1; //!< Model for second object | ||
| 63 | |||
| 64 | #ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! | ||
| 65 | SVCache SepVector; | ||
| 66 | udword CountDown; | ||
| 67 | bool HullTest; | ||
| 68 | #endif // __MESHMERIZER_H__ | ||
| 69 | }; | ||
| 70 | |||
| 71 | class OPCODE_API AABBTreeCollider : public Collider | ||
| 72 | { | ||
| 73 | public: | ||
| 74 | // Constructor / Destructor | ||
| 75 | AABBTreeCollider(); | ||
| 76 | virtual ~AABBTreeCollider(); | ||
| 77 | // Generic collision query | ||
| 78 | |||
| 79 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 80 | /** | ||
| 81 | * Generic collision query for generic OPCODE models. After the call, access the results with: | ||
| 82 | * - GetContactStatus() | ||
| 83 | * - GetNbPairs() | ||
| 84 | * - GetPairs() | ||
| 85 | * | ||
| 86 | * \param cache [in] collision cache for model pointers and a colliding pair of primitives | ||
| 87 | * \param world0 [in] world matrix for first object, or null | ||
| 88 | * \param world1 [in] world matrix for second object, or null | ||
| 89 | * \return true if success | ||
| 90 | * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. | ||
| 91 | */ | ||
| 92 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 93 | bool Collide(BVTCache& cache, const Matrix4x4* world0=null, const Matrix4x4* world1=null); | ||
| 94 | |||
| 95 | // Collision queries | ||
| 96 | bool Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); | ||
| 97 | bool Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); | ||
| 98 | bool Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); | ||
| 99 | bool Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); | ||
| 100 | // Settings | ||
| 101 | |||
| 102 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 103 | /** | ||
| 104 | * Settings: selects between full box-box tests or "SAT-lite" tests (where Class III axes are discarded) | ||
| 105 | * \param flag [in] true for full tests, false for coarse tests | ||
| 106 | * \see SetFullPrimBoxTest(bool flag) | ||
| 107 | */ | ||
| 108 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 109 | inline_ void SetFullBoxBoxTest(bool flag) { mFullBoxBoxTest = flag; } | ||
| 110 | |||
| 111 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 112 | /** | ||
| 113 | * Settings: selects between full triangle-box tests or "SAT-lite" tests (where Class III axes are discarded) | ||
| 114 | * \param flag [in] true for full tests, false for coarse tests | ||
| 115 | * \see SetFullBoxBoxTest(bool flag) | ||
| 116 | */ | ||
| 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 118 | inline_ void SetFullPrimBoxTest(bool flag) { mFullPrimBoxTest = flag; } | ||
| 119 | |||
| 120 | // Stats | ||
| 121 | |||
| 122 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 123 | /** | ||
| 124 | * Stats: gets the number of BV-BV overlap tests after a collision query. | ||
| 125 | * \see GetNbPrimPrimTests() | ||
| 126 | * \see GetNbBVPrimTests() | ||
| 127 | * \return the number of BV-BV tests performed during last query | ||
| 128 | */ | ||
| 129 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 130 | inline_ udword GetNbBVBVTests() const { return mNbBVBVTests; } | ||
| 131 | |||
| 132 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 133 | /** | ||
| 134 | * Stats: gets the number of Triangle-Triangle overlap tests after a collision query. | ||
| 135 | * \see GetNbBVBVTests() | ||
| 136 | * \see GetNbBVPrimTests() | ||
| 137 | * \return the number of Triangle-Triangle tests performed during last query | ||
| 138 | */ | ||
| 139 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 140 | inline_ udword GetNbPrimPrimTests() const { return mNbPrimPrimTests; } | ||
| 141 | |||
| 142 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 143 | /** | ||
| 144 | * Stats: gets the number of BV-Triangle overlap tests after a collision query. | ||
| 145 | * \see GetNbBVBVTests() | ||
| 146 | * \see GetNbPrimPrimTests() | ||
| 147 | * \return the number of BV-Triangle tests performed during last query | ||
| 148 | */ | ||
| 149 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 150 | inline_ udword GetNbBVPrimTests() const { return mNbBVPrimTests; } | ||
| 151 | |||
| 152 | // Data access | ||
| 153 | |||
| 154 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 155 | /** | ||
| 156 | * Gets the number of contacts after a collision query. | ||
| 157 | * \see GetContactStatus() | ||
| 158 | * \see GetPairs() | ||
| 159 | * \return the number of contacts / colliding pairs. | ||
| 160 | */ | ||
| 161 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 162 | inline_ udword GetNbPairs() const { return mPairs.GetNbEntries()>>1; } | ||
| 163 | |||
| 164 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 165 | /** | ||
| 166 | * Gets the pairs of colliding triangles after a collision query. | ||
| 167 | * \see GetContactStatus() | ||
| 168 | * \see GetNbPairs() | ||
| 169 | * \return the list of colliding pairs (triangle indices) | ||
| 170 | */ | ||
| 171 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 172 | inline_ const Pair* GetPairs() const { return (const Pair*)mPairs.GetEntries(); } | ||
| 173 | |||
| 174 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 175 | /** | ||
| 176 | * Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider. | ||
| 177 | * \return null if everything is ok, else a string describing the problem | ||
| 178 | */ | ||
| 179 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 180 | override(Collider) const char* ValidateSettings(); | ||
| 181 | |||
| 182 | protected: | ||
| 183 | // Colliding pairs | ||
| 184 | Container mPairs; //!< Pairs of colliding primitives | ||
| 185 | // User mesh interfaces | ||
| 186 | const MeshInterface* mIMesh0; //!< User-defined mesh interface for object0 | ||
| 187 | const MeshInterface* mIMesh1; //!< User-defined mesh interface for object1 | ||
| 188 | // Stats | ||
| 189 | udword mNbBVBVTests; //!< Number of BV-BV tests | ||
| 190 | udword mNbPrimPrimTests; //!< Number of Primitive-Primitive tests | ||
| 191 | udword mNbBVPrimTests; //!< Number of BV-Primitive tests | ||
| 192 | // Precomputed data | ||
| 193 | Matrix3x3 mAR; //!< Absolute rotation matrix | ||
| 194 | Matrix3x3 mR0to1; //!< Rotation from object0 to object1 | ||
| 195 | Matrix3x3 mR1to0; //!< Rotation from object1 to object0 | ||
| 196 | Point mT0to1; //!< Translation from object0 to object1 | ||
| 197 | Point mT1to0; //!< Translation from object1 to object0 | ||
| 198 | // Dequantization coeffs | ||
| 199 | Point mCenterCoeff0; | ||
| 200 | Point mExtentsCoeff0; | ||
| 201 | Point mCenterCoeff1; | ||
| 202 | Point mExtentsCoeff1; | ||
| 203 | // Leaf description | ||
| 204 | Point mLeafVerts[3]; //!< Triangle vertices | ||
| 205 | udword mLeafIndex; //!< Triangle index | ||
| 206 | // Settings | ||
| 207 | bool mFullBoxBoxTest; //!< Perform full BV-BV tests (true) or SAT-lite tests (false) | ||
| 208 | bool mFullPrimBoxTest; //!< Perform full Primitive-BV tests (true) or SAT-lite tests (false) | ||
| 209 | // Internal methods | ||
| 210 | |||
| 211 | // Standard AABB trees | ||
| 212 | void _Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1); | ||
| 213 | // Quantized AABB trees | ||
| 214 | void _Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb); | ||
| 215 | // No-leaf AABB trees | ||
| 216 | void _CollideTriBox(const AABBNoLeafNode* b); | ||
| 217 | void _CollideBoxTri(const AABBNoLeafNode* b); | ||
| 218 | void _Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b); | ||
| 219 | // Quantized no-leaf AABB trees | ||
| 220 | void _CollideTriBox(const AABBQuantizedNoLeafNode* b); | ||
| 221 | void _CollideBoxTri(const AABBQuantizedNoLeafNode* b); | ||
| 222 | void _Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b); | ||
| 223 | // Overlap tests | ||
| 224 | void PrimTest(udword id0, udword id1); | ||
| 225 | inline_ void PrimTestTriIndex(udword id1); | ||
| 226 | inline_ void PrimTestIndexTri(udword id0); | ||
| 227 | |||
| 228 | inline_ BOOL BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb); | ||
| 229 | inline_ BOOL TriBoxOverlap(const Point& center, const Point& extents); | ||
| 230 | inline_ BOOL TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2); | ||
| 231 | // Init methods | ||
| 232 | void InitQuery(const Matrix4x4* world0=null, const Matrix4x4* world1=null); | ||
| 233 | bool CheckTemporalCoherence(Pair* cache); | ||
| 234 | |||
| 235 | inline_ BOOL Setup(const MeshInterface* mi0, const MeshInterface* mi1) | ||
| 236 | { | ||
| 237 | mIMesh0 = mi0; | ||
| 238 | mIMesh1 = mi1; | ||
| 239 | |||
| 240 | if(!mIMesh0 || !mIMesh1) return FALSE; | ||
| 241 | |||
| 242 | return TRUE; | ||
| 243 | } | ||
| 244 | }; | ||
| 245 | |||
| 246 | #endif // __OPC_TREECOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h b/libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h new file mode 100644 index 0000000..b3a9bde --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_TriBoxOverlap.h | |||
| @@ -0,0 +1,339 @@ | |||
| 1 | |||
| 2 | //! This macro quickly finds the min & max values among 3 variables | ||
| 3 | #define FINDMINMAX(x0, x1, x2, min, max) \ | ||
| 4 | min = max = x0; \ | ||
| 5 | if(x1<min) min=x1; \ | ||
| 6 | if(x1>max) max=x1; \ | ||
| 7 | if(x2<min) min=x2; \ | ||
| 8 | if(x2>max) max=x2; | ||
| 9 | |||
| 10 | //! TO BE DOCUMENTED | ||
| 11 | inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox) | ||
| 12 | { | ||
| 13 | Point vmin, vmax; | ||
| 14 | for(udword q=0;q<=2;q++) | ||
| 15 | { | ||
| 16 | if(normal[q]>0.0f) { vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; } | ||
| 17 | else { vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; } | ||
| 18 | } | ||
| 19 | if((normal|vmin)+d>0.0f) return FALSE; | ||
| 20 | if((normal|vmax)+d>=0.0f) return TRUE; | ||
| 21 | |||
| 22 | return FALSE; | ||
| 23 | } | ||
| 24 | |||
| 25 | //! TO BE DOCUMENTED | ||
| 26 | #define AXISTEST_X01(a, b, fa, fb) \ | ||
| 27 | min = a*v0.y - b*v0.z; \ | ||
| 28 | max = a*v2.y - b*v2.z; \ | ||
| 29 | if(min>max) {const float tmp=max; max=min; min=tmp; } \ | ||
| 30 | rad = fa * extents.y + fb * extents.z; \ | ||
| 31 | if(min>rad || max<-rad) return FALSE; | ||
| 32 | |||
| 33 | //! TO BE DOCUMENTED | ||
| 34 | #define AXISTEST_X2(a, b, fa, fb) \ | ||
| 35 | min = a*v0.y - b*v0.z; \ | ||
| 36 | max = a*v1.y - b*v1.z; \ | ||
| 37 | if(min>max) {const float tmp=max; max=min; min=tmp; } \ | ||
| 38 | rad = fa * extents.y + fb * extents.z; \ | ||
| 39 | if(min>rad || max<-rad) return FALSE; | ||
| 40 | |||
| 41 | //! TO BE DOCUMENTED | ||
| 42 | #define AXISTEST_Y02(a, b, fa, fb) \ | ||
| 43 | min = b*v0.z - a*v0.x; \ | ||
| 44 | max = b*v2.z - a*v2.x; \ | ||
| 45 | if(min>max) {const float tmp=max; max=min; min=tmp; } \ | ||
| 46 | rad = fa * extents.x + fb * extents.z; \ | ||
| 47 | if(min>rad || max<-rad) return FALSE; | ||
| 48 | |||
| 49 | //! TO BE DOCUMENTED | ||
| 50 | #define AXISTEST_Y1(a, b, fa, fb) \ | ||
| 51 | min = b*v0.z - a*v0.x; \ | ||
| 52 | max = b*v1.z - a*v1.x; \ | ||
| 53 | if(min>max) {const float tmp=max; max=min; min=tmp; } \ | ||
| 54 | rad = fa * extents.x + fb * extents.z; \ | ||
| 55 | if(min>rad || max<-rad) return FALSE; | ||
| 56 | |||
| 57 | //! TO BE DOCUMENTED | ||
| 58 | #define AXISTEST_Z12(a, b, fa, fb) \ | ||
| 59 | min = a*v1.x - b*v1.y; \ | ||
| 60 | max = a*v2.x - b*v2.y; \ | ||
| 61 | if(min>max) {const float tmp=max; max=min; min=tmp; } \ | ||
| 62 | rad = fa * extents.x + fb * extents.y; \ | ||
| 63 | if(min>rad || max<-rad) return FALSE; | ||
| 64 | |||
| 65 | //! TO BE DOCUMENTED | ||
| 66 | #define AXISTEST_Z0(a, b, fa, fb) \ | ||
| 67 | min = a*v0.x - b*v0.y; \ | ||
| 68 | max = a*v1.x - b*v1.y; \ | ||
| 69 | if(min>max) {const float tmp=max; max=min; min=tmp; } \ | ||
| 70 | rad = fa * extents.x + fb * extents.y; \ | ||
| 71 | if(min>rad || max<-rad) return FALSE; | ||
| 72 | |||
| 73 | // compute triangle edges | ||
| 74 | // - edges lazy evaluated to take advantage of early exits | ||
| 75 | // - fabs precomputed (half less work, possible since extents are always >0) | ||
| 76 | // - customized macros to take advantage of the null component | ||
| 77 | // - axis vector discarded, possibly saves useless movs | ||
| 78 | #define IMPLEMENT_CLASS3_TESTS \ | ||
| 79 | float rad; \ | ||
| 80 | float min, max; \ | ||
| 81 | \ | ||
| 82 | const float fey0 = fabsf(e0.y); \ | ||
| 83 | const float fez0 = fabsf(e0.z); \ | ||
| 84 | AXISTEST_X01(e0.z, e0.y, fez0, fey0); \ | ||
| 85 | const float fex0 = fabsf(e0.x); \ | ||
| 86 | AXISTEST_Y02(e0.z, e0.x, fez0, fex0); \ | ||
| 87 | AXISTEST_Z12(e0.y, e0.x, fey0, fex0); \ | ||
| 88 | \ | ||
| 89 | const float fey1 = fabsf(e1.y); \ | ||
| 90 | const float fez1 = fabsf(e1.z); \ | ||
| 91 | AXISTEST_X01(e1.z, e1.y, fez1, fey1); \ | ||
| 92 | const float fex1 = fabsf(e1.x); \ | ||
| 93 | AXISTEST_Y02(e1.z, e1.x, fez1, fex1); \ | ||
| 94 | AXISTEST_Z0(e1.y, e1.x, fey1, fex1); \ | ||
| 95 | \ | ||
| 96 | const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \ | ||
| 97 | const float fey2 = fabsf(e2.y); \ | ||
| 98 | const float fez2 = fabsf(e2.z); \ | ||
| 99 | AXISTEST_X2(e2.z, e2.y, fez2, fey2); \ | ||
| 100 | const float fex2 = fabsf(e2.x); \ | ||
| 101 | AXISTEST_Y1(e2.z, e2.x, fez2, fex2); \ | ||
| 102 | AXISTEST_Z12(e2.y, e2.x, fey2, fex2); | ||
| 103 | |||
| 104 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 105 | /** | ||
| 106 | * Triangle-Box overlap test using the separating axis theorem. | ||
| 107 | * This is the code from Tomas Möller, a bit optimized: | ||
| 108 | * - with some more lazy evaluation (faster path on PC) | ||
| 109 | * - with a tiny bit of assembly | ||
| 110 | * - with "SAT-lite" applied if needed | ||
| 111 | * - and perhaps with some more minor modifs... | ||
| 112 | * | ||
| 113 | * \param center [in] box center | ||
| 114 | * \param extents [in] box extents | ||
| 115 | * \return true if triangle & box overlap | ||
| 116 | */ | ||
| 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 118 | inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents) | ||
| 119 | { | ||
| 120 | // Stats | ||
| 121 | mNbBVPrimTests++; | ||
| 122 | |||
| 123 | // use separating axis theorem to test overlap between triangle and box | ||
| 124 | // need to test for overlap in these directions: | ||
| 125 | // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle | ||
| 126 | // we do not even need to test these) | ||
| 127 | // 2) normal of the triangle | ||
| 128 | // 3) crossproduct(edge from tri, {x,y,z}-directin) | ||
| 129 | // this gives 3x3=9 more tests | ||
| 130 | |||
| 131 | // move everything so that the boxcenter is in (0,0,0) | ||
| 132 | Point v0, v1, v2; | ||
| 133 | v0.x = mLeafVerts[0].x - center.x; | ||
| 134 | v1.x = mLeafVerts[1].x - center.x; | ||
| 135 | v2.x = mLeafVerts[2].x - center.x; | ||
| 136 | |||
| 137 | // First, test overlap in the {x,y,z}-directions | ||
| 138 | #ifdef OPC_USE_FCOMI | ||
| 139 | // find min, max of the triangle in x-direction, and test for overlap in X | ||
| 140 | if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE; | ||
| 141 | if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE; | ||
| 142 | |||
| 143 | // same for Y | ||
| 144 | v0.y = mLeafVerts[0].y - center.y; | ||
| 145 | v1.y = mLeafVerts[1].y - center.y; | ||
| 146 | v2.y = mLeafVerts[2].y - center.y; | ||
| 147 | |||
| 148 | if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE; | ||
| 149 | if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE; | ||
| 150 | |||
| 151 | // same for Z | ||
| 152 | v0.z = mLeafVerts[0].z - center.z; | ||
| 153 | v1.z = mLeafVerts[1].z - center.z; | ||
| 154 | v2.z = mLeafVerts[2].z - center.z; | ||
| 155 | |||
| 156 | if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE; | ||
| 157 | if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE; | ||
| 158 | #else | ||
| 159 | float min,max; | ||
| 160 | // Find min, max of the triangle in x-direction, and test for overlap in X | ||
| 161 | FINDMINMAX(v0.x, v1.x, v2.x, min, max); | ||
| 162 | if(min>extents.x || max<-extents.x) return FALSE; | ||
| 163 | |||
| 164 | // Same for Y | ||
| 165 | v0.y = mLeafVerts[0].y - center.y; | ||
| 166 | v1.y = mLeafVerts[1].y - center.y; | ||
| 167 | v2.y = mLeafVerts[2].y - center.y; | ||
| 168 | |||
| 169 | FINDMINMAX(v0.y, v1.y, v2.y, min, max); | ||
| 170 | if(min>extents.y || max<-extents.y) return FALSE; | ||
| 171 | |||
| 172 | // Same for Z | ||
| 173 | v0.z = mLeafVerts[0].z - center.z; | ||
| 174 | v1.z = mLeafVerts[1].z - center.z; | ||
| 175 | v2.z = mLeafVerts[2].z - center.z; | ||
| 176 | |||
| 177 | FINDMINMAX(v0.z, v1.z, v2.z, min, max); | ||
| 178 | if(min>extents.z || max<-extents.z) return FALSE; | ||
| 179 | #endif | ||
| 180 | // 2) Test if the box intersects the plane of the triangle | ||
| 181 | // compute plane equation of triangle: normal*x+d=0 | ||
| 182 | // ### could be precomputed since we use the same leaf triangle several times | ||
| 183 | const Point e0 = v1 - v0; | ||
| 184 | const Point e1 = v2 - v1; | ||
| 185 | const Point normal = e0 ^ e1; | ||
| 186 | const float d = -normal|v0; | ||
| 187 | if(!planeBoxOverlap(normal, d, extents)) return FALSE; | ||
| 188 | |||
| 189 | // 3) "Class III" tests | ||
| 190 | if(mFullPrimBoxTest) | ||
| 191 | { | ||
| 192 | IMPLEMENT_CLASS3_TESTS | ||
| 193 | } | ||
| 194 | return TRUE; | ||
| 195 | } | ||
| 196 | |||
| 197 | //! A dedicated version where the box is constant | ||
| 198 | inline_ BOOL OBBCollider::TriBoxOverlap() | ||
| 199 | { | ||
| 200 | // Stats | ||
| 201 | mNbVolumePrimTests++; | ||
| 202 | |||
| 203 | // Hook | ||
| 204 | const Point& extents = mBoxExtents; | ||
| 205 | const Point& v0 = mLeafVerts[0]; | ||
| 206 | const Point& v1 = mLeafVerts[1]; | ||
| 207 | const Point& v2 = mLeafVerts[2]; | ||
| 208 | |||
| 209 | // use separating axis theorem to test overlap between triangle and box | ||
| 210 | // need to test for overlap in these directions: | ||
| 211 | // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle | ||
| 212 | // we do not even need to test these) | ||
| 213 | // 2) normal of the triangle | ||
| 214 | // 3) crossproduct(edge from tri, {x,y,z}-directin) | ||
| 215 | // this gives 3x3=9 more tests | ||
| 216 | |||
| 217 | // Box center is already in (0,0,0) | ||
| 218 | |||
| 219 | // First, test overlap in the {x,y,z}-directions | ||
| 220 | #ifdef OPC_USE_FCOMI | ||
| 221 | // find min, max of the triangle in x-direction, and test for overlap in X | ||
| 222 | if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x) return FALSE; | ||
| 223 | if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x) return FALSE; | ||
| 224 | |||
| 225 | if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y) return FALSE; | ||
| 226 | if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y) return FALSE; | ||
| 227 | |||
| 228 | if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z) return FALSE; | ||
| 229 | if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z) return FALSE; | ||
| 230 | #else | ||
| 231 | float min,max; | ||
| 232 | // Find min, max of the triangle in x-direction, and test for overlap in X | ||
| 233 | FINDMINMAX(v0.x, v1.x, v2.x, min, max); | ||
| 234 | if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE; | ||
| 235 | |||
| 236 | FINDMINMAX(v0.y, v1.y, v2.y, min, max); | ||
| 237 | if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE; | ||
| 238 | |||
| 239 | FINDMINMAX(v0.z, v1.z, v2.z, min, max); | ||
| 240 | if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE; | ||
| 241 | #endif | ||
| 242 | // 2) Test if the box intersects the plane of the triangle | ||
| 243 | // compute plane equation of triangle: normal*x+d=0 | ||
| 244 | // ### could be precomputed since we use the same leaf triangle several times | ||
| 245 | const Point e0 = v1 - v0; | ||
| 246 | const Point e1 = v2 - v1; | ||
| 247 | const Point normal = e0 ^ e1; | ||
| 248 | const float d = -normal|v0; | ||
| 249 | if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE; | ||
| 250 | |||
| 251 | // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV) | ||
| 252 | { | ||
| 253 | IMPLEMENT_CLASS3_TESTS | ||
| 254 | } | ||
| 255 | return TRUE; | ||
| 256 | } | ||
| 257 | |||
| 258 | //! ...and another one, jeez | ||
| 259 | inline_ BOOL AABBCollider::TriBoxOverlap() | ||
| 260 | { | ||
| 261 | // Stats | ||
| 262 | mNbVolumePrimTests++; | ||
| 263 | |||
| 264 | // Hook | ||
| 265 | const Point& center = mBox.mCenter; | ||
| 266 | const Point& extents = mBox.mExtents; | ||
| 267 | |||
| 268 | // use separating axis theorem to test overlap between triangle and box | ||
| 269 | // need to test for overlap in these directions: | ||
| 270 | // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle | ||
| 271 | // we do not even need to test these) | ||
| 272 | // 2) normal of the triangle | ||
| 273 | // 3) crossproduct(edge from tri, {x,y,z}-directin) | ||
| 274 | // this gives 3x3=9 more tests | ||
| 275 | |||
| 276 | // move everything so that the boxcenter is in (0,0,0) | ||
| 277 | Point v0, v1, v2; | ||
| 278 | v0.x = mLeafVerts[0].x - center.x; | ||
| 279 | v1.x = mLeafVerts[1].x - center.x; | ||
| 280 | v2.x = mLeafVerts[2].x - center.x; | ||
| 281 | |||
| 282 | // First, test overlap in the {x,y,z}-directions | ||
| 283 | #ifdef OPC_USE_FCOMI | ||
| 284 | // find min, max of the triangle in x-direction, and test for overlap in X | ||
| 285 | if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE; | ||
| 286 | if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE; | ||
| 287 | |||
| 288 | // same for Y | ||
| 289 | v0.y = mLeafVerts[0].y - center.y; | ||
| 290 | v1.y = mLeafVerts[1].y - center.y; | ||
| 291 | v2.y = mLeafVerts[2].y - center.y; | ||
| 292 | |||
| 293 | if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE; | ||
| 294 | if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE; | ||
| 295 | |||
| 296 | // same for Z | ||
| 297 | v0.z = mLeafVerts[0].z - center.z; | ||
| 298 | v1.z = mLeafVerts[1].z - center.z; | ||
| 299 | v2.z = mLeafVerts[2].z - center.z; | ||
| 300 | |||
| 301 | if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE; | ||
| 302 | if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE; | ||
| 303 | #else | ||
| 304 | float min,max; | ||
| 305 | // Find min, max of the triangle in x-direction, and test for overlap in X | ||
| 306 | FINDMINMAX(v0.x, v1.x, v2.x, min, max); | ||
| 307 | if(min>extents.x || max<-extents.x) return FALSE; | ||
| 308 | |||
| 309 | // Same for Y | ||
| 310 | v0.y = mLeafVerts[0].y - center.y; | ||
| 311 | v1.y = mLeafVerts[1].y - center.y; | ||
| 312 | v2.y = mLeafVerts[2].y - center.y; | ||
| 313 | |||
| 314 | FINDMINMAX(v0.y, v1.y, v2.y, min, max); | ||
| 315 | if(min>extents.y || max<-extents.y) return FALSE; | ||
| 316 | |||
| 317 | // Same for Z | ||
| 318 | v0.z = mLeafVerts[0].z - center.z; | ||
| 319 | v1.z = mLeafVerts[1].z - center.z; | ||
| 320 | v2.z = mLeafVerts[2].z - center.z; | ||
| 321 | |||
| 322 | FINDMINMAX(v0.z, v1.z, v2.z, min, max); | ||
| 323 | if(min>extents.z || max<-extents.z) return FALSE; | ||
| 324 | #endif | ||
| 325 | // 2) Test if the box intersects the plane of the triangle | ||
| 326 | // compute plane equation of triangle: normal*x+d=0 | ||
| 327 | // ### could be precomputed since we use the same leaf triangle several times | ||
| 328 | const Point e0 = v1 - v0; | ||
| 329 | const Point e1 = v2 - v1; | ||
| 330 | const Point normal = e0 ^ e1; | ||
| 331 | const float d = -normal|v0; | ||
| 332 | if(!planeBoxOverlap(normal, d, extents)) return FALSE; | ||
| 333 | |||
| 334 | // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV) | ||
| 335 | { | ||
| 336 | IMPLEMENT_CLASS3_TESTS | ||
| 337 | } | ||
| 338 | return TRUE; | ||
| 339 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_TriTriOverlap.h b/libraries/ode-0.9/OPCODE/OPC_TriTriOverlap.h new file mode 100644 index 0000000..1e71c6a --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_TriTriOverlap.h | |||
| @@ -0,0 +1,279 @@ | |||
| 1 | |||
| 2 | //! if OPC_TRITRI_EPSILON_TEST is true then we do a check (if |dv|<EPSILON then dv=0.0;) else no check is done (which is less robust, but faster) | ||
| 3 | #define LOCAL_EPSILON 0.000001f | ||
| 4 | |||
| 5 | //! sort so that a<=b | ||
| 6 | #define SORT(a,b) \ | ||
| 7 | if(a>b) \ | ||
| 8 | { \ | ||
| 9 | const float c=a; \ | ||
| 10 | a=b; \ | ||
| 11 | b=c; \ | ||
| 12 | } | ||
| 13 | |||
| 14 | //! Edge to edge test based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202 | ||
| 15 | #define EDGE_EDGE_TEST(V0, U0, U1) \ | ||
| 16 | Bx = U0[i0] - U1[i0]; \ | ||
| 17 | By = U0[i1] - U1[i1]; \ | ||
| 18 | Cx = V0[i0] - U0[i0]; \ | ||
| 19 | Cy = V0[i1] - U0[i1]; \ | ||
| 20 | f = Ay*Bx - Ax*By; \ | ||
| 21 | d = By*Cx - Bx*Cy; \ | ||
| 22 | if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f)) \ | ||
| 23 | { \ | ||
| 24 | const float e=Ax*Cy - Ay*Cx; \ | ||
| 25 | if(f>0.0f) \ | ||
| 26 | { \ | ||
| 27 | if(e>=0.0f && e<=f) return TRUE; \ | ||
| 28 | } \ | ||
| 29 | else \ | ||
| 30 | { \ | ||
| 31 | if(e<=0.0f && e>=f) return TRUE; \ | ||
| 32 | } \ | ||
| 33 | } | ||
| 34 | |||
| 35 | //! TO BE DOCUMENTED | ||
| 36 | #define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2) \ | ||
| 37 | { \ | ||
| 38 | float Bx,By,Cx,Cy,d,f; \ | ||
| 39 | const float Ax = V1[i0] - V0[i0]; \ | ||
| 40 | const float Ay = V1[i1] - V0[i1]; \ | ||
| 41 | /* test edge U0,U1 against V0,V1 */ \ | ||
| 42 | EDGE_EDGE_TEST(V0, U0, U1); \ | ||
| 43 | /* test edge U1,U2 against V0,V1 */ \ | ||
| 44 | EDGE_EDGE_TEST(V0, U1, U2); \ | ||
| 45 | /* test edge U2,U1 against V0,V1 */ \ | ||
| 46 | EDGE_EDGE_TEST(V0, U2, U0); \ | ||
| 47 | } | ||
| 48 | |||
| 49 | //! TO BE DOCUMENTED | ||
| 50 | #define POINT_IN_TRI(V0, U0, U1, U2) \ | ||
| 51 | { \ | ||
| 52 | /* is T1 completly inside T2? */ \ | ||
| 53 | /* check if V0 is inside tri(U0,U1,U2) */ \ | ||
| 54 | float a = U1[i1] - U0[i1]; \ | ||
| 55 | float b = -(U1[i0] - U0[i0]); \ | ||
| 56 | float c = -a*U0[i0] - b*U0[i1]; \ | ||
| 57 | float d0 = a*V0[i0] + b*V0[i1] + c; \ | ||
| 58 | \ | ||
| 59 | a = U2[i1] - U1[i1]; \ | ||
| 60 | b = -(U2[i0] - U1[i0]); \ | ||
| 61 | c = -a*U1[i0] - b*U1[i1]; \ | ||
| 62 | const float d1 = a*V0[i0] + b*V0[i1] + c; \ | ||
| 63 | \ | ||
| 64 | a = U0[i1] - U2[i1]; \ | ||
| 65 | b = -(U0[i0] - U2[i0]); \ | ||
| 66 | c = -a*U2[i0] - b*U2[i1]; \ | ||
| 67 | const float d2 = a*V0[i0] + b*V0[i1] + c; \ | ||
| 68 | if(d0*d1>0.0f) \ | ||
| 69 | { \ | ||
| 70 | if(d0*d2>0.0f) return TRUE; \ | ||
| 71 | } \ | ||
| 72 | } | ||
| 73 | |||
| 74 | //! TO BE DOCUMENTED | ||
| 75 | BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2) | ||
| 76 | { | ||
| 77 | float A[3]; | ||
| 78 | short i0,i1; | ||
| 79 | /* first project onto an axis-aligned plane, that maximizes the area */ | ||
| 80 | /* of the triangles, compute indices: i0,i1. */ | ||
| 81 | A[0] = fabsf(n[0]); | ||
| 82 | A[1] = fabsf(n[1]); | ||
| 83 | A[2] = fabsf(n[2]); | ||
| 84 | if(A[0]>A[1]) | ||
| 85 | { | ||
| 86 | if(A[0]>A[2]) | ||
| 87 | { | ||
| 88 | i0=1; /* A[0] is greatest */ | ||
| 89 | i1=2; | ||
| 90 | } | ||
| 91 | else | ||
| 92 | { | ||
| 93 | i0=0; /* A[2] is greatest */ | ||
| 94 | i1=1; | ||
| 95 | } | ||
| 96 | } | ||
| 97 | else /* A[0]<=A[1] */ | ||
| 98 | { | ||
| 99 | if(A[2]>A[1]) | ||
| 100 | { | ||
| 101 | i0=0; /* A[2] is greatest */ | ||
| 102 | i1=1; | ||
| 103 | } | ||
| 104 | else | ||
| 105 | { | ||
| 106 | i0=0; /* A[1] is greatest */ | ||
| 107 | i1=2; | ||
| 108 | } | ||
| 109 | } | ||
| 110 | |||
| 111 | /* test all edges of triangle 1 against the edges of triangle 2 */ | ||
| 112 | EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2); | ||
| 113 | EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2); | ||
| 114 | EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2); | ||
| 115 | |||
| 116 | /* finally, test if tri1 is totally contained in tri2 or vice versa */ | ||
| 117 | POINT_IN_TRI(v0, u0, u1, u2); | ||
| 118 | POINT_IN_TRI(u0, v0, v1, v2); | ||
| 119 | |||
| 120 | return FALSE; | ||
| 121 | } | ||
| 122 | |||
| 123 | //! TO BE DOCUMENTED | ||
| 124 | #define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1) \ | ||
| 125 | { \ | ||
| 126 | if(D0D1>0.0f) \ | ||
| 127 | { \ | ||
| 128 | /* here we know that D0D2<=0.0 */ \ | ||
| 129 | /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ | ||
| 130 | A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \ | ||
| 131 | } \ | ||
| 132 | else if(D0D2>0.0f) \ | ||
| 133 | { \ | ||
| 134 | /* here we know that d0d1<=0.0 */ \ | ||
| 135 | A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \ | ||
| 136 | } \ | ||
| 137 | else if(D1*D2>0.0f || D0!=0.0f) \ | ||
| 138 | { \ | ||
| 139 | /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ | ||
| 140 | A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2; \ | ||
| 141 | } \ | ||
| 142 | else if(D1!=0.0f) \ | ||
| 143 | { \ | ||
| 144 | A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \ | ||
| 145 | } \ | ||
| 146 | else if(D2!=0.0f) \ | ||
| 147 | { \ | ||
| 148 | A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \ | ||
| 149 | } \ | ||
| 150 | else \ | ||
| 151 | { \ | ||
| 152 | /* triangles are coplanar */ \ | ||
| 153 | return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2); \ | ||
| 154 | } \ | ||
| 155 | } | ||
| 156 | |||
| 157 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 158 | /** | ||
| 159 | * Triangle/triangle intersection test routine, | ||
| 160 | * by Tomas Moller, 1997. | ||
| 161 | * See article "A Fast Triangle-Triangle Intersection Test", | ||
| 162 | * Journal of Graphics Tools, 2(2), 1997 | ||
| 163 | * | ||
| 164 | * Updated June 1999: removed the divisions -- a little faster now! | ||
| 165 | * Updated October 1999: added {} to CROSS and SUB macros | ||
| 166 | * | ||
| 167 | * int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3], | ||
| 168 | * float U0[3],float U1[3],float U2[3]) | ||
| 169 | * | ||
| 170 | * \param V0 [in] triangle 0, vertex 0 | ||
| 171 | * \param V1 [in] triangle 0, vertex 1 | ||
| 172 | * \param V2 [in] triangle 0, vertex 2 | ||
| 173 | * \param U0 [in] triangle 1, vertex 0 | ||
| 174 | * \param U1 [in] triangle 1, vertex 1 | ||
| 175 | * \param U2 [in] triangle 1, vertex 2 | ||
| 176 | * \return true if triangles overlap | ||
| 177 | */ | ||
| 178 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 179 | inline_ BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2) | ||
| 180 | { | ||
| 181 | // Stats | ||
| 182 | mNbPrimPrimTests++; | ||
| 183 | |||
| 184 | // Compute plane equation of triangle(V0,V1,V2) | ||
| 185 | Point E1 = V1 - V0; | ||
| 186 | Point E2 = V2 - V0; | ||
| 187 | const Point N1 = E1 ^ E2; | ||
| 188 | const float d1 =-N1 | V0; | ||
| 189 | // Plane equation 1: N1.X+d1=0 | ||
| 190 | |||
| 191 | // Put U0,U1,U2 into plane equation 1 to compute signed distances to the plane | ||
| 192 | float du0 = (N1|U0) + d1; | ||
| 193 | float du1 = (N1|U1) + d1; | ||
| 194 | float du2 = (N1|U2) + d1; | ||
| 195 | |||
| 196 | // Coplanarity robustness check | ||
| 197 | #ifdef OPC_TRITRI_EPSILON_TEST | ||
| 198 | if(fabsf(du0)<LOCAL_EPSILON) du0 = 0.0f; | ||
| 199 | if(fabsf(du1)<LOCAL_EPSILON) du1 = 0.0f; | ||
| 200 | if(fabsf(du2)<LOCAL_EPSILON) du2 = 0.0f; | ||
| 201 | #endif | ||
| 202 | const float du0du1 = du0 * du1; | ||
| 203 | const float du0du2 = du0 * du2; | ||
| 204 | |||
| 205 | if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? | ||
| 206 | return FALSE; // no intersection occurs | ||
| 207 | |||
| 208 | // Compute plane of triangle (U0,U1,U2) | ||
| 209 | E1 = U1 - U0; | ||
| 210 | E2 = U2 - U0; | ||
| 211 | const Point N2 = E1 ^ E2; | ||
| 212 | const float d2=-N2 | U0; | ||
| 213 | // plane equation 2: N2.X+d2=0 | ||
| 214 | |||
| 215 | // put V0,V1,V2 into plane equation 2 | ||
| 216 | float dv0 = (N2|V0) + d2; | ||
| 217 | float dv1 = (N2|V1) + d2; | ||
| 218 | float dv2 = (N2|V2) + d2; | ||
| 219 | |||
| 220 | #ifdef OPC_TRITRI_EPSILON_TEST | ||
| 221 | if(fabsf(dv0)<LOCAL_EPSILON) dv0 = 0.0f; | ||
| 222 | if(fabsf(dv1)<LOCAL_EPSILON) dv1 = 0.0f; | ||
| 223 | if(fabsf(dv2)<LOCAL_EPSILON) dv2 = 0.0f; | ||
| 224 | #endif | ||
| 225 | |||
| 226 | const float dv0dv1 = dv0 * dv1; | ||
| 227 | const float dv0dv2 = dv0 * dv2; | ||
| 228 | |||
| 229 | if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? | ||
| 230 | return FALSE; // no intersection occurs | ||
| 231 | |||
| 232 | // Compute direction of intersection line | ||
| 233 | const Point D = N1^N2; | ||
| 234 | |||
| 235 | // Compute and index to the largest component of D | ||
| 236 | float max=fabsf(D[0]); | ||
| 237 | short index=0; | ||
| 238 | float bb=fabsf(D[1]); | ||
| 239 | float cc=fabsf(D[2]); | ||
| 240 | if(bb>max) max=bb,index=1; | ||
| 241 | if(cc>max) max=cc,index=2; | ||
| 242 | |||
| 243 | // This is the simplified projection onto L | ||
| 244 | const float vp0 = V0[index]; | ||
| 245 | const float vp1 = V1[index]; | ||
| 246 | const float vp2 = V2[index]; | ||
| 247 | |||
| 248 | const float up0 = U0[index]; | ||
| 249 | const float up1 = U1[index]; | ||
| 250 | const float up2 = U2[index]; | ||
| 251 | |||
| 252 | // Compute interval for triangle 1 | ||
| 253 | float a,b,c,x0,x1; | ||
| 254 | NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1); | ||
| 255 | |||
| 256 | // Compute interval for triangle 2 | ||
| 257 | float d,e,f,y0,y1; | ||
| 258 | NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1); | ||
| 259 | |||
| 260 | const float xx=x0*x1; | ||
| 261 | const float yy=y0*y1; | ||
| 262 | const float xxyy=xx*yy; | ||
| 263 | |||
| 264 | float isect1[2], isect2[2]; | ||
| 265 | |||
| 266 | float tmp=a*xxyy; | ||
| 267 | isect1[0]=tmp+b*x1*yy; | ||
| 268 | isect1[1]=tmp+c*x0*yy; | ||
| 269 | |||
| 270 | tmp=d*xxyy; | ||
| 271 | isect2[0]=tmp+e*xx*y1; | ||
| 272 | isect2[1]=tmp+f*xx*y0; | ||
| 273 | |||
| 274 | SORT(isect1[0],isect1[1]); | ||
| 275 | SORT(isect2[0],isect2[1]); | ||
| 276 | |||
| 277 | if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return FALSE; | ||
| 278 | return TRUE; | ||
| 279 | } | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_VolumeCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_VolumeCollider.cpp new file mode 100644 index 0000000..f374209 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_VolumeCollider.cpp | |||
| @@ -0,0 +1,103 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains base volume collider class. | ||
| 12 | * \file OPC_VolumeCollider.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | /** | ||
| 20 | * Contains the abstract class for volume colliders. | ||
| 21 | * | ||
| 22 | * \class VolumeCollider | ||
| 23 | * \author Pierre Terdiman | ||
| 24 | * \version 1.3 | ||
| 25 | * \date June, 2, 2001 | ||
| 26 | */ | ||
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 28 | |||
| 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 30 | // Precompiled Header | ||
| 31 | #include "Stdafx.h" | ||
| 32 | |||
| 33 | using namespace Opcode; | ||
| 34 | |||
| 35 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 36 | /** | ||
| 37 | * Constructor. | ||
| 38 | */ | ||
| 39 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 40 | VolumeCollider::VolumeCollider() : | ||
| 41 | mTouchedPrimitives (null), | ||
| 42 | mNbVolumeBVTests (0), | ||
| 43 | mNbVolumePrimTests (0) | ||
| 44 | { | ||
| 45 | } | ||
| 46 | |||
| 47 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 48 | /** | ||
| 49 | * Destructor. | ||
| 50 | */ | ||
| 51 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 52 | VolumeCollider::~VolumeCollider() | ||
| 53 | { | ||
| 54 | mTouchedPrimitives = null; | ||
| 55 | } | ||
| 56 | |||
| 57 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 58 | /** | ||
| 59 | * Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider. | ||
| 60 | * \return null if everything is ok, else a string describing the problem | ||
| 61 | */ | ||
| 62 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 63 | const char* VolumeCollider::ValidateSettings() | ||
| 64 | { | ||
| 65 | return null; | ||
| 66 | } | ||
| 67 | |||
| 68 | // Pretty dumb way to dump - to do better - one day... | ||
| 69 | |||
| 70 | #define IMPLEMENT_NOLEAFDUMP(type) \ | ||
| 71 | void VolumeCollider::_Dump(const type* node) \ | ||
| 72 | { \ | ||
| 73 | if(node->HasPosLeaf()) mTouchedPrimitives->Add(udword(node->GetPosPrimitive())); \ | ||
| 74 | else _Dump(node->GetPos()); \ | ||
| 75 | \ | ||
| 76 | if(ContactFound()) return; \ | ||
| 77 | \ | ||
| 78 | if(node->HasNegLeaf()) mTouchedPrimitives->Add(udword(node->GetNegPrimitive())); \ | ||
| 79 | else _Dump(node->GetNeg()); \ | ||
| 80 | } | ||
| 81 | |||
| 82 | #define IMPLEMENT_LEAFDUMP(type) \ | ||
| 83 | void VolumeCollider::_Dump(const type* node) \ | ||
| 84 | { \ | ||
| 85 | if(node->IsLeaf()) \ | ||
| 86 | { \ | ||
| 87 | mTouchedPrimitives->Add(udword(node->GetPrimitive())); \ | ||
| 88 | } \ | ||
| 89 | else \ | ||
| 90 | { \ | ||
| 91 | _Dump(node->GetPos()); \ | ||
| 92 | \ | ||
| 93 | if(ContactFound()) return; \ | ||
| 94 | \ | ||
| 95 | _Dump(node->GetNeg()); \ | ||
| 96 | } \ | ||
| 97 | } | ||
| 98 | |||
| 99 | IMPLEMENT_NOLEAFDUMP(AABBNoLeafNode) | ||
| 100 | IMPLEMENT_NOLEAFDUMP(AABBQuantizedNoLeafNode) | ||
| 101 | |||
| 102 | IMPLEMENT_LEAFDUMP(AABBCollisionNode) | ||
| 103 | IMPLEMENT_LEAFDUMP(AABBQuantizedNode) | ||
diff --git a/libraries/ode-0.9/OPCODE/OPC_VolumeCollider.h b/libraries/ode-0.9/OPCODE/OPC_VolumeCollider.h new file mode 100644 index 0000000..c0b812e --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_VolumeCollider.h | |||
| @@ -0,0 +1,138 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Contains base volume collider class. | ||
| 12 | * \file OPC_VolumeCollider.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date June, 2, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPC_VOLUMECOLLIDER_H__ | ||
| 21 | #define __OPC_VOLUMECOLLIDER_H__ | ||
| 22 | |||
| 23 | struct OPCODE_API VolumeCache | ||
| 24 | { | ||
| 25 | VolumeCache() : Model(null) {} | ||
| 26 | ~VolumeCache() {} | ||
| 27 | |||
| 28 | Container TouchedPrimitives; //!< Indices of touched primitives | ||
| 29 | const BaseModel* Model; //!< Owner | ||
| 30 | }; | ||
| 31 | |||
| 32 | class OPCODE_API VolumeCollider : public Collider | ||
| 33 | { | ||
| 34 | public: | ||
| 35 | // Constructor / Destructor | ||
| 36 | VolumeCollider(); | ||
| 37 | virtual ~VolumeCollider() = 0; | ||
| 38 | |||
| 39 | // Collision report | ||
| 40 | |||
| 41 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 42 | /** | ||
| 43 | * Gets the number of touched primitives after a collision query. | ||
| 44 | * \see GetContactStatus() | ||
| 45 | * \see GetTouchedPrimitives() | ||
| 46 | * \return the number of touched primitives | ||
| 47 | */ | ||
| 48 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 49 | inline_ udword GetNbTouchedPrimitives() const { return mTouchedPrimitives ? mTouchedPrimitives->GetNbEntries() : 0; } | ||
| 50 | |||
| 51 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 52 | /** | ||
| 53 | * Gets the list of touched primitives after a collision query. | ||
| 54 | * \see GetContactStatus() | ||
| 55 | * \see GetNbTouchedPrimitives() | ||
| 56 | * \return the list of touched primitives (primitive indices) | ||
| 57 | */ | ||
| 58 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 59 | inline_ const udword* GetTouchedPrimitives() const { return mTouchedPrimitives ? mTouchedPrimitives->GetEntries() : null; } | ||
| 60 | |||
| 61 | // Stats | ||
| 62 | |||
| 63 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 64 | /** | ||
| 65 | * Stats: gets the number of Volume-BV overlap tests after a collision query. | ||
| 66 | * \see GetNbVolumePrimTests() | ||
| 67 | * \return the number of Volume-BV tests performed during last query | ||
| 68 | */ | ||
| 69 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 70 | inline_ udword GetNbVolumeBVTests() const { return mNbVolumeBVTests; } | ||
| 71 | |||
| 72 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 73 | /** | ||
| 74 | * Stats: gets the number of Volume-Triangle overlap tests after a collision query. | ||
| 75 | * \see GetNbVolumeBVTests() | ||
| 76 | * \return the number of Volume-Triangle tests performed during last query | ||
| 77 | */ | ||
| 78 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 79 | inline_ udword GetNbVolumePrimTests() const { return mNbVolumePrimTests; } | ||
| 80 | |||
| 81 | // Settings | ||
| 82 | |||
| 83 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 84 | /** | ||
| 85 | * Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider. | ||
| 86 | * \return null if everything is ok, else a string describing the problem | ||
| 87 | */ | ||
| 88 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 89 | override(Collider) const char* ValidateSettings(); | ||
| 90 | |||
| 91 | protected: | ||
| 92 | // Touched primitives | ||
| 93 | Container* mTouchedPrimitives; //!< List of touched primitives | ||
| 94 | |||
| 95 | // Dequantization coeffs | ||
| 96 | Point mCenterCoeff; | ||
| 97 | Point mExtentsCoeff; | ||
| 98 | // Stats | ||
| 99 | udword mNbVolumeBVTests; //!< Number of Volume-BV tests | ||
| 100 | udword mNbVolumePrimTests; //!< Number of Volume-Primitive tests | ||
| 101 | // Internal methods | ||
| 102 | void _Dump(const AABBCollisionNode* node); | ||
| 103 | void _Dump(const AABBNoLeafNode* node); | ||
| 104 | void _Dump(const AABBQuantizedNode* node); | ||
| 105 | void _Dump(const AABBQuantizedNoLeafNode* node); | ||
| 106 | |||
| 107 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 108 | /** | ||
| 109 | * Initializes a query | ||
| 110 | */ | ||
| 111 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 112 | override(Collider) inline_ void InitQuery() | ||
| 113 | { | ||
| 114 | // Reset stats & contact status | ||
| 115 | mNbVolumeBVTests = 0; | ||
| 116 | mNbVolumePrimTests = 0; | ||
| 117 | Collider::InitQuery(); | ||
| 118 | } | ||
| 119 | |||
| 120 | inline_ BOOL IsCacheValid(VolumeCache& cache) | ||
| 121 | { | ||
| 122 | // We're going to do a volume-vs-model query. | ||
| 123 | if(cache.Model!=mCurrentModel) | ||
| 124 | { | ||
| 125 | // Cached list was for another model so we can't keep it | ||
| 126 | // Keep track of new owner and reset cache | ||
| 127 | cache.Model = mCurrentModel; | ||
| 128 | return FALSE; | ||
| 129 | } | ||
| 130 | else | ||
| 131 | { | ||
| 132 | // Same models, no problem | ||
| 133 | return TRUE; | ||
| 134 | } | ||
| 135 | } | ||
| 136 | }; | ||
| 137 | |||
| 138 | #endif // __OPC_VOLUMECOLLIDER_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/Opcode.cpp b/libraries/ode-0.9/OPCODE/Opcode.cpp new file mode 100644 index 0000000..c9fa104 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Opcode.cpp | |||
| @@ -0,0 +1,65 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Main file for Opcode.dll. | ||
| 12 | * \file Opcode.cpp | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /* | ||
| 19 | Finding a good name is difficult! | ||
| 20 | Here's the draft for this lib.... Spooky, uh? | ||
| 21 | |||
| 22 | VOID? Very Optimized Interference Detection | ||
| 23 | ZOID? Zappy's Optimized Interference Detection | ||
| 24 | CID? Custom/Clever Interference Detection | ||
| 25 | AID / ACID! Accurate Interference Detection | ||
| 26 | QUID? Quick Interference Detection | ||
| 27 | RIDE? Realtime Interference DEtection | ||
| 28 | WIDE? Wicked Interference DEtection (....) | ||
| 29 | GUID! | ||
| 30 | KID ! k-dop interference detection :) | ||
| 31 | OPCODE! OPtimized COllision DEtection | ||
| 32 | */ | ||
| 33 | |||
| 34 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 35 | // Precompiled Header | ||
| 36 | #include "Stdafx.h" | ||
| 37 | |||
| 38 | bool Opcode::InitOpcode() | ||
| 39 | { | ||
| 40 | Log("// Initializing OPCODE\n\n"); | ||
| 41 | // LogAPIInfo(); | ||
| 42 | return true; | ||
| 43 | } | ||
| 44 | |||
| 45 | void ReleasePruningSorters(); | ||
| 46 | bool Opcode::CloseOpcode() | ||
| 47 | { | ||
| 48 | Log("// Closing OPCODE\n\n"); | ||
| 49 | |||
| 50 | ReleasePruningSorters(); | ||
| 51 | |||
| 52 | return true; | ||
| 53 | } | ||
| 54 | |||
| 55 | #ifdef ICE_MAIN | ||
| 56 | |||
| 57 | void ModuleAttach(HINSTANCE hinstance) | ||
| 58 | { | ||
| 59 | } | ||
| 60 | |||
| 61 | void ModuleDetach() | ||
| 62 | { | ||
| 63 | } | ||
| 64 | |||
| 65 | #endif | ||
diff --git a/libraries/ode-0.9/OPCODE/Opcode.dsp b/libraries/ode-0.9/OPCODE/Opcode.dsp new file mode 100644 index 0000000..560cf56 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Opcode.dsp | |||
| @@ -0,0 +1,470 @@ | |||
| 1 | # Microsoft Developer Studio Project File - Name="OPCODE" - Package Owner=<4> | ||
| 2 | # Microsoft Developer Studio Generated Build File, Format Version 6.00 | ||
| 3 | # ** DO NOT EDIT ** | ||
| 4 | |||
| 5 | # TARGTYPE "Win32 (x86) Static Library" 0x0104 | ||
| 6 | |||
| 7 | CFG=OPCODE - Win32 Debug | ||
| 8 | !MESSAGE This is not a valid makefile. To build this project using NMAKE, | ||
| 9 | !MESSAGE use the Export Makefile command and run | ||
| 10 | !MESSAGE | ||
| 11 | !MESSAGE NMAKE /f "Opcode.mak". | ||
| 12 | !MESSAGE | ||
| 13 | !MESSAGE You can specify a configuration when running NMAKE | ||
| 14 | !MESSAGE by defining the macro CFG on the command line. For example: | ||
| 15 | !MESSAGE | ||
| 16 | !MESSAGE NMAKE /f "Opcode.mak" CFG="OPCODE - Win32 Debug" | ||
| 17 | !MESSAGE | ||
| 18 | !MESSAGE Possible choices for configuration are: | ||
| 19 | !MESSAGE | ||
| 20 | !MESSAGE "OPCODE - Win32 Release" (based on "Win32 (x86) Static Library") | ||
| 21 | !MESSAGE "OPCODE - Win32 Debug" (based on "Win32 (x86) Static Library") | ||
| 22 | !MESSAGE | ||
| 23 | |||
| 24 | # Begin Project | ||
| 25 | # PROP AllowPerConfigDependencies 0 | ||
| 26 | # PROP Scc_ProjName ""$/TR4/ODE/VC6", WNKAAAAA" | ||
| 27 | # PROP Scc_LocalPath "..\vc6" | ||
| 28 | CPP=cl.exe | ||
| 29 | RSC=rc.exe | ||
| 30 | |||
| 31 | !IF "$(CFG)" == "OPCODE - Win32 Release" | ||
| 32 | |||
| 33 | # PROP BASE Use_MFC 0 | ||
| 34 | # PROP BASE Use_Debug_Libraries 0 | ||
| 35 | # PROP BASE Output_Dir "Release" | ||
| 36 | # PROP BASE Intermediate_Dir "Release" | ||
| 37 | # PROP BASE Target_Dir "" | ||
| 38 | # PROP Use_MFC 0 | ||
| 39 | # PROP Use_Debug_Libraries 0 | ||
| 40 | # PROP Output_Dir "Release" | ||
| 41 | # PROP Intermediate_Dir "Release" | ||
| 42 | # PROP Target_Dir "" | ||
| 43 | # ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c | ||
| 44 | # ADD CPP /nologo /G6 /Zp4 /MD /O2 /Ob0 /I ".\\" /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /D "ICE_NO_DLL" /FD /c | ||
| 45 | # SUBTRACT CPP /Fr /YX | ||
| 46 | # ADD BASE RSC /l 0x409 /d "NDEBUG" | ||
| 47 | # ADD RSC /l 0x409 /d "NDEBUG" | ||
| 48 | BSC32=bscmake.exe | ||
| 49 | # ADD BASE BSC32 /nologo | ||
| 50 | # ADD BSC32 /nologo | ||
| 51 | LIB32=link.exe -lib | ||
| 52 | # ADD BASE LIB32 /nologo | ||
| 53 | # ADD LIB32 /nologo /out:"..\lib\OPCODE.lib" | ||
| 54 | |||
| 55 | !ELSEIF "$(CFG)" == "OPCODE - Win32 Debug" | ||
| 56 | |||
| 57 | # PROP BASE Use_MFC 0 | ||
| 58 | # PROP BASE Use_Debug_Libraries 1 | ||
| 59 | # PROP BASE Output_Dir "Debug" | ||
| 60 | # PROP BASE Intermediate_Dir "Debug" | ||
| 61 | # PROP BASE Target_Dir "" | ||
| 62 | # PROP Use_MFC 0 | ||
| 63 | # PROP Use_Debug_Libraries 1 | ||
| 64 | # PROP Output_Dir "Debug" | ||
| 65 | # PROP Intermediate_Dir "Debug" | ||
| 66 | # PROP Target_Dir "" | ||
| 67 | # ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c | ||
| 68 | # ADD CPP /nologo /G6 /Zp4 /MDd /Gm /ZI /Od /I ".\\" /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /D "ICE_NO_DLL" /FR /FD /GZ /c | ||
| 69 | # SUBTRACT CPP /YX | ||
| 70 | # ADD BASE RSC /l 0x409 /d "_DEBUG" | ||
| 71 | # ADD RSC /l 0x409 /d "_DEBUG" | ||
| 72 | BSC32=bscmake.exe | ||
| 73 | # ADD BASE BSC32 /nologo | ||
| 74 | # ADD BSC32 /nologo | ||
| 75 | LIB32=link.exe -lib | ||
| 76 | # ADD BASE LIB32 /nologo | ||
| 77 | # ADD LIB32 /nologo /out:"..\lib\OPCODE_D.lib" | ||
| 78 | |||
| 79 | !ENDIF | ||
| 80 | |||
| 81 | # Begin Target | ||
| 82 | |||
| 83 | # Name "OPCODE - Win32 Release" | ||
| 84 | # Name "OPCODE - Win32 Debug" | ||
| 85 | # Begin Source File | ||
| 86 | |||
| 87 | SOURCE=.\Ice\IceAABB.cpp | ||
| 88 | # End Source File | ||
| 89 | # Begin Source File | ||
| 90 | |||
| 91 | SOURCE=.\Ice\IceAABB.h | ||
| 92 | # End Source File | ||
| 93 | # Begin Source File | ||
| 94 | |||
| 95 | SOURCE=.\Ice\IceAxes.h | ||
| 96 | # End Source File | ||
| 97 | # Begin Source File | ||
| 98 | |||
| 99 | SOURCE=.\Ice\IceBoundingSphere.h | ||
| 100 | # End Source File | ||
| 101 | # Begin Source File | ||
| 102 | |||
| 103 | SOURCE=.\Ice\IceContainer.cpp | ||
| 104 | # End Source File | ||
| 105 | # Begin Source File | ||
| 106 | |||
| 107 | SOURCE=.\Ice\IceContainer.h | ||
| 108 | # End Source File | ||
| 109 | # Begin Source File | ||
| 110 | |||
| 111 | SOURCE=.\Ice\IceFPU.h | ||
| 112 | # End Source File | ||
| 113 | # Begin Source File | ||
| 114 | |||
| 115 | SOURCE=.\Ice\IceHPoint.cpp | ||
| 116 | # End Source File | ||
| 117 | # Begin Source File | ||
| 118 | |||
| 119 | SOURCE=.\Ice\IceHPoint.h | ||
| 120 | # End Source File | ||
| 121 | # Begin Source File | ||
| 122 | |||
| 123 | SOURCE=.\Ice\IceIndexedTriangle.cpp | ||
| 124 | # End Source File | ||
| 125 | # Begin Source File | ||
| 126 | |||
| 127 | SOURCE=.\Ice\IceIndexedTriangle.h | ||
| 128 | # End Source File | ||
| 129 | # Begin Source File | ||
| 130 | |||
| 131 | SOURCE=.\Ice\IceLSS.h | ||
| 132 | # End Source File | ||
| 133 | # Begin Source File | ||
| 134 | |||
| 135 | SOURCE=.\Ice\IceMatrix3x3.cpp | ||
| 136 | # End Source File | ||
| 137 | # Begin Source File | ||
| 138 | |||
| 139 | SOURCE=.\Ice\IceMatrix3x3.h | ||
| 140 | # End Source File | ||
| 141 | # Begin Source File | ||
| 142 | |||
| 143 | SOURCE=.\Ice\IceMatrix4x4.cpp | ||
| 144 | # End Source File | ||
| 145 | # Begin Source File | ||
| 146 | |||
| 147 | SOURCE=.\Ice\IceMatrix4x4.h | ||
| 148 | # End Source File | ||
| 149 | # Begin Source File | ||
| 150 | |||
| 151 | SOURCE=.\Ice\IceMemoryMacros.h | ||
| 152 | # End Source File | ||
| 153 | # Begin Source File | ||
| 154 | |||
| 155 | SOURCE=.\Ice\IceOBB.cpp | ||
| 156 | # End Source File | ||
| 157 | # Begin Source File | ||
| 158 | |||
| 159 | SOURCE=.\Ice\IceOBB.h | ||
| 160 | # End Source File | ||
| 161 | # Begin Source File | ||
| 162 | |||
| 163 | SOURCE=.\Ice\IcePairs.h | ||
| 164 | # End Source File | ||
| 165 | # Begin Source File | ||
| 166 | |||
| 167 | SOURCE=.\Ice\IcePlane.cpp | ||
| 168 | # End Source File | ||
| 169 | # Begin Source File | ||
| 170 | |||
| 171 | SOURCE=.\Ice\IcePlane.h | ||
| 172 | # End Source File | ||
| 173 | # Begin Source File | ||
| 174 | |||
| 175 | SOURCE=.\Ice\IcePoint.cpp | ||
| 176 | # End Source File | ||
| 177 | # Begin Source File | ||
| 178 | |||
| 179 | SOURCE=.\Ice\IcePoint.h | ||
| 180 | # End Source File | ||
| 181 | # Begin Source File | ||
| 182 | |||
| 183 | SOURCE=.\Ice\IcePreprocessor.h | ||
| 184 | # End Source File | ||
| 185 | # Begin Source File | ||
| 186 | |||
| 187 | SOURCE=.\Ice\IceRandom.cpp | ||
| 188 | # End Source File | ||
| 189 | # Begin Source File | ||
| 190 | |||
| 191 | SOURCE=.\Ice\IceRandom.h | ||
| 192 | # End Source File | ||
| 193 | # Begin Source File | ||
| 194 | |||
| 195 | SOURCE=.\Ice\IceRay.cpp | ||
| 196 | # End Source File | ||
| 197 | # Begin Source File | ||
| 198 | |||
| 199 | SOURCE=.\Ice\IceRay.h | ||
| 200 | # End Source File | ||
| 201 | # Begin Source File | ||
| 202 | |||
| 203 | SOURCE=.\Ice\IceRevisitedRadix.cpp | ||
| 204 | # End Source File | ||
| 205 | # Begin Source File | ||
| 206 | |||
| 207 | SOURCE=.\Ice\IceRevisitedRadix.h | ||
| 208 | # End Source File | ||
| 209 | # Begin Source File | ||
| 210 | |||
| 211 | SOURCE=.\Ice\IceSegment.cpp | ||
| 212 | # End Source File | ||
| 213 | # Begin Source File | ||
| 214 | |||
| 215 | SOURCE=.\Ice\IceSegment.h | ||
| 216 | # End Source File | ||
| 217 | # Begin Source File | ||
| 218 | |||
| 219 | SOURCE=.\Ice\IceTriangle.cpp | ||
| 220 | # End Source File | ||
| 221 | # Begin Source File | ||
| 222 | |||
| 223 | SOURCE=.\Ice\IceTriangle.h | ||
| 224 | # End Source File | ||
| 225 | # Begin Source File | ||
| 226 | |||
| 227 | SOURCE=.\Ice\IceTrilist.h | ||
| 228 | # End Source File | ||
| 229 | # Begin Source File | ||
| 230 | |||
| 231 | SOURCE=.\Ice\IceTypes.h | ||
| 232 | # End Source File | ||
| 233 | # Begin Source File | ||
| 234 | |||
| 235 | SOURCE=.\Ice\IceUtils.cpp | ||
| 236 | # End Source File | ||
| 237 | # Begin Source File | ||
| 238 | |||
| 239 | SOURCE=.\Ice\IceUtils.h | ||
| 240 | # End Source File | ||
| 241 | # Begin Source File | ||
| 242 | |||
| 243 | SOURCE=.\OPC_AABBCollider.cpp | ||
| 244 | # End Source File | ||
| 245 | # Begin Source File | ||
| 246 | |||
| 247 | SOURCE=.\OPC_AABBCollider.h | ||
| 248 | # End Source File | ||
| 249 | # Begin Source File | ||
| 250 | |||
| 251 | SOURCE=.\OPC_AABBTree.cpp | ||
| 252 | # End Source File | ||
| 253 | # Begin Source File | ||
| 254 | |||
| 255 | SOURCE=.\OPC_AABBTree.h | ||
| 256 | # End Source File | ||
| 257 | # Begin Source File | ||
| 258 | |||
| 259 | SOURCE=.\OPC_BaseModel.cpp | ||
| 260 | # End Source File | ||
| 261 | # Begin Source File | ||
| 262 | |||
| 263 | SOURCE=.\OPC_BaseModel.h | ||
| 264 | # End Source File | ||
| 265 | # Begin Source File | ||
| 266 | |||
| 267 | SOURCE=.\OPC_BoxBoxOverlap.h | ||
| 268 | # End Source File | ||
| 269 | # Begin Source File | ||
| 270 | |||
| 271 | SOURCE=.\OPC_BoxPruning.cpp | ||
| 272 | # End Source File | ||
| 273 | # Begin Source File | ||
| 274 | |||
| 275 | SOURCE=.\OPC_BoxPruning.h | ||
| 276 | # End Source File | ||
| 277 | # Begin Source File | ||
| 278 | |||
| 279 | SOURCE=.\OPC_Collider.cpp | ||
| 280 | # End Source File | ||
| 281 | # Begin Source File | ||
| 282 | |||
| 283 | SOURCE=.\OPC_Collider.h | ||
| 284 | # End Source File | ||
| 285 | # Begin Source File | ||
| 286 | |||
| 287 | SOURCE=.\OPC_Common.cpp | ||
| 288 | # End Source File | ||
| 289 | # Begin Source File | ||
| 290 | |||
| 291 | SOURCE=.\OPC_Common.h | ||
| 292 | # End Source File | ||
| 293 | # Begin Source File | ||
| 294 | |||
| 295 | SOURCE=.\OPC_HybridModel.cpp | ||
| 296 | # End Source File | ||
| 297 | # Begin Source File | ||
| 298 | |||
| 299 | SOURCE=.\OPC_HybridModel.h | ||
| 300 | # End Source File | ||
| 301 | # Begin Source File | ||
| 302 | |||
| 303 | SOURCE=.\OPC_IceHook.h | ||
| 304 | # End Source File | ||
| 305 | # Begin Source File | ||
| 306 | |||
| 307 | SOURCE=.\OPC_LSSAABBOverlap.h | ||
| 308 | # End Source File | ||
| 309 | # Begin Source File | ||
| 310 | |||
| 311 | SOURCE=.\OPC_LSSCollider.cpp | ||
| 312 | # End Source File | ||
| 313 | # Begin Source File | ||
| 314 | |||
| 315 | SOURCE=.\OPC_LSSCollider.h | ||
| 316 | # End Source File | ||
| 317 | # Begin Source File | ||
| 318 | |||
| 319 | SOURCE=.\OPC_LSSTriOverlap.h | ||
| 320 | # End Source File | ||
| 321 | # Begin Source File | ||
| 322 | |||
| 323 | SOURCE=.\OPC_MeshInterface.cpp | ||
| 324 | # End Source File | ||
| 325 | # Begin Source File | ||
| 326 | |||
| 327 | SOURCE=.\OPC_MeshInterface.h | ||
| 328 | # End Source File | ||
| 329 | # Begin Source File | ||
| 330 | |||
| 331 | SOURCE=.\OPC_Model.cpp | ||
| 332 | # End Source File | ||
| 333 | # Begin Source File | ||
| 334 | |||
| 335 | SOURCE=.\OPC_Model.h | ||
| 336 | # End Source File | ||
| 337 | # Begin Source File | ||
| 338 | |||
| 339 | SOURCE=.\OPC_OBBCollider.cpp | ||
| 340 | # End Source File | ||
| 341 | # Begin Source File | ||
| 342 | |||
| 343 | SOURCE=.\OPC_OBBCollider.h | ||
| 344 | # End Source File | ||
| 345 | # Begin Source File | ||
| 346 | |||
| 347 | SOURCE=.\OPC_OptimizedTree.cpp | ||
| 348 | # End Source File | ||
| 349 | # Begin Source File | ||
| 350 | |||
| 351 | SOURCE=.\OPC_OptimizedTree.h | ||
| 352 | # End Source File | ||
| 353 | # Begin Source File | ||
| 354 | |||
| 355 | SOURCE=.\OPC_Picking.cpp | ||
| 356 | # End Source File | ||
| 357 | # Begin Source File | ||
| 358 | |||
| 359 | SOURCE=.\OPC_Picking.h | ||
| 360 | # End Source File | ||
| 361 | # Begin Source File | ||
| 362 | |||
| 363 | SOURCE=.\OPC_PlanesAABBOverlap.h | ||
| 364 | # End Source File | ||
| 365 | # Begin Source File | ||
| 366 | |||
| 367 | SOURCE=.\OPC_PlanesCollider.cpp | ||
| 368 | # End Source File | ||
| 369 | # Begin Source File | ||
| 370 | |||
| 371 | SOURCE=.\OPC_PlanesCollider.h | ||
| 372 | # End Source File | ||
| 373 | # Begin Source File | ||
| 374 | |||
| 375 | SOURCE=.\OPC_PlanesTriOverlap.h | ||
| 376 | # End Source File | ||
| 377 | # Begin Source File | ||
| 378 | |||
| 379 | SOURCE=.\OPC_RayAABBOverlap.h | ||
| 380 | # End Source File | ||
| 381 | # Begin Source File | ||
| 382 | |||
| 383 | SOURCE=.\OPC_RayCollider.cpp | ||
| 384 | # End Source File | ||
| 385 | # Begin Source File | ||
| 386 | |||
| 387 | SOURCE=.\OPC_RayCollider.h | ||
| 388 | # End Source File | ||
| 389 | # Begin Source File | ||
| 390 | |||
| 391 | SOURCE=.\OPC_RayTriOverlap.h | ||
| 392 | # End Source File | ||
| 393 | # Begin Source File | ||
| 394 | |||
| 395 | SOURCE=.\OPC_Settings.h | ||
| 396 | # End Source File | ||
| 397 | # Begin Source File | ||
| 398 | |||
| 399 | SOURCE=.\OPC_SphereAABBOverlap.h | ||
| 400 | # End Source File | ||
| 401 | # Begin Source File | ||
| 402 | |||
| 403 | SOURCE=.\OPC_SphereCollider.cpp | ||
| 404 | # End Source File | ||
| 405 | # Begin Source File | ||
| 406 | |||
| 407 | SOURCE=.\OPC_SphereCollider.h | ||
| 408 | # End Source File | ||
| 409 | # Begin Source File | ||
| 410 | |||
| 411 | SOURCE=.\OPC_SphereTriOverlap.h | ||
| 412 | # End Source File | ||
| 413 | # Begin Source File | ||
| 414 | |||
| 415 | SOURCE=.\OPC_SweepAndPrune.cpp | ||
| 416 | # End Source File | ||
| 417 | # Begin Source File | ||
| 418 | |||
| 419 | SOURCE=.\OPC_SweepAndPrune.h | ||
| 420 | # End Source File | ||
| 421 | # Begin Source File | ||
| 422 | |||
| 423 | SOURCE=.\OPC_TreeBuilders.cpp | ||
| 424 | # End Source File | ||
| 425 | # Begin Source File | ||
| 426 | |||
| 427 | SOURCE=.\OPC_TreeBuilders.h | ||
| 428 | # End Source File | ||
| 429 | # Begin Source File | ||
| 430 | |||
| 431 | SOURCE=.\OPC_TreeCollider.cpp | ||
| 432 | # End Source File | ||
| 433 | # Begin Source File | ||
| 434 | |||
| 435 | SOURCE=.\OPC_TreeCollider.h | ||
| 436 | # End Source File | ||
| 437 | # Begin Source File | ||
| 438 | |||
| 439 | SOURCE=.\OPC_TriBoxOverlap.h | ||
| 440 | # End Source File | ||
| 441 | # Begin Source File | ||
| 442 | |||
| 443 | SOURCE=.\OPC_TriTriOverlap.h | ||
| 444 | # End Source File | ||
| 445 | # Begin Source File | ||
| 446 | |||
| 447 | SOURCE=.\OPC_VolumeCollider.cpp | ||
| 448 | # End Source File | ||
| 449 | # Begin Source File | ||
| 450 | |||
| 451 | SOURCE=.\OPC_VolumeCollider.h | ||
| 452 | # End Source File | ||
| 453 | # Begin Source File | ||
| 454 | |||
| 455 | SOURCE=.\Opcode.cpp | ||
| 456 | # End Source File | ||
| 457 | # Begin Source File | ||
| 458 | |||
| 459 | SOURCE=.\Opcode.h | ||
| 460 | # End Source File | ||
| 461 | # Begin Source File | ||
| 462 | |||
| 463 | SOURCE=.\StdAfx.cpp | ||
| 464 | # End Source File | ||
| 465 | # Begin Source File | ||
| 466 | |||
| 467 | SOURCE=.\StdAfx.h | ||
| 468 | # End Source File | ||
| 469 | # End Target | ||
| 470 | # End Project | ||
diff --git a/libraries/ode-0.9/OPCODE/Opcode.dsw b/libraries/ode-0.9/OPCODE/Opcode.dsw new file mode 100644 index 0000000..27f5c28 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Opcode.dsw | |||
| @@ -0,0 +1,29 @@ | |||
| 1 | Microsoft Developer Studio Workspace File, Format Version 6.00 | ||
| 2 | # WARNING: DO NOT EDIT OR DELETE THIS WORKSPACE FILE! | ||
| 3 | |||
| 4 | ############################################################################### | ||
| 5 | |||
| 6 | Project: "OPCODE"=.\Opcode.dsp - Package Owner=<4> | ||
| 7 | |||
| 8 | Package=<5> | ||
| 9 | {{{ | ||
| 10 | }}} | ||
| 11 | |||
| 12 | Package=<4> | ||
| 13 | {{{ | ||
| 14 | }}} | ||
| 15 | |||
| 16 | ############################################################################### | ||
| 17 | |||
| 18 | Global: | ||
| 19 | |||
| 20 | Package=<5> | ||
| 21 | {{{ | ||
| 22 | }}} | ||
| 23 | |||
| 24 | Package=<3> | ||
| 25 | {{{ | ||
| 26 | }}} | ||
| 27 | |||
| 28 | ############################################################################### | ||
| 29 | |||
diff --git a/libraries/ode-0.9/OPCODE/Opcode.h b/libraries/ode-0.9/OPCODE/Opcode.h new file mode 100644 index 0000000..2a7a6c5 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Opcode.h | |||
| @@ -0,0 +1,113 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 10 | /** | ||
| 11 | * Main file for Opcode.dll. | ||
| 12 | * \file Opcode.h | ||
| 13 | * \author Pierre Terdiman | ||
| 14 | * \date March, 20, 2001 | ||
| 15 | */ | ||
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 17 | |||
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 19 | // Include Guard | ||
| 20 | #ifndef __OPCODE_H__ | ||
| 21 | #define __OPCODE_H__ | ||
| 22 | |||
| 23 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 24 | // Things to help us compile on non-windows platforms | ||
| 25 | |||
| 26 | #if defined(__APPLE__) || defined(__MACOSX__) | ||
| 27 | #if __APPLE_CC__ < 1495 | ||
| 28 | #define sqrtf sqrt | ||
| 29 | #define sinf sin | ||
| 30 | #define cosf cos | ||
| 31 | #define acosf acos | ||
| 32 | #define asinf asin | ||
| 33 | #endif | ||
| 34 | #endif | ||
| 35 | |||
| 36 | #ifndef _MSC_VER | ||
| 37 | #ifndef __int64 | ||
| 38 | #define __int64 long long int | ||
| 39 | #endif | ||
| 40 | #ifndef __stdcall /* this is defined in MinGW and CygWin, so avoid the warning */ | ||
| 41 | #define __stdcall /* */ | ||
| 42 | #endif | ||
| 43 | #endif | ||
| 44 | |||
| 45 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 46 | // Compilation messages | ||
| 47 | #ifdef _MSC_VER | ||
| 48 | #if defined(OPCODE_EXPORTS) | ||
| 49 | // #pragma message("Compiling OPCODE") | ||
| 50 | #elif !defined(OPCODE_EXPORTS) | ||
| 51 | // #pragma message("Using OPCODE") | ||
| 52 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 53 | // Automatic linking | ||
| 54 | #ifndef BAN_OPCODE_AUTOLINK | ||
| 55 | #ifdef _DEBUG | ||
| 56 | //#pragma comment(lib, "Opcode_D.lib") | ||
| 57 | #else | ||
| 58 | //#pragma comment(lib, "Opcode.lib") | ||
| 59 | #endif | ||
| 60 | #endif | ||
| 61 | #endif | ||
| 62 | #endif | ||
| 63 | |||
| 64 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 65 | // Preprocessor | ||
| 66 | #ifndef ICE_NO_DLL | ||
| 67 | #ifdef OPCODE_EXPORTS | ||
| 68 | #define OPCODE_API// __declspec(dllexport) | ||
| 69 | #else | ||
| 70 | #define OPCODE_API// __declspec(dllimport) | ||
| 71 | #endif | ||
| 72 | #else | ||
| 73 | #define OPCODE_API | ||
| 74 | #endif | ||
| 75 | |||
| 76 | #include "OPC_IceHook.h" | ||
| 77 | |||
| 78 | namespace Opcode | ||
| 79 | { | ||
| 80 | // Bulk-of-the-work | ||
| 81 | #include "OPC_Settings.h" | ||
| 82 | #include "OPC_Common.h" | ||
| 83 | #include "OPC_MeshInterface.h" | ||
| 84 | // Builders | ||
| 85 | #include "OPC_TreeBuilders.h" | ||
| 86 | // Trees | ||
| 87 | #include "OPC_AABBTree.h" | ||
| 88 | #include "OPC_OptimizedTree.h" | ||
| 89 | // Models | ||
| 90 | #include "OPC_BaseModel.h" | ||
| 91 | #include "OPC_Model.h" | ||
| 92 | #include "OPC_HybridModel.h" | ||
| 93 | // Colliders | ||
| 94 | #include "OPC_Collider.h" | ||
| 95 | #include "OPC_VolumeCollider.h" | ||
| 96 | #include "OPC_TreeCollider.h" | ||
| 97 | #include "OPC_RayCollider.h" | ||
| 98 | #include "OPC_SphereCollider.h" | ||
| 99 | #include "OPC_OBBCollider.h" | ||
| 100 | #include "OPC_AABBCollider.h" | ||
| 101 | #include "OPC_LSSCollider.h" | ||
| 102 | #include "OPC_PlanesCollider.h" | ||
| 103 | // Usages | ||
| 104 | #include "OPC_Picking.h" | ||
| 105 | // Sweep-and-prune | ||
| 106 | #include "OPC_BoxPruning.h" | ||
| 107 | #include "OPC_SweepAndPrune.h" | ||
| 108 | |||
| 109 | FUNCTION OPCODE_API bool InitOpcode(); | ||
| 110 | FUNCTION OPCODE_API bool CloseOpcode(); | ||
| 111 | } | ||
| 112 | |||
| 113 | #endif // __OPCODE_H__ | ||
diff --git a/libraries/ode-0.9/OPCODE/README-ODE.txt b/libraries/ode-0.9/OPCODE/README-ODE.txt new file mode 100644 index 0000000..c5d5800 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/README-ODE.txt | |||
| @@ -0,0 +1,13 @@ | |||
| 1 | |||
| 2 | This is a copy of the OPCODE collision detection library by Pierre Terdiman. | ||
| 3 | See http://www.codercorner.com/Opcode.htm for more information, and read | ||
| 4 | the ReadMe.txt in this directory. | ||
| 5 | |||
| 6 | If you want to use the TriList (triangle mesh) geometry class in ODE, the | ||
| 7 | OPCODE library must be compiled. If you are using the autotools support to | ||
| 8 | compile ODE, you just have to specify --with-trimesh=opcode when calling ./configure. | ||
| 9 | |||
| 10 | This code was originally written for and compiled on windows, but it has been | ||
| 11 | ported so that it should compile under unix/gcc too. Your mileage may vary. | ||
| 12 | |||
| 13 | Russ Smith, April 12 2005. | ||
diff --git a/libraries/ode-0.9/OPCODE/ReadMe.txt b/libraries/ode-0.9/OPCODE/ReadMe.txt new file mode 100644 index 0000000..8a39eff --- /dev/null +++ b/libraries/ode-0.9/OPCODE/ReadMe.txt | |||
| @@ -0,0 +1,171 @@ | |||
| 1 | |||
| 2 | OPCODE distribution 1.3 (june 2003) | ||
| 3 | ----------------------- | ||
| 4 | |||
| 5 | New in Opcode 1.3: | ||
| 6 | - fixed the divide by 0 bug that was happening when all centers where located on a coordinate axis (thanks to Jorrit T) | ||
| 7 | - linearized "complete" vanilla AABB trees | ||
| 8 | - ANSI-compliant "for" loops (for the ones porting it to Linux...) | ||
| 9 | - callbacks & pointers moved to mesh interface | ||
| 10 | - support for triangle & vertex strides | ||
| 11 | - optimized the sphere-triangle overlap code a bit | ||
| 12 | - dynamic trees (refit) | ||
| 13 | - more builders | ||
| 14 | - ValidateSubdivision in builders | ||
| 15 | - LSS collider | ||
| 16 | - primitive-bv tests can now be skipped in most volume queries | ||
| 17 | - temporal coherence now also works for airborne objects | ||
| 18 | - temporal coherence completed for boxes / all contacts, LSS, etc | ||
| 19 | - ray-collider now uses a callback | ||
| 20 | - some common "usages" have been introduced (only picking for now) | ||
| 21 | - SPLIT_COMPLETE removed (now implicitely using mLimit = 1) | ||
| 22 | - hybrid collision models | ||
| 23 | - sweep-and-prune code added, moved from my old Z-Collide lib | ||
| 24 | - it now works with meshes made of only 1 triangle (except in mesh-mesh case!) | ||
| 25 | |||
| 26 | Disclaimer: | ||
| 27 | |||
| 28 | - I forced myself to actually *do* the release today no matter what. Else it would never have been done. That's | ||
| 29 | why the code may not be very polished. I also removed a *lot* of things (more usages, distance queries, etc...) | ||
| 30 | that weren't ready for prime-time (or that were linked to too many of my supporting libs) | ||
| 31 | |||
| 32 | - Some comments may also be obsolete here and there. The old User Manual for Opcode 1.2 may not fit version 1.3 | ||
| 33 | either, since there's a new "mesh interface" to support strides, etc. | ||
| 34 | |||
| 35 | - Everything in the "Ice" directory has been hacked out of my engine and edited until everything compiled. Don't | ||
| 36 | expect anything out there to be cute or something. In particular, some CPP files are not even included when not | ||
| 37 | needed, so you can expect some linker errors if you try messing around with them... | ||
| 38 | |||
| 39 | Otherwise, it should be just like previous version, only better. In particular, hybrid models can be very | ||
| 40 | memory-friendly (sometimes using like 10 times less ram than the best trees from version 1.2). The possible | ||
| 41 | speed hit is often invisible (if it even exists), especially using temporal coherence in "all contacts" mode. | ||
| 42 | (Admittedly, this depends on your particular usage pattern / what you do on collided triangles). | ||
| 43 | |||
| 44 | The sweep-and-prune code is similar to the "vanilla" version found in V-Collide (but that one's better IMHO...) | ||
| 45 | The simple "radix" version is often just as good, see for yourself. | ||
| 46 | |||
| 47 | OPCODE distribution 1.2 (august 2002) | ||
| 48 | ----------------------- | ||
| 49 | |||
| 50 | New in Opcode 1.2: | ||
| 51 | - new VolumeCollider base class | ||
| 52 | - simplified callback setup | ||
| 53 | - you can now use callbacks or pointers (setup at compile time) | ||
| 54 | - destination array not needed anymore in the RayCollider (faster in-out tests) | ||
| 55 | - renamed classes: AABBRayCollider => RayCollider, AABBSphereCollider => SphereCollider | ||
| 56 | - the sphere query now only returns a list of faces (extra info discarded). On the other hand it's a lot faster. | ||
| 57 | - OBB, AABB and planes queries. Original OBB and AABB queries contributed by Erwin de Vries. | ||
| 58 | - cosmetic changes in OPC_BoxBoxOverlap.h contributed by Gottfried Chen | ||
| 59 | - some inlining problems fixed | ||
| 60 | - faster ray-mesh tests using the separating axis theorem | ||
| 61 | - new split value in AABB tree construction (contributed by Igor Kravtchenko). Provides faster queries most of the time. | ||
| 62 | - improved temporal coherence for sphere & AABB queries (works in "All contacts" mode) | ||
| 63 | |||
| 64 | Notes: | ||
| 65 | |||
| 66 | - Everything in the "Ice code" directory (in VC++) is basically copy-pasted from my engine, with a lot | ||
| 67 | of code removed until there was no link error anymore. Don't expect those files to be cute or anything, | ||
| 68 | they've never been meant to be released and they're often updated/modified/messy. | ||
| 69 | - Some experimental features have been removed as well. Else I would never have released the 1.2... | ||
| 70 | - Not as polished/optimal as I would like it to be, but that's life. I promised myself to release it | ||
| 71 | before october 2002 (one YEAR later ?!).... That's the only reason why it's there. | ||
| 72 | - Some people reported ColDet was faster. Uh, come on. They were using Opcode in | ||
| 73 | "All contacts" mode whereas ColDet was doing "first contact"... | ||
| 74 | |||
| 75 | OPCODE distribution 1.1 (october 2001) | ||
| 76 | ----------------------- | ||
| 77 | |||
| 78 | New in Opcode 1.1: | ||
| 79 | - stabbing queries | ||
| 80 | - sphere queries | ||
| 81 | - abtract base class for colliders | ||
| 82 | - settings validation methods | ||
| 83 | - compilation flags now grouped in OPC_Settings.h | ||
| 84 | - smaller files, new VC++ virtual dirs (cleaner) | ||
| 85 | |||
| 86 | Notes: | ||
| 87 | |||
| 88 | - "override(baseclass)" is a personal cosmetic thing. It's the same as "virtual", but provides more info. | ||
| 89 | - I code in 1600*1200, so some lines may look a bit long.. | ||
| 90 | - This version is not as polished as the previous one due to lack of time. The stabbing & sphere queries | ||
| 91 | can still be optimized: for example by trying other atomic overlap tests. I'm using my first ray-AABB | ||
| 92 | code, but the newer one seems better. Tim Schröder's one is good as well. See: www.codercorner.com/RayAABB.cpp | ||
| 93 | - The trees can easily be compressed even more, I save this for later (lack of time, lack of time!) | ||
| 94 | - I removed various tests before releasing this one: | ||
| 95 | - a separation line, a.k.a. "front" in QuickCD, because gains were unclear | ||
| 96 | - distance queries in a PQP style, because it was way too slow | ||
| 97 | - support for deformable models, too slow as well | ||
| 98 | - You can easily use Opcode to do your player-vs-world collision detection, in a Nettle/Telemachos way. | ||
| 99 | If someone out there wants to donate some art / level for the cause, I'd be glad to release a demo. (current | ||
| 100 | demo uses copyrighted art I'm not allowed to spread) | ||
| 101 | - Sorry for the lack of real docs and/or solid examples. I just don't have enough time. | ||
| 102 | |||
| 103 | OPCODE distribution 1.0 (march 2001) | ||
| 104 | ----------------------- | ||
| 105 | |||
| 106 | - First release | ||
| 107 | |||
| 108 | =============================================================================== | ||
| 109 | |||
| 110 | WHAT ? | ||
| 111 | |||
| 112 | OPCODE means OPtimized COllision DEtection. | ||
| 113 | So this is a collision detection package similar to RAPID. Here's a | ||
| 114 | quick list of features: | ||
| 115 | |||
| 116 | - C++ interface, developed for Windows systems using VC++ 6.0 | ||
| 117 | - Works on arbitrary meshes (convex or non-convex), even polygon soups | ||
| 118 | - Current implementation uses AABB-trees | ||
| 119 | - Introduces Primitive-BV overlap tests during recursive collision queries (whereas | ||
| 120 | standard libraries only rely on Primitive-Primitive and BV-BV tests) | ||
| 121 | - Introduces no-leaf trees, i.e. collision trees whose leaf nodes have been removed | ||
| 122 | - Supports collision queries on quantized trees (decompressed on-the-fly) | ||
| 123 | - Supports "first contact" or "all contacts" modes (ā la RAPID) | ||
| 124 | - Uses temporal coherence for "first contact" mode (~10 to 20 times faster, useful | ||
| 125 | in rigid body simulation during bisection) | ||
| 126 | - Memory footprint is 7.2 times smaller than RAPID's one, which is ideal for console | ||
| 127 | games with limited ram (actually, if you use the unmodified RAPID code using double | ||
| 128 | precision, it's more like 13 times smaller...) | ||
| 129 | - And yet it often runs faster than RAPID (according to RDTSC, sometimes more than 5 | ||
| 130 | times faster when objects are deeply overlapping) | ||
| 131 | - Performance is usually close to RAPID's one in close-proximity situations | ||
| 132 | - Stabbing, planes & volume queries (sphere, AABB, OBB, LSS) | ||
| 133 | - Sweep-and-prune | ||
| 134 | - Now works with deformable meshes | ||
| 135 | - Hybrid trees | ||
| 136 | |||
| 137 | |||
| 138 | What it can be used for: | ||
| 139 | - standard mesh-mesh collision detection (similar to RAPID, SOLID, QuickCD, PQP, ColDet...) | ||
| 140 | - N-body collisions (similar to V-Collide) | ||
| 141 | - camera-vs-world collisions (similar to Telemachos/Paul Nettle/Stan Melax articles) | ||
| 142 | - shadow feelers to speed up lightmap computations | ||
| 143 | - in-out tests to speed up voxelization processes | ||
| 144 | - picking | ||
| 145 | - rigid body simulation | ||
| 146 | - view frustum culling | ||
| 147 | - etc | ||
| 148 | |||
| 149 | WHY ? | ||
| 150 | |||
| 151 | - Because RAPID uses too many bytes. | ||
| 152 | - Because the idea was nice... | ||
| 153 | |||
| 154 | WHEN ? | ||
| 155 | |||
| 156 | It's been coded in march 2001 following a thread on the GD-Algorithms list. | ||
| 157 | |||
| 158 | GDAlgorithms-list mailing list | ||
| 159 | GDAlgorithms-list@lists.sourceforge.net | ||
| 160 | http://lists.sourceforge.net/lists/listinfo/gdalgorithms-list | ||
| 161 | |||
| 162 | WHO ? | ||
| 163 | |||
| 164 | Pierre Terdiman | ||
| 165 | June, 1, 2003 | ||
| 166 | |||
| 167 | p.terdiman@wanadoo.fr | ||
| 168 | p.terdiman@codercorner.com | ||
| 169 | |||
| 170 | http://www.codercorner.com | ||
| 171 | http://www.codercorner.com/Opcode.htm | ||
diff --git a/libraries/ode-0.9/OPCODE/StdAfx.cpp b/libraries/ode-0.9/OPCODE/StdAfx.cpp new file mode 100644 index 0000000..9c381f6 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/StdAfx.cpp | |||
| @@ -0,0 +1,10 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | //#define ICE_MAIN | ||
| 10 | #include "Stdafx.h" | ||
diff --git a/libraries/ode-0.9/OPCODE/Stdafx.h b/libraries/ode-0.9/OPCODE/Stdafx.h new file mode 100644 index 0000000..0223a6c --- /dev/null +++ b/libraries/ode-0.9/OPCODE/Stdafx.h | |||
| @@ -0,0 +1,24 @@ | |||
| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 2 | /* | ||
| 3 | * OPCODE - Optimized Collision Detection | ||
| 4 | * Copyright (C) 2001 Pierre Terdiman | ||
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm | ||
| 6 | */ | ||
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #if !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_) | ||
| 10 | #define AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_ | ||
| 11 | |||
| 12 | #if _MSC_VER > 1000 | ||
| 13 | #pragma once | ||
| 14 | #endif // _MSC_VER > 1000 | ||
| 15 | |||
| 16 | // Insert your headers here | ||
| 17 | #define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers | ||
| 18 | |||
| 19 | #include "Opcode.h" | ||
| 20 | |||
| 21 | //{{AFX_INSERT_LOCATION}} | ||
| 22 | // Microsoft Visual C++ will insert additional declarations immediately before the previous line. | ||
| 23 | |||
| 24 | #endif // !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_) | ||
diff --git a/libraries/ode-0.9/OPCODE/TemporalCoherence.txt b/libraries/ode-0.9/OPCODE/TemporalCoherence.txt new file mode 100644 index 0000000..fb85931 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/TemporalCoherence.txt | |||
| @@ -0,0 +1,32 @@ | |||
| 1 | |||
| 2 | > Hi John, | ||
| 3 | > | ||
| 4 | > I know I'll forget to tell you this if I don't write it right now.... | ||
| 5 | > | ||
| 6 | > >(2) How is the receiving geometry for the shadow decided? | ||
| 7 | > | ||
| 8 | > I wrote about an LSS-test but actually performing a new VFC test (from the | ||
| 9 | > light's view) is the same. In both cases, here's a trick to take advantage | ||
| 10 | > of temporal coherence : test the world against a slightly larger than | ||
| 11 | > necessary LSS or frustum. Keep the list of touched surfaces. Then next | ||
| 12 | > frame, if the new volume is still contained within the previous one used | ||
| 13 | for | ||
| 14 | > the query, you can reuse the same list immediately. Actually it's a bit | ||
| 15 | > similar to what you did in your sphere-tree, I think. Anyway, now the | ||
| 16 | O(log | ||
| 17 | > N) VFC is O(1) for some frames. It's not worth it for the "real" VFC, but | ||
| 18 | > when you have N virtual frustum to test to drop N shadows, that's another | ||
| 19 | > story. | ||
| 20 | > | ||
| 21 | > Two downsides: | ||
| 22 | > - You need more ram to keep track of one list of meshes / shadow, but | ||
| 23 | > usually it's not a lot. | ||
| 24 | > - By using a larger volume for the query you possibly touch more | ||
| 25 | > faces/surfaces, which will be rendered in the shadow pass. Usually it's | ||
| 26 | not | ||
| 27 | > a problem either since rendering is simply faster than geometric queries | ||
| 28 | > those days. But of course, "your mileage may vary". | ||
| 29 | > | ||
| 30 | > Happy new year ! | ||
| 31 | > | ||
| 32 | > Pierre | ||