src/vamos/geometry/Three_Vector.cc

Go to the documentation of this file.

//      Vamos Automotive Simulator
//  Copyright (C) 2001--2002 Sam Varner
//
//  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/geometry/Three_Vector.h>
#include <vamos/geometry/Three_Matrix.h>

#include <cmath>

const Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::X = Three_Vector (1.0, 0.0, 0.0);
const Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::Y = Three_Vector (0.0, 1.0, 0.0);
const Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::Z = Three_Vector (0.0, 0.0, 1.0);

// Constructors.

// Default constructor.  Set all elements to 0.0.
Vamos_Geometry::
Three_Vector::Three_Vector ()
{
  m_vec [0] = 0.0;
  m_vec [1] = 0.0;
  m_vec [2] = 0.0;
}

// Elements are initialized by arguments.
Vamos_Geometry::
Three_Vector::Three_Vector (double x, double y, double z)
{
  m_vec [0] = x;
  m_vec [1] = y;
  m_vec [2] = z;
}

// Elements are initialized by a C-style array.
Vamos_Geometry::
Three_Vector::Three_Vector (double vec [3])
{
  m_vec [0] = vec [0];
  m_vec [1] = vec [1];
  m_vec [2] = vec [2];
}

// Copy assignment.
Vamos_Geometry::Three_Vector& Vamos_Geometry::
Three_Vector::operator = (const Three_Vector& vec)
{
  if (&vec != this)
        {
          m_vec [0] = vec [0];
          m_vec [1] = vec [1];
          m_vec [2] = vec [2];
        }
  return *this;
}

// Zero all elements.
void Vamos_Geometry::
Three_Vector::zero ()
{
  m_vec [0] = m_vec [1] = m_vec [2] = 0.0;
}

// Return the dot product with the argument.
double Vamos_Geometry::
Three_Vector::dot (const Three_Vector& vec) const
{
  return m_vec [0] * vec [0] + m_vec [1] * vec [1] + m_vec [2] * vec [2];
}

// Return the cross product with the argument.
Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::cross (const Three_Vector& vec) const
{
  return Three_Vector (m_vec [1] * vec [2] - m_vec [2] * vec [1],
                                           m_vec [2] * vec [0] - m_vec [0] * vec [2],
                                           m_vec [0] * vec [1] - m_vec [1] * vec [0]);
}

// Return the projection along the argument.
Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::project (const Three_Vector& vec) const
{
  double dot_prod = dot (vec);
  double vec_abs = vec.abs ();
  if (vec.abs () == 0.0)
        return Three_Vector (0.0, 0.0, 0.0);
  return vec.unit () * dot_prod / vec_abs;
}

Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::back_project (const Three_Vector& vec) const
{
  double dot_prod = dot (vec);
  if (dot_prod == 0.0)
        return Three_Vector (0.0, 0.0, 0.0);
  double this_abs = abs ();
  return this_abs * this_abs * vec / dot_prod;
}

double Vamos_Geometry::
Three_Vector::perp_distance (const Three_Vector& point1,
                                                         const Three_Vector& point2) const
{
  Three_Vector vec1 = (point2 - point1);
  Three_Vector vec2 = vec1.project (*this);
  return (vec1 - vec2).abs ();
}

double Vamos_Geometry::
Three_Vector::component (const Three_Vector& vec) const
{
  double dot_prod = dot (vec);
  if (dot_prod == 0.0)
        return 0.0;
  return dot_prod / vec.abs ();
}  

// Return the unit vector that points along this vector.
Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::unit () const
{
  Three_Vector vec = *this;
  double vec_abs = abs ();
  if (vec_abs == 0.0)
        return Three_Vector (0.0, 0.0, 1.0);
  return vec / vec_abs;
}

// Return the angle between this vector and `vec'.
double Vamos_Geometry::
Three_Vector::angle (const Three_Vector& vec) const
{
  return acos ((*this).dot (vec) / (*this).abs () / vec.abs ());
}

Vamos_Geometry::Three_Vector Vamos_Geometry::
Three_Vector::rotate (double x, double y, double z) const
{
  Three_Matrix r;
        r.identity();
  r.rotate (Three_Vector (x, y, z));
  return r * *this;
}

//* Member operators.
Vamos_Geometry::Three_Vector& Vamos_Geometry::
Three_Vector::operator += (const Three_Vector& vec)
{
  m_vec [0] += vec [0];
  m_vec [1] += vec [1];
  m_vec [2] += vec [2];
  return *this;
}

Vamos_Geometry::Three_Vector& Vamos_Geometry::
Three_Vector::operator -= (const Three_Vector& vec)
{
  m_vec [0] -= vec [0];
  m_vec [1] -= vec [1];
  m_vec [2] -= vec [2];
  return *this;
}

Vamos_Geometry::Three_Vector& Vamos_Geometry::
Three_Vector::operator *= (double factor)
{
  m_vec [0] *= factor;
  m_vec [1] *= factor;
  m_vec [2] *= factor;
  return *this;
}

Vamos_Geometry::Three_Vector& Vamos_Geometry::
Three_Vector::operator /= (double factor)
{
  m_vec [0] /= factor;
  m_vec [1] /= factor;
  m_vec [2] /= factor;
  return *this;
}

//* Non-Member Operators

// Addition
const Vamos_Geometry::Three_Vector Vamos_Geometry::
operator + (const Three_Vector& vec1, const Three_Vector& vec2)
{
  Three_Vector out_vec = vec1;
  return out_vec += vec2;
}

// Negation
const Vamos_Geometry::Three_Vector Vamos_Geometry::
operator - (const Three_Vector& vec)
{
  Three_Vector out_vec = vec;
  return out_vec *= -1.0;
}

// Subtraction
const Vamos_Geometry::Three_Vector Vamos_Geometry::
operator - (const Three_Vector& vec1, const Three_Vector& vec2)
{
  Three_Vector out_vec = vec1;
  return out_vec -= vec2;
}

// Scalar Multiplication
const Vamos_Geometry::Three_Vector Vamos_Geometry::
operator * (const Three_Vector& vec, double factor)
{
  Three_Vector out_vec = vec;
  return out_vec *= factor;
}

const Vamos_Geometry::Three_Vector Vamos_Geometry::
operator * (double factor, const Three_Vector& vec)
{
  return vec * factor;
}

// Scalar Division
const Vamos_Geometry::Three_Vector Vamos_Geometry::
operator / (const Three_Vector& vec, double factor)
{
  Three_Vector out_vec = vec;
  if (factor == 0.0)
        return Three_Vector (0.0, 0.0, 0.0);
  return out_vec /= factor;
}

const Vamos_Geometry::Three_Vector Vamos_Geometry::
operator / (double factor, const Three_Vector& vec)
{
  return vec / factor;
}

bool Vamos_Geometry::
operator == (const Three_Vector& vec1, const Three_Vector& vec2)
{
  return (vec1 [0] == vec2 [0])
    && (vec1 [1] == vec2 [1])
    && (vec1 [2] == vec2 [2]);
}

// Stream Operators
std::ostream& Vamos_Geometry::
operator << (std::ostream& os, const Three_Vector& vec)
{
  os << "[ " << vec [0] 
         << ", " << vec [1]
         << ", " << vec [2] << " ]";
  return os;
}

std::istream& Vamos_Geometry::
operator >> (std::istream& is, Three_Vector& vec)
{
  char delim;
  is >> delim >> vec [0] >> delim >> vec [1] >> delim >> vec [2] >> delim;
  return is;
}

---