add ode

1 files changed, 398 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/libccd/src/testsuites/polytope.c b/libs/ode-0.16.1/libccd/src/testsuites/polytope.c
new file mode 100644
index 0000000..65686fb
--- /dev/null
+++ b/libs/ode-0.16.1/libccd/src/testsuites/polytope.c
@@ -0,0 +1,398 @@
+//#undef NDEBUG
+#include <cu/cu.h>
+#include <ccd/polytope.h>
+#include <ccd/dbg.h>
+
+TEST(ptSetUp)
+{
+}
+
+TEST(ptTearDown)
+{
+}
+
+
+TEST(ptCreate1)
+{
+    ccd_pt_t pt;
+    ccd_pt_vertex_t *v[3];
+    ccd_pt_edge_t *e[3];
+    ccd_pt_face_t *f;
+    ccd_vec3_t u;
+    int res;
+    size_t i;
+
+    DBG2("------");
+
+    ccdPtInit(&pt);
+    ccdPtDestroy(&pt);
+
+
+    ccdPtInit(&pt);
+
+    ccdVec3Set(&u, -1., -1., 0.);
+    v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.);
+    assertTrue(ccdVec3Eq(&u, &v[0]->v.v));
+
+    ccdVec3Set(&u, 1., 0., 0.);
+    v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.);
+    assertTrue(ccdVec3Eq(&u, &v[1]->v.v));
+
+    ccdVec3Set(&u, 0., 0., 1.);
+    v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.);
+    assertTrue(ccdVec3Eq(&u, &v[2]->v.v));
+
+    for (i = 0; i < 3; i++){
+        assertTrue(ccdEq(v[i]->dist, ccdVec3Len2(&v[i]->v.v)));
+    }
+
+
+    e[0] = ccdPtAddEdge(&pt, v[0], v[1]);
+    e[1] = ccdPtAddEdge(&pt, v[1], v[2]);
+    e[2] = ccdPtAddEdge(&pt, v[2], v[0]);
+    for (i = 0; i < 3; i++){
+        DBG("e[%d]->dist: %lf", i, e[i]->dist);
+        DBG_VEC3(&e[i]->witness, "     ->witness: ");
+    }
+
+    f = ccdPtAddFace(&pt, e[0], e[1], e[2]);
+    DBG("f->dist: %lf", f->dist);
+    DBG_VEC3(&f->witness, "     ->witness: ");
+
+    for (i = 0; i < 3; i++){
+        res = ccdPtDelVertex(&pt, v[i]);
+        assertFalse(res == 0);
+        res = ccdPtDelEdge(&pt, e[i]);
+        assertFalse(res == 0);
+    }
+
+    ccdPtDelFace(&pt, f);
+    for (i = 0; i < 3; i++){
+        res = ccdPtDelVertex(&pt, v[i]);
+        assertFalse(res == 0);
+    }
+    for (i = 0; i < 3; i++){
+        res = ccdPtDelEdge(&pt, e[i]);
+        assertTrue(res == 0);
+    }
+    for (i = 0; i < 3; i++){
+        res = ccdPtDelVertex(&pt, v[i]);
+        assertTrue(res == 0);
+    }
+
+    v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.);
+    v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.);
+    v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.);
+
+    e[0] = ccdPtAddEdge(&pt, v[0], v[1]);
+    e[1] = ccdPtAddEdge(&pt, v[1], v[2]);
+    e[2] = ccdPtAddEdge(&pt, v[2], v[0]);
+
+    f = ccdPtAddFace(&pt, e[0], e[1], e[2]);
+
+    ccdPtDestroy(&pt);
+}
+
+TEST(ptCreate2)
+{
+    ccd_pt_t pt;
+    ccd_pt_vertex_t *v[4];
+    ccd_pt_edge_t *e[6];
+    ccd_pt_face_t *f[4];
+    ccd_vec3_t u;
+    int res;
+    unsigned int i;
+
+    DBG2("------");
+
+    ccdPtInit(&pt);
+
+    ccdVec3Set(&u, -1., -1., 0.);
+    v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.);
+    assertTrue(ccdVec3Eq(&u, &v[0]->v.v));
+
+    ccdVec3Set(&u, 1., 0., 0.);
+    v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.);
+    assertTrue(ccdVec3Eq(&u, &v[1]->v.v));
+
+    ccdVec3Set(&u, 0., 0., 1.);
+    v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.);
+    assertTrue(ccdVec3Eq(&u, &v[2]->v.v));
+
+    ccdVec3Set(&u, 0., 1., 0.);
+    v[3] = ccdPtAddVertexCoords(&pt, 0., 1., 0.);
+    assertTrue(ccdVec3Eq(&u, &v[3]->v.v));
+
+    for (i = 0; i < 4; i++){
+        assertTrue(ccdEq(v[i]->dist, ccdVec3Len2(&v[i]->v.v)));
+    }
+    for (i = 0; i < 4; i++){
+        DBG("v[%d]->dist: %lf", i, v[i]->dist);
+        DBG_VEC3(&v[i]->witness, "     ->witness: ");
+    }
+
+    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]);
+    for (i = 0; i < 6; i++){
+        DBG("e[%d]->dist: %lf", i, e[i]->dist);
+        DBG_VEC3(&e[i]->witness, "     ->witness: ");
+    }
+
+    f[0] = ccdPtAddFace(&pt, e[0], e[1], e[2]);
+    f[1] = ccdPtAddFace(&pt, e[3], e[4], e[0]);
+    f[2] = ccdPtAddFace(&pt, e[4], e[5], e[1]);
+    f[3] = ccdPtAddFace(&pt, e[5], e[3], e[2]);
+    for (i = 0; i < 4; i++){
+        DBG("f[%d]->dist: %lf", i, f[i]->dist);
+        DBG_VEC3(&f[i]->witness, "     ->witness: ");
+    }
+
+    for (i = 0; i < 4; i++){
+        res = ccdPtDelVertex(&pt, v[i]);
+        assertFalse(res == 0);
+    }
+    for (i = 0; i < 6; i++){
+        res = ccdPtDelEdge(&pt, e[i]);
+        assertFalse(res == 0);
+    }
+
+    res = ccdPtDelFace(&pt, f[0]);
+    for (i = 0; i < 6; i++){
+        res = ccdPtDelEdge(&pt, e[i]);
+        assertFalse(res == 0);
+    }
+
+    res = ccdPtDelFace(&pt, f[1]);
+    assertTrue(ccdPtDelEdge(&pt, e[0]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[1]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[2]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[3]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[4]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[5]) == 0);
+    for (i = 0; i < 4; i++){
+        res = ccdPtDelVertex(&pt, v[i]);
+        assertFalse(res == 0);
+    }
+
+    res = ccdPtDelFace(&pt, f[2]);
+    assertTrue(ccdPtDelEdge(&pt, e[1]) == 0);
+    assertTrue(ccdPtDelEdge(&pt, e[4]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[2]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[3]) == 0);
+    assertFalse(ccdPtDelEdge(&pt, e[5]) == 0);
+
+    assertTrue(ccdPtDelVertex(&pt, v[1]) == 0);
+    assertFalse(ccdPtDelVertex(&pt, v[0]) == 0);
+    assertFalse(ccdPtDelVertex(&pt, v[2]) == 0);
+    assertFalse(ccdPtDelVertex(&pt, v[3]) == 0);
+
+    res = ccdPtDelFace(&pt, f[3]);
+    assertTrue(ccdPtDelEdge(&pt, e[2]) == 0);
+    assertTrue(ccdPtDelEdge(&pt, e[3]) == 0);
+    assertTrue(ccdPtDelEdge(&pt, e[5]) == 0);
+
+    assertTrue(ccdPtDelVertex(&pt, v[0]) == 0);
+    assertTrue(ccdPtDelVertex(&pt, v[2]) == 0);
+    assertTrue(ccdPtDelVertex(&pt, v[3]) == 0);
+
+
+    v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.);
+    v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.);
+    v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.);
+    v[3] = ccdPtAddVertexCoords(&pt, 0., 1., 0.);
+
+    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]);
+
+    f[0] = ccdPtAddFace(&pt, e[0], e[1], e[2]);
+    f[1] = ccdPtAddFace(&pt, e[3], e[4], e[0]);
+    f[2] = ccdPtAddFace(&pt, e[4], e[5], e[1]);
+    f[3] = ccdPtAddFace(&pt, e[5], e[3], e[2]);
+
+    ccdPtDestroy(&pt);
+}
+
+TEST(ptNearest)
+{
+    ccd_pt_t pt;
+    ccd_pt_vertex_t *v[4];
+    ccd_pt_edge_t *e[6];
+    ccd_pt_face_t *f[4];
+    ccd_pt_el_t *nearest;
+
+    DBG2("------");
+
+    ccdPtInit(&pt);
+
+    v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.);
+    v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.);
+    v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.);
+    v[3] = ccdPtAddVertexCoords(&pt, 0., 1., 0.);
+
+    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]);
+
+    f[0] = ccdPtAddFace(&pt, e[0], e[1], e[2]);
+    f[1] = ccdPtAddFace(&pt, e[3], e[4], e[0]);
+    f[2] = ccdPtAddFace(&pt, e[4], e[5], e[1]);
+    f[3] = ccdPtAddFace(&pt, e[5], e[3], e[2]);
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_FACE);
+    assertEquals(nearest, (ccd_pt_el_t *)f[1]);
+    assertTrue(ccdPtDelFace(&pt, f[1]) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_FACE);
+    assertTrue(nearest == (ccd_pt_el_t *)f[0]
+                || nearest == (ccd_pt_el_t *)f[3]);
+    assertTrue(ccdPtDelFace(&pt, (ccd_pt_face_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_FACE);
+    assertTrue(nearest == (ccd_pt_el_t *)f[0]
+                || nearest == (ccd_pt_el_t *)f[3]);
+    assertTrue(ccdPtDelFace(&pt, (ccd_pt_face_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_EDGE);
+    assertTrue(nearest == (ccd_pt_el_t *)e[0]
+                || nearest == (ccd_pt_el_t *)e[3]);
+    assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_EDGE);
+    assertTrue(nearest == (ccd_pt_el_t *)e[0]
+                || nearest == (ccd_pt_el_t *)e[3]);
+    assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_FACE);
+    assertEquals(nearest, (ccd_pt_el_t *)f[2]);
+    assertTrue(ccdPtDelFace(&pt, f[2]) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_EDGE);
+    assertTrue(nearest == (ccd_pt_el_t *)e[1]
+                || nearest == (ccd_pt_el_t *)e[4]
+                || nearest == (ccd_pt_el_t *)e[5]);
+    assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_EDGE);
+    assertTrue(nearest == (ccd_pt_el_t *)e[1]
+                || nearest == (ccd_pt_el_t *)e[4]
+                || nearest == (ccd_pt_el_t *)e[5]);
+    assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_EDGE);
+    assertTrue(nearest == (ccd_pt_el_t *)e[1]
+                || nearest == (ccd_pt_el_t *)e[4]
+                || nearest == (ccd_pt_el_t *)e[5]);
+    assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_EDGE);
+    assertTrue(nearest == (ccd_pt_el_t *)e[2]);
+    assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_VERTEX);
+    assertTrue(nearest == (ccd_pt_el_t *)v[1]
+                || nearest == (ccd_pt_el_t *)v[2]
+                || nearest == (ccd_pt_el_t *)v[3]);
+    assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_VERTEX);
+    assertTrue(nearest == (ccd_pt_el_t *)v[1]
+                || nearest == (ccd_pt_el_t *)v[2]
+                || nearest == (ccd_pt_el_t *)v[3]);
+    assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_VERTEX);
+    assertTrue(nearest == (ccd_pt_el_t *)v[1]
+                || nearest == (ccd_pt_el_t *)v[2]
+                || nearest == (ccd_pt_el_t *)v[3]);
+    assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    //DBG("nearest->type: %d", nearest->type);
+    //DBG("       ->dist: %lf", nearest->dist);
+    //DBG_VEC3(&nearest->witness, "       ->witness: ");
+    assertEquals(nearest->type, CCD_PT_VERTEX);
+    assertTrue(nearest == (ccd_pt_el_t *)v[0]);
+    assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0);
+
+
+    nearest = ccdPtNearest(&pt);
+    assertTrue(nearest == NULL);
+
+    ccdPtDestroy(&pt);
+}