Matrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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 #include "gazebo/util/system.hh"
24
25 namespace gazebo
26 {
27  namespace math
28  {
31
34  class GZ_MATH_VISIBLE Matrix3
35  {
37  public: Matrix3();
38
41  public: Matrix3(const Matrix3 &_m);
42
53  public: Matrix3(double _v00, double _v01, double _v02,
54  double _v10, double _v11, double _v12,
55  double _v20, double _v21, double _v22);
56
58  public: virtual ~Matrix3();
59
64  public: void SetFromAxes(const Vector3 &_xAxis,
65  const Vector3 &_yAxis,
66  const Vector3 &_zAxis);
67
68
72  public: void SetFromAxis(const Vector3 &_axis, double _angle);
73
77  public: void SetCol(unsigned int _c, const Vector3 &_v);
78
81  public: Matrix3 Inverse() const;
82
84  public: Matrix3 operator-(const Matrix3 &_m) const
85  {
86  return Matrix3(
87  // first row
88  this->m[0][0]-_m[0][0], this->m[0][1]-_m[0][1], this->m[0][2]-_m[0][2],
89  this->m[1][0]-_m[1][0], this->m[1][1]-_m[1][1], this->m[1][2]-_m[1][2],
90  this->m[2][0]-_m[2][0], this->m[2][1]-_m[2][1], this->m[2][2]-_m[2][2]);
91  }
92
94  public: Matrix3 operator+(const Matrix3 &_m) const
95  {
96  return Matrix3(
97  // first row
98  this->m[0][0]+_m[0][0], this->m[0][1]+_m[0][1], this->m[0][2]+_m[0][2],
99  this->m[1][0]+_m[1][0], this->m[1][1]+_m[1][1], this->m[1][2]+_m[1][2],
100  this->m[2][0]+_m[2][0], this->m[2][1]+_m[2][1], this->m[2][2]+_m[2][2]);
101  }
102
104  public: Matrix3 operator*(const double &_s) const
105  {
106  return Matrix3(
107  // first row
108  _s * this->m[0][0], _s * this->m[0][1], _s * this->m[0][2],
109  _s * this->m[1][0], _s * this->m[1][1], _s * this->m[1][2],
110  _s * this->m[2][0], _s * this->m[2][1], _s * this->m[2][2]);
111  }
112
117  public: friend inline Matrix3 operator*(double _s,
118  const Matrix3 &_m)
119  { return _m * _s; }
120
124  public: Matrix3 operator*(const Matrix3 &_m) const
125  {
126  return Matrix3(
127  // first row
128  this->m[0][0]*_m[0][0]+this->m[0][1]*_m[1][0]+this->m[0][2]*_m[2][0],
129  this->m[0][0]*_m[0][1]+this->m[0][1]*_m[1][1]+this->m[0][2]*_m[2][1],
130  this->m[0][0]*_m[0][2]+this->m[0][1]*_m[1][2]+this->m[0][2]*_m[2][2],
131  // second row
132  this->m[1][0]*_m[0][0]+this->m[1][1]*_m[1][0]+this->m[1][2]*_m[2][0],
133  this->m[1][0]*_m[0][1]+this->m[1][1]*_m[1][1]+this->m[1][2]*_m[2][1],
134  this->m[1][0]*_m[0][2]+this->m[1][1]*_m[1][2]+this->m[1][2]*_m[2][2],
135  // third row
136  this->m[2][0]*_m[0][0]+this->m[2][1]*_m[1][0]+this->m[2][2]*_m[2][0],
137  this->m[2][0]*_m[0][1]+this->m[2][1]*_m[1][1]+this->m[2][2]*_m[2][1],
138  this->m[2][0]*_m[0][2]+this->m[2][1]*_m[1][2]+this->m[2][2]*_m[2][2]);
139  }
140
144  public: bool operator==(const Matrix3 &_m) const;
145
149  public: inline math::Vector3 operator*(const math::Vector3 &_v) const
150  {
151  return math::Vector3(
152  this->m[0][0]*_v.x + this->m[0][1]*_v.y + this->m[0][2]*_v.z,
153  this->m[1][0]*_v.x + this->m[1][1]*_v.y + this->m[1][2]*_v.z,
154  this->m[2][0]*_v.x + this->m[2][1]*_v.y + this->m[2][2]*_v.z);
155  }
156
160  public: inline const double *operator[](size_t _row) const
161  {
162  assert(_row < 3);
163  return this->m[_row];
164  }
165
169  public: inline double *operator[](size_t _row)
170  {
171  assert(_row < 3);
172  return this->m[_row];
173  }
174
175
180  public: friend std::ostream &operator<<(std::ostream &_out,
181  const gazebo::math::Matrix3 &_m)
182  {
183  for (int i = 0; i < 3; i++)
184  {
185  for (int j = 0; j < 3; j++)
186  {
187  _out << _m.m[i][j] << " ";
188  }
189  _out << "\n";
190  }
191
192  return _out;
193  }
194
196  public: static const Matrix3 IDENTITY;
197
199  public: static const Matrix3 ZERO;
200
202  protected: double m[3][3];
203
204  friend class Matrix4;
205  };
207  }
208 }
209 #endif
210
211
212
double x
X location.
Definition: Vector3.hh:311
const double * operator[](size_t _row) const
Array subscript operator.
Definition: Matrix3.hh:160
double y
Y location.
Definition: Vector3.hh:314
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
A 3x3 matrix class.
Definition: Matrix4.hh:40
friend Matrix3 operator*(double _s, const Matrix3 &_m)
Multiplication operators.
Definition: Matrix3.hh:117
Matrix3 operator*(const double &_s) const
returns the element wise scalar multiplication
Definition: Matrix3.hh:104
static const Matrix3 IDENTITY
Identity matrix.
Definition: Matrix3.hh:196
double z
Z location.
Definition: Vector3.hh:317
Matrix3 operator*(const Matrix3 &_m) const
Matrix multiplication operator.
Definition: Matrix3.hh:124
A 3x3 matrix class.
Definition: Matrix3.hh:34
math::Vector3 operator*(const math::Vector3 &_v) const
Matrix times Vector3 operator.
Definition: Matrix3.hh:149
double m[3][3]
the 3x3 matrix
Definition: Matrix3.hh:202
double * operator[](size_t _row)
Array subscript operator.
Definition: Matrix3.hh:169
Matrix3 operator+(const Matrix3 &_m) const
returns the element wise sum of two matrices
Definition: Matrix3.hh:94
friend std::ostream & operator<<(std::ostream &_out, const gazebo::math::Matrix3 &_m)
Stream insertion operator.
Definition: Matrix3.hh:180
Matrix3 operator-(const Matrix3 &_m) const
returns the element wise difference of two matrices
Definition: Matrix3.hh:84
static const Matrix3 ZERO
Zero matrix.
Definition: Matrix3.hh:199