Simple game engine with complete export to scripting language
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 

238 lines
6.7 KiB

#ifndef BLUECORE_QUATERNION_H
#define BLUECORE_QUATERNION_H
#include "Vector.h"
namespace BlueCore
{
template<class T> class QuaternionTemplate
{
public:
T w, x, y, z;
inline QuaternionTemplate()
{
w = static_cast<T>(1.0);
x = static_cast<T>(0.0);
y = static_cast<T>(0.0);
z = static_cast<T>(0.0);
}
template<class S> inline QuaternionTemplate(S w, S x, S y, S z)
{
this->w = static_cast<T>(w);
this->x = static_cast<T>(x);
this->y = static_cast<T>(y);
this->z = static_cast<T>(z);
}
template<class S> inline QuaternionTemplate(Vector3Template<S> axis, S angle)
{
T half_angle = static_cast<T>(angle)/static_cast<T>(2.0);
T sin_half_angle = static_cast<T>(sin(half_angle) );
w = static_cast<T>(cos(half_angle) );
x = sin_half_angle * static_cast<T>(axis.x);
y = sin_half_angle * static_cast<T>(axis.y);
z = sin_half_angle * static_cast<T>(axis.z);
}
template<class S> inline QuaternionTemplate(S h, S a, S b)
{
T c1 = static_cast<T>(cos(h / 2.0) );
T c2 = static_cast<T>(cos(a / 2.0) );
T c3 = static_cast<T>(cos(b / 2.0) );
T s1 = static_cast<T>(sin(h / 2.0) );
T s2 = static_cast<T>(sin(a / 2.0) );
T s3 = static_cast<T>(sin(b / 2.0) );
w = c1 * c2 * c3 - s1 * s2 * s3;
x = s1 * s2 * c3 + c1 * c2 * s3;
y = s1 * c2 * c3 + c1 * s2 * s3;
z = c1 * s2 * c3 - s1 * c2 * s3;
}
inline void identity()
{
w = static_cast<T>(1.0);
x = static_cast<T>(0.0);
y = static_cast<T>(0.0);
z = static_cast<T>(0.0);
}
template<class S> inline QuaternionTemplate<T> operator +(
const QuaternionTemplate<S> &a) const
{
return QuaternionTemplate<T>(w + static_cast<T>(a.w), x
+ static_cast<T>(a.x), y + static_cast<T>(a.y), z
+ static_cast<T>(a.z) );
}
template<class S> inline QuaternionTemplate<T> &operator +=(
const QuaternionTemplate<S> &a)
{
w += static_cast<T>(a.w);
x += static_cast<T>(a.x);
y += static_cast<T>(a.y);
z += static_cast<T>(a.z);
return *this;
}
template<class S> inline QuaternionTemplate<T> operator -(
const QuaternionTemplate<S> &a) const
{
return QuaternionTemplate<T>(w - static_cast<T>(a.w), x
- static_cast<T>(a.x), y - static_cast<T>(a.y), z
- static_cast<T>(a.z) );
}
template<class S> inline QuaternionTemplate<T> operator *(
const QuaternionTemplate<S> &a) const
{
return QuaternionTemplate<T>(w * static_cast<T>(a.w) - x
* static_cast<T>(a.x) - y * static_cast<T>(a.y) - z
* static_cast<T>(a.z), w * static_cast<T>(a.x) + x
* static_cast<T>(a.w) + y * static_cast<T>(a.z) - z
* static_cast<T>(a.y), w * static_cast<T>(a.y) - x
* static_cast<T>(a.z) + y * static_cast<T>(a.w) + z
* static_cast<T>(a.x), w * static_cast<T>(a.z) + x
* static_cast<T>(a.y) - y * static_cast<T>(a.x) + z
* static_cast<T>(a.w) );
}
template<class S> inline QuaternionTemplate<T> &operator *=(
const QuaternionTemplate<S> &a)
{
w = w * static_cast<T>(a.w) - x * static_cast<T>(a.x) - y
* static_cast<T>(a.y) - z * static_cast<T>(a.z);
x = w * static_cast<T>(a.x) + x * static_cast<T>(a.w) + y
* static_cast<T>(a.z) - z * static_cast<T>(a.y);
y = w * static_cast<T>(a.y) - x * static_cast<T>(a.z) + y
* static_cast<T>(a.w) + z * static_cast<T>(a.x);
z = w * static_cast<T>(a.z) + x * static_cast<T>(a.y) - y
* static_cast<T>(a.x) + z * static_cast<T>(a.w);
return *this;
}
template<class S> inline QuaternionTemplate<T> &operator -=(
const QuaternionTemplate<S> &a)
{
w -= static_cast<T>(a.w);
x -= static_cast<T>(a.x);
y -= static_cast<T>(a.y);
z -= static_cast<T>(a.z);
return *this;
}
template<class S> inline QuaternionTemplate<T> &operator =(
const QuaternionTemplate<S> &a)
{
w = static_cast<T>(a.w);
x = static_cast<T>(a.x);
y = static_cast<T>(a.y);
z = static_cast<T>(a.z);
return *this;
}
inline QuaternionTemplate<T> unit() const
{
T d = 1/sqrt(w*w + x*x + y*y + z*z);
return QuaternionTemplate<T>(w * d, x * d, y * d, z * d);
}
inline QuaternionTemplate<T> inversed() const
{
return QuaternionTemplate<T>(w, -x, -y, -z);
}
inline void inverse()
{
x = -x;
y = -y;
z = -z;
}
inline QuaternionTemplate<T> &normalize()
{
T d = 1/sqrt(w*w + x*x + y*y + z*z);
w *= d;
x *= d;
y *= d;
z *= d;
return *this;
}
template<class S> inline const Vector3Template<T> apply(
const Vector3Template<S> &a) const
{
T xx = x*x, xy = x*y, xz = x*z, xw = x*w, yy = y*y, yz = y*z, yw = y*w,
zz = z*z, zw = z*w;
return Vector3Template<T>( 2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy
- zz ) + static_cast<T>(a.y) * (xy - zw ) + static_cast<T>(a.z)
* (xz + yw ) ),
2.0 * ( static_cast<T>(a.x) * (xy + zw ) + static_cast<T>(a.y) * ( 0.5
- xx - zz ) + static_cast<T>(a.z) * (yz - xw ) ),
2.0 * ( static_cast<T>(a.x) * (xz - yw ) + static_cast<T>(a.y) * (yz
+ xw ) + static_cast<T>(a.z) * ( 0.5 - xx - yy ) ));
}
template<class S> inline const Vector3Template<T> operator *(
const Vector3Template<S> &a) const
{
T xx = x*x, xy = x*y, xz = x*z, xw = x*w, yy = y*y, yz = y*z, yw = y*w,
zz = z*z, zw = z*w;
return Vector3Template<T>( 2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy
- zz ) + static_cast<T>(a.y) * (xy - zw ) + static_cast<T>(a.z)
* (xz + yw ) ),
2.0 * ( static_cast<T>(a.x) * (xy + zw ) + static_cast<T>(a.y) * ( 0.5
- xx - zz ) + static_cast<T>(a.z) * (yz - xw ) ),
2.0 * ( static_cast<T>(a.x) * (xz - yw ) + static_cast<T>(a.y) * (yz
+ xw ) + static_cast<T>(a.z) * ( 0.5 - xx - yy ) ));
}
template<class S> inline Vector3Template<T> applyInversed(
const Vector3Template<S> &a) const
{
T xx = x*x, xy = x*y, xz = x*z, xw = -x*w, yy = y*y, yz = y*z, yw = -y
*w, zz = z*z, zw = -z*w;
return Vector3Template<T>( 2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy
- zz ) + static_cast<T>(a.y) * (xy - zw ) + static_cast<T>(a.z)
* (xz + yw ) ),
2.0 * ( static_cast<T>(a.x) * (xy + zw ) + static_cast<T>(a.y) * ( 0.5
- xx - zz ) + static_cast<T>(a.z) * (yz - xw ) ),
2.0 * ( static_cast<T>(a.x) * (xz - yw ) + static_cast<T>(a.y) * (yz
+ xw ) + static_cast<T>(a.z) * ( 0.5 - xx - yy ) ));
}
inline QuaternionTemplate<T> slerp(const QuaternionTemplate<T> &q,
const Scalar &t)
{
T phi = acos(w*q.w + x*q.x + y*q.y + z*q.z);
T s = 1 / sin(phi);
T a = sin(phi*(1-t)) * s;
T b = sin(phi*t) * s;
return QuaternionTemplate<T>(a*w+b*q.w, a*x+b*q.x, a*y+b*q.y, a*z+b*q.z);
}
};
typedef QuaternionTemplate<float> QuaternionFloat;
typedef QuaternionTemplate<double> QuaternionDouble;
typedef QuaternionTemplate<Scalar> Quaternion;
}
#endif