other avatars motion states

interface/src/avatar/AvatarManager.cpp

@@ -407,21 +407,51 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
             if (isInPhysics) {
                 transaction.objectsToRemove.push_back(avatar->_motionState);
                 avatar->_motionState = nullptr;
+                auto& detailedMotionStates = avatar->getDetailedMotionStates();
+                for (auto& mState : detailedMotionStates) {
+                    if (mState) {
+                        transaction.objectsToRemove.push_back(mState);
+                    }
+                }
+                qCDebug(animation) << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
+                avatar->resetDetailedMotionStates();
+
             } else {
-                ShapeInfo shapeInfo;
-                avatar->computeShapeInfo(shapeInfo);
-                btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
-                if (shape) {
-                    AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
-                    motionState->setMass(avatar->computeMass());
-                    avatar->_motionState = motionState;
-                    transaction.objectsToAdd.push_back(motionState);
-                } else {
-                    failedShapeBuilds.insert(avatar);
+                {
+                    ShapeInfo shapeInfo;
+                    avatar->computeShapeInfo(shapeInfo);
+                    btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+                    if (shape) {
+                        AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
+                        motionState->setMass(avatar->computeMass());
+                        avatar->_motionState = motionState;
+                        transaction.objectsToAdd.push_back(motionState);
+                    }
+                    else {
+                        failedShapeBuilds.insert(avatar);
+                    }
+                }
+
+                {
+                    for (int i = 0; i < avatar->getJointCount(); i++) {
+                        avatar->addNewMotionState(avatar, i);
+                    }
+                    auto& detailedMotionStates = avatar->getDetailedMotionStates();
+                    for (auto& mState : detailedMotionStates) {
+                        transaction.objectsToAdd.push_back(mState);
+                    }
+                    qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
                 }
             }
         } else if (isInPhysics) {
             transaction.objectsToChange.push_back(avatar->_motionState);
+            auto& detailedMotionStates = avatar->getDetailedMotionStates();
+            for (auto& mState : detailedMotionStates) {
+                if (mState) {
+                    transaction.objectsToChange.push_back(mState);
+                }
+            }
+            qCDebug(animation) << "Updating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
         }
     }
     _avatarsToChangeInPhysics.swap(failedShapeBuilds);
@@ -519,7 +549,7 @@ void AvatarManager::deleteAllAvatars() {
         avatar->die();
         if (avatar != _myAvatar) {
             auto otherAvatar = std::static_pointer_cast<OtherAvatar>(avatar);
-            assert(!otherAvatar->_motionState);
+            assert(!otherAvatar->_motionState && !otherAvatar->_motionState2);
         }
     }
 }

interface/src/avatar/AvatarManager.h

@@ -29,6 +29,7 @@
 #include <EntitySimulation.h> // for SetOfEntities
 
 #include "AvatarMotionState.h"
+#include "DetailedMotionState.h"
 #include "MyAvatar.h"
 #include "OtherAvatar.h"
 

interface/src/avatar/DetailedMotionState.cpp

@@ -0,0 +1,207 @@
+//
+//  DetailedMotionState.cpp
+//  interface/src/avatar/
+//
+//  Created by Andrew Meadows 2015.05.14
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "DetailedMotionState.h"
+
+#include <PhysicsCollisionGroups.h>
+#include <PhysicsEngine.h>
+#include <PhysicsHelpers.h>
+
+
+DetailedMotionState::DetailedMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : 
+    ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) {
+    assert(_avatar);
+    _type = MOTIONSTATE_TYPE_DETAILED;
+    cacheShapeDiameter();
+}
+
+void DetailedMotionState::handleEasyChanges(uint32_t& flags) {
+    ObjectMotionState::handleEasyChanges(flags);
+    if (flags & Simulation::DIRTY_PHYSICS_ACTIVATION && !_body->isActive()) {
+        _body->activate();
+    }
+}
+
+bool DetailedMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
+    return ObjectMotionState::handleHardAndEasyChanges(flags, engine);
+}
+
+DetailedMotionState::~DetailedMotionState() {
+    assert(_avatar);
+    _avatar = nullptr;
+}
+
+// virtual
+uint32_t DetailedMotionState::getIncomingDirtyFlags() {
+    return _body ? _dirtyFlags : 0;
+}
+
+void DetailedMotionState::clearIncomingDirtyFlags() {
+    if (_body) {
+        _dirtyFlags = 0;
+    }
+}
+
+PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const {
+    // TODO?: support non-DYNAMIC motion for avatars? (e.g. when sitting)
+    return MOTION_TYPE_KINEMATIC;
+}
+
+// virtual and protected
+const btCollisionShape* DetailedMotionState::computeNewShape() {
+    ShapeInfo shapeInfo;
+    _avatar->computeShapeInfo(shapeInfo);
+    return getShapeManager()->getShape(shapeInfo);
+}
+
+// virtual
+bool DetailedMotionState::isMoving() const {
+    return false;
+}
+
+// virtual
+void DetailedMotionState::getWorldTransform(btTransform& worldTrans) const {
+    worldTrans.setOrigin(glmToBullet(getObjectPosition()));
+    worldTrans.setRotation(glmToBullet(getObjectRotation()));
+    if (_body) {
+        _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
+        _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
+    }
+}
+
+// virtual
+void DetailedMotionState::setWorldTransform(const btTransform& worldTrans) {
+    const float SPRING_TIMESCALE = 0.5f;
+    float tau = PHYSICS_ENGINE_FIXED_SUBSTEP / SPRING_TIMESCALE;
+    btVector3 currentPosition = worldTrans.getOrigin();
+    btVector3 offsetToTarget = glmToBullet(getObjectPosition()) - currentPosition;
+    float distance = offsetToTarget.length();
+    if ((1.0f - tau) * distance > _diameter) {
+        // the avatar body is far from its target --> slam position
+        btTransform newTransform;
+        newTransform.setOrigin(currentPosition + offsetToTarget);
+        newTransform.setRotation(glmToBullet(getObjectRotation()));
+        _body->setWorldTransform(newTransform);
+        _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
+        _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
+    } else {
+        // the avatar body is near its target --> slam velocity
+        btVector3 velocity = glmToBullet(getObjectLinearVelocity()) + (1.0f / SPRING_TIMESCALE) * offsetToTarget;
+        _body->setLinearVelocity(velocity);
+        _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
+        // slam its rotation
+        btTransform newTransform = worldTrans;
+        newTransform.setRotation(glmToBullet(getObjectRotation()));
+        _body->setWorldTransform(newTransform);
+    }
+}
+
+// These pure virtual methods must be implemented for each MotionState type
+// and make it possible to implement more complicated methods in this base class.
+
+// virtual
+float DetailedMotionState::getObjectRestitution() const {
+    return 0.5f;
+}
+
+// virtual
+float DetailedMotionState::getObjectFriction() const {
+    return 0.5f;
+}
+
+// virtual
+float DetailedMotionState::getObjectLinearDamping() const {
+    return 0.5f;
+}
+
+// virtual
+float DetailedMotionState::getObjectAngularDamping() const {
+    return 0.5f;
+}
+
+// virtual
+glm::vec3 DetailedMotionState::getObjectPosition() const {
+    return _avatar->getJointPosition(_jointIndex);
+}
+
+// virtual
+glm::quat DetailedMotionState::getObjectRotation() const {
+    return _avatar->getWorldOrientation() * _avatar->getAbsoluteJointRotationInObjectFrame(_jointIndex);
+}
+
+// virtual
+glm::vec3 DetailedMotionState::getObjectLinearVelocity() const {
+    return _avatar->getWorldVelocity();
+}
+
+// virtual
+glm::vec3 DetailedMotionState::getObjectAngularVelocity() const {
+    // HACK: avatars use a capusle collision shape and their angularVelocity in the local simulation is unimportant.
+    // Therefore, as optimization toward support for larger crowds we ignore it and return zero.
+    //return _avatar->getWorldAngularVelocity();
+    return glm::vec3(0.0f);
+}
+
+// virtual
+glm::vec3 DetailedMotionState::getObjectGravity() const {
+    return _avatar->getAcceleration();
+}
+
+// virtual
+const QUuid DetailedMotionState::getObjectID() const {
+    return _avatar->getSessionUUID();
+}
+
+QString DetailedMotionState::getName() const {
+    return _avatar->getName();
+}
+
+// virtual
+QUuid DetailedMotionState::getSimulatorID() const {
+    return _avatar->getSessionUUID();
+}
+
+// virtual
+void DetailedMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const {
+    group = BULLET_COLLISION_GROUP_OTHER_AVATAR;
+    mask = Physics::getDefaultCollisionMask(group);
+}
+
+// virtual
+float DetailedMotionState::getMass() const {
+    return _avatar->computeMass();
+}
+
+void DetailedMotionState::cacheShapeDiameter() {
+    if (_shape) {
+        // shape is capsuleY
+        btVector3 aabbMin, aabbMax;
+        btTransform transform;
+        transform.setIdentity();
+        _shape->getAabb(transform, aabbMin, aabbMax);
+        _diameter = (aabbMax - aabbMin).getX();
+    } else {
+        _diameter = 0.0f;
+    }
+}
+
+void DetailedMotionState::setRigidBody(btRigidBody* body) {
+    ObjectMotionState::setRigidBody(body);
+    if (_body) {
+        // remove angular dynamics from this body
+        _body->setAngularFactor(0.0f);
+    }
+}
+
+void DetailedMotionState::setShape(const btCollisionShape* shape) {
+    ObjectMotionState::setShape(shape);
+    cacheShapeDiameter();
+}

