sbg_matrix3.h
Go to the documentation of this file.
1 
38 #ifndef SBG_MATRIX_3_H
39 #define SBG_MATRIX_3_H
40 
41 // Sbg headers
42 #include <sbgDefines.h>
43 #include "sbg_vector3.h"
44 
45 //SbgRos message headers
46 #include "sbg_driver/SbgEkfQuat.h"
47 #include "sbg_driver/SbgEkfEuler.h"
48 
49 namespace sbg
50 {
51 
55 template <class T>
57 {
58 private:
59 
60  //---------------------------------------------------------------------//
61  //- Private variables -//
62  //---------------------------------------------------------------------//
63 
64  std::array<T, 9> m_data;
65 
66 public:
67 
68  //---------------------------------------------------------------------//
69  //- Constructor -//
70  //---------------------------------------------------------------------//
71 
75  SbgMatrix3(void)
76  {
77  m_data[0] = static_cast<T>(0.0);
78  m_data[1] = static_cast<T>(0.0);
79  m_data[2] = static_cast<T>(0.0);
80  m_data[3] = static_cast<T>(0.0);
81  m_data[4] = static_cast<T>(0.0);
82  m_data[5] = static_cast<T>(0.0);
83  m_data[6] = static_cast<T>(0.0);
84  m_data[7] = static_cast<T>(0.0);
85  m_data[8] = static_cast<T>(0.0);
86  }
87 
101  SbgMatrix3(T value00, T value01, T value02, T value10, T value11, T value12, T value20, T value21, T value22)
102  {
103  m_data[0] = value00;
104  m_data[1] = value01;
105  m_data[2] = value02;
106  m_data[3] = value10;
107  m_data[4] = value11;
108  m_data[5] = value12;
109  m_data[6] = value20;
110  m_data[7] = value21;
111  m_data[8] = value22;
112  };
113 
120  SbgMatrix3(const T* p_raw_data, size_t array_size)
121  {
122  assert(array_size == 9);
123 
124  m_data[0] = p_raw_data[0];
125  m_data[1] = p_raw_data[1];
126  m_data[2] = p_raw_data[2];
127  m_data[3] = p_raw_data[3];
128  m_data[4] = p_raw_data[4];
129  m_data[5] = p_raw_data[5];
130  m_data[6] = p_raw_data[6];
131  m_data[7] = p_raw_data[7];
132  m_data[8] = p_raw_data[8];
133  };
134 
135  //---------------------------------------------------------------------//
136  //- Parameters -//
137  //---------------------------------------------------------------------//
138 
139  //---------------------------------------------------------------------//
140  //- Operations -//
141  //---------------------------------------------------------------------//
142 
150  const T operator() (int i, int j) const
151  {
152  assert(i * 3 + j < 9);
153 
154  return m_data[i * 3 + j];
155  }
156 
162  const T *data(void) const
163  {
164  return static_cast<const T*>(m_data.data());
165  };
166 
167  const SbgVector3<T> operator*(const SbgVector3<T>& vect) const
168  {
169  T x = m_data[0] * vect(0) + m_data[1] * vect(1) + m_data[2] * vect(2);
170  T y = m_data[3] * vect(0) + m_data[4] * vect(1) + m_data[5] * vect(2);
171  T z = m_data[6] * vect(0) + m_data[7] * vect(1) + m_data[8] * vect(2);
172 
173  return SbgVector3<T>(x, y, z);
174  }
175 
176  void transpose(void)
177  {
178  T swap = m_data[1];
179  m_data[1] = m_data[3];
180  m_data[3] = swap;
181 
182  swap = m_data[2];
183  m_data[2] = m_data[6];
184  m_data[6] = swap;
185 
186  swap = m_data[5];
187  m_data[5] = m_data[7];
188  m_data[7] = swap;
189  }
190 
199  void makeDcm(const SbgVector3f& euler)
200  {
201  float cr = cosf(euler(0));
202  float sr = sinf(euler(0));
203  float cp = cosf(euler(1));
204  float sp = sinf(euler(1));
205  float cy = cosf(euler(2));
206  float sy = sinf(euler(2));
207 
208  m_data[0] = cp * cy;
209  m_data[3] = cp * sy;
210  m_data[6] = -sp;
211 
212  m_data[1] = (sr * sp * cy) - (cr * sy);
213  m_data[4] = (sr * sp * sy) + (cr * cy);
214  m_data[7] = sr * cp;
215 
216  m_data[2] = (cr * sp * cy) + (sy * sr);
217  m_data[5] = (cr * sp * sy) - (sr * cy);
218  m_data[8] = cr * cp;
219 
220  }
221 
230  void makeDcm(float w, float x, float y, float z)
231  {
232  float wx = w * x;
233  float wy = w * y;
234  float wz = w * z;
235  float xy = x * y;
236  float xz = x * z;
237  float yz = y * z;
238 
239  m_data[0] = (2 * powf(w, 2)) + (2 * powf(x, 2)) - 1;
240  m_data[3] = (2 * xy) + (2 * wz);
241  m_data[6] = (2 * xz) - (2 * wy);
242 
243  m_data[1] = (2 * xy) - (2 * wz);
244  m_data[4] = (2 * powf(w, 2)) + (2 * powf(y, 2)) - 1;
245  m_data[7] = (2 * yz) + (2 * wx);
246 
247  m_data[2] = (2 * wy) + (2 * xz);
248  m_data[5] = (2 * yz) - (2 * wx);
249  m_data[8] = (2 * powf(w, 2)) + (2 * powf(z, 2)) - 1;
250  }
251 
252 };
253 
254 
257 
258 }
259 
260 #endif // SBG_MATRIX_3_H
const SbgVector3< T > operator*(const SbgVector3< T > &vect) const
Definition: sbg_matrix3.h:167
SbgMatrix3< double > SbgMatrix3d
Definition: sbg_matrix3.h:256
void transpose(void)
Definition: sbg_matrix3.h:176
std::array< T, 9 > m_data
Definition: sbg_matrix3.h:64
void makeDcm(float w, float x, float y, float z)
Definition: sbg_matrix3.h:230
void makeDcm(const SbgVector3f &euler)
Definition: sbg_matrix3.h:199
SbgMatrix3(T value00, T value01, T value02, T value10, T value11, T value12, T value20, T value21, T value22)
Definition: sbg_matrix3.h:101
Header file that contains all common definitions.
Handle a three components vector.
void swap(scoped_ptr< T > &, scoped_ptr< T > &)
const T operator()(int i, int j) const
Definition: sbg_matrix3.h:150
SbgMatrix3< float > SbgMatrix3f
Definition: sbg_matrix3.h:255
const T * data(void) const
Definition: sbg_matrix3.h:162
SbgMatrix3(const T *p_raw_data, size_t array_size)
Definition: sbg_matrix3.h:120


sbg_driver
Author(s): SBG Systems
autogenerated on Sat Sep 3 2022 02:53:35