text:
/***
 * ---------------------------------
 * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
 *
 *  This file was ported from mpr.c file, part of libccd.
 *  The Minkoski Portal Refinement implementation was ported
 *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
 *  The original MPR idea and implementation is by Gary Snethen
 *  in XenoCollide, see http://github.com/erwincoumans/xenocollide
 *
 *  Distributed under the OSI-approved BSD License (the "License");
 *  see <http://www.opensource.org/licenses/bsd-license.php>.
 *  This software is distributed WITHOUT ANY WARRANTY; without even the
 *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *  See the License for more information.
 */

///2014 Oct, Erwin Coumans, Use templates to avoid void* casts

#ifndef BT_MPR_PENETRATION_H
#define BT_MPR_PENETRATION_H

#define BT_DEBUG_MPR1

#include "LinearMath/btTransform.h"
#include "LinearMath/btAlignedObjectArray.h"

//#define MPR_AVERAGE_CONTACT_POSITIONS

struct btMprCollisionDescription
{
    btVector3	m_firstDir;
    int			m_maxGjkIterations;
    btScalar	m_maximumDistanceSquared;
    btScalar	m_gjkRelError2;

    btMprCollisionDescription()
    :	m_firstDir(0,1,0),
        m_maxGjkIterations(1000),
        m_maximumDistanceSquared(1e30f),
        m_gjkRelError2(1.0e-6)
    {
    }
    virtual ~btMprCollisionDescription()
    {
    }
};

struct btMprDistanceInfo
{
    btVector3	m_pointOnA;
    btVector3	m_pointOnB;
    btVector3	m_normalBtoA;
    btScalar	m_distance;
};

#ifdef __cplusplus
#define BT_MPR_SQRT sqrtf
#else
#define BT_MPR_SQRT sqrt
#endif
#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
#define BT_MPR_FABS fabs

#define BT_MPR_TOLERANCE 1E-6f
#define BT_MPR_MAX_ITERATIONS 1000

struct _btMprSupport_t
{
    btVector3 v;  //!< Support point in minkowski sum
    btVector3 v1; //!< Support point in obj1
    btVector3 v2; //!< Support point in obj2
};
typedef struct _btMprSupport_t btMprSupport_t;

struct _btMprSimplex_t
{
    btMprSupport_t ps[4];
    int last; //!< index of last added point
};
typedef struct _btMprSimplex_t btMprSimplex_t;

inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx)
{
    return &s->ps[idx];
}

inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
{
    s->last = size - 1;
}

#ifdef DEBUG_MPR
inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index)
{
    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(),
           portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(),
           portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z());
}
#endif //DEBUG_MPR

inline int btMprSimplexSize(const btMprSimplex_t *s)
{
    return s->last + 1;
}

inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx)
{
    // here is no check on boundaries
    return &s->ps[idx];
}

inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
{
    *d = *s;
}

inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
{
    btMprSupportCopy(s->ps + pos, a);
}

inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
{
    btMprSupport_t supp;

    btMprSupportCopy(&supp, &s->ps[pos1]);
    btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
    btMprSupportCopy(&s->ps[pos2], &supp);
}

inline int btMprIsZero(float val)
{
    return BT_MPR_FABS(val) < FLT_EPSILON;
}

inline int btMprEq(float _a, float _b)
{
    float ab;
    float a, b;

    ab = BT_MPR_FABS(_a - _b);
    if (BT_MPR_FABS(ab) < FLT_EPSILON)
        return 1;

    a = BT_MPR_FABS(_a);
    b = BT_MPR_FABS(_b);
    if (b > a){
        return ab < FLT_EPSILON * b;
    }else{
        return ab < FLT_EPSILON * a;
    }
}

inline int btMprVec3Eq(const btVector3* a, const btVector3 *b)
{
    return btMprEq((*a).x(), (*b).x())
            && btMprEq((*a).y(), (*b).y())
            && btMprEq((*a).z(), (*b).z());
}

template <typename btConvexTemplate>
inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center)
{

	center->v1 = a.getObjectCenterInWorld();
    center->v2 = b.getObjectCenterInWorld();
    center->v = center->v1 - center->v2;
}

inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
{
	v->setValue(x,y,z);
}

inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
{
	*v += *w;
}

inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
{
    *v = *w;
}

inline void btMprVec3Scale(btVector3 *d, float k)
{
    *d *= k;
}

inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
{
    float dot;

	dot = btDot(*a,*b);
    return dot;
}

inline float btMprVec3Len2(const btVector3 *v)
{
    return btMprVec3Dot(v, v);
}

inline void btMprVec3Normalize(btVector3 *d)
{
    float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
    btMprVec3Scale(d, k);
}

inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
{
	*d = btCross(*a,*b);

}

inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
{
	*d = *v - *w;
}

inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
{
    btVector3 v2v1, v3v1;

    btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
                       &btMprSimplexPoint(portal, 1)->v);
    btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
                       &btMprSimplexPoint(portal, 1)->v);
    btMprVec3Cross(dir, &v2v1, &v3v1);
    btMprVec3Normalize(dir);
}

inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
                                       const btVector3 *dir)
{
    float dot;
    dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
    return btMprIsZero(dot) || dot > 0.f;
}

inline int portalReachTolerance(const btMprSimplex_t *portal,
                                     const btMprSupport_t *v4,
                                     const btVector3 *dir)
{
    float dv1, dv2, dv3, dv4;
    float dot1, dot2, dot3;

    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}

    dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
    dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
    dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
    dv4 = btMprVec3Dot(&v4->v, dir);

    dot1 = dv4 - dv1;
    dot2 = dv4 - dv2;
    dot3 = dv4 - dv3;

    dot1 = BT_MPR_FMIN(dot1, dot2);
    dot1 = BT_MPR_FMIN(dot1, dot3);

    return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
}

inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
                                         const btMprSupport_t *v4,
                                         const btVector3 *dir)
{
    float dot;
    dot = btMprVec3Dot(&v4->v, dir);
    return btMprIsZero(dot) || dot > 0.f;
}

inline void btExpandPortal(btMprSimplex_t *portal,
                              const btMprSupport_t *v4)
{
    float dot;
    btVector3 v4v0;

    btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
    if (dot > 0.f){
        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
        if (dot > 0.f){
            btMprSimplexSet(portal, 1, v4);
        }else{
            btMprSimplexSet(portal, 3, v4);
        }
    }else{
        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
        if (dot > 0.f){
            btMprSimplexSet(portal, 2, v4);
        }else{
            btMprSimplexSet(portal, 1, v4);
        }
    }
}
template <typename btConvexTemplate>
inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b,
                         const btMprCollisionDescription& colDesc,
													const btVector3& dir, btMprSupport_t *supp)
{
	btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis();
	btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis();

	btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
	btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);

	supp->v1 = a.getWorldTransform()(pInA);
	supp->v2 = b.getWorldTransform()(qInB);
	supp->v = supp->v1 - supp->v2;
}

