diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h new file mode 100644 index 000000000..50a785b07 --- /dev/null +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h @@ -0,0 +1,176 @@ +#ifndef B3_CLIP_FACES_H +#define B3_CLIP_FACES_H + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t) +{ + return b3MakeFloat4( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + //double-check next test + // if (numVertsIn < 2) + // return 0; + + b3Float4 firstVertex=pVtxIn[numVertsIn-1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + de = b3Dot(planeNormalWS,endVertex)+planeEqWS; + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global b3Int4* clippingFacesOut, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + __global b3Float4* worldVertsB2, + int vertexFaceCapacity, + int pairIndex + ) +{ +// int i = get_global_id(0); + //int pairIndex = i; + int i = pairIndex; + + float minDist = -1e30f; + float maxDist = 0.02f; + +// if (i=0) + { + + + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + + for(int e0=0;e0 b3VertexArray; #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h" @@ -3579,9 +3583,50 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* //clipFacesAndFindContacts - bool clipFacesAndFindContactsCPU = false; + if (clipFacesAndFindContactsCPU) { + + b3AlignedObjectArray clippingFacesOutCPU; + b3AlignedObjectArray worldVertsA1CPU; + b3AlignedObjectArray worldNormalsACPU; + b3AlignedObjectArray worldVertsB1CPU; + + clippingFacesOutGPU.copyToHost(clippingFacesOutCPU); + worldVertsA1GPU.copyToHost(worldVertsA1CPU); + worldNormalsAGPU.copyToHost(worldNormalsACPU); + worldVertsB1GPU.copyToHost(worldVertsB1CPU); + + + + b3AlignedObjectArrayconcaveHasSeparatingNormalsCPU; + m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU); + + b3AlignedObjectArray concaveSepNormalsHost; + m_concaveSepNormals.copyToHost(concaveSepNormalsHost); + + b3AlignedObjectArray worldVertsB2CPU; + worldVertsB2CPU.resize(worldVertsB2GPU.size()); + + + for (int i=0;i* { + B3_PROFILE("clipFacesAndFindContacts"); //nContacts = m_totalContactsOut.at(0); //int h = m_hasSeparatingNormals.at(0); @@ -3596,13 +3642,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_concaveSepNormals.getBufferCL()), b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()), - b3BufferInfoCL( contactOut->getBufferCL()), b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), b3BufferInfoCL( worldVertsA1GPU.getBufferCL()), b3BufferInfoCL( worldNormalsAGPU.getBufferCL()), b3BufferInfoCL( worldVertsB1GPU.getBufferCL()), - b3BufferInfoCL( worldVertsB2GPU.getBufferCL()), - b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + b3BufferInfoCL( worldVertsB2GPU.getBufferCL()) }; b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts,"m_clipFacesAndFindContacts"); launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); @@ -3749,13 +3793,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_sepNormals.getBufferCL()), b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), - b3BufferInfoCL( contactOut->getBufferCL()), b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), b3BufferInfoCL( worldVertsA1GPU.getBufferCL()), b3BufferInfoCL( worldNormalsAGPU.getBufferCL()), b3BufferInfoCL( worldVertsB1GPU.getBufferCL()), - b3BufferInfoCL( worldVertsB2GPU.getBufferCL()), - b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + b3BufferInfoCL( worldVertsB2GPU.getBufferCL()) }; b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts,"m_clipFacesAndFindContacts"); diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl index 98253c95c..5e14b7453 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl @@ -1671,13 +1671,11 @@ __kernel void findClippingFacesKernel( __global const int4* pairs, __kernel void clipFacesAndFindContactsKernel( __global const float4* separatingNormals, __global const int* hasSeparatingAxis, - __global struct b3Contact4Data* globalContactsOut, __global int4* clippingFacesOut, __global float4* worldVertsA1, __global float4* worldNormalsA1, __global float4* worldVertsB1, __global float4* worldVertsB2, - counter32_t nGlobalContactsOut, int vertexFaceCapacity, int numPairs, int debugMode diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h index cbf9bce24..e6fa5cbc6 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h @@ -1865,13 +1865,11 @@ static const char* satClipKernelsCL= \ "}\n" "__kernel void clipFacesAndFindContactsKernel( __global const float4* separatingNormals,\n" " __global const int* hasSeparatingAxis,\n" -" __global struct b3Contact4Data* globalContactsOut,\n" " __global int4* clippingFacesOut,\n" " __global float4* worldVertsA1,\n" " __global float4* worldNormalsA1,\n" " __global float4* worldVertsB1,\n" " __global float4* worldVertsB2,\n" -" counter32_t nGlobalContactsOut,\n" " int vertexFaceCapacity,\n" " int numPairs,\n" " int debugMode\n"