interface/src/avatar/DetailedMotionState.h

@@ -0,0 +1,95 @@
+//
+//  DetailedMotionState.h
+//  interface/src/avatar/
+//
+//  Created by Andrew Meadows 2015.05.14
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_DetailedMotionState_h
+#define hifi_DetailedMotionState_h
+
+#include <QSet>
+
+#include <ObjectMotionState.h>
+#include <BulletUtil.h>
+
+#include "OtherAvatar.h"
+
+class DetailedMotionState : public ObjectMotionState {
+public:
+    DetailedMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape, int jointIndex);
+
+    virtual void handleEasyChanges(uint32_t& flags) override;
+    virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
+
+    virtual PhysicsMotionType getMotionType() const override { return _motionType; }
+
+    virtual uint32_t getIncomingDirtyFlags() override;
+    virtual void clearIncomingDirtyFlags() override;
+
+    virtual PhysicsMotionType computePhysicsMotionType() const override;
+
+    virtual bool isMoving() const override;
+
+    // this relays incoming position/rotation to the RigidBody
+    virtual void getWorldTransform(btTransform& worldTrans) const override;
+
+    // this relays outgoing position/rotation to the EntityItem
+    virtual void setWorldTransform(const btTransform& worldTrans) override;
+
+
+    // These pure virtual methods must be implemented for each MotionState type
+    // and make it possible to implement more complicated methods in this base class.
+
+    // pure virtual overrides from ObjectMotionState
+    virtual float getObjectRestitution() const override;
+    virtual float getObjectFriction() const override;
+    virtual float getObjectLinearDamping() const override;
+    virtual float getObjectAngularDamping() const override;
+
+    virtual glm::vec3 getObjectPosition() const override;
+    virtual glm::quat getObjectRotation() const override;
+    virtual glm::vec3 getObjectLinearVelocity() const override;
+    virtual glm::vec3 getObjectAngularVelocity() const override;
+    virtual glm::vec3 getObjectGravity() const override;
+
+    virtual const QUuid getObjectID() const override;
+
+    virtual QString getName() const override;
+    virtual QUuid getSimulatorID() const override;
+
+    void setBoundingBox(const glm::vec3& corner, const glm::vec3& diagonal);
+
+    void addDirtyFlags(uint32_t flags) { _dirtyFlags |= flags; }
+
+    virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
+
+    virtual float getMass() const override;
+
+    friend class AvatarManager;
+    friend class Avatar;
+
+protected:
+    void setRigidBody(btRigidBody* body) override;
+    void setShape(const btCollisionShape* shape) override;
+    void cacheShapeDiameter();
+
+    // the dtor had been made protected to force the compiler to verify that it is only
+    // ever called by the Avatar class dtor.
+    ~DetailedMotionState();
+
+    virtual bool isReadyToComputeShape() const override { return true; }
+    virtual const btCollisionShape* computeNewShape() override;
+
+    OtherAvatarPointer _avatar;
+    float _diameter { 0.0f };
+
+    uint32_t _dirtyFlags;
+    int _jointIndex { -1 };
+};
+
+#endif // hifi_DetailedMotionState_h

interface/src/avatar/OtherAvatar.cpp

@@ -10,6 +10,7 @@
 #include "Application.h"
 
 #include "AvatarMotionState.h"
