public static Matrix4d LookAt(Vector3d eye, Vector3d target, Vector3d up)
{
Vector3d vector3d1 = Vector3d.Normalize(eye - target);
Vector3d right = Vector3d.Normalize(Vector3d.Cross(up, vector3d1));
Vector3d vector3d2 = Vector3d.Normalize(Vector3d.Cross(vector3d1, right));
Matrix4d matrix4d = new Matrix4d(new Vector4d(right.X, vector3d2.X, vector3d1.X, 0.0), new Vector4d(right.Y, vector3d2.Y, vector3d1.Y, 0.0), new Vector4d(right.Z, vector3d2.Z, vector3d1.Z, 0.0), Vector4d.UnitW);
return(Matrix4d.CreateTranslation(-eye) * matrix4d);
}
public static void CreateFromAxisAngle(Vector3d axis, double angle, out Matrix4d result)
{
double num1 = Math.Cos(-angle);
double num2 = Math.Sin(-angle);
double num3 = 1.0 - num1;
axis.Normalize();
result = new Matrix4d(num3 * axis.X * axis.X + num1, num3 * axis.X * axis.Y - num2 * axis.Z, num3 * axis.X * axis.Z + num2 * axis.Y, 0.0, num3 * axis.X * axis.Y + num2 * axis.Z, num3 * axis.Y * axis.Y + num1, num3 * axis.Y * axis.Z - num2 * axis.X, 0.0, num3 * axis.X * axis.Z - num2 * axis.Y, num3 * axis.Y * axis.Z + num2 * axis.X, num3 * axis.Z * axis.Z + num1, 0.0, 0.0, 0.0, 0.0, 1.0);
}
/// <summary>
/// Build a rotation matrix from the specified axis/angle rotation.
/// </summary>
/// <param name="axis">The axis to rotate about.</param>
/// <param name="angle">Angle in radians to rotate counter-clockwise (looking in the direction of the given axis).</param>
/// <param name="result">A matrix instance.</param>
public static void CreateFromAxisAngle(Vector3d axis, double angle, out Matrix4d result)
{
double cos = System.Math.Cos(-angle);
double sin = System.Math.Sin(-angle);
double t = 1.0 - cos;
axis.Normalize();
result = new Matrix4d(t * axis.X * axis.X + cos, t * axis.X * axis.Y - sin * axis.Z, t * axis.X * axis.Z + sin * axis.Y, 0.0,
t * axis.X * axis.Y + sin * axis.Z, t * axis.Y * axis.Y + cos, t * axis.Y * axis.Z - sin * axis.X, 0.0,
t * axis.X * axis.Z - sin * axis.Y, t * axis.Y * axis.Z + sin * axis.X, t * axis.Z * axis.Z + cos, 0.0,
0, 0, 0, 1);
}
/// <summary>
/// Build a world space to camera space matrix
/// </summary>
/// <param name="eye">Eye (camera) position in world space</param>
/// <param name="target">Target position in world space</param>
/// <param name="up">Up vector in world space (should not be parallel to the camera direction, that is target - eye)</param>
/// <returns>A Matrix that transforms world space to camera space</returns>
public static Matrix4d LookAt(Vector3d eye, Vector3d target, Vector3d up)
{
Vector3d z = Vector3d.Normalize(eye - target);
Vector3d x = Vector3d.Normalize(Vector3d.Cross(up, z));
Vector3d y = Vector3d.Normalize(Vector3d.Cross(z, x));
Matrix4d rot = new Matrix4d(new Vector4d(x.X, y.X, z.X, 0.0),
new Vector4d(x.Y, y.Y, z.Y, 0.0),
new Vector4d(x.Z, y.Z, z.Z, 0.0),
Vector4d.UnitW);
Matrix4d trans = Matrix4d.CreateTranslation(-eye);
return(trans * rot);
}
/// <summary>Constructs left Quaterniond from the given axis and angle.</summary>
/// <param name="axis">The axis for the Quaterniond.</param>
/// <param name="angle">The angle for the quaternione.</param>
public Quaterniond(ref Vector3d axis, double angle)
{
double halfAngle = Functions.DTOR * angle / 2;
this.W = System.Math.Cos(halfAngle);
double sin = System.Math.Sin(halfAngle);
Vector3d axisNormalized;
Vector3d.Normalize(ref axis, out axisNormalized);
this.X = axisNormalized.X * sin;
this.Y = axisNormalized.Y * sin;
this.Z = axisNormalized.Z * sin;
}
public static Quaterniond FromAxisAngle(Vector3d axis, double angle)
{
if (axis.LengthSquared == 0.0)
{
return(Quaterniond.Identity);
}
Quaterniond q = Quaterniond.Identity;
angle *= 0.5;
axis.Normalize();
q.Xyz = axis * Math.Sin(angle);
q.W = Math.Cos(angle);
return(Quaterniond.Normalize(q));
}
/// <summary>
/// Build a Quaterniond from the given axis and angle
/// </summary>
/// <param name="axis">The axis to rotate about</param>
/// <param name="angle">The rotation angle in radians</param>
/// <returns></returns>
public static Quaterniond FromAxisAngle(Vector3d axis, double angle)
{
if (axis.LengthSquared == 0.0f)
{
return(Identity);
}
Quaterniond result = Identity;
angle *= 0.5f;
axis.Normalize();
result.Xyz = axis * (double)System.Math.Sin(angle);
result.W = (double)System.Math.Cos(angle);
return(Normalize(result));
}
/// <summary>
/// Build a rotation matrix to rotate about the given axis
/// </summary>
/// <param name="axis">the axis to rotate about</param>
/// <param name="angle">angle in radians to rotate counter-clockwise (looking in the direction of the given axis)</param>
/// <returns>A rotation matrix</returns>
public static Matrix4d Rotate(Vector3d axis, double angle)
{
double cos = System.Math.Cos(-angle);
double sin = System.Math.Sin(-angle);
double t = 1.0 - cos;
axis.Normalize();
Matrix4d result;
result.Row0 = new Vector4d(t * axis.X * axis.X + cos, t * axis.X * axis.Y - sin * axis.Z, t * axis.X * axis.Z + sin * axis.Y, 0.0);
result.Row1 = new Vector4d(t * axis.X * axis.Y + sin * axis.Z, t * axis.Y * axis.Y + cos, t * axis.Y * axis.Z - sin * axis.X, 0.0);
result.Row2 = new Vector4d(t * axis.X * axis.Z - sin * axis.Y, t * axis.Y * axis.Z + sin * axis.X, t * axis.Z * axis.Z + cos, 0.0);
result.Row3 = Vector4d.UnitW;
return(result);
}
public static Matrix4d Rotate(Vector3d axis, double angle)
{
double num1 = Math.Cos(-angle);
double num2 = Math.Sin(-angle);
double num3 = 1.0 - num1;
axis.Normalize();
Matrix4d matrix4d;
matrix4d.Row0 = new Vector4d(num3 * axis.X * axis.X + num1, num3 * axis.X * axis.Y - num2 * axis.Z, num3 * axis.X * axis.Z + num2 * axis.Y, 0.0);
matrix4d.Row1 = new Vector4d(num3 * axis.X * axis.Y + num2 * axis.Z, num3 * axis.Y * axis.Y + num1, num3 * axis.Y * axis.Z - num2 * axis.X, 0.0);
matrix4d.Row2 = new Vector4d(num3 * axis.X * axis.Z - num2 * axis.Y, num3 * axis.Y * axis.Z + num2 * axis.X, num3 * axis.Z * axis.Z + num1, 0.0);
matrix4d.Row3 = Vector4d.UnitW;
return(matrix4d);
}