diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h | 1445 |
1 files changed, 0 insertions, 1445 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h deleted file mode 100644 index 74959a931c..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h +++ /dev/null @@ -1,1445 +0,0 @@ -//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"; |