Merge pull request #9759 from sethalves/voxel-paint-opti

no bullet shape for collisionless polyvox

libraries/entities-renderer/src/EntityTreeRenderer.cpp

@@ -944,7 +944,10 @@ void EntityTreeRenderer::entityScriptChanging(const EntityItemID& entityID, cons
 void EntityTreeRenderer::checkAndCallPreload(const EntityItemID& entityID, const bool reload, const bool unloadFirst) {
     if (_tree && !_shuttingDown) {
         EntityItemPointer entity = getTree()->findEntityByEntityItemID(entityID);
-        bool shouldLoad = entity && entity->shouldPreloadScript() && _entitiesScriptEngine;
+        if (!entity) {
+            return;
+        }
+        bool shouldLoad = entity->shouldPreloadScript() && _entitiesScriptEngine;
         QString scriptUrl = entity->getScript();
         if ((unloadFirst && shouldLoad) || scriptUrl.isEmpty()) {
             _entitiesScriptEngine->unloadEntityScript(entityID);

libraries/entities-renderer/src/RenderablePolyVoxEntityItem.cpp

@@ -266,6 +266,35 @@ void RenderablePolyVoxEntityItem::forEachVoxelValue(quint16 voxelXSize, quint16
     });
 }
 
+QByteArray RenderablePolyVoxEntityItem::volDataToArray(quint16 voxelXSize, quint16 voxelYSize, quint16 voxelZSize) const {
+    int totalSize = voxelXSize * voxelYSize * voxelZSize;
+    QByteArray result = QByteArray(totalSize, '\0');
+    int index = 0;
+    int lowX = 0;
+    int lowY = 0;
+    int lowZ = 0;
+
+    withReadLock([&] {
+        if (isEdged(_voxelSurfaceStyle)) {
+            lowX++;
+            lowY++;
+            lowZ++;
+            voxelXSize++;
+            voxelYSize++;
+            voxelZSize++;
+        }
+
+        for (int z = lowZ; z < voxelZSize; z++) {
+            for (int y = lowY; y < voxelYSize; y++) {
+                for (int x = lowX; x < voxelXSize; x++) {
+                    result[index++] = _volData->getVoxelAt(x, y, z);
+                }
+            }
+        }
+    });
+
+    return result;
+}
 
 bool RenderablePolyVoxEntityItem::setAll(uint8_t toValue) {
     bool result = false;
@@ -365,12 +394,28 @@ bool RenderablePolyVoxEntityItem::setSphere(glm::vec3 centerWorldCoords, float r
     }
 
     glm::mat4 vtwMatrix = voxelToWorldMatrix();
+    glm::mat4 wtvMatrix = glm::inverse(vtwMatrix);
 
-    // This three-level for loop iterates over every voxel in the volume
+    glm::vec3 dimensions = getDimensions();
+    glm::vec3 voxelSize = dimensions / _voxelVolumeSize;
+    float smallestDimensionSize = voxelSize.x;
+    smallestDimensionSize = glm::min(smallestDimensionSize, voxelSize.y);
+    smallestDimensionSize = glm::min(smallestDimensionSize, voxelSize.z);
+
+    glm::vec3 maxRadiusInVoxelCoords = glm::vec3(radiusWorldCoords / smallestDimensionSize);
+    glm::vec3 centerInVoxelCoords = wtvMatrix * glm::vec4(centerWorldCoords, 1.0f);
+
+    glm::vec3 low = glm::floor(centerInVoxelCoords - maxRadiusInVoxelCoords);
+    glm::vec3 high = glm::ceil(centerInVoxelCoords + maxRadiusInVoxelCoords);
+
+    glm::ivec3 lowI = glm::clamp(low, glm::vec3(0.0f), _voxelVolumeSize);
+    glm::ivec3 highI = glm::clamp(high, glm::vec3(0.0f), _voxelVolumeSize);
+
+    // This three-level for loop iterates over every voxel in the volume that might be in the sphere
     withWriteLock([&] {
-        for (int z = 0; z < _voxelVolumeSize.z; z++) {
-            for (int y = 0; y < _voxelVolumeSize.y; y++) {
-                for (int x = 0; x < _voxelVolumeSize.x; x++) {
+        for (int z = lowI.z; z < highI.z; z++) {
+            for (int y = lowI.y; y < highI.y; y++) {
+                for (int x = lowI.x; x < highI.x; x++) {
                     // Store our current position as a vector...
                     glm::vec4 pos(x + 0.5f, y + 0.5f, z + 0.5f, 1.0); // consider voxels cenetered on their coordinates
                     // convert to world coordinates
@@ -392,6 +437,59 @@ bool RenderablePolyVoxEntityItem::setSphere(glm::vec3 centerWorldCoords, float r
     return result;
 }
 
+bool RenderablePolyVoxEntityItem::setCapsule(glm::vec3 startWorldCoords, glm::vec3 endWorldCoords,
+                                             float radiusWorldCoords, uint8_t toValue) {
+    bool result = false;
+    if (_locked) {
+        return result;
+    }
+
+    glm::mat4 vtwMatrix = voxelToWorldMatrix();
+    glm::mat4 wtvMatrix = glm::inverse(vtwMatrix);
+
+    glm::vec3 dimensions = getDimensions();
+    glm::vec3 voxelSize = dimensions / _voxelVolumeSize;
+    float smallestDimensionSize = voxelSize.x;
+    smallestDimensionSize = glm::min(smallestDimensionSize, voxelSize.y);
+    smallestDimensionSize = glm::min(smallestDimensionSize, voxelSize.z);
+
+    glm::vec3 maxRadiusInVoxelCoords = glm::vec3(radiusWorldCoords / smallestDimensionSize);
+
+    glm::vec3 startInVoxelCoords = wtvMatrix * glm::vec4(startWorldCoords, 1.0f);
+    glm::vec3 endInVoxelCoords = wtvMatrix * glm::vec4(endWorldCoords, 1.0f);
+
+    glm::vec3 low = glm::min(glm::floor(startInVoxelCoords - maxRadiusInVoxelCoords),
+                             glm::floor(endInVoxelCoords - maxRadiusInVoxelCoords));
+    glm::vec3 high = glm::max(glm::ceil(startInVoxelCoords + maxRadiusInVoxelCoords),
+                              glm::ceil(endInVoxelCoords + maxRadiusInVoxelCoords));
+
+    glm::ivec3 lowI = glm::clamp(low, glm::vec3(0.0f), _voxelVolumeSize);
+    glm::ivec3 highI = glm::clamp(high, glm::vec3(0.0f), _voxelVolumeSize);
+
+    // This three-level for loop iterates over every voxel in the volume that might be in the capsule
+    withWriteLock([&] {
+        for (int z = lowI.z; z < highI.z; z++) {
+            for (int y = lowI.y; y < highI.y; y++) {
+                for (int x = lowI.x; x < highI.x; x++) {
+                    // Store our current position as a vector...
+                    glm::vec4 pos(x + 0.5f, y + 0.5f, z + 0.5f, 1.0); // consider voxels cenetered on their coordinates
+                    // convert to world coordinates
+                    glm::vec3 worldPos = glm::vec3(vtwMatrix * pos);
+                    if (pointInCapsule(worldPos, startWorldCoords, endWorldCoords, radiusWorldCoords)) {
+                        result |= setVoxelInternal(x, y, z, toValue);
+                    }
+                }
+            }
+        }
+    });
+
+    if (result) {
+        compressVolumeDataAndSendEditPacket();
+    }
+    return result;
+}
+
+
 class RaycastFunctor
 {
 public:
@@ -501,6 +599,9 @@ PolyVox::RaycastResult RenderablePolyVoxEntityItem::doRayCast(glm::vec4 originIn
 
 // virtual
 ShapeType RenderablePolyVoxEntityItem::getShapeType() const {
+    if (_collisionless) {
+        return SHAPE_TYPE_NONE;
+    }
     return SHAPE_TYPE_COMPOUND;
 }
 
@@ -512,6 +613,11 @@ void RenderablePolyVoxEntityItem::updateRegistrationPoint(const glm::vec3& value
 }
 
 bool RenderablePolyVoxEntityItem::isReadyToComputeShape() {
+    ShapeType shapeType = getShapeType();
+    if (shapeType == SHAPE_TYPE_NONE) {
+        return true;
+    }
+
     // we determine if we are ready to compute the physics shape by actually doing so.
     // if _voxelDataDirty or _volDataDirty is set, don't do this yet -- wait for their
     // threads to finish before creating the collision shape.
@@ -524,6 +630,12 @@ bool RenderablePolyVoxEntityItem::isReadyToComputeShape() {
 }
 
 void RenderablePolyVoxEntityItem::computeShapeInfo(ShapeInfo& info) {
+    ShapeType shapeType = getShapeType();
+    if (shapeType == SHAPE_TYPE_NONE) {
+        info.setParams(getShapeType(), 0.5f * getDimensions());
+        return;
+    }
+
     // the shape was actually computed in isReadyToComputeShape.  Just hand it off, here.
     withWriteLock([&] {
         info = _shapeInfo;
@@ -736,7 +848,7 @@ glm::vec3 RenderablePolyVoxEntityItem::localCoordsToVoxelCoords(glm::vec3& local
 
 void RenderablePolyVoxEntityItem::setVoxelVolumeSize(glm::vec3 voxelVolumeSize) {
     // This controls how many individual voxels are in the entity.  This is unrelated to
-    // the dimentions of the entity -- it defines the size of the arrays that hold voxel values.
+    // the dimentions of the entity -- it defines the sizes of the arrays that hold voxel values.
     // In addition to setting the number of voxels, this is used in a few places for its
     // side-effect of allocating _volData to be the correct size.
     withWriteLock([&] {
@@ -807,7 +919,7 @@ uint8_t RenderablePolyVoxEntityItem::getVoxel(int x, int y, int z) {
 }
 
 
-uint8_t RenderablePolyVoxEntityItem::getVoxelInternal(int x, int y, int z) {
+uint8_t RenderablePolyVoxEntityItem::getVoxelInternal(int x, int y, int z) const {
     if (!inUserBounds(_volData, _voxelSurfaceStyle, x, y, z)) {
         return 0;
     }
@@ -949,17 +1061,8 @@ void RenderablePolyVoxEntityItem::compressVolumeDataAndSendEditPacket() {
     EntityTreePointer tree = element ? element->getTree() : nullptr;
 
     QtConcurrent::run([voxelXSize, voxelYSize, voxelZSize, entity, tree] {
-        int rawSize = voxelXSize * voxelYSize * voxelZSize;
-        QByteArray uncompressedData = QByteArray(rawSize, '\0');
-
         auto polyVoxEntity = std::static_pointer_cast<RenderablePolyVoxEntityItem>(entity);
-        polyVoxEntity->forEachVoxelValue(voxelXSize, voxelYSize, voxelZSize, [&] (int x, int y, int z, uint8_t uVoxelValue) {
-            int uncompressedIndex =
-                z * voxelYSize * voxelXSize +
-                y * voxelXSize +
-                x;
-            uncompressedData[uncompressedIndex] = uVoxelValue;
-        });
+        QByteArray uncompressedData = polyVoxEntity->volDataToArray(voxelXSize, voxelYSize, voxelZSize);
 
         QByteArray newVoxelData;
         QDataStream writer(&newVoxelData, QIODevice::WriteOnly | QIODevice::Truncate);
@@ -1174,7 +1277,9 @@ void RenderablePolyVoxEntityItem::setMesh(model::MeshPointer mesh) {
     // this catches the payload from getMesh
     bool neighborsNeedUpdate;
     withWriteLock([&] {
-        _dirtyFlags |= Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS;
+        if (!_collisionless) {
+            _dirtyFlags |= Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS;
+        }
         _mesh = mesh;
         _meshDirty = true;
         _meshInitialized = true;

libraries/entities-renderer/src/RenderablePolyVoxEntityItem.h

@@ -94,6 +94,8 @@ public:
 
     // coords are in world-space
     virtual bool setSphere(glm::vec3 center, float radius, uint8_t toValue) override;
+    virtual bool setCapsule(glm::vec3 startWorldCoords, glm::vec3 endWorldCoords,
+                            float radiusWorldCoords, uint8_t toValue) override;
     virtual bool setAll(uint8_t toValue) override;
     virtual bool setCuboid(const glm::vec3& lowPosition, const glm::vec3& cuboidSize, int toValue) override;
 
@@ -128,12 +130,13 @@ public:
     void setVoxelsFromData(QByteArray uncompressedData, quint16 voxelXSize, quint16 voxelYSize, quint16 voxelZSize);
     void forEachVoxelValue(quint16 voxelXSize, quint16 voxelYSize, quint16 voxelZSize,
                            std::function<void(int, int, int, uint8_t)> thunk);
+    QByteArray volDataToArray(quint16 voxelXSize, quint16 voxelYSize, quint16 voxelZSize) const;
 
     void setMesh(model::MeshPointer mesh);
     void setCollisionPoints(ShapeInfo::PointCollection points, AABox box);
     PolyVox::SimpleVolume<uint8_t>* getVolData() { return _volData; }
 
-    uint8_t getVoxelInternal(int x, int y, int z);
+    uint8_t getVoxelInternal(int x, int y, int z) const;
     bool setVoxelInternal(int x, int y, int z, uint8_t toValue);
 
     void setVolDataDirty() { withWriteLock([&] { _volDataDirty = true; }); }

libraries/entities/src/EntityScriptingInterface.cpp

@@ -285,7 +285,7 @@ EntityItemProperties EntityScriptingInterface::getEntityProperties(QUuid identit
                     desiredProperties = entity->getEntityProperties(params);
                     desiredProperties.setHasProperty(PROP_LOCAL_POSITION);
                     desiredProperties.setHasProperty(PROP_LOCAL_ROTATION);
-                 }
+                }
 
                 results = entity->getProperties(desiredProperties);
 
@@ -825,7 +825,7 @@ bool EntityScriptingInterface::setVoxels(QUuid entityID,
 
     EntityItemPointer entity = _entityTree->findEntityByEntityItemID(entityID);
     if (!entity) {
-        qCDebug(entities) << "EntityScriptingInterface::setVoxelSphere no entity with ID" << entityID;
+        qCDebug(entities) << "EntityScriptingInterface::setVoxels no entity with ID" << entityID;
         return false;
     }
 
@@ -887,24 +887,34 @@ bool EntityScriptingInterface::setVoxelSphere(QUuid entityID, const glm::vec3& c
     PROFILE_RANGE(script_entities, __FUNCTION__);
 
     return setVoxels(entityID, [center, radius, value](PolyVoxEntityItem& polyVoxEntity) {
-            return polyVoxEntity.setSphere(center, radius, value);
-        });
+        return polyVoxEntity.setSphere(center, radius, value);
+    });
+}
+
+bool EntityScriptingInterface::setVoxelCapsule(QUuid entityID,
+                                               const glm::vec3& start, const glm::vec3& end,
+                                               float radius, int value) {
+    PROFILE_RANGE(script_entities, __FUNCTION__);
+
+    return setVoxels(entityID, [start, end, radius, value](PolyVoxEntityItem& polyVoxEntity) {
+        return polyVoxEntity.setCapsule(start, end, radius, value);
+    });
 }
 
 bool EntityScriptingInterface::setVoxel(QUuid entityID, const glm::vec3& position, int value) {
     PROFILE_RANGE(script_entities, __FUNCTION__);
 
     return setVoxels(entityID, [position, value](PolyVoxEntityItem& polyVoxEntity) {
-            return polyVoxEntity.setVoxelInVolume(position, value);
-        });
+        return polyVoxEntity.setVoxelInVolume(position, value);
+    });
 }
 
 bool EntityScriptingInterface::setAllVoxels(QUuid entityID, int value) {
     PROFILE_RANGE(script_entities, __FUNCTION__);
 
     return setVoxels(entityID, [value](PolyVoxEntityItem& polyVoxEntity) {
-            return polyVoxEntity.setAll(value);
-        });
+        return polyVoxEntity.setAll(value);
+    });
 }
 
 bool EntityScriptingInterface::setVoxelsInCuboid(QUuid entityID, const glm::vec3& lowPosition,
@@ -912,8 +922,8 @@ bool EntityScriptingInterface::setVoxelsInCuboid(QUuid entityID, const glm::vec3
     PROFILE_RANGE(script_entities, __FUNCTION__);
 
     return setVoxels(entityID, [lowPosition, cuboidSize, value](PolyVoxEntityItem& polyVoxEntity) {
-            return polyVoxEntity.setCuboid(lowPosition, cuboidSize, value);
-        });
+        return polyVoxEntity.setCuboid(lowPosition, cuboidSize, value);
+    });
 }
 
 bool EntityScriptingInterface::setAllPoints(QUuid entityID, const QVector<glm::vec3>& points) {
@@ -1020,25 +1030,25 @@ QUuid EntityScriptingInterface::addAction(const QString& actionTypeString,
     auto actionFactory = DependencyManager::get<EntityActionFactoryInterface>();
     bool success = false;
     actionWorker(entityID, [&](EntitySimulationPointer simulation, EntityItemPointer entity) {
-            // create this action even if the entity doesn't have physics info.  it will often be the
-            // case that a script adds an action immediately after an object is created, and the physicsInfo
-            // is computed asynchronously.
-            // if (!entity->getPhysicsInfo()) {
-            //     return false;
-            // }
-            EntityActionType actionType = EntityActionInterface::actionTypeFromString(actionTypeString);
-            if (actionType == ACTION_TYPE_NONE) {
-                return false;
-            }
-            EntityActionPointer action = actionFactory->factory(actionType, actionID, entity, arguments);
-            if (!action) {
-                return false;
-            }
-            action->setIsMine(true);
-            success = entity->addAction(simulation, action);
-            entity->grabSimulationOwnership();
-            return false; // Physics will cause a packet to be sent, so don't send from here.
-        });
+        // create this action even if the entity doesn't have physics info.  it will often be the
+        // case that a script adds an action immediately after an object is created, and the physicsInfo
+        // is computed asynchronously.
+        // if (!entity->getPhysicsInfo()) {
+        //     return false;
+        // }
+        EntityActionType actionType = EntityActionInterface::actionTypeFromString(actionTypeString);
+        if (actionType == ACTION_TYPE_NONE) {
+            return false;
+        }
+        EntityActionPointer action = actionFactory->factory(actionType, actionID, entity, arguments);
+        if (!action) {
+            return false;
+        }
+        action->setIsMine(true);
+        success = entity->addAction(simulation, action);
+        entity->grabSimulationOwnership();
+        return false; // Physics will cause a packet to be sent, so don't send from here.
+    });
     if (success) {
         return actionID;
     }
@@ -1050,12 +1060,12 @@ bool EntityScriptingInterface::updateAction(const QUuid& entityID, const QUuid&
     PROFILE_RANGE(script_entities, __FUNCTION__);
 
     return actionWorker(entityID, [&](EntitySimulationPointer simulation, EntityItemPointer entity) {
-            bool success = entity->updateAction(simulation, actionID, arguments);
-            if (success) {
-                entity->grabSimulationOwnership();
-            }
-            return success;
-        });
+        bool success = entity->updateAction(simulation, actionID, arguments);
+        if (success) {
+            entity->grabSimulationOwnership();
+        }
+        return success;
+    });
 }
 
 bool EntityScriptingInterface::deleteAction(const QUuid& entityID, const QUuid& actionID) {
@@ -1063,13 +1073,13 @@ bool EntityScriptingInterface::deleteAction(const QUuid& entityID, const QUuid&
 
     bool success = false;
     actionWorker(entityID, [&](EntitySimulationPointer simulation, EntityItemPointer entity) {
-            success = entity->removeAction(simulation, actionID);
-            if (success) {
-                // reduce from grab to poke
-                entity->pokeSimulationOwnership();
-            }
-            return false; // Physics will cause a packet to be sent, so don't send from here.
-        });
+        success = entity->removeAction(simulation, actionID);
+        if (success) {
+            // reduce from grab to poke
+            entity->pokeSimulationOwnership();
+        }
+        return false; // Physics will cause a packet to be sent, so don't send from here.
+    });
     return success;
 }
 
@@ -1078,10 +1088,10 @@ QVector<QUuid> EntityScriptingInterface::getActionIDs(const QUuid& entityID) {
 
     QVector<QUuid> result;
     actionWorker(entityID, [&](EntitySimulationPointer simulation, EntityItemPointer entity) {
-            QList<QUuid> actionIDs = entity->getActionIDs();
-            result = QVector<QUuid>::fromList(actionIDs);
-            return false; // don't send an edit packet
-        });
+        QList<QUuid> actionIDs = entity->getActionIDs();
+        result = QVector<QUuid>::fromList(actionIDs);
+        return false; // don't send an edit packet
+    });
     return result;
 }
 
@@ -1090,9 +1100,9 @@ QVariantMap EntityScriptingInterface::getActionArguments(const QUuid& entityID,
 
     QVariantMap result;
     actionWorker(entityID, [&](EntitySimulationPointer simulation, EntityItemPointer entity) {
-            result = entity->getActionArguments(actionID);
-            return false; // don't send an edit packet
-        });
+        result = entity->getActionArguments(actionID);
+        return false; // don't send an edit packet
+    });
     return result;
 }
 
@@ -1523,3 +1533,11 @@ QObject* EntityScriptingInterface::getWebViewRoot(const QUuid& entityID) {
         return nullptr;
     }
 }
+
+// TODO move this someplace that makes more sense...
+bool EntityScriptingInterface::AABoxIntersectsCapsule(const glm::vec3& low, const glm::vec3& dimensions,
+                                                      const glm::vec3& start, const glm::vec3& end, float radius) {
+    glm::vec3 penetration;
+    AABox aaBox(low, dimensions);
+    return aaBox.findCapsulePenetration(start, end, radius, penetration);
+}

libraries/entities/src/EntityScriptingInterface.h

@@ -223,6 +223,8 @@ public slots:
     Q_INVOKABLE bool getDrawZoneBoundaries() const;
 
     Q_INVOKABLE bool setVoxelSphere(QUuid entityID, const glm::vec3& center, float radius, int value);
+    Q_INVOKABLE bool setVoxelCapsule(QUuid entityID, const glm::vec3& start, const glm::vec3& end, float radius, int value);
+
     Q_INVOKABLE bool setVoxel(QUuid entityID, const glm::vec3& position, int value);
     Q_INVOKABLE bool setAllVoxels(QUuid entityID, int value);
     Q_INVOKABLE bool setVoxelsInCuboid(QUuid entityID, const glm::vec3& lowPosition,
@@ -287,6 +289,10 @@ public slots:
 
     Q_INVOKABLE QObject* getWebViewRoot(const QUuid& entityID);
 
+    Q_INVOKABLE bool AABoxIntersectsCapsule(const glm::vec3& low, const glm::vec3& dimensions,
+                                            const glm::vec3& start, const glm::vec3& end, float radius);
+
+
 signals:
     void collisionWithEntity(const EntityItemID& idA, const EntityItemID& idB, const Collision& collision);
 

libraries/entities/src/PolyVoxEntityItem.h

@@ -86,6 +86,8 @@ class PolyVoxEntityItem : public EntityItem {
 
     // coords are in world-space
     virtual bool setSphere(glm::vec3 center, float radius, uint8_t toValue) { return false; }
+    virtual bool setCapsule(glm::vec3 startWorldCoords, glm::vec3 endWorldCoords,
+                            float radiusWorldCoords, uint8_t toValue) { return false; }
     virtual bool setAll(uint8_t toValue) { return false; }
     virtual bool setCuboid(const glm::vec3& lowPosition, const glm::vec3& cuboidSize, int value) { return false; }
 

libraries/shared/src/GeometryUtil.cpp

@@ -205,6 +205,33 @@ bool findRaySphereIntersection(const glm::vec3& origin, const glm::vec3& directi
     return true;
 }
 
+bool pointInSphere(const glm::vec3& origin, const glm::vec3& center, float radius) {
+    glm::vec3 relativeOrigin = origin - center;
+    float c = glm::dot(relativeOrigin, relativeOrigin) - radius * radius;
+    return c <= 0.0f;
+}
+
+
+bool pointInCapsule(const glm::vec3& origin, const glm::vec3& start, const glm::vec3& end, float radius) {
+    glm::vec3 relativeOrigin = origin - start;
+    glm::vec3 relativeEnd = end - start;
+    float capsuleLength = glm::length(relativeEnd);
+    relativeEnd /= capsuleLength;
+    float originProjection = glm::dot(relativeEnd, relativeOrigin);
+    glm::vec3 constant = relativeOrigin - relativeEnd * originProjection;
+    float c = glm::dot(constant, constant) - radius * radius;
+    if (c < 0.0f) { // starts inside cylinder
+        if (originProjection < 0.0f) { // below start
+            return pointInSphere(origin, start, radius);
+        } else if (originProjection > capsuleLength) { // above end
+            return pointInSphere(origin, end, radius);
+        } else { // between start and end
+            return true;
+        }
+    }
+    return false;
+}
+
 bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direction,
         const glm::vec3& start, const glm::vec3& end, float radius, float& distance) {
     if (start == end) {

libraries/shared/src/GeometryUtil.h

@@ -73,6 +73,9 @@ glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3&
 bool findRaySphereIntersection(const glm::vec3& origin, const glm::vec3& direction,
     const glm::vec3& center, float radius, float& distance);
 
+bool pointInSphere(const glm::vec3& origin, const glm::vec3& center, float radius);
+bool pointInCapsule(const glm::vec3& origin, const glm::vec3& start, const glm::vec3& end, float radius);
+
 bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direction,
     const glm::vec3& start, const glm::vec3& end, float radius, float& distance);
 

plugins/openvr/src/OpenVrDisplayPlugin.cpp

@@ -545,7 +545,7 @@ bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) {
     // HACK: when interface is launched and steam vr is NOT running, openvr will return bad HMD poses for a few frames
     // To workaround this, filter out any hmd poses that are obviously bad, i.e. beneath the floor.
     if (isBadPose(&nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking)) {
-        qDebug() << "WARNING: ignoring bad hmd pose from openvr";
+        // qDebug() << "WARNING: ignoring bad hmd pose from openvr";
 
         // use the last known good HMD pose
         nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking = _lastGoodHMDPose;