//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 \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 .\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;iv,\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 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 (im_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;im_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;im_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;i0)\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(Max00.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 (inumUnitSphereDirections)\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