1 /* This file is part of StepCore library.
2 Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
3
4 StepCore library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
8
9 StepCore library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with StepCore; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
17 */
18
19 #include "particle.h"
20 #include "types.h"
21 #include <cstring>
22 #include <cmath>
23
24 namespace StepCore
25 {
26
27 STEPCORE_META_OBJECT(Particle, QT_TRANSLATE_NOOP("ObjectClass", "Particle"), QT_TRANSLATE_NOOP("ObjectDescription", "Simple zero-size particle"), 0,
28 STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Body),
29 STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, position, QT_TRANSLATE_NOOP("PropertyName", "position"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "position"), position, setPosition)
30 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)
31 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, acceleration, QT_TRANSLATE_NOOP("PropertyName", "acceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
32 QT_TRANSLATE_NOOP("PropertyDescription", "acceleration"), acceleration)
33 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, force, QT_TRANSLATE_NOOP("PropertyName", "force"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "force"), force)
34 STEPCORE_PROPERTY_RW(double, mass, QT_TRANSLATE_NOOP("PropertyName", "mass"), QT_TRANSLATE_NOOP("Units", "kg"), QT_TRANSLATE_NOOP("PropertyDescription", "mass"), mass, setMass)
35 STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentum, QT_TRANSLATE_NOOP("PropertyName", "momentum"), QT_TRANSLATE_NOOP("Units", "kg m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "momentum"),
36 StepCore::MetaProperty::DYNAMIC, momentum, setMomentum)
37 STEPCORE_PROPERTY_RWF(double, kineticEnergy, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergy"), QT_TRANSLATE_NOOP("Units", "J"), QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy"),
38 StepCore::MetaProperty::DYNAMIC, kineticEnergy, setKineticEnergy))
39
40 STEPCORE_META_OBJECT(ParticleErrors, QT_TRANSLATE_NOOP("ObjectClass", "ParticleErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for Particle"), 0, STEPCORE_SUPER_CLASS(ObjectErrors),
41 STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, positionVariance, QT_TRANSLATE_NOOP("PropertyName", "positionVariance"), QT_TRANSLATE_NOOP("Units", "m"),
42 QT_TRANSLATE_NOOP("PropertyDescription", "position variance"), positionVariance, setPositionVariance)
43 STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocityVariance, QT_TRANSLATE_NOOP("PropertyName", "velocityVariance"), QT_TRANSLATE_NOOP("Units", "m/s"),
44 QT_TRANSLATE_NOOP("PropertyDescription", "velocity variance"), velocityVariance, setVelocityVariance)
45 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, accelerationVariance, QT_TRANSLATE_NOOP("PropertyName", "accelerationVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
46 QT_TRANSLATE_NOOP("PropertyDescription", "acceleration variance"), accelerationVariance)
47 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, forceVariance, QT_TRANSLATE_NOOP("PropertyName", "forceVariance"), QT_TRANSLATE_NOOP("Units", "N"),
48 QT_TRANSLATE_NOOP("PropertyDescription", "force variance"), forceVariance)
49 STEPCORE_PROPERTY_RW(double, massVariance, QT_TRANSLATE_NOOP("PropertyName", "massVariance"), QT_TRANSLATE_NOOP("Units", "kg"),
50 QT_TRANSLATE_NOOP("PropertyDescription", "mass variance"), massVariance, setMassVariance )
51 STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentumVariance, QT_TRANSLATE_NOOP("PropertyName", "momentumVariance"), QT_TRANSLATE_NOOP("Units", "kg m/s"),
52 QT_TRANSLATE_NOOP("PropertyDescription", "momentum variance"), StepCore::MetaProperty::DYNAMIC, momentumVariance, setMomentumVariance)
53 STEPCORE_PROPERTY_RWF(double, kineticEnergyVariance, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergyVariance"), QT_TRANSLATE_NOOP("Units", "J"),
54 QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy variance"), StepCore::MetaProperty::DYNAMIC, kineticEnergyVariance, setKineticEnergyVariance))
55
56 STEPCORE_META_OBJECT(ChargedParticle, QT_TRANSLATE_NOOP("ObjectClass", "ChargedParticle"), QT_TRANSLATE_NOOP("ObjectDescription", "Charged zero-size particle"), 0, STEPCORE_SUPER_CLASS(Particle),
57 STEPCORE_PROPERTY_RW(double, charge, QT_TRANSLATE_NOOP("PropertyName", "charge"), QT_TRANSLATE_NOOP("Units", "C"), QT_TRANSLATE_NOOP("PropertyDescription", "charge"), charge, setCharge))
58
59 STEPCORE_META_OBJECT(ChargedParticleErrors, QT_TRANSLATE_NOOP("ObjectClass", "ChargedParticleErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for ChargedParticle"), 0,
60 STEPCORE_SUPER_CLASS(ParticleErrors),
61 STEPCORE_PROPERTY_RW(double, chargeVariance, QT_TRANSLATE_NOOP("PropertyName", "chargeVariance"), QT_TRANSLATE_NOOP("Units", "kg"),
62 QT_TRANSLATE_NOOP("PropertyDescription", "charge variance"), chargeVariance, setChargeVariance ))
63
particle() const64 Particle* ParticleErrors::particle() const
65 {
66 return static_cast<Particle*>(owner());
67 }
68
accelerationVariance() const69 Vector2d ParticleErrors::accelerationVariance() const
70 {
71 return _forceVariance/square(particle()->mass()) +
72 _massVariance*(particle()->force()/square(particle()->mass())).array().square().matrix();
73 }
74
momentumVariance() const75 Vector2d ParticleErrors::momentumVariance() const
76 {
77 return _velocityVariance * square(particle()->mass()) +
78 (particle()->velocity().array().square()).matrix() * _massVariance;
79 }
80
setMomentumVariance(const Vector2d & momentumVariance)81 void ParticleErrors::setMomentumVariance(const Vector2d& momentumVariance)
82 {
83 _velocityVariance = (momentumVariance - (particle()->velocity().array().square()).matrix() * _massVariance) /
84 square(particle()->mass());
85 }
86
kineticEnergyVariance() const87 double ParticleErrors::kineticEnergyVariance() const
88 {
89 return ((particle()->velocity().array().square()).matrix()).dot(_velocityVariance) * square(particle()->mass()) +
90 square(particle()->velocity().squaredNorm()/2) * _massVariance;
91 }
92
setKineticEnergyVariance(double kineticEnergyVariance)93 void ParticleErrors::setKineticEnergyVariance(double kineticEnergyVariance)
94 {
95 _velocityVariance = (kineticEnergyVariance - square(particle()->velocity().squaredNorm()/2) * _massVariance) /
96 square(particle()->mass()) / 2 *
97 (particle()->velocity().array().square().array().inverse());
98 if(!std::isfinite(_velocityVariance[0]) || _velocityVariance[0] < 0 ||
99 !std::isfinite(_velocityVariance[1]) || _velocityVariance[1]) {
100 _velocityVariance.setZero();
101 }
102 }
103
chargedParticle() const104 ChargedParticle* ChargedParticleErrors::chargedParticle() const
105 {
106 return static_cast<ChargedParticle*>(owner());
107 }
108
Particle(const Vector2d & position,const Vector2d & velocity,double mass)109 Particle::Particle(const Vector2d &position, const Vector2d &velocity, double mass)
110 : _position(position), _velocity(velocity), _force(Vector2d::Zero()), _mass(mass)
111 {
112 }
113
getVariables(double * position,double * velocity,double * positionVariance,double * velocityVariance)114 void Particle::getVariables(double* position, double* velocity,
115 double* positionVariance, double* velocityVariance)
116 {
117 Vector2d::Map(position) = _position;
118 Vector2d::Map(velocity) = _velocity;
119 if(positionVariance) {
120 ParticleErrors* pe = particleErrors();
121 Vector2d::Map(positionVariance) = pe->_positionVariance;
122 Vector2d::Map(velocityVariance) = pe->_velocityVariance;
123 }
124 }
125
setVariables(const double * position,const double * velocity,const double * positionVariance,const double * velocityVariance)126 void Particle::setVariables(const double* position, const double* velocity,
127 const double* positionVariance, const double* velocityVariance)
128 {
129 _position = Vector2d::Map(position);
130 _velocity = Vector2d::Map(velocity);
131 _force.setZero();
132 if(positionVariance) {
133 ParticleErrors* pe = particleErrors();
134 pe->_positionVariance = Vector2d::Map(positionVariance);
135 pe->_velocityVariance = Vector2d::Map(velocityVariance);
136 pe->_forceVariance.setZero();
137 }
138 }
139
getAccelerations(double * acceleration,double * accelerationVariance)140 void Particle::getAccelerations(double* acceleration, double* accelerationVariance)
141 {
142 acceleration[0] = _force[0] / _mass;
143 acceleration[1] = _force[1] / _mass;
144 if(accelerationVariance) {
145 ParticleErrors* pe = particleErrors();
146 accelerationVariance[0] = pe->_forceVariance[0]/square(_mass) +
147 square(_force[0]/square(_mass))*pe->_massVariance;
148 accelerationVariance[1] = pe->_forceVariance[1]/square(_mass) +
149 square(_force[1]/square(_mass))*pe->_massVariance;
150 }
151 }
152
addForce(const double * force,const double * forceVariance)153 void Particle::addForce(const double* force, const double* forceVariance)
154 {
155 _force[0] += force[0];
156 _force[1] += force[1];
157 if(forceVariance) {
158 ParticleErrors* pe = particleErrors();
159 pe->_forceVariance[0] += forceVariance[0];
160 pe->_forceVariance[1] += forceVariance[1];
161 }
162 }
163
resetForce(bool resetVariance)164 void Particle::resetForce(bool resetVariance)
165 {
166 _force.setZero();
167 if(resetVariance) particleErrors()->_forceVariance.setZero();
168 }
169
getInverseMass(VectorXd * inverseMass,DynSparseRowMatrix * variance,int offset)170 void Particle::getInverseMass(VectorXd* inverseMass,
171 DynSparseRowMatrix* variance, int offset)
172 {
173 inverseMass->coeffRef(offset) = ( 1/_mass);
174 inverseMass->coeffRef(offset+1) = ( 1/_mass);
175 if(variance) {
176 double v = particleErrors()->_massVariance / square(square(_mass));
177 variance->coeffRef(offset, offset) = (v);
178 variance->coeffRef(offset+1, offset+1) = (v);
179 }
180 }
181
setKineticEnergy(double kineticEnergy)182 void Particle::setKineticEnergy(double kineticEnergy)
183 {
184 double v = _velocity.norm();
185 _velocity = sqrt(kineticEnergy*2/_mass) * (v>0 ? (_velocity/v).eval() : Vector2d(1,0));
186 }
187
188 } // namespace StepCore
189
190