ConstraintSolver.h

#pragma once
 
#include <iostream>
#include <vector>
 
#include "RigidElement.h"
#include "SparseMatrixBuilder.h"
#include "ConjugateGradient.h"
 
namespace CoRiBoDynamics
{
class ConstraintSet;
 
class ConstraintSolver
{
    friend class ConstraintSet;
public:
    explicit ConstraintSolver(double TimeConstant);
    ConstraintSolver(double TimeConstant,int numThreads, bool no_hyperthreading = false);
    ConstraintSolver(double TimeConstant, CoreBoundThreadPool* ThreadPool);
    virtual ~ConstraintSolver();
 
    void UpdateElements(const double T, const double * const X); // update rigid elements with new states. computes base inertia matrix, and base element forces.
    void ComputeDynamic(const double T, const double * const X, const Eigen::Matrix<double,Eigen::Dynamic,1>& External_Force);
    void ComputeDynamic(const double T, const double * const X); // computes constraints, and solves ode dynamics
    void WriteDynamic(const double * const X,double * const XDot); // writes result of ode dynamics and kinematics to XDot
    int addRigidElement(RigidElement* element); // adds this RigidElement to system. takes over memory management.
 
    void SetSymplecticStepSize(double h);
    double GetSymplecticStepSize();
    double ComputeKineticEnergy(const double T, const double * const X);
    int NumElements(){return (int)m_element.size();}
 
    CoreBoundThreadPool* GetThreadPool();
    ConjugateGradientSolver* GetCGSolver(){return m_CG_solver;}
 
    void setConstraintSetPriority(ConstraintSet* constraintSet, int priority); 
 
    void PrintElement(int index);
 
    SparseMatrixBuilder* GetSparseMatrixBuilder(){return m_MatrixBuilder;}
 
protected:
    void Initialize(double TimeConstant);
    void ComputeDynamicInternal(const double T, const double * const X, const Vector* External_Force);
    void addConstraintSet(ConstraintSet* constraintSet); 
 
    //std::vector<ConstraintSet*> m_constraint_set; /// list of all constraint sets in the system
 
    std::vector<RigidElement*> m_element; 
 
    Vector m_SecondDerivatives; 
 
    void addForceThreadSafe(int element, const vec6& Force);
 
    std::mutex* m_ForceVectorLocks;
    Vector m_Force; 
 
    SparseMatrixBuilder* m_MatrixBuilder;
    CoreBoundThreadPool* m_ThreadPool;
 
    std::mutex m_ConstraintPoolLock;
 
    double m_TimeConstant;
    double m_symplecticH;
    bool m_elements_updated;
 
    ConjugateGradientSolver* m_CG_solver;
 
    class ConstraintSetTask: public CoreBoundThreadPool::Task{
    public:
        explicit ConstraintSetTask();
        void SetConstraintSet(ConstraintSet* CSet);
        void SetStates(double T, const double* X);
        void Execute();
 
        ConstraintSet* GetConstraintSet();
    protected:
        ConstraintSet* m_ConstraintSet;
        double m_T;
        const double* m_X;
    };
    std::map<int,std::vector<ConstraintSetTask> > m_ConstraintSet;
    std::map<ConstraintSet*,int> m_priority_index;
 
    class SubDomainInertiaTask: public CoreBoundThreadPool::Task{
    public:
        void init(int start_index, int end_index, SparseMatrixBuilder* MatrixBuilder, Vector* Force, std::vector<RigidElement*>* element);
        void ComputeSubDomain(double T, const double * const X);
        void Execute();
    protected:
        int m_ix0; 
        int m_ix1; 
        std::vector<RigidElement*>* m_element;
        SparseMatrixBuilder* m_MatrixBuilder;
        Vector* m_Force;
 
        const double* m_X;
        double m_T;
    };
    std::vector<SubDomainInertiaTask> m_InertiaComputers;
 
    class SubDomainWriteDynamic: public CoreBoundThreadPool::Task{
    public:
        void init(int start_index, int end_index, std::vector<RigidElement*>* element, Vector* SecondDerivatives, double symplecticH, double TimeConstant);
        void ComputeSubDomain(double * XDot);
        void Execute();
    protected:
        int m_ix0; 
        int m_ix1; 
        std::vector<RigidElement*>* m_element;
        Vector* m_SecondDerivatives;
        double* m_XDot;
        double m_symplecticH;
        double m_TimeConstant;
    };
    std::vector<SubDomainWriteDynamic> m_WriteDynamic;
 
};
}