1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/quat.h	Sat Apr 12 23:37:55 2014 +0300
     1.3 @@ -0,0 +1,108 @@
     1.4 +#ifndef QUATERNION_H_
     1.5 +#define	QUATERNION_H_
     1.6 +
     1.7 +class Quat;
     1.8 +
     1.9 +inline Quat operator *(const Quat &a, const Quat &b);
    1.10 +
    1.11 +class Quat{
    1.12 +public:
    1.13 +	float x, y, z, w;
    1.14 +
    1.15 +	Quat() : x(0), y(0), z(0), w(1) {}
    1.16 +	Quat(float xx, float yy, float zz, float ww) : x(xx), y(yy), z(zz), w(ww) {}
    1.17 +
    1.18 +	void set_identity()
    1.19 +	{
    1.20 +		x = y = z = 0.0f;
    1.21 +		w = 1.0f;
    1.22 +	}
    1.23 +
    1.24 +	Quat conjugate() const
    1.25 +	{
    1.26 +		return Quat(-x, -y, -z, w);
    1.27 +	}
    1.28 +
    1.29 +	float length() const
    1.30 +	{
    1.31 +		return (float)sqrt(x * x + y * y + z * z + w * w);
    1.32 +	}
    1.33 +
    1.34 +	float length_sq() const
    1.35 +	{
    1.36 +		return x * x + y * y + z * z + w * w;
    1.37 +	}
    1.38 +
    1.39 +	void normalize()
    1.40 +	{
    1.41 +		float len = length();
    1.42 +		if(len != 0.0) {
    1.43 +			x /= len;
    1.44 +			y /= len;
    1.45 +			z /= len;
    1.46 +			w /= len;
    1.47 +		}
    1.48 +	}
    1.49 +
    1.50 +	Quat inverse() const
    1.51 +	{
    1.52 +		Quat inv = conjugate();
    1.53 +		float len_sq = length_sq();
    1.54 +		if(len_sq != 0.0) {
    1.55 +			inv.x /= len_sq;
    1.56 +			inv.y /= len_sq;
    1.57 +			inv.z /= len_sq;
    1.58 +			inv.w /= len_sq;
    1.59 +		}
    1.60 +		return inv;
    1.61 +	}
    1.62 +
    1.63 +	void set_rotation(float angle, float axis_x, float axis_y, float axis_z)
    1.64 +	{
    1.65 +		float half_angle = angle * 0.5;
    1.66 +		float sin_half = sin(half_angle);
    1.67 +
    1.68 +		w = cos(half_angle);
    1.69 +		x = axis_x * sin_half;
    1.70 +		y = axis_y * sin_half;
    1.71 +		z = axis_z * sin_half;
    1.72 +	}
    1.73 +
    1.74 +	void rotate(float angle, float x, float y, float z)
    1.75 +	{
    1.76 +		Quat q;
    1.77 +		q.set_rotation(angle, x, y, z);
    1.78 +		*this = *this * q;
    1.79 +	}
    1.80 +
    1.81 +	void rotate(const Quat &q)
    1.82 +	{
    1.83 +		*this = q * *this * q.conjugate();
    1.84 +	}
    1.85 +
    1.86 +	Matrix4x4 get_matrix() const
    1.87 +	{
    1.88 +		return Matrix4x4(
    1.89 +			1.0 - 2.0 * y*y - 2.0 * z*z,	2.0 * x * y - 2.0 * w * z,		2.0 * z * x + 2.0 * w * y, 0,
    1.90 +			2.0 * x * y + 2.0 * w * z,		1.0 - 2.0 * x*x - 2.0 * z*z,	2.0 * y * z - 2.0 * w * x, 0,
    1.91 +			2.0 * z * x - 2.0 * w * y,		2.0 * y * z + 2.0 * w * x,		1.0 - 2.0 * x*x - 2.0 * y*y, 0,
    1.92 +			0, 0, 0, 1);
    1.93 +	}
    1.94 +};
    1.95 +
    1.96 +inline Quat operator *(const Quat &a, const Quat &b)
    1.97 +{
    1.98 +	float dot = a.x * b.x + a.y * b.y + a.z * b.z;
    1.99 +	float cross_x = a.y * b.z - a.z * b.y;
   1.100 +	float cross_y = a.z * b.x - a.x * b.z;
   1.101 +	float cross_z = a.x * b.y - a.y * b.x;
   1.102 +
   1.103 +	float w = a.w * b.w - dot;
   1.104 +	float x = a.x * b.w + b.x * a.w + cross_x;
   1.105 +	float y = a.y * b.w + b.y * a.w + cross_y;
   1.106 +	float z = a.z * b.w + b.z * a.w + cross_z;
   1.107 +
   1.108 +	return Quat(x, y, z, w);
   1.109 +}
   1.110 +
   1.111 +#endif	// QUATERNION_H_