martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
rbdlsim/3rdparty/libccd/src/ccd.c

997 lines
31 KiB
C
Raw Blame History

/***
* libccd
* ---------------------------------
* Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
*
*
* This file is part of libccd.
*
* Distributed under the OSI-approved BSD License (the "License");
* see accompanying file BDS-LICENSE for details or see
* <http://www.opensource.org/licenses/bsd-license.php>.
*
* This software is distributed WITHOUT ANY WARRANTY; without even the
* implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the License for more information.
*/
#include <stdio.h>
#include <float.h>
#include <ccd/ccd.h>
#include <ccd/vec3.h>
#include "simplex.h"
#include "polytope.h"
#include "alloc.h"
#include "dbg.h"
/** Performs GJK algorithm. Returns 0 if intersection was found and simplex
* is filled with resulting polytope. */
static int __ccdGJK(const void *obj1, const void *obj2,
const ccd_t *ccd, ccd_simplex_t *simplex);
/** Performs GJK+EPA algorithm. Returns 0 if intersection was found and
* pt is filled with resulting polytope and nearest with pointer to
* nearest element (vertex, edge, face) of polytope to origin. */
static int __ccdGJKEPA(const void *obj1, const void *obj2,
const ccd_t *ccd,
ccd_pt_t *pt, ccd_pt_el_t **nearest);
/** Returns true if simplex contains origin.
* This function also alteres simplex and dir according to further
* processing of GJK algorithm. */
static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir);
static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir);
static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir);
static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir);
/** d = a x b x c */
_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
const ccd_vec3_t *c, ccd_vec3_t *d);
/** Transforms simplex to polytope. It is assumed that simplex has 4
* vertices. */
static int simplexToPolytope4(const void *obj1, const void *obj2,
const ccd_t *ccd,
ccd_simplex_t *simplex,
ccd_pt_t *pt, ccd_pt_el_t **nearest);
/** Transforms simplex to polytope, three vertices required */
static int simplexToPolytope3(const void *obj1, const void *obj2,
const ccd_t *ccd,
const ccd_simplex_t *simplex,
ccd_pt_t *pt, ccd_pt_el_t **nearest);
/** Transforms simplex to polytope, two vertices required */
static int simplexToPolytope2(const void *obj1, const void *obj2,
const ccd_t *ccd,
const ccd_simplex_t *simplex,
ccd_pt_t *pt, ccd_pt_el_t **nearest);
/** Expands polytope using new vertex v.
* Return 0 on success, -2 on memory allocation failure.*/
static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el,
const ccd_support_t *newv);
/** Finds next support point (at stores it in out argument).
* Returns 0 on success, -1 otherwise */
static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd,
const ccd_pt_el_t *el,
ccd_support_t *out);
void ccdFirstDirDefault(const void *o1, const void *o2, ccd_vec3_t *dir)
{
ccdVec3Set(dir, CCD_ONE, CCD_ZERO, CCD_ZERO);
}
int ccdGJKIntersect(const void *obj1, const void *obj2, const ccd_t *ccd)
{
ccd_simplex_t simplex;
return __ccdGJK(obj1, obj2, ccd, &simplex) == 0;
}
int ccdGJKSeparate(const void *obj1, const void *obj2, const ccd_t *ccd,
ccd_vec3_t *sep)
{
ccd_pt_t polytope;
ccd_pt_el_t *nearest;
int ret;
ccdPtInit(&polytope);
ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest);
// set separation vector
if (nearest)
ccdVec3Copy(sep, &nearest->witness);
ccdPtDestroy(&polytope);
return ret;
}
static int penEPAPosCmp(const void *a, const void *b)
{
ccd_pt_vertex_t *v1, *v2;
v1 = *(ccd_pt_vertex_t **)a;
v2 = *(ccd_pt_vertex_t **)b;
if (ccdEq(v1->dist, v2->dist)){
return 0;
}else if (v1->dist < v2->dist){
return -1;
}else{
return 1;
}
}
static int penEPAPos(const ccd_pt_t *pt, const ccd_pt_el_t *nearest,
ccd_vec3_t *pos)
{
ccd_pt_vertex_t *v;
ccd_pt_vertex_t **vs;
size_t i, len;
ccd_real_t scale;
// compute median
len = 0;
ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
len++;
}
vs = CCD_ALLOC_ARR(ccd_pt_vertex_t *, len);
if (vs == NULL)
return -1;
i = 0;
ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
vs[i++] = v;
}
qsort(vs, len, sizeof(ccd_pt_vertex_t *), penEPAPosCmp);
ccdVec3Set(pos, CCD_ZERO, CCD_ZERO, CCD_ZERO);
scale = CCD_ZERO;
if (len % 2 == 1)
len++;
for (i = 0; i < len / 2; i++){
ccdVec3Add(pos, &vs[i]->v.v1);
ccdVec3Add(pos, &vs[i]->v.v2);
scale += CCD_REAL(2.);
}
ccdVec3Scale(pos, CCD_ONE / scale);
free(vs);
return 0;
}
int ccdGJKPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
{
ccd_pt_t polytope;
ccd_pt_el_t *nearest;
int ret;
ccdPtInit(&polytope);
ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest);
// set separation vector
if (ret == 0 && nearest){
// compute depth of penetration
*depth = CCD_SQRT(nearest->dist);
// store normalized direction vector
ccdVec3Copy(dir, &nearest->witness);
ccdVec3Normalize(dir);
// compute position
if (penEPAPos(&polytope, nearest, pos) != 0){
ccdPtDestroy(&polytope);
return -2;
}
}
ccdPtDestroy(&polytope);
return ret;
}
static int __ccdGJK(const void *obj1, const void *obj2,
const ccd_t *ccd, ccd_simplex_t *simplex)
{
unsigned long iterations;
ccd_vec3_t dir; // direction vector
ccd_support_t last; // last support point
int do_simplex_res;
// initialize simplex struct
ccdSimplexInit(simplex);
// get first direction
ccd->first_dir(obj1, obj2, &dir);
// get first support point
__ccdSupport(obj1, obj2, &dir, ccd, &last);
// and add this point to simplex as last one
ccdSimplexAdd(simplex, &last);
// set up direction vector to as (O - last) which is exactly -last
ccdVec3Copy(&dir, &last.v);
ccdVec3Scale(&dir, -CCD_ONE);
// start iterations
for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
// obtain support point
__ccdSupport(obj1, obj2, &dir, ccd, &last);
// check if farthest point in Minkowski difference in direction dir
// isn't somewhere before origin (the test on negative dot product)
// - because if it is, objects are not intersecting at all.
if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){
return -1; // intersection not found
}
// add last support vector to simplex
ccdSimplexAdd(simplex, &last);
// if doSimplex returns 1 if objects intersect, -1 if objects don't
// intersect and 0 if algorithm should continue
do_simplex_res = doSimplex(simplex, &dir);
if (do_simplex_res == 1){
return 0; // intersection found
}else if (do_simplex_res == -1){
return -1; // intersection not found
}
if (ccdIsZero(ccdVec3Len2(&dir))){
return -1; // intersection not found
}
}
// intersection wasn't found
return -1;
}
static int __ccdGJKEPA(const void *obj1, const void *obj2,
const ccd_t *ccd,
ccd_pt_t *polytope, ccd_pt_el_t **nearest)
{
ccd_simplex_t simplex;
ccd_support_t supp; // support point
int ret, size;
*nearest = NULL;
// run GJK and obtain terminal simplex
ret = __ccdGJK(obj1, obj2, ccd, &simplex);
if (ret != 0)
return -1;
// transform simplex to polytope - simplex won't be used anymore
size = ccdSimplexSize(&simplex);
if (size == 4){
ret = simplexToPolytope4(obj1, obj2, ccd, &simplex, polytope, nearest);
}else if (size == 3){
ret = simplexToPolytope3(obj1, obj2, ccd, &simplex, polytope, nearest);
}else{ // size == 2
ret = simplexToPolytope2(obj1, obj2, ccd, &simplex, polytope, nearest);
}
if (ret == -1){
// touching contact
return 0;
}else if (ret == -2){
// failed memory allocation
return -2;
}
while (1){
// get triangle nearest to origin
*nearest = ccdPtNearest(polytope);
// get next support point
if (nextSupport(obj1, obj2, ccd, *nearest, &supp) != 0)
break;
// expand nearest triangle using new point - supp
if (expandPolytope(polytope, *nearest, &supp) != 0)
return -2;
}
return 0;
}
static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B;
ccd_vec3_t AB, AO, tmp;
ccd_real_t dot;
// get last added as A
A = ccdSimplexLast(simplex);
// get the other point
B = ccdSimplexPoint(simplex, 0);
// compute AB oriented segment
ccdVec3Sub2(&AB, &B->v, &A->v);
// compute AO vector
ccdVec3Copy(&AO, &A->v);
ccdVec3Scale(&AO, -CCD_ONE);
// dot product AB . AO
dot = ccdVec3Dot(&AB, &AO);
// check if origin doesn't lie on AB segment
ccdVec3Cross(&tmp, &AB, &AO);
if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
return 1;
}
// check if origin is in area where AB segment is
if (ccdIsZero(dot) || dot < CCD_ZERO){
// origin is in outside are of A
ccdSimplexSet(simplex, 0, A);
ccdSimplexSetSize(simplex, 1);
ccdVec3Copy(dir, &AO);
}else{
// origin is in area where AB segment is
// keep simplex untouched and set direction to
// AB x AO x AB
tripleCross(&AB, &AO, &AB, dir);
}
return 0;
}
static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B, *C;
ccd_vec3_t AO, AB, AC, ABC, tmp;
ccd_real_t dot, dist;
// get last added as A
A = ccdSimplexLast(simplex);
// get the other points
B = ccdSimplexPoint(simplex, 1);
C = ccdSimplexPoint(simplex, 0);
// check touching contact
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
if (ccdIsZero(dist)){
return 1;
}
// check if triangle is really triangle (has area > 0)
// if not simplex can't be expanded and thus no itersection is found
if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){
return -1;
}
// compute AO vector
ccdVec3Copy(&AO, &A->v);
ccdVec3Scale(&AO, -CCD_ONE);
// compute AB and AC segments and ABC vector (perpendircular to triangle)
ccdVec3Sub2(&AB, &B->v, &A->v);
ccdVec3Sub2(&AC, &C->v, &A->v);
ccdVec3Cross(&ABC, &AB, &AC);
ccdVec3Cross(&tmp, &ABC, &AC);
dot = ccdVec3Dot(&tmp, &AO);
if (ccdIsZero(dot) || dot > CCD_ZERO){
dot = ccdVec3Dot(&AC, &AO);
if (ccdIsZero(dot) || dot > CCD_ZERO){
// C is already in place
ccdSimplexSet(simplex, 1, A);
ccdSimplexSetSize(simplex, 2);
tripleCross(&AC, &AO, &AC, dir);
}else{
ccd_do_simplex3_45:
dot = ccdVec3Dot(&AB, &AO);
if (ccdIsZero(dot) || dot > CCD_ZERO){
ccdSimplexSet(simplex, 0, B);
ccdSimplexSet(simplex, 1, A);
ccdSimplexSetSize(simplex, 2);
tripleCross(&AB, &AO, &AB, dir);
}else{
ccdSimplexSet(simplex, 0, A);
ccdSimplexSetSize(simplex, 1);
ccdVec3Copy(dir, &AO);
}
}
}else{
ccdVec3Cross(&tmp, &AB, &ABC);
dot = ccdVec3Dot(&tmp, &AO);
if (ccdIsZero(dot) || dot > CCD_ZERO){
goto ccd_do_simplex3_45;
}else{
dot = ccdVec3Dot(&ABC, &AO);
if (ccdIsZero(dot) || dot > CCD_ZERO){
ccdVec3Copy(dir, &ABC);
}else{
ccd_support_t Ctmp;
ccdSupportCopy(&Ctmp, C);
ccdSimplexSet(simplex, 0, B);
ccdSimplexSet(simplex, 1, &Ctmp);
ccdVec3Copy(dir, &ABC);
ccdVec3Scale(dir, -CCD_ONE);
}
}
}
return 0;
}
static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B, *C, *D;
ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
int B_on_ACD, C_on_ADB, D_on_ABC;
int AB_O, AC_O, AD_O;
ccd_real_t dist;
// get last added as A
A = ccdSimplexLast(simplex);
// get the other points
B = ccdSimplexPoint(simplex, 2);
C = ccdSimplexPoint(simplex, 1);
D = ccdSimplexPoint(simplex, 0);
// check if tetrahedron is really tetrahedron (has volume > 0)
// if it is not simplex can't be expanded and thus no intersection is
// found
dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
if (ccdIsZero(dist)){
return -1;
}
// check if origin lies on some of tetrahedron's face - if so objects
// intersect
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
if (ccdIsZero(dist))
return 1;
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
if (ccdIsZero(dist))
return 1;
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
if (ccdIsZero(dist))
return 1;
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
if (ccdIsZero(dist))
return 1;
// compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
ccdVec3Copy(&AO, &A->v);
ccdVec3Scale(&AO, -CCD_ONE);
ccdVec3Sub2(&AB, &B->v, &A->v);
ccdVec3Sub2(&AC, &C->v, &A->v);
ccdVec3Sub2(&AD, &D->v, &A->v);
ccdVec3Cross(&ABC, &AB, &AC);
ccdVec3Cross(&ACD, &AC, &AD);
ccdVec3Cross(&ADB, &AD, &AB);
// side (positive or negative) of B, C, D relative to planes ACD, ADB
// and ABC respectively
B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
// whether origin is on same side of ACD, ADB, ABC as B, C, D
// respectively
AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
if (AB_O && AC_O && AD_O){
// origin is in tetrahedron
return 1;
// rearrange simplex to triangle and call doSimplex3()
}else if (!AB_O){
// B is farthest from the origin among all of the tetrahedron's
// points, so remove it from the list and go on with the triangle
// case
// D and C are in place
ccdSimplexSet(simplex, 2, A);
ccdSimplexSetSize(simplex, 3);
}else if (!AC_O){
// C is farthest
ccdSimplexSet(simplex, 1, D);
ccdSimplexSet(simplex, 0, B);
ccdSimplexSet(simplex, 2, A);
ccdSimplexSetSize(simplex, 3);
}else{ // (!AD_O)
ccdSimplexSet(simplex, 0, C);
ccdSimplexSet(simplex, 1, B);
ccdSimplexSet(simplex, 2, A);
ccdSimplexSetSize(simplex, 3);
}
return doSimplex3(simplex, dir);
}
static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
if (ccdSimplexSize(simplex) == 2){
// simplex contains segment only one segment
return doSimplex2(simplex, dir);
}else if (ccdSimplexSize(simplex) == 3){
// simplex contains triangle
return doSimplex3(simplex, dir);
}else{ // ccdSimplexSize(simplex) == 4
// tetrahedron - this is the only shape which can encapsule origin
// so doSimplex4() also contains test on it
return doSimplex4(simplex, dir);
}
}
_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
const ccd_vec3_t *c, ccd_vec3_t *d)
{
ccd_vec3_t e;
ccdVec3Cross(&e, a, b);
ccdVec3Cross(d, &e, c);
}
/** Transforms simplex to polytope. It is assumed that simplex has 4
* vertices! */
static int simplexToPolytope4(const void *obj1, const void *obj2,
const ccd_t *ccd,
ccd_simplex_t *simplex,
ccd_pt_t *pt, ccd_pt_el_t **nearest)
{
const ccd_support_t *a, *b, *c, *d;
int use_polytope3;
ccd_real_t dist;
ccd_pt_vertex_t *v[4];
ccd_pt_edge_t *e[6];
size_t i;
a = ccdSimplexPoint(simplex, 0);
b = ccdSimplexPoint(simplex, 1);
c = ccdSimplexPoint(simplex, 2);
d = ccdSimplexPoint(simplex, 3);
// check if origin lies on some of tetrahedron's face - if so use
// simplexToPolytope3()
use_polytope3 = 0;
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL);
if (ccdIsZero(dist)){
use_polytope3 = 1;
}
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL);
if (ccdIsZero(dist)){
use_polytope3 = 1;
ccdSimplexSet(simplex, 1, c);
ccdSimplexSet(simplex, 2, d);
}
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &d->v, NULL);
if (ccdIsZero(dist)){
use_polytope3 = 1;
ccdSimplexSet(simplex, 2, d);
}
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &b->v, &c->v, &d->v, NULL);
if (ccdIsZero(dist)){
use_polytope3 = 1;
ccdSimplexSet(simplex, 0, b);
ccdSimplexSet(simplex, 1, c);
ccdSimplexSet(simplex, 2, d);
}
if (use_polytope3){
ccdSimplexSetSize(simplex, 3);
return simplexToPolytope3(obj1, obj2, ccd, simplex, pt, nearest);
}
// no touching contact - simply create tetrahedron
for (i = 0; i < 4; i++){
v[i] = ccdPtAddVertex(pt, ccdSimplexPoint(simplex, i));
}
e[0] = ccdPtAddEdge(pt, v[0], v[1]);
e[1] = ccdPtAddEdge(pt, v[1], v[2]);
e[2] = ccdPtAddEdge(pt, v[2], v[0]);
e[3] = ccdPtAddEdge(pt, v[3], v[0]);
e[4] = ccdPtAddEdge(pt, v[3], v[1]);
e[5] = ccdPtAddEdge(pt, v[3], v[2]);
// ccdPtAdd*() functions return NULL either if the memory allocation
// failed of if any of the input pointers are NULL, so the bad
// allocation can be checked by the last calls of ccdPtAddFace()
// because the rest of the bad allocations eventually "bubble up" here
if (ccdPtAddFace(pt, e[0], e[1], e[2]) == NULL
|| ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL
|| ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL
|| ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){
return -2;
}
return 0;
}
/** Transforms simplex to polytope, three vertices required */
static int simplexToPolytope3(const void *obj1, const void *obj2,
const ccd_t *ccd,
const ccd_simplex_t *simplex,
ccd_pt_t *pt, ccd_pt_el_t **nearest)
{
const ccd_support_t *a, *b, *c;
ccd_support_t d, d2;
ccd_vec3_t ab, ac, dir;
ccd_pt_vertex_t *v[5];
ccd_pt_edge_t *e[9];
ccd_real_t dist, dist2;
*nearest = NULL;
a = ccdSimplexPoint(simplex, 0);
b = ccdSimplexPoint(simplex, 1);
c = ccdSimplexPoint(simplex, 2);
// If only one triangle left from previous GJK run origin lies on this
// triangle. So it is necessary to expand triangle into two
// tetrahedrons connected with base (which is exactly abc triangle).
// get next support point in direction of normal of triangle
ccdVec3Sub2(&ab, &b->v, &a->v);
ccdVec3Sub2(&ac, &c->v, &a->v);
ccdVec3Cross(&dir, &ab, &ac);
__ccdSupport(obj1, obj2, &dir, ccd, &d);
dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL);
// and second one take in opposite direction
ccdVec3Scale(&dir, -CCD_ONE);
__ccdSupport(obj1, obj2, &dir, ccd, &d2);
dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL);
// check if face isn't already on edge of minkowski sum and thus we
// have touching contact
if (ccdIsZero(dist) || ccdIsZero(dist2)){
v[0] = ccdPtAddVertex(pt, a);
v[1] = ccdPtAddVertex(pt, b);
v[2] = ccdPtAddVertex(pt, c);
e[0] = ccdPtAddEdge(pt, v[0], v[1]);
e[1] = ccdPtAddEdge(pt, v[1], v[2]);
e[2] = ccdPtAddEdge(pt, v[2], v[0]);
*nearest = (ccd_pt_el_t *)ccdPtAddFace(pt, e[0], e[1], e[2]);
if (*nearest == NULL)
return -2;
return -1;
}
// form polyhedron
v[0] = ccdPtAddVertex(pt, a);
v[1] = ccdPtAddVertex(pt, b);
v[2] = ccdPtAddVertex(pt, c);
v[3] = ccdPtAddVertex(pt, &d);
v[4] = ccdPtAddVertex(pt, &d2);
e[0] = ccdPtAddEdge(pt, v[0], v[1]);
e[1] = ccdPtAddEdge(pt, v[1], v[2]);
e[2] = ccdPtAddEdge(pt, v[2], v[0]);
e[3] = ccdPtAddEdge(pt, v[3], v[0]);
e[4] = ccdPtAddEdge(pt, v[3], v[1]);
e[5] = ccdPtAddEdge(pt, v[3], v[2]);
e[6] = ccdPtAddEdge(pt, v[4], v[0]);
e[7] = ccdPtAddEdge(pt, v[4], v[1]);
e[8] = ccdPtAddEdge(pt, v[4], v[2]);
if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL
|| ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL
|| ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL
|| ccdPtAddFace(pt, e[6], e[7], e[0]) == NULL
|| ccdPtAddFace(pt, e[7], e[8], e[1]) == NULL
|| ccdPtAddFace(pt, e[8], e[6], e[2]) == NULL){
return -2;
}
return 0;
}
/** Transforms simplex to polytope, two vertices required */
static int simplexToPolytope2(const void *obj1, const void *obj2,
const ccd_t *ccd,
const ccd_simplex_t *simplex,
ccd_pt_t *pt, ccd_pt_el_t **nearest)
{
const ccd_support_t *a, *b;
ccd_vec3_t ab, ac, dir;
ccd_support_t supp[4];
ccd_pt_vertex_t *v[6];
ccd_pt_edge_t *e[12];
size_t i;
int found;
a = ccdSimplexPoint(simplex, 0);
b = ccdSimplexPoint(simplex, 1);
// This situation is a bit tricky. If only one segment comes from
// previous run of GJK - it means that either this segment is on
// minkowski edge (and thus we have touch contact) or it it isn't and
// therefore segment is somewhere *inside* minkowski sum and it *must*
// be possible to fully enclose this segment with polyhedron formed by
// at least 8 triangle faces.
// get first support point (any)
found = 0;
for (i = 0; i < ccd_points_on_sphere_len; i++){
__ccdSupport(obj1, obj2, &ccd_points_on_sphere[i], ccd, &supp[0]);
if (!ccdVec3Eq(&a->v, &supp[0].v) && !ccdVec3Eq(&b->v, &supp[0].v)){
found = 1;
break;
}
}
if (!found)
goto simplexToPolytope2_touching_contact;
// get second support point in opposite direction than supp[0]
ccdVec3Copy(&dir, &supp[0].v);
ccdVec3Scale(&dir, -CCD_ONE);
__ccdSupport(obj1, obj2, &dir, ccd, &supp[1]);
if (ccdVec3Eq(&a->v, &supp[1].v) || ccdVec3Eq(&b->v, &supp[1].v))
goto simplexToPolytope2_touching_contact;
// next will be in direction of normal of triangle a,supp[0],supp[1]
ccdVec3Sub2(&ab, &supp[0].v, &a->v);
ccdVec3Sub2(&ac, &supp[1].v, &a->v);
ccdVec3Cross(&dir, &ab, &ac);
__ccdSupport(obj1, obj2, &dir, ccd, &supp[2]);
if (ccdVec3Eq(&a->v, &supp[2].v) || ccdVec3Eq(&b->v, &supp[2].v))
goto simplexToPolytope2_touching_contact;
// and last one will be in opposite direction
ccdVec3Scale(&dir, -CCD_ONE);
__ccdSupport(obj1, obj2, &dir, ccd, &supp[3]);
if (ccdVec3Eq(&a->v, &supp[3].v) || ccdVec3Eq(&b->v, &supp[3].v))
goto simplexToPolytope2_touching_contact;
goto simplexToPolytope2_not_touching_contact;
simplexToPolytope2_touching_contact:
v[0] = ccdPtAddVertex(pt, a);
v[1] = ccdPtAddVertex(pt, b);
*nearest = (ccd_pt_el_t *)ccdPtAddEdge(pt, v[0], v[1]);
if (*nearest == NULL)
return -2;
return -1;
simplexToPolytope2_not_touching_contact:
// form polyhedron
v[0] = ccdPtAddVertex(pt, a);
v[1] = ccdPtAddVertex(pt, &supp[0]);
v[2] = ccdPtAddVertex(pt, b);
v[3] = ccdPtAddVertex(pt, &supp[1]);
v[4] = ccdPtAddVertex(pt, &supp[2]);
v[5] = ccdPtAddVertex(pt, &supp[3]);
e[0] = ccdPtAddEdge(pt, v[0], v[1]);
e[1] = ccdPtAddEdge(pt, v[1], v[2]);
e[2] = ccdPtAddEdge(pt, v[2], v[3]);
e[3] = ccdPtAddEdge(pt, v[3], v[0]);
e[4] = ccdPtAddEdge(pt, v[4], v[0]);
e[5] = ccdPtAddEdge(pt, v[4], v[1]);
e[6] = ccdPtAddEdge(pt, v[4], v[2]);
e[7] = ccdPtAddEdge(pt, v[4], v[3]);
e[8] = ccdPtAddEdge(pt, v[5], v[0]);
e[9] = ccdPtAddEdge(pt, v[5], v[1]);
e[10] = ccdPtAddEdge(pt, v[5], v[2]);
e[11] = ccdPtAddEdge(pt, v[5], v[3]);
if (ccdPtAddFace(pt, e[4], e[5], e[0]) == NULL
|| ccdPtAddFace(pt, e[5], e[6], e[1]) == NULL
|| ccdPtAddFace(pt, e[6], e[7], e[2]) == NULL
|| ccdPtAddFace(pt, e[7], e[4], e[3]) == NULL
|| ccdPtAddFace(pt, e[8], e[9], e[0]) == NULL
|| ccdPtAddFace(pt, e[9], e[10], e[1]) == NULL
|| ccdPtAddFace(pt, e[10], e[11], e[2]) == NULL
|| ccdPtAddFace(pt, e[11], e[8], e[3]) == NULL){
return -2;
}
return 0;
}
/** Expands polytope's tri by new vertex v. Triangle tri is replaced by
* three triangles each with one vertex in v. */
static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el,
const ccd_support_t *newv)
{
ccd_pt_vertex_t *v[5];
ccd_pt_edge_t *e[8];
ccd_pt_face_t *f[2];
// element can be either segment or triangle
if (el->type == CCD_PT_EDGE){
// In this case, segment should be replaced by new point.
// Simpliest case is when segment stands alone and in this case
// this segment is replaced by two other segments both connected to
// newv.
// Segment can be also connected to max two faces and in that case
// each face must be replaced by two other faces. To do this
// correctly it is necessary to have correctly ordered edges and
// vertices which is exactly what is done in following code.
//
ccdPtEdgeVertices((const ccd_pt_edge_t *)el, &v[0], &v[2]);
ccdPtEdgeFaces((ccd_pt_edge_t *)el, &f[0], &f[1]);
if (f[0]){
ccdPtFaceEdges(f[0], &e[0], &e[1], &e[2]);
if (e[0] == (ccd_pt_edge_t *)el){
e[0] = e[2];
}else if (e[1] == (ccd_pt_edge_t *)el){
e[1] = e[2];
}
ccdPtEdgeVertices(e[0], &v[1], &v[3]);
if (v[1] != v[0] && v[3] != v[0]){
e[2] = e[0];
e[0] = e[1];
e[1] = e[2];
if (v[1] == v[2])
v[1] = v[3];
}else{
if (v[1] == v[0])
v[1] = v[3];
}
if (f[1]){
ccdPtFaceEdges(f[1], &e[2], &e[3], &e[4]);
if (e[2] == (ccd_pt_edge_t *)el){
e[2] = e[4];
}else if (e[3] == (ccd_pt_edge_t *)el){
e[3] = e[4];
}
ccdPtEdgeVertices(e[2], &v[3], &v[4]);
if (v[3] != v[2] && v[4] != v[2]){
e[4] = e[2];
e[2] = e[3];
e[3] = e[4];
if (v[3] == v[0])
v[3] = v[4];
}else{
if (v[3] == v[2])
v[3] = v[4];
}
}
v[4] = ccdPtAddVertex(pt, newv);
ccdPtDelFace(pt, f[0]);
if (f[1]){
ccdPtDelFace(pt, f[1]);
ccdPtDelEdge(pt, (ccd_pt_edge_t *)el);
}
e[4] = ccdPtAddEdge(pt, v[4], v[2]);
e[5] = ccdPtAddEdge(pt, v[4], v[0]);
e[6] = ccdPtAddEdge(pt, v[4], v[1]);
if (f[1])
e[7] = ccdPtAddEdge(pt, v[4], v[3]);
if (ccdPtAddFace(pt, e[1], e[4], e[6]) == NULL
|| ccdPtAddFace(pt, e[0], e[6], e[5]) == NULL){
return -2;
}
if (f[1]){
if (ccdPtAddFace(pt, e[3], e[5], e[7]) == NULL
|| ccdPtAddFace(pt, e[4], e[7], e[2]) == NULL){
return -2;
}
}else{
if (ccdPtAddFace(pt, e[4], e[5], (ccd_pt_edge_t *)el) == NULL)
return -2;
}
}
}else{ // el->type == CCD_PT_FACE
// replace triangle by tetrahedron without base (base would be the
// triangle that will be removed)
// get triplet of surrounding edges and vertices of triangle face
ccdPtFaceEdges((const ccd_pt_face_t *)el, &e[0], &e[1], &e[2]);
ccdPtEdgeVertices(e[0], &v[0], &v[1]);
ccdPtEdgeVertices(e[1], &v[2], &v[3]);
// following code sorts edges to have e[0] between vertices 0-1,
// e[1] between 1-2 and e[2] between 2-0
if (v[2] != v[1] && v[3] != v[1]){
// swap e[1] and e[2]
e[3] = e[1];
e[1] = e[2];
e[2] = e[3];
}
if (v[3] != v[0] && v[3] != v[1])
v[2] = v[3];
// remove triangle face
ccdPtDelFace(pt, (ccd_pt_face_t *)el);
// expand triangle to tetrahedron
v[3] = ccdPtAddVertex(pt, newv);
e[3] = ccdPtAddEdge(pt, v[3], v[0]);
e[4] = ccdPtAddEdge(pt, v[3], v[1]);
e[5] = ccdPtAddEdge(pt, v[3], v[2]);
if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL
|| ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL
|| ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){
return -2;
}
}
return 0;
}
/** Finds next support point (and stores it in out argument).
* Returns 0 on success, -1 otherwise */
static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd,
const ccd_pt_el_t *el,
ccd_support_t *out)
{
ccd_vec3_t *a, *b, *c;
ccd_real_t dist;
if (el->type == CCD_PT_VERTEX)
return -1;
// touch contact
if (ccdIsZero(el->dist))
return -1;
__ccdSupport(obj1, obj2, &el->witness, ccd, out);
// Compute dist of support point along element witness point direction
// so we can determine whether we expanded a polytope surrounding the
// origin a bit.
dist = ccdVec3Dot(&out->v, &el->witness);
if (dist - el->dist < ccd->epa_tolerance)
return -1;
if (el->type == CCD_PT_EDGE){
// fetch end points of edge
ccdPtEdgeVec3((ccd_pt_edge_t *)el, &a, &b);
// get distance from segment
dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL);
}else{ // el->type == CCD_PT_FACE
// fetch vertices of triangle face
ccdPtFaceVec3((ccd_pt_face_t *)el, &a, &b, &c);
// check if new point can significantly expand polytope
dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL);
}
if (dist < ccd->epa_tolerance)
return -1;
return 0;
}
Reference in New Issue View Git Blame Copy Permalink