rtcSMat3.h
Go to the documentation of this 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 .......: rtcSMat3.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_SMAT3_H
00020 #define RTC_SMAT3_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcVec3.h"
00025 #include "rtc/rtcSMat.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 Vec3; // 3d Vector
00033 template <class T> class SMat3; // 3x3 Matrix
00034 
00039 template <class T>
00040 class SMat3: public SMat<T,3> {
00041 public:
00042   // Data
00043   using SMat<T,3>::x;
00044   using SMat<T,3>::set;
00045 
00046   // Constructors
00047   SMat3();
00048   SMat3(const T* d);
00049   SMat3(const T diagVal);
00050   SMat3(const Vec3<T>& diagVec);
00051   SMat3(const Mat<T,3,3>& m);
00052   SMat3(const T x11, const T x12, const T x13,
00053         const T x21, const T x22, const T x23,
00054         const T x31, const T x32, const T x33);
00055   SMat3(const Vec3<T>& q0, const Vec3<T>& q1, const Vec3<T>& q2);
00056 
00057   // Casting Operation
00058   template <class U> SMat3(const Mat<U,3,3>& m);
00059 
00060   // Named Constructors
00061   static SMat3<T> fromRows(const Vec3<T>& v0, const Vec3<T>& v1, const Vec3<T>& v2);
00062   static SMat3<T> fromCols(const Vec3<T>& v0, const Vec3<T>& v1, const Vec3<T>& v2);
00063 
00064   // Mutators
00065   void set(const T x11, const T x12, const T x13,
00066            const T x21, const T x22, const T x23,
00067            const T x31, const T x32, const T x33);
00068   void setRows(const Vec3<T>& v0, const Vec3<T>& v1, const Vec3<T>& v2);
00069   void setCols(const Vec3<T>& v0, const Vec3<T>& v1, const Vec3<T>& v2);
00070 
00071   // Determinant, inverse, etc.
00072   T det() const;
00073   SMat3<T> inverted() const;
00074   int invert();
00075 };
00076 
00077 // Declare a few common typdefs
00078 typedef SMat3<bool> SMat3b;
00079 typedef SMat3<char> SMat3c;
00080 typedef SMat3<unsigned char> SMat3uc;
00081 typedef SMat3<int> SMat3i;
00082 typedef SMat3<float> SMat3f;
00083 typedef SMat3<double> SMat3d;
00084 
00085 //==============================================================================
00086 // SMat3<T>
00087 //==============================================================================
00088 
00089 // Constructors
00090 
00093 template <class T>
00094 inline SMat3<T>::SMat3() {}
00095 
00099 template <class T>
00100 inline SMat3<T>::SMat3(const T* d) : SMat<T,3>(d) {}
00101 
00105 template <class T>
00106 inline SMat3<T>::SMat3(const T diagVal) : SMat<T,3>(diagVal)  {}
00107 
00111 template <class T>
00112 inline SMat3<T>::SMat3(const Vec3<T>& diagVec) : SMat<T,3>(diagVec) { }
00113 
00116 template <class T>
00117 inline SMat3<T>::SMat3(const Mat<T,3,3>& m) : SMat<T,3>(m) {}
00118 
00121 template <class T>
00122 inline SMat3<T>::SMat3(const T x11, const T x12, const T x13,
00123                        const T x21, const T x22, const T x23,
00124                        const T x31, const T x32, const T x33) {
00125   set(x11, x12, x13, x21, x22, x23, x31, x32, x33);
00126 }
00127 
00130 template <class T>
00131 inline SMat3<T>::SMat3(const Vec3<T>& v0, const Vec3<T>& v1,
00132      const Vec3<T>& v2) {
00133   setCols(v0,v1,v2);
00134 }
00135 
00136 // Casting Operation
00137 
00140 template <class T> template <class U>
00141 inline SMat3<T>::SMat3(const Mat<U,3,3>& m) : SMat<T,3>(m) {}
00142 
00143 // Named Constructors
00144 
00147 template <class T>
00148 inline SMat3<T> SMat3<T>::fromRows(const Vec3<T>& v0, const Vec3<T>& v1,
00149            const Vec3<T>& v2) {
00150   SMat3<T> m;
00151   m.setRows(v0,v1,v2);
00152   return m;
00153 }
00154 
00157 template <class T>
00158 inline SMat3<T> SMat3<T>::fromCols(const Vec3<T>& v0, const Vec3<T>& v1,
00159            const Vec3<T>& v2) {
00160   SMat3<T> m;
00161   m.setCols(v0,v1,v2);
00162   return m;
00163 }
00164 
00165 // Mutators
00166 
00169 template <class T>
00170 inline void SMat3<T>::set(const T x11, const T x12, const T x13,
00171                           const T x21, const T x22, const T x23,
00172                           const T x31, const T x32, const T x33) {
00173   x[0] = x11; x[1] = x12; x[2] = x13;
00174   x[3] = x21; x[4] = x22; x[5] = x23;
00175   x[6] = x31; x[7] = x32; x[8] = x33;
00176 }
00177 
00180 template <class T>
00181 inline void SMat3<T>::setRows(const Vec3<T>& v0, const Vec3<T>& v1,
00182       const Vec3<T>& v2) {
00183   setRow(0,v0); setRow(1,v1); setRow(2,v2);
00184 }
00185 
00188 template <class T>
00189 inline void SMat3<T>::setCols(const Vec3<T>& v0, const Vec3<T>& v1,
00190       const Vec3<T>& v2) {
00191   setCol(0,v0); setCol(1,v1); setCol(2,v2);
00192 }
00193 
00194 // Determinant and Inverse
00195 
00198 template <class T>
00199 inline  T SMat3<T>::det() const {
00200   return (x[0]*(x[4]*x[8] - x[7]*x[5]) -
00201           x[1]*(x[3]*x[8] - x[6]*x[5]) +
00202           x[2]*(x[3]*x[7] - x[6]*x[4]));
00203 }
00204 
00208 template <class T>
00209 inline SMat3<T> SMat3<T>::inverted() const {
00210   T d = det();
00211   if (d == 0.0) {
00212     throw Exception( "SMat3::inverted(): error, can't take inverse of singular matrix.");
00213   }
00214   T di = T(1)/d;
00215   return SMat3<T>((x[4]*x[8] - x[5]*x[7])*di, (x[2]*x[7] - x[1]*x[8])*di,
00216                   (x[1]*x[5] - x[2]*x[4])*di, (x[5]*x[6] - x[3]*x[8])*di,
00217                   (x[0]*x[8] - x[2]*x[6])*di, (x[2]*x[3] - x[0]*x[5])*di,
00218                   (x[3]*x[7] - x[4]*x[6])*di, (x[1]*x[6] - x[0]*x[7])*di,
00219                   (x[0]*x[4] - x[1]*x[3])*di);
00220 }
00221 
00224 template <class T>
00225 inline int SMat3<T>::invert() {
00226   T d = det();
00227   if (d == 0.0) {
00228     throw Exception( "SMat3::inverted(): error, can't take inverse of singular matrix.");
00229   }
00230   T di = T(1)/d;
00231   set((x[4]*x[8] - x[5]*x[7])*di, (x[2]*x[7] - x[1]*x[8])*di,
00232       (x[1]*x[5] - x[2]*x[4])*di, (x[5]*x[6] - x[3]*x[8])*di,
00233       (x[0]*x[8] - x[2]*x[6])*di, (x[2]*x[3] - x[0]*x[5])*di,
00234       (x[3]*x[7] - x[4]*x[6])*di, (x[1]*x[6] - x[0]*x[7])*di,
00235       (x[0]*x[4] - x[1]*x[3])*di);
00236   return 0;
00237 }
00238 
00239 //==============================================================================
00240 } // namespace rtc
00241 //==============================================================================
00242 #endif // RTC_SMAT3_H defined
00243 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:53