ScrewJoint.h

/****************************************************************************
 * Copyright 2013 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 _SCREW_JOINT_H
#define _SCREW_JOINT_H

#include <Moby/Joint.h>

namespace Moby {

class ScrewJoint : public Joint
{
  public:
    ScrewJoint();
    virtual ~ScrewJoint() {}
    ScrewJoint(boost::weak_ptr<RigidBody> inboard, boost::weak_ptr<RigidBody> outboard);
    const Ravelin::Vector3d& get_axis() const { return _u; }
    void set_axis(const Ravelin::Vector3d& axis);
    virtual void update_spatial_axes();    
    virtual void determine_q(VectorN& q);
    virtual boost::shared_ptr<const Ravelin::Pose3d>& get_induced_pose();
    virtual const std::vector<Ravelin::SVelocityd>& get_spatial_axes_dot();
    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;
    virtual unsigned num_dof() const { return 1; }
    virtual void evaluate_constraints(double C[]);

    double get_pitch() const { return _pitch; }

    virtual bool is_sngular_config() const { return false; }

    void set_pitch(double pitch) { _pitch = pitch; update_spatial_axes(); }

  private:
    virtual void calc_constraint_jacobian(RigidBodyPtr body, unsigned index, double Cq[7]);
    virtual void calc_constraint_jacobian_dot(RigidBodyPtr body, unsigned index, double Cq[7]);

    double _pitch;

    Ravelin::Vector3d _u;

    Ravelin::Vector3d _ui, _uj; 

    std::vector<Ravelin::SVelocityd> _s_dot;

    Ravelin::Vector3d _v2;
}; // end class
} // end namespace

#endif