template <typename btConvexTemplate>
static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b,
                            const btMprCollisionDescription& colDesc,
													btMprSimplex_t *portal)
{
    btVector3 dir, va, vb;
    float dot;
    int cont;

    // vertex 0 is center of portal
    btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0));

    // vertex 0 is center of portal
    btMprSimplexSetSize(portal, 1);

	btVector3 zero = btVector3(0,0,0);
	btVector3* org = &zero;

    if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){
        // Portal's center lies on origin (0,0,0) => we know that objects
        // intersect but we would need to know penetration info.
        // So move center little bit...
        btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
        btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
    }

    // vertex 1 = support in direction of origin
    btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
    btMprVec3Scale(&dir, -1.f);
    btMprVec3Normalize(&dir);

    btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1));

    btMprSimplexSetSize(portal, 2);

    // test if origin isn't outside of v1
    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);

    if (btMprIsZero(dot) || dot < 0.f)
        return -1;

    // vertex 2
    btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
                       &btMprSimplexPoint(portal, 1)->v);
    if (btMprIsZero(btMprVec3Len2(&dir))){
        if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){
            // origin lies on v1
            return 1;
        }else{
            // origin lies on v0-v1 segment
            return 2;
        }
    }

    btMprVec3Normalize(&dir);
    btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2));

    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
    if (btMprIsZero(dot) || dot < 0.f)
        return -1;

    btMprSimplexSetSize(portal, 3);

    // vertex 3 direction
    btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
                     &btMprSimplexPoint(portal, 0)->v);
    btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
                     &btMprSimplexPoint(portal, 0)->v);
    btMprVec3Cross(&dir, &va, &vb);
    btMprVec3Normalize(&dir);

    // it is better to form portal faces to be oriented "outside" origin
    dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
    if (dot > 0.f){
        btMprSimplexSwap(portal, 1, 2);
        btMprVec3Scale(&dir, -1.f);
    }

    while (btMprSimplexSize(portal) < 4){
		 btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3));

        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
        if (btMprIsZero(dot) || dot < 0.f)
            return -1;

        cont = 0;

        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
        // continue
        btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
                          &btMprSimplexPoint(portal, 3)->v);
        dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
        if (dot < 0.f && !btMprIsZero(dot)){
            btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
            cont = 1;
        }

        if (!cont){
            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
            // continue
            btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
                              &btMprSimplexPoint(portal, 2)->v);
            dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
            if (dot < 0.f && !btMprIsZero(dot)){
                btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
                cont = 1;
            }
        }

        if (cont){
            btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
                             &btMprSimplexPoint(portal, 0)->v);
            btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
                             &btMprSimplexPoint(portal, 0)->v);
            btMprVec3Cross(&dir, &va, &vb);
            btMprVec3Normalize(&dir);
        }else{
            btMprSimplexSetSize(portal, 4);
        }
    }

    return 0;
}

template <typename btConvexTemplate>
static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc,
							btMprSimplex_t *portal)
{
    btVector3 dir;
    btMprSupport_t v4;

	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
    //while (1)
	{
        // compute direction outside the portal (from v0 throught v1,v2,v3
        // face)
        btPortalDir(portal, &dir);

        // test if origin is inside the portal
        if (portalEncapsulesOrigin(portal, &dir))
            return 0;

        // get next support point

		 btMprSupport(a,b,colDesc, dir, &v4);

        // test if v4 can expand portal to contain origin and if portal
        // expanding doesn't reach given tolerance
        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
                || portalReachTolerance(portal, &v4, &dir))
		{
            return -1;
        }

        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
        // difference (direction from v0).
        btExpandPortal(portal, &v4);
    }

    return -1;
}

static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
{

	btVector3 zero = btVector3(0,0,0);
	btVector3* origin = &zero;

    btVector3 dir;
    size_t i;
    float b[4], sum, inv;
    btVector3 vec, p1, p2;

    btPortalDir(portal, &dir);

    // use barycentric coordinates of tetrahedron to find origin
    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
                       &btMprSimplexPoint(portal, 2)->v);
    b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);

    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
                       &btMprSimplexPoint(portal, 2)->v);
    b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);

    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
                       &btMprSimplexPoint(portal, 1)->v);
    b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);

    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
                       &btMprSimplexPoint(portal, 1)->v);
    b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);

	sum = b[0] + b[1] + b[2] + b[3];

    if (btMprIsZero(sum) || sum < 0.f){
		b[0] = 0.f;

        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
                           &btMprSimplexPoint(portal, 3)->v);
        b[1] = btMprVec3Dot(&vec, &dir);
        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
                           &btMprSimplexPoint(portal, 1)->v);
        b[2] = btMprVec3Dot(&vec, &dir);
        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
                           &btMprSimplexPoint(portal, 2)->v);
        b[3] = btMprVec3Dot(&vec, &dir);

		sum = b[1] + b[2] + b[3];
	}

	inv = 1.f / sum;

    btMprVec3Copy(&p1, origin);
    btMprVec3Copy(&p2, origin);
    for (i = 0; i < 4; i++){
        btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
        btMprVec3Scale(&vec, b[i]);
        btMprVec3Add(&p1, &vec);

        btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
        btMprVec3Scale(&vec, b[i]);
        btMprVec3Add(&p2, &vec);
    }
    btMprVec3Scale(&p1, inv);
    btMprVec3Scale(&p2, inv);
#ifdef MPR_AVERAGE_CONTACT_POSITIONS
    btMprVec3Copy(pos, &p1);
    btMprVec3Add(pos, &p2);
    btMprVec3Scale(pos, 0.5);
#else
    btMprVec3Copy(pos, &p2);
#endif//MPR_AVERAGE_CONTACT_POSITIONS
}

inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
{
    btVector3 ab;
    btMprVec3Sub2(&ab, a, b);
    return btMprVec3Len2(&ab);
}

inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
                                                  const btVector3 *x0,
                                                  const btVector3 *b,
                                                  btVector3 *witness)
{
    // The computation comes from solving equation of segment:
    //      S(t) = x0 + t.d
    //          where - x0 is initial point of segment
    //                - d is direction of segment from x0 (|d| > 0)
    //                - t belongs to <0, 1> interval
    //
    // Than, distance from a segment to some point P can be expressed:
    //      D(t) = |x0 + t.d - P|^2
    //          which is distance from any point on segment. Minimization
    //          of this function brings distance from P to segment.
    // Minimization of D(t) leads to simple quadratic equation that's
    // solving is straightforward.
    //
    // Bonus of this method is witness point for free.

    float dist, t;
    btVector3 d, a;

    // direction of segment
    btMprVec3Sub2(&d, b, x0);

    // precompute vector from P to x0
    btMprVec3Sub2(&a, x0, P);

    t  = -1.f * btMprVec3Dot(&a, &d);
    t /= btMprVec3Len2(&d);

    if (t < 0.f || btMprIsZero(t)){
        dist = btMprVec3Dist2(x0, P);
        if (witness)
            btMprVec3Copy(witness, x0);
    }else if (t > 1.f || btMprEq(t, 1.f)){
        dist = btMprVec3Dist2(b, P);
        if (witness)
            btMprVec3Copy(witness, b);
    }else{
        if (witness){
            btMprVec3Copy(witness, &d);
            btMprVec3Scale(witness, t);
            btMprVec3Add(witness, x0);
            dist = btMprVec3Dist2(witness, P);
        }else{
            // recycling variables
            btMprVec3Scale(&d, t);
            btMprVec3Add(&d, &a);
            dist = btMprVec3Len2(&d);
        }
    }

    return dist;
}

