Cosmetic: Cosmetic code corrections in QuickStep

[ode.git] / ode / src / convex.cpp

/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/
/*
Code for Convex Collision Detection
By Rodrigo Hernandez
*/
#include <ode/common.h>
#include <ode/collision.h>
#include <ode/rotation.h>
#include "config.h"
#include "matrix.h"
#include "odemath.h"
#include "collision_kernel.h"
#include "collision_std.h"
#include "collision_util.h"

#ifdef _MSC_VER
#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
#endif

#if 1
#define dMIN(A,B)  ((A)>(B) ? (B) : (A))
#define dMAX(A,B)  ((A)>(B) ? (A) : (B))
#else
#define dMIN(A,B)  std::min(A,B)
#define dMAX(A,B)  std::max(A,B)
#endif

//****************************************************************************
// Convex public API
dxConvex::dxConvex (dSpaceID space,
                    const dReal *_planes,
                    unsigned int _planecount,
                    const dReal *_points,
                    unsigned int _pointcount,
                    const unsigned int *_polygons) :
dxGeom (space,1)
{
    dAASSERT (_planes != NULL);
    dAASSERT (_points != NULL);
    dAASSERT (_polygons != NULL);
    //fprintf(stdout,"dxConvex Constructor planes %X\n",_planes);
    type = dConvexClass;
    planes = _planes;
    planecount = _planecount;
    // we need points as well
    points = _points;
    pointcount = _pointcount;
    polygons=_polygons;
    edges = NULL;
    FillEdges();
#ifndef dNODEBUG
    // Check for properly build polygons by calculating the determinant
    // of the 3x3 matrix composed of the first 3 points in the polygon.
    const unsigned int *points_in_poly=polygons;
    const unsigned int *index=polygons+1;

    for(unsigned int i=0;i<planecount;++i)
    {
        dAASSERT (*points_in_poly > 2 );
        if((
            points[(index[0]*3)+0]*points[(index[1]*3)+1]*points[(index[2]*3)+2] +
            points[(index[0]*3)+1]*points[(index[1]*3)+2]*points[(index[2]*3)+0] +
            points[(index[0]*3)+2]*points[(index[1]*3)+0]*points[(index[2]*3)+1] -
            points[(index[0]*3)+2]*points[(index[1]*3)+1]*points[(index[2]*3)+0] -
            points[(index[0]*3)+1]*points[(index[1]*3)+0]*points[(index[2]*3)+2] -
            points[(index[0]*3)+0]*points[(index[1]*3)+2]*points[(index[2]*3)+1])<0)
        {
            fprintf(stdout,"WARNING: Polygon %d is not defined counterclockwise\n",i);
        }
        points_in_poly+=(*points_in_poly+1);
        index=points_in_poly+1;
        if(planes[(i*4)+3]<0) fprintf(stdout,"WARNING: Plane %d does not contain the origin\n",i);
    }
#endif

    //CreateTree();
}


void dxConvex::computeAABB()
{
    // this can, and should be optimized
    dVector3 point;
    dMultiply0_331 (point,final_posr->R,points);
    aabb[0] = point[0]+final_posr->pos[0];
    aabb[1] = point[0]+final_posr->pos[0];
    aabb[2] = point[1]+final_posr->pos[1];
    aabb[3] = point[1]+final_posr->pos[1];
    aabb[4] = point[2]+final_posr->pos[2];
    aabb[5] = point[2]+final_posr->pos[2];
    for(unsigned int i=3;i<(pointcount*3);i+=3)
    {
        dMultiply0_331 (point,final_posr->R,&points[i]);
        aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]);
        aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]);
        aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]);
        aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]);
        aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]);
        aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]);
    }
}

