path: root/libs/ode-0.16.1/ode/src/collision_libccd.cpp

/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 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.                     *
 *                                                                       *
 *************************************************************************/

#include <ode/collision.h>
#include <ccd/ccd.h>
#include "ccdcustom/vec3.h"
#include "ccdcustom/quat.h"
#include "config.h"
#include "odemath.h"
#include "collision_libccd.h"
#include "collision_trimesh_internal.h"
#include "collision_std.h"
#include "collision_util.h"
#include "error.h"


struct _ccd_obj_t {
    ccd_vec3_t pos;
    ccd_quat_t rot, rot_inv;
};
typedef struct _ccd_obj_t ccd_obj_t;

struct _ccd_box_t {
    ccd_obj_t o;
    ccd_real_t dim[3];
};
typedef struct _ccd_box_t ccd_box_t;

struct _ccd_cap_t {
    ccd_obj_t o;
    ccd_real_t radius;
    ccd_vec3_t axis;
    ccd_vec3_t p1;
    ccd_vec3_t p2;
};
typedef struct _ccd_cap_t ccd_cap_t;

struct _ccd_cyl_t {
    ccd_obj_t o;
    ccd_real_t radius;
    ccd_vec3_t axis;
    ccd_vec3_t p1;
    ccd_vec3_t p2;
};
typedef struct _ccd_cyl_t ccd_cyl_t;

struct _ccd_sphere_t {
    ccd_obj_t o;
    ccd_real_t radius;
};
typedef struct _ccd_sphere_t ccd_sphere_t;

struct _ccd_convex_t {
    ccd_obj_t o;
    dxConvex *convex;
};
typedef struct _ccd_convex_t ccd_convex_t;

struct _ccd_triangle_t {
    ccd_obj_t o;
    ccd_vec3_t vertices[3];
};
typedef struct _ccd_triangle_t ccd_triangle_t;

/** Transforms geom to ccd struct */
static void ccdGeomToObj(const dGeomID g, ccd_obj_t *);
static void ccdGeomToBox(const dGeomID g, ccd_box_t *);
static void ccdGeomToCap(const dGeomID g, ccd_cap_t *);
static void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *);
static void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *);
static void ccdGeomToConvex(const dGeomID g, ccd_convex_t *);

/** Support functions */
static void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v);
static void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v);
static void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v);
static void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v);
static void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v);

/** Center function */
static void ccdCenter(const void *obj, ccd_vec3_t *c);

/** General collide function */
static int ccdCollide(dGeomID o1, dGeomID o2, int flags,
    dContactGeom *contact, int skip,
    void *obj1, ccd_support_fn supp1, ccd_center_fn cen1,
    void *obj2, ccd_support_fn supp2, ccd_center_fn cen2);

static int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip);
static bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth);
// Adds a contact between 2 cylinders
static int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts, ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip);

static unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, 
    const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip,
    ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount);
static bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices);
static unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip);
static void setObjPosToTriangleCenter(ccd_triangle_t *t); 
static void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v);


static 
void ccdGeomToObj(const dGeomID g, ccd_obj_t *o)
{
    const dReal *ode_pos;
    dQuaternion ode_rot;

    ode_pos = dGeomGetPosition(g);
    dGeomGetQuaternion(g, ode_rot);

    ccdVec3Set(&o->pos, ode_pos[0], ode_pos[1], ode_pos[2]);
    ccdQuatSet(&o->rot, ode_rot[1], ode_rot[2], ode_rot[3], ode_rot[0]);

    ccdQuatInvert2(&o->rot_inv, &o->rot);
}

static 
void ccdGeomToBox(const dGeomID g, ccd_box_t *box)
{
    dVector3 dim;

    ccdGeomToObj(g, (ccd_obj_t *)box);

    dGeomBoxGetLengths(g, dim);
    box->dim[0] = (ccd_real_t)(dim[0] * 0.5);
    box->dim[1] = (ccd_real_t)(dim[1] * 0.5);
    box->dim[2] = (ccd_real_t)(dim[2] * 0.5);
}

static 
void ccdGeomToCap(const dGeomID g, ccd_cap_t *cap)
{
    dReal r, h;
    ccdGeomToObj(g, (ccd_obj_t *)cap);

    dGeomCapsuleGetParams(g, &r, &h);
    cap->radius = r;
    ccdVec3Set(&cap->axis, 0.0, 0.0, h / 2);
    ccdQuatRotVec(&cap->axis, &cap->o.rot);
    ccdVec3Copy(&cap->p1, &cap->axis);
    ccdVec3Copy(&cap->p2, &cap->axis);
    ccdVec3Scale(&cap->p2, -1.0);
    ccdVec3Add(&cap->p1, &cap->o.pos);
    ccdVec3Add(&cap->p2, &cap->o.pos);
}

