Showing the changes to libraries/physics/src/ObjectActionSpring.cpp (1 of 36 files). Show all
Unified · Split
| @@ -38,74 +38,72 @@ ObjectActionSpring::~ObjectActionSpring() { · 37 unmodified lines | |||
| 38 | } | 38 | } |
| 39 | 39 | ||
| 40 | void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) { | 40 | void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) { |
| 41 | if (!tryLockForRead()) { | 41 | auto lockResult = withTryReadLock([&]{ |
| 42 | // don't risk hanging the thread running the physics simulation | 42 | auto ownerEntity = _ownerEntity.lock(); |
| 43 | qDebug() << "ObjectActionSpring::updateActionWorker lock failed"; | 43 | if (!ownerEntity) { |
| 44 | return; | 44 | return; |
| 45 | } | 45 | } |
| 46 | 46 | ||
| 47 | auto ownerEntity = _ownerEntity.lock(); | 47 | void* physicsInfo = ownerEntity->getPhysicsInfo(); |
| 48 | if (!ownerEntity) { | 48 | if (!physicsInfo) { |
| 49 | return; | 49 | return; |
| 50 | } | 50 | } |
| 51 | ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo); | ||
| 52 | btRigidBody* rigidBody = motionState->getRigidBody(); | ||
| 53 | if (!rigidBody) { | ||
| 54 | qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody"; | ||
| 55 | return; | ||
| 56 | } | ||
| 51 | 57 | ||
| 52 | void* physicsInfo = ownerEntity->getPhysicsInfo(); | 58 | const float MAX_TIMESCALE = 600.0f; // 10 min is a long time |
| 53 | if (!physicsInfo) { | 59 | if (_linearTimeScale < MAX_TIMESCALE) { |
| 54 | unlock(); | 60 | btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget); |
| 55 | return; | 61 | float offsetLength = offset.length(); |
| 56 | } | 62 | btVector3 targetVelocity(0.0f, 0.0f, 0.0f); |
| 57 | ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo); | ||
| 58 | btRigidBody* rigidBody = motionState->getRigidBody(); | ||
| 59 | if (!rigidBody) { | ||
| 60 | unlock(); | ||
| 61 | qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody"; | ||
| 62 | return; | ||
| 63 | } | ||
| 64 | 63 | ||
| 65 | const float MAX_TIMESCALE = 600.0f; // 10 min is a long time | 64 | if (offsetLength > 0) { |
| 66 | if (_linearTimeScale < MAX_TIMESCALE) { | 65 | float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f; |
| 67 | btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget); | 66 | targetVelocity = (-speed / offsetLength) * offset; |
| 68 | float offsetLength = offset.length(); | 67 | } |
| 69 | btVector3 targetVelocity(0.0f, 0.0f, 0.0f); | ||
| 70 | 68 | ||
| 71 | if (offsetLength > 0) { | 69 | // this action is aggresively critically damped and defeats the current velocity |
| 72 | float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f; | 70 | rigidBody->setLinearVelocity(targetVelocity); |
| 73 | targetVelocity = (-speed / offsetLength) * offset; | ||
| 74 | } | 71 | } |
| 75 | 72 | ||
| 76 | // this action is aggresively critically damped and defeats the current velocity | 73 | if (_angularTimeScale < MAX_TIMESCALE) { |
| 77 | rigidBody->setLinearVelocity(targetVelocity); | 74 | btVector3 targetVelocity(0.0f, 0.0f, 0.0f); |
| 78 | } | 75 | |
| 79 | 76 | btQuaternion bodyRotation = rigidBody->getOrientation(); | |
| 80 | if (_angularTimeScale < MAX_TIMESCALE) { | 77 | auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget)); |
| 81 | btVector3 targetVelocity(0.0f, 0.0f, 0.0f); | 78 | const float ALMOST_ONE = 0.99999f; |
| 82 | 79 | if (glm::abs(alignmentDot) < ALMOST_ONE) { | |
| 83 | btQuaternion bodyRotation = rigidBody->getOrientation(); | 80 | btQuaternion target = glmToBullet(_rotationalTarget); |
| 84 | auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget)); | 81 | if (alignmentDot < 0.0f) { |
| 85 | const float ALMOST_ONE = 0.99999f; | 82 | target = -target; |
| 86 | if (glm::abs(alignmentDot) < ALMOST_ONE) { | 83 | } |
| 87 | btQuaternion target = glmToBullet(_rotationalTarget); | 84 | // if dQ is the incremental rotation that gets an object from Q0 to Q1 then: |
| 88 | if (alignmentDot < 0.0f) { | 85 | // |
| 89 | target = -target; | 86 | // Q1 = dQ * Q0 |
| 90 | } | 87 | // |
| 91 | // if dQ is the incremental rotation that gets an object from Q0 to Q1 then: | 88 | // solving for dQ gives: |
| 92 | // | 89 | // |
| 93 | // Q1 = dQ * Q0 | 90 | // dQ = Q1 * Q0^ |
| 94 | // | 91 | btQuaternion deltaQ = target * bodyRotation.inverse(); |
| 95 | // solving for dQ gives: | 92 | float angle = deltaQ.getAngle(); |
| 96 | // | 93 | const float MIN_ANGLE = 1.0e-4f; |
| 97 | // dQ = Q1 * Q0^ | 94 | if (angle > MIN_ANGLE) { |
| 98 | btQuaternion deltaQ = target * bodyRotation.inverse(); | 95 | targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis(); |
| 99 | float angle = deltaQ.getAngle(); | 96 | } |
| 100 | const float MIN_ANGLE = 1.0e-4f; | ||
| 101 | if (angle > MIN_ANGLE) { | ||
| 102 | targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis(); | ||
| 103 | } | 97 | } |
| 98 | // this action is aggresively critically damped and defeats the current velocity | ||
| 99 | rigidBody->setAngularVelocity(targetVelocity); | ||
| 104 | } | 100 | } |
| 105 | // this action is aggresively critically damped and defeats the current velocity | 101 | }); |
| 106 | rigidBody->setAngularVelocity(targetVelocity); | 102 | if (!lockResult) { |
| 103 | // don't risk hanging the thread running the physics simulation | ||
| 104 | qDebug() << "ObjectActionSpring::updateActionWorker lock failed"; | ||
| 105 | return; | ||
| 107 | } | 106 | } |
| 108 | unlock(); | ||
| 109 | } | 107 | } |
| 110 | 108 | ||
| 111 | const float MIN_TIMESCALE = 0.1f; | 109 | const float MIN_TIMESCALE = 0.1f; |
| @@ -144,29 +142,27 @@ bool ObjectActionSpring::updateArguments(QVariantMap arguments) { · 32 unmodified lines | |||
| 144 | || rotationalTarget != _rotationalTarget | 142 | || rotationalTarget != _rotationalTarget |
| 145 | || angularTimeScale != _angularTimeScale) { | 143 | || angularTimeScale != _angularTimeScale) { |
| 146 | // something changed | 144 | // something changed |
| 147 | lockForWrite(); | 145 | withWriteLock([&] { |
| 148 | _positionalTarget = positionalTarget; | 146 | _positionalTarget = positionalTarget; |
| 149 | _linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale)); | 147 | _linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale)); |
| 150 | _rotationalTarget = rotationalTarget; | 148 | _rotationalTarget = rotationalTarget; |
| 151 | _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale)); | 149 | _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale)); |
| 152 | _active = true; | 150 | _active = true; |
| 153 | activateBody(); | 151 | activateBody(); |
| 154 | unlock(); | 152 | }); |
| 155 | } | 153 | } |
| 156 | return true; | 154 | return true; |
| 157 | } | 155 | } |
| 158 | 156 | ||
| 159 | QVariantMap ObjectActionSpring::getArguments() { | 157 | QVariantMap ObjectActionSpring::getArguments() { |
| 160 | QVariantMap arguments; | 158 | QVariantMap arguments; |
| 161 | lockForRead(); | 159 | withReadLock([&] { |
| 162 | 160 | arguments["linearTimeScale"] = _linearTimeScale; | |
| 163 | arguments["linearTimeScale"] = _linearTimeScale; | 161 | arguments["targetPosition"] = glmToQMap(_positionalTarget); |
| 164 | arguments["targetPosition"] = glmToQMap(_positionalTarget); | ||
| 165 | |||
| 166 | arguments["targetRotation"] = glmToQMap(_rotationalTarget); | ||
| 167 | arguments["angularTimeScale"] = _angularTimeScale; | ||
| 168 | 162 | ||
| 169 | unlock(); | 163 | arguments["targetRotation"] = glmToQMap(_rotationalTarget); |
| 164 | arguments["angularTimeScale"] = _angularTimeScale; | ||
| 165 | }); | ||
| 170 | return arguments; | 166 | return arguments; |
| 171 | } | 167 | } |
| 172 | 168 | ||