diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h | 2889 |
1 files changed, 1444 insertions, 1445 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h index 7ed4b382c3..74959a931c 100644 --- a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h +++ b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h @@ -1,1446 +1,1445 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* mprKernelsCL= \ -"/***\n" -" * ---------------------------------\n" -" * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>\n" -" *\n" -" * This file was ported from mpr.c file, part of libccd.\n" -" * The Minkoski Portal Refinement implementation was ported \n" -" * to OpenCL by Erwin Coumans for the Bullet 3 Physics library.\n" -" * at http://github.com/erwincoumans/bullet3\n" -" *\n" -" * Distributed under the OSI-approved BSD License (the \"License\");\n" -" * see <http://www.opensource.org/licenses/bsd-license.php>.\n" -" * This software is distributed WITHOUT ANY WARRANTY; without even the\n" -" * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\n" -" * See the License for more information.\n" -" */\n" -"#ifndef B3_MPR_PENETRATION_H\n" -"#define B3_MPR_PENETRATION_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_RIGIDBODY_DATA_H\n" -"#define B3_RIGIDBODY_DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_QUAT_H\n" -"#define B3_QUAT_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif\n" -"#endif\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Quat;\n" -" #define b3QuatConstArg const b3Quat\n" -" \n" -" \n" -"inline float4 b3FastNormalize4(float4 v)\n" -"{\n" -" v = (float4)(v.xyz,0.f);\n" -" return fast_normalize(v);\n" -"}\n" -" \n" -"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" -"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" -"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" -"{\n" -" b3Quat ans;\n" -" ans = b3Cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" -"{\n" -" b3Quat q;\n" -" q=in;\n" -" //return b3FastNormalize4(in);\n" -" float len = native_sqrt(dot(q, q));\n" -" if(len > 0.f)\n" -" {\n" -" q *= 1.f / len;\n" -" }\n" -" else\n" -" {\n" -" q.x = q.y = q.z = 0.f;\n" -" q.w = 1.f;\n" -" }\n" -" return q;\n" -"}\n" -"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" b3Quat qInv = b3QuatInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" return b3QuatRotate( b3QuatInvert( q ), vec );\n" -"}\n" -"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" -"{\n" -" return b3QuatRotate( orientation, point ) + (translation);\n" -"}\n" -" \n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifndef B3_MAT3x3_H\n" -"#define B3_MAT3x3_H\n" -"#ifndef B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"typedef struct\n" -"{\n" -" b3Float4 m_row[3];\n" -"}b3Mat3x3;\n" -"#define b3Mat3x3ConstArg const b3Mat3x3\n" -"#define b3GetRow(m,row) (m.m_row[row])\n" -"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" -"{\n" -" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" -" b3Mat3x3 out;\n" -" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" -" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" -" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" -" out.m_row[0].w = 0.f;\n" -" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" -" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" -" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" -" out.m_row[1].w = 0.f;\n" -" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" -" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" -" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" -" out.m_row[2].w = 0.f;\n" -" return out;\n" -"}\n" -"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = fabs(matIn.m_row[0]);\n" -" out.m_row[1] = fabs(matIn.m_row[1]);\n" -" out.m_row[2] = fabs(matIn.m_row[2]);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtZero();\n" -"__inline\n" -"b3Mat3x3 mtIdentity();\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Mat3x3 mtZero()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(0.f);\n" -" m.m_row[1] = (b3Float4)(0.f);\n" -" m.m_row[2] = (b3Float4)(0.f);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtIdentity()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(1,0,0,0);\n" -" m.m_row[1] = (b3Float4)(0,1,0,0);\n" -" m.m_row[2] = (b3Float4)(0,0,1,0);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" -" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" -" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" -"{\n" -" b3Mat3x3 transB;\n" -" transB = mtTranspose( b );\n" -" b3Mat3x3 ans;\n" -" // why this doesn't run when 0ing in the for{}\n" -" a.m_row[0].w = 0.f;\n" -" a.m_row[1].w = 0.f;\n" -" a.m_row[2].w = 0.f;\n" -" for(int i=0; i<3; i++)\n" -" {\n" -"// a.m_row[i].w = 0.f;\n" -" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" -" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" -" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" -" ans.m_row[i].w = 0.f;\n" -" }\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" -"{\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a.m_row[0], b );\n" -" ans.y = b3Dot3F4( a.m_row[1], b );\n" -" ans.z = b3Dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" -"{\n" -" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a, colx );\n" -" ans.y = b3Dot3F4( a, coly );\n" -" ans.z = b3Dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"#endif\n" -"#endif //B3_MAT3x3_H\n" -"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" -"struct b3RigidBodyData\n" -"{\n" -" b3Float4 m_pos;\n" -" b3Quat m_quat;\n" -" b3Float4 m_linVel;\n" -" b3Float4 m_angVel;\n" -" int m_collidableIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"};\n" -"typedef struct b3InertiaData b3InertiaData_t;\n" -"struct b3InertiaData\n" -"{\n" -" b3Mat3x3 m_invInertiaWorld;\n" -" b3Mat3x3 m_initInvInertia;\n" -"};\n" -"#endif //B3_RIGIDBODY_DATA_H\n" -" \n" -"#ifndef B3_CONVEX_POLYHEDRON_DATA_H\n" -"#define B3_CONVEX_POLYHEDRON_DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"typedef struct b3GpuFace b3GpuFace_t;\n" -"struct b3GpuFace\n" -"{\n" -" b3Float4 m_plane;\n" -" int m_indexOffset;\n" -" int m_numIndices;\n" -" int m_unusedPadding1;\n" -" int m_unusedPadding2;\n" -"};\n" -"typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;\n" -"struct b3ConvexPolyhedronData\n" -"{\n" -" b3Float4 m_localCenter;\n" -" b3Float4 m_extents;\n" -" b3Float4 mC;\n" -" b3Float4 mE;\n" -" float m_radius;\n" -" int m_faceOffset;\n" -" int m_numFaces;\n" -" int m_numVertices;\n" -" int m_vertexOffset;\n" -" int m_uniqueEdgesOffset;\n" -" int m_numUniqueEdges;\n" -" int m_unused;\n" -"};\n" -"#endif //B3_CONVEX_POLYHEDRON_DATA_H\n" -"#ifndef B3_COLLIDABLE_H\n" -"#define B3_COLLIDABLE_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"enum b3ShapeTypes\n" -"{\n" -" SHAPE_HEIGHT_FIELD=1,\n" -" SHAPE_CONVEX_HULL=3,\n" -" SHAPE_PLANE=4,\n" -" SHAPE_CONCAVE_TRIMESH=5,\n" -" SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" -" SHAPE_SPHERE=7,\n" -" MAX_NUM_SHAPE_TYPES,\n" -"};\n" -"typedef struct b3Collidable b3Collidable_t;\n" -"struct b3Collidable\n" -"{\n" -" union {\n" -" int m_numChildShapes;\n" -" int m_bvhIndex;\n" -" };\n" -" union\n" -" {\n" -" float m_radius;\n" -" int m_compoundBvhIndex;\n" -" };\n" -" int m_shapeType;\n" -" int m_shapeIndex;\n" -"};\n" -"typedef struct b3GpuChildShape b3GpuChildShape_t;\n" -"struct b3GpuChildShape\n" -"{\n" -" b3Float4 m_childPosition;\n" -" b3Quat m_childOrientation;\n" -" int m_shapeIndex;\n" -" int m_unused0;\n" -" int m_unused1;\n" -" int m_unused2;\n" -"};\n" -"struct b3CompoundOverlappingPair\n" -"{\n" -" int m_bodyIndexA;\n" -" int m_bodyIndexB;\n" -"// int m_pairType;\n" -" int m_childShapeIndexA;\n" -" int m_childShapeIndexB;\n" -"};\n" -"#endif //B3_COLLIDABLE_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#define B3_MPR_SQRT sqrt\n" -"#endif\n" -"#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))\n" -"#define B3_MPR_FABS fabs\n" -"#define B3_MPR_TOLERANCE 1E-6f\n" -"#define B3_MPR_MAX_ITERATIONS 1000\n" -"struct _b3MprSupport_t \n" -"{\n" -" b3Float4 v; //!< Support point in minkowski sum\n" -" b3Float4 v1; //!< Support point in obj1\n" -" b3Float4 v2; //!< Support point in obj2\n" -"};\n" -"typedef struct _b3MprSupport_t b3MprSupport_t;\n" -"struct _b3MprSimplex_t \n" -"{\n" -" b3MprSupport_t ps[4];\n" -" int last; //!< index of last added point\n" -"};\n" -"typedef struct _b3MprSimplex_t b3MprSimplex_t;\n" -"inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)\n" -"{\n" -" return &s->ps[idx];\n" -"}\n" -"inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)\n" -"{\n" -" s->last = size - 1;\n" -"}\n" -"inline int b3MprSimplexSize(const b3MprSimplex_t *s)\n" -"{\n" -" return s->last + 1;\n" -"}\n" -"inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)\n" -"{\n" -" // here is no check on boundaries\n" -" return &s->ps[idx];\n" -"}\n" -"inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)\n" -"{\n" -" *d = *s;\n" -"}\n" -"inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)\n" -"{\n" -" b3MprSupportCopy(s->ps + pos, a);\n" -"}\n" -"inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)\n" -"{\n" -" b3MprSupport_t supp;\n" -" b3MprSupportCopy(&supp, &s->ps[pos1]);\n" -" b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);\n" -" b3MprSupportCopy(&s->ps[pos2], &supp);\n" -"}\n" -"inline int b3MprIsZero(float val)\n" -"{\n" -" return B3_MPR_FABS(val) < FLT_EPSILON;\n" -"}\n" -"inline int b3MprEq(float _a, float _b)\n" -"{\n" -" float ab;\n" -" float a, b;\n" -" ab = B3_MPR_FABS(_a - _b);\n" -" if (B3_MPR_FABS(ab) < FLT_EPSILON)\n" -" return 1;\n" -" a = B3_MPR_FABS(_a);\n" -" b = B3_MPR_FABS(_b);\n" -" if (b > a){\n" -" return ab < FLT_EPSILON * b;\n" -" }else{\n" -" return ab < FLT_EPSILON * a;\n" -" }\n" -"}\n" -"inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)\n" -"{\n" -" return b3MprEq((*a).x, (*b).x)\n" -" && b3MprEq((*a).y, (*b).y)\n" -" && b3MprEq((*a).z, (*b).z);\n" -"}\n" -"inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA)\n" -"{\n" -" b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" -" float maxDot = -B3_LARGE_FLOAT;\n" -" if( 0 < hull->m_numVertices )\n" -" {\n" -" const b3Float4 scaled = supportVec;\n" -" int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" -" return verticesA[hull->m_vertexOffset+index];\n" -" }\n" -" return supVec;\n" -"}\n" -"B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" -" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" -" b3ConstArray(b3Collidable_t) cpuCollidables,\n" -" b3ConstArray(b3Float4) cpuVertices,\n" -" __global b3Float4* sepAxis,\n" -" const b3Float4* _dir, b3Float4* outp, int logme)\n" -"{\n" -" //dir is in worldspace, move to local space\n" -" \n" -" b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;\n" -" b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;\n" -" \n" -" b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);\n" -" \n" -" const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);\n" -" \n" -" //find local support vertex\n" -" int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;\n" -" \n" -" b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);\n" -" __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];\n" -" \n" -" b3Float4 pInA;\n" -" if (logme)\n" -" {\n" -" b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" -" float maxDot = -B3_LARGE_FLOAT;\n" -" if( 0 < hull->m_numVertices )\n" -" {\n" -" const b3Float4 scaled = localDir;\n" -" int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" -" pInA = cpuVertices[hull->m_vertexOffset+index];\n" -" \n" -" }\n" -" } else\n" -" {\n" -" pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);\n" -" }\n" -" //move vertex to world space\n" -" *outp = b3TransformPoint(pInA,pos,orn);\n" -" \n" -"}\n" -"inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" -" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" -" b3ConstArray(b3Collidable_t) cpuCollidables,\n" -" b3ConstArray(b3Float4) cpuVertices,\n" -" __global b3Float4* sepAxis,\n" -" const b3Float4* _dir, b3MprSupport_t *supp)\n" -"{\n" -" b3Float4 dir;\n" -" dir = *_dir;\n" -" b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);\n" -" dir = *_dir*-1.f;\n" -" b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);\n" -" supp->v = supp->v1 - supp->v2;\n" -"}\n" -"inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)\n" -"{\n" -" center->v1 = cpuBodyBuf[bodyIndexA].m_pos;\n" -" center->v2 = cpuBodyBuf[bodyIndexB].m_pos;\n" -" center->v = center->v1 - center->v2;\n" -"}\n" -"inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)\n" -"{\n" -" (*v).x = x;\n" -" (*v).y = y;\n" -" (*v).z = z;\n" -" (*v).w = 0.f;\n" -"}\n" -"inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)\n" -"{\n" -" (*v).x += (*w).x;\n" -" (*v).y += (*w).y;\n" -" (*v).z += (*w).z;\n" -"}\n" -"inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)\n" -"{\n" -" *v = *w;\n" -"}\n" -"inline void b3MprVec3Scale(b3Float4 *d, float k)\n" -"{\n" -" *d *= k;\n" -"}\n" -"inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)\n" -"{\n" -" float dot;\n" -" dot = b3Dot3F4(*a,*b);\n" -" return dot;\n" -"}\n" -"inline float b3MprVec3Len2(const b3Float4 *v)\n" -"{\n" -" return b3MprVec3Dot(v, v);\n" -"}\n" -"inline void b3MprVec3Normalize(b3Float4 *d)\n" -"{\n" -" float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));\n" -" b3MprVec3Scale(d, k);\n" -"}\n" -"inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)\n" -"{\n" -" *d = b3Cross3(*a,*b);\n" -" \n" -"}\n" -"inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)\n" -"{\n" -" *d = *v - *w;\n" -"}\n" -"inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)\n" -"{\n" -" b3Float4 v2v1, v3v1;\n" -" b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,\n" -" &b3MprSimplexPoint(portal, 1)->v);\n" -" b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,\n" -" &b3MprSimplexPoint(portal, 1)->v);\n" -" b3MprVec3Cross(dir, &v2v1, &v3v1);\n" -" b3MprVec3Normalize(dir);\n" -"}\n" -"inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,\n" -" const b3Float4 *dir)\n" -"{\n" -" float dot;\n" -" dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);\n" -" return b3MprIsZero(dot) || dot > 0.f;\n" -"}\n" -"inline int portalReachTolerance(const b3MprSimplex_t *portal,\n" -" const b3MprSupport_t *v4,\n" -" const b3Float4 *dir)\n" -"{\n" -" float dv1, dv2, dv3, dv4;\n" -" float dot1, dot2, dot3;\n" -" // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}\n" -" dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);\n" -" dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);\n" -" dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);\n" -" dv4 = b3MprVec3Dot(&v4->v, dir);\n" -" dot1 = dv4 - dv1;\n" -" dot2 = dv4 - dv2;\n" -" dot3 = dv4 - dv3;\n" -" dot1 = B3_MPR_FMIN(dot1, dot2);\n" -" dot1 = B3_MPR_FMIN(dot1, dot3);\n" -" return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;\n" -"}\n" -"inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, \n" -" const b3MprSupport_t *v4,\n" -" const b3Float4 *dir)\n" -"{\n" -" float dot;\n" -" dot = b3MprVec3Dot(&v4->v, dir);\n" -" return b3MprIsZero(dot) || dot > 0.f;\n" -"}\n" -"inline void b3ExpandPortal(b3MprSimplex_t *portal,\n" -" const b3MprSupport_t *v4)\n" -"{\n" -" float dot;\n" -" b3Float4 v4v0;\n" -" b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);\n" -" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);\n" -" if (dot > 0.f){\n" -" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);\n" -" if (dot > 0.f){\n" -" b3MprSimplexSet(portal, 1, v4);\n" -" }else{\n" -" b3MprSimplexSet(portal, 3, v4);\n" -" }\n" -" }else{\n" -" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);\n" -" if (dot > 0.f){\n" -" b3MprSimplexSet(portal, 2, v4);\n" -" }else{\n" -" b3MprSimplexSet(portal, 1, v4);\n" -" }\n" -" }\n" -"}\n" -"B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" -" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" -" b3ConstArray(b3Collidable_t) cpuCollidables,\n" -" b3ConstArray(b3Float4) cpuVertices,\n" -" __global b3Float4* sepAxis,\n" -" __global int* hasSepAxis,\n" -" b3MprSimplex_t *portal)\n" -"{\n" -" b3Float4 dir, va, vb;\n" -" float dot;\n" -" int cont;\n" -" \n" -" \n" -" // vertex 0 is center of portal\n" -" b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));\n" -" // vertex 0 is center of portal\n" -" b3MprSimplexSetSize(portal, 1);\n" -" \n" -" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" -" b3Float4* b3mpr_vec3_origin = &zero;\n" -" if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){\n" -" // Portal's center lies on origin (0,0,0) => we know that objects\n" -" // intersect but we would need to know penetration info.\n" -" // So move center little bit...\n" -" b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);\n" -" b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);\n" -" }\n" -" // vertex 1 = support in direction of origin\n" -" b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" -" b3MprVec3Scale(&dir, -1.f);\n" -" b3MprVec3Normalize(&dir);\n" -" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));\n" -" b3MprSimplexSetSize(portal, 2);\n" -" // test if origin isn't outside of v1\n" -" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);\n" -" \n" -" if (b3MprIsZero(dot) || dot < 0.f)\n" -" return -1;\n" -" // vertex 2\n" -" b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,\n" -" &b3MprSimplexPoint(portal, 1)->v);\n" -" if (b3MprIsZero(b3MprVec3Len2(&dir))){\n" -" if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){\n" -" // origin lies on v1\n" -" return 1;\n" -" }else{\n" -" // origin lies on v0-v1 segment\n" -" return 2;\n" -" }\n" -" }\n" -" b3MprVec3Normalize(&dir);\n" -" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));\n" -" \n" -" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);\n" -" if (b3MprIsZero(dot) || dot < 0.f)\n" -" return -1;\n" -" b3MprSimplexSetSize(portal, 3);\n" -" // vertex 3 direction\n" -" b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" -" &b3MprSimplexPoint(portal, 0)->v);\n" -" b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" -" &b3MprSimplexPoint(portal, 0)->v);\n" -" b3MprVec3Cross(&dir, &va, &vb);\n" -" b3MprVec3Normalize(&dir);\n" -" // it is better to form portal faces to be oriented \"outside\" origin\n" -" dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" -" if (dot > 0.f){\n" -" b3MprSimplexSwap(portal, 1, 2);\n" -" b3MprVec3Scale(&dir, -1.f);\n" -" }\n" -" while (b3MprSimplexSize(portal) < 4){\n" -" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));\n" -" \n" -" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);\n" -" if (b3MprIsZero(dot) || dot < 0.f)\n" -" return -1;\n" -" cont = 0;\n" -" // test if origin is outside (v1, v0, v3) - set v2 as v3 and\n" -" // continue\n" -" b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,\n" -" &b3MprSimplexPoint(portal, 3)->v);\n" -" dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" -" if (dot < 0.f && !b3MprIsZero(dot)){\n" -" b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));\n" -" cont = 1;\n" -" }\n" -" if (!cont){\n" -" // test if origin is outside (v3, v0, v2) - set v1 as v3 and\n" -" // continue\n" -" b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,\n" -" &b3MprSimplexPoint(portal, 2)->v);\n" -" dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" -" if (dot < 0.f && !b3MprIsZero(dot)){\n" -" b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));\n" -" cont = 1;\n" -" }\n" -" }\n" -" if (cont){\n" -" b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" -" &b3MprSimplexPoint(portal, 0)->v);\n" -" b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" -" &b3MprSimplexPoint(portal, 0)->v);\n" -" b3MprVec3Cross(&dir, &va, &vb);\n" -" b3MprVec3Normalize(&dir);\n" -" }else{\n" -" b3MprSimplexSetSize(portal, 4);\n" -" }\n" -" }\n" -" return 0;\n" -"}\n" -"B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" -" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" -" b3ConstArray(b3Collidable_t) cpuCollidables,\n" -" b3ConstArray(b3Float4) cpuVertices,\n" -" __global b3Float4* sepAxis,\n" -" b3MprSimplex_t *portal)\n" -"{\n" -" b3Float4 dir;\n" -" b3MprSupport_t v4;\n" -" for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)\n" -" //while (1)\n" -" {\n" -" // compute direction outside the portal (from v0 throught v1,v2,v3\n" -" // face)\n" -" b3PortalDir(portal, &dir);\n" -" // test if origin is inside the portal\n" -" if (portalEncapsulesOrigin(portal, &dir))\n" -" return 0;\n" -" // get next support point\n" -" \n" -" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);\n" -" // test if v4 can expand portal to contain origin and if portal\n" -" // expanding doesn't reach given tolerance\n" -" if (!portalCanEncapsuleOrigin(portal, &v4, &dir)\n" -" || portalReachTolerance(portal, &v4, &dir))\n" -" {\n" -" return -1;\n" -" }\n" -" // v1-v2-v3 triangle must be rearranged to face outside Minkowski\n" -" // difference (direction from v0).\n" -" b3ExpandPortal(portal, &v4);\n" -" }\n" -" return -1;\n" -"}\n" -"B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)\n" -"{\n" -" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" -" b3Float4* b3mpr_vec3_origin = &zero;\n" -" b3Float4 dir;\n" -" size_t i;\n" -" float b[4], sum, inv;\n" -" b3Float4 vec, p1, p2;\n" -" b3PortalDir(portal, &dir);\n" -" // use barycentric coordinates of tetrahedron to find origin\n" -" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,\n" -" &b3MprSimplexPoint(portal, 2)->v);\n" -" b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" -" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" -" &b3MprSimplexPoint(portal, 2)->v);\n" -" b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" -" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,\n" -" &b3MprSimplexPoint(portal, 1)->v);\n" -" b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" -" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" -" &b3MprSimplexPoint(portal, 1)->v);\n" -" b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" -" sum = b[0] + b[1] + b[2] + b[3];\n" -" if (b3MprIsZero(sum) || sum < 0.f){\n" -" b[0] = 0.f;\n" -" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" -" &b3MprSimplexPoint(portal, 3)->v);\n" -" b[1] = b3MprVec3Dot(&vec, &dir);\n" -" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" -" &b3MprSimplexPoint(portal, 1)->v);\n" -" b[2] = b3MprVec3Dot(&vec, &dir);\n" -" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,\n" -" &b3MprSimplexPoint(portal, 2)->v);\n" -" b[3] = b3MprVec3Dot(&vec, &dir);\n" -" sum = b[1] + b[2] + b[3];\n" -" }\n" -" inv = 1.f / sum;\n" -" b3MprVec3Copy(&p1, b3mpr_vec3_origin);\n" -" b3MprVec3Copy(&p2, b3mpr_vec3_origin);\n" -" for (i = 0; i < 4; i++){\n" -" b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);\n" -" b3MprVec3Scale(&vec, b[i]);\n" -" b3MprVec3Add(&p1, &vec);\n" -" b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);\n" -" b3MprVec3Scale(&vec, b[i]);\n" -" b3MprVec3Add(&p2, &vec);\n" -" }\n" -" b3MprVec3Scale(&p1, inv);\n" -" b3MprVec3Scale(&p2, inv);\n" -" b3MprVec3Copy(pos, &p1);\n" -" b3MprVec3Add(pos, &p2);\n" -" b3MprVec3Scale(pos, 0.5);\n" -"}\n" -"inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)\n" -"{\n" -" b3Float4 ab;\n" -" b3MprVec3Sub2(&ab, a, b);\n" -" return b3MprVec3Len2(&ab);\n" -"}\n" -"inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,\n" -" const b3Float4 *x0,\n" -" const b3Float4 *b,\n" -" b3Float4 *witness)\n" -"{\n" -" // The computation comes from solving equation of segment:\n" -" // S(t) = x0 + t.d\n" -" // where - x0 is initial point of segment\n" -" // - d is direction of segment from x0 (|d| > 0)\n" -" // - t belongs to <0, 1> interval\n" -" // \n" -" // Than, distance from a segment to some point P can be expressed:\n" -" // D(t) = |x0 + t.d - P|^2\n" -" // which is distance from any point on segment. Minimization\n" -" // of this function brings distance from P to segment.\n" -" // Minimization of D(t) leads to simple quadratic equation that's\n" -" // solving is straightforward.\n" -" //\n" -" // Bonus of this method is witness point for free.\n" -" float dist, t;\n" -" b3Float4 d, a;\n" -" // direction of segment\n" -" b3MprVec3Sub2(&d, b, x0);\n" -" // precompute vector from P to x0\n" -" b3MprVec3Sub2(&a, x0, P);\n" -" t = -1.f * b3MprVec3Dot(&a, &d);\n" -" t /= b3MprVec3Len2(&d);\n" -" if (t < 0.f || b3MprIsZero(t)){\n" -" dist = b3MprVec3Dist2(x0, P);\n" -" if (witness)\n" -" b3MprVec3Copy(witness, x0);\n" -" }else if (t > 1.f || b3MprEq(t, 1.f)){\n" -" dist = b3MprVec3Dist2(b, P);\n" -" if (witness)\n" -" b3MprVec3Copy(witness, b);\n" -" }else{\n" -" if (witness){\n" -" b3MprVec3Copy(witness, &d);\n" -" b3MprVec3Scale(witness, t);\n" -" b3MprVec3Add(witness, x0);\n" -" dist = b3MprVec3Dist2(witness, P);\n" -" }else{\n" -" // recycling variables\n" -" b3MprVec3Scale(&d, t);\n" -" b3MprVec3Add(&d, &a);\n" -" dist = b3MprVec3Len2(&d);\n" -" }\n" -" }\n" -" return dist;\n" -"}\n" -"inline float b3MprVec3PointTriDist2(const b3Float4 *P,\n" -" const b3Float4 *x0, const b3Float4 *B,\n" -" const b3Float4 *C,\n" -" b3Float4 *witness)\n" -"{\n" -" // Computation comes from analytic expression for triangle (x0, B, C)\n" -" // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and\n" -" // Then equation for distance is:\n" -" // D(s, t) = | T(s, t) - P |^2\n" -" // This leads to minimization of quadratic function of two variables.\n" -" // The solution from is taken only if s is between 0 and 1, t is\n" -" // between 0 and 1 and t + s < 1, otherwise distance from segment is\n" -" // computed.\n" -" b3Float4 d1, d2, a;\n" -" float u, v, w, p, q, r;\n" -" float s, t, dist, dist2;\n" -" b3Float4 witness2;\n" -" b3MprVec3Sub2(&d1, B, x0);\n" -" b3MprVec3Sub2(&d2, C, x0);\n" -" b3MprVec3Sub2(&a, x0, P);\n" -" u = b3MprVec3Dot(&a, &a);\n" -" v = b3MprVec3Dot(&d1, &d1);\n" -" w = b3MprVec3Dot(&d2, &d2);\n" -" p = b3MprVec3Dot(&a, &d1);\n" -" q = b3MprVec3Dot(&a, &d2);\n" -" r = b3MprVec3Dot(&d1, &d2);\n" -" s = (q * r - w * p) / (w * v - r * r);\n" -" t = (-s * r - q) / w;\n" -" if ((b3MprIsZero(s) || s > 0.f)\n" -" && (b3MprEq(s, 1.f) || s < 1.f)\n" -" && (b3MprIsZero(t) || t > 0.f)\n" -" && (b3MprEq(t, 1.f) || t < 1.f)\n" -" && (b3MprEq(t + s, 1.f) || t + s < 1.f)){\n" -" if (witness){\n" -" b3MprVec3Scale(&d1, s);\n" -" b3MprVec3Scale(&d2, t);\n" -" b3MprVec3Copy(witness, x0);\n" -" b3MprVec3Add(witness, &d1);\n" -" b3MprVec3Add(witness, &d2);\n" -" dist = b3MprVec3Dist2(witness, P);\n" -" }else{\n" -" dist = s * s * v;\n" -" dist += t * t * w;\n" -" dist += 2.f * s * t * r;\n" -" dist += 2.f * s * p;\n" -" dist += 2.f * t * q;\n" -" dist += u;\n" -" }\n" -" }else{\n" -" dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);\n" -" dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);\n" -" if (dist2 < dist){\n" -" dist = dist2;\n" -" if (witness)\n" -" b3MprVec3Copy(witness, &witness2);\n" -" }\n" -" dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);\n" -" if (dist2 < dist){\n" -" dist = dist2;\n" -" if (witness)\n" -" b3MprVec3Copy(witness, &witness2);\n" -" }\n" -" }\n" -" return dist;\n" -"}\n" -"B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" -" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" -" b3ConstArray(b3Collidable_t) cpuCollidables,\n" -" b3ConstArray(b3Float4) cpuVertices,\n" -" __global b3Float4* sepAxis,\n" -" b3MprSimplex_t *portal,\n" -" float *depth, b3Float4 *pdir, b3Float4 *pos)\n" -"{\n" -" b3Float4 dir;\n" -" b3MprSupport_t v4;\n" -" unsigned long iterations;\n" -" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" -" b3Float4* b3mpr_vec3_origin = &zero;\n" -" iterations = 1UL;\n" -" for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)\n" -" //while (1)\n" -" {\n" -" // compute portal direction and obtain next support point\n" -" b3PortalDir(portal, &dir);\n" -" \n" -" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);\n" -" // reached tolerance -> find penetration info\n" -" if (portalReachTolerance(portal, &v4, &dir)\n" -" || iterations ==B3_MPR_MAX_ITERATIONS)\n" -" {\n" -" *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);\n" -" *depth = B3_MPR_SQRT(*depth);\n" -" \n" -" if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))\n" -" {\n" -" \n" -" *pdir = dir;\n" -" } \n" -" b3MprVec3Normalize(pdir);\n" -" \n" -" // barycentric coordinates:\n" -" b3FindPos(portal, pos);\n" -" return;\n" -" }\n" -" b3ExpandPortal(portal, &v4);\n" -" iterations++;\n" -" }\n" -"}\n" -"B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)\n" -"{\n" -" // Touching contact on portal's v1 - so depth is zero and direction\n" -" // is unimportant and pos can be guessed\n" -" *depth = 0.f;\n" -" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" -" b3Float4* b3mpr_vec3_origin = &zero;\n" -" b3MprVec3Copy(dir, b3mpr_vec3_origin);\n" -" b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" -" b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" -" b3MprVec3Scale(pos, 0.5);\n" -"}\n" -"B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,\n" -" float *depth, b3Float4 *dir, b3Float4 *pos)\n" -"{\n" -" \n" -" // Origin lies on v0-v1 segment.\n" -" // Depth is distance to v1, direction also and position must be\n" -" // computed\n" -" b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" -" b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" -" b3MprVec3Scale(pos, 0.5f);\n" -" \n" -" b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);\n" -" *depth = B3_MPR_SQRT(b3MprVec3Len2(dir));\n" -" b3MprVec3Normalize(dir);\n" -"}\n" -"inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,\n" -" b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,\n" -" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" -" b3ConstArray(b3Collidable_t) cpuCollidables,\n" -" b3ConstArray(b3Float4) cpuVertices,\n" -" __global b3Float4* sepAxis,\n" -" __global int* hasSepAxis,\n" -" float *depthOut, b3Float4* dirOut, b3Float4* posOut)\n" -"{\n" -" \n" -" b3MprSimplex_t portal;\n" -" \n" -"// if (!hasSepAxis[pairIndex])\n" -" // return -1;\n" -" \n" -" hasSepAxis[pairIndex] = 0;\n" -" int res;\n" -" // Phase 1: Portal discovery\n" -" res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);\n" -" \n" -" \n" -" //sepAxis[pairIndex] = *pdir;//or -dir?\n" -" switch (res)\n" -" {\n" -" case 0:\n" -" {\n" -" // Phase 2: Portal refinement\n" -" \n" -" res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);\n" -" if (res < 0)\n" -" return -1;\n" -" // Phase 3. Penetration info\n" -" b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);\n" -" hasSepAxis[pairIndex] = 1;\n" -" sepAxis[pairIndex] = -*dirOut;\n" -" break;\n" -" }\n" -" case 1:\n" -" {\n" -" // Touching contact on portal's v1.\n" -" b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);\n" -" break;\n" -" }\n" -" case 2:\n" -" {\n" -" \n" -" b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);\n" -" break;\n" -" }\n" -" default:\n" -" {\n" -" hasSepAxis[pairIndex]=0;\n" -" //if (res < 0)\n" -" //{\n" -" // Origin isn't inside portal - no collision.\n" -" return -1;\n" -" //}\n" -" }\n" -" };\n" -" \n" -" return 0;\n" -"};\n" -"#endif //B3_MPR_PENETRATION_H\n" -"#ifndef B3_CONTACT4DATA_H\n" -"#define B3_CONTACT4DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"typedef struct b3Contact4Data b3Contact4Data_t;\n" -"struct b3Contact4Data\n" -"{\n" -" b3Float4 m_worldPosB[4];\n" -"// b3Float4 m_localPosA[4];\n" -"// b3Float4 m_localPosB[4];\n" -" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" -" unsigned short m_restituitionCoeffCmp;\n" -" unsigned short m_frictionCoeffCmp;\n" -" int m_batchIdx;\n" -" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" -" int m_bodyBPtrAndSignBit;\n" -" int m_childIndexA;\n" -" int m_childIndexB;\n" -" int m_unused1;\n" -" int m_unused2;\n" -"};\n" -"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" -"{\n" -" return (int)contact->m_worldNormalOnB.w;\n" -"};\n" -"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" -"{\n" -" contact->m_worldNormalOnB.w = (float)numPoints;\n" -"};\n" -"#endif //B3_CONTACT4DATA_H\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n" -"#ifdef cl_ext_atomic_counters_32\n" -" #pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -" #define counter32_t volatile __global int*\n" -"#endif\n" -"__kernel void mprPenetrationKernel( __global int4* pairs,\n" -" __global const b3RigidBodyData_t* rigidBodies, \n" -" __global const b3Collidable_t* collidables,\n" -" __global const b3ConvexPolyhedronData_t* convexShapes, \n" -" __global const float4* vertices,\n" -" __global float4* separatingNormals,\n" -" __global int* hasSeparatingAxis,\n" -" __global struct b3Contact4Data* restrict globalContactsOut,\n" -" counter32_t nGlobalContactsOut,\n" -" int contactCapacity,\n" -" int numPairs)\n" -"{\n" -" int i = get_global_id(0);\n" -" int pairIndex = i;\n" -" if (i<numPairs)\n" -" {\n" -" int bodyIndexA = pairs[i].x;\n" -" int bodyIndexB = pairs[i].y;\n" -" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" -" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" -" \n" -" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" -" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" -" \n" -" \n" -" //once the broadphase avoids static-static pairs, we can remove this test\n" -" if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))\n" -" {\n" -" return;\n" -" }\n" -" \n" -" if ((collidables[collidableIndexA].m_shapeType!=SHAPE_CONVEX_HULL) ||(collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL))\n" -" {\n" -" return;\n" -" }\n" -" float depthOut;\n" -" b3Float4 dirOut;\n" -" b3Float4 posOut;\n" -" int res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB,rigidBodies,convexShapes,collidables,vertices,separatingNormals,hasSeparatingAxis,&depthOut, &dirOut, &posOut);\n" -" \n" -" \n" -" \n" -" \n" -" if (res==0)\n" -" {\n" -" //add a contact\n" -" int dstIdx;\n" -" AppendInc( nGlobalContactsOut, dstIdx );\n" -" if (dstIdx<contactCapacity)\n" -" {\n" -" pairs[pairIndex].z = dstIdx;\n" -" __global struct b3Contact4Data* c = globalContactsOut + dstIdx;\n" -" c->m_worldNormalOnB = -dirOut;//normal;\n" -" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" -" c->m_batchIdx = pairIndex;\n" -" int bodyA = pairs[pairIndex].x;\n" -" int bodyB = pairs[pairIndex].y;\n" -" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;\n" -" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;\n" -" c->m_childIndexA = -1;\n" -" c->m_childIndexB = -1;\n" -" //for (int i=0;i<nContacts;i++)\n" -" posOut.w = -depthOut;\n" -" c->m_worldPosB[0] = posOut;//localPoints[contactIdx[i]];\n" -" GET_NPOINTS(*c) = 1;//nContacts;\n" -" }\n" -" }\n" -" }\n" -"}\n" -"typedef float4 Quaternion;\n" -"#define make_float4 (float4)\n" -"__inline\n" -"float dot3F4(float4 a, float4 b)\n" -"{\n" -" float4 a1 = make_float4(a.xyz,0.f);\n" -" float4 b1 = make_float4(b.xyz,0.f);\n" -" return dot(a1, b1);\n" -"}\n" -"__inline\n" -"float4 cross3(float4 a, float4 b)\n" -"{\n" -" return cross(a,b);\n" -"}\n" -"__inline\n" -"Quaternion qtMul(Quaternion a, Quaternion b)\n" -"{\n" -" Quaternion ans;\n" -" ans = cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"__inline\n" -"Quaternion qtInvert(Quaternion q)\n" -"{\n" -" return (Quaternion)(-q.xyz, q.w);\n" -"}\n" -"__inline\n" -"float4 qtRotate(Quaternion q, float4 vec)\n" -"{\n" -" Quaternion qInv = qtInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"__inline\n" -"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" -"{\n" -" return qtRotate( *orientation, *p ) + (*translation);\n" -"}\n" -"__inline\n" -"float4 qtInvRotate(const Quaternion q, float4 vec)\n" -"{\n" -" return qtRotate( qtInvert( q ), vec );\n" -"}\n" -"inline void project(__global const b3ConvexPolyhedronData_t* hull, const float4 pos, const float4 orn, \n" -"const float4* dir, __global const float4* vertices, float* min, float* max)\n" -"{\n" -" min[0] = FLT_MAX;\n" -" max[0] = -FLT_MAX;\n" -" int numVerts = hull->m_numVertices;\n" -" const float4 localDir = qtInvRotate(orn,*dir);\n" -" float offset = dot(pos,*dir);\n" -" for(int i=0;i<numVerts;i++)\n" -" {\n" -" float dp = dot(vertices[hull->m_vertexOffset+i],localDir);\n" -" if(dp < min[0]) \n" -" min[0] = dp;\n" -" if(dp > max[0]) \n" -" max[0] = dp;\n" -" }\n" -" if(min[0]>max[0])\n" -" {\n" -" float tmp = min[0];\n" -" min[0] = max[0];\n" -" max[0] = tmp;\n" -" }\n" -" min[0] += offset;\n" -" max[0] += offset;\n" -"}\n" -"bool findSeparatingAxisUnitSphere( __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, \n" -" const float4 posA1,\n" -" const float4 ornA,\n" -" const float4 posB1,\n" -" const float4 ornB,\n" -" const float4 DeltaC2,\n" -" __global const float4* vertices,\n" -" __global const float4* unitSphereDirections,\n" -" int numUnitSphereDirections,\n" -" float4* sep,\n" -" float* dmin)\n" -"{\n" -" \n" -" float4 posA = posA1;\n" -" posA.w = 0.f;\n" -" float4 posB = posB1;\n" -" posB.w = 0.f;\n" -" int curPlaneTests=0;\n" -" int curEdgeEdge = 0;\n" -" // Test unit sphere directions\n" -" for (int i=0;i<numUnitSphereDirections;i++)\n" -" {\n" -" float4 crossje;\n" -" crossje = unitSphereDirections[i]; \n" -" if (dot3F4(DeltaC2,crossje)>0)\n" -" crossje *= -1.f;\n" -" {\n" -" float dist;\n" -" bool result = true;\n" -" float Min0,Max0;\n" -" float Min1,Max1;\n" -" project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0);\n" -" project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1);\n" -" \n" -" if(Max0<Min1 || Max1<Min0)\n" -" return false;\n" -" \n" -" float d0 = Max0 - Min1;\n" -" float d1 = Max1 - Min0;\n" -" dist = d0<d1 ? d0:d1;\n" -" result = true;\n" -" \n" -" if(dist<*dmin)\n" -" {\n" -" *dmin = dist;\n" -" *sep = crossje;\n" -" }\n" -" }\n" -" }\n" -" \n" -" if((dot3F4(-DeltaC2,*sep))>0.0f)\n" -" {\n" -" *sep = -(*sep);\n" -" }\n" -" return true;\n" -"}\n" -"__kernel void findSeparatingAxisUnitSphereKernel( __global const int4* pairs, \n" -" __global const b3RigidBodyData_t* rigidBodies, \n" -" __global const b3Collidable_t* collidables,\n" -" __global const b3ConvexPolyhedronData_t* convexShapes, \n" -" __global const float4* vertices,\n" -" __global const float4* unitSphereDirections,\n" -" __global float4* separatingNormals,\n" -" __global int* hasSeparatingAxis,\n" -" __global float* dmins,\n" -" int numUnitSphereDirections,\n" -" int numPairs\n" -" )\n" -"{\n" -" int i = get_global_id(0);\n" -" \n" -" if (i<numPairs)\n" -" {\n" -" if (hasSeparatingAxis[i])\n" -" {\n" -" \n" -" int bodyIndexA = pairs[i].x;\n" -" int bodyIndexB = pairs[i].y;\n" -" \n" -" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" -" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" -" \n" -" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" -" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" -" \n" -" \n" -" int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" -" \n" -" float dmin = dmins[i];\n" -" \n" -" float4 posA = rigidBodies[bodyIndexA].m_pos;\n" -" posA.w = 0.f;\n" -" float4 posB = rigidBodies[bodyIndexB].m_pos;\n" -" posB.w = 0.f;\n" -" float4 c0local = convexShapes[shapeIndexA].m_localCenter;\n" -" float4 ornA = rigidBodies[bodyIndexA].m_quat;\n" -" float4 c0 = transform(&c0local, &posA, &ornA);\n" -" float4 c1local = convexShapes[shapeIndexB].m_localCenter;\n" -" float4 ornB =rigidBodies[bodyIndexB].m_quat;\n" -" float4 c1 = transform(&c1local,&posB,&ornB);\n" -" const float4 DeltaC2 = c0 - c1;\n" -" float4 sepNormal = separatingNormals[i];\n" -" \n" -" int numEdgeEdgeDirections = convexShapes[shapeIndexA].m_numUniqueEdges*convexShapes[shapeIndexB].m_numUniqueEdges;\n" -" if (numEdgeEdgeDirections>numUnitSphereDirections)\n" -" {\n" -" bool sepEE = findSeparatingAxisUnitSphere( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,\n" -" posB,ornB,\n" -" DeltaC2,\n" -" vertices,unitSphereDirections,numUnitSphereDirections,&sepNormal,&dmin);\n" -" if (!sepEE)\n" -" {\n" -" hasSeparatingAxis[i] = 0;\n" -" } else\n" -" {\n" -" hasSeparatingAxis[i] = 1;\n" -" separatingNormals[i] = sepNormal;\n" -" }\n" -" }\n" -" } //if (hasSeparatingAxis[i])\n" -" }//(i<numPairs)\n" -"}\n" -; +static const char* mprKernelsCL = + "/***\n" + " * ---------------------------------\n" + " * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>\n" + " *\n" + " * This file was ported from mpr.c file, part of libccd.\n" + " * The Minkoski Portal Refinement implementation was ported \n" + " * to OpenCL by Erwin Coumans for the Bullet 3 Physics library.\n" + " * at http://github.com/erwincoumans/bullet3\n" + " *\n" + " * Distributed under the OSI-approved BSD License (the \"License\");\n" + " * see <http://www.opensource.org/licenses/bsd-license.php>.\n" + " * This software is distributed WITHOUT ANY WARRANTY; without even the\n" + " * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\n" + " * See the License for more information.\n" + " */\n" + "#ifndef B3_MPR_PENETRATION_H\n" + "#define B3_MPR_PENETRATION_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_RIGIDBODY_DATA_H\n" + "#define B3_RIGIDBODY_DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_QUAT_H\n" + "#define B3_QUAT_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif\n" + "#endif\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Quat;\n" + " #define b3QuatConstArg const b3Quat\n" + " \n" + " \n" + "inline float4 b3FastNormalize4(float4 v)\n" + "{\n" + " v = (float4)(v.xyz,0.f);\n" + " return fast_normalize(v);\n" + "}\n" + " \n" + "inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" + "inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" + "inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" + "{\n" + " b3Quat ans;\n" + " ans = b3Cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - b3Dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" + "{\n" + " b3Quat q;\n" + " q=in;\n" + " //return b3FastNormalize4(in);\n" + " float len = native_sqrt(dot(q, q));\n" + " if(len > 0.f)\n" + " {\n" + " q *= 1.f / len;\n" + " }\n" + " else\n" + " {\n" + " q.x = q.y = q.z = 0.f;\n" + " q.w = 1.f;\n" + " }\n" + " return q;\n" + "}\n" + "inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " b3Quat qInv = b3QuatInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " return b3QuatRotate( b3QuatInvert( q ), vec );\n" + "}\n" + "inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" + "{\n" + " return b3QuatRotate( orientation, point ) + (translation);\n" + "}\n" + " \n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifndef B3_MAT3x3_H\n" + "#define B3_MAT3x3_H\n" + "#ifndef B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "typedef struct\n" + "{\n" + " b3Float4 m_row[3];\n" + "}b3Mat3x3;\n" + "#define b3Mat3x3ConstArg const b3Mat3x3\n" + "#define b3GetRow(m,row) (m.m_row[row])\n" + "inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" + "{\n" + " b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" + " b3Mat3x3 out;\n" + " out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" + " out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" + " out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" + " out.m_row[0].w = 0.f;\n" + " out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" + " out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" + " out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" + " out.m_row[1].w = 0.f;\n" + " out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" + " out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" + " out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" + " out.m_row[2].w = 0.f;\n" + " return out;\n" + "}\n" + "inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = fabs(matIn.m_row[0]);\n" + " out.m_row[1] = fabs(matIn.m_row[1]);\n" + " out.m_row[2] = fabs(matIn.m_row[2]);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtZero();\n" + "__inline\n" + "b3Mat3x3 mtIdentity();\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Mat3x3 mtZero()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(0.f);\n" + " m.m_row[1] = (b3Float4)(0.f);\n" + " m.m_row[2] = (b3Float4)(0.f);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtIdentity()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(1,0,0,0);\n" + " m.m_row[1] = (b3Float4)(0,1,0,0);\n" + " m.m_row[2] = (b3Float4)(0,0,1,0);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" + " out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" + " out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" + "{\n" + " b3Mat3x3 transB;\n" + " transB = mtTranspose( b );\n" + " b3Mat3x3 ans;\n" + " // why this doesn't run when 0ing in the for{}\n" + " a.m_row[0].w = 0.f;\n" + " a.m_row[1].w = 0.f;\n" + " a.m_row[2].w = 0.f;\n" + " for(int i=0; i<3; i++)\n" + " {\n" + "// a.m_row[i].w = 0.f;\n" + " ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" + " ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" + " ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" + " ans.m_row[i].w = 0.f;\n" + " }\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" + "{\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a.m_row[0], b );\n" + " ans.y = b3Dot3F4( a.m_row[1], b );\n" + " ans.z = b3Dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" + "{\n" + " b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a, colx );\n" + " ans.y = b3Dot3F4( a, coly );\n" + " ans.z = b3Dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "#endif\n" + "#endif //B3_MAT3x3_H\n" + "typedef struct b3RigidBodyData b3RigidBodyData_t;\n" + "struct b3RigidBodyData\n" + "{\n" + " b3Float4 m_pos;\n" + " b3Quat m_quat;\n" + " b3Float4 m_linVel;\n" + " b3Float4 m_angVel;\n" + " int m_collidableIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "};\n" + "typedef struct b3InertiaData b3InertiaData_t;\n" + "struct b3InertiaData\n" + "{\n" + " b3Mat3x3 m_invInertiaWorld;\n" + " b3Mat3x3 m_initInvInertia;\n" + "};\n" + "#endif //B3_RIGIDBODY_DATA_H\n" + " \n" + "#ifndef B3_CONVEX_POLYHEDRON_DATA_H\n" + "#define B3_CONVEX_POLYHEDRON_DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "typedef struct b3GpuFace b3GpuFace_t;\n" + "struct b3GpuFace\n" + "{\n" + " b3Float4 m_plane;\n" + " int m_indexOffset;\n" + " int m_numIndices;\n" + " int m_unusedPadding1;\n" + " int m_unusedPadding2;\n" + "};\n" + "typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;\n" + "struct b3ConvexPolyhedronData\n" + "{\n" + " b3Float4 m_localCenter;\n" + " b3Float4 m_extents;\n" + " b3Float4 mC;\n" + " b3Float4 mE;\n" + " float m_radius;\n" + " int m_faceOffset;\n" + " int m_numFaces;\n" + " int m_numVertices;\n" + " int m_vertexOffset;\n" + " int m_uniqueEdgesOffset;\n" + " int m_numUniqueEdges;\n" + " int m_unused;\n" + "};\n" + "#endif //B3_CONVEX_POLYHEDRON_DATA_H\n" + "#ifndef B3_COLLIDABLE_H\n" + "#define B3_COLLIDABLE_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "enum b3ShapeTypes\n" + "{\n" + " SHAPE_HEIGHT_FIELD=1,\n" + " SHAPE_CONVEX_HULL=3,\n" + " SHAPE_PLANE=4,\n" + " SHAPE_CONCAVE_TRIMESH=5,\n" + " SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" + " SHAPE_SPHERE=7,\n" + " MAX_NUM_SHAPE_TYPES,\n" + "};\n" + "typedef struct b3Collidable b3Collidable_t;\n" + "struct b3Collidable\n" + "{\n" + " union {\n" + " int m_numChildShapes;\n" + " int m_bvhIndex;\n" + " };\n" + " union\n" + " {\n" + " float m_radius;\n" + " int m_compoundBvhIndex;\n" + " };\n" + " int m_shapeType;\n" + " int m_shapeIndex;\n" + "};\n" + "typedef struct b3GpuChildShape b3GpuChildShape_t;\n" + "struct b3GpuChildShape\n" + "{\n" + " b3Float4 m_childPosition;\n" + " b3Quat m_childOrientation;\n" + " int m_shapeIndex;\n" + " int m_unused0;\n" + " int m_unused1;\n" + " int m_unused2;\n" + "};\n" + "struct b3CompoundOverlappingPair\n" + "{\n" + " int m_bodyIndexA;\n" + " int m_bodyIndexB;\n" + "// int m_pairType;\n" + " int m_childShapeIndexA;\n" + " int m_childShapeIndexB;\n" + "};\n" + "#endif //B3_COLLIDABLE_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#define B3_MPR_SQRT sqrt\n" + "#endif\n" + "#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))\n" + "#define B3_MPR_FABS fabs\n" + "#define B3_MPR_TOLERANCE 1E-6f\n" + "#define B3_MPR_MAX_ITERATIONS 1000\n" + "struct _b3MprSupport_t \n" + "{\n" + " b3Float4 v; //!< Support point in minkowski sum\n" + " b3Float4 v1; //!< Support point in obj1\n" + " b3Float4 v2; //!< Support point in obj2\n" + "};\n" + "typedef struct _b3MprSupport_t b3MprSupport_t;\n" + "struct _b3MprSimplex_t \n" + "{\n" + " b3MprSupport_t ps[4];\n" + " int last; //!< index of last added point\n" + "};\n" + "typedef struct _b3MprSimplex_t b3MprSimplex_t;\n" + "inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)\n" + "{\n" + " return &s->ps[idx];\n" + "}\n" + "inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)\n" + "{\n" + " s->last = size - 1;\n" + "}\n" + "inline int b3MprSimplexSize(const b3MprSimplex_t *s)\n" + "{\n" + " return s->last + 1;\n" + "}\n" + "inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)\n" + "{\n" + " // here is no check on boundaries\n" + " return &s->ps[idx];\n" + "}\n" + "inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)\n" + "{\n" + " *d = *s;\n" + "}\n" + "inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)\n" + "{\n" + " b3MprSupportCopy(s->ps + pos, a);\n" + "}\n" + "inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)\n" + "{\n" + " b3MprSupport_t supp;\n" + " b3MprSupportCopy(&supp, &s->ps[pos1]);\n" + " b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);\n" + " b3MprSupportCopy(&s->ps[pos2], &supp);\n" + "}\n" + "inline int b3MprIsZero(float val)\n" + "{\n" + " return B3_MPR_FABS(val) < FLT_EPSILON;\n" + "}\n" + "inline int b3MprEq(float _a, float _b)\n" + "{\n" + " float ab;\n" + " float a, b;\n" + " ab = B3_MPR_FABS(_a - _b);\n" + " if (B3_MPR_FABS(ab) < FLT_EPSILON)\n" + " return 1;\n" + " a = B3_MPR_FABS(_a);\n" + " b = B3_MPR_FABS(_b);\n" + " if (b > a){\n" + " return ab < FLT_EPSILON * b;\n" + " }else{\n" + " return ab < FLT_EPSILON * a;\n" + " }\n" + "}\n" + "inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)\n" + "{\n" + " return b3MprEq((*a).x, (*b).x)\n" + " && b3MprEq((*a).y, (*b).y)\n" + " && b3MprEq((*a).z, (*b).z);\n" + "}\n" + "inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA)\n" + "{\n" + " b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" + " float maxDot = -B3_LARGE_FLOAT;\n" + " if( 0 < hull->m_numVertices )\n" + " {\n" + " const b3Float4 scaled = supportVec;\n" + " int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" + " return verticesA[hull->m_vertexOffset+index];\n" + " }\n" + " return supVec;\n" + "}\n" + "B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" + " b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" + " b3ConstArray(b3Collidable_t) cpuCollidables,\n" + " b3ConstArray(b3Float4) cpuVertices,\n" + " __global b3Float4* sepAxis,\n" + " const b3Float4* _dir, b3Float4* outp, int logme)\n" + "{\n" + " //dir is in worldspace, move to local space\n" + " \n" + " b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;\n" + " b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;\n" + " \n" + " b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);\n" + " \n" + " const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);\n" + " \n" + " //find local support vertex\n" + " int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;\n" + " \n" + " b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);\n" + " __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];\n" + " \n" + " b3Float4 pInA;\n" + " if (logme)\n" + " {\n" + " b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" + " float maxDot = -B3_LARGE_FLOAT;\n" + " if( 0 < hull->m_numVertices )\n" + " {\n" + " const b3Float4 scaled = localDir;\n" + " int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" + " pInA = cpuVertices[hull->m_vertexOffset+index];\n" + " \n" + " }\n" + " } else\n" + " {\n" + " pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);\n" + " }\n" + " //move vertex to world space\n" + " *outp = b3TransformPoint(pInA,pos,orn);\n" + " \n" + "}\n" + "inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" + " b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" + " b3ConstArray(b3Collidable_t) cpuCollidables,\n" + " b3ConstArray(b3Float4) cpuVertices,\n" + " __global b3Float4* sepAxis,\n" + " const b3Float4* _dir, b3MprSupport_t *supp)\n" + "{\n" + " b3Float4 dir;\n" + " dir = *_dir;\n" + " b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);\n" + " dir = *_dir*-1.f;\n" + " b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);\n" + " supp->v = supp->v1 - supp->v2;\n" + "}\n" + "inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)\n" + "{\n" + " center->v1 = cpuBodyBuf[bodyIndexA].m_pos;\n" + " center->v2 = cpuBodyBuf[bodyIndexB].m_pos;\n" + " center->v = center->v1 - center->v2;\n" + "}\n" + "inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)\n" + "{\n" + " (*v).x = x;\n" + " (*v).y = y;\n" + " (*v).z = z;\n" + " (*v).w = 0.f;\n" + "}\n" + "inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)\n" + "{\n" + " (*v).x += (*w).x;\n" + " (*v).y += (*w).y;\n" + " (*v).z += (*w).z;\n" + "}\n" + "inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)\n" + "{\n" + " *v = *w;\n" + "}\n" + "inline void b3MprVec3Scale(b3Float4 *d, float k)\n" + "{\n" + " *d *= k;\n" + "}\n" + "inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)\n" + "{\n" + " float dot;\n" + " dot = b3Dot3F4(*a,*b);\n" + " return dot;\n" + "}\n" + "inline float b3MprVec3Len2(const b3Float4 *v)\n" + "{\n" + " return b3MprVec3Dot(v, v);\n" + "}\n" + "inline void b3MprVec3Normalize(b3Float4 *d)\n" + "{\n" + " float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));\n" + " b3MprVec3Scale(d, k);\n" + "}\n" + "inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)\n" + "{\n" + " *d = b3Cross3(*a,*b);\n" + " \n" + "}\n" + "inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)\n" + "{\n" + " *d = *v - *w;\n" + "}\n" + "inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)\n" + "{\n" + " b3Float4 v2v1, v3v1;\n" + " b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,\n" + " &b3MprSimplexPoint(portal, 1)->v);\n" + " b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,\n" + " &b3MprSimplexPoint(portal, 1)->v);\n" + " b3MprVec3Cross(dir, &v2v1, &v3v1);\n" + " b3MprVec3Normalize(dir);\n" + "}\n" + "inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,\n" + " const b3Float4 *dir)\n" + "{\n" + " float dot;\n" + " dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);\n" + " return b3MprIsZero(dot) || dot > 0.f;\n" + "}\n" + "inline int portalReachTolerance(const b3MprSimplex_t *portal,\n" + " const b3MprSupport_t *v4,\n" + " const b3Float4 *dir)\n" + "{\n" + " float dv1, dv2, dv3, dv4;\n" + " float dot1, dot2, dot3;\n" + " // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}\n" + " dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);\n" + " dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);\n" + " dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);\n" + " dv4 = b3MprVec3Dot(&v4->v, dir);\n" + " dot1 = dv4 - dv1;\n" + " dot2 = dv4 - dv2;\n" + " dot3 = dv4 - dv3;\n" + " dot1 = B3_MPR_FMIN(dot1, dot2);\n" + " dot1 = B3_MPR_FMIN(dot1, dot3);\n" + " return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;\n" + "}\n" + "inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, \n" + " const b3MprSupport_t *v4,\n" + " const b3Float4 *dir)\n" + "{\n" + " float dot;\n" + " dot = b3MprVec3Dot(&v4->v, dir);\n" + " return b3MprIsZero(dot) || dot > 0.f;\n" + "}\n" + "inline void b3ExpandPortal(b3MprSimplex_t *portal,\n" + " const b3MprSupport_t *v4)\n" + "{\n" + " float dot;\n" + " b3Float4 v4v0;\n" + " b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);\n" + " dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);\n" + " if (dot > 0.f){\n" + " dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);\n" + " if (dot > 0.f){\n" + " b3MprSimplexSet(portal, 1, v4);\n" + " }else{\n" + " b3MprSimplexSet(portal, 3, v4);\n" + " }\n" + " }else{\n" + " dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);\n" + " if (dot > 0.f){\n" + " b3MprSimplexSet(portal, 2, v4);\n" + " }else{\n" + " b3MprSimplexSet(portal, 1, v4);\n" + " }\n" + " }\n" + "}\n" + "B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" + " b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" + " b3ConstArray(b3Collidable_t) cpuCollidables,\n" + " b3ConstArray(b3Float4) cpuVertices,\n" + " __global b3Float4* sepAxis,\n" + " __global int* hasSepAxis,\n" + " b3MprSimplex_t *portal)\n" + "{\n" + " b3Float4 dir, va, vb;\n" + " float dot;\n" + " int cont;\n" + " \n" + " \n" + " // vertex 0 is center of portal\n" + " b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));\n" + " // vertex 0 is center of portal\n" + " b3MprSimplexSetSize(portal, 1);\n" + " \n" + " b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" + " b3Float4* b3mpr_vec3_origin = &zero;\n" + " if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){\n" + " // Portal's center lies on origin (0,0,0) => we know that objects\n" + " // intersect but we would need to know penetration info.\n" + " // So move center little bit...\n" + " b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);\n" + " b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);\n" + " }\n" + " // vertex 1 = support in direction of origin\n" + " b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" + " b3MprVec3Scale(&dir, -1.f);\n" + " b3MprVec3Normalize(&dir);\n" + " b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));\n" + " b3MprSimplexSetSize(portal, 2);\n" + " // test if origin isn't outside of v1\n" + " dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);\n" + " \n" + " if (b3MprIsZero(dot) || dot < 0.f)\n" + " return -1;\n" + " // vertex 2\n" + " b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,\n" + " &b3MprSimplexPoint(portal, 1)->v);\n" + " if (b3MprIsZero(b3MprVec3Len2(&dir))){\n" + " if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){\n" + " // origin lies on v1\n" + " return 1;\n" + " }else{\n" + " // origin lies on v0-v1 segment\n" + " return 2;\n" + " }\n" + " }\n" + " b3MprVec3Normalize(&dir);\n" + " b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));\n" + " \n" + " dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);\n" + " if (b3MprIsZero(dot) || dot < 0.f)\n" + " return -1;\n" + " b3MprSimplexSetSize(portal, 3);\n" + " // vertex 3 direction\n" + " b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" + " &b3MprSimplexPoint(portal, 0)->v);\n" + " b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" + " &b3MprSimplexPoint(portal, 0)->v);\n" + " b3MprVec3Cross(&dir, &va, &vb);\n" + " b3MprVec3Normalize(&dir);\n" + " // it is better to form portal faces to be oriented \"outside\" origin\n" + " dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" + " if (dot > 0.f){\n" + " b3MprSimplexSwap(portal, 1, 2);\n" + " b3MprVec3Scale(&dir, -1.f);\n" + " }\n" + " while (b3MprSimplexSize(portal) < 4){\n" + " b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));\n" + " \n" + " dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);\n" + " if (b3MprIsZero(dot) || dot < 0.f)\n" + " return -1;\n" + " cont = 0;\n" + " // test if origin is outside (v1, v0, v3) - set v2 as v3 and\n" + " // continue\n" + " b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,\n" + " &b3MprSimplexPoint(portal, 3)->v);\n" + " dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" + " if (dot < 0.f && !b3MprIsZero(dot)){\n" + " b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));\n" + " cont = 1;\n" + " }\n" + " if (!cont){\n" + " // test if origin is outside (v3, v0, v2) - set v1 as v3 and\n" + " // continue\n" + " b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,\n" + " &b3MprSimplexPoint(portal, 2)->v);\n" + " dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" + " if (dot < 0.f && !b3MprIsZero(dot)){\n" + " b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));\n" + " cont = 1;\n" + " }\n" + " }\n" + " if (cont){\n" + " b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" + " &b3MprSimplexPoint(portal, 0)->v);\n" + " b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" + " &b3MprSimplexPoint(portal, 0)->v);\n" + " b3MprVec3Cross(&dir, &va, &vb);\n" + " b3MprVec3Normalize(&dir);\n" + " }else{\n" + " b3MprSimplexSetSize(portal, 4);\n" + " }\n" + " }\n" + " return 0;\n" + "}\n" + "B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" + " b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" + " b3ConstArray(b3Collidable_t) cpuCollidables,\n" + " b3ConstArray(b3Float4) cpuVertices,\n" + " __global b3Float4* sepAxis,\n" + " b3MprSimplex_t *portal)\n" + "{\n" + " b3Float4 dir;\n" + " b3MprSupport_t v4;\n" + " for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)\n" + " //while (1)\n" + " {\n" + " // compute direction outside the portal (from v0 throught v1,v2,v3\n" + " // face)\n" + " b3PortalDir(portal, &dir);\n" + " // test if origin is inside the portal\n" + " if (portalEncapsulesOrigin(portal, &dir))\n" + " return 0;\n" + " // get next support point\n" + " \n" + " b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);\n" + " // test if v4 can expand portal to contain origin and if portal\n" + " // expanding doesn't reach given tolerance\n" + " if (!portalCanEncapsuleOrigin(portal, &v4, &dir)\n" + " || portalReachTolerance(portal, &v4, &dir))\n" + " {\n" + " return -1;\n" + " }\n" + " // v1-v2-v3 triangle must be rearranged to face outside Minkowski\n" + " // difference (direction from v0).\n" + " b3ExpandPortal(portal, &v4);\n" + " }\n" + " return -1;\n" + "}\n" + "B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)\n" + "{\n" + " b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" + " b3Float4* b3mpr_vec3_origin = &zero;\n" + " b3Float4 dir;\n" + " size_t i;\n" + " float b[4], sum, inv;\n" + " b3Float4 vec, p1, p2;\n" + " b3PortalDir(portal, &dir);\n" + " // use barycentric coordinates of tetrahedron to find origin\n" + " b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,\n" + " &b3MprSimplexPoint(portal, 2)->v);\n" + " b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" + " b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" + " &b3MprSimplexPoint(portal, 2)->v);\n" + " b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" + " b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,\n" + " &b3MprSimplexPoint(portal, 1)->v);\n" + " b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" + " b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" + " &b3MprSimplexPoint(portal, 1)->v);\n" + " b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" + " sum = b[0] + b[1] + b[2] + b[3];\n" + " if (b3MprIsZero(sum) || sum < 0.f){\n" + " b[0] = 0.f;\n" + " b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" + " &b3MprSimplexPoint(portal, 3)->v);\n" + " b[1] = b3MprVec3Dot(&vec, &dir);\n" + " b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" + " &b3MprSimplexPoint(portal, 1)->v);\n" + " b[2] = b3MprVec3Dot(&vec, &dir);\n" + " b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,\n" + " &b3MprSimplexPoint(portal, 2)->v);\n" + " b[3] = b3MprVec3Dot(&vec, &dir);\n" + " sum = b[1] + b[2] + b[3];\n" + " }\n" + " inv = 1.f / sum;\n" + " b3MprVec3Copy(&p1, b3mpr_vec3_origin);\n" + " b3MprVec3Copy(&p2, b3mpr_vec3_origin);\n" + " for (i = 0; i < 4; i++){\n" + " b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);\n" + " b3MprVec3Scale(&vec, b[i]);\n" + " b3MprVec3Add(&p1, &vec);\n" + " b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);\n" + " b3MprVec3Scale(&vec, b[i]);\n" + " b3MprVec3Add(&p2, &vec);\n" + " }\n" + " b3MprVec3Scale(&p1, inv);\n" + " b3MprVec3Scale(&p2, inv);\n" + " b3MprVec3Copy(pos, &p1);\n" + " b3MprVec3Add(pos, &p2);\n" + " b3MprVec3Scale(pos, 0.5);\n" + "}\n" + "inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)\n" + "{\n" + " b3Float4 ab;\n" + " b3MprVec3Sub2(&ab, a, b);\n" + " return b3MprVec3Len2(&ab);\n" + "}\n" + "inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,\n" + " const b3Float4 *x0,\n" + " const b3Float4 *b,\n" + " b3Float4 *witness)\n" + "{\n" + " // The computation comes from solving equation of segment:\n" + " // S(t) = x0 + t.d\n" + " // where - x0 is initial point of segment\n" + " // - d is direction of segment from x0 (|d| > 0)\n" + " // - t belongs to <0, 1> interval\n" + " // \n" + " // Than, distance from a segment to some point P can be expressed:\n" + " // D(t) = |x0 + t.d - P|^2\n" + " // which is distance from any point on segment. Minimization\n" + " // of this function brings distance from P to segment.\n" + " // Minimization of D(t) leads to simple quadratic equation that's\n" + " // solving is straightforward.\n" + " //\n" + " // Bonus of this method is witness point for free.\n" + " float dist, t;\n" + " b3Float4 d, a;\n" + " // direction of segment\n" + " b3MprVec3Sub2(&d, b, x0);\n" + " // precompute vector from P to x0\n" + " b3MprVec3Sub2(&a, x0, P);\n" + " t = -1.f * b3MprVec3Dot(&a, &d);\n" + " t /= b3MprVec3Len2(&d);\n" + " if (t < 0.f || b3MprIsZero(t)){\n" + " dist = b3MprVec3Dist2(x0, P);\n" + " if (witness)\n" + " b3MprVec3Copy(witness, x0);\n" + " }else if (t > 1.f || b3MprEq(t, 1.f)){\n" + " dist = b3MprVec3Dist2(b, P);\n" + " if (witness)\n" + " b3MprVec3Copy(witness, b);\n" + " }else{\n" + " if (witness){\n" + " b3MprVec3Copy(witness, &d);\n" + " b3MprVec3Scale(witness, t);\n" + " b3MprVec3Add(witness, x0);\n" + " dist = b3MprVec3Dist2(witness, P);\n" + " }else{\n" + " // recycling variables\n" + " b3MprVec3Scale(&d, t);\n" + " b3MprVec3Add(&d, &a);\n" + " dist = b3MprVec3Len2(&d);\n" + " }\n" + " }\n" + " return dist;\n" + "}\n" + "inline float b3MprVec3PointTriDist2(const b3Float4 *P,\n" + " const b3Float4 *x0, const b3Float4 *B,\n" + " const b3Float4 *C,\n" + " b3Float4 *witness)\n" + "{\n" + " // Computation comes from analytic expression for triangle (x0, B, C)\n" + " // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and\n" + " // Then equation for distance is:\n" + " // D(s, t) = | T(s, t) - P |^2\n" + " // This leads to minimization of quadratic function of two variables.\n" + " // The solution from is taken only if s is between 0 and 1, t is\n" + " // between 0 and 1 and t + s < 1, otherwise distance from segment is\n" + " // computed.\n" + " b3Float4 d1, d2, a;\n" + " float u, v, w, p, q, r;\n" + " float s, t, dist, dist2;\n" + " b3Float4 witness2;\n" + " b3MprVec3Sub2(&d1, B, x0);\n" + " b3MprVec3Sub2(&d2, C, x0);\n" + " b3MprVec3Sub2(&a, x0, P);\n" + " u = b3MprVec3Dot(&a, &a);\n" + " v = b3MprVec3Dot(&d1, &d1);\n" + " w = b3MprVec3Dot(&d2, &d2);\n" + " p = b3MprVec3Dot(&a, &d1);\n" + " q = b3MprVec3Dot(&a, &d2);\n" + " r = b3MprVec3Dot(&d1, &d2);\n" + " s = (q * r - w * p) / (w * v - r * r);\n" + " t = (-s * r - q) / w;\n" + " if ((b3MprIsZero(s) || s > 0.f)\n" + " && (b3MprEq(s, 1.f) || s < 1.f)\n" + " && (b3MprIsZero(t) || t > 0.f)\n" + " && (b3MprEq(t, 1.f) || t < 1.f)\n" + " && (b3MprEq(t + s, 1.f) || t + s < 1.f)){\n" + " if (witness){\n" + " b3MprVec3Scale(&d1, s);\n" + " b3MprVec3Scale(&d2, t);\n" + " b3MprVec3Copy(witness, x0);\n" + " b3MprVec3Add(witness, &d1);\n" + " b3MprVec3Add(witness, &d2);\n" + " dist = b3MprVec3Dist2(witness, P);\n" + " }else{\n" + " dist = s * s * v;\n" + " dist += t * t * w;\n" + " dist += 2.f * s * t * r;\n" + " dist += 2.f * s * p;\n" + " dist += 2.f * t * q;\n" + " dist += u;\n" + " }\n" + " }else{\n" + " dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);\n" + " dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);\n" + " if (dist2 < dist){\n" + " dist = dist2;\n" + " if (witness)\n" + " b3MprVec3Copy(witness, &witness2);\n" + " }\n" + " dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);\n" + " if (dist2 < dist){\n" + " dist = dist2;\n" + " if (witness)\n" + " b3MprVec3Copy(witness, &witness2);\n" + " }\n" + " }\n" + " return dist;\n" + "}\n" + "B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" + " b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" + " b3ConstArray(b3Collidable_t) cpuCollidables,\n" + " b3ConstArray(b3Float4) cpuVertices,\n" + " __global b3Float4* sepAxis,\n" + " b3MprSimplex_t *portal,\n" + " float *depth, b3Float4 *pdir, b3Float4 *pos)\n" + "{\n" + " b3Float4 dir;\n" + " b3MprSupport_t v4;\n" + " unsigned long iterations;\n" + " b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" + " b3Float4* b3mpr_vec3_origin = &zero;\n" + " iterations = 1UL;\n" + " for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)\n" + " //while (1)\n" + " {\n" + " // compute portal direction and obtain next support point\n" + " b3PortalDir(portal, &dir);\n" + " \n" + " b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);\n" + " // reached tolerance -> find penetration info\n" + " if (portalReachTolerance(portal, &v4, &dir)\n" + " || iterations ==B3_MPR_MAX_ITERATIONS)\n" + " {\n" + " *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);\n" + " *depth = B3_MPR_SQRT(*depth);\n" + " \n" + " if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))\n" + " {\n" + " \n" + " *pdir = dir;\n" + " } \n" + " b3MprVec3Normalize(pdir);\n" + " \n" + " // barycentric coordinates:\n" + " b3FindPos(portal, pos);\n" + " return;\n" + " }\n" + " b3ExpandPortal(portal, &v4);\n" + " iterations++;\n" + " }\n" + "}\n" + "B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)\n" + "{\n" + " // Touching contact on portal's v1 - so depth is zero and direction\n" + " // is unimportant and pos can be guessed\n" + " *depth = 0.f;\n" + " b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" + " b3Float4* b3mpr_vec3_origin = &zero;\n" + " b3MprVec3Copy(dir, b3mpr_vec3_origin);\n" + " b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" + " b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" + " b3MprVec3Scale(pos, 0.5);\n" + "}\n" + "B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,\n" + " float *depth, b3Float4 *dir, b3Float4 *pos)\n" + "{\n" + " \n" + " // Origin lies on v0-v1 segment.\n" + " // Depth is distance to v1, direction also and position must be\n" + " // computed\n" + " b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" + " b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" + " b3MprVec3Scale(pos, 0.5f);\n" + " \n" + " b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);\n" + " *depth = B3_MPR_SQRT(b3MprVec3Len2(dir));\n" + " b3MprVec3Normalize(dir);\n" + "}\n" + "inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,\n" + " b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,\n" + " b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" + " b3ConstArray(b3Collidable_t) cpuCollidables,\n" + " b3ConstArray(b3Float4) cpuVertices,\n" + " __global b3Float4* sepAxis,\n" + " __global int* hasSepAxis,\n" + " float *depthOut, b3Float4* dirOut, b3Float4* posOut)\n" + "{\n" + " \n" + " b3MprSimplex_t portal;\n" + " \n" + "// if (!hasSepAxis[pairIndex])\n" + " // return -1;\n" + " \n" + " hasSepAxis[pairIndex] = 0;\n" + " int res;\n" + " // Phase 1: Portal discovery\n" + " res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);\n" + " \n" + " \n" + " //sepAxis[pairIndex] = *pdir;//or -dir?\n" + " switch (res)\n" + " {\n" + " case 0:\n" + " {\n" + " // Phase 2: Portal refinement\n" + " \n" + " res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);\n" + " if (res < 0)\n" + " return -1;\n" + " // Phase 3. Penetration info\n" + " b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);\n" + " hasSepAxis[pairIndex] = 1;\n" + " sepAxis[pairIndex] = -*dirOut;\n" + " break;\n" + " }\n" + " case 1:\n" + " {\n" + " // Touching contact on portal's v1.\n" + " b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);\n" + " break;\n" + " }\n" + " case 2:\n" + " {\n" + " \n" + " b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);\n" + " break;\n" + " }\n" + " default:\n" + " {\n" + " hasSepAxis[pairIndex]=0;\n" + " //if (res < 0)\n" + " //{\n" + " // Origin isn't inside portal - no collision.\n" + " return -1;\n" + " //}\n" + " }\n" + " };\n" + " \n" + " return 0;\n" + "};\n" + "#endif //B3_MPR_PENETRATION_H\n" + "#ifndef B3_CONTACT4DATA_H\n" + "#define B3_CONTACT4DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "typedef struct b3Contact4Data b3Contact4Data_t;\n" + "struct b3Contact4Data\n" + "{\n" + " b3Float4 m_worldPosB[4];\n" + "// b3Float4 m_localPosA[4];\n" + "// b3Float4 m_localPosB[4];\n" + " b3Float4 m_worldNormalOnB; // w: m_nPoints\n" + " unsigned short m_restituitionCoeffCmp;\n" + " unsigned short m_frictionCoeffCmp;\n" + " int m_batchIdx;\n" + " int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" + " int m_bodyBPtrAndSignBit;\n" + " int m_childIndexA;\n" + " int m_childIndexB;\n" + " int m_unused1;\n" + " int m_unused2;\n" + "};\n" + "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" + "{\n" + " return (int)contact->m_worldNormalOnB.w;\n" + "};\n" + "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" + "{\n" + " contact->m_worldNormalOnB.w = (float)numPoints;\n" + "};\n" + "#endif //B3_CONTACT4DATA_H\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n" + "#ifdef cl_ext_atomic_counters_32\n" + " #pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + " #define counter32_t volatile __global int*\n" + "#endif\n" + "__kernel void mprPenetrationKernel( __global int4* pairs,\n" + " __global const b3RigidBodyData_t* rigidBodies, \n" + " __global const b3Collidable_t* collidables,\n" + " __global const b3ConvexPolyhedronData_t* convexShapes, \n" + " __global const float4* vertices,\n" + " __global float4* separatingNormals,\n" + " __global int* hasSeparatingAxis,\n" + " __global struct b3Contact4Data* restrict globalContactsOut,\n" + " counter32_t nGlobalContactsOut,\n" + " int contactCapacity,\n" + " int numPairs)\n" + "{\n" + " int i = get_global_id(0);\n" + " int pairIndex = i;\n" + " if (i<numPairs)\n" + " {\n" + " int bodyIndexA = pairs[i].x;\n" + " int bodyIndexB = pairs[i].y;\n" + " int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" + " int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" + " \n" + " int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" + " int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" + " \n" + " \n" + " //once the broadphase avoids static-static pairs, we can remove this test\n" + " if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))\n" + " {\n" + " return;\n" + " }\n" + " \n" + " if ((collidables[collidableIndexA].m_shapeType!=SHAPE_CONVEX_HULL) ||(collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL))\n" + " {\n" + " return;\n" + " }\n" + " float depthOut;\n" + " b3Float4 dirOut;\n" + " b3Float4 posOut;\n" + " int res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB,rigidBodies,convexShapes,collidables,vertices,separatingNormals,hasSeparatingAxis,&depthOut, &dirOut, &posOut);\n" + " \n" + " \n" + " \n" + " \n" + " if (res==0)\n" + " {\n" + " //add a contact\n" + " int dstIdx;\n" + " AppendInc( nGlobalContactsOut, dstIdx );\n" + " if (dstIdx<contactCapacity)\n" + " {\n" + " pairs[pairIndex].z = dstIdx;\n" + " __global struct b3Contact4Data* c = globalContactsOut + dstIdx;\n" + " c->m_worldNormalOnB = -dirOut;//normal;\n" + " c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" + " c->m_batchIdx = pairIndex;\n" + " int bodyA = pairs[pairIndex].x;\n" + " int bodyB = pairs[pairIndex].y;\n" + " c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;\n" + " c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;\n" + " c->m_childIndexA = -1;\n" + " c->m_childIndexB = -1;\n" + " //for (int i=0;i<nContacts;i++)\n" + " posOut.w = -depthOut;\n" + " c->m_worldPosB[0] = posOut;//localPoints[contactIdx[i]];\n" + " GET_NPOINTS(*c) = 1;//nContacts;\n" + " }\n" + " }\n" + " }\n" + "}\n" + "typedef float4 Quaternion;\n" + "#define make_float4 (float4)\n" + "__inline\n" + "float dot3F4(float4 a, float4 b)\n" + "{\n" + " float4 a1 = make_float4(a.xyz,0.f);\n" + " float4 b1 = make_float4(b.xyz,0.f);\n" + " return dot(a1, b1);\n" + "}\n" + "__inline\n" + "float4 cross3(float4 a, float4 b)\n" + "{\n" + " return cross(a,b);\n" + "}\n" + "__inline\n" + "Quaternion qtMul(Quaternion a, Quaternion b)\n" + "{\n" + " Quaternion ans;\n" + " ans = cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "__inline\n" + "Quaternion qtInvert(Quaternion q)\n" + "{\n" + " return (Quaternion)(-q.xyz, q.w);\n" + "}\n" + "__inline\n" + "float4 qtRotate(Quaternion q, float4 vec)\n" + "{\n" + " Quaternion qInv = qtInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = qtMul(qtMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "__inline\n" + "float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" + "{\n" + " return qtRotate( *orientation, *p ) + (*translation);\n" + "}\n" + "__inline\n" + "float4 qtInvRotate(const Quaternion q, float4 vec)\n" + "{\n" + " return qtRotate( qtInvert( q ), vec );\n" + "}\n" + "inline void project(__global const b3ConvexPolyhedronData_t* hull, const float4 pos, const float4 orn, \n" + "const float4* dir, __global const float4* vertices, float* min, float* max)\n" + "{\n" + " min[0] = FLT_MAX;\n" + " max[0] = -FLT_MAX;\n" + " int numVerts = hull->m_numVertices;\n" + " const float4 localDir = qtInvRotate(orn,*dir);\n" + " float offset = dot(pos,*dir);\n" + " for(int i=0;i<numVerts;i++)\n" + " {\n" + " float dp = dot(vertices[hull->m_vertexOffset+i],localDir);\n" + " if(dp < min[0]) \n" + " min[0] = dp;\n" + " if(dp > max[0]) \n" + " max[0] = dp;\n" + " }\n" + " if(min[0]>max[0])\n" + " {\n" + " float tmp = min[0];\n" + " min[0] = max[0];\n" + " max[0] = tmp;\n" + " }\n" + " min[0] += offset;\n" + " max[0] += offset;\n" + "}\n" + "bool findSeparatingAxisUnitSphere( __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, \n" + " const float4 posA1,\n" + " const float4 ornA,\n" + " const float4 posB1,\n" + " const float4 ornB,\n" + " const float4 DeltaC2,\n" + " __global const float4* vertices,\n" + " __global const float4* unitSphereDirections,\n" + " int numUnitSphereDirections,\n" + " float4* sep,\n" + " float* dmin)\n" + "{\n" + " \n" + " float4 posA = posA1;\n" + " posA.w = 0.f;\n" + " float4 posB = posB1;\n" + " posB.w = 0.f;\n" + " int curPlaneTests=0;\n" + " int curEdgeEdge = 0;\n" + " // Test unit sphere directions\n" + " for (int i=0;i<numUnitSphereDirections;i++)\n" + " {\n" + " float4 crossje;\n" + " crossje = unitSphereDirections[i]; \n" + " if (dot3F4(DeltaC2,crossje)>0)\n" + " crossje *= -1.f;\n" + " {\n" + " float dist;\n" + " bool result = true;\n" + " float Min0,Max0;\n" + " float Min1,Max1;\n" + " project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0);\n" + " project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1);\n" + " \n" + " if(Max0<Min1 || Max1<Min0)\n" + " return false;\n" + " \n" + " float d0 = Max0 - Min1;\n" + " float d1 = Max1 - Min0;\n" + " dist = d0<d1 ? d0:d1;\n" + " result = true;\n" + " \n" + " if(dist<*dmin)\n" + " {\n" + " *dmin = dist;\n" + " *sep = crossje;\n" + " }\n" + " }\n" + " }\n" + " \n" + " if((dot3F4(-DeltaC2,*sep))>0.0f)\n" + " {\n" + " *sep = -(*sep);\n" + " }\n" + " return true;\n" + "}\n" + "__kernel void findSeparatingAxisUnitSphereKernel( __global const int4* pairs, \n" + " __global const b3RigidBodyData_t* rigidBodies, \n" + " __global const b3Collidable_t* collidables,\n" + " __global const b3ConvexPolyhedronData_t* convexShapes, \n" + " __global const float4* vertices,\n" + " __global const float4* unitSphereDirections,\n" + " __global float4* separatingNormals,\n" + " __global int* hasSeparatingAxis,\n" + " __global float* dmins,\n" + " int numUnitSphereDirections,\n" + " int numPairs\n" + " )\n" + "{\n" + " int i = get_global_id(0);\n" + " \n" + " if (i<numPairs)\n" + " {\n" + " if (hasSeparatingAxis[i])\n" + " {\n" + " \n" + " int bodyIndexA = pairs[i].x;\n" + " int bodyIndexB = pairs[i].y;\n" + " \n" + " int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" + " int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" + " \n" + " int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" + " int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" + " \n" + " \n" + " int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" + " \n" + " float dmin = dmins[i];\n" + " \n" + " float4 posA = rigidBodies[bodyIndexA].m_pos;\n" + " posA.w = 0.f;\n" + " float4 posB = rigidBodies[bodyIndexB].m_pos;\n" + " posB.w = 0.f;\n" + " float4 c0local = convexShapes[shapeIndexA].m_localCenter;\n" + " float4 ornA = rigidBodies[bodyIndexA].m_quat;\n" + " float4 c0 = transform(&c0local, &posA, &ornA);\n" + " float4 c1local = convexShapes[shapeIndexB].m_localCenter;\n" + " float4 ornB =rigidBodies[bodyIndexB].m_quat;\n" + " float4 c1 = transform(&c1local,&posB,&ornB);\n" + " const float4 DeltaC2 = c0 - c1;\n" + " float4 sepNormal = separatingNormals[i];\n" + " \n" + " int numEdgeEdgeDirections = convexShapes[shapeIndexA].m_numUniqueEdges*convexShapes[shapeIndexB].m_numUniqueEdges;\n" + " if (numEdgeEdgeDirections>numUnitSphereDirections)\n" + " {\n" + " bool sepEE = findSeparatingAxisUnitSphere( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,\n" + " posB,ornB,\n" + " DeltaC2,\n" + " vertices,unitSphereDirections,numUnitSphereDirections,&sepNormal,&dmin);\n" + " if (!sepEE)\n" + " {\n" + " hasSeparatingAxis[i] = 0;\n" + " } else\n" + " {\n" + " hasSeparatingAxis[i] = 1;\n" + " separatingNormals[i] = sepNormal;\n" + " }\n" + " }\n" + " } //if (hasSeparatingAxis[i])\n" + " }//(i<numPairs)\n" + "}\n"; |