rtc: rtc: rtcSMat2.h Source File

00001 /*
00002  * Copyright (C) 2008
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 .......: rtcSMat2.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_SMAT2_H
00020 #define RTC_SMAT2_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcVec2.h"
00025 #include "rtc/rtcSMat.h"
00026 
00027 //== NAMESPACES ================================================================
00028 namespace rtc {
00029 
00030 // Forward declaration
00031 template <class T, int M> class SMat; // MxM Square Matrix
00032 template <class T> class Vec2; // 2d Vector
00033 template <class T> class SMat2; // 2x2 Matrix
00034 
00039 template <class T>
00040 class SMat2: public SMat<T,2> {
00041 public:
00042   // Data
00043   using SMat<T,2>::x;
00044   using SMat<T,2>::set;
00045 
00046   // Constructors
00047   SMat2();
00048   SMat2(const T* d);
00049   SMat2(const T diagVal);
00050   SMat2(const Vec2<T>& diagVec);
00051   SMat2(const Mat<T,2,2>& m);
00052   SMat2(const T x11, const T x12,
00053   const T x21, const T x22);
00054   SMat2(const Vec2<T>& q0, const Vec2<T>& q1);
00055 
00056   // Casting Operation
00057   template <class U> SMat2(const Mat<U,2,2>& m);
00058 
00059   // Named Constructors
00060   static SMat2<T> fromRows(const Vec2<T>& v0, const Vec2<T>& v1);
00061   static SMat2<T> fromCols(const Vec2<T>& v0, const Vec2<T>& v1);
00062 
00063   // Mutators
00064   void set(const T x11, const T x12, const T x21, const T x22);
00065   void setRows(const Vec2<T>& v0, const Vec2<T>& v1);
00066   void setCols(const Vec2<T>& v0, const Vec2<T>& v1);
00067 
00068   // Determinant, inverse, etc.
00069   T det() const;
00070   SMat2<T> inverted() const;
00071   int invert();
00072 };
00073 
00074 // Declare a few common typdefs
00075 typedef SMat2<bool> SMat2b;
00076 typedef SMat2<char> SMat2c;
00077 typedef SMat2<unsigned char> SMat2uc;
00078 typedef SMat2<int> SMat2i;
00079 typedef SMat2<float> SMat2f;
00080 typedef SMat2<double> SMat2d;
00081 
00082 //==============================================================================
00083 // SMat2<T>
00084 //==============================================================================
00085 
00086 // Constructors
00087 
00090 template <class T>
00091 inline SMat2<T>::SMat2() {}
00092 
00096 template <class T>
00097 inline SMat2<T>::SMat2(const T* d) : SMat<T,2>(d) { }
00098 
00102 template <class T>
00103 inline SMat2<T>::SMat2(const T diagVal) : SMat<T,2>(diagVal) {}
00104 
00108 template <class T>
00109 inline SMat2<T>::SMat2(const Vec2<T>& diagVec) : SMat<T,2>(diagVec) {}
00110 
00111 
00114 template <class T>
00115 inline SMat2<T>::SMat2(const Mat<T,2,2>& m) : SMat<T,2>(m) {}
00116 
00119 template <class T>
00120 inline SMat2<T>::SMat2(const T x11, const T x12,
00121      const T x21, const T x22) {
00122   set(x11, x12, x21, x22);
00123 }
00124 
00127 template <class T>
00128 inline SMat2<T>::SMat2(const Vec2<T>& v0,
00129      const Vec2<T>& v1) {
00130   setCols(v0,v1);
00131 }
00132 
00133 // Casting Operation
00134 
00137 template <class T> template <class U>
00138 inline SMat2<T>::SMat2(const Mat<U,2,2>& m) : SMat<T,2>(m) {}
00139 
00140 // Named Constructors
00141 
00144 template <class T>
00145 inline SMat2<T> SMat2<T>::fromRows(const Vec2<T>& v0, const Vec2<T>& v1) {
00146   SMat2<T> m;
00147   m.setRows(v0,v1);
00148   return m;
00149 }
00150 
00153 template <class T>
00154 inline SMat2<T> SMat2<T>::fromCols(const Vec2<T>& v0,
00155            const Vec2<T>& v1) {
00156   SMat2<T> m;
00157   m.setCols(v0,v1);
00158   return m;
00159 }
00160 
00161 // Mutators
00162 
00165 template <class T>
00166 inline void SMat2<T>::set(const T x11, const T x12,
00167         const T x21, const T x22) {
00168   x[0] = x11; x[1] = x12;
00169   x[2] = x21; x[3] = x22;
00170 }
00171 
00174 template <class T>
00175 inline void SMat2<T>::setRows(const Vec2<T>& v0,
00176       const Vec2<T>& v1) {
00177   setRow(0,v0); setRow(1,v1);
00178 }
00179 
00182 template <class T>
00183 inline void SMat2<T>::setCols(const Vec2<T>& v0,
00184       const Vec2<T>& v1) {
00185   setCol(0,v0); setCol(1,v1);
00186 }
00187 
00188 // Determinant and Inverse
00189 
00192 template <class T>
00193 inline T SMat2<T>::det() const {
00194   return (x[0]*x[3] - x[1]*x[2]);
00195 }
00196 
00200 template <class T>
00201 inline SMat2<T> SMat2<T>::inverted() const {
00202   T d = det();
00203   if (d == 0.0) {
00204     throw Exception("SMat2::inverted(): error, can't take inverse of singular matrix.");
00205   }
00206   T di = T(1)/d;
00207   return SMat2<T>(x[3]*di,-x[1]*di,-x[2]*di,x[0]*di);
00208 }
00209 
00212 template <class T>
00213 inline int SMat2<T>::invert() {
00214   T d = det();
00215   if (d == 0.0) {
00216     throw Exception("SMat2::invert(): error, can't take inverse of singular matrix.");
00217   }
00218   T di = T(1)/d;
00219   set(x[3]*di,-x[1]*di,-x[2]*di,x[0]*di);
00220   return 0;
00221 }
00222 
00223 //==============================================================================
00224 } // namespace rtc
00225 //==============================================================================
00226 #endif // RTC_SMAT2_H defined
00227 //==============================================================================