static 
void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *cyl)
{
    dReal r, h;
    ccdGeomToObj(g, (ccd_obj_t *)cyl);

    dGeomCylinderGetParams(g, &r, &h);
    cyl->radius = r;
    ccdVec3Set(&cyl->axis, 0.0, 0.0, h / 2);
    ccdQuatRotVec(&cyl->axis, &cyl->o.rot);
    ccdVec3Copy(&cyl->p1, &cyl->axis);
    ccdVec3Copy(&cyl->p2, &cyl->axis);
    int cylAxisNormalizationResult = ccdVec3SafeNormalize(&cyl->axis);
    dUVERIFY(cylAxisNormalizationResult == 0, "Invalid cylinder has been passed");
    ccdVec3Scale(&cyl->p2, -1.0);
    ccdVec3Add(&cyl->p1, &cyl->o.pos);
    ccdVec3Add(&cyl->p2, &cyl->o.pos);
}

static 
void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *s)
{
    ccdGeomToObj(g, (ccd_obj_t *)s);
    s->radius = dGeomSphereGetRadius(g);
}

static 
void ccdGeomToConvex(const dGeomID g, ccd_convex_t *c)
{
    ccdGeomToObj(g, (ccd_obj_t *)c);
    c->convex = (dxConvex *)g;
}


static 
void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
    const ccd_box_t *o = (const ccd_box_t *)obj;
    ccd_vec3_t dir;

    ccdVec3Copy(&dir, _dir);
    ccdQuatRotVec(&dir, &o->o.rot_inv);

    ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
        ccdSign(ccdVec3Y(&dir)) * o->dim[1],
        ccdSign(ccdVec3Z(&dir)) * o->dim[2]);

    // transform support vertex
    ccdQuatRotVec(v, &o->o.rot);
    ccdVec3Add(v, &o->o.pos);
}

static 
void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
    const ccd_cap_t *o = (const ccd_cap_t *)obj;

    ccdVec3Copy(v, _dir);
    ccdVec3Scale(v, o->radius);

    if (ccdVec3Dot(_dir, &o->axis) > 0.0){
        ccdVec3Add(v, &o->p1);
    }else{
        ccdVec3Add(v, &o->p2);
    }

}

static 
void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
    const ccd_cyl_t *cyl = (const ccd_cyl_t *)obj;
    ccd_vec3_t dir;
    ccd_real_t len;
    
    ccd_real_t dot = ccdVec3Dot(_dir, &cyl->axis);
    if (dot > 0.0){
        ccdVec3Copy(v, &cyl->p1);
    } else{
        ccdVec3Copy(v, &cyl->p2);
    }
    // project dir onto cylinder's 'top'/'bottom' plane
    ccdVec3Copy(&dir, &cyl->axis);
    ccdVec3Scale(&dir, -dot);
    ccdVec3Add(&dir, _dir);
    len = CCD_SQRT(ccdVec3Len2(&dir));
    if (!ccdIsZero(len)) {
        ccdVec3Scale(&dir, cyl->radius / len);
        ccdVec3Add(v, &dir);
    }
}

static 
void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
    const ccd_sphere_t *s = (const ccd_sphere_t *)obj;

    ccdVec3Copy(v, _dir);
    ccdVec3Scale(v, s->radius);
    dIASSERT(dFabs(CCD_SQRT(ccdVec3Len2(_dir)) - REAL(1.0)) < 1e-6); // ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(_dir)));

    ccdVec3Add(v, &s->o.pos);
}

static 
void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
    const ccd_convex_t *c = (const ccd_convex_t *)obj;
    ccd_vec3_t dir, p;
    ccd_real_t maxdot, dot;
    sizeint i;
    const dReal *curp;

    ccdVec3Copy(&dir, _dir);
    ccdQuatRotVec(&dir, &c->o.rot_inv);

    maxdot = -CCD_REAL_MAX;
    curp = c->convex->points;
    for (i = 0; i < c->convex->pointcount; i++, curp += 3){
        ccdVec3Set(&p, curp[0], curp[1], curp[2]);
        dot = ccdVec3Dot(&dir, &p);
        if (dot > maxdot){
            ccdVec3Copy(v, &p);
            maxdot = dot;
        }
    }


    // transform support vertex
    ccdQuatRotVec(v, &c->o.rot);
    ccdVec3Add(v, &c->o.pos);
}

static 
void ccdCenter(const void *obj, ccd_vec3_t *c)
{
    const ccd_obj_t *o = (const ccd_obj_t *)obj;
    ccdVec3Copy(c, &o->pos);
}

