raycast_mesh.cpp 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964
  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. RmUint32 *WidgetID,
  553. const RmReal *vertices,
  554. const RmUint32 *indices,
  555. const RmUint32 *grids,
  556. const RmUint32 *widgets,
  557. RmReal &nearestDistance,
  558. NodeInterface *callback,
  559. RmUint32 *raycastTriangles,
  560. RmUint32 raycastFrame,
  561. const TriVector &leafTriangles,
  562. RmUint32 &nearestTriIndex)
  563. {
  564. RmReal sect[3];
  565. RmReal nd = nearestDistance;
  566. if ( !intersectLineSegmentAABB(mBounds.mMin,mBounds.mMax,from,dir,nd,sect) )
  567. {
  568. return;
  569. }
  570. if ( mLeafTriangleIndex != TRI_EOF )
  571. {
  572. const RmUint32 *scan = &leafTriangles[mLeafTriangleIndex];
  573. RmUint32 count = *scan++;
  574. for (RmUint32 i=0; i<count; i++)
  575. {
  576. RmUint32 tri = *scan++;
  577. if ( raycastTriangles[tri] != raycastFrame )
  578. {
  579. raycastTriangles[tri] = raycastFrame;
  580. RmUint32 i1 = indices[tri*3+0];
  581. RmUint32 i2 = indices[tri*3+1];
  582. RmUint32 i3 = indices[tri*3+2];
  583. const RmReal *p1 = &vertices[i1*3];
  584. const RmReal *p2 = &vertices[i2*3];
  585. const RmReal *p3 = &vertices[i3*3];
  586. RmReal t;
  587. if ( rayIntersectsTriangle(from,dir,p1,p2,p3,t))
  588. {
  589. bool accept = false;
  590. if ( t == nearestDistance && tri < nearestTriIndex )
  591. {
  592. accept = true;
  593. }
  594. if ( t < nearestDistance || accept )
  595. {
  596. nearestDistance = t;
  597. if ( hitLocation )
  598. {
  599. hitLocation[0] = from[0]+dir[0]*t;
  600. hitLocation[1] = from[1]+dir[1]*t;
  601. hitLocation[2] = from[2]+dir[2]*t;
  602. }
  603. if ( hitNormal )
  604. {
  605. callback->getFaceNormal(tri,hitNormal);
  606. }
  607. if ( hitDistance )
  608. {
  609. *hitDistance = t;
  610. }
  611. if(GridID) {
  612. *GridID = grids[tri];
  613. }
  614. if(WidgetID) {
  615. *WidgetID = widgets[tri];
  616. }
  617. nearestTriIndex = tri;
  618. hit = true;
  619. }
  620. }
  621. }
  622. }
  623. }
  624. else
  625. {
  626. if ( mLeft )
  627. {
  628. mLeft->raycast(hit,from,to,dir,hitLocation,hitNormal,hitDistance,GridID,WidgetID,vertices,indices,grids,widgets,nearestDistance,callback,raycastTriangles,raycastFrame,leafTriangles,nearestTriIndex);
  629. }
  630. if ( mRight )
  631. {
  632. mRight->raycast(hit,from,to,dir,hitLocation,hitNormal,hitDistance,GridID,WidgetID,vertices,indices,grids,widgets,nearestDistance,callback,raycastTriangles,raycastFrame,leafTriangles,nearestTriIndex);
  633. }
  634. }
  635. }
  636. NodeAABB *mLeft; // left node
  637. NodeAABB *mRight; // right node
  638. BoundsAABB mBounds; // bounding volume of node
  639. RmUint32 mLeafTriangleIndex; // if it is a leaf node; then these are the triangle indices.
  640. };
  641. class MyRaycastMesh : public RaycastMesh, public NodeInterface
  642. {
  643. public:
  644. MyRaycastMesh(RmUint32 vcount,const RmReal *vertices,RmUint32 tcount,const RmUint32 *indices,const RmUint32 *grids,const RmUint32 *widgets,RmUint32 maxDepth,RmUint32 minLeafSize,RmReal minAxisSize)
  645. {
  646. mRaycastFrame = 0;
  647. if ( maxDepth < 2 )
  648. {
  649. maxDepth = 2;
  650. }
  651. if ( maxDepth > 15 )
  652. {
  653. maxDepth = 15;
  654. }
  655. RmUint32 pow2Table[16] = { 1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 65536 };
  656. mMaxNodeCount = 0;
  657. for (RmUint32 i=0; i<=maxDepth; i++)
  658. {
  659. mMaxNodeCount+=pow2Table[i];
  660. }
  661. mNodes = new NodeAABB[mMaxNodeCount];
  662. mNodeCount = 0;
  663. mVcount = vcount;
  664. mVertices = (RmReal *)::malloc(sizeof(RmReal)*3*vcount);
  665. memcpy(mVertices,vertices,sizeof(RmReal)*3*vcount);
  666. mTcount = tcount;
  667. mIndices = (RmUint32 *)::malloc(sizeof(RmUint32)*tcount*3);
  668. memcpy(mIndices,indices,sizeof(RmUint32)*tcount*3);
  669. mRaycastTriangles = (RmUint32 *)::malloc(tcount*sizeof(RmUint32));
  670. memset(mRaycastTriangles,0,tcount*sizeof(RmUint32));
  671. mGrids = (RmUint32 *)::malloc(sizeof(RmUint32)*tcount);
  672. memcpy(mGrids,grids,sizeof(RmUint32)*tcount);
  673. mWidgets = (RmUint32 *)::malloc(sizeof(RmUint32)*tcount);
  674. memcpy(mWidgets,widgets,sizeof(RmUint32)*tcount);
  675. mRoot = getNode();
  676. mFaceNormals = NULL;
  677. new ( mRoot ) NodeAABB(mVcount,mVertices,mTcount,mIndices,maxDepth,minLeafSize,minAxisSize,this,mLeafTriangles);
  678. }
  679. ~MyRaycastMesh(void)
  680. {
  681. delete []mNodes;
  682. ::free(mVertices);
  683. ::free(mIndices);
  684. ::free(mFaceNormals);
  685. ::free(mRaycastTriangles);
  686. ::free(mGrids);
  687. ::free(mWidgets);
  688. }
  689. virtual bool raycast(const RmReal *from,const RmReal *to,RmReal *hitLocation,RmReal *hitNormal,RmReal *hitDistance,RmUint32 *GridID,RmUint32 *WidgetID)
  690. {
  691. bool ret = false;
  692. RmReal dir[3];
  693. dir[0] = to[0] - from[0];
  694. dir[1] = to[1] - from[1];
  695. dir[2] = to[2] - from[2];
  696. RmReal distance = sqrtf( dir[0]*dir[0] + dir[1]*dir[1]+dir[2]*dir[2] );
  697. if ( distance < 0.0000000001f ) return false;
  698. RmReal recipDistance = 1.0f / distance;
  699. dir[0]*=recipDistance;
  700. dir[1]*=recipDistance;
  701. dir[2]*=recipDistance;
  702. mRaycastFrame++;
  703. RmUint32 nearestTriIndex=TRI_EOF;
  704. mRoot->raycast(ret,from,to,dir,hitLocation,hitNormal,hitDistance,GridID,WidgetID,mVertices,mIndices,mGrids,mWidgets,distance,this,mRaycastTriangles,mRaycastFrame,mLeafTriangles,nearestTriIndex);
  705. return ret;
  706. }
  707. virtual void release(void)
  708. {
  709. delete this;
  710. }
  711. virtual const RmReal * getBoundMin(void) const // return the minimum bounding box
  712. {
  713. return mRoot->mBounds.mMin;
  714. }
  715. virtual const RmReal * getBoundMax(void) const // return the maximum bounding box.
  716. {
  717. return mRoot->mBounds.mMax;
  718. }
  719. virtual NodeAABB * getNode(void)
  720. {
  721. assert( mNodeCount < mMaxNodeCount );
  722. NodeAABB *ret = &mNodes[mNodeCount];
  723. mNodeCount++;
  724. return ret;
  725. }
  726. virtual void getFaceNormal(RmUint32 tri,RmReal *faceNormal)
  727. {
  728. if ( mFaceNormals == NULL )
  729. {
  730. mFaceNormals = (RmReal *)::malloc(sizeof(RmReal)*3*mTcount);
  731. for (RmUint32 i=0; i<mTcount; i++)
  732. {
  733. RmUint32 i1 = mIndices[i*3+0];
  734. RmUint32 i2 = mIndices[i*3+1];
  735. RmUint32 i3 = mIndices[i*3+2];
  736. const RmReal*p1 = &mVertices[i1*3];
  737. const RmReal*p2 = &mVertices[i2*3];
  738. const RmReal*p3 = &mVertices[i3*3];
  739. RmReal *dest = &mFaceNormals[i*3];
  740. computePlane(p3,p2,p1,dest);
  741. }
  742. }
  743. const RmReal *src = &mFaceNormals[tri*3];
  744. faceNormal[0] = src[0];
  745. faceNormal[1] = src[1];
  746. faceNormal[2] = src[2];
  747. }
  748. virtual bool bruteForceRaycast(const RmReal *from,const RmReal *to,RmReal *hitLocation,RmReal *hitNormal,RmReal *hitDistance,RmUint32 *GridID,RmUint32 *WidgetID)
  749. {
  750. bool ret = false;
  751. RmReal dir[3];
  752. dir[0] = to[0] - from[0];
  753. dir[1] = to[1] - from[1];
  754. dir[2] = to[2] - from[2];
  755. RmReal distance = sqrtf( dir[0]*dir[0] + dir[1]*dir[1]+dir[2]*dir[2] );
  756. if ( distance < 0.0000000001f ) return false;
  757. RmReal recipDistance = 1.0f / distance;
  758. dir[0]*=recipDistance;
  759. dir[1]*=recipDistance;
  760. dir[2]*=recipDistance;
  761. const RmUint32 *indices = mIndices;
  762. const RmReal *vertices = mVertices;
  763. RmReal nearestDistance = distance;
  764. for (RmUint32 tri=0; tri<mTcount; tri++)
  765. {
  766. RmUint32 i1 = indices[tri*3+0];
  767. RmUint32 i2 = indices[tri*3+1];
  768. RmUint32 i3 = indices[tri*3+2];
  769. const RmReal *p1 = &vertices[i1*3];
  770. const RmReal *p2 = &vertices[i2*3];
  771. const RmReal *p3 = &vertices[i3*3];
  772. RmReal t;
  773. if ( rayIntersectsTriangle(from,dir,p1,p2,p3,t))
  774. {
  775. if ( t < nearestDistance )
  776. {
  777. nearestDistance = t;
  778. if ( hitLocation )
  779. {
  780. hitLocation[0] = from[0]+dir[0]*t;
  781. hitLocation[1] = from[1]+dir[1]*t;
  782. hitLocation[2] = from[2]+dir[2]*t;
  783. }
  784. if ( hitNormal )
  785. {
  786. getFaceNormal(tri,hitNormal);
  787. }
  788. if ( hitDistance )
  789. {
  790. *hitDistance = t;
  791. }
  792. if(GridID) {
  793. *GridID = mGrids[tri];
  794. }
  795. if(WidgetID) {
  796. *WidgetID = mWidgets[tri];
  797. }
  798. ret = true;
  799. }
  800. }
  801. }
  802. return ret;
  803. }
  804. RmUint32 mRaycastFrame;
  805. RmUint32 *mRaycastTriangles;
  806. RmUint32 mVcount;
  807. RmReal *mVertices;
  808. RmReal *mFaceNormals;
  809. RmUint32 mTcount;
  810. RmUint32 *mIndices;
  811. NodeAABB *mRoot;
  812. RmUint32 mNodeCount;
  813. RmUint32 mMaxNodeCount;
  814. NodeAABB *mNodes;
  815. TriVector mLeafTriangles;
  816. RmUint32 *mGrids;
  817. RmUint32 *mWidgets;
  818. };
  819. };
  820. using namespace RAYCAST_MESH;
  821. RaycastMesh * createRaycastMesh(RmUint32 vcount, // The number of vertices in the source triangle mesh
  822. const RmReal *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc.
  823. RmUint32 tcount, // The number of triangles in the source triangle mesh
  824. const RmUint32 *indices, // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ...
  825. const RmUint32 *grids,
  826. const RmUint32 *widgets,
  827. RmUint32 maxDepth, // Maximum recursion depth for the triangle mesh.
  828. RmUint32 minLeafSize, // minimum triangles to treat as a 'leaf' node.
  829. RmReal minAxisSize ) // once a particular axis is less than this size, stop sub-dividing.
  830. {
  831. auto m = new MyRaycastMesh(vcount, vertices, tcount, indices, grids, widgets, maxDepth, minLeafSize, minAxisSize);
  832. return static_cast< RaycastMesh * >(m);
  833. }