mirror of
https://github.com/overte-org/overte.git
synced 2025-07-23 06:44:07 +02:00
220 lines
7.6 KiB
C++
220 lines
7.6 KiB
C++
//
|
|
// CapsuleShape.cpp
|
|
// libraries/shared/src
|
|
//
|
|
// Created by Andrew Meadows on 02/20/2014.
|
|
// Copyright 2014 High Fidelity, Inc.
|
|
//
|
|
// Distributed under the Apache License, Version 2.0.
|
|
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
|
|
//
|
|
|
|
#include <iostream>
|
|
#include <glm/gtx/vector_angle.hpp>
|
|
|
|
#include "CapsuleShape.h"
|
|
|
|
#include "GeometryUtil.h"
|
|
#include "SharedUtil.h"
|
|
|
|
|
|
CapsuleShape::CapsuleShape() : Shape(CAPSULE_SHAPE), _radius(0.0f), _halfHeight(0.0f) {}
|
|
|
|
CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(CAPSULE_SHAPE),
|
|
_radius(radius), _halfHeight(halfHeight) {
|
|
updateBoundingRadius();
|
|
}
|
|
|
|
CapsuleShape::CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation) :
|
|
Shape(CAPSULE_SHAPE, position, rotation), _radius(radius), _halfHeight(halfHeight) {
|
|
updateBoundingRadius();
|
|
}
|
|
|
|
CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint) :
|
|
Shape(CAPSULE_SHAPE), _radius(radius), _halfHeight(0.0f) {
|
|
setEndPoints(startPoint, endPoint);
|
|
}
|
|
|
|
/// \param[out] startPoint is the center of start cap
|
|
void CapsuleShape::getStartPoint(glm::vec3& startPoint) const {
|
|
startPoint = _translation - _rotation * glm::vec3(0.0f, _halfHeight, 0.0f);
|
|
}
|
|
|
|
/// \param[out] endPoint is the center of the end cap
|
|
void CapsuleShape::getEndPoint(glm::vec3& endPoint) const {
|
|
endPoint = _translation + _rotation * glm::vec3(0.0f, _halfHeight, 0.0f);
|
|
}
|
|
|
|
void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const {
|
|
// default axis of a capsule is along the yAxis
|
|
axis = _rotation * DEFAULT_CAPSULE_AXIS;
|
|
}
|
|
|
|
void CapsuleShape::setRadius(float radius) {
|
|
_radius = radius;
|
|
updateBoundingRadius();
|
|
}
|
|
|
|
void CapsuleShape::setHalfHeight(float halfHeight) {
|
|
_halfHeight = halfHeight;
|
|
updateBoundingRadius();
|
|
}
|
|
|
|
void CapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight) {
|
|
_radius = radius;
|
|
_halfHeight = halfHeight;
|
|
updateBoundingRadius();
|
|
}
|
|
|
|
void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint) {
|
|
glm::vec3 axis = endPoint - startPoint;
|
|
_translation = 0.5f * (endPoint + startPoint);
|
|
float height = glm::length(axis);
|
|
if (height > EPSILON) {
|
|
_halfHeight = 0.5f * height;
|
|
axis /= height;
|
|
_rotation = computeNewRotation(axis);
|
|
}
|
|
updateBoundingRadius();
|
|
}
|
|
|
|
// helper
|
|
bool findRayIntersectionWithCap(const glm::vec3& sphereCenter, float sphereRadius,
|
|
const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) {
|
|
float r2 = sphereRadius * sphereRadius;
|
|
|
|
// compute closest approach (CA)
|
|
float a = glm::dot(sphereCenter - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA
|
|
float b2 = glm::distance2(sphereCenter, intersection._rayStart + a * intersection._rayDirection); // b2 = squared distance from sphere-center to CA
|
|
if (b2 > r2) {
|
|
// ray does not hit sphere
|
|
return false;
|
|
}
|
|
float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along intersection._rayDirection
|
|
float d2 = glm::distance2(intersection._rayStart, sphereCenter); // d2 = squared distance from sphere-center to ray-start
|
|
float distance = FLT_MAX;
|
|
if (a < 0.0f) {
|
|
// ray points away from sphere-center
|
|
if (d2 > r2) {
|
|
// ray starts outside sphere
|
|
return false;
|
|
}
|
|
// ray starts inside sphere
|
|
distance = c + a;
|
|
} else if (d2 > r2) {
|
|
// ray starts outside sphere
|
|
distance = a - c;
|
|
} else {
|
|
// ray starts inside sphere
|
|
distance = a + c;
|
|
}
|
|
if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) {
|
|
glm::vec3 sphereCenterToHitPoint = intersection._rayStart + distance * intersection._rayDirection - sphereCenter;
|
|
if (glm::dot(sphereCenterToHitPoint, sphereCenter - capsuleCenter) >= 0.0f) {
|
|
intersection._hitDistance = distance;
|
|
intersection._hitNormal = glm::normalize(sphereCenterToHitPoint);
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool CapsuleShape::findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const {
|
|
glm::vec3 capCenter;
|
|
getStartPoint(capCenter);
|
|
bool hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection);
|
|
getEndPoint(capCenter);
|
|
hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection) || hit;
|
|
if (hit) {
|
|
intersection._hitShape = const_cast<CapsuleShape*>(this);
|
|
}
|
|
return hit;
|
|
}
|
|
|
|
bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const {
|
|
// ray is U, capsule is V
|
|
glm::vec3 axisV;
|
|
computeNormalizedAxis(axisV);
|
|
glm::vec3 centerV = getTranslation();
|
|
|
|
// first handle parallel case
|
|
float uDotV = glm::dot(axisV, intersection._rayDirection);
|
|
glm::vec3 UV = intersection._rayStart - centerV;
|
|
if (glm::abs(1.0f - glm::abs(uDotV)) < EPSILON) {
|
|
// line and cylinder are parallel
|
|
float distanceV = glm::dot(UV, intersection._rayDirection);
|
|
if (glm::length2(UV - distanceV * intersection._rayDirection) <= _radius * _radius) {
|
|
// ray is inside cylinder's radius and might intersect caps
|
|
return findRayIntersectionWithCaps(centerV, intersection);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// Given a line with point 'U' and normalized direction 'u' and
|
|
// a cylinder with axial point 'V', radius 'r', and normalized direction 'v'
|
|
// the intersection of the two is on the line at distance 't' from 'U'.
|
|
//
|
|
// Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0
|
|
//
|
|
// where:
|
|
//
|
|
// UV = U-V
|
|
// w = u-(u.v)v
|
|
// Q = UV-(UV.v)v
|
|
//
|
|
// A = w^2
|
|
// B = 2(w.Q)
|
|
// C = Q^2 - r^2
|
|
|
|
glm::vec3 w = intersection._rayDirection - uDotV * axisV;
|
|
glm::vec3 Q = UV - glm::dot(UV, axisV) * axisV;
|
|
|
|
// we save a few multiplies by storing 2*A rather than just A
|
|
float A2 = 2.0f * glm::dot(w, w);
|
|
float B = 2.0f * glm::dot(w, Q);
|
|
|
|
// since C is only ever used once (in the determinant) we compute it inline
|
|
float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - _radius * _radius);
|
|
if (determinant < 0.0f) {
|
|
return false;
|
|
}
|
|
float hitLow = (-B - sqrtf(determinant)) / A2;
|
|
float hitHigh = -(hitLow + 2.0f * B / A2);
|
|
|
|
if (hitLow > hitHigh) {
|
|
// re-arrange so hitLow is always the smaller value
|
|
float temp = hitHigh;
|
|
hitHigh = hitLow;
|
|
hitLow = temp;
|
|
}
|
|
if (hitLow < 0.0f) {
|
|
if (hitHigh < 0.0f) {
|
|
// capsule is completely behind rayStart
|
|
return false;
|
|
}
|
|
hitLow = hitHigh;
|
|
}
|
|
|
|
glm::vec3 p = intersection._rayStart + hitLow * intersection._rayDirection;
|
|
float d = glm::dot(p - centerV, axisV);
|
|
if (glm::abs(d) <= getHalfHeight()) {
|
|
// we definitely hit the cylinder wall
|
|
intersection._hitDistance = hitLow;
|
|
intersection._hitNormal = glm::normalize(p - centerV - d * axisV);
|
|
intersection._hitShape = const_cast<CapsuleShape*>(this);
|
|
return true;
|
|
}
|
|
|
|
// ray still might hit the caps
|
|
return findRayIntersectionWithCaps(centerV, intersection);
|
|
}
|
|
|
|
// static
|
|
glm::quat CapsuleShape::computeNewRotation(const glm::vec3& newAxis) {
|
|
float angle = glm::angle(newAxis, DEFAULT_CAPSULE_AXIS);
|
|
if (angle > EPSILON) {
|
|
glm::vec3 rotationAxis = glm::normalize(glm::cross(DEFAULT_CAPSULE_AXIS, newAxis));
|
|
return glm::angleAxis(angle, rotationAxis);
|
|
}
|
|
return glm::quat();
|
|
}
|