rigidbody.h 13.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
/* This file is part of StepCore library.
   Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>

   StepCore library is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   StepCore library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with StepCore; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

/** \file rigidbody.h
 *  \brief RigidBody class
 */

#ifndef STEPCORE_RIGIDBODY_H
#define STEPCORE_RIGIDBODY_H

#include "world.h"
#include "vector.h"
#include "object.h"

namespace StepCore {

32 33 34 35 36 37 38 39 40 41 42 43
class RigidBody;

/** \ingroup errors
 *  \brief Errors object for RigidBody
 */
class RigidBodyErrors: public ObjectErrors
{
    STEPCORE_OBJECT(RigidBodyErrors)

public:
    /** Constructs RigidBodyErrors */
    RigidBodyErrors(Item* owner = 0)
44
        : ObjectErrors(owner), _positionVariance(0,0), _angleVariance(0), _velocityVariance(0,0),
45
          _angularVelocityVariance(0), _forceVariance(0,0), _torqueVariance(0),
46
          _massVariance(0), _inertiaVariance(0) {}
47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109

    /** Get owner as RigidBody */
    RigidBody* rigidBody() const;

    /** Get position variance */
    const Vector2d& positionVariance() const { return _positionVariance; }
    /** Set position variance */
    void setPositionVariance(const Vector2d& positionVariance) {
        _positionVariance = positionVariance; }

    /** Get angle variance */
    double angleVariance() const { return _angleVariance; }
    /** Set angle variance */
    void setAngleVariance(double angleVariance) { _angleVariance = angleVariance; }

    /** Get velocity variance */
    const Vector2d& velocityVariance() const { return _velocityVariance; }
    /** Set velocity variance */
    void setVelocityVariance(const Vector2d& velocityVariance) {
        _velocityVariance = velocityVariance; }

    /** Get angularVelocity variance */
    double angularVelocityVariance() const { return _angularVelocityVariance; }
    /** Set angularVelocity variance */
    void setAngularVelocityVariance(double angularVelocityVariance) {
        _angularVelocityVariance = angularVelocityVariance; }

    /** Get acceleration variance */
    Vector2d accelerationVariance() const;

    /** Get angularAcceleration variance */
    double angularAccelerationVariance() const;

    /** Get force variance */
    const Vector2d& forceVariance() const { return _forceVariance; }
    /** Set force variance */
    void setForceVariance(const Vector2d& forceVariance) {
        _forceVariance = forceVariance; }

    /** Get torque variance */
    double torqueVariance() const { return _torqueVariance; }

    /** Apply force (and torque) variance to the body at given position (in World coordinates) */
    void applyForceVariance(const Vector2d& force,
                            const Vector2d& position,
                            const Vector2d& forceVariance,
                            const Vector2d& positionVariance);

    /** Apply torque (but no force) variancee to the body */
    void applyTorqueVariance(double torqueVariance) { _torqueVariance += torqueVariance; }

    /** Get mass variance */
    double massVariance() const { return _massVariance; }
    /** Set mass variance */
    void   setMassVariance(double massVariance) {
        _massVariance = massVariance; }

    /** Get inertia variance */
    double inertiaVariance() const { return _inertiaVariance; }
    /** Set inertia variance */
    void   setInertiaVariance(double inertiaVariance) {
        _inertiaVariance = inertiaVariance; }

110 111 112 113 114 115 116 117 118 119 120 121 122 123 124
    /** Get momentum variance */
    Vector2d momentumVariance() const;
    /** Set momentum variance (will modify velocity variance) */
    void setMomentumVariance(const Vector2d& momentumVariance);

    /** Get angular momentum variance */
    double angularMomentumVariance() const;
    /** Set angular momentum variance (will modify angularVelocity variance) */
    void setAngularMomentumVariance(double angularMomentumVariance);

    /** Get kinetic energy variance */
    double kineticEnergyVariance() const;
    /** Set kinetic energy variance (will modify velocity variance) */
    void setKineticEnergyVariance(double kineticEnergyVariance);

125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
protected:
    Vector2d _positionVariance;
    double   _angleVariance;

    Vector2d _velocityVariance;
    double   _angularVelocityVariance;

    Vector2d _forceVariance;
    double   _torqueVariance;

    double _massVariance;
    double _inertiaVariance;

    friend class RigidBody;
};

Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
141 142 143
/** \ingroup bodies
 *  \brief Rigid body
 */
Inge Wallin's avatar
Inge Wallin committed
144
class RigidBody: public Body
145 146 147 148
{
    STEPCORE_OBJECT(RigidBody)

public:
149 150 151 152 153
    enum {
        PositionOffset = 0, ///< Offset of body position in variables array
        AngleOffset = 2     ///< Offset of body angle in variables array
    };

154
    /** Constructs RigidBody */
155 156
    explicit RigidBody(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172
              double mass = 1, double inertia = 1);

    /** Get position of the center of mass of the body  */
    const Vector2d& position() const { return _position; }
    /** Set position of the center of mass of the body */
    void setPosition(const Vector2d& position) { _position = position; }

    /** Get angle of the body */
    double angle() const { return _angle; }
    /** Set angle of the body */
    void setAngle(double angle) { _angle = angle; }

    /** Get velocity of the center of mass of the body */
    const Vector2d& velocity() const { return _velocity; }
    /** Set velocity of the particle */
    void setVelocity(const Vector2d& velocity) { _velocity = velocity; }
173 174
    /** Get velocity of given (world) point on the body */
    Vector2d velocityWorld(const Vector2d& worldPoint) const;
175 176
    /** Get velocity of given (local) point on the body */
    Vector2d velocityLocal(const Vector2d& localPoint) const;
177

Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
178
    /** Get angular velocity of the body */
179
    double angularVelocity() const { return _angularVelocity; }
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
180
    /** Set angular velocity of the body */
181 182
    void setAngularVelocity(double angularVelocity) { _angularVelocity = angularVelocity; }

183 184 185 186 187 188
    /** Get acceleration of the center of mass of the body */
    Vector2d acceleration() const { return _force/_mass; }

    /** Get angular acceleration of the body */
    double angularAcceleration() const { return _torque/_inertia; }

189 190 191 192 193 194 195 196 197 198 199
    /** Get force that acts upon the body */
    const Vector2d& force() const { return _force; }
    /** Set force that acts upon the body */
    void setForce(const Vector2d& force) { _force = force; }

    /** Get torque that acts upon the body */
    double torque() const { return _torque; }
    /** Set torque that acts upon the body */
    void setTorque(double torque) { _torque = torque; }

    //void applyForceLocal(const Vector2d& localPosition = Vector2d(0,0));
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
200

201
    /** Apply force (and torque) to the body at given position (in World coordinates) */
202
    void applyForce(const Vector2d& force, const Vector2d& position);
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
203

204
    /** Apply torque (but no force) to the body */
205
    void applyTorque(double torque) { _torque += torque; }
206 207 208 209 210 211

    /** Get mass of the body */
    double mass() const { return _mass; }
    /** Set mass of the body */
    void   setMass(double mass) { _mass = mass; }

Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
212
    /** Get inertia "tensor" of the body */
213
    double inertia() const { return _inertia; }
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
214
    /** Set inertia "tensor" of the body */
215 216
    void   setInertia(double inertia) { _inertia = inertia; }

217 218 219 220 221 222 223 224 225 226 227 228 229
    /** Get momentum of the body */
    Vector2d momentum() const { return _velocity * _mass; }
    /** Set momentum of the body (will modify only velocity) */
    void setMomentum(const Vector2d& momentum) { _velocity = momentum / _mass; }

    /** Get angular momentum of the body */
    double angularMomentum() const { return _angularVelocity * _inertia; }
    /** Set angular momentum of the body (will modify only angularVelocity) */
    void setAngularMomentum(double angularMomentum) {
        _angularVelocity = angularMomentum / _inertia; }

    /** Get kinetic energy of the body */
    double kineticEnergy() const {
230
        return _mass * _velocity.squaredNorm()/2 + _inertia * square(_angularVelocity)/2; }
231 232 233
    /** Set kinetic energy of the body (will modify only velocity and (possibly) angularVelocity) */
    void setKineticEnergy(double kineticEnergy);

234
    /** Translate local vector on body to world vector */
235
    Vector2d vectorLocalToWorld(const Vector2d& v) const;
236
    /** Translate world vector to local vector on body */
237
    Vector2d vectorWorldToLocal(const Vector2d& v) const;
238 239

    /** Translate local coordinates on body to world coordinates */
240
    Vector2d pointLocalToWorld(const Vector2d& p) const;
241
    /** Translate world coordinates to local coordinates on body */
242
    Vector2d pointWorldToLocal(const Vector2d& p) const;
243 244 245 246 247

    //---------- Integration over body

    //---------- Shape
    // XXX
248
    //const Vector2dList& vertexes() const;
249

250 251 252 253 254
    int  variablesCount() { return 3; }
    void getVariables(double* position, double* velocity,
                          double* positionVariance, double* velocityVariance);
    void setVariables(const double* position, const double* velocity,
              const double* positionVariance, const double* velocityVariance);
255 256 257
    void addForce(const double* force, const double* forceVariance);
    void resetForce(bool resetVariance);
    void getAccelerations(double* acceleration, double* accelerationVariance);
258
    void getInverseMass(VectorXd* inverseMass,
259
                        DynSparseRowMatrix* variance, int offset);
260 261 262
    /** Get (and possibly create) RigidBodyErrors object */
    RigidBodyErrors* rigidBodyErrors() {
        return static_cast<RigidBodyErrors*>(objectErrors()); }
263 264

protected:
265 266
    ObjectErrors* createObjectErrors() { return new RigidBodyErrors(this); }

267 268 269 270 271 272 273 274 275 276 277
    Vector2d _position;
    double   _angle;

    Vector2d _velocity;
    double   _angularVelocity;

    Vector2d _force;
    double   _torque;

    double   _mass;
    double   _inertia;
278 279

    friend class RigidBodyErrors;
280 281
};

Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
282 283 284 285 286 287 288 289
/** \ingroup bodies
 *  \brief Rigid disk
 */
class Disk: public RigidBody
{
    STEPCORE_OBJECT(Disk)
public:
    /** Constructs Disk */
290 291
    explicit Disk(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
292
              double mass = 1, double inertia = 1, double radius = 0.5)
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
293 294 295 296 297 298 299 300 301 302 303 304
        : RigidBody(position, angle, velocity, angularVelocity, mass, inertia),
          _radius(radius) {}