inline float btMprVec3PointTriDist2(const btVector3 *P,
                                const btVector3 *x0, const btVector3 *B,
                                const btVector3 *C,
                                btVector3 *witness)
{
    // Computation comes from analytic expression for triangle (x0, B, C)
    //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
    // Then equation for distance is:
    //      D(s, t) = | T(s, t) - P |^2
    // This leads to minimization of quadratic function of two variables.
    // The solution from is taken only if s is between 0 and 1, t is
    // between 0 and 1 and t + s < 1, otherwise distance from segment is
    // computed.

    btVector3 d1, d2, a;
    float u, v, w, p, q, r;
    float s, t, dist, dist2;
    btVector3 witness2;

    btMprVec3Sub2(&d1, B, x0);
    btMprVec3Sub2(&d2, C, x0);
    btMprVec3Sub2(&a, x0, P);

    u = btMprVec3Dot(&a, &a);
    v = btMprVec3Dot(&d1, &d1);
    w = btMprVec3Dot(&d2, &d2);
    p = btMprVec3Dot(&a, &d1);
    q = btMprVec3Dot(&a, &d2);
    r = btMprVec3Dot(&d1, &d2);

	btScalar div = (w * v - r * r);
	if (btMprIsZero(div))
	{
		s=-1;
	} else
	{
		s = (q * r - w * p) / div;
		t = (-s * r - q) / w;
	}

    if ((btMprIsZero(s) || s > 0.f)
            && (btMprEq(s, 1.f) || s < 1.f)
            && (btMprIsZero(t) || t > 0.f)
            && (btMprEq(t, 1.f) || t < 1.f)
            && (btMprEq(t + s, 1.f) || t + s < 1.f)){

        if (witness){
            btMprVec3Scale(&d1, s);
            btMprVec3Scale(&d2, t);
            btMprVec3Copy(witness, x0);
            btMprVec3Add(witness, &d1);
            btMprVec3Add(witness, &d2);

            dist = btMprVec3Dist2(witness, P);
        }else{
            dist  = s * s * v;
            dist += t * t * w;
            dist += 2.f * s * t * r;
            dist += 2.f * s * p;
            dist += 2.f * t * q;
            dist += u;
        }
    }else{
        dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);

        dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
        if (dist2 < dist){
            dist = dist2;
            if (witness)
                btMprVec3Copy(witness, &witness2);
        }

        dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
        if (dist2 < dist){
            dist = dist2;
            if (witness)
                btMprVec3Copy(witness, &witness2);
        }
    }

    return dist;
}

template <typename btConvexTemplate>
static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b,
                         const btMprCollisionDescription& colDesc,
                         btMprSimplex_t *portal,
                         float *depth, btVector3 *pdir, btVector3 *pos)
{
    btVector3 dir;
    btMprSupport_t v4;
    unsigned long iterations;

	btVector3 zero = btVector3(0,0,0);
	btVector3* origin = &zero;

    iterations = 1UL;
	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
    //while (1)
	{
        // compute portal direction and obtain next support point
        btPortalDir(portal, &dir);

		 btMprSupport(a,b,colDesc, dir, &v4);

        // reached tolerance -> find penetration info
        if (portalReachTolerance(portal, &v4, &dir)
                || iterations ==BT_MPR_MAX_ITERATIONS)
		{
            *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir);
            *depth = BT_MPR_SQRT(*depth);

			if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
			{

				*pdir = dir;
			}
			btMprVec3Normalize(pdir);

            // barycentric coordinates:
            btFindPos(portal, pos);

            return;
        }

        btExpandPortal(portal, &v4);

        iterations++;
    }
}

static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos)
{
    // Touching contact on portal's v1 - so depth is zero and direction
    // is unimportant and pos can be guessed
    *depth = 0.f;
    btVector3 zero = btVector3(0,0,0);
	btVector3* origin = &zero;

	btMprVec3Copy(dir, origin);
#ifdef MPR_AVERAGE_CONTACT_POSITIONS
    btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
    btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
    btMprVec3Scale(pos, 0.5);
#else
     btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
#endif
}

static void btFindPenetrSegment(btMprSimplex_t *portal,
                              float *depth, btVector3 *dir, btVector3 *pos)
{

    // Origin lies on v0-v1 segment.
    // Depth is distance to v1, direction also and position must be
    // computed
#ifdef MPR_AVERAGE_CONTACT_POSITIONS
    btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
    btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
    btMprVec3Scale(pos, 0.5f);
#else
     btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
#endif//MPR_AVERAGE_CONTACT_POSITIONS

    btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
    *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
    btMprVec3Normalize(dir);

}

template <typename btConvexTemplate>
inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b,
                            const btMprCollisionDescription& colDesc,
					float *depthOut, btVector3* dirOut, btVector3* posOut)
{

	 btMprSimplex_t portal;

    // Phase 1: Portal discovery
    int result = btDiscoverPortal(a,b,colDesc, &portal);

	//sepAxis[pairIndex] = *pdir;//or -dir?

	switch (result)
	{
	case 0:
		{
			// Phase 2: Portal refinement

			result = btRefinePortal(a,b,colDesc, &portal);
			if (result < 0)
				return -1;

			// Phase 3. Penetration info
			btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut);

			break;
		}
	case 1:
		{
			 // Touching contact on portal's v1.
			btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
			result=0;
			break;
		}
	case 2:
		{

			btFindPenetrSegment( &portal, depthOut, dirOut, posOut);
			result=0;
			break;
		}
	default:
		{
			//if (res < 0)
			//{
				// Origin isn't inside portal - no collision.
				result = -1;
			//}
		}
	};

	return result;
};

template<typename btConvexTemplate, typename btMprDistanceTemplate>
inline int	btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const
                                    btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo)
{
	btVector3 dir,pos;
	float depth;

	int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos);
	if (res==0)
	{
		distInfo->m_distance = -depth;
		distInfo->m_pointOnB = pos;
		distInfo->m_normalBtoA = -dir;
		distInfo->m_pointOnA = pos-distInfo->m_distance*dir;
		return 0;
	}

	return -1;
}

#endif //BT_MPR_PENETRATION_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef GJK_COLLISION_DESCRIPTION_H
#define GJK_COLLISION_DESCRIPTION_H

#include "LinearMath/btVector3.h"

struct btGjkCollisionDescription
{
    btVector3	m_firstDir;
    int			m_maxGjkIterations;
    btScalar	m_maximumDistanceSquared;
    btScalar	m_gjkRelError2;
    btGjkCollisionDescription()
    :m_firstDir(0,1,0),
    m_maxGjkIterations(1000),
    m_maximumDistanceSquared(1e30f),
    m_gjkRelError2(1.0e-6)
    {
    }
    virtual ~btGjkCollisionDescription()
    {
    }
};

