9
3

raycast_mesh.cpp 25 KB

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