00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <geometry/hom_vector.h>
00025 #include <cmath>
00026 #include <cstdio>
00027 #include <exception>
00028
00029 namespace fawkes {
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 HomVector::HomVector(float x, float y, float z)
00042 : HomCoord(x, y, z, 0.0)
00043 {
00044 }
00045
00046
00047
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
00061 HomVector::~HomVector()
00062 {
00063 }
00064
00065
00066
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
00077
00078
00079 HomVector&
00080 HomVector::unit()
00081 {
00082 set_length(1.0);
00083
00084 return *this;
00085 }
00086
00087
00088
00089
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
00106
00107
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 }