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() ||
00113        0.0 == v.length() )
00114     { return 0.0; }
00115
00116   float s = x() * v.x() + y() * v.y() + z() * v.z();
00117   float l = length() * v.length();
00118   float d = s / l;
00119
00120   if (-1.0 >= d || d >= 1.0 )
00121     { return 0.0; }
00122
00123   float a = acos( s / l );
00124
00125   HomVector n(1.0, 0.0);
00126   float a1 = acos(   x() /   length() );
00127   float a2 = acos( v.x() / v.length() );
00128
00129   if ( a == (a2 - a1) )
00130     { return a; }
00131   else
00132     { return -a; }
00133 }
00134
00135 } // end namespace fawkes