    /** Get disk radius */
    double radius() const { return _radius; }
    /** Set disk radius */
    void setRadius(double radius) { _radius = radius; }

protected:
    double _radius;
};

Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
305
/** \ingroup bodies
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
306
 *  \brief Base class for all polygons
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
307
 */
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
308 309 310 311 312
class BasePolygon: public RigidBody
{
    STEPCORE_OBJECT(BasePolygon)
public:
    /** Constructs BasePolygon */
313 314
    explicit BasePolygon(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
315 316 317 318 319 320 321 322 323 324 325 326 327 328 329
              double mass = 1, double inertia = 1)
        : RigidBody(position, angle, velocity, angularVelocity, mass, inertia) {}

    /** Get vertex list (constant) */
    const Vector2dList& vertexes() const { return _vertexes; }

protected:
    Vector2dList _vertexes;
};

class Box: public BasePolygon
{
    STEPCORE_OBJECT(Box)
public:
    /** Constructs Box */
330 331
    explicit Box(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
332 333 334 335 336 337 338 339 340 341 342 343 344
              double mass = 1, double inertia = 1, Vector2d size = Vector2d(1,1));

    /** Get box size */
    Vector2d size() const { return Vector2d(_vertexes[1][0] - _vertexes[0][0],
                                            _vertexes[3][1] - _vertexes[0][1]); }
    /** Set box size */
    void setSize(const Vector2d& size);
};

/** \ingroup bodies
 *  \brief Rigid arbitrary-shaped polygon
 */
class Polygon: public BasePolygon
345 346
{
    STEPCORE_OBJECT(Polygon)
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
347

348
public:
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
349
    /** Get vertex list (constant) */
350
    const Vector2dList& vertexes() const { return _vertexes; }
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
351
    /** Get vertex list (editable) */
352
    Vector2dList& vertexes() { return _vertexes; }
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
353
    /** Set vertex list */
354
    void setVertexes(const Vector2dList& vertexes) { _vertexes = vertexes; }
355 356
};

357 358
#if 0
/** \ingroup bodies
Enrique Matías Sánchez's avatar
Enrique Matías Sánchez committed
359
 *  \brief Unmovable rigid plane
360
 */
Inge Wallin's avatar
Inge Wallin committed
361
class Plane: public Body
362 363 364 365 366
{
    STEPCORE_OBJECT(Plane)

public:
    /** Constructs a plane defined by two points */
367
    explicit Plane(Vector2d point1 = Vector2d::Zero(), Vector2d point2 = Vector2d(1,0))
368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385
        : _point1(point1), _point2(point2) {}

    /** Get first point */
    const Vector2d& point1() const { return _point1; }
    /** Set first point */
    void setPoint2(const Vector2d& point1) { _point1 = point1; }

    /** Get second point */
    const Vector2d& point2() const { return _point2; }
    /** Set second point */
    void setPoint2(const Vector2d& point2) { _point2 = point2; }

protected:
    Vector2d _point1;
    Vector2d _point2;
};
#endif

386 387 388 389
} // namespace StepCore

#endif