SphericalJoint.h

/****************************************************************************
 * Copyright 2007 Evan Drumwright
 * This library is distributed under the terms of the GNU General Public 
 * License (found in COPYING).
 ****************************************************************************/

#ifndef _SPHERICAL_JOINT_H
#define _SPHERICAL_JOINT_H

#include <Physsim/Joint.h>

namespace Physsim {

class SphericalJoint : public Joint
{
        public:
                enum Axis { eAxis1, eAxis2, eAxis3 };

                SphericalJoint();
                virtual ~SphericalJoint() {}
                virtual void clone(BasePtr& cloned, bimap<BasePtr, BasePtr>* obj_map = NULL) const;
                virtual void load_state(BaseConstPtr object, bimap<BasePtr, BasePtr>* correspondence = NULL);
                virtual void save_state(BasePtr object, bimap<BasePtr, BasePtr>* correspondence = NULL) const;
                SphericalJoint(boost::weak_ptr<RigidBody> inboard, boost::weak_ptr<RigidBody> outboard);
                Vector3 get_axis_global(Axis a) const;
                void set_axis_global(const Vector3& axis, Axis a);
                void set_axis_local(const Vector3& axis, Axis a);
                virtual void update_spatial_axes();             
                virtual void determine_Q();
                virtual const Matrix4& get_transform();
                virtual const SMatrixN& get_spatial_axes(ReferenceFrameType type);
                virtual const SMatrixN& get_spatial_axes_deriv(ReferenceFrameType type);
                virtual void load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map);
                virtual void save_to_xml(XMLTreePtr node, std::list<BaseConstPtr>& shared_objects) const;
                virtual unsigned get_num_dof() const { return 3; }

                virtual bool is_singular_config() const { return std::fabs(std::sin(get_q()[DOF_1])) < _SINGULAR_TOL && std::fabs(std::cos(get_q()[DOF_2])) < _SINGULAR_TOL; }
                        

                const Vector3& get_axis_local(Axis a) const { return _u[a]; }

        private:
                virtual void update_visualization();
                Matrix3 get_rotation() const;


                static const Real _SINGULAR_TOL = 1e-2;

                Matrix3 _R;

                bool _si_computed;

                bool _si_dot_computed;

                Real _last_q2, _last_q1;

                Real _last_sid_q2, _last_sid_q1, _last_qd2, _last_qd1;

                std::vector<Vector3> _u;

                Matrix4 _T;

                SMatrixN _si_dot_i;

                SMatrixN _si_dot_0;
}; // end class
} // end namespace

#endif

---