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 
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