From 9cd76983a2f5ffed6f00485de8bab953019418f0 Mon Sep 17 00:00:00 2001 From: ZappoMan Date: Thu, 25 Sep 2014 14:35:20 -0700 Subject: [PATCH] swith to using shape collider instead of spheres --- .../entities/src/EntityCollisionSystem.cpp | 24 +++- libraries/entities/src/EntityItem.cpp | 15 ++- libraries/entities/src/EntityItem.h | 17 ++- libraries/entities/src/EntityTreeElement.cpp | 22 ++-- libraries/octree/src/Octree.cpp | 1 - libraries/shared/src/AACubeShape.h | 14 +++ libraries/shared/src/CollisionInfo.h | 2 + libraries/shared/src/Shape.h | 24 ++++ libraries/shared/src/ShapeCollider.cpp | 107 ++++++++++++++++++ libraries/shared/src/SphereShape.h | 22 ++++ 10 files changed, 224 insertions(+), 24 deletions(-) diff --git a/libraries/entities/src/EntityCollisionSystem.cpp b/libraries/entities/src/EntityCollisionSystem.cpp index c9040c7a6e..1bba7c4764 100644 --- a/libraries/entities/src/EntityCollisionSystem.cpp +++ b/libraries/entities/src/EntityCollisionSystem.cpp @@ -13,8 +13,10 @@ #include #include #include +#include #include #include +#include #include "EntityItem.h" #include "EntityCollisionSystem.h" @@ -106,8 +108,26 @@ void EntityCollisionSystem::updateCollisionWithEntities(EntityItem* entityA) { glm::vec3 center = entityA->getPosition() * (float)(TREE_SCALE); float radius = entityA->getRadius() * (float)(TREE_SCALE); glm::vec3 penetration; - EntityItem* entityB; - if (_entities->findSpherePenetration(center, radius, penetration, (void**)&entityB, Octree::NoLock)) { + EntityItem* entityB = NULL; + + const float MAX_COLLISIONS_PER_ENTITY = 32; + CollisionList collisions(MAX_COLLISIONS_PER_ENTITY); + bool shapeCollisionsAccurate = false; + bool shapeCollisions = _entities->findShapeCollisions(&entityA->getCollisionShapeInMeters(), + collisions, Octree::NoLock, &shapeCollisionsAccurate); + + if (shapeCollisions) { + for(int i = 0; i < collisions.size(); i++) { + CollisionInfo* collision = collisions[i]; + penetration = collision->_penetration; + entityB = static_cast(collision->_extraData); + + // TODO: how to handle multiple collisions? + break; + } + } + + if (shapeCollisions) { // NOTE: 'penetration' is the depth that 'entityA' overlaps 'entityB'. It points from A into B. glm::vec3 penetrationInTreeUnits = penetration / (float)(TREE_SCALE); diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index cfed16c443..84ba0bd45d 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -70,6 +70,8 @@ void EntityItem::initFromEntityItemID(const EntityItemID& entityItemID) { _angularVelocity = DEFAULT_ANGULAR_VELOCITY; _angularDamping = DEFAULT_ANGULAR_DAMPING; _visible = DEFAULT_VISIBLE; + + recalculateCollisionShape(); } EntityItem::EntityItem(const EntityItemID& entityItemID) { @@ -490,6 +492,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef bytesRead += readEntitySubclassDataFromBuffer(dataAt, (bytesLeftToRead - bytesRead), args, propertyFlags, overwriteLocalData); + recalculateCollisionShape(); } return bytesRead; } @@ -675,7 +678,7 @@ void EntityItem::update(const quint64& updateTime) { velocity = NO_VELOCITY; } - setPosition(position); + setPosition(position); // this will automatically recalculate our collision shape setVelocity(velocity); if (wantDebug) { @@ -749,7 +752,7 @@ bool EntityItem::setProperties(const EntityItemProperties& properties, bool forc } } - SET_ENTITY_PROPERTY_FROM_PROPERTIES(position, setPositionInMeters); + SET_ENTITY_PROPERTY_FROM_PROPERTIES(position, setPositionInMeters); // this will call recalculate collision shape if needed SET_ENTITY_PROPERTY_FROM_PROPERTIES(dimensions, setDimensionsInMeters); // NOTE: radius is obsolete SET_ENTITY_PROPERTY_FROM_PROPERTIES(rotation, setRotation); SET_ENTITY_PROPERTY_FROM_PROPERTIES(mass, setMass); @@ -903,3 +906,11 @@ float EntityItem::getRadius() const { return radius; } +void EntityItem::recalculateCollisionShape() { + AACube entityAACube = getMinimumAACube(); + entityAACube.scale(TREE_SCALE); // scale to meters + _collisionShape.setTranslation(entityAACube.calcCenter()); + _collisionShape.setScale(entityAACube.getScale()); +} + + diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index bbfba30e9c..bdd0d572d3 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -16,6 +16,7 @@ #include +#include #include // for Animation, AnimationCache, and AnimationPointer classes #include // for EncodeBitstreamParams class #include // for OctreeElement::AppendState @@ -123,7 +124,9 @@ public: EntityTypes::EntityType getType() const { return _type; } const glm::vec3& getPosition() const { return _position; } /// get position in domain scale units (0.0 - 1.0) glm::vec3 getPositionInMeters() const { return _position * (float) TREE_SCALE; } /// get position in meters - void setPosition(const glm::vec3& value) { _position = value; } /// set position in domain scale units (0.0 - 1.0) + + /// set position in domain scale units (0.0 - 1.0) + void setPosition(const glm::vec3& value) { _position = value; recalculateCollisionShape(); } void setPositionInMeters(const glm::vec3& value) /// set position in meter units (0.0 - TREE_SCALE) { setPosition(glm::clamp(value / (float) TREE_SCALE, 0.0f, 1.0f)); } @@ -137,14 +140,14 @@ public: float getLargestDimension() const { return glm::length(_dimensions); } /// get the largest possible dimension /// set dimensions in domain scale units (0.0 - 1.0) this will also reset radius appropriately - void setDimensions(const glm::vec3& value) { _dimensions = value; } + void setDimensions(const glm::vec3& value) { _dimensions = value; ; recalculateCollisionShape(); } /// set dimensions in meter units (0.0 - TREE_SCALE) this will also reset radius appropriately void setDimensionsInMeters(const glm::vec3& value) { setDimensions(value / (float) TREE_SCALE); } static const glm::quat DEFAULT_ROTATION; const glm::quat& getRotation() const { return _rotation; } - void setRotation(const glm::quat& rotation) { _rotation = rotation; } + void setRotation(const glm::quat& rotation) { _rotation = rotation; ; recalculateCollisionShape(); } static const float DEFAULT_GLOW_LEVEL; float getGlowLevel() const { return _glowLevel; } @@ -207,7 +210,10 @@ public: static const glm::vec3 DEFAULT_REGISTRATION_POINT; const glm::vec3& getRegistrationPoint() const { return _registrationPoint; } /// registration point as ratio of entity - void setRegistrationPoint(const glm::vec3& value) { _registrationPoint = glm::clamp(value, 0.0f, 1.0f); } /// registration point as ratio of entity + + /// registration point as ratio of entity + void setRegistrationPoint(const glm::vec3& value) + { _registrationPoint = glm::clamp(value, 0.0f, 1.0f); recalculateCollisionShape(); } static const glm::vec3 NO_ANGULAR_VELOCITY; static const glm::vec3 DEFAULT_ANGULAR_VELOCITY; @@ -229,9 +235,11 @@ public: float getRadius() const; void applyHardCollision(const CollisionInfo& collisionInfo); + virtual const Shape& getCollisionShapeInMeters() const { return _collisionShape; } protected: virtual void initFromEntityItemID(const EntityItemID& entityItemID); // maybe useful to allow subclasses to init + virtual void recalculateCollisionShape(); EntityTypes::EntityType _type; QUuid _id; @@ -264,6 +272,7 @@ protected: /// set radius in domain scale units (0.0 - 1.0) this will also reset dimensions to be equal for each axis void setRadius(float value); + AACubeShape _collisionShape; }; diff --git a/libraries/entities/src/EntityTreeElement.cpp b/libraries/entities/src/EntityTreeElement.cpp index bfc0f9af30..6e9ecbbd9a 100644 --- a/libraries/entities/src/EntityTreeElement.cpp +++ b/libraries/entities/src/EntityTreeElement.cpp @@ -551,25 +551,17 @@ bool EntityTreeElement::findSpherePenetration(const glm::vec3& center, float rad bool EntityTreeElement::findShapeCollisions(const Shape* shape, CollisionList& collisions) const { bool atLeastOneCollision = false; - //AACube cube = getAACube(); - //return ShapeCollider::collideShapeWithAACubeLegacy(shape, cube.calcCenter(), cube.getScale(), collisions); - QList::iterator entityItr = _entityItems->begin(); QList::const_iterator entityEnd = _entityItems->end(); while(entityItr != entityEnd) { EntityItem* entity = (*entityItr); - glm::vec3 entityCenter = entity->getPosition(); - float entityRadius = entity->getRadius(); - - // don't collide with yourself??? - //if (entityCenter == center && entityRadius == radius) { - // return false; - //} - AACube entityAACube = entity->getMinimumAACube(); - AACubeShape aaCube(entityAACube.getScale(), entityAACube.calcCenter()); - - if (ShapeCollider::collideShapes(shape, &aaCube, collisions)) { - atLeastOneCollision = true; + const Shape* otherCollisionShape = &entity->getCollisionShapeInMeters(); + if (shape != otherCollisionShape) { + if (ShapeCollider::collideShapes(shape, otherCollisionShape, collisions)) { + CollisionInfo* lastCollision = collisions.getLastCollision(); + lastCollision->_extraData = entity; + atLeastOneCollision = true; + } } ++entityItr; } diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index a834fb887a..ceda91a441 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -832,7 +832,6 @@ bool findCapsulePenetrationOp(OctreeElement* element, void* extraData) { bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { ShapeArgs* args = static_cast(extraData); - // coarse check against bounds AACube cube = element->getAACube(); cube.scale(TREE_SCALE); diff --git a/libraries/shared/src/AACubeShape.h b/libraries/shared/src/AACubeShape.h index 4b834aa1bf..da7ba9d53f 100644 --- a/libraries/shared/src/AACubeShape.h +++ b/libraries/shared/src/AACubeShape.h @@ -12,6 +12,7 @@ #ifndef hifi_AACubeShape_h #define hifi_AACubeShape_h +#include #include "Shape.h" class AACubeShape : public Shape { @@ -28,9 +29,22 @@ public: bool findRayIntersection(RayIntersectionInfo& intersection) const; float getVolume() const { return _scale * _scale * _scale; } + virtual QDebug& dumpToDebug(QDebug& debugConext) const; protected: float _scale; }; +inline QDebug& AACubeShape::dumpToDebug(QDebug& debugConext) const { + debugConext << "AACubeShape[ (" + << "type: " << getType() + << "position: " + << getTranslation().x << ", " << getTranslation().y << ", " << getTranslation().z + << "scale: " + << getScale() + << "]"; + + return debugConext; +} + #endif // hifi_AACubeShape_h diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index 6e70654d15..0f134c1b23 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -54,6 +54,8 @@ public: const Shape* _shapeA; // pointer to shapeA in this collision const Shape* _shapeB; // pointer to shapeB in this collision + void* _extraData; // pointer to extraData for this collision, opaque to the collision info, useful for external data + float _damping; // range [0,1] of friction coeficient float _elasticity; // range [0,1] of energy conservation glm::vec3 _contactPoint; // world-frame point on BodyA that is deepest into BodyB diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 4b85234eb3..cdf3ba72e5 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -80,6 +81,8 @@ public: virtual float getVolume() const { return 1.0; } virtual void getVerletPoints(QVector& points) {} + + virtual QDebug& dumpToDebug(QDebug& debugConext) const; protected: // these ctors are protected (used by derived classes only) @@ -113,4 +116,25 @@ protected: float _mass; }; +inline QDebug& Shape::dumpToDebug(QDebug& debugConext) const { + debugConext << "Shape[ (" + << "type: " << getType() + << "position: " + << getTranslation().x << ", " << getTranslation().y << ", " << getTranslation().z + << "radius: " + << getBoundingRadius() + << "]"; + + return debugConext; +} + +inline QDebug operator<<(QDebug debug, const Shape& shape) { + return shape.dumpToDebug(debug); +} + +inline QDebug operator<<(QDebug debug, const Shape* shape) { + return shape->dumpToDebug(debug); +} + + #endif // hifi_Shape_h diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index be3b086776..3f79fa081a 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -20,6 +20,8 @@ #include "PlaneShape.h" #include "SphereShape.h" +#include "StreamUtils.h" + // NOTE: // // * Large ListShape's are inefficient keep the lists short. @@ -978,7 +980,112 @@ bool aaCubeVsCapsule(const Shape* shapeA, const Shape* shapeB, CollisionList& co return capsuleVsAACube(shapeB, shapeA, collisions); } +// helper function +CollisionInfo* aaCubeVsAACubeHelper(const glm::vec3& cubeCenterA, float cubeSideA, const glm::vec3& cubeCenterB, + float cubeSideB, CollisionList& collisions) { + // cube is A + // cube is B + // BA = B - A = from center of A to center of B + float halfCubeSideA = 0.5f * cubeSideA; + float halfCubeSideB = 0.5f * cubeSideB; + glm::vec3 BA = cubeCenterB - cubeCenterA; + + float distance = glm::length(BA); + + if (distance > EPSILON) { + float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); + if (maxBA > halfCubeSideB + halfCubeSideA) { + // cube misses cube entirely + return NULL; + } + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + return NULL; // no more room for collisions + } + if (maxBA > halfCubeSideB) { + // cube hits cube but its center is outside cube + // compute contact anti-pole on cube (in cube frame) + glm::vec3 cubeContact = glm::abs(BA); + if (cubeContact.x > halfCubeSideB) { + cubeContact.x = halfCubeSideB; + } + if (cubeContact.y > halfCubeSideB) { + cubeContact.y = halfCubeSideB; + } + if (cubeContact.z > halfCubeSideB) { + cubeContact.z = halfCubeSideB; + } + glm::vec3 signs = glm::sign(BA); + cubeContact.x *= signs.x; + cubeContact.y *= signs.y; + cubeContact.z *= signs.z; + + // compute penetration direction + glm::vec3 direction = BA - cubeContact; + + float lengthDirection = glm::length(direction); + + if (lengthDirection < EPSILON) { + // cubeCenterA is touching cube B surface, so we can't use the difference between those two + // points to compute the penetration direction. Instead we use the unitary components of + // cubeContact. + glm::modf(cubeContact / halfCubeSideB, direction); + lengthDirection = glm::length(direction); + } else if (lengthDirection > halfCubeSideA) { + collisions.deleteLastCollision(); + return NULL; + } + direction /= lengthDirection; + + // compute collision details + collision->_contactPoint = cubeCenterA + halfCubeSideA * direction; + collision->_penetration = halfCubeSideA * direction - (BA - cubeContact); + } else { + // cube center is inside cube + // --> push out nearest face + glm::vec3 direction; + BA /= maxBA; + glm::modf(BA, direction); + float lengthDirection = glm::length(direction); + direction /= lengthDirection; + + // compute collision details + collision->_floatData = cubeSideB; + collision->_vecData = cubeCenterB; + collision->_penetration = (halfCubeSideB * lengthDirection + halfCubeSideA - maxBA * glm::dot(BA, direction)) * direction; + collision->_contactPoint = cubeCenterA + halfCubeSideA * direction; + } + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return collision; + } else if (halfCubeSideA + halfCubeSideB > distance) { + // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means + // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) + CollisionInfo* collision = collisions.getNewCollision(); + if (collision) { + // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis) + collision->_penetration = (halfCubeSideA + halfCubeSideB) * glm::vec3(0.0f, -1.0f, 0.0f); + // contactPoint is on surface of A + collision->_contactPoint = cubeCenterA + collision->_penetration; + collision->_shapeA = NULL; + collision->_shapeB = NULL; + return collision; + } + } + return NULL; +} + bool aaCubeVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { + // BA = B - A = from center of A to center of B + const AACubeShape* cubeA = static_cast(shapeA); + const AACubeShape* cubeB = static_cast(shapeB); + CollisionInfo* collision = aaCubeVsAACubeHelper( cubeA->getTranslation(), cubeA->getScale(), + cubeB->getTranslation(), cubeB->getScale(), collisions); + if (collision) { + collision->_shapeA = shapeA; + collision->_shapeB = shapeB; + return true; + } return false; } diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index b5f2c50d8f..59a53c97d6 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -39,4 +39,26 @@ public: float getVolume() const { return 1.3333333333f * PI * _boundingRadius * _boundingRadius * _boundingRadius; } }; +inline QDebug operator<<(QDebug debug, const SphereShape& shape) { + debug << "SphereShape[ (" + << "position: " + << shape.getTranslation().x << ", " << shape.getTranslation().y << ", " << shape.getTranslation().z + << "radius: " + << shape.getRadius() + << "]"; + + return debug; +} + +inline QDebug operator<<(QDebug debug, const SphereShape* shape) { + debug << "SphereShape[ (" + << "center: " + << shape->getTranslation().x << ", " << shape->getTranslation().y << ", " << shape->getTranslation().z + << "radius: " + << shape->getRadius() + << "]"; + + return debug; +} + #endif // hifi_SphereShape_h