Adding basic physics shapes (unfinished)

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 <glm/gtx/vector_angle.hpp>
+
+#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();
+}
+

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__) */

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 <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
+
+
+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__) */

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<const SphereShape*>(shapeA);
+        if (shapeB->getType() == Shape::SPHERE_SHAPE) {
+            return sphereSphere(sphereA, static_cast<const SphereShape*>(shapeB), 
+                    rotationAB, offsetB, collision);
+        } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
+            return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB), 
+                    rotationAB, offsetB, collision);
+        }
+    } else if (shapeA->getType() == Shape::CAPSULE_SHAPE) {
+        const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
+        if (shapeB->getType() == Shape::SPHERE_SHAPE) {
+            return capsuleSphere(capsuleA, static_cast<const SphereShape*>(shapeB), 
+                    rotationAB, offsetB, collision);
+        } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) {
+            return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(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

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__

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__) */