diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp new file mode 100644 index 0000000000..68e0acf561 --- /dev/null +++ b/libraries/shared/src/CapsuleShape.cpp @@ -0,0 +1,61 @@ +// +// CapsuleShape.cpp +// hifi +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#include + +#include "CapsuleShape.h" +#include "SharedUtil.h" + + +// default axis of CapsuleShape is Y-axis + +CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE) {} + +CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(Shape::CAPSULE_SHAPE), + _radius(radius), _halfHeight(halfHeight) { + updateBoundingRadius(); +} + +CapsuleShape::CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation) : + Shape(Shape::CAPSULE_SHAPE, position, rotation), _radius(radius), _halfHeight(halfHeight) { + updateBoundingRadius(); +} + +CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint) : + Shape(Shape::CAPSULE_SHAPE), _radius(radius), _halfHeight(0.f) { + glm::vec3 axis = endPoint - startPoint; + float height = glm::length(axis); + if (height > EPSILON) { + _halfHeight = 0.5f * height; + axis /= height; + glm::vec3 yAxis(0.f, 1.f, 0.f); + float angle = glm::angle(axis, yAxis); + if (angle > EPSILON) { + axis = glm::normalize(glm::cross(yAxis, axis)); + _rotation = glm::angleAxis(angle, axis); + } + } + updateBoundingRadius(); +} + +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(); +} + diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h new file mode 100644 index 0000000000..726d880c37 --- /dev/null +++ b/libraries/shared/src/CapsuleShape.h @@ -0,0 +1,37 @@ +// +// CapsuleShape.h +// hifi +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __hifi__CapsuleShape__ +#define __hifi__CapsuleShape__ + +#include "Shape.h" + +// default axis of CapsuleShape is Y-axis + +class CapsuleShape : public Shape { +public: + CapsuleShape(); + CapsuleShape(float radius, float halfHeight); + CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation); + CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint); + + float getRadius() const { return _radius; } + float getHalfHeight() const { return _halfHeight; } + + void setRadius(float radius); + void setHalfHeight(float height); + void setRadiusAndHalfHeight(float radius, float height); + +protected: + void updateBoundingRadius() { _boundingRadius = _radius + _halfHeight; } + + float _radius; + float _halfHeight; +}; + +#endif /* defined(__hifi__CapsuleShape__) */ diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h new file mode 100644 index 0000000000..f3f3c2c74a --- /dev/null +++ b/libraries/shared/src/Shape.h @@ -0,0 +1,48 @@ +// +// Shape.h +// hifi +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __hifi__Shape__ +#define __hifi__Shape__ + +#include +#include + + +class Shape { +public: + enum Type{ + UNKNOWN_SHAPE = 0, + SPHERE_SHAPE, + CAPSULE_SHAPE, + BOX_SHAPE, + }; + + Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { } + + int getType() const { return _type; } + float getBoundingRadius() const { return _boundingRadius; } + const glm::vec3& getPosition() const { return _position; } + const glm::quat& getRotation() const { return _rotation; } + + void setPosition(const glm::vec3& position) { _position = position; } + void setRotation(const glm::quat& rotation) { _rotation = rotation; } + +protected: + Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {} + Shape(Type type, const glm::vec3& position, const glm::quat& rotation) + : _type(type), _boundingRadius(0.f), _position(position), _rotation(rotation) {} + + void setBoundingRadius(float radius) { _boundingRadius = radius; } + + int _type; + float _boundingRadius; + glm::vec3 _position; + glm::quat _rotation; +}; + +#endif /* defined(__hifi__Shape__) */ diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp new file mode 100644 index 0000000000..0f3aabc51e --- /dev/null +++ b/libraries/shared/src/ShapeCollider.cpp @@ -0,0 +1,156 @@ +// +// ShapeCollider.cpp +// hifi +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#include "ShapeCollider.h" + +namespace ShapeCollider { + +bool shapeShape(const Shape* shapeA, const Shape* shapeB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + // ATM we only have two shape types so we just check every case. + // TODO: make a fast lookup for correct method + if (shapeA->getType() == Shape::SPHERE_SHAPE) { + const SphereShape* sphereA = static_cast(shapeA); + if (shapeB->getType() == Shape::SPHERE_SHAPE) { + return sphereSphere(sphereA, static_cast(shapeB), + rotationAB, offsetB, collision); + } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { + return sphereCapsule(sphereA, static_cast(shapeB), + rotationAB, offsetB, collision); + } + } else if (shapeA->getType() == Shape::CAPSULE_SHAPE) { + const CapsuleShape* capsuleA = static_cast(shapeA); + if (shapeB->getType() == Shape::SPHERE_SHAPE) { + return capsuleSphere(capsuleA, static_cast(shapeB), + rotationAB, offsetB, collision); + } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { + return capsuleCapsule(capsuleA, static_cast(shapeB), + rotationAB, offsetB, collision); + } + } + return false; +} + +bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + // A in B's frame + glm::vec3 A = offsetB + rotationAB * sphereA->getPosition(); + // BA = B - A = from A to B, in B's frame + glm::vec3 BA = sphereB->getPosition() - A; + float distanceSquared = glm::dot(BA, BA); + float totalRadius = sphereA->getRadius() + sphereB->getRadius(); + if (distanceSquared < totalRadius * totalRadius) { + // normalize BA + float distance = sqrtf(distanceSquared); + if (distanceSquared < 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; + } + // store the collision details in B's frame + collision._penetration = BA * (totalRadius - distance); + collision._contactPoint = A + sphereA->getRadius() * BA; + return true; + } + return false; +} + +// everything in the capsule's natural frame (where its axis is along yAxis) +bool sphereCapsuleHelper(float sphereRadius, const glm::vec3 sphereCenter, + const CapsuleShape* capsule, CollisionInfo& collision) { + float xzSquared = sphereCenter.x * sphereCenter.x + sphereCenter.y * sphereCenter.y; + float totalRadius = sphereRadius + capsule->getRadius(); + if (xzSquared < totalRadius * totalRadius) { + float fabsY = fabs(sphereCenter.y); + float halfHeight = capsule->getHalfHeight(); + if (fabsY < halfHeight) { + // sphere collides with cylindrical wall + glm::vec3 BA = -sphereCenter; + BA.y = 0.f; + float distance = sqrtf(xzSquared); + if (distance < EPSILON) { + // for now we don't handle this singular case + return false; + } + BA /= distance; + collision._penetration = BA * (totalRadius - distance); + collision._contactPoint = BA * (sphereRadius - totalRadius + distance); + collision._contactPoint.y = fabsY; + if (sphereCenter.y < 0.f) { + // negate the y elements of the collision info + collision._penetration.y *= -1.f; + collision._contactPoint.y *= -1.f; + } + return true; + } else { + // tansform into frame where cap is at origin + float newY = fabsY - halfHeight; + float distance = sqrtf(newY * newY + xzSquared); + if (distance < totalRadius) { + // sphere collides with cap + + // BA points from sphere to cap + glm::vec3 BA(-sphereCenter.x, newY, -sphereCenter.z); + if (distance < EPSILON) { + // for now we don't handle this singular case + return false; + } + BA /= distance; + collision._penetration = BA * (totalRadius - distance); + collision._contactPoint = BA * (sphereRadius - totalRadius + distance); + collision._contactPoint.y += halfHeight; + + if (sphereCenter.y < 0.f) { + // negate the y elements of the collision info + collision._penetration.y *= -1.f; + collision._contactPoint.y *= -1.f; + } + return true; + } + } + } + return false; +} + +bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + // transform sphereA all the way to capsuleB's natural frame + glm::quat rotationB = capsuleB->getRotation(); + glm::vec3 sphereCenter = rotationB * (offsetB + rotationAB * sphereA->getPosition() - capsuleB->getPosition()); + if (sphereCapsuleHelper(sphereA->getRadius(), sphereCenter, capsuleB, collision)) { + // need to transform collision details back into B's offset frame + collision.rotateThenTranslate(glm::inverse(rotationB), capsuleB->getPosition()); + return true; + } + return false; +} + +bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + // transform sphereB all the way to capsuleA's natural frame + glm::quat rotationBA = glm::inverse(rotationAB); + glm::quat rotationA = capsuleA->getRotation(); + glm::vec3 offsetA = rotationBA * (-offsetB); + glm::vec3 sphereCenter = rotationA * (offsetA + rotationBA * sphereB->getPosition() - capsuleA->getPosition()); + if (sphereCapsuleHelper(sphereB->getRadius(), sphereCenter, capsuleA, collision)) { + // need to transform collision details back into B's offset frame + // BUG: these back translations are probably not right + collision.rotateThenTranslate(glm::inverse(rotationA), capsuleA->getPosition()); + collision.rotateThenTranslate(glm::inverse(rotationAB), -offsetB); + return true; + } + return false; +} + +bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + return false; +} + +} // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h new file mode 100644 index 0000000000..f6505707fa --- /dev/null +++ b/libraries/shared/src/ShapeCollider.h @@ -0,0 +1,66 @@ +// +// ShapeCollider.h +// hifi +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __hifi__ShapeCollider__ +#define __hifi__ShapeCollider__ + +#include "CapsuleShape.h" +#include "CollisionInfo.h" +#include "SharedUtil.h" +#include "SphereShape.h" + +namespace ShapeCollider { + +/// \param shapeA pointer to first shape +/// \param shapeB pointer to second shape +/// \param rotationAB rotation from A into reference frame of B +/// \param offsetB offset of B (in B's frame) +/// \param[out] collision where to store collision details +/// \return true of shapes collide +bool shapeShape(const Shape* shapeA, const Shape* shapeB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + +/// \param sphereA pointer to first shape (sphere) +/// \param sphereB pointer to second shape (sphere) +/// \param rotationAB rotation from A into reference frame of B +/// \param offsetB offset of B (in B's frame) +/// \param[out] collision where to store collision details +/// \return true of shapes collide +bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + +/// \param sphereA pointer to first shape (sphere) +/// \param capsuleB pointer to second shape (capsule) +/// \param rotationAB rotation from A into reference frame of B +/// \param offsetB offset of B (in B's frame) +/// \param[out] collision where to store collision details +/// \return true of shapes collide +bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + +/// \param capsuleA pointer to first shape (capsule) +/// \param sphereB pointer to second shape (sphere) +/// \param rotationAB rotation from A into reference frame of B +/// \param offsetB offset of B (in B's frame) +/// \param[out] collision where to store collision details +/// \return true of shapes collide +bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + +/// \param capsuleA pointer to first shape (capsule) +/// \param capsuleB pointer to second shape (capsule) +/// \param rotationAB rotation from A into reference frame of B +/// \param offsetB offset of B (in B's frame) +/// \param[out] collision where to store collision details +/// \return true of shapes collide +bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, + const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision); + +} // namespace ShapeCollider + +#endif // __hifi__ShapeCollider__ diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h new file mode 100644 index 0000000000..879fcb3ff8 --- /dev/null +++ b/libraries/shared/src/SphereShape.h @@ -0,0 +1,23 @@ +// +// SphereShape.h +// hifi +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __hifi__SphereShape__ +#define __hifi__SphereShape__ + +#include "Shape.h" + +class SphereShape : public Shape { +public: + SphereShape() : Shape(Shape::SPHERE_SHAPE) {} + + float getRadius() const { return _boundingRadius; } + + void setRadius(float radius) { _boundingRadius = radius; } +}; + +#endif /* defined(__hifi__SphereShape__) */