step/stepcore/spring.cc

0001 /*
0002     SPDX-FileCopyrightText: 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
0003
0004     SPDX-License-Identifier: GPL-2.0-or-later
0005 */
0006
0007 #include "spring.h"
0008 #include "types.h"
0009
0010 #include <algorithm>
0011 #include <cmath>
0012 #include <QtGlobal>
0013
0014 namespace StepCore {
0015
0016 STEPCORE_META_OBJECT(Spring, QT_TRANSLATE_NOOP("ObjectClass", "Spring"), QT_TRANSLATE_NOOP("ObjectDescription", "Massless spring which can be connected to bodies"), 0,
0017     STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Force),
0018     STEPCORE_PROPERTY_RW(double, restLength, QT_TRANSLATE_NOOP("PropertyName", "restLength"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Rest length"), restLength, setRestLength)
0019     STEPCORE_PROPERTY_R_D(double, length, QT_TRANSLATE_NOOP("PropertyName", "length"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Current length"), length)
0020     STEPCORE_PROPERTY_RW(double, stiffness, QT_TRANSLATE_NOOP("PropertyName", "stiffness"), QT_TRANSLATE_NOOP("Units", "N/m"), QT_TRANSLATE_NOOP("PropertyDescription", "Stiffness"), stiffness, setStiffness)
0021     STEPCORE_PROPERTY_RW(double, damping, QT_TRANSLATE_NOOP("PropertyName", "damping"), QT_TRANSLATE_NOOP("Units", "N s/m"), QT_TRANSLATE_NOOP("PropertyDescription", "Damping"), damping, setDamping)
0022     STEPCORE_PROPERTY_RW(Object*, body1, QT_TRANSLATE_NOOP("PropertyName", "body1"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body1"), body1, setBody1)
0023     STEPCORE_PROPERTY_RW(Object*, body2, QT_TRANSLATE_NOOP("PropertyName", "body2"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body2"), body2, setBody2)
0024     STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition1, QT_TRANSLATE_NOOP("PropertyName", "localPosition1"), QT_TRANSLATE_NOOP("Units", "m"),
0025                     QT_TRANSLATE_NOOP("PropertyDescription", "Local position 1"), localPosition1, setLocalPosition1)
0026     STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition2, QT_TRANSLATE_NOOP("PropertyName", "localPosition2"), QT_TRANSLATE_NOOP("Units", "m"),
0027                     QT_TRANSLATE_NOOP("PropertyDescription", "Local position 2"), localPosition2, setLocalPosition2)
0028     STEPCORE_PROPERTY_R_D(StepCore::Vector2d, position1, QT_TRANSLATE_NOOP("PropertyName", "position1"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position1"), position1)
0029     STEPCORE_PROPERTY_R_D(StepCore::Vector2d, position2, QT_TRANSLATE_NOOP("PropertyName", "position2"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position2"), position2)
0030     STEPCORE_PROPERTY_R_D(double, force, QT_TRANSLATE_NOOP("PropertyName", "force"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "Spring tension force"), force)
0031     )
0032
0033 STEPCORE_META_OBJECT(SpringErrors, QT_TRANSLATE_NOOP("ObjectClass", "SpringErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for Spring"), 0,
0034     STEPCORE_SUPER_CLASS(ObjectErrors),
0035     STEPCORE_PROPERTY_RW(double, restLengthVariance, QT_TRANSLATE_NOOP("PropertyName", "restLengthVariance"), QT_TRANSLATE_NOOP("Units", "m"),
0036                     QT_TRANSLATE_NOOP("PropertyDescription", "Rest length variance"), restLengthVariance, setRestLengthVariance)
0037     STEPCORE_PROPERTY_R_D(double, lengthVariance, QT_TRANSLATE_NOOP("PropertyName", "lengthVariance"), QT_TRANSLATE_NOOP("Units", "m"),
0038                     QT_TRANSLATE_NOOP("PropertyDescription", "Current length variance"), lengthVariance)
0039     STEPCORE_PROPERTY_RW(double, stiffnessVariance, QT_TRANSLATE_NOOP("PropertyName", "stiffnessVariance"), QT_TRANSLATE_NOOP("Units", "N/m"),
0040                     QT_TRANSLATE_NOOP("PropertyDescription", "Stiffness variance"), stiffnessVariance, setStiffnessVariance)
0041     STEPCORE_PROPERTY_RW(double, dampingVariance, QT_TRANSLATE_NOOP("PropertyName", "dampingVariance"), QT_TRANSLATE_NOOP("Units", "N/m"),
0042                     QT_TRANSLATE_NOOP("PropertyDescription", "Damping variance"), dampingVariance, setDampingVariance)
0043     STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition1Variance, QT_TRANSLATE_NOOP("PropertyName", "localPosition1Variance"), QT_TRANSLATE_NOOP("Units", "m"),
0044                     QT_TRANSLATE_NOOP("PropertyDescription", "Local position 1 variance"), localPosition1Variance, setLocalPosition1Variance)
0045     STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition2Variance, QT_TRANSLATE_NOOP("PropertyName", "localPosition2Variance"), QT_TRANSLATE_NOOP("Units", "m"),
0046                     QT_TRANSLATE_NOOP("PropertyDescription", "Local position 2 variance"), localPosition2Variance, setLocalPosition2Variance)
0047     STEPCORE_PROPERTY_R_D(StepCore::Vector2d, position1Variance, QT_TRANSLATE_NOOP("PropertyName", "position1Variance"), QT_TRANSLATE_NOOP("Units", "m"),
0048                     QT_TRANSLATE_NOOP("PropertyDescription", "Position1 variance"), position1Variance)
0049     STEPCORE_PROPERTY_R_D(StepCore::Vector2d, position2Variance, QT_TRANSLATE_NOOP("PropertyName", "position2Variance"), QT_TRANSLATE_NOOP("Units", "m"),
0050                     QT_TRANSLATE_NOOP("PropertyDescription", "Position2 variance"), position2Variance)
0051     STEPCORE_PROPERTY_R_D(double, forceVariance, QT_TRANSLATE_NOOP("PropertyName", "forceVariance"), QT_TRANSLATE_NOOP("Units", "N"),
0052                     QT_TRANSLATE_NOOP("PropertyDescription", "Spring tension force variance"), forceVariance)
0053     )
0054
0055 Spring* SpringErrors::spring() const
0056 {
0057     return static_cast<Spring*>(owner());
0058 }
0059
0060 Spring::Spring(double restLength, double stiffness, double damping, Item* body1, Item* body2)
0061     : Force()
0062     , _restLength(restLength)
0063     , _stiffness(stiffness), _damping(damping)
0064     , _localPosition1(0,0), _localPosition2(0,0)
0065 {
0066     setColor(0xff00ff00);
0067     setBody1(body1);
0068     setBody2(body2);
0069 }
0070
0071 void Spring::calcForce(bool calcVariances)
0072 {
0073     if(!_body1 && !_body2) return;
0074
0075     Vector2d position1 = this->position1();
0076     Vector2d position2 = this->position2();
0077     Vector2d r = position2   - position1;
0078     Vector2d v = velocity2() - velocity1();
0079
0080     double l = r.norm();
0081     if(l == 0) return; // XXX: take orientation from previous step
0082
0083     double dl = l - _restLength;
0084     double vr = r.dot(v);
0085     Vector2d force = (_stiffness*dl + _damping*vr/l) / l * r;
0086
0087     if(_p1) _p1->applyForce(force);
0088     else if(_r1) _r1->applyForce(force, position1);
0089
0090     force = -force;
0091     if(_p2) _p2->applyForce(force);
0092     else if(_r2) _r2->applyForce(force, position2);
0093
0094     if(calcVariances) {
0095         SpringErrors* se = springErrors();
0096
0097         Vector2d rV = se->position2Variance() + se->position1Variance();
0098         Vector2d vV = se->velocity2Variance() + se->velocity1Variance();
0099
0100         Vector2d forceV = (se->_restLengthVariance * square(_stiffness) +
0101                            se->_stiffnessVariance * square(dl) +
0102                            se->_dampingVariance * square(vr/l) +
0103                            ( (_damping/l*r).array().square() ).matrix().dot(vV)
0104                            )/square(l)*r.array().square();
0105
0106         forceV[0] += rV[0] * square(_stiffness*( 1 - _restLength/l*(1 - square(r[0]/l)) ) +
0107                                     _damping/(l*l)*( v[0]*r[0] + vr - 2*vr*square(r[0]/l) )) +
0108                      rV[1] * square(_stiffness*_restLength*r[0]*r[1]/(l*l*l) +
0109                                     _damping/(l*l)*( v[1]*r[0] - 2*vr*r[0]*r[1]/(l*l) ));
0110         forceV[1] += rV[1] * square(_stiffness*( 1 - _restLength/l*(1 - square(r[1]/l)) ) +
0111                                     _damping/(l*l)*( v[1]*r[1] + vr - 2*vr*square(r[1]/l) )) +
0112                      rV[0] * square(_stiffness*_restLength*r[0]*r[1]/(l*l*l) +
0113                                     _damping/(l*l)*( v[0]*r[1] - 2*vr*r[0]*r[1]/(l*l) ));
0114
0115         // TODO: position1() and force is correlated, we should take it into account
0116         if(_p1) _p1->particleErrors()->applyForceVariance(forceV);
0117         else if(_r1) _r1->rigidBodyErrors()->applyForceVariance(force, position1,
0118                                                 forceV, se->position1Variance() );
0119
0120         if(_p2) _p2->particleErrors()->applyForceVariance(forceV);
0121         else if(_r2) _r2->rigidBodyErrors()->applyForceVariance(force, position2,
0122                                                 forceV, se->position2Variance() );
0123     }
0124 }
0125
0126 void Spring::setBody1(Object* body1)
0127 {
0128     if(body1) {
0129         if(body1->metaObject()->inherits<Particle>()) {
0130             _body1 = body1;
0131             _p1 = static_cast<Particle*>(body1);
0132             _r1 = nullptr;
0133             return;
0134         } else if(body1->metaObject()->inherits<RigidBody>()) {
0135             _body1 = body1;
0136             _p1 = nullptr;
0137             _r1 = static_cast<RigidBody*>(body1);
0138             return;
0139         }
0140     }
0141     _body1 = nullptr;
0142     _p1 = nullptr;
0143     _r1 = nullptr;
0144 }
0145
0146 void Spring::setBody2(Object* body2)
0147 {
0148     if(body2) {
0149         if(body2->metaObject()->inherits<Particle>()) {
0150             _body2 = body2;
0151             _p2 = static_cast<Particle*>(body2);
0152             _r2 = nullptr;
0153             return;
0154         } else if(body2->metaObject()->inherits<RigidBody>()) {
0155             _body2 = body2;
0156             _p2 = nullptr;
0157             _r2 = static_cast<RigidBody*>(body2);
0158             return;
0159         }
0160     }
0161     _body2 = nullptr;
0162     _p2 = nullptr;
0163     _r2 = nullptr;
0164 }
0165
0166 Vector2d Spring::position1() const
0167 {
0168     if(_p1) return _p1->position() + _localPosition1;
0169     else if(_r1) return _r1->pointLocalToWorld(_localPosition1);
0170     else return _localPosition1;
0171 }
0172
0173 Vector2d SpringErrors::position1Variance() const
0174 {
0175     if(spring()->_p1)
0176         return spring()->_p1->particleErrors()->positionVariance() + _localPosition1Variance;
0177     // XXX: TODO
0178     //RigidBody* _r1 = dynamic_cast<RigidBody*>(_body1);
0179     //if(_r1) return _r1->pointLocalToWorld(_localPosition1);
0180 #ifdef __GNUC__
0181 #warning variance calculation for spring connected to rigidbody is not finished !
0182 #warning consider unification of some part of Particle and RigidBody
0183 #endif
0184     else return _localPosition1Variance;
0185 }
0186
0187 Vector2d Spring::position2() const
0188 {
0189     if(_p2) return _p2->position() + _localPosition2;
0190     else if(_r2) return _r2->pointLocalToWorld(_localPosition2);
0191     else return _localPosition2;
0192 }
0193
0194 Vector2d SpringErrors::position2Variance() const
0195 {
0196     if(spring()->_p2)
0197         return spring()->_p2->particleErrors()->positionVariance() + _localPosition2Variance;
0198     // XXX: TODO
0199     //RigidBody* _r2 = dynamic_cast<RigidBody*>(_body2);
0200     //if(_r2) return _r2->pointLocalToWorld(_localPosition2);
0201     else return _localPosition2Variance;
0202 }
0203
0204 double SpringErrors::lengthVariance() const
0205 {
0206     Vector2d r = spring()->position2() - spring()->position1();
0207     Vector2d rV = position2Variance() + position1Variance();
0208     return (r[0]*r[0]*rV[0] + r[1]*r[1]*rV[1])/r.squaredNorm();
0209 }
0210
0211 Vector2d Spring::velocity1() const
0212 {
0213     if(_p1) return _p1->velocity();
0214     else if(_r1) return _r1->velocityLocal(_localPosition1);
0215     else return Vector2d::Zero();
0216 }
0217
0218 Vector2d SpringErrors::velocity1Variance() const
0219 {
0220     if(spring()->_p1)
0221         return spring()->_p1->particleErrors()->velocityVariance();
0222     // XXX: TODO
0223     //RigidBody* _r1 = dynamic_cast<RigidBody*>(_body1);
0224     //if(_r1) return _r1->pointLocalToWorld(_localPosition1);
0225     else return Vector2d::Zero();
0226 }
0227
0228 Vector2d Spring::velocity2() const
0229 {
0230     if(_p2) return _p2->velocity();
0231     else if(_r2) return _r2->velocityLocal(_localPosition2);
0232     else return Vector2d::Zero();
0233 }
0234
0235 Vector2d SpringErrors::velocity2Variance() const
0236 {
0237     if(spring()->_p2)
0238         return spring()->_p2->particleErrors()->velocityVariance();
0239     // XXX: TODO
0240     //RigidBody* _r2 = dynamic_cast<RigidBody*>(_body2);
0241     //if(_r2) return _r2->pointLocalToWorld(_localPosition2);
0242     else return Vector2d::Zero();
0243 }
0244
0245 double Spring::force() const
0246 {
0247     Vector2d r = position2() - position1();
0248     Vector2d v = velocity2() - velocity1();
0249     double l = r.norm();
0250     return _stiffness * (l - _restLength) +
0251                 _damping * r.dot(v)/l;
0252 }
0253
0254 double SpringErrors::forceVariance() const
0255 {
0256     Spring* s = spring();
0257     Vector2d r = s->position2() - s->position1();
0258     Vector2d v = s->velocity2() - s->velocity1();
0259     Vector2d rV = position2Variance() + position1Variance();
0260     Vector2d vV = velocity2Variance() + velocity1Variance();
0261     double l = r.norm();
0262     double dl = l - s->restLength();
0263     // XXX: CHECKME
0264     return square(dl) * _stiffnessVariance +
0265            square(s->stiffness()) * _restLengthVariance +
0266            square(r.dot(v)/l) * _dampingVariance +
0267            vV.dot((s->damping()/l*r).array().square().matrix()) +
0268            rV.dot((( s->stiffness() - s->damping()*r.dot(v) / (l*l) ) / l * r +
0269               s->damping() / l * v).array().square().matrix());
0270 }
0271
0272 /*
0273 void Spring::worldItemRemoved(Item* item)
0274 {
0275     if(item == NULL) return;
0276     else if(item == _body1) setBody1(NULL);
0277     else if(item == _body2) setBody2(NULL);
0278 }
0279 */
0280
0281 #if 0
0282 void Spring::setWorld(World* world)
0283 {
0284     if(world == NULL) {
0285         setBody1(NULL);
0286         setBody2(NULL);
0287     } else if(this->world() != NULL) {
0288 #ifdef __GNUC__
0289 #warning Use map instead of search-by-name here !
0290 #endif
0291         if(_body1 != NULL) setBody1(world->item(body1()->name())); //XXX
0292         if(_body2 != NULL) setBody2(world->item(body2()->name())); //XXX
0293     }
0294     Item::setWorld(world);
0295 }
0296 #endif
0297
0298 } // namespace StepCore
0299