/*! \brief Populates the edges set, should be called only once whenever the polygon array gets updated */
void dxConvex::FillEdges()
{
    const unsigned int *points_in_poly=polygons;
    const unsigned int *index=polygons+1;
    if (edges!=NULL) delete[] edges;
    edgecount = 0;
    edge e;
    bool isinset;
    for(unsigned int i=0;i<planecount;++i)
    {
        for(unsigned int j=0;j<*points_in_poly;++j)
        {
            e.first = dMIN(index[j],index[(j+1)%*points_in_poly]);
            e.second = dMAX(index[j],index[(j+1)%*points_in_poly]);
            isinset=false;
            for(unsigned int k=0;k<edgecount;++k)
            {
                if((edges[k].first==e.first)&&(edges[k].second==e.second))
                {
                    isinset=true;
                    break;
                }
            }
            if(!isinset)
            {
                edge* tmp = new edge[edgecount+1];
                if(edgecount!=0)
                {
                    memcpy(tmp,edges,(edgecount)*sizeof(edge));
                    delete[] edges;
                }
                tmp[edgecount].first=e.first;
                tmp[edgecount].second=e.second;
                edges = tmp;
                ++edgecount;
            }
        }
        points_in_poly+=(*points_in_poly+1);
        index=points_in_poly+1;
    }
}
#if 0
dxConvex::BSPNode* dxConvex::CreateNode(std::vector<Arc> Arcs,std::vector<Polygon> Polygons)
{
#if 0
    dVector3 ea,eb,e;
    dVector3Copy(points+((edges.begin()+Arcs[0].edge)first*3),ea);
    dMultiply0_331(e1b,cvx1.final_posr->R,cvx1.points+(i->second*3));

    dVector3Copy(points[edges[Arcs[0].edge]
#endif
    return NULL;
}

void dxConvex::CreateTree()
{
    std::vector<Arc> A;
    A.reserve(edgecount);
    for(unsigned int i=0;i<edgecount;++i)
    {
        this->GetFacesSharedByEdge(i,A[i].normals);
        A[i].edge = i;
    }
    std::vector<Polygon> S;
    S.reserve(pointcount);
    for(unsigned int i=0;i<pointcount;++i)
    {
        this->GetFacesSharedByVertex(i,S[i].normals);
        S[i].vertex=i;
    }
    this->tree = CreateNode(A,S);
}

void dxConvex::GetFacesSharedByVertex(int i, std::vector<int> f)
{
}
void dxConvex::GetFacesSharedByEdge(int i, int* f)
{
}
void dxConvex::GetFaceNormal(int i, dVector3 normal)
{
}
#endif

dGeomID dCreateConvex (dSpaceID space,const dReal *_planes,unsigned int _planecount,
                       const dReal *_points,
                       unsigned int _pointcount,
                       const unsigned int *_polygons)
{
    //fprintf(stdout,"dxConvex dCreateConvex\n");
    return new dxConvex(space,_planes, _planecount,
        _points,
        _pointcount,
        _polygons);
}

void dGeomSetConvex (dGeomID g,const dReal *_planes,unsigned int _planecount,
                     const dReal *_points,
                     unsigned int _pointcount,
                     const unsigned int *_polygons)
{
    //fprintf(stdout,"dxConvex dGeomSetConvex\n");
    dUASSERT (g && g->type == dConvexClass,"argument not a convex shape");
    dxConvex *s = (dxConvex*) g;
    s->planes = _planes;
    s->planecount = _planecount;
    s->points = _points;
    s->pointcount = _pointcount;
    s->polygons=_polygons;
}

//****************************************************************************
// Helper Inlines
//

/*! \brief Returns Whether or not the segment ab intersects plane p
  \param a origin of the segment
  \param b segment destination
  \param p plane to test for intersection
  \param t returns the time "t" in the segment ray that gives us the intersecting
  point
  \param q returns the intersection point
  \return true if there is an intersection, otherwise false.
*/
bool IntersectSegmentPlane(dVector3 a,
                           dVector3 b,
                           dVector4 p,
                           dReal &t,
                           dVector3 q)
{
    // Compute the t value for the directed line ab intersecting the plane
    dVector3 ab;
    ab[0]= b[0] - a[0];
    ab[1]= b[1] - a[1];
    ab[2]= b[2] - a[2];

    t = (p[3] - dCalcVectorDot3(p,a)) / dCalcVectorDot3(p,ab);

    // If t in [0..1] compute and return intersection point
    if (t >= 0.0 && t <= 1.0)
    {
        q[0] = a[0] + t * ab[0];
        q[1] = a[1] + t * ab[1];
        q[2] = a[2] + t * ab[2];
        return true;
    }
    // Else no intersection
    return false;
}

/*! \brief Returns the Closest Point in Ray 1 to Ray 2
  \param Origin1 The origin of Ray 1
  \param Direction1 The direction of Ray 1
  \param Origin1 The origin of Ray 2
  \param Direction1 The direction of Ray 3
  \param t the time "t" in Ray 1 that gives us the closest point
  (closest_point=Origin1+(Direction1*t).
  \return true if there is a closest point, false if the rays are paralell.
*/
inline bool ClosestPointInRay(const dVector3 Origin1,
                              const dVector3 Direction1,
                              const dVector3 Origin2,
                              const dVector3 Direction2,
                              dReal& t)
{
    dVector3 w = {Origin1[0]-Origin2[0],
        Origin1[1]-Origin2[1],
        Origin1[2]-Origin2[2]};
    dReal a = dCalcVectorDot3(Direction1 , Direction1);
    dReal b = dCalcVectorDot3(Direction1 , Direction2);
    dReal c = dCalcVectorDot3(Direction2 , Direction2);
    dReal d = dCalcVectorDot3(Direction1 , w);
    dReal e = dCalcVectorDot3(Direction2 , w);
    dReal denominator = (a*c)-(b*b);
    if(denominator==0.0f)
    {
        return false;
    }
    t = ((a*e)-(b*d))/denominator;
    return true;
}

/*! \brief Returns the Closest Points from Segment 1 to Segment 2
  \param p1 start of segment 1
  \param q1 end of segment 1
  \param p2 start of segment 2
  \param q2 end of segment 2
  \param t the time "t" in Ray 1 that gives us the closest point
  (closest_point=Origin1+(Direction1*t).
  \return true if there is a closest point, false if the rays are paralell.
  \note Adapted from Christer Ericson's Real Time Collision Detection Book.
*/
inline void ClosestPointBetweenSegments(dVector3& p1,
                                         dVector3& q1,
                                         dVector3& p2,
                                         dVector3& q2,
                                         dVector3& c1,
                                         dVector3& c2)
{
    // s & t were originaly part of the output args, but since
    // we don't really need them, we'll just declare them in here
    dReal s;
    dReal t;
    dVector3 d1 = {q1[0] - p1[0],
        q1[1] - p1[1],
        q1[2] - p1[2]};
    dVector3 d2 = {q2[0] - p2[0],
        q2[1] - p2[1],
        q2[2] - p2[2]};
    dVector3 r  = {p1[0] - p2[0],
        p1[1] - p2[1],
        p1[2] - p2[2]};
    dReal a = dCalcVectorDot3(d1, d1);
    dReal e = dCalcVectorDot3(d2, d2);
    dReal f = dCalcVectorDot3(d2, r);
    // Check if either or both segments degenerate into points
    if (a <= dEpsilon && e <= dEpsilon)
    {
        // Both segments degenerate into points
        s = t = 0.0f;
        dVector3Copy(p1,c1);
        dVector3Copy(p2,c2);
        return;
    }
    if (a <= dEpsilon)
    {
        // First segment degenerates into a point
        s = 0.0f;
        t = f / e; // s = 0 => t = (b*s + f) / e = f / e
        t = dxClamp(t, 0.0f, 1.0f);
    }
    else
    {
        dReal c = dCalcVectorDot3(d1, r);
        if (e <= dEpsilon)
        {
            // Second segment degenerates into a point
            t = 0.0f;
            s = dxClamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
        }
        else
        {
            // The general non degenerate case starts here
            dReal b = dCalcVectorDot3(d1, d2);
            dReal denom = a*e-b*b; // Always nonnegative

            // If segments not parallel, compute closest point on L1 to L2, and
            // clamp to segment S1. Else pick arbitrary s (here 0)
            if (denom != 0.0f)
            {
                s = dxClamp((b*f - c*e) / denom, 0.0f, 1.0f);
            }
            else s = 0.0f;
#if 0
            // Compute point on L2 closest to S1(s) using
            // t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e
            t = (b*s + f) / e;

            // If t in [0,1] done. Else clamp t, recompute s for the new value
            // of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a
            // and clamp s to [0, 1]
            if (t < 0.0f) {
                t = 0.0f;
                s = dxClamp(-c / a, 0.0f, 1.0f);
            } else if (t > 1.0f) {
                t = 1.0f;
                s = dxClamp((b - c) / a, 0.0f, 1.0f);
            }
#else
            dReal tnom = b*s + f;
            if (tnom < 0.0f)
            {
                t = 0.0f;
                s = dxClamp(-c / a, 0.0f, 1.0f);
            }
            else if (tnom > e)
            {
                t = 1.0f;
                s = dxClamp((b - c) / a, 0.0f, 1.0f);
            }
            else
            {
                t = tnom / e;
            }
#endif
        }
    }

    c1[0] = p1[0] + d1[0] * s;
    c1[1] = p1[1] + d1[1] * s;
    c1[2] = p1[2] + d1[2] * s;
    c2[0] = p2[0] + d2[0] * t;
    c2[1] = p2[1] + d2[1] * t;
    c2[2] = p2[2] + d2[2] * t;
}

#if 0
dReal tnom = b*s + f;
if (tnom < 0.0f) {
    t = 0.0f;
    s = dxClamp(-c / a, 0.0f, 1.0f);
} else if (tnom > e) {
    t = 1.0f;
    s = dxClamp((b - c) / a, 0.0f, 1.0f);
} else {
    t = tnom / e;
}
#endif

/*! \brief Returns the Ray on which 2 planes intersect if they do.
  \param p1 Plane 1
  \param p2 Plane 2
  \param p Contains the origin of the ray upon returning if planes intersect
  \param d Contains the direction of the ray upon returning if planes intersect
  \return true if the planes intersect, false if paralell.
*/
inline bool IntersectPlanes(const dVector4 p1, const dVector4 p2, dVector3 p, dVector3 d)
{
    // Compute direction of intersection line
    dCalcVectorCross3(d,p1,p2);
    // If d is (near) zero, the planes are parallel (and separated)
    // or coincident, so they're not considered intersecting
    dReal denom = dCalcVectorDot3(d, d);
    if (denom < dEpsilon) return false;
    dVector3 n;
    n[0]=p1[3]*p2[0] - p2[3]*p1[0];
    n[1]=p1[3]*p2[1] - p2[3]*p1[1];
    n[2]=p1[3]*p2[2] - p2[3]*p1[2];
    // Compute point on intersection line
    dCalcVectorCross3(p,n,d);
    p[0]/=denom;
    p[1]/=denom;
    p[2]/=denom;
    return true;
}


#if 0
/*! \brief Finds out if a point lies inside a convex
  \param p Point to test
  \param convex a pointer to convex to test against
  \return true if the point lies inside the convex, false if not.
*/
inline bool IsPointInConvex(dVector3 p,
                            dxConvex *convex)
{
    dVector3 lp,tmp;
    // move point into convex space to avoid plane local to world calculations
    tmp[0] = p[0] - convex->final_posr->pos[0];
    tmp[1] = p[1] - convex->final_posr->pos[1];
    tmp[2] = p[2] - convex->final_posr->pos[2];
    dMultiply1_331 (lp,convex->final_posr->R,tmp);
    for(unsigned int i=0;i<convex->planecount;++i)
    {
        if((
            ((convex->planes+(i*4))[0]*lp[0])+
            ((convex->planes+(i*4))[1]*lp[1])+
            ((convex->planes+(i*4))[2]*lp[2])+
            -(convex->planes+(i*4))[3]
        )>0)
        {
            return false;
        }
    }
    return true;
}
#endif

/*! \brief Finds out if a point lies inside a 2D polygon
  \param p Point to test
  \param polygon a pointer to the start of the convex polygon index buffer
  \param out the closest point in the polygon if the point is not inside
  \return true if the point lies inside of the polygon, false if not.
*/
inline bool IsPointInPolygon(dVector3 p,
                             const unsigned int *polygon,
                             dReal *plane,
                             dxConvex *convex,
                             dVector3 out)
{
    // p is the point we want to check,
    // polygon is a pointer to the polygon we
    // are checking against, remember it goes
    // number of vertices then that many indexes
    // out returns the closest point on the border of the
    // polygon if the point is not inside it.
    dVector3 a;
    dVector3 b;
    dVector3 ab;
    dVector3 ap;
    dVector3 v;

    unsigned pointcount=polygon[0];
    dIASSERT(pointcount != 0);
    polygon++; // skip past pointcount

    dMultiply0_331 (b,convex->final_posr->R,
        &convex->points[(polygon[pointcount-1]*3)]);
    b[0]=convex->final_posr->pos[0]+b[0];
    b[1]=convex->final_posr->pos[1]+b[1];
    b[2]=convex->final_posr->pos[2]+b[2];

    for(unsigned i=0; i != pointcount; ++i)
    {
        a[0] = b[0];
        a[1] = b[1];
        a[2] = b[2];

        dMultiply0_331 (b,convex->final_posr->R,&convex->points[(polygon[i]*3)]);
        b[0]=convex->final_posr->pos[0]+b[0];
        b[1]=convex->final_posr->pos[1]+b[1];
        b[2]=convex->final_posr->pos[2]+b[2];

        ab[0] = b[0] - a[0];
        ab[1] = b[1] - a[1];
        ab[2] = b[2] - a[2];
        ap[0] = p[0] - a[0];
        ap[1] = p[1] - a[1];
        ap[2] = p[2] - a[2];

        dCalcVectorCross3(v, ab, plane);

        if (dCalcVectorDot3(ap, v) > REAL(0.0))
        {
            dReal ab_m2 = dCalcVectorDot3(ab, ab);
            dReal s = ab_m2 != REAL(0.0) ? dCalcVectorDot3(ab, ap) / ab_m2 : REAL(0.0);

            if (s <= REAL(0.0))
            {
                out[0] = a[0];
                out[1] = a[1];
                out[2] = a[2];
            }
            else if (s >= REAL(1.0)) 
            {
                out[0] = b[0];
                out[1] = b[1];
                out[2] = b[2];
            }
            else
            {
                out[0] = a[0] + ab[0] * s;
                out[1] = a[1] + ab[1] * s;
                out[2] = a[2] + ab[2] * s;
            }

            return false;
        }
    }

    return true;
}

int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
                         dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dConvexClass);
    dIASSERT (o2->type == dPlaneClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    dxConvex *Convex = (dxConvex*) o1;
    dxPlane *Plane = (dxPlane*) o2;
    unsigned int contacts=0;
    unsigned int maxc = flags & NUMC_MASK;
    dVector3 v2;

#define LTEQ_ZERO       0x10000000
#define GTEQ_ZERO       0x20000000
#define BOTH_SIGNS      (LTEQ_ZERO | GTEQ_ZERO)
    dIASSERT((BOTH_SIGNS & NUMC_MASK) == 0); // used in conditional operator later

    unsigned int totalsign = 0;
    for(unsigned int i=0;i<Convex->pointcount;++i)
    {
        dMultiply0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]);
        dVector3Add(Convex->final_posr->pos, v2, v2);

        unsigned int distance2sign = GTEQ_ZERO;
        dReal distance2 = dVector3Dot(Plane->p, v2) - Plane->p[3]; // Ax + By + Cz - D
        if((distance2 <= REAL(0.0)))
        {
            distance2sign = distance2 != REAL(0.0) ? LTEQ_ZERO : BOTH_SIGNS;

            if (contacts != maxc)
            {
                dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
                dVector3Copy(Plane->p, target->normal);
                dVector3Copy(v2, target->pos);
                target->depth = -distance2;
                target->g1 = Convex;
                target->g2 = Plane;
                target->side1 = -1; // TODO: set plane index?
                target->side2 = -1;
                contacts++;
            }
        }

        // Take new sign into account
        totalsign |= distance2sign;
        // Check if contacts are full and both signs have been already found
        if (((contacts ^ maxc) | totalsign) == BOTH_SIGNS) // harder to comprehend but requires one register less
        {
            break; // Nothing can be changed any more
        }
    }
    if (totalsign == BOTH_SIGNS) return contacts;
    return 0;
