/// <summary> /// /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <param name="t"></param> /// <returns></returns> public static Vec4d Lerp(Vec4d v0, Vec4d v1, double t) { v0.X += (v1.X - v0.X) * t; v0.Y += (v1.Y - v0.Y) * t; v0.Z += (v1.Z - v0.Z) * t; v0.W += (v1.W - v0.W) * t; return(v0); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="tolerance"></param> /// <returns></returns> public bool ApproxEquals(Vec4d other, Vec4d tolerance) { return (Math.Abs(other.X - X) < tolerance.X && Math.Abs(other.Y - Y) < tolerance.Y && Math.Abs(other.Z - Z) < tolerance.Z && Math.Abs(other.W - W) < tolerance.W); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="tolerance"></param> /// <returns></returns> public bool ApproxEquals(Vec4d other, Vec4d tolerance) { return (SlurMath.ApproxEquals(X, other.X, tolerance.X) && SlurMath.ApproxEquals(Y, other.Y, tolerance.Y) && SlurMath.ApproxEquals(Z, other.Z, tolerance.Z) && SlurMath.ApproxEquals(W, other.W, tolerance.W)); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <returns></returns> public double SquareDistanceTo(Vec4d other) { other.X -= X; other.Y -= Y; other.Z -= Z; other.W -= W; return(other.SquareLength); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="tolerance"></param> /// <returns></returns> public bool ApproxEquals(Vec4d other, double tolerance = SlurMath.ZeroTolerance) { return (SlurMath.ApproxEquals(X, other.X, tolerance) && SlurMath.ApproxEquals(Y, other.Y, tolerance) && SlurMath.ApproxEquals(Z, other.Z, tolerance) && SlurMath.ApproxEquals(W, other.W, tolerance)); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <returns></returns> public double ManhattanDistanceTo(Vec4d other) { other.X -= X; other.Y -= Y; other.Z -= Z; other.W -= W; return(other.ManhattanLength); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="tolerance"></param> /// <returns></returns> public bool ApproxEquals(Vec4d other, double tolerance = SlurMath.ZeroTolerance) { return (Math.Abs(other.X - X) < tolerance && Math.Abs(other.Y - Y) < tolerance && Math.Abs(other.Z - Z) < tolerance && Math.Abs(other.W - W) < tolerance); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="factor"></param> /// <returns></returns> public Vec4d LerpTo(Vec4d other, double factor) { return(new Vec4d( X + (other.X - X) * factor, Y + (other.Y - Y) * factor, Z + (other.Z - Z) * factor, W + (other.W - W) * factor)); }
/// <summary> /// Matrix vector multiplication /// </summary> /// <returns></returns> public static Vec4d Multiply(ref Matrix4d matrix, Vec4d vector) { return(new Vec4d( Vec4d.Dot(vector, matrix.Row0), Vec4d.Dot(vector, matrix.Row1), Vec4d.Dot(vector, matrix.Row2), Vec4d.Dot(vector, matrix.Row3) )); }
/// <summary> /// Matrix vector multiplication /// </summary> /// <returns></returns> public static Vec4d operator *(Matrix4d matrix, Vec4d vector) { return new Vec4d( Vec4d.Dot(vector, matrix.Row0), Vec4d.Dot(vector, matrix.Row1), Vec4d.Dot(vector, matrix.Row2), Vec4d.Dot(vector, matrix.Row3) ); }
/// <summary> /// Applies this transformation to the given vector. /// </summary> /// <param name="vector"></param> /// <returns></returns> public Vec4d Apply(Vec4d vector) { return(new Vec4d( Vec4d.Dot(Row0, vector), Vec4d.Dot(Row1, vector), Vec4d.Dot(Row2, vector), Vec4d.Dot(Row3, vector) )); }
/* * /// <summary> * /// Matrix vector multiplication * /// </summary> * /// <returns></returns> * public static Vec4d Multiply(ref Matrix4d matrix, Vec4d vector) * { * return matrix.Apply(vector); * } * * * /// <summary> * /// Matrix multiplication * /// </summary> * /// <returns></returns> * public static Matrix4d Multiply(ref Matrix4d m0, ref Matrix4d m1) * { * return m0.Apply(m1); * } */ /// <summary> /// /// </summary> /// <param name="row0"></param> /// <param name="row1"></param> /// <returns></returns> public static Matrix4d CreateFromRows(Vec4d row0, Vec4d row1, Vec4d row2, Vec4d row3) { return(new Matrix4d( row0.X, row0.Y, row0.Z, row0.W, row1.X, row1.Y, row1.Z, row1.W, row2.X, row2.Y, row2.Z, row2.W, row3.X, row3.Y, row3.Z, row3.W )); }
/// <summary> /// /// </summary> /// <param name="row0"></param> /// <param name="row1"></param> /// <returns></returns> public static Matrix4d CreateFromColumns(Vec4d column0, Vec4d column1, Vec4d column2, Vec4d column3) { return(new Matrix4d( column0.X, column1.X, column2.X, column3.X, column0.Y, column1.Y, column2.Y, column3.Y, column0.Z, column1.Z, column2.Z, column3.Z, column0.W, column1.W, column2.W, column3.W )); }
/// <summary> /// Returns the angle between two vectors. /// If either vector is zero length, Double.NaN is returned. /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <returns></returns> public static double Angle(Vec4d v0, Vec4d v1) { double d = v0.SquareLength * v1.SquareLength; if (d > 0.0) { return(Math.Acos(SlurMath.Clamp(Dot(v0, v1) / Math.Sqrt(d), -1.0, 1.0))); // clamp dot product to remove noise } return(double.NaN); }
/// <summary> /// Returns a numerical approximation of the Jacobian of the given function with respect to the given vector. /// </summary> /// <param name="function"></param> /// <param name="vector"></param> /// <param name="epsilon"></param> /// <returns></returns> public static Matrix4d CreateJacobian(Func <Vec4d, Vec4d> function, Vec4d vector, double epsilon = SlurMath.ZeroTolerance) { (var x, var y, var z, var w) = vector; var col0 = function(new Vec4d(x + epsilon, y, z, w)) - function(new Vec4d(x - epsilon, y, z, w)); var col1 = function(new Vec4d(x, y + epsilon, z, w)) - function(new Vec4d(x, y - epsilon, z, w)); var col2 = function(new Vec4d(x, y, z + epsilon, w)) - function(new Vec4d(x, y, z - epsilon, w)); var col3 = function(new Vec4d(x, y, z, w + epsilon)) - function(new Vec4d(x, y, z, w - epsilon)); return(new Matrix4d(col0, col1, col2, col3) / (2.0 * epsilon)); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="factor"></param> /// <returns></returns> public Vec4d SlerpTo(Vec4d other, double angle, double factor) { double sa = Math.Sin(angle); // handle aligned cases if (sa > 0.0) { var saInv = 1.0 / sa; var af = angle * factor; return(this * Math.Sin(angle - af) * saInv + other * Math.Sin(af) * saInv); } return(this); }
/// <summary> /// /// </summary> /// <param name="column0"></param> /// <param name="column1"></param> /// <param name="column2"></param> /// <param name="column3"></param> public Matrix4d(Vec4d column0, Vec4d column1, Vec4d column2, Vec4d column3) { M00 = column0.X; M01 = column1.X; M02 = column2.X; M03 = column3.X; M10 = column0.Y; M11 = column1.Y; M12 = column2.Y; M13 = column3.Y; M20 = column0.Z; M21 = column1.Z; M22 = column2.Z; M23 = column3.Z; M30 = column0.W; M31 = column1.W; M32 = column2.W; M33 = column3.W; }
/// <summary> /// Linear interpolation between this quaternion and another. /// </summary> /// <param name="other"></param> /// <param name="factor"></param> /// <returns></returns> public Quaterniond LerpTo(Quaterniond other, double factor) { var ca = Vec4d.Dot(this, other); if (ca < 0.0) { return(new Quaterniond( X + (-other.X - X) * factor, Y + (-other.Y - Y) * factor, Z + (-other.Z - Z) * factor, W + (-other.W - W) * factor )); } else { return(new Quaterniond( X + (other.X - X) * factor, Y + (other.Y - Y) * factor, Z + (other.Z - Z) * factor, W + (other.W - W) * factor )); } }
/// <summary> /// /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <param name="angle"></param> /// <param name="factor"></param> /// <returns></returns> public static Vec4d Slerp(Vec4d v0, Vec4d v1, double angle, double factor) { return(v0.SlerpTo(v1, angle, factor)); }
/// <summary> /// /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <param name="factor"></param> /// <returns></returns> public static Vec4d Slerp(Vec4d v0, Vec4d v1, double factor) { return(v0.SlerpTo(v1, Angle(v0, v1), factor)); }
/// <summary> /// /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <param name="factor"></param> /// <returns></returns> public static Vec4d Lerp(Vec4d v0, Vec4d v1, double factor) { return(v0.LerpTo(v1, factor)); }
/// <summary> /// Returns a vector parallel to v0 whose projection onto v2 equals the projection of v1 onto v2 /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <param name="v2"></param> /// <returns></returns> public static Vec4d MatchProjection(Vec4d v0, Vec4d v1, Vec4d v2) { return(Dot(v1, v2) / Dot(v0, v2) * v0); }
/// <summary> /// Returns a vector parallel to v0 whos projection onto v1 equals v1 /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <returns></returns> public static Vec4d MatchProjection(Vec4d v0, Vec4d v1) { return(v1.SquareLength / Dot(v0, v1) * v0); }
/// <summary> /// Returns the reflection of v0 about v1. /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <returns></returns> public static Vec4d Reflect(Vec4d v0, Vec4d v1) { //return Project(v0, v1) * 2.0 - v0; return(v1 * (Dot(v0, v1) / v1.SquareLength * 2.0) - v0); }
/// <summary> /// Returns the rejection of v0 onto v1. /// This is the perpendicular component of v0 with respect to v1. /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <returns></returns> public static Vec4d Reject(Vec4d v0, Vec4d v1) { return(v0 - Project(v0, v1)); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="factor"></param> /// <returns></returns> public Vec4d SlerpTo(Vec4d other, double factor) { return(SlerpTo(other, Angle(this, other), factor)); }
/// <summary> /// /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <returns></returns> public static double AbsDot(Vec4d v0, Vec4d v1) { return(Math.Abs(v0.X * v1.X) + Math.Abs(v0.Y * v1.Y) + Math.Abs(v0.Z * v1.Z) + Math.Abs(v0.W * v1.W)); }
/// <summary> /// /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <returns></returns> public static double Dot(Vec4d v0, Vec4d v1) { return(v0.X * v1.X + v0.Y * v1.Y + v0.Z * v1.Z + v0.W * v1.W); }
/// <summary> /// /// </summary> /// <param name="other"></param> /// <param name="factor"></param> /// <returns></returns> public Vec4d SlerpTo(Vec4d other, double angle, double factor) { double st = 1.0 / Math.Sin(angle); return(this * (Math.Sin((1.0 - factor) * angle) * st) + other * (Math.Sin(factor * angle) * st)); }
/// <summary> /// Returns the projection of v0 onto v1. /// </summary> /// <param name="v0"></param> /// <param name="v1"></param> /// <returns></returns> public static Vec4d Project(Vec4d v0, Vec4d v1) { return(Dot(v0, v1) / v1.SquareLength * v1); }