Matrix4.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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 _GAZEBO_MATRIX4_HH_
18 #define _GAZEBO_MATRIX4_HH_
19 
20 #include <assert.h>
21 #include <iostream>
22 #include <ignition/math/Matrix4.hh>
23 
24 #include "gazebo/math/Vector3.hh"
25 #include "gazebo/math/Matrix3.hh"
26 #include "gazebo/util/system.hh"
27 
28 namespace gazebo
29 {
30  namespace math
31  {
32  class Quaternion;
33  class Pose;
34 
37 
40  class GZ_MATH_VISIBLE Matrix4
41  {
43  public: Matrix4();
44 
47  public: Matrix4(const Matrix4 &_m);
48 
51  public: Matrix4(const ignition::math::Matrix4d &_m);
52 
70  public: Matrix4(double _v00, double _v01, double _v02, double _v03,
71  double _v10, double _v11, double _v12, double _v13,
72  double _v20, double _v21, double _v22, double _v23,
73  double _v30, double _v31, double _v32, double _v33);
74 
76  public: virtual ~Matrix4();
77 
95  public: void Set(double _v00, double _v01, double _v02, double _v03,
96  double _v10, double _v11, double _v12, double _v13,
97  double _v20, double _v21, double _v22, double _v23,
98  double _v30, double _v31, double _v32, double _v33);
99 
100 
101 
104  public: void SetTranslate(const Vector3 &_t);
105 
108  public: Vector3 GetTranslation() const;
109 
112  public: Quaternion GetRotation() const;
113 
116  public: Vector3 GetEulerRotation(unsigned int solution_number = 1) const;
117 
120  public: math::Pose GetAsPose() const;
121 
124  public: void SetScale(const Vector3 &_s);
125 
128  public: bool IsAffine() const;
129 
133  public: Vector3 TransformAffine(const Vector3 &_v) const;
134 
137  public: Matrix4 Inverse() const;
138 
142  public: Matrix4 &operator =(const Matrix4 &_mat);
143 
147  public: Matrix4 &operator=(const ignition::math::Matrix4d &_mat);
148 
152  public: const Matrix4 & operator =(const Matrix3 &_mat);
153 
157  public: Matrix4 operator*(const Matrix4 &_mat) const;
158 
162  public: Matrix4 operator*(const Matrix3 &_mat) const;
163 
164 
168  public: Vector3 operator*(const Vector3 &_vec) const;
169 
173  public: inline double *operator[](size_t _row)
174  {
175  assert(_row < 4);
176  return this->m[_row];
177  }
180  public: inline const double *operator[](size_t _row) const
181  {
182  assert(_row < 4);
183  return this->m[_row];
184  }
185 
190  public: bool operator==(const Matrix4 &_m) const;
191 
194  public: ignition::math::Matrix4d Ign() const;
195 
200  public: friend std::ostream &operator<<(std::ostream &_out,
201  const gazebo::math::Matrix4 &_m)
202  {
203  for (int i = 0; i < 4; i++)
204  {
205  for (int j = 0; j < 4; j++)
206  {
207  _out << (fabs(_m.m[i][j]) < 1e-6 ? 0 : _m.m[i][j]) << " ";
208  }
209  _out << "\n";
210  }
211 
212  return _out;
213  }
214 
216  public: static const Matrix4 IDENTITY;
217 
219  public: static const Matrix4 ZERO;
220 
222  protected: double m[4][4];
223  };
225  }
226 }
227 #endif
228 
229 
230 
Encapsulates a position and rotation in three space.
Definition: Pose.hh:37
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
static const Matrix4 IDENTITY
Identity matrix.
Definition: Matrix4.hh:216
A 3x3 matrix class.
Definition: Matrix4.hh:40
double * operator[](size_t _row)
Array subscript operator.
Definition: Matrix4.hh:173
A 3x3 matrix class.
Definition: Matrix3.hh:34
A quaternion class.
Definition: Quaternion.hh:42
GAZEBO_VISIBLE void Set(common::Image &_img, const msgs::Image &_msg)
Convert a msgs::Image to a common::Image.
double m[4][4]
The 4x4 matrix.
Definition: Matrix4.hh:222
const double * operator[](size_t _row) const
Definition: Matrix4.hh:180
static const Matrix4 ZERO
Zero matrix.
Definition: Matrix4.hh:219
friend std::ostream & operator<<(std::ostream &_out, const gazebo::math::Matrix4 &_m)
Stream insertion operator.
Definition: Matrix4.hh:200