#undef BOTH_SIGNS
#undef GTEQ_ZERO
#undef LTEQ_ZERO
}

int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
                          dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dSphereClass);
    dIASSERT (o2->type == dConvexClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    dxSphere *Sphere = (dxSphere*) o1;
    dxConvex *Convex = (dxConvex*) o2;
    dReal dist,closestdist=dInfinity;
    dVector4 plane;
    // dVector3 contactpoint;
    dVector3 offsetpos,out,temp;
    const unsigned int *pPoly=Convex->polygons;
    int closestplane=-1;
    bool sphereinside=true;
    /*
    Do a good old sphere vs plane check first,
    if a collision is found then check if the contact point
    is within the polygon
    */
    // offset the sphere final_posr->position into the convex space
    offsetpos[0]=Sphere->final_posr->pos[0]-Convex->final_posr->pos[0];
    offsetpos[1]=Sphere->final_posr->pos[1]-Convex->final_posr->pos[1];
    offsetpos[2]=Sphere->final_posr->pos[2]-Convex->final_posr->pos[2];
    for(unsigned int i=0;i<Convex->planecount;++i)
    {
        // apply rotation to the plane
        dMultiply0_331(plane,Convex->final_posr->R,&Convex->planes[(i*4)]);
        plane[3]=(&Convex->planes[(i*4)])[3];
        // Get the distance from the sphere origin to the plane
        dist = dVector3Dot(plane, offsetpos) - plane[3]; // Ax + By + Cz - D
        if(dist>0)
        {
            // if we get here, we know the center of the sphere is
            // outside of the convex hull.
            if(dist<Sphere->radius)
            {
                // if we get here we know the sphere surface penetrates
                // the plane
                if(IsPointInPolygon(Sphere->final_posr->pos,pPoly,plane,Convex,out))
                {
                    // finally if we get here we know that the
                    // sphere is directly touching the inside of the polyhedron
                    contact->normal[0] = plane[0];
                    contact->normal[1] = plane[1];
                    contact->normal[2] = plane[2];
                    contact->pos[0] = Sphere->final_posr->pos[0]+
                        (-contact->normal[0]*Sphere->radius);
                    contact->pos[1] = Sphere->final_posr->pos[1]+
                        (-contact->normal[1]*Sphere->radius);
                    contact->pos[2] = Sphere->final_posr->pos[2]+
                        (-contact->normal[2]*Sphere->radius);
                    contact->depth = Sphere->radius-dist;
                    contact->g1 = Sphere;
                    contact->g2 = Convex;
                    contact->side1 = -1;
                    contact->side2 = -1; // TODO: set plane index?
                    return 1;
                }
                else
                {
                    // the sphere may not be directly touching
                    // the polyhedron, but it may be touching
                    // a point or an edge, if the distance between
                    // the closest point on the poly (out) and the
                    // center of the sphere is less than the sphere
                    // radius we have a hit.
                    temp[0] = (Sphere->final_posr->pos[0]-out[0]);
                    temp[1] = (Sphere->final_posr->pos[1]-out[1]);
                    temp[2] = (Sphere->final_posr->pos[2]-out[2]);
                    dist=(temp[0]*temp[0])+(temp[1]*temp[1])+(temp[2]*temp[2]);
                    // avoid the sqrt unless really necesary
                    if(dist<(Sphere->radius*Sphere->radius))
                    {
                        // We got an indirect hit
                        dist=dSqrt(dist);
                        contact->normal[0] = temp[0]/dist;
                        contact->normal[1] = temp[1]/dist;
                        contact->normal[2] = temp[2]/dist;
                        contact->pos[0] = Sphere->final_posr->pos[0]+
                            (-contact->normal[0]*Sphere->radius);
                        contact->pos[1] = Sphere->final_posr->pos[1]+
                            (-contact->normal[1]*Sphere->radius);
                        contact->pos[2] = Sphere->final_posr->pos[2]+
                            (-contact->normal[2]*Sphere->radius);
                        contact->depth = Sphere->radius-dist;
                        contact->g1 = Sphere;
                        contact->g2 = Convex;
                        contact->side1 = -1;
                        contact->side2 = -1; // TODO: set plane index?
                        return 1;
                    }
                }
            }
            sphereinside=false;
        }
        if(sphereinside)
        {
            if(closestdist>dFabs(dist))
            {
                closestdist=dFabs(dist);
                closestplane=i;
            }
        }
        pPoly+=pPoly[0]+1;
    }
    if(sphereinside)
    {
        // if the center of the sphere is inside
        // the Convex, we need to pop it out
        dMultiply0_331(contact->normal,
            Convex->final_posr->R,
            &Convex->planes[(closestplane*4)]);
        contact->pos[0] = Sphere->final_posr->pos[0];
        contact->pos[1] = Sphere->final_posr->pos[1];
        contact->pos[2] = Sphere->final_posr->pos[2];
        contact->depth = closestdist+Sphere->radius;
        contact->g1 = Sphere;
        contact->g2 = Convex;
        contact->side1 = -1;
        contact->side2 = -1; // TODO: set plane index?
        return 1;
    }
    return 0;
}

