JointConstraint.h

#pragma once
 
#include "ConstraintSet.h"
#include<Eigen/StdVector>
 
namespace CoRiBoDynamics{
 
class JointConstraint : public ConstraintSet
{
public:
    explicit JointConstraint(ConstraintSolver* solver);
    virtual ~JointConstraint();
 
    virtual void ComputeConstraints(const double T, const double * const X);
 
    void addBallJointCoupling(int elementA, int elementB, vec3 vectorA, vec3 vectorB, double Beta, double Alpha, double Gamma);
 
    void addHingeCoupling(int elementA, int elementB, vec3 vectorA, vec3 vectorB, vec3 axisA, vec3 axisB, double Beta, double Alpha, double Gamma, double AlphaFriction, double GammaFriction);
 
    void addRigidCoupling(int elementA, int elementB, vec3 vectorA, vec3 vectorB, double BetaN, double AlphaN, double GammaN, const Quat& nullAngle, const Quat& BetaM, const Quat& AlphaM, const Quat& GammaM);
 
    int addExternalBallJointCoupling(int element, vec3 vector, double Beta, double Alpha, double Gamma, const double* Position, const double* Velocity);
 
    void updateExternalBallJointCoupling(int ballJointCoupling, const double* Position, const double* Velocity);
 
    vec3 getExternalBallJointCouplingForce(int ballJointCoupling);
 
    int addExternalRigidCoupling(int element, vec3 vectorExternalObject, vec3 vectorInternalObject, double BetaN, double AlphaN, double GammaN, const Quat& nullAngle, const Quat& BetaM, const Quat& AlphaM, const Quat& GammaM, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity);
 
    void updateExternalRigidCoupling(int rigidCoupling, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity);
 
    vec6 getExternalRigidCouplingForce(int rigidCoupling);
 
    /*
        PA + QA(vectorA) - PB - QB(vectorB) = 0
        QA(axisA) - QB(axisB) = 0
    */
    struct HingeCoupling{
        int elementA; // index of element A
        int elementB; // index of element B
        vec3 vectorA; // body space vector from A center to hinge
        vec3 vectorB; // body space vector from B center to hinge
        vec3 axisA; // body space vector of hinge rotation. Both hinge axes must have same length. Longer vectors will give stronger restoring.
        vec3 axisB; // body space vector of hinge rotation.
        double Beta2; // Beta^2 of axial forces and hinge restoring
        double Alpha2; // 2*Alpha of axial forces and hinge restoring
        double Gamma; // Gamma value for axial forces and hinge restoring
        double Alpha2Friction; // 2*Alpha for hinge rotational friction
        double GammaFriction; // Gamma for hinge rotational friction
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
 
    /*
        PA + QA(vectorA) - PB - QB(vectorB) = 0
    */
    struct BallJointCoupling{
        int elementA; // index of element A
        int elementB; // index of element B
        vec3 vectorA; // body space vector from A center to joint
        vec3 vectorB; // body space vector from B center to joint
        double Beta2; // Beta^2 of axial forces
        double Alpha2;// 2*Alpha of axial forces
        double Gamma; // Gamma value for axial forces
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    /*
        PA + QA(vectorA) - PB - QB(vectorB) = 0
        QA^-1 * QB - nullAngle = 0
    */
    struct RigidCoupling{
        int elementA; // index of element A
        int elementB; // index of element B
        vec3 vectorA; // body space vector from A center to joint
        vec3 vectorB; // body space vector from B center to joint
        double BetaN2; // Beta^2 of axial forces
        double Alpha2N;// 2*Alpha of axial forces
        double GammaN;  // Gamma value for axial forces
        Quat nullAngle; // "relative orientation of zero static torque"
        Quat BetaM2; // 4 x Beta^2 for torque
        Quat Alpha2M;// 4 x 2*Alpha of torque
        Quat GammaM; // 4 x Gamma value for torque
        vec6 ReactionForce; // resulting reaction force. Must add in force X vector to get correct torque for each element
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    struct ExternalBallJointCoupling{
        int element;
        vec3 vector;
        double Beta2;
        double Alpha2;
        double Gamma;
        const double* extPos;
        const double* extVel;
        ExternalBallJointCoupling(){}
        ExternalBallJointCoupling(int i, vec3 v, double B, double A, double G){element = i; vector = v; Beta2 = B; Alpha2 = A; Gamma = G;}
        void SetStates(const double* E_P, const double* E_V){extPos = E_P; extVel = E_V;}
        vec3 ReactionForce; // Constraint force from perspective of external object. The force should be adjusted with the baseMatrix*internal_element_acceleration
        mat36 baseMatrix; // Response matrix to adjust reaction force with element acceleration
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    struct ExternalRigidCoupling{
        int element;
        vec3 vectorExt;
        vec3 vectorInt;
        double BetaN2;
        double Alpha2N;
        double GammaN;
        Quat nullAngle;
        Quat BetaM2;
        Quat Alpha2M;
        Quat GammaM;
        vec6 ReactionForce; // Constraint force from perspective of external object. The force should be adjusted with the baseMatrix*internal_element_acceleration
        mat6 baseMatrix; // Response matrix to adjust reaction force with element acceleration
        const double* extPos;
        const double* extVel;
        const double* extQuat;
        const double* extOmega;
        void SetValues(int i, vec3 vExt, vec3 vInt, const Quat& e, double BN2, double A2N, double GN, const Quat& BM2, const Quat& A2M, const Quat& GM){element = i; vectorExt = vExt; vectorInt = vInt; nullAngle = e; BetaN2 = BN2; Alpha2N = A2N; GammaN = GN; BetaM2 = BM2; Alpha2M = A2M; GammaM = GM;}
        void SetStates(const double* E_P, const double* E_V, const double* E_Q, const double* E_W){extPos = E_P; extVel = E_V; extQuat = E_Q; extOmega = E_W;}
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    vec3 getExternalBallJointCouplingForce( ExternalBallJointCoupling&  Constraint);
    vec6 getExternalRigidCouplingForce(     ExternalRigidCoupling&      Constraint);
 
protected:
    static mat4  QuaternionConstraint_Jacobian(const Quat& q);
    static mat43 Quaternion_Kinematics(const Quat& q);
 
    std::vector<HingeCoupling       ,Eigen::aligned_allocator<HingeCoupling     > > m_HingeCoupling;        
    std::vector<BallJointCoupling   ,Eigen::aligned_allocator<BallJointCoupling > > m_BallJointCoupling;    
    std::vector<RigidCoupling       ,Eigen::aligned_allocator<RigidCoupling     > > m_RigidCoupling;        
 
    std::vector<ExternalBallJointCoupling,Eigen::aligned_allocator<ExternalBallJointCoupling>> m_ExternalBallJointCoupling; 
    std::vector<ExternalRigidCoupling,Eigen::aligned_allocator<ExternalRigidCoupling> > m_ExternalRigidCoupling;
 
    void ComputeHingeCoupling       ( HingeCoupling&        Constraint );
    void ComputeBallJointCoupling   ( BallJointCoupling&    Constraint );
    void ComputeRigidCoupling       ( RigidCoupling&        Constraint );
 
    void ComputeExternalBallJointCoupling(  ExternalBallJointCoupling&  Constraint );
    void ComputeExternalVaryingBallJointCoupling(   ExternalBallJointCoupling&  Constraint, double vector_length_changerate );
    void ComputeExternalRigidCoupling(      ExternalRigidCoupling&      Constraint );
};
 
}