summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h')
-rw-r--r--thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h219
1 files changed, 101 insertions, 118 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
index 8009e7d6e0..0d9b13f1d6 100644
--- a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
+++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
@@ -1,7 +1,6 @@
#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"
@@ -10,38 +9,36 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
-
-inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t)
+inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
{
- return b3MakeFloat4( a.x + (b.x - a.x) * 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 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];
+ //double-check next test
+ // if (numVertsIn < 2)
+ // return 0;
+
+ b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
b3Float4 endVertex = pVtxIn[0];
-
- ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS;
-
+
+ ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
+
for (ve = 0; ve < numVertsIn; ve++)
{
- endVertex=pVtxIn[ve];
- de = b3Dot(planeNormalWS,endVertex)+planeEqWS;
- if (ds<0)
+ endVertex = pVtxIn[ve];
+ de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
+ if (ds < 0)
{
- if (de<0)
+ if (de < 0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut[numVertsOut++] = endVertex;
@@ -49,15 +46,15 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
else
{
// Start < 0, end >= 0, so output intersection
- ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+ ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
}
}
else
{
- if (de<0)
+ if (de < 0)
{
// Start >= 0, end < 0 so output intersection and end
- ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+ ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
ppVtxOut[numVertsOut++] = endVertex;
}
}
@@ -67,90 +64,81 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
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
- )
+__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 i = get_global_id(0);
//int pairIndex = i;
int i = pairIndex;
-
+
float minDist = -1e30f;
float maxDist = 0.02f;
-
-// if (i<numPairs)
+
+ // if (i<numPairs)
{
-
if (hasSeparatingAxis[i])
{
-
-// int bodyIndexA = pairs[i].x;
- // int bodyIndexB = pairs[i].y;
-
- int numLocalContactsOut = 0;
-
- int capacityWorldVertsB2 = vertexFaceCapacity;
-
- __global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2];
- __global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2];
-
-
- {
- __global b3Int4* clippingFaces = clippingFacesOut;
-
-
- int closestFaceA = clippingFaces[pairIndex].x;
- // int closestFaceB = clippingFaces[pairIndex].y;
- int numVertsInA = clippingFaces[pairIndex].z;
- int numVertsInB = clippingFaces[pairIndex].w;
-
- int numVertsOut = 0;
-
- if (closestFaceA>=0)
- {
-
-
-
- // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
-
- for(int e0=0;e0<numVertsInA;e0++)
- {
- const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0];
- const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)];
- const b3Float4 WorldEdge0 = aw - bw;
- b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
- b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1);
- b3Float4 worldA1 = aw;
- float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1);
- b3Float4 planeNormalWS = planeNormalWS1;
- float planeEqWS=planeEqWS1;
- numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut);
- __global b3Float4* tmp = pVtxOut;
- pVtxOut = pVtxIn;
- pVtxIn = tmp;
- numVertsInB = numVertsOut;
- numVertsOut = 0;
- }
-
- b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
- float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]);
-
- for (int i=0;i<numVertsInB;i++)
- {
- float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS;
- if (depth <=minDist)
- {
- depth = minDist;
- }
-/*
+ // int bodyIndexA = pairs[i].x;
+ // int bodyIndexB = pairs[i].y;
+
+ int numLocalContactsOut = 0;
+
+ int capacityWorldVertsB2 = vertexFaceCapacity;
+
+ __global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
+ __global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
+
+ {
+ __global b3Int4* clippingFaces = clippingFacesOut;
+
+ int closestFaceA = clippingFaces[pairIndex].x;
+ // int closestFaceB = clippingFaces[pairIndex].y;
+ int numVertsInA = clippingFaces[pairIndex].z;
+ int numVertsInB = clippingFaces[pairIndex].w;
+
+ int numVertsOut = 0;
+
+ if (closestFaceA >= 0)
+ {
+ // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+
+ for (int e0 = 0; e0 < numVertsInA; e0++)
+ {
+ const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
+ const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
+ const b3Float4 WorldEdge0 = aw - bw;
+ b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
+ b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
+ b3Float4 worldA1 = aw;
+ float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
+ b3Float4 planeNormalWS = planeNormalWS1;
+ float planeEqWS = planeEqWS1;
+ numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
+ __global b3Float4* tmp = pVtxOut;
+ pVtxOut = pVtxIn;
+ pVtxIn = tmp;
+ numVertsInB = numVertsOut;
+ numVertsOut = 0;
+ }
+
+ b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
+ float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
+
+ for (int i = 0; i < numVertsInB; i++)
+ {
+ float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
+ if (depth <= minDist)
+ {
+ depth = minDist;
+ }
+ /*
static float maxDepth = 0.f;
if (depth < maxDepth)
{
@@ -163,26 +151,21 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
}
*/
- if (depth <=maxDist)
- {
- b3Float4 pointInWorld = pVtxIn[i];
- pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
- }
- }
-
- }
- clippingFaces[pairIndex].w =numLocalContactsOut;
-
-
- }
-
- for (int i=0;i<numLocalContactsOut;i++)
- pVtxIn[i] = pVtxOut[i];
-
- }// if (hasSeparatingAxis[i])
- }// if (i<numPairs)
-
-}
+ if (depth <= maxDist)
+ {
+ b3Float4 pointInWorld = pVtxIn[i];
+ pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
+ }
+ }
+ }
+ clippingFaces[pairIndex].w = numLocalContactsOut;
+ }
+
+ for (int i = 0; i < numLocalContactsOut; i++)
+ pVtxIn[i] = pVtxOut[i];
-#endif //B3_CLIP_FACES_H
+ } // if (hasSeparatingAxis[i])
+ } // if (i<numPairs)
+}
+#endif //B3_CLIP_FACES_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-11 23:53:07 +0000