static 
int ccdCollide(
    dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, int skip,
    void *obj1, ccd_support_fn supp1, ccd_center_fn cen1,
    void *obj2, ccd_support_fn supp2, ccd_center_fn cen2)
{
    ccd_t ccd;
    int res;
    ccd_real_t depth;
    ccd_vec3_t dir, pos;
    int max_contacts = (flags & NUMC_MASK);

    if (max_contacts < 1)
        return 0;

    CCD_INIT(&ccd);
    ccd.support1 = supp1;
    ccd.support2 = supp2;
    ccd.center1  = cen1;
    ccd.center2  = cen2;
    ccd.max_iterations = 500;
    ccd.mpr_tolerance = (ccd_real_t)1E-6;


    if (flags & CONTACTS_UNIMPORTANT){
        if (ccdMPRIntersect(obj1, obj2, &ccd)){
            return 1;
        }else{
            return 0;
        }
    }

    res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
    if (res == 0){
        contact->g1 = o1;
        contact->g2 = o2;

        contact->side1 = contact->side2 = -1;

        contact->depth = depth;

        contact->pos[0] = ccdVec3X(&pos);
        contact->pos[1] = ccdVec3Y(&pos);
        contact->pos[2] = ccdVec3Z(&pos);

        ccdVec3Scale(&dir, -1.);
        contact->normal[0] = ccdVec3X(&dir);
        contact->normal[1] = ccdVec3Y(&dir);
        contact->normal[2] = ccdVec3Z(&dir);

        return 1;
    }

    return 0;
}

/*extern */
int dCollideBoxCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_cyl_t cyl;
    ccd_box_t box;

    ccdGeomToBox(o1, &box);
    ccdGeomToCyl(o2, &cyl);

    return ccdCollide(o1, o2, flags, contact, skip,
        &box, ccdSupportBox, ccdCenter,
        &cyl, ccdSupportCyl, ccdCenter);
}

/*extern */
int dCollideCapsuleCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_cap_t cap;
    ccd_cyl_t cyl;

    ccdGeomToCap(o1, &cap);
    ccdGeomToCyl(o2, &cyl);

    return ccdCollide(o1, o2, flags, contact, skip,
        &cap, ccdSupportCap, ccdCenter,
        &cyl, ccdSupportCyl, ccdCenter);
}

/*extern */
int dCollideConvexBoxCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_box_t box;
    ccd_convex_t conv;

    ccdGeomToConvex(o1, &conv);
    ccdGeomToBox(o2, &box);

    return ccdCollide(o1, o2, flags, contact, skip,
        &conv, ccdSupportConvex, ccdCenter,
        &box, ccdSupportBox, ccdCenter);
}

/*extern */
int dCollideConvexCapsuleCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_cap_t cap;
    ccd_convex_t conv;

    ccdGeomToConvex(o1, &conv);
    ccdGeomToCap(o2, &cap);

    return ccdCollide(o1, o2, flags, contact, skip,
        &conv, ccdSupportConvex, ccdCenter,
        &cap, ccdSupportCap, ccdCenter);
}

/*extern */
int dCollideConvexSphereCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_sphere_t sphere;
    ccd_convex_t conv;

    ccdGeomToConvex(o1, &conv);
    ccdGeomToSphere(o2, &sphere);

    return ccdCollide(o1, o2, flags, contact, skip,
        &conv, ccdSupportConvex, ccdCenter,
        &sphere, ccdSupportSphere, ccdCenter);
}

/*extern */
int dCollideConvexCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_cyl_t cyl;
    ccd_convex_t conv;

    ccdGeomToConvex(o1, &conv);
    ccdGeomToCyl(o2, &cyl);

    return ccdCollide(o1, o2, flags, contact, skip,
        &conv, ccdSupportConvex, ccdCenter,
        &cyl, ccdSupportCyl, ccdCenter);
}

/*extern */
int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_convex_t c1, c2;

    ccdGeomToConvex(o1, &c1);
    ccdGeomToConvex(o2, &c2);

    return ccdCollide(o1, o2, flags, contact, skip,
        &c1, ccdSupportConvex, ccdCenter,
        &c2, ccdSupportConvex, ccdCenter);
}


/*extern */
int dCollideCylinderCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    ccd_cyl_t cyl1, cyl2;
    
    ccdGeomToCyl(o1, &cyl1);
    ccdGeomToCyl(o2, &cyl2);
    
    int numContacts = collideCylCyl(o1, o2, &cyl1, &cyl2, flags, contact, skip);
    if (numContacts < 0) {
        numContacts = ccdCollide(o1, o2, flags, contact, skip,
                                 &cyl1, ccdSupportCyl, ccdCenter,
                                 &cyl2, ccdSupportCyl, ccdCenter);
    }
    return numContacts;
}

