git.mutantstargoat.com Git - summerhack/blob - tools/curve_draw/vmath/quaternion.cc

   1 /*
   2 This file is part of XRay, a photorealistic 3D rendering library.
   3 Copyright (C) 2005 John Tsiombikas
   4
   5 XRay is free software; you can redistribute it and/or modify
   6 it under the terms of the GNU General Public License as published by
   7 the Free Software Foundation; either version 2 of the License, or
   8 (at your option) any later version.
   9
  10 XRay is distributed in the hope that it will be useful,
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 GNU General Public License for more details.
  14
  15 You should have received a copy of the GNU General Public License
  16 along with XRay; if not, write to the Free Software
  17 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  18 */
  19
  20 /**
  21  * @file quaternion.cxx
  22  * @author John Tsiombikas
  23  * @date 28 June 2005
  24  *
  25  * Quaternion math.
  26  */
  27
  28 #include "quaternion.h"
  29
  30 Quaternion::Quaternion() {
  31         s = 1.0;
  32         v.x = v.y = v.z = 0.0;
  33 }
  34
  35 Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z) {
  36         v.x = x;
  37         v.y = y;
  38         v.z = z;
  39         this->s = s;
  40 }
  41
  42 Quaternion::Quaternion(scalar_t s, const Vector3 &v) {
  43         this->s = s;
  44         this->v = v;
  45 }
  46
  47 Quaternion::Quaternion(const Vector3 &axis, scalar_t angle) {
  48         set_rotation(axis, angle);
  49 }
  50
  51 Quaternion Quaternion::operator +(const Quaternion &quat) const {
  52         return Quaternion(s + quat.s, v + quat.v);
  53 }
  54
  55 Quaternion Quaternion::operator -(const Quaternion &quat) const {
  56         return Quaternion(s - quat.s, v - quat.v);
  57 }
  58
  59 Quaternion Quaternion::operator -() const {
  60         return Quaternion(-s, -v);
  61 }
  62
  63 /** Quaternion Multiplication:
  64  * Q1*Q2 = [s1*s2 - v1.v2,  s1*v2 + s2*v1 + v1(x)v2]
  65  */
  66 Quaternion Quaternion::operator *(const Quaternion &quat) const {
  67         Quaternion newq;
  68         newq.s = s * quat.s - dot_product(v, quat.v);
  69         newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
  70         return newq;
  71 }
  72
  73 void Quaternion::operator +=(const Quaternion &quat) {
  74         *this = Quaternion(s + quat.s, v + quat.v);
  75 }
  76
  77 void Quaternion::operator -=(const Quaternion &quat) {
  78         *this = Quaternion(s - quat.s, v - quat.v);
  79 }
  80
  81 void Quaternion::operator *=(const Quaternion &quat) {
  82         *this = *this * quat;
  83 }
  84
  85 void Quaternion::reset_identity() {
  86         s = 1.0;
  87         v.x = v.y = v.z = 0.0;
  88 }
  89
  90 Quaternion Quaternion::conjugate() const {
  91         return Quaternion(s, -v);
  92 }
  93
  94 scalar_t Quaternion::length() const {
  95         return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
  96 }
  97
  98 /** Q * ~Q = ||Q||^2 */
  99 scalar_t Quaternion::length_sq() const {
 100         return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
 101 }
 102
 103 void Quaternion::normalize() {
 104         scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
 105         v.x /= len;
 106         v.y /= len;
 107         v.z /= len;
 108         s /= len;
 109 }
 110
 111 Quaternion Quaternion::normalized() const {
 112         Quaternion nq = *this;
 113         scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
 114         nq.v.x /= len;
 115         nq.v.y /= len;
 116         nq.v.z /= len;
 117         nq.s /= len;
 118         return nq;
 119 }
 120
 121 /** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
 122 Quaternion Quaternion::inverse() const {
 123         Quaternion inv = conjugate();
 124         scalar_t lensq = length_sq();
 125         inv.v /= lensq;
 126         inv.s /= lensq;
 127
 128         return inv;
 129 }
 130
 131
 132 void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle) {
 133         scalar_t HalfAngle = angle / 2.0;
 134         s = cos(HalfAngle);
 135         v = axis * sin(HalfAngle);
 136 }
 137
 138 void Quaternion::rotate(const Vector3 &axis, scalar_t angle) {
 139         Quaternion q;
 140         scalar_t HalfAngle = angle / 2.0;
 141         q.s = cos(HalfAngle);
 142         q.v = axis * sin(HalfAngle);
 143
 144         *this *= q;
 145 }
 146
 147 void Quaternion::rotate(const Quaternion &q) {
 148         *this = q * *this * q.conjugate();
 149 }
 150
 151 Matrix3x3 Quaternion::get_rotation_matrix() const {
 152         return Matrix3x3(       1.0 - 2.0 * v.y*v.y - 2.0 * v.z*v.z,    2.0 * v.x * v.y + 2.0 * s * v.z,                2.0 * v.z * v.x - 2.0 * s * v.y,
 153                                                 2.0 * v.x * v.y - 2.0 * s * v.z,                1.0 - 2.0 * v.x*v.x - 2.0 * v.z*v.z,    2.0 * v.y * v.z + 2.0 * s * v.x,
 154                                                 2.0 * v.z * v.x + 2.0 * s * v.y,                2.0 * v.y * v.z - 2.0 * s * v.x,                1.0 - 2.0 * v.x*v.x - 2.0 * v.y*v.y);
 155 }
 156
 157
 158 /** Spherical linear interpolation (slerp) */
 159 Quaternion slerp(const Quaternion &q1, const Quaternion &q2, scalar_t t) {
 160         scalar_t dot = dot_product(Vector4(q1.s, q1.v.x, q1.v.y, q1.v.z), Vector4(q2.s, q2.v.x, q2.v.y, q2.v.z));
 161
 162         if(fabs(1.0 - dot) < xsmall_number) return q1;  // avoids divisions by zero later on if q1 == q2
 163
 164         if(dot >= 0.0f) {
 165                 scalar_t angle = acos(dot);
 166                 scalar_t coef1 = (angle * sin(1.0 - t)) / sin(angle);
 167                 scalar_t coef2 = sin(angle * t) / sin(angle);
 168                 return Quaternion(q1.s * coef1 + q2.s * coef2, q1.v * coef1 + q2.v * coef2).normalized();
 169         } else {
 170                 scalar_t angle = acos(-dot);
 171                 scalar_t coef1 = (angle * sin(1.0 - t)) / sin(angle);
 172                 scalar_t coef2 = sin(angle * t) / sin(angle);
 173                 return Quaternion(q2.s * coef1 + q1.s * coef2, q2.v * coef1 + q1.v * coef2).normalized();
 174         }
 175 }
 176
 177
 178
 179 std::ostream &operator <<(std::ostream &out, const Quaternion &q) {
 180         out << "(" << q.s << ", " << q.v << ")";
 181         return out;
 182 }