int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
                       dContactGeom * /*contact*/, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dConvexClass);
    dIASSERT (o2->type == dBoxClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    //dxConvex *Convex = (dxConvex*) o1;
    //dxBox *Box = (dxBox*) o2;

    return 0;
}

int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
                           int flags, dContactGeom * /*contact*/, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dConvexClass);
    dIASSERT (o2->type == dCapsuleClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    //dxConvex *Convex = (dxConvex*) o1;
    //dxCapsule *Capsule = (dxCapsule*) o2;

    return 0;
}

inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max)
{
    /* TODO: Use Support points here */
    dVector3 point;
    dReal value;
    //fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]);
    dMultiply0_331(point,cvx.final_posr->R,cvx.points);
    //fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]);
    point[0]+=cvx.final_posr->pos[0];
    point[1]+=cvx.final_posr->pos[1];
    point[2]+=cvx.final_posr->pos[2];
    max = min = dCalcVectorDot3(point,axis)-axis[3];//(*)
    for (unsigned int i = 1; i < cvx.pointcount; ++i)
    {
        dMultiply0_331(point,cvx.final_posr->R,cvx.points+(i*3));
        point[0]+=cvx.final_posr->pos[0];
        point[1]+=cvx.final_posr->pos[1];
        point[2]+=cvx.final_posr->pos[2];
        value=dCalcVectorDot3(point,axis)-axis[3];//(*)
        if(value<min)
        {
            min=value;
        }
        else if(value>max)
        {
            max=value;
        }
    }
    // *: usually using the distance part of the plane (axis) is
    // not necesary, however, here we need it here in order to know
    // which face to pick when there are 2 parallel sides.
}

