00001 #ifndef vm_m3math_h_INCLUDED
00002 #define vm_m3math_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 #include <cstdio>
00044
00045
00046
00047
00048
00076 template <class T_fp>
00077 class vm_M3Math
00078 : public vm_VMath<T_fp,9>
00079 {
00080 private:
00081
00082 enum ENelts_ { ENelts = 9 };
00083 enum EColStride_ { EColStride = 3 };
00084 enum ERows_ { ERow0 = 0, ERow1 = 3, ERow2 = 6 };
00085
00086 public:
00087
00088
00089 typedef T_fp value_type;
00090
00095
00108 static int at( int i, int j );
00115
00124 inline static void init_by_row( T_fp m[],
00125 T_fp const row0[],
00126 T_fp const row1[],
00127 T_fp const row2[] );
00128
00137 inline static void init_by_col( T_fp m[],
00138 T_fp const col0[],
00139 T_fp const col1[],
00140 T_fp const col2[] );
00141
00151 inline static void dyad_product( T_fp m[], T_fp const v1[], T_fp const v2[] );
00158
00166 inline static void inject_row( T_fp m[],
00167 T_fp const row[], int whichrow );
00168
00169
00170
00178 inline static void inject_col( T_fp m[],
00179 T_fp const col[], int whichcol );
00180
00188 inline static void extract_row( T_fp const m[],
00189 T_fp row[], int whichrow );
00190
00198 inline static void extract_col( T_fp const m[],
00199 T_fp col[], int whichcol );
00202
00207
00217 inline static void mvmult( T_fp res[], T_fp const m[], T_fp const v[] );
00218
00228 inline static void mtvmult( T_fp res[], T_fp const m[], T_fp const v[] );
00231
00236
00237 inline static void mmult( T_fp mres[], T_fp const m1[], T_fp const m2[] );
00240
00253 inline static std::ostream&
00254 print_on( std::ostream& os, T_fp const m[],
00255 char const prefix[] = "", char const postfix[] = "" );
00256
00265 inline static void
00266 cprint_on( FILE* of, T_fp const m[],
00267 char const prefix[] = "", char const postfix[] = "" );
00270 };
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288 template <class T_fp>
00289 inline int vm_M3Math<T_fp>::
00290 at( int i, int j )
00291 { return i * EColStride + j; }
00292
00293
00294
00295
00296 template <class T_fp>
00297 inline void vm_M3Math<T_fp>::
00298 init_by_row( T_fp m[],
00299 T_fp const row0[],
00300 T_fp const row1[],
00301 T_fp const row2[] )
00302 { memcpy( &m[ERow0], row0, 3 * sizeof(T_fp) );
00303 memcpy( &m[ERow1], row1, 3 * sizeof(T_fp) );
00304 memcpy( &m[ERow2], row2, 3 * sizeof(T_fp) ); }
00305
00306 template <class T_fp>
00307 inline void vm_M3Math<T_fp>::
00308 init_by_col( T_fp m[],
00309 T_fp const col0[],
00310 T_fp const col1[],
00311 T_fp const col2[] )
00312 { m[0] = col0[0]; m[3] = col0[1]; m[6] = col0[2];
00313 m[1] = col1[0]; m[4] = col1[1]; m[7] = col1[2];
00314 m[2] = col2[0]; m[5] = col2[1]; m[8] = col2[2]; }
00315
00316
00317
00318
00319 template <class T_fp>
00320 inline void vm_M3Math<T_fp>::
00321 inject_row( T_fp m[],
00322 T_fp const row[], int whichrow )
00323 { memcpy( &m[whichrow*EColStride], row, 3 * sizeof(T_fp) ); }
00324
00325 template <class T_fp>
00326 inline void vm_M3Math<T_fp>::
00327 inject_col( T_fp m[],
00328 T_fp const col[], int whichcol )
00329 {
00330 double* pm = &m[whichcol];
00331 *pm = *col++; pm += EColStride;
00332 *pm = *col++; pm += EColStride;
00333 *pm = *col;
00334 }
00335
00336
00337
00338
00339 template <class T_fp>
00340 inline void vm_M3Math<T_fp>::
00341 extract_row( T_fp const m[],
00342 T_fp row[], int whichrow )
00343 { memcpy( row, &m[whichrow*EColStride], 3 * sizeof(T_fp) ); }
00344
00345 template <class T_fp>
00346 inline void vm_M3Math<T_fp>::
00347 extract_col( T_fp const m[],
00348 T_fp col[], int whichcol )
00349 {
00350 double const* pm = &m[whichcol];
00351 *col = *pm; pm += EColStride; ++col;
00352 *col = *pm; pm += EColStride; ++col;
00353 *col = *pm;
00354 }
00355
00356
00357
00358
00359 template <class T_fp>
00360 inline void vm_M3Math<T_fp>::
00361 dyad_product( T_fp m[], T_fp const v1[], T_fp const v2[] )
00362 {
00363 m[0] = v1[0] * v2[0]; m[1] = v1[0] * v2[1]; m[2] = v1[0] * v2[2];
00364 m[3] = v1[1] * v2[0]; m[4] = v1[1] * v2[1]; m[5] = v1[1] * v2[2];
00365 m[6] = v1[2] * v2[0]; m[7] = v1[2] * v2[1]; m[8] = v1[2] * v2[2];
00366 }
00367
00368
00369
00370
00371 template <class T_fp>
00372 inline void vm_M3Math<T_fp>::
00373 mvmult( T_fp res[], T_fp const m[], T_fp const v[] )
00374 {
00375 res[0] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2];
00376 res[1] = m[3] * v[0] + m[4] * v[1] + m[5] * v[2];
00377 res[2] = m[6] * v[0] + m[7] * v[1] + m[8] * v[2];
00378 }
00379
00380 template <class T_fp>
00381 inline void vm_M3Math<T_fp>::
00382 mtvmult( T_fp res[], T_fp const m[], T_fp const v[] )
00383 {
00384 res[0] = m[0] * v[0] + m[3] * v[1] + m[6] * v[2];
00385 res[1] = m[1] * v[0] + m[4] * v[1] + m[7] * v[2];
00386 res[2] = m[2] * v[0] + m[5] * v[1] + m[8] * v[2];
00387 }
00388
00389 template <class T_fp>
00390 inline void vm_M3Math<T_fp>::
00391 mmult( T_fp mres[], T_fp const m1[], T_fp const m2[] )
00392 {
00393 mres[0] = m1[0] * m2[0] + m1[1] * m2[3] + m1[2] * m2[6];
00394 mres[1] = m1[0] * m2[1] + m1[1] * m2[4] + m1[2] * m2[7];
00395 mres[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2] * m2[8];
00396
00397 mres[3] = m1[3] * m2[0] + m1[4] * m2[3] + m1[5] * m2[6];
00398 mres[4] = m1[3] * m2[1] + m1[4] * m2[4] + m1[5] * m2[7];
00399 mres[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5] * m2[8];
00400
00401 mres[6] = m1[6] * m2[0] + m1[7] * m2[3] + m1[8] * m2[6];
00402 mres[7] = m1[6] * m2[1] + m1[7] * m2[4] + m1[8] * m2[7];
00403 mres[8] = m1[6] * m2[2] + m1[7] * m2[5] + m1[8] * m2[8];
00404 }
00405
00406 template <class T_fp>
00407 inline std::ostream& vm_M3Math<T_fp>::
00408 print_on( std::ostream& os, T_fp const m[],
00409 char const prefix[], char const postfix[] )
00410 {
00411 if ( std::strlen(prefix) ) { os << prefix; }
00412 os << m[0] << " " << m[1] << " " << m[2] << "\n"
00413 << m[3] << " " << m[4] << " " << m[5] << "\n"
00414 << m[6] << " " << m[7] << " " << m[8] << "\n";
00415 if ( std::strlen(postfix) ) { os << postfix; }
00416 return os;
00417 }
00418
00419 template <class T_fp>
00420 inline void vm_M3Math<T_fp>::
00421 cprint_on( FILE* of, T_fp const m[],
00422 char const prefix[], char const postfix[] )
00423 {
00424 if ( std::strlen(prefix) ) { std::fprintf(of, "%s", prefix); }
00425 std::fprintf(of, "%.18e %.18e %.18e\n", m[0], m[1], m[2]);
00426 std::fprintf(of, "%.18e %.18e %.18e\n", m[3], m[4], m[5]);
00427 std::fprintf(of, "%.18e %.18e %.18e\n", m[6], m[7], m[8]);
00428 if ( std::strlen(postfix) ) { std::fprintf(of, "%s", postfix); }
00429 }
00430
00431 #endif