# | Line 6 | Line 6 | |
---|---|---|
6 | * redistribute this software in source and binary code form, provided | |
7 | * that the following conditions are met: | |
8 | * | |
9 | < | * 1. Acknowledgement of the program authors must be made in any |
10 | < | * publication of scientific results based in part on use of the |
11 | < | * program. An acceptable form of acknowledgement is citation of |
12 | < | * the article in which the program was described (Matthew |
13 | < | * A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher |
14 | < | * J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented |
15 | < | * Parallel Simulation Engine for Molecular Dynamics," |
16 | < | * J. Comput. Chem. 26, pp. 252-271 (2005)) |
17 | < | * |
18 | < | * 2. Redistributions of source code must retain the above copyright |
9 | > | * 1. Redistributions of source code must retain the above copyright |
10 | * notice, this list of conditions and the following disclaimer. | |
11 | * | |
12 | < | * 3. Redistributions in binary form must reproduce the above copyright |
12 | > | * 2. Redistributions in binary form must reproduce the above copyright |
13 | * notice, this list of conditions and the following disclaimer in the | |
14 | * documentation and/or other materials provided with the | |
15 | * distribution. | |
# | Line 37 | Line 28 | |
28 | * arising out of the use of or inability to use software, even if the | |
29 | * University of Notre Dame has been advised of the possibility of | |
30 | * such damages. | |
31 | + | * |
32 | + | * SUPPORT OPEN SCIENCE! If you use OpenMD or its source code in your |
33 | + | * research, please cite the appropriate papers when you publish your |
34 | + | * work. Good starting points are: |
35 | + | * |
36 | + | * [1] Meineke, et al., J. Comp. Chem. 26, 252-271 (2005). |
37 | + | * [2] Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006). |
38 | + | * [3] Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008). |
39 | + | * [4] Vardeman & Gezelter, in progress (2009). |
40 | */ | |
41 | ||
42 | /** | |
# | Line 49 | Line 49 | |
49 | #ifndef MATH_QUATERNION_HPP | |
50 | #define MATH_QUATERNION_HPP | |
51 | ||
52 | < | #include "math/Vector.hpp" |
52 | > | #include "math/Vector3.hpp" |
53 | #include "math/SquareMatrix.hpp" | |
54 | + | #define ISZERO(a,eps) ( (a)>-(eps) && (a)<(eps) ) |
55 | + | const RealType tiny=1.0e-6; |
56 | ||
57 | < | namespace oopse{ |
57 | > | namespace OpenMD{ |
58 | ||
59 | /** | |
60 | * @class Quaternion Quaternion.hpp "math/Quaternion.hpp" | |
# | Line 64 | Line 66 | namespace oopse{ | |
66 | */ | |
67 | template<typename Real> | |
68 | class Quaternion : public Vector<Real, 4> { | |
69 | + | |
70 | public: | |
71 | Quaternion() : Vector<Real, 4>() {} | |
72 | ||
# | Line 78 | Line 81 | namespace oopse{ | |
81 | /** Constructs and initializes a Quaternion from a Vector<Real,4> */ | |
82 | Quaternion(const Vector<Real,4>& v) | |
83 | : Vector<Real, 4>(v){ | |
84 | < | } |
84 | > | } |
85 | ||
86 | /** copy assignment */ | |
87 | Quaternion& operator =(const Vector<Real, 4>& v){ | |
88 | if (this == & v) | |
89 | return *this; | |
90 | < | |
90 | > | |
91 | Vector<Real, 4>::operator=(v); | |
92 | < | |
92 | > | |
93 | return *this; | |
94 | } | |
95 | < | |
95 | > | |
96 | /** | |
97 | * Returns the value of the first element of this quaternion. | |
98 | * @return the value of the first element of this quaternion | |
# | Line 242 | Line 245 | namespace oopse{ | |
245 | * Returns the conjugate quaternion of this quaternion | |
246 | * @return the conjugate quaternion of this quaternion | |
247 | */ | |
248 | < | Quaternion<Real> conjugate() { |
248 | > | Quaternion<Real> conjugate() const { |
249 | return Quaternion<Real>(w(), -x(), -y(), -z()); | |
250 | } | |
251 | ||
252 | + | |
253 | /** | |
254 | + | return rotation angle from -PI to PI |
255 | + | */ |
256 | + | inline Real get_rotation_angle() const{ |
257 | + | if( w < (Real)0.0 ) |
258 | + | return 2.0*atan2(-sqrt( x()*x() + y()*y() + z()*z() ), -w() ); |
259 | + | else |
260 | + | return 2.0*atan2( sqrt( x()*x() + y()*y() + z()*z() ), w() ); |
261 | + | } |
262 | + | |
263 | + | /** |
264 | + | create a unit quaternion from axis angle representation |
265 | + | */ |
266 | + | Quaternion<Real> fromAxisAngle(const Vector3<Real>& axis, |
267 | + | const Real& angle){ |
268 | + | Vector3<Real> v(axis); |
269 | + | v.normalize(); |
270 | + | Real half_angle = angle*0.5; |
271 | + | Real sin_a = sin(half_angle); |
272 | + | *this = Quaternion<Real>(cos(half_angle), |
273 | + | v.x()*sin_a, |
274 | + | v.y()*sin_a, |
275 | + | v.z()*sin_a); |
276 | + | return *this; |
277 | + | } |
278 | + | |
279 | + | /** |
280 | + | convert a quaternion to axis angle representation, |
281 | + | preserve the axis direction and angle from -PI to +PI |
282 | + | */ |
283 | + | void toAxisAngle(Vector3<Real>& axis, Real& angle)const { |
284 | + | Real vl = sqrt( x()*x() + y()*y() + z()*z() ); |
285 | + | if( vl > tiny ) { |
286 | + | Real ivl = 1.0/vl; |
287 | + | axis.x() = x() * ivl; |
288 | + | axis.y() = y() * ivl; |
289 | + | axis.z() = z() * ivl; |
290 | + | |
291 | + | if( w() < 0 ) |
292 | + | angle = 2.0*atan2(-vl, -w()); //-PI,0 |
293 | + | else |
294 | + | angle = 2.0*atan2( vl, w()); //0,PI |
295 | + | } else { |
296 | + | axis = Vector3<Real>(0.0,0.0,0.0); |
297 | + | angle = 0.0; |
298 | + | } |
299 | + | } |
300 | + | |
301 | + | /** |
302 | + | shortest arc quaternion rotate one vector to another by shortest path. |
303 | + | create rotation from -> to, for any length vectors. |
304 | + | */ |
305 | + | Quaternion<Real> fromShortestArc(const Vector3d& from, |
306 | + | const Vector3d& to ) { |
307 | + | |
308 | + | Vector3d c( cross(from,to) ); |
309 | + | *this = Quaternion<Real>(dot(from,to), |
310 | + | c.x(), |
311 | + | c.y(), |
312 | + | c.z()); |
313 | + | |
314 | + | this->normalize(); // if "from" or "to" not unit, normalize quat |
315 | + | w += 1.0f; // reducing angle to halfangle |
316 | + | if( w <= 1e-6 ) { // angle close to PI |
317 | + | if( ( from.z()*from.z() ) > ( from.x()*from.x() ) ) { |
318 | + | this->data_[0] = w; |
319 | + | this->data_[1] = 0.0; //cross(from , Vector3d(1,0,0)) |
320 | + | this->data_[2] = from.z(); |
321 | + | this->data_[3] = -from.y(); |
322 | + | } else { |
323 | + | this->data_[0] = w; |
324 | + | this->data_[1] = from.y(); //cross(from, Vector3d(0,0,1)) |
325 | + | this->data_[2] = -from.x(); |
326 | + | this->data_[3] = 0.0; |
327 | + | } |
328 | + | } |
329 | + | this->normalize(); |
330 | + | } |
331 | + | |
332 | + | Real ComputeTwist(const Quaternion& q) { |
333 | + | return (Real)2.0 * atan2(q.z(), q.w()); |
334 | + | } |
335 | + | |
336 | + | void RemoveTwist(Quaternion& q) { |
337 | + | Real t = ComputeTwist(q); |
338 | + | Quaternion rt = fromAxisAngle(V3Z, t); |
339 | + | |
340 | + | q *= rt.inverse(); |
341 | + | } |
342 | + | |
343 | + | void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle, |
344 | + | Vector3<Real>& swingAxis) { |
345 | + | |
346 | + | twistAngle = (Real)2.0 * atan2(z(), w()); |
347 | + | Quaternion rt, rs; |
348 | + | rt.fromAxisAngle(V3Z, twistAngle); |
349 | + | rs = *this * rt.inverse(); |
350 | + | |
351 | + | Real vl = sqrt( rs.x()*rs.x() + rs.y()*rs.y() + rs.z()*rs.z() ); |
352 | + | if( vl > tiny ) { |
353 | + | Real ivl = 1.0 / vl; |
354 | + | swingAxis.x() = rs.x() * ivl; |
355 | + | swingAxis.y() = rs.y() * ivl; |
356 | + | swingAxis.z() = rs.z() * ivl; |
357 | + | |
358 | + | if( rs.w() < 0.0 ) |
359 | + | swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0 |
360 | + | else |
361 | + | swingAngle = 2.0*atan2( vl, rs.w()); //0,PI |
362 | + | } else { |
363 | + | swingAxis = Vector3<Real>(1.0,0.0,0.0); |
364 | + | swingAngle = 0.0; |
365 | + | } |
366 | + | } |
367 | + | |
368 | + | |
369 | + | Vector3<Real> rotate(const Vector3<Real>& v) { |
370 | + | |
371 | + | Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(), |
372 | + | v.y() * w() + v.x() * z() - v.z() * x(), |
373 | + | v.z() * w() + v.y() * x() - v.x() * y(), |
374 | + | v.x() * x() + v.y() * y() + v.z() * z()); |
375 | + | |
376 | + | return Vector3<Real>(w()*q.x() + x()*q.w() + y()*q.z() - z()*q.y(), |
377 | + | w()*q.y() + y()*q.w() + z()*q.x() - x()*q.z(), |
378 | + | w()*q.z() + z()*q.w() + x()*q.y() - y()*q.x())* |
379 | + | ( 1.0/this->lengthSquare() ); |
380 | + | } |
381 | + | |
382 | + | Quaternion<Real>& align (const Vector3<Real>& V1, |
383 | + | const Vector3<Real>& V2) { |
384 | + | |
385 | + | // If V1 and V2 are not parallel, the axis of rotation is the unit-length |
386 | + | // vector U = Cross(V1,V2)/Length(Cross(V1,V2)). The angle of rotation, |
387 | + | // A, is the angle between V1 and V2. The quaternion for the rotation is |
388 | + | // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz). |
389 | + | // |
390 | + | // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then |
391 | + | // compute sin(A/2) and cos(A/2), we reduce the computational costs |
392 | + | // by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) = |
393 | + | // Dot(V1,B). |
394 | + | // |
395 | + | // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but |
396 | + | // Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in |
397 | + | // which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where |
398 | + | // C = Cross(V1,B). |
399 | + | // |
400 | + | // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0). If V1 = -V2, |
401 | + | // then B = 0. This can happen even if V1 is approximately -V2 using |
402 | + | // floating point arithmetic, since Vector3::Normalize checks for |
403 | + | // closeness to zero and returns the zero vector accordingly. The test |
404 | + | // for exactly zero is usually not recommend for floating point |
405 | + | // arithmetic, but the implementation of Vector3::Normalize guarantees |
406 | + | // the comparison is robust. In this case, the A = pi and any axis |
407 | + | // perpendicular to V1 may be used as the rotation axis. |
408 | + | |
409 | + | Vector3<Real> Bisector = V1 + V2; |
410 | + | Bisector.normalize(); |
411 | + | |
412 | + | Real CosHalfAngle = dot(V1,Bisector); |
413 | + | |
414 | + | this->data_[0] = CosHalfAngle; |
415 | + | |
416 | + | if (CosHalfAngle != (Real)0.0) { |
417 | + | Vector3<Real> Cross = cross(V1, Bisector); |
418 | + | this->data_[1] = Cross.x(); |
419 | + | this->data_[2] = Cross.y(); |
420 | + | this->data_[3] = Cross.z(); |
421 | + | } else { |
422 | + | Real InvLength; |
423 | + | if (fabs(V1[0]) >= fabs(V1[1])) { |
424 | + | // V1.x or V1.z is the largest magnitude component |
425 | + | InvLength = (Real)1.0/sqrt(V1[0]*V1[0] + V1[2]*V1[2]); |
426 | + | |
427 | + | this->data_[1] = -V1[2]*InvLength; |
428 | + | this->data_[2] = (Real)0.0; |
429 | + | this->data_[3] = +V1[0]*InvLength; |
430 | + | } else { |
431 | + | // V1.y or V1.z is the largest magnitude component |
432 | + | InvLength = (Real)1.0 / sqrt(V1[1]*V1[1] + V1[2]*V1[2]); |
433 | + | |
434 | + | this->data_[1] = (Real)0.0; |
435 | + | this->data_[2] = +V1[2]*InvLength; |
436 | + | this->data_[3] = -V1[1]*InvLength; |
437 | + | } |
438 | + | } |
439 | + | return *this; |
440 | + | } |
441 | + | |
442 | + | void toTwistSwing ( Real& tw, Real& sx, Real& sy ) { |
443 | + | |
444 | + | // First test if the swing is in the singularity: |
445 | + | |
446 | + | if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; } |
447 | + | |
448 | + | // Decompose into twist-swing by solving the equation: |
449 | + | // |
450 | + | // Qtwist(t*2) * Qswing(s*2) = q |
451 | + | // |
452 | + | // note: (x,y) is the normalized swing axis (x*x+y*y=1) |
453 | + | // |
454 | + | // ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz ) |
455 | + | // ( CtCs xSsCt-yStSs xStSs+ySsCt StCs ) = ( qw qx qy qz ) (1) |
456 | + | // From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2) |
457 | + | // |
458 | + | // The swing rotation/2 s comes from: |
459 | + | // |
460 | + | // From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 => |
461 | + | // Cs = sqrt ( qw^2 + qz^2 ) (3) |
462 | + | // |
463 | + | // From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 => |
464 | + | // Ss = sqrt ( qx^2 + qy^2 ) (4) |
465 | + | // From (1): |SsCt -StSs| |x| = |qx| |
466 | + | // |StSs +SsCt| |y| |qy| (5) |
467 | + | |
468 | + | Real qw, qx, qy, qz; |
469 | + | |
470 | + | if ( w()<0 ) { |
471 | + | qw=-w(); |
472 | + | qx=-x(); |
473 | + | qy=-y(); |
474 | + | qz=-z(); |
475 | + | } else { |
476 | + | qw=w(); |
477 | + | qx=x(); |
478 | + | qy=y(); |
479 | + | qz=z(); |
480 | + | } |
481 | + | |
482 | + | Real t = atan2 ( qz, qw ); // from (2) |
483 | + | Real s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3) |
484 | + | // and (4) |
485 | + | |
486 | + | Real x=0.0, y=0.0, sins=sin(s); |
487 | + | |
488 | + | if ( !ISZERO(sins,tiny) ) { |
489 | + | Real sint = sin(t); |
490 | + | Real cost = cos(t); |
491 | + | |
492 | + | // by solving the linear system in (5): |
493 | + | y = (-qx*sint + qy*cost)/sins; |
494 | + | x = ( qx*cost + qy*sint)/sins; |
495 | + | } |
496 | + | |
497 | + | tw = (Real)2.0*t; |
498 | + | sx = (Real)2.0*x*s; |
499 | + | sy = (Real)2.0*y*s; |
500 | + | } |
501 | + | |
502 | + | void toSwingTwist(Real& sx, Real& sy, Real& tw ) { |
503 | + | |
504 | + | // Decompose q into swing-twist using a similar development as |
505 | + | // in function toTwistSwing |
506 | + | |
507 | + | if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; } |
508 | + | |
509 | + | Real qw, qx, qy, qz; |
510 | + | if ( w() < 0 ){ |
511 | + | qw=-w(); |
512 | + | qx=-x(); |
513 | + | qy=-y(); |
514 | + | qz=-z(); |
515 | + | } else { |
516 | + | qw=w(); |
517 | + | qx=x(); |
518 | + | qy=y(); |
519 | + | qz=z(); |
520 | + | } |
521 | + | |
522 | + | // Get the twist t: |
523 | + | Real t = 2.0 * atan2(qz,qw); |
524 | + | |
525 | + | Real bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); |
526 | + | Real gam = t/2.0; |
527 | + | Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet; |
528 | + | Real singam = sin(gam); |
529 | + | Real cosgam = cos(gam); |
530 | + | |
531 | + | sx = Real( (2.0/sinc) * (cosgam*qx - singam*qy) ); |
532 | + | sy = Real( (2.0/sinc) * (singam*qx + cosgam*qy) ); |
533 | + | tw = Real( t ); |
534 | + | } |
535 | + | |
536 | + | |
537 | + | |
538 | + | /** |
539 | * Returns the corresponding rotation matrix (3x3) | |
540 | * @return a 3x3 rotation matrix | |
541 | */ | |
542 | SquareMatrix<Real, 3> toRotationMatrix3() { | |
543 | SquareMatrix<Real, 3> rotMat3; | |
544 | < | |
544 | > | |
545 | Real w2; | |
546 | Real x2; | |
547 | Real y2; |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |