21 #ifndef BT_MPR_PENETRATION_H 22 #define BT_MPR_PENETRATION_H 41 m_maxGjkIterations(1000),
42 m_maximumDistanceSquared(1e30f),
43 m_gjkRelError2(1.0e-6)
60 #define BT_MPR_SQRT sqrtf 62 #define BT_MPR_SQRT sqrt 64 #define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) 65 #define BT_MPR_FABS fabs 67 #define BT_MPR_TOLERANCE 1E-6f 68 #define BT_MPR_MAX_ITERATIONS 1000 98 printf(
"portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->
ps[index].
v.
x(),portal->
ps[index].
v.
y(),portal->
ps[index].
v.
z(),
99 portal->
ps[index].
v1.
x(),portal->
ps[index].
v1.
y(),portal->
ps[index].
v1.
z(),
100 portal->
ps[index].
v2.
x(),portal->
ps[index].
v2.
y(),portal->
ps[index].
v2.
z());
159 return ab < FLT_EPSILON * b;
161 return ab < FLT_EPSILON * a;
168 return btMprEq((*a).x(), (*b).x())
170 &&
btMprEq((*a).z(), (*b).z());
183 template <
typename btConvexTemplate>
187 center->
v1 = a.getObjectCenterInWorld();
188 center->
v2 = b.getObjectCenterInWorld();
189 center->
v = center->
v1 - center->
v2;
269 float dv1, dv2, dv3, dv4;
270 float dot1, dot2, dot3;
322 template <
typename btConvexTemplate>
323 inline void btMprSupport(
const btConvexTemplate& a,
const btConvexTemplate& b,
327 btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis();
328 btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis();
330 btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
331 btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
333 supp->
v1 = a.getWorldTransform()(pInA);
334 supp->
v2 = b.getWorldTransform()(qInB);
335 supp->
v = supp->
v1 - supp->
v2;
339 template <
typename btConvexTemplate>
474 template <
typename btConvexTemplate>
521 float b[4],
sum, inv;
543 sum = b[0] + b[1] + b[2] + b[3];
558 sum = b[1] + b[2] + b[3];
565 for (i = 0; i < 4; i++){
576 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 582 #endif//MPR_AVERAGE_CONTACT_POSITIONS 628 }
else if (t > 1.f ||
btMprEq(t, 1.f)){
666 float u, v, w, p, q, r;
667 float s, t, dist, dist2;
687 s = (q * r - w * p) / div;
688 t = (-s * r - q) / w;
692 && (
btMprEq(s, 1.f) || s < 1.f)
694 && (
btMprEq(t, 1.f) || t < 1.f)
695 && (
btMprEq(t + s, 1.f) || t + s < 1.f)){
708 dist += 2.f * s * t * r;
734 template <
typename btConvexTemplate>
735 static void btFindPenetr(
const btConvexTemplate& a,
const btConvexTemplate& b,
742 unsigned long iterations;
760 || iterations ==BT_MPR_MAX_ITERATIONS)
795 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 811 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 817 #endif//MPR_AVERAGE_CONTACT_POSITIONS 827 template <
typename btConvexTemplate>
853 btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut);
886 template<
typename btConvexTemplate,
typename btMprDistanceTemplate>
896 distInfo->m_distance = -depth;
897 distInfo->m_pointOnB = pos;
898 distInfo->m_normalBtoA = -dir;
899 distInfo->m_pointOnA = pos-distInfo->m_distance*dir;
908 #endif //BT_MPR_PENETRATION_H static T sum(const btAlignedObjectArray< T > &items)
float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
void btMprVec3Set(btVector3 *v, float x, float y, float z)
const btMprSupport_t * btMprSimplexPoint(const btMprSimplex_t *s, int idx)
void btExpandPortal(btMprSimplex_t *portal, const btMprSupport_t *v4)
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
int btMprEq(float _a, float _b)
static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal, float *depth, btVector3 *pdir, btVector3 *pos)
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
void btMprSimplexSetSize(btMprSimplex_t *s, int size)
btVector3 v
Support point in minkowski sum.
void btMprVec3Normalize(btVector3 *d)
static void btFindPenetrSegment(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
btVector3 v1
Support point in obj1.
void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, const btVector3 &dir, btMprSupport_t *supp)
int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, float *depthOut, btVector3 *dirOut, btVector3 *posOut)
const btScalar & x() const
Return the x value.
float btMprVec3PointTriDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *B, const btVector3 *C, btVector3 *witness)
int btMprIsZero(float val)
int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
void btMprVec3Scale(btVector3 *d, float k)
#define BT_MPR_MAX_ITERATIONS
int portalReachTolerance(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
void btMprVec3Copy(btVector3 *v, const btVector3 *w)
btVector3 v2
Support point in obj2.
const btScalar & y() const
Return the y value.
int btMprSimplexSize(const btMprSimplex_t *s)
float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
void btMprVec3Add(btVector3 *v, const btVector3 *w)
btVector3 can be used to represent 3D points and vectors.
float _btMprVec3PointSegmentDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *b, btVector3 *witness)
static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
int last
index of last added point
btMprSupport_t * btMprSimplexPointW(btMprSimplex_t *s, int idx)
void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
float btMprVec3Len2(const btVector3 *v)
btMprCollisionDescription()
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
int portalEncapsulesOrigin(const btMprSimplex_t *portal, const btVector3 *dir)
virtual ~btMprCollisionDescription()
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
#define BT_MPR_FMIN(x, y)
btScalar m_maximumDistanceSquared
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
const btScalar & z() const
Return the z value.