History

cleanup of ObjectActionSpring

Andrew Meadows committed 11 years ago

Commit 56f00a51 parent 90ae1fe2 files · +78 −112Browse files

Showing the changes to libraries/physics/src/ObjectActionSpring.cpp (1 of 2 files). Show all

Unified · Split

libraries/physics/src/ObjectActionSpring.cpp+77−112
@@ -17,14 +17,16 @@ const float SPRING_MAX_SPEED = 10.0f; · 16 unmodified lines
17 17
18const uint16_t ObjectActionSpring::springVersion = 1;18const uint16_t ObjectActionSpring::springVersion = 1;
19 19
20
20ObjectActionSpring::ObjectActionSpring(const QUuid& id, EntityItemPointer ownerEntity) :21ObjectActionSpring::ObjectActionSpring(const QUuid& id, EntityItemPointer ownerEntity) :
21 ObjectAction(ACTION_TYPE_SPRING, id, ownerEntity),22 ObjectAction(ACTION_TYPE_SPRING, id, ownerEntity),
22 _positionalTarget(glm::vec3(0.0f)),23 _positionalTarget(glm::vec3(0.0f)),
23 _linearTimeScale(0.2f),24 _linearTimeScale(FLT_MAX),
24 _positionalTargetSet(false),25 _positionalTargetSet(true),
25 _rotationalTarget(glm::quat()),26 _rotationalTarget(glm::quat()),
26 _angularTimeScale(0.2f),27 _angularTimeScale(FLT_MAX),
27 _rotationalTargetSet(false) {28 _rotationalTargetSet(true),
29 _needsActivation(true) {
28 #if WANT_DEBUG30 #if WANT_DEBUG
29 qDebug() << "ObjectActionSpring::ObjectActionSpring";31 qDebug() << "ObjectActionSpring::ObjectActionSpring";
30 #endif32 #endif
@@ -61,130 +63,97 @@ void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) { · 30 unmodified lines
61 return;63 return;
62 }64 }
63 65
64 // handle the linear part66 const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
65 if (_positionalTargetSet) {67 if (_linearTimeScale < MAX_TIMESCALE) {
66 // check for NaN68 btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);
67 if (_positionalTarget.x != _positionalTarget.x ||69 float offsetLength = offset.length();
68 _positionalTarget.y != _positionalTarget.y ||70 //float blend = glm::min(1.0f, deltaTimeStep / _linearTimeScale);
69 _positionalTarget.z != _positionalTarget.z) {71 float blend = 1.0f;
70 qDebug() << "ObjectActionSpring::updateActionWorker -- target position includes NaN";72 float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f;
71 unlock();73 rigidBody->setLinearVelocity((1.0f - blend) * rigidBody->getLinearVelocity() - (blend * speed / (offsetLength * _linearTimeScale)) * offset);
72 lockForWrite();
73 _active = false;
74 unlock();
75 return;
76 }
77 glm::vec3 offset = _positionalTarget - bulletToGLM(rigidBody->getCenterOfMassPosition());
78 float offsetLength = glm::length(offset);
79 float speed = offsetLength / _linearTimeScale;
80
81 // cap speed
82 if (speed > SPRING_MAX_SPEED) {
83 speed = SPRING_MAX_SPEED;
84 }
85
86 if (offsetLength > IGNORE_POSITION_DELTA) {
87 glm::vec3 newVelocity = glm::normalize(offset) * speed;
88 rigidBody->setLinearVelocity(glmToBullet(newVelocity));
89 rigidBody->activate();
90 } else {
91 rigidBody->setLinearVelocity(glmToBullet(glm::vec3(0.0f)));
92 }
93 }74 }
94 75
95 // handle rotation76 if (_angularTimeScale < MAX_TIMESCALE) {
96 if (_rotationalTargetSet) {77 btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
97 if (_rotationalTarget.x != _rotationalTarget.x ||
98 _rotationalTarget.y != _rotationalTarget.y ||
99 _rotationalTarget.z != _rotationalTarget.z ||
100 _rotationalTarget.w != _rotationalTarget.w) {
101 qDebug() << "AvatarActionHold::updateActionWorker -- target rotation includes NaN";
102 unlock();
103 lockForWrite();
104 _active = false;
105 unlock();
106 return;
107 }
108 78
109 glm::quat bodyRotation = bulletToGLM(rigidBody->getOrientation());79 btQuaternion bodyRotation = rigidBody->getOrientation();
110 // if qZero and qOne are too close to each other, we can get NaN for angle.80 auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget));
111 auto alignmentDot = glm::dot(bodyRotation, _rotationalTarget);81 const float ALMOST_ONE = 0.99999f;
112 const float almostOne = 0.99999f;82 if (glm::abs(alignmentDot) < ALMOST_ONE) {
113 if (glm::abs(alignmentDot) < almostOne) {83 btQuaternion target = glmToBullet(_rotationalTarget);
114 glm::quat target = _rotationalTarget;84 if (alignmentDot < 0.0f) {
115 if (alignmentDot < 0) {
116 target = -target;85 target = -target;
117 }86 }
118 glm::quat qZeroInverse = glm::inverse(bodyRotation);87 // if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
119 glm::quat deltaQ = target * qZeroInverse;88 //
120 glm::vec3 axis = glm::axis(deltaQ);89 // Q1 = dQ * Q0
121 float angle = glm::angle(deltaQ);90 //
122 assert(!isNaN(angle));91 // solving for dQ gives:
123 glm::vec3 newAngularVelocity = (angle / _angularTimeScale) * glm::normalize(axis);92 //
124 rigidBody->setAngularVelocity(glmToBullet(newAngularVelocity));93 // dQ = Q1 * Q0^
125 rigidBody->activate();94 btQuaternion deltaQ = target * bodyRotation.inverse();
126 } else {95 float angle = deltaQ.getAngle();
127 rigidBody->setAngularVelocity(glmToBullet(glm::vec3(0.0f)));96 const float MIN_ANGLE = 1.0e-4;
97 if (angle > MIN_ANGLE) {
98 targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis();
99 }
128 }100 }
101 //float blend = glm::min(1.0f, deltaTimeStep / _angularTimeScale);
102 float blend = 1.0f;
103 rigidBody->setAngularVelocity((1.0f - blend) * rigidBody->getAngularVelocity() + blend * targetVelocity);
104 }
105 if (_needsActivation) {
106 rigidBody->activate();
107 _needsActivation = false;
129 }108 }
130
131 unlock();109 unlock();
132}110}
133 111
112const float MIN_TIMESCALE = 0.1f;
134 113
135bool ObjectActionSpring::updateArguments(QVariantMap arguments) {114bool ObjectActionSpring::updateArguments(QVariantMap arguments) {
136 // targets are required, spring-constants are optional115 // targets are required, spring-constants are optional
137 bool ptOk = true;116 bool ok = true;
138 glm::vec3 positionalTarget =117 glm::vec3 positionalTarget =
139 EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ptOk, false);118 EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ok, false);
140 bool pscOk = true;119 if (!ok) {
120 positionalTarget = _positionalTarget;
121 }
122 ok = true;
141 float linearTimeScale =123 float linearTimeScale =
142 EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", pscOk, false);124 EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", ok, false);
143 if (ptOk && pscOk && linearTimeScale <= 0.0f) {125 if (!ok || linearTimeScale <= 0.0f) {
144 qDebug() << "spring action -- linearTimeScale must be greater than zero.";126 linearTimeScale = _linearTimeScale;
145 return false;
146 }127 }
147 128
148 bool rtOk = true;129 ok = true;
149 glm::quat rotationalTarget =130 glm::quat rotationalTarget =
150 EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", rtOk, false);131 EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", ok, false);
151 bool rscOk = true;132 if (!ok) {
152 float angularTimeScale =133 rotationalTarget = _rotationalTarget;
153 EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", rscOk, false);
154
155 if (!ptOk && !rtOk) {
156 qDebug() << "spring action requires at least one of targetPosition or targetRotation argument";
157 return false;
158 }134 }
159 135
160 lockForWrite();136 ok = true;
161 137 float angularTimeScale =
162 _positionalTargetSet = _rotationalTargetSet = false;138 EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);
163 139 if (!ok) {
164 if (ptOk) {140 angularTimeScale = _angularTimeScale;
165 _positionalTarget = positionalTarget;
166 _positionalTargetSet = true;
167
168 if (pscOk) {
169 _linearTimeScale = linearTimeScale;
170 } else {
171 _linearTimeScale = 0.1f;
172 }
173 }141 }
174 142
175 if (rtOk) {143 if (positionalTarget != _positionalTarget
144 || linearTimeScale != _linearTimeScale
145 || rotationalTarget != _rotationalTarget
146 || angularTimeScale != _angularTimeScale) {
147 // something changed
148 lockForWrite();
149 _positionalTarget = positionalTarget;
150 _linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale));
176 _rotationalTarget = rotationalTarget;151 _rotationalTarget = rotationalTarget;
177 _rotationalTargetSet = true;152 _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));
178 153 _active = true;
179 if (rscOk) {154 _needsActivation = true;
180 _angularTimeScale = angularTimeScale;155 unlock();
181 } else {
182 _angularTimeScale = 0.1f;
183 }
184 }156 }
185
186 _active = true;
187 unlock();
188 return true;157 return true;
189}158}
190 159
@@ -192,15 +161,11 @@ QVariantMap ObjectActionSpring::getArguments() { · 1 unmodified line
192 QVariantMap arguments;161 QVariantMap arguments;
193 lockForRead();162 lockForRead();
194 163
195 if (_positionalTargetSet) {164 arguments["linearTimeScale"] = _linearTimeScale;
196 arguments["linearTimeScale"] = _linearTimeScale;165 arguments["targetPosition"] = glmToQMap(_positionalTarget);
197 arguments["targetPosition"] = glmToQMap(_positionalTarget);
198 }
199 166
200 if (_rotationalTargetSet) {167 arguments["targetRotation"] = glmToQMap(_rotationalTarget);
201 arguments["targetRotation"] = glmToQMap(_rotationalTarget);168 arguments["angularTimeScale"] = _angularTimeScale;
202 arguments["angularTimeScale"] = _angularTimeScale;
203 }
204 169
205 unlock();170 unlock();
206 return arguments;171 return arguments;