diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 67b1be9c4d..fa5565417d 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -2988,6 +2988,9 @@ void Application::update(float deltaTime) { _physicsEngine->changeObjects(motionStates); myAvatar->prepareForPhysicsSimulation(); + _physicsEngine->forEachAction([&](EntityActionPointer action) { + action->prepareForPhysicsSimulation(); + }); getEntities()->getTree()->withWriteLock([&] { _physicsEngine->stepSimulation(); diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index fab838aa68..be562e2773 100644 --- a/interface/src/avatar/AvatarActionHold.cpp +++ b/interface/src/avatar/AvatarActionHold.cpp @@ -14,6 +14,7 @@ #include #include "avatar/AvatarManager.h" +#include "CharacterController.h" const uint16_t AvatarActionHold::holdVersion = 1; @@ -32,6 +33,64 @@ AvatarActionHold::~AvatarActionHold() { #endif } +bool AvatarActionHold::getAvatarRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) { + MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + MyCharacterController* controller = myAvatar ? myAvatar->getCharacterController() : nullptr; + if (!controller) { + qDebug() << "AvatarActionHold::getAvatarRigidBodyLocation failed to get character controller"; + return false; + } + controller->getRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation); + return true; +} + +void AvatarActionHold::prepareForPhysicsSimulation() { + auto avatarManager = DependencyManager::get(); + auto holdingAvatar = std::static_pointer_cast(avatarManager->getAvatarBySessionID(_holderID)); + + if (!holdingAvatar || !holdingAvatar->isMyAvatar()) { + return; + } + + withWriteLock([&]{ + if (_ignoreIK) { + return; + } + + glm::vec3 palmPosition; + glm::quat palmRotation; + if (_hand == "right") { + palmPosition = holdingAvatar->getRightPalmPosition(); + palmRotation = holdingAvatar->getRightPalmRotation(); + } else { + palmPosition = holdingAvatar->getLeftPalmPosition(); + palmRotation = holdingAvatar->getLeftPalmRotation(); + } + + glm::vec3 avatarRigidBodyPosition; + glm::quat avatarRigidBodyRotation; + getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation); + + // determine the difference in translation and rotation between the avatar's + // rigid body and the palm position. The avatar's rigid body will be moved by bullet + // between this call and the call to getTarget, below. A call to get*PalmPosition in + // getTarget would get the palm position of the previous location of the avatar (because + // bullet has moved the av's rigid body but the rigid body's location has not yet been + // copied out into the Avatar class. + glm::quat avatarRotationInverse = glm::inverse(avatarRigidBodyRotation); + + // the offset should be in the frame of the avatar, but something about the order + // things are updated makes this wrong: + // _palmOffsetFromRigidBody = avatarRotationInverse * (palmPosition - avatarRigidBodyPosition); + // I'll leave it here as a comment in case avatar handling changes. + _palmOffsetFromRigidBody = palmPosition - avatarRigidBodyPosition; + + // rotation should also be needed, but again, the order of updates makes this unneeded. leaving + // code here for future reference. + // _palmRotationFromRigidBody = avatarRotationInverse * palmRotation; + }); +} + std::shared_ptr AvatarActionHold::getTarget(glm::quat& rotation, glm::vec3& position) { auto avatarManager = DependencyManager::get(); auto holdingAvatar = std::static_pointer_cast(avatarManager->getAvatarBySessionID(_holderID)); @@ -40,11 +99,11 @@ std::shared_ptr AvatarActionHold::getTarget(glm::quat& rotation, glm::ve return holdingAvatar; } - withTryReadLock([&]{ + withReadLock([&]{ bool isRightHand = (_hand == "right"); glm::vec3 palmPosition { Vectors::ZERO }; glm::quat palmRotation { Quaternions::IDENTITY }; - + if (_ignoreIK && holdingAvatar->isMyAvatar()) { // We cannot ignore other avatars IK and this is not the point of this option // This is meant to make the grabbing behavior more reactive. @@ -55,6 +114,31 @@ std::shared_ptr AvatarActionHold::getTarget(glm::quat& rotation, glm::ve palmPosition = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getPosition(); palmRotation = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getRotation(); } + } else if (holdingAvatar->isMyAvatar()) { + glm::vec3 avatarRigidBodyPosition; + glm::quat avatarRigidBodyRotation; + getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation); + + // the offset and rotation between the avatar's rigid body and the palm were determined earlier + // in prepareForPhysicsSimulation. At this point, the avatar's rigid body has been moved by bullet + // and the data in the Avatar class is stale. This means that the result of get*PalmPosition will + // be stale. Instead, determine the current palm position with the current avatar's rigid body + // location and the saved offsets. + + // this line is more correct but breaks for the current way avatar data is updated. + // palmPosition = avatarRigidBodyPosition + avatarRigidBodyRotation * _palmOffsetFromRigidBody; + // instead, use this for now: + palmPosition = avatarRigidBodyPosition + _palmOffsetFromRigidBody; + + // the item jitters the least by getting the rotation based on the opinion of Avatar.h rather + // than that of the rigid body. leaving this next line here for future reference: + // palmRotation = avatarRigidBodyRotation * _palmRotationFromRigidBody; + + if (isRightHand) { + palmRotation = holdingAvatar->getRightPalmRotation(); + } else { + palmRotation = holdingAvatar->getLeftPalmRotation(); + } } else { if (isRightHand) { palmPosition = holdingAvatar->getRightPalmPosition(); @@ -103,21 +187,19 @@ void AvatarActionHold::updateActionWorker(float deltaTimeStep) { if (valid && holdCount > 0) { position /= holdCount; - bool gotLock = withTryWriteLock([&]{ + withWriteLock([&]{ _positionalTarget = position; _rotationalTarget = rotation; _positionalTargetSet = true; _rotationalTargetSet = true; _active = true; }); - if (gotLock) { - if (_kinematic) { - doKinematicUpdate(deltaTimeStep); - } else { - activateBody(); - forceBodyNonStatic(); - ObjectActionSpring::updateActionWorker(deltaTimeStep); - } + if (_kinematic) { + doKinematicUpdate(deltaTimeStep); + } else { + activateBody(); + forceBodyNonStatic(); + ObjectActionSpring::updateActionWorker(deltaTimeStep); } } } diff --git a/interface/src/avatar/AvatarActionHold.h b/interface/src/avatar/AvatarActionHold.h index 63f30a75d9..b97ec59780 100644 --- a/interface/src/avatar/AvatarActionHold.h +++ b/interface/src/avatar/AvatarActionHold.h @@ -25,18 +25,21 @@ public: AvatarActionHold(const QUuid& id, EntityItemPointer ownerEntity); virtual ~AvatarActionHold(); - virtual bool updateArguments(QVariantMap arguments); - virtual QVariantMap getArguments(); + virtual bool updateArguments(QVariantMap arguments) override; + virtual QVariantMap getArguments() override; - virtual void updateActionWorker(float deltaTimeStep); + virtual void updateActionWorker(float deltaTimeStep) override; QByteArray serialize() const; - virtual void deserialize(QByteArray serializedArguments); + virtual void deserialize(QByteArray serializedArguments) override; - virtual bool shouldSuppressLocationEdits() { return _active && !_ownerEntity.expired(); } + virtual bool shouldSuppressLocationEdits() override { return _active && !_ownerEntity.expired(); } + bool getAvatarRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation); std::shared_ptr getTarget(glm::quat& rotation, glm::vec3& position); + virtual void prepareForPhysicsSimulation() override; + private: void doKinematicUpdate(float deltaTimeStep); @@ -56,6 +59,10 @@ private: float _previousDeltaTimeStep = 0.0f; glm::vec3 _previousPositionalDelta; + + glm::vec3 _palmOffsetFromRigidBody; + // leaving this here for future refernece. + // glm::quat _palmRotationFromRigidBody; }; #endif // hifi_AvatarActionHold_h diff --git a/libraries/entities-renderer/src/RenderableModelEntityItem.cpp b/libraries/entities-renderer/src/RenderableModelEntityItem.cpp index b5203ea460..14e89b59fd 100644 --- a/libraries/entities-renderer/src/RenderableModelEntityItem.cpp +++ b/libraries/entities-renderer/src/RenderableModelEntityItem.cpp @@ -625,3 +625,11 @@ glm::vec3 RenderableModelEntityItem::getAbsoluteJointTranslationInObjectFrame(in } return glm::vec3(0.0f); } + +void RenderableModelEntityItem::locationChanged() { + EntityItem::locationChanged(); + if (_model && _model->isActive()) { + _model->setRotation(getRotation()); + _model->setTranslation(getPosition()); + } +} diff --git a/libraries/entities-renderer/src/RenderableModelEntityItem.h b/libraries/entities-renderer/src/RenderableModelEntityItem.h index 143113146e..cf948bd7a0 100644 --- a/libraries/entities-renderer/src/RenderableModelEntityItem.h +++ b/libraries/entities-renderer/src/RenderableModelEntityItem.h @@ -73,6 +73,7 @@ public: virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override; virtual void loader() override; + virtual void locationChanged() override; private: void remapTextures(); diff --git a/libraries/entities/src/EntityActionInterface.h b/libraries/entities/src/EntityActionInterface.h index a192661e52..ba59d66cf4 100644 --- a/libraries/entities/src/EntityActionInterface.h +++ b/libraries/entities/src/EntityActionInterface.h @@ -58,6 +58,8 @@ public: virtual bool shouldSuppressLocationEdits() { return false; } + virtual void prepareForPhysicsSimulation() { } + // these look in the arguments map for a named argument. if it's not found or isn't well formed, // ok will be set to false (note that it's never set to true -- set it to true before calling these). // if required is true, failure to extract an argument will cause a warning to be printed. diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 44d4269e0e..86d57b7ee9 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -15,6 +15,7 @@ #include "BulletUtil.h" #include "PhysicsCollisionGroups.h" +#include "ObjectMotionState.h" const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f); const float JUMP_SPEED = 3.5f; @@ -379,3 +380,15 @@ void CharacterController::preSimulation() { void CharacterController::postSimulation() { // postSimulation() exists for symmetry and just in case we need to do something here later } + + +bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) { + if (!_rigidBody) { + return false; + } + + const btTransform& worldTrans = _rigidBody->getCenterOfMassTransform(); + avatarRigidBodyPosition = bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset(); + avatarRigidBodyRotation = bulletToGLM(worldTrans.getRotation()); + return true; +} diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 88c02d0940..7bdc35fc0b 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -79,6 +79,8 @@ public: void setEnabled(bool enabled); bool isEnabled() const { return _enabled && _dynamicsWorld; } + bool getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation); + protected: void updateUpAxis(const glm::quat& rotation); diff --git a/libraries/physics/src/ObjectAction.h b/libraries/physics/src/ObjectAction.h index e44036eadc..4ca13f2fbf 100644 --- a/libraries/physics/src/ObjectAction.h +++ b/libraries/physics/src/ObjectAction.h @@ -29,12 +29,12 @@ public: ObjectAction(EntityActionType type, const QUuid& id, EntityItemPointer ownerEntity); virtual ~ObjectAction(); - virtual void removeFromSimulation(EntitySimulation* simulation) const; - virtual EntityItemWeakPointer getOwnerEntity() const { return _ownerEntity; } - virtual void setOwnerEntity(const EntityItemPointer ownerEntity) { _ownerEntity = ownerEntity; } + virtual void removeFromSimulation(EntitySimulation* simulation) const override; + virtual EntityItemWeakPointer getOwnerEntity() const override { return _ownerEntity; } + virtual void setOwnerEntity(const EntityItemPointer ownerEntity) override { _ownerEntity = ownerEntity; } - virtual bool updateArguments(QVariantMap arguments); - virtual QVariantMap getArguments(); + virtual bool updateArguments(QVariantMap arguments) override; + virtual QVariantMap getArguments() override; // this is called from updateAction and should be overridden by subclasses virtual void updateActionWorker(float deltaTimeStep) = 0; @@ -43,25 +43,25 @@ public: virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep); virtual void debugDraw(btIDebugDraw* debugDrawer); - virtual QByteArray serialize() const = 0; - virtual void deserialize(QByteArray serializedArguments) = 0; + virtual QByteArray serialize() const override = 0; + virtual void deserialize(QByteArray serializedArguments) override = 0; - virtual bool lifetimeIsOver(); - virtual quint64 getExpires() { return _expires; } + virtual bool lifetimeIsOver() override; + virtual quint64 getExpires() override { return _expires; } protected: quint64 localTimeToServerTime(quint64 timeValue) const; quint64 serverTimeToLocalTime(quint64 timeValue) const; virtual btRigidBody* getRigidBody(); - virtual glm::vec3 getPosition(); - virtual void setPosition(glm::vec3 position); - virtual glm::quat getRotation(); - virtual void setRotation(glm::quat rotation); - virtual glm::vec3 getLinearVelocity(); - virtual void setLinearVelocity(glm::vec3 linearVelocity); - virtual glm::vec3 getAngularVelocity(); - virtual void setAngularVelocity(glm::vec3 angularVelocity); + virtual glm::vec3 getPosition() override; + virtual void setPosition(glm::vec3 position) override; + virtual glm::quat getRotation() override; + virtual void setRotation(glm::quat rotation) override; + virtual glm::vec3 getLinearVelocity() override; + virtual void setLinearVelocity(glm::vec3 linearVelocity) override; + virtual glm::vec3 getAngularVelocity() override; + virtual void setAngularVelocity(glm::vec3 angularVelocity) override; virtual void activateBody(); virtual void forceBodyNonStatic(); diff --git a/libraries/physics/src/ObjectActionOffset.h b/libraries/physics/src/ObjectActionOffset.h index 1918da6996..ea01b10d33 100644 --- a/libraries/physics/src/ObjectActionOffset.h +++ b/libraries/physics/src/ObjectActionOffset.h @@ -22,13 +22,13 @@ public: ObjectActionOffset(const QUuid& id, EntityItemPointer ownerEntity); virtual ~ObjectActionOffset(); - virtual bool updateArguments(QVariantMap arguments); - virtual QVariantMap getArguments(); + virtual bool updateArguments(QVariantMap arguments) override; + virtual QVariantMap getArguments() override; - virtual void updateActionWorker(float deltaTimeStep); + virtual void updateActionWorker(float deltaTimeStep) override; - virtual QByteArray serialize() const; - virtual void deserialize(QByteArray serializedArguments); + virtual QByteArray serialize() const override; + virtual void deserialize(QByteArray serializedArguments) override; private: static const uint16_t offsetVersion; diff --git a/libraries/physics/src/ObjectActionSpring.h b/libraries/physics/src/ObjectActionSpring.h index caa64c3d3a..96bb900bf6 100644 --- a/libraries/physics/src/ObjectActionSpring.h +++ b/libraries/physics/src/ObjectActionSpring.h @@ -19,13 +19,13 @@ public: ObjectActionSpring(const QUuid& id, EntityItemPointer ownerEntity); virtual ~ObjectActionSpring(); - virtual bool updateArguments(QVariantMap arguments); - virtual QVariantMap getArguments(); + virtual bool updateArguments(QVariantMap arguments) override; + virtual QVariantMap getArguments() override; - virtual void updateActionWorker(float deltaTimeStep); + virtual void updateActionWorker(float deltaTimeStep) override; - virtual QByteArray serialize() const; - virtual void deserialize(QByteArray serializedArguments); + virtual QByteArray serialize() const override; + virtual void deserialize(QByteArray serializedArguments) override; protected: static const uint16_t springVersion; diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 10e285186c..22695a1b66 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -497,3 +497,11 @@ void PhysicsEngine::removeAction(const QUuid actionID) { _objectActions.remove(actionID); } } + +void PhysicsEngine::forEachAction(std::function actor) { + QHashIterator iter(_objectActions); + while (iter.hasNext()) { + iter.next(); + actor(iter.value()); + } +} diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index e7b5fd79d4..05032ccae2 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -97,6 +97,7 @@ public: EntityActionPointer getActionByID(const QUuid& actionID) const; void addAction(EntityActionPointer action); void removeAction(const QUuid actionID); + void forEachAction(std::function actor); private: void removeContacts(ObjectMotionState* motionState);