Mirrors/overte-thingvellir
3
0
Fork 0
mirror of https://thingvellir.net/git/overte synced 2025-03-27 23:52:03 +01:00
Code Issues Projects Releases Packages Wiki Activity Actions

capsuleCapsule() collision with preliminary tests

Browse source
This commit is contained in:
Andrew Meadows 2014-02-25 10:59:12 -08:00
parent 8a3640f016
commit 9a70a50bdb
2 changed files with 243 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -42,9 +42,10 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis
if (distanceSquared < totalRadius * totalRadius) {
// normalize BA
float distance = sqrtf(distanceSquared);
if (distanceSquared < EPSILON) {
if (distance < EPSILON) {
// the spheres are on top of each other, so we pick an arbitrary penetration direction
BA = glm::vec3(0.f, 1.f, 0.f);
distance = totalRadius;
} else {
BA /= distance;
}
@ -166,6 +167,128 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col
}
bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision) {
glm::vec3 axisA;
capsuleA->computeNormalizedAxis(axisA);
glm::vec3 axisB;
capsuleB->computeNormalizedAxis(axisB);
glm::vec3 centerA = capsuleA->getPosition();
glm::vec3 centerB = capsuleB->getPosition();
// NOTE: The formula for closest approach between two lines is:
// d = [(B - A) . (a - (a.b)b)] / (1 - (a.b)^2)
float aDotB = glm::dot(axisA, axisB);
float denominator = 1.f - aDotB * aDotB;
float totalRadius = capsuleA->getRadius() + capsuleB->getRadius();
if (denominator > EPSILON) {
// distances to points of closest approach
float distanceA = glm::dot((centerB - centerA), (axisA - (aDotB) * axisB)) / denominator;
float distanceB = glm::dot((centerA - centerB), (axisB - (aDotB) * axisA)) / denominator;
// clamp the distances to the ends of the capsule line segments
float absDistanceA = fabs(distanceA);
if (absDistanceA > capsuleA->getHalfHeight() + capsuleA->getRadius()) {
float signA = distanceA < 0.f ? -1.f : 1.f;
distanceA = signA * capsuleA->getHalfHeight();
}
float absDistanceB = fabs(distanceB);
if (absDistanceB > capsuleB->getHalfHeight() + capsuleB->getRadius()) {
float signB = distanceB < 0.f ? -1.f : 1.f;
distanceB = signB * capsuleB->getHalfHeight();
}
// collide like spheres at closest approaches (do most of the math relative to B)
glm::vec3 BA = (centerB + distanceB * axisB) - (centerA + distanceA * axisA);
float distanceSquared = glm::dot(BA, BA);
if (distanceSquared < totalRadius * totalRadius) {
// normalize BA
float distance = sqrtf(distanceSquared);
if (distance < EPSILON) {
// the contact spheres are on top of each other, so we need to pick a penetration direction...
// try vector between the capsule centers...
BA = centerB - centerA;
distanceSquared = glm::length2(BA);
if (distanceSquared > EPSILON * EPSILON) {
distance = sqrtf(distanceSquared);
BA /= distance;
} else
{
// the capsule centers are on top of each other!
// give up on a valid penetration direction and just use the yAxis
BA = glm::vec3(0.f, 1.f, 0.f);
distance = glm::max(capsuleB->getRadius(), capsuleA->getRadius());
}
} else {
BA /= distance;
}
// penetration points from A into B
collision._penetration = BA * (totalRadius - distance);
// contactPoint is on surface of A
collision._contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA;
return true;
}
} else {
// capsules are approximiately parallel but might still collide
glm::vec3 BA = centerB - centerA;
float axialDistance = glm::dot(BA, axisB);
float maxAxialDistance = totalRadius + capsuleA->getHalfHeight() + capsuleB->getHalfHeight();
if (axialDistance > totalRadius + capsuleA->getHalfHeight() + capsuleB->getHalfHeight()) {
return false;
}
BA = BA - axialDistance * axisB; // BA now points from centerA to axisB (perp to axis)
float distanceSquared = glm::length2(BA);
if (distanceSquared < totalRadius * totalRadius) {
// We have all the info we need to compute the penetration vector...
// normalize BA
float distance = sqrtf(distanceSquared);
if (distance < EPSILON) {
// the spheres are on top of each other, so we pick an arbitrary penetration direction
BA = glm::vec3(0.f, 1.f, 0.f);
} else {
BA /= distance;
}
// penetration points from A into B
collision._penetration = BA * (totalRadius - distance);
// However we need some more world-frame info to compute the contactPoint,
// which is on the surface of capsuleA...
//
// Find the overlapping secion of the capsules --> they collide as if there were
// two spheres at the midpoint of this overlapping section.
// So we project all endpoints to axisB, find the interior pair,
// and put A's proxy sphere on axisA at the midpoint of this section.
// sort the projections as much as possible during calculation
float points[5];
points[0] = -capsuleB->getHalfHeight();
points[1] = axialDistance - capsuleA->getHalfHeight();
points[2] = axialDistance + capsuleA->getHalfHeight();
points[3] = capsuleB->getHalfHeight();
// Since there are only three comparisons to do we unroll the sort algorithm...
// and use a fifth slot as temp during swap.
if (points[4] > points[2]) {
points[5] = points[1];
points[1] = points[2];
points[2] = points[4]
}
if (points[2] > points[3]) {
points[4] = points[2];
points[2] = points[3];
points[3] = points[4];
}
if (points[0] > points[1]) {
points[4] = points[0];
points[0] = points[1];
points[1] = points[4];
}
// average the internal pair, and then do the math from centerB
collision._contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB
+ (capsuleA->getRadius() - distance) * BA
return true;
}
}
return false;
}

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

@ -386,13 +386,130 @@ void ShapeColliderTests::sphereTouchesCapsule() {
}
void ShapeColliderTests::capsuleMissesCapsule() {
// TODO: implement this
// non-overlapping capsules
float radiusA = 2.f;
float halfHeightA = 3.f;
float radiusB = 3.f;
float halfHeightB = 4.f;
float totalRadius = radiusA + radiusB;
float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
CapsuleShape capsuleA(radiusA, halfHeightA);
CapsuleShape capsuleB(radiusA, halfHeightA);
// side by side
capsuleB.setPosition((1.01f * totalRadius) * xAxis);
CollisionInfo collision;
if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
// end to end
capsuleB.setPosition((1.01f * totalHalfLength) * xAxis);
if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
// rotate B and move it to the side
glm::quat rotation = glm::angleAxis(rightAngle, zAxis);
capsuleB.setRotation(rotation);
capsuleB.setPosition((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
}
void ShapeColliderTests::capsuleTouchesCapsule() {
// TODO: implement this
// overlapping capsules
float radiusA = 2.f;
float halfHeightA = 3.f;
float radiusB = 3.f;
float halfHeightB = 4.f;
float totalRadius = radiusA + radiusB;
float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
CapsuleShape capsuleA(radiusA, halfHeightA);
CapsuleShape capsuleB(radiusB, halfHeightB);
// side by side
capsuleB.setPosition((0.95f * totalRadius) * xAxis);
CollisionInfo collision;
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
}
if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
}
// end to end
capsuleB.setPosition((0.99f * totalHalfLength) * yAxis);
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
}
if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
}
// rotate B and move it to the side
glm::quat rotation = glm::angleAxis(rightAngle, zAxis);
capsuleB.setRotation(rotation);
capsuleB.setPosition((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
}
if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
}
}
void ShapeColliderTests::runAllTests() {
sphereMissesSphere();
sphereTouchesSphere();