bool CheckEdgeIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,int& curc,
                           dContactGeom *contact, int skip)
{
    int maxc = flags & NUMC_MASK;
    dIASSERT(maxc != 0);
    dVector3 e1,e2,q;
    dVector4 plane,depthplane;
    dReal t;
    for(unsigned int i = 0;i<cvx1.edgecount;++i)
    {
        // Rotate
        dMultiply0_331(e1,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].first*3));
        // translate
        e1[0]+=cvx1.final_posr->pos[0];
        e1[1]+=cvx1.final_posr->pos[1];
        e1[2]+=cvx1.final_posr->pos[2];
        // Rotate
        dMultiply0_331(e2,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].second*3));
        // translate
        e2[0]+=cvx1.final_posr->pos[0];
        e2[1]+=cvx1.final_posr->pos[1];
        e2[2]+=cvx1.final_posr->pos[2];
        const unsigned int* pPoly=cvx2.polygons;
        for(sizeint j=0;j<cvx2.planecount;++j)
        {
            // Rotate
            dMultiply0_331(plane,cvx2.final_posr->R,cvx2.planes+(j*4));
            dNormalize3(plane);
            // Translate
            plane[3]=
                (cvx2.planes[(j*4)+3])+
                ((plane[0] * cvx2.final_posr->pos[0]) +
                (plane[1] * cvx2.final_posr->pos[1]) +
                (plane[2] * cvx2.final_posr->pos[2]));
            dContactGeom *target = SAFECONTACT(flags, contact, curc, skip);
            target->g1=&cvx1; // g1 is the one pushed
            target->g2=&cvx2;
            if(IntersectSegmentPlane(e1,e2,plane,t,target->pos))
            {
                if(IsPointInPolygon(target->pos,pPoly,plane,&cvx2,q))
                {
                    target->depth = dInfinity;
                    for(sizeint k=0;k<cvx2.planecount;++k)
                    {
                        if(k==j) continue; // we're already at 0 depth on this plane
                        // Rotate
                        dMultiply0_331(depthplane,cvx2.final_posr->R,cvx2.planes+(k*4));
                        dNormalize3(depthplane);
                        // Translate
                        depthplane[3]=
                            (cvx2.planes[(k*4)+3])+
                            ((plane[0] * cvx2.final_posr->pos[0]) +
                            (plane[1] * cvx2.final_posr->pos[1]) +
                            (plane[2] * cvx2.final_posr->pos[2]));
                        dReal depth = (dVector3Dot(depthplane, target->pos) - depthplane[3]); // Ax + By + Cz - D
                        if((fabs(depth)<fabs(target->depth))&&((depth<-dEpsilon)||(depth>dEpsilon)))
                        {
                            target->depth=depth;
                            dVector3Copy(depthplane,target->normal);
                        }
                    }
                    ++curc;
                    if(curc==maxc)
                        return true;
                }
            }
            pPoly+=pPoly[0]+1;
        }
    }
    return false;
}

/*
Helper struct
*/
struct ConvexConvexSATOutput
{
    dReal min_depth;
    int depth_type;
    dVector3 dist; // distance from center to center, from cvx1 to cvx2
    dVector3 e1a,e1b,e2a,e2b; // e1a to e1b = edge in cvx1,e2a to e2b = edge in cvx2.
};

/*! \brief Does an axis separation test using cvx1 planes on cvx1 and cvx2, returns true for a collision false for no collision
  \param cvx1 [IN] First Convex object, its planes are used to do the tests
  \param cvx2 [IN] Second Convex object
  \param min_depth [IN/OUT] Used to input as well as output the minimum depth so far, must be set to a huge value such as dInfinity for initialization.
  \param g1 [OUT] Pointer to the convex which should be used in the returned contact as g1
  \param g2 [OUT] Pointer to the convex which should be used in the returned contact as g2
*/
inline bool CheckSATConvexFaces(dxConvex& cvx1,
                                dxConvex& cvx2,
                                ConvexConvexSATOutput& ccso)
{
    dReal min,max,min1,max1,min2,max2,depth;
    dVector4 plane;
    for(unsigned int i=0;i<cvx1.planecount;++i)
    {
        // -- Apply Transforms --
        // Rotate
        dMultiply0_331(plane,cvx1.final_posr->R,cvx1.planes+(i*4));
        dNormalize3(plane);
        // Translate
        plane[3]=
            (cvx1.planes[(i*4)+3])+
            ((plane[0] * cvx1.final_posr->pos[0]) +
            (plane[1] * cvx1.final_posr->pos[1])  +
            (plane[2] * cvx1.final_posr->pos[2]));
        ComputeInterval(cvx1,plane,min1,max1);
        ComputeInterval(cvx2,plane,min2,max2);
        if(max2<min1 || max1<min2) return false;
        min = dMAX(min1, min2);
        max = dMIN(max1, max2);
        depth = max-min;
        /*
        Take only into account the faces that penetrate cvx1 to determine
        minimum depth
        ((max2*min2)<=0) = different sign, or one is zero and thus
        cvx2 barelly touches cvx1
        */
        if (((max2*min2)<=0) && (dFabs(depth)<dFabs(ccso.min_depth)))
        {
            // Flip plane because the contact normal must point INTO g1,
            // plus the integrator seems to like positive depths better than negative ones
            ccso.min_depth=-depth;
            ccso.depth_type = 1; // 1 = face-something
        }
    }
    return true;
}
/*! \brief Does an axis separation test using cvx1 and cvx2 edges, returns true for a collision false for no collision
  \param cvx1 [IN] First Convex object
  \param cvx2 [IN] Second Convex object
  \param min_depth [IN/OUT] Used to input as well as output the minimum depth so far, must be set to a huge value such as dInfinity for initialization.
  \param g1 [OUT] Pointer to the convex which should be used in the returned contact as g1
  \param g2 [OUT] Pointer to the convex which should be used in the returned contact as g2
*/
inline bool CheckSATConvexEdges(dxConvex& cvx1,
                                dxConvex& cvx2,
                                ConvexConvexSATOutput& ccso)
{
    // Test cross products of pairs of edges
    dReal depth,min,max,min1,max1,min2,max2;
    dVector4 plane;
    dVector3 e1,e2,e1a,e1b,e2a,e2b;
    dVector3 dist;
    dVector3Copy(ccso.dist,dist);
    unsigned int s1 = cvx1.SupportIndex(dist);
    // invert direction
    dVector3Inv(dist);
    unsigned int s2 = cvx2.SupportIndex(dist);
    for(unsigned int i = 0;i<cvx1.edgecount;++i)
    {
        // Skip edge if it doesn't contain the extremal vertex
        if((cvx1.edges[i].first!=s1)&&(cvx1.edges[i].second!=s1)) continue;
        // we only need to apply rotation here
        dMultiply0_331(e1a,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].first*3));
        dMultiply0_331(e1b,cvx1.final_posr->R,cvx1.points+(cvx1.edges[i].second*3));
        e1[0]=e1b[0]-e1a[0];
        e1[1]=e1b[1]-e1a[1];
        e1[2]=e1b[2]-e1a[2];
        for(unsigned int j = 0;j<cvx2.edgecount;++j)
        {
            // Skip edge if it doesn't contain the extremal vertex
            if((cvx2.edges[j].first!=s2)&&(cvx2.edges[j].second!=s2)) continue;
            // we only need to apply rotation here
            dMultiply0_331 (e2a,cvx2.final_posr->R,cvx2.points+(cvx2.edges[j].first*3));
            dMultiply0_331 (e2b,cvx2.final_posr->R,cvx2.points+(cvx2.edges[j].second*3));
            e2[0]=e2b[0]-e2a[0];
            e2[1]=e2b[1]-e2a[1];
            e2[2]=e2b[2]-e2a[2];
            dCalcVectorCross3(plane,e1,e2);
            if(dCalcVectorDot3(plane,plane)<dEpsilon) /* edges are parallel */ continue;
            dNormalize3(plane);
            plane[3]=0;
            ComputeInterval(cvx1,plane,min1,max1);
            ComputeInterval(cvx2,plane,min2,max2);
            if(max2 < min1 || max1 < min2) return false;
            min = dMAX(min1, min2);
            max = dMIN(max1, max2);
            depth = max-min;
            if (((dFabs(depth)+dEpsilon)<dFabs(ccso.min_depth)))
            {
                ccso.min_depth=depth;
                ccso.depth_type = 2; // 2 means edge-edge
                // use cached values, add position
                dVector3Copy(e1a,ccso.e1a);
                dVector3Copy(e1b,ccso.e1b);
                ccso.e1a[0]+=cvx1.final_posr->pos[0];
                ccso.e1a[1]+=cvx1.final_posr->pos[1];
                ccso.e1a[2]+=cvx1.final_posr->pos[2];
                ccso.e1b[0]+=cvx1.final_posr->pos[0];
                ccso.e1b[1]+=cvx1.final_posr->pos[1];
                ccso.e1b[2]+=cvx1.final_posr->pos[2];
                dVector3Copy(e2a,ccso.e2a);
                dVector3Copy(e2b,ccso.e2b);
                ccso.e2a[0]+=cvx2.final_posr->pos[0];
                ccso.e2a[1]+=cvx2.final_posr->pos[1];
                ccso.e2a[2]+=cvx2.final_posr->pos[2];
                ccso.e2b[0]+=cvx2.final_posr->pos[0];
                ccso.e2b[1]+=cvx2.final_posr->pos[1];
                ccso.e2b[2]+=cvx2.final_posr->pos[2];
            }
        }
    }
    return true;
}

