rigidbody.h

Go to the documentation of this file.

/* 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
*/

#ifndef STEPCORE_RIGIDBODY_H
#define STEPCORE_RIGIDBODY_H

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

namespace StepCore {

class RigidBody;

class RigidBodyErrors: public ObjectErrors
{
    STEPCORE_OBJECT(RigidBodyErrors)

public:
    RigidBodyErrors(Item* owner = 0)
        : ObjectErrors(owner), _positionVariance(0,0), _angleVariance(0), _velocityVariance(0,0),
          _angularVelocityVariance(0), _forceVariance(0,0), _torqueVariance(0),
          _massVariance(0), _inertiaVariance(0) {}

    RigidBody* rigidBody() const;

    const Vector2d& positionVariance() const { return _positionVariance; }
    void setPositionVariance(const Vector2d& positionVariance) {
        _positionVariance = positionVariance; }

    double angleVariance() const { return _angleVariance; }
    void setAngleVariance(double angleVariance) { _angleVariance = angleVariance; }

    const Vector2d& velocityVariance() const { return _velocityVariance; }
    void setVelocityVariance(const Vector2d& velocityVariance) {
        _velocityVariance = velocityVariance; }

    double angularVelocityVariance() const { return _angularVelocityVariance; }
    void setAngularVelocityVariance(double angularVelocityVariance) {
        _angularVelocityVariance = angularVelocityVariance; }

    Vector2d accelerationVariance() const;

    double angularAccelerationVariance() const;

    const Vector2d& forceVariance() const { return _forceVariance; }
    void setForceVariance(const Vector2d& forceVariance) {
        _forceVariance = forceVariance; }

    double torqueVariance() const { return _torqueVariance; }

    void applyForceVariance(const Vector2d& force,
                            const Vector2d& position,
                            const Vector2d& forceVariance,
                            const Vector2d& positionVariance);

    void applyTorqueVariance(double torqueVariance) { _torqueVariance += torqueVariance; }

    double massVariance() const { return _massVariance; }
    void   setMassVariance(double massVariance) {
        _massVariance = massVariance; }

    double inertiaVariance() const { return _inertiaVariance; }
    void   setInertiaVariance(double inertiaVariance) {
        _inertiaVariance = inertiaVariance; }

    Vector2d momentumVariance() const;
    void setMomentumVariance(const Vector2d& momentumVariance);

    double angularMomentumVariance() const;
    void setAngularMomentumVariance(double angularMomentumVariance);

    double kineticEnergyVariance() const;
    void setKineticEnergyVariance(double kineticEnergyVariance);

protected:
    Vector2d _positionVariance;
    double   _angleVariance;

    Vector2d _velocityVariance;
    double   _angularVelocityVariance;

    Vector2d _forceVariance;
    double   _torqueVariance;

    double _massVariance;
    double _inertiaVariance;

    friend class RigidBody;
};

class RigidBody: public Item, public Body
{
    STEPCORE_OBJECT(RigidBody)

public:
    enum {
        PositionOffset = 0, 
        AngleOffset = 2     
    };

    explicit RigidBody(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
              double mass = 1, double inertia = 1);

    const Vector2d& position() const { return _position; }
    void setPosition(const Vector2d& position) { _position = position; }

    double angle() const { return _angle; }
    void setAngle(double angle) { _angle = angle; }

    const Vector2d& velocity() const { return _velocity; }
    void setVelocity(const Vector2d& velocity) { _velocity = velocity; }
    Vector2d velocityWorld(const Vector2d& worldPoint) const;
    Vector2d velocityLocal(const Vector2d& localPoint) const;

    double angularVelocity() const { return _angularVelocity; }
    void setAngularVelocity(double angularVelocity) { _angularVelocity = angularVelocity; }

    Vector2d acceleration() const { return _force/_mass; }

    double angularAcceleration() const { return _torque/_inertia; }

    const Vector2d& force() const { return _force; }
    void setForce(const Vector2d& force) { _force = force; }