#endif //GJK_COLLISION_DESCRIPTION_H

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_RAYCAST_TRI_CALLBACK_H
#define BT_RAYCAST_TRI_CALLBACK_H

#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
#include "LinearMath/btTransform.h"
struct btBroadphaseProxy;
class btConvexShape;

class btTriangleRaycastCallback : public btTriangleCallback
{
public:
	//input
	btVector3 m_from;
	btVector3 m_to;

	//@BP Mod - allow backface filtering and unflipped normals
	enum EFlags
	{
		kF_None                           = 0,
		kF_FilterBackfaces                = 1 << 0,
		kF_KeepUnflippedNormal            = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
		                                            // SubSimplexConvexCastRaytest is the default, even if kF_None is set.
		kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm
		kF_UseGjkConvexCastRaytest        = 1 << 3,
		kF_Terminator                     = 0xFFFFFFFF
	};

	unsigned int m_flags;
	btScalar     m_hitFraction;

	btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags = 0);

	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);

	virtual btScalar reportHit(
	    const btVector3& hitNormalLocal,
	    btScalar         hitFraction,
	    int              partId,
	    int              triangleIndex) = 0;
};

class btTriangleConvexcastCallback : public btTriangleCallback
{
public:
	const btConvexShape* m_convexShape;
	btTransform          m_convexShapeFrom;
	btTransform          m_convexShapeTo;
	btTransform          m_triangleToWorld;
	btScalar             m_hitFraction;
	btScalar             m_triangleCollisionMargin;
	btScalar             m_allowedPenetration;

	btTriangleConvexcastCallback(
	    const btConvexShape* convexShape,
	    const btTransform&   convexShapeFrom,
	    const btTransform&   convexShapeTo,
	    const btTransform&   triangleToWorld,
	    const btScalar       triangleCollisionMargin);

	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);

	virtual btScalar reportHit(
	    const btVector3& hitNormalLocal,
	    const btVector3& hitPointLocal,
	    btScalar         hitFraction,
	    int              partId,
	    int              triangleIndex) = 0;
};

#endif //BT_RAYCAST_TRI_CALLBACK_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H

#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
#include "btGjkEpa3.h"
#include "btGjkCollisionDescription.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"

template <typename btConvexTemplate>
bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
                          const btGjkCollisionDescription& colDesc,
                          btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
{
    (void)v;

    //	const btScalar				radialmargin(btScalar(0.));

    btVector3	guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input?

    btGjkEpaSolver3::sResults	results;

    if(btGjkEpaSolver3_Penetration(a,b,guessVector,results))

    {
        //	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
        //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
        wWitnessOnA = results.witnesses[0];
        wWitnessOnB = results.witnesses[1];
        v = results.normal;
        return true;
    } else
    {
        if(btGjkEpaSolver3_Distance(a,b,guessVector,results))
        {
            wWitnessOnA = results.witnesses[0];
            wWitnessOnB = results.witnesses[1];
            v = results.normal;
            return false;
        }
    }
    return false;
}

template <typename btConvexTemplate, typename btGjkDistanceTemplate>
int	btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
{

    bool m_catchDegeneracies  = true;
    btScalar m_cachedSeparatingDistance = 0.f;

    btScalar distance=btScalar(0.);
    btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));

    btVector3 pointOnA,pointOnB;
    btTransform	localTransA = a.getWorldTransform();
    btTransform localTransB = b.getWorldTransform();

    btScalar marginA = a.getMargin();
    btScalar marginB = b.getMargin();

    int m_curIter = 0;
    int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN?
    btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;

    bool isValid = false;
    bool checkSimplex = false;
    bool checkPenetration = true;
    int m_degenerateSimplex = 0;

    int m_lastUsedMethod = -1;

    {
        btScalar squaredDistance = BT_LARGE_FLOAT;
        btScalar delta = btScalar(0.);

        btScalar margin = marginA + marginB;

        simplexSolver.reset();

        for ( ; ; )
            //while (true)
        {

            btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
            btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();

            btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
            btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);

            btVector3  pWorld = localTransA(pInA);
            btVector3  qWorld = localTransB(qInB);

            btVector3 w	= pWorld - qWorld;
            delta = m_cachedSeparatingAxis.dot(w);

            // potential exit, they don't overlap
            if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
            {
                m_degenerateSimplex = 10;
                checkSimplex=true;
                //checkPenetration = false;
                break;
            }

            //exit 0: the new point is already in the simplex, or we didn't come any closer
            if (simplexSolver.inSimplex(w))
            {
                m_degenerateSimplex = 1;
                checkSimplex = true;
                break;
            }
            // are we getting any closer ?
            btScalar f0 = squaredDistance - delta;
            btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;

            if (f0 <= f1)
            {
                if (f0 <= btScalar(0.))
                {
                    m_degenerateSimplex = 2;
                } else
                {
                    m_degenerateSimplex = 11;
                }
                checkSimplex = true;
                break;
            }

            //add current vertex to simplex
            simplexSolver.addVertex(w, pWorld, qWorld);
            btVector3 newCachedSeparatingAxis;

            //calculate the closest point to the origin (update vector v)
            if (!simplexSolver.closest(newCachedSeparatingAxis))
            {
                m_degenerateSimplex = 3;
                checkSimplex = true;
                break;
            }

            if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2)
            {
                m_cachedSeparatingAxis = newCachedSeparatingAxis;
                m_degenerateSimplex = 6;
                checkSimplex = true;
                break;
            }

            btScalar previousSquaredDistance = squaredDistance;
            squaredDistance = newCachedSeparatingAxis.length2();
#if 0
            ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
            if (squaredDistance>previousSquaredDistance)
            {
                m_degenerateSimplex = 7;
                squaredDistance = previousSquaredDistance;
                checkSimplex = false;
                break;
            }
#endif //

            //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);

            //are we getting any closer ?
            if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
            {
                //				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
                checkSimplex = true;
                m_degenerateSimplex = 12;

                break;
            }

            m_cachedSeparatingAxis = newCachedSeparatingAxis;

            //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
            if (m_curIter++ > gGjkMaxIter)
            {
#if defined(DEBUG) || defined (_DEBUG)

                printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
                printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
                       m_cachedSeparatingAxis.getX(),
                       m_cachedSeparatingAxis.getY(),
                       m_cachedSeparatingAxis.getZ(),
                       squaredDistance);