static 
int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip) 
{
    int maxContacts = (flags & NUMC_MASK);
    dAASSERT(maxContacts != 0);

    maxContacts = maxContacts > 8 ? 8 : maxContacts;
    
    dReal axesProd = dFabs(ccdVec3Dot(&cyl1->axis, &cyl2->axis));
    // Check if cylinders' axes are in line
    if (REAL(1.0) - axesProd < 1e-3f) {
        ccd_vec3_t p, proj;
        dReal r1, l1;
        dReal r2, l2;
        dGeomCylinderGetParams(o1, &r1, &l1);
        dGeomCylinderGetParams(o2, &r2, &l2);
        l1 *= 0.5f;
        l2 *= 0.5f;
       
        // Determine the cylinder with smaller radius (minCyl) and bigger radius (maxCyl) and their respective properties: radius, length
        bool r1IsMin;
        dReal rmin, rmax;
        ccd_cyl_t *minCyl, *maxCyl;
        if (r1 <= r2) {
            rmin = r1; rmax = r2;
            minCyl = cyl1; maxCyl = cyl2;
            r1IsMin = true;
        }
        else {
            rmin = r2; rmax = r1;
            minCyl = cyl2; maxCyl = cyl1;
            r1IsMin = false;
        }

        dReal lSum = l1 + l2;

        ccdVec3Copy(&p, &minCyl->o.pos);
        ccdVec3Sub(&p, &maxCyl->o.pos);
        dReal dot = ccdVec3Dot(&p, &maxCyl->axis);
        
        // Maximum possible contact depth
        dReal depth_v = lSum - dFabs(dot) + dSqrt(dMax(0, REAL(1.0) - axesProd * axesProd)) * rmin;
        if (depth_v < 0) {
            return 0;
        }

        // Project the smaller cylinder's center onto the larger cylinder's plane
        ccdVec3Copy(&proj, &maxCyl->axis);
        ccdVec3Scale(&proj, -dot);
        ccdVec3Add(&proj, &p);
        dReal radiiDiff = (dReal)sqrt(ccdVec3Len2(&proj));
        dReal depth_h = r1 + r2 - radiiDiff;

        // Check the distance between cylinders' centers
        if (depth_h < 0) {
            return 0;
        }

        // Check if "vertical" contact depth is less than "horizontal" contact depth
        if (depth_v < depth_h) {
            int contactCount = 0;
            dReal dot2 = -ccdVec3Dot(&p, &minCyl->axis);
            // lmin, lmax - distances from cylinders' centers to potential contact points relative to cylinders' axes
            dReal lmax = r1IsMin ? l2 : l1;
            dReal lmin = r1IsMin ? l1 : l2;
            lmin = dot2 < 0 ? -lmin : lmin;
            lmax = dot < 0 ? -lmax : lmax;
            // Contact normal direction, relative to o1's axis
            dReal normaldir = (dot < 0) != r1IsMin ? REAL(1.0) : -REAL(1.0);
            
            if (rmin + radiiDiff <= rmax) {
                // Case 1: The smaller disc is fully contained within the larger one
                // Simply generate N points on the rim of the smaller disc
                dReal maxContactsRecip = (dReal)(0 < maxContacts ? (2.0 * M_PI / maxContacts) : (2.0 * M_PI)); // The 'else' value does not matter. Just try helping the optimizer.
                for (int i = 0; i < maxContacts; i++) {
                    dReal depth;
                    dReal a = maxContactsRecip * i;
                    if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) {
                        contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip);
                        if ((flags & CONTACTS_UNIMPORTANT) != 0) {
                            dIASSERT(contactCount != 0);
                            break;
                        }
                    }
                }
                return contactCount;

            } else {
                // Case 2: Discs intersect
                // Firstly, find intersections assuming the larger cylinder is placed at (0,0,0)
                // http://math.stackexchange.com/questions/256100/how-can-i-find-the-points-at-which-two-circles-intersect
                ccd_vec3_t proj2;
                ccdVec3Copy(&proj2, &proj);
                ccdQuatRotVec(&proj, &maxCyl->o.rot_inv);
                dReal d = dSqrt(ccdVec3X(&proj) * ccdVec3X(&proj) + ccdVec3Y(&proj) * ccdVec3Y(&proj));
                dIASSERT(d != REAL(0.0));
                
                dReal dRecip = REAL(1.0) / d;
                dReal rmaxSquare = rmax * rmax, rminSquare = rmin * rmin, dSquare = d * d;

                dReal minA, diffA, minB, diffB;

                {
                    dReal l = (rmaxSquare - rminSquare + dSquare) * (REAL(0.5) * dRecip);
                    dReal h = dSqrt(rmaxSquare - l * l);
                    dReal divLbyD = l * dRecip, divHbyD = h * dRecip;
                    dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj);
                    dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj);
                    dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj);
                    dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj);
                    // Map the intersection points to angles
                    dReal ap1 = dAtan2(y1, x1);
                    dReal ap2 = dAtan2(y2, x2);
                    minA = dMin(ap1, ap2);
                    dReal maxA = dMax(ap1, ap2);
                    // If the segment connecting cylinders' centers does not intersect the arc, change the angles
                    dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj));
                    if (a < minA || a > maxA) {
                        a = maxA;
                        maxA = (dReal)(minA + M_PI * 2.0);
                        minA = a;
                    }
                    diffA = maxA - minA;
                }
                
                // Do the same for the smaller cylinder assuming it is placed at (0,0,0) now
                ccdVec3Copy(&proj, &proj2);
                ccdVec3Scale(&proj, -1);
                ccdQuatRotVec(&proj, &minCyl->o.rot_inv);
                
                {
                    dReal l = (rminSquare - rmaxSquare + dSquare) * (REAL(0.5) * dRecip);
                    dReal h = dSqrt(rminSquare - l * l);
                    dReal divLbyD = l * dRecip, divHbyD = h * dRecip;
                    dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj);
                    dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj);
                    dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj);
                    dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj);
                    dReal ap1 = dAtan2(y1, x1);
                    dReal ap2 = dAtan2(y2, x2);
                    minB = dMin(ap1, ap2);
                    dReal maxB = dMax(ap1, ap2);
                    dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj));
                    if (a < minB || a > maxB) {
                        a = maxB;
                        maxB = (dReal)(minB + M_PI * 2.0);
                        minB = a;
                    }
                    diffB = maxB - minB;
                }

                // Find contact point distribution ratio based on arcs lengths
                dReal ratio = diffA * rmax  / (diffA * rmax + diffB  * rmin);
                dIASSERT(ratio <= REAL(1.0)); 
                dIASSERT(ratio >= REAL(0.0));

                int nMax = (int)dFloor(ratio * maxContacts + REAL(0.5));
                int nMin = maxContacts - nMax;
                dIASSERT(nMax <= maxContacts);

                // Make sure there is at least one point on the smaller radius rim
                if (nMin < 1) {
                    nMin = 1; nMax -= 1;
                }
                // Otherwise transfer one point to the larger radius rim as it is going to fill the rim intersection points
                else if (nMin > 1) {
                    nMin -= 1; nMax += 1;
                }

                // Smaller disc first, skipping the overlapping points
                dReal nMinRecip = 0 < nMin ? diffB / (nMin + 1) : diffB; // The 'else' value does not matter. Just try helping the optimizer.
                for (int i = 1; i <= nMin; i++) {
                    dReal depth;
                    dReal a = minB + nMinRecip * i;
                    if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) {
                        contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip);
                        if ((flags & CONTACTS_UNIMPORTANT) != 0) {
                            dIASSERT(contactCount != 0);
                            break;
                        }
                    }
                }

                if (contactCount == 0 || (flags & CONTACTS_UNIMPORTANT) == 0) {
                    // Then the larger disc, + additional point as the start/end points of arcs overlap
                    // (or a single contact at the arc middle point if just one is required)
                    dReal nMaxRecip = nMax > 1 ? diffA / (nMax - 1) : diffA; // The 'else' value does not matter. Just try helping the optimizer.
                    dReal adjustedMinA = nMax == 1 ? minA + REAL(0.5) * diffA : minA;

                    for (int i = 0; i < nMax; i++) {
                        dReal depth;
                        dReal a = adjustedMinA + nMaxRecip * i;
                        if (testAndPrepareDiscContactForAngle(a, rmax, lmax, lSum, maxCyl, minCyl, p, depth)) {
                            contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip);
                            if ((flags & CONTACTS_UNIMPORTANT) != 0) {
                                dIASSERT(contactCount != 0);
                                break;
                            }
                        }
                    }
                }

                return contactCount;
            }
        }
    }
    return -1;
}

static 
bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth)
{
    bool ret = false;

    ccd_vec3_t p2;
    ccdVec3Set(&p, dCos(angle) * radius, dSin(angle) * radius, 0);
    ccdQuatRotVec(&p, &priCyl->o.rot);
    ccdVec3Add(&p, &priCyl->o.pos);
    ccdVec3Copy(&p2, &p);
    ccdVec3Sub(&p2, &secCyl->o.pos);
    dReal depth = lSum - dFabs(ccdVec3Dot(&p2, &secCyl->axis));

    if (depth >= 0) {
        ccdVec3Copy(&p2, &priCyl->axis);
        ccdVec3Scale(&p2, length);
        ccdVec3Add(&p, &p2);

        out_depth = depth;
        ret = true;
    }

    return ret;
}

static 
int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts,
               ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip)
{
    dIASSERT(depth >= 0);

    dContactGeom* contact = SAFECONTACT(flags, contacts, j, skip);
    contact->g1 = o1;
    contact->g2 = o2;
    contact->side1 = -1;
    contact->side2 = -1;
    contact->normal[0] = normaldir * ccdVec3X(axis);
    contact->normal[1] = normaldir * ccdVec3Y(axis);
    contact->normal[2] = normaldir * ccdVec3Z(axis);
    contact->depth = depth;
    contact->pos[0] = ccdVec3X(p);
    contact->pos[1] = ccdVec3Y(p);
    contact->pos[2] = ccdVec3Z(p);

    return j + 1;
}


