From aebdd3b33592575714107a5e937dfcaa74d289b1 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 27 Jan 2015 15:53:37 -0800 Subject: [PATCH 1/4] fix bad extrapolation of rapidly spinning objects --- libraries/physics/src/BulletUtil.cpp | 52 +++++++++++++++++++++ libraries/physics/src/BulletUtil.h | 2 + libraries/physics/src/ObjectMotionState.cpp | 23 +++++---- 3 files changed, 67 insertions(+), 10 deletions(-) create mode 100644 libraries/physics/src/BulletUtil.cpp diff --git a/libraries/physics/src/BulletUtil.cpp b/libraries/physics/src/BulletUtil.cpp new file mode 100644 index 0000000000..26102ad980 --- /dev/null +++ b/libraries/physics/src/BulletUtil.cpp @@ -0,0 +1,52 @@ +// +// BulletUtil.cpp +// libraries/physcis/src +// +// The implementation of bulletRotationStep() was copied from Bullet-2.82 so we include the Bullet license here: +// +/* + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it freely, + * subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. + * If you use this software in a product, an acknowledgment in the product documentation would be appreciated but + * is not required. + * 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + * + * Copied and modified from LinearMath/btTransformUtil.h by AndrewMeadows on 2015.01.27. + * */ + +#include +#include "BulletUtil.h" + +glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep) { + // Bullet uses an exponential map approximation technique to integrate rotation. + // The reason for this is to make it easy to compute the derivative of angular motion for various consraints. + // Here we reproduce the same approximation so that our extrapolations agree well with Bullet's integration. + // + // Exponential map + // google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia + + float speed = glm::length(angularVelocity); + // limit the angular motion because the exponential approximation fails for large steps + if (speed * timeStep > ANGULAR_MOTION_THRESHOLD) { + speed = ANGULAR_MOTION_THRESHOLD / timeStep; + } + + glm::vec3 axis = angularVelocity; + if (speed < 0.001f) { + // use Taylor's expansions of sync function + axis *= (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f * speed * speed)); + } else { + // sync(speed) = sin(c * speed)/t + axis *= (sinf(0.5f * speed * timeStep) / speed ); + } + return glm::quat(cosf(0.5f * speed * timeStep), axis.x, axis.y, axis.z); +} diff --git a/libraries/physics/src/BulletUtil.h b/libraries/physics/src/BulletUtil.h index 52bf2c8b06..15fb21364a 100644 --- a/libraries/physics/src/BulletUtil.h +++ b/libraries/physics/src/BulletUtil.h @@ -32,4 +32,6 @@ inline btQuaternion glmToBullet(const glm::quat& g) { return btQuaternion(g.x, g.y, g.z, g.w); } +glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float deltaTime); + #endif // hifi_BulletUtil_h diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index dfa059d47f..e39455373d 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -107,12 +107,14 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { if (_sentFrame == 0) { _sentPosition = bulletToGLM(_body->getWorldTransform().getOrigin()); _sentVelocity = bulletToGLM(_body->getLinearVelocity()); + _sentRotation = bulletToGLM(_body->getWorldTransform().getRotation()); _sentAngularVelocity = bulletToGLM(_body->getAngularVelocity()); _sentFrame = simulationFrame; return false; } - float dt = (float)(simulationFrame - _sentFrame) * PHYSICS_ENGINE_FIXED_SUBSTEP; + int numFrames = simulationFrame - _sentFrame; + float dt = (float)(numFrames) * PHYSICS_ENGINE_FIXED_SUBSTEP; _sentFrame = simulationFrame; bool isActive = _body->isActive(); @@ -152,18 +154,19 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { return true; } - if (glm::length2(_sentAngularVelocity) > 0.0f) { + float spin = glm::length(_sentAngularVelocity); + if (spin > 0.0f) { // compute rotation error - _sentAngularVelocity *= powf(1.0f - _angularDamping, dt); - - float spin = glm::length(_sentAngularVelocity); - const float MIN_SPIN = 1.0e-4f; - if (spin > MIN_SPIN) { - glm::vec3 axis = _sentAngularVelocity / spin; - _sentRotation = glm::normalize(glm::angleAxis(dt * spin, axis) * _sentRotation); + float attenuation = powf(1.0f - _angularDamping, dt); + _sentAngularVelocity *= attenuation; + + // Bullet caps the effective rotation velocity inside its rotation integration step, therefore + // we must integrate with the same algorithm and timestep in order achieve similar results. + for (int i = 0; i < numFrames; ++i) { + _sentRotation = glm::normalize(bulletRotationStep(_sentAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * _sentRotation); } } - const float MIN_ROTATION_DOT = 0.98f; + const float MIN_ROTATION_DOT = 0.99f; // 0.99 dot threshold coresponds to about 16 degrees of slop glm::quat actualRotation = bulletToGLM(worldTrans.getRotation()); return (fabsf(glm::dot(actualRotation, _sentRotation)) < MIN_ROTATION_DOT); } From d5c0c200de30a2eddf9fedd7fa09aa7929ddb00a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 27 Jan 2015 15:54:32 -0800 Subject: [PATCH 2/4] optimization for when physics engine doesn't step --- libraries/physics/src/PhysicsEngine.cpp | 38 ++++++++++++++----------- libraries/physics/src/PhysicsEngine.h | 1 + 2 files changed, 22 insertions(+), 17 deletions(-) diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index dfd28dd8a4..f4aa06e31d 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -29,23 +29,27 @@ PhysicsEngine::~PhysicsEngine() { // begin EntitySimulation overrides void PhysicsEngine::updateEntitiesInternal(const quint64& now) { - // NOTE: the grand order of operations is: - // (1) relay incoming changes - // (2) step simulation - // (3) synchronize outgoing motion states - // (4) send outgoing packets - - // this is step (4) - QSet::iterator stateItr = _outgoingPackets.begin(); - while (stateItr != _outgoingPackets.end()) { - ObjectMotionState* state = *stateItr; - if (state->doesNotNeedToSendUpdate()) { - stateItr = _outgoingPackets.erase(stateItr); - } else if (state->shouldSendUpdate(_numSubsteps)) { - state->sendUpdate(_entityPacketSender, _numSubsteps); - ++stateItr; - } else { - ++stateItr; + // no need to send updates unless the physics simulation has actually stepped + if (_lastNumSubstepsAtUpdateInternal != _numSubsteps) { + _lastNumSubstepsAtUpdateInternal = _numSubsteps; + // NOTE: the grand order of operations is: + // (1) relay incoming changes + // (2) step simulation + // (3) synchronize outgoing motion states + // (4) send outgoing packets + + // this is step (4) + QSet::iterator stateItr = _outgoingPackets.begin(); + while (stateItr != _outgoingPackets.end()) { + ObjectMotionState* state = *stateItr; + if (state->doesNotNeedToSendUpdate()) { + stateItr = _outgoingPackets.erase(stateItr); + } else if (state->shouldSendUpdate(_numSubsteps)) { + state->sendUpdate(_entityPacketSender, _numSubsteps); + ++stateItr; + } else { + ++stateItr; + } } } } diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 347f7c2620..67a0076b6c 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -114,6 +114,7 @@ private: ContactMap _contactMap; uint32_t _numContactFrames = 0; + uint32_t _lastNumSubstepsAtUpdateInternal = 0; }; #endif // hifi_PhysicsEngine_h From 23bd3f90d9dbbf65d45e3e6b20464228947e196e Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 27 Jan 2015 17:27:15 -0800 Subject: [PATCH 3/4] Bullet-style rotation integration for kinematics and for dead reckoning after wire-transfer --- libraries/entities/src/EntityItem.cpp | 21 +++++++++++++---- libraries/physics/src/EntityMotionState.cpp | 1 + libraries/physics/src/ObjectMotionState.cpp | 1 + libraries/physics/src/PhysicsEngine.cpp | 1 + libraries/physics/src/PhysicsEngine.h | 2 -- .../src/PhysicsHelpers.cpp} | 20 ++++++++++------ libraries/shared/src/PhysicsHelpers.h | 23 +++++++++++++++++++ 7 files changed, 56 insertions(+), 13 deletions(-) rename libraries/{physics/src/BulletUtil.cpp => shared/src/PhysicsHelpers.cpp} (81%) create mode 100644 libraries/shared/src/PhysicsHelpers.h diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 64cbf8ab04..a6e3a90015 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include // usecTimestampNow() @@ -711,10 +712,22 @@ void EntityItem::simulateKinematicMotion(float timeElapsed) { } 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 = glm::normalize(dQ * getRotation()); + glm::vec3 angularVelocity = glm::radians(_angularVelocity); + // 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 = bulletRotationStep(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 = bulletRotationStep(angularVelocity, dt); + rotation = glm::normalize(dQ * rotation); + setRotation(rotation); } } diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index acff3ed64a..810bf9a6a8 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -15,6 +15,7 @@ #include "BulletUtil.h" #include "EntityMotionState.h" #include "PhysicsEngine.h" +#include "PhysicsHelpers.h" QSet* _outgoingEntityList; diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index c54e883525..a36418171f 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -14,6 +14,7 @@ #include "BulletUtil.h" #include "ObjectMotionState.h" #include "PhysicsEngine.h" +#include "PhysicsHelpers.h" const float DEFAULT_FRICTION = 0.5f; const float MAX_FRICTION = 10.0f; diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index f4aa06e31d..7e40d13cad 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -11,6 +11,7 @@ #include "PhysicsEngine.h" #include "ShapeInfoUtil.h" +#include "PhysicsHelpers.h" #include "ThreadSafeDynamicsWorld.h" static uint32_t _numSubsteps; diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 67a0076b6c..993b4aeade 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -14,8 +14,6 @@ #include -const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 60.0f; - #include #include diff --git a/libraries/physics/src/BulletUtil.cpp b/libraries/shared/src/PhysicsHelpers.cpp similarity index 81% rename from libraries/physics/src/BulletUtil.cpp rename to libraries/shared/src/PhysicsHelpers.cpp index 26102ad980..eed31b36e6 100644 --- a/libraries/physics/src/BulletUtil.cpp +++ b/libraries/shared/src/PhysicsHelpers.cpp @@ -1,9 +1,18 @@ // -// BulletUtil.cpp -// libraries/physcis/src +// PhysicsHelpers.cpp +// libraries/shared/src // -// The implementation of bulletRotationStep() was copied from Bullet-2.82 so we include the Bullet license here: +// Created by Andrew Meadows 2015.01.27 +// Unless otherwise copyrighted: Copyright 2015 High Fidelity, Inc. // +// Unless otherwise licensced: Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "PhysicsHelpers.h" +#include "SharedUtil.h" + +// This chunk of code was copied from Bullet-2.82, so we include the Bullet license here: /* * Bullet Continuous Collision Detection and Physics Library * Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org @@ -22,10 +31,6 @@ * * Copied and modified from LinearMath/btTransformUtil.h by AndrewMeadows on 2015.01.27. * */ - -#include -#include "BulletUtil.h" - glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep) { // Bullet uses an exponential map approximation technique to integrate rotation. // The reason for this is to make it easy to compute the derivative of angular motion for various consraints. @@ -36,6 +41,7 @@ glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep) { float speed = glm::length(angularVelocity); // limit the angular motion because the exponential approximation fails for large steps + const float ANGULAR_MOTION_THRESHOLD = 0.5f * PI_OVER_TWO; if (speed * timeStep > ANGULAR_MOTION_THRESHOLD) { speed = ANGULAR_MOTION_THRESHOLD / timeStep; } diff --git a/libraries/shared/src/PhysicsHelpers.h b/libraries/shared/src/PhysicsHelpers.h new file mode 100644 index 0000000000..cc5f4017b8 --- /dev/null +++ b/libraries/shared/src/PhysicsHelpers.h @@ -0,0 +1,23 @@ +// +// PhysicsHelpers.h +// libraries/shared/src +// +// Created by Andrew Meadows 2015.01.27 +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_PhysicsHelpers_h +#define hifi_PhysicsHelpers_h + +#include +#include + +const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 60.0f; + +// return incremental rotation (Bullet-style) caused by angularVelocity over timeStep +glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep); + +#endif // hifi_PhysicsHelpers_h From 58bb137c4d01c175751fcdceb6302f9f48624efb Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 27 Jan 2015 17:37:12 -0800 Subject: [PATCH 4/4] bulletRotationStep --> computeBulletRotationStep --- libraries/entities/src/EntityItem.cpp | 4 ++-- libraries/physics/src/BulletUtil.h | 2 -- libraries/physics/src/ObjectMotionState.cpp | 2 +- libraries/shared/src/PhysicsHelpers.cpp | 2 +- libraries/shared/src/PhysicsHelpers.h | 2 +- 5 files changed, 5 insertions(+), 7 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index a6e3a90015..9012b2e50b 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -718,14 +718,14 @@ void EntityItem::simulateKinematicMotion(float timeElapsed) { glm::quat rotation = getRotation(); float dt = timeElapsed; while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) { - glm::quat dQ = bulletRotationStep(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 = bulletRotationStep(angularVelocity, dt); + glm::quat dQ = computeBulletRotationStep(angularVelocity, dt); rotation = glm::normalize(dQ * rotation); setRotation(rotation); diff --git a/libraries/physics/src/BulletUtil.h b/libraries/physics/src/BulletUtil.h index 15fb21364a..52bf2c8b06 100644 --- a/libraries/physics/src/BulletUtil.h +++ b/libraries/physics/src/BulletUtil.h @@ -32,6 +32,4 @@ inline btQuaternion glmToBullet(const glm::quat& g) { return btQuaternion(g.x, g.y, g.z, g.w); } -glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float deltaTime); - #endif // hifi_BulletUtil_h diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index a36418171f..21dead5b0b 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -179,7 +179,7 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { // Bullet caps the effective rotation velocity inside its rotation integration step, therefore // we must integrate with the same algorithm and timestep in order achieve similar results. for (int i = 0; i < numFrames; ++i) { - _sentRotation = glm::normalize(bulletRotationStep(_sentAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * _sentRotation); + _sentRotation = glm::normalize(computeBulletRotationStep(_sentAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * _sentRotation); } } const float MIN_ROTATION_DOT = 0.99f; // 0.99 dot threshold coresponds to about 16 degrees of slop diff --git a/libraries/shared/src/PhysicsHelpers.cpp b/libraries/shared/src/PhysicsHelpers.cpp index eed31b36e6..80b8d11f26 100644 --- a/libraries/shared/src/PhysicsHelpers.cpp +++ b/libraries/shared/src/PhysicsHelpers.cpp @@ -31,7 +31,7 @@ * * Copied and modified from LinearMath/btTransformUtil.h by AndrewMeadows on 2015.01.27. * */ -glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep) { +glm::quat computeBulletRotationStep(const glm::vec3& angularVelocity, float timeStep) { // Bullet uses an exponential map approximation technique to integrate rotation. // The reason for this is to make it easy to compute the derivative of angular motion for various consraints. // Here we reproduce the same approximation so that our extrapolations agree well with Bullet's integration. diff --git a/libraries/shared/src/PhysicsHelpers.h b/libraries/shared/src/PhysicsHelpers.h index cc5f4017b8..bef7275067 100644 --- a/libraries/shared/src/PhysicsHelpers.h +++ b/libraries/shared/src/PhysicsHelpers.h @@ -18,6 +18,6 @@ const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 60.0f; // return incremental rotation (Bullet-style) caused by angularVelocity over timeStep -glm::quat bulletRotationStep(const glm::vec3& angularVelocity, float timeStep); +glm::quat computeBulletRotationStep(const glm::vec3& angularVelocity, float timeStep); #endif // hifi_PhysicsHelpers_h