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
1717
1818 const uint16_t ObjectActionSpring::springVersion = 1;
1919
20+
2021 ObjectActionSpring::ObjectActionSpring(const QUuid& id, EntityItemPointer ownerEntity) :
2122 ObjectAction(ACTION_TYPE_SPRING, id, ownerEntity),
2223 _positionalTarget(glm::vec3(0.0f)),
23− _linearTimeScale(0.2f),
24− _positionalTargetSet(false),
24+ _linearTimeScale(FLT_MAX),
25+ _positionalTargetSet(true),
2526 _rotationalTarget(glm::quat()),
26− _angularTimeScale(0.2f),
27− _rotationalTargetSet(false) {
27+ _angularTimeScale(FLT_MAX),
28+ _rotationalTargetSet(true),
29+ _needsActivation(true) {
2830 #if WANT_DEBUG
2931 qDebug() << "ObjectActionSpring::ObjectActionSpring";
3032 #endif
@@ -61,130 +63,97 @@ void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) { · 30 unmodified lines
6163 return;
6264 }
6365
64− // handle the linear part
65− if (_positionalTargetSet) {
66− // check for NaN
67− if (_positionalTarget.x != _positionalTarget.x ||
68− _positionalTarget.y != _positionalTarget.y ||
69− _positionalTarget.z != _positionalTarget.z) {
70− qDebug() << "ObjectActionSpring::updateActionWorker -- target position includes NaN";
71− unlock();
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− }
66+ const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
67+ if (_linearTimeScale < MAX_TIMESCALE) {
68+ btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);
69+ float offsetLength = offset.length();
70+ //float blend = glm::min(1.0f, deltaTimeStep / _linearTimeScale);
71+ float blend = 1.0f;
72+ float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f;
73+ rigidBody->setLinearVelocity((1.0f - blend) * rigidBody->getLinearVelocity() - (blend * speed / (offsetLength * _linearTimeScale)) * offset);
9374 }
9475
95− // handle rotation
96− if (_rotationalTargetSet) {
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− }
76+ if (_angularTimeScale < MAX_TIMESCALE) {
77+ btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
10878
109− glm::quat bodyRotation = bulletToGLM(rigidBody->getOrientation());
110− // if qZero and qOne are too close to each other, we can get NaN for angle.
111− auto alignmentDot = glm::dot(bodyRotation, _rotationalTarget);
112− const float almostOne = 0.99999f;
113− if (glm::abs(alignmentDot) < almostOne) {
114− glm::quat target = _rotationalTarget;
115− if (alignmentDot < 0) {
79+ btQuaternion bodyRotation = rigidBody->getOrientation();
80+ auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget));
81+ const float ALMOST_ONE = 0.99999f;
82+ if (glm::abs(alignmentDot) < ALMOST_ONE) {
83+ btQuaternion target = glmToBullet(_rotationalTarget);
84+ if (alignmentDot < 0.0f) {
11685 target = -target;
11786 }
118− glm::quat qZeroInverse = glm::inverse(bodyRotation);
119− glm::quat deltaQ = target * qZeroInverse;
120− glm::vec3 axis = glm::axis(deltaQ);
121− float angle = glm::angle(deltaQ);
122− assert(!isNaN(angle));
123− glm::vec3 newAngularVelocity = (angle / _angularTimeScale) * glm::normalize(axis);
124− rigidBody->setAngularVelocity(glmToBullet(newAngularVelocity));
125− rigidBody->activate();
126− } else {
127− rigidBody->setAngularVelocity(glmToBullet(glm::vec3(0.0f)));
87+ // if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
88+ //
89+ // Q1 = dQ * Q0
90+ //
91+ // solving for dQ gives:
92+ //
93+ // dQ = Q1 * Q0^
94+ btQuaternion deltaQ = target * bodyRotation.inverse();
95+ float angle = deltaQ.getAngle();
96+ const float MIN_ANGLE = 1.0e-4;
97+ if (angle > MIN_ANGLE) {
98+ targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis();
99+ }
128100 }
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;
129108 }
130−
131109 unlock();
132110 }
133111
112+const float MIN_TIMESCALE = 0.1f;
134113
135114 bool ObjectActionSpring::updateArguments(QVariantMap arguments) {
136115 // targets are required, spring-constants are optional
137− bool ptOk = true;
116+ bool ok = true;
138117 glm::vec3 positionalTarget =
139− EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ptOk, false);
140− bool pscOk = true;
118+ EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ok, false);
119+ if (!ok) {
120+ positionalTarget = _positionalTarget;
121+ }
122+ ok = true;
141123 float linearTimeScale =
142− EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", pscOk, false);
143− if (ptOk && pscOk && linearTimeScale <= 0.0f) {
144− qDebug() << "spring action -- linearTimeScale must be greater than zero.";
145− return false;
124+ EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", ok, false);
125+ if (!ok || linearTimeScale <= 0.0f) {
126+ linearTimeScale = _linearTimeScale;
146127 }
147128
148− bool rtOk = true;
129+ ok = true;
149130 glm::quat rotationalTarget =
150− EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", rtOk, false);
151− bool rscOk = true;
152− float angularTimeScale =
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;
131+ EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", ok, false);
132+ if (!ok) {
133+ rotationalTarget = _rotationalTarget;
158134 }
159135
160− lockForWrite();
161−
162− _positionalTargetSet = _rotationalTargetSet = false;
163−
164− if (ptOk) {
165− _positionalTarget = positionalTarget;
166− _positionalTargetSet = true;
167−
168− if (pscOk) {
169− _linearTimeScale = linearTimeScale;
170− } else {
171− _linearTimeScale = 0.1f;
172− }
136+ ok = true;
137+ float angularTimeScale =
138+ EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);
139+ if (!ok) {
140+ angularTimeScale = _angularTimeScale;
173141 }
174142
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));
176151 _rotationalTarget = rotationalTarget;
177− _rotationalTargetSet = true;
178−
179− if (rscOk) {
180− _angularTimeScale = angularTimeScale;
181− } else {
182− _angularTimeScale = 0.1f;
183− }
152+ _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));
153+ _active = true;
154+ _needsActivation = true;
155+ unlock();
184156 }
185−
186− _active = true;
187− unlock();
188157 return true;
189158 }
190159
@@ -192,15 +161,11 @@ QVariantMap ObjectActionSpring::getArguments() { · 1 unmodified line
192161 QVariantMap arguments;
193162 lockForRead();
194163
195− if (_positionalTargetSet) {
196− arguments["linearTimeScale"] = _linearTimeScale;
197− arguments["targetPosition"] = glmToQMap(_positionalTarget);
198− }
164+ arguments["linearTimeScale"] = _linearTimeScale;
165+ arguments["targetPosition"] = glmToQMap(_positionalTarget);
199166
200− if (_rotationalTargetSet) {
201− arguments["targetRotation"] = glmToQMap(_rotationalTarget);
202− arguments["angularTimeScale"] = _angularTimeScale;
203− }
167+ arguments["targetRotation"] = glmToQMap(_rotationalTarget);
168+ arguments["angularTimeScale"] = _angularTimeScale;
204169
205170 unlock();
206171 return arguments;