Fawkes API
Fawkes Development Version
|
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