#if dTRIMESH_ENABLED

const static float CONTACT_DEPTH_EPSILON = 0.0001f;
const static float CONTACT_POS_EPSILON = 0.0001f;
const static float CONTACT_PERTURBATION_ANGLE = 0.001f;
const static float NORMAL_PROJ_EPSILON = 0.0001f;


/*extern */
unsigned dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip)
{
    ccd_convex_t c1;
    ccd_triangle_t c2;
    dVector3 triangle[dMTV__MAX];
    unsigned maxContacts = (flags & NUMC_MASK);
    unsigned contactCount = 0;
    ccdGeomToConvex(o1, &c1);
    ccdGeomToObj(o2, (ccd_obj_t *)&c2);

    IFaceAngleStorageView *meshFaceAngleView = dxGeomTriMeshGetFaceAngleView(o2);
    dUASSERT(meshFaceAngleView != NULL, "Please preprocess the trimesh data with dTRIDATAPREPROCESS_BUILD_FACE_ANGLES");

    for (unsigned i = 0; i != numIndices; ++i) {
        dContactGeom tempContact;
        dGeomTriMeshGetTriangle(o2, indices[i], &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]);

        for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) {
            ccdVec3Set(&c2.vertices[j], (ccd_real_t)triangle[j][dV3E_X], (ccd_real_t)triangle[j][dV3E_Y], (ccd_real_t)triangle[j][dV3E_Z]);
        }

        setObjPosToTriangleCenter(&c2);

        if (ccdCollide(o1, o2, flags, &tempContact, skip, &c1, &ccdSupportConvex, &ccdCenter, &c2, &ccdSupportTriangle, &ccdCenter) == 1) {
            tempContact.side2 = i;
            
            if (meshFaceAngleView == NULL || correctTriangleContactNormal(&c2, &tempContact, meshFaceAngleView, indices, numIndices)) {
                contactCount = addUniqueContact(contacts, &tempContact, contactCount, maxContacts, flags, skip);

                if ((flags & CONTACTS_UNIMPORTANT) != 0) {
                    break;
                }
            }
        }
    }

    if ((flags & CONTACTS_UNIMPORTANT) == 0 && contactCount == 1) {
        dContactGeom *contact = SAFECONTACT(flags, contacts, 0, skip);
        dGeomTriMeshGetTriangle(o2, contact->side2, &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]);
        contactCount = addTrianglePerturbedContacts(o1, o2, meshFaceAngleView, indices, numIndices, flags, contacts, skip, &c1, &c2, triangle, contact, contactCount);
    }

    // Normalize accumulated normals, if necessary
    for (unsigned k = 0; k != contactCount; ) {
        dContactGeom *contact = SAFECONTACT(flags, contacts, k, skip);
        bool stayWithinThisIndex = false;

        // Only the merged contact normals need to be normalized
        if (*_const_type_cast_union<bool>(&contact->normal[dV3E_PAD])) {
        
            if (!dxSafeNormalize3(contact->normal)) {
                // If the contact normals have added up to zero, erase the contact
                // Normally the time step is to be shorter so that the objects do not get into each other that deep
                --contactCount;

                if (k != contactCount) {
                    dContactGeom *lastContact = SAFECONTACT(flags, contacts, contactCount, skip);
                    *contact = *lastContact;
                }

                stayWithinThisIndex = true;
            }
        }

        if (!stayWithinThisIndex) {
            ++k;
        }
    }

    return contactCount;
}

static 
unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, 
    const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip,
    ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount)
{
    unsigned maxContacts = (flags & NUMC_MASK);
    
    dVector3 pos;
    dCopyVector3(pos, contact->pos);

    dQuaternion q1[2], q2[2];
    dReal perturbationAngle = CONTACT_PERTURBATION_ANGLE;

    dVector3 upAxis;
    bool upAvailable = false;
    if (fabs(contact->normal[dV3E_Y]) > 0.7) {
        dAssignVector3(upAxis, 0, 0, 1);
    }
    else {
        dAssignVector3(upAxis, 0, 1, 0);
    }

    dVector3 cross;
    dCalcVectorCross3(cross, contact->normal, upAxis);
    
    if (dSafeNormalize3(cross)) {
        dCalcVectorCross3(upAxis, cross, contact->normal);

        if (dSafeNormalize3(upAxis)) {
            upAvailable = true;
        }
    }

    for (unsigned j = upAvailable ? 0 : 2; j != 2; ++j) {
        dQFromAxisAndAngle(q1[j], upAxis[dV3E_X], upAxis[dV3E_Y], upAxis[dV3E_Z], perturbationAngle);
        dQFromAxisAndAngle(q2[j], cross[dV3E_X], cross[dV3E_Y], cross[dV3E_Z], perturbationAngle);
        perturbationAngle = -perturbationAngle;
    }

    for (unsigned k = upAvailable ? 0 : 4; k != 4; ++k) {
        dQuaternion qr;
        dQMultiply0(qr, q1[k % 2], q2[k / 2]);

        for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) {
            dVector3 p, perturbed;
            dSubtractVectors3(p, triangle[j], pos);
            dQuatTransform(qr, p, perturbed);
            dAddVectors3(perturbed, perturbed, pos);

            ccdVec3Set(&c2->vertices[j], (ccd_real_t)perturbed[dV3E_X], (ccd_real_t)perturbed[dV3E_Y], (ccd_real_t)perturbed[dV3E_Z]);
        }

        dContactGeom perturbedContact;
        setObjPosToTriangleCenter(c2);

        if (ccdCollide(o1, o2, flags, &perturbedContact, skip, c1, &ccdSupportConvex, &ccdCenter, c2, &ccdSupportTriangle, &ccdCenter) == 1) {
            perturbedContact.side2 = contact->side2;
            
            if (meshFaceAngleView == NULL || correctTriangleContactNormal(c2, &perturbedContact, meshFaceAngleView, indices, numIndices)) {
                contacCount = addUniqueContact(contacts, &perturbedContact, contacCount, maxContacts, flags, skip);
            }
        }
    }

    return contacCount;
}

