Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-23 15:13:41 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #3312 from AndrewMeadows/ragdoll

Browse source 
use dispatchTable for shape-vs-shape collision queries
This commit is contained in:
Andrzej Kapolka 2014-08-25 15:43:48 -07:00
commit e29a4e7683
17 changed files with 413 additions and 578 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
interface/src/avatar/MyAvatar.cpp
View file

@ -81,6 +81,7 @@ MyAvatar::MyAvatar() :
_billboardValid(false),
_physicsSimulation()
{
ShapeCollider::initDispatchTable();
for (int i = 0; i < MAX_DRIVE_KEYS; i++) {
_driveKeys[i] = 0.0f;
}

18
interface/src/avatar/SkeletonModel.cpp
View file

@ -620,19 +620,19 @@ void SkeletonModel::buildShapes() {
Shape::Type type = joint.shapeType;
int parentIndex = joint.parentIndex;
if (parentIndex == -1 || radius < EPSILON) {
type = Shape::UNKNOWN_SHAPE;
} else if (type == Shape::CAPSULE_SHAPE && halfHeight < EPSILON) {
type = UNKNOWN_SHAPE;
} else if (type == CAPSULE_SHAPE && halfHeight < EPSILON) {
// this shape is forced to be a sphere
type = Shape::SPHERE_SHAPE;
type = SPHERE_SHAPE;
}
Shape* shape = NULL;
if (type == Shape::SPHERE_SHAPE) {
if (type == SPHERE_SHAPE) {
shape = new VerletSphereShape(radius, &(points[i]));
shape->setEntity(this);
float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
points[i].setMass(mass);
totalMass += mass;
} else if (type == Shape::CAPSULE_SHAPE) {
} else if (type == CAPSULE_SHAPE) {
assert(parentIndex != -1);
shape = new VerletCapsuleShape(radius, &(points[parentIndex]), &(points[i]));
shape->setEntity(this);
@ -731,7 +731,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
shapeExtents.reset();
glm::vec3 localPosition = shape->getTranslation();
int type = shape->getType();
if (type == Shape::CAPSULE_SHAPE) {
if (type == CAPSULE_SHAPE) {
// add the two furthest surface points of the capsule
CapsuleShape* capsule = static_cast<CapsuleShape*>(shape);
glm::vec3 axis;
@ -743,7 +743,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
shapeExtents.addPoint(localPosition + axis);
shapeExtents.addPoint(localPosition - axis);
totalExtents.addExtents(shapeExtents);
} else if (type == Shape::SPHERE_SHAPE) {
} else if (type == SPHERE_SHAPE) {
float radius = shape->getBoundingRadius();
glm::vec3 axis = glm::vec3(radius);
shapeExtents.addPoint(localPosition + axis);
@ -847,13 +847,13 @@ void SkeletonModel::renderJointCollisionShapes(float alpha) {
glPushMatrix();
// shapes are stored in simulation-frame but we want position to be model-relative
if (shape->getType() == Shape::SPHERE_SHAPE) {
if (shape->getType() == SPHERE_SHAPE) {
glm::vec3 position = shape->getTranslation() - simulationTranslation;
glTranslatef(position.x, position.y, position.z);
// draw a grey sphere at shape position
glColor4f(0.75f, 0.75f, 0.75f, alpha);
glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
} else if (shape->getType() == Shape::CAPSULE_SHAPE) {
} else if (shape->getType() == CAPSULE_SHAPE) {
CapsuleShape* capsule = static_cast<CapsuleShape*>(shape);
// draw a blue sphere at the capsule endpoint

2
interface/src/renderer/JointState.cpp
View file

@ -73,7 +73,7 @@ void JointState::setFBXJoint(const FBXJoint* joint) {
}
}
void JointState::updateConstraint() {
void JointState::buildConstraint() {
if (_constraint) {
delete _constraint;
_constraint = NULL;

2
interface/src/renderer/JointState.h
View file

@ -32,7 +32,7 @@ public:
void setFBXJoint(const FBXJoint* joint);
const FBXJoint& getFBXJoint() const { return *_fbxJoint; }
void updateConstraint();
void buildConstraint();
void copyState(const JointState& state);
void initTransform(const glm::mat4& parentTransform);

4
interface/src/renderer/Model.cpp
View file

@ -547,7 +547,7 @@ void Model::setJointStates(QVector<JointState> states) {
if (distance > radius) {
radius = distance;
}
_jointStates[i].updateConstraint();
_jointStates[i].buildConstraint();
}
for (int i = 0; i < _jointStates.size(); i++) {
_jointStates[i].slaveVisibleTransform();
@ -1194,7 +1194,7 @@ void Model::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm:
}
// Apply the rotation, but use mixRotationDelta() which blends a bit of the default pose
// at in the process. This provides stability to the IK solution for most models.
// in the process. This provides stability to the IK solution for most models.
glm::quat oldNextRotation = nextState.getRotation();
float mixFactor = 0.03f;
nextState.mixRotationDelta(deltaRotation, mixFactor, priority);

8
libraries/fbx/src/FBXReader.cpp
View file

@ -1503,7 +1503,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
joint.inverseBindRotation = joint.inverseDefaultRotation;
joint.name = model.name;
joint.shapePosition = glm::vec3(0.f);
joint.shapeType = Shape::UNKNOWN_SHAPE;
joint.shapeType = UNKNOWN_SHAPE;
geometry.joints.append(joint);
geometry.jointIndices.insert(model.name, geometry.joints.size());
@ -1848,10 +1848,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
if (collideLikeCapsule) {
joint.shapeRotation = rotationBetween(defaultCapsuleAxis, jointShapeInfo.boneBegin);
joint.shapePosition = 0.5f * jointShapeInfo.boneBegin;
joint.shapeType = Shape::CAPSULE_SHAPE;
joint.shapeType = CAPSULE_SHAPE;
} else {
// collide the joint like a sphere
joint.shapeType = Shape::SPHERE_SHAPE;
joint.shapeType = SPHERE_SHAPE;
if (jointShapeInfo.numVertices > 0) {
jointShapeInfo.averageVertex /= (float)jointShapeInfo.numVertices;
joint.shapePosition = jointShapeInfo.averageVertex;
@ -1872,7 +1872,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
// The shape is further from both joint endpoints than the endpoints are from each other
// which probably means the model has a bad transform somewhere. We disable this shape
// by setting its type to UNKNOWN_SHAPE.
joint.shapeType = Shape::UNKNOWN_SHAPE;
joint.shapeType = UNKNOWN_SHAPE;
}
}
}

8
libraries/shared/src/CapsuleShape.cpp
View file

@ -18,20 +18,20 @@
#include "SharedUtil.h"
CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE), _radius(0.0f), _halfHeight(0.0f) {}
CapsuleShape::CapsuleShape() : Shape(CAPSULE_SHAPE), _radius(0.0f), _halfHeight(0.0f) {}
CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(Shape::CAPSULE_SHAPE),
CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(CAPSULE_SHAPE),
_radius(radius), _halfHeight(halfHeight) {
updateBoundingRadius();
}
CapsuleShape::CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation) :
Shape(Shape::CAPSULE_SHAPE, position, rotation), _radius(radius), _halfHeight(halfHeight) {
Shape(CAPSULE_SHAPE, position, rotation), _radius(radius), _halfHeight(halfHeight) {
updateBoundingRadius();
}
CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint) :
Shape(Shape::CAPSULE_SHAPE), _radius(radius), _halfHeight(0.0f) {
Shape(CAPSULE_SHAPE), _radius(radius), _halfHeight(0.0f) {
setEndPoints(startPoint, endPoint);
}

2
libraries/shared/src/PhysicsEntity.cpp
View file

@ -12,8 +12,10 @@
#include "PhysicsEntity.h"
#include "PhysicsSimulation.h"
#include "PlaneShape.h"
#include "Shape.h"
#include "ShapeCollider.h"
#include "SphereShape.h"
PhysicsEntity::PhysicsEntity() :
_translation(0.0f),

3
libraries/shared/src/PhysicsSimulation.cpp
View file

@ -16,8 +16,9 @@
#include "PerfStat.h"
#include "PhysicsEntity.h"
#include "Ragdoll.h"
#include "SharedUtil.h"
#include "Shape.h"
#include "ShapeCollider.h"
#include "SharedUtil.h"
int MAX_DOLLS_PER_SIMULATION = 16;
int MAX_ENTITIES_PER_SIMULATION = 64;

2
libraries/shared/src/PlaneShape.cpp
View file

@ -15,7 +15,7 @@
const glm::vec3 UNROTATED_NORMAL(0.0f, 1.0f, 0.0f);
PlaneShape::PlaneShape(const glm::vec4& coefficients) :
Shape(Shape::PLANE_SHAPE) {
Shape(PLANE_SHAPE) {
glm::vec3 normal = glm::vec3(coefficients);
_translation = -normal * coefficients.w;

23
libraries/shared/src/Shape.h
View file

@ -22,17 +22,18 @@ class VerletPoint;
const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX)
const quint8 SPHERE_SHAPE = 0;
const quint8 CAPSULE_SHAPE = 1;
const quint8 PLANE_SHAPE = 2;
const quint8 LIST_SHAPE = 3;
const quint8 UNKNOWN_SHAPE = 4;
class Shape {
public:
static quint32 getNextID() { static quint32 nextID = 0; return ++nextID; }
enum Type{
UNKNOWN_SHAPE = 0,
SPHERE_SHAPE,
CAPSULE_SHAPE,
PLANE_SHAPE,
LIST_SHAPE
};
typedef quint8 Type;
static quint32 getNextID() { static quint32 nextID = 0; return ++nextID; }
Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f),
_translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) {
@ -40,7 +41,7 @@ public:
}
virtual ~Shape() { }
int getType() const { return _type; }
Type getType() const { return _type; }
quint32 getID() const { return _id; }
void setEntity(PhysicsEntity* entity) { _owningEntity = entity; }
@ -95,8 +96,8 @@ protected:
void setBoundingRadius(float radius) { _boundingRadius = radius; }
int _type;
unsigned int _id;
Type _type;
quint32 _id;
PhysicsEntity* _owningEntity;
float _boundingRadius;
glm::vec3 _translation;

373
libraries/shared/src/ShapeCollider.cpp
View file

@ -15,85 +15,70 @@
#include "GeometryUtil.h"
#include "ShapeCollider.h"
#include "CapsuleShape.h"
#include "ListShape.h"
#include "PlaneShape.h"
#include "SphereShape.h"
// NOTE:
//
// * Large ListShape's are inefficient keep the lists short.
// * Collisions between lists of lists work in theory but are not recommended.
const Shape::Type NUM_SHAPE_TYPES = 5;
const quint8 NUM__DISPATCH_CELLS = NUM_SHAPE_TYPES * NUM_SHAPE_TYPES;
Shape::Type getDispatchKey(Shape::Type typeA, Shape::Type typeB) {
return typeA + NUM_SHAPE_TYPES * typeB;
}
// dummy dispatch for any non-implemented pairings
bool notImplemented(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
return false;
}
// NOTE: hardcode the number of dispatchTable entries (NUM_SHAPE_TYPES ^2)
bool (*dispatchTable[NUM__DISPATCH_CELLS])(const Shape*, const Shape*, CollisionList&);
namespace ShapeCollider {
bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
// TODO: make a fast lookup for correct method
int typeA = shapeA->getType();
int typeB = shapeB->getType();
if (typeA == Shape::SPHERE_SHAPE) {
const SphereShape* sphereA = static_cast<const SphereShape*>(shapeA);
if (typeB == Shape::SPHERE_SHAPE) {
return sphereSphere(sphereA, static_cast<const SphereShape*>(shapeB), collisions);
} else if (typeB == Shape::CAPSULE_SHAPE) {
return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB), collisions);
} else if (typeB == Shape::PLANE_SHAPE) {
return spherePlane(sphereA, static_cast<const PlaneShape*>(shapeB), collisions);
}
} else if (typeA == Shape::CAPSULE_SHAPE) {
const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
if (typeB == Shape::SPHERE_SHAPE) {
return capsuleSphere(capsuleA, static_cast<const SphereShape*>(shapeB), collisions);
} else if (typeB == Shape::CAPSULE_SHAPE) {
return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(shapeB), collisions);
} else if (typeB == Shape::PLANE_SHAPE) {
return capsulePlane(capsuleA, static_cast<const PlaneShape*>(shapeB), collisions);
}
} else if (typeA == Shape::PLANE_SHAPE) {
const PlaneShape* planeA = static_cast<const PlaneShape*>(shapeA);
if (typeB == Shape::SPHERE_SHAPE) {
return planeSphere(planeA, static_cast<const SphereShape*>(shapeB), collisions);
} else if (typeB == Shape::CAPSULE_SHAPE) {
return planeCapsule(planeA, static_cast<const CapsuleShape*>(shapeB), collisions);
} else if (typeB == Shape::PLANE_SHAPE) {
return planePlane(planeA, static_cast<const PlaneShape*>(shapeB), collisions);
}
} else if (typeA == Shape::LIST_SHAPE) {
const ListShape* listA = static_cast<const ListShape*>(shapeA);
if (typeB == Shape::SPHERE_SHAPE) {
return listSphere(listA, static_cast<const SphereShape*>(shapeB), collisions);
} else if (typeB == Shape::CAPSULE_SHAPE) {
return listCapsule(listA, static_cast<const CapsuleShape*>(shapeB), collisions);
} else if (typeB == Shape::PLANE_SHAPE) {
return listPlane(listA, static_cast<const PlaneShape*>(shapeB), collisions);
}
// NOTE: the dispatch table must be initialized before the ShapeCollider is used.
void initDispatchTable() {
for (Shape::Type i = 0; i < NUM__DISPATCH_CELLS; ++i) {
dispatchTable[i] = &notImplemented;
}
return false;
// NOTE: no need to update any that are notImplemented, but we leave them
// commented out in the code so that we remember that they exist.
dispatchTable[getDispatchKey(SPHERE_SHAPE, SPHERE_SHAPE)] = &sphereVsSphere;
dispatchTable[getDispatchKey(SPHERE_SHAPE, CAPSULE_SHAPE)] = &sphereVsCapsule;
dispatchTable[getDispatchKey(SPHERE_SHAPE, PLANE_SHAPE)] = &sphereVsPlane;
dispatchTable[getDispatchKey(SPHERE_SHAPE, LIST_SHAPE)] = &shapeVsList;
dispatchTable[getDispatchKey(CAPSULE_SHAPE, SPHERE_SHAPE)] = &capsuleVsSphere;
dispatchTable[getDispatchKey(CAPSULE_SHAPE, CAPSULE_SHAPE)] = &capsuleVsCapsule;
dispatchTable[getDispatchKey(CAPSULE_SHAPE, PLANE_SHAPE)] = &capsuleVsPlane;
dispatchTable[getDispatchKey(CAPSULE_SHAPE, LIST_SHAPE)] = &shapeVsList;
dispatchTable[getDispatchKey(PLANE_SHAPE, SPHERE_SHAPE)] = &planeVsSphere;
dispatchTable[getDispatchKey(PLANE_SHAPE, CAPSULE_SHAPE)] = &planeVsCapsule;
dispatchTable[getDispatchKey(PLANE_SHAPE, PLANE_SHAPE)] = &planeVsPlane;
dispatchTable[getDispatchKey(PLANE_SHAPE, LIST_SHAPE)] = &shapeVsList;
dispatchTable[getDispatchKey(LIST_SHAPE, SPHERE_SHAPE)] = &listVsShape;
dispatchTable[getDispatchKey(LIST_SHAPE, CAPSULE_SHAPE)] = &listVsShape;
dispatchTable[getDispatchKey(LIST_SHAPE, PLANE_SHAPE)] = &listVsShape;
dispatchTable[getDispatchKey(LIST_SHAPE, LIST_SHAPE)] = &listVsList;
// all of the UNKNOWN_SHAPE pairings are notImplemented
}
bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
return (*dispatchTable[shapeA->getType() + NUM_SHAPE_TYPES * shapeB->getType()])(shapeA, shapeB, collisions);
}
static CollisionList tempCollisions(32);
bool collideShapesCoarse(const QVector<const Shape*>& shapesA, const QVector<const Shape*>& shapesB, CollisionInfo& collision) {
tempCollisions.clear();
foreach (const Shape* shapeA, shapesA) {
foreach (const Shape* shapeB, shapesB) {
collideShapes(shapeA, shapeB, tempCollisions);
}
}
if (tempCollisions.size() > 0) {
glm::vec3 totalPenetration(0.0f);
glm::vec3 averageContactPoint(0.0f);
for (int j = 0; j < tempCollisions.size(); ++j) {
CollisionInfo* c = tempCollisions.getCollision(j);
totalPenetration = addPenetrations(totalPenetration, c->_penetration);
averageContactPoint += c->_contactPoint;
}
collision._penetration = totalPenetration;
collision._contactPoint = averageContactPoint / (float)(tempCollisions.size());
// there are no valid shape pointers for this collision so we set them NULL
collision._shapeA = NULL;
collision._shapeB = NULL;
return true;
}
return false;
}
bool collideShapeWithShapes(const Shape* shapeA, const QVector<Shape*>& shapes, int startIndex, CollisionList& collisions) {
bool collided = false;
if (shapeA) {
@ -133,21 +118,21 @@ bool collideShapesWithShapes(const QVector<Shape*>& shapesA, const QVector<Shape
}
bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
int typeA = shapeA->getType();
if (typeA == Shape::SPHERE_SHAPE) {
return sphereAACube(static_cast<const SphereShape*>(shapeA), cubeCenter, cubeSide, collisions);
} else if (typeA == Shape::CAPSULE_SHAPE) {
return capsuleAACube(static_cast<const CapsuleShape*>(shapeA), cubeCenter, cubeSide, collisions);
} else if (typeA == Shape::LIST_SHAPE) {
Shape::Type typeA = shapeA->getType();
if (typeA == SPHERE_SHAPE) {
return sphereVsAACube(static_cast<const SphereShape*>(shapeA), cubeCenter, cubeSide, collisions);
} else if (typeA == CAPSULE_SHAPE) {
return capsuleVsAACube(static_cast<const CapsuleShape*>(shapeA), cubeCenter, cubeSide, collisions);
} else if (typeA == LIST_SHAPE) {
const ListShape* listA = static_cast<const ListShape*>(shapeA);
bool touching = false;
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = sphereAACube(static_cast<const SphereShape*>(subShape), cubeCenter, cubeSide, collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsuleAACube(static_cast<const CapsuleShape*>(subShape), cubeCenter, cubeSide, collisions) || touching;
if (subType == SPHERE_SHAPE) {
touching = sphereVsAACube(static_cast<const SphereShape*>(subShape), cubeCenter, cubeSide, collisions) || touching;
} else if (subType == CAPSULE_SHAPE) {
touching = capsuleVsAACube(static_cast<const CapsuleShape*>(subShape), cubeCenter, cubeSide, collisions) || touching;
}
}
return touching;
@ -155,7 +140,9 @@ bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, fl
return false;
}
bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions) {
bool sphereVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
const SphereShape* sphereA = static_cast<const SphereShape*>(shapeA);
const SphereShape* sphereB = static_cast<const SphereShape*>(shapeB);
glm::vec3 BA = sphereB->getTranslation() - sphereA->getTranslation();
float distanceSquared = glm::dot(BA, BA);
float totalRadius = sphereA->getRadius() + sphereB->getRadius();
@ -183,7 +170,9 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis
return false;
}
bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions) {
bool sphereVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
const SphereShape* sphereA = static_cast<const SphereShape*>(shapeA);
const CapsuleShape* capsuleB = static_cast<const CapsuleShape*>(shapeB);
// find sphereA's closest approach to axis of capsuleB
glm::vec3 BA = capsuleB->getTranslation() - sphereA->getTranslation();
glm::vec3 capsuleAxis;
@ -252,7 +241,9 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col
return false;
}
bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, CollisionList& collisions) {
bool sphereVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
const SphereShape* sphereA = static_cast<const SphereShape*>(shapeA);
const PlaneShape* planeB = static_cast<const PlaneShape*>(shapeB);
glm::vec3 penetration;
if (findSpherePlanePenetration(sphereA->getTranslation(), sphereA->getRadius(), planeB->getCoefficients(), penetration)) {
CollisionInfo* collision = collisions.getNewCollision();
@ -268,79 +259,8 @@ bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, Collision
return false;
}
bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions) {
// find sphereB's closest approach to axis of capsuleA
glm::vec3 AB = capsuleA->getTranslation() - sphereB->getTranslation();
glm::vec3 capsuleAxis;
capsuleA->computeNormalizedAxis(capsuleAxis);
float axialDistance = - glm::dot(AB, capsuleAxis);
float absAxialDistance = fabsf(axialDistance);
float totalRadius = sphereB->getRadius() + capsuleA->getRadius();
if (absAxialDistance < totalRadius + capsuleA->getHalfHeight()) {
glm::vec3 radialAxis = AB + axialDistance * capsuleAxis; // from sphereB to axis of capsuleA
float radialDistance2 = glm::length2(radialAxis);
float totalRadius2 = totalRadius * totalRadius;
if (radialDistance2 > totalRadius2) {
// sphere is too far from capsule axis
return false;
}
// closestApproach = point on capsuleA's axis that is closest to sphereB's center
glm::vec3 closestApproach = capsuleA->getTranslation() + axialDistance * capsuleAxis;
if (absAxialDistance > capsuleA->getHalfHeight()) {
// sphere hits capsule on a cap
// --> recompute radialAxis and closestApproach
float sign = (axialDistance > 0.0f) ? 1.0f : -1.0f;
closestApproach = capsuleA->getTranslation() + (sign * capsuleA->getHalfHeight()) * capsuleAxis;
radialAxis = closestApproach - sphereB->getTranslation();
radialDistance2 = glm::length2(radialAxis);
if (radialDistance2 > totalRadius2) {
return false;
}
}
if (radialDistance2 > EPSILON * EPSILON) {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// normalize the radialAxis
float radialDistance = sqrtf(radialDistance2);
radialAxis /= radialDistance;
// penetration points from A into B
collision->_penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B
// contactPoint is on surface of capsuleA
collision->_contactPoint = closestApproach - capsuleA->getRadius() * radialAxis;
collision->_shapeA = capsuleA;
collision->_shapeB = sphereB;
} else {
// A is on B's axis, so the penetration is undefined...
if (absAxialDistance > capsuleA->getHalfHeight()) {
// ...for the cylinder case (for now we pretend the collision doesn't exist)
return false;
} else {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// ... but still defined for the cap case
if (axialDistance < 0.0f) {
// we're hitting the start cap, so we negate the capsuleAxis
capsuleAxis *= -1;
}
float sign = (axialDistance > 0.0f) ? 1.0f : -1.0f;
collision->_penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis;
// contactPoint is on surface of sphereA
collision->_contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis;
collision->_shapeA = capsuleA;
collision->_shapeB = sphereB;
}
}
return true;
}
return false;
bool capsuleVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
return sphereVsCapsule(shapeB, shapeA, collisions);
}
/// \param lineP point on line
@ -409,7 +329,9 @@ bool lineCylinder(const glm::vec3& lineP, const glm::vec3& lineDir,
return true;
}
bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions) {
bool capsuleVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
const CapsuleShape* capsuleB = static_cast<const CapsuleShape*>(shapeB);
glm::vec3 axisA;
capsuleA->computeNormalizedAxis(axisA);
glm::vec3 axisB;
@ -568,7 +490,9 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
return false;
}
bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, CollisionList& collisions) {
bool capsuleVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
const PlaneShape* planeB = static_cast<const PlaneShape*>(shapeB);
glm::vec3 start, end, penetration;
capsuleA->getStartPoint(start);
capsuleA->getEndPoint(end);
@ -588,147 +512,44 @@ bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, Collis
return false;
}
bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, CollisionList& collisions) {
glm::vec3 penetration;
if (findSpherePlanePenetration(sphereB->getTranslation(), sphereB->getRadius(), planeA->getCoefficients(), penetration)) {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
return false; // collision list is full
}
collision->_penetration = -penetration;
collision->_contactPoint = sphereB->getTranslation() +
(sphereB->getRadius() / glm::length(penetration) - 1.0f) * penetration;
collision->_shapeA = planeA;
collision->_shapeB = sphereB;
return true;
}
return false;
bool planeVsSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
return sphereVsPlane(shapeB, shapeA, collisions);
}
bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, CollisionList& collisions) {
glm::vec3 start, end, penetration;
capsuleB->getStartPoint(start);
capsuleB->getEndPoint(end);
glm::vec4 plane = planeA->getCoefficients();
if (findCapsulePlanePenetration(start, end, capsuleB->getRadius(), plane, penetration)) {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
return false; // collision list is full
}
collision->_penetration = -penetration;
glm::vec3 deepestEnd = (glm::dot(start, glm::vec3(plane)) < glm::dot(end, glm::vec3(plane))) ? start : end;
collision->_contactPoint = deepestEnd + (capsuleB->getRadius() / glm::length(penetration) - 1.0f) * penetration;
collision->_shapeA = planeA;
collision->_shapeB = capsuleB;
return true;
}
return false;
bool planeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
return capsuleVsPlane(shapeB, shapeA, collisions);
}
bool planePlane(const PlaneShape* planeA, const PlaneShape* planeB, CollisionList& collisions) {
bool planeVsPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
// technically, planes always collide unless they're parallel and not coincident; however, that's
// not going to give us any useful information
return false;
}
bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions) {
bool shapeVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
bool touching = false;
const ListShape* listB = static_cast<const ListShape*>(shapeB);
for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listB->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = sphereSphere(sphereA, static_cast<const SphereShape*>(subShape), collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = sphereCapsule(sphereA, static_cast<const CapsuleShape*>(subShape), collisions) || touching;
} else if (subType == Shape::PLANE_SHAPE) {
touching = spherePlane(sphereA, static_cast<const PlaneShape*>(subShape), collisions) || touching;
}
touching = collideShapes(shapeA, subShape, collisions) || touching;
}
return touching;
}
bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listB->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = capsuleSphere(capsuleA, static_cast<const SphereShape*>(subShape), collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(subShape), collisions) || touching;
} else if (subType == Shape::PLANE_SHAPE) {
touching = capsulePlane(capsuleA, static_cast<const PlaneShape*>(subShape), collisions) || touching;
}
}
return touching;
}
bool planeList(const PlaneShape* planeA, const ListShape* listB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listB->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = planeSphere(planeA, static_cast<const SphereShape*>(subShape), collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = planeCapsule(planeA, static_cast<const CapsuleShape*>(subShape), collisions) || touching;
} else if (subType == Shape::PLANE_SHAPE) {
touching = planePlane(planeA, static_cast<const PlaneShape*>(subShape), collisions) || touching;
}
}
return touching;
}
bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions) {
bool listVsShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
bool touching = false;
const ListShape* listA = static_cast<const ListShape*>(shapeA);
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = sphereSphere(static_cast<const SphereShape*>(subShape), sphereB, collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsuleSphere(static_cast<const CapsuleShape*>(subShape), sphereB, collisions) || touching;
} else if (subType == Shape::PLANE_SHAPE) {
touching = planeSphere(static_cast<const PlaneShape*>(subShape), sphereB, collisions) || touching;
}
touching = collideShapes(subShape, shapeB, collisions) || touching;
}
return touching;
}
bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = sphereCapsule(static_cast<const SphereShape*>(subShape), capsuleB, collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsuleCapsule(static_cast<const CapsuleShape*>(subShape), capsuleB, collisions) || touching;
} else if (subType == Shape::PLANE_SHAPE) {
touching = planeCapsule(static_cast<const PlaneShape*>(subShape), capsuleB, collisions) || touching;
}
}
return touching;
}
bool listPlane(const ListShape* listA, const PlaneShape* planeB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = spherePlane(static_cast<const SphereShape*>(subShape), planeB, collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsulePlane(static_cast<const CapsuleShape*>(subShape), planeB, collisions) || touching;
} else if (subType == Shape::PLANE_SHAPE) {
touching = planePlane(static_cast<const PlaneShape*>(subShape), planeB, collisions) || touching;
}
}
return touching;
}
bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions) {
bool listVsList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
bool touching = false;
const ListShape* listA = static_cast<const ListShape*>(shapeA);
const ListShape* listB = static_cast<const ListShape*>(shapeB);
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
for (int j = 0; j < listB->size() && !collisions.isFull(); ++j) {
@ -739,7 +560,7 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col
}
// helper function
bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter,
bool sphereVsAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter,
float cubeSide, CollisionList& collisions) {
// sphere is A
// cube is B
@ -887,11 +708,11 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius,
}
*/
bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
return sphereAACube(sphereA->getTranslation(), sphereA->getRadius(), cubeCenter, cubeSide, collisions);
bool sphereVsAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
return sphereVsAACube(sphereA->getTranslation(), sphereA->getRadius(), cubeCenter, cubeSide, collisions);
}
bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
// find nerest approach of capsule line segment to cube
glm::vec3 capsuleAxis;
capsuleA->computeNormalizedAxis(capsuleAxis);
@ -904,7 +725,7 @@ bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, fl
}
glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis;
// collide nearest approach like a sphere at that point
return sphereAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions);
return sphereVsAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions);
}
bool findRayIntersectionWithShapes(const QVector<Shape*> shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) {

83
libraries/shared/src/ShapeCollider.h
View file

@ -14,27 +14,24 @@
#include <QVector>
#include "CapsuleShape.h"
#include "CollisionInfo.h"
#include "ListShape.h"
#include "PlaneShape.h"
#include "SharedUtil.h"
#include "SphereShape.h"
class Shape;
class SphereShape;
class CapsuleShape;
namespace ShapeCollider {
/// MUST CALL this FIRST before using the ShapeCollider
void initDispatchTable();
/// \param shapeA pointer to first shape (cannot be NULL)
/// \param shapeB pointer to second shape (cannot be NULL)
/// \param collisions[out] collision details
/// \return true if shapes collide
bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions);
/// \param shapesA list of shapes
/// \param shapeB list of shapes
/// \param collisions[out] average collision details
/// \return true if any shapes collide
bool collideShapesCoarse(const QVector<const Shape*>& shapesA, const QVector<const Shape*>& shapesB, CollisionInfo& collision);
bool collideShapeWithShapes(const Shape* shapeA, const QVector<Shape*>& shapes, int startIndex, CollisionList& collisions);
bool collideShapesWithShapes(const QVector<Shape*>& shapesA, const QVector<Shape*>& shapesB, CollisionList& collisions);
@ -49,111 +46,87 @@ namespace ShapeCollider {
/// \param sphereB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions);
bool sphereVsSphere(const Shape* sphereA, const Shape* sphereB, CollisionList& collisions);
/// \param sphereA pointer to first shape (cannot be NULL)
/// \param capsuleB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions);
bool sphereVsCapsule(const Shape* sphereA, const Shape* capsuleB, CollisionList& collisions);
/// \param sphereA pointer to first shape (cannot be NULL)
/// \param planeB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, CollisionList& collisions);
bool sphereVsPlane(const Shape* sphereA, const Shape* planeB, CollisionList& collisions);
/// \param capsuleA pointer to first shape (cannot be NULL)
/// \param sphereB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions);
bool capsuleVsSphere(const Shape* capsuleA, const Shape* sphereB, CollisionList& collisions);
/// \param capsuleA pointer to first shape (cannot be NULL)
/// \param capsuleB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions);
bool capsuleVsCapsule(const Shape* capsuleA, const Shape* capsuleB, CollisionList& collisions);
/// \param capsuleA pointer to first shape (cannot be NULL)
/// \param planeB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, CollisionList& collisions);
bool capsuleVsPlane(const Shape* capsuleA, const Shape* planeB, CollisionList& collisions);
/// \param planeA pointer to first shape (cannot be NULL)
/// \param sphereB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, CollisionList& collisions);
bool planeVsSphere(const Shape* planeA, const Shape* sphereB, CollisionList& collisions);
/// \param planeA pointer to first shape (cannot be NULL)
/// \param capsuleB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, CollisionList& collisions);
bool planeVsCapsule(const Shape* planeA, const Shape* capsuleB, CollisionList& collisions);
/// \param planeA pointer to first shape (cannot be NULL)
/// \param planeB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool planePlane(const PlaneShape* planeA, const PlaneShape* planeB, CollisionList& collisions);
bool planeVsPlane(const Shape* planeA, const Shape* planeB, CollisionList& collisions);
/// \param sphereA pointer to first shape (cannot be NULL)
/// \param shapeA pointer to first shape (cannot be NULL)
/// \param listB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions);
bool shapeVsList(const Shape* shapeA, const Shape* listB, CollisionList& collisions);
/// \param capuleA pointer to first shape (cannot be NULL)
/// \param listA pointer to first shape (cannot be NULL)
/// \param shapeB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listVsShape(const Shape* listA, const Shape* shapeB, CollisionList& collisions);
/// \param listA pointer to first shape (cannot be NULL)
/// \param listB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions);
/// \param planeA pointer to first shape (cannot be NULL)
/// \param listB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool planeList(const PlaneShape* planeA, const ListShape* listB, CollisionList& collisions);
/// \param listA pointer to first shape (cannot be NULL)
/// \param sphereB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions);
/// \param listA pointer to first shape (cannot be NULL)
/// \param capsuleB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions);
/// \param listA pointer to first shape (cannot be NULL)
/// \param planeB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listPlane(const ListShape* listA, const PlaneShape* planeB, CollisionList& collisions);
/// \param listA pointer to first shape (cannot be NULL)
/// \param capsuleB pointer to second shape (cannot be NULL)
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions);
bool listVsList(const Shape* listA, const Shape* listB, CollisionList& collisions);
/// \param sphereA pointer to sphere (cannot be NULL)
/// \param cubeCenter center of cube
/// \param cubeSide lenght of side of cube
/// \param[out] collisions where to append collision details
/// \return true if sphereA collides with axis aligned cube
bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
bool sphereVsAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
/// \param capsuleA pointer to capsule (cannot be NULL)
/// \param cubeCenter center of cube
/// \param cubeSide lenght of side of cube
/// \param[out] collisions where to append collision details
/// \return true if capsuleA collides with axis aligned cube
bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
bool capsuleVsAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
/// \param shapes list of pointers to shapes (shape pointers may be NULL)
/// \param startPoint beginning of ray

6
libraries/shared/src/SphereShape.h
View file

@ -18,13 +18,13 @@
class SphereShape : public Shape {
public:
SphereShape() : Shape(Shape::SPHERE_SHAPE) {}
SphereShape() : Shape(SPHERE_SHAPE) {}
SphereShape(float radius) : Shape(Shape::SPHERE_SHAPE) {
SphereShape(float radius) : Shape(SPHERE_SHAPE) {
_boundingRadius = radius;
}
SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE, position) {
SphereShape(float radius, const glm::vec3& position) : Shape(SPHERE_SHAPE, position) {
_boundingRadius = radius;
}

297
tests/physics/src/ShapeColliderTests.cpp
View file

@ -16,7 +16,9 @@
#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>
#include <CapsuleShape.h>
#include <CollisionInfo.h>
#include <PlaneShape.h>
#include <ShapeCollider.h>
#include <SharedUtil.h>
#include <SphereShape.h>
@ -71,8 +73,7 @@ void ShapeColliderTests::sphereMissesSphere() {
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size()
<< std::endl;
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
@ -112,6 +113,7 @@ void ShapeColliderTests::sphereTouchesSphere() {
if (!collision) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: null collision" << std::endl;
return;
}
// penetration points from sphereA into sphereB
@ -119,7 +121,7 @@ void ShapeColliderTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -129,7 +131,7 @@ void ShapeColliderTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
@ -149,7 +151,7 @@ void ShapeColliderTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -159,7 +161,7 @@ void ShapeColliderTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
}
@ -199,23 +201,20 @@ void ShapeColliderTests::sphereMissesCapsule() {
if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " 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;
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
@ -241,8 +240,7 @@ void ShapeColliderTests::sphereTouchesCapsule() {
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -254,7 +252,7 @@ void ShapeColliderTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -263,15 +261,14 @@ void ShapeColliderTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
@ -279,33 +276,41 @@ void ShapeColliderTests::sphereTouchesCapsule() {
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - (radialOffset - totalRadius) * xAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// 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);
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
closestApproach = sphereA.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;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
{ // sphereA hits end cap at axis
glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setTranslation(axialOffset * yAxis);
sphereA.setTranslation(axialOffset);
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -317,7 +322,7 @@ void ShapeColliderTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -326,15 +331,14 @@ void ShapeColliderTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
@ -342,33 +346,40 @@ void ShapeColliderTests::sphereTouchesCapsule() {
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 endPoint;
capsuleB.getEndPoint(endPoint);
expectedContactPoint = endPoint + radiusB * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedContactPoint = axialOffset - radiusA * yAxis;
}
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
{ // sphereA hits start cap at axis
glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setTranslation(axialOffset * yAxis);
sphereA.setTranslation(axialOffset);
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -380,7 +391,7 @@ void ShapeColliderTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -389,15 +400,14 @@ void ShapeColliderTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
@ -405,22 +415,30 @@ void ShapeColliderTests::sphereTouchesCapsule() {
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 startPoint;
capsuleB.getStartPoint(startPoint);
expectedContactPoint = startPoint - radiusB * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedContactPoint = axialOffset + radiusA * yAxis;
}
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
if (collisions.size() != numCollisions) {
@ -450,14 +468,12 @@ void ShapeColliderTests::capsuleMissesCapsule() {
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
// end to end
@ -465,14 +481,12 @@ void ShapeColliderTests::capsuleMissesCapsule() {
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
// rotate B and move it to the side
@ -482,20 +496,17 @@ void ShapeColliderTests::capsuleMissesCapsule() {
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " 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;
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
@ -520,16 +531,14 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -541,16 +550,14 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -564,16 +571,14 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -590,8 +595,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -602,7 +606,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis;
@ -610,15 +614,14 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB vs capsuleA
if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -629,8 +632,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
<< " actual = " << collision->_penetration << std::endl;
}
expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis;
@ -638,8 +640,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
@ -655,8 +656,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -667,8 +667,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
<< " actual = " << collision->_penetration << std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis;
@ -676,8 +675,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
}
@ -710,8 +708,9 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() {
sphereCenter = cubeCenter + sphereOffset * axis;
sphere.setTranslation(sphereCenter);
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl;
if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis
<< std::endl;
}
CollisionInfo* collision = collisions[0];
if (!collision) {
@ -721,17 +720,13 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() {
glm::vec3 expectedPenetration = - overlap * axis;
if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration
<< " expected " << expectedPenetration
<< " axis = " << axis
<< std::endl;
<< " expected " << expectedPenetration << " axis = " << axis << std::endl;
}
glm::vec3 expectedContact = sphereCenter - sphereRadius * axis;
if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint
<< " expected " << expectedContact
<< " axis = " << axis
<< std::endl;
<< " expected " << expectedContact << " axis = " << axis << std::endl;
}
}
@ -743,32 +738,26 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() {
sphereCenter = cubeCenter + sphereOffset * axis;
sphere.setTranslation(sphereCenter);
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube."
<< " axis = " << axis
<< std::endl;
<< " axis = " << axis << std::endl;
}
CollisionInfo* collision = collisions[0];
if (!collision) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo on y-axis."
<< " axis = " << axis
<< std::endl;
<< " axis = " << axis << std::endl;
}
glm::vec3 expectedPenetration = - overlap * axis;
if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration
<< " expected " << expectedPenetration
<< " axis = " << axis
<< std::endl;
<< " expected " << expectedPenetration << " axis = " << axis << std::endl;
}
glm::vec3 expectedContact = sphereCenter - sphereRadius * axis;
if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint
<< " expected " << expectedContact
<< " axis = " << axis
<< std::endl;
<< " expected " << expectedContact << " axis = " << axis << std::endl;
}
}
}
@ -817,7 +806,7 @@ void ShapeColliderTests::sphereTouchesAACubeEdges() {
sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis;
sphere.setTranslation(sphereCenter);
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (!ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl;
}
CollisionInfo* collision = collisions[i];
@ -828,17 +817,13 @@ void ShapeColliderTests::sphereTouchesAACubeEdges() {
glm::vec3 expectedPenetration = - overlap * axis;
if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration
<< " expected " << expectedPenetration
<< " axis = " << axis
<< std::endl;
<< " expected " << expectedPenetration << " axis = " << axis << std::endl;
}
glm::vec3 expectedContact = sphereCenter - sphereRadius * axis;
if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint
<< " expected " << expectedContact
<< " axis = " << axis
<< std::endl;
<< " expected " << expectedContact << " axis = " << axis << std::endl;
}
}
}
@ -858,42 +843,42 @@ void ShapeColliderTests::sphereMissesAACube() {
// top
sphereCenter = cubeCenter + sphereOffset * yAxis;
sphere.setTranslation(sphereCenter);
if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl;
}
// bottom
sphereCenter = cubeCenter - sphereOffset * yAxis;
sphere.setTranslation(sphereCenter);
if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl;
}
// left
sphereCenter = cubeCenter + sphereOffset * xAxis;
sphere.setTranslation(sphereCenter);
if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl;
}
// right
sphereCenter = cubeCenter - sphereOffset * xAxis;
sphere.setTranslation(sphereCenter);
if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl;
}
// forward
sphereCenter = cubeCenter + sphereOffset * zAxis;
sphere.setTranslation(sphereCenter);
if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl;
}
// back
sphereCenter = cubeCenter - sphereOffset * zAxis;
sphere.setTranslation(sphereCenter);
if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
if (ShapeCollider::sphereVsAACube(&sphere, cubeCenter, cubeSide, collisions)){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl;
}
}
@ -965,7 +950,8 @@ void ShapeColliderTests::rayHitsSphere() {
float expectedDistance = startDistance - radius;
float relativeError = fabsf(distance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = "
<< relativeError << std::endl;
}
}
}
@ -1022,7 +1008,8 @@ void ShapeColliderTests::rayBarelyMissesSphere() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
// translate and rotate the whole system...
@ -1040,7 +1027,8 @@ void ShapeColliderTests::rayBarelyMissesSphere() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
}
@ -1062,7 +1050,8 @@ void ShapeColliderTests::rayHitsCapsule() {
float expectedDistance = startDistance - radius;
float relativeError = fabsf(distance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
// toward top of cylindrical wall
@ -1073,7 +1062,8 @@ void ShapeColliderTests::rayHitsCapsule() {
}
relativeError = fabsf(distance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
// toward top cap
@ -1085,7 +1075,8 @@ void ShapeColliderTests::rayHitsCapsule() {
}
relativeError = fabsf(distance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
const float EDGE_CASE_SLOP_FACTOR = 20.0f;
@ -1100,7 +1091,8 @@ void ShapeColliderTests::rayHitsCapsule() {
relativeError = fabsf(distance - expectedDistance) / startDistance;
// for edge cases we allow a LOT of error
if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
// toward tip of bottom cap
@ -1113,7 +1105,8 @@ void ShapeColliderTests::rayHitsCapsule() {
relativeError = fabsf(distance - expectedDistance) / startDistance;
// for edge cases we allow a LOT of error
if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
// toward edge of capsule cylindrical face
@ -1127,7 +1120,8 @@ void ShapeColliderTests::rayHitsCapsule() {
relativeError = fabsf(distance - expectedDistance) / startDistance;
// for edge cases we allow a LOT of error
if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
}
// TODO: test at steep angles near cylinder/cap junction
@ -1154,7 +1148,8 @@ void ShapeColliderTests::rayMissesCapsule() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
// below bottom cap
@ -1164,7 +1159,8 @@ void ShapeColliderTests::rayMissesCapsule() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
// past edge of capsule cylindrical face
@ -1175,7 +1171,8 @@ void ShapeColliderTests::rayMissesCapsule() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
}
// TODO: test at steep angles near edge
@ -1201,7 +1198,8 @@ void ShapeColliderTests::rayHitsPlane() {
float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
float relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = "
<< relativeError << std::endl;
}
// rotate the whole system and try again
@ -1222,7 +1220,8 @@ void ShapeColliderTests::rayHitsPlane() {
expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " << relativeError << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = "
<< relativeError << std::endl;
}
}
@ -1243,7 +1242,8 @@ void ShapeColliderTests::rayMissesPlane() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
// rotate the whole system and try again
@ -1261,7 +1261,8 @@ void ShapeColliderTests::rayMissesPlane() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
}
@ -1275,7 +1276,8 @@ void ShapeColliderTests::rayMissesPlane() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
// rotate the whole system and try again
@ -1293,12 +1295,47 @@ void ShapeColliderTests::rayMissesPlane() {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
}
}
void ShapeColliderTests::measureTimeOfCollisionDispatch() {
/* KEEP for future manual testing
// create two non-colliding spheres
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;
SphereShape sphereA(radiusA, origin);
SphereShape sphereB(radiusB, offsetDistance * offsetDirection);
CollisionList collisions(16);
//int numTests = 1;
quint64 oldTime;
quint64 newTime;
int numTests = 100000000;
{
quint64 startTime = usecTimestampNow();
for (int i = 0; i < numTests; ++i) {
ShapeCollider::collideShapes(&sphereA, &sphereB, collisions);
}
quint64 endTime = usecTimestampNow();
std::cout << numTests << " non-colliding collisions in " << (endTime - startTime) << " usec" << std::endl;
newTime = endTime - startTime;
}
*/
}
void ShapeColliderTests::runAllTests() {
ShapeCollider::initDispatchTable();
//measureTimeOfCollisionDispatch();
sphereMissesSphere();
sphereTouchesSphere();

2
tests/physics/src/ShapeColliderTests.h
View file

@ -35,6 +35,8 @@ namespace ShapeColliderTests {
void rayHitsPlane();
void rayMissesPlane();
void measureTimeOfCollisionDispatch();
void runAllTests();
}

157
tests/physics/src/VerletShapeTests.cpp
View file

@ -102,8 +102,7 @@ void VerletShapeTests::sphereMissesSphere() {
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size()
<< std::endl;
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
@ -159,7 +158,7 @@ void VerletShapeTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -169,7 +168,7 @@ void VerletShapeTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
@ -189,7 +188,7 @@ void VerletShapeTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -199,7 +198,7 @@ void VerletShapeTests::sphereTouchesSphere() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
}
@ -247,23 +246,20 @@ void VerletShapeTests::sphereMissesCapsule() {
if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " 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;
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
@ -297,8 +293,7 @@ void VerletShapeTests::sphereTouchesCapsule() {
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -310,7 +305,7 @@ void VerletShapeTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -319,15 +314,14 @@ void VerletShapeTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
@ -335,33 +329,41 @@ void VerletShapeTests::sphereTouchesCapsule() {
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - (radialOffset - totalRadius) * xAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// 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);
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
closestApproach = sphereA.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;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
{ // sphereA hits end cap at axis
glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setTranslation(axialOffset * yAxis);
sphereA.setTranslation(axialOffset);
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -373,7 +375,7 @@ void VerletShapeTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -382,15 +384,14 @@ void VerletShapeTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
@ -398,33 +399,40 @@ void VerletShapeTests::sphereTouchesCapsule() {
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 endPoint;
capsuleB.getEndPoint(endPoint);
expectedContactPoint = endPoint + radiusB * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedContactPoint = axialOffset - radiusA * yAxis;
}
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
{ // sphereA hits start cap at axis
glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setTranslation(axialOffset * yAxis);
sphereA.setTranslation(axialOffset);
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -436,7 +444,7 @@ void VerletShapeTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
@ -445,15 +453,14 @@ void VerletShapeTests::sphereTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
@ -461,22 +468,30 @@ void VerletShapeTests::sphereTouchesCapsule() {
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 startPoint;
capsuleB.getStartPoint(startPoint);
expectedContactPoint = startPoint - radiusB * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedContactPoint = axialOffset + radiusA * yAxis;
}
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
if (collisions.size() != numCollisions) {
@ -515,14 +530,12 @@ void VerletShapeTests::capsuleMissesCapsule() {
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
// end to end
@ -530,14 +543,12 @@ void VerletShapeTests::capsuleMissesCapsule() {
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
// rotate B and move it to the side
@ -547,20 +558,17 @@ void VerletShapeTests::capsuleMissesCapsule() {
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
<< " 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;
<< " 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;
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
@ -594,16 +602,14 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -615,16 +621,14 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -638,16 +642,14 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " 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;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -664,8 +666,7 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -676,7 +677,7 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration;
<< " actual = " << collision->_penetration << std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis;
@ -684,15 +685,14 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint;
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB vs capsuleA
if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -703,8 +703,7 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
<< " actual = " << collision->_penetration << std::endl;
}
expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis;
@ -712,8 +711,7 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
@ -729,8 +727,7 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
@ -741,8 +738,7 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
<< " actual = " << collision->_penetration << std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis;
@ -750,13 +746,14 @@ void VerletShapeTests::capsuleTouchesCapsule() {
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
<< " actual = " << collision->_contactPoint << std::endl;
}
}
}
void VerletShapeTests::runAllTests() {
ShapeCollider::initDispatchTable();
setSpherePosition();
sphereMissesSphere();
sphereTouchesSphere();