diff options
Diffstat (limited to 'libs/ode-0.16.1/tests/collision.cpp')
| -rw-r--r-- | libs/ode-0.16.1/tests/collision.cpp | 224 |
1 files changed, 224 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/tests/collision.cpp b/libs/ode-0.16.1/tests/collision.cpp new file mode 100644 index 0000000..6eef4cf --- /dev/null +++ b/libs/ode-0.16.1/tests/collision.cpp @@ -0,0 +1,224 @@ +#include <UnitTest++.h> +#include <ode/ode.h> +#include "common.h" + +TEST(test_collision_trimesh_sphere_exact) +{ + /* + * This tests some extreme cases, where a sphere barely touches some triangles + * with zero depth. + */ + + #ifdef dTRIMESH_GIMPACT + /* + * Although GIMPACT is algorithmically able to handle this extreme case, + * the numerical approximation used for the square root produces inexact results. + */ + return; + #endif + + { + const int VertexCount = 4; + const int IndexCount = 2*3; + // this is a square on the XY plane + /* + 3 2 + +----+ + | /| + | / | + | / | + |/ | + +----+ + 0 1 + */ + float vertices[VertexCount * 3] = { + -1,-1,0, + 1,-1,0, + 1,1,0, + -1,1,0 + }; + dTriIndex indices[IndexCount] = { + 0,1,2, + 0,2,3 + }; + + dTriMeshDataID data = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(data, + vertices, + 3 * sizeof(float), + VertexCount, + indices, + IndexCount, + 3 * sizeof(dTriIndex)); + dGeomID trimesh = dCreateTriMesh(0, data, 0, 0, 0); + const dReal radius = 4; + dGeomID sphere = dCreateSphere(0, radius); + dContactGeom cg[4]; + int nc; + dVector3 trinormal = { 0, 0, -1 }; + + // Test case: sphere touches the diagonal edge + dGeomSetPosition(sphere, 0,0,radius); + nc = dCollide(trimesh, sphere, 4, &cg[0], sizeof cg[0]); + CHECK_EQUAL(2, nc); + for (int i=0; i<nc; ++i) { + CHECK_EQUAL(0, cg[i].depth); + CHECK_ARRAY_EQUAL(trinormal, cg[i].normal, 3); + } + + // now translate both geoms + dGeomSetPosition(trimesh, 10,30,40); + dGeomSetPosition(sphere, 10,30,40+radius); + // check extreme case, again + nc = dCollide(trimesh, sphere, 4, &cg[0], sizeof cg[0]); + CHECK_EQUAL(2, nc); + for (int i=0; i<nc; ++i) { + CHECK_EQUAL(0, cg[i].depth); + CHECK_ARRAY_EQUAL(trinormal, cg[i].normal, 3); + } + + // and now, let's rotate the trimesh, 90 degrees on X + dMatrix3 rot = { 1, 0, 0, 0, + 0, 0, -1, 0, + 0, 1, 0, 0 }; + dGeomSetPosition(trimesh, 10,30,40); + dGeomSetRotation(trimesh, rot); + + dGeomSetPosition(sphere, 10,30-radius,40); + // check extreme case, again + nc = dCollide(trimesh, sphere, 4, &cg[0], sizeof cg[0]); + CHECK_EQUAL(2, nc); + dVector3 rtrinormal = { 0, 1, 0 }; + for (int i=0; i<nc; ++i) { + CHECK_EQUAL(0, cg[i].depth); + CHECK_ARRAY_EQUAL(rtrinormal, cg[i].normal, 3); + } + } +} + + + +TEST(test_collision_heightfield_ray_fail) +{ + /* + * This test demonstrated a bug in the AABB handling of the + * heightfield. + */ + { + // Create quick heightfield with dummy data + dHeightfieldDataID heightfieldData = dGeomHeightfieldDataCreate(); + unsigned char dataBuffer[16+1] = "1234567890123456"; + dGeomHeightfieldDataBuildByte(heightfieldData, dataBuffer, 0, 4, 4, 4, 4, 1, 0, 0, 0); + dGeomHeightfieldDataSetBounds(heightfieldData, '0', '9'); + dGeomID height = dCreateHeightfield(0, heightfieldData, 1); + + // Create ray outside bounds + dGeomID ray = dCreateRay(0, 20); + dGeomRaySet(ray, 5, 10, 1, 0, -1, 0); + dContact contactBuf[10]; + + // Make sure it does not crash! + dCollide(ray, height, 10, &(contactBuf[0].geom), sizeof(dContact)); + + dGeomDestroy(height); + dGeomDestroy(ray); + dGeomHeightfieldDataDestroy(heightfieldData); + } +} + +#include "../ode/demo/convex_prism.h" + +TEST(test_collision_ray_convex) +{ + /* + * Issue 55: ray vs convex collider does not consider the position of the convex geometry. + */ + { + dContact contact; + + // Create convex + dGeomID convex = dCreateConvex(0, + prism_planes, + prism_planecount, + prism_points, + prism_pointcount, + prism_polygons); + dGeomSetPosition(convex,0,0,0); + + // Create ray + dGeomID ray = dCreateRay(0, 20); + + dGeomRaySet(ray, 0, -10, 0, 0, 1, 0); + + int count = dCollide(ray, convex, 1, &contact.geom, sizeof(dContact)); + + CHECK_EQUAL(1,count); + CHECK_CLOSE(0.0,contact.geom.pos[0], dEpsilon); + CHECK_CLOSE(-1.0,contact.geom.pos[1], dEpsilon); + CHECK_CLOSE(0.0,contact.geom.pos[2], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[0], dEpsilon); + CHECK_CLOSE(-1.0, contact.geom.normal[1], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[2], dEpsilon); + CHECK_CLOSE(9.0, contact.geom.depth, dEpsilon); + + // Move Ray + dGeomRaySet(ray, 5, -10, 0, 0, 1, 0); + + count = dCollide(ray, convex, 1, &contact.geom, sizeof(dContact)); + + CHECK_EQUAL(1,count); + CHECK_CLOSE(5.0, contact.geom.pos[0], dEpsilon); + CHECK_CLOSE(-1.0, contact.geom.pos[1], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.pos[2], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[0], dEpsilon); + CHECK_CLOSE(-1.0, contact.geom.normal[1], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[2], dEpsilon); + CHECK_CLOSE(9.0, contact.geom.depth, dEpsilon); + + // Rotate Convex + dMatrix3 rotate90z = + { + 0,-1,0,0, + 1,0,0,0, + 0,0,1,0 + }; + dGeomSetRotation(convex, rotate90z); + + count = dCollide(ray, convex, 1, &contact.geom, sizeof(dContact)); + + CHECK_EQUAL(0,count); + + // Move Ray + dGeomRaySet(ray, 10, 0, 0, -1, 0, 0); + count = dCollide(ray, convex, 1, &contact.geom, sizeof(dContact)); + + CHECK_EQUAL(1,count); + CHECK_CLOSE(1.0, contact.geom.pos[0], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.pos[1], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.pos[2], dEpsilon); + CHECK_CLOSE(1.0, contact.geom.normal[0], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[1], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[2], dEpsilon); + CHECK_CLOSE(9.0,contact.geom.depth, dEpsilon); + + + // Move Ray + dGeomRaySet(ray, 10, 1000, 1000, -1, 0, 0); + // Move Geom + dGeomSetPosition(convex, 0, 1000, 1000); + + count = dCollide(ray, convex, 1, &contact.geom, sizeof(dContact)); + + CHECK_EQUAL(1, count); + CHECK_CLOSE(1.0, contact.geom.pos[0], dEpsilon); + CHECK_CLOSE(1000.0, contact.geom.pos[1], dEpsilon); + CHECK_CLOSE(1000.0, contact.geom.pos[2], dEpsilon); + CHECK_CLOSE(1.0, contact.geom.normal[0], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[1], dEpsilon); + CHECK_CLOSE(0.0, contact.geom.normal[2], dEpsilon); + CHECK_CLOSE(9.0, contact.geom.depth, dEpsilon); + + dGeomDestroy(convex); + dGeomDestroy(ray); + } +} |