rtc: rtc: rtcSMat5.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 .......: rtcSMat5.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_SMAT5_H
00020 #define RTC_SMAT5_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "Vec5.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 Vec5; // 4d Vector
00033 template <class T> class SMat5; // 4x4 Matrix
00034 
00039 template <class T>
00040 class SMat5: public SMat<T,5> {
00041 public:
00042   // Data
00043   using SMat<T,5>::x;
00044   using SMat<T,5>::set;
00045 
00046   // Constructors
00047   SMat5();
00048   SMat5(const T* d);
00049   SMat5(const T diagVal);
00050   SMat5(const Vec5<T>& diagVec);
00051   SMat5(const Mat<T,5,5>& m);
00052   SMat5(const T x11, const T x12, const T x13, const T x14, const T x15,
00053         const T x21, const T x22, const T x23, const T x24, const T x25,
00054         const T x31, const T x32, const T x33, const T x34, const T x35,
00055         const T x41, const T x42, const T x43, const T x44, const T x45,
00056         const T x51, const T x52, const T x53, const T x54, const T x55);
00057   SMat5(const Vec5<T>& q0,const Vec5<T>& q1, const Vec5<T>& q2,const Vec5<T>& q3,const Vec5<T>& q4);
00058 
00059 
00060   // Casting Operation
00061   template <class U> SMat5(const Mat<U,5,5>& m);
00062 
00063   // Named Constructors
00064   static SMat5<T> fromRows(const Vec5<T>& q0, const Vec5<T>& q1, const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4);
00065   static SMat5<T> fromCols(const Vec5<T>& q0, const Vec5<T>& q1, const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4);
00066 
00067   // Mutators
00068   void set(const T x11, const T x12, const T x13, const T x14, const T x15,
00069            const T x21, const T x22, const T x23, const T x24, const T x25,
00070            const T x31, const T x32, const T x33, const T x34, const T x35,
00071            const T x41, const T x42, const T x43, const T x44, const T x45,
00072            const T x51, const T x52, const T x53, const T x54, const T x55);
00073   void setRows(const Vec5<T>& q0, const Vec5<T>& q1, const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4);
00074   void setCols(const Vec5<T>& q0, const Vec5<T>& q1, const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4);
00075 }; // end class SMat5<T>
00076 
00077 // Declare a few common typdefs
00078 typedef SMat5<bool> SMat5b;
00079 typedef SMat5<char> SMat5c;
00080 typedef SMat5<unsigned char> SMat5uc;
00081 typedef SMat5<int> SMat5i;
00082 typedef SMat5<float> SMat5f;
00083 typedef SMat5<double> SMat5d;
00084 
00085 
00087 // DEFINITIONS
00089 
00091 
00092 // Constructors
00093 
00096 template <class T>
00097 inline SMat5<T>::SMat5() {}
00098 
00102 template <class T>
00103 inline SMat5<T>::SMat5(const T* d) : SMat<T,5>(d) {}
00104 
00108 template <class T>
00109 inline SMat5<T>::SMat5(const T diagVal) : SMat<T,5>(diagVal) {}
00110 
00114 template <class T>
00115 inline SMat5<T>::SMat5(const Vec5<T>& diagVec) : SMat<T,5>(diagVec) {}
00116 
00119 template <class T>
00120 inline SMat5<T>::SMat5(const Mat<T,5,5>& m) : SMat<T,5>(m) {}
00121 
00124 template <class T>
00125 inline SMat5<T>::SMat5(const T x11, const T x12, const T x13, const T x14, const T x15,
00126                        const T x21, const T x22, const T x23, const T x24, const T x25,
00127                        const T x31, const T x32, const T x33, const T x34, const T x35,
00128                        const T x41, const T x42, const T x43, const T x44, const T x45,
00129                        const T x51, const T x52, const T x53, const T x54, const T x55) {
00130   set(x11, x12, x13, x14, x15,
00131       x21, x22, x23, x24, x25,
00132       x31, x32, x33, x34, x35,
00133       x41, x42, x43, x44, x45,
00134       x51, x52, x53, x54, x55);
00135 }
00136 
00139 template <class T>
00140 inline SMat5<T>::SMat5(const Vec5<T>& q0,const Vec5<T>& q1,const Vec5<T>& q2,const Vec5<T>& q3,const Vec5<T>& q4) {
00141   setCols(q0,q1,q2,q3,q4);
00142 }
00143 
00144 // Casting Operation
00145 
00148 template <class T> template <class U>
00149 inline SMat5<T>::SMat5(const Mat<U,5,5>& m) : SMat<T,5>(m) {}
00150 
00151 // Named Constructors
00152 
00155 template <class T>
00156 inline SMat5<T> SMat5<T>::fromRows(const Vec5<T>& q0, const Vec5<T>& q1,const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4) {
00157   SMat5<T> m;
00158   m.setRows(q0,q1,q2,q3,q4);
00159   return m;
00160 }
00161 
00164 template <class T>
00165 inline SMat5<T> SMat5<T>::fromCols(const Vec5<T>& q0, const Vec5<T>& q1,const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4) {
00166   SMat5<T> m;
00167   m.setCols(q0,q1,q2,q3,q4);
00168   return m;
00169 }
00170 
00171 // Mutators
00172 
00175 template <class T>
00176 inline void SMat5<T>::set(const T x11, const T x12, const T x13, const T x14, const T x15,
00177                           const T x21, const T x22, const T x23, const T x24, const T x25,
00178                           const T x31, const T x32, const T x33, const T x34, const T x35,
00179                           const T x41, const T x42, const T x43, const T x44, const T x45,
00180                           const T x51, const T x52, const T x53, const T x54, const T x55){
00181   x[0]  = x11; x[1]  = x12; x[2]  = x13; x[3]  = x14; x[4]  = x15;
00182   x[5]  = x21; x[6]  = x22; x[7]  = x23; x[8]  = x24; x[9]  = x25;
00183   x[10] = x31; x[11] = x32; x[12] = x33; x[13] = x34; x[14] = x35;
00184   x[15] = x41; x[16] = x42; x[17] = x43; x[18] = x44; x[19] = x45;
00185   x[20] = x51; x[21] = x52; x[22] = x53; x[23] = x54; x[24] = x55;
00186 }
00187 
00190 template <class T>
00191 inline void SMat5<T>::setRows(const Vec5<T>& q0, const Vec5<T>& q1, const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4) {
00192   setRow(0,q0); setRow(1,q1); setRow(2,q2); setRow(3,q3); setRow(4,q4);
00193 }
00194 
00197 template <class T>
00198 inline void SMat5<T>::setCols(const Vec5<T>& q0, const Vec5<T>& q1, const Vec5<T>& q2, const Vec5<T>& q3, const Vec5<T>& q4) {
00199   setCol(0,q0); setCol(1,q1); setCol(2,q2); setCol(3,q3); setCol(4,q4);
00200 }
00201 
00202 //==============================================================================
00203 } // namespace rtc
00204 //==============================================================================
00205 #endif // RTC_SMAT5_H defined
00206 //==============================================================================