Main Page   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members   Related Pages  

VMDQuat.C

Go to the documentation of this file.
00001 /***************************************************************************
00002  *cr
00003  *cr            (C) Copyright 1995-2019 The Board of Trustees of the
00004  *cr                        University of Illinois
00005  *cr                         All Rights Reserved
00006  *cr
00007  ***************************************************************************/
00008 
00009 /***************************************************************************
00010  * RCS INFORMATION:
00011  *
00012  *      $RCSfile: VMDQuat.C,v $
00013  *      $Author: johns $        $Locker:  $             $State: Exp $
00014  *      $Revision: 1.14 $      $Date: 2019/09/27 05:28:50 $
00015  *
00016  ***************************************************************************
00017  * DESCRIPTION:
00018  *   Quaternion class used by tracker/tool code 
00019  ***************************************************************************/
00020 
00021 #include <math.h>
00022 #include "VMDQuat.h"
00023 #include "utilities.h"
00024 
00025 void Quat::identity() {
00026   qx = qy = qz = 0;
00027   qw = 1;
00028 }
00029 
00030 Quat::Quat(double x, double y, double z, double w) {
00031   qx = x;
00032   qy = y;
00033   qz = z;
00034   qw = w;
00035 }
00036 
00037 void Quat::rotate(const float *u, float angle) {
00038   Quat q;
00039   double halftheta = 0.5 * DEGTORAD(angle);
00040   double sinhalftheta, coshalftheta;
00041   sincos(halftheta, &sinhalftheta, &coshalftheta);
00042   q.qw = coshalftheta;
00043   q.qx = u[0]*sinhalftheta;
00044   q.qy = u[1]*sinhalftheta;
00045   q.qz = u[2]*sinhalftheta;
00046   mult(q);
00047 }
00048 
00049 void Quat::rotate(char axis, float angle) {
00050   Quat q;
00051   double halftheta = 0.5 * DEGTORAD(angle);
00052   double sinhalftheta, coshalftheta;
00053   sincos(halftheta, &sinhalftheta, &coshalftheta);
00054   q.qw = coshalftheta;
00055   switch(axis) {
00056     case 'x': q.qx = sinhalftheta; q.qy = q.qz = 0; break;
00057     case 'y': q.qy = sinhalftheta; q.qz = q.qx = 0; break;
00058     case 'z': q.qz = sinhalftheta; q.qx = q.qy = 0; break;
00059   }
00060   mult(q); 
00061 }
00062 
00063 void Quat::invert() {
00064   qx = -qx;
00065   qy = -qy;
00066   qz = -qz;
00067 }
00068 
00069 void Quat::mult(const Quat &q) {
00070   double x,y,z,w;
00071   w = q.qw*qw - q.qx*qx - q.qy*qy - q.qz*qz;
00072   x = q.qw*qx + q.qx*qw + q.qy*qz - q.qz*qy;
00073   y = q.qw*qy - q.qx*qz + q.qy*qw + q.qz*qx;
00074   z = q.qw*qz + q.qx*qy - q.qy*qx + q.qz*qw;
00075   qw = w;
00076   qx = x;
00077   qy = y;
00078   qz = z;
00079 }
00080 
00081 void Quat::multpoint3(const float *p, float *out) const {
00082   Quat pquat(p[0], p[1], p[2], 0);
00083   Quat inv(-qx, -qy, -qz, qw);
00084   inv.mult(pquat);
00085   inv.mult(*this);
00086   out[0] = (float) inv.qx;
00087   out[1] = (float) inv.qy;
00088   out[2] = (float) inv.qz;
00089 }
00090 
00091 void Quat::printQuat(float *q) {
00092   q[0] = (float) qx;
00093   q[1] = (float) qy;
00094   q[2] = (float) qz;
00095   q[3] = (float) qw;
00096 }
00097 
00098 void Quat::printMatrix(float *m) {
00099   m[0] = (float) (qw*qw + qx*qx - qy*qy -qz*qz);
00100   m[1] = (float) (2*(qx*qy + qw*qz));
00101   m[2] = (float) (2*(qx*qz - qw*qy));
00102 
00103   m[4] = (float) (2*(qx*qy - qw*qz));
00104   m[5] = (float) (qw*qw - qx*qx + qy*qy - qz*qz);
00105   m[6] = (float) (2*(qy*qz + qw*qx));
00106   
00107   m[8] = (float) (2*(qx*qz + qw*qy));
00108   m[9] = (float) (2*(qy*qz - qw*qx));
00109   m[10]= (float) (qw*qw - qx*qx - qy*qy + qz*qz);
00110   
00111   m[15]= (float) (qw*qw + qx*qx + qy*qy + qz*qz);
00112   
00113   m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0;
00114 }
00115 
00116 static int perm[3] = {1,2,0};
00117 #define mat(a,b) (m[4*a+b])
00118 
00119 void Quat::fromMatrix(const float *m) {
00120   float T = mat(0,0) + mat(1,1) + mat(2,2);
00121   if (T > 0) { // w is the largest element in the quat
00122     double iw = sqrt(T+1.0);
00123     qw = iw * 0.5; 
00124     iw = 0.5/iw;
00125     qx = (mat(1,2) - mat(2,1)) * iw;
00126     qy = (mat(2,0) - mat(0,2)) * iw;
00127     qz = (mat(0,1) - mat(1,0)) * iw;
00128   } else {     // Find the largest diagonal element
00129     int i,j,k;
00130     double &qi = qx, &qj = qy, &qk = qz;
00131     i=0;
00132     if (mat(1,1) > mat(0,0)) {i=1; qi = qy; qj = qz; qk = qx; }
00133     if (mat(2,2) > mat(i,i)) {i=2; qi = qz; qj = qx; qk = qy; }
00134     j=perm[i];
00135     k=perm[j];
00136  
00137     double iqi = sqrt( (mat(i,i) - (mat(j,j) + mat(k,k))) + 1.0);
00138     qi = iqi * 0.5;
00139     iqi = 0.5/iqi;
00140    
00141     qw = (mat(j,k) - mat(k,j))*iqi;
00142     qj = (mat(i,j) + mat(j,i))*iqi;
00143     qk = (mat(i,k) + mat(k,i))*iqi;
00144   }
00145 }
00146  

Generated on Fri Mar 29 02:46:24 2024 for VMD (current) by doxygen1.2.14 written by Dimitri van Heesch, © 1997-2002