rtc: rtc: rtcSMat6.h Source File

00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcSMat6.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 14 $
00018  */
00019 #ifndef RTC_SMAT6_H
00020 #define RTC_SMAT6_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "Vec6.h"
00025 #include "SMat.h"
00026 
00027 //== NAMESPACES ================================================================
00028 namespace rtc {
00029 
00030 // Forward declarations
00031 template <class T, int M> class SMat; // MxM Square Matrix
00032 template <class T> class Vec6; // 4d Vector
00033 template <class T> class SMat6; // 4x4 Matrix
00034 
00036 
00040 template <class T>
00041 class SMat6: public SMat<T,6> {
00042 public:
00043   // Constructors
00044   SMat6();
00045   SMat6(const T* d);
00046   SMat6(const T diagVal);
00047   SMat6(const Vec6<T>& diagVec);
00048   SMat6(const Mat<T,6,6>& m);
00049   SMat6(const T x11, const T x12, const T x13, const T x14, const T x15, const T x16,
00050         const T x21, const T x22, const T x23, const T x24, const T x25, const T x26,
00051         const T x31, const T x32, const T x33, const T x34, const T x35, const T x36,
00052         const T x41, const T x42, const T x43, const T x44, const T x45, const T x46,
00053         const T x51, const T x52, const T x53, const T x54, const T x55, const T x56,
00054         const T x61, const T x62, const T x63, const T x64, const T x65, const T x66);
00055   SMat6(const Vec6<T>& q0,const Vec6<T>& q1, const Vec6<T>& q2,const Vec6<T>& q3,const Vec6<T>& q4,const Vec6<T>& q5);
00056 
00057 
00058   // Casting Operation
00059   template <class U> SMat6(const Mat<U,6,6>& m);
00060 
00061   // Named Constructors
00062   static SMat6<T> fromRows(const Vec6<T>& q0, const Vec6<T>& q1, const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4, const Vec6<T>& q5);
00063   static SMat6<T> fromCols(const Vec6<T>& q0, const Vec6<T>& q1, const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4, const Vec6<T>& q5);
00064 
00065   // Mutators
00066   void set(const T x11, const T x12, const T x13, const T x14, const T x15, const T x16,
00067            const T x21, const T x22, const T x23, const T x24, const T x25, const T x26,
00068            const T x31, const T x32, const T x33, const T x34, const T x35, const T x36,
00069            const T x41, const T x42, const T x43, const T x44, const T x45, const T x46,
00070            const T x51, const T x52, const T x53, const T x54, const T x55, const T x56,
00071            const T x61, const T x62, const T x63, const T x64, const T x65, const T x66);
00072   void setRows(const Vec6<T>& q0, const Vec6<T>& q1, const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4,const Vec6<T>& q5);
00073   void setCols(const Vec6<T>& q0, const Vec6<T>& q1, const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4,const Vec6<T>& q5);
00074 
00075   // Data
00076   using SMat<T,6>::x;
00077   using SMat<T,6>::set;
00078 };
00079 
00080 // Declare a few common typdefs
00081 typedef SMat6<bool> SMat6b;
00082 typedef SMat6<char> SMat6c;
00083 typedef SMat6<unsigned char> SMat6uc;
00084 typedef SMat6<int> SMat6i;
00085 typedef SMat6<float> SMat6f;
00086 typedef SMat6<double> SMat6d;
00087 
00088 
00090 // DEFINITIONS
00092 
00094 
00095 // Constructors
00096 
00099 template <class T>
00100 inline SMat6<T>::SMat6() {}
00101 
00105 template <class T>
00106 inline SMat6<T>::SMat6(const T* d) : SMat<T,6>(d) {}
00107 
00111 template <class T>
00112 inline SMat6<T>::SMat6(const T diagVal) : SMat<T,6>(diagVal) {}
00113 
00117 template <class T>
00118 inline SMat6<T>::SMat6(const Vec6<T>& diagVec) : SMat<T,6>(diagVec) {}
00119 
00122 template <class T>
00123 inline SMat6<T>::SMat6(const Mat<T,6,6>& m) : SMat<T,6>(m) {}
00124 
00127 template <class T>
00128 inline SMat6<T>::SMat6(const T x11, const T x12, const T x13, const T x14, const T x15, const T x16,
00129                        const T x21, const T x22, const T x23, const T x24, const T x25, const T x26,
00130                        const T x31, const T x32, const T x33, const T x34, const T x35, const T x36,
00131                        const T x41, const T x42, const T x43, const T x44, const T x45, const T x46,
00132                        const T x51, const T x52, const T x53, const T x54, const T x55, const T x56,
00133                        const T x61, const T x62, const T x63, const T x64, const T x65, const T x66) {
00134   set(x11, x12, x13, x14, x15, x16,
00135       x21, x22, x23, x24, x25, x26,
00136       x31, x32, x33, x34, x35, x36,
00137       x41, x42, x43, x44, x45, x46,
00138       x51, x52, x53, x54, x55, x56,
00139       x61, x62, x63, x64, x65, x66);
00140 }
00141 
00144 template <class T>
00145 inline SMat6<T>::SMat6(const Vec6<T>& q0,const Vec6<T>& q1,const Vec6<T>& q2,const Vec6<T>& q3,const Vec6<T>& q4,const Vec6<T>& q5) {
00146   setCols(q0,q1,q2,q3,q4,q5);
00147 }
00148 
00149 // Casting Operation
00150 
00153 template <class T> template <class U>
00154 inline SMat6<T>::SMat6(const Mat<U,6,6>& m) : SMat<T,6>(m) {}
00155 
00156 // Named Constructors
00157 
00160 template <class T>
00161 inline SMat6<T> SMat6<T>::fromRows(const Vec6<T>& q0, const Vec6<T>& q1,const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4,const Vec6<T>& q5) {
00162   SMat6<T> m;
00163   m.setRows(q0,q1,q2,q3,q4,q5);
00164   return m;
00165 }
00166 
00169 template <class T>
00170 inline SMat6<T> SMat6<T>::fromCols(const Vec6<T>& q0, const Vec6<T>& q1,const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4, const Vec6<T>& q5) {
00171   SMat6<T> m;
00172   m.setCols(q0,q1,q2,q3,q4,q5);
00173   return m;
00174 }
00175 
00176 // Mutators
00177 
00180 template <class T>
00181 inline void SMat6<T>::set(const T x11, const T x12, const T x13, const T x14, const T x15, const T x16,
00182                           const T x21, const T x22, const T x23, const T x24, const T x25, const T x26,
00183                           const T x31, const T x32, const T x33, const T x34, const T x35, const T x36,
00184                           const T x41, const T x42, const T x43, const T x44, const T x45, const T x46,
00185                           const T x51, const T x52, const T x53, const T x54, const T x55, const T x56,
00186                           const T x61, const T x62, const T x63, const T x64, const T x65, const T x66){
00187   x[0]  = x11; x[1]  = x12; x[2]  = x13; x[3]  = x14; x[4]  = x15; x[5]  = x16;
00188   x[6]  = x21; x[7]  = x22; x[8]  = x23; x[9]  = x24; x[10] = x25; x[11] = x26;
00189   x[12] = x31; x[13] = x32; x[14] = x33; x[15] = x34; x[16] = x35; x[17] = x36;
00190   x[18] = x41; x[19] = x42; x[20] = x43; x[21] = x44; x[22] = x45; x[23] = x46;
00191   x[24] = x51; x[25] = x52; x[26] = x53; x[27] = x54; x[28] = x55; x[29] = x56;
00192   x[30] = x61; x[31] = x62; x[32] = x63; x[33] = x64; x[34] = x65; x[35] = x66;
00193 }
00194 
00197 template <class T>
00198 inline void SMat6<T>::setRows(const Vec6<T>& q0, const Vec6<T>& q1, const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4, const Vec6<T>& q5) {
00199   setRow(0,q0); setRow(1,q1); setRow(2,q2); setRow(3,q3); setRow(4,q4); setRow(5,q5);
00200 }
00201 
00204 template <class T>
00205 inline void SMat6<T>::setCols(const Vec6<T>& q0, const Vec6<T>& q1, const Vec6<T>& q2, const Vec6<T>& q3, const Vec6<T>& q4, const Vec6<T>& q5) {
00206   setCol(0,q0); setCol(1,q1); setCol(2,q2); setCol(3,q3); setCol(4,q4); setCol(5,q5);
00207 }
00208 
00209 //==============================================================================
00210 } // namespace rtc
00211 //==============================================================================
00212 #endif // RTC_SMAT6_H defined
00213 //==============================================================================