File indexing completed on 2024-04-21 03:51:24
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