btBoxBoxDetector.cpp 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718
  1. /*
  2. * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
  3. * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
  4. * All rights reserved. Email: russ@q12.org Web: www.q12.org
  5. Bullet Continuous Collision Detection and Physics Library
  6. Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
  7. This software is provided 'as-is', without any express or implied warranty.
  8. In no event will the authors be held liable for any damages arising from the use of this software.
  9. Permission is granted to anyone to use this software for any purpose,
  10. including commercial applications, and to alter it and redistribute it freely,
  11. subject to the following restrictions:
  12. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  13. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  14. 3. This notice may not be removed or altered from any source distribution.
  15. */
  16. ///ODE box-box collision detection is adapted to work with Bullet
  17. #include "btBoxBoxDetector.h"
  18. #include "bullet/BulletCollision//CollisionShapes/btBoxShape.h"
  19. #include <float.h>
  20. #include <string.h>
  21. btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2)
  22. : m_box1(box1),
  23. m_box2(box2)
  24. {
  25. }
  26. // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
  27. // generate contact points. this returns 0 if there is no contact otherwise
  28. // it returns the number of contacts generated.
  29. // `normal' returns the contact normal.
  30. // `depth' returns the maximum penetration depth along that normal.
  31. // `return_code' returns a number indicating the type of contact that was
  32. // detected:
  33. // 1,2,3 = box 2 intersects with a face of box 1
  34. // 4,5,6 = box 1 intersects with a face of box 2
  35. // 7..15 = edge-edge contact
  36. // `maxc' is the maximum number of contacts allowed to be generated, i.e.
  37. // the size of the `contact' array.
  38. // `contact' and `skip' are the contact array information provided to the
  39. // collision functions. this function only fills in the position and depth
  40. // fields.
  41. struct dContactGeom;
  42. #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
  43. #define dInfinity FLT_MAX
  44. /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
  45. PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
  46. PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
  47. PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
  48. */
  49. static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
  50. static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
  51. static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
  52. static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
  53. #define dMULTIPLYOP1_331(A,op,B,C) \
  54. {\
  55. (A)[0] op dDOT41((B),(C)); \
  56. (A)[1] op dDOT41((B+1),(C)); \
  57. (A)[2] op dDOT41((B+2),(C)); \
  58. }
  59. #define dMULTIPLYOP0_331(A,op,B,C) \
  60. { \
  61. (A)[0] op dDOT((B),(C)); \
  62. (A)[1] op dDOT((B+4),(C)); \
  63. (A)[2] op dDOT((B+8),(C)); \
  64. }
  65. #define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
  66. #define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
  67. typedef btScalar dMatrix3[4*3];
  68. void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
  69. const btVector3& pb, const btVector3& ub,
  70. btScalar *alpha, btScalar *beta);
  71. void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
  72. const btVector3& pb, const btVector3& ub,
  73. btScalar *alpha, btScalar *beta)
  74. {
  75. btVector3 p;
  76. p[0] = pb[0] - pa[0];
  77. p[1] = pb[1] - pa[1];
  78. p[2] = pb[2] - pa[2];
  79. btScalar uaub = dDOT(ua,ub);
  80. btScalar q1 = dDOT(ua,p);
  81. btScalar q2 = -dDOT(ub,p);
  82. btScalar d = 1-uaub*uaub;
  83. if (d <= btScalar(0.0001f)) {
  84. // @@@ this needs to be made more robust
  85. *alpha = 0;
  86. *beta = 0;
  87. }
  88. else {
  89. d = 1.f/d;
  90. *alpha = (q1 + uaub*q2)*d;
  91. *beta = (uaub*q1 + q2)*d;
  92. }
  93. }
  94. // find all the intersection points between the 2D rectangle with vertices
  95. // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
  96. // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
  97. //
  98. // the intersection points are returned as x,y pairs in the 'ret' array.
  99. // the number of intersection points is returned by the function (this will
  100. // be in the range 0 to 8).
  101. static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
  102. {
  103. // q (and r) contain nq (and nr) coordinate points for the current (and
  104. // chopped) polygons
  105. int nq=4,nr=0;
  106. btScalar buffer[16];
  107. btScalar *q = p;
  108. btScalar *r = ret;
  109. for (int dir=0; dir <= 1; dir++) {
  110. // direction notation: xy[0] = x axis, xy[1] = y axis
  111. for (int sign=-1; sign <= 1; sign += 2) {
  112. // chop q along the line xy[dir] = sign*h[dir]
  113. btScalar *pq = q;
  114. btScalar *pr = r;
  115. nr = 0;
  116. for (int i=nq; i > 0; i--) {
  117. // go through all points in q and all lines between adjacent points
  118. if (sign*pq[dir] < h[dir]) {
  119. // this point is inside the chopping line
  120. pr[0] = pq[0];
  121. pr[1] = pq[1];
  122. pr += 2;
  123. nr++;
  124. if (nr & 8) {
  125. q = r;
  126. goto done;
  127. }
  128. }
  129. btScalar *nextq = (i > 1) ? pq+2 : q;
  130. if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
  131. // this line crosses the chopping line
  132. pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
  133. (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
  134. pr[dir] = sign*h[dir];
  135. pr += 2;
  136. nr++;
  137. if (nr & 8) {
  138. q = r;
  139. goto done;
  140. }
  141. }
  142. pq += 2;
  143. }
  144. q = r;
  145. r = (q==ret) ? buffer : ret;
  146. nq = nr;
  147. }
  148. }
  149. done:
  150. if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
  151. return nr;
  152. }
  153. #define M__PI 3.14159265f
  154. // given n points in the plane (array p, of size 2*n), generate m points that
  155. // best represent the whole set. the definition of 'best' here is not
  156. // predetermined - the idea is to select points that give good box-box
  157. // collision detection behavior. the chosen point indexes are returned in the
  158. // array iret (of size m). 'i0' is always the first entry in the array.
  159. // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
  160. // in the range [0..n-1].
  161. void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
  162. void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
  163. {
  164. // compute the centroid of the polygon in cx,cy
  165. int i,j;
  166. btScalar a,cx,cy,q;
  167. if (n==1) {
  168. cx = p[0];
  169. cy = p[1];
  170. }
  171. else if (n==2) {
  172. cx = btScalar(0.5)*(p[0] + p[2]);
  173. cy = btScalar(0.5)*(p[1] + p[3]);
  174. }
  175. else {
  176. a = 0;
  177. cx = 0;
  178. cy = 0;
  179. for (i=0; i<(n-1); i++) {
  180. q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
  181. a += q;
  182. cx += q*(p[i*2]+p[i*2+2]);
  183. cy += q*(p[i*2+1]+p[i*2+3]);
  184. }
  185. q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
  186. if (btFabs(a+q) > SIMD_EPSILON)
  187. {
  188. a = 1.f/(btScalar(3.0)*(a+q));
  189. } else
  190. {
  191. a=BT_LARGE_FLOAT;
  192. }
  193. cx = a*(cx + q*(p[n*2-2]+p[0]));
  194. cy = a*(cy + q*(p[n*2-1]+p[1]));
  195. }
  196. // compute the angle of each point w.r.t. the centroid
  197. btScalar A[8];
  198. for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
  199. // search for points that have angles closest to A[i0] + i*(2*pi/m).
  200. int avail[8];
  201. for (i=0; i<n; i++) avail[i] = 1;
  202. avail[i0] = 0;
  203. iret[0] = i0;
  204. iret++;
  205. for (j=1; j<m; j++) {
  206. a = btScalar(j)*(2*M__PI/m) + A[i0];
  207. if (a > M__PI) a -= 2*M__PI;
  208. btScalar maxdiff=1e9,diff;
  209. *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
  210. for (i=0; i<n; i++) {
  211. if (avail[i]) {
  212. diff = btFabs (A[i]-a);
  213. if (diff > M__PI) diff = 2*M__PI - diff;
  214. if (diff < maxdiff) {
  215. maxdiff = diff;
  216. *iret = i;
  217. }
  218. }
  219. }
  220. #if defined(DEBUG) || defined (_DEBUG)
  221. btAssert (*iret != i0); // ensure iret got set
  222. #endif
  223. avail[*iret] = 0;
  224. iret++;
  225. }
  226. }
  227. int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
  228. const btVector3& side1, const btVector3& p2,
  229. const dMatrix3 R2, const btVector3& side2,
  230. btVector3& normal, btScalar *depth, int *return_code,
  231. int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output);
  232. int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
  233. const btVector3& side1, const btVector3& p2,
  234. const dMatrix3 R2, const btVector3& side2,
  235. btVector3& normal, btScalar *depth, int *return_code,
  236. int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output)
  237. {
  238. const btScalar fudge_factor = btScalar(1.05);
  239. btVector3 p,pp,normalC(0.f,0.f,0.f);
  240. const btScalar *normalR = 0;
  241. btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
  242. Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
  243. int i,j,invert_normal,code;
  244. // get vector from centers of box 1 to box 2, relative to box 1
  245. p = p2 - p1;
  246. dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
  247. // get side lengths / 2
  248. A[0] = side1[0]*btScalar(0.5);
  249. A[1] = side1[1]*btScalar(0.5);
  250. A[2] = side1[2]*btScalar(0.5);
  251. B[0] = side2[0]*btScalar(0.5);
  252. B[1] = side2[1]*btScalar(0.5);
  253. B[2] = side2[2]*btScalar(0.5);
  254. // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  255. R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
  256. R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
  257. R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
  258. Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
  259. Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
  260. Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
  261. // for all 15 possible separating axes:
  262. // * see if the axis separates the boxes. if so, return 0.
  263. // * find the depth of the penetration along the separating axis (s2)
  264. // * if this is the largest depth so far, record it.
  265. // the normal vector will be set to the separating axis with the smallest
  266. // depth. note: normalR is set to point to a column of R1 or R2 if that is
  267. // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  268. // set to a vector relative to body 1. invert_normal is 1 if the sign of
  269. // the normal should be flipped.
  270. #define TST(expr1,expr2,norm,cc) \
  271. s2 = btFabs(expr1) - (expr2); \
  272. if (s2 > 0) return 0; \
  273. if (s2 > s) { \
  274. s = s2; \
  275. normalR = norm; \
  276. invert_normal = ((expr1) < 0); \
  277. code = (cc); \
  278. }
  279. s = -dInfinity;
  280. invert_normal = 0;
  281. code = 0;
  282. // separating axis = u1,u2,u3
  283. TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
  284. TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
  285. TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
  286. // separating axis = v1,v2,v3
  287. TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
  288. TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
  289. TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
  290. // note: cross product axes need to be scaled when s is computed.
  291. // normal (n1,n2,n3) is relative to box 1.
  292. #undef TST
  293. #define TST(expr1,expr2,n1,n2,n3,cc) \
  294. s2 = btFabs(expr1) - (expr2); \
  295. if (s2 > SIMD_EPSILON) return 0; \
  296. l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
  297. if (l > SIMD_EPSILON) { \
  298. s2 /= l; \
  299. if (s2*fudge_factor > s) { \
  300. s = s2; \
  301. normalR = 0; \
  302. normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
  303. invert_normal = ((expr1) < 0); \
  304. code = (cc); \
  305. } \
  306. }
  307. btScalar fudge2 (1.0e-5f);
  308. Q11 += fudge2;
  309. Q12 += fudge2;
  310. Q13 += fudge2;
  311. Q21 += fudge2;
  312. Q22 += fudge2;
  313. Q23 += fudge2;
  314. Q31 += fudge2;
  315. Q32 += fudge2;
  316. Q33 += fudge2;
  317. // separating axis = u1 x (v1,v2,v3)
  318. TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
  319. TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
  320. TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
  321. // separating axis = u2 x (v1,v2,v3)
  322. TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
  323. TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
  324. TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
  325. // separating axis = u3 x (v1,v2,v3)
  326. TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
  327. TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
  328. TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
  329. #undef TST
  330. if (!code) return 0;
  331. // if we get to this point, the boxes interpenetrate. compute the normal
  332. // in global coordinates.
  333. if (normalR) {
  334. normal[0] = normalR[0];
  335. normal[1] = normalR[4];
  336. normal[2] = normalR[8];
  337. }
  338. else {
  339. dMULTIPLY0_331 (normal,R1,normalC);
  340. }
  341. if (invert_normal) {
  342. normal[0] = -normal[0];
  343. normal[1] = -normal[1];
  344. normal[2] = -normal[2];
  345. }
  346. *depth = -s;
  347. // compute contact point(s)
  348. if (code > 6) {
  349. // an edge from box 1 touches an edge from box 2.
  350. // find a point pa on the intersecting edge of box 1
  351. btVector3 pa;
  352. btScalar sign;
  353. for (i=0; i<3; i++) pa[i] = p1[i];
  354. for (j=0; j<3; j++) {
  355. sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
  356. for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
  357. }
  358. // find a point pb on the intersecting edge of box 2
  359. btVector3 pb;
  360. for (i=0; i<3; i++) pb[i] = p2[i];
  361. for (j=0; j<3; j++) {
  362. sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
  363. for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
  364. }
  365. btScalar alpha,beta;
  366. btVector3 ua,ub;
  367. for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
  368. for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
  369. dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
  370. for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
  371. for (i=0; i<3; i++) pb[i] += ub[i]*beta;
  372. {
  373. //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
  374. //contact[0].depth = *depth;
  375. btVector3 pointInWorld;
  376. #ifdef USE_CENTER_POINT
  377. for (i=0; i<3; i++)
  378. pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
  379. output.addContactPoint(-normal,pointInWorld,-*depth);
  380. #else
  381. output.addContactPoint(-normal,pb,-*depth);
  382. #endif //
  383. *return_code = code;
  384. }
  385. return 1;
  386. }
  387. // okay, we have a face-something intersection (because the separating
  388. // axis is perpendicular to a face). define face 'a' to be the reference
  389. // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
  390. // the incident face (the closest face of the other box).
  391. const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
  392. if (code <= 3) {
  393. Ra = R1;
  394. Rb = R2;
  395. pa = p1;
  396. pb = p2;
  397. Sa = A;
  398. Sb = B;
  399. }
  400. else {
  401. Ra = R2;
  402. Rb = R1;
  403. pa = p2;
  404. pb = p1;
  405. Sa = B;
  406. Sb = A;
  407. }
  408. // nr = normal vector of reference face dotted with axes of incident box.
  409. // anr = absolute values of nr.
  410. btVector3 normal2,nr,anr;
  411. if (code <= 3) {
  412. normal2[0] = normal[0];
  413. normal2[1] = normal[1];
  414. normal2[2] = normal[2];
  415. }
  416. else {
  417. normal2[0] = -normal[0];
  418. normal2[1] = -normal[1];
  419. normal2[2] = -normal[2];
  420. }
  421. dMULTIPLY1_331 (nr,Rb,normal2);
  422. anr[0] = btFabs (nr[0]);
  423. anr[1] = btFabs (nr[1]);
  424. anr[2] = btFabs (nr[2]);
  425. // find the largest compontent of anr: this corresponds to the normal
  426. // for the indident face. the other axis numbers of the indicent face
  427. // are stored in a1,a2.
  428. int lanr,a1,a2;
  429. if (anr[1] > anr[0]) {
  430. if (anr[1] > anr[2]) {
  431. a1 = 0;
  432. lanr = 1;
  433. a2 = 2;
  434. }
  435. else {
  436. a1 = 0;
  437. a2 = 1;
  438. lanr = 2;
  439. }
  440. }
  441. else {
  442. if (anr[0] > anr[2]) {
  443. lanr = 0;
  444. a1 = 1;
  445. a2 = 2;
  446. }
  447. else {
  448. a1 = 0;
  449. a2 = 1;
  450. lanr = 2;
  451. }
  452. }
  453. // compute center point of incident face, in reference-face coordinates
  454. btVector3 center;
  455. if (nr[lanr] < 0) {
  456. for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
  457. }
  458. else {
  459. for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
  460. }
  461. // find the normal and non-normal axis numbers of the reference box
  462. int codeN,code1,code2;
  463. if (code <= 3) codeN = code-1; else codeN = code-4;
  464. if (codeN==0) {
  465. code1 = 1;
  466. code2 = 2;
  467. }
  468. else if (codeN==1) {
  469. code1 = 0;
  470. code2 = 2;
  471. }
  472. else {
  473. code1 = 0;
  474. code2 = 1;
  475. }
  476. // find the four corners of the incident face, in reference-face coordinates
  477. btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
  478. btScalar c1,c2,m11,m12,m21,m22;
  479. c1 = dDOT14 (center,Ra+code1);
  480. c2 = dDOT14 (center,Ra+code2);
  481. // optimize this? - we have already computed this data above, but it is not
  482. // stored in an easy-to-index format. for now it's quicker just to recompute
  483. // the four dot products.
  484. m11 = dDOT44 (Ra+code1,Rb+a1);
  485. m12 = dDOT44 (Ra+code1,Rb+a2);
  486. m21 = dDOT44 (Ra+code2,Rb+a1);
  487. m22 = dDOT44 (Ra+code2,Rb+a2);
  488. {
  489. btScalar k1 = m11*Sb[a1];
  490. btScalar k2 = m21*Sb[a1];
  491. btScalar k3 = m12*Sb[a2];
  492. btScalar k4 = m22*Sb[a2];
  493. quad[0] = c1 - k1 - k3;
  494. quad[1] = c2 - k2 - k4;
  495. quad[2] = c1 - k1 + k3;
  496. quad[3] = c2 - k2 + k4;
  497. quad[4] = c1 + k1 + k3;
  498. quad[5] = c2 + k2 + k4;
  499. quad[6] = c1 + k1 - k3;
  500. quad[7] = c2 + k2 - k4;
  501. }
  502. // find the size of the reference face
  503. btScalar rect[2];
  504. rect[0] = Sa[code1];
  505. rect[1] = Sa[code2];
  506. // intersect the incident and reference faces
  507. btScalar ret[16];
  508. int n = intersectRectQuad2 (rect,quad,ret);
  509. if (n < 1) return 0; // this should never happen
  510. // convert the intersection points into reference-face coordinates,
  511. // and compute the contact position and depth for each point. only keep
  512. // those points that have a positive (penetrating) depth. delete points in
  513. // the 'ret' array as necessary so that 'point' and 'ret' correspond.
  514. btScalar point[3*8]; // penetrating contact points
  515. btScalar dep[8]; // depths for those points
  516. btScalar det1 = 1.f/(m11*m22 - m12*m21);
  517. m11 *= det1;
  518. m12 *= det1;
  519. m21 *= det1;
  520. m22 *= det1;
  521. int cnum = 0; // number of penetrating contact points found
  522. for (j=0; j < n; j++) {
  523. btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
  524. btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
  525. for (i=0; i<3; i++) point[cnum*3+i] =
  526. center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
  527. dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
  528. if (dep[cnum] >= 0) {
  529. ret[cnum*2] = ret[j*2];
  530. ret[cnum*2+1] = ret[j*2+1];
  531. cnum++;
  532. }
  533. }
  534. if (cnum < 1) return 0; // this should never happen
  535. // we can't generate more contacts than we actually have
  536. if (maxc > cnum) maxc = cnum;
  537. if (maxc < 1) maxc = 1;
  538. if (cnum <= maxc) {
  539. if (code<4)
  540. {
  541. // we have less contacts than we need, so we use them all
  542. for (j=0; j < cnum; j++)
  543. {
  544. btVector3 pointInWorld;
  545. for (i=0; i<3; i++)
  546. pointInWorld[i] = point[j*3+i] + pa[i];
  547. output.addContactPoint(-normal,pointInWorld,-dep[j]);
  548. }
  549. } else
  550. {
  551. // we have less contacts than we need, so we use them all
  552. for (j=0; j < cnum; j++)
  553. {
  554. btVector3 pointInWorld;
  555. for (i=0; i<3; i++)
  556. pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
  557. //pointInWorld[i] = point[j*3+i] + pa[i];
  558. output.addContactPoint(-normal,pointInWorld,-dep[j]);
  559. }
  560. }
  561. }
  562. else {
  563. // we have more contacts than are wanted, some of them must be culled.
  564. // find the deepest point, it is always the first contact.
  565. int i1 = 0;
  566. btScalar maxdepth = dep[0];
  567. for (i=1; i<cnum; i++) {
  568. if (dep[i] > maxdepth) {
  569. maxdepth = dep[i];
  570. i1 = i;
  571. }
  572. }
  573. int iret[8];
  574. cullPoints2 (cnum,ret,maxc,i1,iret);
  575. for (j=0; j < maxc; j++) {
  576. // dContactGeom *con = CONTACT(contact,skip*j);
  577. // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
  578. // con->depth = dep[iret[j]];
  579. btVector3 posInWorld;
  580. for (i=0; i<3; i++)
  581. posInWorld[i] = point[iret[j]*3+i] + pa[i];
  582. if (code<4)
  583. {
  584. output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
  585. } else
  586. {
  587. output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
  588. }
  589. }
  590. cnum = maxc;
  591. }
  592. *return_code = code;
  593. return cnum;
  594. }
  595. void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
  596. {
  597. const btTransform& transformA = input.m_transformA;
  598. const btTransform& transformB = input.m_transformB;
  599. int skip = 0;
  600. dContactGeom *contact = 0;
  601. dMatrix3 R1;
  602. dMatrix3 R2;
  603. for (int j=0;j<3;j++)
  604. {
  605. R1[0+4*j] = transformA.getBasis()[j].x();
  606. R2[0+4*j] = transformB.getBasis()[j].x();
  607. R1[1+4*j] = transformA.getBasis()[j].y();
  608. R2[1+4*j] = transformB.getBasis()[j].y();
  609. R1[2+4*j] = transformA.getBasis()[j].z();
  610. R2[2+4*j] = transformB.getBasis()[j].z();
  611. }
  612. btVector3 normal;
  613. btScalar depth;
  614. int return_code;
  615. int maxc = 4;
  616. dBoxBox2 (transformA.getOrigin(),
  617. R1,
  618. 2.f*m_box1->getHalfExtentsWithMargin(),
  619. transformB.getOrigin(),
  620. R2,
  621. 2.f*m_box2->getHalfExtentsWithMargin(),
  622. normal, &depth, &return_code,
  623. maxc, contact, skip,
  624. output
  625. );
  626. }