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 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