kubrick/src/quaternion.cpp

   1 /*******************************************************************************
   2   Copyright 2008 Ian Wadham <ianw2@optusnet.com.au>
   3
   4   This program is free software; you can redistribute it and/or modify
   5   it under the terms of the GNU General Public License as published by
   6   the Free Software Foundation; either version 2 of the License, or
   7   (at your option) any later version.
   8
   9   This program is distributed in the hope that it will be useful,
  10   but WITHOUT ANY WARRANTY; without even the implied warranty of
  11   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12   GNU General Public License for more details.
  13
  14   You should have received a copy of the GNU General Public License
  15   along with this program; if not, write to the Free Software
  16   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  17 *******************************************************************************/
  18
  19 /*
  20   A little library to do quaternion arithmetic.  Adapted from C to C++.
  21   Acknowledgements and thanks to John Darrington and the Gnubik package.
  22 */
  23
  24 #include "quaternion.h"
  25
  26 #include <math.h>
  27 #include <stdlib.h>
  28 #include <stdio.h>
  29
  30 Quaternion::Quaternion ()
  31 {
  32     quaternionSetIdentity();
  33 }
  34
  35
  36 Quaternion::Quaternion (double rPart, double iPart, double jPart, double kPart)
  37 {
  38     w = rPart;
  39     x = iPart;
  40     y = jPart;
  41     z = kPart;
  42 }
  43
  44
  45 void Quaternion::quaternionSetIdentity ()
  46 {
  47     w = 1.0;
  48     x = 0.0;
  49     y = 0.0;
  50     z = 0.0;
  51 }
  52
  53
  54 void Quaternion::quaternionAddRotation
  55                         (const double axis[], const double degrees)
  56 {
  57     // If the axis-vector and quaternion are both normalised,
  58     // then the new quaternion will also be normalised.
  59
  60     Quaternion q;
  61     double radians = degrees * M_PI / 180.0;
  62     double s = sin (radians / 2.0);
  63
  64     q.w = cos (radians / 2.0);
  65     q.x = axis [0] * s;
  66     q.y = axis [1] * s;
  67     q.z = axis [2] * s;
  68
  69     quaternionPreMultiply (this, &q);
  70 }
  71
  72
  73 void Quaternion::quaternionPreMultiply
  74                         (Quaternion * q1, const Quaternion * q2) const
  75 {
  76     double s1 = q1->w;
  77     double x1 = q1->x;
  78     double y1 = q1->y;
  79     double z1 = q1->z;
  80
  81     double s2 = q2->w;
  82     double x2 = q2->x;
  83     double y2 = q2->y;
  84     double z2 = q2->z;
  85
  86     double dot_product;
  87     double cross_product [3];
  88 /*
  89     printf("Q mult\n");
  90     quaternionPrint(q1);
  91     quaternionPrint(q2);
  92 */
  93     dot_product = x1*x2 + y1*y2 + z1*z2;
  94 /*
  95     printf("Dot product is %f\n",dot_product);
  96 */
  97     cross_product [0] = y1*z2 - z1*y2;
  98     cross_product [1] = z1*x2 - x1*z2;
  99     cross_product [2] = x1*y2 - y1*x2;
 100 /*
 101     printf("Cross product is %f, %f, %f\n",cross_product[0],
 102     cross_product[1],
 103     cross_product[2]);
 104 */
 105     q1->w = s1*s2 - dot_product;
 106     q1->x = s1*x2 + s2*x1 + cross_product[0];
 107     q1->y = s1*y2 + s2*y1 + cross_product[1];
 108     q1->z = s1*z2 + s2*z1 + cross_product[2];
 109 }
 110
 111
 112 void Quaternion::quaternionToMatrix (Matrix M) const
 113 {
 114     double ww = w * w;
 115     double xx = x * x;
 116     double yy = y * y;
 117     double zz = z * z;
 118
 119     double wx = w * x;
 120     double wy = w * y;
 121     double wz = w * z;
 122
 123     double xy = x * y;
 124     double yz = y * z;
 125     double zx = z * x;
 126
 127     int    dim = DIMENSIONS;
 128
 129     /* Diagonal */
 130     M [0 + dim * 0] = ww + xx - yy - zz; // If q is normalised, 1 - 2*y2 - 2*z2.
 131     M [1 + dim * 1] = ww - xx + yy - zz; // etc.
 132     M [2 + dim * 2] = ww - xx - yy + zz; // etc.
 133     M [3 + dim * 3] = ww + xx + yy + zz; // If q is normalised, this is 1.0.
 134
 135     /* Last row */
 136     M [0 + dim * 3] = 0.0;
 137     M [1 + dim * 3] = 0.0;
 138     M [2 + dim * 3] = 0.0;
 139
 140     /* Last Column */
 141     M [3 + dim * 0] = 0.0;
 142     M [3 + dim * 1] = 0.0;
 143     M [3 + dim * 2] = 0.0;
 144
 145     /* Others */
 146     M [0 + dim * 1] = 2.0 * (xy + wz);
 147     M [0 + dim * 2] = 2.0 * (zx - wy);
 148     M [1 + dim * 2] = 2.0 * (yz + wx);
 149
 150     M [1 + dim * 0] = 2.0 * (xy - wz);
 151     M [2 + dim * 0] = 2.0 * (zx + wy);
 152     M [2 + dim * 1] = 2.0 * (yz - wx);
 153 }
 154
 155
 156 void Quaternion::quaternionInvert()
 157 {
 158     x = -x;
 159     y = -y;
 160     z = -z;
 161 }
 162
 163
 164 void Quaternion::quaternionRotateVector (double axis[3]) const
 165 {
 166     // Make a copy of the quaternion ("this") that will rotate the vector.
 167     Quaternion q  (w,  x,  y,  z);
 168
 169     // Convert the vector to a quaternion.
 170     Quaternion v  (0.0, axis[0], axis[1], axis[2]);
 171
 172     // Get the inverse of "this" quaternion.
 173     Quaternion q1 (w, -x, -y, -z);
 174
 175     // Multiply the three quaternions together: q*v*q1.
 176     quaternionPreMultiply (&q1, &v);
 177     quaternionPreMultiply (&q1, &q);
 178
 179     // Return the rotated vector.
 180     axis[0] = q1.x;
 181     axis[1] = q1.y;
 182     axis[2] = q1.z;
 183 }
 184
 185
 186 void Quaternion::quaternionPrint() const
 187 {
 188     printf ("Quaternion (%6.3f, %6.3f, %6.3f, %6.3f)\n", w, x, y, z);
 189     Quaternion p (w, x, y, z);
 190     double mod = sqrt (p.w * p.w + p.x * p.x + p.y * p.y + p.z * p.z);
 191     p.w = p.w / mod;
 192     p.x = p.x / mod;
 193     p.y = p.y / mod;
 194     p.z = p.z / mod;
 195     double rad = acos (p.w);
 196     if (fabs (rad) >= 0.0001) {
 197         double s   = sin  (rad);
 198         printf ("Angle %8.3f, axis (%6.3f, %6.3f, %6.3f)\n",
 199                 2.0 * rad * 180.0 / M_PI, p.x/s, p.y/s, p.z/s);
 200     }
 201     else {
 202         printf ("Angle %8.3f, axis (%6.3f, %6.3f, %6.3f)\n",
 203                 0.0, 0.0, 0.0, 0.0);
 204     }
 205 }