step/stepcore/particle.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 "particle.h"
0008 #include "types.h"
0009 #include <cstring>
0010 #include <cmath>
0011
0012 namespace StepCore
0013 {
0014
0015 STEPCORE_META_OBJECT(Particle, QT_TRANSLATE_NOOP("ObjectClass", "Particle"), QT_TRANSLATE_NOOP("ObjectDescription", "Simple zero-size particle"), 0,
0016         STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Body),
0017         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, position, QT_TRANSLATE_NOOP("PropertyName", "position"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "position"), position, setPosition)
0018         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocity, QT_TRANSLATE_NOOP("PropertyName", "velocity"), QT_TRANSLATE_NOOP("Units", "m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "velocity"), velocity, setVelocity)
0019         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, acceleration, QT_TRANSLATE_NOOP("PropertyName", "acceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
0020                                                             QT_TRANSLATE_NOOP("PropertyDescription", "acceleration"), acceleration)
0021         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, force, QT_TRANSLATE_NOOP("PropertyName", "force"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "force"), force)
0022         STEPCORE_PROPERTY_RW(double, mass, QT_TRANSLATE_NOOP("PropertyName", "mass"), QT_TRANSLATE_NOOP("Units", "kg"), QT_TRANSLATE_NOOP("PropertyDescription", "mass"), mass, setMass)
0023         STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentum, QT_TRANSLATE_NOOP("PropertyName", "momentum"), QT_TRANSLATE_NOOP("Units", "kg m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "momentum"),
0024                         StepCore::MetaProperty::DYNAMIC, momentum, setMomentum)
0025         STEPCORE_PROPERTY_RWF(double, kineticEnergy, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergy"), QT_TRANSLATE_NOOP("Units", "J"), QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy"),
0026                         StepCore::MetaProperty::DYNAMIC, kineticEnergy, setKineticEnergy))
0027
0028 STEPCORE_META_OBJECT(ParticleErrors, QT_TRANSLATE_NOOP("ObjectClass", "ParticleErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for Particle"), 0, STEPCORE_SUPER_CLASS(ObjectErrors),
0029         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, positionVariance, QT_TRANSLATE_NOOP("PropertyName", "positionVariance"), QT_TRANSLATE_NOOP("Units", "m"),
0030                     QT_TRANSLATE_NOOP("PropertyDescription", "position variance"), positionVariance, setPositionVariance)
0031         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocityVariance, QT_TRANSLATE_NOOP("PropertyName", "velocityVariance"), QT_TRANSLATE_NOOP("Units", "m/s"),
0032                     QT_TRANSLATE_NOOP("PropertyDescription", "velocity variance"), velocityVariance, setVelocityVariance)
0033         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, accelerationVariance, QT_TRANSLATE_NOOP("PropertyName", "accelerationVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
0034                     QT_TRANSLATE_NOOP("PropertyDescription", "acceleration variance"), accelerationVariance)
0035         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, forceVariance, QT_TRANSLATE_NOOP("PropertyName", "forceVariance"), QT_TRANSLATE_NOOP("Units", "N"),
0036                     QT_TRANSLATE_NOOP("PropertyDescription", "force variance"), forceVariance)
0037         STEPCORE_PROPERTY_RW(double, massVariance, QT_TRANSLATE_NOOP("PropertyName", "massVariance"), QT_TRANSLATE_NOOP("Units", "kg"),
0038                     QT_TRANSLATE_NOOP("PropertyDescription", "mass variance"), massVariance, setMassVariance )
0039         STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentumVariance, QT_TRANSLATE_NOOP("PropertyName", "momentumVariance"), QT_TRANSLATE_NOOP("Units", "kg m/s"),
0040                     QT_TRANSLATE_NOOP("PropertyDescription", "momentum variance"), StepCore::MetaProperty::DYNAMIC, momentumVariance, setMomentumVariance)
0041         STEPCORE_PROPERTY_RWF(double, kineticEnergyVariance, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergyVariance"), QT_TRANSLATE_NOOP("Units", "J"),
0042                     QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy variance"), StepCore::MetaProperty::DYNAMIC, kineticEnergyVariance, setKineticEnergyVariance))
0043
0044 STEPCORE_META_OBJECT(ChargedParticle, QT_TRANSLATE_NOOP("ObjectClass", "ChargedParticle"), QT_TRANSLATE_NOOP("ObjectDescription", "Charged zero-size particle"), 0, STEPCORE_SUPER_CLASS(Particle),
0045         STEPCORE_PROPERTY_RW(double, charge, QT_TRANSLATE_NOOP("PropertyName", "charge"), QT_TRANSLATE_NOOP("Units", "C"), QT_TRANSLATE_NOOP("PropertyDescription", "charge"), charge, setCharge))
0046
0047 STEPCORE_META_OBJECT(ChargedParticleErrors, QT_TRANSLATE_NOOP("ObjectClass", "ChargedParticleErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for ChargedParticle"), 0,
0048         STEPCORE_SUPER_CLASS(ParticleErrors),
0049         STEPCORE_PROPERTY_RW(double, chargeVariance, QT_TRANSLATE_NOOP("PropertyName", "chargeVariance"), QT_TRANSLATE_NOOP("Units", "kg"),
0050                     QT_TRANSLATE_NOOP("PropertyDescription", "charge variance"), chargeVariance, setChargeVariance ))
0051
0052 Particle* ParticleErrors::particle() const
0053 {
0054     return static_cast<Particle*>(owner());
0055 }
0056
0057 Vector2d ParticleErrors::accelerationVariance() const
0058 {
0059     return _forceVariance/square(particle()->mass()) +
0060         _massVariance*(particle()->force()/square(particle()->mass())).array().square().matrix();
0061 }
0062
0063 Vector2d ParticleErrors::momentumVariance() const
0064 {
0065     return _velocityVariance * square(particle()->mass()) +
0066            (particle()->velocity().array().square()).matrix() * _massVariance;
0067 }
0068
0069 void ParticleErrors::setMomentumVariance(const Vector2d& momentumVariance)
0070 {
0071     _velocityVariance = (momentumVariance - (particle()->velocity().array().square()).matrix() * _massVariance) /
0072                         square(particle()->mass());
0073 }
0074
0075 double ParticleErrors::kineticEnergyVariance() const
0076 {
0077     return ((particle()->velocity().array().square()).matrix()).dot(_velocityVariance) * square(particle()->mass()) +
0078            square(particle()->velocity().squaredNorm()/2) * _massVariance;
0079 }
0080
0081 void ParticleErrors::setKineticEnergyVariance(double kineticEnergyVariance)
0082 {
0083     _velocityVariance = (kineticEnergyVariance - square(particle()->velocity().squaredNorm()/2) * _massVariance) /
0084                         square(particle()->mass()) / 2 *
0085                         (particle()->velocity().array().square().array().inverse());
0086     if(!std::isfinite(_velocityVariance[0]) || _velocityVariance[0] < 0 ||
0087        !std::isfinite(_velocityVariance[1]) || _velocityVariance[1]) {
0088         _velocityVariance.setZero();
0089     }
0090 }
0091
0092 ChargedParticle* ChargedParticleErrors::chargedParticle() const
0093 {
0094     return static_cast<ChargedParticle*>(owner());
0095 }
0096
0097 Particle::Particle(const Vector2d &position, const Vector2d &velocity, double mass)
0098     : _position(position), _velocity(velocity), _force(Vector2d::Zero()), _mass(mass)
0099 {
0100 }
0101
0102 void Particle::getVariables(double* position, double* velocity,
0103                      double* positionVariance, double* velocityVariance)
0104 {
0105     Vector2d::Map(position) = _position;
0106     Vector2d::Map(velocity) = _velocity;
0107     if(positionVariance) {
0108         ParticleErrors* pe = particleErrors();
0109         Vector2d::Map(positionVariance) = pe->_positionVariance;
0110         Vector2d::Map(velocityVariance) = pe->_velocityVariance;
0111     }
0112 }
0113
0114 void Particle::setVariables(const double* position, const double* velocity,
0115                const double* positionVariance, const double* velocityVariance)
0116 {
0117     _position = Vector2d::Map(position);
0118     _velocity = Vector2d::Map(velocity);
0119     _force.setZero();
0120     if(positionVariance) {
0121         ParticleErrors* pe = particleErrors();
0122         pe->_positionVariance = Vector2d::Map(positionVariance);
0123         pe->_velocityVariance = Vector2d::Map(velocityVariance);
0124         pe->_forceVariance.setZero();
0125     }
0126 }
0127
0128 void Particle::getAccelerations(double* acceleration, double* accelerationVariance)
0129 {
0130     acceleration[0] = _force[0] / _mass;
0131     acceleration[1] = _force[1] / _mass;
0132     if(accelerationVariance) {
0133         ParticleErrors* pe = particleErrors();
0134         accelerationVariance[0] = pe->_forceVariance[0]/square(_mass) +
0135                                         square(_force[0]/square(_mass))*pe->_massVariance;
0136         accelerationVariance[1] = pe->_forceVariance[1]/square(_mass) +
0137                                         square(_force[1]/square(_mass))*pe->_massVariance;
0138     }
0139 }
0140
0141 void Particle::addForce(const double* force, const double* forceVariance)
0142 {
0143     _force[0] += force[0];
0144     _force[1] += force[1];
0145     if(forceVariance) {
0146         ParticleErrors* pe = particleErrors();
0147         pe->_forceVariance[0] += forceVariance[0];
0148         pe->_forceVariance[1] += forceVariance[1];
0149     }
0150 }
0151
0152 void Particle::resetForce(bool resetVariance)
0153 {
0154     _force.setZero();
0155     if(resetVariance) particleErrors()->_forceVariance.setZero();
0156 }
0157
0158 void Particle::getInverseMass(VectorXd* inverseMass,
0159                               DynSparseRowMatrix* variance, int offset)
0160 {
0161     inverseMass->coeffRef(offset) = ( 1/_mass);
0162     inverseMass->coeffRef(offset+1) = ( 1/_mass);
0163     if(variance) {
0164         double v = particleErrors()->_massVariance / square(square(_mass));
0165         variance->coeffRef(offset, offset) = (v);
0166         variance->coeffRef(offset+1, offset+1) = (v);
0167     }
0168 }
0169
0170 void Particle::setKineticEnergy(double kineticEnergy)
0171 {
0172     double v = _velocity.norm();
0173     _velocity = sqrt(kineticEnergy*2/_mass) * (v>0 ? (_velocity/v).eval() : Vector2d(1,0));
0174 }
0175
0176 } // namespace StepCore
0177