File indexing completed on 2024-04-21 07:38:04

0001 /*
0002     SPDX-FileCopyrightText: 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
0003 
0004     SPDX-License-Identifier: GPL-2.0-or-later
0005 */
0006 
0007 /** \file rigidbody.h
0008  *  \brief RigidBody class
0009  */
0010 
0011 #ifndef STEPCORE_RIGIDBODY_H
0012 #define STEPCORE_RIGIDBODY_H
0013 
0014 #include "world.h"
0015 #include "vector.h"
0016 #include "object.h"
0017 
0018 namespace StepCore {
0019 
0020 class RigidBody;
0021 
0022 /** \ingroup errors
0023  *  \brief Errors object for RigidBody
0024  */
0025 class RigidBodyErrors: public ObjectErrors
0026 {
0027     STEPCORE_OBJECT(RigidBodyErrors)
0028 
0029 public:
0030     /** Constructs RigidBodyErrors */
0031     explicit RigidBodyErrors(Item* owner = nullptr)
0032         : ObjectErrors(owner), _positionVariance(0,0), _angleVariance(0), _velocityVariance(0,0),
0033           _angularVelocityVariance(0), _forceVariance(0,0), _torqueVariance(0),
0034           _massVariance(0), _inertiaVariance(0) {}
0035 
0036     /** Get owner as RigidBody */
0037     RigidBody* rigidBody() const;
0038 
0039     /** Get position variance */
0040     const Vector2d& positionVariance() const { return _positionVariance; }
0041     /** Set position variance */
0042     void setPositionVariance(const Vector2d& positionVariance) {
0043         _positionVariance = positionVariance; }
0044 
0045     /** Get angle variance */
0046     double angleVariance() const { return _angleVariance; }
0047     /** Set angle variance */
0048     void setAngleVariance(double angleVariance) { _angleVariance = angleVariance; }
0049 
0050     /** Get velocity variance */
0051     const Vector2d& velocityVariance() const { return _velocityVariance; }
0052     /** Set velocity variance */
0053     void setVelocityVariance(const Vector2d& velocityVariance) {
0054         _velocityVariance = velocityVariance; }
0055 
0056     /** Get angularVelocity variance */
0057     double angularVelocityVariance() const { return _angularVelocityVariance; }
0058     /** Set angularVelocity variance */
0059     void setAngularVelocityVariance(double angularVelocityVariance) {
0060         _angularVelocityVariance = angularVelocityVariance; }
0061 
0062     /** Get acceleration variance */
0063     Vector2d accelerationVariance() const;
0064 
0065     /** Get angularAcceleration variance */
0066     double angularAccelerationVariance() const;
0067 
0068     /** Get force variance */
0069     const Vector2d& forceVariance() const { return _forceVariance; }
0070     /** Set force variance */
0071     void setForceVariance(const Vector2d& forceVariance) {
0072         _forceVariance = forceVariance; }
0073 
0074     /** Get torque variance */
0075     double torqueVariance() const { return _torqueVariance; }
0076 
0077     /** Apply force (and torque) variance to the body at given position (in World coordinates) */
0078     void applyForceVariance(const Vector2d& force,
0079                             const Vector2d& position,
0080                             const Vector2d& forceVariance,
0081                             const Vector2d& positionVariance);
0082 
0083     /** Apply torque (but no force) variance to the body */
0084     void applyTorqueVariance(double torqueVariance) { _torqueVariance += torqueVariance; }
0085 
0086     /** Get mass variance */
0087     double massVariance() const { return _massVariance; }
0088     /** Set mass variance */
0089     void   setMassVariance(double massVariance) {
0090         _massVariance = massVariance; }
0091 
0092     /** Get inertia variance */
0093     double inertiaVariance() const { return _inertiaVariance; }
0094     /** Set inertia variance */
0095     void   setInertiaVariance(double inertiaVariance) {
0096         _inertiaVariance = inertiaVariance; }
0097 
0098     /** Get momentum variance */
0099     Vector2d momentumVariance() const;
0100     /** Set momentum variance (will modify velocity variance) */
0101     void setMomentumVariance(const Vector2d& momentumVariance);
0102 
0103     /** Get angular momentum variance */
0104     double angularMomentumVariance() const;
0105     /** Set angular momentum variance (will modify angularVelocity variance) */
0106     void setAngularMomentumVariance(double angularMomentumVariance);
0107 
0108     /** Get kinetic energy variance */
0109     double kineticEnergyVariance() const;
0110     /** Set kinetic energy variance (will modify velocity variance) */
0111     void setKineticEnergyVariance(double kineticEnergyVariance);
0112 
0113 protected:
0114     Vector2d _positionVariance;
0115     double   _angleVariance;
0116 
0117     Vector2d _velocityVariance;
0118     double   _angularVelocityVariance;
0119 
0120     Vector2d _forceVariance;
0121     double   _torqueVariance;
0122 
0123     double _massVariance;
0124     double _inertiaVariance;
0125 
0126     friend class RigidBody;
0127 };
0128 
0129 /** \ingroup bodies
0130  *  \brief Rigid body
0131  */
0132 class RigidBody: public Body
0133 {
0134     STEPCORE_OBJECT(RigidBody)
0135 
0136 public:
0137     enum {
0138         PositionOffset = 0, ///< Offset of body position in variables array
0139         AngleOffset = 2     ///< Offset of body angle in variables array
0140     };
0141 
0142     /** Constructs RigidBody */
0143     explicit RigidBody(const Vector2d &position = Vector2d::Zero(), double angle = 0,
0144               const Vector2d &velocity = Vector2d::Zero(), double angularVelocity = 0,
0145               double mass = 1, double inertia = 1);
0146 
0147     /** Get position of the center of mass of the body  */
0148     const Vector2d& position() const { return _position; }
0149     /** Set position of the center of mass of the body */
0150     void setPosition(const Vector2d& position) { _position = position; }
0151 
0152     /** Get angle of the body */
0153     double angle() const { return _angle; }
0154     /** Set angle of the body */
0155     void setAngle(double angle) { _angle = angle; }
0156 
0157     /** Get velocity of the center of mass of the body */
0158     const Vector2d& velocity() const { return _velocity; }
0159     /** Set velocity of the particle */
0160     void setVelocity(const Vector2d& velocity) { _velocity = velocity; }
0161     /** Get velocity of given (world) point on the body */
0162     Vector2d velocityWorld(const Vector2d& worldPoint) const;
0163     /** Get velocity of given (local) point on the body */
0164     Vector2d velocityLocal(const Vector2d& localPoint) const;
0165 
0166     /** Get angular velocity of the body */
0167     double angularVelocity() const { return _angularVelocity; }
0168     /** Set angular velocity of the body */
0169     void setAngularVelocity(double angularVelocity) { _angularVelocity = angularVelocity; }
0170 
0171     /** Get acceleration of the center of mass of the body */
0172     Vector2d acceleration() const { return _force/_mass; }
0173 
0174     /** Get angular acceleration of the body */
0175     double angularAcceleration() const { return _torque/_inertia; }
0176 
0177     /** Get force that acts upon the body */
0178     const Vector2d& force() const { return _force; }
0179     /** Set force that acts upon the body */
0180     void setForce(const Vector2d& force) { _force = force; }
0181 
0182     /** Get torque that acts upon the body */
0183     double torque() const { return _torque; }
0184     /** Set torque that acts upon the body */
0185     void setTorque(double torque) { _torque = torque; }
0186 
0187     //void applyForceLocal(const Vector2d& localPosition = Vector2d(0,0));
0188 
0189     /** Apply force (and torque) to the body at given position (in World coordinates) */
0190     void applyForce(const Vector2d& force, const Vector2d& position);
0191 
0192     /** Apply torque (but no force) to the body */
0193     void applyTorque(double torque) { _torque += torque; }
0194 
0195     /** Get mass of the body */
0196     double mass() const { return _mass; }
0197     /** Set mass of the body */
0198     void   setMass(double mass) { _mass = mass; }
0199 
0200     /** Get inertia "tensor" of the body */
0201     double inertia() const { return _inertia; }
0202     /** Set inertia "tensor" of the body */
0203     void   setInertia(double inertia) { _inertia = inertia; }
0204 
0205     /** Get momentum of the body */
0206     Vector2d momentum() const { return _velocity * _mass; }
0207     /** Set momentum of the body (will modify only velocity) */
0208     void setMomentum(const Vector2d& momentum) { _velocity = momentum / _mass; }
0209 
0210     /** Get angular momentum of the body */
0211     double angularMomentum() const { return _angularVelocity * _inertia; }
0212     /** Set angular momentum of the body (will modify only angularVelocity) */
0213     void setAngularMomentum(double angularMomentum) {
0214         _angularVelocity = angularMomentum / _inertia; }
0215 
0216     /** Get kinetic energy of the body */
0217     double kineticEnergy() const {
0218         return _mass * _velocity.squaredNorm()/2 + _inertia * square(_angularVelocity)/2; }
0219     /** Set kinetic energy of the body (will modify only velocity and (possibly) angularVelocity) */
0220     void setKineticEnergy(double kineticEnergy);
0221 
0222     /** Translate local vector on body to world vector */
0223     Vector2d vectorLocalToWorld(const Vector2d& v) const;
0224     /** Translate world vector to local vector on body */
0225     Vector2d vectorWorldToLocal(const Vector2d& v) const;
0226 
0227     /** Translate local coordinates on body to world coordinates */
0228     Vector2d pointLocalToWorld(const Vector2d& p) const;
0229     /** Translate world coordinates to local coordinates on body */
0230     Vector2d pointWorldToLocal(const Vector2d& p) const;
0231 
0232     //---------- Integration over body
0233 
0234     //---------- Shape
0235     // XXX
0236     //const Vector2dList& vertexes() const;
0237 
0238     int  variablesCount() override { return 3; }
0239     void getVariables(double* position, double* velocity,
0240                           double* positionVariance, double* velocityVariance) override;
0241     void setVariables(const double* position, const double* velocity,
0242               const double* positionVariance, const double* velocityVariance) override;
0243     void addForce(const double* force, const double* forceVariance) override;
0244     void resetForce(bool resetVariance) override;
0245     void getAccelerations(double* acceleration, double* accelerationVariance) override;
0246     void getInverseMass(VectorXd* inverseMass,
0247                         DynSparseRowMatrix* variance, int offset) override;
0248     /** Get (and possibly create) RigidBodyErrors object */
0249     RigidBodyErrors* rigidBodyErrors() {
0250         return static_cast<RigidBodyErrors*>(objectErrors()); }
0251 
0252 protected:
0253     ObjectErrors* createObjectErrors() override { return new RigidBodyErrors(this); }
0254 
0255     Vector2d _position;
0256     double   _angle;
0257 
0258     Vector2d _velocity;
0259     double   _angularVelocity;
0260 
0261     Vector2d _force;
0262     double   _torque;
0263 
0264     double   _mass;
0265     double   _inertia;
0266 
0267     friend class RigidBodyErrors;
0268 };
0269 
0270 /** \ingroup bodies
0271  *  \brief Rigid disk
0272  */
0273 class Disk: public RigidBody
0274 {
0275     STEPCORE_OBJECT(Disk)
0276 public:
0277     /** Constructs Disk */
0278     explicit Disk(const Vector2d &position = Vector2d::Zero(), double angle = 0,
0279               const Vector2d &velocity = Vector2d::Zero(), double angularVelocity = 0,
0280               double mass = 1, double inertia = 1, double radius = 0.5)
0281         : RigidBody(position, angle, velocity, angularVelocity, mass, inertia),
0282           _radius(radius) {}
0283 
0284     /** Get disk radius */
0285     double radius() const { return _radius; }
0286     /** Set disk radius */
0287     void setRadius(double radius) { _radius = radius; }
0288 
0289 protected:
0290     double _radius;
0291 };
0292 
0293 /** \ingroup bodies
0294  *  \brief Base class for all polygons
0295  */
0296 class BasePolygon: public RigidBody
0297 {
0298     STEPCORE_OBJECT(BasePolygon)
0299 public:
0300     /** Constructs BasePolygon */
0301     explicit BasePolygon(const Vector2d &position = Vector2d::Zero(), double angle = 0,
0302               const Vector2d &velocity = Vector2d::Zero(), double angularVelocity = 0,
0303               double mass = 1, double inertia = 1)
0304         : RigidBody(position, angle, velocity, angularVelocity, mass, inertia) {}
0305 
0306     /** Get vertex list (constant) */
0307     const Vector2dList& vertexes() const { return _vertexes; }
0308 
0309 protected:
0310     Vector2dList _vertexes;
0311 };
0312 
0313 class Box: public BasePolygon
0314 {
0315     STEPCORE_OBJECT(Box)
0316 public:
0317     /** Constructs Box */
0318     explicit Box(const Vector2d &position = Vector2d::Zero(), double angle = 0,
0319               const Vector2d &velocity = Vector2d::Zero(), double angularVelocity = 0,
0320               double mass = 1, double inertia = 1, const Vector2d &size = Vector2d(1,1));
0321 
0322     /** Get box size */
0323     Vector2d size() const { return Vector2d(_vertexes[1][0] - _vertexes[0][0],
0324                                             _vertexes[3][1] - _vertexes[0][1]); }
0325     /** Set box size */
0326     void setSize(const Vector2d& size);
0327 };
0328 
0329 /** \ingroup bodies
0330  *  \brief Rigid arbitrary-shaped polygon
0331  */
0332 class Polygon: public BasePolygon
0333 {
0334     STEPCORE_OBJECT(Polygon)
0335 
0336 public:
0337     /** Get vertex list (constant) */
0338     const Vector2dList& vertexes() const { return _vertexes; }
0339     /** Get vertex list (editable) */
0340     Vector2dList& vertexes() { return _vertexes; }
0341     /** Set vertex list */
0342     void setVertexes(const Vector2dList& vertexes) { _vertexes = vertexes; }
0343 };
0344 
0345 #if 0
0346 /** \ingroup bodies
0347  *  \brief Unmovable rigid plane
0348  */
0349 class Plane: public Body
0350 {
0351     STEPCORE_OBJECT(Plane)
0352 
0353 public:
0354     /** Constructs a plane defined by two points */
0355     explicit Plane(Vector2d point1 = Vector2d::Zero(), Vector2d point2 = Vector2d(1,0))
0356         : _point1(point1), _point2(point2) {}
0357 
0358     /** Get first point */
0359     const Vector2d& point1() const { return _point1; }
0360     /** Set first point */
0361     void setPoint1(const Vector2d& point1) { _point1 = point1; }
0362 
0363     /** Get second point */
0364     const Vector2d& point2() const { return _point2; }
0365     /** Set second point */
0366     void setPoint2(const Vector2d& point2) { _point2 = point2; }
0367 
0368 protected:
0369     Vector2d _point1;
0370     Vector2d _point2;
0371 };
0372 #endif
0373 
0374 } // namespace StepCore
0375 
0376 #endif
0377