#if 0
/*! \brief Returns the index of the plane/side of the incident convex (ccso.g2)
 *  which is closer to the reference convex (ccso.g1) side
 *
 *  This function just looks for the incident face that is facing the reference face
 *  and is the closest to being parallel to it, which sometimes is.
 */
inline unsigned int GetIncidentSide(ConvexConvexSATOutput& ccso)
{
    dVector3 nis; // (N)ormal in (I)ncident convex (S)pace
    dReal SavedDot;
    dReal Dot;
    unsigned int incident_side=0;
    // Rotate the plane normal into incident convex space
    // (things like this should be done all over this file,
    //  will look into that)
    dMultiply1_331(nis,ccso.g2->final_posr->R,ccso.plane);
    SavedDot = dCalcVectorDot3(nis,ccso.g2->planes);
    for(unsigned int i=1;i<ccso.g2->planecount;++i)
    {
        Dot = dCalcVectorDot3(nis,ccso.g2->planes+(i*4));
        if(Dot>SavedDot)
        {
            SavedDot=Dot;
            incident_side=i;
        }
    }
    return incident_side;
}
#endif

inline unsigned int GetSupportSide(dVector3& dir,dxConvex& cvx)
{
    dVector3 dics,tmp; // Direction in convex space
    dReal SavedDot;
    dReal Dot;
    unsigned int side=0;
    dVector3Copy(dir,tmp);
    dNormalize3(tmp);
    dMultiply1_331(dics,cvx.final_posr->R,tmp);
    SavedDot = dCalcVectorDot3(dics,cvx.planes);
    for(unsigned int i=1;i<cvx.planecount;++i)
    {
        Dot = dCalcVectorDot3(dics,cvx.planes+(i*4));
        if(Dot>SavedDot)
        {
            SavedDot=Dot;
            side=i;
        }
    }
    return side;
}

