All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Matrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
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 
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 
80  public: Matrix3 operator-(const Matrix3 &_m) const
81  {
82  return Matrix3(
83  // first row
84  this->m[0][0]-_m[0][0], this->m[0][1]-_m[0][1], this->m[0][2]-_m[0][2],
85  this->m[1][0]-_m[1][0], this->m[1][1]-_m[1][1], this->m[1][2]-_m[1][2],
86  this->m[2][0]-_m[2][0], this->m[2][1]-_m[2][1], this->m[2][2]-_m[2][2]);
87  }
88 
90  public: Matrix3 operator+(const Matrix3 &_m) const
91  {
92  return Matrix3(
93  // first row
94  this->m[0][0]+_m[0][0], this->m[0][1]+_m[0][1], this->m[0][2]+_m[0][2],
95  this->m[1][0]+_m[1][0], this->m[1][1]+_m[1][1], this->m[1][2]+_m[1][2],
96  this->m[2][0]+_m[2][0], this->m[2][1]+_m[2][1], this->m[2][2]+_m[2][2]);
97  }
98 
100  public: Matrix3 operator*(const double &_s) const
101  {
102  return Matrix3(
103  // first row
104  _s * this->m[0][0], _s * this->m[0][1], _s * this->m[0][2],
105  _s * this->m[1][0], _s * this->m[1][1], _s * this->m[1][2],
106  _s * this->m[2][0], _s * this->m[2][1], _s * this->m[2][2]);
107  }
108 
113  public: friend inline Matrix3 operator*(double _s,
114  const Matrix3 &_m)
115  { return _m * _s; }
116 
120  public: Matrix3 operator*(const Matrix3 &_m) const
121  {
122  return Matrix3(
123  // first row
124  this->m[0][0]*_m[0][0]+this->m[0][1]*_m[1][0]+this->m[0][2]*_m[2][0],
125  this->m[0][0]*_m[0][1]+this->m[0][1]*_m[1][1]+this->m[0][2]*_m[2][1],
126  this->m[0][0]*_m[0][2]+this->m[0][1]*_m[1][2]+this->m[0][2]*_m[2][2],
127  // second row
128  this->m[1][0]*_m[0][0]+this->m[1][1]*_m[1][0]+this->m[1][2]*_m[2][0],
129  this->m[1][0]*_m[0][1]+this->m[1][1]*_m[1][1]+this->m[1][2]*_m[2][1],
130  this->m[1][0]*_m[0][2]+this->m[1][1]*_m[1][2]+this->m[1][2]*_m[2][2],
131  // third row
132  this->m[2][0]*_m[0][0]+this->m[2][1]*_m[1][0]+this->m[2][2]*_m[2][0],
133  this->m[2][0]*_m[0][1]+this->m[2][1]*_m[1][1]+this->m[2][2]*_m[2][1],
134  this->m[2][0]*_m[0][2]+this->m[2][1]*_m[1][2]+this->m[2][2]*_m[2][2]);
135  }
136 
140  public: bool operator==(const Matrix3 &_m) const;
141 
145  public: inline math::Vector3 operator*(const math::Vector3 &_v) const
146  {
147  return math::Vector3(
148  this->m[0][0]*_v.x + this->m[0][1]*_v.y + this->m[0][2]*_v.z,
149  this->m[1][0]*_v.x + this->m[1][1]*_v.y + this->m[1][2]*_v.z,
150  this->m[2][0]*_v.x + this->m[2][1]*_v.y + this->m[2][2]*_v.z);
151  }
152 
156  public: inline const double *operator[](size_t _row) const
157  {
158  assert(_row < 3);
159  return this->m[_row];
160  }
161 
165  public: inline double *operator[](size_t _row)
166  {
167  assert(_row < 3);
168  return this->m[_row];
169  }
170 
171 
176  public: friend std::ostream &operator<<(std::ostream &_out,
177  const gazebo::math::Matrix3 &_m)
178  {
179  for (int i = 0; i < 3; i++)
180  {
181  for (int j = 0; j < 3; j++)
182  {
183  _out << _m.m[i][j] << " ";
184  }
185  _out << "\n";
186  }
187 
188  return _out;
189  }
190 
192  public: static const Matrix3 IDENTITY;
193 
195  public: static const Matrix3 ZERO;
196 
198  protected: double m[3][3];
199 
200  friend class Matrix4;
201  };
203  }
204 }
205 #endif
206 
207 
208