gr_rigid_object.h

Go to the documentation of this file.

/* =========================================================================== *
|
|  Copyright (c) 1994-2008 by Kobus Barnard (author).
|
|  Personal and educational use of this code is granted, provided that this
|  header is kept intact, and that the authorship is not misrepresented, that
|  its use is acknowledged in publications, and relevant papers are cited.
|
|  For other use contact the author (kobus AT cs DOT arizona DOT edu).
|
|  Please note that the code in this file has not necessarily been adequately
|  tested. Naturally, there is no guarantee of performance, support, or fitness
|  for any particular task. Nonetheless, I am interested in hearing about
|  problems that you encounter.
|
| Authors:
|     Joseph Schlecht, Luca Del Pero
|
* =========================================================================== */

#ifndef KJB_RIGID_OBJECT_H
#define KJB_RIGID_OBJECT_H

#include <m_cpp/m_vector.h>

#include "l_cpp/l_transformable.h"
#include "l_cpp/l_cloneable.h"
#include "g_cpp/g_quaternion.h"
#include <m_cpp/m_matrix.h>

#define RO_PITCH 0
#define RO_YAW 1
#define RO_ROLL 2

#define RO_ANGLE_EPSILON 1e-120

namespace kjb
{
    /* @class Rigid object: Implements basic transformation for rigid objects,
     * such translations and rotations. The object can be translated and rotated
     * around its three main axes using pitch, yaw and roll angles, following
     * classic Tait-Bryan formulation (ie, changing the pitch at a given moment
     * will rotate the object around its x-axis). Rotations are always performed in
     * the following order: pitch, yaw and roll. Quaternions are used to
     * perform rotations.
     */
    class Rigid_object : public Transformable, public Cloneable
    {
    typedef Rigid_object Self;
    public:
        Rigid_object() : Transformable(), Cloneable(),
        q(0.0, 0.0, 0.0, kjb::Quaternion::XYZR),temp_matrix(4,4)
        {

        }

        Rigid_object(const Rigid_object & ro);
        virtual Rigid_object& operator=(const Rigid_object& src);
        virtual ~Rigid_object() {}

        virtual void swap(Self& other)
        {
            q.swap(other.q);
            temp_matrix.swap(other.temp_matrix);
        }

        virtual void translate(double dx, double dy, double dz);

        virtual void rotate(double dpitch, double dyaw, double droll);

        virtual void compute_new_euler_angles_on_rotations(double dpitch, double dyaw, double droll, kjb::Vector & angles) const;

        virtual void set_rotations(double pitch, double yaw, double roll);

        virtual void set_rotations_and_translate(double pitch, double yaw, double roll, double dx, double dy, double dz);


        virtual Rigid_object * clone() const = 0;

        virtual void transform(const kjb::Matrix & M) = 0;

        inline const Matrix & get_rotations() const
        {
            //return transformation_matrix;
            return q.get_rotation_matrix();
        }

        inline const Vector& get_euler_angles() const
        {
            return q.get_euler_angles();
        }

        inline void set_rotation_mode(kjb::Quaternion::Euler_mode imode)
        {
            q.set_euler_mode(imode);
        }

        inline const kjb::Quaternion & get_orientation() const
        {
            return q;
        }

        void set_orientation(const Quaternion& orientation);

    private:

        Quaternion q;
        Matrix temp_matrix;

    };
}

#endif