fahhh

App/util/Quaternion.h

@@ -0,0 +1,112 @@
+/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
+
+#pragma once
+
+#include "Util/G3DCore.h"
+
+namespace RBX {
+
+class Quaternion {
+public:
+	float x, y, z, w;
+
+    Quaternion(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}
+
+	Quaternion(const G3D::Vector3& v, float _w = 0) : x((float)v.x), y((float)v.y), z((float)v.z), w(_w) {}
+
+	Quaternion() : x(0.0f), y(0.0f), z(0.0f), w(1.0f) {}
+
+    Quaternion(const G3D::Matrix3& rot);
+
+    Quaternion& operator= (const Quaternion& other);
+
+	inline const G3D::Vector3& imag() const {
+		return *(reinterpret_cast<const G3D::Vector3*>(this));
+    }
+
+	inline G3D::Vector3& imag() {
+		return *(reinterpret_cast<G3D::Vector3*>(this));
+    }
+
+    void toRotationMatrix(
+		Matrix3& rot) const;
+
+	inline float dot(const Quaternion& other) const {
+	    return (x * other.x) + (y * other.y) + (z * other.z) + (w * other.w);
+	}
+
+	inline float magnitude() const { 
+	    return x*x + y*y + z*z + w*w; 
+	}
+
+	float maxComponent() const {
+		return std::max(	std::max(fabs(x), fabs(y)),  std::max(fabs(z), fabs(w)));
+	}
+
+	// Return the angle of the axis-angle representation of the quaternion
+	inline float getAngle() const {
+		return 2.0f * acos(w);
+	}
+
+	// Return the axis of the axis-angle representation of the quaternion
+	inline Vector3 getAxis() const {
+		float sinSquared = 1.f - w * w;
+		if (sinSquared < 1e-6) //Check for divide by zero
+			return Vector3(1.0, 0.0, 0.0); // Arbitrary
+		float sinRecip = 1.f/ sqrtf(sinSquared);
+		return Vector3(x * sinRecip, y * sinRecip, z * sinRecip);
+	}
+
+	inline Quaternion conjugate() const {
+		return Quaternion(-x, -y, -z, w);
+	}
+
+	inline float& operator[] (int i) const {
+	    return ((float*)this)[i];
+	}
+
+	inline operator float* () {
+		return (float*)this;
+	}
+
+	inline operator const float* () const {
+		return (float*)this;
+	}
+
+	inline Quaternion operator*(const Quaternion& other) const {
+		// Following Watt & Watt, page 360
+		const Vector3& v1 = imag();
+		const Vector3& v2 = other.imag();
+		float         s1 = w;
+		float         s2 = other.w;
+		return Quaternion(s1*v2 + s2*v1 + v1.cross(v2), s1*s2 - v1.dot(v2));
+	}
+
+	inline Quaternion operator+ (const Quaternion& other) const {
+		return Quaternion(x + other.x, y + other.y, z + other.z, w + other.w);
+	}
+
+	inline Quaternion operator- (const Quaternion& other) const {
+	    return Quaternion(x - other.x, y - other.y, z - other.z, w - other.w);
+	}
+
+	inline Quaternion operator* (float s) const {
+		return Quaternion(s*x, s*y, s*z, s*w);
+	}
+
+	// inline
+	Quaternion& operator*=(float fScalar);
+
+	// inline
+	Quaternion& operator+=(const Quaternion& rkQuaternion);
+
+	void normalize() {
+		*this *= 1.0f / sqrtf(magnitude());
+	}
+};
+
+} // namespace
+
+
+#include "Quaternion.inl"
+