#endif

                break;

            }

            bool check = (!simplexSolver.fullSimplex());
            //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());

            if (!check)
            {
                //do we need this backup_closest here ?
                //				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
                m_degenerateSimplex = 13;
                break;
            }
        }

        if (checkSimplex)
        {
            simplexSolver.compute_points(pointOnA, pointOnB);
            normalInB = m_cachedSeparatingAxis;

            btScalar lenSqr =m_cachedSeparatingAxis.length2();

            //valid normal
            if (lenSqr < 0.0001)
            {
                m_degenerateSimplex = 5;
            }
            if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
            {
                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
                normalInB *= rlen; //normalize

                btScalar s = btSqrt(squaredDistance);

                btAssert(s > btScalar(0.0));
                pointOnA -= m_cachedSeparatingAxis * (marginA / s);
                pointOnB += m_cachedSeparatingAxis * (marginB / s);
                distance = ((btScalar(1.)/rlen) - margin);
                isValid = true;

                m_lastUsedMethod = 1;
            } else
            {
                m_lastUsedMethod = 2;
            }
        }

        bool catchDegeneratePenetrationCase =
        (m_catchDegeneracies &&  m_degenerateSimplex && ((distance+margin) < 0.01));

        //if (checkPenetration && !isValid)
        if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
        {
            //penetration case

            //if there is no way to handle penetrations, bail out

            // Penetration depth case.
            btVector3 tmpPointOnA,tmpPointOnB;

            m_cachedSeparatingAxis.setZero();

            bool isValid2 = btGjkEpaCalcPenDepth(a,b,
                                                 colDesc,
                                                 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);

            if (isValid2)
            {
                btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
                btScalar lenSqr = tmpNormalInB.length2();
                if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
                {
                    tmpNormalInB = m_cachedSeparatingAxis;
                    lenSqr = m_cachedSeparatingAxis.length2();
                }

                if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
                {
                    tmpNormalInB /= btSqrt(lenSqr);
                    btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
                    //only replace valid penetrations when the result is deeper (check)
                    if (!isValid || (distance2 < distance))
                    {
                        distance = distance2;
                        pointOnA = tmpPointOnA;
                        pointOnB = tmpPointOnB;
                        normalInB = tmpNormalInB;

                        isValid = true;
                        m_lastUsedMethod = 3;
                    } else
                    {
                        m_lastUsedMethod = 8;
                    }
                } else
                {
                    m_lastUsedMethod = 9;
                }
            } else

            {
                ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
                ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
                ///reports a valid positive distance. Use the results of the second GJK instead of failing.
                ///thanks to Jacob.Langford for the reproduction case
                ///http://code.google.com/p/bullet/issues/detail?id=250

                if (m_cachedSeparatingAxis.length2() > btScalar(0.))
                {
                    btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
                    //only replace valid distances when the distance is less
                    if (!isValid || (distance2 < distance))
                    {
                        distance = distance2;
                        pointOnA = tmpPointOnA;
                        pointOnB = tmpPointOnB;
                        pointOnA -= m_cachedSeparatingAxis * marginA ;
                        pointOnB += m_cachedSeparatingAxis * marginB ;
                        normalInB = m_cachedSeparatingAxis;
                        normalInB.normalize();

                        isValid = true;
                        m_lastUsedMethod = 6;
                    } else
                    {
                        m_lastUsedMethod = 5;
                    }
                }
            }
        }
    }

    if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared)))
    {

        m_cachedSeparatingAxis = normalInB;
        m_cachedSeparatingDistance = distance;
        distInfo->m_distance = distance;
        distInfo->m_normalBtoA = normalInB;
        distInfo->m_pointOnB = pointOnB;
        distInfo->m_pointOnA = pointOnB+normalInB*distance;
        return 0;
    }
    return -m_lastUsedMethod;
}

#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_MANIFOLD_CONTACT_POINT_H
#define BT_MANIFOLD_CONTACT_POINT_H

#include "LinearMath/btVector3.h"
#include "LinearMath/btTransformUtil.h"

#ifdef PFX_USE_FREE_VECTORMATH
#	include "physics_effects/base_level/solver/pfx_constraint_row.h"
typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow;
#else
/// Don't change following order of parameters
ATTRIBUTE_ALIGNED16(struct) btConstraintRow
{
	btScalar m_normal[3];
	btScalar m_rhs;
	btScalar m_jacDiagInv;
	btScalar m_lowerLimit;
	btScalar m_upperLimit;
	btScalar m_accumImpulse;
};
typedef btConstraintRow PfxConstraintRow;
#endif //PFX_USE_FREE_VECTORMATH

enum btContactPointFlags
{
	BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED = 1,
	BT_CONTACT_FLAG_HAS_CONTACT_CFM              = 2,
	BT_CONTACT_FLAG_HAS_CONTACT_ERP              = 4,
	BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING    = 8,
	BT_CONTACT_FLAG_FRICTION_ANCHOR              = 16,
};

/// ManifoldContactPoint collects and maintains persistent contactpoints.
/// used to improve stability and performance of rigidbody dynamics response.
class btManifoldPoint
{
public:
	btManifoldPoint()
	    : m_userPersistentData(0),
	      m_contactPointFlags(0),
	      m_appliedImpulse(0.f),
	      m_appliedImpulseLateral1(0.f),
	      m_appliedImpulseLateral2(0.f),
	      m_contactMotion1(0.f),
	      m_contactMotion2(0.f),
	      m_contactCFM(0.f),
	      m_contactERP(0.f),
	      m_frictionCFM(0.f),
	      m_lifeTime(0)
	{
	}

	btManifoldPoint(const btVector3& pointA, const btVector3& pointB, const btVector3& normal, btScalar distance)
	    : m_localPointA(pointA),
	      m_localPointB(pointB),
	      m_normalWorldOnB(normal),
	      m_distance1(distance),
	      m_combinedFriction(btScalar(0.)),
	      m_combinedRollingFriction(btScalar(0.)),
	      m_combinedSpinningFriction(btScalar(0.)),
	      m_combinedRestitution(btScalar(0.)),
	      m_userPersistentData(0),
	      m_contactPointFlags(0),
	      m_appliedImpulse(0.f),
	      m_appliedImpulseLateral1(0.f),
	      m_appliedImpulseLateral2(0.f),
	      m_contactMotion1(0.f),
	      m_contactMotion2(0.f),
	      m_contactCFM(0.f),
	      m_contactERP(0.f),
	      m_frictionCFM(0.f),
	      m_lifeTime(0)
	{
	}

	btVector3 m_localPointA;
	btVector3 m_localPointB;
	btVector3 m_positionWorldOnB;

	/// m_positionWorldOnA is redundant information, see getPositionWorldOnA(),
	/// but for clarity
	btVector3 m_positionWorldOnA;

	btVector3 m_normalWorldOnB;

	btScalar m_distance1;
	btScalar m_combinedFriction;
	btScalar m_combinedRollingFriction;  // torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever
	btScalar m_combinedSpinningFriction; // torsional friction around contact normal, useful for grasping objects
	btScalar m_combinedRestitution;

	// BP mod, store contact triangles.
	int m_partId0;
	int m_partId1;
	int m_index0;
	int m_index1;

	mutable void* m_userPersistentData;
	//bool m_lateralFrictionInitialized;
	int m_contactPointFlags;

	btScalar m_appliedImpulse;
	btScalar m_appliedImpulseLateral1;
	btScalar m_appliedImpulseLateral2;
	btScalar m_contactMotion1;
	btScalar m_contactMotion2;

	union {
		btScalar m_contactCFM;
		btScalar m_combinedContactStiffness1;
	};

	union {
		btScalar m_contactERP;
		btScalar m_combinedContactDamping1;
	};

	btScalar m_frictionCFM;

	/// lifetime of the contactpoint in frames
	int m_lifeTime;

	btVector3 m_lateralFrictionDir1;
	btVector3 m_lateralFrictionDir2;

