mirror of
https://github.com/HifiExperiments/overte.git
synced 2025-08-09 14:29:13 +02:00
forgot to add the test files
This commit is contained in:
Andrew Meadows
parent
3791b4712b
commit
715cc3467d
2 changed files with 798 additions and 0 deletions
768
tests/physics/src/VerletShapeTests.cpp
Normal file
768
tests/physics/src/VerletShapeTests.cpp
Normal file
|
|
@ -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 <stdio.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include <glm/glm.hpp>
|
||||||
|
#include <glm/gtx/quaternion.hpp>
|
||||||
|
|
||||||
|
#include <CollisionInfo.h>
|
||||||
|
#include <ShapeCollider.h>
|
||||||
|
#include <SharedUtil.h>
|
||||||
|
#include <VerletCapsuleShape.h>
|
||||||
|
#include <VerletSphereShape.h>
|
||||||
|
#include <StreamUtils.h>
|
||||||
|
|
||||||
|
#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();
|
||||||
|
}
|
||||||
30
tests/physics/src/VerletShapeTests.h
Normal file
30
tests/physics/src/VerletShapeTests.h
Normal file
|
|
@ -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
|
||||||
Loading…
Reference in a new issue