+#include "DetailedMotionState.h"
 
 static glm::u8vec3 getLoadingOrbColor(Avatar::LoadingStatus loadingStatus) {
 
@@ -107,10 +108,47 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) {
     int32_t bytesRead = Avatar::parseDataFromBuffer(buffer);
     if (_moving && _motionState) {
         _motionState->addDirtyFlags(Simulation::DIRTY_POSITION);
+        for (auto mState : _detailedMotionStates) {
+            if (mState) {
+                mState->addDirtyFlags(Simulation::DIRTY_POSITION);
+            }
+        }
     }
     return bytesRead;
 }
 
+void OtherAvatar::addNewMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex) {
+    std::lock_guard<std::mutex> lock(_mStateLock);
+    if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) {
+        auto& data = _multiSphereShapes[jointIndex].getSpheresData();
+        std::vector<btVector3> positions;
+        std::vector<btScalar> radiuses;
+        for (auto& sphere : data) {
+            positions.push_back(glmToBullet(sphere._position));
+            radiuses.push_back(sphere._radius);
+        }
+        btCollisionShape* shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size());
+        if (shape) {
+            DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex);
+            motionState->setMass(computeMass());
+            assert(_detailedMotionStates.size() == jointIndex);
+            _detailedMotionStates.push_back(motionState);
+        } else {
+            _detailedMotionStates.push_back(nullptr);
+        }
+    }
+}
+const std::vector<DetailedMotionState*>& OtherAvatar::getDetailedMotionStates() {
+    std::lock_guard<std::mutex> lock(_mStateLock);
+    return _detailedMotionStates; 
+}
+void OtherAvatar::resetDetailedMotionStates() {
+    for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
+        _detailedMotionStates[i] = nullptr;
+    }
+    _detailedMotionStates.clear();
+}
+
 void OtherAvatar::setWorkloadRegion(uint8_t region) {
     _workloadRegion = region;
 }

interface/src/avatar/OtherAvatar.h

@@ -20,6 +20,7 @@
 
 class AvatarManager;
 class AvatarMotionState;
+class DetailedMotionState;
 
 class OtherAvatar : public Avatar {
 public:
@@ -45,14 +46,20 @@ public:
     bool shouldBeInPhysicsSimulation() const;
     bool needsPhysicsUpdate() const;
 
+    void addNewMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
+    const std::vector<DetailedMotionState*>& getDetailedMotionStates();
+    void resetDetailedMotionStates();
+
     friend AvatarManager;
 
 protected:
     std::shared_ptr<Sphere3DOverlay> _otherAvatarOrbMeshPlaceholder { nullptr };
     OverlayID _otherAvatarOrbMeshPlaceholderID { UNKNOWN_OVERLAY_ID };
     AvatarMotionState* _motionState { nullptr };
+    std::vector<DetailedMotionState*> _detailedMotionStates;
     int32_t _spaceIndex { -1 };
     uint8_t _workloadRegion { workload::Region::INVALID };
+    std::mutex _mStateLock;
 };
 
 using OtherAvatarPointer = std::shared_ptr<OtherAvatar>;

libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp

@@ -296,7 +296,9 @@ void Avatar::setTargetScale(float targetScale) {
         _targetScale = newValue;
         _scaleChanged = usecTimestampNow();
         _isAnimatingScale = true;
-
+        for (auto& sphere : _multiSphereShapes) {
+            sphere.setScale(_targetScale);
+        }
         emit targetScaleChanged(targetScale);
     }
 }