	btScalar getDistance() const { return m_distance1; }
	int      getLifeTime() const { return m_lifeTime; }

	const btVector3& getPositionWorldOnA() const
	{
		return m_positionWorldOnA;
		//return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
	}

	const btVector3& getPositionWorldOnB() const { return m_positionWorldOnB; }
	void             setDistance(btScalar dist) { m_distance1 = dist; }

	/// this returns the most recent applied impulse, to satisfy contact
	/// constraints by the constraint solver
	btScalar getAppliedImpulse() const
	{
		return m_appliedImpulse;
	}
};

#endif //BT_MANIFOLD_CONTACT_POINT_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_CONVEX_CAST_H
#define BT_CONVEX_CAST_H

#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btScalar.h"
class btMinkowskiSumShape;
#include "LinearMath/btIDebugDraw.h"

/// btConvexCast is an interface for Casting
class btConvexCast
{
public:

	virtual ~btConvexCast();

	///RayResult stores the closest result
	/// alternatively, add a callback method to decide about closest/all results
	struct	CastResult
	{
		//virtual bool	addRayResult(const btVector3& normal,btScalar	fraction) = 0;

		virtual void	DebugDraw(btScalar	fraction) {(void)fraction;}
		virtual void	drawCoordSystem(const btTransform& trans) {(void)trans;}
		virtual void	reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;}
		CastResult()
			:m_fraction(btScalar(BT_LARGE_FLOAT)),
			m_debugDrawer(0),
			m_allowedPenetration(btScalar(0))
		{
		}

		virtual ~CastResult() {};

		btTransform	m_hitTransformA;
		btTransform	m_hitTransformB;
		btVector3	m_normal;
		btVector3   m_hitPoint;
		btScalar	m_fraction; //input and output
		btIDebugDraw* m_debugDrawer;
		btScalar	m_allowedPenetration;

	};

	/// cast a convex against another convex object
	virtual bool	calcTimeOfImpact(
					const btTransform& fromA,
					const btTransform& toA,
					const btTransform& fromB,
					const btTransform& toB,
					CastResult& result) = 0;
};

#endif //BT_CONVEX_CAST_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_GJK_CONVEX_CAST_H
#define BT_GJK_CONVEX_CAST_H

#include "BulletCollision/CollisionShapes/btCollisionMargin.h"

#include "LinearMath/btVector3.h"
#include "btConvexCast.h"
class btConvexShape;
class btMinkowskiSumShape;
#include "btSimplexSolverInterface.h"

///GjkConvexCast performs a raycast on a convex object using support mapping.
class btGjkConvexCast : public btConvexCast
{
	btSimplexSolverInterface*	m_simplexSolver;
	const btConvexShape*	m_convexA;
	const btConvexShape*	m_convexB;

public:

	btGjkConvexCast(const btConvexShape*	convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);

	/// cast a convex against another convex object
	virtual bool	calcTimeOfImpact(
					const btTransform& fromA,
					const btTransform& toA,
					const btTransform& fromB,
					const btTransform& toB,
					CastResult& result);

};

#endif //BT_GJK_CONVEX_CAST_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_POINT_COLLECTOR_H
#define BT_POINT_COLLECTOR_H

#include "btDiscreteCollisionDetectorInterface.h"

struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
{

	btVector3 m_normalOnBInWorld;
	btVector3 m_pointInWorld;
	btScalar	m_distance;//negative means penetration

	bool	m_hasResult;

	btPointCollector ()
		: m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false)
	{
	}

	virtual void setShapeIdentifiersA(int partId0,int index0)
	{
		(void)partId0;
		(void)index0;

	}
	virtual void setShapeIdentifiersB(int partId1,int index1)
	{
		(void)partId1;
		(void)index1;
	}

	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
	{
		if (depth< m_distance)
		{
			m_hasResult = true;
			m_normalOnBInWorld = normalOnBInWorld;
			m_pointInWorld = pointInWorld;
			//negative means penetration
			m_distance = depth;
		}
	}
};

#endif //BT_POINT_COLLECTOR_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_GJK_PAIR_DETECTOR_H
#define BT_GJK_PAIR_DETECTOR_H

#include "btDiscreteCollisionDetectorInterface.h"
#include "BulletCollision/CollisionShapes/btCollisionMargin.h"

class btConvexShape;
#include "btSimplexSolverInterface.h"
class btConvexPenetrationDepthSolver;

/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
{

	btVector3	m_cachedSeparatingAxis;
	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
	btSimplexSolverInterface* m_simplexSolver;
	const btConvexShape* m_minkowskiA;
	const btConvexShape* m_minkowskiB;
	int	m_shapeTypeA;
	int m_shapeTypeB;
	btScalar	m_marginA;
	btScalar	m_marginB;

	bool		m_ignoreMargin;
	btScalar	m_cachedSeparatingDistance;

public:

	//some debugging to fix degeneracy problems
	int			m_lastUsedMethod;
	int			m_curIter;
	int			m_degenerateSimplex;
	int			m_catchDegeneracies;
	int			m_fixContactNormalDirection;

	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
	virtual ~btGjkPairDetector() {};

	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);

	void	getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);

	void setMinkowskiA(const btConvexShape* minkA)
	{
		m_minkowskiA = minkA;
	}

	void setMinkowskiB(const btConvexShape* minkB)
	{
		m_minkowskiB = minkB;
	}
	void setCachedSeperatingAxis(const btVector3& seperatingAxis)
	{
		m_cachedSeparatingAxis = seperatingAxis;
	}

	const btVector3& getCachedSeparatingAxis() const
	{
		return m_cachedSeparatingAxis;
	}
	btScalar	getCachedSeparatingDistance() const
	{
		return m_cachedSeparatingDistance;
	}

	void	setPenetrationDepthSolver(btConvexPenetrationDepthSolver*	penetrationDepthSolver)
	{
		m_penetrationDepthSolver = penetrationDepthSolver;
	}

	///don't use setIgnoreMargin, it's for Bullet's internal use
	void	setIgnoreMargin(bool ignoreMargin)
	{
		m_ignoreMargin = ignoreMargin;
	}

};

#endif //BT_GJK_PAIR_DETECTOR_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_PERSISTENT_MANIFOLD_H
#define BT_PERSISTENT_MANIFOLD_H

#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
#include "btManifoldPoint.h"
class btCollisionObject;
#include "LinearMath/btAlignedAllocator.h"

struct btCollisionResult;

///maximum contact breaking and merging threshold
extern btScalar gContactBreakingThreshold;

#ifndef SWIG
class btPersistentManifold;

typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp, void* body0, void* body1);
typedef void (*ContactStartedCallback)(btPersistentManifold* const& manifold);
typedef void (*ContactEndedCallback)(btPersistentManifold* const& manifold);
extern ContactDestroyedCallback gContactDestroyedCallback;
extern ContactProcessedCallback gContactProcessedCallback;
extern ContactStartedCallback   gContactStartedCallback;
extern ContactEndedCallback     gContactEndedCallback;
#endif //SWIG

