Fawkes API  Fawkes Development Version
hom_vector.cpp
00001 
00002 /***************************************************************************
00003  *  hom_vector.cpp - Homogenous vector
00004  *
00005  *  Created: Wed Sep 26 17:14:08 2007
00006  *  Copyright  2007-2008  Daniel Beck
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #include <geometry/hom_vector.h>
00025 #include <cmath>
00026 #include <cstdio>
00027 #include <exception>
00028 
00029 namespace fawkes {
00030 
00031 /** @class HomVector geometry/hom_vector.h
00032  * A homogeneous vector.
00033  * @author Daniel Beck
00034  */
00035 
00036 /**Constructor.
00037  * @param x the x-coordinate
00038  * @param y the y-coordinate
00039  * @param z the z-coordinate
00040  */
00041 HomVector::HomVector(float x, float y, float z) 
00042   : HomCoord(x, y, z, 0.0)
00043 {
00044 }
00045 
00046 /** Constructor.
00047  * @param h a HomCoord
00048  */
00049 HomVector::HomVector(const HomCoord& h)
00050   : HomCoord(h)
00051 {
00052   if ( 0.0 != w() )
00053     { 
00054       printf("HomVector(const HomCoord& h): The fourth component of a "
00055              "homogeneous vector has to be 0.0 but it is %f\n", w()); 
00056       throw std::exception();
00057     }
00058 }
00059 
00060 /** Destructor. */
00061 HomVector::~HomVector()
00062 {
00063 }
00064 
00065 /** Calculates the length of the vector
00066  * @return the length
00067  */
00068 float
00069 HomVector::length() const
00070 {  
00071   return sqrt( length_square() );
00072 }
00073 
00074 /** Calculates the squared length length of the vector (faster than length
00075  * @return the squared length
00076  */
00077 float
00078 HomVector::length_square() const
00079 {
00080   return x() * x() + y() * y() + z() * z();
00081 }
00082 
00083 /** Brings the vector to unit-length.
00084  * @return a reference to itself
00085  */
00086 HomVector&
00087 HomVector::unit()
00088 {
00089   set_length(1.0);
00090 
00091   return *this;
00092 }
00093 
00094 /** Scales the vector such that it has the given length.
00095  * @param length the new length
00096  * @return reference to a vector with given length
00097  */
00098 HomVector&
00099 HomVector::set_length(float length)
00100 {
00101   if (this->length() == 0.0) return *this;
00102 
00103   float scale_factor = length / this->length();
00104 
00105   x() = x() * scale_factor;
00106   y() = y() * scale_factor;
00107   z() = z() * scale_factor;
00108 
00109   return *this;
00110 }
00111 
00112 /** Compute the angle between two vectors.
00113  * @param v the other vector
00114  * @return the angle (-M_PI ... M_PI)
00115  */
00116 float
00117 HomVector::angle_xy(const HomVector& v) const
00118 {
00119   if ( 0.0 == length() || 0.0 == v.length() )
00120   { return 0.0; }
00121 
00122   float a = atan2f(v.y(), v.x()) - atan2f(y(), x());
00123 
00124   if ( a > M_PI ) { a -= 2 * M_PI; }
00125   else if ( a < -M_PI ) { a += 2 * M_PI; }
00126 
00127   return a;
00128 }
00129 
00130 } // end namespace fawkes