FSABAlgorithm.h

/****************************************************************************
 * Copyright 2005 Evan Drumwright
 * This library is distributed under the terms of the Apache V2.0 
 * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0).
 ****************************************************************************/

#ifndef FSAB_ALGORITHM 
#error This class is not to be included by the user directly. Use FSABAlgorithmd.h or FSABAlgorithmf.h instead.
#endif

class RC_ARTICULATED_BODY;


class FSAB_ALGORITHM 
{
  friend class RC_ARTICULATED_BODY;

  public:
    FSAB_ALGORITHM();
    ~FSAB_ALGORITHM() {}
    boost::shared_ptr<RC_ARTICULATED_BODY> get_body() const { return boost::shared_ptr<RC_ARTICULATED_BODY>(_body); }
    void set_body(boost::shared_ptr<RC_ARTICULATED_BODY> body) { _body = body; }
    void calc_fwd_dyn();
    void calc_inverse_generalized_inertia_noprecalc(MATRIXN& iM);
    void solve_generalized_inertia_noprecalc(SHAREDVECTORN& v);
    void solve_generalized_inertia_noprecalc(SHAREDMATRIXN& Y);
    void apply_generalized_impulse(const VECTORN& gj);
    void apply_impulse(const SMOMENTUM& j, boost::shared_ptr<RIGIDBODY> link);
    void calc_spatial_inertias(boost::shared_ptr<RC_ARTICULATED_BODY> body);

    boost::weak_ptr<RC_ARTICULATED_BODY> _body;

    std::vector<SACCEL> _a;

    std::vector<SPATIAL_AB_INERTIA> _I;

    std::vector<SFORCE> _Z;

    std::vector<SVELOCITY> _dv;

    std::vector<SACCEL> _c;

    std::vector<std::vector<SMOMENTUM> > _Is;

    std::vector<MATRIXN> _sIs;

    std::vector<MATRIXN> _usIs, _vsIs;
    std::vector<VECTORN> _ssIs;

    std::vector<bool> _rank_deficient;

    std::vector<VECTORN> _mu;

  private:
    // pointer to the linear algebra routines
    boost::shared_ptr<LINALG> _LA;

    VECTORN _workv, _workv2, _sTY, _qd_delta, _sIsmu, _Qi, _Q;
    MATRIXN _sIss, _workM;
    std::vector<SMOMENTUM> _Y;

    std::vector<bool> _processed;

    void calc_fwd_dyn_special();
    static REAL sgn(REAL x);
    static void push_children(boost::shared_ptr<RIGIDBODY> link, std::queue<boost::shared_ptr<RIGIDBODY> >& q);
    void apply_coulomb_joint_friction(boost::shared_ptr<RC_ARTICULATED_BODY> body);
    void apply_generalized_impulse(unsigned index, VECTORN& vgj);
    void set_spatial_velocities(boost::shared_ptr<RC_ARTICULATED_BODY> body);
    void calc_spatial_accelerations(boost::shared_ptr<RC_ARTICULATED_BODY> body);
    void calc_spatial_zero_accelerations(boost::shared_ptr<RC_ARTICULATED_BODY> body);
    void calc_spatial_coriolis_vectors(boost::shared_ptr<RC_ARTICULATED_BODY> body);
    VECTORN& solve_sIs(unsigned idx, const VECTORN& v, VECTORN& result) const;
    MATRIXN& solve_sIs(unsigned idx, const MATRIXN& v, MATRIXN& result) const;
    MATRIXN& transpose_solve_sIs(unsigned idx, const std::vector<SVELOCITY>& m, MATRIXN& result) const;
}; // end class