Matrix4.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 _MATRIX4_HH_
18 #define _MATRIX4_HH_
19
20 #include <assert.h>
21 #include <iostream>
22
23 #include "gazebo/math/Vector3.hh"
24 #include "gazebo/math/Matrix3.hh"
25
26 namespace gazebo
27 {
28  namespace math
29  {
30  class Quaternion;
31  class Pose;
32
35
38  class Matrix4
39  {
41  public: Matrix4();
42
45  public: Matrix4(const Matrix4 &_m);
46
64  public: Matrix4(double _v00, double _v01, double _v02, double _v03,
65  double _v10, double _v11, double _v12, double _v13,
66  double _v20, double _v21, double _v22, double _v23,
67  double _v30, double _v31, double _v32, double _v33);
68
70  public: virtual ~Matrix4();
71
89  public: void Set(double _v00, double _v01, double _v02, double _v03,
90  double _v10, double _v11, double _v12, double _v13,
91  double _v20, double _v21, double _v22, double _v23,
92  double _v30, double _v31, double _v32, double _v33);
93
94
95
98  public: void SetTranslate(const Vector3 &_t);
99
102  public: Vector3 GetTranslation() const;
103
106  public: Quaternion GetRotation() const;
107
110  public: Vector3 GetEulerRotation(unsigned int solution_number = 1) const;
111
114  public: math::Pose GetAsPose() const;
115
118  public: void SetScale(const Vector3 &_s);
119
122  public: bool IsAffine() const;
123
127  public: Vector3 TransformAffine(const Vector3 &_v) const;
128
130  public: Matrix4 Inverse() const;
131
135  public: Matrix4 &operator =(const Matrix4 &_mat);
136
140  public: const Matrix4 & operator =(const Matrix3 &_mat);
141
145  public: Matrix4 operator*(const Matrix4 &_mat) const;
146
150  public: Matrix4 operator*(const Matrix3 &_mat) const;
151
152
156  public: Vector3 operator*(const Vector3 &_vec) const;
157
161  public: inline double *operator[](size_t _row)
162  {
163  assert(_row < 4);
164  return this->m[_row];
165  }
168  public: inline const double *operator[](size_t _row) const
169  {
170  assert(_row < 4);
171  return this->m[_row];
172  }
173
178  public: bool operator==(const Matrix4 &_m) const;
179
184  public: friend std::ostream &operator<<(std::ostream &_out,
185  const gazebo::math::Matrix4 &_m)
186  {
187  for (int i = 0; i < 4; i++)
188  {
189  for (int j = 0; j < 4; j++)
190  {
191  _out << (fabs(_m.m[i][j]) < 1e-6 ? 0 : _m.m[i][j]) << " ";
192  }
193  _out << "\n";
194  }
195
196  return _out;
197  }
198
200  public: static const Matrix4 IDENTITY;
201
203  public: static const Matrix4 ZERO;
204
206  protected: double m[4][4];
207  };
209  }
210 }
211 #endif
212
213
214