/// <summary>
/// Creates the matrix need to look at target from position.
/// </summary>
static public Matrix4x4f LookAt(Vector3f position, Vector3f target, Vector3f Up)
{
Vector3f zaxis = (position - target).Normalized;
Vector3f xaxis = Up.Cross(zaxis).Normalized;
Vector3f yaxis = zaxis.Cross(xaxis);
return(new Matrix4x4f(xaxis.x, xaxis.y, xaxis.z, -Vector3f.Dot(xaxis, position),
yaxis.x, yaxis.y, yaxis.z, -Vector3f.Dot(yaxis, position),
zaxis.x, zaxis.y, zaxis.z, -Vector3f.Dot(zaxis, position),
0, 0, 0, 1));
}
/// <summary>
/// A quaternion with the rotation required to
/// rotation from the from direction to the to direction.
/// </summary>
public Quaternion3f(Vector3f to, Vector3f from)
{
Vector3f f = from.Normalized;
Vector3f t = to.Normalized;
float dotProdPlus1 = 1.0f + Vector3f.Dot(f, t);
if (dotProdPlus1 < FMath.EPS)
{
w = 0;
if (Math.Abs(f.x) < 0.6f)
{
float norm = (float)Math.Sqrt(1 - f.x * f.x);
x = 0;
y = f.z / norm;
z = -f.y / norm;
}
else if (Math.Abs(f.y) < 0.6f)
{
float norm = (float)Math.Sqrt(1 - f.y * f.y);
x = -f.z / norm;
y = 0;
z = f.x / norm;
}
else
{
float norm = (float)Math.Sqrt(1 - f.z * f.z);
x = f.y / norm;
y = -f.x / norm;
z = 0;
}
}
else
{
float s = (float)Math.Sqrt(0.5f * dotProdPlus1);
Vector3f tmp = (f.Cross(t)) / (2.0f * s);
x = tmp.x;
y = tmp.y;
z = tmp.z;
w = s;
}
}