166 lines
4.6 KiB
C
166 lines
4.6 KiB
C
#include <cu/cu.h>
|
|
#include <ccd/ccd.h>
|
|
#include "common.h"
|
|
#include "support.h"
|
|
|
|
#define TOSVT() \
|
|
svtObjPen(&box, &cyl, stdout, "Pen 1", depth, &dir, &pos); \
|
|
ccdVec3Scale(&dir, depth); \
|
|
ccdVec3Add(&cyl.pos, &dir); \
|
|
svtObjPen(&box, &cyl, stdout, "Pen 1", depth, &dir, &pos)
|
|
|
|
TEST(mprBoxcylIntersect)
|
|
{
|
|
ccd_t ccd;
|
|
CCD_BOX(box);
|
|
CCD_CYL(cyl);
|
|
int res;
|
|
ccd_vec3_t axis;
|
|
|
|
box.x = 0.5;
|
|
box.y = 1.;
|
|
box.z = 1.5;
|
|
cyl.radius = 0.4;
|
|
cyl.height = 0.7;
|
|
|
|
CCD_INIT(&ccd);
|
|
ccd.support1 = ccdSupport;
|
|
ccd.support2 = ccdSupport;
|
|
ccd.center1 = ccdObjCenter;
|
|
ccd.center2 = ccdObjCenter;
|
|
|
|
ccdVec3Set(&cyl.pos, 0.1, 0., 0.);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
|
|
ccdVec3Set(&cyl.pos, .6, 0.6, 0.);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
|
|
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
|
|
ccdVec3Set(&axis, 0., 1., 0.);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
|
|
ccdVec3Set(&axis, 0.67, 1.1, 0.12);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
|
|
ccdVec3Set(&axis, -0.1, 2.2, -1.);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
|
|
ccdVec3Set(&axis, 1., 1., 0.);
|
|
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
|
|
ccdVec3Set(&box.pos, .6, 0., 0.5);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
|
|
ccdVec3Set(&axis, -0.1, 2.2, -1.);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
|
|
ccdVec3Set(&axis, 1., 1., 0.);
|
|
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
|
|
ccdVec3Set(&box.pos, .9, 0.8, 0.5);
|
|
res = ccdMPRIntersect(&box, &cyl, &ccd);
|
|
assertTrue(res);
|
|
}
|
|
|
|
|
|
|
|
TEST(mprBoxcylPen)
|
|
{
|
|
ccd_t ccd;
|
|
CCD_BOX(box);
|
|
CCD_CYL(cyl);
|
|
int res;
|
|
ccd_vec3_t axis;
|
|
ccd_real_t depth;
|
|
ccd_vec3_t dir, pos;
|
|
|
|
box.x = 0.5;
|
|
box.y = 1.;
|
|
box.z = 1.5;
|
|
cyl.radius = 0.4;
|
|
cyl.height = 0.7;
|
|
|
|
CCD_INIT(&ccd);
|
|
ccd.support1 = ccdSupport;
|
|
ccd.support2 = ccdSupport;
|
|
ccd.center1 = ccdObjCenter;
|
|
ccd.center2 = ccdObjCenter;
|
|
|
|
ccdVec3Set(&cyl.pos, 0.1, 0., 0.);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 1");
|
|
//TOSVT();
|
|
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 2");
|
|
//TOSVT();
|
|
|
|
ccdVec3Set(&cyl.pos, .6, 0.6, 0.);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 3");
|
|
//TOSVT();
|
|
|
|
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 4");
|
|
//TOSVT();
|
|
|
|
ccdVec3Set(&axis, 0., 1., 0.);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 5");
|
|
//TOSVT();
|
|
|
|
ccdVec3Set(&axis, 0.67, 1.1, 0.12);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 6");
|
|
//TOSVT();
|
|
|
|
ccdVec3Set(&axis, -0.1, 2.2, -1.);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
|
|
ccdVec3Set(&axis, 1., 1., 0.);
|
|
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
|
|
ccdVec3Set(&box.pos, .6, 0., 0.5);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 7");
|
|
//TOSVT();
|
|
|
|
ccdVec3Set(&axis, -0.1, 2.2, -1.);
|
|
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
|
|
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
|
|
ccdVec3Set(&axis, 1., 1., 0.);
|
|
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
|
|
ccdVec3Set(&box.pos, .9, 0.8, 0.5);
|
|
res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos);
|
|
assertTrue(res == 0);
|
|
recPen(depth, &dir, &pos, stdout, "Pen 8");
|
|
//TOSVT();
|
|
}
|
|
|