2 This file is part of XRay, a photorealistic 3D rendering library.
3 Copyright (C) 2005 John Tsiombikas
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.
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.
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
21 * @file quaternion.cxx
22 * @author John Tsiombikas
28 #include "quaternion.h"
30 Quaternion::Quaternion() {
32 v.x = v.y = v.z = 0.0;
35 Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z) {
42 Quaternion::Quaternion(scalar_t s, const Vector3 &v) {
47 Quaternion::Quaternion(const Vector3 &axis, scalar_t angle) {
48 set_rotation(axis, angle);
51 Quaternion Quaternion::operator +(const Quaternion &quat) const {
52 return Quaternion(s + quat.s, v + quat.v);
55 Quaternion Quaternion::operator -(const Quaternion &quat) const {
56 return Quaternion(s - quat.s, v - quat.v);
59 Quaternion Quaternion::operator -() const {
60 return Quaternion(-s, -v);
63 /** Quaternion Multiplication:
64 * Q1*Q2 = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1(x)v2]
66 Quaternion Quaternion::operator *(const Quaternion &quat) const {
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);
73 void Quaternion::operator +=(const Quaternion &quat) {
74 *this = Quaternion(s + quat.s, v + quat.v);
77 void Quaternion::operator -=(const Quaternion &quat) {
78 *this = Quaternion(s - quat.s, v - quat.v);
81 void Quaternion::operator *=(const Quaternion &quat) {
85 void Quaternion::reset_identity() {
87 v.x = v.y = v.z = 0.0;
90 Quaternion Quaternion::conjugate() const {
91 return Quaternion(s, -v);
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);
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;
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);
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);
121 /** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
122 Quaternion Quaternion::inverse() const {
123 Quaternion inv = conjugate();
124 scalar_t lensq = length_sq();
132 void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle) {
133 scalar_t HalfAngle = angle / 2.0;
135 v = axis * sin(HalfAngle);
138 void Quaternion::rotate(const Vector3 &axis, scalar_t angle) {
140 scalar_t HalfAngle = angle / 2.0;
141 q.s = cos(HalfAngle);
142 q.v = axis * sin(HalfAngle);
147 void Quaternion::rotate(const Quaternion &q) {
148 *this = q * *this * q.conjugate();
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);
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));
162 if(fabs(1.0 - dot) < xsmall_number) return q1; // avoids divisions by zero later on if q1 == q2
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();
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();
179 std::ostream &operator <<(std::ostream &out, const Quaternion &q) {
180 out << "(" << q.s << ", " << q.v << ")";