diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index e56f2c267a..15107269d2 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -885,83 +885,93 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { return; } - if (hasAngularVelocity()) { - glm::vec3 angularVelocity = getAngularVelocity(); + if (hasLocalAngularVelocity()) { + glm::vec3 localAngularVelocity = getLocalAngularVelocity(); // angular damping if (_angularDamping > 0.0f) { - angularVelocity *= powf(1.0f - _angularDamping, timeElapsed); + localAngularVelocity *= powf(1.0f - _angularDamping, timeElapsed); #ifdef WANT_DEBUG qCDebug(entities) << " angularDamping :" << _angularDamping; - qCDebug(entities) << " newAngularVelocity:" << angularVelocity; + qCDebug(entities) << " newAngularVelocity:" << localAngularVelocity; #endif } - float angularSpeed = glm::length(angularVelocity); + float angularSpeed = glm::length(localAngularVelocity); 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; + localAngularVelocity = 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(localAngularVelocity, 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(localAngularVelocity, dt); rotation = glm::normalize(dQ * rotation); setRotation(rotation); } - setAngularVelocity(angularVelocity); + setLocalAngularVelocity(localAngularVelocity); } - if (hasVelocity()) { + if (hasLocalVelocity()) { + + // acceleration is in the global frame, so transform it into the local frame. + // TODO: Move this into SpatiallyNestable. + bool success; + Transform transform = getParentTransform(success); + glm::vec3 localAcceleration(glm::vec3::_null); + if (success) { + localAcceleration = glm::inverse(transform.getRotation()) * getAcceleration(); + } else { + localAcceleration = getAcceleration(); + } + // linear damping - glm::vec3 velocity = getVelocity(); + glm::vec3 localVelocity = getLocalVelocity(); if (_damping > 0.0f) { - velocity *= powf(1.0f - _damping, timeElapsed); + localVelocity *= powf(1.0f - _damping, timeElapsed); #ifdef WANT_DEBUG qCDebug(entities) << " damping:" << _damping; - qCDebug(entities) << " velocity AFTER dampingResistance:" << velocity; - qCDebug(entities) << " glm::length(velocity):" << glm::length(velocity); + qCDebug(entities) << " velocity AFTER dampingResistance:" << localVelocity; + qCDebug(entities) << " glm::length(velocity):" << glm::length(localVelocity); #endif } // integrate position forward - glm::vec3 position = getPosition(); - glm::vec3 newPosition = position + (velocity * timeElapsed); + glm::vec3 localPosition = getLocalPosition(); + glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed; #ifdef WANT_DEBUG qCDebug(entities) << " EntityItem::simulate()...."; qCDebug(entities) << " timeElapsed:" << timeElapsed; qCDebug(entities) << " old AACube:" << getMaximumAACube(); - qCDebug(entities) << " old position:" << position; - qCDebug(entities) << " old velocity:" << velocity; + qCDebug(entities) << " old position:" << localPosition; + qCDebug(entities) << " old velocity:" << localVelocity; qCDebug(entities) << " old getAABox:" << getAABox(); qCDebug(entities) << " newPosition:" << newPosition; - qCDebug(entities) << " glm::distance(newPosition, position):" << glm::distance(newPosition, position); + qCDebug(entities) << " glm::distance(newPosition, position):" << glm::distance(newLocalPosition, localPosition); #endif - position = newPosition; + localPosition = newLocalPosition; // apply effective acceleration, which will be the same as gravity if the Entity isn't at rest. - if (hasAcceleration()) { - velocity += getAcceleration() * timeElapsed; - } + localVelocity += localAcceleration * timeElapsed; - float speed = glm::length(velocity); + float speed = glm::length(localVelocity); const float EPSILON_LINEAR_VELOCITY_LENGTH = 0.001f; // 1mm/sec if (speed < EPSILON_LINEAR_VELOCITY_LENGTH) { setVelocity(ENTITY_ITEM_ZERO_VEC3); @@ -969,8 +979,8 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; } } else { - setPosition(position); - setVelocity(velocity); + setLocalPosition(localPosition); + setLocalVelocity(localVelocity); } #ifdef WANT_DEBUG @@ -986,6 +996,10 @@ bool EntityItem::isMoving() const { return hasVelocity() || hasAngularVelocity(); } +bool EntityItem::isMovingRelativeToParent() const { + return hasLocalVelocity() || hasLocalAngularVelocity(); +} + EntityTreePointer EntityItem::getTree() const { EntityTreeElementPointer containingElement = getElement(); EntityTreePointer tree = containingElement ? containingElement->getTree() : nullptr; diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index c8b54bca87..4cdacdbbf5 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -194,6 +194,7 @@ public: float getDensity() const { return _density; } bool hasVelocity() const { return getVelocity() != ENTITY_ITEM_ZERO_VEC3; } + bool hasLocalVelocity() const { return getLocalVelocity() != 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 @@ -254,6 +255,7 @@ public: { _registrationPoint = glm::clamp(value, 0.0f, 1.0f); requiresRecalcBoxes(); } bool hasAngularVelocity() const { return getAngularVelocity() != ENTITY_ITEM_ZERO_VEC3; } + bool hasLocalAngularVelocity() const { return getLocalAngularVelocity() != ENTITY_ITEM_ZERO_VEC3; } float getAngularDamping() const { return _angularDamping; } void setAngularDamping(float value) { _angularDamping = value; } @@ -339,6 +341,7 @@ public: void clearDirtyFlags(uint32_t mask = 0xffffffff) { _dirtyFlags &= ~mask; } bool isMoving() const; + bool isMovingRelativeToParent() const; bool isSimulated() const { return _simulated; } diff --git a/libraries/entities/src/EntitySimulation.cpp b/libraries/entities/src/EntitySimulation.cpp index 23e5b99337..a1f55a92a3 100644 --- a/libraries/entities/src/EntitySimulation.cpp +++ b/libraries/entities/src/EntitySimulation.cpp @@ -254,7 +254,7 @@ void EntitySimulation::moveSimpleKinematics(const quint64& now) { SetOfEntities::iterator itemItr = _simpleKinematicEntities.begin(); while (itemItr != _simpleKinematicEntities.end()) { EntityItemPointer entity = *itemItr; - if (entity->isMoving() && !entity->getPhysicsInfo()) { + if (entity->isMovingRelativeToParent() && !entity->getPhysicsInfo()) { entity->simulate(now); _entitiesToSort.insert(entity); ++itemItr; diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index d3048194b4..95e8f62d21 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -416,7 +416,7 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) { Transform parentTransform = getParentTransform(success); _velocityLock.withWriteLock([&] { // TODO: take parent angularVelocity into account. - _velocity = glm::inverse(parentTransform.getRotation()) * velocity - parentVelocity; + _velocity = glm::inverse(parentTransform.getRotation()) * (velocity - parentVelocity); }); } @@ -460,7 +460,7 @@ void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, boo glm::vec3 parentAngularVelocity = getParentAngularVelocity(success); Transform parentTransform = getParentTransform(success); _angularVelocityLock.withWriteLock([&] { - _angularVelocity = glm::inverse(parentTransform.getRotation()) * angularVelocity - parentAngularVelocity; + _angularVelocity = glm::inverse(parentTransform.getRotation()) * (angularVelocity - parentAngularVelocity); }); }