Other avatar detailed collisions on motion state

interface/src/avatar/AvatarManager.cpp

@@ -431,12 +431,13 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
                 auto& detailedMotionStates = avatar->getDetailedMotionStates();
                 if (detailedMotionStates.size() == 0) {
                     for (int i = 0; i < avatar->getJointCount(); i++) {
-                        avatar->addNewMotionState(avatar, i);
+                        auto dMotionState = avatar->createDetailedMotionStateForJoint(avatar, i);
+                        if (dMotionState) {
+                            detailedMotionStates.push_back(dMotionState);
+                            transaction.objectsToAdd.push_back(dMotionState);
                         }
-                    for (auto& mState : detailedMotionStates) {
-                        transaction.objectsToAdd.push_back(mState);
                     }
-                    qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
+                    //qCDebug(animation) << "Creating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
                 }
                 if (avatar->_motionState == nullptr || detailedMotionStates.size() == 0) {
                     failedShapeBuilds.insert(avatar);
@@ -450,7 +451,7 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
                     transaction.objectsToChange.push_back(mState);
                 }
             }
-            qCDebug(animation) << "Updating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
+            //qCDebug(animation) << "Updating " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
         }
     }
     _avatarsToChangeInPhysics.swap(failedShapeBuilds);

interface/src/avatar/AvatarMotionState.cpp

@@ -58,6 +58,7 @@ PhysicsMotionType AvatarMotionState::computePhysicsMotionType() const {
 const btCollisionShape* AvatarMotionState::computeNewShape() {
     ShapeInfo shapeInfo;
     _avatar->computeShapeInfo(shapeInfo);
+    qDebug() << "Creating new Capsule Shape";
     return getShapeManager()->getShape(shapeInfo);
 }
 

interface/src/avatar/DetailedMotionState.cpp

@@ -20,7 +20,6 @@ DetailedMotionState::DetailedMotionState(OtherAvatarPointer avatar, const btColl
     ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) {
     assert(_avatar);
     _type = MOTIONSTATE_TYPE_DETAILED;
-    cacheShapeDiameter();
 }
 
 void DetailedMotionState::handleEasyChanges(uint32_t& flags) {
@@ -37,6 +36,9 @@ bool DetailedMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngin
 DetailedMotionState::~DetailedMotionState() {
     assert(_avatar);
     _avatar = nullptr;
+    if (_shape) {
+        delete _shape;
+    }
 }
 
 // virtual
@@ -57,9 +59,8 @@ PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const {
 
 // virtual and protected
 const btCollisionShape* DetailedMotionState::computeNewShape() {
-    ShapeInfo shapeInfo;
+    auto shape = _avatar->createDetailedCollisionShapeForJoint(_jointIndex);
-    _avatar->computeShapeInfo(shapeInfo);
+    return shape;
-    return getShapeManager()->getShape(shapeInfo);
 }
 
 // virtual
@@ -71,37 +72,11 @@ bool DetailedMotionState::isMoving() const {
 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;
+    _body->setWorldTransform(worldTrans);
-    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
@@ -139,20 +114,17 @@ glm::quat DetailedMotionState::getObjectRotation() const {
 
 // virtual
 glm::vec3 DetailedMotionState::getObjectLinearVelocity() const {
-    return _avatar->getWorldVelocity();
+    return glm::vec3(0.0f);
 }
 
 // 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();
+    return glm::vec3(0.0f);
 }
 
 // virtual
@@ -161,7 +133,7 @@ const QUuid DetailedMotionState::getObjectID() const {
 }
 
 QString DetailedMotionState::getName() const {
-    return _avatar->getName();
+    return _avatar->getName() + "_" + _jointIndex;
 }
 
 // virtual
@@ -171,26 +143,13 @@ QUuid DetailedMotionState::getSimulatorID() const {
 
 // virtual
 void DetailedMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const {
-    group = BULLET_COLLISION_GROUP_OTHER_AVATAR;
+    group = BULLET_COLLISION_GROUP_DETAILED_AVATAR;
     mask = Physics::getDefaultCollisionMask(group);
 }
 
 // virtual
 float DetailedMotionState::getMass() const {
-    return _avatar->computeMass();
+    return 0.0f;
-}
-
-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) {
@@ -203,5 +162,5 @@ void DetailedMotionState::setRigidBody(btRigidBody* body) {
 
 void DetailedMotionState::setShape(const btCollisionShape* shape) {
     ObjectMotionState::setShape(shape);
-    cacheShapeDiameter();
 }
+

interface/src/avatar/DetailedMotionState.h

@@ -62,8 +62,6 @@ public:
     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;
@@ -76,7 +74,6 @@ public:
 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.

interface/src/avatar/OtherAvatar.cpp

@@ -117,7 +117,7 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) {
     return bytesRead;
 }
 
-void OtherAvatar::addNewMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex) {
+btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex) {
     if (jointIndex > -1 && jointIndex < _multiSphereShapes.size()) {
         auto& data = _multiSphereShapes[jointIndex].getSpheresData();
         std::vector<btVector3> positions;
@@ -127,15 +127,19 @@ void OtherAvatar::addNewMotionState(std::shared_ptr<OtherAvatar> avatar, int joi
             radiuses.push_back(sphere._radius);
         }
         btCollisionShape* shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size());
+        return shape;
+    }
+    return nullptr;
+}
+
+DetailedMotionState* OtherAvatar::createDetailedMotionStateForJoint(std::shared_ptr<OtherAvatar> avatar, int jointIndex) {
+    auto shape = createDetailedCollisionShapeForJoint(jointIndex);
     if (shape) {
         DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex);
         motionState->setMass(computeMass());
-            assert(_detailedMotionStates.size() == jointIndex);
+        return motionState;
-            _detailedMotionStates.push_back(motionState);
-        } else {
-            _detailedMotionStates.push_back(nullptr);
-        }
     }
+    return nullptr;
 }
 
 void OtherAvatar::resetDetailedMotionStates() {
@@ -162,4 +166,9 @@ void OtherAvatar::rebuildCollisionShape() {
     if (_motionState) {
         _motionState->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS);
     }
+    for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
+        if (_detailedMotionStates[i]) {
+            _detailedMotionStates[i]->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS);
+        }
+    }
 }

