summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h')
-rw-r--r--thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h162
1 files changed, 77 insertions, 85 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
index 35b5197006..c108255b9f 100644
--- a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
+++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
@@ -3,95 +3,87 @@
inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
{
- if( nPoints == 0 )
- return 0;
-
- if (nPoints <=4)
- return nPoints;
-
-
- if (nPoints >64)
- nPoints = 64;
-
- b3Float4 center = b3MakeFloat4(0,0,0,0);
+ if (nPoints == 0)
+ return 0;
+
+ if (nPoints <= 4)
+ return nPoints;
+
+ if (nPoints > 64)
+ nPoints = 64;
+
+ b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
{
-
- for (int i=0;i<nPoints;i++)
+ for (int i = 0; i < nPoints; i++)
center += p[i];
center /= (float)nPoints;
}
-
-
-
+
// sample 4 directions
-
- b3Float4 aVector = p[0] - center;
- b3Float4 u = b3Cross3( nearNormal, aVector );
- b3Float4 v = b3Cross3( nearNormal, u );
- u = b3FastNormalized3( u );
- v = b3FastNormalized3( v );
-
-
- //keep point with deepest penetration
- float minW= FLT_MAX;
-
- int minIndex=-1;
-
- b3Float4 maxDots;
- maxDots.x = FLT_MIN;
- maxDots.y = FLT_MIN;
- maxDots.z = FLT_MIN;
- maxDots.w = FLT_MIN;
-
- // idx, distance
- for(int ie = 0; ie<nPoints; ie++ )
- {
- if (p[ie].w<minW)
- {
- minW = p[ie].w;
- minIndex=ie;
- }
- float f;
- b3Float4 r = p[ie]-center;
- f = b3Dot3F4( u, r );
- if (f<maxDots.x)
- {
- maxDots.x = f;
- contactIdx[0].x = ie;
- }
-
- f = b3Dot3F4( -u, r );
- if (f<maxDots.y)
- {
- maxDots.y = f;
- contactIdx[0].y = ie;
- }
-
-
- f = b3Dot3F4( v, r );
- if (f<maxDots.z)
- {
- maxDots.z = f;
- contactIdx[0].z = ie;
- }
-
- f = b3Dot3F4( -v, r );
- if (f<maxDots.w)
- {
- maxDots.w = f;
- contactIdx[0].w = ie;
- }
-
- }
-
- if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
- {
- //replace the first contact with minimum (todo: replace contact with least penetration)
- contactIdx[0].x = minIndex;
- }
-
- return 4;
-
+
+ b3Float4 aVector = p[0] - center;
+ b3Float4 u = b3Cross3(nearNormal, aVector);
+ b3Float4 v = b3Cross3(nearNormal, u);
+ u = b3FastNormalized3(u);
+ v = b3FastNormalized3(v);
+
+ //keep point with deepest penetration
+ float minW = FLT_MAX;
+
+ int minIndex = -1;
+
+ b3Float4 maxDots;
+ maxDots.x = FLT_MIN;
+ maxDots.y = FLT_MIN;
+ maxDots.z = FLT_MIN;
+ maxDots.w = FLT_MIN;
+
+ // idx, distance
+ for (int ie = 0; ie < nPoints; ie++)
+ {
+ if (p[ie].w < minW)
+ {
+ minW = p[ie].w;
+ minIndex = ie;
+ }
+ float f;
+ b3Float4 r = p[ie] - center;
+ f = b3Dot3F4(u, r);
+ if (f < maxDots.x)
+ {
+ maxDots.x = f;
+ contactIdx[0].x = ie;
+ }
+
+ f = b3Dot3F4(-u, r);
+ if (f < maxDots.y)
+ {
+ maxDots.y = f;
+ contactIdx[0].y = ie;
+ }
+
+ f = b3Dot3F4(v, r);
+ if (f < maxDots.z)
+ {
+ maxDots.z = f;
+ contactIdx[0].z = ie;
+ }
+
+ f = b3Dot3F4(-v, r);
+ if (f < maxDots.w)
+ {
+ maxDots.w = f;
+ contactIdx[0].w = ie;
+ }
+ }
+
+ if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+ {
+ //replace the first contact with minimum (todo: replace contact with least penetration)
+ contactIdx[0].x = minIndex;
+ }
+
+ return 4;
}
-#endif //B3_REDUCE_CONTACTS_H
+#endif //B3_REDUCE_CONTACTS_H