Simulator.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 _SIMULATOR_H
#define _SIMULATOR_H

#include <sys/times.h>
#include <list>
#include <map>
#include <set>
#include <boost/shared_ptr.hpp>

#include <Moby/Base.h>
#include <Moby/Log.h>
#include <Moby/RigidBody.h>
#include <Moby/Dissipation.h>
#include <Moby/ArticulatedBody.h>

namespace osg { 
  class Node;
  class Group; 
}

namespace Moby {

class RigidBody;
class ArticulatedBody;
class RCArticulatedBody;
class VisualizationData;


class Simulator : public virtual Base
{
  friend class ConstraintStabilization;
  friend class ImpactConstraintHandler;
  friend class RigidBody;
  friend class RCArticulatedBody;

  public:
    Simulator();
    virtual ~Simulator(); 
    virtual double step(double step_size);
    ControlledBodyPtr find_dynamic_body(const std::string& name) const;
    void add_dynamic_body(ControlledBodyPtr body);
    void remove_dynamic_body(ControlledBodyPtr body);
    void update_visualization();
    virtual void save_to_xml(XMLTreePtr node, std::list<boost::shared_ptr<const Base> >& shared_objects) const;
    virtual void load_from_xml(boost::shared_ptr<const XMLTree> node, std::map<std::string, BasePtr>& id_map);  

    double current_time;

    boost::shared_ptr<Dissipation> dissipator;


    const std::vector<ControlledBodyPtr>& get_dynamic_bodies() const { return _bodies; }

    void add_transient_vdata(osg::Node* vdata);

    osg::Node* get_persistent_vdata() const { return (osg::Node*) _persistent_vdata; } 

    osg::Node* get_transient_vdata() const { return (osg::Node*) _transient_vdata; } 

    void (*post_step_callback_fn)(Simulator* s);

    double dynamics_time;

    std::vector<JointPtr> implicit_joints;

  protected:
    void apply_impulse(boost::shared_ptr<Ravelin::DynamicBodyd> db, const Ravelin::SharedVectorNd& gj);
    void solve(const std::vector<boost::shared_ptr<Ravelin::DynamicBodyd> >& island, const std::vector<JointPtr>& island_joints, const Ravelin::VectorNd& v, const Ravelin::VectorNd& f, double dt, Ravelin::VectorNd& a, Ravelin::VectorNd& lambda) const;
    virtual double check_pairwise_constraint_violations(double t) { return 0.0; }
    void find_islands(std::vector<std::vector<boost::shared_ptr<Ravelin::DynamicBodyd> > >& islands);
    unsigned num_generalized_coordinates(const std::vector<boost::shared_ptr<Ravelin::DynamicBodyd> > & island) const;
    osg::Group* _persistent_vdata;
    osg::Group* _transient_vdata;
    void calc_fwd_dyn(double dt);
    void precalc_fwd_dyn();

    std::vector<ControlledBodyPtr> _bodies;
  
    Ravelin::VectorNd _current_dx;

    template <class ForwardIterator>
    double integrate(double step_size, ForwardIterator begin, ForwardIterator end);

    double integrate(double step_size) { return integrate(step_size, _bodies.begin(), _bodies.end()); }

  private:
    static Ravelin::VectorNd& ode(const Ravelin::VectorNd& x, double t, double dt, void* data, Ravelin::VectorNd& dx);
}; // end class

// include inline functions
#include "Simulator.inl"

} // end namespace 

#endif