src/vamos/body/Frame.cc

Go to the documentation of this file.

//  Frame.cc - a coordinate system.
//
//  Copyright (C) 2001--2004 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  This program 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 2 of the License, or
//  (at your option) any later version.
//
//  This program 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 this program; if not, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include <vamos/body/Frame.h>
#include <vamos/geometry/Conversions.h>

#include <cassert>
#include <cmath>

using Vamos_Geometry::Three_Vector;
using Vamos_Geometry::Three_Matrix;

//* Class Frame

//** Constructors

// Specify the position and orientation.
Vamos_Body::Frame::
Frame (const Three_Vector& position, const Three_Matrix& orientation) :
  m_position (position),
  m_orientation (orientation)
{
}

// Take the parent's orientation.
Vamos_Body::Frame::
Frame (const Three_Vector& position) :
  m_position (position)
{
  m_orientation.identity ();
}

// Make a frame that's coincident with the parent frame.
Vamos_Body::Frame::
Frame ()
{
  m_orientation.identity ();
}

// VEC is a vector in the parent's frame.  The representation of VEC
// in this frame is returned.
Three_Vector Vamos_Body::Frame::
transform_in (const Three_Vector& vec) const
{
  return rotate_in (vec - m_position);
}

// VEC is a vector in this frame.  The representation of VEC in the
// parent's frame is returned.
Three_Vector Vamos_Body::Frame::
transform_out (const Three_Vector& vec) const
{
  return rotate_out (vec) + m_position;
}

// Same as transform_out (VEC) above, except that the rotation is
// about the point PIVOT expressed in this frame's coordinates.
Three_Vector Vamos_Body::Frame::
transform_out (const Three_Vector& vec, const Three_Vector& pivot) const
{
  return rotate_out (vec - pivot) + m_position;
}

// Rotate the frame about the vector delta_theta, by an angle equal to
// the magnitude of DELTA_THETA.
void Vamos_Body::Frame::
rotate (const Three_Vector& delta_theta)
{
  m_orientation.rotate (delta_theta);
}

// 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.
Three_Vector Vamos_Body::Frame::
axis_angle (double* angle) const
{
  // To convert the rotation matrix representation of the body's orientation
  // to an axis-angle orientation, we transform first to a quaternion
  // representation.  The matrix-to-quaternion and quaternion-to-axis-angle
  // transformations are described in the Matrix and Quaternion FAQ 
  // (matrixfaq.htm) in the doc directory.

  // Make a local reference to the tranformation matrix for brevity.
  const Three_Matrix& omat = m_orientation;

  // Convert from matrix to quaternion
  double trace = omat [0][0] + omat [1][1] + omat [2][2] + 1.0;
  double s, w, x, y, z;
  s = w = x = y = z = 0.0;
  if (trace > 0.0)
    {
      s = 0.5 / sqrt (trace);
      w = 0.25 / s;
      x = (omat [2][1] - omat [1][2]) * s;
      y = (omat [0][2] - omat [2][0]) * s;
      z = (omat [1][0] - omat [0][1]) * s;
    }
  else
    {
      // Find the largest diagonal element and do the appropriate
      // transformation.
      double largest = omat [0][0];
      int index = 0;
      if (omat [1][1] > largest)
        {
          largest = omat [1][1];
          index = 1;
        }

      if (omat [2][2] > largest)
        {
          largest = omat [2][2];
          s = sqrt (1.0 - omat [0][0] - omat [1][1] + omat [2][2]) * 2.0;
          w = (omat [0][1] + omat [1][0]) / s;
          x = (omat [0][2] + omat [2][0]) / s;
          y = (omat [1][2] + omat [2][1]) / s;
          z = 0.5 / s;
        }
      else if (index == 0)
        {
          s = sqrt (1.0 + omat [0][0] - omat [1][1] - omat [2][2]) * 2.0;
          w = (omat [1][2] + omat [2][1]) / s;
          x = 0.5 / s;
          y = (omat [0][1] + omat [1][0]) / s;
          z = (omat [0][2] + omat [2][0]) / s;
        }
      else
        {
          assert (index == 1);
          s = sqrt (1.0 - omat [0][0] + omat [1][1] - omat [2][2]) * 2.0;
          w = (omat [0][2] + omat [2][0]) / s;
          x = (omat [0][1] + omat [1][0]) / s;
          y = 0.5 / s;
          z = (omat [1][2] + omat [2][1]) / s;
        }
    }

  // Convert from quaternion to angle-axis.
  *angle = Vamos_Geometry::rad_to_deg (acos (w) * 2.0);

  // The return value would be divided by sin (angle) to give a unit
  // vector, but glRotate* () does not care about the length so we'll
  // leave it as is.
  return Three_Vector (x, y, z);
}

---