@@ -1540,6 +1542,7 @@ void Avatar::setModelURLFinished(bool success) {
 // rig is ready
 void Avatar::rigReady() {
     buildUnscaledEyeHeightCache();
+    computeMultiSphereShapes();
 }
 
 // rig has been reset.
@@ -1547,6 +1550,33 @@ void Avatar::rigReset() {
     clearUnscaledEyeHeightCache();
 }
 
+void Avatar::computeMultiSphereShapes() {
+    const Rig& rig = getSkeletonModel()->getRig();
+    auto scale = extractScale(rig.getGeometryToRigTransform());
+    const HFMModel& geometry = getSkeletonModel()->getHFMModel();
+    int jointCount = rig.getJointStateCount();
+    _multiSphereShapes.clear();
+    _multiSphereShapes.reserve(jointCount);
+    for (int i = 0; i < jointCount; i++) {
+        const HFMJointShapeInfo& shapeInfo = geometry.joints[i].shapeInfo;
+        std::vector<btVector3> btPoints;
+        int lineCount = (int)shapeInfo.debugLines.size();
+        btPoints.reserve(lineCount);
+        for (size_t j = 0; j < lineCount; j++) {
+            const glm::vec3 &point = shapeInfo.debugLines[j];
+            auto rigPoint = scale * point;
+            btVector3 btPoint = glmToBullet(rigPoint);
+            btPoints.push_back(btPoint);
+        }
+        auto jointName = rig.nameOfJoint(i).toUpper();
+        MultiSphereShape multiSphereShape;
+        if (multiSphereShape.computeMultiSphereShape(jointName, btPoints, getSensorToWorldScale())) {
+            multiSphereShape.calculateDebugLines();
+        }
+        _multiSphereShapes.push_back(multiSphereShape);
+    }
+}
+
 // create new model, can return an instance of a SoftAttachmentModel rather then Model
 static std::shared_ptr<Model> allocateAttachmentModel(bool isSoft, const Rig& rigOverride, bool isCauterized) {
     if (isSoft) {

libraries/avatars-renderer/src/avatars-renderer/Avatar.h

@@ -29,6 +29,7 @@
 #include <ThreadSafeValueCache.h>
 
 #include "MetaModelPayload.h"
+#include "MultiSphereShape.h"
 
 namespace render {
     template <> const ItemKey payloadGetKey(const AvatarSharedPointer& avatar);
@@ -507,6 +508,8 @@ protected:
     virtual const QString& getSessionDisplayNameForTransport() const override { return _empty; } // Save a tiny bit of bandwidth. Mixer won't look at what we send.
     QString _empty{};
     virtual void maybeUpdateSessionDisplayNameFromTransport(const QString& sessionDisplayName) override { _sessionDisplayName = sessionDisplayName; } // don't use no-op setter!
+    void computeMultiSphereShapes();
+    const std::vector<MultiSphereShape>& getMultiSphereShapes() const { return _multiSphereShapes; }
 
     SkeletonModelPointer _skeletonModel;
 
@@ -628,6 +631,7 @@ protected:
 
     static void metaBlendshapeOperator(render::ItemID renderItemID, int blendshapeNumber, const QVector<BlendshapeOffset>& blendshapeOffsets,
                                        const QVector<int>& blendedMeshSizes, const render::ItemIDs& subItemIDs);
+    std::vector<MultiSphereShape> _multiSphereShapes;
 };
 
 #endif // hifi_Avatar_h

libraries/physics/src/MultiSphereShape.cpp

@@ -0,0 +1,527 @@
+//
+//  MultiSphereShape.cpp
+//  libraries/physics/src
+//
+//  Created by Luis Cuenca 5/11/2018
+//  Copyright 2018 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "MultiSphereShape.h"
+
+void SphereRegion::translate(const glm::vec3& translation) {
+    for (auto &line : _lines) {
+        line.first += translation;
+        line.second += translation;
+    }
+}
+void SphereRegion::dump(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines) {
+    for (auto &line : _lines) {
+        outLines.push_back(line);
+    }
+}
+
+void SphereRegion::insertUnique(const glm::vec3& point, std::vector<glm::vec3>& pointSet) {
+    auto hit = std::find_if(pointSet.begin(), pointSet.end(), [point](const glm::vec3& pointFromSet) -> bool {
+        return (pointFromSet == point);
+    });
+    if (hit == pointSet.end()) {
+        pointSet.push_back(point);
+    }
+}
+
+void SphereRegion::extractEdges(bool reverseY) {
+    if (_lines.size() == 0) {
+        return;
+    }
+    float yVal = _lines[0].first.y;
+    for (size_t i = 0; i < _lines.size(); i++) {
+        yVal = reverseY ? glm::max(yVal, _lines[i].first.y) : glm::min(yVal, _lines[i].first.y);
+    }
+    for (size_t i = 0; i < _lines.size(); i++) {
+        auto line = _lines[i];
+        auto p1 = line.first;
+        auto p2 = line.second;
+        auto vec = p1 - p2;
+        if (vec.z == 0.0f) {
+            insertUnique(p1, _edgesX);
+            insertUnique(p2, _edgesX);
+        }
+        else if (vec.y == 0.0f && p1.y == yVal && p2.y == yVal) {
+            insertUnique(p1, _edgesY);
+            insertUnique(p2, _edgesY);
+        }
+        else if (vec.x == 0.0f) {
+            insertUnique(p1, _edgesZ);
+            insertUnique(p2, _edgesZ);
+        }
+    }
+}
+
+void SphereRegion::extractSphereRegion(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines) {
+    for (size_t i = 0; i < outLines.size(); i++) {
+        auto &line = outLines[i];
+        auto &p1 = line.first;
+        auto &p2 = line.second;
+        p1.x = glm::abs(p1.x) < 0.001f ? 0.0f : p1.x;
+        p1.y = glm::abs(p1.y) < 0.001f ? 0.0f : p1.y;
+        p1.z = glm::abs(p1.z) < 0.001f ? 0.0f : p1.z;
+        p2.x = glm::abs(p2.x) < 0.001f ? 0.0f : p2.x;
+        p2.y = glm::abs(p2.y) < 0.001f ? 0.0f : p2.y;
+        p2.z = glm::abs(p2.z) < 0.001f ? 0.0f : p2.z;
+
+        glm::vec3 point1 = { p1.x != 0.0f ? glm::abs(p1.x) / p1.x : _direction.x,
+            p1.y != 0.0f ? glm::abs(p1.y) / p1.y : _direction.y,
+            p1.z != 0.0f ? glm::abs(p1.z) / p1.z : _direction.z };
+        glm::vec3 point2 = { p2.x != 0.0f ? glm::abs(p2.x) / p2.x : _direction.x,
+            p2.y != 0.0f ? glm::abs(p2.y) / p2.y : _direction.y,
+            p2.z != 0.0f ? glm::abs(p2.z) / p2.z : _direction.z };
+        if (point1 == _direction && point2 == _direction) {
+            _lines.push_back(line);
+        }
+    }
+}
+
+CollisionShapeExtractionMode MultiSphereShape::getExtractionModeByName(const QString& name) {
+    CollisionShapeExtractionMode mode = CollisionShapeExtractionMode::Automatic;
+    bool isSim = name.indexOf("SIM") == 0;
+    bool isFlow = name.indexOf("FLOW") == 0;
+    bool isEye = name.indexOf("EYE") > -1;
+    bool isToe = name.indexOf("TOE") > -1;
+    bool isShoulder = name.indexOf("SHOULDER") > -1;
+    bool isNeck = name.indexOf("NECK") > -1;
+    bool isRightHand = name == "RIGHTHAND";
+    bool isLeftHand = name == "LEFTHAND";
+    bool isRightFinger = name.indexOf("RIGHTHAND") == 0 && !isRightHand;
+    bool isLeftFinger = name.indexOf("LEFTHAND") == 0 && !isLeftHand;
+    
+    //bool isFinger = 
+    if (isNeck || isLeftFinger || isRightFinger) {
+        mode = CollisionShapeExtractionMode::SpheresY;
+    } else if (isShoulder) {
+        mode = CollisionShapeExtractionMode::SphereCollapse;
+    } else if (isRightHand || isLeftHand) {
+        mode = CollisionShapeExtractionMode::SpheresXY;
+    }
+    else if (isSim || isFlow || isEye || isToe) {
+        mode = CollisionShapeExtractionMode::None;
+        //qDebug() << "Trying to add " << (int)positions.size() << " spheres for " << jointName << " length: " << maxLength;
+    }
+    return mode;
+}
+
+void MultiSphereShape::filterUniquePoints(const std::vector<btVector3>& kdop, std::vector<glm::vec3>& uniquePoints) {
+    for (size_t j = 0; j < kdop.size(); j++) {
+        btVector3 btPoint = kdop[j];
+        auto hit = std::find_if(uniquePoints.begin(), uniquePoints.end(), [btPoint](const glm::vec3& point) -> bool {
+            return (btPoint.getX() == point.x
+                && btPoint.getY() == point.y
+                && btPoint.getZ() == point.z);
+        });
+        if (hit == uniquePoints.end()) {
+            uniquePoints.push_back(bulletToGLM(btPoint));
+        }
+    }
+}
+
+bool MultiSphereShape::computeMultiSphereShape(const QString& name, const std::vector<btVector3>& kdop, float scale) {
+    _scale = scale;
+    _mode = getExtractionModeByName(name);
+    if (_mode == CollisionShapeExtractionMode::None || kdop.size() < 4 || kdop.size() > 200) {
+        return false;
+    }
+    std::vector<glm::vec3> points;
+    filterUniquePoints(kdop, points);
+    glm::vec3 min = glm::vec3(100.0f, 100.0f, 100.0f);
+    glm::vec3 max = glm::vec3(-100.0f, -100.0f, -100.0f);
+    _midPoint = glm::vec3(0.0f, 0.0f, 0.0f);
+    std::vector<glm::vec3> relPoints;
+    for (size_t i = 0; i < points.size(); i++) {
+
+        min.x = points[i].x < min.x ? points[i].x : min.x;
+        min.y = points[i].y < min.y ? points[i].y : min.y;
+        min.z = points[i].z < min.z ? points[i].z : min.z;
+
+        max.x = points[i].x > max.x ? points[i].x : max.x;
+        max.y = points[i].y > max.y ? points[i].y : max.y;
+        max.z = points[i].z > max.z ? points[i].z : max.z;
+
+        _midPoint += points[i];
+    }
+
+    _midPoint /= (int)points.size();
+    glm::vec3 dimensions = max - min;
+
+    for (size_t i = 0; i < points.size(); i++) {
+        glm::vec3 relPoint = points[i] - _midPoint;
+        relPoints.push_back(relPoint);
+    }
+    CollisionShapeExtractionMode applyMode = _mode;
+    float xCorrector = dimensions.x > dimensions.y && dimensions.x > dimensions.z ? -1.0f + (dimensions.x / (0.5f * (dimensions.y + dimensions.z))) : 0.0f;
+    float yCorrector = dimensions.y > dimensions.x && dimensions.y > dimensions.z ? -1.0f + (dimensions.y / (0.5f * (dimensions.x + dimensions.z))) : 0.0f;
+    float zCorrector = dimensions.z > dimensions.x && dimensions.z > dimensions.y ? -1.0f + (dimensions.z / (0.5f * (dimensions.x + dimensions.y))) : 0.0f;
+
+    float xyDif = glm::abs(dimensions.x - dimensions.y);
+    float xzDif = glm::abs(dimensions.x - dimensions.z);
+    float yzDif = glm::abs(dimensions.y - dimensions.z);
+
+    float xyEpsilon = (0.05f + zCorrector) * glm::max(dimensions.x, dimensions.y);
+    float xzEpsilon = (0.05f + yCorrector) * glm::max(dimensions.x, dimensions.z);
+    float yzEpsilon = (0.05f + xCorrector) * glm::max(dimensions.y, dimensions.z);
+
+    if (xyDif < 0.5f * xyEpsilon && xzDif < 0.5f * xzEpsilon && yzDif < 0.5f * yzEpsilon) {
+        applyMode = CollisionShapeExtractionMode::Sphere;
+    }
+    else if (xzDif < xzEpsilon) {
+        applyMode = dimensions.y > dimensions.z ? CollisionShapeExtractionMode::SpheresY : CollisionShapeExtractionMode::SpheresXZ;
+    }
+    else if (xyDif < xyEpsilon) {
+        applyMode = dimensions.z > dimensions.y ? CollisionShapeExtractionMode::SpheresZ : CollisionShapeExtractionMode::SpheresXY;
+    }
+    else if (yzDif < yzEpsilon) {
+        applyMode = dimensions.x > dimensions.y ? CollisionShapeExtractionMode::SpheresX : CollisionShapeExtractionMode::SpheresYZ;
+    }
+    else {
+        applyMode = CollisionShapeExtractionMode::SpheresXYZ;
+    }
+
+    if (_mode != CollisionShapeExtractionMode::Automatic && applyMode != _mode) {
+        bool isModeSphereAxis = (_mode >= CollisionShapeExtractionMode::SpheresX && _mode <= CollisionShapeExtractionMode::SpheresZ);
+        bool isApplyModeComplex = (applyMode >= CollisionShapeExtractionMode::SpheresXY && applyMode <= CollisionShapeExtractionMode::SpheresXYZ);
+        applyMode = (isModeSphereAxis && isApplyModeComplex) ? CollisionShapeExtractionMode::Sphere : _mode;
+    }
+
+    std::vector<glm::vec3> axes;
+    glm::vec3 axis, axis1, axis2;
+    SphereShapeData sphere;
+    switch (applyMode) {
+    case CollisionShapeExtractionMode::None:
+        break;
+    case CollisionShapeExtractionMode::Automatic:
+        break;
+    case CollisionShapeExtractionMode::Box:
+        break;
+    case CollisionShapeExtractionMode::Sphere:
+        sphere._radius = 0.5f * (dimensions.x + dimensions.y + dimensions.z) / 3.0f;
+        sphere._position = glm::vec3(0.0f);
+        _spheres.push_back(sphere);
+        break;
+    case CollisionShapeExtractionMode::SphereCollapse:
+        sphere._radius = 0.5f * glm::min(glm::min(dimensions.x, dimensions.y), dimensions.z);
+        sphere._position = glm::vec3(0.0f);
+        _spheres.push_back(sphere);
+        break;
+    case CollisionShapeExtractionMode::SpheresX:
+        axis = 0.5f* dimensions.x * Vectors::UNIT_NEG_X;
+        axes = { axis, -axis };
+        break;
+    case CollisionShapeExtractionMode::SpheresY:
+        axis = 0.5f* dimensions.y * Vectors::UNIT_NEG_Y;
+        axes = { axis, -axis };
+        break;
+    case CollisionShapeExtractionMode::SpheresZ:
+        axis = 0.5f* dimensions.z * Vectors::UNIT_NEG_Z;
+        axes = { axis, -axis };
+        break;
+    case CollisionShapeExtractionMode::SpheresXY:
+        axis1 = glm::vec3(0.5f * dimensions.x, 0.5f * dimensions.y, 0.0f);
+        axis2 = glm::vec3(0.5f * dimensions.x, -0.5f * dimensions.y, 0.0f);
+        axes = { axis1, axis2, -axis1, -axis2 };
+        break;
+    case CollisionShapeExtractionMode::SpheresYZ:
+        axis1 = glm::vec3(0.0f, 0.5f * dimensions.y, 0.5f * dimensions.z);
+        axis2 = glm::vec3(0.0f, 0.5f * dimensions.y, -0.5f * dimensions.z);
+        axes = { axis1, axis2, -axis1, -axis2 };
+        break;
+    case CollisionShapeExtractionMode::SpheresXZ:
+        axis1 = glm::vec3(0.5f * dimensions.x, 0.0f, 0.5f * dimensions.z);
+        axis2 = glm::vec3(-0.5f * dimensions.x, 0.0f, 0.5f * dimensions.z);
+        axes = { axis1, axis2, -axis1, -axis2 };
+        break;
+    case CollisionShapeExtractionMode::SpheresXYZ:
+        for (size_t i = 0; i < CORNER_SIGNS.size(); i++) {
+            axes.push_back(0.5f * (dimensions * CORNER_SIGNS[i]));
+        }
+        break;
+    default:
+        break;
+    }
+    if (axes.size() > 0) {
+        spheresFromAxes(relPoints, axes, _spheres);
+    }
+    for (size_t i = 0; i < _spheres.size(); i++) {
+        _spheres[i]._position += _midPoint;
+    }
+    return _mode != CollisionShapeExtractionMode::None;
+}
+
+void MultiSphereShape::spheresFromAxes(const std::vector<glm::vec3>& points, const std::vector<glm::vec3>& axes, std::vector<SphereShapeData>& spheres) {
+    float maxRadius = 0.0f;
+    float maxAverageRadius = 0.0f;
+    float minAverageRadius = glm::length(points[0]);
+    size_t sphereCount = axes.size();
+    spheres.clear();
+    for (size_t j = 0; j < sphereCount; j++) {
+        SphereShapeData sphere = SphereShapeData();
+        sphere._axis = axes[j];
+        spheres.push_back(sphere);
+    }
+    for (size_t j = 0; j < sphereCount; j++) {
+        auto axis = _spheres[j]._axis;
+        float averageRadius = 0.0f;
+        float maxDistance = glm::length(axis);
+        glm::vec3 axisDir = glm::normalize(axis);
+        for (size_t i = 0; i < points.size(); i++) {
+            float dot = glm::dot(points[i], axisDir);
+            float distance = glm::length(points[i]);
+            if (dot > 0.0f) {
+                float distancePow = glm::pow(distance, 2);
+                float dotPow = glm::pow(dot, 2);
+                float radius = (dot / maxDistance) * glm::sqrt(distancePow - dotPow);
+                averageRadius += radius;
+                maxRadius = radius > maxRadius ? radius : maxRadius;
+            }
+        }
+        averageRadius /= (int)points.size();
+        maxAverageRadius = averageRadius > maxAverageRadius ? averageRadius : maxAverageRadius;
+        minAverageRadius = averageRadius < minAverageRadius ? averageRadius : minAverageRadius;
+        spheres[j]._radius = averageRadius;
+    }
+    float radiusRatio = maxRadius / maxAverageRadius;
+    // Push the sphere into the bounding box
+    float contractionRatio = 0.8f;
+    for (size_t j = 0; j < sphereCount; j++) {
+        auto axis = _spheres[j]._axis;
+        float distance = glm::length(axis);
+        float correntionRatio = radiusRatio * (spheres[j]._radius / maxAverageRadius);
+        float radius = (correntionRatio < 0.8f * radiusRatio ? 0.8f * radiusRatio : correntionRatio) * spheres[j]._radius;
+        if (sphereCount > 3) {
+            distance = contractionRatio * distance;
+        }
+        spheres[j]._radius = radius;
+        if (distance - radius > 0.0f) {
+            spheres[j]._position = (distance - radius) * glm::normalize(axis);
+        } else {
+            spheres[j]._position = glm::vec3(0.0f);
+        }
+    }
+    // Collapse spheres if too close
+    if (sphereCount == 2) {
+        int maxRadiusIndex = spheres[0]._radius > spheres[1]._radius ? 0 : 1;
+        if (glm::length(spheres[0]._position - spheres[1]._position) < 0.2f * spheres[maxRadiusIndex]._radius) {
+            SphereShapeData newSphere;
+            newSphere._position = 0.5f * (spheres[0]._position + spheres[1]._position);
+            newSphere._radius = spheres[maxRadiusIndex]._radius;
+            spheres.clear();
+            spheres.push_back(newSphere);
+        }
+    }
+}
+
+void MultiSphereShape::connectSpheres(int index1, int index2, bool onlyEdges) {
+    auto sphere1 = _spheres[index1]._radius > _spheres[index2]._radius ? _spheres[index1] : _spheres[index2];
+    auto sphere2 = _spheres[index1]._radius <= _spheres[index2]._radius ? _spheres[index1] : _spheres[index2];
+    float distance = glm::length(sphere1._position - sphere2._position);
+
+    auto axis = sphere1._position - sphere2._position;
+
+    float angleOffset = glm::asin((sphere1._radius - sphere2._radius) / distance);
+    float percent1 = ((0.5f * PI) + angleOffset) / PI;
+    float percent2 = ((0.5f * PI) - angleOffset) / PI;
+
+    std::vector<glm::vec3> edge1, edge2;
+    if (onlyEdges) {
+        std::vector<std::pair<glm::vec3, glm::vec3>> debugLines;
+        calculateSphereLines(debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), percent1, &edge1);
+        calculateSphereLines(debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &edge2);
+    } else {
+        calculateSphereLines(_debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), percent1, &edge1);
+        calculateSphereLines(_debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &edge2);
+    }
+    connectEdges(_debugLines, edge1, edge2);
+}
+
+void MultiSphereShape::calculateDebugLines() {
+    if (_spheres.size() == 1) {
+        auto sphere = _spheres[0];
+        calculateSphereLines(_debugLines, sphere._position, sphere._radius);
+    } else if (_spheres.size() == 2) {
+        connectSpheres(0, 1);        
+    } else if (_spheres.size() == 4) {
+        std::vector<glm::vec3> axes;
+        axes.resize(8);
+        for (size_t i = 0; i < CORNER_SIGNS.size(); i++) {
+            for (size_t j = 0; j < 4; j++) {
+                auto axis = _spheres[j]._position - _midPoint;
+                glm::vec3 sign = { axis.x != 0.0f ? glm::abs(axis.x) / axis.x : 0.0f,
+                                   axis.x != 0.0f ? glm::abs(axis.y) / axis.y : 0.0f ,
+                                   axis.z != 0.0f ? glm::abs(axis.z) / axis.z : 0.0f };
+                bool add = false;
+                if (sign.x == 0) {
+                    if (sign.y == CORNER_SIGNS[i].y && sign.z == CORNER_SIGNS[i].z) {
+                        add = true;
+                    }
+                } else if (sign.y == 0) {
+                    if (sign.x == CORNER_SIGNS[i].x && sign.z == CORNER_SIGNS[i].z) {
+                        add = true;
+                    }
+                } else if (sign.z == 0) {
+                    if (sign.x == CORNER_SIGNS[i].x && sign.y == CORNER_SIGNS[i].y) {
+                        add = true;
+                    }
+                } else if (sign == CORNER_SIGNS[i]) {
+                    add = true;
+                }
+                if (add) {
+                    axes[i] = axis;
+                    break;
+                }
+            }            
+        }        
+        calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint);
+    } else if (_spheres.size() == 8) {
+        std::vector<glm::vec3> axes;
+        for (size_t i = 0; i < _spheres.size(); i++) {
+            axes.push_back(_spheres[i]._position - _midPoint);
+        }
+        calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint);
+    }
+}
+
+void MultiSphereShape::connectEdges(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const std::vector<glm::vec3>& edge1, const std::vector<glm::vec3>& edge2, bool reverse) {
+    if (edge1.size() == edge2.size()) {
+        for (size_t i = 0; i < edge1.size(); i++) {
+            size_t j = reverse ? edge1.size() - i - 1 : i;
+            outLines.push_back({ edge1[i], edge2[j] });
+        }
+    }
+}
+
+void MultiSphereShape::calculateChamferBox(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const float& radius, const std::vector<glm::vec3>& axes, const glm::vec3& translation) {
+    std::vector<std::pair<glm::vec3, glm::vec3>> sphereLines;
+    calculateSphereLines(sphereLines, glm::vec3(0.0f), radius);
+
+    std::vector<SphereRegion> regions = {
+        SphereRegion({ 1.0f, 1.0f, 1.0f }),
+        SphereRegion({ -1.0f, 1.0f, 1.0f }),
+        SphereRegion({ -1.0f, 1.0f, -1.0f }),
+        SphereRegion({ 1.0f, 1.0f, -1.0f }),
+        SphereRegion({ 1.0f, -1.0f, 1.0f }),
+        SphereRegion({ -1.0f, -1.0f, 1.0f }),
+        SphereRegion({ -1.0f, -1.0f, -1.0f }),
+        SphereRegion({ 1.0f, -1.0f, -1.0f })
+    };
+    
+    assert(axes.size() == regions.size());
+
+    for (size_t i = 0; i < regions.size(); i++) {
+        regions[i].extractSphereRegion(sphereLines);
+        regions[i].translate(translation + axes[i]);
+        regions[i].extractEdges(axes[i].y < 0);
+        regions[i].dump(outLines);
+    }
+
+    connectEdges(outLines, regions[0].getEdgesZ(), regions[1].getEdgesZ());
+    connectEdges(outLines, regions[1].getEdgesX(), regions[2].getEdgesX());
+    connectEdges(outLines, regions[2].getEdgesZ(), regions[3].getEdgesZ());
+    connectEdges(outLines, regions[3].getEdgesX(), regions[0].getEdgesX());
+
+    connectEdges(outLines, regions[4].getEdgesZ(), regions[5].getEdgesZ());
+    connectEdges(outLines, regions[5].getEdgesX(), regions[6].getEdgesX());
+    connectEdges(outLines, regions[6].getEdgesZ(), regions[7].getEdgesZ());
+    connectEdges(outLines, regions[7].getEdgesX(), regions[4].getEdgesX());
+
+    connectEdges(outLines, regions[0].getEdgesY(), regions[4].getEdgesY());
+    connectEdges(outLines, regions[1].getEdgesY(), regions[5].getEdgesY());
+    connectEdges(outLines, regions[2].getEdgesY(), regions[6].getEdgesY());
+    connectEdges(outLines, regions[3].getEdgesY(), regions[7].getEdgesY());
+
+}
+
+void MultiSphereShape::calculateSphereLines(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const glm::vec3& center, const float& radius,
+                                     const int& subdivisions, const glm::vec3& direction, const float& percentage, std::vector<glm::vec3>* edge) {
+    
+    float uTotalAngle = percentage * PI;
+    float vTotalAngle = 2.0f * PI;
+
+    int uSubdivisions = (int)glm::ceil(subdivisions * 0.5f * percentage);
+    int vSubdivisions = subdivisions;
+
+    float uDeltaAngle = uTotalAngle / uSubdivisions;
+    float vDeltaAngle = vTotalAngle / vSubdivisions;
+
+    float uAngle = 0.0f;
+
+    glm::vec3 uAxis, vAxis;
+    glm::vec3 mainAxis = glm::normalize(direction);
+    if (mainAxis.y == 1.0f || mainAxis.y == -1.0f) {
+        uAxis = glm::vec3(1.0f, 0.0f, 0.0f);
+        vAxis = glm::vec3(0.0f, 0.0f, 1.0f);
+    } else {
+        uAxis = glm::normalize(glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), mainAxis));
+        vAxis = glm::normalize(glm::cross(mainAxis, uAxis));
+        if ((uAxis.z == 0 && uAxis.x < 0) || (uAxis.x == 0 && uAxis.z < 0)) {
+            uAxis = -uAxis;
+        } else if (uAxis.x < 0) {
+            uAxis = -uAxis;
+        }
+        if ((vAxis.z == 0 && vAxis.x < 0) || (vAxis.x == 0 && vAxis.z < 0)) {
+            vAxis = -vAxis;
+        }
+        else if (vAxis.x < 0) {
+            vAxis = -vAxis;
+        }
+    }
+
+    std::vector<std::vector<glm::vec3>> arcs;
+    auto origin = center;
+    for (int u = 0; u < uSubdivisions + 1; u++) {
+        std::vector<glm::vec3> arc;
+        glm::vec3 arcCenter = origin + mainAxis * (glm::cos(uAngle) * radius);
+        float vAngle = 0.0f;
+        for (int v = 0; v < vSubdivisions + 1; v++) {
+            float arcRadius = glm::abs(glm::sin(uAngle) * radius);
+            glm::vec3 arcPoint = arcCenter + (arcRadius * glm::cos(vAngle)) * uAxis + (arcRadius * glm::sin(vAngle)) * vAxis;
+            arc.push_back(arcPoint);
+            if (u == uSubdivisions && edge != nullptr) {
+                edge->push_back(arcPoint);
+            }
+            vAngle += vDeltaAngle;
+        }
+        arc.push_back(arc[0]);
+        arcs.push_back(arc);
+        uAngle += uDeltaAngle;
+    }
+
+    for (size_t i = 1; i < arcs.size(); i++) {
+        auto arc1 = arcs[i];
+        auto arc2 = arcs[i - 1];
+        for (size_t j = 1; j < arc1.size(); j++) {
+            auto point1 = arc1[j];
+            auto point2 = arc1[j - 1];
+            auto point3 = arc2[j];
+            std::pair<glm::vec3, glm::vec3> line1 = { point1, point2 };
+            std::pair<glm::vec3, glm::vec3> line2 = { point1, point3 };
+            outLines.push_back(line1);
+            outLines.push_back(line2);
+        }
+    }
+}
+
+void MultiSphereShape::setScale(float scale) {
+    if (scale != _scale) {
+        float deltaScale = scale / _scale;
+        for (auto& sphere : _spheres) {
+            sphere._axis *= deltaScale;
+            sphere._position *= deltaScale;
+            sphere._radius *= deltaScale;
+        }
+        for (auto& line : _debugLines) {
+            line.first *= deltaScale;
+            line.second *= deltaScale;
+        }
+        _scale = scale;
+    }
+}

