diff options
author | RĂ©mi Verschelde <rverschelde@gmail.com> | 2018-01-13 14:43:30 +0100 |
---|---|---|
committer | GitHub <noreply@github.com> | 2018-01-13 14:43:30 +0100 |
commit | a3ee252993e8200c856be3fe664937f9461ee268 (patch) | |
tree | af68e434545e20c538f896e28b73f2db7d626edd /thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h | |
parent | c01575b3125ce1828f0cacb3f9f00286136f373c (diff) | |
parent | e12c89e8c9896b2e5cdd70dbd2d2acb449ff4b94 (diff) |
Merge pull request #15664 from akien-mga/thirdparty
Bugfix updates to various thirdparty libraries
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h')
-rw-r--r-- | thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h | 97 |
1 files changed, 97 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h new file mode 100644 index 0000000000..35b5197006 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h @@ -0,0 +1,97 @@ +#ifndef B3_REDUCE_CONTACTS_H +#define B3_REDUCE_CONTACTS_H + +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); + { + + 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; + +} + +#endif //B3_REDUCE_CONTACTS_H |