static 
bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, 
    IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices) 
{
    dIASSERT(meshFaceAngleView != NULL);

    bool anyFault = false;

    ccd_vec3_t cntOrigNormal, cntNormal;
    ccdVec3Set(&cntNormal, contact->normal[0], contact->normal[1], contact->normal[2]);
    ccdVec3Copy(&cntOrigNormal, &cntNormal);

    // Check if the contact point is located close to any edge - move it back and forth
    // and check the resulting segment for intersection with the edge plane
    ccd_vec3_t cntScaledNormal;
    ccdVec3CopyScaled(&cntScaledNormal, &cntNormal, contact->depth);

    ccd_vec3_t edges[dMTV__MAX];
    ccdVec3Sub2(&edges[dMTV_THIRD], &t->vertices[0], &t->vertices[2]);
    ccdVec3Sub2(&edges[dMTV_SECOND], &t->vertices[2], &t->vertices[1]);
    ccdVec3Sub2(&edges[dMTV_FIRST], &t->vertices[1], &t->vertices[0]);
    dSASSERT(dMTV__MAX == 3);

    bool contactGenerated = false, contactPreserved = false;
    // Triangle face normal
    ccd_vec3_t triNormal;
    ccdVec3Cross(&triNormal, &edges[dMTV_FIRST], &edges[dMTV_SECOND]);
    if (ccdVec3SafeNormalize(&triNormal) != 0) {
        anyFault = true;
    }

    // Check the edges to see if one of them is involved
    for (unsigned testEdgeIndex = !anyFault ? dMTV__MIN : dMTV__MAX; testEdgeIndex != dMTV__MAX; ++testEdgeIndex) {
        ccd_vec3_t edgeNormal, vertexToPos, v;
        ccd_vec3_t &edgeAxis = edges[testEdgeIndex]; 
        
        // Edge axis
        if (ccdVec3SafeNormalize(&edgeAxis) != 0) {
            // This should not happen normally as in the case on of edges is degenerated
            // the triangle normal calculation would have to fail above. If for some
            // reason the above calculation succeeds and this one would not, it is
            // OK to break as this point as well.
            anyFault = true;
            break;
        }
        
        // Edge Normal
        ccdVec3Cross(&edgeNormal, &edgeAxis, &triNormal);
        // ccdVec3Normalize(&edgeNormal); -- the two vectors above were already normalized and perpendicular

        // Check if the contact point is located close to any edge - move it back and forth
        // and check the resulting segment for intersection with the edge plane
        ccdVec3Set(&vertexToPos, contact->pos[0], contact->pos[1], contact->pos[2]);
        ccdVec3Sub(&vertexToPos, &t->vertices[testEdgeIndex]);
        ccdVec3Sub2(&v, &vertexToPos, &cntScaledNormal);
        
        if (ccdVec3Dot(&edgeNormal, &v) < 0) {
            ccdVec3Add2(&v, &vertexToPos, &cntScaledNormal);
            
            if (ccdVec3Dot(&edgeNormal, &v) > 0) {
                // This is an edge contact
    
                ccd_real_t x = ccdVec3Dot(&triNormal, &cntNormal);
                ccd_real_t y = ccdVec3Dot(&edgeNormal, &cntNormal);
                ccd_real_t contactNormalToTriangleNormalAngle = CCD_ATAN2(y, x);

                dReal angleValueAsDRead;
                FaceAngleDomain angleDomain = meshFaceAngleView->retrieveFacesAngleFromStorage(angleValueAsDRead, contact->side2, (dMeshTriangleVertex)testEdgeIndex);
                ccd_real_t angleValue = (ccd_real_t)angleValueAsDRead;

                ccd_real_t targetAngle;
                contactGenerated = false, contactPreserved = false; // re-assign to make optimizer's task easier

                if (angleDomain != FAD_CONCAVE) {
                    // Convex or flat - ensure the contact normal is within the allowed range
                    // formed by the two triangles' normals.
                    if (contactNormalToTriangleNormalAngle < CCD_ZERO) {
                        targetAngle = CCD_ZERO;
                    }
                    else if (contactNormalToTriangleNormalAngle > angleValue) {
                        targetAngle = angleValue;
                    }
                    else {
                        contactPreserved = true;
                    }
                }
                else {
                    // Concave - rotate the contact normal to the face angle bisect plane
                    // (or to triangle normal-edge plane if negative angles are not stored)
                    targetAngle = angleValue != 0 ? CCD_REAL(0.5) * angleValue : CCD_ZERO;
                    // There is little chance the normal will initially match the correct plane, but still, a small check could save lots of calculations
                    if (contactNormalToTriangleNormalAngle == targetAngle) {
                        contactPreserved = true;
                    }
                }

                if (!contactPreserved) {
                    ccd_quat_t q;
                    ccdQuatSetAngleAxis(&q, targetAngle - contactNormalToTriangleNormalAngle, &edgeAxis);
                    ccdQuatRotVec2(&cntNormal, &cntNormal, &q);
                    contactGenerated = true;
                }

                // Calculated successfully
                break;
            }
        }
    }

    if (!anyFault && !contactPreserved) {
        // No edge contact detected, set contact normal to triangle normal
        const ccd_vec3_t &cntNormalToUse = !contactGenerated ? triNormal : cntNormal;

        contact->normal[dV3E_X] = ccdVec3X(&cntNormalToUse);
        contact->normal[dV3E_Y] = ccdVec3Y(&cntNormalToUse);
        contact->normal[dV3E_Z] = ccdVec3Z(&cntNormalToUse);
        contact->depth *= CCD_FMAX(0.0, ccdVec3Dot(&cntOrigNormal, &cntNormalToUse));
    }

    bool result = !anyFault;
    return result;
}


