Vamos Automotive Simulator git

// Frame.h - a coordinate system.
//
//  Copyright (C) 2001--2019 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  Vamos is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  Vamos is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with Vamos.  If not, see <http://www.gnu.org/licenses/>.

#ifndef _FRAME_H_
#define _FRAME_H_

#include "../geometry/Three_Matrix.hpp"
#include "../geometry/Three_Vector.hpp"

namespace Vamos_Body
{
// A Frame describes a coordinate system.
class Frame
{
    using V3 = Vamos_Geometry::Three_Vector;
    using M3 = Vamos_Geometry::Three_Matrix;
public:
    // Specify the position and orientation.
    Frame(const V3& position, const M3& orientation, const Frame* parent = nullptr);
    // Take the parent's orientation.
    Frame(const V3& position, const Frame* parent = nullptr);
    // Make a frame that's coincident with the parent frame.
    Frame(const Frame *parent = nullptr);

    bool is_world_frame() const { return !mp_parent; }
    M3 orientation() const { return m_orientation; }
    // Return the position of the origin in the parent frame.
    V3 position() const { return m_position; }
    // Return the velocity of the origin relative to the parent frame.
    V3 velocity() const { return m_velocity; }
    // Return the angular velocity of the frame relative to the parent frame.
    V3 angular_velocity() const { return m_angular_velocity; }
    // Give this frame an absolute orientation of NEW_ORIENTATION.
    void set_orientation(const M3& new_orientation) { m_orientation = new_orientation; }
    // Rotate the frame about the vector delta_theta, by an angle equal to the magnitude
    // of DELTA_THETA.
    void rotate(const V3& delta_theta) { m_orientation.rotate(delta_theta); }
    // VEC is a vector in the parent's frame.  The representation of VEC in this frame is
    // returned.
    V3 transform_from_parent(const V3& vec) const;
    V3 transform_from_world(const V3& vec) const;
    // VEC is a vector in this frame.  The representation of VEC in the parent's frame is
    // returned.
    V3 transform_to_parent(const V3& vec) const;
    V3 transform_to_world(const V3& vec) const;
    // VELOCITY is a velocity vector in the world frame.  The representation of VELOCITY
    // in this frame is returned.
    V3 transform_velocity_from_world(const V3& velocity) const;
    // VELOCITY is a vector in this frame.  The representation of VELOCITY in the world
    // frame is returned.
    V3 transform_velocity_to_world(const V3& velocity) const;
    // VELOCITY is a velocity vector in the parent's frame.  The representation of
    // VELOCITY in this frame is returned.
    V3 transform_velocity_from_parent(const V3& velocity) const;
    // VELOCITY is a vector in this frame.  The representation of VELOCITY in the parent's
    // frame is returned.
    V3 transform_velocity_to_parent(const V3& velocity) const;
    // Same as transform_in (VEC) above, except that translation is not performed.
    V3 rotate_from_parent(const V3& vec) const;
    V3 rotate_from_world(const V3& vec) const;
    // Same as transform_out (VEC) above, except that translation is not performed.
    V3 rotate_to_parent(const V3& vec) const;
    V3 rotate_to_world(const V3& vec) const;
    void set_position(const V3& r) { m_position = r; }
    // Change the position by DELTA_R.
    void translate(const V3& delta_r) { m_position += delta_r; }
    void set_velocity(const V3& v) { m_velocity = v; }
    void accelerate(const V3& delta_v) { m_velocity += delta_v; }
    void set_angular_velocity(const V3& omega) { m_angular_velocity = omega; }
    void angular_accelerate(const V3& delta_omega) { m_angular_velocity += delta_omega; }
    // Express the orientation of this frame as a vector in the parent frame and a
    // rotation about that vector.  ANGLE holds the rotation angle when the function
    // returns.  The returned vector has a magnitude of sin (ANGLE).  The values returned
    // are suitable for use with the glRotate functions.
    V3 axis_angle(double *angle) const;

private:
    const Frame *mp_parent;
    M3 m_orientation;
    // The position of the origin of this frame relative to the parent frame.
    V3 m_position;
    // The position of the origin of this frame relative to the parent frame.  This is the
    // vector from the parent frame's origin to this frame's origin, expressed in the
    // coordinates of the parent frame.
    V3 m_velocity;
    // The angular velocity vector of this frame expressed in the coordinates of the
    // parent's frame.
    V3 m_angular_velocity;
};
} // namespace Vamos_Body

#endif // not _FRAME_H_