/// the enum starts at 1024 to avoid type conflicts with btTypedConstraint
enum btContactManifoldTypes
{
	MIN_CONTACT_MANIFOLD_TYPE = 1024,
	BT_PERSISTENT_MANIFOLD_TYPE
};

#define MANIFOLD_CACHE_SIZE 4

/// btPersistentManifold is a contact point cache, it stays persistent as long
/// as objects are overlapping in the broadphase. Those contact points are
/// created by the collision narrow phase. The cache can be empty, or hold
/// 1, 2, 3 or 4 points. Some collision algorithms (GJK) might only add one
/// point at a time. updates/refreshes old contact points, and throw them away
/// if necessary (distance becomes too large) reduces the cache to 4 points,
/// when more then 4 points are added, using following rules: the contact point
/// with deepest penetration is always kept, and it tries to maximuze the area
/// covered by the points. note that some pairs of objects might have more then
/// one contact manifold.
ATTRIBUTE_ALIGNED16(class) btPersistentManifold : public btTypedObject
{
	btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];

	// these two body pointers can point to the physics rigidbody class.
	const btCollisionObject* m_body0;
	const btCollisionObject* m_body1;

	int m_cachedPoints;

	btScalar m_contactBreakingThreshold;
	btScalar m_contactProcessingThreshold;

	/// sort cached points so most isolated points come first
	int sortCachedPoints(const btManifoldPoint& pt);

	int findContactPoint(const btManifoldPoint* unUsed, int numUnused, const btManifoldPoint& pt);

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	int m_companionIdA;
	int m_companionIdB;
	int m_index1a;

	btPersistentManifold();

	btPersistentManifold(
	    const btCollisionObject* body0,
	    const btCollisionObject* body1,
	    int,
	    btScalar contactBreakingThreshold,
	    btScalar contactProcessingThreshold)
	    : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
	      m_body0(body0),
	      m_body1(body1),
	      m_cachedPoints(0),
	      m_contactBreakingThreshold(contactBreakingThreshold),
	      m_contactProcessingThreshold(contactProcessingThreshold)
	{
	}

	SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0; }
	SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1; }

	void setBodies(const btCollisionObject* body0, const btCollisionObject* body1)
	{
		m_body0 = body0;
		m_body1 = body1;
	}

	void clearUserCache(btManifoldPoint & pt);

#ifdef DEBUG_PERSISTENCY
	void DebugPersistency();
#endif

	SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints; }

	/// the setNumContacts API is usually not used, except when you gather/fill
	/// all contacts manually
	void setNumContacts(int cachedPoints) { m_cachedPoints = cachedPoints; }

	SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
	{
		btAssert(index < m_cachedPoints);
		return m_pointCache[index];
	}

	SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
	{
		btAssert(index < m_cachedPoints);
		return m_pointCache[index];
	}

	// @todo: get this margin from the current physics / collision environment
	btScalar getContactBreakingThreshold() const;

	btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; }
	void     setContactBreakingThreshold(btScalar contactBreakingThreshold) { m_contactBreakingThreshold = contactBreakingThreshold; }
	void     setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; }
	int      getCacheEntry(const btManifoldPoint& newPoint) const;
	int      addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive = false);

	void removeContactPoint(int index)
	{
		clearUserCache(m_pointCache[index]);

		int lastUsedIndex = getNumContacts() - 1;
		//m_pointCache[index] = m_pointCache[lastUsedIndex];
		if (index != lastUsedIndex)
		{
			m_pointCache[index] = m_pointCache[lastUsedIndex];

			// get rid of duplicated userPersistentData pointer
			m_pointCache[lastUsedIndex].m_userPersistentData     = 0;
			m_pointCache[lastUsedIndex].m_appliedImpulse         = 0.f;
			m_pointCache[lastUsedIndex].m_contactPointFlags      = 0;
			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
			m_pointCache[lastUsedIndex].m_lifeTime               = 0;
		}

		btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0);
		m_cachedPoints--;

		if (gContactEndedCallback && m_cachedPoints == 0)
		{
			gContactEndedCallback(this);
		}
	}
	void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex)
	{
		btAssert(validContactDistance(newPoint));

#define MAINTAIN_PERSISTENCY 1
#ifdef MAINTAIN_PERSISTENCY
		int      lifeTime               = m_pointCache[insertIndex].getLifeTime();
		btScalar appliedImpulse         = m_pointCache[insertIndex].m_appliedImpulse;
		btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
		btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;

		bool replacePoint = true;

		// we keep existing contact points for friction anchors
		// if the friction force is within the Coulomb friction cone
		if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
		{
			//printf("appliedImpulse=%f\n", appliedImpulse);
			//printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1);
			//printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2);
			//printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction);
			btScalar mu  = m_pointCache[insertIndex].m_combinedFriction;
			btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7
			btScalar a   = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2;
			btScalar b   = eps + mu * appliedImpulse;
			b            = b * b;
			replacePoint = (a) > (b);
		}

		if (replacePoint)
		{
			btAssert(lifeTime >= 0);
			void* cache = m_pointCache[insertIndex].m_userPersistentData;

			m_pointCache[insertIndex]                          = newPoint;
			m_pointCache[insertIndex].m_userPersistentData     = cache;
			m_pointCache[insertIndex].m_appliedImpulse         = appliedImpulse;
			m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
			m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
		}

		m_pointCache[insertIndex].m_lifeTime = lifeTime;
#else
		clearUserCache(m_pointCache[insertIndex]);
		m_pointCache[insertIndex] = newPoint;
#endif
	}

	bool validContactDistance(const btManifoldPoint& pt) const
	{
		return pt.m_distance1 <= getContactBreakingThreshold();
	}

	/// calculated new worldspace coordinates and depth, and reject points that
	/// exceed the collision margin
	void refreshContactPoints(const btTransform& trA, const btTransform& trB);

	SIMD_FORCE_INLINE void clearManifold()
	{
		for (int i = 0; i < m_cachedPoints; i++)
		{
			clearUserCache(m_pointCache[i]);
		}

		if (gContactEndedCallback && m_cachedPoints)
		{
			gContactEndedCallback(this);
		}
		m_cachedPoints = 0;
	}
};

#endif //BT_PERSISTENT_MANIFOLD_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the
use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software in a
product, an acknowledgment in the product documentation would be appreciated
but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

/*
GJK-EPA collision solver by Nathanael Presson, 2008
*/
#ifndef BT_GJK_EPA2_H
#define BT_GJK_EPA2_H

#include "BulletCollision/CollisionShapes/btConvexShape.h"

///btGjkEpaSolver contributed under zlib by Nathanael Presson
struct	btGjkEpaSolver2
{
struct	sResults
	{
	enum eStatus
		{
		Separated,		/* Shapes doesnt penetrate												*/
		Penetrating,	/* Shapes are penetrating												*/
		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/
		EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/
		}		status;
	btVector3	witnesses[2];
	btVector3	normal;
	btScalar	distance;
	};

static int		StackSizeRequirement();

static bool		Distance(	const btConvexShape* shape0,const btTransform& wtrs0,
							const btConvexShape* shape1,const btTransform& wtrs1,
							const btVector3& guess,
							sResults& results);

static bool		Penetration(const btConvexShape* shape0,const btTransform& wtrs0,
							const btConvexShape* shape1,const btTransform& wtrs1,
							const btVector3& guess,
							sResults& results,
							bool usemargins=true);
#ifndef __SPU__
static btScalar	SignedDistance(	const btVector3& position,
								btScalar margin,
								const btConvexShape* shape,
								const btTransform& wtrs,
								sResults& results);

static bool		SignedDistance(	const btConvexShape* shape0,const btTransform& wtrs0,
								const btConvexShape* shape1,const btTransform& wtrs1,
								const btVector3& guess,
								sResults& results);
#endif //__SPU__

};