static 
unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip)
{
    dReal minDepth = c->depth;
    unsigned index = contactcount;
    bool isDuplicate = false;

    dReal c_posX = c->pos[dV3E_X], c_posY = c->pos[dV3E_Y], c_posZ = c->pos[dV3E_Z];
    for (unsigned k = 0; k != contactcount; k++) {
        dContactGeom* pc = SAFECONTACT(flags, contacts, k, skip);
        
        if (fabs(c_posX - pc->pos[dV3E_X]) < CONTACT_POS_EPSILON
            && fabs(c_posY - pc->pos[dV3E_Y]) < CONTACT_POS_EPSILON
            && fabs(c_posZ - pc->pos[dV3E_Z]) < CONTACT_POS_EPSILON) {
                dSASSERT(dV3E__AXES_MAX - dV3E__AXES_MIN == 3);

                // Accumulate similar contacts
                dAddVectors3(pc->normal, pc->normal, c->normal);
                pc->depth = dMax(pc->depth, c->depth);
                *_type_cast_union<bool>(&pc->normal[dV3E_PAD]) = true; // Mark the contact as a merged one

                isDuplicate = true;
                break;
        }
        
        if (contactcount == maxcontacts && pc->depth < minDepth) {
            minDepth = pc->depth;
            index = k;
        }
    }

    if (!isDuplicate && index < maxcontacts) {
        dContactGeom* contact = SAFECONTACT(flags, contacts, index, skip);
        contact->g1 = c->g1;
        contact->g2 = c->g2;
        contact->depth = c->depth;
        contact->side1 = c->side1;
        contact->side2 = c->side2;
        dCopyVector3(contact->pos, c->pos);
        dCopyVector3(contact->normal, c->normal);
        *_type_cast_union<bool>(&contact->normal[dV3E_PAD]) = false; // Indicates whether the contact is merged or not
        contactcount = index == contactcount ? contactcount + 1 : contactcount;
    }

    return contactcount;
}

static 
void setObjPosToTriangleCenter(ccd_triangle_t *t) 
{
    ccdVec3Set(&t->o.pos, 0, 0, 0);
    for (int j = 0; j < 3; j++) {
        ccdVec3Add(&t->o.pos, &t->vertices[j]);
    }
    ccdVec3Scale(&t->o.pos, 1.0f / 3.0f);
}

static 
void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
    const ccd_triangle_t* o = (ccd_triangle_t *) obj;
    ccd_real_t maxdot, dot;
    maxdot = -CCD_REAL_MAX;
    for (unsigned i = 0; i != 3; i++) {
        dot = ccdVec3Dot(_dir, &o->vertices[i]);
        if (dot > maxdot) {
            ccdVec3Copy(v, &o->vertices[i]);
            maxdot = dot;
        }
    }
}


#endif // dTRIMESH_ENABLED