From 8a3640f0162f6587000793abd8c04f2321854ebd Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 24 Feb 2014 11:38:27 -0800
Subject: [PATCH] Adding CapsuleSphere collisions with tests.
---
libraries/shared/src/CapsuleShape.cpp | 17 +
libraries/shared/src/CapsuleShape.h | 9 +
libraries/shared/src/CollisionInfo.cpp | 11 -
libraries/shared/src/CollisionInfo.h | 6 -
libraries/shared/src/Shape.h | 6 +-
libraries/shared/src/ShapeCollider.cpp | 206 ++++++------
libraries/shared/src/ShapeCollider.h | 65 ++--
libraries/shared/src/SphereShape.h | 7 +-
tests/physics/src/CollisionInfoTests.cpp | 6 +-
tests/physics/src/CollisionInfoTests.h | 4 +-
tests/physics/src/ShapeColliderTests.cpp | 391 +++++++++++++++++++++--
tests/physics/src/ShapeColliderTests.h | 10 +-
tests/physics/src/main.cpp | 4 -
13 files changed, 557 insertions(+), 185 deletions(-)
diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp
index 68e0acf561..c71f03aa47 100644
--- a/libraries/shared/src/CapsuleShape.cpp
+++ b/libraries/shared/src/CapsuleShape.cpp
@@ -6,6 +6,7 @@
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
+#include <iostream>
#include <glm/gtx/vector_angle.hpp>
#include "CapsuleShape.h"
@@ -13,6 +14,7 @@
// default axis of CapsuleShape is Y-axis
+const glm::vec3 localAxis(0.f, 1.f, 0.f);
CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE) {}
@@ -43,6 +45,21 @@ CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm:
updateBoundingRadius();
}
+/// \param[out] startPoint is the center of start cap
+void CapsuleShape::getStartPoint(glm::vec3& startPoint) const {
+ startPoint = getPosition() - _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f));
+}
+
+/// \param[out] endPoint is the center of the end cap
+void CapsuleShape::getEndPoint(glm::vec3& endPoint) const {
+ endPoint = getPosition() + _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f));
+}
+
+void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const {
+ // default axis of a capsule is along the yAxis
+ axis = _rotation * glm::vec3(0.f, 1.f, 0.f);
+}
+
void CapsuleShape::setRadius(float radius) {
_radius = radius;
updateBoundingRadius();
diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h
index 726d880c37..6d7e0a50be 100644
--- a/libraries/shared/src/CapsuleShape.h
+++ b/libraries/shared/src/CapsuleShape.h
@@ -11,6 +11,7 @@
#include "Shape.h"
+// adebug bookmark TODO: convert to new world-frame approach
// default axis of CapsuleShape is Y-axis
class CapsuleShape : public Shape {
@@ -23,6 +24,14 @@ public:
float getRadius() const { return _radius; }
float getHalfHeight() const { return _halfHeight; }
+ /// \param[out] startPoint is the center of start cap
+ void getStartPoint(glm::vec3& startPoint) const;
+
+ /// \param[out] endPoint is the center of the end cap
+ void getEndPoint(glm::vec3& endPoint) const;
+
+ void computeNormalizedAxis(glm::vec3& axis) const;
+
void setRadius(float radius);
void setHalfHeight(float height);
void setRadiusAndHalfHeight(float radius, float height);
diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp
index de233d9420..9e76ca59f6 100644
--- a/libraries/shared/src/CollisionInfo.cpp
+++ b/libraries/shared/src/CollisionInfo.cpp
@@ -8,17 +8,6 @@
#include "CollisionInfo.h"
-void CollisionInfo::rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation) {
- _contactPoint = translation + rotation * _contactPoint;
- _penetration = rotation * _penetration;
- _addedVelocity = rotation * _addedVelocity;
-}
-
-void CollisionInfo::translateThenRotate(const glm::vec3& translation, const glm::quat& rotation) {
- _contactPoint = rotation * (_contactPoint + translation);
- _penetration = rotation * _penetration;
- _addedVelocity = rotation * _addedVelocity;
-}
CollisionList::CollisionList(int maxSize) :
_maxSize(maxSize),
diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h
index b71d67e532..629cb6b8f2 100644
--- a/libraries/shared/src/CollisionInfo.h
+++ b/libraries/shared/src/CollisionInfo.h
@@ -56,12 +56,6 @@ public:
~CollisionInfo() {}
- /// Rotate and translate the details.
- void rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation);
-
- /// Translate then rotate the details
- void translateThenRotate(const glm::vec3& translation, const glm::quat& rotation);
-
qint32 _type; // type of Collision (will determine what is supposed to be in _data and _flags)
void* _data; // pointer to user supplied data
quint32 _flags; // 32 bits for whatever
diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h
index 0e6d17c059..5273c69007 100644
--- a/libraries/shared/src/Shape.h
+++ b/libraries/shared/src/Shape.h
@@ -1,6 +1,5 @@
//
// Shape.h
-// hifi
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
@@ -20,6 +19,7 @@ public:
SPHERE_SHAPE,
CAPSULE_SHAPE,
BOX_SHAPE,
+ LIST_SHAPE
};
Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { }
@@ -33,8 +33,12 @@ public:
void setRotation(const glm::quat& rotation) { _rotation = rotation; }
protected:
+ // these ctors are protected (used by derived classes only)
Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {}
+ Shape(Type type, const glm::vec3& position)
+ : _type(type), _boundingRadius(0.f), _position(position), _rotation() {}
+
Shape(Type type, const glm::vec3& position, const glm::quat& rotation)
: _type(type), _boundingRadius(0.f), _position(position), _rotation(rotation) {}
diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp
index f3598ebf39..c5d497694e 100644
--- a/libraries/shared/src/ShapeCollider.cpp
+++ b/libraries/shared/src/ShapeCollider.cpp
@@ -6,42 +6,37 @@
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
+#include <iostream>
+
+#include <glm/gtx/norm.hpp>
+
#include "ShapeCollider.h"
namespace ShapeCollider {
-bool shapeShape(const Shape* shapeA, const Shape* shapeB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) {
+bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision) {
// ATM we only have two shape types so we just check every case.
// TODO: make a fast lookup for correct method
if (shapeA->getType() == Shape::SPHERE_SHAPE) {
const SphereShape* sphereA = static_cast<const SphereShape*>(shapeA);
if (shapeB->getType() == Shape::SPHERE_SHAPE) {
- return sphereSphere(sphereA, static_cast<const SphereShape*>(shapeB),
- rotationAB, offsetA, collision);
+ return sphereSphere(sphereA, static_cast<const SphereShape*>(shapeB), collision);
} else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
- return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB),
- rotationAB, offsetA, collision);
+ return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB), collision);
}
} else if (shapeA->getType() == Shape::CAPSULE_SHAPE) {
const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
if (shapeB->getType() == Shape::SPHERE_SHAPE) {
- return capsuleSphere(capsuleA, static_cast<const SphereShape*>(shapeB),
- rotationAB, offsetA, collision);
+ return capsuleSphere(capsuleA, static_cast<const SphereShape*>(shapeB), collision);
} else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
- return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(shapeB),
- rotationAB, offsetA, collision);
+ return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(shapeB), collision);
}
}
return false;
}
-bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) {
- // A in B's frame
- glm::vec3 A = rotationAB * sphereA->getPosition() + offsetA;
- // BA = B - A = from A to B, in B's frame
- glm::vec3 BA = sphereB->getPosition() - A;
+bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision) {
+ glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition();
float distanceSquared = glm::dot(BA, BA);
float totalRadius = sphereA->getRadius() + sphereB->getRadius();
if (distanceSquared < totalRadius * totalRadius) {
@@ -53,103 +48,124 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB,
} else {
BA /= distance;
}
- // store the collision details in B's frame
+ // penetration points from A into B
collision._penetration = BA * (totalRadius - distance);
- collision._contactPoint = A + sphereA->getRadius() * BA;
+ // contactPoint is on surface of A
+ collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA;
return true;
}
return false;
}
-// everything in the capsule's natural frame (where its axis is along yAxis)
-bool sphereCapsuleHelper(float sphereRadius, const glm::vec3 sphereCenter,
- const CapsuleShape* capsule, CollisionInfo& collision) {
- float xzSquared = sphereCenter.x * sphereCenter.x + sphereCenter.y * sphereCenter.y;
- float totalRadius = sphereRadius + capsule->getRadius();
- if (xzSquared < totalRadius * totalRadius) {
- float fabsY = fabs(sphereCenter.y);
- float halfHeight = capsule->getHalfHeight();
- if (fabsY < halfHeight) {
- // sphere collides with cylindrical wall
- glm::vec3 BA = -sphereCenter;
- BA.y = 0.f;
- float distance = sqrtf(xzSquared);
- if (distance < EPSILON) {
- // for now we don't handle this singular case
+bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision) {
+ // find sphereA's closest approach to axis of capsuleB
+ glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition();
+ glm::vec3 capsuleAxis;
+ capsuleB->computeNormalizedAxis(capsuleAxis);
+ float axialDistance = - glm::dot(BA, capsuleAxis);
+ float absAxialDistance = fabs(axialDistance);
+ float totalRadius = sphereA->getRadius() + capsuleB->getRadius();
+ if (absAxialDistance < totalRadius + capsuleB->getHalfHeight()) {
+ glm::vec3 radialAxis = BA + axialDistance * capsuleAxis; // points from A to axis of B
+ float radialDistance2 = glm::length2(radialAxis);
+ if (radialDistance2 > totalRadius * totalRadius) {
+ // sphere is too far from capsule axis
+ return false;
+ }
+ if (absAxialDistance > capsuleB->getHalfHeight()) {
+ // sphere hits capsule on a cap --> recompute radialAxis to point from spherA to cap center
+ float sign = (axialDistance > 0.f) ? 1.f : -1.f;
+ radialAxis = BA + (sign * capsuleB->getHalfHeight()) * capsuleAxis;
+ radialDistance2 = glm::length2(radialAxis);
+ }
+ if (radialDistance2 > EPSILON * EPSILON) {
+ // normalize the radialAxis
+ float radialDistance = sqrtf(radialDistance2);
+ radialAxis /= radialDistance;
+ // penetration points from A into B
+ collision._penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B
+ // contactPoint is on surface of sphereA
+ collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis;
+ } else {
+ // A is on B's axis, so the penetration is undefined...
+ if (absAxialDistance > capsuleB->getHalfHeight()) {
+ // ...for the cylinder case (for now we pretend the collision doesn't exist)
return false;
}
- BA /= distance;
- collision._penetration = BA * (totalRadius - distance);
- collision._contactPoint = BA * (sphereRadius - totalRadius + distance);
- collision._contactPoint.y = fabsY;
- if (sphereCenter.y < 0.f) {
- // negate the y elements of the collision info
- collision._penetration.y *= -1.f;
- collision._contactPoint.y *= -1.f;
+ // ... but still defined for the cap case
+ if (axialDistance < 0.f) {
+ // we're hitting the start cap, so we negate the capsuleAxis
+ capsuleAxis *= -1;
}
- return true;
+ // penetration points from A into B
+ float sign = (axialDistance > 0.f) ? -1.f : 1.f;
+ collision._penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis;
+ // contactPoint is on surface of sphereA
+ collision._contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis;
+ }
+ return true;
+ }
+ return false;
+}
+
+bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision) {
+ // find sphereB's closest approach to axis of capsuleA
+ glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition();
+ glm::vec3 capsuleAxis;
+ capsuleA->computeNormalizedAxis(capsuleAxis);
+ float axialDistance = - glm::dot(AB, capsuleAxis);
+ float absAxialDistance = fabs(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);
+ if (radialDistance2 > totalRadius * totalRadius) {
+ // 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->getPosition() + axialDistance * capsuleAxis;
+
+ if (absAxialDistance > capsuleA->getHalfHeight()) {
+ // sphere hits capsule on a cap
+ // --> recompute radialAxis and closestApproach
+ float sign = (axialDistance > 0.f) ? 1.f : -1.f;
+ closestApproach = capsuleA->getPosition() + (sign * capsuleA->getHalfHeight()) * capsuleAxis;
+ radialAxis = closestApproach - sphereB->getPosition();
+ radialDistance2 = glm::length2(radialAxis);
+ }
+ if (radialDistance2 > EPSILON * EPSILON) {
+ // 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;
} else {
- // tansform into frame where cap is at origin
- float newY = fabsY - halfHeight;
- float distance = sqrtf(newY * newY + xzSquared);
- if (distance < totalRadius) {
- // sphere collides with cap
-
- // BA points from sphere to cap
- glm::vec3 BA(-sphereCenter.x, newY, -sphereCenter.z);
- if (distance < EPSILON) {
- // for now we don't handle this singular case
- return false;
+ // 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 {
+ // ... but still defined for the cap case
+ if (axialDistance < 0.f) {
+ // we're hitting the start cap, so we negate the capsuleAxis
+ capsuleAxis *= -1;
}
- BA /= distance;
- collision._penetration = BA * (totalRadius - distance);
- collision._contactPoint = BA * (sphereRadius - totalRadius + distance);
- collision._contactPoint.y += halfHeight;
-
- if (sphereCenter.y < 0.f) {
- // negate the y elements of the collision info
- collision._penetration.y *= -1.f;
- collision._contactPoint.y *= -1.f;
- }
- return true;
+ float sign = (axialDistance > 0.f) ? 1.f : -1.f;
+ collision._penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis;
+ // contactPoint is on surface of sphereA
+ collision._contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis;
}
}
- }
- return false;
-}
-
-bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) {
- // transform sphereA all the way to capsuleB's natural frame
- glm::quat rotationB = capsuleB->getRotation();
- glm::vec3 sphereCenter = rotationB * (offsetA + rotationAB * sphereA->getPosition() - capsuleB->getPosition());
- if (sphereCapsuleHelper(sphereA->getRadius(), sphereCenter, capsuleB, collision)) {
- // need to transform collision details back into B's offset frame
- collision.rotateThenTranslate(glm::inverse(rotationB), capsuleB->getPosition());
return true;
}
return false;
}
-bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) {
- // transform sphereB all the way to capsuleA's natural frame
- glm::quat rotationBA = glm::inverse(rotationAB);
- glm::quat rotationA = capsuleA->getRotation();
- glm::vec3 offsetB = rotationBA * (-offsetA);
- glm::vec3 sphereCenter = rotationA * (offsetB + rotationBA * sphereB->getPosition() - capsuleA->getPosition());
- if (sphereCapsuleHelper(sphereB->getRadius(), sphereCenter, capsuleA, collision)) {
- // need to transform collision details back into B's offset frame
- // BUG: these back translations are probably not right
- collision.rotateThenTranslate(glm::inverse(rotationA), capsuleA->getPosition());
- collision.rotateThenTranslate(glm::inverse(rotationAB), -offsetA);
- return true;
- }
- return false;
-}
-
-bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) {
+bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision) {
return false;
}
diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h
index 4cef6f943f..0eaf746595 100644
--- a/libraries/shared/src/ShapeCollider.h
+++ b/libraries/shared/src/ShapeCollider.h
@@ -16,50 +16,35 @@
namespace ShapeCollider {
-/// \param shapeA pointer to first shape
-/// \param shapeB pointer to second shape
-/// \param rotationAB rotation from A into reference frame of B
-/// \param offsetA offset of A (in B's frame)
-/// \param[out] collision where to store collision details
-/// \return true of shapes collide
-bool shapeShape(const Shape* shapeA, const Shape* shapeB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
+ /// \param shapeA pointer to first shape
+ /// \param shapeB pointer to second shape
+ /// \param[out] collision where to store collision details
+ /// \return true of shapes collide
+ bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision);
-/// \param sphereA pointer to first shape (sphere)
-/// \param sphereB pointer to second shape (sphere)
-/// \param rotationAB rotation from A into reference frame of B
-/// \param offsetA offset of A (in B's frame)
-/// \param[out] collision where to store collision details
-/// \return true of shapes collide
-bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
+ /// \param sphereA pointer to first shape (sphere)
+ /// \param sphereB pointer to second shape (sphere)
+ /// \param[out] collision where to store collision details
+ /// \return true of shapes collide
+ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision);
-/// \param sphereA pointer to first shape (sphere)
-/// \param capsuleB pointer to second shape (capsule)
-/// \param rotationAB rotation from A into reference frame of B
-/// \param offsetA offset of A (in B's frame)
-/// \param[out] collision where to store collision details
-/// \return true of shapes collide
-bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
+ /// \param sphereA pointer to first shape (sphere)
+ /// \param capsuleB pointer to second shape (capsule)
+ /// \param[out] collision where to store collision details
+ /// \return true of shapes collide
+ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision);
-/// \param capsuleA pointer to first shape (capsule)
-/// \param sphereB pointer to second shape (sphere)
-/// \param rotationAB rotation from A into reference frame of B
-/// \param offsetA offset of A (in B's frame)
-/// \param[out] collision where to store collision details
-/// \return true of shapes collide
-bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
+ /// \param capsuleA pointer to first shape (capsule)
+ /// \param sphereB pointer to second shape (sphere)
+ /// \param[out] collision where to store collision details
+ /// \return true of shapes collide
+ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision);
-/// \param capsuleA pointer to first shape (capsule)
-/// \param capsuleB pointer to second shape (capsule)
-/// \param rotationAB rotation from A into reference frame of B
-/// \param offsetA offset of A (in B's frame)
-/// \param[out] collision where to store collision details
-/// \return true of shapes collide
-bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
- const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision);
+ /// \param capsuleA pointer to first shape (capsule)
+ /// \param capsuleB pointer to second shape (capsule)
+ /// \param[out] collision where to store collision details
+ /// \return true of shapes collide
+ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision);
} // namespace ShapeCollider
diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h
index a1028956ee..d720dd2289 100644
--- a/libraries/shared/src/SphereShape.h
+++ b/libraries/shared/src/SphereShape.h
@@ -15,9 +15,12 @@ class SphereShape : public Shape {
public:
SphereShape() : Shape(Shape::SPHERE_SHAPE) {}
- SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE) {
+ SphereShape(float radius) : Shape(Shape::SPHERE_SHAPE) {
+ _boundingRadius = radius;
+ }
+
+ SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE, position) {
_boundingRadius = radius;
- setPosition(position);
}
float getRadius() const { return _boundingRadius; }
diff --git a/tests/physics/src/CollisionInfoTests.cpp b/tests/physics/src/CollisionInfoTests.cpp
index 0355e2d27b..813100944b 100644
--- a/tests/physics/src/CollisionInfoTests.cpp
+++ b/tests/physics/src/CollisionInfoTests.cpp
@@ -18,6 +18,7 @@
#include "PhysicsTestUtil.h"
+/*
void CollisionInfoTests::rotateThenTranslate() {
CollisionInfo collision;
collision._penetration = xAxis;
@@ -95,8 +96,9 @@ void CollisionInfoTests::translateThenRotate() {
<< std::endl;
}
}
+*/
void CollisionInfoTests::runAllTests() {
- CollisionInfoTests::rotateThenTranslate();
- CollisionInfoTests::translateThenRotate();
+// CollisionInfoTests::rotateThenTranslate();
+// CollisionInfoTests::translateThenRotate();
}
diff --git a/tests/physics/src/CollisionInfoTests.h b/tests/physics/src/CollisionInfoTests.h
index cc0ef337a2..51579e8f11 100644
--- a/tests/physics/src/CollisionInfoTests.h
+++ b/tests/physics/src/CollisionInfoTests.h
@@ -11,8 +11,8 @@
namespace CollisionInfoTests {
- void rotateThenTranslate();
- void translateThenRotate();
+// void rotateThenTranslate();
+// void translateThenRotate();
void runAllTests();
}
diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp
index b964e2f6de..8380bcf0a7 100644
--- a/tests/physics/src/ShapeColliderTests.cpp
+++ b/tests/physics/src/ShapeColliderTests.cpp
@@ -22,33 +22,384 @@
const glm::vec3 origin(0.f);
-void ShapeColliderTests::sphereSphere() {
+void ShapeColliderTests::sphereMissesSphere() {
+ // non-overlapping spheres of unequal size
+ float radiusA = 7.f;
+ float radiusB = 3.f;
+ float alpha = 1.2f;
+ float beta = 1.3f;
+ glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f));
+ float offsetDistance = alpha * radiusA + beta * radiusB;
+ float expectedPenetrationDistance = 0.f;
+
+ SphereShape sphereA(radiusA, origin);
+ SphereShape sphereB(radiusB, offsetDistance * offsetDirection);
CollisionInfo collision;
- float radius = 2.f;
- SphereShape sphereA(radius, origin);
- SphereShape sphereB(radius, origin);
-
- glm::vec3 translation(0.5f * radius, 0.f, 0.f);
- glm::quat rotation;
-
- // these spheres should be touching
- bool touching = ShapeCollider::sphereSphere(&sphereA, &sphereB, rotation, translation, collision);
- if (!touching) {
- std::cout << __FILE__ << ":" << __LINE__
- << " ERROR: sphereA and sphereB do not touch" << std::endl;
+ // collide A to B...
+ {
+ bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision);
+ if (touching) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+ }
}
- // TODO: verify the collision info is good...
- // penetration should point from A into B
+ // collide B to A...
+ {
+ bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision);
+ if (touching) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+ }
+ }
+
+ // also test shapeShape
+ {
+ bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision);
+ if (touching) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphereA and sphereB should NOT touch" << std::endl;
+ }
+ }
}
-/*
-void ShapeColliderTests::test2() {
+void ShapeColliderTests::sphereTouchesSphere() {
+ // overlapping spheres of unequal size
+ float radiusA = 7.f;
+ float radiusB = 3.f;
+ float alpha = 0.2f;
+ float beta = 0.3f;
+ glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f));
+ float offsetDistance = alpha * radiusA + beta * radiusB;
+ float expectedPenetrationDistance = (1.f - alpha) * radiusA + (1.f - beta) * radiusB;
+ glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection;
+
+ SphereShape sphereA(radiusA, origin);
+ SphereShape sphereB(radiusB, offsetDistance * offsetDirection);
+ CollisionInfo collision;
+
+ // collide A to B...
+ {
+ bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision);
+ if (!touching) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphereA and sphereB should touch" << std::endl;
+ }
+
+ // penetration points from sphereA into sphereB
+ float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of sphereA
+ glm::vec3 AtoB = sphereB.getPosition() - sphereA.getPosition();
+ glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * glm::normalize(AtoB);
+ inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+ << " actual = " << collision._contactPoint
+ << std::endl;
+ }
+ }
+
+ // collide B to A...
+ {
+ bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision);
+ if (!touching) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphereA and sphereB should touch" << std::endl;
+ }
+
+ // penetration points from sphereA into sphereB
+ float inaccuracy = glm::length(collision._penetration + expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of sphereA
+ glm::vec3 BtoA = sphereA.getPosition() - sphereB.getPosition();
+ glm::vec3 expectedContactPoint = sphereB.getPosition() + radiusB * glm::normalize(BtoA);
+ inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+ << " actual = " << collision._contactPoint
+ << std::endl;
+ }
+ }
+}
+
+void ShapeColliderTests::sphereMissesCapsule() {
+ // non-overlapping sphere and capsule
+ float radiusA = 1.5f;
+ float radiusB = 2.3f;
+ float totalRadius = radiusA + radiusB;
+ float halfHeightB = 1.7f;
+ float axialOffset = totalRadius + 1.1f * halfHeightB;
+ float radialOffset = 1.2f * radiusA + 1.3f * radiusB;
+
+ SphereShape sphereA(radiusA);
+ CapsuleShape capsuleB(radiusB, halfHeightB);
+
+ // give the capsule some arbirary transform
+ float angle = 37.8;
+ glm::vec3 axis = glm::normalize( glm::vec3(-7.f, 2.8f, 9.3f) );
+ glm::quat rotation = glm::angleAxis(angle, axis);
+ glm::vec3 translation(15.1f, -27.1f, -38.6f);
+ capsuleB.setRotation(rotation);
+ capsuleB.setPosition(translation);
+
+ // walk sphereA along the local yAxis next to, but not touching, capsuleB
+ glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f);
+ int numberOfSteps = 10;
+ float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1);
+ for (int i = 0; i < numberOfSteps; ++i) {
+ // translate sphereA into world-frame
+ glm::vec3 localPosition = localStartPosition + (float(i) * delta) * yAxis;
+ sphereA.setPosition(rotation * localPosition + translation);
+
+ CollisionInfo collision;
+ // sphereA agains capsuleB
+ if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphere and capsule should NOT touch"
+ << std::endl;
+ }
+
+ // capsuleB against sphereA
+ if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphere and capsule should NOT touch"
+ << std::endl;
+ }
+ }
+}
+
+void ShapeColliderTests::sphereTouchesCapsule() {
+ // overlapping sphere and capsule
+ float radiusA = 2.f;
+ float radiusB = 1.f;
+ float totalRadius = radiusA + radiusB;
+ float halfHeightB = 2.f;
+ float alpha = 0.5f;
+ float beta = 0.5f;
+ float axialOffset = 0.f;
+ float radialOffset = alpha * radiusA + beta * radiusB;
+
+ SphereShape sphereA(radiusA);
+ CapsuleShape capsuleB(radiusB, halfHeightB);
+
+ CollisionInfo collision;
+ { // sphereA collides with capsuleB's cylindrical wall
+ sphereA.setPosition(radialOffset * xAxis);
+
+ if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphere and capsule should touch"
+ << std::endl;
+ }
+
+ // penetration points from sphereA into capsuleB
+ glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis;
+ float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of sphereA
+ glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis;
+ inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+ << " actual = " << collision._contactPoint
+ << std::endl;
+ }
+
+ // capsuleB collides with sphereA
+ if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: capsule and sphere should touch"
+ << std::endl;
+ }
+
+ // penetration points from sphereA into capsuleB
+ expectedPenetration = - (radialOffset - totalRadius) * xAxis;
+ inaccuracy = glm::length(collision._penetration - expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of capsuleB
+ glm::vec3 BtoA = sphereA.getPosition() - capsuleB.getPosition();
+ glm::vec3 closestApproach = capsuleB.getPosition() + 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
+ << std::endl;
+ }
+ }
+ { // sphereA hits end cap at axis
+ glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
+ sphereA.setPosition(axialOffset * yAxis);
+
+ if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphere and capsule should touch"
+ << std::endl;
+ }
+
+ // penetration points from sphereA into capsuleB
+ glm::vec3 expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+ float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of sphereA
+ glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis;
+ inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+ << " actual = " << collision._contactPoint
+ << std::endl;
+ }
+
+ // capsuleB collides with sphereA
+ if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: capsule and sphere should touch"
+ << std::endl;
+ }
+
+ // penetration points from sphereA into capsuleB
+ expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+ inaccuracy = glm::length(collision._penetration - expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of capsuleB
+ glm::vec3 endPoint;
+ capsuleB.getEndPoint(endPoint);
+ expectedContactPoint = endPoint + radiusB * yAxis;
+ inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+ << " actual = " << collision._contactPoint
+ << std::endl;
+ }
+ }
+ { // sphereA hits start cap at axis
+ glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
+ sphereA.setPosition(axialOffset * yAxis);
+
+ if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: sphere and capsule should touch"
+ << std::endl;
+ }
+
+ // penetration points from sphereA into capsuleB
+ glm::vec3 expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+ float inaccuracy = glm::length(collision._penetration - expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of sphereA
+ glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis;
+ inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+ << " actual = " << collision._contactPoint
+ << std::endl;
+ }
+
+ // capsuleB collides with sphereA
+ if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision))
+ {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: capsule and sphere should touch"
+ << std::endl;
+ }
+
+ // penetration points from sphereA into capsuleB
+ expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
+ inaccuracy = glm::length(collision._penetration - expectedPenetration);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad penetration: expected = " << expectedPenetration
+ << " actual = " << collision._penetration
+ << std::endl;
+ }
+
+ // contactPoint is on surface of capsuleB
+ glm::vec3 startPoint;
+ capsuleB.getStartPoint(startPoint);
+ expectedContactPoint = startPoint - radiusB * yAxis;
+ inaccuracy = glm::length(collision._contactPoint - expectedContactPoint);
+ if (fabs(inaccuracy) > EPSILON) {
+ std::cout << __FILE__ << ":" << __LINE__
+ << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+ << " actual = " << collision._contactPoint
+ << std::endl;
+ }
+ }
+}
+
+void ShapeColliderTests::capsuleMissesCapsule() {
+ // TODO: implement this
+}
+
+void ShapeColliderTests::capsuleTouchesCapsule() {
+ // TODO: implement this
}
-*/
void ShapeColliderTests::runAllTests() {
- ShapeColliderTests::sphereSphere();
-// ShapeColliderTests::test2();
+ sphereMissesSphere();
+ sphereTouchesSphere();
+
+ sphereMissesCapsule();
+ sphereTouchesCapsule();
+
+ capsuleMissesCapsule();
+ capsuleTouchesCapsule();
}
diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h
index a3a19805f8..ecd4a7f045 100644
--- a/tests/physics/src/ShapeColliderTests.h
+++ b/tests/physics/src/ShapeColliderTests.h
@@ -11,8 +11,14 @@
namespace ShapeColliderTests {
- void sphereSphere();
- //void test2();
+ void sphereMissesSphere();
+ void sphereTouchesSphere();
+
+ void sphereMissesCapsule();
+ void sphereTouchesCapsule();
+
+ void capsuleMissesCapsule();
+ void capsuleTouchesCapsule();
void runAllTests();
}
diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp
index 3ae5a0a6fe..b0a7adde4e 100644
--- a/tests/physics/src/main.cpp
+++ b/tests/physics/src/main.cpp
@@ -3,13 +3,9 @@
// physics-tests
//
-//#include <QDebug>
-
#include "ShapeColliderTests.h"
-#include "CollisionInfoTests.h"
int main(int argc, char** argv) {
- CollisionInfoTests::runAllTests();
ShapeColliderTests::runAllTests();
return 0;
}