support for variable avatar mass by size

interface/src/avatar/AvatarManager.cpp

@@ -189,6 +189,7 @@ void AvatarManager::updateOtherAvatars(float deltaTime) {
             btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
             if (shape) {
                 AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
+                motionState->setMass(avatar->computeMass());
                 avatar->setPhysicsCallback([=] (uint32_t flags) { motionState->addDirtyFlags(flags); });
                 _motionStates.insert(avatar.get(), motionState);
                 _motionStatesToAddToPhysics.insert(motionState);

interface/src/avatar/AvatarMotionState.cpp

@@ -19,9 +19,6 @@
 AvatarMotionState::AvatarMotionState(AvatarSharedPointer avatar, const btCollisionShape* shape) : ObjectMotionState(shape), _avatar(avatar) {
     assert(_avatar);
     _type = MOTIONSTATE_TYPE_AVATAR;
-    if (_shape) {
-        _mass = 100.0f; // HACK
-    }
 }
 
 AvatarMotionState::~AvatarMotionState() {

interface/src/avatar/MyAvatar.cpp

@@ -236,6 +236,7 @@ MyAvatar::MyAvatar(QThread* thread, RigPointer rig) :
     });
 
     connect(rig.get(), SIGNAL(onLoadComplete()), this, SIGNAL(onLoadComplete()));
+    _characterController.setDensity(_density);
 }
 
 MyAvatar::~MyAvatar() {

interface/src/avatar/MyCharacterController.cpp

@@ -49,12 +49,9 @@ void MyCharacterController::updateShapeIfNecessary() {
             // create RigidBody if it doesn't exist
             if (!_rigidBody) {
                 btCollisionShape* shape = computeShape();
-
-                // HACK: use some simple mass property defaults for now
-                const btScalar DEFAULT_AVATAR_MASS = 100.0f;
-                const btVector3 DEFAULT_AVATAR_INERTIA_TENSOR(30.0f, 8.0f, 30.0f);
-
-                _rigidBody = new btRigidBody(DEFAULT_AVATAR_MASS, nullptr, shape, DEFAULT_AVATAR_INERTIA_TENSOR);
+                btScalar mass = 1.0f;
+                btVector3 inertia(1.0f, 1.0f, 1.0f);
+                _rigidBody = new btRigidBody(mass, nullptr, shape, inertia);
             } else {
                 btCollisionShape* shape = _rigidBody->getCollisionShape();
                 if (shape) {
@@ -63,6 +60,7 @@ void MyCharacterController::updateShapeIfNecessary() {
                 shape = computeShape();
                 _rigidBody->setCollisionShape(shape);
             }
+            updateMassProperties();
 
             _rigidBody->setSleepingThresholds(0.0f, 0.0f);
             _rigidBody->setAngularFactor(0.0f);
@@ -331,3 +329,23 @@ void MyCharacterController::initRayShotgun(const btCollisionWorld* world) {
         }
     }
 }
+
+void MyCharacterController::updateMassProperties() {
+    assert(_rigidBody);
+    // the inertia tensor of a capsule with Y-axis of symmetry, radius R and cylinder height H is:
+    // Ix = density * (volumeCylinder * (H^2 / 12 + R^2 / 4) + volumeSphere * (2R^2 / 5 + H^2 / 2 + 3HR / 8))
+    // Iy = density * (volumeCylinder * (R^2 / 2) + volumeSphere * (2R^2 / 5)
+    btScalar r2 = _radius * _radius;
+    btScalar h2 = 4.0f * _halfHeight * _halfHeight;
+    btScalar volumeSphere = 4.0f * PI * r2 * _radius / 3.0f;
+    btScalar volumeCylinder = TWO_PI * r2 * 2.0f * _halfHeight;
+    btScalar cylinderXZ = volumeCylinder * (h2 / 12.0f + r2 / 4.0f);
+    btScalar capsXZ = volumeSphere * (2.0f * r2 / 5.0f + h2 / 2.0f + 6.0f * _halfHeight * _radius / 8.0f);
+    btScalar inertiaXZ = _density * (cylinderXZ + capsXZ);
+    btScalar inertiaY = _density * ((volumeCylinder * r2 / 2.0f) + volumeSphere * (2.0f * r2 / 5.0f));
+    btVector3 inertia(inertiaXZ, inertiaY, inertiaXZ);
+
+    btScalar mass = _density * (volumeCylinder + volumeSphere);
+
+    _rigidBody->setMassProps(mass, inertia);
+}

interface/src/avatar/MyCharacterController.h

@@ -40,8 +40,11 @@ public:
     /// return true if RayShotgun hits anything
     bool testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result);
 
+    void setDensity(btScalar density) { _density = density; }
+
 protected:
     void initRayShotgun(const btCollisionWorld* world);
+    void updateMassProperties() override;
 
 private:
     btConvexHullShape* computeShape() const;
@@ -52,6 +55,7 @@ protected:
     // shotgun scan data
     btAlignedObjectArray<btVector3> _topPoints;
     btAlignedObjectArray<btVector3> _bottomPoints;
+    btScalar _density { 1.0f };
 };
 
 #endif // hifi_MyCharacterController_h

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

