2 This file is part of the n3dmath2 library.
4 Copyright (c) 2003 - 2005 John Tsiombikas <nuclear@siggraph.org>
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.
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.
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
23 * Author: John Tsiombikas 2003
24 * Modified: John Tsiombikas 2004, 2005
27 #include "n3dmath2.hpp"
29 Quaternion::Quaternion() {
31 v.x = v.y = v.z = 0.0;
34 Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z) {
41 Quaternion::Quaternion(scalar_t s, const Vector3 &v) {
46 Quaternion::Quaternion(const Vector3 &axis, scalar_t angle) {
47 set_rotation(axis, angle);
50 Quaternion Quaternion::operator +(const Quaternion &quat) const {
51 return Quaternion(s + quat.s, v + quat.v);
54 Quaternion Quaternion::operator -(const Quaternion &quat) const {
55 return Quaternion(s - quat.s, v - quat.v);
58 Quaternion Quaternion::operator -() const {
59 return Quaternion(-s, -v);
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 {
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);
71 void Quaternion::operator +=(const Quaternion &quat) {
72 *this = Quaternion(s + quat.s, v + quat.v);
75 void Quaternion::operator -=(const Quaternion &quat) {
76 *this = Quaternion(s - quat.s, v - quat.v);
79 void Quaternion::operator *=(const Quaternion &quat) {
83 void Quaternion::reset_identity() {
85 v.x = v.y = v.z = 0.0;
88 Quaternion Quaternion::conjugate() const {
89 return Quaternion(s, -v);
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);
97 scalar_t Quaternion::length_sq() const {
98 return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
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);
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);
119 // Quaternion Inversion: Q^-1 = ~Q / ||Q||^2
120 Quaternion Quaternion::inverse() const {
121 Quaternion inv = conjugate();
122 scalar_t lensq = length_sq();
130 void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle) {
131 scalar_t HalfAngle = angle / 2.0;
133 v = axis * sin(HalfAngle);
136 void Quaternion::rotate(const Vector3 &axis, scalar_t angle) {
138 scalar_t HalfAngle = angle / 2.0;
139 q.s = cos(HalfAngle);
140 q.v = axis * sin(HalfAngle);
145 void Quaternion::rotate(const Quaternion &q) {
146 *this = q * *this * q.conjugate();
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);
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));
159 if(fabs(1.0 - dot) < xsmall_number) return q1; // avoids divisions by zero later on if q1 == q2
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();
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();
176 std::ostream &operator <<(std::ostream &out, const Quaternion &q) {
177 out << "(" << q.s << ", " << q.v << ")";