37 for (
int i=0; i<3; i++)
38 for (
int j=0; j<3; j++)
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;
59 double SR = sin(Roll),
70 m[1][0] = SR * SP * CY - CR * SY;
71 m[1][1] = SR * SP * SY + CR * CY;
74 m[2][0] = -( CR * SP * CY + SR * SY );
75 m[2][1] = CY * SR - CR * SP * SY;
83 Matrix3::void SetRotationPT(
double Yaw,
double Pitch,
double Roll )
85 double SR = sin(Roll),
96 m[1][0] = SR * SP * CY - CR * SY;
97 m[1][1] = SR * SP * SY + CR * CY;
100 m[2][0] = CR * SP * CY + SR * SY;
101 m[2][1] = CR * SP * SY - CY * SR;
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);
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;
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;
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;
153 *
this = yawm * pitchm * rollm;
167 Roll = atan2 (
m[2][1],
m[2][2]);
168 Pitch = - asin (-
m[2][0]);
169 Yaw = atan2 (-
m[1][0],
m[0][0]);
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];
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];
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;
197 for (
int i=0; i<3; i++)
198 for (
int j=0; j<3; j++)
199 m[i][j] = ot.
m[i][j];
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];
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];
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];
224 for (
int i=0; i<3; i++)
225 for (
int j=0; j<3; j++)
232 for (
int i=0; i<3; i++)
233 for (
int j=0; j<3; j++)
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];
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];
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];
264 double invDet = 1.0f / Det;
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] );
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] );
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] );
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;
double m[3][3]
we define the Matrix3 as 3 colums of 3 rows
Matrix3 operator*(const Matrix3 &ot) const
multiplication with another matrix
void Print(std::ostream &o) const
double Determinant() const
get the determinant
Matrix3 & operator=(const Matrix3 &ot)
copy operator
general : Matrix3 is a class for handling 3x3 Matrix manipulation.
void operator/=(double s)
operator *=
void GetRotationPT(double &Yaw, double &Pitch, double &Roll)
GetRotation in panotools style.
void operator*=(double s)
operator *=
void SetRotationPT(double yaw, double pitch, double roll)
set rotation in panotools style, code adapted from Panotools-Script by Bruno Postle ...
void SetRotationX(double a)
set the matrice to rotation around X
void SetIdentity()
Set the identity matrix.
void SetRotationZ(double a)
Matrix3()
default constructor : initialise to zero
void SetRotationY(double a)
Matrix3 Inverse() const
return inverse if it exists, otherwise identity
void SetRotation(double Yaw, double Pitch, double Roll)
Set the matrice to rotation using yaw, pitch, roll angle.