diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/collision_libccd.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/collision_libccd.cpp | 1080 |
1 files changed, 1080 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/collision_libccd.cpp b/libs/ode-0.16.1/ode/src/collision_libccd.cpp new file mode 100644 index 0000000..ba15e83 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_libccd.cpp @@ -0,0 +1,1080 @@ +/************************************************************************* + * * + * 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 |