mirror of
https://github.com/overte-org/overte.git
synced 2025-08-09 03:17:02 +02:00
fix sphere-cube collisions, with tests
This commit is contained in:
parent
5ac301a77e
commit
c8e56a97cc
3 changed files with 241 additions and 78 deletions
|
@ -15,7 +15,6 @@
|
||||||
|
|
||||||
#include "GeometryUtil.h"
|
#include "GeometryUtil.h"
|
||||||
#include "ShapeCollider.h"
|
#include "ShapeCollider.h"
|
||||||
#include "StreamUtils.h" // adebug
|
|
||||||
|
|
||||||
// NOTE:
|
// NOTE:
|
||||||
//
|
//
|
||||||
|
@ -594,6 +593,95 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col
|
||||||
// helper function
|
// helper function
|
||||||
bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter,
|
bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter,
|
||||||
float cubeSide, CollisionList& collisions) {
|
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;
|
glm::vec3 BA = cubeCenter - sphereCenter;
|
||||||
float distance = glm::length(BA);
|
float distance = glm::length(BA);
|
||||||
if (distance > EPSILON) {
|
if (distance > EPSILON) {
|
||||||
|
@ -608,45 +696,11 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
|
||||||
if (glm::dot(surfaceAB, BA) > 0.f) {
|
if (glm::dot(surfaceAB, BA) > 0.f) {
|
||||||
CollisionInfo* collision = collisions.getNewCollision();
|
CollisionInfo* collision = collisions.getNewCollision();
|
||||||
if (collision) {
|
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
|
// penetration is parallel to box side direction
|
||||||
BA /= maxBA;
|
BA /= maxBA;
|
||||||
glm::vec3 direction;
|
glm::vec3 direction;
|
||||||
glm::modf(BA, direction);
|
glm::modf(BA, direction);
|
||||||
direction = glm::normalize(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
|
// penetration is the projection of surfaceAB on direction
|
||||||
collision->_penetration = glm::dot(surfaceAB, direction) * direction;
|
collision->_penetration = glm::dot(surfaceAB, direction) * direction;
|
||||||
|
@ -669,6 +723,7 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
|
bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
|
||||||
return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, 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);
|
CollisionList collisions(16);
|
||||||
|
|
||||||
glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
|
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 cubeSide = 2.0f;
|
||||||
|
|
||||||
float sphereRadius = 1.0f;
|
float sphereRadius = 1.0f;
|
||||||
glm::vec3 sphereCenter(0.0f);
|
glm::vec3 sphereCenter(0.0f);
|
||||||
SphereShape sphere(sphereRadius, sphereCenter);
|
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
|
for (int i =0; i < axes.size(); ++i) {
|
||||||
sphereCenter = cubeCenter + sphereOffset * yAxis;
|
glm::vec3 axis = axes[i];
|
||||||
sphere.setPosition(sphereCenter);
|
float lengthAxis = glm::length(axis);
|
||||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
axis /= lengthAxis;
|
||||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
float overlap = 0.25f;
|
||||||
}
|
|
||||||
|
|
||||||
// bottom
|
sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis;
|
||||||
sphereCenter = cubeCenter - sphereOffset * yAxis;
|
sphere.setPosition(sphereCenter);
|
||||||
sphere.setPosition(sphereCenter);
|
|
||||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
||||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl;
|
||||||
}
|
}
|
||||||
|
CollisionInfo* collision = collisions[i];
|
||||||
// left
|
if (!collision) {
|
||||||
sphereCenter = cubeCenter + sphereOffset * xAxis;
|
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl;
|
||||||
sphere.setPosition(sphereCenter);
|
}
|
||||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
|
||||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
glm::vec3 expectedPenetration = - overlap * axis;
|
||||||
}
|
if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) {
|
||||||
|
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration
|
||||||
// right
|
<< " expected " << expectedPenetration
|
||||||
sphereCenter = cubeCenter - sphereOffset * xAxis;
|
<< " axis = " << axis
|
||||||
sphere.setPosition(sphereCenter);
|
<< std::endl;
|
||||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
}
|
||||||
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
|
|
||||||
}
|
glm::vec3 expectedContact = sphereCenter - sphereRadius * axis;
|
||||||
|
if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) {
|
||||||
// forward
|
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint
|
||||||
sphereCenter = cubeCenter + sphereOffset * zAxis;
|
<< " expected " << expectedContact
|
||||||
sphere.setPosition(sphereCenter);
|
<< " axis = " << axis
|
||||||
if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
|
<< std::endl;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -802,6 +908,7 @@ void ShapeColliderTests::runAllTests() {
|
||||||
capsuleMissesCapsule();
|
capsuleMissesCapsule();
|
||||||
capsuleTouchesCapsule();
|
capsuleTouchesCapsule();
|
||||||
|
|
||||||
sphereTouchesAACube();
|
sphereTouchesAACubeFaces();
|
||||||
|
sphereTouchesAACubeEdges();
|
||||||
sphereMissesAACube();
|
sphereMissesAACube();
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,7 +23,8 @@ namespace ShapeColliderTests {
|
||||||
void capsuleMissesCapsule();
|
void capsuleMissesCapsule();
|
||||||
void capsuleTouchesCapsule();
|
void capsuleTouchesCapsule();
|
||||||
|
|
||||||
void sphereTouchesAACube();
|
void sphereTouchesAACubeFaces();
|
||||||
|
void sphereTouchesAACubeEdges();
|
||||||
void sphereMissesAACube();
|
void sphereMissesAACube();
|
||||||
|
|
||||||
void runAllTests();
|
void runAllTests();
|
||||||
|
|
Loading…
Reference in a new issue