All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Matrix4.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 _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 #include "gazebo/util/system.hh"
26 
27 namespace gazebo
28 {
29  namespace math
30  {
31  class Quaternion;
32  class Pose;
33 
36 
40  {
42  public: Matrix4();
43 
46  public: Matrix4(const Matrix4 &_m);
47 
65  public: Matrix4(double _v00, double _v01, double _v02, double _v03,
66  double _v10, double _v11, double _v12, double _v13,
67  double _v20, double _v21, double _v22, double _v23,
68  double _v30, double _v31, double _v32, double _v33);
69 
71  public: virtual ~Matrix4();
72 
90  public: void Set(double _v00, double _v01, double _v02, double _v03,
91  double _v10, double _v11, double _v12, double _v13,
92  double _v20, double _v21, double _v22, double _v23,
93  double _v30, double _v31, double _v32, double _v33);
94 
95 
96 
99  public: void SetTranslate(const Vector3 &_t);
100 
103  public: Vector3 GetTranslation() const;
104 
107  public: Quaternion GetRotation() const;
108 
111  public: Vector3 GetEulerRotation(unsigned int solution_number = 1) const;
112 
115  public: math::Pose GetAsPose() const;
116 
119  public: void SetScale(const Vector3 &_s);
120 
123  public: bool IsAffine() const;
124 
128  public: Vector3 TransformAffine(const Vector3 &_v) const;
129 
131  public: Matrix4 Inverse() const;
132 
136  public: Matrix4 &operator =(const Matrix4 &_mat);
137 
141  public: const Matrix4 & operator =(const Matrix3 &_mat);
142 
146  public: Matrix4 operator*(const Matrix4 &_mat) const;
147 
151  public: Matrix4 operator*(const Matrix3 &_mat) const;
152 
153 
157  public: Vector3 operator*(const Vector3 &_vec) const;
158 
162  public: inline double *operator[](size_t _row)
163  {
164  assert(_row < 4);
165  return this->m[_row];
166  }
169  public: inline const double *operator[](size_t _row) const
170  {
171  assert(_row < 4);
172  return this->m[_row];
173  }
174 
179  public: bool operator==(const Matrix4 &_m) const;
180 
185  public: friend std::ostream &operator<<(std::ostream &_out,
186  const gazebo::math::Matrix4 &_m)
187  {
188  for (int i = 0; i < 4; i++)
189  {
190  for (int j = 0; j < 4; j++)
191  {
192  _out << (fabs(_m.m[i][j]) < 1e-6 ? 0 : _m.m[i][j]) << " ";
193  }
194  _out << "\n";
195  }
196 
197  return _out;
198  }
199 
201  public: static const Matrix4 IDENTITY;
202 
204  public: static const Matrix4 ZERO;
205 
207  protected: double m[4][4];
208  };
210  }
211 }
212 #endif
213 
214 
215