397 lines
12 KiB
C
397 lines
12 KiB
C
//#undef NDEBUG
|
|
#include <cu/cu.h>
|
|
#include "../polytope.h"
|
|
#include "../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, 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, 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);
|
|
}
|