From d3c57821c0338b70e068b01f2b5f6c9721609773 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 16 Dec 2015 14:00:04 -0800 Subject: [PATCH] call a method on actions before each physics simulation step. use this to attempt to dejitter held objects --- interface/src/Application.cpp | 3 + interface/src/avatar/AvatarActionHold.cpp | 70 ++++++++++++++++++- interface/src/avatar/AvatarActionHold.h | 14 ++-- .../entities/src/EntityActionInterface.h | 2 + libraries/physics/src/CharacterController.cpp | 13 ++++ libraries/physics/src/CharacterController.h | 2 + libraries/physics/src/ObjectAction.h | 34 ++++----- libraries/physics/src/ObjectActionOffset.h | 10 +-- libraries/physics/src/ObjectActionSpring.h | 10 +-- libraries/physics/src/PhysicsEngine.cpp | 8 +++ libraries/physics/src/PhysicsEngine.h | 1 + 11 files changed, 134 insertions(+), 33 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index f3291469cb..e28d1b2eac 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -2989,6 +2989,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..87cf53143c 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,57 @@ AvatarActionHold::~AvatarActionHold() { #endif } +void AvatarActionHold::prepareForPhysicsSimulation() { + auto avatarManager = DependencyManager::get(); + auto holdingAvatar = std::static_pointer_cast(avatarManager->getAvatarBySessionID(_holderID)); + + if (!holdingAvatar) { + return; + } + + withTryReadLock([&]{ + bool isRightHand = (_hand == "right"); + + if (_ignoreIK && holdingAvatar->isMyAvatar()) { + return; + } + + if (holdingAvatar->isMyAvatar()) { + glm::vec3 palmPosition { Vectors::ZERO }; + glm::quat palmRotation { Quaternions::IDENTITY }; + if (isRightHand) { + palmPosition = holdingAvatar->getHand()->getCopyOfPalmData(HandData::RightHand).getPosition(); + palmRotation = holdingAvatar->getHand()->getCopyOfPalmData(HandData::RightHand).getRotation(); + } else { + palmPosition = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getPosition(); + palmRotation = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getRotation(); + } + + // XXX dup code + MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + MyCharacterController* controller = myAvatar ? myAvatar->getCharacterController() : nullptr; + if (!controller) { + qDebug() << "AvatarActionHold::prepareForPhysicsSimulation failed to get character controller"; + return; + } + glm::vec3 avatarRigidBodyPosition; + glm::quat avatarRigidBodyRotation; + controller->getRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation); + + + if (isRightHand) { + palmPosition = holdingAvatar->getRightPalmPosition(); + palmRotation = holdingAvatar->getRightPalmRotation(); + } else { + palmPosition = holdingAvatar->getLeftPalmPosition(); + palmRotation = holdingAvatar->getLeftPalmRotation(); + } + + _palmOffsetFromRigidBody = palmPosition - avatarRigidBodyPosition; + } + }); +} + std::shared_ptr AvatarActionHold::getTarget(glm::quat& rotation, glm::vec3& position) { auto avatarManager = DependencyManager::get(); auto holdingAvatar = std::static_pointer_cast(avatarManager->getAvatarBySessionID(_holderID)); @@ -44,7 +96,7 @@ std::shared_ptr AvatarActionHold::getTarget(glm::quat& rotation, glm::ve 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 +107,22 @@ 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()) { + // XXX dup code + MyAvatar* myAvatar = DependencyManager::get()->getMyAvatar(); + MyCharacterController* controller = myAvatar ? myAvatar->getCharacterController() : nullptr; + if (!controller) { + qDebug() << "AvatarActionHold::prepareForPhysicsSimulation failed to get character controller"; + return; + } + glm::vec3 avatarRigidBodyPosition; + glm::quat avatarRigidBodyRotation; + controller->getRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation); + + + + palmPosition = avatarRigidBodyPosition + _palmOffsetFromRigidBody; + palmRotation = holdingAvatar->getRightPalmRotation(); // XXX } else { if (isRightHand) { palmPosition = holdingAvatar->getRightPalmPosition(); diff --git a/interface/src/avatar/AvatarActionHold.h b/interface/src/avatar/AvatarActionHold.h index 63f30a75d9..2746c817ca 100644 --- a/interface/src/avatar/AvatarActionHold.h +++ b/interface/src/avatar/AvatarActionHold.h @@ -25,18 +25,20 @@ 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(); } std::shared_ptr getTarget(glm::quat& rotation, glm::vec3& position); + virtual void prepareForPhysicsSimulation() override; + private: void doKinematicUpdate(float deltaTimeStep); @@ -56,6 +58,8 @@ private: float _previousDeltaTimeStep = 0.0f; glm::vec3 _previousPositionalDelta; + + glm::vec3 _palmOffsetFromRigidBody; }; #endif // hifi_AvatarActionHold_h 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);