From f2bcdfa2b479cbe917f0afd1d6a127f34694f421 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 23 Jan 2015 13:10:41 -0800 Subject: [PATCH] update _lastSimulated for kinematic motion --- libraries/entities/src/EntityItem.cpp | 101 ++++---------------- libraries/entities/src/EntityItem.h | 4 +- libraries/physics/src/EntityMotionState.cpp | 20 ++-- libraries/physics/src/EntityMotionState.h | 2 +- libraries/physics/src/ObjectMotionState.h | 2 +- libraries/physics/src/PhysicsEngine.cpp | 7 +- libraries/physics/src/PhysicsEngine.h | 2 +- 7 files changed, 39 insertions(+), 99 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index c830236287..c1fc24fdc7 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -607,11 +607,6 @@ bool EntityItem::isRestingOnSurface() const { } void EntityItem::simulate(const quint64& now) { - if (_physicsInfo) { - // we rely on bullet for simulation, so bail - return; - } - bool wantDebug = false; if (_lastSimulated == 0) { @@ -661,9 +656,13 @@ void EntityItem::simulate(const quint64& now) { qDebug() << " ********** EntityItem::simulate() .... SETTING _lastSimulated=" << _lastSimulated; } - if (hasAngularVelocity()) { - glm::quat rotation = getRotation(); + simulateKinematicMotion(timeElapsed); + _lastSimulated = now; +} +void EntityItem::simulateKinematicMotion(float timeElapsed) { + bool wantDebug = false; + if (hasAngularVelocity()) { // angular damping glm::vec3 angularVelocity = getAngularVelocity(); if (_angularDamping > 0.0f) { @@ -679,6 +678,9 @@ void EntityItem::simulate(const quint64& now) { const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.1f; // if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) { + if (angularSpeed > 0.0f) { + _dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE; + } setAngularVelocity(ENTITY_ITEM_ZERO_VEC3); } else { // NOTE: angularSpeed is currently in degrees/sec!!! @@ -686,7 +688,7 @@ void EntityItem::simulate(const quint64& now) { float angle = timeElapsed * glm::radians(angularSpeed); glm::vec3 axis = _angularVelocity / angularSpeed; glm::quat dQ = glm::angleAxis(angle, axis); - rotation = glm::normalize(dQ * rotation); + glm::quat rotation = glm::normalize(dQ * getRotation()); setRotation(rotation); } } @@ -722,80 +724,6 @@ void EntityItem::simulate(const quint64& now) { position = newPosition; - // handle bounces off the ground... We bounce at the distance to the bottom of our entity - if (position.y <= getDistanceToBottomOfEntity()) { - velocity = velocity * glm::vec3(1,-1,1); - position.y = getDistanceToBottomOfEntity(); - } - - // apply gravity - if (hasGravity()) { - // handle resting on surface case, this is definitely a bit of a hack, and it only works on the - // "ground" plane of the domain, but for now it's what we've got - if (isRestingOnSurface()) { - velocity.y = 0.0f; - position.y = getDistanceToBottomOfEntity(); - } else { - velocity += getGravity() * timeElapsed; - } - } - - // NOTE: we don't zero out very small velocities --> we rely on a remote Bullet simulation - // to tell us when the entity has stopped. - - // NOTE: the simulation should NOT set any DirtyFlags on this entity - setPosition(position); // this will automatically recalculate our collision shape - setVelocity(velocity); - - if (wantDebug) { - qDebug() << " new position:" << position; - qDebug() << " new velocity:" << velocity; - qDebug() << " new AACube:" << getMaximumAACube(); - qDebug() << " old getAABox:" << getAABox(); - } - } - - _lastSimulated = now; -} - -void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) { - if (hasAngularVelocity()) { - // angular damping - glm::vec3 angularVelocity = getAngularVelocity(); - if (_angularDamping > 0.0f) { - angularVelocity *= powf(1.0f - _angularDamping, timeElapsed); - setAngularVelocity(angularVelocity); - } - - float angularSpeed = glm::length(_angularVelocity); - const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.1f; // - if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) { - if (angularSpeed > 0.0f) { - _dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE; - } - setAngularVelocity(ENTITY_ITEM_ZERO_VEC3); - } else { - // NOTE: angularSpeed is currently in degrees/sec!!! - // TODO: Andrew to convert to radians/sec - float angle = timeElapsed * glm::radians(angularSpeed); - glm::vec3 axis = _angularVelocity / angularSpeed; - glm::quat dQ = glm::angleAxis(angle, axis); - glm::quat rotation = getRotation(); - rotation = glm::normalize(dQ * rotation); - setRotation(rotation); - } - } - - if (hasVelocity()) { - // linear damping - glm::vec3 velocity = getVelocity(); - if (_damping > 0.0f) { - velocity *= powf(1.0f - _damping, timeElapsed); - } - - // integrate position forward - glm::vec3 position = getPosition() + (velocity * timeElapsed); - // apply gravity if (hasGravity()) { // handle resting on surface case, this is definitely a bit of a hack, and it only works on the @@ -811,9 +739,16 @@ void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) { _dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE; } } else { - setPosition(position); // this will automatically recalculate our collision shape + setPosition(position); setVelocity(velocity); } + + if (wantDebug) { + qDebug() << " new position:" << position; + qDebug() << " new velocity:" << velocity; + qDebug() << " new AACube:" << getMaximumAACube(); + qDebug() << " old getAABox:" << getAABox(); + } } } diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index a9a82c5209..d266a30f62 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -82,6 +82,7 @@ public: void recordCreationTime(); // set _created to 'now' quint64 getLastSimulated() const { return _lastSimulated; } /// Last simulated time of this entity universal usecs + void setLastSimulated(quint64 now) { _lastSimulated = now; } /// Last edited time of this entity universal usecs quint64 getLastEdited() const { return _lastEdited; } @@ -128,9 +129,8 @@ public: // perform linear extrapolation for SimpleEntitySimulation void simulate(const quint64& now); + void simulateKinematicMotion(float timeElapsed); - void simulateSimpleKinematicMotion(float timeElapsed); - virtual bool needsToCallUpdate() const { return false; } virtual void debugDump() const; diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index b4aa9d0e7e..f22487b8ea 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -54,11 +54,12 @@ void EntityMotionState::updateKinematicState(uint32_t substep) { setKinematic(_entity->isMoving(), substep); } -void EntityMotionState::stepKinematicSimulation(uint32_t substep) { +void EntityMotionState::stepKinematicSimulation(quint64 now) { assert(_isKinematic); - float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP; - _entity->simulateSimpleKinematicMotion(dt); - _lastKinematicSubstep = substep; + // NOTE: this is non-physical kinematic motion which steps to real run-time (now) + // which is different from physical kinematic motion (inside getWorldTransform()) + // which steps in physics simulation time. + _entity->simulate(now); } // This callback is invoked by the physics simulation in two cases: @@ -68,10 +69,15 @@ void EntityMotionState::stepKinematicSimulation(uint32_t substep) { // it is an opportunity for outside code to update the object's simulation position void EntityMotionState::getWorldTransform(btTransform& worldTrans) const { if (_isKinematic) { + // This is physical kinematic motion which steps strictly by the subframe count + // of the physics simulation. uint32_t substep = PhysicsEngine::getNumSubsteps(); - // remove const-ness so we can actually update this instance - EntityMotionState* thisMotion = const_cast(this); - thisMotion->stepKinematicSimulation(substep); + float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP; + _entity->simulateKinematicMotion(dt); + _entity->setLastSimulated(usecTimestampNow()); + + // bypass const-ness so we can remember the substep + const_cast(this)->_lastKinematicSubstep = substep; } worldTrans.setOrigin(glmToBullet(_entity->getPositionInMeters() - ObjectMotionState::getWorldOffset())); worldTrans.setRotation(glmToBullet(_entity->getRotation())); diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index 192ac166b4..5d98e545d9 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -39,7 +39,7 @@ public: MotionType computeMotionType() const; void updateKinematicState(uint32_t substep); - void stepKinematicSimulation(uint32_t substep); + void stepKinematicSimulation(quint64 now); // this relays incoming position/rotation to the RigidBody void getWorldTransform(btTransform& worldTrans) const; diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index 424a7fb680..ceeea219cf 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -99,7 +99,7 @@ public: bool isKinematic() const { return _isKinematic; } void setKinematic(bool kinematic, uint32_t substep); - virtual void stepKinematicSimulation(uint32_t substep) = 0; + virtual void stepKinematicSimulation(quint64 now) = 0; friend class PhysicsEngine; protected: diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index dbeea165c4..62693e3c9a 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -297,7 +297,7 @@ void PhysicsEngine::stepSimulation() { // This is step (2). int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP); _numSubsteps += (uint32_t)numSubsteps; - stepNonPhysicalKinematics(); + stepNonPhysicalKinematics(usecTimestampNow()); unlock(); // This is step (3) which is done outside of stepSimulation() so we can lock _entityTree. @@ -317,12 +317,11 @@ void PhysicsEngine::stepSimulation() { computeCollisionEvents(); } -// TODO: need to update non-physical kinematic objects -void PhysicsEngine::stepNonPhysicalKinematics() { +void PhysicsEngine::stepNonPhysicalKinematics(const quint64& now) { QSet::iterator stateItr = _nonPhysicalKinematicObjects.begin(); while (stateItr != _nonPhysicalKinematicObjects.end()) { ObjectMotionState* motionState = *stateItr; - motionState->stepKinematicSimulation(_numSubsteps); + motionState->stepKinematicSimulation(now); ++stateItr; } } diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 9ae9f88e7e..1dbfe2646e 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -67,7 +67,7 @@ public: virtual void init(EntityEditPacketSender* packetSender); void stepSimulation(); - void stepNonPhysicalKinematics(); + void stepNonPhysicalKinematics(const quint64& now); void computeCollisionEvents();