RigidElement.h

#pragma once
#include "eigen_matrix_defs.h"
 
namespace CoRiBoDynamics
{
    class ConstraintSolver;
 
class RigidElement {
    friend class ConstraintSolver;
public:
 
    RigidElement();
    virtual ~RigidElement(){}
 
    virtual void Setup(double T, const double * const X);
    virtual const mat6& InertialMatrix() = 0; 
    virtual const vec6& Forces() = 0; 
 
    inline const vec3& P(){return m_P;} 
    inline const Quat& Q(){return m_Q;} 
    inline const vec3& V(){return m_V;} 
    inline const vec3& W(){return m_W;} 
 
    inline const vec3 P(const double* const X){return vec3(m_ix_P+X);} 
    inline const Quat Q(const double* const X){return ptrQ(m_ix_Q+X);} 
    inline const vec3 V(const double* const X){return vec3(m_ix_V+X);} 
    inline const vec3 W(const double* const X){return vec3(m_ix_W+X);} 
 
    inline int ix_P(){return m_ix_P;} 
    inline int ix_Q(){return m_ix_Q;} 
    inline int ix_V(){return m_ix_V;} 
    inline int ix_W(){return m_ix_W;} 
    inline int ix_N(){return m_ix_N;} 
 
    inline const mat3& RotationMatrix(){return m_RotationMatrix;} 
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
    void set_element_index(int N){
        m_ix_N = N;
    }
 
    int m_ix_N;
    int m_ix_P;
    int m_ix_Q;
    int m_ix_V;
    int m_ix_W;
 
    vec3 m_P;
    Quat m_Q;
    vec3 m_V;
    vec3 m_W;
 
    mat3 m_RotationMatrix;
};
 
 
class StandardRigidElement : public RigidElement {
public:
    StandardRigidElement(const vec6& DiagonalInertialMatrix, int ix_P, int ix_Q, int ix_V, int ix_W); 
    virtual ~StandardRigidElement(){}
    virtual void Setup( double T, const double * const X );
    virtual const mat6& InertialMatrix();
    virtual const vec6& Forces();
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
    vec6 m_Inertia;
    mat6 m_FullInertiaMatrix;
    vec6 m_CoriolisForce;
};
 
}