diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 21648f9118..b8bb7abe95 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -2992,6 +2992,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/Avatar.cpp b/interface/src/avatar/Avatar.cpp index f8040754d7..6094a0b9fe 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -157,7 +157,7 @@ void Avatar::animateScaleChanges(float deltaTime) { // snap to the end when we get close enough const float MIN_RELATIVE_SCALE_ERROR = 0.03f; - if (fabsf(_targetScale - currentScale) / _targetScale < 0.03f) { + if (fabsf(_targetScale - currentScale) / _targetScale < MIN_RELATIVE_SCALE_ERROR) { animatedScale = _targetScale; } diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index fab838aa68..689d557c48 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/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 5c8230bd88..c989f1dc71 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -509,25 +509,25 @@ glm::vec3 MyAvatar::getRightHandTipPosition() const { controller::Pose MyAvatar::getLeftHandPose() const { auto palmData = getHandData()->getCopyOfPalmData(HandData::LeftHand); return palmData.isValid() ? controller::Pose(palmData.getPosition(), palmData.getRotation(), - palmData.getVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose(); + palmData.getVelocity(), palmData.getRawAngularVelocity()) : controller::Pose(); } controller::Pose MyAvatar::getRightHandPose() const { auto palmData = getHandData()->getCopyOfPalmData(HandData::RightHand); return palmData.isValid() ? controller::Pose(palmData.getPosition(), palmData.getRotation(), - palmData.getVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose(); + palmData.getVelocity(), palmData.getRawAngularVelocity()) : controller::Pose(); } controller::Pose MyAvatar::getLeftHandTipPose() const { auto palmData = getHandData()->getCopyOfPalmData(HandData::LeftHand); return palmData.isValid() ? controller::Pose(palmData.getTipPosition(), palmData.getRotation(), - palmData.getTipVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose(); + palmData.getTipVelocity(), palmData.getRawAngularVelocity()) : controller::Pose(); } controller::Pose MyAvatar::getRightHandTipPose() const { auto palmData = getHandData()->getCopyOfPalmData(HandData::RightHand); return palmData.isValid() ? controller::Pose(palmData.getTipPosition(), palmData.getRotation(), - palmData.getTipVelocity(), palmData.getRawAngularVelocityAsQuat()) : controller::Pose(); + palmData.getTipVelocity(), palmData.getRawAngularVelocity()) : controller::Pose(); } // virtual @@ -536,7 +536,7 @@ void MyAvatar::render(RenderArgs* renderArgs, const glm::vec3& cameraPosition) { if (!_shouldRender) { return; // exit early } - + Avatar::render(renderArgs, cameraPosition); } @@ -799,7 +799,7 @@ void MyAvatar::updateLookAtTargetAvatar() { const float GREATEST_LOOKING_AT_DISTANCE = 10.0f; AvatarHash hash = DependencyManager::get()->getHashCopy(); - + foreach (const AvatarSharedPointer& avatarPointer, hash) { auto avatar = static_pointer_cast(avatarPointer); bool isCurrentTarget = avatar->getIsLookAtTarget(); @@ -1175,7 +1175,7 @@ void MyAvatar::renderBody(RenderArgs* renderArgs, ViewFrustum* renderFrustum, fl if (!_skeletonModel.isRenderable()) { return; // wait until all models are loaded } - + fixupModelsInScene(); // Render head so long as the camera isn't inside it diff --git a/interface/src/ui/ApplicationCompositor.cpp b/interface/src/ui/ApplicationCompositor.cpp index e5ecdfe217..abc3520c83 100644 --- a/interface/src/ui/ApplicationCompositor.cpp +++ b/interface/src/ui/ApplicationCompositor.cpp @@ -34,7 +34,6 @@ static const quint64 MSECS_TO_USECS = 1000ULL; static const quint64 TOOLTIP_DELAY = 500 * MSECS_TO_USECS; -static const float RETICLE_COLOR[] = { 0.0f, 198.0f / 255.0f, 244.0f / 255.0f }; static const float reticleSize = TWO_PI / 100.0f; static const float CURSOR_PIXEL_SIZE = 32.0f; diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp index 5d783a671e..7ba23b01ad 100644 --- a/libraries/avatars/src/HandData.cpp +++ b/libraries/avatars/src/HandData.cpp @@ -14,7 +14,7 @@ #include #include -#include "AvatarData.h" +#include "AvatarData.h" #include "HandData.h" @@ -38,7 +38,7 @@ PalmData& HandData::addNewPalm(Hand whichHand) { PalmData HandData::getCopyOfPalmData(Hand hand) const { QReadLocker locker(&_palmsLock); - // the palms are not necessarily added in left-right order, + // the palms are not necessarily added in left-right order, // so we have to search for the correct hand for (const auto& palm : _palms) { if (palm.whichHand() == hand && palm.isActive()) { @@ -64,7 +64,7 @@ void PalmData::addToPosition(const glm::vec3& delta) { _rawPosition += _owningHandData->worldToLocalVector(delta); } -bool HandData::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, +bool HandData::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, const PalmData*& collidingPalm) const { QReadLocker locker(&_palmsLock); @@ -93,7 +93,7 @@ glm::vec3 HandData::getBasePosition() const { float HandData::getBaseScale() const { return _owningAvatarData->getTargetScale(); } - + glm::vec3 PalmData::getFingerDirection() const { // finger points along yAxis in hand-frame const glm::vec3 LOCAL_FINGER_DIRECTION(0.0f, 1.0f, 0.0f); diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h index d782f240ee..34ed610f80 100644 --- a/libraries/avatars/src/HandData.h +++ b/libraries/avatars/src/HandData.h @@ -38,7 +38,7 @@ public: HandData(AvatarData* owningAvatar); virtual ~HandData() {} - + // position conversion glm::vec3 localToWorldPosition(const glm::vec3& localPosition) { return getBasePosition() + getBaseOrientation() * localPosition * getBaseScale(); @@ -60,7 +60,7 @@ public: /// \param penetration[out] the vector in which to store the penetration /// \param collidingPalm[out] a const PalmData* to the palm that was collided with /// \return whether or not the sphere penetrated - bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, + bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, const PalmData*& collidingPalm) const; glm::quat getBaseOrientation() const; @@ -74,7 +74,7 @@ protected: AvatarData* _owningAvatarData; std::vector _palms; mutable QReadWriteLock _palmsLock{ QReadWriteLock::Recursive }; - + glm::vec3 getBasePosition() const; float getBaseScale() const; @@ -112,13 +112,12 @@ public: void setRawAngularVelocity(const glm::vec3& angularVelocity) { _rawAngularVelocity = angularVelocity; } const glm::vec3& getRawAngularVelocity() const { return _rawAngularVelocity; } - glm::quat getRawAngularVelocityAsQuat() const { return glm::quat(_rawAngularVelocity); } void addToPosition(const glm::vec3& delta); void addToPenetration(const glm::vec3& penetration) { _totalPenetration += penetration; } void resolvePenetrations() { addToPosition(-_totalPenetration); _totalPenetration = glm::vec3(0.0f); } - + void setTipPosition(const glm::vec3& position) { _tipPosition = position; } const glm::vec3 getTipPosition() const { return _owningHandData->localToWorldPosition(_tipPosition); } const glm::vec3& getTipRawPosition() const { return _tipPosition; } @@ -126,16 +125,16 @@ public: void setTipVelocity(const glm::vec3& velocity) { _tipVelocity = velocity; } const glm::vec3 getTipVelocity() const { return _owningHandData->localToWorldDirection(_tipVelocity); } const glm::vec3& getTipRawVelocity() const { return _tipVelocity; } - + void incrementFramesWithoutData() { _numFramesWithoutData++; } void resetFramesWithoutData() { _numFramesWithoutData = 0; } int getFramesWithoutData() const { return _numFramesWithoutData; } - + // FIXME - these are used in SkeletonModel::updateRig() the skeleton/rig should probably get this information // from an action and/or the UserInputMapper instead of piping it through here. void setTrigger(float trigger) { _trigger = trigger; } float getTrigger() const { return _trigger; } - + // return world-frame: glm::vec3 getFingerDirection() const; glm::vec3 getNormal() const; @@ -148,13 +147,13 @@ private: glm::vec3 _rawAngularVelocity; glm::quat _rawDeltaRotation; glm::quat _lastRotation; - + glm::vec3 _tipPosition; glm::vec3 _tipVelocity; glm::vec3 _totalPenetration; /// accumulator for per-frame penetrations float _trigger; - + bool _isActive; /// This has current valid data int _numFramesWithoutData; /// after too many frames without data, this tracked object assumed lost. HandData* _owningHandData; diff --git a/libraries/controllers/src/controllers/Pose.cpp b/libraries/controllers/src/controllers/Pose.cpp index 2281fc98ff..f07472e9e0 100644 --- a/libraries/controllers/src/controllers/Pose.cpp +++ b/libraries/controllers/src/controllers/Pose.cpp @@ -16,7 +16,7 @@ namespace controller { Pose::Pose(const vec3& translation, const quat& rotation, - const vec3& velocity, const quat& angularVelocity) : + const vec3& velocity, const vec3& angularVelocity) : translation(translation), rotation(rotation), velocity(velocity), angularVelocity(angularVelocity), valid (true) { } bool Pose::operator==(const Pose& right) const { @@ -26,7 +26,7 @@ namespace controller { } // FIXME add margin of error? Or add an additional withinEpsilon function? - return translation == right.getTranslation() && rotation == right.getRotation() && + return translation == right.getTranslation() && rotation == right.getRotation() && velocity == right.getVelocity() && angularVelocity == right.getAngularVelocity(); } @@ -35,7 +35,7 @@ namespace controller { obj.setProperty("translation", vec3toScriptValue(engine, pose.translation)); obj.setProperty("rotation", quatToScriptValue(engine, pose.rotation)); obj.setProperty("velocity", vec3toScriptValue(engine, pose.velocity)); - obj.setProperty("angularVelocity", quatToScriptValue(engine, pose.angularVelocity)); + obj.setProperty("angularVelocity", vec3toScriptValue(engine, pose.angularVelocity)); obj.setProperty("valid", pose.valid); return obj; diff --git a/libraries/controllers/src/controllers/Pose.h b/libraries/controllers/src/controllers/Pose.h index b8d27824f3..a8a4452758 100644 --- a/libraries/controllers/src/controllers/Pose.h +++ b/libraries/controllers/src/controllers/Pose.h @@ -23,12 +23,12 @@ namespace controller { vec3 translation; quat rotation; vec3 velocity; - quat angularVelocity; + vec3 angularVelocity; bool valid{ false }; Pose() {} Pose(const vec3& translation, const quat& rotation, - const vec3& velocity = vec3(), const quat& angularVelocity = quat()); + const vec3& velocity = vec3(), const vec3& angularVelocity = vec3()); Pose(const Pose&) = default; Pose& operator = (const Pose&) = default; @@ -38,7 +38,7 @@ namespace controller { vec3 getTranslation() const { return translation; } quat getRotation() const { return rotation; } vec3 getVelocity() const { return velocity; } - quat getAngularVelocity() const { return angularVelocity; } + vec3 getAngularVelocity() const { return angularVelocity; } static QScriptValue toScriptValue(QScriptEngine* engine, const Pose& event); static void fromScriptValue(const QScriptValue& object, Pose& event); diff --git a/libraries/embedded-webserver/src/HTTPManager.cpp b/libraries/embedded-webserver/src/HTTPManager.cpp index 1f1dfa1735..4c804f2e7b 100644 --- a/libraries/embedded-webserver/src/HTTPManager.cpp +++ b/libraries/embedded-webserver/src/HTTPManager.cpp @@ -145,9 +145,14 @@ bool HTTPManager::handleHTTPRequest(HTTPConnection* connection, const QUrl& url, localFileData = localFileString.toLocal8Bit(); } - - connection->respond(HTTPConnection::StatusCode200, localFileData, - qPrintable(mimeDatabase.mimeTypeForFile(filePath).name())); + + // if this is an shtml file just make the MIME type match HTML so browsers aren't confused + // otherwise use the mimeDatabase to look it up + auto mimeType = localFileInfo.suffix() == "shtml" + ? QString { "text/html" } + : mimeDatabase.mimeTypeForFile(filePath).name(); + + connection->respond(HTTPConnection::StatusCode200, localFileData, qPrintable(mimeType)); return true; } 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/networking/src/Assignment.cpp b/libraries/networking/src/Assignment.cpp index 4c72a16f1d..e6163c776b 100644 --- a/libraries/networking/src/Assignment.cpp +++ b/libraries/networking/src/Assignment.cpp @@ -91,7 +91,7 @@ Assignment::Assignment(ReceivedMessage& message) : #endif -Assignment::Assignment(const Assignment& otherAssignment) { +Assignment::Assignment(const Assignment& otherAssignment) : QObject() { _uuid = otherAssignment._uuid; _command = otherAssignment._command; _type = otherAssignment._type; diff --git a/libraries/networking/src/udt/Socket.cpp b/libraries/networking/src/udt/Socket.cpp index 11f1b043c8..8fb5db3853 100644 --- a/libraries/networking/src/udt/Socket.cpp +++ b/libraries/networking/src/udt/Socket.cpp @@ -236,8 +236,14 @@ void Socket::readPendingDatagrams() { auto buffer = std::unique_ptr(new char[packetSizeWithHeader]); // pull the datagram - _udpSocket.readDatagram(buffer.get(), packetSizeWithHeader, - senderSockAddr.getAddressPointer(), senderSockAddr.getPortPointer()); + auto sizeRead = _udpSocket.readDatagram(buffer.get(), packetSizeWithHeader, + senderSockAddr.getAddressPointer(), senderSockAddr.getPortPointer()); + + if (sizeRead <= 0) { + // we either didn't pull anything for this packet or there was an error reading (this seems to trigger + // on windows even if there's not a packet available) + continue; + } auto it = _unfilteredHandlers.find(senderSockAddr); @@ -248,7 +254,7 @@ void Socket::readPendingDatagrams() { it->second(std::move(basePacket)); } - return; + continue; } // check if this was a control packet or a data packet @@ -276,7 +282,7 @@ void Socket::readPendingDatagrams() { packet->getDataSize(), packet->getPayloadSize())) { // the connection indicated that we should not continue processing this packet - return; + continue; } } 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); diff --git a/plugins/hifiSixense/src/SixenseManager.cpp b/plugins/hifiSixense/src/SixenseManager.cpp index 93329d768e..fb47697166 100644 --- a/plugins/hifiSixense/src/SixenseManager.cpp +++ b/plugins/hifiSixense/src/SixenseManager.cpp @@ -191,7 +191,7 @@ void SixenseManager::InputDevice::update(float deltaTime, bool jointsCaptured) { // Rotation of Palm glm::quat rotation(data->rot_quat[3], data->rot_quat[0], data->rot_quat[1], data->rot_quat[2]); handlePoseEvent(deltaTime, position, rotation, left); - rawPoses[i] = controller::Pose(position, rotation, glm::vec3(0), glm::quat()); + rawPoses[i] = controller::Pose(position, rotation, Vectors::ZERO, Vectors::ZERO); } else { _poseStateMap.clear(); _collectedSamples.clear(); @@ -457,25 +457,21 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, glm::vec3 pos rotation = _avatarRotation * postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand; glm::vec3 velocity(0.0f); - glm::quat angularVelocity; + glm::vec3 angularVelocity(0.0f); if (prevPose.isValid() && deltaTime > std::numeric_limits::epsilon()) { + auto& samples = _collectedSamples[hand]; velocity = (position - prevPose.getTranslation()) / deltaTime; + samples.first.addSample(velocity); + velocity = samples.first.average; auto deltaRot = rotation * glm::conjugate(prevPose.getRotation()); auto axis = glm::axis(deltaRot); - auto angle = glm::angle(deltaRot); - angularVelocity = glm::angleAxis(angle / deltaTime, axis); - - // Average - auto& samples = _collectedSamples[hand]; - samples.first.addSample(velocity); - velocity = samples.first.average; - - // FIXME: // Not using quaternion average yet for angular velocity because it s probably wrong but keep the MovingAverage in place - //samples.second.addSample(glm::vec4(angularVelocity.x, angularVelocity.y, angularVelocity.z, angularVelocity.w)); - //angularVelocity = glm::quat(samples.second.average.w, samples.second.average.x, samples.second.average.y, samples.second.average.z); + auto speed = glm::angle(deltaRot) / deltaTime; + angularVelocity = speed * axis; + samples.second.addSample(angularVelocity); + angularVelocity = samples.second.average; } else if (!prevPose.isValid()) { _collectedSamples[hand].first.clear(); _collectedSamples[hand].second.clear(); diff --git a/plugins/hifiSixense/src/SixenseManager.h b/plugins/hifiSixense/src/SixenseManager.h index 5cd4dc2451..0de5fe93aa 100644 --- a/plugins/hifiSixense/src/SixenseManager.h +++ b/plugins/hifiSixense/src/SixenseManager.h @@ -49,12 +49,12 @@ private: static const int CALIBRATION_STATE_IDLE = 0; static const int CALIBRATION_STATE_IN_PROGRESS = 1; static const int CALIBRATION_STATE_COMPLETE = 2; - static const glm::vec3 DEFAULT_AVATAR_POSITION; + static const glm::vec3 DEFAULT_AVATAR_POSITION; static const float CONTROLLER_THRESHOLD; - + template using SampleAverage = MovingAverage; - using Samples = std::pair, SampleAverage>; + using Samples = std::pair, SampleAverage>; using MovingAverageMap = std::map; class InputDevice : public controller::InputDevice { @@ -81,7 +81,7 @@ private: // these are calibration results glm::vec3 _avatarPosition { DEFAULT_AVATAR_POSITION }; // in hydra-frame glm::quat _avatarRotation; // in hydra-frame - + float _lastDistance; bool _requestReset { false }; bool _debugDrawRaw { false };