mirror of
https://github.com/JulianGro/overte.git
synced 2025-04-09 13:12:40 +02:00
fix sphere-cube collisions, with tests
This commit is contained in:
Andrew Meadows
parent
5ac301a77e
commit
c8e56a97cc
3 changed files with 241 additions and 78 deletions
|
|
@ -15,7 +15,6 @@
|
|||
|
||||
#include "GeometryUtil.h"
|
||||
#include "ShapeCollider.h"
|
||||
#include "StreamUtils.h" // adebug
|
||||
|
||||
// NOTE:
|
||||
//
|
||||
|
|
@ -594,6 +593,95 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col
|
|||
// helper function
|
||||
bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter,
|
||||
float cubeSide, CollisionList& collisions) {
|
||||
// sphere is A
|
||||
// cube is B
|
||||
// BA = B - A = from center of A to center of B
|
||||
float halfCubeSide = 0.5f * cubeSide;
|
||||
glm::vec3 BA = cubeCenter - sphereCenter;
|
||||
float distance = glm::length(BA);
|
||||
if (distance > EPSILON) {
|
||||
float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z));
|
||||
if (maxBA > halfCubeSide + sphereRadius) {
|
||||
// sphere misses cube entirely
|
||||
return false;
|
||||
}
|
||||
CollisionInfo* collision = collisions.getNewCollision();
|
||||
if (!collision) {
|
||||
return false;
|
||||
}
|
||||
if (maxBA > halfCubeSide) {
|
||||
// sphere hits cube but its center is outside cube
|
||||
|
||||
// contactPoint is on surface of sphere
|
||||
glm::vec3 cubeContact = glm::abs(BA);
|
||||
glm::vec3 direction = cubeContact - glm::vec3(halfCubeSide);
|
||||
|
||||
if (direction.x < 0.0f) {
|
||||
direction.x = 0.0f;
|
||||
}
|
||||
if (direction.y < 0.0f) {
|
||||
direction.y = 0.0f;
|
||||
}
|
||||
if (direction.z < 0.0f) {
|
||||
direction.z = 0.0f;
|
||||
}
|
||||
|
||||
glm::vec3 signs = glm::sign(BA);
|
||||
direction.x *= signs.x;
|
||||
direction.y *= signs.y;
|
||||
direction.z *= signs.z;
|
||||
direction = glm::normalize(direction);
|
||||
collision->_contactPoint = sphereCenter + sphereRadius * direction;
|
||||
|
||||
// penetration points from contact point on cube to that on sphere
|
||||
if (cubeContact.x > halfCubeSide) {
|
||||
cubeContact.x = halfCubeSide;
|
||||
}
|
||||
cubeContact.x *= -signs.x;
|
||||
if (cubeContact.y > halfCubeSide) {
|
||||
cubeContact.y = halfCubeSide;
|
||||
}
|
||||
cubeContact.y *= -signs.y;
|
||||
if (cubeContact.z > halfCubeSide) {
|
||||
cubeContact.z = halfCubeSide;
|
||||
}
|
||||
cubeContact.z *= -signs.z;
|
||||
//collision->_penetration = collision->_contactPoint - cubeCenter + cubeContact;
|
||||
collision->_penetration = collision->_contactPoint - (cubeCenter + cubeContact);
|
||||
} else {
|
||||
// sphere center is inside cube
|
||||
// --> push out nearest face
|
||||
glm::vec3 direction;
|
||||
BA /= distance;
|
||||
glm::modf(BA, direction);
|
||||
direction = glm::normalize(direction);
|
||||
|
||||
// penetration is the projection of surfaceAB on direction
|
||||
collision->_penetration = (halfCubeSide + sphereRadius - distance * glm::dot(BA, direction)) * direction;
|
||||
// contactPoint is on surface of A
|
||||
collision->_contactPoint = sphereCenter + sphereRadius * direction;
|
||||
}
|
||||
return true;
|
||||
} else if (sphereRadius + halfCubeSide > distance) {
|
||||
// NOTE: for cocentric approximation we collide sphere and cube as two spheres which means
|
||||
// this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON)
|
||||
CollisionInfo* collision = collisions.getNewCollision();
|
||||
if (collision) {
|
||||
// the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis)
|
||||
collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f);
|
||||
// contactPoint is on surface of A
|
||||
collision->_contactPoint = sphereCenter + collision->_penetration;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// helper function
|
||||
/* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding).
|
||||
* We might want to use this code later for sealing boundaries between adjacent voxels.
|
||||
bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter,
|
||||
float cubeSide, CollisionList& collisions) {
|
||||
glm::vec3 BA = cubeCenter - sphereCenter;
|
||||
float distance = glm::length(BA);
|
||||
if (distance > EPSILON) {
|
||||
|
|
@ -608,45 +696,11 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
|
|||
if (glm::dot(surfaceAB, BA) > 0.f) {
|
||||
CollisionInfo* collision = collisions.getNewCollision();
|
||||
if (collision) {
|
||||
/* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding).
|
||||
* We might want to use this code later for sealing boundaries between adjacent voxels.
|
||||
// penetration is parallel to box side direction
|
||||
BA /= maxBA;
|
||||
glm::vec3 direction;
|
||||
glm::modf(BA, direction);
|
||||
direction = glm::normalize(direction);
|
||||
*/
|
||||
|
||||
// For rounded normals at edges and corners:
|
||||
// At this point imagine that sphereCenter touches a "normalized" cube with rounded edges.
|
||||
// This cube has a sidelength of 2 and its smoothing radius is sphereRadius/maxBA.
|
||||
// We're going to try to compute the "negative normal" (and hence direction of penetration)
|
||||
// of this surface.
|
||||
|
||||
float radius = sphereRadius / (distance * maxBA); // normalized radius
|
||||
float shortLength = maxBA - radius;
|
||||
glm::vec3 direction = BA;
|
||||
if (shortLength > 0.0f) {
|
||||
direction = glm::abs(BA) - glm::vec3(shortLength);
|
||||
// Set any negative components to zero, and adopt the sign of the original BA component.
|
||||
// Unfortunately there isn't an easy way to make this fast.
|
||||
if (direction.x < 0.0f) {
|
||||
direction.x = 0.f;
|
||||
} else if (BA.x < 0.f) {
|
||||
direction.x = -direction.x;
|
||||
}
|
||||
if (direction.y < 0.0f) {
|
||||
direction.y = 0.f;
|
||||
} else if (BA.y < 0.f) {
|
||||
direction.y = -direction.y;
|
||||
}
|
||||
if (direction.z < 0.0f) {
|
||||
direction.z = 0.f;
|
||||
} else if (BA.z < 0.f) {
|
||||
direction.z = -direction.z;
|
||||
}
|
||||
}
|
||||
direction = glm::normalize(direction);
|
||||
|
||||
// penetration is the projection of surfaceAB on direction
|
||||
collision->_penetration = glm::dot(surfaceAB, direction) * direction;
|
||||
|
|
@ -669,6 +723,7 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
|
|||
}
|
||||
return false;
|
||||
}
|
||||
*/
|
||||
|
||||
bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
|
||||
return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, collisions);
|
||||
|
|
|
|||
|
|
@ -681,58 +681,164 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
|
|||
}
|
||||
}
|
||||
|
||||
void ShapeColliderTests::sphereTouchesAACube() {
|
||||
void ShapeColliderTests::sphereTouchesAACubeFaces() {
|
||||
CollisionList collisions(16);
|
||||
|
||||
glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
|
||||
float cubeSide = 2.34f;
|
||||
|
||||
float sphereRadius = 1.13f;
|
||||
glm::vec3 sphereCenter(0.0f);
|
||||
SphereShape sphere(sphereRadius, sphereCenter);
|
||||
|
||||
QVector<glm::vec3> axes;
|
||||
axes.push_back(xAxis);
|
||||
axes.push_back(-xAxis);
|
||||
axes.push_back(yAxis);
|
||||
axes.push_back(-yAxis);
|
||||
axes.push_back(zAxis);
|
||||
axes.push_back(-zAxis);
|
||||
|
||||
for (int i = 0; i < axes.size(); ++i) {
|
||||
glm::vec3 axis = axes[i];
|
||||
// outside
|
||||
{
|
||||
collisions.clear();
|
||||
float overlap = 0.25f;
|
||||
float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap;
|
||||
sphereCenter = cubeCenter + sphereOffset * axis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl;
|
||||
}
|
||||
CollisionInfo* collision = collisions[0];
|
||||
if (!collision) {
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. 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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// inside
|
||||
{
|
||||
collisions.clear();
|
||||
float overlap = 1.25f * sphereRadius;
|
||||
float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap;
|
||||
sphereCenter = cubeCenter + sphereOffset * axis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube."
|
||||
<< " axis = " << axis
|
||||
<< std::endl;
|
||||
}
|
||||
CollisionInfo* collision = collisions[0];
|
||||
if (!collision) {
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo on y-axis."
|
||||
<< " 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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ShapeColliderTests::sphereTouchesAACubeEdges() {
|
||||
CollisionList collisions(20);
|
||||
|
||||
glm::vec3 cubeCenter(0.0f, 0.0f, 0.0f);
|
||||
float cubeSide = 2.0f;
|
||||
|
||||
float sphereRadius = 1.0f;
|
||||
glm::vec3 sphereCenter(0.0f);
|
||||
SphereShape sphere(sphereRadius, sphereCenter);
|
||||
|
||||
float sphereOffset = (0.5f * cubeSide + sphereRadius - 0.25f);
|
||||
QVector<glm::vec3> axes;
|
||||
// edges
|
||||
axes.push_back(glm::vec3(0.0f, 1.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(0.0f, 1.0f, -1.0f));
|
||||
axes.push_back(glm::vec3(0.0f, -1.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(0.0f, -1.0f, -1.0f));
|
||||
axes.push_back(glm::vec3(1.0f, 1.0f, 0.0f));
|
||||
axes.push_back(glm::vec3(1.0f, -1.0f, 0.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, 1.0f, 0.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, -1.0f, 0.0f));
|
||||
axes.push_back(glm::vec3(1.0f, 0.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(1.0f, 0.0f, -1.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, 0.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, 0.0f, -1.0f));
|
||||
// and corners
|
||||
axes.push_back(glm::vec3(1.0f, 1.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(1.0f, 1.0f, -1.0f));
|
||||
axes.push_back(glm::vec3(1.0f, -1.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(1.0f, -1.0f, -1.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, 1.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, 1.0f, -1.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, -1.0f, 1.0f));
|
||||
axes.push_back(glm::vec3(-1.0f, -1.0f, -1.0f));
|
||||
|
||||
// top
|
||||
sphereCenter = cubeCenter + sphereOffset * yAxis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
||||
}
|
||||
for (int i =0; i < axes.size(); ++i) {
|
||||
glm::vec3 axis = axes[i];
|
||||
float lengthAxis = glm::length(axis);
|
||||
axis /= lengthAxis;
|
||||
float overlap = 0.25f;
|
||||
|
||||
// bottom
|
||||
sphereCenter = cubeCenter - sphereOffset * yAxis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
||||
}
|
||||
|
||||
// left
|
||||
sphereCenter = cubeCenter + sphereOffset * xAxis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
||||
}
|
||||
|
||||
// right
|
||||
sphereCenter = cubeCenter - sphereOffset * xAxis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
||||
}
|
||||
|
||||
// forward
|
||||
sphereCenter = cubeCenter + sphereOffset * zAxis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
||||
}
|
||||
|
||||
// back
|
||||
sphereCenter = cubeCenter - sphereOffset * zAxis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
||||
sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis;
|
||||
sphere.setPosition(sphereCenter);
|
||||
|
||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl;
|
||||
}
|
||||
CollisionInfo* collision = collisions[i];
|
||||
if (!collision) {
|
||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. 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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -802,6 +908,7 @@ void ShapeColliderTests::runAllTests() {
|
|||
capsuleMissesCapsule();
|
||||
capsuleTouchesCapsule();
|
||||
|
||||
sphereTouchesAACube();
|
||||
sphereTouchesAACubeFaces();
|
||||
sphereTouchesAACubeEdges();
|
||||
sphereMissesAACube();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,7 +23,8 @@ namespace ShapeColliderTests {
|
|||
void capsuleMissesCapsule();
|
||||
void capsuleTouchesCapsule();
|
||||
|
||||
void sphereTouchesAACube();
|
||||
void sphereTouchesAACubeFaces();
|
||||
void sphereTouchesAACubeEdges();
|
||||
void sphereMissesAACube();
|
||||
|
||||
void runAllTests();
|
||||
|
|
|
|||
Loading…
Reference in a new issue