From 715cc3467d91a5813cf6606752931cde7e3e15dc Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 18 Jun 2014 14:47:39 -0700 Subject: [PATCH] forgot to add the test files --- tests/physics/src/VerletShapeTests.cpp | 768 +++++++++++++++++++++++++ tests/physics/src/VerletShapeTests.h | 30 + 2 files changed, 798 insertions(+) create mode 100644 tests/physics/src/VerletShapeTests.cpp create mode 100644 tests/physics/src/VerletShapeTests.h diff --git a/tests/physics/src/VerletShapeTests.cpp b/tests/physics/src/VerletShapeTests.cpp new file mode 100644 index 0000000000..6ae839392e --- /dev/null +++ b/tests/physics/src/VerletShapeTests.cpp @@ -0,0 +1,768 @@ +// +// VerletShapeTests.cpp +// tests/physics/src +// +// Created by Andrew Meadows on 02/21/2014. +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +//#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "VerletShapeTests.h" + +const glm::vec3 origin(0.0f); +static const glm::vec3 xAxis(1.0f, 0.0f, 0.0f); +static const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); +static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f); + +void VerletShapeTests::setSpherePosition() { + float radius = 1.0f; + glm::vec3 offset(1.23f, 4.56f, 7.89f); + glm::vec3 point; + VerletSphereShape sphere(radius, &point); + + point = glm::vec3(0.f); + float d = glm::distance(glm::vec3(0.0f), sphere.getTranslation()); + if (d != 0.0f) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at origin" << std::endl; + } + + point = offset; + d = glm::distance(glm::vec3(0.0f), sphere.getTranslation()); + if (d != glm::length(offset)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at offset" << std::endl; + } +} + +void VerletShapeTests::sphereMissesSphere() { + // non-overlapping spheres of unequal size + + float radiusA = 7.0f; + float radiusB = 3.0f; + float alpha = 1.2f; + float beta = 1.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + + // create points for the sphere centers + glm::vec3 points[2]; + + // give pointers to the spheres + VerletSphereShape sphereA(radiusA, (points + 0)); + VerletSphereShape sphereB(radiusB, (points + 1)); + + // set the positions of the spheres by slamming the points directly + points[0] = origin; + points[1] = offsetDistance * offsetDirection; + + CollisionList collisions(16); + + // collide A to B... + { + bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + // collide B to A... + { + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + // also test shapeShape + { + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::sphereTouchesSphere() { + // overlapping spheres of unequal size + float radiusA = 7.0f; + float radiusB = 3.0f; + float alpha = 0.2f; + float beta = 0.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + float expectedPenetrationDistance = (1.0f - alpha) * radiusA + (1.0f - beta) * radiusB; + glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection; + + // create two points for the sphere centers + glm::vec3 points[2]; + + // give pointers to the spheres + VerletSphereShape sphereA(radiusA, points+0); + VerletSphereShape sphereB(radiusB, points+1); + + // set the positions of the spheres by slamming the points directly + points[0] = origin; + points[1] = offsetDistance * offsetDirection; + + CollisionList collisions(16); + int numCollisions = 0; + + // collide A to B... + { + bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; + } + + // verify state of collisions + if (numCollisions != collisions.size()) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected collisions size of " << numCollisions << " but actual size is " << collisions.size() + << std::endl; + } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision" << std::endl; + } + + // penetration points from sphereA into sphereB + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 AtoB = sphereB.getTranslation() - sphereA.getTranslation(); + glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * glm::normalize(AtoB); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + + // collide B to A... + { + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into sphereB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + float inaccuracy = glm::length(collision->_penetration + expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 BtoA = sphereA.getTranslation() - sphereB.getTranslation(); + glm::vec3 expectedContactPoint = sphereB.getTranslation() + radiusB * glm::normalize(BtoA); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } +} + +void VerletShapeTests::sphereMissesCapsule() { + // non-overlapping sphere and capsule + float radiusA = 1.5f; + float radiusB = 2.3f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 1.7f; + float axialOffset = totalRadius + 1.1f * halfHeightB; + float radialOffset = 1.2f * radiusA + 1.3f * radiusB; + + // create points for the sphere + capsule + glm::vec3 points[3]; + for (int i = 0; i < 3; ++i) { + points[i] = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletSphereShape sphereA(radiusA, points); + VerletCapsuleShape capsuleB(radiusB, points+1, points+2); + capsuleB.setHalfHeight(halfHeightB); + + // give the capsule some arbitrary transform + float angle = 37.8f; + glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); + glm::quat rotation = glm::angleAxis(angle, axis); + glm::vec3 translation(15.1f, -27.1f, -38.6f); + capsuleB.setRotation(rotation); + capsuleB.setTranslation(translation); + + CollisionList collisions(16); + + // walk sphereA along the local yAxis next to, but not touching, capsuleB + glm::vec3 localStartPosition(radialOffset, axialOffset, 0.0f); + int numberOfSteps = 10; + float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1); + for (int i = 0; i < numberOfSteps; ++i) { + // translate sphereA into world-frame + glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis; + sphereA.setTranslation(rotation * localPosition + translation); + + // sphereA agains capsuleB + if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + + // capsuleB against sphereA + if (ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::sphereTouchesCapsule() { + // overlapping sphere and capsule + float radiusA = 2.0f; + float radiusB = 1.0f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 2.0f; + float alpha = 0.5f; + float beta = 0.5f; + float radialOffset = alpha * radiusA + beta * radiusB; + + // create points for the sphere + capsule + glm::vec3 points[3]; + for (int i = 0; i < 3; ++i) { + points[i] = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletSphereShape sphereA(radiusA, points); + VerletCapsuleShape capsuleB(radiusB, points+1, points+2); + capsuleB.setHalfHeight(halfHeightB); + + CollisionList collisions(16); + int numCollisions = 0; + + { // sphereA collides with capsuleB's cylindrical wall + sphereA.setTranslation(radialOffset * xAxis); + + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * xAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = - (radialOffset - totalRadius) * xAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of capsuleB + glm::vec3 BtoA = sphereA.getTranslation() - capsuleB.getTranslation(); + glm::vec3 closestApproach = capsuleB.getTranslation() + glm::dot(BtoA, yAxis) * yAxis; + expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + { // sphereA hits end cap at axis + glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setTranslation(axialOffset * yAxis); + + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of capsuleB + glm::vec3 endPoint; + capsuleB.getEndPoint(endPoint); + expectedContactPoint = endPoint + radiusB * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + { // sphereA hits start cap at axis + glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setTranslation(axialOffset * yAxis); + + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of capsuleB + glm::vec3 startPoint; + capsuleB.getStartPoint(startPoint); + expectedContactPoint = startPoint - radiusB * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + if (collisions.size() != numCollisions) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::capsuleMissesCapsule() { + // non-overlapping capsules + float radiusA = 2.0f; + float halfHeightA = 3.0f; + float radiusB = 3.0f; + float halfHeightB = 4.0f; + + float totalRadius = radiusA + radiusB; + float totalHalfLength = totalRadius + halfHeightA + halfHeightB; + + // create points for the shapes + glm::vec3 points[4]; + for (int i = 0; i < 4; ++i) { + points[i] = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletCapsuleShape capsuleA(radiusA, points+0, points+1); + VerletCapsuleShape capsuleB(radiusB, points+2, points+3); + capsuleA.setHalfHeight(halfHeightA); + capsuleA.setHalfHeight(halfHeightB); + + CollisionList collisions(16); + + // side by side + capsuleB.setTranslation((1.01f * totalRadius) * xAxis); + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + + // end to end + capsuleB.setTranslation((1.01f * totalHalfLength) * xAxis); + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + + // rotate B and move it to the side + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + capsuleB.setTranslation((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::capsuleTouchesCapsule() { + // overlapping capsules + float radiusA = 2.0f; + float halfHeightA = 3.0f; + float radiusB = 3.0f; + float halfHeightB = 4.0f; + + float totalRadius = radiusA + radiusB; + float totalHalfLength = totalRadius + halfHeightA + halfHeightB; + + // create points for the shapes + glm::vec3 points[4]; + for (int i = 0; i < 4; ++i) { + points[i] = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletCapsuleShape capsuleA(radiusA, points+0, points+1); + VerletCapsuleShape capsuleB(radiusB, points+2, points+3); + capsuleA.setHalfHeight(halfHeightA); + capsuleB.setHalfHeight(halfHeightB); + + CollisionList collisions(16); + int numCollisions = 0; + + { // side by side + capsuleB.setTranslation((0.99f * totalRadius) * xAxis); + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + } + + { // end to end + capsuleB.setTranslation((0.99f * totalHalfLength) * yAxis); + + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + } + + { // rotate B and move it to the side + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + capsuleB.setTranslation((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); + + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + } + + { // again, but this time check collision details + float overlap = 0.1f; + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis; + capsuleB.setTranslation(positionB); + + // capsuleA vs capsuleB + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = overlap * xAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB vs capsuleA + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = - overlap * xAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << std::endl; + } + + expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << std::endl; + } + } + + { // collide cylinder wall against cylinder wall + float overlap = 0.137f; + float shift = 0.317f * halfHeightA; + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis; + capsuleB.setTranslation(positionB); + + // capsuleA vs capsuleB + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = overlap * zAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << std::endl; + } + + glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << std::endl; + } + } +} + +void VerletShapeTests::runAllTests() { + setSpherePosition(); + sphereMissesSphere(); + sphereTouchesSphere(); + + sphereMissesCapsule(); + sphereTouchesCapsule(); + + capsuleMissesCapsule(); + capsuleTouchesCapsule(); +} diff --git a/tests/physics/src/VerletShapeTests.h b/tests/physics/src/VerletShapeTests.h new file mode 100644 index 0000000000..36e2fe0cbd --- /dev/null +++ b/tests/physics/src/VerletShapeTests.h @@ -0,0 +1,30 @@ +// +// VerletShapeTests.h +// tests/physics/src +// +// Created by Andrew Meadows on 2014.06.18 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_VerletShapeTests_h +#define hifi_VerletShapeTests_h + +namespace VerletShapeTests { + void setSpherePosition(); + + void sphereMissesSphere(); + void sphereTouchesSphere(); + + void sphereMissesCapsule(); + void sphereTouchesCapsule(); + + void capsuleMissesCapsule(); + void capsuleTouchesCapsule(); + + void runAllTests(); +} + +#endif // hifi_VerletShapeTests_h