// Copy constructor (had to do this because we cannot overload operator "=" in C#
public Quaternion(Quaternion other)
{
m_x = other.X;
m_y = other.Y;
m_z = other.Z;
m_w = other.W;
}
//! Interpolates the Quaternion between to Quaternions based on time
public Quaternion Slerp(Quaternion q1, Quaternion q2, float time)
{
float angle = q1.GetDotProduct(q2);
if (angle < 0.0f)
{
q1 *= -1.0f;
angle *= -1.0f;
}
float scale;
float invscale;
if ((angle + 1.0f) > 0.05f)
{
if ((1.0f - angle) >= 0.05f) // spherical interpolation
{
float theta = (float)Math.Acos(angle);
float invsintheta = 1.0f / (float)Math.Sin(theta);
scale = (float)Math.Sin(theta * (1.0f - time)) * invsintheta;
invscale = (float)Math.Sin(theta * time) * invsintheta;
}
else // linear interploation
{
scale = 1.0f - time;
invscale = time;
}
}
else
{
q2 = new Quaternion(-q1.Y, q1.X, -q1.W, q1.Z);
scale = (float)Math.Sin(Math.PI * (0.5f - time));
invscale = (float)Math.Sin(Math.PI * time);
}
Quaternion t_tmp = (q1 * scale) + (q2 * invscale);
this.m_x = t_tmp.X;
this.m_y = t_tmp.Y;
this.m_z = t_tmp.Z;
this.m_w = t_tmp.W;
return this;
}
//! sets new Quaternion based on euler angles
public static Quaternion FromEulerAngles(float x, float y, float z)
{
Quaternion t_tmp = new Quaternion();
//TODO Duplicated code (Method Set(x,y,z))
double angle;
angle = x * 0.5;
double sr = (float)Math.Sin(angle);
double cr = (float)Math.Cos(angle);
angle = y * 0.5;
double sp = (float)Math.Sin(angle);
double cp = (float)Math.Cos(angle);
angle = z * 0.5;
double sy = (float)Math.Sin(angle);
double cy = (float)Math.Cos(angle);
double cpcy = cp * cy;
double spcy = sp * cy;
double cpsy = cp * sy;
double spsy = sp * sy;
t_tmp.X = (float)(sr * cpcy - cr * spsy);
t_tmp.Y = (float)(cr * spcy + sr * cpsy);
t_tmp.Z = (float)(cr * cpsy - sr * spcy);
t_tmp.W = (float)(cr * cpcy + sr * spsy);
t_tmp.Normalize();
return t_tmp;
}
//! calculates the dot product
public float GetDotProduct(Quaternion q2)
{
return (m_x * q2.X) + (m_y * q2.Y) + (m_z * q2.Z) + (m_w * q2.W);
}
//! multiplication by a quaternion operator
public static Quaternion operator *(Quaternion lhs, Quaternion rhs)
{
Quaternion tmp = new Quaternion();
tmp.W = (rhs.W * lhs.W) - (rhs.X * lhs.X) - (rhs.Y * lhs.Y) - (rhs.Z * lhs.Z);
tmp.X = (rhs.W * lhs.X) + (rhs.X * lhs.W) + (rhs.Y * lhs.Z) - (rhs.Z * lhs.Y);
tmp.Y = (rhs.W * lhs.Y) + (rhs.Y * lhs.W) + (rhs.Z * lhs.X) - (rhs.X * lhs.Z);
tmp.Z = (rhs.W * lhs.Z) + (rhs.Z * lhs.W) + (rhs.X * lhs.Y) - (rhs.Y * lhs.X);
return tmp;
}