From d34ad2af42854e38415d9b1debea048c3a741c05 Mon Sep 17 00:00:00 2001 From: Brad Hefta-Gaub Date: Tue, 20 Oct 2015 21:01:20 -0700 Subject: [PATCH] improved findEntities(sphere) to check against Non-AABBox of entities --- libraries/entities/src/EntityTreeElement.cpp | 84 +++++++++++++++++--- libraries/entities/src/EntityTreeElement.h | 7 +- 2 files changed, 77 insertions(+), 14 deletions(-) diff --git a/libraries/entities/src/EntityTreeElement.cpp b/libraries/entities/src/EntityTreeElement.cpp index 56ab27836c..34433cdbb1 100644 --- a/libraries/entities/src/EntityTreeElement.cpp +++ b/libraries/entities/src/EntityTreeElement.cpp @@ -609,25 +609,83 @@ EntityItemPointer EntityTreeElement::getClosestEntity(glm::vec3 position) const void EntityTreeElement::getEntities(const glm::vec3& searchPosition, float searchRadius, QVector& foundEntities) const { float compareRadius = searchRadius * searchRadius; forEachEntity([&](EntityItemPointer entity) { - // For iteration like this, avoid the use of square roots by comparing distances squared - float distanceSquared = glm::length2(entity->getPosition() - searchPosition); - float otherRadius = entity->getRadius(); - if (distanceSquared < (compareRadius + (otherRadius * otherRadius))) { + + AABox entityBox = entity->getAABox(); + + // if the sphere doesn't intersect with our world frame AABox, we don't need to consider the more complex case + glm::vec3 penetration; + if (entityBox.findSpherePenetration(searchPosition, searchRadius, penetration)) { + + // FIXME - handle entity->getShapeType() == SHAPE_TYPE_SPHERE case better + // FIXME - consider allowing the entity to determine penetration so that + // entities could presumably dull actuall hull testing if they wanted to + + // determine the worldToEntityMatrix that doesn't include scale because + // we're going to use the registration aware aa box in the entity frame + glm::mat4 rotation = glm::mat4_cast(entity->getRotation()); + glm::mat4 translation = glm::translate(entity->getPosition()); + glm::mat4 entityToWorldMatrix = translation * rotation; + glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix); + + glm::vec3 dimensions = entity->getDimensions(); + glm::vec3 registrationPoint = entity->getRegistrationPoint(); + glm::vec3 corner = -(dimensions * registrationPoint); + + AABox entityFrameBox(corner, dimensions); + + glm::vec3 entityFrameSearchPosition = glm::vec3(worldToEntityMatrix * glm::vec4(searchPosition, 1.0f)); + if (entityFrameBox.findSpherePenetration(entityFrameSearchPosition, searchRadius, penetration)) { + foundEntities.push_back(entity); + } + } + }); +} + +void EntityTreeElement::getEntities(const AACube& cube, QVector& foundEntities) { + forEachEntity([&](EntityItemPointer entity) { + AABox entityBox = entity->getAABox(); + // FIXME - handle entity->getShapeType() == SHAPE_TYPE_SPHERE case better + // FIXME - consider allowing the entity to determine penetration so that + // entities could presumably dull actuall hull testing if they wanted to + // FIXME - is there an easy way to translate the search cube into something in the + // entity frame that can be easily tested against? + // simple algorithm is probably: + // if target box is fully inside search box == yes + // if search box is fully inside target box == yes + // for each face of search box: + // translate the triangles of the face into the box frame + // test the triangles of the face against the box? + // if translated search face triangle intersect target box + // add to result + // + + // If the entities AABox touches the search cube then consider it to be found + if (entityBox.touches(cube)) { foundEntities.push_back(entity); } }); } -// TODO: change this to use better bounding shape for entity than sphere -void EntityTreeElement::getEntities(const AACube& box, QVector& foundEntities) { - AACube entityCube; +void EntityTreeElement::getEntities(const AABox& box, QVector& foundEntities) { forEachEntity([&](EntityItemPointer entity) { - float radius = entity->getRadius(); - // NOTE: we actually do cube-cube collision queries here, which is sloppy but good enough for now - // TODO: decide whether to replace entityCube-cube query with sphere-cube (requires a square root - // but will be slightly more accurate). - entityCube.setBox(entity->getPosition() - glm::vec3(radius), 2.0f * radius); - if (entityCube.touches(box)) { + AABox entityBox = entity->getAABox(); + // FIXME - handle entity->getShapeType() == SHAPE_TYPE_SPHERE case better + // FIXME - consider allowing the entity to determine penetration so that + // entities could presumably dull actuall hull testing if they wanted to + // FIXME - is there an easy way to translate the search cube into something in the + // entity frame that can be easily tested against? + // simple algorithm is probably: + // if target box is fully inside search box == yes + // if search box is fully inside target box == yes + // for each face of search box: + // translate the triangles of the face into the box frame + // test the triangles of the face against the box? + // if translated search face triangle intersect target box + // add to result + // + + // If the entities AABox touches the search cube then consider it to be found + if (entityBox.touches(box)) { foundEntities.push_back(entity); } }); diff --git a/libraries/entities/src/EntityTreeElement.h b/libraries/entities/src/EntityTreeElement.h index 0a47542e65..cb5e35945e 100644 --- a/libraries/entities/src/EntityTreeElement.h +++ b/libraries/entities/src/EntityTreeElement.h @@ -180,7 +180,12 @@ public: /// finds all entities that touch a box /// \param box the query box /// \param entities[out] vector of non-const EntityItemPointer - void getEntities(const AACube& box, QVector& foundEntities); + void getEntities(const AACube& cube, QVector& foundEntities); + + /// finds all entities that touch a box + /// \param box the query box + /// \param entities[out] vector of non-const EntityItemPointer + void getEntities(const AABox& box, QVector& foundEntities); EntityItemPointer getEntityWithID(uint32_t id) const; EntityItemPointer getEntityWithEntityItemID(const EntityItemID& id) const;