/*! \brief Does an axis separation test between the 2 convex shapes
using faces and edges */
int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,
                           dContactGeom *contact, int skip)
{
    ConvexConvexSATOutput ccso;
#ifndef dNDEBUG
    memset(&ccso, 0, sizeof(ccso)); // get rid of 'uninitialized values' warning
#endif
    ccso.min_depth=dInfinity; // Min not min at all
    ccso.depth_type=0; // no type
    // precompute distance vector
    dSubtractVectors3(ccso.dist, cvx2.final_posr->pos, cvx1.final_posr->pos);
    int maxc = flags & NUMC_MASK;
    dIASSERT(maxc != 0);
    dVector3 i1,i2,r1,r2; // edges of incident and reference faces respectively
    int contacts=0;
    if(!CheckSATConvexFaces(cvx1,cvx2,ccso))
    {
        return 0;
    }
    else
        if(!CheckSATConvexFaces(cvx2,cvx1,ccso))
        {
            return 0;
        }
        else if(!CheckSATConvexEdges(cvx1,cvx2,ccso))
        {
            return 0;
        }
        // If we get here, there was a collision
        if(ccso.depth_type==1) // face-face
        {
            // cvx1 MUST always be in contact->g1 and cvx2 in contact->g2
            // This was learned the hard way :(
            unsigned int incident_side;
            const unsigned int* pIncidentPoly;
            const unsigned int* pIncidentPoints;
            unsigned int reference_side;
            const unsigned int* pReferencePoly;
            const unsigned int* pReferencePoints;
            dVector4 plane,rplane,iplane;
            dVector3 tmp;
            dVector3 dist,p;
            dReal t,d,d1,d2;
            bool outside,out;
            dVector3Copy(ccso.dist,dist);
            reference_side = GetSupportSide(dist,cvx1);
            dNegateVector3(dist);
            incident_side = GetSupportSide(dist,cvx2);

            pReferencePoly = cvx1.polygons;
            pIncidentPoly  = cvx2.polygons;
            // Get Reference plane (We may not have to apply transforms Optimization Oportunity)
            // Rotate
            dMultiply0_331(rplane,cvx1.final_posr->R,cvx1.planes+(reference_side*4));
            dNormalize3(rplane);
            // Translate
            rplane[3]=
                (cvx1.planes[(reference_side*4)+3])+
                ((rplane[0] * cvx1.final_posr->pos[0]) +
                (rplane[1] * cvx1.final_posr->pos[1]) +
                (rplane[2] * cvx1.final_posr->pos[2]));
            // flip
            rplane[0]=-rplane[0];
            rplane[1]=-rplane[1];
            rplane[2]=-rplane[2];
            rplane[3]=-rplane[3];
            for(unsigned int i=0;i<incident_side;++i)
            {
                pIncidentPoly+=pIncidentPoly[0]+1;
            }
            pIncidentPoints = pIncidentPoly+1;
            // Get the first point of the incident face
            dMultiply0_331(i2,cvx2.final_posr->R,&cvx2.points[(pIncidentPoints[0]*3)]);
            dVector3Add(i2,cvx2.final_posr->pos,i2);
            // Get the same point in the reference convex space
            dVector3Copy(i2,r2);
            dVector3Subtract(r2,cvx1.final_posr->pos,r2);
            dVector3Copy(r2,tmp);
            dMultiply1_331(r2,cvx1.final_posr->R,tmp);
            for(unsigned int i=0;i<pIncidentPoly[0];++i)
            {
                // Move i2 to i1, r2 to r1
                dVector3Copy(i2,i1);
                dVector3Copy(r2,r1);
                dMultiply0_331(i2,cvx2.final_posr->R,&cvx2.points[(pIncidentPoints[(i+1)%pIncidentPoly[0]]*3)]);
                dVector3Add(i2,cvx2.final_posr->pos,i2);
                // Get the same point in the reference convex space
                dVector3Copy(i2,r2);
                dVector3Subtract(r2,cvx1.final_posr->pos,r2);
                dVector3Copy(r2,tmp);
                dMultiply1_331(r2,cvx1.final_posr->R,tmp);
                outside=false;
                for(unsigned int j=0;j<cvx1.planecount;++j)
                {
                    plane[0]=cvx1.planes[(j*4)+0];
                    plane[1]=cvx1.planes[(j*4)+1];
                    plane[2]=cvx1.planes[(j*4)+2];
                    plane[3]=cvx1.planes[(j*4)+3];
                    // Get the distance from the points to the plane
                    d1 = r1[0]*plane[0]+
                        r1[1]*plane[1]+
                        r1[2]*plane[2]-
                        plane[3];
                    d2 = r2[0]*plane[0]+
                        r2[1]*plane[1]+
                        r2[2]*plane[2]-
                        plane[3];
                    if(d1*d2<0)
                    {
                        out = false;

                        // Edge intersects plane
                        if (!IntersectSegmentPlane(r1,r2,plane,t,p))
                        {
                            out = true;
                        }

                        if (!out)
                        {
                            // Check the resulting point again to make sure it is inside the reference convex
                            for (unsigned int k = 0; k < cvx1.planecount; ++k)
                            {
                                d = p[0]*cvx1.planes[(k*4)+0]+
                                    p[1]*cvx1.planes[(k*4)+1]+
                                    p[2]*cvx1.planes[(k*4)+2]-
                                    cvx1.planes[(k*4)+3];
                                if(d>0)
                                {
                                    out = true;
                                    break;
                                }
                            }
                        }

                        if(!out)
                        {
#if 0
                            // Use t to move p into global space
                            p[0] = i1[0]+((i2[0]-i1[0])*t);
                            p[1] = i1[1]+((i2[1]-i1[1])*t);
                            p[2] = i1[2]+((i2[2]-i1[2])*t);
#else
                            // Apply reference convex transformations to p
                            // The commented out piece of code is likelly to
                            // produce less operations than this one, but
                            // this way we know we are getting the right data
                            dMultiply0_331(tmp,cvx1.final_posr->R,p);
                            dVector3Add(tmp,cvx1.final_posr->pos,p);
#endif
                            // get p's distance to reference plane
                            d = p[0]*rplane[0]+
                                p[1]*rplane[1]+
                                p[2]*rplane[2]-
                                rplane[3];
                            if(d>0)
                            {
                                dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
                                dVector3Copy(p, target->pos);
                                dVector3Copy(rplane, target->normal);
                                target->g1 = &cvx1;
                                target->g2 = &cvx2;
                                target->depth = d;
                                ++contacts;
                                if (contacts==maxc) return contacts;
                            }
                        }
                    }
                    if(d1>0)
                    {
                        outside=true;
                    }
                }
                if(outside) continue;
                d = i1[0]*rplane[0]+
                    i1[1]*rplane[1]+
                    i1[2]*rplane[2]-
                    rplane[3];
                if(d>0)
                {
                    dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
                    dVector3Copy(i1, target->pos);
                    dVector3Copy(rplane, target->normal);
                    target->g1 = &cvx1;
                    target->g2 = &cvx2;
                    target->depth = d;
                    ++contacts;
                    if (contacts==maxc) return contacts;
                }
            }
            // IF we get here, we got the easiest contacts to calculate,
            // but there is still space in the contacts array for more.
            // So, project the Reference's face points onto the Incident face
            // plane and test them for inclusion in the reference plane as well.
            // We already have computed intersections so, skip those.

            /* Get Incident plane, we need it for projection */
            /* Rotate */
            dMultiply0_331(iplane,cvx2.final_posr->R,cvx2.planes+(incident_side*4));
            dNormalize3(iplane);
            /* Translate */
            iplane[3]=
                (cvx2.planes[(incident_side*4)+3])     +
                ((iplane[0] * cvx2.final_posr->pos[0]) +
                (iplane[1] * cvx2.final_posr->pos[1])  +
                (iplane[2] * cvx2.final_posr->pos[2]));
            // get reference face
            for(unsigned int i=0;i<reference_side;++i)
            {
                pReferencePoly+=pReferencePoly[0]+1;
            }
            pReferencePoints = pReferencePoly+1;
            for(unsigned int i=0;i<pReferencePoly[0];++i)
            {
                dMultiply0_331(i1,cvx1.final_posr->R,&cvx1.points[(pReferencePoints[i]*3)]);
                dVector3Add(cvx1.final_posr->pos,i1,i1);
                // Project onto Incident face plane
                t = -(i1[0]*iplane[0]+
                    i1[1]*iplane[1]+
                    i1[2]*iplane[2]-
                    iplane[3]);
                i1[0]+=iplane[0]*t;
                i1[1]+=iplane[1]*t;
                i1[2]+=iplane[2]*t;
                // Get the same point in the incident convex space
                dVector3Copy(i1,r1);
                dVector3Subtract(r1,cvx2.final_posr->pos,r1);
                dVector3Copy(r1,tmp);
                dMultiply1_331(r1,cvx2.final_posr->R,tmp);
                // Check if it is outside the incident convex
                out = false;
                for(unsigned int j=0;j<cvx2.planecount;++j)
                {
                    d = r1[0]*cvx2.planes[(j*4)+0]+
                        r1[1]*cvx2.planes[(j*4)+1]+
                        r1[2]*cvx2.planes[(j*4)+2]-
                        cvx2.planes[(j*4)+3];
                    if(d>=0){out = true;break;};
                }
                if(!out)
                {
                    // check that the point is not a duplicate
                    outside = false;
                    for(int j=0;j<contacts;++j)
                    {
                        dContactGeom *cur_contact = SAFECONTACT(flags, contact, j, skip);
                        if((cur_contact->pos[0] == i1[0]) &&
                            (cur_contact->pos[1] == i1[1]) &&
                            (cur_contact->pos[2] == i1[2]))
                        {
                            outside=true;
                        }
                    }
                    if(!outside)
                    {
                        d = i1[0]*rplane[0]+
                            i1[1]*rplane[1]+
                            i1[2]*rplane[2]-
                            rplane[3];
                        if(d>0)
                        {
                            dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
                            dVector3Copy(i1, target->pos);
                            dVector3Copy(rplane, target->normal);
                            target->g1 = &cvx1;
                            target->g2 = &cvx2;
                            target->depth = d;
                            ++contacts;
                            if (contacts==maxc) return contacts;
                        }
                    }
                }
            }
        }
        else if (ccso.depth_type == 2) // edge-edge
        {
            dVector3 c1, c2;
            ClosestPointBetweenSegments(ccso.e1a, ccso.e1b, ccso.e2a, ccso.e2b, c1, c2);

            dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
            dSubtractVectors3(target->normal, c2, c1);
            dReal depth_square = dCalcVectorLengthSquare3(target->normal);

            if (dxSafeNormalize3(target->normal))
            {
                target->depth = dSqrt(depth_square);
            }
            else
            {
                // If edges coincide return direction from one center to the other as the contact normal
                dVector3Copy(ccso.dist, target->normal);

                if (!dxSafeNormalize3(target->normal))
                {
                    // If the both centers coincide as well return an arbitrary vector. The depth is going to be zero anyway.
                    dAssignVector3(target->normal, 1, 0, 0);
                }

                target->depth = 0; // Since the edges coincide, return a contact of zero depth
            }

            target->g1 = &cvx1;
            target->g2 = &cvx2;
            dVector3Copy(c1, target->pos);
            contacts++;
        }
        return contacts;
}

