[37] | 1 | /* |
---|
| 2 | |
---|
| 3 | Brian Curless |
---|
| 4 | |
---|
| 5 | Computer Graphics Laboratory |
---|
| 6 | Stanford University |
---|
| 7 | |
---|
| 8 | --------------------------------------------------------------------- |
---|
| 9 | |
---|
| 10 | Copyright (1997) The Board of Trustees of the Leland Stanford Junior |
---|
| 11 | University. Except for commercial resale, lease, license or other |
---|
| 12 | commercial transactions, permission is hereby given to use, copy, |
---|
| 13 | modify this software for academic purposes only. No part of this |
---|
| 14 | software or any derivatives thereof may be used in the production of |
---|
| 15 | computer models for resale or for use in a commercial |
---|
| 16 | product. STANFORD MAKES NO REPRESENTATIONS OR WARRANTIES OF ANY KIND |
---|
| 17 | CONCERNING THIS SOFTWARE. No support is implied or provided. |
---|
| 18 | |
---|
| 19 | */ |
---|
| 20 | |
---|
| 21 | |
---|
| 22 | #include "Quaternion.h" |
---|
| 23 | #include "Matrix4f.h" |
---|
| 24 | |
---|
| 25 | #define X 0 |
---|
| 26 | #define Y 1 |
---|
| 27 | #define Z 2 |
---|
| 28 | #define W 3 |
---|
| 29 | |
---|
| 30 | |
---|
| 31 | Quaternion::Quaternion() |
---|
| 32 | { |
---|
| 33 | q[0] = 0; |
---|
| 34 | q[1] = 0; |
---|
| 35 | q[2] = 0; |
---|
| 36 | q[3] = 1; |
---|
| 37 | } |
---|
| 38 | |
---|
| 39 | Quaternion::~Quaternion() |
---|
| 40 | { |
---|
| 41 | } |
---|
| 42 | |
---|
| 43 | |
---|
| 44 | void |
---|
| 45 | Quaternion::setValue(float q0, float q1, float q2, float q3) |
---|
| 46 | { |
---|
| 47 | q[0] = q0; |
---|
| 48 | q[1] = q1; |
---|
| 49 | q[2] = q2; |
---|
| 50 | q[3] = q3; |
---|
| 51 | } |
---|
| 52 | |
---|
| 53 | void |
---|
| 54 | Quaternion::setValue(float *quat) |
---|
| 55 | { |
---|
| 56 | q[0] = quat[0]; |
---|
| 57 | q[1] = quat[1]; |
---|
| 58 | q[2] = quat[2]; |
---|
| 59 | q[3] = quat[3]; |
---|
| 60 | } |
---|
| 61 | |
---|
| 62 | |
---|
| 63 | void |
---|
| 64 | Quaternion::setValue(Quaternion &quat) |
---|
| 65 | { |
---|
| 66 | q[0] = quat.q[0]; |
---|
| 67 | q[1] = quat.q[1]; |
---|
| 68 | q[2] = quat.q[2]; |
---|
| 69 | q[3] = quat.q[3]; |
---|
| 70 | } |
---|
| 71 | |
---|
| 72 | |
---|
| 73 | /****************************************************************************** |
---|
| 74 | Convert a quaternion into a rotation matrix. Does *not* assume a unit |
---|
| 75 | quaternion. From Ken Shoemake. |
---|
| 76 | |
---|
| 77 | ******************************************************************************/ |
---|
| 78 | void |
---|
| 79 | Quaternion::toMatrix(Matrix4f &mat) |
---|
| 80 | { |
---|
| 81 | float s; |
---|
| 82 | float xs,ys,zs; |
---|
| 83 | float wx,wy,wz; |
---|
| 84 | float xx,xy,xz; |
---|
| 85 | float yy,yz,zz; |
---|
| 86 | |
---|
| 87 | /* for unit q, just set s = 2 or set xs = this->q[X] + this->q[X], etc. */ |
---|
| 88 | |
---|
| 89 | s = 2 / (this->q[X]*this->q[X] + this->q[Y]*this->q[Y] |
---|
| 90 | + this->q[Z]*this->q[Z] + this->q[W]*this->q[W]); |
---|
| 91 | |
---|
| 92 | xs = this->q[X] * s; |
---|
| 93 | ys = this->q[Y] * s; |
---|
| 94 | zs = this->q[Z] * s; |
---|
| 95 | |
---|
| 96 | wx = this->q[W] * xs; |
---|
| 97 | wy = this->q[W] * ys; |
---|
| 98 | wz = this->q[W] * zs; |
---|
| 99 | |
---|
| 100 | xx = this->q[X] * xs; |
---|
| 101 | xy = this->q[X] * ys; |
---|
| 102 | xz = this->q[X] * zs; |
---|
| 103 | |
---|
| 104 | yy = this->q[Y] * ys; |
---|
| 105 | yz = this->q[Y] * zs; |
---|
| 106 | zz = this->q[Z] * zs; |
---|
| 107 | |
---|
| 108 | mat.m[X][X] = 1 - (yy + zz); |
---|
| 109 | mat.m[X][Y] = xy - wz; |
---|
| 110 | mat.m[X][Z] = xz + wy; |
---|
| 111 | mat.m[X][W] = 0; |
---|
| 112 | |
---|
| 113 | mat.m[Y][X] = xy + wz; |
---|
| 114 | mat.m[Y][Y] = 1 - (xx + zz); |
---|
| 115 | mat.m[Y][Z] = yz - wx; |
---|
| 116 | mat.m[Y][W] = 0; |
---|
| 117 | |
---|
| 118 | mat.m[Z][X] = xz - wy; |
---|
| 119 | mat.m[Z][Y] = yz + wx; |
---|
| 120 | mat.m[Z][Z] = 1 - (xx + yy); |
---|
| 121 | mat.m[Z][W] = 0; |
---|
| 122 | |
---|
| 123 | mat.m[W][X] = 0; |
---|
| 124 | mat.m[W][Y] = 0; |
---|
| 125 | mat.m[W][Z] = 0; |
---|
| 126 | mat.m[W][W] = 1; |
---|
| 127 | } |
---|
| 128 | |
---|
| 129 | |
---|
| 130 | /****************************************************************************** |
---|
| 131 | Convert a rotation mat.mrix into a unit quaternion. From Ken Shoemake. |
---|
| 132 | |
---|
| 133 | ******************************************************************************/ |
---|
| 134 | void |
---|
| 135 | Quaternion::fromMatrix(Matrix4f &mat) |
---|
| 136 | { |
---|
| 137 | int i,j,k; |
---|
| 138 | float tr,s; |
---|
| 139 | static int nxt[3] = {Y, Z, X}; |
---|
| 140 | |
---|
| 141 | tr = mat.m[X][X] + mat.m[Y][Y] + mat.m[Z][Z]; |
---|
| 142 | |
---|
| 143 | if (tr > 0) { |
---|
| 144 | s = sqrt (tr + 1); |
---|
| 145 | this->q[W] = s * 0.5; |
---|
| 146 | s = 0.5 / s; |
---|
| 147 | this->q[X] = (mat.m[Z][Y] - mat.m[Y][Z]) * s; |
---|
| 148 | this->q[Y] = (mat.m[X][Z] - mat.m[Z][X]) * s; |
---|
| 149 | this->q[Z] = (mat.m[Y][X] - mat.m[X][Y]) * s; |
---|
| 150 | } |
---|
| 151 | else { |
---|
| 152 | i = X; |
---|
| 153 | if (mat.m[Y][Y] > mat.m[X][X]) |
---|
| 154 | i = Y; |
---|
| 155 | if (mat.m[Z][Z] > mat.m[i][i]) |
---|
| 156 | i = Z; |
---|
| 157 | j = nxt[i]; |
---|
| 158 | k = nxt[j]; |
---|
| 159 | s = sqrt (1 + (mat.m[i][i] - (mat.m[j][j] + mat.m[k][k]))); |
---|
| 160 | this->q[i] = s * 0.5; |
---|
| 161 | s = 0.5 / s; |
---|
| 162 | this->q[W] = (mat.m[k][j] - mat.m[j][k]) * s; |
---|
| 163 | this->q[j] = (mat.m[j][i] + mat.m[i][j]) * s; |
---|
| 164 | this->q[k] = (mat.m[k][i] + mat.m[i][k]) * s; |
---|
| 165 | } |
---|
| 166 | } |
---|
| 167 | |
---|
| 168 | |
---|
| 169 | void |
---|
| 170 | Quaternion::normalize() |
---|
| 171 | { |
---|
| 172 | float norm; |
---|
| 173 | norm = sqrt(this->q[X]*this->q[X] + this->q[Y]*this->q[Y] |
---|
| 174 | +this->q[Z]*this->q[Z] + this->q[W]*this->q[W]); |
---|
| 175 | |
---|
| 176 | if (norm != 0) { |
---|
| 177 | norm = 1/norm; |
---|
| 178 | this->q[X] = norm*this->q[X]; |
---|
| 179 | this->q[Y] = norm*this->q[Y]; |
---|
| 180 | this->q[Z] = norm*this->q[Z]; |
---|
| 181 | this->q[W] = norm*this->q[W]; |
---|
| 182 | } else { |
---|
| 183 | /* Uh oh. Do nothing? */ |
---|
| 184 | } |
---|
| 185 | } |
---|
| 186 | |
---|
| 187 | void |
---|
| 188 | Quaternion::toAxisAngle(float &angle, Vec3f &axis) |
---|
| 189 | { |
---|
| 190 | float norm; |
---|
| 191 | |
---|
| 192 | angle = acos(this->q[W])*360/M_PI; |
---|
| 193 | norm = sqrt(this->q[X]*this->q[X] |
---|
| 194 | + this->q[Y]*this->q[Y] + this->q[Z]*this->q[Z]); |
---|
| 195 | if (norm != 0) { |
---|
| 196 | norm = 1/norm; |
---|
| 197 | axis.setValue(this->q[X]*norm, |
---|
| 198 | this->q[Y]*norm, this->q[Z]*norm); |
---|
| 199 | } else { |
---|
| 200 | axis.setValue(0,0,1); |
---|
| 201 | } |
---|
| 202 | } |
---|
| 203 | |
---|