#include "stdafx.h"
#include "Rotation.h"
#include "Core/StrBuf.h"

namespace storm {
	namespace geometry {

		Rotation::Rotation(Vector axis, Angle angle) {
			axis = axis.normalized();
			angle = angle * 0.5f;

			Float s = sin(angle);
			x = axis.x * s;
			y = axis.y * s;
			z = axis.z * s;
			w = cos(angle);
		}

		Float Rotation::lengthSq() const {
			return x*x + y*y + z*z + w*w;
		}

		Float Rotation::length() const {
			return sqrt(lengthSq());
		}

		Rotation Rotation::normalized() const {
			Float len = length();
			if (len == 0) {
				return (*this);
			} else {
				return (*this) / len;
			}
		}

		Vector Rotation::axis() const {
			return Vector(x, y, z).normalized();
		}

		Angle Rotation::angle() const {
			return acos(w) * 2.0f;
		}

		void Rotation::toS(StrBuf *to) const {
			*to << S("{ axis: ") << axis() << S(", angle: ") << angle() << S(" }");
		}

		Rotation operator +(Rotation a, Rotation b) {
			// TODO: Does this make sense?
			return Rotation(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
		}

		Rotation operator *(Rotation a, Float b) {
			// TODO: Does this make sense?
			return Rotation(a.x * b, a.y * b, a.z * b, a.w * b);
		}

		Rotation operator /(Rotation a, Float b) {
			// TODO: Does this make sense?
			return a * (1.0f / b);
		}

		Rotation operator *(Rotation a, Rotation b) {
			return Rotation(
				a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y,
				a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x,
				a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w,
				a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z
				);
		}

		Rotation &operator +=(Rotation &a, Rotation b) {
			a.x += b.x;
			a.y += b.y;
			a.z += b.z;
			a.w += b.w;
			return a;
		}

		Rotation &operator *=(Rotation &a, Float b) {
			a.x *= b;
			a.y *= b;
			a.z *= b;
			a.w *= b;
			return a;
		}

		Rotation &operator /=(Rotation &a, Float b) {
			return a *= (1.0f / b);
		}

		Vector operator *(Vector o, Rotation r) {
			// Same code as in Transform::Transform:
			Float x = r.x, y = r.y, z = r.z, w = r.w;
			Float tx = 2*x, ty = 2*y, tz = 2*z; //, tw = 2*w;
			Float txx = tx*x, txy = tx*y, txz = tx*z, txw = tx*w;
			Float tyy = ty*y, tyz = ty*z, tyw = ty*w;
			Float tzz = tz*z, tzw = tz*w;
			// Float tww = tw*w;

			return Vector(
				o.x * (1 - (tyy + tzz)) + o.y * (txy + tzw) + o.z * (txz - tyw),
				o.x * (txy - tzw) + o.y * (1 - (txx + tzz)) + o.z * (tyz + txw),
				o.x * (txz + tyw) + o.y * (tyz - txw) + o.z * (1 - (txx + tyy))
				);
		}

		Point operator *(Point o, Rotation r) {
			// Same code as in Transform::Transform:
			Float x = r.x, y = r.y, z = r.z, w = r.w;
			Float tx = 2*x, ty = 2*y, tz = 2*z;
			Float txx = tx*x, txy = tx*y;
			Float tyy = ty*y;
			Float tzz = tz*z, tzw = tz*w;

			return Point(
				o.x * (1 - (tyy + tzz)) + o.y * (txy + tzw),
				o.x * (txy - tzw) + o.y * (1 - (txx + tzz))
				);
		}

	}
}
