git.mutantstargoat.com Git - summerhack/blob - src/3dengfx/src/n3dmath2/n3dmath2_qua.cpp

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