GLC_lib  2.5.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
glc_matrix4x4.cpp
Go to the documentation of this file.
1 /****************************************************************************
2 
3  This file is part of the GLC-lib library.
4  Copyright (C) 2005-2008 Laurent Ribon (laumaya@users.sourceforge.net)
5  http://glc-lib.sourceforge.net
6 
7  GLC-lib is free software; you can redistribute it and/or modify
8  it under the terms of the GNU Lesser General Public License as published by
9  the Free Software Foundation; either version 3 of the License, or
10  (at your option) any later version.
11 
12  GLC-lib is distributed in the hope that it will be useful,
13  but WITHOUT ANY WARRANTY; without even the implied warranty of
14  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  GNU Lesser General Public License for more details.
16 
17  You should have received a copy of the GNU Lesser General Public License
18  along with GLC-lib; if not, write to the Free Software
19  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 
21 *****************************************************************************/
23 
24 #include "glc_matrix4x4.h"
25 
26 #include <QtDebug>
27 
29 // Set Functions
31 
32 GLC_Matrix4x4& GLC_Matrix4x4::fromEuler(const double angle_x, const double angle_y, const double angle_z)
33 {
34  const double A= cos(angle_x);
35  const double B= sin(angle_x);
36  const double C= cos(angle_y);
37  const double D= sin(angle_y);
38  const double E= cos(angle_z);
39  const double F= sin(angle_z);
40 
41  const double AD= A * D;
42  const double BD= B * D;
43 
44  m_Matrix[0] = C * E;
45  m_Matrix[4] = -C * F;
46  m_Matrix[8] = -D;
47  m_Matrix[1] = -BD * E + A * F;
48  m_Matrix[5] = BD * F + A * E;
49  m_Matrix[9] = -B * C;
50  m_Matrix[2] = AD * E + B * F;
51  m_Matrix[6] = -AD * F + B * E;
52  m_Matrix[10] = A * C;
53 
54  m_Matrix[12]= 0.0; m_Matrix[13]= 0.0; m_Matrix[14]= 0.0; m_Matrix[3]= 0.0; m_Matrix[7]= 0.0; m_Matrix[11] = 0.0;
55  m_Matrix[15] = 1.0;
56 
57  return *this;
58 }
59 
61 {
62  Q_ASSERT(index < 4);
63  index= index * 4;
64  m_Matrix[index]= vector.x();
65  m_Matrix[index + 1]= vector.y();
66  m_Matrix[index + 2]= vector.z();
67 
68  m_Type= General;
69 
70  return *this;
71 }
73 // Private services function
75 
76 QVector<double> GLC_Matrix4x4::toEuler(void) const
77 {
78  double angle_x;
79  double angle_y;
80  double angle_z;
81  double tracex, tracey;
82  angle_y= -asin(m_Matrix[8]);
83  double C= cos(angle_y);
84 
85  if (!(qAbs(C - 0.0) <= glc::EPSILON)) // Gimball lock?
86  {
87  tracex= m_Matrix[10] / C;
88  tracey= - m_Matrix[9] / C;
89  angle_x= atan2( tracey, tracex);
90 
91  tracex= m_Matrix[0] / C;
92  tracey= - m_Matrix[4] / C;
93  angle_z= atan2( tracey, tracex);
94  }
95  else // Gimball lock?
96  {
97  angle_x= 0.0;
98  tracex= m_Matrix[5] / C;
99  tracey= m_Matrix[1] / C;
100  angle_z= atan2( tracey, tracex);
101  }
102  QVector<double> result;
103  result.append(fmod(angle_x, 2.0 * glc::PI));
104  result.append(fmod(angle_y, 2.0 * glc::PI));
105  result.append(fmod(angle_z, 2.0 * glc::PI));
106 
107  return result;
108 }
109 
110 QString GLC_Matrix4x4::toString() const
111 {
112  QString result;
113  for (int i= 0; i < DIMMAT4X4; ++i)
114  {
115  result+= (QString::number(m_Matrix[0 + i])) + QString(" ");
116  result+= (QString::number(m_Matrix[4 + i])) + QString(" ");
117  result+= (QString::number(m_Matrix[8 + i])) + QString(" ");
118  result+= (QString::number(m_Matrix[12 + i])) + QString("\n");
119  }
120  result.remove(result.size() - 1, 1);
121  return result;
122 }
123 
124 QQuaternion GLC_Matrix4x4::quaternion() const
125 {
126  QQuaternion subject;
127  GLC_Matrix4x4 rotMat= rotationMatrix();
128  if ((this->type() != GLC_Matrix4x4::Identity) && (rotMat != GLC_Matrix4x4()))
129  {
130  const double matrixTrace= rotMat.trace();
131  double s, w, x, y, z;
132 
133  if (matrixTrace > 0.0)
134  {
135  s= 0.5 / sqrt(matrixTrace);
136  w= 0.25 / s;
137  x= (rotMat.m_Matrix[9] - rotMat.m_Matrix[6]) * s;
138  y= (rotMat.m_Matrix[2] - rotMat.m_Matrix[8]) * s;
139  z= (rotMat.m_Matrix[4] - rotMat.m_Matrix[1]) * s;
140  }
141  else
142  {
143  if ((abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[5])) && (abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[15])))
144  { // column 0 greater
145  s= sqrt(1.0 + rotMat.m_Matrix[0] - rotMat.m_Matrix[5] - rotMat.m_Matrix[10]) * 2.0;
146 
147  w= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9] ) / s;
148  x= 0.5 / s;
149  y= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4] ) / s;
150  z= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8] ) / s;
151  }
152  else if ((abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[0])) && (abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[15])))
153  { // column 1 greater
154  s= sqrt(1.0 + rotMat.m_Matrix[5] - rotMat.m_Matrix[0] - rotMat.m_Matrix[10]) * 2.0;
155 
156  w= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
157  x= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
158  y= 0.5 / s;
159  z= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
160  }
161  else
162  { // column 3 greater
163  s= sqrt(1.0 + rotMat.m_Matrix[10] - rotMat.m_Matrix[0] - rotMat.m_Matrix[5]) * 2.0;
164 
165  w = (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
166  x = (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
167  y = (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
168  z = 0.5 / s;
169  }
170  }
171  subject= QQuaternion(w, x, y, z);
172  }
173 
174  return subject;
175 }
176 
177 QPair<GLC_Vector3d, double> GLC_Matrix4x4::rotationVectorAndAngle() const
178 {
179  QPair<GLC_Vector3d, double> subject(GLC_Vector3d(), 0.0);
180  if (GLC_Matrix4x4(*this).optimise().type() != GLC_Matrix4x4::Identity)
181  {
182  QQuaternion quaternion= this->quaternion();
183  quaternion.normalize();
184 
185  const double cos_angle= quaternion.scalar();
186  const double angle= acos(cos_angle);
187  double sin_angle= sqrt(1.0 - cos_angle * cos_angle);
188 
189 
190  if (fabs(sin_angle) < 0.0005) sin_angle= 1.0;
191 
192  subject.first.setX(quaternion.x() / sin_angle);
193  subject.first.setY(quaternion.y() / sin_angle);
194  subject.first.setZ(quaternion.z() / sin_angle);
195 
196  subject.second= angle * 2.0;
197  }
198 
199  return subject;
200 }
201 

©2005-2013 Laurent Ribon