public Quaternion Slerp( Quaternion inquat, float u )
{
float theta = this.Dot( inquat );
if( theta < 0.0f )
{
inquat.x = -inquat.x;
inquat.y = -inquat.y;
inquat.z = -inquat.z;
inquat.w = -inquat.w;
theta = -theta;
}
else if( theta >= 1.0f )
{
return inquat;
}
float alp = ( float )System.Math.Acos( theta );
if( alp < 0.001f )
{
return inquat;
}
float salp = ( float )System.Math.Sin(alp);
float invsalp = 1.0f / salp;
float c0 = ( float )System.Math.Sin((1.0f - u) * alp) * invsalp;
float c1 = ( float )System.Math.Sin(u * alp) * invsalp;
return new Quaternion(
(x * c0) + (inquat.x * c1),
(y * c0) + (inquat.y * c1),
(z * c0) + (inquat.z * c1),
(w * c0) + (inquat.w * c1) );
}
public float4x4( Quaternion q )
{
m = new float[4,4];
float ww = q.w * q.w, xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
float s = 2.0f / (ww + xx + yy + zz);
float xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z, wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
m[0,0] = 1.0f - s * (yy + zz);
m[1,0] = s * (xy - wz);
m[2,0] = s * (xz + wy);
m[3,0] = 0.0f;
m[0,1] = s * (xy + wz);
m[1,1] = 1.0f - s * (xx + zz);
m[2,1] = s * (yz - wx);
m[3,1] = 0.0f;
m[0,2] = s * (xz - wy);
m[1,2] = s * (yz + wx);
m[2,2] = 1.0f - s * (xx + yy);
m[3,2] = 0.0f;
m[0,3] = 0.0f;
m[1,3] = 0.0f;
m[2,3] = 0.0f;
m[3,3] = 1.0f;
}
public float Dot( Quaternion B )
{
return x * x + y * y + z * z + w * B.w;
}