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)
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)
const btScalar & x() const
Return the x value. 
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)
const btScalar & y() const
Return the y value. 
const btScalar & z() const
Return the z value. 
void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
void btMprVec3Copy(btVector3 *v, const btVector3 *w)
btVector3 v2
Support point in obj2. 
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)