interface/src/avatar/OtherAvatar.h

@@ -46,8 +46,9 @@ public:
     bool shouldBeInPhysicsSimulation() const;
     bool needsPhysicsUpdate() const;
 
-    void addNewMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
+    btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex);
-    const std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
+    DetailedMotionState* createDetailedMotionStateForJoint(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
+    std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
     void resetDetailedMotionStates();
 
     friend AvatarManager;

libraries/physics/src/ObjectMotionState.cpp

@@ -195,10 +195,14 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) {
 void ObjectMotionState::setShape(const btCollisionShape* shape) {
     if (_shape != shape) {
         if (_shape) {
+            if (_type == MOTIONSTATE_TYPE_DETAILED) {
+                delete _shape;
+            } else {
                 getShapeManager()->releaseShape(_shape);
             }
+        }
         _shape = shape;
-        if (_body) {
+        if (_body && _type != MOTIONSTATE_TYPE_DETAILED) {
             updateCCDConfiguration();
         }
     }
@@ -292,7 +296,7 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine*
         if (!isReadyToComputeShape()) {
             return false;
         }
-        const btCollisionShape* newShape = _type != MOTIONSTATE_TYPE_DETAILED ? computeNewShape() : nullptr;
+        const btCollisionShape* newShape = computeNewShape();
         if (!newShape) {
             qCDebug(physics) << "Warning: failed to generate new shape!";
             // failed to generate new shape! --> keep old shape and remove shape-change flag
@@ -309,8 +313,12 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine*
             if (_shape == newShape) {
                 // the shape didn't actually change, so we clear the DIRTY_SHAPE flag
                 flags &= ~Simulation::DIRTY_SHAPE;
+                if (_type == MOTIONSTATE_TYPE_DETAILED) {
+                    delete _shape;
+                } else {
                     // and clear the reference we just created
                     getShapeManager()->releaseShape(_shape);
+                }            
             } else {
                 _body->setCollisionShape(const_cast<btCollisionShape*>(newShape));
                 setShape(newShape);

libraries/shared/src/PhysicsCollisionGroups.h

@@ -39,6 +39,8 @@ const int32_t BULLET_COLLISION_GROUP_DYNAMIC = 1 << 1;
 const int32_t BULLET_COLLISION_GROUP_KINEMATIC = 1 << 2;
 const int32_t BULLET_COLLISION_GROUP_MY_AVATAR = 1 << 3;
 const int32_t BULLET_COLLISION_GROUP_OTHER_AVATAR = 1 << 4;
+const int32_t BULLET_COLLISION_GROUP_DETAILED_AVATAR = 1 << 5;
+const int32_t BULLET_COLLISION_GROUP_DETAILED_RAY = 1 << 6;
 // ...
 const int32_t BULLET_COLLISION_GROUP_COLLISIONLESS = 1 << 31;
 
@@ -64,7 +66,8 @@ const int32_t BULLET_COLLISION_MASK_MY_AVATAR = ~(BULLET_COLLISION_GROUP_COLLISI
 // to move its avatar around correctly and to communicate its motion through the avatar-mixer.
 // Therefore, they only need to collide against things that can be affected by their motion: dynamic and MyAvatar
 const int32_t BULLET_COLLISION_MASK_OTHER_AVATAR = BULLET_COLLISION_GROUP_DYNAMIC | BULLET_COLLISION_GROUP_MY_AVATAR;
-
+const int32_t BULLET_COLLISION_MASK_DETAILED_AVATAR = BULLET_COLLISION_GROUP_DETAILED_RAY;
+const int32_t BULLET_COLLISION_MASK_DETAILED_RAY = BULLET_COLLISION_GROUP_DETAILED_AVATAR;
 // COLLISIONLESS gets an empty mask.
 const int32_t BULLET_COLLISION_MASK_COLLISIONLESS = 0;
 

libraries/shared/src/PhysicsHelpers.cpp

@@ -78,6 +78,10 @@ int32_t Physics::getDefaultCollisionMask(int32_t group) {
             return BULLET_COLLISION_MASK_MY_AVATAR;
         case  BULLET_COLLISION_GROUP_OTHER_AVATAR:
             return BULLET_COLLISION_MASK_OTHER_AVATAR;
+        case BULLET_COLLISION_GROUP_DETAILED_AVATAR:
+            return BULLET_COLLISION_MASK_DETAILED_AVATAR;
+        case BULLET_COLLISION_GROUP_DETAILED_RAY:
+            return BULLET_COLLISION_MASK_DETAILED_RAY;
         default:
             break;
     };