    double torque() const { return _torque; }
    void setTorque(double torque) { _torque = torque; }

    //void applyForceLocal(const Vector2d& localPosition = Vector2d(0,0));

    void applyForce(const Vector2d& force, const Vector2d& position);

    void applyTorque(double torque) { _torque += torque; }

    double mass() const { return _mass; }
    void   setMass(double mass) { _mass = mass; }

    double inertia() const { return _inertia; }
    void   setInertia(double inertia) { _inertia = inertia; }

    Vector2d momentum() const { return _velocity * _mass; }
    void setMomentum(const Vector2d& momentum) { _velocity = momentum / _mass; }

    double angularMomentum() const { return _angularVelocity * _inertia; }
    void setAngularMomentum(double angularMomentum) {
        _angularVelocity = angularMomentum / _inertia; }

    double kineticEnergy() const {
        return _mass * _velocity.squaredNorm()/2 + _inertia * square(_angularVelocity)/2; }
    void setKineticEnergy(double kineticEnergy);

    Vector2d vectorLocalToWorld(const Vector2d& v) const;
    Vector2d vectorWorldToLocal(const Vector2d& v) const;

    Vector2d pointLocalToWorld(const Vector2d& p) const;
    Vector2d pointWorldToLocal(const Vector2d& p) const;

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

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

    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);
    void addForce(const double* force, const double* forceVariance);
    void resetForce(bool resetVariance);
    void getAccelerations(double* acceleration, double* accelerationVariance);
    void getInverseMass(VectorXd* inverseMass,
                        DynSparseRowMatrix* variance, int offset);
    RigidBodyErrors* rigidBodyErrors() {
        return static_cast<RigidBodyErrors*>(objectErrors()); }

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

    Vector2d _position;
    double   _angle;

    Vector2d _velocity;
    double   _angularVelocity;

    Vector2d _force;
    double   _torque;

    double   _mass;
    double   _inertia;

    friend class RigidBodyErrors;
};

class Disk: public RigidBody
{
    STEPCORE_OBJECT(Disk)
public:
    explicit Disk(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
              double mass = 1, double inertia = 1, double radius = 0.5)
        : RigidBody(position, angle, velocity, angularVelocity, mass, inertia),
          _radius(radius) {}

    double radius() const { return _radius; }
    void setRadius(double radius) { _radius = radius; }

protected:
    double _radius;
};

class BasePolygon: public RigidBody
{
    STEPCORE_OBJECT(BasePolygon)
public:
    explicit BasePolygon(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
              double mass = 1, double inertia = 1)
        : RigidBody(position, angle, velocity, angularVelocity, mass, inertia) {}

    const Vector2dList& vertexes() const { return _vertexes; }

protected:
    Vector2dList _vertexes;
};

class Box: public BasePolygon
{
    STEPCORE_OBJECT(Box)
public:
    explicit Box(Vector2d position = Vector2d::Zero(), double angle = 0,
              Vector2d velocity = Vector2d::Zero(), double angularVelocity = 0,
              double mass = 1, double inertia = 1, Vector2d size = Vector2d(1,1));

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

class Polygon: public BasePolygon
{
    STEPCORE_OBJECT(Polygon)

public:
    const Vector2dList& vertexes() const { return _vertexes; }
    Vector2dList& vertexes() { return _vertexes; }
    void setVertexes(const Vector2dList& vertexes) { _vertexes = vertexes; }
};

#if 0

class Plane: public Item, public Body
{
    STEPCORE_OBJECT(Plane)

public:
    explicit Plane(Vector2d point1 = Vector2d::Zero(), Vector2d point2 = Vector2d(1,0))
        : _point1(point1), _point2(point2) {}

    const Vector2d& point1() const { return _point1; }
    void setPoint2(const Vector2d& point1) { _point1 = point1; }

    const Vector2d& point2() const { return _point2; }
    void setPoint2(const Vector2d& point2) { _point2 = point2; }

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

} // namespace StepCore

#endif