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 2011 Nate Koenig
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 "math/Vector3.hh"
24 #include "math/Matrix3.hh"
25 
26 namespace gazebo
27 {
28  namespace math
29  {
30  class Quaternion;
31  class Pose;
32 
35 
37  class Matrix4
38  {
40  public: Matrix4();
41 
44  public: Matrix4(const Matrix4 &_m);
45 
63  public: Matrix4(double _v00, double _v01, double _v02, double _v03,
64  double _v10, double _v11, double _v12, double _v13,
65  double _v20, double _v21, double _v22, double _v23,
66  double _v30, double _v31, double _v32, double _v33);
67 
69  public: virtual ~Matrix4();
70 
88  public: void Set(double _v00, double _v01, double _v02, double _v03,
89  double _v10, double _v11, double _v12, double _v13,
90  double _v20, double _v21, double _v22, double _v23,
91  double _v30, double _v31, double _v32, double _v33);
92 
93 
94 
97  public: void SetTranslate(const Vector3 &_t);
98 
101  public: Vector3 GetTranslation() const;
102 
105  public: Quaternion GetRotation() const;
106 
109  public: Vector3 GetEulerRotation(unsigned int solution_number = 1) const;
110 
113  public: math::Pose GetAsPose() const;
114 
117  public: void SetScale(const Vector3 &_s);
118 
121  public: bool IsAffine() const;
122 
126  public: Vector3 TransformAffine(const Vector3 &_v) const;
127 
129  public: Matrix4 Inverse() const;
130 
134  public: Matrix4 &operator =(const Matrix4 &_mat);
135 
139  public: const Matrix4 & operator =(const Matrix3 &_mat);
140 
144  public: Matrix4 operator*(const Matrix4 &_mat) const;
145 
149  public: Matrix4 operator*(const Matrix3 &_mat) const;
150 
151 
155  public: Vector3 operator*(const Vector3 &_vec) const;
156 
160  public: inline double *operator[](size_t _row)
161  {
162  assert(_row < 4);
163  return this->m[_row];
164  }
167  public: inline const double *operator[](size_t _row) const
168  {
169  assert(_row < 4);
170  return this->m[_row];
171  }
172 
177  public: bool operator==(const Matrix4 &_m) const;
178 
183  public: friend std::ostream &operator<<(std::ostream &_out,
184  const gazebo::math::Matrix4 &_m)
185  {
186  for (int i = 0; i < 4; i++)
187  {
188  for (int j = 0; j < 4; j++)
189  {
190  _out << (fabs(_m.m[i][j]) < 1e-6 ? 0 : _m.m[i][j]) << " ";
191  }
192  _out << "\n";
193  }
194 
195  return _out;
196  }
197 
199  public: static const Matrix4 IDENTITY;
200 
202  public: static const Matrix4 ZERO;
203 
205  protected: double m[4][4];
206  };
208  }
209 }
210 #endif
211 
212 
213