diff --git a/examples/moving-platform.js b/examples/moving-platform.js new file mode 100644 index 0000000000..a3022a3904 --- /dev/null +++ b/examples/moving-platform.js @@ -0,0 +1,40 @@ +var platform; + +function init() { + platform = Entities.addEntity({ + name: "platform", + type: "Box", + position: { x: 0, y: 0, z: 0 }, + dimensions: { x: 10, y: 2, z: 10 }, + color: { red: 0, green: 0, blue: 255 }, + gravity: { x: 0, y: 0, z: 0 }, + visible: true, + locked: false, + lifetime: 6000, + velocity: { x: 1.0, y: 0, z: 0 }, + damping: 0, + isDynamic: false + }); + if (platform) { + if (MyAvatar.getParentID() != platform) { + MyAvatar.position = { x: 0, y: 3, z: 0 }; + MyAvatar.setParentID(platform); + } + } +} + +function update(dt) { + +} + +function shutdown() { + if (platform) { + MyAvatar.setParentID(0); + Entities.deleteEntity(platform); + } +} + +Script.setTimeout(init, 3000); + +Script.update.connect(update); +Script.scriptEnding.connect(shutdown); diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index b67d176852..2eab67f192 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -86,9 +86,7 @@ Avatar::Avatar(RigPointer rig) : _positionDeltaAccumulator(0.0f), _lastVelocity(0.0f), _acceleration(0.0f), - _angularVelocity(0.0f), _lastAngularVelocity(0.0f), - _angularAcceleration(0.0f), _lastOrientation(), _leanScale(0.5f), _worldUpDirection(DEFAULT_UP_DIRECTION), @@ -235,9 +233,6 @@ void Avatar::simulate(float deltaTime) { _displayNameAlpha = abs(_displayNameAlpha - _displayNameTargetAlpha) < 0.01f ? _displayNameTargetAlpha : _displayNameAlpha; } - // NOTE: we shouldn't extrapolate an Avatar instance forward in time... - // until velocity is included in AvatarData update message. - //_position += _velocity * deltaTime; measureMotionDerivatives(deltaTime); simulateAttachments(deltaTime); @@ -255,7 +250,7 @@ bool Avatar::isLookingAtMe(AvatarSharedPointer avatar) const { void Avatar::slamPosition(const glm::vec3& newPosition) { setPosition(newPosition); _positionDeltaAccumulator = glm::vec3(0.0f); - _velocity = glm::vec3(0.0f); + setVelocity(glm::vec3(0.0f)); _lastVelocity = glm::vec3(0.0f); } @@ -269,15 +264,17 @@ void Avatar::measureMotionDerivatives(float deltaTime) { float invDeltaTime = 1.0f / deltaTime; // Floating point error prevents us from computing velocity in a naive way // (e.g. vel = (pos - oldPos) / dt) so instead we use _positionOffsetAccumulator. - _velocity = _positionDeltaAccumulator * invDeltaTime; + glm::vec3 velocity = _positionDeltaAccumulator * invDeltaTime; _positionDeltaAccumulator = glm::vec3(0.0f); - _acceleration = (_velocity - _lastVelocity) * invDeltaTime; - _lastVelocity = _velocity; + _acceleration = (velocity - _lastVelocity) * invDeltaTime; + _lastVelocity = velocity; + setVelocity(velocity); + // angular glm::quat orientation = getOrientation(); glm::quat delta = glm::inverse(_lastOrientation) * orientation; - _angularVelocity = safeEulerAngles(delta) * invDeltaTime; - _angularAcceleration = (_angularVelocity - _lastAngularVelocity) * invDeltaTime; + glm::vec3 angularVelocity = glm::axis(delta) * glm::angle(delta) * invDeltaTime; + setAngularVelocity(angularVelocity); _lastOrientation = getOrientation(); } diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index b23b64b384..0f84f35a25 100644 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -135,8 +135,6 @@ public: Q_INVOKABLE glm::vec3 getNeckPosition() const; Q_INVOKABLE glm::vec3 getAcceleration() const { return _acceleration; } - Q_INVOKABLE glm::vec3 getAngularVelocity() const { return _angularVelocity; } - Q_INVOKABLE glm::vec3 getAngularAcceleration() const { return _angularAcceleration; } Q_INVOKABLE bool getShouldRender() const { return !_shouldSkipRender; } diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b3cfc10bd3..d86af92a55 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1080,6 +1080,15 @@ void MyAvatar::rebuildCollisionShape() { void MyAvatar::prepareForPhysicsSimulation() { relayDriveKeysToCharacterController(); + + bool success; + glm::vec3 parentVelocity = getParentVelocity(success); + if (!success) { + qDebug() << "Warning: getParentVelocity failed" << getID(); + parentVelocity = glm::vec3(); + } + _characterController.setParentVelocity(parentVelocity); + _characterController.setTargetVelocity(getTargetVelocity()); _characterController.setPositionAndOrientation(getPosition(), getOrientation()); if (qApp->isHMDMode()) { diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp index 07156e9294..ee77859337 100644 --- a/interface/src/avatar/MyCharacterController.cpp +++ b/interface/src/avatar/MyCharacterController.cpp @@ -67,6 +67,7 @@ void MyCharacterController::updateShapeIfNecessary() { _rigidBody->setAngularFactor(0.0f); _rigidBody->setWorldTransform(btTransform(glmToBullet(_avatar->getOrientation()), glmToBullet(_avatar->getPosition()))); + _rigidBody->setDamping(0.0f, 0.0f); if (_state == State::Hover) { _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f)); } else { diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 28ae27d11f..c77c4fdd78 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -148,7 +148,11 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { _rig->updateFromHandParameters(handParams, deltaTime); Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState()); - _rig->computeMotionAnimationState(deltaTime, _owningAvatar->getPosition(), _owningAvatar->getVelocity(), _owningAvatar->getOrientation(), ccState); + + auto velocity = myAvatar->getLocalVelocity(); + auto position = myAvatar->getLocalPosition(); + auto orientation = myAvatar->getLocalOrientation(); + _rig->computeMotionAnimationState(deltaTime, position, velocity, orientation, ccState); // evaluate AnimGraph animation and update jointStates. Model::updateRig(deltaTime, parentTransform); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2fb11b33d7..4146d8e1cf 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -507,32 +507,7 @@ static const std::vector LATERAL_SPEEDS = { 0.2f, 0.65f }; // m/s void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState) { glm::vec3 front = worldRotation * IDENTITY_FRONT; - - // It can be more accurate/smooth to use velocity rather than position, - // but some modes (e.g., hmd standing) update position without updating velocity. - // It's very hard to debug hmd standing. (Look down at yourself, or have a second person observe. HMD third person is a bit undefined...) - // So, let's create our own workingVelocity from the worldPosition... - glm::vec3 workingVelocity = _lastVelocity; - glm::vec3 positionDelta = worldPosition - _lastPosition; - - // Don't trust position delta if deltaTime is 'small'. - // NOTE: This is mostly just a work around for an issue in oculus 0.7 runtime, where - // Application::idle() is being called more frequently and with smaller dt's then expected. - const float SMALL_DELTA_TIME = 0.006f; // 6 ms - if (deltaTime > SMALL_DELTA_TIME) { - workingVelocity = positionDelta / deltaTime; - } - -#if !WANT_DEBUG - // But for smoothest (non-hmd standing) results, go ahead and use velocity: - if (!positionDelta.x && !positionDelta.y && !positionDelta.z) { - workingVelocity = worldVelocity; - } -#endif - - if (deltaTime > SMALL_DELTA_TIME) { - _lastVelocity = workingVelocity; - } + glm::vec3 workingVelocity = worldVelocity; { glm::vec3 localVel = glm::inverse(worldRotation) * workingVelocity; @@ -809,7 +784,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos // compute blend based on velocity const float JUMP_SPEED = 3.5f; - float alpha = glm::clamp(-_lastWorldVelocity.y / JUMP_SPEED, -1.0f, 1.0f) + 1.0f; + float alpha = glm::clamp(-_lastVelocity.y / JUMP_SPEED, -1.0f, 1.0f) + 1.0f; _animVars.set("inAirAlpha", alpha); } @@ -827,7 +802,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos _lastFront = front; _lastPosition = worldPosition; - _lastWorldVelocity = worldVelocity; + _lastVelocity = workingVelocity; } // Allow script to add/remove handlers and report results, from within their thread. diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index a8c082339a..e4668d6c2a 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -267,7 +267,6 @@ public: glm::vec3 _lastFront; glm::vec3 _lastPosition; glm::vec3 _lastVelocity; - glm::vec3 _lastWorldVelocity; glm::vec3 _eyesInRootFrame { Vectors::ZERO }; QUrl _animGraphURL; diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 3933d705fc..d14df7b05a 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -64,7 +64,6 @@ AvatarData::AvatarData() : _billboard(), _errorLogExpiry(0), _owningAvatarMixer(), - _velocity(0.0f), _targetVelocity(0.0f), _localAABox(DEFAULT_LOCAL_AABOX_CORNER, DEFAULT_LOCAL_AABOX_SCALE) { diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index eac1917533..cf6cf80162 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -151,6 +151,9 @@ class AvatarData : public QObject, public SpatiallyNestable { Q_PROPERTY(float headYaw READ getHeadYaw WRITE setHeadYaw) Q_PROPERTY(float headRoll READ getHeadRoll WRITE setHeadRoll) + Q_PROPERTY(glm::vec3 velocity READ getVelocity WRITE setVelocity) + Q_PROPERTY(glm::vec3 angularVelocity READ getAngularVelocity WRITE setAngularVelocity) + Q_PROPERTY(float audioLoudness READ getAudioLoudness WRITE setAudioLoudness) Q_PROPERTY(float audioAverageLoudness READ getAudioAverageLoudness WRITE setAudioAverageLoudness) @@ -334,8 +337,6 @@ public: int getAverageBytesReceivedPerSecond() const; int getReceiveRate() const; - void setVelocity(const glm::vec3 velocity) { _velocity = velocity; } - Q_INVOKABLE glm::vec3 getVelocity() const { return _velocity; } const glm::vec3& getTargetVelocity() const { return _targetVelocity; } bool shouldDie() const { return _owningAvatarMixer.isNull() || getUsecsSinceLastUpdate() > AVATAR_SILENCE_THRESHOLD_USECS; } @@ -406,7 +407,6 @@ protected: /// Loads the joint indices, names from the FST file (if any) virtual void updateJointMappings(); - glm::vec3 _velocity; glm::vec3 _targetVelocity; AABox _localAABox; diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index bb6d5308c9..e56f2c267a 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -49,7 +49,6 @@ EntityItem::EntityItem(const EntityItemID& entityItemID) : _localRenderAlpha(ENTITY_ITEM_DEFAULT_LOCAL_RENDER_ALPHA), _density(ENTITY_ITEM_DEFAULT_DENSITY), _volumeMultiplier(1.0f), - _velocity(ENTITY_ITEM_DEFAULT_VELOCITY), _gravity(ENTITY_ITEM_DEFAULT_GRAVITY), _acceleration(ENTITY_ITEM_DEFAULT_ACCELERATION), _damping(ENTITY_ITEM_DEFAULT_DAMPING), @@ -60,7 +59,6 @@ EntityItem::EntityItem(const EntityItemID& entityItemID) : _scriptTimestamp(ENTITY_ITEM_DEFAULT_SCRIPT_TIMESTAMP), _collisionSoundURL(ENTITY_ITEM_DEFAULT_COLLISION_SOUND_URL), _registrationPoint(ENTITY_ITEM_DEFAULT_REGISTRATION_POINT), - _angularVelocity(ENTITY_ITEM_DEFAULT_ANGULAR_VELOCITY), _angularDamping(ENTITY_ITEM_DEFAULT_ANGULAR_DAMPING), _visible(ENTITY_ITEM_DEFAULT_VISIBLE), _collisionless(ENTITY_ITEM_DEFAULT_COLLISIONLESS), @@ -78,6 +76,8 @@ EntityItem::EntityItem(const EntityItemID& entityItemID) : _physicsInfo(nullptr), _simulated(false) { + setVelocity(ENTITY_ITEM_DEFAULT_VELOCITY); + setAngularVelocity(ENTITY_ITEM_DEFAULT_ANGULAR_VELOCITY); // explicitly set transform parts to set dirty flags used by batch rendering setScale(ENTITY_ITEM_DEFAULT_DIMENSIONS); quint64 now = usecTimestampNow(); @@ -886,41 +886,45 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { } if (hasAngularVelocity()) { + glm::vec3 angularVelocity = getAngularVelocity(); + // angular damping if (_angularDamping > 0.0f) { - _angularVelocity *= powf(1.0f - _angularDamping, timeElapsed); + angularVelocity *= powf(1.0f - _angularDamping, timeElapsed); #ifdef WANT_DEBUG qCDebug(entities) << " angularDamping :" << _angularDamping; - qCDebug(entities) << " newAngularVelocity:" << _angularVelocity; + qCDebug(entities) << " newAngularVelocity:" << angularVelocity; #endif } - float angularSpeed = glm::length(_angularVelocity); + float angularSpeed = glm::length(angularVelocity); const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) { if (setFlags && angularSpeed > 0.0f) { _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; } - _angularVelocity = ENTITY_ITEM_ZERO_VEC3; + angularVelocity = ENTITY_ITEM_ZERO_VEC3; } else { // for improved agreement with the way Bullet integrates rotations we use an approximation // and break the integration into bullet-sized substeps glm::quat rotation = getRotation(); float dt = timeElapsed; while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) { - glm::quat dQ = computeBulletRotationStep(_angularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP); + glm::quat dQ = computeBulletRotationStep(angularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP); rotation = glm::normalize(dQ * rotation); dt -= PHYSICS_ENGINE_FIXED_SUBSTEP; } // NOTE: this final partial substep can drift away from a real Bullet simulation however // it only becomes significant for rapidly rotating objects // (e.g. around PI/4 radians per substep, or 7.5 rotations/sec at 60 substeps/sec). - glm::quat dQ = computeBulletRotationStep(_angularVelocity, dt); + glm::quat dQ = computeBulletRotationStep(angularVelocity, dt); rotation = glm::normalize(dQ * rotation); setRotation(rotation); } + + setAngularVelocity(angularVelocity); } if (hasVelocity()) { @@ -1077,9 +1081,9 @@ EntityItemProperties EntityItem::getProperties(EntityPropertyFlags desiredProper void EntityItem::getAllTerseUpdateProperties(EntityItemProperties& properties) const { // a TerseUpdate includes the transform and its derivatives properties._position = getLocalPosition(); - properties._velocity = _velocity; + properties._velocity = getLocalVelocity(); properties._rotation = getLocalOrientation(); - properties._angularVelocity = _angularVelocity; + properties._angularVelocity = getLocalAngularVelocity(); properties._acceleration = _acceleration; properties._positionChanged = true; @@ -1406,13 +1410,15 @@ void EntityItem::updateVelocity(const glm::vec3& value) { if (shouldSuppressLocationEdits()) { return; } - if (_velocity != value) { + glm::vec3 velocity = getLocalVelocity(); + if (velocity != value) { const float MIN_LINEAR_SPEED = 0.001f; if (glm::length(value) < MIN_LINEAR_SPEED) { - _velocity = ENTITY_ITEM_ZERO_VEC3; + velocity = ENTITY_ITEM_ZERO_VEC3; } else { - _velocity = value; + velocity = value; } + setLocalVelocity(velocity); _dirtyFlags |= Simulation::DIRTY_LINEAR_VELOCITY; } } @@ -1436,13 +1442,15 @@ void EntityItem::updateAngularVelocity(const glm::vec3& value) { if (shouldSuppressLocationEdits()) { return; } - if (_angularVelocity != value) { + glm::vec3 angularVelocity = getLocalAngularVelocity(); + if (angularVelocity != value) { const float MIN_ANGULAR_SPEED = 0.0002f; if (glm::length(value) < MIN_ANGULAR_SPEED) { - _angularVelocity = ENTITY_ITEM_ZERO_VEC3; + angularVelocity = ENTITY_ITEM_ZERO_VEC3; } else { - _angularVelocity = value; + angularVelocity = value; } + setLocalAngularVelocity(angularVelocity); _dirtyFlags |= Simulation::DIRTY_ANGULAR_VELOCITY; } } diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index 8271aedb15..c8b54bca87 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -193,9 +193,7 @@ public: float getDensity() const { return _density; } - const glm::vec3& getVelocity() const { return _velocity; } /// get velocity in meters - void setVelocity(const glm::vec3& value) { _velocity = value; } /// velocity in meters - bool hasVelocity() const { return _velocity != ENTITY_ITEM_ZERO_VEC3; } + bool hasVelocity() const { return getVelocity() != ENTITY_ITEM_ZERO_VEC3; } const glm::vec3& getGravity() const { return _gravity; } /// get gravity in meters void setGravity(const glm::vec3& value) { _gravity = value; } /// gravity in meters @@ -255,9 +253,7 @@ public: void setRegistrationPoint(const glm::vec3& value) { _registrationPoint = glm::clamp(value, 0.0f, 1.0f); requiresRecalcBoxes(); } - const glm::vec3& getAngularVelocity() const { return _angularVelocity; } - void setAngularVelocity(const glm::vec3& value) { _angularVelocity = value; } - bool hasAngularVelocity() const { return _angularVelocity != ENTITY_ITEM_ZERO_VEC3; } + bool hasAngularVelocity() const { return getAngularVelocity() != ENTITY_ITEM_ZERO_VEC3; } float getAngularDamping() const { return _angularDamping; } void setAngularDamping(float value) { _angularDamping = value; } @@ -435,7 +431,6 @@ protected: // rather than in all of the derived classes. If we ever collapse these classes to one we could do it a // different way. float _volumeMultiplier = 1.0f; - glm::vec3 _velocity; glm::vec3 _gravity; glm::vec3 _acceleration; float _damping; @@ -446,7 +441,6 @@ protected: quint64 _scriptTimestamp; QString _collisionSoundURL; glm::vec3 _registrationPoint; - glm::vec3 _angularVelocity; float _angularDamping; bool _visible; bool _collisionless; diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 1f61bb9a59..9dd40a35a7 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -54,7 +54,7 @@ CharacterController::CharacterController() { _floorDistance = MAX_FALL_HEIGHT; - _walkVelocity.setValue(0.0f, 0.0f, 0.0f); + _targetVelocity.setValue(0.0f, 0.0f, 0.0f); _followDesiredBodyTransform.setIdentity(); _followTimeRemaining = 0.0f; _jumpSpeed = JUMP_SPEED; @@ -166,12 +166,12 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { const btScalar MIN_SPEED = 0.001f; - btVector3 actualVelocity = _rigidBody->getLinearVelocity(); + btVector3 actualVelocity = _rigidBody->getLinearVelocity() - _parentVelocity; if (actualVelocity.length() < MIN_SPEED) { actualVelocity = btVector3(0.0f, 0.0f, 0.0f); } - btVector3 desiredVelocity = _walkVelocity; + btVector3 desiredVelocity = _targetVelocity; if (desiredVelocity.length() < MIN_SPEED) { desiredVelocity = btVector3(0.0f, 0.0f, 0.0f); } @@ -212,7 +212,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) { break; } - _rigidBody->setLinearVelocity(finalVelocity); + _rigidBody->setLinearVelocity(finalVelocity + _parentVelocity); // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform. // Rather then add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal. @@ -383,8 +383,11 @@ void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::qu } void CharacterController::setTargetVelocity(const glm::vec3& velocity) { - //_walkVelocity = glmToBullet(_avatarData->getTargetVelocity()); - _walkVelocity = glmToBullet(velocity); + _targetVelocity = glmToBullet(velocity); +} + +void CharacterController::setParentVelocity(const glm::vec3& velocity) { + _parentVelocity = glmToBullet(velocity); } void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix, float timeRemaining) { diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index 86ef350812..5362ca52e4 100644 --- a/libraries/physics/src/CharacterController.h +++ b/libraries/physics/src/CharacterController.h @@ -69,6 +69,7 @@ public: void getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const; void setTargetVelocity(const glm::vec3& velocity); + void setParentVelocity(const glm::vec3& parentVelocity); void setFollowParameters(const glm::mat4& desiredWorldMatrix, float timeRemaining); float getFollowTime() const { return _followTime; } glm::vec3 getFollowLinearDisplacement() const; @@ -105,7 +106,8 @@ protected: protected: btVector3 _currentUp; - btVector3 _walkVelocity; + btVector3 _targetVelocity; + btVector3 _parentVelocity; btTransform _followDesiredBodyTransform; btScalar _followTimeRemaining; btTransform _characterBodyTransform; diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index e5719c17ff..d3048194b4 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -391,6 +391,96 @@ void SpatiallyNestable::setOrientation(const glm::quat& orientation) { #endif } +glm::vec3 SpatiallyNestable::getVelocity(bool& success) const { + glm::vec3 parentVelocity = getParentVelocity(success); + Transform parentTransform = getParentTransform(success); + glm::vec3 result; + _velocityLock.withReadLock([&] { + // TODO: take parent angularVelocity into account. + result = parentVelocity + parentTransform.getRotation() * _velocity; + }); + return result; +} + +glm::vec3 SpatiallyNestable::getVelocity() const { + bool success; + glm::vec3 result = getVelocity(success); + if (!success) { + qDebug() << "Warning -- setVelocity failed" << getID(); + } + return result; +} + +void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) { + glm::vec3 parentVelocity = getParentVelocity(success); + Transform parentTransform = getParentTransform(success); + _velocityLock.withWriteLock([&] { + // TODO: take parent angularVelocity into account. + _velocity = glm::inverse(parentTransform.getRotation()) * velocity - parentVelocity; + }); +} + +void SpatiallyNestable::setVelocity(const glm::vec3& velocity) { + bool success; + setVelocity(velocity, success); + if (!success) { + qDebug() << "Warning -- setVelocity failed" << getID(); + } +} + +glm::vec3 SpatiallyNestable::getParentVelocity(bool& success) const { + glm::vec3 result; + SpatiallyNestablePointer parent = getParentPointer(success); + if (success && parent) { + result = parent->getVelocity(success); + } + return result; +} + +glm::vec3 SpatiallyNestable::getAngularVelocity(bool& success) const { + glm::vec3 parentAngularVelocity = getParentAngularVelocity(success); + Transform parentTransform = getParentTransform(success); + glm::vec3 result; + _angularVelocityLock.withReadLock([&] { + result = parentAngularVelocity + parentTransform.getRotation() * _angularVelocity; + }); + return result; +} + +glm::vec3 SpatiallyNestable::getAngularVelocity() const { + bool success; + glm::vec3 result = getAngularVelocity(success); + if (!success) { + qDebug() << "Warning -- getAngularVelocity failed" << getID(); + } + return result; +} + +void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, bool& success) { + glm::vec3 parentAngularVelocity = getParentAngularVelocity(success); + Transform parentTransform = getParentTransform(success); + _angularVelocityLock.withWriteLock([&] { + _angularVelocity = glm::inverse(parentTransform.getRotation()) * angularVelocity - parentAngularVelocity; + }); +} + +void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity) { + bool success; + setAngularVelocity(angularVelocity, success); + if (!success) { + qDebug() << "Warning -- setAngularVelocity failed" << getID(); + } +} + +glm::vec3 SpatiallyNestable::getParentAngularVelocity(bool& success) const { + glm::vec3 result; + SpatiallyNestablePointer parent = getParentPointer(success); + if (success && parent) { + result = parent->getAngularVelocity(success); + } + return result; +} + const Transform SpatiallyNestable::getTransform(bool& success) const { // return a world-space transform for this object's location Transform parentTransform = getParentTransform(success); @@ -519,6 +609,34 @@ void SpatiallyNestable::setLocalOrientation(const glm::quat& orientation) { locationChanged(); } +glm::vec3 SpatiallyNestable::getLocalVelocity() const { + glm::vec3 result(glm::vec3::_null); + _velocityLock.withReadLock([&] { + result = _velocity; + }); + return result; +} + +void SpatiallyNestable::setLocalVelocity(const glm::vec3& velocity) { + _velocityLock.withWriteLock([&] { + _velocity = velocity; + }); +} + +glm::vec3 SpatiallyNestable::getLocalAngularVelocity() const { + glm::vec3 result(glm::vec3::_null); + _angularVelocityLock.withReadLock([&] { + result = _angularVelocity; + }); + return result; +} + +void SpatiallyNestable::setLocalAngularVelocity(const glm::vec3& angularVelocity) { + _angularVelocityLock.withWriteLock([&] { + _angularVelocity = angularVelocity; + }); +} + glm::vec3 SpatiallyNestable::getLocalScale() const { // TODO: scale glm::vec3 result; diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index 2a0b58f368..0c43aff20d 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -68,6 +68,18 @@ public: virtual void setOrientation(const glm::quat& orientation, bool& success); virtual void setOrientation(const glm::quat& orientation); + virtual glm::vec3 getVelocity(bool& success) const; + virtual glm::vec3 getVelocity() const; + virtual void setVelocity(const glm::vec3& velocity, bool& success); + virtual void setVelocity(const glm::vec3& velocity); + virtual glm::vec3 getParentVelocity(bool& success) const; + + virtual glm::vec3 getAngularVelocity(bool& success) const; + virtual glm::vec3 getAngularVelocity() const; + virtual void setAngularVelocity(const glm::vec3& angularVelocity, bool& success); + virtual void setAngularVelocity(const glm::vec3& angularVelocity); + virtual glm::vec3 getParentAngularVelocity(bool& success) const; + virtual AACube getMaximumAACube(bool& success) const; virtual bool computePuffedQueryAACube(); @@ -94,6 +106,12 @@ public: virtual glm::quat getLocalOrientation() const; virtual void setLocalOrientation(const glm::quat& orientation); + virtual glm::vec3 getLocalVelocity() const; + virtual void setLocalVelocity(const glm::vec3& velocity); + + virtual glm::vec3 getLocalAngularVelocity() const; + virtual void setLocalAngularVelocity(const glm::vec3& angularVelocity); + virtual glm::vec3 getLocalScale() const; virtual void setLocalScale(const glm::vec3& scale); @@ -126,6 +144,7 @@ protected: QUuid _parentID; // what is this thing's transform relative to? quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to? SpatiallyNestablePointer getParentPointer(bool& success) const; + mutable SpatiallyNestableWeakPointer _parent; virtual void beParentOfChild(SpatiallyNestablePointer newChild) const; @@ -146,7 +165,11 @@ protected: private: mutable ReadWriteLockable _transformLock; mutable ReadWriteLockable _idLock; + mutable ReadWriteLockable _velocityLock; + mutable ReadWriteLockable _angularVelocityLock; Transform _transform; // this is to be combined with parent's world-transform to produce this' world-transform. + glm::vec3 _velocity; + glm::vec3 _angularVelocity; mutable bool _parentKnowsMe { false }; bool _isDead { false }; };