From 805d23567dd17277c812a33f79377d881e817ced Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 16 May 2017 16:02:37 -0700 Subject: [PATCH 1/2] fix simple typo --- interface/src/avatar/MyAvatar.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index cbd51aaaf9..521d6e195c 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -125,7 +125,7 @@ class MyAvatar : public Avatar { Q_PROPERTY(controller::Pose rightHandTipPose READ getRightHandTipPose) Q_PROPERTY(float energy READ getEnergy WRITE setEnergy) - Q_PROPERTY(float isAway READ getIsAway WRITE setAway) + Q_PROPERTY(bool isAway READ getIsAway WRITE setAway) Q_PROPERTY(bool hmdLeanRecenterEnabled READ getHMDLeanRecenterEnabled WRITE setHMDLeanRecenterEnabled) Q_PROPERTY(bool collisionsEnabled READ getCollisionsEnabled WRITE setCollisionsEnabled) From d065b569d388340ae9ff0df5902067c4b3994859 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 16 May 2017 16:03:13 -0700 Subject: [PATCH 2/2] support for variable avatar mass by size --- interface/src/avatar/AvatarManager.cpp | 1 + interface/src/avatar/AvatarMotionState.cpp | 3 -- interface/src/avatar/MyAvatar.cpp | 1 + .../src/avatar/MyCharacterController.cpp | 30 +++++++++++--- interface/src/avatar/MyCharacterController.h | 4 ++ .../src/avatars-renderer/Avatar.cpp | 11 +++++ .../src/avatars-renderer/Avatar.h | 1 + libraries/avatars/src/AvatarData.cpp | 4 +- libraries/avatars/src/AvatarData.h | 3 ++ libraries/physics/src/CharacterController.cpp | 7 +++- libraries/physics/src/CharacterController.h | 1 + libraries/physics/src/ObjectMotionState.cpp | 41 +++++++++++++++++-- libraries/physics/src/ObjectMotionState.h | 12 +++--- 13 files changed, 98 insertions(+), 21 deletions(-) diff --git a/interface/src/avatar/AvatarManager.cpp b/interface/src/avatar/AvatarManager.cpp index 1306ce03ea..d47e4cfd10 100644 --- a/interface/src/avatar/AvatarManager.cpp +++ b/interface/src/avatar/AvatarManager.cpp @@ -189,6 +189,7 @@ void AvatarManager::updateOtherAvatars(float deltaTime) { btCollisionShape* shape = const_cast(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); diff --git a/interface/src/avatar/AvatarMotionState.cpp b/interface/src/avatar/AvatarMotionState.cpp index 0305634400..91c83afcbd 100644 --- a/interface/src/avatar/AvatarMotionState.cpp +++ b/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() { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 3da9b8a214..ec4da2c2a1 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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() { diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 0cdbc77626..3d98a0e604 100755 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/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); +} diff --git a/interface/src/avatar/MyCharacterController.h b/interface/src/avatar/MyCharacterController.h index 6b38736352..fd9caface2 100644 --- a/interface/src/avatar/MyCharacterController.h +++ b/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 _topPoints; btAlignedObjectArray _bottomPoints; + btScalar _density { 1.0f }; }; #endif // hifi_MyCharacterController_h diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index 664b0094f4..82e571d0e8 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/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); diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h b/libraries/avatars-renderer/src/avatars-renderer/Avatar.h index 20704a08b2..148821db1e 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.h +++ b/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; diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 26afed152f..c23363e6b6 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/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); diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index e6e0571878..c2bf98287a 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/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); diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index e4ff1b0b44..ee240a6aac 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/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) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 0a11fad0b7..6790495ff8 100644 --- a/libraries/physics/src/CharacterController.h +++ b/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); diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 503b39dc1c..5a36d69035 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/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 { diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index 645bd6fc14..1e582ea854 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/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 };