Matrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2012 Open Source Robotics Foundation
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef _MATRIX3_HH_
18 #define _MATRIX3_HH_
19
20 #include <assert.h>
21
22 #include "gazebo/math/Vector3.hh"
23
24 namespace gazebo
25 {
26  namespace math
27  {
30
33  class Matrix3
34  {
36  public: Matrix3();
37
40  public: Matrix3(const Matrix3 &_m);
41
52  public: Matrix3(double _v00, double _v01, double _v02,
53  double _v10, double _v11, double _v12,
54  double _v20, double _v21, double _v22);
55
57  public: virtual ~Matrix3();
58
63  public: void SetFromAxes(const Vector3 &_xAxis,
64  const Vector3 &_yAxis,
65  const Vector3 &_zAxis);
66
67
71  public: void SetFromAxis(const Vector3 &_axis, double _angle);
72
76  public: void SetCol(unsigned int _c, const Vector3 &_v);
77
79  public: Matrix3 operator-(const Matrix3 &_m) const
80  {
81  return Matrix3(
82  // first row
83  this->m[0][0]-_m[0][0], this->m[0][1]-_m[0][1], this->m[0][2]-_m[0][2],
84  this->m[1][0]-_m[1][0], this->m[1][1]-_m[1][1], this->m[1][2]-_m[1][2],
85  this->m[2][0]-_m[2][0], this->m[2][1]-_m[2][1], this->m[2][2]-_m[2][2]);
86  }
87
89  public: Matrix3 operator+(const Matrix3 &_m) const
90  {
91  return Matrix3(
92  // first row
93  this->m[0][0]+_m[0][0], this->m[0][1]+_m[0][1], this->m[0][2]+_m[0][2],
94  this->m[1][0]+_m[1][0], this->m[1][1]+_m[1][1], this->m[1][2]+_m[1][2],
95  this->m[2][0]+_m[2][0], this->m[2][1]+_m[2][1], this->m[2][2]+_m[2][2]);
96  }
97
99  public: Matrix3 operator*(const double &_s) const
100  {
101  return Matrix3(
102  // first row
103  _s * this->m[0][0], _s * this->m[0][1], _s * this->m[0][2],
104  _s * this->m[1][0], _s * this->m[1][1], _s * this->m[1][2],
105  _s * this->m[2][0], _s * this->m[2][1], _s * this->m[2][2]);
106  }
107
112  public: friend inline Matrix3 operator*(double _s,
113  const Matrix3 &_m)
114  { return _m * _s; }
115
119  public: Matrix3 operator*(const Matrix3 &_m) const
120  {
121  return Matrix3(
122  // first row
123  this->m[0][0]*_m[0][0]+this->m[0][1]*_m[1][0]+this->m[0][2]*_m[2][0],
124  this->m[0][0]*_m[0][1]+this->m[0][1]*_m[1][1]+this->m[0][2]*_m[2][1],
125  this->m[0][0]*_m[0][2]+this->m[0][1]*_m[1][2]+this->m[0][2]*_m[2][2],
126  // second row
127  this->m[1][0]*_m[0][0]+this->m[1][1]*_m[1][0]+this->m[1][2]*_m[2][0],
128  this->m[1][0]*_m[0][1]+this->m[1][1]*_m[1][1]+this->m[1][2]*_m[2][1],
129  this->m[1][0]*_m[0][2]+this->m[1][1]*_m[1][2]+this->m[1][2]*_m[2][2],
130  // third row
131  this->m[2][0]*_m[0][0]+this->m[2][1]*_m[1][0]+this->m[2][2]*_m[2][0],
132  this->m[2][0]*_m[0][1]+this->m[2][1]*_m[1][1]+this->m[2][2]*_m[2][1],
133  this->m[2][0]*_m[0][2]+this->m[2][1]*_m[1][2]+this->m[2][2]*_m[2][2]);
134  }
135
139  public: bool operator==(const Matrix3 &_m) const;
140
144  public: inline const double *operator[](size_t _row) const
145  {
146  assert(_row < 3);
147  return this->m[_row];
148  }
149
153  public: inline double *operator[](size_t _row)
154  {
155  assert(_row < 3);
156  return this->m[_row];
157  }
158
159
164  public: friend std::ostream &operator<<(std::ostream &_out,
165  const gazebo::math::Matrix3 &_m)
166  {
167  for (int i = 0; i < 3; i++)
168  {
169  for (int j = 0; j < 3; j++)
170  {
171  _out << _m.m[i][j] << " ";
172  }
173  _out << "\n";
174  }
175
176  return _out;
177  }
178
180  protected: double m[3][3];
181
182  friend class Matrix4;
183  };
185  }
186 }
187 #endif
188
189
190