/// <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);
}
