21 #ifndef BT_MPR_PENETRATION_H 22 #define BT_MPR_PENETRATION_H 59 #define BT_MPR_SQRT sqrtf 61 #define BT_MPR_SQRT sqrt 63 #define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) 64 #define BT_MPR_FABS fabs 66 #define BT_MPR_TOLERANCE 1E-6f 67 #define BT_MPR_MAX_ITERATIONS 1000 97 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(),
98 portal->
ps[index].
v1.
x(), portal->
ps[index].
v1.
y(), portal->
ps[index].
v1.
z(),
99 portal->
ps[index].
v2.
x(), portal->
ps[index].
v2.
y(), portal->
ps[index].
v2.
z());
151 return ab < FLT_EPSILON * b;
155 return ab < FLT_EPSILON * a;
164 template <
typename btConvexTemplate>
167 center->
v1 = a.getObjectCenterInWorld();
168 center->
v2 = b.getObjectCenterInWorld();
169 center->
v = center->
v1 - center->
v2;
245 float dv1, dv2, dv3, dv4;
246 float dot1, dot2, dot3;
307 template <
typename btConvexTemplate>
308 inline void btMprSupport(
const btConvexTemplate &a,
const btConvexTemplate &b,
312 btVector3 seperatingAxisInA = dir * a.getWorldTransform().getBasis();
313 btVector3 seperatingAxisInB = -dir * b.getWorldTransform().getBasis();
315 btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
316 btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
318 supp->
v1 = a.getWorldTransform()(pInA);
319 supp->
v2 = b.getWorldTransform()(qInB);
320 supp->
v = supp->
v1 - supp->
v2;
323 template <
typename btConvexTemplate>
460 template <
typename btConvexTemplate>
504 float b[4],
sum, inv;
526 sum = b[0] + b[1] + b[2] + b[3];
542 sum = b[1] + b[2] + b[3];
549 for (i = 0; i < 4; i++)
561 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 567 #endif //MPR_AVERAGE_CONTACT_POSITIONS 615 else if (t > 1.f ||
btMprEq(t, 1.f))
657 float u, v, w, p, q, r;
658 float s, t, dist, dist2;
679 s = (q * r - w * p) / div;
680 t = (-s * r - q) / w;
699 dist += 2.f * s * t * r;
729 template <
typename btConvexTemplate>
730 static void btFindPenetr(
const btConvexTemplate &a,
const btConvexTemplate &b,
737 unsigned long iterations;
784 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 799 #ifdef MPR_AVERAGE_CONTACT_POSITIONS 805 #endif //MPR_AVERAGE_CONTACT_POSITIONS 812 template <
typename btConvexTemplate>
835 btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut);
865 template <
typename btConvexTemplate,
typename btMprDistanceTemplate>
874 distInfo->m_distance = -depth;
875 distInfo->m_pointOnB = pos;
876 distInfo->m_normalBtoA = -dir;
877 distInfo->m_pointOnA = pos - distInfo->m_distance * dir;
884 #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)