step/stepcore/rigidbody.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 "rigidbody.h"
0008 #include "types.h"
0009 #include <cstring>
0010 #include <cmath>
0011
0012 namespace StepCore
0013 {
0014
0015 STEPCORE_META_OBJECT(RigidBody, QT_TRANSLATE_NOOP("ObjectClass", "RigidBody"), QT_TRANSLATE_NOOP("ObjectDescription", "Generic rigid body"), 0, STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Body),
0016         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, position, QT_TRANSLATE_NOOP("PropertyName", "position"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position of the center of mass"), position, setPosition)
0017         STEPCORE_PROPERTY_RW_D(double, angle, QT_TRANSLATE_NOOP("PropertyName", "angle"), QT_TRANSLATE_NOOP("Units", "rad"), QT_TRANSLATE_NOOP("PropertyDescription", "Rotation angle"), angle, setAngle)
0018
0019         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocity, QT_TRANSLATE_NOOP("PropertyName", "velocity"), QT_TRANSLATE_NOOP("Units", "m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "Velocity of the center of mass"), velocity, setVelocity)
0020         STEPCORE_PROPERTY_RW_D(double, angularVelocity, QT_TRANSLATE_NOOP("PropertyName", "angularVelocity"), QT_TRANSLATE_NOOP("Units", "rad/s"), QT_TRANSLATE_NOOP("PropertyDescription", "Angular velocity of the body"), angularVelocity, setAngularVelocity)
0021
0022         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, acceleration, QT_TRANSLATE_NOOP("PropertyName", "acceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
0023                                             QT_TRANSLATE_NOOP("PropertyDescription", "Acceleration of the center of mass"), acceleration)
0024         STEPCORE_PROPERTY_R_D(double, angularAcceleration, QT_TRANSLATE_NOOP("PropertyName", "angularAcceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "rad/s²")),
0025                                             QT_TRANSLATE_NOOP("PropertyDescription", "Angular acceleration of the body"), angularAcceleration)
0026
0027         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, force, QT_TRANSLATE_NOOP("PropertyName", "force"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "Force that acts upon the body"), force)
0028         STEPCORE_PROPERTY_R_D(double, torque, QT_TRANSLATE_NOOP("PropertyName", "torque"), QT_TRANSLATE_NOOP("Units", "N m"), QT_TRANSLATE_NOOP("PropertyDescription", "Torque that acts upon the body"), torque)
0029
0030         STEPCORE_PROPERTY_RW(double, mass, QT_TRANSLATE_NOOP("PropertyName", "mass"), QT_TRANSLATE_NOOP("Units", "kg"), QT_TRANSLATE_NOOP("PropertyDescription", "Total mass of the body"), mass, setMass)
0031         STEPCORE_PROPERTY_RW(double, inertia, QT_TRANSLATE_NOOP("PropertyName", "inertia"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m²")),
0032                                     QT_TRANSLATE_NOOP("PropertyDescription", "Inertia \"tensor\" of the body"), inertia, setInertia)
0033         STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentum, QT_TRANSLATE_NOOP("PropertyName", "momentum"), QT_TRANSLATE_NOOP("Units", "kg m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "momentum"),
0034                         StepCore::MetaProperty::DYNAMIC, momentum, setMomentum)
0035         STEPCORE_PROPERTY_RWF(double, angularMomentum, QT_TRANSLATE_NOOP("PropertyName", "angularMomentum"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m² rad/s")), QT_TRANSLATE_NOOP("PropertyDescription", "angular momentum"),
0036                         StepCore::MetaProperty::DYNAMIC, angularMomentum, setAngularMomentum)
0037         STEPCORE_PROPERTY_RWF(double, kineticEnergy, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergy"), QT_TRANSLATE_NOOP("Units", "J"), QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy"),
0038                         StepCore::MetaProperty::DYNAMIC, kineticEnergy, setKineticEnergy))
0039
0040 STEPCORE_META_OBJECT(RigidBodyErrors, QT_TRANSLATE_NOOP("ObjectClass", "RigidBodyErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for RigidBody"), 0, STEPCORE_SUPER_CLASS(ObjectErrors),
0041         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, positionVariance, QT_TRANSLATE_NOOP("PropertyName", "positionVariance"), QT_TRANSLATE_NOOP("Units", "m"),
0042                     QT_TRANSLATE_NOOP("PropertyDescription", "position variance"), positionVariance, setPositionVariance)
0043         STEPCORE_PROPERTY_RW_D(double, angleVariance, QT_TRANSLATE_NOOP("PropertyName", "angleVariance"), QT_TRANSLATE_NOOP("Units", "rad"),
0044                     QT_TRANSLATE_NOOP("PropertyDescription", "angle variance"), angleVariance, setAngleVariance)
0045
0046         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocityVariance, QT_TRANSLATE_NOOP("PropertyName", "velocityVariance"), QT_TRANSLATE_NOOP("Units", "m/s"),
0047                     QT_TRANSLATE_NOOP("PropertyDescription", "velocity variance"), velocityVariance, setVelocityVariance)
0048         STEPCORE_PROPERTY_RW_D(double, angularVelocityVariance, QT_TRANSLATE_NOOP("PropertyName", "angularVelocityVariance"), QT_TRANSLATE_NOOP("Units", "rad/s"),
0049                     QT_TRANSLATE_NOOP("PropertyDescription", "angularVelocity variance"), angularVelocityVariance, setAngularVelocityVariance)
0050
0051         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, accelerationVariance, QT_TRANSLATE_NOOP("PropertyName", "accelerationVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
0052                     QT_TRANSLATE_NOOP("PropertyDescription", "acceleration variance"), accelerationVariance)
0053         STEPCORE_PROPERTY_R_D(double, angularAccelerationVariance, QT_TRANSLATE_NOOP("PropertyName", "angularAccelerationVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "rad/s²")),
0054                     QT_TRANSLATE_NOOP("PropertyDescription", "angularAcceleration variance"), angularAccelerationVariance)
0055
0056         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, forceVariance, QT_TRANSLATE_NOOP("PropertyName", "forceVariance"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "force variance"), forceVariance)
0057         STEPCORE_PROPERTY_R_D(double, torqueVariance, QT_TRANSLATE_NOOP("PropertyName", "torqueVariance"), QT_TRANSLATE_NOOP("Units", "N m"), QT_TRANSLATE_NOOP("PropertyDescription", "torque variance"), torqueVariance)
0058
0059         STEPCORE_PROPERTY_RW(double, massVariance, QT_TRANSLATE_NOOP("PropertyName", "massVariance"), QT_TRANSLATE_NOOP("Units", "kg"),
0060                     QT_TRANSLATE_NOOP("PropertyDescription", "mass variance"), massVariance, setMassVariance )
0061         STEPCORE_PROPERTY_RW(double, inertiaVariance, QT_TRANSLATE_NOOP("PropertyName", "inertiaVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m²")),
0062                     QT_TRANSLATE_NOOP("PropertyDescription", "inertia variance"), inertiaVariance, setInertiaVariance )
0063         STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentumVariance, QT_TRANSLATE_NOOP("PropertyName", "momentumVariance"), QT_TRANSLATE_NOOP("Units", "kg m/s"),
0064                     QT_TRANSLATE_NOOP("PropertyDescription", "momentum variance"), StepCore::MetaProperty::DYNAMIC, momentumVariance, setMomentumVariance)
0065         STEPCORE_PROPERTY_RWF(double, angularMomentumVariance, QT_TRANSLATE_NOOP("PropertyName", "angularMomentumVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m² rad/s")),
0066                     QT_TRANSLATE_NOOP("PropertyDescription", "angular momentum variance"), StepCore::MetaProperty::DYNAMIC,
0067                     angularMomentumVariance, setAngularMomentumVariance)
0068         STEPCORE_PROPERTY_RWF(double, kineticEnergyVariance, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergyVariance"), QT_TRANSLATE_NOOP("Units", "J"),
0069                     QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy variance"), StepCore::MetaProperty::DYNAMIC, kineticEnergyVariance, setKineticEnergyVariance))
0070
0071 STEPCORE_META_OBJECT(Disk, QT_TRANSLATE_NOOP("ObjectClass", "Disk"), QT_TRANSLATE_NOOP("ObjectDescription", "Rigid disk"), 0, STEPCORE_SUPER_CLASS(RigidBody),
0072         STEPCORE_PROPERTY_RW(double, radius, QT_TRANSLATE_NOOP("PropertyName", "radius"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Radius of the disk"), radius, setRadius))
0073
0074 STEPCORE_META_OBJECT(BasePolygon, QT_TRANSLATE_NOOP("ObjectClass", "BasePolygon"), QT_TRANSLATE_NOOP("ObjectDescription", "Base polygon body"), 0, STEPCORE_SUPER_CLASS(RigidBody),)
0075
0076 STEPCORE_META_OBJECT(Box, QT_TRANSLATE_NOOP("ObjectClass", "Box"), QT_TRANSLATE_NOOP("ObjectDescription", "Rigid box"), 0, STEPCORE_SUPER_CLASS(BasePolygon),
0077         STEPCORE_PROPERTY_RW(StepCore::Vector2d, size, QT_TRANSLATE_NOOP("PropertyName", "size"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Size of the box"), size, setSize))
0078
0079 STEPCORE_META_OBJECT(Polygon, QT_TRANSLATE_NOOP("ObjectClass", "Polygon"), QT_TRANSLATE_NOOP("ObjectDescription", "Rigid polygon body"), 0, STEPCORE_SUPER_CLASS(BasePolygon),
0080         STEPCORE_PROPERTY_RW(Vector2dList, vertexes, QT_TRANSLATE_NOOP("PropertyName", "vertices"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Vertex list"), vertexes, setVertexes))
0081
0082 RigidBody* RigidBodyErrors::rigidBody() const
0083 {
0084     return static_cast<RigidBody*>(owner());
0085 }
0086
0087 Vector2d RigidBodyErrors::accelerationVariance() const
0088 {
0089     return _forceVariance/square(rigidBody()->mass()) +
0090         _massVariance*(rigidBody()->force()/square(rigidBody()->mass())).array().square().matrix();
0091 }
0092
0093 double RigidBodyErrors::angularAccelerationVariance() const
0094 {
0095     return _torqueVariance/square(rigidBody()->inertia()) +
0096         _inertiaVariance*square(rigidBody()->torque()/square(rigidBody()->inertia()));
0097 }
0098
0099 Vector2d RigidBodyErrors::momentumVariance() const
0100 {
0101     return _velocityVariance * square(rigidBody()->mass()) +
0102            rigidBody()->velocity().array().square().matrix() * _massVariance;
0103 }
0104
0105 void RigidBodyErrors::setMomentumVariance(const Vector2d& momentumVariance)
0106 {
0107     _velocityVariance = (momentumVariance - rigidBody()->velocity().array().square().matrix() * _massVariance) /
0108                         square(rigidBody()->mass());
0109 }
0110
0111 double RigidBodyErrors::angularMomentumVariance() const
0112 {
0113     return _angularVelocityVariance * square(rigidBody()->inertia()) +
0114            square(rigidBody()->angularVelocity()) * _inertiaVariance;
0115 }
0116
0117 void RigidBodyErrors::setAngularMomentumVariance(double angularMomentumVariance)
0118 {
0119     _angularVelocityVariance =
0120         (angularMomentumVariance - square(rigidBody()->angularVelocity()) * _inertiaVariance) /
0121                         square(rigidBody()->inertia());
0122 }
0123
0124 double RigidBodyErrors::kineticEnergyVariance() const
0125 {
0126     return (rigidBody()->velocity().array().square().matrix()).dot(_velocityVariance) * square(rigidBody()->mass()) +
0127            square(rigidBody()->velocity().squaredNorm()/2) * _massVariance +
0128            _angularVelocityVariance * square(rigidBody()->angularVelocity() * rigidBody()->inertia()) +
0129            square(square(rigidBody()->angularVelocity())/2) * _inertiaVariance;
0130 }
0131
0132 void RigidBodyErrors::setKineticEnergyVariance(double kineticEnergyVariance)
0133 {
0134     double t = kineticEnergyVariance - this->kineticEnergyVariance() +
0135               (rigidBody()->velocity().array().square().matrix()).dot(_velocityVariance) * square(rigidBody()->mass());
0136     _velocityVariance = t / square(rigidBody()->mass()) / 2 *
0137                         (rigidBody()->velocity().array().square().inverse().matrix());
0138     if(!std::isfinite(_velocityVariance[0]) || _velocityVariance[0] < 0 ||
0139        !std::isfinite(_velocityVariance[1]) || _velocityVariance[1]) {
0140         _velocityVariance.setZero();
0141     }
0142     // XXX: change angularVelocity here as well
0143 }
0144
0145 RigidBody::RigidBody(const Vector2d &position, double angle,
0146         const Vector2d &velocity, double angularVelocity, double mass, double inertia)
0147     : _position(position), _angle(angle), _velocity(velocity), _angularVelocity(angularVelocity),
0148       _force(Vector2d::Zero()), _torque(0), _mass(mass), _inertia(inertia)
0149 {
0150 }
0151
0152 void RigidBody::applyForce(const Vector2d& force, const Vector2d& position)
0153 {
0154     _force += force;
0155     _torque += (position[0] - _position[0])*force[1] -
0156                (position[1] - _position[1])*force[0]; // XXX: sign ?
0157 }
0158
0159 void RigidBodyErrors::applyForceVariance(const Vector2d& force,
0160                                          const Vector2d& position,
0161                                          const Vector2d& forceVariance,
0162                                          const Vector2d& positionVariance)
0163 {
0164     _forceVariance += forceVariance;
0165     _torqueVariance += forceVariance[1] * square(position[0] - rigidBody()->_position[0]) +
0166                        forceVariance[0] * square(position[1] - rigidBody()->_position[1]) +
0167                        (positionVariance[0] + _positionVariance[0]) * square(force[1]) +
0168                        (positionVariance[1] + _positionVariance[1]) * square(force[0]);
0169 }
0170
0171 Vector2d RigidBody::velocityWorld(const Vector2d& worldPoint) const
0172 {
0173     Vector2d p = (worldPoint - _position)*_angularVelocity;
0174     return _velocity + Vector2d(-p[1], p[0]);
0175 }
0176
0177 Vector2d RigidBody::velocityLocal(const Vector2d& localPoint) const
0178 {
0179     Vector2d p = vectorLocalToWorld(localPoint)*_angularVelocity;
0180     return _velocity + Vector2d(-p[1], p[0]);
0181 }
0182
0183 Vector2d RigidBody::pointLocalToWorld(const Vector2d& p) const
0184 {
0185     double c = cos(_angle);
0186     double s = sin(_angle);
0187     return Vector2d( p[0]*c - p[1]*s + _position[0],
0188                      p[0]*s + p[1]*c + _position[1]);
0189 }
0190
0191 Vector2d RigidBody::pointWorldToLocal(const Vector2d& p) const
0192 {
0193     double c = cos(_angle);
0194     double s = sin(_angle);
0195     return Vector2d( (p[0]-_position[0])*c + (p[1]-_position[1])*s,
0196                     -(p[0]-_position[0])*s + (p[1]-_position[1])*c);
0197 }
0198
0199 Vector2d RigidBody::vectorLocalToWorld(const Vector2d& v) const
0200 {
0201     double c = cos(_angle);
0202     double s = sin(_angle);
0203     return Vector2d( v[0]*c - v[1]*s,
0204                      v[0]*s + v[1]*c);
0205 }
0206
0207 Vector2d RigidBody::vectorWorldToLocal(const Vector2d& v) const
0208 {
0209     double c = cos(_angle);
0210     double s = sin(_angle);
0211     return Vector2d( v[0]*c + v[1]*s,
0212                     -v[0]*s + v[1]*c);
0213 }
0214
0215 void RigidBody::getVariables(double* position, double* velocity,
0216                      double* positionVariance, double* velocityVariance)
0217 {
0218     Vector2d::Map(position) = _position;
0219     Vector2d::Map(velocity) = _velocity;
0220     position[2] = _angle;
0221     velocity[2] = _angularVelocity;
0222
0223     if(positionVariance) {
0224         RigidBodyErrors* re = rigidBodyErrors();
0225         Vector2d::Map(positionVariance) = re->_positionVariance;
0226         Vector2d::Map(velocityVariance) = re->_velocityVariance;
0227         positionVariance[2] = re->_angleVariance;
0228         velocityVariance[2] = re->_angularVelocityVariance;
0229     }
0230 }
0231
0232 void RigidBody::setVariables(const double* position, const double* velocity,
0233                const double* positionVariance, const double* velocityVariance)
0234 {
0235     _position = Vector2d::Map(position);
0236     _velocity = Vector2d::Map(velocity);
0237     _angle = position[2];
0238     _angularVelocity = velocity[2];
0239
0240     _force.setZero();
0241     _torque = 0;
0242
0243     if(positionVariance) {
0244         RigidBodyErrors* re = rigidBodyErrors();
0245         re->_positionVariance = Vector2d::Map(positionVariance);
0246         re->_velocityVariance = Vector2d::Map(velocityVariance);
0247         re->_angleVariance = positionVariance[2];
0248         re->_angularVelocityVariance = velocityVariance[2];
0249
0250         re->_forceVariance.setZero();
0251         re->_torqueVariance = 0;
0252     }
0253 }
0254
0255 void RigidBody::getAccelerations(double* acceleration, double* accelerationVariance)
0256 {
0257     acceleration[0] = _force[0] / _mass;
0258     acceleration[1] = _force[1] / _mass;
0259     acceleration[2] = _torque / _inertia;
0260     if(accelerationVariance) {
0261         RigidBodyErrors* re = rigidBodyErrors();
0262         accelerationVariance[0] = re->_forceVariance[0]/square(_mass) +
0263                                         square(_force[0]/square(_mass))*re->_massVariance;
0264         accelerationVariance[1] = re->_forceVariance[1]/square(_mass) +
0265                                         square(_force[1]/square(_mass))*re->_massVariance;
0266         accelerationVariance[2] = re->_torqueVariance/square(_inertia) +
0267                                         square(_torque/square(_inertia))*re->_inertiaVariance;
0268     }
0269 }
0270
0271 void RigidBody::addForce(const double* force, const double* forceVariance)
0272 {
0273     _force[0] += force[0];
0274     _force[1] += force[1];
0275     _torque += force[2];
0276     if(forceVariance) {
0277         RigidBodyErrors* re = rigidBodyErrors();
0278         re->_forceVariance[0] += forceVariance[0];
0279         re->_forceVariance[1] += forceVariance[1];
0280         re->_torqueVariance += forceVariance[2];
0281     }
0282 }
0283
0284 void RigidBody::resetForce(bool resetVariance)
0285 {
0286     _force.setZero();
0287     _torque = 0;
0288     if(resetVariance) {
0289         RigidBodyErrors* re = rigidBodyErrors();
0290         re->_forceVariance.setZero();
0291         re->_torqueVariance = 0;
0292     }
0293 }
0294
0295 void RigidBody::getInverseMass(VectorXd* inverseMass,
0296                                DynSparseRowMatrix* variance, int offset)
0297 {
0298     inverseMass->coeffRef(offset) = (1/_mass);
0299     inverseMass->coeffRef(offset+1) = (1/_mass);
0300     inverseMass->coeffRef(offset+2) = (1/_inertia);
0301     if(variance) {
0302         RigidBodyErrors* re = rigidBodyErrors();
0303         double vm = re->_massVariance / square(square(_mass));
0304         double vi = re->_inertiaVariance /  square(square(_inertia));
0305         variance->coeffRef(offset, offset) = ( vm);
0306         variance->coeffRef(offset+1, offset+1) = ( vm);
0307         variance->coeffRef(offset+2, offset+2) = ( vi);
0308     }
0309 }
0310
0311 void RigidBody::setKineticEnergy(double kineticEnergy)
0312 {
0313     double e = kineticEnergy - _inertia * square(_angularVelocity)/2;
0314     if(e > 0) {
0315         double v = _velocity.norm();
0316         _velocity = sqrt(e*2/_mass) * (v>0 ? (_velocity/v).eval() : Vector2d(1,0));
0317     } else {
0318         _velocity.setZero();
0319         _angularVelocity = sqrt(kineticEnergy*2/_inertia);
0320     }
0321 }
0322
0323 Box::Box(const Vector2d &position, double angle,
0324               const Vector2d &velocity, double angularVelocity,
0325               double mass, double inertia, const Vector2d &size)
0326     : BasePolygon(position, angle, velocity, angularVelocity, mass, inertia)
0327 {
0328     _vertexes.resize(4);
0329     setSize(size);
0330 }
0331
0332 void Box::setSize(const Vector2d& size)
0333 {
0334     Vector2d s(size.array().abs().matrix()/2.0);
0335
0336     _vertexes[0] << -s[0], -s[1];
0337     _vertexes[1] <<  s[0], -s[1];
0338     _vertexes[2] <<  s[0],  s[1];
0339     _vertexes[3] << -s[0],  s[1];
0340 }
0341
0342 } // namespace StepCore
0343