diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h')
-rw-r--r-- | thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h | 162 |
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 |