ConstraintSimulator.h

/****************************************************************************
 * Copyright 2015 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 _CONSTRAINT_SIMULATOR_H
#define _CONSTRAINT_SIMULATOR_H

#include <map>
#include <Ravelin/sorted_pair>
#include <Moby/Simulator.h>
#include <Moby/ImpactConstraintHandler.h>
#include <Moby/PenaltyConstraintHandler.h>
#include <Moby/SustainedUnilateralConstraintHandler.h>
#include <Moby/PairwiseDistInfo.h>
#include <Moby/CCD.h>
#include <Moby/UnilateralConstraint.h>
#include <Moby/ConstraintStabilization.h>

namespace Moby {

class Dissipation;
class ContactParameters;
class CollisionDetection;
class CollisionGeometry;

class ConstraintSimulator : public Simulator
{
  friend class CollisionDetection;
  friend class ConstraintStabilization;

  public:
    ConstraintSimulator();
    virtual void load_from_xml(boost::shared_ptr<const XMLTree> node, std::map<std::string, BasePtr>& id_map);
    virtual void save_to_xml(XMLTreePtr node, std::list<boost::shared_ptr<const Base> >& shared_objects) const;
    boost::shared_ptr<ContactParameters> get_contact_parameters(CollisionGeometryPtr geom1, CollisionGeometryPtr geom2) const;
    const std::vector<PairwiseDistInfo>& get_pairwise_distances() const { return _pairwise_distances; }

    ConstraintStabilization cstab;

    std::set<Ravelin::sorted_pair<CollisionGeometryPtr> > unchecked_pairs;

    std::vector<std::pair<ControlledBodyPtr, Ravelin::VectorNd> > _x0, _x1;

    boost::shared_ptr<ContactParameters> (*get_contact_parameters_callback_fn)(CollisionGeometryPtr g1, CollisionGeometryPtr g2);

    void (*post_mini_step_callback_fn)(ConstraintSimulator* s);


    void (*constraint_callback_fn)(std::vector<UnilateralConstraint>&, boost::shared_ptr<void>);

    void (*constraint_post_callback_fn)(const std::vector<UnilateralConstraint>&, boost::shared_ptr<void>);

    boost::shared_ptr<void> constraint_callback_data;
    
    boost::shared_ptr<void> constraint_post_callback_data;
 
    std::vector<UnilateralConstraint>& get_compliant_constraints() { return _compliant_constraints; }

    std::vector<UnilateralConstraint>& get_rigid_constraints() { return _rigid_constraints; }

    std::map<Ravelin::sorted_pair<BasePtr>, boost::shared_ptr<ContactParameters> > contact_params;

    bool render_contact_points;

    boost::shared_ptr<CollisionDetection> get_collision_detection() const { return _coldet; }

  protected:
    void calc_impacting_unilateral_constraint_forces(double dt);
    void find_unilateral_constraints();
    void calc_compliant_unilateral_constraint_forces();
    void preprocess_constraint(UnilateralConstraint& e);
    void determine_geometries();
    void broad_phase(double dt);
    void calc_pairwise_distances();
    void visualize_contact( UnilateralConstraint& constraint );

    ImpactConstraintHandler _impact_constraint_handler;

    PenaltyConstraintHandler _penalty_constraint_handler;
    
    std::vector<UnilateralConstraint> _rigid_constraints;

    std::vector<UnilateralConstraint> _compliant_constraints;

  protected:

    double calc_CA_step();
    double calc_next_CA_Euler_step(double contact_dist_thresh) const;

    std::vector<PairwiseDistInfo> _pairwise_distances;

    Ravelin::VectorNd _workV;

    boost::shared_ptr<CollisionDetection> _coldet;

    std::vector<CollisionGeometryPtr> _geometries;

    std::vector<std::pair<CollisionGeometryPtr, CollisionGeometryPtr> > _pairs_to_check;
}; // end class

} // end namespace

#endif