1 |
tim |
741 |
/********************************************************************** |
2 |
|
|
matrix3x3.cpp - Handle 3D rotation matrix. |
3 |
|
|
|
4 |
|
|
Copyright (C) 1998-2001 by OpenEye Scientific Software, Inc. |
5 |
|
|
Some portions Copyright (C) 2001-2005 by Geoffrey R. Hutchison |
6 |
|
|
|
7 |
|
|
This file is part of the Open Babel project. |
8 |
|
|
For more information, see <http://openbabel.sourceforge.net/> |
9 |
|
|
|
10 |
|
|
This program is free software; you can redistribute it and/or modify |
11 |
|
|
it under the terms of the GNU General Public License as published by |
12 |
|
|
the Free Software Foundation version 2 of the License. |
13 |
|
|
|
14 |
|
|
This program is distributed in the hope that it will be useful, |
15 |
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
16 |
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
17 |
|
|
GNU General Public License for more details. |
18 |
|
|
***********************************************************************/ |
19 |
|
|
|
20 |
|
|
#include <math.h> |
21 |
|
|
|
22 |
|
|
#include "mol.hpp" |
23 |
|
|
#include "matrix3x3.hpp" |
24 |
|
|
|
25 |
|
|
#ifndef true |
26 |
|
|
#define true 1 |
27 |
|
|
#endif |
28 |
|
|
|
29 |
|
|
#ifndef false |
30 |
|
|
#define false 0 |
31 |
|
|
#endif |
32 |
|
|
|
33 |
|
|
using namespace std; |
34 |
|
|
|
35 |
|
|
namespace OpenBabel |
36 |
|
|
{ |
37 |
|
|
|
38 |
|
|
/** \class matrix3x3 |
39 |
|
|
\brief Represents a real 3x3 matrix. |
40 |
|
|
|
41 |
|
|
Rotating points in space can be performed by a vector-matrix |
42 |
|
|
multiplication. The matrix3x3 class is designed as a helper to the |
43 |
|
|
vector3 class for rotating points in space. The rotation matrix may be |
44 |
|
|
initialised by passing in the array of doubleing point values, by |
45 |
|
|
passing euler angles, or a rotation vector and angle of rotation about |
46 |
|
|
that vector. Once set, the matrix3x3 class can be used to rotate |
47 |
|
|
vectors by the overloaded multiplication operator. The following |
48 |
|
|
demonstrates the usage of the matrix3x3 class: |
49 |
|
|
|
50 |
|
|
\code |
51 |
|
|
matrix3x3 mat; |
52 |
|
|
mat.SetupRotMat(0.0,180.0,0.0); //rotate theta by 180 degrees |
53 |
|
|
vector3 v = VX; |
54 |
|
|
v *= mat; //apply the rotation |
55 |
|
|
\endcode |
56 |
|
|
|
57 |
|
|
*/ |
58 |
|
|
|
59 |
|
|
/*! the axis of the rotation will be uniformly distributed on |
60 |
|
|
the unit sphere, the angle will be uniformly distributed in |
61 |
|
|
the interval 0..360 degrees. */ |
62 |
|
|
void matrix3x3::randomRotation(OBRandom &rnd) |
63 |
|
|
{ |
64 |
|
|
double rotAngle; |
65 |
|
|
vector3 v1; |
66 |
|
|
|
67 |
|
|
v1.randomUnitVector(&rnd); |
68 |
|
|
rotAngle = 360.0 * rnd.NextFloat(); |
69 |
|
|
this->RotAboutAxisByAngle(v1,rotAngle); |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
void matrix3x3::SetupRotMat(double phi,double theta,double psi) |
73 |
|
|
{ |
74 |
|
|
double p = phi * DEG_TO_RAD; |
75 |
|
|
double h = theta * DEG_TO_RAD; |
76 |
|
|
double b = psi * DEG_TO_RAD; |
77 |
|
|
|
78 |
|
|
double cx = cos(p); |
79 |
|
|
double sx = sin(p); |
80 |
|
|
double cy = cos(h); |
81 |
|
|
double sy = sin(h); |
82 |
|
|
double cz = cos(b); |
83 |
|
|
double sz = sin(b); |
84 |
|
|
|
85 |
|
|
ele[0][0] = cy*cz; |
86 |
|
|
ele[0][1] = cy*sz; |
87 |
|
|
ele[0][2] = -sy; |
88 |
|
|
|
89 |
|
|
ele[1][0] = sx*sy*cz-cx*sz; |
90 |
|
|
ele[1][1] = sx*sy*sz+cx*cz; |
91 |
|
|
ele[1][2] = sx*cy; |
92 |
|
|
|
93 |
|
|
ele[2][0] = cx*sy*cz+sx*sz; |
94 |
|
|
ele[2][1] = cx*sy*sz-sx*cz; |
95 |
|
|
ele[2][2] = cx*cy; |
96 |
|
|
} |
97 |
|
|
|
98 |
|
|
/*! Replaces *this with a matrix that represents reflection on |
99 |
|
|
the plane through 0 which is given by the normal vector norm. |
100 |
|
|
|
101 |
|
|
\warning If the vector norm has length zero, this method will |
102 |
|
|
generate the 0-matrix. If the length of the axis is close to |
103 |
|
|
zero, but not == 0.0, this method may behave in unexpected |
104 |
|
|
ways and return almost random results; details may depend on |
105 |
|
|
your particular doubleing point implementation. The use of this |
106 |
|
|
method is therefore highly discouraged, unless you are certain |
107 |
|
|
that the length is in a reasonable range, away from 0.0 |
108 |
|
|
(Stefan Kebekus) |
109 |
|
|
|
110 |
|
|
\deprecated This method will probably replaced by a safer |
111 |
|
|
algorithm in the future. |
112 |
|
|
|
113 |
|
|
\todo Replace this method with a more fool-proof version. |
114 |
|
|
|
115 |
|
|
@param norm specifies the normal to the plane |
116 |
|
|
*/ |
117 |
|
|
void matrix3x3::PlaneReflection(const vector3 &norm) |
118 |
|
|
{ |
119 |
|
|
//@@@ add a safety net |
120 |
|
|
|
121 |
|
|
vector3 normtmp = norm; |
122 |
|
|
normtmp.normalize(); |
123 |
|
|
|
124 |
|
|
SetColumn(0, vector3(1,0,0) - 2*normtmp.x()*normtmp); |
125 |
|
|
SetColumn(1, vector3(0,1,0) - 2*normtmp.y()*normtmp); |
126 |
|
|
SetColumn(2, vector3(0,0,1) - 2*normtmp.z()*normtmp); |
127 |
|
|
} |
128 |
|
|
|
129 |
|
|
#define x vtmp.x() |
130 |
|
|
#define y vtmp.y() |
131 |
|
|
#define z vtmp.z() |
132 |
|
|
|
133 |
|
|
/*! Replaces *this with a matrix that represents rotation about the |
134 |
|
|
axis by a an angle. |
135 |
|
|
|
136 |
|
|
\warning If the vector axis has length zero, this method will |
137 |
|
|
generate the 0-matrix. If the length of the axis is close to |
138 |
|
|
zero, but not == 0.0, this method may behave in unexpected ways |
139 |
|
|
and return almost random results; details may depend on your |
140 |
|
|
particular doubleing point implementation. The use of this method |
141 |
|
|
is therefore highly discouraged, unless you are certain that the |
142 |
|
|
length is in a reasonable range, away from 0.0 (Stefan |
143 |
|
|
Kebekus) |
144 |
|
|
|
145 |
|
|
\deprecated This method will probably replaced by a safer |
146 |
|
|
algorithm in the future. |
147 |
|
|
|
148 |
|
|
\todo Replace this method with a more fool-proof version. |
149 |
|
|
|
150 |
|
|
@param v specifies the axis of the rotation |
151 |
|
|
@param angle angle in degrees (0..360) |
152 |
|
|
*/ |
153 |
|
|
void matrix3x3::RotAboutAxisByAngle(const vector3 &v,const double angle) |
154 |
|
|
{ |
155 |
|
|
double theta = angle*DEG_TO_RAD; |
156 |
|
|
double s = sin(theta); |
157 |
|
|
double c = cos(theta); |
158 |
|
|
double t = 1 - c; |
159 |
|
|
|
160 |
|
|
vector3 vtmp = v; |
161 |
|
|
vtmp.normalize(); |
162 |
|
|
|
163 |
|
|
ele[0][0] = t*x*x + c; |
164 |
|
|
ele[0][1] = t*x*y + s*z; |
165 |
|
|
ele[0][2] = t*x*z - s*y; |
166 |
|
|
|
167 |
|
|
ele[1][0] = t*x*y - s*z; |
168 |
|
|
ele[1][1] = t*y*y + c; |
169 |
|
|
ele[1][2] = t*y*z + s*x; |
170 |
|
|
|
171 |
|
|
ele[2][0] = t*x*z + s*y; |
172 |
|
|
ele[2][1] = t*y*z - s*x; |
173 |
|
|
ele[2][2] = t*z*z + c; |
174 |
|
|
} |
175 |
|
|
|
176 |
|
|
#undef x |
177 |
|
|
#undef y |
178 |
|
|
#undef z |
179 |
|
|
|
180 |
|
|
void matrix3x3::SetColumn(int col, const vector3 &v) throw(OBError) |
181 |
|
|
{ |
182 |
|
|
if (col > 2) |
183 |
|
|
{ |
184 |
|
|
OBError er("matrix3x3::SetColumn(int col, const vector3 &v)", |
185 |
|
|
"The method was called with col > 2.", |
186 |
|
|
"This is a programming error in your application."); |
187 |
|
|
throw er; |
188 |
|
|
} |
189 |
|
|
|
190 |
|
|
ele[0][col] = v.x(); |
191 |
|
|
ele[1][col] = v.y(); |
192 |
|
|
ele[2][col] = v.z(); |
193 |
|
|
} |
194 |
|
|
|
195 |
|
|
void matrix3x3::SetRow(int row, const vector3 &v) throw(OBError) |
196 |
|
|
{ |
197 |
|
|
if (row > 2) |
198 |
|
|
{ |
199 |
|
|
OBError er("matrix3x3::SetRow(int row, const vector3 &v)", |
200 |
|
|
"The method was called with row > 2.", |
201 |
|
|
"This is a programming error in your application."); |
202 |
|
|
throw er; |
203 |
|
|
} |
204 |
|
|
|
205 |
|
|
ele[row][0] = v.x(); |
206 |
|
|
ele[row][1] = v.y(); |
207 |
|
|
ele[row][2] = v.z(); |
208 |
|
|
} |
209 |
|
|
|
210 |
|
|
vector3 matrix3x3::GetColumn(unsigned int col) const throw(OBError) |
211 |
|
|
{ |
212 |
|
|
if (col > 2) |
213 |
|
|
{ |
214 |
|
|
OBError er("matrix3x3::GetColumn(unsigned int col) const", |
215 |
|
|
"The method was called with col > 2.", |
216 |
|
|
"This is a programming error in your application."); |
217 |
|
|
throw er; |
218 |
|
|
} |
219 |
|
|
|
220 |
|
|
return vector3(ele[0][col], ele[1][col], ele[2][col]); |
221 |
|
|
} |
222 |
|
|
|
223 |
|
|
vector3 matrix3x3::GetRow(unsigned int row) const throw(OBError) |
224 |
|
|
{ |
225 |
|
|
if (row > 2) |
226 |
|
|
{ |
227 |
|
|
OBError er("matrix3x3::GetRow(unsigned int row) const", |
228 |
|
|
"The method was called with row > 2.", |
229 |
|
|
"This is a programming error in your application."); |
230 |
|
|
throw er; |
231 |
|
|
} |
232 |
|
|
|
233 |
|
|
return vector3(ele[row][0], ele[row][1], ele[row][2]); |
234 |
|
|
} |
235 |
|
|
|
236 |
|
|
/*! calculates the product m*v of the matrix m and the column |
237 |
|
|
vector represented by v |
238 |
|
|
*/ |
239 |
|
|
vector3 operator *(const matrix3x3 &m,const vector3 &v) |
240 |
|
|
{ |
241 |
|
|
vector3 vv; |
242 |
|
|
|
243 |
|
|
vv._vx = v._vx*m.ele[0][0] + v._vy*m.ele[0][1] + v._vz*m.ele[0][2]; |
244 |
|
|
vv._vy = v._vx*m.ele[1][0] + v._vy*m.ele[1][1] + v._vz*m.ele[1][2]; |
245 |
|
|
vv._vz = v._vx*m.ele[2][0] + v._vy*m.ele[2][1] + v._vz*m.ele[2][2]; |
246 |
|
|
|
247 |
|
|
return(vv); |
248 |
|
|
} |
249 |
|
|
|
250 |
|
|
matrix3x3 operator *(const matrix3x3 &A,const matrix3x3 &B) |
251 |
|
|
{ |
252 |
|
|
matrix3x3 result; |
253 |
|
|
|
254 |
|
|
result.ele[0][0] = A.ele[0][0]*B.ele[0][0] + A.ele[0][1]*B.ele[1][0] + A.ele[0][2]*B.ele[2][0]; |
255 |
|
|
result.ele[0][1] = A.ele[0][0]*B.ele[0][1] + A.ele[0][1]*B.ele[1][1] + A.ele[0][2]*B.ele[2][1]; |
256 |
|
|
result.ele[0][2] = A.ele[0][0]*B.ele[0][2] + A.ele[0][1]*B.ele[1][2] + A.ele[0][2]*B.ele[2][2]; |
257 |
|
|
|
258 |
|
|
result.ele[1][0] = A.ele[1][0]*B.ele[0][0] + A.ele[1][1]*B.ele[1][0] + A.ele[1][2]*B.ele[2][0]; |
259 |
|
|
result.ele[1][1] = A.ele[1][0]*B.ele[0][1] + A.ele[1][1]*B.ele[1][1] + A.ele[1][2]*B.ele[2][1]; |
260 |
|
|
result.ele[1][2] = A.ele[1][0]*B.ele[0][2] + A.ele[1][1]*B.ele[1][2] + A.ele[1][2]*B.ele[2][2]; |
261 |
|
|
|
262 |
|
|
result.ele[2][0] = A.ele[2][0]*B.ele[0][0] + A.ele[2][1]*B.ele[1][0] + A.ele[2][2]*B.ele[2][0]; |
263 |
|
|
result.ele[2][1] = A.ele[2][0]*B.ele[0][1] + A.ele[2][1]*B.ele[1][1] + A.ele[2][2]*B.ele[2][1]; |
264 |
|
|
result.ele[2][2] = A.ele[2][0]*B.ele[0][2] + A.ele[2][1]*B.ele[1][2] + A.ele[2][2]*B.ele[2][2]; |
265 |
|
|
|
266 |
|
|
return(result); |
267 |
|
|
} |
268 |
|
|
|
269 |
|
|
/*! calculates the product m*(*this) of the matrix m and the |
270 |
|
|
column vector represented by *this |
271 |
|
|
*/ |
272 |
|
|
vector3 &vector3::operator *= (const matrix3x3 &m) |
273 |
|
|
{ |
274 |
|
|
vector3 vv; |
275 |
|
|
|
276 |
|
|
vv.SetX(_vx*m.Get(0,0) + _vy*m.Get(0,1) + _vz*m.Get(0,2)); |
277 |
|
|
vv.SetY(_vx*m.Get(1,0) + _vy*m.Get(1,1) + _vz*m.Get(1,2)); |
278 |
|
|
vv.SetZ(_vx*m.Get(2,0) + _vy*m.Get(2,1) + _vz*m.Get(2,2)); |
279 |
|
|
_vx = vv.x(); |
280 |
|
|
_vy = vv.y(); |
281 |
|
|
_vz = vv.z(); |
282 |
|
|
|
283 |
|
|
return(*this); |
284 |
|
|
} |
285 |
|
|
|
286 |
|
|
/*! This method checks if the absolute value of the determinant is smaller than 1e-6. If |
287 |
|
|
so, nothing is done and an exception is thrown. Otherwise, the |
288 |
|
|
inverse matrix is calculated and returned. *this is not changed. |
289 |
|
|
|
290 |
|
|
\warning If the determinant is close to zero, but not == 0.0, |
291 |
|
|
this method may behave in unexpected ways and return almost |
292 |
|
|
random results; details may depend on your particular doubleing |
293 |
|
|
point implementation. The use of this method is therefore highly |
294 |
|
|
discouraged, unless you are certain that the determinant is in a |
295 |
|
|
reasonable range, away from 0.0 (Stefan Kebekus) |
296 |
|
|
*/ |
297 |
|
|
matrix3x3 matrix3x3::inverse(void) const throw(OBError) |
298 |
|
|
{ |
299 |
|
|
double det = determinant(); |
300 |
|
|
if (fabs(det) <= 1e-6) |
301 |
|
|
{ |
302 |
|
|
OBError er("matrix3x3::invert(void)", |
303 |
|
|
"The method was called on a matrix with |determinant| <= 1e-6.", |
304 |
|
|
"This is a runtime or a programming error in your application."); |
305 |
|
|
throw er; |
306 |
|
|
} |
307 |
|
|
|
308 |
|
|
matrix3x3 inverse; |
309 |
|
|
inverse.ele[0][0] = ele[1][1]*ele[2][2] - ele[1][2]*ele[2][1]; |
310 |
|
|
inverse.ele[1][0] = ele[1][2]*ele[2][0] - ele[1][0]*ele[2][2]; |
311 |
|
|
inverse.ele[2][0] = ele[1][0]*ele[2][1] - ele[1][1]*ele[2][0]; |
312 |
|
|
inverse.ele[0][1] = ele[2][1]*ele[0][2] - ele[2][2]*ele[0][1]; |
313 |
|
|
inverse.ele[1][1] = ele[2][2]*ele[0][0] - ele[2][0]*ele[0][2]; |
314 |
|
|
inverse.ele[2][1] = ele[2][0]*ele[0][1] - ele[2][1]*ele[0][0]; |
315 |
|
|
inverse.ele[0][2] = ele[0][1]*ele[1][2] - ele[0][2]*ele[1][1]; |
316 |
|
|
inverse.ele[1][2] = ele[0][2]*ele[1][0] - ele[0][0]*ele[1][2]; |
317 |
|
|
inverse.ele[2][2] = ele[0][0]*ele[1][1] - ele[0][1]*ele[1][0]; |
318 |
|
|
|
319 |
|
|
inverse /= det; |
320 |
|
|
|
321 |
|
|
return(inverse); |
322 |
|
|
} |
323 |
|
|
|
324 |
|
|
/* This method returns the transpose of a matrix. The original |
325 |
|
|
matrix remains unchanged. */ |
326 |
|
|
matrix3x3 matrix3x3::transpose(void) const |
327 |
|
|
{ |
328 |
|
|
matrix3x3 transpose; |
329 |
|
|
|
330 |
|
|
for(unsigned int i=0; i<3; i++) |
331 |
|
|
for(unsigned int j=0; j<3; j++) |
332 |
|
|
transpose.ele[i][j] = ele[j][i]; |
333 |
|
|
|
334 |
|
|
return(transpose); |
335 |
|
|
} |
336 |
|
|
|
337 |
|
|
double matrix3x3::determinant(void) const |
338 |
|
|
{ |
339 |
|
|
double x,y,z; |
340 |
|
|
|
341 |
|
|
x = ele[0][0] * (ele[1][1] * ele[2][2] - ele[1][2] * ele[2][1]); |
342 |
|
|
y = ele[0][1] * (ele[1][2] * ele[2][0] - ele[1][0] * ele[2][2]); |
343 |
|
|
z = ele[0][2] * (ele[1][0] * ele[2][1] - ele[1][1] * ele[2][0]); |
344 |
|
|
|
345 |
|
|
return(x + y + z); |
346 |
|
|
} |
347 |
|
|
|
348 |
|
|
/*! This method returns false if there are indices i,j such that |
349 |
|
|
fabs(*this[i][j]-*this[j][i]) > 1e-6. Otherwise, it returns |
350 |
|
|
true. */ |
351 |
|
|
bool matrix3x3::isSymmetric(void) const |
352 |
|
|
{ |
353 |
|
|
if (fabs(ele[0][1] - ele[1][0]) > 1e-6) |
354 |
|
|
return false; |
355 |
|
|
if (fabs(ele[0][2] - ele[2][0]) > 1e-6) |
356 |
|
|
return false; |
357 |
|
|
if (fabs(ele[1][2] - ele[2][1]) > 1e-6) |
358 |
|
|
return false; |
359 |
|
|
return true; |
360 |
|
|
} |
361 |
|
|
|
362 |
|
|
/*! This method returns false if there are indices i != j such |
363 |
|
|
that fabs(*this[i][j]) > 1e-6. Otherwise, it returns true. */ |
364 |
|
|
bool matrix3x3::isDiagonal(void) const |
365 |
|
|
{ |
366 |
|
|
if (fabs(ele[0][1]) > 1e-6) |
367 |
|
|
return false; |
368 |
|
|
if (fabs(ele[0][2]) > 1e-6) |
369 |
|
|
return false; |
370 |
|
|
if (fabs(ele[1][2]) > 1e-6) |
371 |
|
|
return false; |
372 |
|
|
|
373 |
|
|
if (fabs(ele[1][0]) > 1e-6) |
374 |
|
|
return false; |
375 |
|
|
if (fabs(ele[2][0]) > 1e-6) |
376 |
|
|
return false; |
377 |
|
|
if (fabs(ele[2][1]) > 1e-6) |
378 |
|
|
return false; |
379 |
|
|
|
380 |
|
|
return true; |
381 |
|
|
} |
382 |
|
|
|
383 |
|
|
/*! This method returns false if there are indices i != j such |
384 |
|
|
that fabs(*this[i][j]) > 1e-6, or if there is an index i such |
385 |
|
|
that fabs(*this[i][j]-1) > 1e-6. Otherwise, it returns |
386 |
|
|
true. */ |
387 |
|
|
bool matrix3x3::isUnitMatrix(void) const |
388 |
|
|
{ |
389 |
|
|
if (!isDiagonal()) |
390 |
|
|
return false; |
391 |
|
|
|
392 |
|
|
if (fabs(ele[0][0]-1) > 1e-6) |
393 |
|
|
return false; |
394 |
|
|
if (fabs(ele[1][1]-1) > 1e-6) |
395 |
|
|
return false; |
396 |
|
|
if (fabs(ele[2][2]-1) > 1e-6) |
397 |
|
|
return false; |
398 |
|
|
|
399 |
|
|
return true; |
400 |
|
|
} |
401 |
|
|
|
402 |
|
|
/*! This method employs the static method matrix3x3::jacobi(...) |
403 |
|
|
to find the eigenvalues and eigenvectors of a symmetric |
404 |
|
|
matrix. On entry it is checked if the matrix really is |
405 |
|
|
symmetric: if isSymmetric() returns 'false', an OBError is |
406 |
|
|
thrown. |
407 |
|
|
|
408 |
|
|
\note The jacobi algorithm is should work great for all |
409 |
|
|
symmetric 3x3 matrices. If you need to find the eigenvectors |
410 |
|
|
of a non-symmetric matrix, you might want to resort to the |
411 |
|
|
sophisticated routines of LAPACK. |
412 |
|
|
|
413 |
|
|
@param eigenvals a reference to a vector3 where the |
414 |
|
|
eigenvalues will be stored. The eigenvalues are ordered so |
415 |
|
|
that eigenvals[0] <= eigenvals[1] <= eigenvals[2]. |
416 |
|
|
|
417 |
|
|
@return an orthogonal matrix whose ith column is an |
418 |
|
|
eigenvector for the eigenvalue eigenvals[i]. Here 'orthogonal' |
419 |
|
|
means that all eigenvectors have length one and are mutually |
420 |
|
|
orthogonal. The ith eigenvector can thus be conveniently |
421 |
|
|
accessed by the GetColumn() method, as in the following |
422 |
|
|
example. |
423 |
|
|
\code |
424 |
|
|
// Calculate eigenvectors and -values |
425 |
|
|
vector3 eigenvals; |
426 |
|
|
matrix3x3 eigenmatrix = somematrix.findEigenvectorsIfSymmetric(eigenvals); |
427 |
|
|
|
428 |
|
|
// Print the 2nd eigenvector |
429 |
|
|
cout << eigenmatrix.GetColumn(1) << endl; |
430 |
|
|
\endcode |
431 |
|
|
With these conventions, a matrix is diagonalized in the following way: |
432 |
|
|
\code |
433 |
|
|
// Diagonalize the matrix |
434 |
|
|
matrix3x3 diagonalMatrix = eigenmatrix.inverse() * somematrix * eigenmatrix; |
435 |
|
|
\endcode |
436 |
|
|
|
437 |
|
|
*/ |
438 |
|
|
matrix3x3 matrix3x3::findEigenvectorsIfSymmetric(vector3 &eigenvals) const throw(OBError) |
439 |
|
|
{ |
440 |
|
|
matrix3x3 result; |
441 |
|
|
|
442 |
|
|
if (!isSymmetric()) |
443 |
|
|
{ |
444 |
|
|
OBError er("matrix3x3::findEigenvectorsIfSymmetric(vector3 &eigenvals) const throw(OBError)", |
445 |
|
|
"The method was called on a matrix that was not symmetric, i.e. where isSymetric() == false.", |
446 |
|
|
"This is a runtime or a programming error in your application."); |
447 |
|
|
throw er; |
448 |
|
|
} |
449 |
|
|
|
450 |
|
|
double d[3]; |
451 |
|
|
matrix3x3 copyOfThis = *this; |
452 |
|
|
|
453 |
|
|
jacobi(3, copyOfThis.ele[0], d, result.ele[0]); |
454 |
|
|
eigenvals.Set(d); |
455 |
|
|
|
456 |
|
|
return result; |
457 |
|
|
} |
458 |
|
|
|
459 |
|
|
matrix3x3 &matrix3x3::operator/=(const double &c) |
460 |
|
|
{ |
461 |
|
|
for (int row = 0;row < 3; row++) |
462 |
|
|
for (int col = 0;col < 3; col++) |
463 |
|
|
ele[row][col] /= c; |
464 |
|
|
|
465 |
|
|
return(*this); |
466 |
|
|
} |
467 |
|
|
|
468 |
|
|
#ifndef SQUARE |
469 |
|
|
#define SQUARE(x) ((x)*(x)) |
470 |
|
|
#endif |
471 |
|
|
|
472 |
|
|
void matrix3x3::FillOrth(double Alpha,double Beta, double Gamma, |
473 |
|
|
double A, double B, double C) |
474 |
|
|
{ |
475 |
|
|
double V; |
476 |
|
|
|
477 |
|
|
Alpha *= DEG_TO_RAD; |
478 |
|
|
Beta *= DEG_TO_RAD; |
479 |
|
|
Gamma *= DEG_TO_RAD; |
480 |
|
|
|
481 |
|
|
// from the PDB specification: |
482 |
|
|
// http://www.rcsb.org/pdb/docs/format/pdbguide2.2/part_75.html |
483 |
|
|
|
484 |
|
|
|
485 |
|
|
// since we'll ultimately divide by (a * b), we've factored those out here |
486 |
|
|
V = C * sqrt(1 - SQUARE(cos(Alpha)) - SQUARE(cos(Beta)) - SQUARE(cos(Gamma)) |
487 |
|
|
+ 2 * cos(Alpha) * cos(Beta) * cos(Gamma)); |
488 |
|
|
|
489 |
|
|
ele[0][0] = A; |
490 |
|
|
ele[0][1] = B*cos(Gamma); |
491 |
|
|
ele[0][2] = C*cos(Beta); |
492 |
|
|
|
493 |
|
|
ele[1][0] = 0.0; |
494 |
|
|
ele[1][1] = B*sin(Gamma); |
495 |
|
|
ele[1][2] = C*(cos(Alpha)-cos(Beta)*cos(Gamma))/sin(Gamma); |
496 |
|
|
|
497 |
|
|
ele[2][0] = 0.0; |
498 |
|
|
ele[2][1] = 0.0; |
499 |
|
|
ele[2][2] = V / (sin(Gamma)); // again, we factored out A * B when defining V |
500 |
|
|
} |
501 |
|
|
|
502 |
|
|
ostream& operator<< ( ostream& co, const matrix3x3& m ) |
503 |
|
|
|
504 |
|
|
{ |
505 |
|
|
co << "[ " |
506 |
|
|
<< m.ele[0][0] |
507 |
|
|
<< ", " |
508 |
|
|
<< m.ele[0][1] |
509 |
|
|
<< ", " |
510 |
|
|
<< m.ele[0][2] |
511 |
|
|
<< " ]" << endl; |
512 |
|
|
|
513 |
|
|
co << "[ " |
514 |
|
|
<< m.ele[1][0] |
515 |
|
|
<< ", " |
516 |
|
|
<< m.ele[1][1] |
517 |
|
|
<< ", " |
518 |
|
|
<< m.ele[1][2] |
519 |
|
|
<< " ]" << endl; |
520 |
|
|
|
521 |
|
|
co << "[ " |
522 |
|
|
<< m.ele[2][0] |
523 |
|
|
<< ", " |
524 |
|
|
<< m.ele[2][1] |
525 |
|
|
<< ", " |
526 |
|
|
<< m.ele[2][2] |
527 |
|
|
<< " ]" << endl; |
528 |
|
|
|
529 |
|
|
return co ; |
530 |
|
|
} |
531 |
|
|
|
532 |
|
|
/*! This static function computes the eigenvalues and |
533 |
|
|
eigenvectors of a SYMMETRIC nxn matrix. This method is used |
534 |
|
|
internally by OpenBabel, but may be useful as a general |
535 |
|
|
eigenvalue finder. |
536 |
|
|
|
537 |
|
|
The algorithm uses Jacobi transformations. It is described |
538 |
|
|
e.g. in Wilkinson, Reinsch "Handbook for automatic computation, |
539 |
|
|
Volume II: Linear Algebra", part II, contribution II/1. The |
540 |
|
|
implementation is also similar to the implementation in this |
541 |
|
|
book. This method is adequate to solve the eigenproblem for |
542 |
|
|
small matrices, of size perhaps up to 10x10. For bigger |
543 |
|
|
problems, you might want to resort to the sophisticated routines |
544 |
|
|
of LAPACK. |
545 |
|
|
|
546 |
|
|
\note If you plan to find the eigenvalues of a symmetric 3x3 |
547 |
|
|
matrix, you will probably prefer to use the more convenient |
548 |
|
|
method findEigenvectorsIfSymmetric() |
549 |
|
|
|
550 |
|
|
@param n the size of the matrix that should be diagonalized |
551 |
|
|
|
552 |
|
|
@param a array of size n^2 which holds the symmetric matrix |
553 |
|
|
whose eigenvectors are to be computed. The convention is that |
554 |
|
|
the entry in row r and column c is addressed as a[n*r+c] where, |
555 |
|
|
of course, 0 <= r < n and 0 <= c < n. There is no check that the |
556 |
|
|
matrix is actually symmetric. If it is not, the behaviour of |
557 |
|
|
this function is undefined. On return, the matrix is |
558 |
|
|
overwritten with junk. |
559 |
|
|
|
560 |
|
|
@param d pointer to a field of at least n doubles which will be |
561 |
|
|
overwritten. On return of this function, the entries d[0]..d[n-1] |
562 |
|
|
will contain the eigenvalues of the matrix. |
563 |
|
|
|
564 |
|
|
@param v an array of size n^2 where the eigenvectors will be |
565 |
|
|
stored. On return, the columns of this matrix will contain the |
566 |
|
|
eigenvectors. The eigenvectors are normalized and mutually |
567 |
|
|
orthogonal. |
568 |
|
|
*/ |
569 |
|
|
void matrix3x3::jacobi(unsigned int n, double *a, double *d, double *v) |
570 |
|
|
{ |
571 |
|
|
double onorm, dnorm; |
572 |
|
|
double b, dma, q, t, c, s; |
573 |
|
|
double atemp, vtemp, dtemp; |
574 |
|
|
register int i, j, k, l; |
575 |
|
|
int nrot; |
576 |
|
|
|
577 |
|
|
int MAX_SWEEPS=50; |
578 |
|
|
|
579 |
|
|
// Set v to the identity matrix, set the vector d to contain the |
580 |
|
|
// diagonal elements of the matrix a |
581 |
|
|
for (j = 0; j < static_cast<int>(n); j++) |
582 |
|
|
{ |
583 |
|
|
for (i = 0; i < static_cast<int>(n); i++) |
584 |
|
|
v[n*i+j] = 0.0; |
585 |
|
|
v[n*j+j] = 1.0; |
586 |
|
|
d[j] = a[n*j+j]; |
587 |
|
|
} |
588 |
|
|
|
589 |
|
|
nrot = MAX_SWEEPS; |
590 |
|
|
for (l = 1; l <= nrot; l++) |
591 |
|
|
{ |
592 |
|
|
// Set dnorm to be the maximum norm of the diagonal elements, set |
593 |
|
|
// onorm to the maximum norm of the off-diagonal elements |
594 |
|
|
dnorm = 0.0; |
595 |
|
|
onorm = 0.0; |
596 |
|
|
for (j = 0; j < static_cast<int>(n); j++) |
597 |
|
|
{ |
598 |
|
|
dnorm += (double)fabs(d[j]); |
599 |
|
|
for (i = 0; i < j; i++) |
600 |
|
|
onorm += (double)fabs(a[n*i+j]); |
601 |
|
|
} |
602 |
|
|
// Normal end point of this algorithm. |
603 |
|
|
if((onorm/dnorm) <= 1.0e-12) |
604 |
|
|
goto Exit_now; |
605 |
|
|
|
606 |
|
|
for (j = 1; j < static_cast<int>(n); j++) |
607 |
|
|
{ |
608 |
|
|
for (i = 0; i <= j - 1; i++) |
609 |
|
|
{ |
610 |
|
|
|
611 |
|
|
b = a[n*i+j]; |
612 |
|
|
if(fabs(b) > 0.0) |
613 |
|
|
{ |
614 |
|
|
dma = d[j] - d[i]; |
615 |
|
|
if((fabs(dma) + fabs(b)) <= fabs(dma)) |
616 |
|
|
t = b / dma; |
617 |
|
|
else |
618 |
|
|
{ |
619 |
|
|
q = 0.5 * dma / b; |
620 |
|
|
t = 1.0/((double)fabs(q) + (double)sqrt(1.0+q*q)); |
621 |
|
|
if (q < 0.0) |
622 |
|
|
t = -t; |
623 |
|
|
} |
624 |
|
|
|
625 |
|
|
c = 1.0/(double)sqrt(t*t + 1.0); |
626 |
|
|
s = t * c; |
627 |
|
|
a[n*i+j] = 0.0; |
628 |
|
|
|
629 |
|
|
for (k = 0; k <= i-1; k++) |
630 |
|
|
{ |
631 |
|
|
atemp = c * a[n*k+i] - s * a[n*k+j]; |
632 |
|
|
a[n*k+j] = s * a[n*k+i] + c * a[n*k+j]; |
633 |
|
|
a[n*k+i] = atemp; |
634 |
|
|
} |
635 |
|
|
|
636 |
|
|
for (k = i+1; k <= j-1; k++) |
637 |
|
|
{ |
638 |
|
|
atemp = c * a[n*i+k] - s * a[n*k+j]; |
639 |
|
|
a[n*k+j] = s * a[n*i+k] + c * a[n*k+j]; |
640 |
|
|
a[n*i+k] = atemp; |
641 |
|
|
} |
642 |
|
|
|
643 |
|
|
for (k = j+1; k < static_cast<int>(n); k++) |
644 |
|
|
{ |
645 |
|
|
atemp = c * a[n*i+k] - s * a[n*j+k]; |
646 |
|
|
a[n*j+k] = s * a[n*i+k] + c * a[n*j+k]; |
647 |
|
|
a[n*i+k] = atemp; |
648 |
|
|
} |
649 |
|
|
|
650 |
|
|
for (k = 0; k < static_cast<int>(n); k++) |
651 |
|
|
{ |
652 |
|
|
vtemp = c * v[n*k+i] - s * v[n*k+j]; |
653 |
|
|
v[n*k+j] = s * v[n*k+i] + c * v[n*k+j]; |
654 |
|
|
v[n*k+i] = vtemp; |
655 |
|
|
} |
656 |
|
|
|
657 |
|
|
dtemp = c*c*d[i] + s*s*d[j] - 2.0*c*s*b; |
658 |
|
|
d[j] = s*s*d[i] + c*c*d[j] + 2.0*c*s*b; |
659 |
|
|
d[i] = dtemp; |
660 |
|
|
} /* end if */ |
661 |
|
|
} /* end for i */ |
662 |
|
|
} /* end for j */ |
663 |
|
|
} /* end for l */ |
664 |
|
|
|
665 |
|
|
Exit_now: |
666 |
|
|
|
667 |
|
|
// Now sort the eigenvalues (and the eigenvectors) so that the |
668 |
|
|
// smallest eigenvalues come first. |
669 |
|
|
nrot = l; |
670 |
|
|
|
671 |
|
|
for (j = 0; j < static_cast<int>(n)-1; j++) |
672 |
|
|
{ |
673 |
|
|
k = j; |
674 |
|
|
dtemp = d[k]; |
675 |
|
|
for (i = j+1; i < static_cast<int>(n); i++) |
676 |
|
|
if(d[i] < dtemp) |
677 |
|
|
{ |
678 |
|
|
k = i; |
679 |
|
|
dtemp = d[k]; |
680 |
|
|
} |
681 |
|
|
|
682 |
|
|
if(k > j) |
683 |
|
|
{ |
684 |
|
|
d[k] = d[j]; |
685 |
|
|
d[j] = dtemp; |
686 |
|
|
for (i = 0; i < static_cast<int>(n); i++) |
687 |
|
|
{ |
688 |
|
|
dtemp = v[n*i+k]; |
689 |
|
|
v[n*i+k] = v[n*i+j]; |
690 |
|
|
v[n*i+j] = dtemp; |
691 |
|
|
} |
692 |
|
|
} |
693 |
|
|
} |
694 |
|
|
} |
695 |
|
|
|
696 |
|
|
} // end namespace OpenBabel |
697 |
|
|
|
698 |
|
|
//! \file matrix3x3.cpp |
699 |
|
|
//! \brief Handle 3D rotation matrix. |
700 |
|
|
|