int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
                          dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dConvexClass);
    dIASSERT (o2->type == dConvexClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);
    dxConvex *Convex1 = (dxConvex*) o1;
    dxConvex *Convex2 = (dxConvex*) o2;
    return TestConvexIntersection(*Convex1,*Convex2,flags,
        contact,skip);
}

#if 0
int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags,
                       dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT( o1->type == dRayClass );
    dIASSERT( o2->type == dConvexClass );
    dIASSERT ((flags & NUMC_MASK) >= 1);
    dxRay* ray = (dxRay*) o1;
    dxConvex* convex = (dxConvex*) o2;
    dVector3 origin,destination,contactpoint,out;
    dReal depth;
    dVector4 plane;
    unsigned int *pPoly=convex->polygons;
    // Calculate ray origin and destination
    destination[0]=0;
    destination[1]=0;
    destination[2]= ray->length;
    // -- Rotate --
    dMultiply0_331(destination,ray->final_posr->R,destination);
    origin[0]=ray->final_posr->pos[0];
    origin[1]=ray->final_posr->pos[1];
    origin[2]=ray->final_posr->pos[2];
    destination[0]+=origin[0];
    destination[1]+=origin[1];
    destination[2]+=origin[2];
    for(int i=0;i<convex->planecount;++i)
    {
        // Rotate
        dMultiply0_331(plane,convex->final_posr->R,convex->planes+(i*4));
        // Translate
        plane[3]=
            (convex->planes[(i*4)+3])+
            ((plane[0] * convex->final_posr->pos[0]) +
            (plane[1] * convex->final_posr->pos[1]) +
            (plane[2] * convex->final_posr->pos[2]));
        if(IntersectSegmentPlane(origin,
            destination,
            plane,
            depth,
            contactpoint))
        {
            if(IsPointInPolygon(contactpoint,pPoly,plane,convex,out))
            {
                contact->pos[0]=contactpoint[0];
                contact->pos[1]=contactpoint[1];
                contact->pos[2]=contactpoint[2];
                contact->normal[0]=plane[0];
                contact->normal[1]=plane[1];
                contact->normal[2]=plane[2];
                contact->depth=depth;
                contact->g1 = ray;
                contact->g2 = convex;
                contact->side1 = -1;
                contact->side2 = -1; // TODO: set plane index?
                return 1;
            }
        }
        pPoly+=pPoly[0]+1;
    }
    return 0;
}
#else
// Ray - Convex collider by David Walters, June 2006
int dCollideRayConvex(dxGeom *o1, dxGeom *o2,
                      int flags, dContactGeom *contact, int skip)
{
    dIASSERT(skip >= (int)sizeof(dContactGeom));
    dIASSERT(o1->type == dRayClass);
    dIASSERT(o2->type == dConvexClass);
    dIASSERT((flags & NUMC_MASK) >= 1);

    dxRay* ray = (dxRay*)o1;
    dxConvex* convex = (dxConvex*)o2;

    contact->g1 = ray;
    contact->g2 = convex;
    contact->side1 = -1;
    contact->side2 = -1; // TODO: set plane index?

    dReal alpha, beta, nsign;
    int flag = 0;

    //
    // Compute some useful info
    //

    dVector3 ray_pos = {
        ray->final_posr->pos[0] - convex->final_posr->pos[0],
        ray->final_posr->pos[1] - convex->final_posr->pos[1],
        ray->final_posr->pos[2] - convex->final_posr->pos[2]
    };

    dVector3 ray_dir = {
        ray->final_posr->R[0 * 4 + 2],
        ray->final_posr->R[1 * 4 + 2],
        ray->final_posr->R[2 * 4 + 2]
    };

    dMultiply1_331(ray_pos, convex->final_posr->R, ray_pos);
    dMultiply1_331(ray_dir, convex->final_posr->R, ray_dir);

    for (unsigned int i = 0; i < convex->planecount; ++i)
    {
        // Alias this plane.
        const dReal* plane = convex->planes + (i * 4);

        // If alpha >= 0 then start point is outside of plane.
        alpha = dCalcVectorDot3(plane, ray_pos) - plane[3];

        // If any alpha is positive, then
        // the ray start is _outside_ of the hull
        if (alpha >= 0)
        {
            flag = 1;
            break;
        }
    }

    // If the ray starts inside the convex hull, then everything is flipped.
    nsign = (flag) ? REAL(1.0) : REAL(-1.0);


    //
    // Find closest contact point
    //

    // Assume no contacts.
    contact->depth = dInfinity;

    for (unsigned int i = 0; i < convex->planecount; ++i)
    {
        // Alias this plane.
        const dReal* plane = convex->planes + (i * 4);

        // If alpha >= 0 then point is outside of plane.
        alpha = nsign * (dCalcVectorDot3(plane, ray_pos) - plane[3]);

        // Compute [ plane-normal DOT ray-normal ], (/flip)
        beta = dCalcVectorDot3(plane, ray_dir) * nsign;

        // Ray is pointing at the plane? ( beta < 0 )
        // Ray start to plane is within maximum ray length?
        // Ray start to plane is closer than the current best distance?
        if (beta < -dEpsilon &&
            alpha >= 0 && alpha <= ray->length &&
            alpha < contact->depth)
        {
            // Compute contact point on convex hull surface.
            contact->pos[0] = ray_pos[0] + alpha * ray_dir[0];
            contact->pos[1] = ray_pos[1] + alpha * ray_dir[1];
            contact->pos[2] = ray_pos[2] + alpha * ray_dir[2];

            flag = 0;

            // For all _other_ planes.
            for (unsigned int j = 0; j < convex->planecount; ++j)
            {
                if (i == j)
                    continue;   // Skip self.

                // Alias this plane.
                const dReal* planej = convex->planes + (j * 4);

                // If beta >= 0 then start is outside of plane.
                beta = dCalcVectorDot3(planej, contact->pos) - planej[3];

                // If any beta is positive, then the contact point
                // is not on the surface of the convex hull - it's just
                // intersecting some part of its infinite extent.
                if (beta > dEpsilon)
                {
                    flag = 1;
                    break;
                }
            }

            // Contact point isn't outside hull's surface? then it's a good contact!
            if (flag == 0)
            {
                // Store the contact normal, possibly flipped.
                contact->normal[0] = nsign * plane[0];
                contact->normal[1] = nsign * plane[1];
                contact->normal[2] = nsign * plane[2];

                // Store depth
                contact->depth = alpha;

                if ((flags & CONTACTS_UNIMPORTANT) && contact->depth <= ray->length)
                {
                    // Break on any contact if contacts are not important
                    break;
                }
            }
        }
    }
    // Contact?
    if (contact->depth <= ray->length)
    {
        // Adjust contact position and normal back to global space
        dMultiply0_331(contact->pos, convex->final_posr->R, contact->pos);
        dMultiply0_331(contact->normal, convex->final_posr->R, contact->normal);
        contact->pos[0] += convex->final_posr->pos[0];
        contact->pos[1] += convex->final_posr->pos[1];
        contact->pos[2] += convex->final_posr->pos[2];
        return true;
    }
    return false;
}

#endif
//<-- Convex Collision