add ode

1 files changed, 322 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/libccd/src/ccd/polytope.h b/libs/ode-0.16.1/libccd/src/ccd/polytope.h
new file mode 100644
index 0000000..c9ee2ab
--- /dev/null
+++ b/libs/ode-0.16.1/libccd/src/ccd/polytope.h
@@ -0,0 +1,322 @@
+/***
+ * 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.
+ */
+
+#ifndef __CCD_POLYTOPE_H__
+#define __CCD_POLYTOPE_H__
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <ccd/support.h>
+#include <ccd/list.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+#define CCD_PT_VERTEX 1
+#define CCD_PT_EDGE   2
+#define CCD_PT_FACE   3
+
+
+#define __CCD_PT_EL \
+    int type;           /*! type of element */ \
+    ccd_real_t dist;        /*! distance from origin */ \
+    ccd_vec3_t witness; /*! witness point of projection of origin */ \
+    ccd_list_t list;    /*! list of elements of same type */
+
+/**
+ * General polytope element.
+ * Could be vertex, edge or triangle.
+ */
+struct _ccd_pt_el_t {
+    __CCD_PT_EL
+};
+typedef struct _ccd_pt_el_t ccd_pt_el_t;
+
+struct _ccd_pt_edge_t;
+struct _ccd_pt_face_t;
+
+/**
+ * Polytope's vertex.
+ */
+struct _ccd_pt_vertex_t {
+    __CCD_PT_EL
+
+    int id;
+    ccd_support_t v;
+    ccd_list_t edges; //!< List of edges
+};
+typedef struct _ccd_pt_vertex_t ccd_pt_vertex_t;
+
+/**
+ * Polytope's edge.
+ */
+struct _ccd_pt_edge_t {
+    __CCD_PT_EL
+
+    ccd_pt_vertex_t *vertex[2]; //!< Reference to vertices
+    struct _ccd_pt_face_t *faces[2]; //!< Reference to faces
+
+    ccd_list_t vertex_list[2]; //!< List items in vertices' lists
+};
+typedef struct _ccd_pt_edge_t ccd_pt_edge_t;
+
+/**
+ * Polytope's triangle faces.
+ */
+struct _ccd_pt_face_t {
+    __CCD_PT_EL
+
+    ccd_pt_edge_t *edge[3]; //!< Reference to surrounding edges
+};
+typedef struct _ccd_pt_face_t ccd_pt_face_t;
+
+
+/**
+ * Struct containing polytope.
+ */
+struct _ccd_pt_t {
+    ccd_list_t vertices; //!< List of vertices
+    ccd_list_t edges; //!< List of edges
+    ccd_list_t faces; //!< List of faces
+
+    ccd_pt_el_t *nearest;
+    ccd_real_t nearest_dist;
+    int nearest_type;
+};
+typedef struct _ccd_pt_t ccd_pt_t;
+
+
+void ccdPtInit(ccd_pt_t *pt);
+void ccdPtDestroy(ccd_pt_t *pt);
+
+/**
+ * Returns vertices surrounding given triangle face.
+ */
+_ccd_inline void ccdPtFaceVec3(const ccd_pt_face_t *face,
+                               ccd_vec3_t **a,
+                               ccd_vec3_t **b,
+                               ccd_vec3_t **c);
+_ccd_inline void ccdPtFaceVertices(const ccd_pt_face_t *face,
+                                   ccd_pt_vertex_t **a,
+                                   ccd_pt_vertex_t **b,
+                                   ccd_pt_vertex_t **c);
+_ccd_inline void ccdPtFaceEdges(const ccd_pt_face_t *f,
+                                ccd_pt_edge_t **a,
+                                ccd_pt_edge_t **b,
+                                ccd_pt_edge_t **c);
+
+_ccd_inline void ccdPtEdgeVec3(const ccd_pt_edge_t *e,
+                               ccd_vec3_t **a,
+                               ccd_vec3_t **b);
+_ccd_inline void ccdPtEdgeVertices(const ccd_pt_edge_t *e,
+                                   ccd_pt_vertex_t **a,
+                                   ccd_pt_vertex_t **b);
+_ccd_inline void ccdPtEdgeFaces(const ccd_pt_edge_t *e,
+                                ccd_pt_face_t **f1,
+                                ccd_pt_face_t **f2);
+
+
+/**
+ * Adds vertex to polytope and returns pointer to newly created vertex.
+ */
+ccd_pt_vertex_t *ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v);
+_ccd_inline ccd_pt_vertex_t *ccdPtAddVertexCoords(ccd_pt_t *pt,
+                                                  ccd_real_t x, ccd_real_t y, ccd_real_t z);
+
+/**
+ * Adds edge to polytope.
+ */
+ccd_pt_edge_t *ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1,
+                                          ccd_pt_vertex_t *v2);
+
+/**
+ * Adds face to polytope.
+ */
+ccd_pt_face_t *ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1,
+                                          ccd_pt_edge_t *e2,
+                                          ccd_pt_edge_t *e3);
+
+/**
+ * Deletes vertex from polytope.
+ * Returns 0 on success, -1 otherwise.
+ */
+_ccd_inline int ccdPtDelVertex(ccd_pt_t *pt, ccd_pt_vertex_t *);
+_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *);
+_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *);
+
+
+/**
+ * Recompute distances from origin for all elements in pt.
+ */
+void ccdPtRecomputeDistances(ccd_pt_t *pt);
+
+/**
+ * Returns nearest element to origin.
+ */
+ccd_pt_el_t *ccdPtNearest(ccd_pt_t *pt);
+
+
+void ccdPtDumpSVT(ccd_pt_t *pt, const char *fn);
+void ccdPtDumpSVT2(ccd_pt_t *pt, FILE *);
+
+
+/**** INLINES ****/
+_ccd_inline ccd_pt_vertex_t *ccdPtAddVertexCoords(ccd_pt_t *pt,
+                                                  ccd_real_t x, ccd_real_t y, ccd_real_t z)
+{
+    ccd_support_t s;
+    ccdVec3Set(&s.v, x, y, z);
+    return ccdPtAddVertex(pt, &s);
+}
+
+_ccd_inline int ccdPtDelVertex(ccd_pt_t *pt, ccd_pt_vertex_t *v)
+{
+    // test if any edge is connected to this vertex
+    if (!ccdListEmpty(&v->edges))
+        return -1;
+
+    // delete vertex from main list
+    ccdListDel(&v->list);
+
+    if ((void *)pt->nearest == (void *)v){
+        pt->nearest = NULL;
+    }
+
+    free(v);
+    return 0;
+}
+
+_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *e)
+{
+    // text if any face is connected to this edge (faces[] is always
+    // aligned to lower indices)
+    if (e->faces[0] != NULL)
+        return -1;
+
+    // disconnect edge from lists of edges in vertex struct
+    ccdListDel(&e->vertex_list[0]);
+    ccdListDel(&e->vertex_list[1]);
+
+    // disconnect edge from main list
+    ccdListDel(&e->list);
+
+    if ((void *)pt->nearest == (void *)e){
+        pt->nearest = NULL;
+    }
+
+    free(e);
+    return 0;
+}
+
+_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *f)
+{
+    ccd_pt_edge_t *e;
+    size_t i;
+
+    // remove face from edges' recerence lists
+    for (i = 0; i < 3; i++){
+        e = f->edge[i];
+        if (e->faces[0] == f){
+            e->faces[0] = e->faces[1];
+        }
+        e->faces[1] = NULL;
+    }
+
+    // remove face from list of all faces
+    ccdListDel(&f->list);
+
+    if ((void *)pt->nearest == (void *)f){
+        pt->nearest = NULL;
+    }
+
+    free(f);
+    return 0;
+}
+
+_ccd_inline void ccdPtFaceVec3(const ccd_pt_face_t *face,
+                               ccd_vec3_t **a,
+                               ccd_vec3_t **b,
+                               ccd_vec3_t **c)
+{
+    *a = &face->edge[0]->vertex[0]->v.v;
+    *b = &face->edge[0]->vertex[1]->v.v;
+
+    if (face->edge[1]->vertex[0] != face->edge[0]->vertex[0]
+            && face->edge[1]->vertex[0] != face->edge[0]->vertex[1]){
+        *c = &face->edge[1]->vertex[0]->v.v;
+    }else{
+        *c = &face->edge[1]->vertex[1]->v.v;
+    }
+}
+
+_ccd_inline void ccdPtFaceVertices(const ccd_pt_face_t *face,
+                                   ccd_pt_vertex_t **a,
+                                   ccd_pt_vertex_t **b,
+                                   ccd_pt_vertex_t **c)
+{
+    *a = face->edge[0]->vertex[0];
+    *b = face->edge[0]->vertex[1];
+
+    if (face->edge[1]->vertex[0] != face->edge[0]->vertex[0]
+            && face->edge[1]->vertex[0] != face->edge[0]->vertex[1]){
+        *c = face->edge[1]->vertex[0];
+    }else{
+        *c = face->edge[1]->vertex[1];
+    }
+}
+
+_ccd_inline void ccdPtFaceEdges(const ccd_pt_face_t *f,
+                                ccd_pt_edge_t **a,
+                                ccd_pt_edge_t **b,
+                                ccd_pt_edge_t **c)
+{
+    *a = f->edge[0];
+    *b = f->edge[1];
+    *c = f->edge[2];
+}
+
+_ccd_inline void ccdPtEdgeVec3(const ccd_pt_edge_t *e,
+                               ccd_vec3_t **a,
+                               ccd_vec3_t **b)
+{
+    *a = &e->vertex[0]->v.v;
+    *b = &e->vertex[1]->v.v;
+}
+
+_ccd_inline void ccdPtEdgeVertices(const ccd_pt_edge_t *e,
+                                   ccd_pt_vertex_t **a,
+                                   ccd_pt_vertex_t **b)
+{
+    *a = e->vertex[0];
+    *b = e->vertex[1];
+}
+
+_ccd_inline void ccdPtEdgeFaces(const ccd_pt_edge_t *e,
+                                ccd_pt_face_t **f1,
+                                ccd_pt_face_t **f2)
+{
+    *f1 = e->faces[0];
+    *f2 = e->faces[1];
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+#endif /* __CCD_POLYTOPE_H__ */