step/stepcore/gravitation.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 "gravitation.h"
0008 #include "particle.h"
0009 #include "rigidbody.h"
0010 #include <cmath>
0011
0012 namespace StepCore
0013 {
0014
0015 STEPCORE_META_OBJECT(GravitationForce, QT_TRANSLATE_NOOP("ObjectClass", "GravitationForce"), QT_TRANSLATE_NOOP("ObjectDescription", "Gravitation force"), 0,
0016     STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Force),
0017     STEPCORE_PROPERTY_RW(double, gravitationConst, QT_TRANSLATE_NOOP("PropertyName", "gravitationConst"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "N m²/kg²")),
0018             QT_TRANSLATE_NOOP("PropertyDescription", "Gravitation constant"), gravitationConst, setGravitationConst))
0019
0020 STEPCORE_META_OBJECT(GravitationForceErrors, QT_TRANSLATE_NOOP("ObjectClass", "GravitationForceErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for GravitationForce"), 0,
0021     STEPCORE_SUPER_CLASS(ObjectErrors),
0022     STEPCORE_PROPERTY_RW(double, gravitationConstVariance, QT_TRANSLATE_NOOP("PropertyName", "gravitationConstVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "N m²/kg²")),
0023             QT_TRANSLATE_NOOP("PropertyDescription", "Gravitation constant variance"), gravitationConstVariance, setGravitationConstVariance))
0024
0025 STEPCORE_META_OBJECT(WeightForce, QT_TRANSLATE_NOOP("ObjectClass", "WeightForce"), QT_TRANSLATE_NOOP("ObjectDescription", "Weight force"), 0,
0026     STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Force),
0027     STEPCORE_PROPERTY_RW(double, weightConst, QT_TRANSLATE_NOOP("PropertyName", "weightConst"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")), QT_TRANSLATE_NOOP("PropertyDescription", "Weight constant"),
0028                             weightConst, setWeightConst))
0029
0030 STEPCORE_META_OBJECT(WeightForceErrors, QT_TRANSLATE_NOOP("ObjectClass", "WeightForceErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for WeightForce"), 0,
0031     STEPCORE_SUPER_CLASS(ObjectErrors),
0032     STEPCORE_PROPERTY_RW(double, weightConstVariance, QT_TRANSLATE_NOOP("PropertyName", "weightConstVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
0033             QT_TRANSLATE_NOOP("PropertyDescription", "Weight constant variance"), weightConstVariance, setWeightConstVariance))
0034
0035 GravitationForce* GravitationForceErrors::gravitationForce() const
0036 {
0037     return static_cast<GravitationForce*>(owner());
0038 }
0039
0040 WeightForce* WeightForceErrors::weightForce() const
0041 {
0042     return static_cast<WeightForce*>(owner());
0043 }
0044
0045 GravitationForce::GravitationForce(double gravitationConst)
0046     : Force()
0047     , _gravitationConst(gravitationConst)
0048 {
0049     gravitationForceErrors()->setGravitationConstVariance(
0050         square(Constants::GravitationalError));
0051 }
0052
0053 void GravitationForce::calcForce(bool calcVariances)
0054 {
0055     const BodyList::const_iterator end = world()->bodies().end();
0056     for(BodyList::const_iterator b1 = world()->bodies().begin(); b1 != end; ++b1) {
0057         if(!(*b1)->metaObject()->inherits<Particle>()) continue;
0058         for(BodyList::const_iterator b2 = b1+1; b2 != end; ++b2) {
0059             if(!(*b2)->metaObject()->inherits<Particle>()) continue;
0060             Particle* p1 = static_cast<Particle*>(*b1);
0061             Particle* p2 = static_cast<Particle*>(*b2);
0062
0063             Vector2d r = p2->position() - p1->position();
0064             double rnorm2 = r.squaredNorm();
0065             Vector2d force = _gravitationConst * p1->mass() * p2->mass() * r / (rnorm2*sqrt(rnorm2));
0066             p1->applyForce(force);
0067             force = -force;
0068             p2->applyForce(force);
0069
0070             if(calcVariances) {
0071                 // XXX: CHECKME
0072                 ParticleErrors* pe1 = p1->particleErrors();
0073                 ParticleErrors* pe2 = p2->particleErrors();
0074                 Vector2d rV = pe2->positionVariance() + pe1->positionVariance();
0075                 Vector2d forceV = force.array().square()* (
0076                     Vector2d(rV[0] * square(1/r[0] - 3*r[0]/rnorm2) + rV[1] * square(3*r[1]/rnorm2),
0077                     rV[1] * square(1/r[1] - 3*r[1]/rnorm2) + rV[0] * square(3*r[0]/rnorm2))).array();
0078                 pe1->applyForceVariance(forceV);
0079                 pe2->applyForceVariance(forceV);
0080             }
0081         }
0082     }
0083 }
0084
0085 WeightForce::WeightForce(double weightConst)
0086     : Force()
0087     , _weightConst(weightConst)
0088 {
0089     weightForceErrors()->setWeightConstVariance(
0090         square(Constants::WeightAccelError));
0091 }
0092
0093 void WeightForce::calcForce(bool calcVariances)
0094 {
0095     Vector2d g(0., -_weightConst);
0096
0097     const BodyList::const_iterator end = world()->bodies().end();
0098     for(BodyList::const_iterator b1 = world()->bodies().begin(); b1 != end; ++b1) {
0099         if((*b1)->metaObject()->inherits<Particle>()) {
0100             Particle* p1 = static_cast<Particle*>(*b1);
0101             p1->applyForce(g*p1->mass());
0102             if(calcVariances) {
0103                 ParticleErrors* pe1 = p1->particleErrors();
0104                 Vector2d forceV(0., square(_weightConst)*pe1->massVariance()+
0105                                     square(p1->mass())*weightForceErrors()->weightConstVariance());
0106                 pe1->applyForceVariance(forceV);
0107             }
0108         } else if((*b1)->metaObject()->inherits<RigidBody>()) {
0109             RigidBody* rb1 = static_cast<RigidBody*>(*b1);
0110             rb1->applyForce(g*rb1->mass(), rb1->position());
0111             if(calcVariances) {
0112                 RigidBodyErrors* rbe1 = rb1->rigidBodyErrors();
0113                 Vector2d forceV(0., square(_weightConst)*rbe1->massVariance()+
0114                                     square(rb1->mass())*weightForceErrors()->weightConstVariance());
0115                 rbe1->applyForceVariance(g*rb1->mass(), rb1->position(),
0116                                          forceV, rbe1->positionVariance());
0117             }
0118         }
0119     }
0120 }
0121
0122 } // namespace StepCore
0123