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

Capsule/box collisions.

Browse source
This commit is contained in:
Andrzej Kapolka 2013-05-27 14:05:44 -07:00
parent 70b25fd6c4
commit cfb66bee95
6 changed files with 186 additions and 63 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
interface/src/Avatar.cpp
View file

@ -615,13 +615,9 @@ void Avatar::updateCollisionWithEnvironment() {
void Avatar::updateCollisionWithVoxels() {
float radius = _height * 0.125f;
glm::vec3 penetration;
//if (Application::getInstance()->getVoxels()->findCapsulePenetration(
// _position - glm::vec3(0.0f, _pelvisFloatingHeight - radius, 0.0f),
// _position + glm::vec3(0.0f, _height - _pelvisFloatingHeight - radius, 0.0f), radius, penetration)) {
// applyCollisionWithScene(penetration);
//}
if (Application::getInstance()->getVoxels()->findSpherePenetration(_position, _height/2, penetration)) {
if (Application::getInstance()->getVoxels()->findCapsulePenetration(
_position - glm::vec3(0.0f, _pelvisFloatingHeight - radius, 0.0f),
_position + glm::vec3(0.0f, _height - _pelvisFloatingHeight - radius, 0.0f), radius, penetration)) {
applyCollisionWithScene(penetration);
}
}

131
libraries/voxels/src/AABox.cpp
View file

@ -107,6 +107,30 @@ static bool findIntersection(float origin, float direction, float corner, float
return false;
}
bool AABox::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const {
// handle the trivial cases where the expanded box contains the start or end
if (expandedContains(start, expansion) || expandedContains(end, expansion)) {
return true;
}
// check each axis
glm::vec3 expandedCorner = _corner - glm::vec3(expansion, expansion, expansion);
glm::vec3 expandedSize = _size + glm::vec3(expansion, expansion, expansion) * 2.0f;
glm::vec3 direction = end - start;
float axisDistance;
return (findIntersection(start.x, direction.x, expandedCorner.x, expandedSize.x, axisDistance) &&
axisDistance >= 0.0f && axisDistance <= 1.0f &&
isWithin(start.y + axisDistance*direction.y, expandedCorner.y, expandedSize.y) &&
isWithin(start.z + axisDistance*direction.z, expandedCorner.z, expandedSize.z)) ||
(findIntersection(start.y, direction.y, expandedCorner.y, expandedSize.y, axisDistance) &&
axisDistance >= 0.0f && axisDistance <= 1.0f &&
isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x) &&
isWithin(start.z + axisDistance*direction.z, expandedCorner.z, expandedSize.z)) ||
(findIntersection(start.z, direction.z, expandedCorner.z, expandedSize.z, axisDistance) &&
axisDistance >= 0.0f && axisDistance <= 1.0f &&
isWithin(start.y + axisDistance*direction.y, expandedCorner.y, expandedSize.y) &&
isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x));
}
bool AABox::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face) const {
// handle the trivial case where the box contains the origin
if (contains(origin)) {
@ -144,12 +168,15 @@ bool AABox::findSpherePenetration(const glm::vec3& center, float radius, glm::ve
float minPenetrationLength = FLT_MAX;
for (int i = 0; i < FACE_COUNT; i++) {
glm::vec4 facePlane = getPlane((BoxFace)i);
glm::vec3 vector = getClosestPointOnFace(center, (BoxFace)i) - center;
if (glm::dot(center4, getPlane((BoxFace)i)) >= 0.0f) {
return ::findSpherePenetration(vector, radius, penetration);
// outside this face, so use vector to closest point to determine penetration
return ::findSpherePenetration(vector, glm::vec3(-facePlane), radius, penetration);
}
float vectorLength = glm::length(vector);
if (vectorLength < minPenetrationLength) {
// remember the smallest penetration vector; if we're inside all faces, we'll use that
penetration = vector * ((vectorLength + radius) / -vectorLength);
minPenetrationLength = vectorLength;
}
@ -158,6 +185,33 @@ bool AABox::findSpherePenetration(const glm::vec3& center, float radius, glm::ve
return true;
}
bool AABox::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration) const {
glm::vec4 start4 = glm::vec4(start, 1.0f);
glm::vec4 end4 = glm::vec4(end, 1.0f);
glm::vec4 startToEnd = glm::vec4(end - start, 0.0f);
float minPenetrationLength = FLT_MAX;
for (int i = 0; i < FACE_COUNT; i++) {
// find the vector from the segment to the closest point on the face (starting from deeper end)
glm::vec4 facePlane = getPlane((BoxFace)i);
glm::vec3 closest = (glm::dot(start4, facePlane) <= glm::dot(end4, facePlane)) ?
getClosestPointOnFace(start4, startToEnd, (BoxFace)i) : getClosestPointOnFace(end4, -startToEnd, (BoxFace)i);
glm::vec3 vector = -computeVectorFromPointToSegment(closest, start, end);
if (glm::dot(vector, glm::vec3(facePlane)) < 0.0f) {
// outside this face, so use vector to closest point to determine penetration
return ::findSpherePenetration(vector, glm::vec3(-facePlane), radius, penetration);
}
float vectorLength = glm::length(vector);
if (vectorLength < minPenetrationLength) {
// remember the smallest penetration vector; if we're inside all faces, we'll use that
penetration = vector * ((vectorLength + radius) / -vectorLength);
minPenetrationLength = vectorLength;
}
}
return true;
}
glm::vec3 AABox::getClosestPointOnFace(const glm::vec3& point, BoxFace face) const {
switch (face) {
case MIN_X_FACE:
@ -186,6 +240,70 @@ glm::vec3 AABox::getClosestPointOnFace(const glm::vec3& point, BoxFace face) con
}
}
glm::vec3 AABox::getClosestPointOnFace(const glm::vec4& origin, const glm::vec4& direction, BoxFace face) const {
// check against the four planes that border the face
BoxFace oppositeFace = getOppositeFace(face);
bool anyOutside = false;
for (int i = 0; i < FACE_COUNT; i++) {
if (i == face || i == oppositeFace) {
continue;
}
glm::vec4 iPlane = getPlane((BoxFace)i);
float originDistance = glm::dot(origin, iPlane);
if (originDistance < 0.0f) {
continue; // inside the border
}
anyOutside = true;
float divisor = glm::dot(direction, iPlane);
if (fabs(divisor) < EPSILON) {
continue; // segment is parallel to plane
}
// find intersection and see if it lies within face bounds
float directionalDistance = -originDistance / divisor;
glm::vec4 intersection = origin + direction * directionalDistance;
BoxFace iOppositeFace = getOppositeFace((BoxFace)i);
for (int j = 0; j < FACE_COUNT; j++) {
if (j == face || j == oppositeFace || j == i || j == iOppositeFace) {
continue;
}
if (glm::dot(intersection, getPlane((BoxFace)j)) > 0.0f) {
goto outerContinue; // intersection is out of bounds
}
}
return getClosestPointOnFace(glm::vec3(intersection), face);
outerContinue: ;
}
// if we were outside any of the sides, we must check against the diagonals
if (anyOutside) {
int faceAxis = face / 2;
int secondAxis = (faceAxis + 1) % 3;
int thirdAxis = (faceAxis + 2) % 3;
glm::vec4 secondAxisMinPlane = getPlane((BoxFace)(secondAxis * 2));
glm::vec4 secondAxisMaxPlane = getPlane((BoxFace)(secondAxis * 2 + 1));
glm::vec4 thirdAxisMaxPlane = getPlane((BoxFace)(thirdAxis * 2 + 1));
glm::vec4 offset = glm::vec4(0.0f, 0.0f, 0.0f,
glm::dot(glm::vec3(secondAxisMaxPlane + thirdAxisMaxPlane), _size) * 0.5f);
glm::vec4 diagonals[] = { secondAxisMinPlane + thirdAxisMaxPlane + offset,
secondAxisMaxPlane + thirdAxisMaxPlane + offset };
for (int i = 0; i < sizeof(diagonals) / sizeof(diagonals[0]); i++) {
float divisor = glm::dot(direction, diagonals[i]);
if (fabs(divisor) < EPSILON) {
continue; // segment is parallel to diagonal plane
}
float directionalDistance = -glm::dot(origin, diagonals[i]) / divisor;
return getClosestPointOnFace(glm::vec3(origin + direction * directionalDistance), face);
}
}
// last resort or all inside: clamp origin to face
return getClosestPointOnFace(glm::vec3(origin), face);
}
glm::vec4 AABox::getPlane(BoxFace face) const {
switch (face) {
case MIN_X_FACE: return glm::vec4(-1.0f, 0.0f, 0.0f, _corner.x);
@ -196,3 +314,14 @@ glm::vec4 AABox::getPlane(BoxFace face) const {
case MAX_Z_FACE: return glm::vec4(0.0f, 0.0f, 1.0f, -_corner.z - _size.z);
}
}
BoxFace AABox::getOppositeFace(BoxFace face) {
switch (face) {
case MIN_X_FACE: return MAX_X_FACE;
case MAX_X_FACE: return MIN_X_FACE;
case MIN_Y_FACE: return MAX_Y_FACE;
case MAX_Y_FACE: return MIN_Y_FACE;
case MIN_Z_FACE: return MAX_Z_FACE;
case MAX_Z_FACE: return MIN_Z_FACE;
}
}

5
libraries/voxels/src/AABox.h
View file

@ -48,14 +48,19 @@ public:
bool contains(const glm::vec3& point) const;
bool expandedContains(const glm::vec3& point, float expansion) const;
bool expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const;
bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face) const;
bool findSpherePenetration(const glm::vec3& center, float radius, glm::vec3& penetration) const;
bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration) const;
private:
glm::vec3 getClosestPointOnFace(const glm::vec3& point, BoxFace face) const;
glm::vec3 getClosestPointOnFace(const glm::vec4& origin, const glm::vec4& direction, BoxFace face) const;
glm::vec4 getPlane(BoxFace face) const;
static BoxFace getOppositeFace(BoxFace face);
glm::vec3 _corner;
glm::vec3 _center;
glm::vec3 _size;

76
libraries/voxels/src/GeometryUtil.cpp
View file

@ -5,41 +5,18 @@
// Created by Andrzej Kapolka on 5/21/13.
// Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
#include <SharedUtil.h>
#include "GeometryUtil.h"
bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, float combinedRadius, glm::vec3& penetration) {
float vectorLength = glm::length(penetratorToPenetratee);
float distance = vectorLength - combinedRadius;
if (distance < 0.0f) {
penetration = penetratorToPenetratee * (-distance / vectorLength);
return true;
}
return false;
}
bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeLocation, glm::vec3& penetration) {
return findSpherePenetration(penetrateeLocation - penetratorCenter, penetratorRadius, penetration);
}
bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
return findSpherePointPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, penetrateeCenter, penetration);
}
bool findSphereLinePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeOrigin, const glm::vec3& penetrateeDirection,
glm::vec3& penetration) {
// compute the projection of the penetrator vector onto the line
float proj = glm::dot(penetratorCenter - penetrateeOrigin, penetrateeDirection);
return findSpherePenetration((penetrateeOrigin + penetrateeDirection*proj) - penetratorCenter,
penetratorRadius, penetration);
}
static glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) {
glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) {
// compute the projection of the point vector onto the segment vector
glm::vec3 segmentVector = end - start;
float proj = glm::dot(point - start, segmentVector) / glm::dot(segmentVector, segmentVector);
float lengthSquared = glm::dot(segmentVector, segmentVector);
if (lengthSquared < EPSILON) {
return start - point; // start and end the same
}
float proj = glm::dot(point - start, segmentVector) / lengthSquared;
if (proj <= 0.0f) { // closest to the start
return start - point;
@ -51,10 +28,36 @@ static glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const g
}
}
bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction,
float combinedRadius, glm::vec3& penetration) {
float vectorLength = glm::length(penetratorToPenetratee);
if (vectorLength < EPSILON) {
penetration = direction * combinedRadius;
return true;
}
float distance = vectorLength - combinedRadius;
if (distance < 0.0f) {
penetration = penetratorToPenetratee * (-distance / vectorLength);
return true;
}
return false;
}
bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeLocation, glm::vec3& penetration) {
return findSpherePenetration(penetrateeLocation - penetratorCenter, glm::vec3(0.0f, -1.0f, 0.0f),
penetratorRadius, penetration);
}
bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
return findSpherePointPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, penetrateeCenter, penetration);
}
bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration) {
return findSpherePenetration(computeVectorFromPointToSegment(penetratorCenter, penetrateeStart, penetrateeEnd),
penetratorRadius, penetration);
glm::vec3(0.0f, -1.0f, 0.0f), penetratorRadius, penetration);
}
bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart,
@ -73,15 +76,6 @@ bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetra
return false;
}
bool findCapsulePointPenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
const glm::vec3& penetrateeLocation, glm::vec3& penetration) {
if (findSphereSegmentPenetration(penetrateeLocation, penetratorRadius, penetratorStart, penetratorEnd, penetration)) {
penetration = -penetration;
return true;
}
return false;
}
bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
if (findSphereCapsulePenetration(penetrateeCenter, penetrateeRadius,

13
libraries/voxels/src/GeometryUtil.h
View file

@ -11,17 +11,17 @@
#include <glm/glm.hpp>
bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, float combinedRadius, glm::vec3& penetration);
glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end);
bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction,
float combinedRadius, glm::vec3& penetration);
bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeLocation, glm::vec3& penetration);
bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);
bool findSphereLinePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeOrigin, const glm::vec3& penetrateeDirection, glm::vec3& penetration);
bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration);
@ -30,9 +30,6 @@ bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penet
bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
const glm::vec4& penetrateePlane, glm::vec3& penetration);
bool findCapsulePointPenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
const glm::vec3& penetrateeLocation, glm::vec3& penetration);
bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);

14
libraries/voxels/src/VoxelTree.cpp
View file

@ -753,18 +753,20 @@ public:
bool findCapsulePenetrationOp(VoxelNode* node, void* extraData) {
CapsuleArgs* args = static_cast<CapsuleArgs*>(extraData);
// currently, we treat each node as a sphere enveloping the box
glm::vec3 nodePenetration;
if (!findCapsuleSpherePenetration(args->start, args->end, args->radius, node->getCenter(),
node->getEnclosingRadius(), nodePenetration)) {
// coarse check against bounds
const AABox& box = node->getAABox();
if (!box.expandedIntersectsSegment(args->start, args->end, args->radius)) {
return false;
}
if (!node->isLeaf()) {
return true; // recurse on children
}
if (node->isColored()) {
args->penetration = addPenetrations(args->penetration, nodePenetration * (float)TREE_SCALE);
args->found = true;
glm::vec3 nodePenetration;
if (box.findCapsulePenetration(args->start, args->end, args->radius, nodePenetration)) {
args->penetration = addPenetrations(args->penetration, nodePenetration * (float)TREE_SCALE);
args->found = true;
}
}
return false;
}