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   float length = sqrt( x() * x() + y() * y() + z() * z() );
00072 
00073   return length;
00074 }
00075 
00076 /** Brings the vector to unit-length.
00077  * @return a reference to itself
00078  */
00079 HomVector&
00080 HomVector::unit()
00081 {
00082   set_length(1.0);
00083 
00084   return *this;
00085 }
00086 
00087 /** Scales the vector such that it has the given length.
00088  * @param length the new length
00089  * @return reference to a vector with given length
00090  */
00091 HomVector&
00092 HomVector::set_length(float length)
00093 {
00094   if (this->length() == 0.0) return *this;
00095 
00096   float scale_factor = length / this->length();
00097 
00098   x() = x() * scale_factor;
00099   y() = y() * scale_factor;
00100   z() = z() * scale_factor;
00101 
00102   return *this;
00103 }
00104 
00105 /** Compute the angle between two vectors.
00106  * @param v the other vector
00107  * @return the angle (-M_PI ... M_PI)
00108  */
00109 float
00110 HomVector::angle_xy(const HomVector& v) const
00111 {
00112   if ( 0.0 == length() || 0.0 == v.length() )
00113   { return 0.0; }
00114 
00115   float a = atan2f(v.y(), v.x()) - atan2f(y(), x());
00116 
00117   if ( a > M_PI ) { a -= 2 * M_PI; }
00118   else if ( a < -M_PI ) { a += 2 * M_PI; }
00119 
00120   return a;
00121 }
00122 
00123 } // end namespace fawkes
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends