00001 #ifndef vm_v3math_h_INCLUDED
00002 #define vm_v3math_h_INCLUDED
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <vm_math/vm_vmath.h>
00040 #include <cstring>
00041 #include <cmath>
00042 #include <iostream>
00043
00044
00045
00046
00047
00068 template <class T_fp>
00069 class vm_V3Math
00070 : public vm_VMath<T_fp,3>
00071 {
00072 private:
00073
00074 enum ENelts_ { ENelts = 3 };
00075
00076 public:
00077
00083 typedef T_fp value_type;
00084
00085
00090
00099 inline static void set( T_fp v[], T_fp x, T_fp y, T_fp z );
00100
00107 inline static void set( T_fp v[], T_fp x );
00110
00122 inline static T_fp unitize( T_fp v[] );
00123
00132 inline static T_fp unitize( T_fp vu[], T_fp const vi[] );
00135
00149 inline static int is_unit_vector( T_fp const v[], T_fp const tol );
00150
00161 inline static int are_orthogonal( T_fp const v[], T_fp const other[],
00162 T_fp const tol );
00163
00174 inline static int are_orthonormal( T_fp const v[], T_fp const other[],
00175 T_fp const tol );
00178
00183
00192 inline static T_fp dot( T_fp const v1[], T_fp const v2[] );
00193
00203 inline static void cross( T_fp prod[],
00204 T_fp const v1[], T_fp const v2[] );
00207
00220 inline static std::ostream&
00221 print_on( std::ostream& os, T_fp const v[],
00222 char const prefix[] = "", char const postfix[] = "" );
00223
00232 inline static void
00233 cprint_on( FILE* of, T_fp const v[],
00234 char const prefix[] = "", char const postfix[] = "" );
00237 };
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253 template <class T_fp>
00254 inline void vm_V3Math<T_fp>::
00255 set( T_fp v[], T_fp x, T_fp y, T_fp z )
00256 { v[0] = x; v[1] = y; v[2] = z; }
00257
00258 template <class T_fp>
00259 inline void vm_V3Math<T_fp>::
00260 set( T_fp v[], T_fp x )
00261 { vm_VMath<T_fp,3>::set( v, x ); }
00262
00263 template <class T_fp>
00264 inline T_fp vm_V3Math<T_fp>::
00265 dot( T_fp const v1[], T_fp const v2[] )
00266 {
00267 return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
00268 }
00269
00270 template <class T_fp>
00271 inline void vm_V3Math<T_fp>::
00272 cross( T_fp result[],
00273 T_fp const v1[], T_fp const v2[] )
00274 {
00275 result[0] = v1[1] * v2[2] - v1[2] * v2[1];
00276 result[1] = v1[2] * v2[0] - v1[0] * v2[2];
00277 result[2] = v1[0] * v2[1] - v1[1] * v2[0];
00278 }
00279
00280 template <class T_fp>
00281 inline T_fp vm_V3Math<T_fp>::
00282 unitize( T_fp v[] )
00283 {
00284 T_fp size = sqrt( dot( v, v ) );
00285 if ( size == 0.0 )
00286 {
00287 return 0.0;
00288 }
00289 div_eq( v, size );
00290
00291 return size;
00292 }
00293
00294 template <class T_fp>
00295 inline T_fp vm_V3Math<T_fp>::
00296 unitize( T_fp vu[], T_fp const vi[] )
00297 {
00298 vm_V3Math<T_fp>::copy( vu, vi );
00299 return vm_V3Math<T_fp>::unitize( vu );
00300 }
00301
00302 template <class T_fp>
00303 inline int vm_V3Math<T_fp>::
00304 is_unit_vector( T_fp const v[], T_fp const tol )
00305 {
00306 return !( (fabs(sqrt(dot(v, v))-1.0) > tol ) ? 1 : 0 );
00307 }
00308
00309 template <class T_fp>
00310 inline int vm_V3Math<T_fp>::
00311 are_orthogonal( T_fp const v[],
00312 T_fp const other[],
00313 T_fp const tol )
00314 {
00315 return !( (fabs(dot(v, other)) > tol ) ? 1 : 0 );
00316 }
00317
00318 template <class T_fp>
00319 inline int vm_V3Math<T_fp>::
00320 are_orthonormal( T_fp const v[],
00321 T_fp const other[],
00322 T_fp const tol )
00323 {
00324 return is_unit_vector(v,tol) &&
00325 is_unit_vector(other,tol) &&
00326 are_orthogonal(v,other,tol);
00327 }
00328
00329 template <class T_fp>
00330 inline std::ostream& vm_V3Math<T_fp>::
00331 print_on( std::ostream& os, T_fp const v[],
00332 char const prefix[], char const postfix[] )
00333 { if ( std::strlen(prefix) ) { os << prefix; }
00334 os << v[0] << " " << v[1] << " " << v[2];
00335 if ( std::strlen(postfix) ) { os << postfix; }
00336 return os; }
00337
00338 template <class T_fp>
00339 inline void vm_V3Math<T_fp>::
00340 cprint_on( FILE* of, T_fp const v[],
00341 char const prefix[], char const postfix[] )
00342 { if ( std::strlen(prefix) ) { std::fprintf(of, "%s", prefix); }
00343 std::fprintf(of, "%.18e %.18e %.18e\n", v[0], v[1], v[2]);
00344 if ( std::strlen(postfix) ) { std::fprintf(of, "%s", postfix); } }
00345
00346 #endif