@@ -1293,6 +1293,17 @@ void Avatar::getCapsule(glm::vec3& start, glm::vec3& end, float& radius) {
     radius = halfExtents.x;
 }
 
+float Avatar::computeMass() {
+    float radius;
+    glm::vec3 start, end;
+    getCapsule(start, end, radius);
+    // NOTE:
+    // volumeOfCapsule = volumeOfCylinder + volumeOfSphere
+    // volumeOfCapsule = (2PI * R^2 * H) + (4PI * R^3 / 3)
+    // volumeOfCapsule = 2PI * R^2 * (H + 2R/3)
+    return _density * TWO_PI * radius * radius * (glm::length(end - start) + 2.0f * radius / 3.0f);
+}
+
 // virtual
 void Avatar::rebuildCollisionShape() {
     addPhysicsFlags(Simulation::DIRTY_SHAPE);

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

@@ -196,6 +196,7 @@ public:
 
     virtual void computeShapeInfo(ShapeInfo& shapeInfo);
     void getCapsule(glm::vec3& start, glm::vec3& end, float& radius);
+    float computeMass();
 
     using SpatiallyNestable::setPosition;
     virtual void setPosition(const glm::vec3& position) override;

libraries/avatars/src/AvatarData.cpp

@@ -52,6 +52,7 @@ const QString AvatarData::FRAME_NAME = "com.highfidelity.recording.AvatarData";
 static const int TRANSLATION_COMPRESSION_RADIX = 12;
 static const int SENSOR_TO_WORLD_SCALE_RADIX = 10;
 static const float AUDIO_LOUDNESS_SCALE = 1024.0f;
+static const float DEFAULT_AVATAR_DENSITY = 1000.0f; // density of water
 
 #define ASSERT(COND)  do { if (!(COND)) { abort(); } } while(0)
 
@@ -65,7 +66,8 @@ AvatarData::AvatarData() :
     _headData(NULL),
     _errorLogExpiry(0),
     _owningAvatarMixer(),
-    _targetVelocity(0.0f)
+    _targetVelocity(0.0f),
+    _density(DEFAULT_AVATAR_DENSITY)
 {
     setBodyPitch(0.0f);
     setBodyYaw(-90.0f);

libraries/avatars/src/AvatarData.h

@@ -626,6 +626,8 @@ public:
         _identityUpdatedAt = usecTimestampNow();
     }
 
+    float getDensity() const { return _density; }
+
 signals:
     void displayNameChanged();
 
@@ -782,6 +784,7 @@ protected:
 
     bool _identityDataChanged { false };
     quint64 _identityUpdatedAt { 0 };
+    float _density;
 
 private:
     friend void avatarStateFromFrame(const QByteArray& frameData, AvatarData* _avatar);

libraries/physics/src/CharacterController.cpp

@@ -112,6 +112,9 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
             _dynamicsWorld = nullptr;
         }
         int16_t collisionGroup = computeCollisionGroup();
+        if (_rigidBody) {
+            updateMassProperties();
+        }
         if (world && _rigidBody) {
             // add to new world
             _dynamicsWorld = world;
@@ -127,7 +130,9 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
         _ghost.setCollisionGroupAndMask(collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ collisionGroup));
         _ghost.setCollisionWorld(_dynamicsWorld);
         _ghost.setRadiusAndHalfHeight(_radius, _halfHeight);
