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