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 "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