From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/libccd/src/polytope.c | 287 ++++++++++++++++++++++++++++++++++ 1 file changed, 287 insertions(+) create mode 100644 libs/ode-0.16.1/libccd/src/polytope.c (limited to 'libs/ode-0.16.1/libccd/src/polytope.c') diff --git a/libs/ode-0.16.1/libccd/src/polytope.c b/libs/ode-0.16.1/libccd/src/polytope.c new file mode 100644 index 0000000..c340b8c --- /dev/null +++ b/libs/ode-0.16.1/libccd/src/polytope.c @@ -0,0 +1,287 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * 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 +#include +#include +#include + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +_ccd_inline void _ccdPtNearestUpdate(ccd_pt_t *pt, ccd_pt_el_t *el) +{ + if (ccdEq(pt->nearest_dist, el->dist)){ + if (el->type < pt->nearest_type){ + pt->nearest = el; + pt->nearest_dist = el->dist; + pt->nearest_type = el->type; + } + }else if (el->dist < pt->nearest_dist){ + pt->nearest = el; + pt->nearest_dist = el->dist; + pt->nearest_type = el->type; + } +} + +static void _ccdPtNearestRenew(ccd_pt_t *pt) +{ + ccd_pt_vertex_t *v; + ccd_pt_edge_t *e; + ccd_pt_face_t *f; + + pt->nearest_dist = CCD_REAL_MAX; + pt->nearest_type = 3; + pt->nearest = NULL; + + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)v); + } + + ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){ + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)e); + } + + ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){ + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)f); + } +} + + + +void ccdPtInit(ccd_pt_t *pt) +{ + ccdListInit(&pt->vertices); + ccdListInit(&pt->edges); + ccdListInit(&pt->faces); + + pt->nearest = NULL; + pt->nearest_dist = CCD_REAL_MAX; + pt->nearest_type = 3; +} + +void ccdPtDestroy(ccd_pt_t *pt) +{ + ccd_pt_face_t *f, *f2; + ccd_pt_edge_t *e, *e2; + ccd_pt_vertex_t *v, *v2; + + // first delete all faces + ccdListForEachEntrySafe(&pt->faces, f, ccd_pt_face_t, f2, ccd_pt_face_t, list){ + ccdPtDelFace(pt, f); + } + + // delete all edges + ccdListForEachEntrySafe(&pt->edges, e, ccd_pt_edge_t, e2, ccd_pt_edge_t, list){ + ccdPtDelEdge(pt, e); + } + + // delete all vertices + ccdListForEachEntrySafe(&pt->vertices, v, ccd_pt_vertex_t, v2, ccd_pt_vertex_t, list){ + ccdPtDelVertex(pt, v); + } +} + + +ccd_pt_vertex_t *ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v) +{ + ccd_pt_vertex_t *vert; + + vert = CCD_ALLOC(ccd_pt_vertex_t); + vert->type = CCD_PT_VERTEX; + ccdSupportCopy(&vert->v, v); + + vert->dist = ccdVec3Len2(&vert->v.v); + ccdVec3Copy(&vert->witness, &vert->v.v); + + ccdListInit(&vert->edges); + + // add vertex to list + ccdListAppend(&pt->vertices, &vert->list); + + // update position in .nearest array + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)vert); + + return vert; +} + +ccd_pt_edge_t *ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1, + ccd_pt_vertex_t *v2) +{ + const ccd_vec3_t *a, *b; + ccd_pt_edge_t *edge; + + edge = CCD_ALLOC(ccd_pt_edge_t); + edge->type = CCD_PT_EDGE; + edge->vertex[0] = v1; + edge->vertex[1] = v2; + edge->faces[0] = edge->faces[1] = NULL; + + a = &edge->vertex[0]->v.v; + b = &edge->vertex[1]->v.v; + edge->dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &edge->witness); + + ccdListAppend(&edge->vertex[0]->edges, &edge->vertex_list[0]); + ccdListAppend(&edge->vertex[1]->edges, &edge->vertex_list[1]); + + ccdListAppend(&pt->edges, &edge->list); + + // update position in .nearest array + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)edge); + + return edge; +} + +ccd_pt_face_t *ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1, + ccd_pt_edge_t *e2, + ccd_pt_edge_t *e3) +{ + const ccd_vec3_t *a, *b, *c; + ccd_pt_face_t *face; + ccd_pt_edge_t *e; + size_t i; + + face = CCD_ALLOC(ccd_pt_face_t); + face->type = CCD_PT_FACE; + face->edge[0] = e1; + face->edge[1] = e2; + face->edge[2] = e3; + + // obtain triplet of vertices + a = &face->edge[0]->vertex[0]->v.v; + b = &face->edge[0]->vertex[1]->v.v; + e = face->edge[1]; + if (e->vertex[0] != face->edge[0]->vertex[0] + && e->vertex[0] != face->edge[0]->vertex[1]){ + c = &e->vertex[0]->v.v; + }else{ + c = &e->vertex[1]->v.v; + } + face->dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &face->witness); + + + for (i = 0; i < 3; i++){ + if (face->edge[i]->faces[0] == NULL){ + face->edge[i]->faces[0] = face; + }else{ + face->edge[i]->faces[1] = face; + } + } + + ccdListAppend(&pt->faces, &face->list); + + // update position in .nearest array + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)face); + + return face; +} + + +void ccdPtRecomputeDistances(ccd_pt_t *pt) +{ + ccd_pt_vertex_t *v; + ccd_pt_edge_t *e; + ccd_pt_face_t *f; + const ccd_vec3_t *a, *b, *c; + ccd_real_t dist; + + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + dist = ccdVec3Len2(&v->v.v); + v->dist = dist; + ccdVec3Copy(&v->witness, &v->v.v); + } + + ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){ + a = &e->vertex[0]->v.v; + b = &e->vertex[1]->v.v; + dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &e->witness); + e->dist = dist; + } + + ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){ + // obtain triplet of vertices + a = &f->edge[0]->vertex[0]->v.v; + b = &f->edge[0]->vertex[1]->v.v; + e = f->edge[1]; + if (e->vertex[0] != f->edge[0]->vertex[0] + && e->vertex[0] != f->edge[0]->vertex[1]){ + c = &e->vertex[0]->v.v; + }else{ + c = &e->vertex[1]->v.v; + } + + dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &f->witness); + f->dist = dist; + } +} + +ccd_pt_el_t *ccdPtNearest(ccd_pt_t *pt) +{ + if (!pt->nearest){ + _ccdPtNearestRenew(pt); + } + return pt->nearest; +} + + +void ccdPtDumpSVT(ccd_pt_t *pt, const char *fn) +{ + FILE *fout; + + fout = fopen(fn, "a"); + if (fout == NULL) + return; + + ccdPtDumpSVT2(pt, fout); + + fclose(fout); +} + +void ccdPtDumpSVT2(ccd_pt_t *pt, FILE *fout) +{ + ccd_pt_vertex_t *v, *a, *b, *c; + ccd_pt_edge_t *e; + ccd_pt_face_t *f; + size_t i; + + fprintf(fout, "-----\n"); + + fprintf(fout, "Points:\n"); + i = 0; + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + v->id = i++; + fprintf(fout, "%lf %lf %lf\n", + ccdVec3X(&v->v.v), ccdVec3Y(&v->v.v), ccdVec3Z(&v->v.v)); + } + + fprintf(fout, "Edges:\n"); + ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){ + fprintf(fout, "%d %d\n", e->vertex[0]->id, e->vertex[1]->id); + } + + fprintf(fout, "Faces:\n"); + ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){ + a = f->edge[0]->vertex[0]; + b = f->edge[0]->vertex[1]; + c = f->edge[1]->vertex[0]; + if (c == a || c == b){ + c = f->edge[1]->vertex[1]; + } + fprintf(fout, "%d %d %d\n", a->id, b->id, c->id); + } +} -- cgit v1.2.1