libraries/physics/src/MultiSphereShape.h

@@ -0,0 +1,102 @@
+//
+//  MultiSphereShape.h
+//  libraries/physics/src
+//
+//  Created by Luis Cuenca 5/11/2018
+//  Copyright 2018 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_MultiSphereShape_h
+#define hifi_MultiSphereShape_h
+
+#include <stdint.h>
+#include <btBulletDynamicsCommon.h>
+#include <GLMHelpers.h>
+#include "BulletUtil.h"
+
+
+enum CollisionShapeExtractionMode {
+    None = 0,
+    Automatic,
+    Box,
+    Sphere,
+    SphereCollapse,
+    SpheresX,
+    SpheresY,
+    SpheresZ,
+    SpheresXY,
+    SpheresYZ,
+    SpheresXZ,
+    SpheresXYZ
+};
+
+struct SphereShapeData {
+    SphereShapeData() {}
+    glm::vec3 _position;
+    glm::vec3 _axis;
+    float _radius;    
+};
+
+class SphereRegion {
+public:
+    SphereRegion() {}
+    SphereRegion(const glm::vec3& direction) : _direction(direction) {}
+    void extractSphereRegion(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines);
+    void extractEdges(bool reverseY = false);
+    void translate(const glm::vec3& translation);
+    void dump(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines);
+    const glm::vec3& getDirection() const { return _direction; }
+    const std::vector<glm::vec3>& getEdgesX() const { return _edgesX; }
+    const std::vector<glm::vec3>& getEdgesY() const { return _edgesY; }
+    const std::vector<glm::vec3>& getEdgesZ() const { return _edgesZ; }
+private:
+    void insertUnique(const glm::vec3& point, std::vector<glm::vec3>& pointSet);
+    
+    std::vector<std::pair<glm::vec3, glm::vec3>> _lines;
+    std::vector<glm::vec3> _edgesX;
+    std::vector<glm::vec3> _edgesY;
+    std::vector<glm::vec3> _edgesZ;
+    glm::vec3 _direction;
+};
+
+const int DEFAULT_SPHERE_SUBDIVISIONS = 16;
+
+const std::vector<glm::vec3> CORNER_SIGNS = {
+    glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(-1.0f, 1.0f, 1.0f),
+    glm::vec3(-1.0f, 1.0f, -1.0f), glm::vec3(1.0f, 1.0f, -1.0f),
+    glm::vec3(1.0f, -1.0f, 1.0f), glm::vec3(-1.0f, -1.0f, 1.0f),
+    glm::vec3(-1.0f, -1.0f, -1.0f), glm::vec3(1.0f, -1.0f, -1.0f) };
+
+class MultiSphereShape {
+public:
+    MultiSphereShape() {};
+    bool computeMultiSphereShape(const QString& name, const std::vector<btVector3>& points, float scale = 1.0f);
+    void calculateDebugLines();
+    const std::vector<SphereShapeData>& getSpheresData() const { return _spheres; }
+    const std::vector<std::pair<glm::vec3, glm::vec3>>& getDebugLines() const { return _debugLines; }
+    void setScale(float scale);
+
+private:
+    CollisionShapeExtractionMode getExtractionModeByName(const QString& name);
+    void filterUniquePoints(const std::vector<btVector3>& kdop, std::vector<glm::vec3>& uniquePoints);
+    void spheresFromAxes(const std::vector<glm::vec3>& points, const std::vector<glm::vec3>& axes, 
+                              std::vector<SphereShapeData>& spheres);
+    
+    void calculateSphereLines(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const glm::vec3& center, const float& radius,
+                              const int& subdivisions = DEFAULT_SPHERE_SUBDIVISIONS, const glm::vec3& direction = Vectors::UNIT_Y, 
+                              const float& percentage = 1.0f, std::vector<glm::vec3>* edge = nullptr);
+    void calculateChamferBox(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const float& radius, const std::vector<glm::vec3>& axes, const glm::vec3& translation);
+    void connectEdges(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const std::vector<glm::vec3>& edge1, 
+                      const std::vector<glm::vec3>& edge2, bool reverse = false);
+    void connectSpheres(int index1, int index2, bool onlyEdges = false);
+    std::vector<SphereShapeData> _spheres;
+    std::vector<std::pair<glm::vec3, glm::vec3>> _debugLines;
+    CollisionShapeExtractionMode _mode;
+    glm::vec3 _midPoint;
+    float _scale { 1.0f };
+};
+
+#endif // hifi_MultiSphereShape_h

libraries/physics/src/ObjectMotionState.cpp

@@ -292,7 +292,7 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine*
         if (!isReadyToComputeShape()) {
             return false;
         }
-        const btCollisionShape* newShape = computeNewShape();
+        const btCollisionShape* newShape = _type != MOTIONSTATE_TYPE_DETAILED ? computeNewShape() : nullptr;
         if (!newShape) {
             qCDebug(physics) << "Warning: failed to generate new shape!";
             // failed to generate new shape! --> keep old shape and remove shape-change flag

libraries/physics/src/ObjectMotionState.h

@@ -58,7 +58,8 @@ inline QString motionTypeToString(PhysicsMotionType motionType) {
 enum MotionStateType {
     MOTIONSTATE_TYPE_INVALID,
     MOTIONSTATE_TYPE_ENTITY,
-    MOTIONSTATE_TYPE_AVATAR
+    MOTIONSTATE_TYPE_AVATAR,
+    MOTIONSTATE_TYPE_DETAILED
 };
 
 // The update flags trigger two varieties of updates: "hard" which require the body to be pulled