Class
List
Hierarchy
Modules
Common
Events
Math
Messages
Physics
Rendering
Sensors
Transport
Links
Gazebo Website
Wiki
Tutorials
Download
Report Documentation Issues
All
Classes
Namespaces
Files
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Macros
Groups
Pages
gazebo
math
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
39
class
GAZEBO_VISIBLE
Matrix4
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