lolengine
/
lol
espelhamento de https://github.com/lolengine/lol
1
0
Fork 0
Código Issues 0 Versões 0 Wiki Atividade
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.
2694 Commits
5 Branches
212 MiB
 
 
 
Tag: 1dcb065121
Branches Tags
${ item.name }
Criar branch ${ searchTerm }
de 1dcb065121
${ noResults }
lol/src/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h

921 linhas
26 KiB
Original Anotar Histórico 

  1. 

  2. /***

  3.  * ---------------------------------

  4.  * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>

  5.  *

  6.  *  This file was ported from mpr.c file, part of libccd.

  7.  *  The Minkoski Portal Refinement implementation was ported 

  8.  *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.

  9.  *  at http://github.com/erwincoumans/bullet3

  10.  *

  11.  *  Distributed under the OSI-approved BSD License (the "License");

  12.  *  see <http://www.opensource.org/licenses/bsd-license.php>.

  13.  *  This software is distributed WITHOUT ANY WARRANTY; without even the

  14.  *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

  15.  *  See the License for more information.

  16.  */

  17. 

  18. 

  19. 

  20. 

  21. #ifndef B3_MPR_PENETRATION_H

  22. #define B3_MPR_PENETRATION_H

  23. 

  24. #include "Bullet3Common/shared/b3PlatformDefinitions.h"

  25. #include "Bullet3Common/shared/b3Float4.h"

  26. #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"

  27. #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"

  28. #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"

  29. 

  30. 

  31. 

  32. 

  33. #ifdef __cplusplus

  34. #define B3_MPR_SQRT sqrtf

  35. #else

  36. #define B3_MPR_SQRT sqrt

  37. #endif

  38. #define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))

  39. #define B3_MPR_FABS fabs

  40. 

  41. #define B3_MPR_TOLERANCE 1E-6f

  42. #define B3_MPR_MAX_ITERATIONS 1000

  43. 

  44. struct _b3MprSupport_t 

  45. {

  46.     b3Float4 v;  //!< Support point in minkowski sum

  47.     b3Float4 v1; //!< Support point in obj1

  48.     b3Float4 v2; //!< Support point in obj2

  49. };

  50. typedef struct _b3MprSupport_t b3MprSupport_t;

  51. 

  52. struct _b3MprSimplex_t 

  53. {

  54.     b3MprSupport_t ps[4];

  55.     int last; //!< index of last added point

  56. };

  57. typedef struct _b3MprSimplex_t b3MprSimplex_t;

  58. 

  59. inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)

  60. {

  61.     return &s->ps[idx];

  62. }

  63. 

  64. inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)

  65. {

  66.     s->last = size - 1;

  67. }

  68. 

  69. 

  70. inline int b3MprSimplexSize(const b3MprSimplex_t *s)

  71. {

  72.     return s->last + 1;

  73. }

  74. 

  75. 

  76. inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)

  77. {

  78.     // here is no check on boundaries

  79.     return &s->ps[idx];

  80. }

  81. 

  82. inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)

  83. {

  84.     *d = *s;

  85. }

  86. 

  87. inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)

  88. {

  89.     b3MprSupportCopy(s->ps + pos, a);

  90. }

  91. 

  92. 

  93. inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)

  94. {

  95.     b3MprSupport_t supp;

  96. 

  97.     b3MprSupportCopy(&supp, &s->ps[pos1]);

  98.     b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);

  99.     b3MprSupportCopy(&s->ps[pos2], &supp);

  100. }

  101. 

  102. 

  103. inline int b3MprIsZero(float val)

  104. {

  105.     return B3_MPR_FABS(val) < FLT_EPSILON;

  106. }

  107. 

  108. 

  109. 

  110. inline int b3MprEq(float _a, float _b)

  111. {

  112.     float ab;

  113.     float a, b;

  114. 

  115.     ab = B3_MPR_FABS(_a - _b);

  116.     if (B3_MPR_FABS(ab) < FLT_EPSILON)

  117.         return 1;

  118. 

  119.     a = B3_MPR_FABS(_a);

  120.     b = B3_MPR_FABS(_b);

  121.     if (b > a){

  122.         return ab < FLT_EPSILON * b;

  123.     }else{

  124.         return ab < FLT_EPSILON * a;

  125.     }

  126. }

  127. 

  128. 

  129. inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)

  130. {

  131.     return b3MprEq((*a).x, (*b).x)

  132.             && b3MprEq((*a).y, (*b).y)

  133.             && b3MprEq((*a).z, (*b).z);

  134. }

  135. 

  136. 

  137. 

  138. inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, 	b3ConstArray(b3Float4) verticesA)

  139. {

  140. 	b3Float4 supVec = b3MakeFloat4(0,0,0,0);

  141. 	float maxDot = -B3_LARGE_FLOAT;

  142. 

  143.     if( 0 < hull->m_numVertices )

  144.     {

  145.         const b3Float4 scaled = supportVec;

  146. 		int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);

  147.         return verticesA[hull->m_vertexOffset+index];

  148.     }

  149. 

  150.     return supVec;

  151. 

  152. }

  153. 

  154. 

  155. B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 

  156. 													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 

  157. 													b3ConstArray(b3Collidable_t)				cpuCollidables,

  158. 													b3ConstArray(b3Float4)					cpuVertices,

  159. 													__global b3Float4* sepAxis,

  160. 														const b3Float4* _dir, b3Float4* outp, int logme)

  161. {

  162. 	//dir is in worldspace, move to local space

  163. 	

  164. 	b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;

  165. 	b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;

  166. 	

  167. 	b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);

  168. 	

  169. 	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);

  170. 	

  171. 

  172. 	//find local support vertex

  173. 	int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;

  174. 	

  175. 	b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);

  176. 	__global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];

  177. 	

  178. 	b3Float4 pInA;

  179. 	if (logme)

  180. 	{

  181. 

  182. 

  183. 		b3Float4 supVec = b3MakeFloat4(0,0,0,0);

  184. 		float maxDot = -B3_LARGE_FLOAT;

  185. 

  186. 		if( 0 < hull->m_numVertices )

  187. 		{

  188. 			const b3Float4 scaled = localDir;

  189. 			int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);

  190. 			pInA = cpuVertices[hull->m_vertexOffset+index];

  191. 			

  192. 		}

  193. 

  194. 

  195. 	} else

  196. 	{

  197. 		pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);

  198. 	}

  199. 

  200. 	//move vertex to world space

  201. 	*outp = b3TransformPoint(pInA,pos,orn);

  202. 	

  203. }

  204. 

  205. inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB,   b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 

  206. 													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 

  207. 													b3ConstArray(b3Collidable_t)				cpuCollidables,

  208. 													b3ConstArray(b3Float4)					cpuVertices,

  209. 													__global b3Float4* sepAxis,

  210. 													const b3Float4* _dir, b3MprSupport_t *supp)

  211. {

  212.     b3Float4 dir;

  213. 	dir = *_dir;

  214. 	b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);

  215.     dir = *_dir*-1.f;

  216. 	b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);

  217.     supp->v = supp->v1 - supp->v2;

  218. }

  219. 

  220. 

  221. 

  222. 

  223. 

  224. 

  225. 

  226. 

  227. 

  228. inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)

  229. {

  230. 

  231.     center->v1 = cpuBodyBuf[bodyIndexA].m_pos;

  232. 	center->v2 = cpuBodyBuf[bodyIndexB].m_pos;

  233.     center->v = center->v1 - center->v2;

  234. }

  235. 

  236. inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)

  237. {

  238. 	(*v).x = x;

  239. 	(*v).y = y;

  240. 	(*v).z = z;

  241. 	(*v).w = 0.f;

  242. }

  243. 

  244. inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)

  245. {

  246.     (*v).x += (*w).x;

  247.     (*v).y += (*w).y;

  248.     (*v).z += (*w).z;

  249. }

  250. 

  251. inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)

  252. {

  253.     *v = *w;

  254. }

  255. 

  256. inline void b3MprVec3Scale(b3Float4 *d, float k)

  257. {

  258.     *d *= k;

  259. }

  260. 

  261. inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)

  262. {

  263.     float dot;

  264. 

  265. 	dot = b3Dot3F4(*a,*b);

  266.     return dot;

  267. }

  268. 

  269. 

  270. inline float b3MprVec3Len2(const b3Float4 *v)

  271. {

  272.     return b3MprVec3Dot(v, v);

  273. }

  274. 

  275. inline void b3MprVec3Normalize(b3Float4 *d)

  276. {

  277.     float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));

  278.     b3MprVec3Scale(d, k);

  279. }

  280. 

  281. inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)

  282. {

  283. 	*d = b3Cross3(*a,*b);

  284. 	

  285. }

  286. 

  287. 

  288. inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)

  289. {

  290. 	*d = *v - *w;

  291. }

  292. 

  293. inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)

  294. {

  295.     b3Float4 v2v1, v3v1;

  296. 

  297.     b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,

  298.                        &b3MprSimplexPoint(portal, 1)->v);

  299.     b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,

  300.                        &b3MprSimplexPoint(portal, 1)->v);

  301.     b3MprVec3Cross(dir, &v2v1, &v3v1);

  302.     b3MprVec3Normalize(dir);

  303. }

  304. 

  305. 

  306. inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,

  307.                                        const b3Float4 *dir)

  308. {

  309.     float dot;

  310.     dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);

  311.     return b3MprIsZero(dot) || dot > 0.f;

  312. }

  313. 

  314. inline int portalReachTolerance(const b3MprSimplex_t *portal,

  315.                                      const b3MprSupport_t *v4,

  316.                                      const b3Float4 *dir)

  317. {

  318.     float dv1, dv2, dv3, dv4;

  319.     float dot1, dot2, dot3;

  320. 

  321.     // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}

  322. 

  323.     dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);

  324.     dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);

  325.     dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);

  326.     dv4 = b3MprVec3Dot(&v4->v, dir);

  327. 

  328.     dot1 = dv4 - dv1;

  329.     dot2 = dv4 - dv2;

  330.     dot3 = dv4 - dv3;

  331. 

  332.     dot1 = B3_MPR_FMIN(dot1, dot2);

  333.     dot1 = B3_MPR_FMIN(dot1, dot3);

  334. 

  335.     return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;

  336. }

  337. 

  338. inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,   

  339.                                          const b3MprSupport_t *v4,

  340.                                          const b3Float4 *dir)

  341. {

  342.     float dot;

  343.     dot = b3MprVec3Dot(&v4->v, dir);

  344.     return b3MprIsZero(dot) || dot > 0.f;

  345. }

  346. 

  347. inline void b3ExpandPortal(b3MprSimplex_t *portal,

  348.                               const b3MprSupport_t *v4)

  349. {

  350.     float dot;

  351.     b3Float4 v4v0;

  352. 

  353.     b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);

  354.     dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);

  355.     if (dot > 0.f){

  356.         dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);

  357.         if (dot > 0.f){

  358.             b3MprSimplexSet(portal, 1, v4);

  359.         }else{

  360.             b3MprSimplexSet(portal, 3, v4);

  361.         }

  362.     }else{

  363.         dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);

  364.         if (dot > 0.f){

  365.             b3MprSimplexSet(portal, 2, v4);

  366.         }else{

  367.             b3MprSimplexSet(portal, 1, v4);

  368.         }

  369.     }

  370. }

  371. 

  372. 

  373. 

  374. B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 

  375. 													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 

  376. 													b3ConstArray(b3Collidable_t)				cpuCollidables,

  377. 													b3ConstArray(b3Float4)					cpuVertices,

  378. 													__global b3Float4* sepAxis,

  379. 													__global int*	hasSepAxis,

  380. 													b3MprSimplex_t *portal)

  381. {

  382.     b3Float4 dir, va, vb;

  383.     float dot;

  384.     int cont;

  385. 	

  386. 	

  387. 

  388.     // vertex 0 is center of portal

  389.     b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));

  390.     // vertex 0 is center of portal

  391.     b3MprSimplexSetSize(portal, 1);

  392. 	

  393. 

  394. 

  395. 	b3Float4 zero = b3MakeFloat4(0,0,0,0);

  396. 	b3Float4* b3mpr_vec3_origin = &zero;

  397. 

  398.     if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){

  399.         // Portal's center lies on origin (0,0,0) => we know that objects

  400.         // intersect but we would need to know penetration info.

  401.         // So move center little bit...

  402.         b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);

  403.         b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);

  404.     }

  405. 

  406. 

  407.     // vertex 1 = support in direction of origin

  408.     b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);

  409.     b3MprVec3Scale(&dir, -1.f);

  410.     b3MprVec3Normalize(&dir);

  411. 

  412. 

  413.     b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));

  414. 

  415.     b3MprSimplexSetSize(portal, 2);

  416. 

  417.     // test if origin isn't outside of v1

  418.     dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);

  419. 	

  420. 

  421.     if (b3MprIsZero(dot) || dot < 0.f)

  422.         return -1;

  423. 

  424. 

  425.     // vertex 2

  426.     b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,

  427.                        &b3MprSimplexPoint(portal, 1)->v);

  428.     if (b3MprIsZero(b3MprVec3Len2(&dir))){

  429.         if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){

  430.             // origin lies on v1

  431.             return 1;

  432.         }else{

  433.             // origin lies on v0-v1 segment

  434.             return 2;

  435.         }

  436.     }

  437. 

  438.     b3MprVec3Normalize(&dir);

  439. 	 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));

  440.     

  441.     dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);

  442.     if (b3MprIsZero(dot) || dot < 0.f)

  443.         return -1;

  444. 

  445.     b3MprSimplexSetSize(portal, 3);

  446. 

  447.     // vertex 3 direction

  448.     b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,

  449.                      &b3MprSimplexPoint(portal, 0)->v);

  450.     b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,

  451.                      &b3MprSimplexPoint(portal, 0)->v);

  452.     b3MprVec3Cross(&dir, &va, &vb);

  453.     b3MprVec3Normalize(&dir);

  454. 

  455.     // it is better to form portal faces to be oriented "outside" origin

  456.     dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);

  457.     if (dot > 0.f){

  458.         b3MprSimplexSwap(portal, 1, 2);

  459.         b3MprVec3Scale(&dir, -1.f);

  460.     }

  461. 

  462.     while (b3MprSimplexSize(portal) < 4){

  463. 		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));

  464.         

  465.         dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);

  466.         if (b3MprIsZero(dot) || dot < 0.f)

  467.             return -1;

  468. 

  469.         cont = 0;

  470. 

  471.         // test if origin is outside (v1, v0, v3) - set v2 as v3 and

  472.         // continue

  473.         b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,

  474.                           &b3MprSimplexPoint(portal, 3)->v);

  475.         dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);

  476.         if (dot < 0.f && !b3MprIsZero(dot)){

  477.             b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));

  478.             cont = 1;

  479.         }

  480. 

  481.         if (!cont){

  482.             // test if origin is outside (v3, v0, v2) - set v1 as v3 and

  483.             // continue

  484.             b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,

  485.                               &b3MprSimplexPoint(portal, 2)->v);

  486.             dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);

  487.             if (dot < 0.f && !b3MprIsZero(dot)){

  488.                 b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));

  489.                 cont = 1;

  490.             }

  491.         }

  492. 

  493.         if (cont){

  494.             b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,

  495.                              &b3MprSimplexPoint(portal, 0)->v);

  496.             b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,

  497.                              &b3MprSimplexPoint(portal, 0)->v);

  498.             b3MprVec3Cross(&dir, &va, &vb);

  499.             b3MprVec3Normalize(&dir);

  500.         }else{

  501.             b3MprSimplexSetSize(portal, 4);

  502.         }

  503.     }

  504. 

  505.     return 0;

  506. }

  507. 

  508. 

  509. B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 

  510. 													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 

  511. 													b3ConstArray(b3Collidable_t)				cpuCollidables,

  512. 													b3ConstArray(b3Float4)					cpuVertices,

  513. 													__global b3Float4* sepAxis,

  514. 													b3MprSimplex_t *portal)

  515. {

  516.     b3Float4 dir;

  517.     b3MprSupport_t v4;

  518. 

  519. 	for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)

  520.     //while (1)

  521. 	{

  522.         // compute direction outside the portal (from v0 throught v1,v2,v3

  523.         // face)

  524.         b3PortalDir(portal, &dir);

  525. 

  526.         // test if origin is inside the portal

  527.         if (portalEncapsulesOrigin(portal, &dir))

  528.             return 0;

  529. 

  530.         // get next support point

  531.         

  532. 		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);

  533. 

  534. 

  535.         // test if v4 can expand portal to contain origin and if portal

  536.         // expanding doesn't reach given tolerance

  537.         if (!portalCanEncapsuleOrigin(portal, &v4, &dir)

  538.                 || portalReachTolerance(portal, &v4, &dir))

  539. 		{

  540.             return -1;

  541.         }

  542. 

  543.         // v1-v2-v3 triangle must be rearranged to face outside Minkowski

  544.         // difference (direction from v0).

  545.         b3ExpandPortal(portal, &v4);

  546.     }

  547. 

  548.     return -1;

  549. }

  550. 

  551. B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)

  552. {

  553. 

  554. 	b3Float4 zero = b3MakeFloat4(0,0,0,0);

  555. 	b3Float4* b3mpr_vec3_origin = &zero;

  556. 

  557.     b3Float4 dir;

  558.     size_t i;

  559.     float b[4], sum, inv;

  560.     b3Float4 vec, p1, p2;

  561. 

  562.     b3PortalDir(portal, &dir);

  563. 

  564.     // use barycentric coordinates of tetrahedron to find origin

  565.     b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,

  566.                        &b3MprSimplexPoint(portal, 2)->v);

  567.     b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);

  568. 

  569.     b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,

  570.                        &b3MprSimplexPoint(portal, 2)->v);

  571.     b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);

  572. 

  573.     b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,

  574.                        &b3MprSimplexPoint(portal, 1)->v);

  575.     b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);

  576. 

  577.     b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,

  578.                        &b3MprSimplexPoint(portal, 1)->v);

  579.     b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);

  580. 

  581. 	sum = b[0] + b[1] + b[2] + b[3];

  582. 

  583.     if (b3MprIsZero(sum) || sum < 0.f){

  584. 		b[0] = 0.f;

  585. 

  586.         b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,

  587.                            &b3MprSimplexPoint(portal, 3)->v);

  588.         b[1] = b3MprVec3Dot(&vec, &dir);

  589.         b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,

  590.                            &b3MprSimplexPoint(portal, 1)->v);

  591.         b[2] = b3MprVec3Dot(&vec, &dir);

  592.         b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,

  593.                            &b3MprSimplexPoint(portal, 2)->v);

  594.         b[3] = b3MprVec3Dot(&vec, &dir);

  595. 

  596. 		sum = b[1] + b[2] + b[3];

  597. 	}

  598. 

  599. 	inv = 1.f / sum;

  600. 

  601.     b3MprVec3Copy(&p1, b3mpr_vec3_origin);

  602.     b3MprVec3Copy(&p2, b3mpr_vec3_origin);

  603.     for (i = 0; i < 4; i++){

  604.         b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);

  605.         b3MprVec3Scale(&vec, b[i]);

  606.         b3MprVec3Add(&p1, &vec);

  607. 

  608.         b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);

  609.         b3MprVec3Scale(&vec, b[i]);

  610.         b3MprVec3Add(&p2, &vec);

  611.     }

  612.     b3MprVec3Scale(&p1, inv);

  613.     b3MprVec3Scale(&p2, inv);

  614. 

  615.     b3MprVec3Copy(pos, &p1);

  616.     b3MprVec3Add(pos, &p2);

  617.     b3MprVec3Scale(pos, 0.5);

  618. }

  619. 

  620. inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)

  621. {

  622.     b3Float4 ab;

  623.     b3MprVec3Sub2(&ab, a, b);

  624.     return b3MprVec3Len2(&ab);

  625. }

  626. 

  627. inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,

  628.                                                   const b3Float4 *x0,

  629.                                                   const b3Float4 *b,

  630.                                                   b3Float4 *witness)

  631. {

  632.     // The computation comes from solving equation of segment:

  633.     //      S(t) = x0 + t.d

  634.     //          where - x0 is initial point of segment

  635.     //                - d is direction of segment from x0 (|d| > 0)

  636.     //                - t belongs to <0, 1> interval

  637.     // 

  638.     // Than, distance from a segment to some point P can be expressed:

  639.     //      D(t) = |x0 + t.d - P|^2

  640.     //          which is distance from any point on segment. Minimization

  641.     //          of this function brings distance from P to segment.

  642.     // Minimization of D(t) leads to simple quadratic equation that's

  643.     // solving is straightforward.

  644.     //

  645.     // Bonus of this method is witness point for free.

  646. 

  647.     float dist, t;

  648.     b3Float4 d, a;

  649. 

  650.     // direction of segment

  651.     b3MprVec3Sub2(&d, b, x0);

  652. 

  653.     // precompute vector from P to x0

  654.     b3MprVec3Sub2(&a, x0, P);

  655. 

  656.     t  = -1.f * b3MprVec3Dot(&a, &d);

  657.     t /= b3MprVec3Len2(&d);

  658. 

  659.     if (t < 0.f || b3MprIsZero(t)){

  660.         dist = b3MprVec3Dist2(x0, P);

  661.         if (witness)

  662.             b3MprVec3Copy(witness, x0);

  663.     }else if (t > 1.f || b3MprEq(t, 1.f)){

  664.         dist = b3MprVec3Dist2(b, P);

  665.         if (witness)

  666.             b3MprVec3Copy(witness, b);

  667.     }else{

  668.         if (witness){

  669.             b3MprVec3Copy(witness, &d);

  670.             b3MprVec3Scale(witness, t);

  671.             b3MprVec3Add(witness, x0);

  672.             dist = b3MprVec3Dist2(witness, P);

  673.         }else{

  674.             // recycling variables

  675.             b3MprVec3Scale(&d, t);

  676.             b3MprVec3Add(&d, &a);

  677.             dist = b3MprVec3Len2(&d);

  678.         }

  679.     }

  680. 

  681.     return dist;

  682. }

  683. 

  684. 

  685. inline float b3MprVec3PointTriDist2(const b3Float4 *P,

  686.                                 const b3Float4 *x0, const b3Float4 *B,

  687.                                 const b3Float4 *C,

  688.                                 b3Float4 *witness)

  689. {

  690.     // Computation comes from analytic expression for triangle (x0, B, C)

  691.     //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and

  692.     // Then equation for distance is:

  693.     //      D(s, t) = | T(s, t) - P |^2

  694.     // This leads to minimization of quadratic function of two variables.

  695.     // The solution from is taken only if s is between 0 and 1, t is

  696.     // between 0 and 1 and t + s < 1, otherwise distance from segment is

  697.     // computed.

  698. 

  699.     b3Float4 d1, d2, a;

  700.     float u, v, w, p, q, r;

  701.     float s, t, dist, dist2;

  702.     b3Float4 witness2;

  703. 

  704.     b3MprVec3Sub2(&d1, B, x0);

  705.     b3MprVec3Sub2(&d2, C, x0);

  706.     b3MprVec3Sub2(&a, x0, P);

  707. 

  708.     u = b3MprVec3Dot(&a, &a);

  709.     v = b3MprVec3Dot(&d1, &d1);

  710.     w = b3MprVec3Dot(&d2, &d2);

  711.     p = b3MprVec3Dot(&a, &d1);

  712.     q = b3MprVec3Dot(&a, &d2);

  713.     r = b3MprVec3Dot(&d1, &d2);

  714. 

  715.     s = (q * r - w * p) / (w * v - r * r);

  716.     t = (-s * r - q) / w;

  717. 

  718.     if ((b3MprIsZero(s) || s > 0.f)

  719.             && (b3MprEq(s, 1.f) || s < 1.f)

  720.             && (b3MprIsZero(t) || t > 0.f)

  721.             && (b3MprEq(t, 1.f) || t < 1.f)

  722.             && (b3MprEq(t + s, 1.f) || t + s < 1.f)){

  723. 

  724.         if (witness){

  725.             b3MprVec3Scale(&d1, s);

  726.             b3MprVec3Scale(&d2, t);

  727.             b3MprVec3Copy(witness, x0);

  728.             b3MprVec3Add(witness, &d1);

  729.             b3MprVec3Add(witness, &d2);

  730. 

  731.             dist = b3MprVec3Dist2(witness, P);

  732.         }else{

  733.             dist  = s * s * v;

  734.             dist += t * t * w;

  735.             dist += 2.f * s * t * r;

  736.             dist += 2.f * s * p;

  737.             dist += 2.f * t * q;

  738.             dist += u;

  739.         }

  740.     }else{

  741.         dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);

  742. 

  743.         dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);

  744.         if (dist2 < dist){

  745.             dist = dist2;

  746.             if (witness)

  747.                 b3MprVec3Copy(witness, &witness2);

  748.         }

  749. 

  750.         dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);

  751.         if (dist2 < dist){

  752.             dist = dist2;

  753.             if (witness)

  754.                 b3MprVec3Copy(witness, &witness2);

  755.         }

  756.     }

  757. 

  758.     return dist;

  759. }

  760. 

  761. 

  762. B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 

  763. 													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 

  764. 													b3ConstArray(b3Collidable_t)				cpuCollidables,

  765. 													b3ConstArray(b3Float4)					cpuVertices,

  766. 													__global b3Float4* sepAxis,

  767.                        b3MprSimplex_t *portal,

  768.                        float *depth, b3Float4 *pdir, b3Float4 *pos)

  769. {

  770.     b3Float4 dir;

  771.     b3MprSupport_t v4;

  772.     unsigned long iterations;

  773. 

  774. 	b3Float4 zero = b3MakeFloat4(0,0,0,0);

  775. 	b3Float4* b3mpr_vec3_origin = &zero;

  776. 

  777. 

  778.     iterations = 1UL;

  779. 	for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)

  780.     //while (1)

  781. 	{

  782.         // compute portal direction and obtain next support point

  783.         b3PortalDir(portal, &dir);

  784.         

  785. 		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);

  786. 

  787. 

  788.         // reached tolerance -> find penetration info

  789.         if (portalReachTolerance(portal, &v4, &dir)

  790.                 || iterations ==B3_MPR_MAX_ITERATIONS)

  791. 		{

  792.             *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);

  793.             *depth = B3_MPR_SQRT(*depth);

  794. 			

  795. 			if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))

  796. 			{

  797. 				

  798. 				*pdir = dir;

  799. 			} 

  800. 			b3MprVec3Normalize(pdir);

  801. 			

  802.             // barycentric coordinates:

  803.             b3FindPos(portal, pos);

  804. 

  805. 

  806.             return;

  807.         }

  808. 

  809.         b3ExpandPortal(portal, &v4);

  810. 

  811.         iterations++;

  812.     }

  813. }

  814. 

  815. B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)

  816. {

  817.     // Touching contact on portal's v1 - so depth is zero and direction

  818.     // is unimportant and pos can be guessed

  819.     *depth = 0.f;

  820.     b3Float4 zero = b3MakeFloat4(0,0,0,0);

  821. 	b3Float4* b3mpr_vec3_origin = &zero;

  822. 

  823. 

  824. 	b3MprVec3Copy(dir, b3mpr_vec3_origin);

  825. 

  826.     b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);

  827.     b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);

  828.     b3MprVec3Scale(pos, 0.5);

  829. }

  830. 

  831. B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,

  832.                               float *depth, b3Float4 *dir, b3Float4 *pos)

  833. {

  834.     

  835.     // Origin lies on v0-v1 segment.

  836.     // Depth is distance to v1, direction also and position must be

  837.     // computed

  838. 

  839.     b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);

  840.     b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);

  841.     b3MprVec3Scale(pos, 0.5f);

  842. 

  843.     

  844.     b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);

  845.     *depth = B3_MPR_SQRT(b3MprVec3Len2(dir));

  846.     b3MprVec3Normalize(dir);

  847. }

  848. 

  849. 

  850. 

  851. inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,

  852. 					b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,

  853. 					b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 

  854. 					b3ConstArray(b3Collidable_t)	cpuCollidables,

  855. 					b3ConstArray(b3Float4)	cpuVertices,

  856. 					__global b3Float4* sepAxis,

  857. 					__global int*	hasSepAxis,

  858. 					float *depthOut, b3Float4* dirOut, b3Float4* posOut)

  859. {

  860. 	

  861. 	 b3MprSimplex_t portal;

  862. 

  863. 	 

  864. //	if (!hasSepAxis[pairIndex])

  865. 	//	return -1;

  866. 	

  867. 	hasSepAxis[pairIndex] = 0;

  868. 	 int res;

  869. 

  870.     // Phase 1: Portal discovery

  871.     res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);

  872. 	

  873. 	  

  874. 	//sepAxis[pairIndex] = *pdir;//or -dir?

  875. 

  876. 	switch (res)

  877. 	{

  878. 	case 0:

  879. 		{

  880. 			// Phase 2: Portal refinement

  881. 		

  882. 			res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);

  883. 			if (res < 0)

  884. 				return -1;

  885. 

  886. 			// Phase 3. Penetration info

  887. 			b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);

  888. 			hasSepAxis[pairIndex] = 1;

  889. 			sepAxis[pairIndex] = -*dirOut;

  890. 			break;

  891. 		}

  892. 	case 1:

  893. 		{

  894. 			 // Touching contact on portal's v1.

  895. 			b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);

  896. 			break;

  897. 		}

  898. 	case 2:

  899. 		{

  900. 			

  901. 			b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);

  902. 			break;

  903. 		}

  904. 	default:

  905. 		{

  906. 			hasSepAxis[pairIndex]=0;

  907. 			//if (res < 0)

  908. 			//{

  909. 				// Origin isn't inside portal - no collision.

  910. 				return -1;

  911. 			//}

  912. 		}

  913. 	};

  914. 	

  915. 	return 0;

  916. };

  917. 

  918. 

  919. 

  920. #endif //B3_MPR_PENETRATION_H