-        _ghost.setWorldTransform(_rigidBody->getWorldTransform());
+        if (_rigidBody) {
+            _ghost.setWorldTransform(_rigidBody->getWorldTransform());
+        }
     }
     if (_dynamicsWorld) {
         if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {

libraries/physics/src/CharacterController.h

@@ -128,6 +128,7 @@ protected:
     void setState(State state);
 #endif
 
+    virtual void updateMassProperties() = 0;
     void updateGravity();
     void updateUpAxis(const glm::quat& rotation);
     bool checkForSupport(btCollisionWorld* collisionWorld);

libraries/physics/src/ObjectMotionState.cpp

@@ -63,10 +63,7 @@ ShapeManager* ObjectMotionState::getShapeManager() {
 }
 
 ObjectMotionState::ObjectMotionState(const btCollisionShape* shape) :
-    _motionType(MOTION_TYPE_STATIC),
     _shape(shape),
-    _body(nullptr),
-    _mass(0.0f),
     _lastKinematicStep(worldSimulationStep)
 {
 }
@@ -74,7 +71,43 @@ ObjectMotionState::ObjectMotionState(const btCollisionShape* shape) :
 ObjectMotionState::~ObjectMotionState() {
     assert(!_body);
     setShape(nullptr);
-    _type = MOTIONSTATE_TYPE_INVALID;
+}
+
+void ObjectMotionState::setMass(float mass) {
+    _density = 1.0f;
+    if (_shape) {
+        // we compute the density for the current shape's Aabb volume
+        // and save that instead of the total mass
+        btTransform transform;
+        transform.setIdentity();
+        btVector3 minCorner, maxCorner;
+        _shape->getAabb(transform, minCorner, maxCorner);
+        btVector3 diagonal = maxCorner - minCorner;
+        float volume = diagonal.getX() * diagonal.getY() * diagonal.getZ();
+        if (volume > EPSILON) {
+            _density = fabsf(mass) / volume;
+        }
+    }
+}
+
+float ObjectMotionState::getMass() const {
+    if (_shape) {
+        // scale the density by the current Aabb volume to get mass
+        btTransform transform;
+        transform.setIdentity();
+        btVector3 minCorner, maxCorner;
+        _shape->getAabb(transform, minCorner, maxCorner);
+        btVector3 diagonal = maxCorner - minCorner;
+        float volume = diagonal.getX() * diagonal.getY() * diagonal.getZ();
+
+        // cap the max mass for numerical stability
+        const float MIN_OBJECT_MASS = 0.0f;
+        const float MAX_OBJECT_DENSITY = 20000.0f; // kg/m^3 density of Tungsten
+        const float MAX_OBJECT_VOLUME = 1.0e6f;
+        const float MAX_OBJECT_MASS = MAX_OBJECT_DENSITY * MAX_OBJECT_VOLUME;
+        return glm::clamp(_density * volume, MIN_OBJECT_MASS, MAX_OBJECT_MASS);
+    }
+    return 0.0f;
 }
 
 void ObjectMotionState::setBodyLinearVelocity(const glm::vec3& velocity) const {

libraries/physics/src/ObjectMotionState.h

@@ -93,8 +93,8 @@ public:
     MotionStateType getType() const { return _type; }
     virtual PhysicsMotionType getMotionType() const { return _motionType; }
 
-    void setMass(float mass) { _mass = fabsf(mass); }
-    float getMass() { return _mass; }
+    void setMass(float mass);
+    float getMass() const;
 
     void setBodyLinearVelocity(const glm::vec3& velocity) const;
     void setBodyAngularVelocity(const glm::vec3& velocity) const;
@@ -159,12 +159,12 @@ protected:
     void setRigidBody(btRigidBody* body);
     virtual void setShape(const btCollisionShape* shape);
 
-    MotionStateType _type = MOTIONSTATE_TYPE_INVALID; // type of MotionState
-    PhysicsMotionType _motionType; // type of motion: KINEMATIC, DYNAMIC, or STATIC
+    MotionStateType _type { MOTIONSTATE_TYPE_INVALID }; // type of MotionState
+    PhysicsMotionType _motionType { MOTION_TYPE_STATIC }; // type of motion: KINEMATIC, DYNAMIC, or STATIC
 
     const btCollisionShape* _shape;
-    btRigidBody* _body;
-    float _mass;
+    btRigidBody* _body { nullptr };
+    float _density { 1.0f };
 
     uint32_t _lastKinematicStep;
     bool _hasInternalKinematicChanges { false };