--- /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