summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/libccd/src/polytope.c
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/libccd/src/polytope.c')
-rw-r--r--libs/ode-0.16.1/libccd/src/polytope.c287
1 files changed, 287 insertions, 0 deletions
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 <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/polytope.h>
+#include <ccd/alloc.h>
+
+#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);
+ }
+}
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 03:37:58 +0000