1 // -*- c-basic-offset: 4 -*-
2 /** @file Matrix3.h
3 *
4 * @author Alexandre Jenny <alexandre.jenny@le-geo.com>
5 *
6 * $Id$
7 *
8 * This is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU General Public
10 * License as published by the Free Software Foundation; either
11 * version 2 of the License, or (at your option) any later version.
12 *
13 * This software is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 * Lesser General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public
19 * License along with this software. If not, see
20 * <http://www.gnu.org/licenses/>.
21 *
22 */
23
24 #ifndef _HUGIN_MATH_MATRIX3_H_
25 #define _HUGIN_MATH_MATRIX3_H_
26
27 #include <hugin_shared.h>
28 #include <math.h>
29 #include <hugin_math/Vector3.h>
30
31
32 /** general : Matrix3 is a class for handling 3x3 Matrix manipulation.
33 *
34 * We do not use 4x4 matrix for view point changement as the calculus could be inefficent
35 * (some of the coefficients are null, m14 = m24 = m34 = 0 et m44 = 1.0 always).
36 */
37 class IMPEX Matrix3
38 {
39 public:
40 /** we define the Matrix3 as 3 colums of 3 rows */
41 double m[3][3];
42
43 static Matrix3 Identity;
44
45 public:
46 /** default constructor : initialise to zero */
47 Matrix3();
48
49 /** copy constructor */
50 Matrix3(const Matrix3& ot);
51
52 /** Set the identity matrix */
53 void SetIdentity();
54
55 /** Set the matrice to rotation using yaw, pitch, roll angle */
56 void SetRotation( double Yaw, double Pitch, double Roll );
57
58 #if 0
59 /** Set the matrice to rotation using yaw, pitch, roll angle, panotools style,
60 * copied from Panotools-Script by Bruno Postle
61 */
62 void SetRotationPT( double Yaw, double Pitch, double Roll );
63 #endif
64
65
66 // [Ippei note]: Why the hell is this a method of general Matrix3 class?
67 // Should be subclassed or externally provided
68 // eg. static Matrix3 RotationMatrixPanoCommand::makeRotationMatrixPT(double yaw, double pitch, double roll)
69
70 /** set rotation in panotools style,
71 * code adapted from Panotools-Script by Bruno Postle
72 */
73 void SetRotationPT( double yaw, double pitch, double roll );
74
75 /** GetRotation in panotools style. */
76 void GetRotationPT( double & Yaw, double & Pitch, double & Roll );
77
78
79 /** set the matrice to rotation around X */
80 void SetRotationX( double a );
81
82 void SetRotationY( double a );
83
84 void SetRotationZ( double a );
85
86 /** copy operator */
87 Matrix3& operator= (const Matrix3& ot);
88
89 /** multiplication with another matrix */
90 Matrix3 operator*(const Matrix3& ot) const;
91
92 /** operator *= */
93 void operator/=(double s);
94
95 /** operator *= */
96 void operator*=(double s);
97
98 /** operator *= */
99 void operator*=(Matrix3 ot);
100
101 /** comparison */
102 inline bool operator==(Matrix3& ot) const
103 {
104 for(int i=0; i<3; i++)
105 for(int j=0; j<3; j++)
106 if(m[i][j] != ot.m[i][j])
107 return false;
108 return true;
109 }
110
111 /** comparison */
112 inline bool operator!=(Matrix3& ot) const
113 {
114 return !(*this == ot);
115 }
116
117 /** retrieves transpose */
Transpose()118 inline Matrix3 Transpose()
119 {
120 Matrix3 Result;
121 Result.m[0][0] = m[0][0];
122 Result.m[0][1] = m[1][0];
123 Result.m[0][2] = m[2][0];
124 Result.m[1][0] = m[0][1];
125 Result.m[1][1] = m[1][1];
126 Result.m[1][2] = m[2][1];
127 Result.m[2][0] = m[0][2];
128 Result.m[2][1] = m[1][2];
129 Result.m[2][2] = m[2][2];
130 return Result;
131 }
132
133 /** get the determinant */
Determinant()134 inline double Determinant() const
135 {
136 double result = m[0][0] * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] );
137 result -= m[1][0] * ( m[0][1] * m[2][2] - m[2][1] * m[0][2] );
138 result += m[2][0] * ( m[0][1] * m[1][2] - m[1][1] * m[0][2] );
139 return result;
140 }
141
142 /** transforms a vector */
TransformVector(const Vector3 & V)143 inline Vector3 TransformVector(const Vector3 &V) const
144 {
145 Vector3 Result;
146 Result.x = V.x * m[0][0] + V.y * m[1][0] + V.z * m[2][0];
147 Result.y = V.x * m[0][1] + V.y * m[1][1] + V.z * m[2][1];
148 Result.z = V.x * m[0][2] + V.y * m[1][2] + V.z * m[2][2];
149 return Result;
150 }
151
152 /** return inverse if it exists, otherwise identity */
153 Matrix3 Inverse() const;
154
155 ///
156 void Print(std::ostream & o) const;
157
158 };
159
160 /** return the rotation matrix around vector U : checked */
GetRotationAroundU(const Vector3 & U,double Angle)161 inline Matrix3 GetRotationAroundU(const Vector3& U, double Angle)
162 {
163 // is debugged and optimized
164 Vector3 u = U.GetNormalized();
165 if (u.Norm()<0.01) {
166 Matrix3 r;
167 r.SetIdentity();
168 return r;
169 }
170 double cs, ss, ux2, uy2, uz2, uxy, uxz, uyz;
171
172 cs = cos(Angle);
173 ss = sin(Angle);
174 ux2 = u.x*u.x;
175 uy2 = u.y*u.y;
176 uz2 = u.z*u.z;
177 uxy = u.x*u.y;
178 uxz = u.x*u.z;
179 uyz = u.y*u.z;
180 Matrix3 m;
181 m.m[0][0] = ux2 + cs*(1-ux2);
182 m.m[1][0] = uxy*(1-cs) - u.z*ss;
183 m.m[2][0] = uxz*(1-cs) + u.y*ss;
184 m.m[0][1] = uxy*(1-cs) + u.z*ss;
185 m.m[1][1] = uy2 + cs*(1-uy2);
186 m.m[2][1] = uyz*(1-cs)-u.x*ss;
187 m.m[0][2] = uxz*(1-cs)-u.y*ss;
188 m.m[1][2] = uyz*(1-cs)+u.x*ss;
189 m.m[2][2] = uz2 + cs*(1-uz2);
190 return m;
191 /*
192 return Matrix3( ux2 + cs*(1-ux2),
193 uxy*(1-cs) - u.z*ss,
194 uxz*(1-cs) + u.y*ss,
195 uxy*(1-cs) + u.z*ss,
196 uy2 + cs*(1-uy2),
197 uyz*(1-cs)-u.x*ss,
198 uxz*(1-cs)-u.y*ss,
199 uyz*(1-cs)+u.x*ss,
200 uz2 + cs*(1-uz2) );
201 */
202 }
203
GetRotationAroundU(const Vector3 & da)204 inline Matrix3 GetRotationAroundU(const Vector3& da)
205 {
206 return GetRotationAroundU(da, da.Norm());
207 }
208
209 /*// return the rotation matrix around X
210 Matrix3 GetRotationX(double Ang)
211 {
212 double a = cos(Ang);
213 double b = sin(Ang);
214 return Matrix3(
215 1.0, 0.0, 0.0,
216 0.0, a, b,
217 0.0, -b, a );
218 }
219
220 //return the rotation matrix around Y
221 Matrix3 GetRotationY(double Ang)
222 {
223 double a = cos(Ang);
224 double b = -sin(Ang);
225 return Matrix3(
226 a, 0.0, b,
227 0.0, 1.0, 0.0,
228 -b, 0.0, a );
229 }
230
231 //return the rotation matrix around Z
232 Matrix3 GetRotationZ(double Ang)
233 {
234 double a = cos(Ang);
235 double b = sin(Ang);
236 return Matrix3(
237 a, b, 0.0,
238 -b, a, 0.0,
239 0.0, 0.0, 1.0);
240 }
241 */
242
243 inline std::ostream & operator<<(std::ostream & s, const Matrix3 & m)
244 {
245 m.Print(s);
246 return s;
247 }
248
249 #endif // _H
250