raycast_mesh.cpp 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951
  1. #include "raycast_mesh.h"
  2. #include <math.h>
  3. #include <assert.h>
  4. #include <stdio.h>
  5. #include <stdlib.h>
  6. #include <stdint.h>
  7. #include <string.h>
  8. #include <vector>
  9. // This code snippet allows you to create an axis aligned bounding volume tree for a triangle mesh so that you can do
  10. // high-speed raycasting.
  11. //
  12. // There are much better implementations of this available on the internet. In particular I recommend that you use
  13. // OPCODE written by Pierre Terdiman.
  14. // @see: http://www.codercorner.com/Opcode.htm
  15. //
  16. // OPCODE does a whole lot more than just raycasting, and is a rather significant amount of source code.
  17. //
  18. // I am providing this code snippet for the use case where you *only* want to do quick and dirty optimized raycasting.
  19. // I have not done performance testing between this version and OPCODE; so I don't know how much slower it is. However,
  20. // anytime you switch to using a spatial data structure for raycasting, you increase your performance by orders and orders
  21. // of magnitude; so this implementation should work fine for simple tools and utilities.
  22. //
  23. // It also serves as a nice sample for people who are trying to learn the algorithm of how to implement AABB trees.
  24. // AABB = Axis Aligned Bounding Volume trees.
  25. //
  26. // http://www.cgal.org/Manual/3.5/doc_html/cgal_manual/AABB_tree/Chapter_main.html
  27. //
  28. //
  29. // This code snippet was written by John W. Ratcliff on August 18, 2011 and released under the MIT. license.
  30. //
  31. // mailto:jratcliffscarab@gmail.com
  32. //
  33. // The official source can be found at: http://code.google.com/p/raycastmesh/
  34. //
  35. //
  36. #pragma warning(disable:4100)
  37. namespace RAYCAST_MESH
  38. {
  39. typedef std::vector< RmUint32 > TriVector;
  40. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  41. /**
  42. * A method to compute a ray-AABB intersection.
  43. * Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990
  44. * Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
  45. * Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)
  46. *
  47. * Hence this version is faster as well as more robust than the original one.
  48. *
  49. * Should work provided:
  50. * 1) the integer representation of 0.0f is 0x00000000
  51. * 2) the sign bit of the RmReal is the most significant one
  52. *
  53. * Report bugs: p.terdiman@codercorner.com
  54. *
  55. * \param aabb [in] the axis-aligned bounding box
  56. * \param origin [in] ray origin
  57. * \param dir [in] ray direction
  58. * \param coord [out] impact coordinates
  59. * \return true if ray intersects AABB
  60. */
  61. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  62. #define RAYAABB_EPSILON 0.00001f
  63. //! Integer representation of a RmRealing-point value.
  64. #define IR(x) ((RmUint32&)x)
  65. bool intersectRayAABB(const RmReal MinB[3],const RmReal MaxB[3],const RmReal origin[3],const RmReal dir[3],RmReal coord[3])
  66. {
  67. bool Inside = true;
  68. RmReal MaxT[3];
  69. MaxT[0]=MaxT[1]=MaxT[2]=-1.0f;
  70. // Find candidate planes.
  71. for(RmUint32 i=0;i<3;i++)
  72. {
  73. if(origin[i] < MinB[i])
  74. {
  75. coord[i] = MinB[i];
  76. Inside = false;
  77. // Calculate T distances to candidate planes
  78. if(IR(dir[i])) MaxT[i] = (MinB[i] - origin[i]) / dir[i];
  79. }
  80. else if(origin[i] > MaxB[i])
  81. {
  82. coord[i] = MaxB[i];
  83. Inside = false;
  84. // Calculate T distances to candidate planes
  85. if(IR(dir[i])) MaxT[i] = (MaxB[i] - origin[i]) / dir[i];
  86. }
  87. }
  88. // Ray origin inside bounding box
  89. if(Inside)
  90. {
  91. coord[0] = origin[0];
  92. coord[1] = origin[1];
  93. coord[2] = origin[2];
  94. return true;
  95. }
  96. // Get largest of the maxT's for final choice of intersection
  97. RmUint32 WhichPlane = 0;
  98. if(MaxT[1] > MaxT[WhichPlane]) WhichPlane = 1;
  99. if(MaxT[2] > MaxT[WhichPlane]) WhichPlane = 2;
  100. // Check final candidate actually inside box
  101. if(IR(MaxT[WhichPlane])&0x80000000) return false;
  102. for(RmUint32 i=0;i<3;i++)
  103. {
  104. if(i!=WhichPlane)
  105. {
  106. coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
  107. #ifdef RAYAABB_EPSILON
  108. if(coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] + RAYAABB_EPSILON) return false;
  109. #else
  110. if(coord[i] < MinB[i] || coord[i] > MaxB[i]) return false;
  111. #endif
  112. }
  113. }
  114. return true; // ray hits box
  115. }
  116. bool intersectLineSegmentAABB(const RmReal bmin[3],const RmReal bmax[3],const RmReal p1[3],const RmReal dir[3],RmReal &dist,RmReal intersect[3])
  117. {
  118. bool ret = false;
  119. if ( dist > RAYAABB_EPSILON )
  120. {
  121. ret = intersectRayAABB(bmin,bmax,p1,dir,intersect);
  122. if ( ret )
  123. {
  124. RmReal dx = p1[0]-intersect[0];
  125. RmReal dy = p1[1]-intersect[1];
  126. RmReal dz = p1[2]-intersect[2];
  127. RmReal d = dx*dx+dy*dy+dz*dz;
  128. if ( d < dist*dist )
  129. {
  130. dist = sqrtf(d);
  131. }
  132. else
  133. {
  134. ret = false;
  135. }
  136. }
  137. }
  138. return ret;
  139. }
  140. /* a = b - c */
  141. #define vector(a,b,c) \
  142. (a)[0] = (b)[0] - (c)[0]; \
  143. (a)[1] = (b)[1] - (c)[1]; \
  144. (a)[2] = (b)[2] - (c)[2];
  145. #define innerProduct(v,q) \
  146. ((v)[0] * (q)[0] + \
  147. (v)[1] * (q)[1] + \
  148. (v)[2] * (q)[2])
  149. #define crossProduct(a,b,c) \
  150. (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
  151. (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
  152. (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
  153. static inline bool rayIntersectsTriangle(const RmReal *p,const RmReal *d,const RmReal *v0,const RmReal *v1,const RmReal *v2,RmReal &t)
  154. {
  155. RmReal e1[3],e2[3],h[3],s[3],q[3];
  156. RmReal a,f,u,v;
  157. vector(e1,v1,v0);
  158. vector(e2,v2,v0);
  159. crossProduct(h,d,e2);
  160. a = innerProduct(e1,h);
  161. if (a > -0.00001 && a < 0.00001)
  162. return(false);
  163. f = 1/a;
  164. vector(s,p,v0);
  165. u = f * (innerProduct(s,h));
  166. if (u < 0.0 || u > 1.0)
  167. return(false);
  168. crossProduct(q,s,e1);
  169. v = f * innerProduct(d,q);
  170. if (v < 0.0 || u + v > 1.0)
  171. return(false);
  172. // at this stage we can compute t to find out where
  173. // the intersection point is on the line
  174. t = f * innerProduct(e2,q);
  175. if (t > 0) // ray intersection
  176. return(true);
  177. else // this means that there is a line intersection
  178. // but not a ray intersection
  179. return (false);
  180. }
  181. static RmReal computePlane(const RmReal *A,const RmReal *B,const RmReal *C,RmReal *n) // returns D
  182. {
  183. RmReal vx = (B[0] - C[0]);
  184. RmReal vy = (B[1] - C[1]);
  185. RmReal vz = (B[2] - C[2]);
  186. RmReal wx = (A[0] - B[0]);
  187. RmReal wy = (A[1] - B[1]);
  188. RmReal wz = (A[2] - B[2]);
  189. RmReal vw_x = vy * wz - vz * wy;
  190. RmReal vw_y = vz * wx - vx * wz;
  191. RmReal vw_z = vx * wy - vy * wx;
  192. RmReal mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
  193. if ( mag < 0.000001f )
  194. {
  195. mag = 0;
  196. }
  197. else
  198. {
  199. mag = 1.0f/mag;
  200. }
  201. RmReal x = vw_x * mag;
  202. RmReal y = vw_y * mag;
  203. RmReal z = vw_z * mag;
  204. RmReal D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
  205. n[0] = x;
  206. n[1] = y;
  207. n[2] = z;
  208. return D;
  209. }
  210. #define TRI_EOF 0xFFFFFFFF
  211. enum AxisAABB
  212. {
  213. AABB_XAXIS,
  214. AABB_YAXIS,
  215. AABB_ZAXIS
  216. };
  217. enum ClipCode
  218. {
  219. CC_MINX = (1<<0),
  220. CC_MAXX = (1<<1),
  221. CC_MINY = (1<<2),
  222. CC_MAXY = (1<<3),
  223. CC_MINZ = (1<<4),
  224. CC_MAXZ = (1<<5),
  225. };
  226. class BoundsAABB
  227. {
  228. public:
  229. void setMin(const RmReal *v)
  230. {
  231. mMin[0] = v[0];
  232. mMin[1] = v[1];
  233. mMin[2] = v[2];
  234. }
  235. void setMax(const RmReal *v)
  236. {
  237. mMax[0] = v[0];
  238. mMax[1] = v[1];
  239. mMax[2] = v[2];
  240. }
  241. void setMin(RmReal x,RmReal y,RmReal z)
  242. {
  243. mMin[0] = x;
  244. mMin[1] = y;
  245. mMin[2] = z;
  246. }
  247. void setMax(RmReal x,RmReal y,RmReal z)
  248. {
  249. mMax[0] = x;
  250. mMax[1] = y;
  251. mMax[2] = z;
  252. }
  253. void include(const RmReal *v)
  254. {
  255. if ( v[0] < mMin[0] ) mMin[0] = v[0];
  256. if ( v[1] < mMin[1] ) mMin[1] = v[1];
  257. if ( v[2] < mMin[2] ) mMin[2] = v[2];
  258. if ( v[0] > mMax[0] ) mMax[0] = v[0];
  259. if ( v[1] > mMax[1] ) mMax[1] = v[1];
  260. if ( v[2] > mMax[2] ) mMax[2] = v[2];
  261. }
  262. void getCenter(RmReal *center) const
  263. {
  264. center[0] = (mMin[0]+mMax[0])*0.5f;
  265. center[1] = (mMin[1]+mMax[1])*0.5f;
  266. center[2] = (mMin[2]+mMax[2])*0.5f;
  267. }
  268. bool intersects(const BoundsAABB &b) const
  269. {
  270. if ((mMin[0] > b.mMax[0]) || (b.mMin[0] > mMax[0])) return false;
  271. if ((mMin[1] > b.mMax[1]) || (b.mMin[1] > mMax[1])) return false;
  272. if ((mMin[2] > b.mMax[2]) || (b.mMin[2] > mMax[2])) return false;
  273. return true;
  274. }
  275. bool containsTriangle(const RmReal *p1,const RmReal *p2,const RmReal *p3) const
  276. {
  277. BoundsAABB b;
  278. b.setMin(p1);
  279. b.setMax(p1);
  280. b.include(p2);
  281. b.include(p3);
  282. return intersects(b);
  283. }
  284. bool containsTriangleExact(const RmReal *p1,const RmReal *p2,const RmReal *p3,RmUint32 &orCode) const
  285. {
  286. bool ret = false;
  287. RmUint32 andCode;
  288. orCode = getClipCode(p1,p2,p3,andCode);
  289. if ( andCode == 0 )
  290. {
  291. ret = true;
  292. }
  293. return ret;
  294. }
  295. inline RmUint32 getClipCode(const RmReal *p1,const RmReal *p2,const RmReal *p3,RmUint32 &andCode) const
  296. {
  297. andCode = 0xFFFFFFFF;
  298. RmUint32 c1 = getClipCode(p1);
  299. RmUint32 c2 = getClipCode(p2);
  300. RmUint32 c3 = getClipCode(p3);
  301. andCode&=c1;
  302. andCode&=c2;
  303. andCode&=c3;
  304. return c1|c2|c3;
  305. }
  306. inline RmUint32 getClipCode(const RmReal *p) const
  307. {
  308. RmUint32 ret = 0;
  309. if ( p[0] < mMin[0] )
  310. {
  311. ret|=CC_MINX;
  312. }
  313. else if ( p[0] > mMax[0] )
  314. {
  315. ret|=CC_MAXX;
  316. }
  317. if ( p[1] < mMin[1] )
  318. {
  319. ret|=CC_MINY;
  320. }
  321. else if ( p[1] > mMax[1] )
  322. {
  323. ret|=CC_MAXY;
  324. }
  325. if ( p[2] < mMin[2] )
  326. {
  327. ret|=CC_MINZ;
  328. }
  329. else if ( p[2] > mMax[2] )
  330. {
  331. ret|=CC_MAXZ;
  332. }
  333. return ret;
  334. }
  335. inline void clamp(const BoundsAABB &aabb)
  336. {
  337. if ( mMin[0] < aabb.mMin[0] ) mMin[0] = aabb.mMin[0];
  338. if ( mMin[1] < aabb.mMin[1] ) mMin[1] = aabb.mMin[1];
  339. if ( mMin[2] < aabb.mMin[2] ) mMin[2] = aabb.mMin[2];
  340. if ( mMax[0] > aabb.mMax[0] ) mMax[0] = aabb.mMax[0];
  341. if ( mMax[1] > aabb.mMax[1] ) mMax[1] = aabb.mMax[1];
  342. if ( mMax[2] > aabb.mMax[2] ) mMax[2] = aabb.mMax[2];
  343. }
  344. RmReal mMin[3];
  345. RmReal mMax[3];
  346. };
  347. class NodeAABB;
  348. class NodeInterface
  349. {
  350. public:
  351. virtual NodeAABB * getNode(void) = 0;
  352. virtual void getFaceNormal(RmUint32 tri,RmReal *faceNormal) = 0;
  353. };
  354. class NodeAABB
  355. {
  356. public:
  357. NodeAABB(void)
  358. {
  359. mLeft = NULL;
  360. mRight = NULL;
  361. mLeafTriangleIndex= TRI_EOF;
  362. }
  363. NodeAABB(RmUint32 vcount,const RmReal *vertices,RmUint32 tcount,RmUint32 *indices,
  364. RmUint32 maxDepth, // Maximum recursion depth for the triangle mesh.
  365. RmUint32 minLeafSize, // minimum triangles to treat as a 'leaf' node.
  366. RmReal minAxisSize,
  367. NodeInterface *callback,
  368. TriVector &leafTriangles) // once a particular axis is less than this size, stop sub-dividing.
  369. {
  370. mLeft = NULL;
  371. mRight = NULL;
  372. mLeafTriangleIndex = TRI_EOF;
  373. TriVector triangles;
  374. triangles.reserve(tcount);
  375. for (RmUint32 i=0; i<tcount; i++)
  376. {
  377. triangles.push_back(i);
  378. }
  379. mBounds.setMin( vertices );
  380. mBounds.setMax( vertices );
  381. const RmReal *vtx = vertices+3;
  382. for (RmUint32 i=1; i<vcount; i++)
  383. {
  384. mBounds.include( vtx );
  385. vtx+=3;
  386. }
  387. split(triangles,vcount,vertices,tcount,indices,0,maxDepth,minLeafSize,minAxisSize,callback,leafTriangles);
  388. }
  389. NodeAABB(const BoundsAABB &aabb)
  390. {
  391. mBounds = aabb;
  392. mLeft = NULL;
  393. mRight = NULL;
  394. mLeafTriangleIndex = TRI_EOF;
  395. }
  396. ~NodeAABB(void)
  397. {
  398. }
  399. // here is where we split the mesh..
  400. void split(const TriVector &triangles,
  401. RmUint32 vcount,
  402. const RmReal *vertices,
  403. RmUint32 tcount,
  404. const RmUint32 *indices,
  405. RmUint32 depth,
  406. RmUint32 maxDepth, // Maximum recursion depth for the triangle mesh.
  407. RmUint32 minLeafSize, // minimum triangles to treat as a 'leaf' node.
  408. RmReal minAxisSize,
  409. NodeInterface *callback,
  410. TriVector &leafTriangles) // once a particular axis is less than this size, stop sub-dividing.
  411. {
  412. // Find the longest axis of the bounding volume of this node
  413. RmReal dx = mBounds.mMax[0] - mBounds.mMin[0];
  414. RmReal dy = mBounds.mMax[1] - mBounds.mMin[1];
  415. RmReal dz = mBounds.mMax[2] - mBounds.mMin[2];
  416. AxisAABB axis = AABB_XAXIS;
  417. RmReal laxis = dx;
  418. if ( dy > dx )
  419. {
  420. axis = AABB_YAXIS;
  421. laxis = dy;
  422. }
  423. if ( dz > dx && dz > dy )
  424. {
  425. axis = AABB_ZAXIS;
  426. laxis = dz;
  427. }
  428. RmUint32 count = triangles.size();
  429. // if the number of triangles is less than the minimum allowed for a leaf node or...
  430. // we have reached the maximum recursion depth or..
  431. // the width of the longest axis is less than the minimum axis size then...
  432. // we create the leaf node and copy the triangles into the leaf node triangle array.
  433. if ( count < minLeafSize || depth >= maxDepth || laxis < minAxisSize )
  434. {
  435. // Copy the triangle indices into the leaf triangles array
  436. mLeafTriangleIndex = leafTriangles.size(); // assign the array start location for these leaf triangles.
  437. leafTriangles.push_back(count);
  438. for (auto i = triangles.begin(); i != triangles.end(); ++i) {
  439. RmUint32 tri = *i;
  440. leafTriangles.push_back(tri);
  441. }
  442. }
  443. else
  444. {
  445. RmReal center[3];
  446. mBounds.getCenter(center);
  447. BoundsAABB b1,b2;
  448. splitRect(axis,mBounds,b1,b2,center);
  449. // Compute two bounding boxes based upon the split of the longest axis
  450. BoundsAABB leftBounds,rightBounds;
  451. TriVector leftTriangles;
  452. TriVector rightTriangles;
  453. // Create two arrays; one of all triangles which intersect the 'left' half of the bounding volume node
  454. // and another array that includes all triangles which intersect the 'right' half of the bounding volume node.
  455. for (auto i = triangles.begin(); i != triangles.end(); ++i) {
  456. RmUint32 tri = (*i);
  457. {
  458. RmUint32 i1 = indices[tri*3+0];
  459. RmUint32 i2 = indices[tri*3+1];
  460. RmUint32 i3 = indices[tri*3+2];
  461. const RmReal *p1 = &vertices[i1*3];
  462. const RmReal *p2 = &vertices[i2*3];
  463. const RmReal *p3 = &vertices[i3*3];
  464. RmUint32 addCount = 0;
  465. RmUint32 orCode=0xFFFFFFFF;
  466. if ( b1.containsTriangleExact(p1,p2,p3,orCode))
  467. {
  468. addCount++;
  469. if ( leftTriangles.empty() )
  470. {
  471. leftBounds.setMin(p1);
  472. leftBounds.setMax(p1);
  473. }
  474. leftBounds.include(p1);
  475. leftBounds.include(p2);
  476. leftBounds.include(p3);
  477. leftTriangles.push_back(tri); // Add this triangle to the 'left triangles' array and revise the left triangles bounding volume
  478. }
  479. // if the orCode is zero; meaning the triangle was fully self-contiained int he left bounding box; then we don't need to test against the right
  480. if ( orCode && b2.containsTriangleExact(p1,p2,p3,orCode))
  481. {
  482. addCount++;
  483. if ( rightTriangles.empty() )
  484. {
  485. rightBounds.setMin(p1);
  486. rightBounds.setMax(p1);
  487. }
  488. rightBounds.include(p1);
  489. rightBounds.include(p2);
  490. rightBounds.include(p3);
  491. rightTriangles.push_back(tri); // Add this triangle to the 'right triangles' array and revise the right triangles bounding volume.
  492. }
  493. assert( addCount );
  494. }
  495. }
  496. if ( !leftTriangles.empty() ) // If there are triangles in the left half then...
  497. {
  498. leftBounds.clamp(b1); // we have to clamp the bounding volume so it stays inside the parent volume.
  499. mLeft = callback->getNode(); // get a new AABB node
  500. new ( mLeft ) NodeAABB(leftBounds); // initialize it to default constructor values.
  501. // Then recursively split this node.
  502. mLeft->split(leftTriangles,vcount,vertices,tcount,indices,depth+1,maxDepth,minLeafSize,minAxisSize,callback,leafTriangles);
  503. }
  504. if ( !rightTriangles.empty() ) // If there are triangles in the right half then..
  505. {
  506. rightBounds.clamp(b2); // clamps the bounding volume so it stays restricted to the size of the parent volume.
  507. mRight = callback->getNode(); // allocate and default initialize a new node
  508. new ( mRight ) NodeAABB(rightBounds);
  509. // Recursively split this node.
  510. mRight->split(rightTriangles,vcount,vertices,tcount,indices,depth+1,maxDepth,minLeafSize,minAxisSize,callback,leafTriangles);
  511. }
  512. }
  513. }
  514. void splitRect(AxisAABB axis,const BoundsAABB &source,BoundsAABB &b1,BoundsAABB &b2,const RmReal *midpoint)
  515. {
  516. switch ( axis )
  517. {
  518. case AABB_XAXIS:
  519. {
  520. b1.setMin( source.mMin );
  521. b1.setMax( midpoint[0], source.mMax[1], source.mMax[2] );
  522. b2.setMin( midpoint[0], source.mMin[1], source.mMin[2] );
  523. b2.setMax(source.mMax);
  524. }
  525. break;
  526. case AABB_YAXIS:
  527. {
  528. b1.setMin(source.mMin);
  529. b1.setMax(source.mMax[0], midpoint[1], source.mMax[2]);
  530. b2.setMin(source.mMin[0], midpoint[1], source.mMin[2]);
  531. b2.setMax(source.mMax);
  532. }
  533. break;
  534. case AABB_ZAXIS:
  535. {
  536. b1.setMin(source.mMin);
  537. b1.setMax(source.mMax[0], source.mMax[1], midpoint[2]);
  538. b2.setMin(source.mMin[0], source.mMin[1], midpoint[2]);
  539. b2.setMax(source.mMax);
  540. }
  541. break;
  542. }
  543. }
  544. virtual void raycast(bool &hit,
  545. const RmReal *from,
  546. const RmReal *to,
  547. const RmReal *dir,
  548. RmReal *hitLocation,
  549. RmReal *hitNormal,
  550. RmReal *hitDistance,
  551. RmUint32 *GridID,
  552. const RmReal *vertices,
  553. const RmUint32 *indices,
  554. const RmUint32 *grids,
  555. RmReal &nearestDistance,
  556. NodeInterface *callback,
  557. RmUint32 *raycastTriangles,
  558. RmUint32 raycastFrame,
  559. const TriVector &leafTriangles,
  560. RmUint32 &nearestTriIndex)
  561. {
  562. RmReal sect[3];
  563. RmReal nd = nearestDistance;
  564. if ( !intersectLineSegmentAABB(mBounds.mMin,mBounds.mMax,from,dir,nd,sect) )
  565. {
  566. return;
  567. }
  568. if ( mLeafTriangleIndex != TRI_EOF )
  569. {
  570. const RmUint32 *scan = &leafTriangles[mLeafTriangleIndex];
  571. RmUint32 count = *scan++;
  572. for (RmUint32 i=0; i<count; i++)
  573. {
  574. RmUint32 tri = *scan++;
  575. if ( raycastTriangles[tri] != raycastFrame )
  576. {
  577. raycastTriangles[tri] = raycastFrame;
  578. RmUint32 i1 = indices[tri*3+0];
  579. RmUint32 i2 = indices[tri*3+1];
  580. RmUint32 i3 = indices[tri*3+2];
  581. const RmReal *p1 = &vertices[i1*3];
  582. const RmReal *p2 = &vertices[i2*3];
  583. const RmReal *p3 = &vertices[i3*3];
  584. RmReal t;
  585. if ( rayIntersectsTriangle(from,dir,p1,p2,p3,t))
  586. {
  587. bool accept = false;
  588. if ( t == nearestDistance && tri < nearestTriIndex )
  589. {
  590. accept = true;
  591. }
  592. if ( t < nearestDistance || accept )
  593. {
  594. nearestDistance = t;
  595. if ( hitLocation )
  596. {
  597. hitLocation[0] = from[0]+dir[0]*t;
  598. hitLocation[1] = from[1]+dir[1]*t;
  599. hitLocation[2] = from[2]+dir[2]*t;
  600. }
  601. if ( hitNormal )
  602. {
  603. callback->getFaceNormal(tri,hitNormal);
  604. }
  605. if ( hitDistance )
  606. {
  607. *hitDistance = t;
  608. }
  609. if(GridID) {
  610. *GridID = grids[tri];
  611. }
  612. nearestTriIndex = tri;
  613. hit = true;
  614. }
  615. }
  616. }
  617. }
  618. }
  619. else
  620. {
  621. if ( mLeft )
  622. {
  623. mLeft->raycast(hit,from,to,dir,hitLocation,hitNormal,hitDistance,GridID,vertices,indices,grids,nearestDistance,callback,raycastTriangles,raycastFrame,leafTriangles,nearestTriIndex);
  624. }
  625. if ( mRight )
  626. {
  627. mRight->raycast(hit,from,to,dir,hitLocation,hitNormal,hitDistance,GridID,vertices,indices,grids,nearestDistance,callback,raycastTriangles,raycastFrame,leafTriangles,nearestTriIndex);
  628. }
  629. }
  630. }
  631. NodeAABB *mLeft; // left node
  632. NodeAABB *mRight; // right node
  633. BoundsAABB mBounds; // bounding volume of node
  634. RmUint32 mLeafTriangleIndex; // if it is a leaf node; then these are the triangle indices.
  635. };
  636. class MyRaycastMesh : public RaycastMesh, public NodeInterface
  637. {
  638. public:
  639. MyRaycastMesh(RmUint32 vcount,const RmReal *vertices,RmUint32 tcount,const RmUint32 *indices,const RmUint32 *grids,RmUint32 maxDepth,RmUint32 minLeafSize,RmReal minAxisSize)
  640. {
  641. mRaycastFrame = 0;
  642. if ( maxDepth < 2 )
  643. {
  644. maxDepth = 2;
  645. }
  646. if ( maxDepth > 15 )
  647. {
  648. maxDepth = 15;
  649. }
  650. RmUint32 pow2Table[16] = { 1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 65536 };
  651. mMaxNodeCount = 0;
  652. for (RmUint32 i=0; i<=maxDepth; i++)
  653. {
  654. mMaxNodeCount+=pow2Table[i];
  655. }
  656. mNodes = new NodeAABB[mMaxNodeCount];
  657. mNodeCount = 0;
  658. mVcount = vcount;
  659. mVertices = (RmReal *)::malloc(sizeof(RmReal)*3*vcount);
  660. memcpy(mVertices,vertices,sizeof(RmReal)*3*vcount);
  661. mTcount = tcount;
  662. mIndices = (RmUint32 *)::malloc(sizeof(RmUint32)*tcount*3);
  663. memcpy(mIndices,indices,sizeof(RmUint32)*tcount*3);
  664. mRaycastTriangles = (RmUint32 *)::malloc(tcount*sizeof(RmUint32));
  665. memset(mRaycastTriangles,0,tcount*sizeof(RmUint32));
  666. mGrids = (RmUint32 *)::malloc(sizeof(RmUint32)*tcount);
  667. memcpy(mGrids,grids,sizeof(RmUint32)*tcount);
  668. mRoot = getNode();
  669. mFaceNormals = NULL;
  670. new ( mRoot ) NodeAABB(mVcount,mVertices,mTcount,mIndices,maxDepth,minLeafSize,minAxisSize,this,mLeafTriangles);
  671. }
  672. ~MyRaycastMesh(void)
  673. {
  674. delete []mNodes;
  675. ::free(mVertices);
  676. ::free(mIndices);
  677. ::free(mFaceNormals);
  678. ::free(mRaycastTriangles);
  679. ::free(mGrids);
  680. }
  681. virtual bool raycast(const RmReal *from,const RmReal *to,RmReal *hitLocation,RmReal *hitNormal,RmReal *hitDistance,RmUint32 *GridID)
  682. {
  683. bool ret = false;
  684. RmReal dir[3];
  685. dir[0] = to[0] - from[0];
  686. dir[1] = to[1] - from[1];
  687. dir[2] = to[2] - from[2];
  688. RmReal distance = sqrtf( dir[0]*dir[0] + dir[1]*dir[1]+dir[2]*dir[2] );
  689. if ( distance < 0.0000000001f ) return false;
  690. RmReal recipDistance = 1.0f / distance;
  691. dir[0]*=recipDistance;
  692. dir[1]*=recipDistance;
  693. dir[2]*=recipDistance;
  694. mRaycastFrame++;
  695. RmUint32 nearestTriIndex=TRI_EOF;
  696. mRoot->raycast(ret,from,to,dir,hitLocation,hitNormal,hitDistance,GridID,mVertices,mIndices,mGrids,distance,this,mRaycastTriangles,mRaycastFrame,mLeafTriangles,nearestTriIndex);
  697. return ret;
  698. }
  699. virtual void release(void)
  700. {
  701. delete this;
  702. }
  703. virtual const RmReal * getBoundMin(void) const // return the minimum bounding box
  704. {
  705. return mRoot->mBounds.mMin;
  706. }
  707. virtual const RmReal * getBoundMax(void) const // return the maximum bounding box.
  708. {
  709. return mRoot->mBounds.mMax;
  710. }
  711. virtual NodeAABB * getNode(void)
  712. {
  713. assert( mNodeCount < mMaxNodeCount );
  714. NodeAABB *ret = &mNodes[mNodeCount];
  715. mNodeCount++;
  716. return ret;
  717. }
  718. virtual void getFaceNormal(RmUint32 tri,RmReal *faceNormal)
  719. {
  720. if ( mFaceNormals == NULL )
  721. {
  722. mFaceNormals = (RmReal *)::malloc(sizeof(RmReal)*3*mTcount);
  723. for (RmUint32 i=0; i<mTcount; i++)
  724. {
  725. RmUint32 i1 = mIndices[i*3+0];
  726. RmUint32 i2 = mIndices[i*3+1];
  727. RmUint32 i3 = mIndices[i*3+2];
  728. const RmReal*p1 = &mVertices[i1*3];
  729. const RmReal*p2 = &mVertices[i2*3];
  730. const RmReal*p3 = &mVertices[i3*3];
  731. RmReal *dest = &mFaceNormals[i*3];
  732. computePlane(p3,p2,p1,dest);
  733. }
  734. }
  735. const RmReal *src = &mFaceNormals[tri*3];
  736. faceNormal[0] = src[0];
  737. faceNormal[1] = src[1];
  738. faceNormal[2] = src[2];
  739. }
  740. virtual bool bruteForceRaycast(const RmReal *from,const RmReal *to,RmReal *hitLocation,RmReal *hitNormal,RmReal *hitDistance,RmUint32 *GridID)
  741. {
  742. bool ret = false;
  743. RmReal dir[3];
  744. dir[0] = to[0] - from[0];
  745. dir[1] = to[1] - from[1];
  746. dir[2] = to[2] - from[2];
  747. RmReal distance = sqrtf( dir[0]*dir[0] + dir[1]*dir[1]+dir[2]*dir[2] );
  748. if ( distance < 0.0000000001f ) return false;
  749. RmReal recipDistance = 1.0f / distance;
  750. dir[0]*=recipDistance;
  751. dir[1]*=recipDistance;
  752. dir[2]*=recipDistance;
  753. const RmUint32 *indices = mIndices;
  754. const RmReal *vertices = mVertices;
  755. RmReal nearestDistance = distance;
  756. for (RmUint32 tri=0; tri<mTcount; tri++)
  757. {
  758. RmUint32 i1 = indices[tri*3+0];
  759. RmUint32 i2 = indices[tri*3+1];
  760. RmUint32 i3 = indices[tri*3+2];
  761. const RmReal *p1 = &vertices[i1*3];
  762. const RmReal *p2 = &vertices[i2*3];
  763. const RmReal *p3 = &vertices[i3*3];
  764. RmReal t;
  765. if ( rayIntersectsTriangle(from,dir,p1,p2,p3,t))
  766. {
  767. if ( t < nearestDistance )
  768. {
  769. nearestDistance = t;
  770. if ( hitLocation )
  771. {
  772. hitLocation[0] = from[0]+dir[0]*t;
  773. hitLocation[1] = from[1]+dir[1]*t;
  774. hitLocation[2] = from[2]+dir[2]*t;
  775. }
  776. if ( hitNormal )
  777. {
  778. getFaceNormal(tri,hitNormal);
  779. }
  780. if ( hitDistance )
  781. {
  782. *hitDistance = t;
  783. }
  784. if(GridID) {
  785. *GridID = mGrids[tri];
  786. }
  787. ret = true;
  788. }
  789. }
  790. }
  791. return ret;
  792. }
  793. RmUint32 mRaycastFrame;
  794. RmUint32 *mRaycastTriangles;
  795. RmUint32 mVcount;
  796. RmReal *mVertices;
  797. RmReal *mFaceNormals;
  798. RmUint32 mTcount;
  799. RmUint32 *mIndices;
  800. NodeAABB *mRoot;
  801. RmUint32 mNodeCount;
  802. RmUint32 mMaxNodeCount;
  803. NodeAABB *mNodes;
  804. TriVector mLeafTriangles;
  805. RmUint32 *mGrids;
  806. };
  807. };
  808. using namespace RAYCAST_MESH;
  809. RaycastMesh * createRaycastMesh(RmUint32 vcount, // The number of vertices in the source triangle mesh
  810. const RmReal *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc.
  811. RmUint32 tcount, // The number of triangles in the source triangle mesh
  812. const RmUint32 *indices, // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ...
  813. const RmUint32 *grids,
  814. RmUint32 maxDepth, // Maximum recursion depth for the triangle mesh.
  815. RmUint32 minLeafSize, // minimum triangles to treat as a 'leaf' node.
  816. RmReal minAxisSize ) // once a particular axis is less than this size, stop sub-dividing.
  817. {
  818. auto m = new MyRaycastMesh(vcount, vertices, tcount, indices, grids, maxDepth, minLeafSize, minAxisSize);
  819. return static_cast< RaycastMesh * >(m);
  820. }