+++ /dev/null
-
-/*******************************************************************************
-
- Copyright (c) 2009, Charles McGarvey
- All rights reserved.
-
- Redistribution and use in source and binary forms, with or without
- modification, are permitted provided that the following conditions are met:
-
- * Redistributions of source code must retain the above copyright notice,
- this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice,
- this list of conditions and the following disclaimer in the documentation
- and/or other materials provided with the distribution.
-
- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*******************************************************************************/
-
-#ifndef _QUATERNION_HH_
-#define _QUATERNION_HH_
-
-#include <cassert>
-
-#include "math.hh"
-#include "matrix.hh"
-#include "vector.hh"
-
-
-//
-// Quaternion -> 3x3 Matrix
-//
-// | w^2 + x^2 - y^2 - z^2 2xy - 2wz 2xz + 2yw |
-// | 2xy + 2wz w^2 - x^2 + y^2 - z^2 2yz - 2wx |
-// | 2xz - 2wy 2yz - 2wx w^2 - x^2 - y^2 + z^2 |
-//
-
-namespace dc {
-
-
-class quaternion
-{
-public:
- // Constructors.
- quaternion() {}
- quaternion(scalar X, scalar Y, scalar Z, scalar W) {
- vec.x = X; vec.y = Y; vec.z = Z; w = W;
- }
- quaternion(vector3 v, scalar W = 1.0) {
- vec = v; w = W;
- }
- quaternion(scalar q[4]) {
- vec.x = q[0]; vec.y = q[1]; vec.z = q[2]; w = q[3];
- }
-
- // Accessible by index.
- const scalar& operator [] (int i) const {
- assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds.");
- return *((&vec.x) + i);
- }
- scalar& operator [] (int i) {
- assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds.");
- //return vec[i];
- return *((&vec.x) + i);
- }
-
- // Basic maths.
- quaternion operator + (const quaternion& q) const {
- return quaternion(vec + q.vec, w + q.w);
- }
- quaternion operator - (const quaternion& q) const {
- return quaternion(vec - q.vec, w - q.w);
- }
- quaternion operator * (const quaternion& q) const {
- return quaternion(q.w * vec + w * q.vec + q.vec.cross(vec),
- w * q.w - q.vec.dot(vec));
- }
- quaternion operator * (scalar s) const {
- return quaternion(vec * s, w * s);
- }
- quaternion operator / (scalar s) const {
- return quaternion(vec / s, w / s);
- }
-
- quaternion& operator += (const quaternion& q) {
- vec += q.vec; w += q.w;
- return *this;
- }
- quaternion& operator -= (const quaternion& q) {
- vec -= q.vec; w -= q.w;
- return *this;
- }
- quaternion& operator *= (const quaternion& q) {
- scalar W = w;
- w = W * q.w - q.vec.dot(vec);
- vec = q.w * vec + W * q.vec + q.vec.cross(vec);
- return *this;
- }
- quaternion& operator *= (scalar s) {
- vec *= s; w *= s;
- return *this;
- }
- quaternion& operator /= (scalar s) {
- vec /= s; w /= s;
- return *this;
- }
-
- // Unary operators.
- quaternion operator - () const {
- return quaternion(-vec, -w);
- }
-
- // Quaternion operations.
- quaternion conjugate() const {
- return quaternion(-vec, w);
- }
- void normalize(const vector3& v)
- {
- scalar len = length();
- if (len != 0.0) { *this /= len; }
- }
-
- scalar length2() const {
- return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z + w * w;
- }
- scalar length() const {
- return std::sqrt(length2());
- }
-
- // Converting to other types.
- matrix3 matrix() const {
- scalar x2 = vec.x * vec.x;
- scalar y2 = vec.y * vec.y;
- scalar z2 = vec.z * vec.z;
- scalar w2 = w * w;
- scalar xy = vec.x * vec.y;
- scalar xz = vec.x * vec.z;
- scalar yzwx = 2 * (vec.y * vec.z - w * vec.x);
- scalar wy = w * vec.y;
- scalar wz = w * vec.z;
- return matrix3(x2 - y2 - z2 + w2, 2 * (xy - wz), 2 * (xz + wy),
- 2 * (xy + wz), w2 - x2 + y2 - z2, yzwx,
- 2 * (xz - wy), yzwx, w2 - x2 - y2 + z2);
- }
- vector3 axis(scalar& angle) { // Axis of rotation, w/ angle of rotation.
- vector3 axisVec = axis();
- angle = 2 * std::acos(w);
- return axisVec;
- }
- vector3 axis() {
- scalar sa = std::sqrt(1 - w * w);
- return vec / sa;
- }
-
- // Checking equality.
- bool operator == (const quaternion& q) const {
- return vec == q.vec && equals(w,q.w);
- }
- bool operator != (const quaternion& q) const {
- return !(*this == q);
- }
-
- // Data.
- vector3 vec;
- scalar w;
-};
-
-inline quaternion operator * (scalar s, const quaternion& q) {
- return q * s;
-}
-inline quaternion operator / (scalar s, const quaternion& q) {
- return quaternion(s / q.vec, s / q.w);
-}
-
-} // namespace dc
-
-
-#endif // _QUATERNION_HH_
-
projects / chaz / yoink / blobdiff