#endif //BT_GJK_EPA2_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

EPA Copyright (c) Ricardo Padrela 2006

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
#define BT_GJP_EPA_PENETRATION_DEPTH_H

#include "btConvexPenetrationDepthSolver.h"

///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
///calculate the penetration depth between two convex shapes.
class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{
	public :

		btGjkEpaPenetrationDepthSolver()
		{
		}

		bool			calcPenDepth( btSimplexSolverInterface& simplexSolver,
									  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
									  const btTransform& transformA, const btTransform& transformB,
									  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
									  class btIDebugDraw* debugDraw);

	private :

};

#endif	// BT_GJP_EPA_PENETRATION_DEPTH_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

///This file was written by Erwin Coumans

#ifndef BT_POLYHEDRAL_CONTACT_CLIPPING_H
#define BT_POLYHEDRAL_CONTACT_CLIPPING_H

#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include "btDiscreteCollisionDetectorInterface.h"

class btConvexPolyhedron;

typedef btAlignedObjectArray<btVector3> btVertexArray;

// Clips a face to the back of a plane
struct btPolyhedralContactClipping
{

	static void	clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut);

	static void	clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);

	static bool findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);

	///the clipFace method is used internally
	static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS);

};

#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
#define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H

#include "btConvexCast.h"
#include "btSimplexSolverInterface.h"
class btConvexPenetrationDepthSolver;
class btConvexShape;
class btStaticPlaneShape;

/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
class btContinuousConvexCollision : public btConvexCast
{
	btSimplexSolverInterface* m_simplexSolver;
	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
	const btConvexShape*	m_convexA;
	//second object is either a convex or a plane (code sharing)
	const btConvexShape*	m_convexB1;
	const btStaticPlaneShape*	m_planeShape;

	void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector);

public:

	btContinuousConvexCollision (const btConvexShape*	shapeA,const btConvexShape*	shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);

	btContinuousConvexCollision(const btConvexShape*	shapeA,const btStaticPlaneShape*	plane );

	virtual bool	calcTimeOfImpact(
				const btTransform& fromA,
				const btTransform& toA,
				const btTransform& fromB,
				const btTransform& toB,
				CastResult& result);

};

#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_CONVEX_PENETRATION_DEPTH_H
#define BT_CONVEX_PENETRATION_DEPTH_H

class btVector3;
#include "btSimplexSolverInterface.h"
class btConvexShape;
class btTransform;

///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
class btConvexPenetrationDepthSolver
{
public:

	virtual ~btConvexPenetrationDepthSolver() {};
	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
		const btConvexShape* convexA,const btConvexShape* convexB,
					const btTransform& transA,const btTransform& transB,
				btVector3& v, btVector3& pa, btVector3& pb,
				class btIDebugDraw* debugDraw) = 0;

};
#endif //BT_CONVEX_PENETRATION_DEPTH_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_VORONOI_SIMPLEX_SOLVER_H
#define BT_VORONOI_SIMPLEX_SOLVER_H

#include "btSimplexSolverInterface.h"

#define VORONOI_SIMPLEX_MAX_VERTS 5

///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure
#define BT_USE_EQUAL_VERTEX_THRESHOLD

#ifdef BT_USE_DOUBLE_PRECISION
#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f
#else
#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
#endif//BT_USE_DOUBLE_PRECISION

struct btUsageBitfield{
	btUsageBitfield()
	{
		reset();
	}

	void reset()
	{
		usedVertexA = false;
		usedVertexB = false;
		usedVertexC = false;
		usedVertexD = false;
	}
	unsigned short usedVertexA	: 1;
	unsigned short usedVertexB	: 1;
	unsigned short usedVertexC	: 1;
	unsigned short usedVertexD	: 1;
	unsigned short unused1		: 1;
	unsigned short unused2		: 1;
	unsigned short unused3		: 1;
	unsigned short unused4		: 1;
};

struct	btSubSimplexClosestResult
{
	btVector3	m_closestPointOnSimplex;
	//MASK for m_usedVertices
	//stores the simplex vertex-usage, using the MASK,
	// if m_usedVertices & MASK then the related vertex is used
	btUsageBitfield	m_usedVertices;
	btScalar	m_barycentricCoords[4];
	bool m_degenerate;

	void	reset()
	{
		m_degenerate = false;
		setBarycentricCoordinates();
		m_usedVertices.reset();
	}
	bool	isValid()
	{
		bool valid = (m_barycentricCoords[0] >= btScalar(0.)) &&
			(m_barycentricCoords[1] >= btScalar(0.)) &&
			(m_barycentricCoords[2] >= btScalar(0.)) &&
			(m_barycentricCoords[3] >= btScalar(0.));

		return valid;
	}
	void	setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.))
	{
		m_barycentricCoords[0] = a;
		m_barycentricCoords[1] = b;
		m_barycentricCoords[2] = c;
		m_barycentricCoords[3] = d;
	}

};

/// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
/// Can be used with GJK, as an alternative to Johnson distance algorithm.
#ifdef NO_VIRTUAL_INTERFACE
ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver
#else
ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface
#endif
{
public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	int	m_numVertices;

	btVector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
	btVector3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
	btVector3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];

	btVector3	m_cachedP1;
	btVector3	m_cachedP2;
	btVector3	m_cachedV;
	btVector3	m_lastW;

	btScalar	m_equalVertexThreshold;
	bool		m_cachedValidClosest;

	btSubSimplexClosestResult m_cachedBC;

	bool	m_needsUpdate;

	void	removeVertex(int index);
	void	reduceVertices (const btUsageBitfield& usedVerts);
	bool	updateClosestVectorAndPoints();

	bool	closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult);
	int		pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
	bool	closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result);

public:

	btVoronoiSimplexSolver()
		:  m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
	{
	}
	 void reset();

	 void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);

	 void	setEqualVertexThreshold(btScalar threshold)
	 {
		 m_equalVertexThreshold = threshold;
	 }

	 btScalar	getEqualVertexThreshold() const
	 {
		 return m_equalVertexThreshold;
	 }

	 bool closest(btVector3& v);

	 btScalar maxVertex();

	 bool fullSimplex() const
	 {
		 return (m_numVertices == 4);
	 }

	 int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;

	 bool inSimplex(const btVector3& w);

	 void backup_closest(btVector3& v) ;

	 bool emptySimplex() const ;

	 void compute_points(btVector3& p1, btVector3& p2) ;

	 int numVertices() const
	 {
		 return m_numVertices;
	 }

};

#endif //BT_VORONOI_SIMPLEX_SOLVER_H