From ef6d2a624bc2e2225c29c16acc893e845591f95d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 21 Feb 2014 16:24:40 -0800 Subject: [PATCH 01/33] helper methods to rotate/translate CollisionInfo --- libraries/shared/src/CollisionInfo.cpp | 12 ++++++++++++ libraries/shared/src/CollisionInfo.h | 7 +++++++ 2 files changed, 19 insertions(+) diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index 5d74d591c6..de233d9420 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -8,6 +8,18 @@ #include "CollisionInfo.h" +void CollisionInfo::rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation) { + _contactPoint = translation + rotation * _contactPoint; + _penetration = rotation * _penetration; + _addedVelocity = rotation * _addedVelocity; +} + +void CollisionInfo::translateThenRotate(const glm::vec3& translation, const glm::quat& rotation) { + _contactPoint = rotation * (_contactPoint + translation); + _penetration = rotation * _penetration; + _addedVelocity = rotation * _addedVelocity; +} + CollisionList::CollisionList(int maxSize) : _maxSize(maxSize), _size(0) { diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index acd127435c..b71d67e532 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -10,6 +10,7 @@ #define __hifi__CollisionInfo__ #include +#include #include @@ -55,6 +56,12 @@ public: ~CollisionInfo() {} + /// Rotate and translate the details. + void rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation); + + /// Translate then rotate the details + void translateThenRotate(const glm::vec3& translation, const glm::quat& rotation); + qint32 _type; // type of Collision (will determine what is supposed to be in _data and _flags) void* _data; // pointer to user supplied data quint32 _flags; // 32 bits for whatever From 072369abfeac261549f562784d46f8e076533942 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 21 Feb 2014 16:27:44 -0800 Subject: [PATCH 02/33] Adding physics test framework --- tests/physics/CMakeLists.txt | 34 ++++++++ tests/physics/src/CollisionInfoTests.cpp | 102 +++++++++++++++++++++++ tests/physics/src/CollisionInfoTests.h | 21 +++++ tests/physics/src/PhysicsTestUtil.cpp | 21 +++++ tests/physics/src/PhysicsTestUtil.h | 26 ++++++ tests/physics/src/ShapeColliderTests.cpp | 33 ++++++++ tests/physics/src/ShapeColliderTests.h | 20 +++++ tests/physics/src/main.cpp | 15 ++++ 8 files changed, 272 insertions(+) create mode 100644 tests/physics/CMakeLists.txt create mode 100644 tests/physics/src/CollisionInfoTests.cpp create mode 100644 tests/physics/src/CollisionInfoTests.h create mode 100644 tests/physics/src/PhysicsTestUtil.cpp create mode 100644 tests/physics/src/PhysicsTestUtil.h create mode 100644 tests/physics/src/ShapeColliderTests.cpp create mode 100644 tests/physics/src/ShapeColliderTests.h create mode 100644 tests/physics/src/main.cpp diff --git a/tests/physics/CMakeLists.txt b/tests/physics/CMakeLists.txt new file mode 100644 index 0000000000..482e6e0ec3 --- /dev/null +++ b/tests/physics/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8) + +set(TARGET_NAME physics-tests) + +set(ROOT_DIR ../..) +set(MACRO_DIR ${ROOT_DIR}/cmake/macros) + +# setup for find modules +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../../cmake/modules/") + +#find_package(Qt5Network REQUIRED) +#find_package(Qt5Script REQUIRED) +#find_package(Qt5Widgets REQUIRED) + +include(${MACRO_DIR}/SetupHifiProject.cmake) +setup_hifi_project(${TARGET_NAME} TRUE) + +include(${MACRO_DIR}/AutoMTC.cmake) +auto_mtc(${TARGET_NAME} ${ROOT_DIR}) + +#qt5_use_modules(${TARGET_NAME} Network Script Widgets) + +#include glm +include(${MACRO_DIR}/IncludeGLM.cmake) +include_glm(${TARGET_NAME} ${ROOT_DIR}) + +# link in the shared libraries +include(${MACRO_DIR}/LinkHifiLibrary.cmake) +link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR}) + +IF (WIN32) + #target_link_libraries(${TARGET_NAME} Winmm Ws2_32) +ENDIF(WIN32) + diff --git a/tests/physics/src/CollisionInfoTests.cpp b/tests/physics/src/CollisionInfoTests.cpp new file mode 100644 index 0000000000..0355e2d27b --- /dev/null +++ b/tests/physics/src/CollisionInfoTests.cpp @@ -0,0 +1,102 @@ +// +// CollisionInfoTests.cpp +// physics-tests +// +// Created by Andrew Meadows on 2014.02.21 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#include + +#include +#include + +#include +#include + +#include "CollisionInfoTests.h" +#include "PhysicsTestUtil.h" + + +void CollisionInfoTests::rotateThenTranslate() { + CollisionInfo collision; + collision._penetration = xAxis; + collision._contactPoint = yAxis; + collision._addedVelocity = xAxis + yAxis + zAxis; + + glm::quat rotation = glm::angleAxis(rightAngle, zAxis); + float distance = 3.f; + glm::vec3 translation = distance * yAxis; + + collision.rotateThenTranslate(rotation, translation); + + float error = glm::distance(collision._penetration, yAxis); + if (error > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: collision._penetration = " << collision._penetration + << " but we expected " << yAxis + << std::endl; + } + + glm::vec3 expectedContactPoint = -xAxis + translation; + error = glm::distance(collision._contactPoint, expectedContactPoint); + if (error > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: collision._contactPoint = " << collision._contactPoint + << " but we expected " << expectedContactPoint + << std::endl; + } + + glm::vec3 expectedAddedVelocity = yAxis - xAxis + zAxis; + error = glm::distance(collision._addedVelocity, expectedAddedVelocity); + if (error > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: collision._addedVelocity = " << collision._contactPoint + << " but we expected " << expectedAddedVelocity + << std::endl; + } +} + +void CollisionInfoTests::translateThenRotate() { + CollisionInfo collision; + collision._penetration = xAxis; + collision._contactPoint = yAxis; + collision._addedVelocity = xAxis + yAxis + zAxis; + + glm::quat rotation = glm::angleAxis( -rightAngle, zAxis); + float distance = 3.f; + glm::vec3 translation = distance * yAxis; + + collision.translateThenRotate(translation, rotation); + + float error = glm::distance(collision._penetration, -yAxis); + if (error > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: collision._penetration = " << collision._penetration + << " but we expected " << -yAxis + << std::endl; + } + + glm::vec3 expectedContactPoint = (1.f + distance) * xAxis; + error = glm::distance(collision._contactPoint, expectedContactPoint); + if (error > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: collision._contactPoint = " << collision._contactPoint + << " but we expected " << expectedContactPoint + << std::endl; + } + + glm::vec3 expectedAddedVelocity = - yAxis + xAxis + zAxis; + error = glm::distance(collision._addedVelocity, expectedAddedVelocity); + if (error > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: collision._addedVelocity = " << collision._contactPoint + << " but we expected " << expectedAddedVelocity + << std::endl; + } +} + +void CollisionInfoTests::runAllTests() { + CollisionInfoTests::rotateThenTranslate(); + CollisionInfoTests::translateThenRotate(); +} diff --git a/tests/physics/src/CollisionInfoTests.h b/tests/physics/src/CollisionInfoTests.h new file mode 100644 index 0000000000..cc0ef337a2 --- /dev/null +++ b/tests/physics/src/CollisionInfoTests.h @@ -0,0 +1,21 @@ +// +// CollisionInfoTests.h +// physics-tests +// +// Created by Andrew Meadows on 2014.02.21 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __tests__CollisionInfoTests__ +#define __tests__CollisionInfoTests__ + +namespace CollisionInfoTests { + + void rotateThenTranslate(); + void translateThenRotate(); + + void runAllTests(); +} + +#endif // __tests__CollisionInfoTests__ + diff --git a/tests/physics/src/PhysicsTestUtil.cpp b/tests/physics/src/PhysicsTestUtil.cpp new file mode 100644 index 0000000000..f14739c07f --- /dev/null +++ b/tests/physics/src/PhysicsTestUtil.cpp @@ -0,0 +1,21 @@ +// +// PhysicsTestUtil.cpp +// physics-tests +// +// Created by Andrew Meadows on 2014.02.21 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#include "PhysicsTestUtil.h" + +std::ostream& operator<<(std::ostream& s, const glm::vec3& v) { + s << "<" << v.x << "," << v.y << "," << v.z << ">"; + return s; +} + +std::ostream& operator<<(std::ostream& s, const CollisionInfo& c) { + s << "[penetration=" << c._penetration + << ", contactPoint=" << c._contactPoint + << ", addedVelocity=" << c._addedVelocity; + return s; +} diff --git a/tests/physics/src/PhysicsTestUtil.h b/tests/physics/src/PhysicsTestUtil.h new file mode 100644 index 0000000000..f63e7e5910 --- /dev/null +++ b/tests/physics/src/PhysicsTestUtil.h @@ -0,0 +1,26 @@ +// +// PhysicsTestUtil.h +// physics-tests +// +// Created by Andrew Meadows on 2014.02.21 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __tests__PhysicsTestUtil__ +#define __tests__PhysicsTestUtil__ + +#include +#include + +#include + +const glm::vec3 xAxis(1.f, 0.f, 0.f); +const glm::vec3 yAxis(0.f, 1.f, 0.f); +const glm::vec3 zAxis(0.f, 0.f, 1.f); + +const float rightAngle = 90.f; // degrees + +std::ostream& operator<<(std::ostream& s, const glm::vec3& v); +std::ostream& operator<<(std::ostream& s, const CollisionInfo& c); + +#endif // __tests__PhysicsTestUtil__ diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp new file mode 100644 index 0000000000..c103179b07 --- /dev/null +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -0,0 +1,33 @@ +// +// ShapeColliderTests.cpp +// physics-tests +// +// Created by Andrew Meadows on 2014.02.21 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +//#include +#include + +#include +#include + +#include +#include + +#include "PhysicsTestUtil.h" +#include "ShapeColliderTests.h" + + +/* +void ShapeColliderTests::test1() { +} + +void ShapeColliderTests::test2() { +} +*/ + +void ShapeColliderTests::runAllTests() { +// ShapeColliderTests::test1(); +// ShapeColliderTests::test2(); +} diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h new file mode 100644 index 0000000000..a51fb83854 --- /dev/null +++ b/tests/physics/src/ShapeColliderTests.h @@ -0,0 +1,20 @@ +// +// ShapeColliderTests.h +// physics-tests +// +// Created by Andrew Meadows on 2014.02.21 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __tests__ShapeColliderTests__ +#define __tests__ShapeColliderTests__ + +namespace ShapeColliderTests { + + //void rotateThenTranslate(); + //void translateThenRotate(); + + void runAllTests(); +} + +#endif // __tests__ShapeColliderTests__ diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp new file mode 100644 index 0000000000..3ae5a0a6fe --- /dev/null +++ b/tests/physics/src/main.cpp @@ -0,0 +1,15 @@ +// +// main.cpp +// physics-tests +// + +//#include + +#include "ShapeColliderTests.h" +#include "CollisionInfoTests.h" + +int main(int argc, char** argv) { + CollisionInfoTests::runAllTests(); + ShapeColliderTests::runAllTests(); + return 0; +} From 8c1dc40d39a1d57edd406d5a0723a8ee1fe9ab27 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 21 Feb 2014 16:29:07 -0800 Subject: [PATCH 03/33] Adding basic physics shapes (unfinished) --- libraries/shared/src/CapsuleShape.cpp | 61 ++++++++++ libraries/shared/src/CapsuleShape.h | 37 ++++++ libraries/shared/src/Shape.h | 48 ++++++++ libraries/shared/src/ShapeCollider.cpp | 156 +++++++++++++++++++++++++ libraries/shared/src/ShapeCollider.h | 66 +++++++++++ libraries/shared/src/SphereShape.h | 23 ++++ 6 files changed, 391 insertions(+) create mode 100644 libraries/shared/src/CapsuleShape.cpp create mode 100644 libraries/shared/src/CapsuleShape.h create mode 100644 libraries/shared/src/Shape.h create mode 100644 libraries/shared/src/ShapeCollider.cpp create mode 100644 libraries/shared/src/ShapeCollider.h create mode 100644 libraries/shared/src/SphereShape.h 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__) */ From 0e28e0947c6accd27b33953ec406ea96051c5707 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Sat, 22 Feb 2014 13:20:47 -0800 Subject: [PATCH 04/33] Adding ShapeColliderTests.* --- libraries/shared/src/Shape.h | 1 + libraries/shared/src/ShapeCollider.cpp | 28 ++++++++++++------------ libraries/shared/src/ShapeCollider.h | 20 ++++++++--------- libraries/shared/src/SphereShape.h | 5 +++++ tests/physics/src/ShapeColliderTests.cpp | 27 ++++++++++++++++++++--- tests/physics/src/ShapeColliderTests.h | 4 ++-- 6 files changed, 56 insertions(+), 29 deletions(-) diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index f3f3c2c74a..0e6d17c059 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -34,6 +34,7 @@ public: 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) {} diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 0f3aabc51e..f3598ebf39 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -11,35 +11,35 @@ namespace ShapeCollider { bool shapeShape(const Shape* shapeA, const Shape* shapeB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, 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); + rotationAB, offsetA, collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { return sphereCapsule(sphereA, static_cast(shapeB), - rotationAB, offsetB, collision); + rotationAB, offsetA, 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); + rotationAB, offsetA, collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { return capsuleCapsule(capsuleA, static_cast(shapeB), - rotationAB, offsetB, collision); + rotationAB, offsetA, collision); } } return false; } bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { // A in B's frame - glm::vec3 A = offsetB + rotationAB * sphereA->getPosition(); + glm::vec3 A = rotationAB * sphereA->getPosition() + offsetA; // BA = B - A = from A to B, in B's frame glm::vec3 BA = sphereB->getPosition() - A; float distanceSquared = glm::dot(BA, BA); @@ -119,10 +119,10 @@ bool sphereCapsuleHelper(float sphereRadius, const glm::vec3 sphereCenter, } bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, 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()); + glm::vec3 sphereCenter = rotationB * (offsetA + 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()); @@ -132,24 +132,24 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, } bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, 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()); + glm::vec3 offsetB = rotationBA * (-offsetA); + glm::vec3 sphereCenter = rotationA * (offsetB + 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); + collision.rotateThenTranslate(glm::inverse(rotationAB), -offsetA); return true; } return false; } bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetB, CollisionInfo& collision) { + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { return false; } diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index f6505707fa..4cef6f943f 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -19,47 +19,47 @@ 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 offsetA offset of A (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); + const glm::quat& rotationAB, const glm::vec3& offsetA, 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 offsetA offset of A (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); + const glm::quat& rotationAB, const glm::vec3& offsetA, 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 offsetA offset of A (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); + const glm::quat& rotationAB, const glm::vec3& offsetA, 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 offsetA offset of A (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); + const glm::quat& rotationAB, const glm::vec3& offsetA, 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 offsetA offset of A (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); + const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); } // namespace ShapeCollider diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index 879fcb3ff8..a1028956ee 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -15,6 +15,11 @@ class SphereShape : public Shape { public: SphereShape() : Shape(Shape::SPHERE_SHAPE) {} + SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE) { + _boundingRadius = radius; + setPosition(position); + } + float getRadius() const { return _boundingRadius; } void setRadius(float radius) { _boundingRadius = radius; } diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index c103179b07..b964e2f6de 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -13,21 +13,42 @@ #include #include +#include #include +#include #include "PhysicsTestUtil.h" #include "ShapeColliderTests.h" +const glm::vec3 origin(0.f); -/* -void ShapeColliderTests::test1() { +void ShapeColliderTests::sphereSphere() { + CollisionInfo collision; + + float radius = 2.f; + SphereShape sphereA(radius, origin); + SphereShape sphereB(radius, origin); + + glm::vec3 translation(0.5f * radius, 0.f, 0.f); + glm::quat rotation; + + // these spheres should be touching + bool touching = ShapeCollider::sphereSphere(&sphereA, &sphereB, rotation, translation, collision); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB do not touch" << std::endl; + } + + // TODO: verify the collision info is good... + // penetration should point from A into B } +/* void ShapeColliderTests::test2() { } */ void ShapeColliderTests::runAllTests() { -// ShapeColliderTests::test1(); + ShapeColliderTests::sphereSphere(); // ShapeColliderTests::test2(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index a51fb83854..a3a19805f8 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -11,8 +11,8 @@ namespace ShapeColliderTests { - //void rotateThenTranslate(); - //void translateThenRotate(); + void sphereSphere(); + //void test2(); void runAllTests(); } From 8a3640f0162f6587000793abd8c04f2321854ebd Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 24 Feb 2014 11:38:27 -0800 Subject: [PATCH 05/33] Adding CapsuleSphere collisions with tests. --- libraries/shared/src/CapsuleShape.cpp | 17 + libraries/shared/src/CapsuleShape.h | 9 + libraries/shared/src/CollisionInfo.cpp | 11 - libraries/shared/src/CollisionInfo.h | 6 - libraries/shared/src/Shape.h | 6 +- libraries/shared/src/ShapeCollider.cpp | 206 ++++++------ libraries/shared/src/ShapeCollider.h | 65 ++-- libraries/shared/src/SphereShape.h | 7 +- tests/physics/src/CollisionInfoTests.cpp | 6 +- tests/physics/src/CollisionInfoTests.h | 4 +- tests/physics/src/ShapeColliderTests.cpp | 391 +++++++++++++++++++++-- tests/physics/src/ShapeColliderTests.h | 10 +- tests/physics/src/main.cpp | 4 - 13 files changed, 557 insertions(+), 185 deletions(-) diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index 68e0acf561..c71f03aa47 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -6,6 +6,7 @@ // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. // +#include #include #include "CapsuleShape.h" @@ -13,6 +14,7 @@ // default axis of CapsuleShape is Y-axis +const glm::vec3 localAxis(0.f, 1.f, 0.f); CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE) {} @@ -43,6 +45,21 @@ CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm: updateBoundingRadius(); } +/// \param[out] startPoint is the center of start cap +void CapsuleShape::getStartPoint(glm::vec3& startPoint) const { + startPoint = getPosition() - _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f)); +} + +/// \param[out] endPoint is the center of the end cap +void CapsuleShape::getEndPoint(glm::vec3& endPoint) const { + endPoint = getPosition() + _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f)); +} + +void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { + // default axis of a capsule is along the yAxis + axis = _rotation * glm::vec3(0.f, 1.f, 0.f); +} + void CapsuleShape::setRadius(float radius) { _radius = radius; updateBoundingRadius(); diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h index 726d880c37..6d7e0a50be 100644 --- a/libraries/shared/src/CapsuleShape.h +++ b/libraries/shared/src/CapsuleShape.h @@ -11,6 +11,7 @@ #include "Shape.h" +// adebug bookmark TODO: convert to new world-frame approach // default axis of CapsuleShape is Y-axis class CapsuleShape : public Shape { @@ -23,6 +24,14 @@ public: float getRadius() const { return _radius; } float getHalfHeight() const { return _halfHeight; } + /// \param[out] startPoint is the center of start cap + void getStartPoint(glm::vec3& startPoint) const; + + /// \param[out] endPoint is the center of the end cap + void getEndPoint(glm::vec3& endPoint) const; + + void computeNormalizedAxis(glm::vec3& axis) const; + void setRadius(float radius); void setHalfHeight(float height); void setRadiusAndHalfHeight(float radius, float height); diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index de233d9420..9e76ca59f6 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -8,17 +8,6 @@ #include "CollisionInfo.h" -void CollisionInfo::rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation) { - _contactPoint = translation + rotation * _contactPoint; - _penetration = rotation * _penetration; - _addedVelocity = rotation * _addedVelocity; -} - -void CollisionInfo::translateThenRotate(const glm::vec3& translation, const glm::quat& rotation) { - _contactPoint = rotation * (_contactPoint + translation); - _penetration = rotation * _penetration; - _addedVelocity = rotation * _addedVelocity; -} CollisionList::CollisionList(int maxSize) : _maxSize(maxSize), diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index b71d67e532..629cb6b8f2 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -56,12 +56,6 @@ public: ~CollisionInfo() {} - /// Rotate and translate the details. - void rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation); - - /// Translate then rotate the details - void translateThenRotate(const glm::vec3& translation, const glm::quat& rotation); - qint32 _type; // type of Collision (will determine what is supposed to be in _data and _flags) void* _data; // pointer to user supplied data quint32 _flags; // 32 bits for whatever diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 0e6d17c059..5273c69007 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -1,6 +1,5 @@ // // Shape.h -// hifi // // Created by Andrew Meadows on 2014.02.20 // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. @@ -20,6 +19,7 @@ public: SPHERE_SHAPE, CAPSULE_SHAPE, BOX_SHAPE, + LIST_SHAPE }; Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { } @@ -33,8 +33,12 @@ public: void setRotation(const glm::quat& rotation) { _rotation = rotation; } protected: + // these ctors are protected (used by derived classes only) Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {} + Shape(Type type, const glm::vec3& position) + : _type(type), _boundingRadius(0.f), _position(position), _rotation() {} + Shape(Type type, const glm::vec3& position, const glm::quat& rotation) : _type(type), _boundingRadius(0.f), _position(position), _rotation(rotation) {} diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index f3598ebf39..c5d497694e 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -6,42 +6,37 @@ // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. // +#include + +#include + #include "ShapeCollider.h" namespace ShapeCollider { -bool shapeShape(const Shape* shapeA, const Shape* shapeB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { +bool shapeShape(const Shape* shapeA, const Shape* shapeB, 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, offsetA, collision); + return sphereSphere(sphereA, static_cast(shapeB), collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return sphereCapsule(sphereA, static_cast(shapeB), - rotationAB, offsetA, collision); + return sphereCapsule(sphereA, static_cast(shapeB), 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, offsetA, collision); + return capsuleSphere(capsuleA, static_cast(shapeB), collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return capsuleCapsule(capsuleA, static_cast(shapeB), - rotationAB, offsetA, collision); + return capsuleCapsule(capsuleA, static_cast(shapeB), collision); } } return false; } -bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { - // A in B's frame - glm::vec3 A = rotationAB * sphereA->getPosition() + offsetA; - // BA = B - A = from A to B, in B's frame - glm::vec3 BA = sphereB->getPosition() - A; +bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision) { + glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition(); float distanceSquared = glm::dot(BA, BA); float totalRadius = sphereA->getRadius() + sphereB->getRadius(); if (distanceSquared < totalRadius * totalRadius) { @@ -53,103 +48,124 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, } else { BA /= distance; } - // store the collision details in B's frame + // penetration points from A into B collision._penetration = BA * (totalRadius - distance); - collision._contactPoint = A + sphereA->getRadius() * BA; + // contactPoint is on surface of A + collision._contactPoint = sphereA->getPosition() + 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 +bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision) { + // find sphereA's closest approach to axis of capsuleB + glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition(); + glm::vec3 capsuleAxis; + capsuleB->computeNormalizedAxis(capsuleAxis); + float axialDistance = - glm::dot(BA, capsuleAxis); + float absAxialDistance = fabs(axialDistance); + float totalRadius = sphereA->getRadius() + capsuleB->getRadius(); + if (absAxialDistance < totalRadius + capsuleB->getHalfHeight()) { + glm::vec3 radialAxis = BA + axialDistance * capsuleAxis; // points from A to axis of B + float radialDistance2 = glm::length2(radialAxis); + if (radialDistance2 > totalRadius * totalRadius) { + // sphere is too far from capsule axis + return false; + } + if (absAxialDistance > capsuleB->getHalfHeight()) { + // sphere hits capsule on a cap --> recompute radialAxis to point from spherA to cap center + float sign = (axialDistance > 0.f) ? 1.f : -1.f; + radialAxis = BA + (sign * capsuleB->getHalfHeight()) * capsuleAxis; + radialDistance2 = glm::length2(radialAxis); + } + if (radialDistance2 > EPSILON * EPSILON) { + // normalize the radialAxis + float radialDistance = sqrtf(radialDistance2); + radialAxis /= radialDistance; + // penetration points from A into B + collision._penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B + // contactPoint is on surface of sphereA + collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis; + } else { + // A is on B's axis, so the penetration is undefined... + if (absAxialDistance > capsuleB->getHalfHeight()) { + // ...for the cylinder case (for now we pretend the collision doesn't exist) 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; + // ... but still defined for the cap case + if (axialDistance < 0.f) { + // we're hitting the start cap, so we negate the capsuleAxis + capsuleAxis *= -1; } - return true; + // penetration points from A into B + float sign = (axialDistance > 0.f) ? -1.f : 1.f; + collision._penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; + // contactPoint is on surface of sphereA + collision._contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis; + } + return true; + } + return false; +} + +bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision) { + // find sphereB's closest approach to axis of capsuleA + glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition(); + glm::vec3 capsuleAxis; + capsuleA->computeNormalizedAxis(capsuleAxis); + float axialDistance = - glm::dot(AB, capsuleAxis); + float absAxialDistance = fabs(axialDistance); + float totalRadius = sphereB->getRadius() + capsuleA->getRadius(); + if (absAxialDistance < totalRadius + capsuleA->getHalfHeight()) { + glm::vec3 radialAxis = AB + axialDistance * capsuleAxis; // from sphereB to axis of capsuleA + float radialDistance2 = glm::length2(radialAxis); + if (radialDistance2 > totalRadius * totalRadius) { + // sphere is too far from capsule axis + return false; + } + + // closestApproach = point on capsuleA's axis that is closest to sphereB's center + glm::vec3 closestApproach = capsuleA->getPosition() + axialDistance * capsuleAxis; + + if (absAxialDistance > capsuleA->getHalfHeight()) { + // sphere hits capsule on a cap + // --> recompute radialAxis and closestApproach + float sign = (axialDistance > 0.f) ? 1.f : -1.f; + closestApproach = capsuleA->getPosition() + (sign * capsuleA->getHalfHeight()) * capsuleAxis; + radialAxis = closestApproach - sphereB->getPosition(); + radialDistance2 = glm::length2(radialAxis); + } + if (radialDistance2 > EPSILON * EPSILON) { + // normalize the radialAxis + float radialDistance = sqrtf(radialDistance2); + radialAxis /= radialDistance; + // penetration points from A into B + collision._penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B + // contactPoint is on surface of capsuleA + collision._contactPoint = closestApproach - capsuleA->getRadius() * radialAxis; } 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; + // A is on B's axis, so the penetration is undefined... + if (absAxialDistance > capsuleA->getHalfHeight()) { + // ...for the cylinder case (for now we pretend the collision doesn't exist) + return false; + } else { + // ... but still defined for the cap case + if (axialDistance < 0.f) { + // we're hitting the start cap, so we negate the capsuleAxis + capsuleAxis *= -1; } - 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; + float sign = (axialDistance > 0.f) ? 1.f : -1.f; + collision._penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis; + // contactPoint is on surface of sphereA + collision._contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis; } } - } - return false; -} - -bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { - // transform sphereA all the way to capsuleB's natural frame - glm::quat rotationB = capsuleB->getRotation(); - glm::vec3 sphereCenter = rotationB * (offsetA + 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& offsetA, 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 offsetB = rotationBA * (-offsetA); - glm::vec3 sphereCenter = rotationA * (offsetB + 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), -offsetA); - return true; - } - return false; -} - -bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { +bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision) { return false; } diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 4cef6f943f..0eaf746595 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -16,50 +16,35 @@ 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 offsetA offset of A (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& offsetA, CollisionInfo& collision); + /// \param shapeA pointer to first shape + /// \param shapeB pointer to second shape + /// \param[out] collision where to store collision details + /// \return true of shapes collide + bool shapeShape(const Shape* shapeA, const Shape* shapeB, 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 offsetA offset of A (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& offsetA, CollisionInfo& collision); + /// \param sphereA pointer to first shape (sphere) + /// \param sphereB pointer to second shape (sphere) + /// \param[out] collision where to store collision details + /// \return true of shapes collide + bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, 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 offsetA offset of A (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& offsetA, CollisionInfo& collision); + /// \param sphereA pointer to first shape (sphere) + /// \param capsuleB pointer to second shape (capsule) + /// \param[out] collision where to store collision details + /// \return true of shapes collide + bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, 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 offsetA offset of A (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& offsetA, CollisionInfo& collision); + /// \param capsuleA pointer to first shape (capsule) + /// \param sphereB pointer to second shape (sphere) + /// \param[out] collision where to store collision details + /// \return true of shapes collide + bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, 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 offsetA offset of A (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& offsetA, CollisionInfo& collision); + /// \param capsuleA pointer to first shape (capsule) + /// \param capsuleB pointer to second shape (capsule) + /// \param[out] collision where to store collision details + /// \return true of shapes collide + bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision); } // namespace ShapeCollider diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index a1028956ee..d720dd2289 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -15,9 +15,12 @@ class SphereShape : public Shape { public: SphereShape() : Shape(Shape::SPHERE_SHAPE) {} - SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE) { + SphereShape(float radius) : Shape(Shape::SPHERE_SHAPE) { + _boundingRadius = radius; + } + + SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE, position) { _boundingRadius = radius; - setPosition(position); } float getRadius() const { return _boundingRadius; } diff --git a/tests/physics/src/CollisionInfoTests.cpp b/tests/physics/src/CollisionInfoTests.cpp index 0355e2d27b..813100944b 100644 --- a/tests/physics/src/CollisionInfoTests.cpp +++ b/tests/physics/src/CollisionInfoTests.cpp @@ -18,6 +18,7 @@ #include "PhysicsTestUtil.h" +/* void CollisionInfoTests::rotateThenTranslate() { CollisionInfo collision; collision._penetration = xAxis; @@ -95,8 +96,9 @@ void CollisionInfoTests::translateThenRotate() { << std::endl; } } +*/ void CollisionInfoTests::runAllTests() { - CollisionInfoTests::rotateThenTranslate(); - CollisionInfoTests::translateThenRotate(); +// CollisionInfoTests::rotateThenTranslate(); +// CollisionInfoTests::translateThenRotate(); } diff --git a/tests/physics/src/CollisionInfoTests.h b/tests/physics/src/CollisionInfoTests.h index cc0ef337a2..51579e8f11 100644 --- a/tests/physics/src/CollisionInfoTests.h +++ b/tests/physics/src/CollisionInfoTests.h @@ -11,8 +11,8 @@ namespace CollisionInfoTests { - void rotateThenTranslate(); - void translateThenRotate(); +// void rotateThenTranslate(); +// void translateThenRotate(); void runAllTests(); } diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index b964e2f6de..8380bcf0a7 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -22,33 +22,384 @@ const glm::vec3 origin(0.f); -void ShapeColliderTests::sphereSphere() { +void ShapeColliderTests::sphereMissesSphere() { + // non-overlapping spheres of unequal size + float radiusA = 7.f; + float radiusB = 3.f; + float alpha = 1.2f; + float beta = 1.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + float expectedPenetrationDistance = 0.f; + + SphereShape sphereA(radiusA, origin); + SphereShape sphereB(radiusB, offsetDistance * offsetDirection); CollisionInfo collision; - float radius = 2.f; - SphereShape sphereA(radius, origin); - SphereShape sphereB(radius, origin); - - glm::vec3 translation(0.5f * radius, 0.f, 0.f); - glm::quat rotation; - - // these spheres should be touching - bool touching = ShapeCollider::sphereSphere(&sphereA, &sphereB, rotation, translation, collision); - if (!touching) { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: sphereA and sphereB do not touch" << std::endl; + // collide A to B... + { + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } } - // TODO: verify the collision info is good... - // penetration should point from A into B + // collide B to A... + { + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + // also test shapeShape + { + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } } -/* -void ShapeColliderTests::test2() { +void ShapeColliderTests::sphereTouchesSphere() { + // overlapping spheres of unequal size + float radiusA = 7.f; + float radiusB = 3.f; + float alpha = 0.2f; + float beta = 0.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + float expectedPenetrationDistance = (1.f - alpha) * radiusA + (1.f - beta) * radiusB; + glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection; + + SphereShape sphereA(radiusA, origin); + SphereShape sphereB(radiusB, offsetDistance * offsetDirection); + CollisionInfo collision; + + // collide A to B... + { + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } + + // penetration points from sphereA into sphereB + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 AtoB = sphereB.getPosition() - sphereA.getPosition(); + glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * glm::normalize(AtoB); + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } + + // collide B to A... + { + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } + + // penetration points from sphereA into sphereB + float inaccuracy = glm::length(collision._penetration + expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 BtoA = sphereA.getPosition() - sphereB.getPosition(); + glm::vec3 expectedContactPoint = sphereB.getPosition() + radiusB * glm::normalize(BtoA); + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } +} + +void ShapeColliderTests::sphereMissesCapsule() { + // non-overlapping sphere and capsule + float radiusA = 1.5f; + float radiusB = 2.3f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 1.7f; + float axialOffset = totalRadius + 1.1f * halfHeightB; + float radialOffset = 1.2f * radiusA + 1.3f * radiusB; + + SphereShape sphereA(radiusA); + CapsuleShape capsuleB(radiusB, halfHeightB); + + // give the capsule some arbirary transform + float angle = 37.8; + glm::vec3 axis = glm::normalize( glm::vec3(-7.f, 2.8f, 9.3f) ); + glm::quat rotation = glm::angleAxis(angle, axis); + glm::vec3 translation(15.1f, -27.1f, -38.6f); + capsuleB.setRotation(rotation); + capsuleB.setPosition(translation); + + // walk sphereA along the local yAxis next to, but not touching, capsuleB + glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f); + int numberOfSteps = 10; + float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1); + for (int i = 0; i < numberOfSteps; ++i) { + // translate sphereA into world-frame + glm::vec3 localPosition = localStartPosition + (float(i) * delta) * yAxis; + sphereA.setPosition(rotation * localPosition + translation); + + CollisionInfo collision; + // sphereA agains capsuleB + if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + + // capsuleB against sphereA + if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + } +} + +void ShapeColliderTests::sphereTouchesCapsule() { + // overlapping sphere and capsule + float radiusA = 2.f; + float radiusB = 1.f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 2.f; + float alpha = 0.5f; + float beta = 0.5f; + float axialOffset = 0.f; + float radialOffset = alpha * radiusA + beta * radiusB; + + SphereShape sphereA(radiusA); + CapsuleShape capsuleB(radiusB, halfHeightB); + + CollisionInfo collision; + { // sphereA collides with capsuleB's cylindrical wall + sphereA.setPosition(radialOffset * xAxis); + + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + expectedPenetration = - (radialOffset - totalRadius) * xAxis; + inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of capsuleB + glm::vec3 BtoA = sphereA.getPosition() - capsuleB.getPosition(); + glm::vec3 closestApproach = capsuleB.getPosition() + glm::dot(BtoA, yAxis) * yAxis; + expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } + { // sphereA hits end cap at axis + glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setPosition(axialOffset * yAxis); + + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + glm::vec3 expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of capsuleB + glm::vec3 endPoint; + capsuleB.getEndPoint(endPoint); + expectedContactPoint = endPoint + radiusB * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } + { // sphereA hits start cap at axis + glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setPosition(axialOffset * yAxis); + + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + glm::vec3 expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of capsuleB + glm::vec3 startPoint; + capsuleB.getStartPoint(startPoint); + expectedContactPoint = startPoint - radiusB * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } +} + +void ShapeColliderTests::capsuleMissesCapsule() { + // TODO: implement this +} + +void ShapeColliderTests::capsuleTouchesCapsule() { + // TODO: implement this } -*/ void ShapeColliderTests::runAllTests() { - ShapeColliderTests::sphereSphere(); -// ShapeColliderTests::test2(); + sphereMissesSphere(); + sphereTouchesSphere(); + + sphereMissesCapsule(); + sphereTouchesCapsule(); + + capsuleMissesCapsule(); + capsuleTouchesCapsule(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index a3a19805f8..ecd4a7f045 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -11,8 +11,14 @@ namespace ShapeColliderTests { - void sphereSphere(); - //void test2(); + void sphereMissesSphere(); + void sphereTouchesSphere(); + + void sphereMissesCapsule(); + void sphereTouchesCapsule(); + + void capsuleMissesCapsule(); + void capsuleTouchesCapsule(); void runAllTests(); } diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp index 3ae5a0a6fe..b0a7adde4e 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -3,13 +3,9 @@ // physics-tests // -//#include - #include "ShapeColliderTests.h" -#include "CollisionInfoTests.h" int main(int argc, char** argv) { - CollisionInfoTests::runAllTests(); ShapeColliderTests::runAllTests(); return 0; } From 9a70a50bdbcf0e21c91422182db1587abb6f54f2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 25 Feb 2014 10:59:12 -0800 Subject: [PATCH 06/33] capsuleCapsule() collision with preliminary tests --- libraries/shared/src/ShapeCollider.cpp | 125 ++++++++++++++++++++++- tests/physics/src/ShapeColliderTests.cpp | 121 +++++++++++++++++++++- 2 files changed, 243 insertions(+), 3 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index c5d497694e..de3c7c27b7 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -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; } diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 8380bcf0a7..9ade128b17 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -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(); From 28d9fb97479c2128375d14d79000fbce16b8d935 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 25 Feb 2014 11:17:48 -0800 Subject: [PATCH 07/33] fix missing semicolon --- libraries/shared/src/ShapeCollider.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index de3c7c27b7..897816ce0d 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -270,7 +270,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, if (points[4] > points[2]) { points[5] = points[1]; points[1] = points[2]; - points[2] = points[4] + points[2] = points[4]; } if (points[2] > points[3]) { points[4] = points[2]; From 362708be603e49acdf3cf8f01319b3174d17979d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 25 Feb 2014 11:19:07 -0800 Subject: [PATCH 08/33] fix another missing semicolon --- libraries/shared/src/ShapeCollider.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 897816ce0d..bef31a8e41 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -285,7 +285,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, // 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 + + (capsuleA->getRadius() - distance) * BA; return true; } } From f0e1a6ccd391605e121a14428b2e168040626b8c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 25 Feb 2014 11:38:08 -0800 Subject: [PATCH 09/33] more capsuleCapsule tests --- tests/physics/src/ShapeColliderTests.cpp | 181 ++++++++++++++++++----- 1 file changed, 141 insertions(+), 40 deletions(-) diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 9ade128b17..ba648995c0 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -460,52 +460,153 @@ void ShapeColliderTests::capsuleTouchesCapsule() { 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; + + { // side by side + capsuleB.setPosition((0.99f * totalRadius) * 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; + } } - // 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; + { // 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; + { // 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; + } } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) - { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: capsule and capsule should touch" - << std::endl; + + { // again, but this time check collision details + float overlap = 0.1f; + glm::quat rotation = glm::angleAxis(rightAngle, zAxis); + capsuleB.setRotation(rotation); + glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis; + capsuleB.setPosition(positionB); + + // capsuleA vs capsuleB + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } + + glm::vec3 expectedPenetration = overlap * xAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * xAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + + // capsuleB vs capsuleA + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } + + expectedPenetration = - overlap * xAxis; + inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + expectedContactPoint = capsuleB.getPosition() - (radiusB + halfHeightB) * xAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } + + { // collide cylinder wall against cylinder wall + float overlap = 0.137f; + float shift = 0.317f * halfHeightA; + glm::quat rotation = glm::angleAxis(rightAngle, zAxis); + capsuleB.setRotation(rotation); + glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis; + capsuleB.setPosition(positionB); + + // capsuleA vs capsuleB + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } + + glm::vec3 expectedPenetration = overlap * zAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * zAxis + shift * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } } } From da0276ac7e1b31be06b91e8e9ec4af29cfa0a56a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 26 Feb 2014 12:10:16 -0800 Subject: [PATCH 10/33] ShapeCollider takes CollisionList argument Also, adding first pass at ListShape --- libraries/shared/src/CollisionInfo.cpp | 2 +- libraries/shared/src/CollisionInfo.h | 4 +- libraries/shared/src/ListShape.cpp | 81 ++++++++ libraries/shared/src/ListShape.h | 65 ++++++ libraries/shared/src/Shape.h | 4 +- libraries/shared/src/ShapeCollider.cpp | 83 +++++--- libraries/shared/src/ShapeCollider.h | 10 +- tests/physics/src/PhysicsTestUtil.cpp | 16 ++ tests/physics/src/PhysicsTestUtil.h | 2 + tests/physics/src/ShapeColliderTests.cpp | 241 +++++++++++++++-------- 10 files changed, 395 insertions(+), 113 deletions(-) create mode 100644 libraries/shared/src/ListShape.cpp create mode 100644 libraries/shared/src/ListShape.h diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index 9e76ca59f6..e54ff12f47 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -17,7 +17,7 @@ CollisionList::CollisionList(int maxSize) : CollisionInfo* CollisionList::getNewCollision() { // return pointer to existing CollisionInfo, or NULL of list is full - return (_size < _maxSize) ? &(_collisions[++_size]) : NULL; + return (_size < _maxSize) ? &(_collisions[_size++]) : NULL; } CollisionInfo* CollisionList::getCollision(int index) { diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index 629cb6b8f2..da89870226 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -87,8 +87,8 @@ public: void clear(); private: - int _maxSize; - int _size; + int _maxSize; // the container cannot get larger than this + int _size; // the current number of valid collisions in the list QVector _collisions; }; diff --git a/libraries/shared/src/ListShape.cpp b/libraries/shared/src/ListShape.cpp new file mode 100644 index 0000000000..60fd964ee4 --- /dev/null +++ b/libraries/shared/src/ListShape.cpp @@ -0,0 +1,81 @@ +// +// ListShape.cpp +// +// ListShape: A collection of shapes, each with a local transform. +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#include "ListShape.h" + +// ListShapeEntry + +void ListShapeEntry::updateTransform(const glm::vec3& rootPosition, const glm::quat& rootRotation) { + _shape->setPosition(rootPosition + rootRotation * _localPosition); + _shape->setRotation(_localRotation * rootRotation); +} + +// ListShape + +ListShape::~ListShape() { + clear(); +} + +void ListShape::setPosition(const glm::vec3& position) { + _subShapeTransformsAreDirty = true; + Shape::setPosition(position); +} + +void ListShape::setRotation(const glm::quat& rotation) { + _subShapeTransformsAreDirty = true; + Shape::setRotation(rotation); +} + +void ListShape::updateSubTransforms() { + if (_subShapeTransformsAreDirty) { + for (int i = 0; i < _subShapeEntries.size(); ++i) { + _subShapeEntries[i].updateTransform(_position, _rotation); + } + _subShapeTransformsAreDirty = false; + } +} + +void ListShape::addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation) { + if (shape) { + ListShapeEntry entry; + entry._shape = shape; + entry._localPosition = localPosition; + entry._localRotation = localRotation; + _subShapeEntries.push_back(entry); + } +} + +void ListShape::setShapes(QVector& shapes) { + clear(); + _subShapeEntries.swap(shapes); + // TODO: audit our new list of entries and delete any that have null pointers + computeBoundingRadius(); +} + +void ListShape::clear() { + // the ListShape owns its subShapes, so they need to be deleted + for (int i = 0; i < _subShapeEntries.size(); ++i) { + delete _subShapeEntries[i]._shape; + } + _subShapeEntries.clear(); + setBoundingRadius(0.f); +} + +void ListShape::computeBoundingRadius() { + float maxRadius = 0.f; + for (int i = 0; i < _subShapeEntries.size(); ++i) { + ListShapeEntry& entry = _subShapeEntries[i]; + float radius = glm::length(entry._localPosition) + entry._shape->getBoundingRadius(); + if (radius > maxRadius) { + maxRadius = radius; + } + } + setBoundingRadius(maxRadius); +} + diff --git a/libraries/shared/src/ListShape.h b/libraries/shared/src/ListShape.h new file mode 100644 index 0000000000..fe3726c2d1 --- /dev/null +++ b/libraries/shared/src/ListShape.h @@ -0,0 +1,65 @@ +// +// ListShape.h +// +// ListShape: A collection of shapes, each with a local transform. +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __hifi__ListShape__ +#define __hifi__ListShape__ + +#include + +#include +#include +#include + +#include "Shape.h" + + +class ListShapeEntry { +public: + void updateTransform(const glm::vec3& position, const glm::quat& rotation); + + Shape* _shape; + glm::vec3 _localPosition; + glm::quat _localRotation; +}; + +class ListShape : public Shape { +public: + + ListShape() : Shape(LIST_SHAPE), _subShapeEntries(), _subShapeTransformsAreDirty(false) {} + + ListShape(const glm::vec3& position, const glm::quat& rotation) : + Shape(LIST_SHAPE, position, rotation), _subShapeEntries(), _subShapeTransformsAreDirty(false) {} + + ~ListShape(); + + void setPosition(const glm::vec3& position); + void setRotation(const glm::quat& rotation); + + void updateSubTransforms(); + + int size() { return _subShapeEntries.size(); } + + void addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation); + + void setShapes(QVector& shapes); + + //const QVector& getSubShapes() { return _subShapeEntries; } + +protected: + void clear(); + void computeBoundingRadius(); + + QVector _subShapeEntries; + bool _subShapeTransformsAreDirty; + +private: + ListShape(const ListShape& otherList); // don't implement this +}; + +#endif // __hifi__ListShape__ diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 5273c69007..5bb94d32ff 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -29,8 +29,8 @@ public: 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; } + virtual void setPosition(const glm::vec3& position) { _position = position; } + virtual void setRotation(const glm::quat& rotation) { _rotation = rotation; } protected: // these ctors are protected (used by derived classes only) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index bef31a8e41..83b3582395 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -14,28 +14,28 @@ namespace ShapeCollider { -bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision) { +bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { // 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), collision); + return sphereSphere(sphereA, static_cast(shapeB), collisions); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return sphereCapsule(sphereA, static_cast(shapeB), collision); + return sphereCapsule(sphereA, static_cast(shapeB), collisions); } } 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), collision); + return capsuleSphere(capsuleA, static_cast(shapeB), collisions); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return capsuleCapsule(capsuleA, static_cast(shapeB), collision); + return capsuleCapsule(capsuleA, static_cast(shapeB), collisions); } } return false; } -bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision) { +bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions) { glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition(); float distanceSquared = glm::dot(BA, BA); float totalRadius = sphereA->getRadius() + sphereB->getRadius(); @@ -50,15 +50,18 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis BA /= distance; } // penetration points from A into B - collision._penetration = BA * (totalRadius - distance); - // contactPoint is on surface of A - collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA; - return true; + CollisionInfo* collision = collisions.getNewCollision(); + if (collision) { + collision->_penetration = BA * (totalRadius - distance); + // contactPoint is on surface of A + collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA; + return true; + } } return false; } -bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision) { +bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions) { // find sphereA's closest approach to axis of capsuleB glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition(); glm::vec3 capsuleAxis; @@ -80,19 +83,29 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col radialDistance2 = glm::length2(radialAxis); } if (radialDistance2 > EPSILON * EPSILON) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // normalize the radialAxis float radialDistance = sqrtf(radialDistance2); radialAxis /= radialDistance; // penetration points from A into B - collision._penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B + collision->_penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B // contactPoint is on surface of sphereA - collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis; + collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis; } else { // A is on B's axis, so the penetration is undefined... if (absAxialDistance > capsuleB->getHalfHeight()) { // ...for the cylinder case (for now we pretend the collision doesn't exist) return false; } + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // ... but still defined for the cap case if (axialDistance < 0.f) { // we're hitting the start cap, so we negate the capsuleAxis @@ -100,16 +113,16 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col } // penetration points from A into B float sign = (axialDistance > 0.f) ? -1.f : 1.f; - collision._penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; + collision->_penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; // contactPoint is on surface of sphereA - collision._contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis; + collision->_contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis; } return true; } return false; } -bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision) { +bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions) { // find sphereB's closest approach to axis of capsuleA glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition(); glm::vec3 capsuleAxis; @@ -137,28 +150,38 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col radialDistance2 = glm::length2(radialAxis); } if (radialDistance2 > EPSILON * EPSILON) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // normalize the radialAxis float radialDistance = sqrtf(radialDistance2); radialAxis /= radialDistance; // penetration points from A into B - collision._penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B + collision->_penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B // contactPoint is on surface of capsuleA - collision._contactPoint = closestApproach - capsuleA->getRadius() * radialAxis; + collision->_contactPoint = closestApproach - capsuleA->getRadius() * radialAxis; } else { // A is on B's axis, so the penetration is undefined... if (absAxialDistance > capsuleA->getHalfHeight()) { // ...for the cylinder case (for now we pretend the collision doesn't exist) return false; } else { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // ... but still defined for the cap case if (axialDistance < 0.f) { // we're hitting the start cap, so we negate the capsuleAxis capsuleAxis *= -1; } float sign = (axialDistance > 0.f) ? 1.f : -1.f; - collision._penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis; + collision->_penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis; // contactPoint is on surface of sphereA - collision._contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis; + collision->_contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis; } } return true; @@ -166,7 +189,7 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col return false; } -bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision) { +bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions) { glm::vec3 axisA; capsuleA->computeNormalizedAxis(axisA); glm::vec3 axisB; @@ -201,6 +224,11 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, glm::vec3 BA = (centerB + distanceB * axisB) - (centerA + distanceA * axisA); float distanceSquared = glm::dot(BA, BA); if (distanceSquared < totalRadius * totalRadius) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // normalize BA float distance = sqrtf(distanceSquared); if (distance < EPSILON) { @@ -222,9 +250,9 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, BA /= distance; } // penetration points from A into B - collision._penetration = BA * (totalRadius - distance); + collision->_penetration = BA * (totalRadius - distance); // contactPoint is on surface of A - collision._contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA; + collision->_contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA; return true; } } else { @@ -238,6 +266,11 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, BA = BA - axialDistance * axisB; // BA now points from centerA to axisB (perp to axis) float distanceSquared = glm::length2(BA); if (distanceSquared < totalRadius * totalRadius) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // We have all the info we need to compute the penetration vector... // normalize BA float distance = sqrtf(distanceSquared); @@ -248,7 +281,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, BA /= distance; } // penetration points from A into B - collision._penetration = BA * (totalRadius - distance); + collision->_penetration = BA * (totalRadius - distance); // However we need some more world-frame info to compute the contactPoint, // which is on the surface of capsuleA... @@ -284,7 +317,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, } // average the internal pair, and then do the math from centerB - collision._contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB + collision->_contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB + (capsuleA->getRadius() - distance) * BA; return true; } diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 0eaf746595..2be804e207 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -20,31 +20,31 @@ namespace ShapeCollider { /// \param shapeB pointer to second shape /// \param[out] collision where to store collision details /// \return true of shapes collide - bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision); + bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); /// \param sphereA pointer to first shape (sphere) /// \param sphereB pointer to second shape (sphere) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision); + bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions); /// \param sphereA pointer to first shape (sphere) /// \param capsuleB pointer to second shape (capsule) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision); + bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions); /// \param capsuleA pointer to first shape (capsule) /// \param sphereB pointer to second shape (sphere) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision); + bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions); /// \param capsuleA pointer to first shape (capsule) /// \param capsuleB pointer to second shape (capsule) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision); + bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions); } // namespace ShapeCollider diff --git a/tests/physics/src/PhysicsTestUtil.cpp b/tests/physics/src/PhysicsTestUtil.cpp index f14739c07f..fb940d2043 100644 --- a/tests/physics/src/PhysicsTestUtil.cpp +++ b/tests/physics/src/PhysicsTestUtil.cpp @@ -6,6 +6,8 @@ // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. // +#include + #include "PhysicsTestUtil.h" std::ostream& operator<<(std::ostream& s, const glm::vec3& v) { @@ -13,6 +15,20 @@ std::ostream& operator<<(std::ostream& s, const glm::vec3& v) { return s; } +std::ostream& operator<<(std::ostream& s, const glm::quat& q) { + s << "<" << q.x << "," << q.y << "," << q.z << "," << q.w << ">"; + return s; +} + +std::ostream& operator<<(std::ostream& s, const glm::mat4& m) { + s << "["; + for (int j = 0; j < 4; ++j) { + s << " " << m[0][j] << " " << m[1][j] << " " << m[2][j] << " " << m[3][j] << ";"; + } + s << " ]"; + return s; +} + std::ostream& operator<<(std::ostream& s, const CollisionInfo& c) { s << "[penetration=" << c._penetration << ", contactPoint=" << c._contactPoint diff --git a/tests/physics/src/PhysicsTestUtil.h b/tests/physics/src/PhysicsTestUtil.h index f63e7e5910..dcbeaed346 100644 --- a/tests/physics/src/PhysicsTestUtil.h +++ b/tests/physics/src/PhysicsTestUtil.h @@ -21,6 +21,8 @@ const glm::vec3 zAxis(0.f, 0.f, 1.f); const float rightAngle = 90.f; // degrees std::ostream& operator<<(std::ostream& s, const glm::vec3& v); +std::ostream& operator<<(std::ostream& s, const glm::quat& q); +std::ostream& operator<<(std::ostream& s, const glm::mat4& m); std::ostream& operator<<(std::ostream& s, const CollisionInfo& c); #endif // __tests__PhysicsTestUtil__ diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index ba648995c0..d5cb74dd87 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -34,11 +34,11 @@ void ShapeColliderTests::sphereMissesSphere() { SphereShape sphereA(radiusA, origin); SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionInfo collision; + CollisionList collisions(16); // collide A to B... { - bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; @@ -47,7 +47,7 @@ void ShapeColliderTests::sphereMissesSphere() { // collide B to A... { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; @@ -56,12 +56,18 @@ void ShapeColliderTests::sphereMissesSphere() { // also test shapeShape { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; } } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::sphereTouchesSphere() { @@ -77,62 +83,80 @@ void ShapeColliderTests::sphereTouchesSphere() { SphereShape sphereA(radiusA, origin); SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionInfo collision; + CollisionList collisions(16); + int numCollisions = 0; // collide A to B... { - bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions); if (!touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; + } + + // verify state of collisions + if (numCollisions != collisions.size()) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected collisions size of " << numCollisions << " but actual size is " << collisions.size() + << std::endl; + } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision" << std::endl; } // penetration points from sphereA into sphereB - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 AtoB = sphereB.getPosition() - sphereA.getPosition(); glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * glm::normalize(AtoB); - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } // collide B to A... { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); if (!touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into sphereB - float inaccuracy = glm::length(collision._penetration + expectedPenetration); + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + float inaccuracy = glm::length(collision->_penetration + expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 BtoA = sphereA.getPosition() - sphereB.getPosition(); glm::vec3 expectedContactPoint = sphereB.getPosition() + radiusB * glm::normalize(BtoA); - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -158,6 +182,8 @@ void ShapeColliderTests::sphereMissesCapsule() { capsuleB.setRotation(rotation); capsuleB.setPosition(translation); + CollisionList collisions(16); + // walk sphereA along the local yAxis next to, but not touching, capsuleB glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f); int numberOfSteps = 10; @@ -167,9 +193,8 @@ void ShapeColliderTests::sphereMissesCapsule() { glm::vec3 localPosition = localStartPosition + (float(i) * delta) * yAxis; sphereA.setPosition(rotation * localPosition + translation); - CollisionInfo collision; // sphereA agains capsuleB - if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should NOT touch" @@ -177,13 +202,19 @@ void ShapeColliderTests::sphereMissesCapsule() { } // capsuleB against sphereA - if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should NOT touch" << std::endl; } } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::sphereTouchesCapsule() { @@ -200,52 +231,60 @@ void ShapeColliderTests::sphereTouchesCapsule() { SphereShape sphereA(radiusA); CapsuleShape capsuleB(radiusB, halfHeightB); - CollisionInfo collision; + CollisionList collisions(16); + int numCollisions = 0; + { // sphereA collides with capsuleB's cylindrical wall sphereA.setPosition(radialOffset * xAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = - (radialOffset - totalRadius) * xAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } @@ -253,11 +292,11 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 BtoA = sphereA.getPosition() - capsuleB.getPosition(); glm::vec3 closestApproach = capsuleB.getPosition() + glm::dot(BtoA, yAxis) * yAxis; expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -265,48 +304,54 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; sphereA.setPosition(axialOffset * yAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } @@ -314,11 +359,11 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 endPoint; capsuleB.getEndPoint(endPoint); expectedContactPoint = endPoint + radiusB * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -326,48 +371,54 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; sphereA.setPosition(axialOffset * yAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } @@ -375,14 +426,19 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 startPoint; capsuleB.getStartPoint(startPoint); expectedContactPoint = startPoint - radiusB * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } + if (collisions.size() != numCollisions) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::capsuleMissesCapsule() { @@ -398,16 +454,17 @@ void ShapeColliderTests::capsuleMissesCapsule() { CapsuleShape capsuleA(radiusA, halfHeightA); CapsuleShape capsuleB(radiusA, halfHeightA); + CollisionList collisions(16); + // side by side capsuleB.setPosition((1.01f * totalRadius) * xAxis); - CollisionInfo collision; - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" @@ -416,13 +473,13 @@ void ShapeColliderTests::capsuleMissesCapsule() { // end to end capsuleB.setPosition((1.01f * totalHalfLength) * xAxis); - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" @@ -433,18 +490,24 @@ void ShapeColliderTests::capsuleMissesCapsule() { glm::quat rotation = glm::angleAxis(rightAngle, zAxis); capsuleB.setRotation(rotation); capsuleB.setPosition((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::capsuleTouchesCapsule() { @@ -460,38 +523,47 @@ void ShapeColliderTests::capsuleTouchesCapsule() { CapsuleShape capsuleA(radiusA, halfHeightA); CapsuleShape capsuleB(radiusB, halfHeightB); - CollisionInfo collision; + CollisionList collisions(16); + int numCollisions = 0; { // side by side capsuleB.setPosition((0.99f * totalRadius) * xAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } } { // end to end capsuleB.setPosition((0.99f * totalHalfLength) * yAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } } @@ -500,17 +572,21 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setRotation(rotation); capsuleB.setPosition((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } } @@ -522,54 +598,60 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setPosition(positionB); // capsuleA vs capsuleB - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = overlap * xAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * xAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB vs capsuleA - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = - overlap * xAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } expectedContactPoint = capsuleB.getPosition() - (radiusB + halfHeightB) * xAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -583,28 +665,31 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setPosition(positionB); // capsuleA vs capsuleB - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = overlap * zAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * zAxis + shift * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } From 07eb3661389bdf2fd45c03c3bb9cd963a170ad56 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 26 Feb 2014 14:32:46 -0800 Subject: [PATCH 11/33] stubbed out collision handlers for ListShape --- libraries/shared/src/ShapeCollider.cpp | 33 ++++++++++++++++++---- libraries/shared/src/ShapeCollider.h | 39 +++++++++++++++++++------- 2 files changed, 56 insertions(+), 16 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 83b3582395..57ea13a2aa 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -17,20 +17,29 @@ namespace ShapeCollider { bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { // 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) { + int typeA = shapeA->getType(); + int typeB = shapeB->getType(); + if (typeA == Shape::SPHERE_SHAPE) { const SphereShape* sphereA = static_cast(shapeA); - if (shapeB->getType() == Shape::SPHERE_SHAPE) { + if (typeB == Shape::SPHERE_SHAPE) { return sphereSphere(sphereA, static_cast(shapeB), collisions); - } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { + } else if (typeB == Shape::CAPSULE_SHAPE) { return sphereCapsule(sphereA, static_cast(shapeB), collisions); } - } else if (shapeA->getType() == Shape::CAPSULE_SHAPE) { + } else if (typeA == Shape::CAPSULE_SHAPE) { const CapsuleShape* capsuleA = static_cast(shapeA); - if (shapeB->getType() == Shape::SPHERE_SHAPE) { + if (typeB == Shape::SPHERE_SHAPE) { return capsuleSphere(capsuleA, static_cast(shapeB), collisions); - } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { + } else if (typeB == Shape::CAPSULE_SHAPE) { return capsuleCapsule(capsuleA, static_cast(shapeB), collisions); } + } else if (typeA == Shape::LIST_SHAPE) { + const ListShape* listA = static_cast(shapeA); + if (typeB == Shape::SPHERE_SHAPE) { + return listSphere(listA, static_cast(shapeB), collisions); + } else if (typeB == Shape::CAPSULE_SHAPE) { + return listCapsule(listA, static_cast(shapeB), collisions); + } } return false; } @@ -325,4 +334,16 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, return false; } +bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions) { + return false; +} + +bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions) { + return false; +} + +bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions) { + return false; +} + } // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 2be804e207..ea73c783eb 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -11,6 +11,7 @@ #include "CapsuleShape.h" #include "CollisionInfo.h" +#include "ListShape.h" #include "SharedUtil.h" #include "SphereShape.h" @@ -18,34 +19,52 @@ namespace ShapeCollider { /// \param shapeA pointer to first shape /// \param shapeB pointer to second shape - /// \param[out] collision where to store collision details - /// \return true of shapes collide + /// \param[out] collisions where to append collision details + /// \return true if shapes collide bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); /// \param sphereA pointer to first shape (sphere) /// \param sphereB pointer to second shape (sphere) - /// \param[out] collision where to store collision details - /// \return true of shapes collide + /// \param[out] collisions where to append collision details + /// \return true if shapes collide bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions); /// \param sphereA pointer to first shape (sphere) /// \param capsuleB pointer to second shape (capsule) - /// \param[out] collision where to store collision details - /// \return true of shapes collide + /// \param[out] collisions where to append collision details + /// \return true if shapes collide bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions); /// \param capsuleA pointer to first shape (capsule) /// \param sphereB pointer to second shape (sphere) - /// \param[out] collision where to store collision details - /// \return true of shapes collide + /// \param[out] collisions where to append collision details + /// \return true if shapes collide bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions); /// \param capsuleA pointer to first shape (capsule) /// \param capsuleB pointer to second shape (capsule) - /// \param[out] collision where to store collision details - /// \return true of shapes collide + /// \param[out] collisions where to append collision details + /// \return true if shapes collide bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions); + /// \param listA pointer to first shape (list) + /// \param sphereB pointer to second shape (sphere) + /// \param[out] collisions where to append collision details + /// \return true if shapes collide + bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions); + + /// \param listA pointer to first shape (list) + /// \param capsuleB pointer to second shape (capsule) + /// \param[out] collisions where to append collision details + /// \return true if shapes collide + bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions); + + /// \param listA pointer to first shape (list) + /// \param capsuleB pointer to second shape (capsule) + /// \param[out] collisions where to append collision details + /// \return true if shapes collide + bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions); + } // namespace ShapeCollider #endif // __hifi__ShapeCollider__ From 5954bb91d40a8a063ec1c0e44bc815b8eec10e31 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 26 Feb 2014 15:36:19 -0800 Subject: [PATCH 12/33] ListShape now handled by ShapeCollider --- interface/src/avatar/Avatar.cpp | 7 ++- libraries/shared/src/CollisionInfo.h | 3 ++ libraries/shared/src/ListShape.cpp | 7 +++ libraries/shared/src/ListShape.h | 6 +-- libraries/shared/src/ShapeCollider.cpp | 66 ++++++++++++++++++++++++-- libraries/shared/src/ShapeCollider.h | 40 ++++++++++------ 6 files changed, 105 insertions(+), 24 deletions(-) diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index 9ff1ec8cb1..adb8215849 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -396,10 +396,9 @@ bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direc bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, CollisionList& collisions, int skeletonSkipIndex) { - // Temporarily disabling collisions against the skeleton because the collision proxies up - // near the neck are bad and prevent the hand from hitting the face. - //return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions, 1.0f, skeletonSkipIndex); - return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions); + return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions, 1.0f, skeletonSkipIndex); + // Temporarily disabling collisions against the head because most of its collision proxies are bad. + //return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions); } bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) { diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index da89870226..2de13e30d1 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -80,6 +80,9 @@ public: /// \return pointer to collision by index. NULL if index out of bounds. CollisionInfo* getCollision(int index); + /// \return true if list is full + bool isFull() const { return _size == _maxSize; } + /// \return number of valid collisions int size() const { return _size; } diff --git a/libraries/shared/src/ListShape.cpp b/libraries/shared/src/ListShape.cpp index 60fd964ee4..593304c75a 100644 --- a/libraries/shared/src/ListShape.cpp +++ b/libraries/shared/src/ListShape.cpp @@ -32,6 +32,13 @@ void ListShape::setRotation(const glm::quat& rotation) { Shape::setRotation(rotation); } +const Shape* ListShape::getSubShape(int index) const { + if (index < 0 || index > _subShapeEntries.size()) { + return NULL; + } + return _subShapeEntries[index]._shape; +} + void ListShape::updateSubTransforms() { if (_subShapeTransformsAreDirty) { for (int i = 0; i < _subShapeEntries.size(); ++i) { diff --git a/libraries/shared/src/ListShape.h b/libraries/shared/src/ListShape.h index fe3726c2d1..d6005ddfb9 100644 --- a/libraries/shared/src/ListShape.h +++ b/libraries/shared/src/ListShape.h @@ -41,16 +41,16 @@ public: void setPosition(const glm::vec3& position); void setRotation(const glm::quat& rotation); + const Shape* getSubShape(int index) const; + void updateSubTransforms(); - int size() { return _subShapeEntries.size(); } + int size() const { return _subShapeEntries.size(); } void addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation); void setShapes(QVector& shapes); - //const QVector& getSubShapes() { return _subShapeEntries; } - protected: void clear(); void computeBoundingRadius(); diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 57ea13a2aa..80d7c9e08a 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -12,6 +12,11 @@ #include "ShapeCollider.h" +// NOTE: +// +// * Large ListShape's are inefficient keep the lists short. +// * Collisions between lists of lists do not work. + namespace ShapeCollider { bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { @@ -334,16 +339,71 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, return false; } +bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions) { + bool touching = false; + for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listB->getSubShape(i); + int subType = subShape->getType(); + if (subType == Shape::SPHERE_SHAPE) { + touching = sphereSphere(sphereA, static_cast(subShape), collisions) || touching; + } else if (subType == Shape::CAPSULE_SHAPE) { + touching = sphereCapsule(sphereA, static_cast(subShape), collisions) || touching; + } + } + return touching; +} + +bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions) { + bool touching = false; + for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listB->getSubShape(i); + int subType = subShape->getType(); + if (subType == Shape::SPHERE_SHAPE) { + touching = capsuleSphere(capsuleA, static_cast(subShape), collisions) || touching; + } else if (subType == Shape::CAPSULE_SHAPE) { + touching = capsuleCapsule(capsuleA, static_cast(subShape), collisions) || touching; + } + } + return touching; +} + bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions) { - return false; + bool touching = false; + for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listA->getSubShape(i); + int subType = subShape->getType(); + if (subType == Shape::SPHERE_SHAPE) { + touching = sphereSphere(static_cast(subShape), sphereB, collisions) || touching; + } else if (subType == Shape::CAPSULE_SHAPE) { + touching = capsuleSphere(static_cast(subShape), sphereB, collisions) || touching; + } + } + return touching; } bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions) { - return false; + bool touching = false; + for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listA->getSubShape(i); + int subType = subShape->getType(); + if (subType == Shape::SPHERE_SHAPE) { + touching = sphereCapsule(static_cast(subShape), capsuleB, collisions) || touching; + } else if (subType == Shape::CAPSULE_SHAPE) { + touching = capsuleCapsule(static_cast(subShape), capsuleB, collisions) || touching; + } + } + return touching; } bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions) { - return false; + bool touching = false; + for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) { + const Shape* subShape = listA->getSubShape(i); + for (int j = 0; j < listB->size() && !collisions.isFull(); ++j) { + touching = shapeShape(subShape, listB->getSubShape(j), collisions) || touching; + } + } + return touching; } } // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index ea73c783eb..e3e044c8fe 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -23,44 +23,56 @@ namespace ShapeCollider { /// \return true if shapes collide bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); - /// \param sphereA pointer to first shape (sphere) - /// \param sphereB pointer to second shape (sphere) + /// \param sphereA pointer to first shape + /// \param sphereB pointer to second shape /// \param[out] collisions where to append collision details /// \return true if shapes collide bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions); - /// \param sphereA pointer to first shape (sphere) - /// \param capsuleB pointer to second shape (capsule) + /// \param sphereA pointer to first shape + /// \param capsuleB pointer to second shape /// \param[out] collisions where to append collision details /// \return true if shapes collide bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions); - /// \param capsuleA pointer to first shape (capsule) - /// \param sphereB pointer to second shape (sphere) + /// \param capsuleA pointer to first shape + /// \param sphereB pointer to second shape /// \param[out] collisions where to append collision details /// \return true if shapes collide bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions); - /// \param capsuleA pointer to first shape (capsule) - /// \param capsuleB pointer to second shape (capsule) + /// \param capsuleA pointer to first shape + /// \param capsuleB pointer to second shape /// \param[out] collisions where to append collision details /// \return true if shapes collide bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions); - /// \param listA pointer to first shape (list) - /// \param sphereB pointer to second shape (sphere) + /// \param sphereA pointer to first shape + /// \param listB pointer to second shape + /// \param[out] collisions where to append collision details + /// \return true if shapes collide + bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions); + + /// \param capuleA pointer to first shape + /// \param listB pointer to second shape + /// \param[out] collisions where to append collision details + /// \return true if shapes collide + bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions); + + /// \param listA pointer to first shape + /// \param sphereB pointer to second shape /// \param[out] collisions where to append collision details /// \return true if shapes collide bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions); - /// \param listA pointer to first shape (list) - /// \param capsuleB pointer to second shape (capsule) + /// \param listA pointer to first shape + /// \param capsuleB pointer to second shape /// \param[out] collisions where to append collision details /// \return true if shapes collide bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions); - /// \param listA pointer to first shape (list) - /// \param capsuleB pointer to second shape (capsule) + /// \param listA pointer to first shape + /// \param capsuleB pointer to second shape /// \param[out] collisions where to append collision details /// \return true if shapes collide bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions); From 25bdfd76777a189d7de2efc9251cb868104394f5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 26 Feb 2014 15:38:33 -0800 Subject: [PATCH 13/33] Update to comment. --- libraries/shared/src/ShapeCollider.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 80d7c9e08a..9c14a5d033 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -15,7 +15,7 @@ // NOTE: // // * Large ListShape's are inefficient keep the lists short. -// * Collisions between lists of lists do not work. +// * Collisions between lists of lists work in theory but are not recommended. namespace ShapeCollider { From 9dc26ddfa755f392ab0c8808e6e6066d2577f993 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 27 Feb 2014 15:50:16 -0800 Subject: [PATCH 14/33] First pass: add collision Shapes to Model --- interface/src/renderer/Model.cpp | 121 ++++++++++++++++++++----------- interface/src/renderer/Model.h | 7 ++ 2 files changed, 85 insertions(+), 43 deletions(-) diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index f1916db4d1..647332998e 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -13,6 +13,9 @@ #include "Application.h" #include "Model.h" +#include +#include + using namespace std; Model::Model(QObject* parent) : @@ -89,6 +92,67 @@ void Model::reset() { } } +void Model::createJointStates() { + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + foreach (const FBXJoint& joint, geometry.joints) { + JointState state; + state.translation = joint.translation; + state.rotation = joint.rotation; + _jointStates.append(state); + } + foreach (const FBXMesh& mesh, geometry.meshes) { + MeshState state; + state.clusterMatrices.resize(mesh.clusters.size()); + if (mesh.springiness > 0.0f) { + state.worldSpaceVertices.resize(mesh.vertices.size()); + state.vertexVelocities.resize(mesh.vertices.size()); + state.worldSpaceNormals.resize(mesh.vertices.size()); + } + _meshStates.append(state); + } + foreach (const FBXAttachment& attachment, geometry.attachments) { + Model* model = new Model(this); + model->init(); + model->setURL(attachment.url); + _attachments.append(model); + } + _resetStates = true; + + for (int i = 0; i < _jointStates.size(); i++) { + updateJointState(i); + } + createCollisionShapes(); +} + +void Model::clearShapes() { + for (int i = 0; i < _shapes.size(); ++i) { + delete _shapes[i]; + } + _shapes.clear(); +} + +void Model::createCollisionShapes() { + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + float uniformScale = extractUniformScale(_scale); + for (int i = 0; i < _jointStates.size(); i++) { + glm::vec3 position = extractTranslation(_jointStates[i].transform); + const FBXJoint& joint = geometry.joints[i]; + // for now make everything a sphere at joint end + float radius = uniformScale * joint.boneRadius; + SphereShape* shape = new SphereShape(radius, position); + _shapes.push_back(shape); + } +} + +void Model::updateShapePositions() { + if (_shapes.size() == _jointStates.size()) { + for (int i = 0; i < _jointStates.size(); i++) { + _shapes[i]->setPosition(extractTranslation(_jointStates[i].transform)); + _shapes[i]->setRotation(_jointStates[i].combinedRotation); + } + } +} + void Model::simulate(float deltaTime) { // update our LOD if (_geometry) { @@ -105,39 +169,17 @@ void Model::simulate(float deltaTime) { } // set up world vertices on first simulate after load - const FBXGeometry& geometry = _geometry->getFBXGeometry(); if (_jointStates.isEmpty()) { - foreach (const FBXJoint& joint, geometry.joints) { - JointState state; - state.translation = joint.translation; - state.rotation = joint.rotation; - _jointStates.append(state); + createJointStates(); + } else { + // update the world space transforms for all joints + for (int i = 0; i < _jointStates.size(); i++) { + updateJointState(i); } - foreach (const FBXMesh& mesh, geometry.meshes) { - MeshState state; - state.clusterMatrices.resize(mesh.clusters.size()); - if (mesh.springiness > 0.0f) { - state.worldSpaceVertices.resize(mesh.vertices.size()); - state.vertexVelocities.resize(mesh.vertices.size()); - state.worldSpaceNormals.resize(mesh.vertices.size()); - } - _meshStates.append(state); - } - foreach (const FBXAttachment& attachment, geometry.attachments) { - Model* model = new Model(this); - model->init(); - model->setURL(attachment.url); - _attachments.append(model); - } - _resetStates = true; - } - - // update the world space transforms for all joints - for (int i = 0; i < _jointStates.size(); i++) { - updateJointState(i); } // update the attachment transforms and simulate them + const FBXGeometry& geometry = _geometry->getFBXGeometry(); for (int i = 0; i < _attachments.size(); i++) { const FBXAttachment& attachment = geometry.attachments.at(i); Model* model = _attachments.at(i); @@ -706,34 +748,26 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons void Model::renderCollisionProxies(float alpha) { glPushMatrix(); Application::getInstance()->loadTranslatedViewMatrix(_translation); - - const FBXGeometry& geometry = _geometry->getFBXGeometry(); + updateShapePositions(); float uniformScale = extractUniformScale(_scale); - for (int i = 0; i < _jointStates.size(); i++) { + for (int i = 0; i < _shapes.size(); i++) { glPushMatrix(); + + Shape* shape = _shapes[i]; - glm::vec3 position = extractTranslation(_jointStates[i].transform); + glm::vec3 position = shape->getPosition(); glTranslatef(position.x, position.y, position.z); - glm::quat rotation; - getJointRotation(i, rotation); + const glm::quat& rotation = shape->getRotation(); glm::vec3 axis = glm::axis(rotation); glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z); glColor4f(0.75f, 0.75f, 0.75f, alpha); - float scaledRadius = geometry.joints[i].boneRadius * uniformScale; const int BALL_SUBDIVISIONS = 10; - glutSolidSphere(scaledRadius, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); glPopMatrix(); - - int parentIndex = geometry.joints[i].parentIndex; - if (parentIndex != -1) { - Avatar::renderJointConnectingCone(extractTranslation(_jointStates[parentIndex].transform), position, - geometry.joints[parentIndex].boneRadius * uniformScale, scaledRadius); - } } - glPopMatrix(); } @@ -796,6 +830,7 @@ void Model::deleteGeometry() { _blendedVertexBufferIDs.clear(); _jointStates.clear(); _meshStates.clear(); + clearShapes(); } void Model::renderMeshes(float alpha, bool translucent) { diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 28189d0379..07fd697479 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -17,6 +17,8 @@ #include "ProgramObject.h" #include "TextureCache.h" +class Shape; + /// A generic 3D model displaying geometry loaded from a URL. class Model : public QObject { Q_OBJECT @@ -48,6 +50,10 @@ public: void init(); void reset(); + void createJointStates(); + void clearShapes(); + void createCollisionShapes(); + void updateShapePositions(); void simulate(float deltaTime); bool render(float alpha); @@ -183,6 +189,7 @@ protected: }; QVector _jointStates; + QVector _shapes; class MeshState { public: From 50a592457481b4f4e3448f604a0a2bbc5f232a96 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 27 Feb 2014 17:33:36 -0800 Subject: [PATCH 15/33] Using shapes for collisions against Model rather than building tapered capsule shapes on the fly --- interface/src/avatar/Avatar.cpp | 2 +- interface/src/avatar/Hand.cpp | 9 +++--- interface/src/renderer/Model.cpp | 50 ++++++++++++-------------------- interface/src/renderer/Model.h | 3 +- 4 files changed, 26 insertions(+), 38 deletions(-) diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index adb8215849..1f08d785a6 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -396,7 +396,7 @@ bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direc bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, CollisionList& collisions, int skeletonSkipIndex) { - return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions, 1.0f, skeletonSkipIndex); + return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions, skeletonSkipIndex); // Temporarily disabling collisions against the head because most of its collision proxies are bad. //return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions); } diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index efbdfa2438..d82aa33cbc 100644 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -215,11 +215,12 @@ void Hand::collideAgainstOurself() { getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex); float scaledPalmRadius = PALM_COLLISION_RADIUS * _owningAvatar->getScale(); + const Model& skeletonModel = _owningAvatar->getSkeletonModel(); for (size_t i = 0; i < getNumPalms(); i++) { PalmData& palm = getPalms()[i]; if (!palm.isActive()) { continue; - } + } const Model& skeletonModel = _owningAvatar->getSkeletonModel(); // ignoring everything below the parent of the parent of the last free joint int skipIndex = skeletonModel.getParentJointIndex(skeletonModel.getParentJointIndex( @@ -227,15 +228,15 @@ void Hand::collideAgainstOurself() { (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1))); handCollisions.clear(); - glm::vec3 totalPenetration; if (_owningAvatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, handCollisions, skipIndex)) { + glm::vec3 totalPenetration; for (int j = 0; j < handCollisions.size(); ++j) { CollisionInfo* collision = handCollisions.getCollision(j); totalPenetration = addPenetrations(totalPenetration, collision->_penetration); } + // resolve penetration + palm.addToPosition(-totalPenetration); } - // resolve penetration - palm.addToPosition(-totalPenetration); } } diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 647332998e..64b078291a 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -15,12 +15,14 @@ #include #include +#include using namespace std; Model::Model(QObject* parent) : QObject(parent), - _pupilDilation(0.0f) + _pupilDilation(0.0f), + _shapesAreDirty(true) { // we may have been created in the network thread, but we live in the main thread moveToThread(Application::getInstance()->thread()); @@ -145,11 +147,13 @@ void Model::createCollisionShapes() { } void Model::updateShapePositions() { - if (_shapes.size() == _jointStates.size()) { + if (_shapesAreDirty && _shapes.size() == _jointStates.size()) { for (int i = 0; i < _jointStates.size(); i++) { - _shapes[i]->setPosition(extractTranslation(_jointStates[i].transform)); + // shape positions are stored in world-frame + _shapes[i]->setPosition(_rotation * extractTranslation(_jointStates[i].transform) + _translation); _shapes[i]->setRotation(_jointStates[i].combinedRotation); } + _shapesAreDirty = false; } } @@ -500,20 +504,14 @@ bool Model::findRayIntersection(const glm::vec3& origin, const glm::vec3& direct return false; } -bool Model::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, - CollisionList& collisions, float boneScale, int skipIndex) const { +bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, + CollisionList& collisions, int skipIndex) { bool collided = false; - const glm::vec3 relativeCenter = penetratorCenter - _translation; + updateShapePositions(); + SphereShape sphere(sphereRadius, sphereCenter); const FBXGeometry& geometry = _geometry->getFBXGeometry(); - glm::vec3 totalPenetration; - float radiusScale = extractUniformScale(_scale) * boneScale; - for (int i = 0; i < _jointStates.size(); i++) { + for (int i = 0; i < _shapes.size(); i++) { const FBXJoint& joint = geometry.joints[i]; - glm::vec3 end = extractTranslation(_jointStates[i].transform); - float endRadius = joint.boneRadius * radiusScale; - glm::vec3 start = end; - float startRadius = joint.boneRadius * radiusScale; - glm::vec3 bonePenetration; if (joint.parentIndex != -1) { if (skipIndex != -1) { int ancestorIndex = joint.parentIndex; @@ -525,24 +523,9 @@ bool Model::findSphereCollisions(const glm::vec3& penetratorCenter, float penetr } while (ancestorIndex != -1); } - start = extractTranslation(_jointStates[joint.parentIndex].transform); - startRadius = geometry.joints[joint.parentIndex].boneRadius * radiusScale; } - if (findSphereCapsuleConePenetration(relativeCenter, penetratorRadius, start, end, - startRadius, endRadius, bonePenetration)) { - totalPenetration = addPenetrations(totalPenetration, bonePenetration); - CollisionInfo* collision = collisions.getNewCollision(); - if (collision) { - collision->_type = MODEL_COLLISION; - collision->_data = (void*)(this); - collision->_flags = i; - collision->_contactPoint = penetratorCenter + penetratorRadius * glm::normalize(totalPenetration); - collision->_penetration = totalPenetration; - collided = true; - } else { - // collisions are full, so we might as well break - break; - } + if (ShapeCollider::shapeShape(&sphere, _shapes[i], collisions)) { + collided = true; } outerContinue: ; } @@ -550,6 +533,7 @@ bool Model::findSphereCollisions(const glm::vec3& penetratorCenter, float penetr } void Model::updateJointState(int index) { + _shapesAreDirty = true; JointState& state = _jointStates[index]; const FBXGeometry& geometry = _geometry->getFBXGeometry(); const FBXJoint& joint = geometry.joints.at(index); @@ -750,12 +734,14 @@ void Model::renderCollisionProxies(float alpha) { Application::getInstance()->loadTranslatedViewMatrix(_translation); updateShapePositions(); float uniformScale = extractUniformScale(_scale); + glm::quat inverseRotation = glm::inverse(_rotation); for (int i = 0; i < _shapes.size(); i++) { glPushMatrix(); Shape* shape = _shapes[i]; - glm::vec3 position = shape->getPosition(); + // shapes are stored in world-frame, so we have to transform into local frame + glm::vec3 position = inverseRotation * (shape->getPosition() - _translation); glTranslatef(position.x, position.y, position.z); const glm::quat& rotation = shape->getRotation(); diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 07fd697479..eff2bb0594 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -159,7 +159,7 @@ public: bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const; bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, - CollisionList& collisions, float boneScale = 1.0f, int skipIndex = -1) const; + CollisionList& collisions, int skipIndex = -1); void renderCollisionProxies(float alpha); @@ -188,6 +188,7 @@ protected: glm::quat combinedRotation; }; + bool _shapesAreDirty; QVector _jointStates; QVector _shapes; From b8c1bab4ae35fa4fcad5fae48c2e7537c7ef60ba Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 27 Feb 2014 17:52:35 -0800 Subject: [PATCH 16/33] Using pre-computed Model shapes for collisions ...instead of generating collision shapes on the fly --- interface/src/renderer/Model.cpp | 4 ++++ libraries/shared/src/CollisionInfo.cpp | 4 ++++ libraries/shared/src/CollisionInfo.h | 3 +++ 3 files changed, 11 insertions(+) diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 64b078291a..47a6013b5f 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -525,6 +525,10 @@ bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadi } } if (ShapeCollider::shapeShape(&sphere, _shapes[i], collisions)) { + CollisionInfo* collision = collisions.getLastCollision(); + collision->_type = MODEL_COLLISION; + collision->_data = (void*)(this); + collision->_flags = i; collided = true; } outerContinue: ; diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index e54ff12f47..f6c0d057a1 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -24,6 +24,10 @@ CollisionInfo* CollisionList::getCollision(int index) { return (index > -1 && index < _size) ? &(_collisions[index]) : NULL; } +CollisionInfo* CollisionList::getLastCollision() { + return (_size > 0) ? &(_collisions[_size - 1]) : NULL; +} + void CollisionList::clear() { for (int i = 0; i < _size; ++i) { // we only clear the important stuff diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index 2de13e30d1..868d259ce3 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -80,6 +80,9 @@ public: /// \return pointer to collision by index. NULL if index out of bounds. CollisionInfo* getCollision(int index); + /// \return pointer to last collision on the list. NULL if list is empty + CollisionInfo* getLastCollision(); + /// \return true if list is full bool isFull() const { return _size == _maxSize; } From 702612506b66fde9223e8bacea85d7ffa1a6b397 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 28 Feb 2014 16:42:12 -0800 Subject: [PATCH 17/33] very basic bounding info for joint meshes --- interface/src/renderer/FBXReader.cpp | 91 ++++++++++++++++++++++------ interface/src/renderer/FBXReader.h | 19 +++--- 2 files changed, 85 insertions(+), 25 deletions(-) diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp index 8b881940ca..0b22adb0a4 100644 --- a/interface/src/renderer/FBXReader.cpp +++ b/interface/src/renderer/FBXReader.cpp @@ -6,6 +6,7 @@ // Copyright (c) 2013 High Fidelity, Inc. All rights reserved. // +#include #include #include #include @@ -26,6 +27,11 @@ #include "FBXReader.h" #include "Util.h" +std::ostream& operator<<(std::ostream& s, const glm::vec3& v) { + s << "<" << v.x << "," << v.y << "," << v.z << ">"; + return s; +} + using namespace std; template QVariant readBinaryArray(QDataStream& in) { @@ -536,8 +542,9 @@ public: FBXBlendshape blendshape; }; -void printNode(const FBXNode& node, int indent) { - QByteArray spaces(indent, ' '); +void printNode(const FBXNode& node, int indentLevel) { + int indentLength = 2; + QByteArray spaces(indentLevel * indentLength, ' '); QDebug nodeDebug = qDebug(); nodeDebug.nospace() << spaces.data() << node.name.data() << ": "; @@ -546,7 +553,7 @@ void printNode(const FBXNode& node, int indent) { } foreach (const FBXNode& child, node.children) { - printNode(child, indent + 1); + printNode(child, indentLevel + 1); } } @@ -841,6 +848,9 @@ QString getString(const QVariant& value) { } FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) { +// std::cout << "adebug beginPrintNode " << std::endl; // adebug +// printNode(node, 0); // adebug +// std::cout << "adebug endPrintNode " << std::endl; // adebug QHash meshes; QVector blendshapes; QMultiHash parentMap; @@ -1248,6 +1258,12 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) } joint.boneRadius = 0.0f; joint.inverseBindRotation = joint.inverseDefaultRotation; + joint.name = model.name; + joint.extents.minimum = glm::vec3(FLT_MAX); + joint.extents.maximum = glm::vec3(-FLT_MAX); + joint.averageVertex = glm::vec3(0.f); + joint.numVertices = 0; + joint.averageRadius = 0.f; geometry.joints.append(joint); geometry.jointIndices.insert(model.name, geometry.joints.size() - 1); } @@ -1272,10 +1288,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) geometry.neckPivot = glm::vec3(transform[3][0], transform[3][1], transform[3][2]); } - geometry.bindExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX); - geometry.bindExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX); - geometry.staticExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX); - geometry.staticExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX); + geometry.bindExtents.minimum = glm::vec3(FLT_MAX); + geometry.bindExtents.maximum = glm::vec3(-FLT_MAX); + geometry.staticExtents.minimum = glm::vec3(FLT_MAX); + geometry.staticExtents.maximum = glm::vec3(-FLT_MAX); QVariantHash springs = mapping.value("spring").toHash(); QVariant defaultSpring = springs.value("default"); @@ -1405,11 +1421,6 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) cluster.jointIndex = 0; } extracted.mesh.clusters.append(cluster); - // BUG: joints that fall into this context do not get their bindTransform and - // inverseBindRotation data members properly set. This causes bad boneRadius - // and boneLength calculations for collision proxies. Affected joints are usually: - // hair, teeth, tongue. I tried to figure out how to fix this but was going - // crosseyed trying to understand FBX so I gave up for the time being -- Andrew. } // whether we're skinned depends on how many clusters are attached @@ -1426,12 +1437,15 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) const FBXCluster& fbxCluster = extracted.mesh.clusters.at(i); int jointIndex = fbxCluster.jointIndex; FBXJoint& joint = geometry.joints[jointIndex]; - glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform); + glm::mat4 transformJointToMesh = inverseModelTransform * joint.bindTransform; + glm::quat rotateMeshToJoint = glm::inverse(extractRotation(transformJointToMesh)); + glm::vec3 boneEnd = extractTranslation(transformJointToMesh); + glm::vec3 boneBegin = boneEnd; glm::vec3 boneDirection; float boneLength; if (joint.parentIndex != -1) { - boneDirection = boneEnd - extractTranslation(inverseModelTransform * - geometry.joints[joint.parentIndex].bindTransform); + boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform); + boneDirection = boneEnd - boneBegin; boneLength = glm::length(boneDirection); if (boneLength > EPSILON) { boneDirection /= boneLength; @@ -1453,9 +1467,13 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) const glm::vec3& vertex = extracted.mesh.vertices.at(it.value()); float proj = glm::dot(boneDirection, vertex - boneEnd); if (proj < 0.0f && proj > -boneLength) { - joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance( - vertex, boneEnd + boneDirection * proj)); + joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(vertex, boneEnd + boneDirection * proj)); } + glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd)); + joint.extents.minimum = glm::min(joint.extents.minimum, vertexInJointFrame); + joint.extents.maximum = glm::max(joint.extents.maximum, vertexInJointFrame); + joint.averageVertex += vertexInJointFrame; + ++joint.numVertices; if (jointIsStatic) { // expand the extents of static (nonmovable) joints geometry.staticExtents.minimum = glm::min(geometry.staticExtents.minimum, vertex + jointTranslation); @@ -1482,7 +1500,11 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) } else { int jointIndex = maxJointIndex; FBXJoint& joint = geometry.joints[jointIndex]; - glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform); + + glm::mat4 transformJointToMesh = inverseModelTransform * joint.bindTransform; + glm::quat rotateMeshToJoint = glm::inverse(extractRotation(transformJointToMesh)); + glm::vec3 boneEnd = extractTranslation(transformJointToMesh); + glm::vec3 boneDirection; float boneLength; if (joint.parentIndex != -1) { @@ -1493,6 +1515,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) boneDirection /= boneLength; } } + float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix); foreach (const glm::vec3& vertex, extracted.mesh.vertices) { float proj = glm::dot(boneDirection, vertex - boneEnd); @@ -1500,6 +1523,22 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance( vertex, boneEnd + boneDirection * proj)); } + glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd)); + joint.extents.minimum = glm::min(joint.extents.minimum, vertexInJointFrame); + joint.extents.maximum = glm::max(joint.extents.maximum, vertexInJointFrame); + joint.averageVertex += vertex; + ++joint.numVertices; + } + if (joint.numVertices > 0) { + joint.averageVertex /= float(joint.numVertices); + float averageRadius = 0.f; + foreach (const glm::vec3& vertex, extracted.mesh.vertices) { + averageRadius += glm::distance(vertex, joint.averageVertex); + } + joint.boneRadius = averageRadius * (radiusScale / float(joint.numVertices)); + //joint.averageVertex /= float(joint.numVertices); + joint.averageVertex = rotateMeshToJoint * (radiusScale * (joint.averageVertex - boneEnd)); + joint.numVertices = 1; } } extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex); @@ -1579,6 +1618,22 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) geometry.attachments.append(attachment); } + for (int i = 0; i < geometry.joints.size(); ++i) { + FBXJoint& joint = geometry.joints[i]; + if (joint.numVertices > 0) { + //joint.averageVertex /= 1.f; + joint.averageVertex /= float(joint.numVertices); + } + /* + std::cout << joint.name.toStdString().c_str() + << " bR = " << joint.boneRadius + << " extents = " << (joint.extents.maximum - joint.extents.minimum) + << " avgV = " << joint.averageVertex + << " numV = " << joint.numVertices + << std::endl; // adebug + */ + } + return geometry; } diff --git a/interface/src/renderer/FBXReader.h b/interface/src/renderer/FBXReader.h index b89d0954b4..0f8cc4c1e1 100644 --- a/interface/src/renderer/FBXReader.h +++ b/interface/src/renderer/FBXReader.h @@ -24,6 +24,13 @@ typedef QList FBXNodeList; /// The names of the blendshapes expected by Faceshift, terminated with an empty string. extern const char* FACESHIFT_BLENDSHAPES[]; +class Extents { +public: + + glm::vec3 minimum; + glm::vec3 maximum; +}; + /// A node within an FBX document. class FBXNode { public: @@ -63,6 +70,11 @@ public: glm::quat inverseDefaultRotation; glm::quat inverseBindRotation; glm::mat4 bindTransform; + QString name; + Extents extents; + int numVertices; + glm::vec3 averageVertex; + float averageRadius; }; /// A single binding to a joint in an FBX document. @@ -124,13 +136,6 @@ public: glm::vec3 scale; }; -class Extents { -public: - - glm::vec3 minimum; - glm::vec3 maximum; -}; - /// A set of meshes extracted from an FBX document. class FBXGeometry { public: From 4d984b10548ed76e05ffe97c0e3185a121d94b91 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 28 Feb 2014 16:55:16 -0800 Subject: [PATCH 18/33] cleanup --- interface/src/renderer/FBXReader.cpp | 49 +++++++++------------------- 1 file changed, 16 insertions(+), 33 deletions(-) diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp index 0b22adb0a4..26d8079842 100644 --- a/interface/src/renderer/FBXReader.cpp +++ b/interface/src/renderer/FBXReader.cpp @@ -848,9 +848,6 @@ QString getString(const QVariant& value) { } FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) { -// std::cout << "adebug beginPrintNode " << std::endl; // adebug -// printNode(node, 0); // adebug -// std::cout << "adebug endPrintNode " << std::endl; // adebug QHash meshes; QVector blendshapes; QMultiHash parentMap; @@ -1517,28 +1514,23 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) } float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix); + glm::vec3 averageVertex(0.f); foreach (const glm::vec3& vertex, extracted.mesh.vertices) { - float proj = glm::dot(boneDirection, vertex - boneEnd); - if (proj < 0.0f && proj > -boneLength) { - joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance( - vertex, boneEnd + boneDirection * proj)); - } glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd)); joint.extents.minimum = glm::min(joint.extents.minimum, vertexInJointFrame); joint.extents.maximum = glm::max(joint.extents.maximum, vertexInJointFrame); - joint.averageVertex += vertex; - ++joint.numVertices; + joint.averageVertex += vertexInJointFrame; + averageVertex += vertex; } - if (joint.numVertices > 0) { - joint.averageVertex /= float(joint.numVertices); + int numVertices = extracted.mesh.vertices.size(); + joint.numVertices = numVertices; + if (numVertices > 0) { + averageVertex /= float(joint.numVertices); float averageRadius = 0.f; foreach (const glm::vec3& vertex, extracted.mesh.vertices) { - averageRadius += glm::distance(vertex, joint.averageVertex); + averageRadius += glm::distance(vertex, averageVertex); } - joint.boneRadius = averageRadius * (radiusScale / float(joint.numVertices)); - //joint.averageVertex /= float(joint.numVertices); - joint.averageVertex = rotateMeshToJoint * (radiusScale * (joint.averageVertex - boneEnd)); - joint.numVertices = 1; + joint.boneRadius = averageRadius * (radiusScale / float(numVertices)); } } extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex); @@ -1588,6 +1580,13 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) geometry.meshes.append(extracted.mesh); } + // now that all joints have been scanned, divide the averages by number of vertices + for (int i = 0; i < geometry.joints.size(); ++i) { + FBXJoint& joint = geometry.joints[i]; + if (joint.numVertices > 0) { + joint.averageVertex /= float(joint.numVertices); + } + } geometry.palmDirection = parseVec3(mapping.value("palmDirection", "0, -1, 0").toString()); // process attachments @@ -1618,22 +1617,6 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) geometry.attachments.append(attachment); } - for (int i = 0; i < geometry.joints.size(); ++i) { - FBXJoint& joint = geometry.joints[i]; - if (joint.numVertices > 0) { - //joint.averageVertex /= 1.f; - joint.averageVertex /= float(joint.numVertices); - } - /* - std::cout << joint.name.toStdString().c_str() - << " bR = " << joint.boneRadius - << " extents = " << (joint.extents.maximum - joint.extents.minimum) - << " avgV = " << joint.averageVertex - << " numV = " << joint.numVertices - << std::endl; // adebug - */ - } - return geometry; } From 8ef657e5b2066ca9544f39b4c2f228a740718cb5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 28 Feb 2014 17:05:08 -0800 Subject: [PATCH 19/33] Render avatar collision shapes in the right spot --- interface/src/renderer/Model.cpp | 11 ++++++++--- interface/src/renderer/Model.h | 8 ++++---- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 47a6013b5f..48cfde8be6 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -137,9 +137,10 @@ void Model::createCollisionShapes() { const FBXGeometry& geometry = _geometry->getFBXGeometry(); float uniformScale = extractUniformScale(_scale); for (int i = 0; i < _jointStates.size(); i++) { - glm::vec3 position = extractTranslation(_jointStates[i].transform); const FBXJoint& joint = geometry.joints[i]; - // for now make everything a sphere at joint end + glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.averageVertex; + glm::vec3 position = _rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation; + float radius = uniformScale * joint.boneRadius; SphereShape* shape = new SphereShape(radius, position); _shapes.push_back(shape); @@ -147,10 +148,14 @@ void Model::createCollisionShapes() { } void Model::updateShapePositions() { + float uniformScale = extractUniformScale(_scale); + const FBXGeometry& geometry = _geometry->getFBXGeometry(); if (_shapesAreDirty && _shapes.size() == _jointStates.size()) { for (int i = 0; i < _jointStates.size(); i++) { + const FBXJoint& joint = geometry.joints[i]; // shape positions are stored in world-frame - _shapes[i]->setPosition(_rotation * extractTranslation(_jointStates[i].transform) + _translation); + glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.averageVertex; + _shapes[i]->setPosition(_rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation); _shapes[i]->setRotation(_jointStates[i].combinedRotation); } _shapesAreDirty = false; diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index eff2bb0594..12691dbf03 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -182,10 +182,10 @@ protected: class JointState { public: - glm::vec3 translation; - glm::quat rotation; - glm::mat4 transform; - glm::quat combinedRotation; + glm::vec3 translation; // translation relative to parent + glm::quat rotation; // rotation relative to parent + glm::mat4 transform; // rotation to world frame + translation in model frame + glm::quat combinedRotation; // rotation to world frame }; bool _shapesAreDirty; From 1c0826d696dc7bc1ff5ff156befdc5c214bab989 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 3 Mar 2014 12:31:09 -0800 Subject: [PATCH 20/33] Add JointShapeInfo for joint shape calculations --- interface/src/renderer/FBXReader.cpp | 128 ++++++++++++++++++++------- interface/src/renderer/FBXReader.h | 15 ++-- interface/src/renderer/Model.cpp | 4 +- 3 files changed, 109 insertions(+), 38 deletions(-) diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp index 26d8079842..5ff56b46dd 100644 --- a/interface/src/renderer/FBXReader.cpp +++ b/interface/src/renderer/FBXReader.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include "FBXReader.h" @@ -34,6 +35,22 @@ std::ostream& operator<<(std::ostream& s, const glm::vec3& v) { using namespace std; +void Extents::reset() { + minimum = glm::vec3(FLT_MAX); + maximum = glm::vec3(-FLT_MAX); +} + +bool Extents::containsPoint(const glm::vec3& point) const { + return (point.x >= minimum.x && point.x <= maximum.x + && point.y >= minimum.y && point.y <= maximum.y + && point.z >= minimum.z && point.z <= maximum.z); +} + +void Extents::addPoint(const glm::vec3& point) { + minimum = glm::min(minimum, point); + maximum = glm::max(maximum, point); +} + template QVariant readBinaryArray(QDataStream& in) { quint32 arrayLength; quint32 encoding; @@ -847,6 +864,21 @@ QString getString(const QVariant& value) { return list.isEmpty() ? value.toString() : list.at(0).toString(); } +class JointShapeInfo { +public: + JointShapeInfo() : numVertices(0), numProjectedVertices(0), averageVertex(0.f), boneBegin(0.f), averageRadius(0.f) { + extents.reset(); + } + + // NOTE: the points here are in the "joint frame" which has the "jointEnd" at the origin + int numVertices; // num vertices from contributing meshes + int numProjectedVertices; // num vertices that successfully project onto bone axis + Extents extents; // max and min extents of mesh vertices (in joint frame) + glm::vec3 averageVertex; // average of all mesh vertices (in joint frame) + glm::vec3 boneBegin; // parent joint location (in joint frame) + float averageRadius; // average distance from mesh points to averageVertex +}; + FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) { QHash meshes; QVector blendshapes; @@ -1256,14 +1288,14 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) joint.boneRadius = 0.0f; joint.inverseBindRotation = joint.inverseDefaultRotation; joint.name = model.name; - joint.extents.minimum = glm::vec3(FLT_MAX); - joint.extents.maximum = glm::vec3(-FLT_MAX); - joint.averageVertex = glm::vec3(0.f); - joint.numVertices = 0; - joint.averageRadius = 0.f; + joint.shapePosition = glm::vec3(0.f); + joint.shapeType = Shape::UNKNOWN_SHAPE; geometry.joints.append(joint); geometry.jointIndices.insert(model.name, geometry.joints.size() - 1); } + // for each joint we allocate a JointShapeInfo in which we'll store collision shape info + QVector jointShapeInfos; + jointShapeInfos.resize(geometry.joints.size()); // find our special joints geometry.leftEyeJointIndex = modelIDs.indexOf(jointEyeLeftID); @@ -1285,10 +1317,8 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) geometry.neckPivot = glm::vec3(transform[3][0], transform[3][1], transform[3][2]); } - geometry.bindExtents.minimum = glm::vec3(FLT_MAX); - geometry.bindExtents.maximum = glm::vec3(-FLT_MAX); - geometry.staticExtents.minimum = glm::vec3(FLT_MAX); - geometry.staticExtents.maximum = glm::vec3(-FLT_MAX); + geometry.bindExtents.reset(); + geometry.staticExtents.reset(); QVariantHash springs = mapping.value("spring").toHash(); QVariant defaultSpring = springs.value("default"); @@ -1404,8 +1434,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) // update the bind pose extents glm::vec3 bindTranslation = extractTranslation(geometry.offset * joint.bindTransform); - geometry.bindExtents.minimum = glm::min(geometry.bindExtents.minimum, bindTranslation); - geometry.bindExtents.maximum = glm::max(geometry.bindExtents.maximum, bindTranslation); + geometry.bindExtents.addPoint(bindTranslation); } } @@ -1448,9 +1477,12 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) boneDirection /= boneLength; } } + float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix); + JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex]; + jointShapeInfo.boneBegin = rotateMeshToJoint * (radiusScale * (boneBegin - boneEnd)); + bool jointIsStatic = joint.freeLineage.isEmpty(); glm::vec3 jointTranslation = extractTranslation(geometry.offset * joint.bindTransform); - float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix); float totalWeight = 0.0f; for (int j = 0; j < cluster.indices.size(); j++) { int oldIndex = cluster.indices.at(j); @@ -1464,17 +1496,17 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) const glm::vec3& vertex = extracted.mesh.vertices.at(it.value()); float proj = glm::dot(boneDirection, vertex - boneEnd); if (proj < 0.0f && proj > -boneLength) { - joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(vertex, boneEnd + boneDirection * proj)); + joint.boneRadius = glm::max(joint.boneRadius, + radiusScale * glm::distance(vertex, boneEnd + boneDirection * proj)); + ++jointShapeInfo.numProjectedVertices; } glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd)); - joint.extents.minimum = glm::min(joint.extents.minimum, vertexInJointFrame); - joint.extents.maximum = glm::max(joint.extents.maximum, vertexInJointFrame); - joint.averageVertex += vertexInJointFrame; - ++joint.numVertices; + jointShapeInfo.extents.addPoint(vertexInJointFrame); + jointShapeInfo.averageVertex += vertexInJointFrame; + ++jointShapeInfo.numVertices; if (jointIsStatic) { // expand the extents of static (nonmovable) joints - geometry.staticExtents.minimum = glm::min(geometry.staticExtents.minimum, vertex + jointTranslation); - geometry.staticExtents.maximum = glm::max(geometry.staticExtents.maximum, vertex + jointTranslation); + geometry.staticExtents.addPoint(vertex + jointTranslation); } } @@ -1497,40 +1529,47 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) } else { int jointIndex = maxJointIndex; FBXJoint& joint = geometry.joints[jointIndex]; + JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex]; glm::mat4 transformJointToMesh = inverseModelTransform * joint.bindTransform; glm::quat rotateMeshToJoint = glm::inverse(extractRotation(transformJointToMesh)); glm::vec3 boneEnd = extractTranslation(transformJointToMesh); + glm::vec3 boneBegin = boneEnd; glm::vec3 boneDirection; float boneLength; if (joint.parentIndex != -1) { - boneDirection = boneEnd - extractTranslation(inverseModelTransform * - geometry.joints[joint.parentIndex].bindTransform); + boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform); + boneDirection = boneEnd - boneBegin; boneLength = glm::length(boneDirection); if (boneLength > EPSILON) { boneDirection /= boneLength; } } - float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix); + jointShapeInfo.boneBegin = rotateMeshToJoint * (radiusScale * (boneBegin - boneEnd)); + glm::vec3 averageVertex(0.f); foreach (const glm::vec3& vertex, extracted.mesh.vertices) { + float proj = glm::dot(boneDirection, vertex - boneEnd); + if (proj < 0.0f && proj > -boneLength) { + joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(vertex, boneEnd + boneDirection * proj)); + ++jointShapeInfo.numProjectedVertices; + } glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd)); - joint.extents.minimum = glm::min(joint.extents.minimum, vertexInJointFrame); - joint.extents.maximum = glm::max(joint.extents.maximum, vertexInJointFrame); - joint.averageVertex += vertexInJointFrame; + jointShapeInfo.extents.addPoint(vertexInJointFrame); + jointShapeInfo.averageVertex += vertexInJointFrame; averageVertex += vertex; } int numVertices = extracted.mesh.vertices.size(); - joint.numVertices = numVertices; + jointShapeInfo.numVertices = numVertices; if (numVertices > 0) { - averageVertex /= float(joint.numVertices); + averageVertex /= float(jointShapeInfo.numVertices); float averageRadius = 0.f; foreach (const glm::vec3& vertex, extracted.mesh.vertices) { averageRadius += glm::distance(vertex, averageVertex); } - joint.boneRadius = averageRadius * (radiusScale / float(numVertices)); + jointShapeInfo.averageRadius = averageRadius * radiusScale; } } extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex); @@ -1580,11 +1619,38 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) geometry.meshes.append(extracted.mesh); } - // now that all joints have been scanned, divide the averages by number of vertices + // now that all joints have been scanned, compute a collision shape for each joint + glm::vec3 yAxis(0.f, 1.f, 0.f); for (int i = 0; i < geometry.joints.size(); ++i) { FBXJoint& joint = geometry.joints[i]; - if (joint.numVertices > 0) { - joint.averageVertex /= float(joint.numVertices); + JointShapeInfo& jointShapeInfo = jointShapeInfos[i]; + + // we use a capsule if the joint ANY mesh vertices that successfully projected onto the bone + // AND its boneRadius is not too close to zero + bool collideLikeCapsule = jointShapeInfo.numProjectedVertices > 0 + && glm::length(jointShapeInfo.boneBegin) > EPSILON; + + if (collideLikeCapsule) { + joint.shapeRotation = rotationBetween(yAxis, jointShapeInfo.boneBegin); + joint.shapePosition = 0.5f * jointShapeInfo.boneBegin; + //joint.shapePosition = glm::vec3(0.f); + joint.shapeType = Shape::CAPSULE_SHAPE; + } else { + // collide the joint like a sphere + if (jointShapeInfo.numVertices > 0) { + jointShapeInfo.averageVertex /= float(jointShapeInfo.numVertices); + joint.shapePosition = jointShapeInfo.averageVertex; + } else { + joint.shapePosition = glm::vec3(0.f); + joint.shapeType = Shape::SPHERE_SHAPE; + } + if (jointShapeInfo.numProjectedVertices == 0 + && jointShapeInfo.numVertices > 0) { + // the bone projection algorithm was not able to compute the joint radius + // so we use an alternative measure + jointShapeInfo.averageRadius /= float(jointShapeInfo.numVertices); + joint.boneRadius = jointShapeInfo.averageRadius; + } } } geometry.palmDirection = parseVec3(mapping.value("palmDirection", "0, -1, 0").toString()); diff --git a/interface/src/renderer/FBXReader.h b/interface/src/renderer/FBXReader.h index 0f8cc4c1e1..48c300358e 100644 --- a/interface/src/renderer/FBXReader.h +++ b/interface/src/renderer/FBXReader.h @@ -26,7 +26,10 @@ extern const char* FACESHIFT_BLENDSHAPES[]; class Extents { public: - + //Extents() : minimum(FLT_MAX), maximum(-FLT_MAX) {} + void reset(); + void addPoint(const glm::vec3& point); + bool containsPoint(const glm::vec3& point) const; glm::vec3 minimum; glm::vec3 maximum; }; @@ -70,13 +73,15 @@ public: glm::quat inverseDefaultRotation; glm::quat inverseBindRotation; glm::mat4 bindTransform; + // TODO: add some comments to these data members + // Trying to provide enough info so that the proper shape can be generated in Model QString name; - Extents extents; - int numVertices; - glm::vec3 averageVertex; - float averageRadius; + glm::vec3 shapePosition; // in joint frame (where boneEnd = origin) + glm::quat shapeRotation; + int shapeType; }; + /// A single binding to a joint in an FBX document. class FBXCluster { public: diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 48cfde8be6..aac4b000bb 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -138,7 +138,7 @@ void Model::createCollisionShapes() { float uniformScale = extractUniformScale(_scale); for (int i = 0; i < _jointStates.size(); i++) { const FBXJoint& joint = geometry.joints[i]; - glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.averageVertex; + glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.shapePosition; glm::vec3 position = _rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation; float radius = uniformScale * joint.boneRadius; @@ -154,7 +154,7 @@ void Model::updateShapePositions() { for (int i = 0; i < _jointStates.size(); i++) { const FBXJoint& joint = geometry.joints[i]; // shape positions are stored in world-frame - glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.averageVertex; + glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.shapePosition; _shapes[i]->setPosition(_rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation); _shapes[i]->setRotation(_jointStates[i].combinedRotation); } From f23dda7317e51c197125e56465425f61e49f5a99 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 3 Mar 2014 17:34:16 -0800 Subject: [PATCH 21/33] Less math for the same effect. --- libraries/shared/src/CapsuleShape.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index c71f03aa47..bae5f201ca 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -47,12 +47,12 @@ CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm: /// \param[out] startPoint is the center of start cap void CapsuleShape::getStartPoint(glm::vec3& startPoint) const { - startPoint = getPosition() - _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f)); + startPoint = getPosition() - _rotation * glm::vec3(0.f, _halfHeight, 0.f); } /// \param[out] endPoint is the center of the end cap void CapsuleShape::getEndPoint(glm::vec3& endPoint) const { - endPoint = getPosition() + _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f)); + endPoint = getPosition() + _rotation * glm::vec3(0.f, _halfHeight, 0.f); } void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { From 1a8c10e1323aad93b5c304738e1943155b851506 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 3 Mar 2014 17:35:52 -0800 Subject: [PATCH 22/33] Correcting a comment. --- interface/src/renderer/Model.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 12691dbf03..d9cce3aef9 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -185,7 +185,7 @@ protected: glm::vec3 translation; // translation relative to parent glm::quat rotation; // rotation relative to parent glm::mat4 transform; // rotation to world frame + translation in model frame - glm::quat combinedRotation; // rotation to world frame + glm::quat combinedRotation; // rotation to model frame }; bool _shapesAreDirty; From 4ef20674212e8ce48ba2d8c2f88cb8dfb21ae81c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 3 Mar 2014 17:49:24 -0800 Subject: [PATCH 23/33] Use capsules for some joints and render them. --- interface/src/renderer/FBXReader.cpp | 6 +-- interface/src/renderer/FBXReader.h | 17 +++--- interface/src/renderer/Model.cpp | 79 +++++++++++++++++++++------- 3 files changed, 75 insertions(+), 27 deletions(-) diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp index 5ff56b46dd..a68d727db0 100644 --- a/interface/src/renderer/FBXReader.cpp +++ b/interface/src/renderer/FBXReader.cpp @@ -1620,18 +1620,18 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) } // now that all joints have been scanned, compute a collision shape for each joint - glm::vec3 yAxis(0.f, 1.f, 0.f); + glm::vec3 defaultCapsuleAxis(0.f, 1.f, 0.f); for (int i = 0; i < geometry.joints.size(); ++i) { FBXJoint& joint = geometry.joints[i]; JointShapeInfo& jointShapeInfo = jointShapeInfos[i]; - // we use a capsule if the joint ANY mesh vertices that successfully projected onto the bone + // we use a capsule if the joint ANY mesh vertices successfully projected onto the bone // AND its boneRadius is not too close to zero bool collideLikeCapsule = jointShapeInfo.numProjectedVertices > 0 && glm::length(jointShapeInfo.boneBegin) > EPSILON; if (collideLikeCapsule) { - joint.shapeRotation = rotationBetween(yAxis, jointShapeInfo.boneBegin); + joint.shapeRotation = rotationBetween(defaultCapsuleAxis, jointShapeInfo.boneBegin); joint.shapePosition = 0.5f * jointShapeInfo.boneBegin; //joint.shapePosition = glm::vec3(0.f); joint.shapeType = Shape::CAPSULE_SHAPE; diff --git a/interface/src/renderer/FBXReader.h b/interface/src/renderer/FBXReader.h index 48c300358e..b96df3a03b 100644 --- a/interface/src/renderer/FBXReader.h +++ b/interface/src/renderer/FBXReader.h @@ -26,10 +26,17 @@ extern const char* FACESHIFT_BLENDSHAPES[]; class Extents { public: - //Extents() : minimum(FLT_MAX), maximum(-FLT_MAX) {} + /// set minimum and maximum to FLT_MAX and -FLT_MAX respectively void reset(); + + /// \param point new point to compare against existing limits + /// compare point to current limits and expand them if necessary to contain point void addPoint(const glm::vec3& point); + + /// \param point + /// \return true if point is within current limits bool containsPoint(const glm::vec3& point) const; + glm::vec3 minimum; glm::vec3 maximum; }; @@ -73,11 +80,9 @@ public: glm::quat inverseDefaultRotation; glm::quat inverseBindRotation; glm::mat4 bindTransform; - // TODO: add some comments to these data members - // Trying to provide enough info so that the proper shape can be generated in Model - QString name; - glm::vec3 shapePosition; // in joint frame (where boneEnd = origin) - glm::quat shapeRotation; + QString name; // temp field for debugging + glm::vec3 shapePosition; // in joint frame + glm::quat shapeRotation; // in joint frame int shapeType; }; diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index aac4b000bb..64200fd992 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -142,8 +142,15 @@ void Model::createCollisionShapes() { glm::vec3 position = _rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation; float radius = uniformScale * joint.boneRadius; - SphereShape* shape = new SphereShape(radius, position); - _shapes.push_back(shape); + if (joint.shapeType == Shape::CAPSULE_SHAPE) { + float halfHeight = 0.5f * uniformScale * joint.distanceToParent; + CapsuleShape* shape = new CapsuleShape(radius, halfHeight); + shape->setPosition(position); + _shapes.push_back(shape); + } else { + SphereShape* shape = new SphereShape(radius, position); + _shapes.push_back(shape); + } } } @@ -153,10 +160,14 @@ void Model::updateShapePositions() { if (_shapesAreDirty && _shapes.size() == _jointStates.size()) { for (int i = 0; i < _jointStates.size(); i++) { const FBXJoint& joint = geometry.joints[i]; - // shape positions are stored in world-frame - glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.shapePosition; - _shapes[i]->setPosition(_rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation); - _shapes[i]->setRotation(_jointStates[i].combinedRotation); + + // shape position and rotation are stored in world-frame + glm::vec3 localPosition = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition); + glm::vec3 worldPosition = _rotation * (extractTranslation(_jointStates[i].transform) + localPosition) + _translation; + _shapes[i]->setPosition(worldPosition); + glm::quat localRotation = _jointStates[i].combinedRotation * joint.shapeRotation; + glm::quat worldRotation = _rotation * localRotation; + _shapes[i]->setRotation(worldRotation); } _shapesAreDirty = false; } @@ -743,24 +754,56 @@ void Model::renderCollisionProxies(float alpha) { Application::getInstance()->loadTranslatedViewMatrix(_translation); updateShapePositions(); float uniformScale = extractUniformScale(_scale); + const int BALL_SUBDIVISIONS = 10; glm::quat inverseRotation = glm::inverse(_rotation); for (int i = 0; i < _shapes.size(); i++) { glPushMatrix(); Shape* shape = _shapes[i]; - // shapes are stored in world-frame, so we have to transform into local frame - glm::vec3 position = inverseRotation * (shape->getPosition() - _translation); - glTranslatef(position.x, position.y, position.z); - - const glm::quat& rotation = shape->getRotation(); - glm::vec3 axis = glm::axis(rotation); - glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z); - - glColor4f(0.75f, 0.75f, 0.75f, alpha); - const int BALL_SUBDIVISIONS = 10; - glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); - + if (shape->getType() == Shape::SPHERE_SHAPE) { + // shapes are stored in world-frame, so we have to transform into model frame + glm::vec3 position = inverseRotation * (shape->getPosition() - _translation); + glTranslatef(position.x, position.y, position.z); + const glm::quat& rotation = inverseRotation; + glm::vec3 axis = glm::axis(rotation); + glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z); + + // draw a grey sphere at shape position + glColor4f(0.75f, 0.75f, 0.75f, alpha); + glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + } else if (shape->getType() == Shape::CAPSULE_SHAPE) { + CapsuleShape* capsule = static_cast(shape); + + // translate to capsule center + glm::vec3 point = inverseRotation * (capsule->getPosition() - _translation); + + // draw a blue sphere at the capsule endpoint + glm::vec3 endPoint; + capsule->getEndPoint(endPoint); + endPoint = inverseRotation * (endPoint - _translation); + glTranslatef(endPoint.x, endPoint.y, endPoint.z); + glColor4f(0.65f, 0.65f, 0.95f, alpha); + glutSolidSphere(0.95f * capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + + // draw a red sphere at the capsule startpoint + glm::vec3 startPoint; + capsule->getStartPoint(startPoint); + startPoint = inverseRotation * (startPoint - _translation); + glm::vec3 axis = endPoint - startPoint; + glTranslatef(-axis.x, -axis.y, -axis.z); + glColor4f(0.95f, 0.65f, 0.65f, alpha); + glutSolidSphere(0.95f * capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + + // draw a green cylinder between the two points + glm::vec3 origin(0.f); + glColor4f(0.05f, 0.95f, 0.65f, alpha); + Avatar::renderJointConnectingCone( + origin, + axis, + 1.05f * capsule->getRadius(), + 1.05f * capsule->getRadius()); + } glPopMatrix(); } glPopMatrix(); From d0ac8eec74e781dc6f730fe2631a723551679c0d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 4 Mar 2014 09:54:12 -0800 Subject: [PATCH 24/33] removing unused variable --- interface/src/avatar/Hand.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index e845f259fc..41b4214e72 100644 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -153,7 +153,6 @@ void Hand::collideAgainstOurself() { if (!palm.isActive()) { continue; } - const Model& skeletonModel = _owningAvatar->getSkeletonModel(); // ignoring everything below the parent of the parent of the last free joint int skipIndex = skeletonModel.getParentJointIndex(skeletonModel.getParentJointIndex( skeletonModel.getLastFreeJointIndex((i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() : From 3d426a24f1a1e156ec0afb6d854d6526f520fe6d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 4 Mar 2014 09:54:34 -0800 Subject: [PATCH 25/33] Fixing build warnings --- interface/src/renderer/Model.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 0b524fcc25..7e2e5413b7 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -21,9 +21,9 @@ using namespace std; Model::Model(QObject* parent) : QObject(parent), + _shapesAreDirty(true), _lodDistance(0.0f), - _pupilDilation(0.0f), - _shapesAreDirty(true) { + _pupilDilation(0.0f) { // we may have been created in the network thread, but we live in the main thread moveToThread(Application::getInstance()->thread()); } @@ -741,7 +741,6 @@ void Model::renderCollisionProxies(float alpha) { glPushMatrix(); Application::getInstance()->loadTranslatedViewMatrix(_translation); updateShapePositions(); - float uniformScale = extractUniformScale(_scale); const int BALL_SUBDIVISIONS = 10; glm::quat inverseRotation = glm::inverse(_rotation); for (int i = 0; i < _shapes.size(); i++) { @@ -763,9 +762,6 @@ void Model::renderCollisionProxies(float alpha) { } else if (shape->getType() == Shape::CAPSULE_SHAPE) { CapsuleShape* capsule = static_cast(shape); - // translate to capsule center - glm::vec3 point = inverseRotation * (capsule->getPosition() - _translation); - // draw a blue sphere at the capsule endpoint glm::vec3 endPoint; capsule->getEndPoint(endPoint); From 343998a4a87ec7a497d71b3c44eb4f11201ca1d8 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 4 Mar 2014 09:55:08 -0800 Subject: [PATCH 26/33] Adding virtual destructor to Shape class --- libraries/shared/src/Shape.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 5bb94d32ff..924d13000e 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -23,6 +23,7 @@ public: }; Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { } + virtual ~Shape() {} int getType() const { return _type; } float getBoundingRadius() const { return _boundingRadius; } From e22e0222524fe79c4ac5f9ee090ee5519cfcd631 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 4 Mar 2014 09:55:32 -0800 Subject: [PATCH 27/33] Fixing warnings and out-of-range index. --- libraries/shared/src/ShapeCollider.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 9c14a5d033..7f36d948e8 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -273,7 +273,6 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, // 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; } @@ -315,7 +314,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, // 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[4] = points[1]; points[1] = points[2]; points[2] = points[4]; } From ea0bc52e12cd594aea1bb926a4bbbea76a9b40ab Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 4 Mar 2014 09:56:06 -0800 Subject: [PATCH 28/33] removing unused variables --- tests/physics/src/ShapeColliderTests.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index d5cb74dd87..314498b1d6 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -30,7 +30,6 @@ void ShapeColliderTests::sphereMissesSphere() { float beta = 1.3f; glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f)); float offsetDistance = alpha * radiusA + beta * radiusB; - float expectedPenetrationDistance = 0.f; SphereShape sphereA(radiusA, origin); SphereShape sphereB(radiusB, offsetDistance * offsetDirection); @@ -225,7 +224,6 @@ void ShapeColliderTests::sphereTouchesCapsule() { float halfHeightB = 2.f; float alpha = 0.5f; float beta = 0.5f; - float axialOffset = 0.f; float radialOffset = alpha * radiusA + beta * radiusB; SphereShape sphereA(radiusA); From a014d7883eded72b61d63ffea2d0497020fad674 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Mar 2014 07:05:27 -0800 Subject: [PATCH 29/33] minor whitespace formatting --- libraries/avatars/src/HandData.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h index 5f7a49e0a2..cdab9f71e9 100755 --- a/libraries/avatars/src/HandData.h +++ b/libraries/avatars/src/HandData.h @@ -89,7 +89,7 @@ public: friend class AvatarData; protected: AvatarData* _owningAvatarData; - std::vector _palms; + std::vector _palms; glm::quat getBaseOrientation() const; glm::vec3 getBasePosition() const; From 7db5aaaf3745b5e3067bfb7c0aeb448ad93f4bad Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Mar 2014 07:08:59 -0800 Subject: [PATCH 30/33] new collision pipeline for avatar shapes --- interface/src/avatar/Avatar.cpp | 10 ++++ interface/src/avatar/Avatar.h | 5 ++ interface/src/avatar/Hand.cpp | 76 ++++++++++++++++++++++---- interface/src/avatar/Hand.h | 1 + interface/src/avatar/MyAvatar.cpp | 6 ++ interface/src/avatar/SkeletonModel.cpp | 11 ++++ interface/src/avatar/SkeletonModel.h | 4 ++ interface/src/renderer/Model.cpp | 19 ++++++- interface/src/renderer/Model.h | 5 ++ 9 files changed, 122 insertions(+), 15 deletions(-) diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index d484ce2aa0..3963a84724 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -489,6 +489,16 @@ bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penet //return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions); } +bool Avatar::findCollisions(const QVector& shapes, CollisionList& collisions) { + _skeletonModel.updateShapePositions(); + bool collided = _skeletonModel.findCollisions(shapes, collisions); + + Model& headModel = getHead()->getFaceModel(); + headModel.updateShapePositions(); + collided = headModel.findCollisions(shapes, collisions); + return collided; +} + bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) { if (_collisionFlags & COLLISION_GROUP_PARTICLES) { return false; diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index 8de5da8d50..1bbf7df039 100755 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -99,6 +99,11 @@ public: bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const; + /// \param shapes list of shapes to collide against avatar + /// \param collisions list to store collision results + /// \return true if at least one shape collided with avatar + bool findCollisions(const QVector& shapes, CollisionList& collisions); + /// Checks for penetration between the described sphere and the avatar. /// \param penetratorCenter the center of the penetration test sphere /// \param penetratorRadius the radius of the penetration test sphere diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index 41b4214e72..ce2f2a242e 100644 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -96,7 +96,7 @@ void Hand::playSlaps(PalmData& palm, Avatar* avatar) const float MAX_COLLISIONS_PER_AVATAR = 32; static CollisionList handCollisions(MAX_COLLISIONS_PER_AVATAR); -void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) { +void Hand::collideAgainstAvatarOld(Avatar* avatar, bool isMyHand) { if (!avatar || avatar == _owningAvatar) { // don't collide with our own hands (that is done elsewhere) return; @@ -117,17 +117,9 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) { for (int j = 0; j < handCollisions.size(); ++j) { CollisionInfo* collision = handCollisions.getCollision(j); if (isMyHand) { - if (!avatar->collisionWouldMoveAvatar(*collision)) { - // we resolve the hand from collision when it belongs to MyAvatar AND the other Avatar is - // not expected to respond to the collision (hand hit unmovable part of their Avatar) - totalPenetration = addPenetrations(totalPenetration, collision->_penetration); - } - } else { - // when !isMyHand then avatar is MyAvatar and we apply the collision - // which might not do anything (hand hit unmovable part of MyAvatar) however - // we don't resolve the hand's penetration because we expect the remote - // simulation to do the right thing. - avatar->applyCollision(*collision); + // we resolve the hand from collision when it belongs to MyAvatar AND the other Avatar is + // not expected to respond to the collision (hand hit unmovable part of their Avatar) + totalPenetration = addPenetrations(totalPenetration, collision->_penetration); } } } @@ -138,6 +130,66 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) { } } +void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) { + if (!avatar || avatar == _owningAvatar) { + // don't collide hands against ourself (that is done elsewhere) + return; + } + + // 2 = NUM_HANDS + int palmIndices[2]; + getLeftRightPalmIndices(*palmIndices, *(palmIndices + 1)); + + const SkeletonModel& skeletonModel = _owningAvatar->getSkeletonModel(); + int jointIndices[2]; + jointIndices[0] = skeletonModel.getLeftHandJointIndex(); + jointIndices[1] = skeletonModel.getRightHandJointIndex(); + + palmIndices[1] = -1; // adebug temporarily disable right hand + jointIndices[1] = -1; // adebug temporarily disable right hand + + for (size_t i = 0; i < 1; i++) { + int palmIndex = palmIndices[i]; + int jointIndex = jointIndices[i]; + if (palmIndex == -1 || jointIndex == -1) { + continue; + } + PalmData& palm = _palms[palmIndex]; + if (!palm.isActive()) { + continue; + } + if (isMyHand && Menu::getInstance()->isOptionChecked(MenuOption::PlaySlaps)) { + playSlaps(palm, avatar); + } + + handCollisions.clear(); + QVector shapes; + skeletonModel.getHandShapes(jointIndex, shapes); + bool collided = isMyHand ? avatar->findCollisions(shapes, handCollisions) : avatar->findCollisions(shapes, handCollisions); + if (collided) { + //if (avatar->findCollisions(shapes, handCollisions)) { + glm::vec3 averagePenetration; + glm::vec3 averageContactPoint; + for (int j = 0; j < handCollisions.size(); ++j) { + CollisionInfo* collision = handCollisions.getCollision(j); + averagePenetration += collision->_penetration; + averageContactPoint += collision->_contactPoint; + } + averagePenetration /= float(handCollisions.size()); + if (isMyHand) { + // our hand against other avatar + // for now we resolve it to test shapes/collisions + // TODO: only partially resolve this penetration + palm.addToPosition(-averagePenetration); + } else { + // someone else's hand against MyAvatar + // TODO: submit collision info to MyAvatar which should lean accordingly + averageContactPoint /= float(handCollisions.size()); + } + } + } +} + void Hand::collideAgainstOurself() { if (!Menu::getInstance()->isOptionChecked(MenuOption::HandsCollideWithSelf)) { return; diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h index bf335d1bdd..a1b1875424 100755 --- a/interface/src/avatar/Hand.h +++ b/interface/src/avatar/Hand.h @@ -56,6 +56,7 @@ public: const glm::vec3& getLeapFingerTipBallPosition (int ball) const { return _leapFingerTipBalls [ball].position;} const glm::vec3& getLeapFingerRootBallPosition(int ball) const { return _leapFingerRootBalls[ball].position;} + void collideAgainstAvatarOld(Avatar* avatar, bool isMyHand); void collideAgainstAvatar(Avatar* avatar, bool isMyHand); void collideAgainstOurself(); diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b0401fc52d..de07ed55fa 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -941,6 +941,11 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) { } float theirBoundingRadius = avatar->getBoundingRadius(); if (distance < myBoundingRadius + theirBoundingRadius) { + _skeletonModel.updateShapePositions(); + Model& headModel = getHead()->getFaceModel(); + headModel.updateShapePositions(); + + /* TODO: Andrew to fix Avatar-Avatar body collisions Extents theirStaticExtents = _skeletonModel.getStaticExtents(); glm::vec3 staticScale = theirStaticExtents.maximum - theirStaticExtents.minimum; float theirCapsuleRadius = 0.25f * (staticScale.x + staticScale.z); @@ -952,6 +957,7 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) { // move the avatar out by half the penetration setPosition(_position - 0.5f * penetration); } + */ // collide our hands against them getHand()->collideAgainstAvatar(avatar, true); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index edb11a631e..7c7202566a 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -73,6 +73,17 @@ bool SkeletonModel::render(float alpha) { return true; } +void SkeletonModel::getHandShapes(int jointIndex, QVector& shapes) const { + if (jointIndex == -1) { + return; + } + if (jointIndex == getLeftHandJointIndex() + || jointIndex == getRightHandJointIndex()) { + // TODO: also add fingers and other hand-parts + shapes.push_back(_shapes[jointIndex]); + } +} + class IndexValue { public: int index; diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index 6f80d77edc..3d95d805ea 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -24,6 +24,10 @@ public: void simulate(float deltaTime, bool delayLoad = false); bool render(float alpha); + + /// \param jointIndex index of hand joint + /// \param shapes[out] list in which is stored pointers to hand shapes + void getHandShapes(int jointIndex, QVector& shapes) const; protected: diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 7e2e5413b7..4bf604db69 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -135,12 +135,11 @@ void Model::createCollisionShapes() { } void Model::updateShapePositions() { - float uniformScale = extractUniformScale(_scale); - const FBXGeometry& geometry = _geometry->getFBXGeometry(); if (_shapesAreDirty && _shapes.size() == _jointStates.size()) { + float uniformScale = extractUniformScale(_scale); + const FBXGeometry& geometry = _geometry->getFBXGeometry(); for (int i = 0; i < _jointStates.size(); i++) { const FBXJoint& joint = geometry.joints[i]; - // shape position and rotation are stored in world-frame glm::vec3 localPosition = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition); glm::vec3 worldPosition = _rotation * (extractTranslation(_jointStates[i].transform) + localPosition) + _translation; @@ -508,6 +507,20 @@ bool Model::findRayIntersection(const glm::vec3& origin, const glm::vec3& direct return false; } +bool Model::findCollisions(const QVector shapes, CollisionList& collisions) { + bool collided = false; + for (int i = 0; i < shapes.size(); ++i) { + const Shape* theirShape = shapes[i]; + for (int j = 0; j < _shapes.size(); ++j) { + const Shape* ourShape = _shapes[j]; + if (ShapeCollider::shapeShape(theirShape, ourShape, collisions)) { + collided = true; + } + } + } + return collided; +} + bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, CollisionList& collisions, int skipIndex) { bool collided = false; diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index d77a755538..4f0e789ef0 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -169,6 +169,11 @@ public: glm::vec4 computeAverageColor() const; bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const; + + /// \param shapes list of pointers shapes to test against Model + /// \param collisions list to store collision results + /// \return true if at least one shape collided agains Model + bool findCollisions(const QVector shapes, CollisionList& collisions); bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, CollisionList& collisions, int skipIndex = -1); From 2bc8e142b44010513f6167da66dfa43509b51605 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Mar 2014 11:51:42 -0800 Subject: [PATCH 31/33] cruft removal --- interface/src/renderer/FBXReader.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp index 756270bc6e..b1482beb06 100644 --- a/interface/src/renderer/FBXReader.cpp +++ b/interface/src/renderer/FBXReader.cpp @@ -1630,7 +1630,6 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) if (collideLikeCapsule) { joint.shapeRotation = rotationBetween(defaultCapsuleAxis, jointShapeInfo.boneBegin); joint.shapePosition = 0.5f * jointShapeInfo.boneBegin; - //joint.shapePosition = glm::vec3(0.f); joint.shapeType = Shape::CAPSULE_SHAPE; } else { // collide the joint like a sphere From 89f664c0ee4210aeef24d517982bb69fe4533a9f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Mar 2014 11:52:47 -0800 Subject: [PATCH 32/33] fixing incorrect documentation comment --- interface/src/renderer/Model.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 4f0e789ef0..fc4643374f 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -202,7 +202,7 @@ protected: glm::vec3 translation; // translation relative to parent glm::quat rotation; // rotation relative to parent glm::mat4 transform; // rotation to world frame + translation in model frame - glm::quat combinedRotation; // rotation to model frame + glm::quat combinedRotation; // rotation from joint local to world frame }; bool _shapesAreDirty; From a1856ac18bf1b16dad06e37736596a32d068416c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Mar 2014 11:57:07 -0800 Subject: [PATCH 33/33] fix bad transform math for placing shapes --- interface/src/renderer/Model.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 4bf604db69..1e2c445f37 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -140,13 +140,11 @@ void Model::updateShapePositions() { const FBXGeometry& geometry = _geometry->getFBXGeometry(); for (int i = 0; i < _jointStates.size(); i++) { const FBXJoint& joint = geometry.joints[i]; - // shape position and rotation are stored in world-frame - glm::vec3 localPosition = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition); - glm::vec3 worldPosition = _rotation * (extractTranslation(_jointStates[i].transform) + localPosition) + _translation; + // shape position and rotation need to be in world-frame + glm::vec3 jointToShapeOffset = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition); + glm::vec3 worldPosition = extractTranslation(_jointStates[i].transform) + jointToShapeOffset + _translation; _shapes[i]->setPosition(worldPosition); - glm::quat localRotation = _jointStates[i].combinedRotation * joint.shapeRotation; - glm::quat worldRotation = _rotation * localRotation; - _shapes[i]->setRotation(worldRotation); + _shapes[i]->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation); } _shapesAreDirty = false; } @@ -755,7 +753,6 @@ void Model::renderCollisionProxies(float alpha) { Application::getInstance()->loadTranslatedViewMatrix(_translation); updateShapePositions(); const int BALL_SUBDIVISIONS = 10; - glm::quat inverseRotation = glm::inverse(_rotation); for (int i = 0; i < _shapes.size(); i++) { glPushMatrix(); @@ -763,9 +760,9 @@ void Model::renderCollisionProxies(float alpha) { if (shape->getType() == Shape::SPHERE_SHAPE) { // shapes are stored in world-frame, so we have to transform into model frame - glm::vec3 position = inverseRotation * (shape->getPosition() - _translation); + glm::vec3 position = shape->getPosition() - _translation; glTranslatef(position.x, position.y, position.z); - const glm::quat& rotation = inverseRotation; + const glm::quat& rotation = shape->getRotation(); glm::vec3 axis = glm::axis(rotation); glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z); @@ -778,7 +775,7 @@ void Model::renderCollisionProxies(float alpha) { // draw a blue sphere at the capsule endpoint glm::vec3 endPoint; capsule->getEndPoint(endPoint); - endPoint = inverseRotation * (endPoint - _translation); + endPoint = endPoint - _translation; glTranslatef(endPoint.x, endPoint.y, endPoint.z); glColor4f(0.6f, 0.6f, 0.8f, alpha); glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); @@ -786,7 +783,7 @@ void Model::renderCollisionProxies(float alpha) { // draw a yellow sphere at the capsule startpoint glm::vec3 startPoint; capsule->getStartPoint(startPoint); - startPoint = inverseRotation * (startPoint - _translation); + startPoint = startPoint - _translation; glm::vec3 axis = endPoint - startPoint; glTranslatef(-axis.x, -axis.y, -axis.z); glColor4f(0.8f, 0.8f, 0.6f, alpha);