From 3ebd8c1969dada783945613ce7681144cc59b319 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 21 Aug 2014 16:40:25 -0700 Subject: [PATCH] use dispatchTable for shape-vs-shape collisions --- interface/src/avatar/MyAvatar.cpp | 1 + interface/src/avatar/SkeletonModel.cpp | 18 +- libraries/fbx/src/FBXReader.cpp | 8 +- libraries/shared/src/CapsuleShape.cpp | 8 +- libraries/shared/src/PhysicsEntity.cpp | 2 + libraries/shared/src/PhysicsSimulation.cpp | 3 +- libraries/shared/src/PlaneShape.cpp | 2 +- libraries/shared/src/Shape.h | 23 +- libraries/shared/src/ShapeCollider.cpp | 232 ++++++++++++--------- libraries/shared/src/ShapeCollider.h | 48 +++-- libraries/shared/src/SphereShape.h | 6 +- tests/physics/src/ShapeColliderTests.cpp | 37 ++++ tests/physics/src/ShapeColliderTests.h | 2 + tests/physics/src/VerletShapeTests.cpp | 2 + 14 files changed, 240 insertions(+), 152 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 1d271d5f40..239ca16186 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -81,6 +81,7 @@ MyAvatar::MyAvatar() : _billboardValid(false), _physicsSimulation() { + ShapeCollider::initDispatchTable(); for (int i = 0; i < MAX_DRIVE_KEYS; i++) { _driveKeys[i] = 0.0f; } diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index dd398f2b2b..e5e91cb886 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -618,19 +618,19 @@ void SkeletonModel::buildShapes() { Shape::Type type = joint.shapeType; int parentIndex = joint.parentIndex; if (parentIndex == -1 || radius < EPSILON) { - type = Shape::UNKNOWN_SHAPE; - } else if (type == Shape::CAPSULE_SHAPE && halfHeight < EPSILON) { + type = UNKNOWN_SHAPE; + } else if (type == CAPSULE_SHAPE && halfHeight < EPSILON) { // this shape is forced to be a sphere - type = Shape::SPHERE_SHAPE; + type = SPHERE_SHAPE; } Shape* shape = NULL; - if (type == Shape::SPHERE_SHAPE) { + if (type == SPHERE_SHAPE) { shape = new VerletSphereShape(radius, &(points[i])); shape->setEntity(this); float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()); points[i].setMass(mass); totalMass += mass; - } else if (type == Shape::CAPSULE_SHAPE) { + } else if (type == CAPSULE_SHAPE) { assert(parentIndex != -1); shape = new VerletCapsuleShape(radius, &(points[parentIndex]), &(points[i])); shape->setEntity(this); @@ -729,7 +729,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { shapeExtents.reset(); glm::vec3 localPosition = shape->getTranslation(); int type = shape->getType(); - if (type == Shape::CAPSULE_SHAPE) { + if (type == CAPSULE_SHAPE) { // add the two furthest surface points of the capsule CapsuleShape* capsule = static_cast(shape); glm::vec3 axis; @@ -741,7 +741,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { shapeExtents.addPoint(localPosition + axis); shapeExtents.addPoint(localPosition - axis); totalExtents.addExtents(shapeExtents); - } else if (type == Shape::SPHERE_SHAPE) { + } else if (type == SPHERE_SHAPE) { float radius = shape->getBoundingRadius(); glm::vec3 axis = glm::vec3(radius); shapeExtents.addPoint(localPosition + axis); @@ -845,13 +845,13 @@ void SkeletonModel::renderJointCollisionShapes(float alpha) { glPushMatrix(); // shapes are stored in simulation-frame but we want position to be model-relative - if (shape->getType() == Shape::SPHERE_SHAPE) { + if (shape->getType() == SPHERE_SHAPE) { glm::vec3 position = shape->getTranslation() - simulationTranslation; glTranslatef(position.x, position.y, position.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) { + } else if (shape->getType() == CAPSULE_SHAPE) { CapsuleShape* capsule = static_cast(shape); // draw a blue sphere at the capsule endpoint diff --git a/libraries/fbx/src/FBXReader.cpp b/libraries/fbx/src/FBXReader.cpp index 1a152dc217..d8b52fb794 100644 --- a/libraries/fbx/src/FBXReader.cpp +++ b/libraries/fbx/src/FBXReader.cpp @@ -1503,7 +1503,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) joint.inverseBindRotation = joint.inverseDefaultRotation; joint.name = model.name; joint.shapePosition = glm::vec3(0.f); - joint.shapeType = Shape::UNKNOWN_SHAPE; + joint.shapeType = UNKNOWN_SHAPE; geometry.joints.append(joint); geometry.jointIndices.insert(model.name, geometry.joints.size()); @@ -1848,10 +1848,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) if (collideLikeCapsule) { joint.shapeRotation = rotationBetween(defaultCapsuleAxis, jointShapeInfo.boneBegin); joint.shapePosition = 0.5f * jointShapeInfo.boneBegin; - joint.shapeType = Shape::CAPSULE_SHAPE; + joint.shapeType = CAPSULE_SHAPE; } else { // collide the joint like a sphere - joint.shapeType = Shape::SPHERE_SHAPE; + joint.shapeType = SPHERE_SHAPE; if (jointShapeInfo.numVertices > 0) { jointShapeInfo.averageVertex /= (float)jointShapeInfo.numVertices; joint.shapePosition = jointShapeInfo.averageVertex; @@ -1872,7 +1872,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) // The shape is further from both joint endpoints than the endpoints are from each other // which probably means the model has a bad transform somewhere. We disable this shape // by setting its type to UNKNOWN_SHAPE. - joint.shapeType = Shape::UNKNOWN_SHAPE; + joint.shapeType = UNKNOWN_SHAPE; } } } diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index 03bc48bd94..09776a233f 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -18,20 +18,20 @@ #include "SharedUtil.h" -CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE), _radius(0.0f), _halfHeight(0.0f) {} +CapsuleShape::CapsuleShape() : Shape(CAPSULE_SHAPE), _radius(0.0f), _halfHeight(0.0f) {} -CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(Shape::CAPSULE_SHAPE), +CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(CAPSULE_SHAPE), _radius(radius), _halfHeight(halfHeight) { updateBoundingRadius(); } CapsuleShape::CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation) : - Shape(Shape::CAPSULE_SHAPE, position, rotation), _radius(radius), _halfHeight(halfHeight) { + 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.0f) { + Shape(CAPSULE_SHAPE), _radius(radius), _halfHeight(0.0f) { setEndPoints(startPoint, endPoint); } diff --git a/libraries/shared/src/PhysicsEntity.cpp b/libraries/shared/src/PhysicsEntity.cpp index 09b00c201c..6be37a7528 100644 --- a/libraries/shared/src/PhysicsEntity.cpp +++ b/libraries/shared/src/PhysicsEntity.cpp @@ -12,8 +12,10 @@ #include "PhysicsEntity.h" #include "PhysicsSimulation.h" +#include "PlaneShape.h" #include "Shape.h" #include "ShapeCollider.h" +#include "SphereShape.h" PhysicsEntity::PhysicsEntity() : _translation(0.0f), diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index 6c4901bcd5..b58f62dfd4 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -16,8 +16,9 @@ #include "PerfStat.h" #include "PhysicsEntity.h" #include "Ragdoll.h" -#include "SharedUtil.h" +#include "Shape.h" #include "ShapeCollider.h" +#include "SharedUtil.h" int MAX_DOLLS_PER_SIMULATION = 16; int MAX_ENTITIES_PER_SIMULATION = 64; diff --git a/libraries/shared/src/PlaneShape.cpp b/libraries/shared/src/PlaneShape.cpp index 15ea281510..72704c3116 100644 --- a/libraries/shared/src/PlaneShape.cpp +++ b/libraries/shared/src/PlaneShape.cpp @@ -15,7 +15,7 @@ const glm::vec3 UNROTATED_NORMAL(0.0f, 1.0f, 0.0f); PlaneShape::PlaneShape(const glm::vec4& coefficients) : - Shape(Shape::PLANE_SHAPE) { + Shape(PLANE_SHAPE) { glm::vec3 normal = glm::vec3(coefficients); _translation = -normal * coefficients.w; diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 2efa5b824f..9c30a0fdf4 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -22,17 +22,18 @@ class VerletPoint; const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX) +const quint8 SPHERE_SHAPE = 0; +const quint8 CAPSULE_SHAPE = 1; +const quint8 PLANE_SHAPE = 2; +const quint8 LIST_SHAPE = 3; +const quint8 UNKNOWN_SHAPE = 4; + class Shape { public: - static quint32 getNextID() { static quint32 nextID = 0; return ++nextID; } - enum Type{ - UNKNOWN_SHAPE = 0, - SPHERE_SHAPE, - CAPSULE_SHAPE, - PLANE_SHAPE, - LIST_SHAPE - }; + typedef quint8 Type; + + static quint32 getNextID() { static quint32 nextID = 0; return ++nextID; } Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) { @@ -40,7 +41,7 @@ public: } virtual ~Shape() { } - int getType() const { return _type; } + Type getType() const { return _type; } quint32 getID() const { return _id; } void setEntity(PhysicsEntity* entity) { _owningEntity = entity; } @@ -95,8 +96,8 @@ protected: void setBoundingRadius(float radius) { _boundingRadius = radius; } - int _type; - unsigned int _id; + Type _type; + quint32 _id; PhysicsEntity* _owningEntity; float _boundingRadius; glm::vec3 _translation; diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 805e7f30f6..8d09c0408a 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -15,56 +15,66 @@ #include "GeometryUtil.h" #include "ShapeCollider.h" +#include "CapsuleShape.h" +#include "ListShape.h" +#include "PlaneShape.h" +#include "SphereShape.h" // NOTE: // // * Large ListShape's are inefficient keep the lists short. // * Collisions between lists of lists work in theory but are not recommended. +const Shape::Type NUM_SHAPE_TYPES = 5; +const quint8 NUM__DISPATCH_CELLS = NUM_SHAPE_TYPES * NUM_SHAPE_TYPES; + +Shape::Type getDispatchKey(Shape::Type typeA, Shape::Type typeB) { + return typeA + NUM_SHAPE_TYPES * typeB; +} + +// dummy dispatch for any non-implemented pairings +bool notImplemented(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + return false; +} + +// NOTE: hardcode the number of dispatchTable entries (NUM_SHAPE_TYPES ^2) +bool (*dispatchTable[NUM__DISPATCH_CELLS])(const Shape*, const Shape*, CollisionList&); + namespace ShapeCollider { -bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - // TODO: make a fast lookup for correct method - int typeA = shapeA->getType(); - int typeB = shapeB->getType(); - if (typeA == Shape::SPHERE_SHAPE) { - const SphereShape* sphereA = static_cast(shapeA); - if (typeB == Shape::SPHERE_SHAPE) { - return sphereSphere(sphereA, static_cast(shapeB), collisions); - } else if (typeB == Shape::CAPSULE_SHAPE) { - return sphereCapsule(sphereA, static_cast(shapeB), collisions); - } else if (typeB == Shape::PLANE_SHAPE) { - return spherePlane(sphereA, static_cast(shapeB), collisions); - } - } else if (typeA == Shape::CAPSULE_SHAPE) { - const CapsuleShape* capsuleA = static_cast(shapeA); - if (typeB == Shape::SPHERE_SHAPE) { - return capsuleSphere(capsuleA, static_cast(shapeB), collisions); - } else if (typeB == Shape::CAPSULE_SHAPE) { - return capsuleCapsule(capsuleA, static_cast(shapeB), collisions); - } else if (typeB == Shape::PLANE_SHAPE) { - return capsulePlane(capsuleA, static_cast(shapeB), collisions); - } - } else if (typeA == Shape::PLANE_SHAPE) { - const PlaneShape* planeA = static_cast(shapeA); - if (typeB == Shape::SPHERE_SHAPE) { - return planeSphere(planeA, static_cast(shapeB), collisions); - } else if (typeB == Shape::CAPSULE_SHAPE) { - return planeCapsule(planeA, static_cast(shapeB), collisions); - } else if (typeB == Shape::PLANE_SHAPE) { - return planePlane(planeA, 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); - } else if (typeB == Shape::PLANE_SHAPE) { - return listPlane(listA, static_cast(shapeB), collisions); - } +// NOTE: the dispatch table must be initialized before the ShapeCollider is used. +void initDispatchTable() { + for (Shape::Type i = 0; i < NUM__DISPATCH_CELLS; ++i) { + dispatchTable[i] = ¬Implemented; } - return false; + + // NOTE: no need to update any that are notImplemented, but we leave them + // commented out in the code so that we remember that they exist. + dispatchTable[getDispatchKey(SPHERE_SHAPE, SPHERE_SHAPE)] = &sphereSphere; + dispatchTable[getDispatchKey(SPHERE_SHAPE, CAPSULE_SHAPE)] = &sphereCapsule; + dispatchTable[getDispatchKey(SPHERE_SHAPE, PLANE_SHAPE)] = &spherePlane; + //dispatchTable[getDispatchKey(SPHERE_SHAPE, LIST_SHAPE)] = ¬Implemented; + + dispatchTable[getDispatchKey(CAPSULE_SHAPE, SPHERE_SHAPE)] = &capsuleSphere; + dispatchTable[getDispatchKey(CAPSULE_SHAPE, CAPSULE_SHAPE)] = &capsuleCapsule; + dispatchTable[getDispatchKey(CAPSULE_SHAPE, PLANE_SHAPE)] = &capsulePlane; + //dispatchTable[getDispatchKey(CAPSULE_SHAPE, LIST_SHAPE)] = ¬Implemented; + + dispatchTable[getDispatchKey(PLANE_SHAPE, SPHERE_SHAPE)] = &planeSphere; + dispatchTable[getDispatchKey(PLANE_SHAPE, CAPSULE_SHAPE)] = &planeCapsule; + dispatchTable[getDispatchKey(PLANE_SHAPE, PLANE_SHAPE)] = &planePlane; + //dispatchTable[getDispatchKey(PLANE_SHAPE, LIST_SHAPE)] = ¬Implemented; + + //dispatchTable[getDispatchKey(LIST_SHAPE, SPHERE_SHAPE)] = ¬Implemented; + //dispatchTable[getDispatchKey(LIST_SHAPE, CAPSULE_SHAPE)] = ¬Implemented; + //dispatchTable[getDispatchKey(LIST_SHAPE, PLANE_SHAPE)] = ¬Implemented; + //dispatchTable[getDispatchKey(LIST_SHAPE, LIST_SHAPE)] = ¬Implemented; + + // all of the UNKNOWN_SHAPE pairings point at notImplemented +} + +bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + return (*dispatchTable[shapeA->getType() + NUM_SHAPE_TYPES * shapeB->getType()])(shapeA, shapeB, collisions); } static CollisionList tempCollisions(32); @@ -133,20 +143,20 @@ bool collideShapesWithShapes(const QVector& shapesA, const QVectorgetType(); - if (typeA == Shape::SPHERE_SHAPE) { + Shape::Type typeA = shapeA->getType(); + if (typeA == SPHERE_SHAPE) { return sphereAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); - } else if (typeA == Shape::CAPSULE_SHAPE) { + } else if (typeA == CAPSULE_SHAPE) { return capsuleAACube(static_cast(shapeA), cubeCenter, cubeSide, collisions); - } else if (typeA == Shape::LIST_SHAPE) { + } else if (typeA == LIST_SHAPE) { const ListShape* listA = static_cast(shapeA); 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) { + if (subType == SPHERE_SHAPE) { touching = sphereAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; - } else if (subType == Shape::CAPSULE_SHAPE) { + } else if (subType == CAPSULE_SHAPE) { touching = capsuleAACube(static_cast(subShape), cubeCenter, cubeSide, collisions) || touching; } } @@ -155,7 +165,9 @@ bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, fl return false; } -bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions) { +bool sphereSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const SphereShape* sphereA = static_cast(shapeA); + const SphereShape* sphereB = static_cast(shapeB); glm::vec3 BA = sphereB->getTranslation() - sphereA->getTranslation(); float distanceSquared = glm::dot(BA, BA); float totalRadius = sphereA->getRadius() + sphereB->getRadius(); @@ -183,7 +195,9 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis return false; } -bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions) { +bool sphereCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const SphereShape* sphereA = static_cast(shapeA); + const CapsuleShape* capsuleB = static_cast(shapeB); // find sphereA's closest approach to axis of capsuleB glm::vec3 BA = capsuleB->getTranslation() - sphereA->getTranslation(); glm::vec3 capsuleAxis; @@ -252,7 +266,9 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col return false; } -bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, CollisionList& collisions) { +bool spherePlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const SphereShape* sphereA = static_cast(shapeA); + const PlaneShape* planeB = static_cast(shapeB); glm::vec3 penetration; if (findSpherePlanePenetration(sphereA->getTranslation(), sphereA->getRadius(), planeB->getCoefficients(), penetration)) { CollisionInfo* collision = collisions.getNewCollision(); @@ -268,7 +284,9 @@ bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, Collision return false; } -bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions) { +bool capsuleSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const CapsuleShape* capsuleA = static_cast(shapeA); + const SphereShape* sphereB = static_cast(shapeB); // find sphereB's closest approach to axis of capsuleA glm::vec3 AB = capsuleA->getTranslation() - sphereB->getTranslation(); glm::vec3 capsuleAxis; @@ -409,7 +427,9 @@ bool lineCylinder(const glm::vec3& lineP, const glm::vec3& lineDir, return true; } -bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions) { +bool capsuleCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const CapsuleShape* capsuleA = static_cast(shapeA); + const CapsuleShape* capsuleB = static_cast(shapeB); glm::vec3 axisA; capsuleA->computeNormalizedAxis(axisA); glm::vec3 axisB; @@ -568,7 +588,9 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, return false; } -bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, CollisionList& collisions) { +bool capsulePlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const CapsuleShape* capsuleA = static_cast(shapeA); + const PlaneShape* planeB = static_cast(shapeB); glm::vec3 start, end, penetration; capsuleA->getStartPoint(start); capsuleA->getEndPoint(end); @@ -588,7 +610,9 @@ bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, Collis return false; } -bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, CollisionList& collisions) { +bool planeSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const PlaneShape* planeA = static_cast(shapeA); + const SphereShape* sphereB = static_cast(shapeB); glm::vec3 penetration; if (findSpherePlanePenetration(sphereB->getTranslation(), sphereB->getRadius(), planeA->getCoefficients(), penetration)) { CollisionInfo* collision = collisions.getNewCollision(); @@ -605,7 +629,9 @@ bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, Collision return false; } -bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, CollisionList& collisions) { +bool planeCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + const PlaneShape* planeA = static_cast(shapeA); + const CapsuleShape* capsuleB = static_cast(shapeB); glm::vec3 start, end, penetration; capsuleB->getStartPoint(start); capsuleB->getEndPoint(end); @@ -625,110 +651,118 @@ bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, Collis return false; } -bool planePlane(const PlaneShape* planeA, const PlaneShape* planeB, CollisionList& collisions) { +bool planePlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { // technically, planes always collide unless they're parallel and not coincident; however, that's // not going to give us any useful information return false; } -bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions) { +bool sphereList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { bool touching = false; + const ListShape* listB = static_cast(shapeB); 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; - } else if (subType == Shape::PLANE_SHAPE) { - touching = spherePlane(sphereA, static_cast(subShape), collisions) || touching; + if (subType == SPHERE_SHAPE) { + touching = sphereSphere(shapeA, subShape, collisions) || touching; + } else if (subType == CAPSULE_SHAPE) { + touching = sphereCapsule(shapeA, subShape, collisions) || touching; + } else if (subType == PLANE_SHAPE) { + touching = spherePlane(shapeA, subShape, collisions) || touching; } } return touching; } -bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions) { +bool capsuleList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { bool touching = false; + const ListShape* listB = static_cast(shapeB); 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; - } else if (subType == Shape::PLANE_SHAPE) { - touching = capsulePlane(capsuleA, static_cast(subShape), collisions) || touching; + if (subType == SPHERE_SHAPE) { + touching = capsuleSphere(shapeA, subShape, collisions) || touching; + } else if (subType == CAPSULE_SHAPE) { + touching = capsuleCapsule(shapeA, subShape, collisions) || touching; + } else if (subType == PLANE_SHAPE) { + touching = capsulePlane(shapeA, subShape, collisions) || touching; } } return touching; } -bool planeList(const PlaneShape* planeA, const ListShape* listB, CollisionList& collisions) { +bool planeList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { bool touching = false; + const ListShape* listB = static_cast(shapeB); 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 = planeSphere(planeA, static_cast(subShape), collisions) || touching; - } else if (subType == Shape::CAPSULE_SHAPE) { - touching = planeCapsule(planeA, static_cast(subShape), collisions) || touching; - } else if (subType == Shape::PLANE_SHAPE) { - touching = planePlane(planeA, static_cast(subShape), collisions) || touching; + if (subType == SPHERE_SHAPE) { + touching = planeSphere(shapeA, subShape, collisions) || touching; + } else if (subType == CAPSULE_SHAPE) { + touching = planeCapsule(shapeA, subShape, collisions) || touching; + } else if (subType == PLANE_SHAPE) { + touching = planePlane(shapeA, subShape, collisions) || touching; } } return touching; } -bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions) { +bool listSphere(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { bool touching = false; + const ListShape* listA = static_cast(shapeA); 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; - } else if (subType == Shape::PLANE_SHAPE) { - touching = planeSphere(static_cast(subShape), sphereB, collisions) || touching; + if (subType == SPHERE_SHAPE) { + touching = sphereSphere(subShape, shapeB, collisions) || touching; + } else if (subType == CAPSULE_SHAPE) { + touching = capsuleSphere(subShape, shapeB, collisions) || touching; + } else if (subType == PLANE_SHAPE) { + touching = planeSphere(subShape, shapeB, collisions) || touching; } } return touching; } -bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions) { +bool listCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { bool touching = false; + const ListShape* listA = static_cast(shapeA); 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; - } else if (subType == Shape::PLANE_SHAPE) { - touching = planeCapsule(static_cast(subShape), capsuleB, collisions) || touching; + if (subType == SPHERE_SHAPE) { + touching = sphereCapsule(subShape, shapeB, collisions) || touching; + } else if (subType == CAPSULE_SHAPE) { + touching = capsuleCapsule(subShape, shapeB, collisions) || touching; + } else if (subType == PLANE_SHAPE) { + touching = planeCapsule(subShape, shapeB, collisions) || touching; } } return touching; } -bool listPlane(const ListShape* listA, const PlaneShape* planeB, CollisionList& collisions) { +bool listPlane(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { bool touching = false; + const ListShape* listA = static_cast(shapeA); 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 = spherePlane(static_cast(subShape), planeB, collisions) || touching; - } else if (subType == Shape::CAPSULE_SHAPE) { - touching = capsulePlane(static_cast(subShape), planeB, collisions) || touching; - } else if (subType == Shape::PLANE_SHAPE) { - touching = planePlane(static_cast(subShape), planeB, collisions) || touching; + if (subType == SPHERE_SHAPE) { + touching = spherePlane(subShape, shapeB, collisions) || touching; + } else if (subType == CAPSULE_SHAPE) { + touching = capsulePlane(subShape, shapeB, collisions) || touching; + } else if (subType == PLANE_SHAPE) { + touching = planePlane(subShape, shapeB, collisions) || touching; } } return touching; } -bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions) { +bool listList(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { bool touching = false; + const ListShape* listA = static_cast(shapeA); + const ListShape* listB = static_cast(shapeB); 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) { diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index b1be75fa40..11768f0a48 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -14,20 +14,28 @@ #include -#include "CapsuleShape.h" #include "CollisionInfo.h" -#include "ListShape.h" -#include "PlaneShape.h" #include "SharedUtil.h" -#include "SphereShape.h" +//#include "CapsuleShape.h" +//#include "ListShape.h" +//#include "PlaneShape.h" +//#include "SphereShape.h" + +class Shape; +class SphereShape; +class CapsuleShape; namespace ShapeCollider { + /// MUST CALL this FIRST before using the ShapeCollider + void initDispatchTable(); + /// \param shapeA pointer to first shape (cannot be NULL) /// \param shapeB pointer to second shape (cannot be NULL) /// \param collisions[out] collision details /// \return true if shapes collide bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); + bool collideShapesOld(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); /// \param shapesA list of shapes /// \param shapeB list of shapes @@ -49,97 +57,97 @@ namespace ShapeCollider { /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions); + bool sphereSphere(const Shape* sphereA, const Shape* sphereB, CollisionList& collisions); /// \param sphereA pointer to first shape (cannot be NULL) /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions); + bool sphereCapsule(const Shape* sphereA, const Shape* capsuleB, CollisionList& collisions); /// \param sphereA pointer to first shape (cannot be NULL) /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, CollisionList& collisions); + bool spherePlane(const Shape* sphereA, const Shape* planeB, CollisionList& collisions); /// \param capsuleA pointer to first shape (cannot be NULL) /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions); + bool capsuleSphere(const Shape* capsuleA, const Shape* sphereB, CollisionList& collisions); /// \param capsuleA pointer to first shape (cannot be NULL) /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions); + bool capsuleCapsule(const Shape* capsuleA, const Shape* capsuleB, CollisionList& collisions); /// \param capsuleA pointer to first shape (cannot be NULL) /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, CollisionList& collisions); + bool capsulePlane(const Shape* capsuleA, const Shape* planeB, CollisionList& collisions); /// \param planeA pointer to first shape (cannot be NULL) /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, CollisionList& collisions); + bool planeSphere(const Shape* planeA, const Shape* sphereB, CollisionList& collisions); /// \param planeA pointer to first shape (cannot be NULL) /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, CollisionList& collisions); + bool planeCapsule(const Shape* planeA, const Shape* capsuleB, CollisionList& collisions); /// \param planeA pointer to first shape (cannot be NULL) /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool planePlane(const PlaneShape* planeA, const PlaneShape* planeB, CollisionList& collisions); + bool planePlane(const Shape* planeA, const Shape* planeB, CollisionList& collisions); /// \param sphereA pointer to first shape (cannot be NULL) /// \param listB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions); + bool sphereList(const Shape* sphereA, const Shape* listB, CollisionList& collisions); /// \param capuleA pointer to first shape (cannot be NULL) /// \param listB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions); + bool capsuleList(const Shape* capsuleA, const Shape* listB, CollisionList& collisions); /// \param planeA pointer to first shape (cannot be NULL) /// \param listB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool planeList(const PlaneShape* planeA, const ListShape* listB, CollisionList& collisions); + bool planeList(const Shape* planeA, const Shape* listB, CollisionList& collisions); /// \param listA pointer to first shape (cannot be NULL) /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions); + bool listSphere(const Shape* listA, const Shape* sphereB, CollisionList& collisions); /// \param listA pointer to first shape (cannot be NULL) /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions); + bool listCapsule(const Shape* listA, const Shape* capsuleB, CollisionList& collisions); /// \param listA pointer to first shape (cannot be NULL) /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool listPlane(const ListShape* listA, const PlaneShape* planeB, CollisionList& collisions); + bool listPlane(const Shape* listA, const Shape* planeB, CollisionList& collisions); /// \param listA pointer to first shape (cannot be NULL) /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide - bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions); + bool listList(const Shape* listA, const Shape* listB, CollisionList& collisions); /// \param sphereA pointer to sphere (cannot be NULL) /// \param cubeCenter center of cube diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index d2f2a8596f..0626927453 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -18,13 +18,13 @@ class SphereShape : public Shape { public: - SphereShape() : Shape(Shape::SPHERE_SHAPE) {} + SphereShape() : Shape(SPHERE_SHAPE) {} - SphereShape(float radius) : Shape(Shape::SPHERE_SHAPE) { + SphereShape(float radius) : Shape(SPHERE_SHAPE) { _boundingRadius = radius; } - SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE, position) { + SphereShape(float radius, const glm::vec3& position) : Shape(SPHERE_SHAPE, position) { _boundingRadius = radius; } diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index bde29ea588..db4833b20a 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -16,7 +16,9 @@ #include #include +#include #include +#include #include #include #include @@ -112,6 +114,7 @@ void ShapeColliderTests::sphereTouchesSphere() { if (!collision) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: null collision" << std::endl; + return; } // penetration points from sphereA into sphereB @@ -1298,7 +1301,41 @@ void ShapeColliderTests::rayMissesPlane() { } } +void ShapeColliderTests::measureTimeOfCollisionDispatch() { + /* KEEP for future manual testing + // create two non-colliding spheres + float radiusA = 7.0f; + float radiusB = 3.0f; + float alpha = 1.2f; + float beta = 1.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + + SphereShape sphereA(radiusA, origin); + SphereShape sphereB(radiusB, offsetDistance * offsetDirection); + CollisionList collisions(16); + + //int numTests = 1; + quint64 oldTime; + quint64 newTime; + int numTests = 100000000; + { + quint64 startTime = usecTimestampNow(); + for (int i = 0; i < numTests; ++i) { + ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); + } + quint64 endTime = usecTimestampNow(); + std::cout << numTests << " non-colliding collisions in " << (endTime - startTime) << " usec" << std::endl; + newTime = endTime - startTime; + } + */ +} + void ShapeColliderTests::runAllTests() { + ShapeCollider::initDispatchTable(); + + //measureTimeOfCollisionDispatch(); + sphereMissesSphere(); sphereTouchesSphere(); diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index fd9f1f9706..4a51651cb8 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -35,6 +35,8 @@ namespace ShapeColliderTests { void rayHitsPlane(); void rayMissesPlane(); + void measureTimeOfCollisionDispatch(); + void runAllTests(); } diff --git a/tests/physics/src/VerletShapeTests.cpp b/tests/physics/src/VerletShapeTests.cpp index 3a3bd43278..705ddeeac3 100644 --- a/tests/physics/src/VerletShapeTests.cpp +++ b/tests/physics/src/VerletShapeTests.cpp @@ -757,6 +757,8 @@ void VerletShapeTests::capsuleTouchesCapsule() { } void VerletShapeTests::runAllTests() { + ShapeCollider::initDispatchTable(); + setSpherePosition(); sphereMissesSphere(); sphereTouchesSphere();