Hugintrunk  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Matrix3.cpp
Go to the documentation of this file.
1 // -*- c-basic-offset: 4 -*-
24 #include "Matrix3.h"
25 
26 inline Matrix3 getIdentity()
27 {
28  Matrix3 tmp;
29  tmp.SetIdentity();
30  return tmp;
31 }
32 Matrix3 Matrix3::Identity = getIdentity();
33 
34 
36 Matrix3::Matrix3() {
37  for (int i=0; i<3; i++)
38  for (int j=0; j<3; j++)
39  m[i][j] = 0.0;
40 }
41 
43 Matrix3::Matrix3(const Matrix3& ot)
44 {
45  (*this) = ot; // call copy operator
46 }
47 
49 void Matrix3::SetIdentity()
50 {
51  m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;
52  m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;
53  m[2][0] = 0; m[2][1] = 0; m[2][2] = 1;
54 }
55 
57 void Matrix3::SetRotation( double Yaw, double Pitch, double Roll )
58 {
59  double SR = sin(Roll),
60  SP = sin(Pitch),
61  SY = sin(Yaw),
62  CR = cos(Roll),
63  CP = cos(Pitch),
64  CY = cos(Yaw);
65 
66  m[0][0] = CP * CY;
67  m[0][1] = CP * SY;
68  m[0][2] = SP;
69 
70  m[1][0] = SR * SP * CY - CR * SY;
71  m[1][1] = SR * SP * SY + CR * CY;
72  m[1][2] = - SR * CP;
73 
74  m[2][0] = -( CR * SP * CY + SR * SY );
75  m[2][1] = CY * SR - CR * SP * SY;
76  m[2][2] = CR * CP;
77 }
78 
79 #if 0
80 
83 Matrix3::void SetRotationPT( double Yaw, double Pitch, double Roll )
84 {
85  double SR = sin(Roll),
86  SP = sin(Pitch),
87  SY = sin(Yaw),
88  CR = cos(Roll),
89  CP = cos(Pitch),
90  CY = cos(Yaw);
91 
92  m[0][0] = CP * CY;
93  m[0][1] = CP * SY;
94  m[0][2] = -SP;
95 
96  m[1][0] = SR * SP * CY - CR * SY;
97  m[1][1] = SR * SP * SY + CR * CY;
98  m[1][2] = SR * CP;
99 
100  m[2][0] = CR * SP * CY + SR * SY;
101  m[2][1] = CR * SP * SY - CY * SR;
102  m[2][2] = CR * CP;
103 }
104 #endif
105 
106 
110 void Matrix3::SetRotationPT( double yaw, double pitch, double roll )
111 {
112  double cosr = cos (roll);
113  double sinr = sin (roll);
114  double cosp = cos (pitch);
115  double sinp = sin (0 - pitch);
116  double cosy = cos (yaw);
117  double siny = sin (0 - yaw);
118 
119  Matrix3 rollm;
120 
121  /*
122  rollm[0][0] = new Math::Matrix ([ 1, 0, 0 ],
123  [ 0, cosr,-1*sinr ],
124  [ 0, sinr, cosr ]);
125  */
126 
127  rollm.m[0][0] = 1.0; rollm.m[0][1] = 0.0; rollm.m[0][2] = 0.0;
128  rollm.m[1][0] = 0.0; rollm.m[1][1] = cosr; rollm.m[1][2] = -sinr;
129  rollm.m[2][0] = 0.0; rollm.m[2][1] = sinr; rollm.m[2][2] = cosr;
130 
131  /*
132 my pitchm = new Math::Matrix ([ cosp, 0, sinp ],
133  [ 0, 1, 0 ],
134  [ -1*sinp, 0, cosp ]);
135  */
136 
137  Matrix3 pitchm;
138  pitchm.m[0][0] = cosp; pitchm.m[0][1] = 0.0; pitchm.m[0][2] = sinp;
139  pitchm.m[1][0] = 0.0; pitchm.m[1][1] = 1; pitchm.m[1][2] = 0.0;
140  pitchm.m[2][0] = -sinp; pitchm.m[2][1] = 0.0; pitchm.m[2][2] = cosp;
141 
142  /*
143 my yawm = new Math::Matrix ([ cosy,-1*siny, 0 ],
144  [ siny, cosy, 0 ],
145  [ 0, 0, 1 ]);
146  */
147  Matrix3 yawm;
148  yawm.m[0][0] = cosy; yawm.m[0][1] = -siny; yawm.m[0][2] = 0.0;
149  yawm.m[1][0] = siny; yawm.m[1][1] = cosy; yawm.m[1][2] = 0.0;
150  yawm.m[2][0] = 0.0; yawm.m[2][1] = 0.0; yawm.m[2][2] = 1.0;
151 
152 
153  *this = yawm * pitchm * rollm;
154 }
155 
157 void Matrix3::GetRotationPT( double & Yaw, double & Pitch, double & Roll )
158 {
159  /*
160  my $matrix = shift;
161  my $roll = atan2 ($matrix->[2]->[1], $matrix->[2]->[2]);
162  my $pitch = -1 * asin (-1 * $matrix->[2]->[0]);
163  my $yaw = atan2 (-1 * $matrix->[1]->[0], $matrix->[0]->[0]);
164  return ($roll, $pitch, $yaw);
165 
166  */
167  Roll = atan2 (m[2][1], m[2][2]);
168  Pitch = - asin (- m[2][0]);
169  Yaw = atan2 (- m[1][0], m[0][0]);
170 }
171 
173 void Matrix3::SetRotationX( double a )
174 {
175  m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0;
176  m[1][0] = 0.0; m[1][1] = cos(a); m[1][2] = sin(a);
177  m[2][0] = 0.0; m[2][1] =-m[1][2]; m[2][2] = m[1][1];
178 }
179 
180 void Matrix3::SetRotationY( double a )
181 {
182  m[0][0] = cos(a); m[0][1] = 0.0; m[0][2] =-sin(a);
183  m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0;
184  m[2][0] =-m[0][2]; m[2][1] = 0.0; m[2][2] = m[0][0];
185 }
186 
187 void Matrix3::SetRotationZ( double a )
188 {
189  m[0][0] = cos(a); m[0][1] = sin(a); m[0][2] = 0.0;
190  m[1][0] =-m[0][1]; m[1][1] = m[0][0]; m[1][2] = 0.0;
191  m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0;
192 }
193 
195 Matrix3& Matrix3::operator= (const Matrix3& ot)
196 {
197  for (int i=0; i<3; i++)
198  for (int j=0; j<3; j++)
199  m[i][j] = ot.m[i][j];
200  return *this;
201 }
202 
204 Matrix3 Matrix3::operator*(const Matrix3& ot) const
205 {
206  Matrix3 Result;
207  Result.m[0][0] = m[0][0] * ot.m[0][0] + m[0][1] * ot.m[1][0] + m[0][2] * ot.m[2][0];
208  Result.m[0][1] = m[0][0] * ot.m[0][1] + m[0][1] * ot.m[1][1] + m[0][2] * ot.m[2][1];
209  Result.m[0][2] = m[0][0] * ot.m[0][2] + m[0][1] * ot.m[1][2] + m[0][2] * ot.m[2][2];
210 
211  Result.m[1][0] = m[1][0] * ot.m[0][0] + m[1][1] * ot.m[1][0] + m[1][2] * ot.m[2][0];
212  Result.m[1][1] = m[1][0] * ot.m[0][1] + m[1][1] * ot.m[1][1] + m[1][2] * ot.m[2][1];
213  Result.m[1][2] = m[1][0] * ot.m[0][2] + m[1][1] * ot.m[1][2] + m[1][2] * ot.m[2][2];
214 
215  Result.m[2][0] = m[2][0] * ot.m[0][0] + m[2][1] * ot.m[1][0] + m[2][2] * ot.m[2][0];
216  Result.m[2][1] = m[2][0] * ot.m[0][1] + m[2][1] * ot.m[1][1] + m[2][2] * ot.m[2][1];
217  Result.m[2][2] = m[2][0] * ot.m[0][2] + m[2][1] * ot.m[1][2] + m[2][2] * ot.m[2][2];
218  return Result;
219 }
220 
222 void Matrix3::operator/=(double s)
223 {
224  for (int i=0; i<3; i++)
225  for (int j=0; j<3; j++)
226  m[i][j] /= s;
227 }
228 
230 void Matrix3::operator*=(double s)
231 {
232  for (int i=0; i<3; i++)
233  for (int j=0; j<3; j++)
234  m[i][j] *= s;
235 }
236 
238 void Matrix3::operator*=(Matrix3 ot)
239 {
240  Matrix3 Result;
241  Result.m[0][0] = m[0][0] * ot.m[0][0] + m[0][1] * ot.m[1][0] + m[0][2] * ot.m[2][0];
242  Result.m[0][1] = m[0][0] * ot.m[0][1] + m[0][1] * ot.m[1][1] + m[0][2] * ot.m[2][1];
243  Result.m[0][2] = m[0][0] * ot.m[0][2] + m[0][1] * ot.m[1][2] + m[0][2] * ot.m[2][2];
244 
245  Result.m[1][0] = m[1][0] * ot.m[0][0] + m[1][1] * ot.m[1][0] + m[1][2] * ot.m[2][0];
246  Result.m[1][1] = m[1][0] * ot.m[0][1] + m[1][1] * ot.m[1][1] + m[1][2] * ot.m[2][1];
247  Result.m[1][2] = m[1][0] * ot.m[0][2] + m[1][1] * ot.m[1][2] + m[1][2] * ot.m[2][2];
248 
249  Result.m[2][0] = m[2][0] * ot.m[0][0] + m[2][1] * ot.m[1][0] + m[2][2] * ot.m[2][0];
250  Result.m[2][1] = m[2][0] * ot.m[0][1] + m[2][1] * ot.m[1][1] + m[2][2] * ot.m[2][1];
251  Result.m[2][2] = m[2][0] * ot.m[0][2] + m[2][1] * ot.m[1][2] + m[2][2] * ot.m[2][2];
252  *this = Result;
253 }
254 
256 Matrix3 Matrix3::Inverse() const
257 {
258  Matrix3 Result;
259  double Det = Determinant();
260 
261  if (Det == 0.0f)
262  return Matrix3::Identity;
263 
264  double invDet = 1.0f / Det;
265 
266  Result.m[0][0] = invDet * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] );
267  Result.m[0][1] = -invDet * ( m[0][1] * m[2][2] - m[2][1] * m[0][2] );
268  Result.m[0][2] = invDet * ( m[0][1] * m[1][2] - m[1][1] * m[0][2] );
269 
270  Result.m[1][0] = -invDet * ( m[1][0] * m[2][2] - m[2][0] * m[1][2] );
271  Result.m[1][1] = invDet * ( m[0][0] * m[2][2] - m[2][0] * m[0][2] );
272  Result.m[1][2] = -invDet * ( m[0][0] * m[1][2] - m[1][0] * m[0][2] );
273 
274  Result.m[2][0] = invDet * ( m[1][0] * m[2][1] - m[2][0] * m[1][1] );
275  Result.m[2][1] = -invDet * ( m[0][0] * m[2][1] - m[2][0] * m[0][1] );
276  Result.m[2][2] = invDet * ( m[0][0] * m[1][1] - m[1][0] * m[0][1] );
277  return Result;
278 }
279 
280 void Matrix3::Print(std::ostream & o) const
281 {
282  o << "[ " << m[0][0] << "\t" << m[0][1] << "\t" << m[0][2] << std::endl
283  << " " << m[1][0] << "\t" << m[1][1] << "\t" << m[1][2] << std::endl
284  << " " << m[2][0] << "\t" << m[2][1] << "\t" << m[2][2] << std::endl;
285 }
286 
287 
288 /*// return the rotation matrix around X
289 Matrix3 GetRotationX(double Ang)
290 {
291  double a = cos(Ang);
292  double b = sin(Ang);
293  return Matrix3(
294  1.0, 0.0, 0.0,
295  0.0, a, b,
296  0.0, -b, a );
297 }
298 
299 //return the rotation matrix around Y
300 Matrix3 GetRotationY(double Ang)
301 {
302  double a = cos(Ang);
303  double b = -sin(Ang);
304  return Matrix3(
305  a, 0.0, b,
306  0.0, 1.0, 0.0,
307  -b, 0.0, a );
308 }
309 
310 //return the rotation matrix around Z
311 Matrix3 GetRotationZ(double Ang)
312 {
313  double a = cos(Ang);
314  double b = sin(Ang);
315  return Matrix3(
316  a, b, 0.0,
317  -b, a, 0.0,
318  0.0, 0.0, 1.0);
319 }
320 */
Matrix3::m
double m[3][3]
we define the Matrix3 as 3 colums of 3 rows
Definition: Matrix3.h:41
Matrix3::Identity
static Matrix3 Identity
Definition: Matrix3.h:43
Matrix3::operator*
Matrix3 operator*(const Matrix3 &ot) const
multiplication with another matrix
Definition: Matrix3.cpp:204
Matrix3::Print
void Print(std::ostream &o) const
Definition: Matrix3.cpp:280
Matrix3::Determinant
double Determinant() const
get the determinant
Definition: Matrix3.h:134
Matrix3::operator=
Matrix3 & operator=(const Matrix3 &ot)
copy operator
Definition: Matrix3.cpp:195
Matrix3
general : Matrix3 is a class for handling 3x3 Matrix manipulation.
Definition: Matrix3.h:37
Matrix3::operator/=
void operator/=(double s)
operator *=
Definition: Matrix3.cpp:222
Matrix3::GetRotationPT
void GetRotationPT(double &Yaw, double &Pitch, double &Roll)
GetRotation in panotools style.
Definition: Matrix3.cpp:157
Matrix3::operator*=
void operator*=(double s)
operator *=
Definition: Matrix3.cpp:230
Matrix3::SetRotationPT
void SetRotationPT(double yaw, double pitch, double roll)
set rotation in panotools style, code adapted from Panotools-Script by Bruno Postle ...
Definition: Matrix3.cpp:110
Matrix3::SetRotationX
void SetRotationX(double a)
set the matrice to rotation around X
Definition: Matrix3.cpp:173
Matrix3::SetIdentity
void SetIdentity()
Set the identity matrix.
Definition: Matrix3.cpp:49
Matrix3::SetRotationZ
void SetRotationZ(double a)
Definition: Matrix3.cpp:187
Yaw
double Yaw
Definition: image_variables.h:80
Matrix3::Matrix3
Matrix3()
default constructor : initialise to zero
Definition: Matrix3.cpp:36
Matrix3::SetRotationY
void SetRotationY(double a)
Definition: Matrix3.cpp:180
Matrix3::Inverse
Matrix3 Inverse() const
return inverse if it exists, otherwise identity
Definition: Matrix3.cpp:256
Roll
double Roll
Definition: image_variables.h:78
Matrix3::SetRotation
void SetRotation(double Yaw, double Pitch, double Roll)
Set the matrice to rotation using yaw, pitch, roll angle.
Definition: Matrix3.cpp:57
Pitch
double Pitch
Definition: image_variables.h:79
getIdentity
Matrix3 getIdentity()
Definition: Matrix3.cpp:26
Generated on Sun Nov 17 2024 01:25:43 for Hugintrunk by   doxygen 1.8.5