/// <summary>
/// http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static Quaterniond CreateFromTo(Vec3d from, Vec3d to)
{
// TODO
// test implementation
// https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
if (!from.Unitize() || !to.Unitize())
{
return(Identity);
}
var ct = Vec3d.Dot(from, to);
// parallel check
if (1.0 - Math.Abs(ct) < SlurMath.ZeroTolerance)
{
//opposite check
if (ct < 0.0)
{
var perp = from.X < 1.0 ? from.CrossX() : from.CrossY();
var t = 1.0 / perp.Length;
return(new Quaterniond(perp.X * t, perp.Y * t, perp.Z * t, 0.0));
}
return(Identity);
}
// can assume axis is valid
var axis = Vec3d.Cross(from, to);
var q = new Quaterniond(axis.X, axis.Y, axis.Z, ct + 1);
q.Unitize();
return(q);
}
/// <summary>
///
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static Quaterniond CreateFromTo(Vec3d from, Vec3d to)
{
// impl refs
// https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
// http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
if (!from.Unitize() || !to.Unitize())
{
return(Identity);
}
var ca = Vec3d.Dot(from, to);
// parallel check
if (SlurMath.ApproxEquals(Math.Abs(ca), 1.0))
{
//opposite check
if (ca < 0.0)
{
var perp = from.X < 1.0 ? from.CrossX() : from.CrossY();
var t = 1.0 / perp.Length;
return(new Quaterniond(perp.X * t, perp.Y * t, perp.Z * t, 0.0));
}
return(Identity);
}
// can assume axis is valid
var axis = Vec3d.Cross(from, to);
var q = new Quaterniond(axis.X, axis.Y, axis.Z, ca + 1);
q.Unitize();
return(q);
}
/// <summary>
/// Returns the area gradient of the given trianglue with respect to p0.
/// https://www.cs.cmu.edu/~kmcrane/Projects/DDG/paper.pdf p 64
/// </summary>
/// <param name="p0"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public static Vec3d GetTriAreaGradient(Vec3d p0, Vec3d p1, Vec3d p2)
{
var d = p2 - p1;
var n = Vec3d.Cross(d, p0 - p1);
n.Unitize();
return(Vec3d.Cross(n, d) * 0.5);
}
/// <summary>
///
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="rightNormal"></param>
/// <param name="leftNormal"></param>
/// <returns></returns>
public static double GetDihedralAngle(Vec3d unitAxis, Vec3d leftNormal, Vec3d rightNormal)
{
// impl ref
// http://brickisland.net/DDGFall2017/2017/10/12/assignment-1-coding-investigating-curvature/
return
(Math.Atan2(
Vec3d.Dot(unitAxis, Vec3d.Cross(leftNormal, rightNormal)),
Vec3d.Dot(leftNormal, rightNormal)
) + Math.PI);
}
/// <summary>
/// Returns true if the given vectors form a valid orthonormal basis i.e. they aren't parallel or zero length.
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <returns></returns>
public bool Set(Vec3d x, Vec3d y)
{
Vec3d z = Vec3d.Cross(x, y);
double m = z.SquareLength;
if (m > 0.0)
{
SetZX(z / Math.Sqrt(m), x / x.Length);
return(true);
}
return(false);
}
/// <summary>
///
/// </summary>
/// <param name="p0"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public bool Set(Vec3d p0, Vec3d p1, Vec3d p2)
{
var n = Vec3d.Cross(p1 - p0, p2 - p1);
if (!n.Unitize())
{
return(false);
}
_normal = n;
_distance = Vec3d.Dot(p0, n);
return(true);
}
/// <summary>
/// Returns the area gradient of the given triangle with respect to each vertex
/// https://www.cs.cmu.edu/~kmcrane/Projects/DDG/paper.pdf p 64
/// </summary>
/// <param name="p0"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="g0"></param>
/// <param name="g1"></param>
/// <param name="g2"></param>
/// <returns></returns>
public static void GetTriAreaGradients(Vec3d p0, Vec3d p1, Vec3d p2, out Vec3d g0, out Vec3d g1, out Vec3d g2)
{
var d0 = p1 - p0;
var d1 = p2 - p1;
var d2 = p0 - p2;
var n = Vec3d.Cross(d0, d1);
n.Unitize();
g0 = Vec3d.Cross(n, d1) * 0.5;
g1 = Vec3d.Cross(n, d2) * 0.5;
g2 = g0 + g1;
}
/// <summary>
/// Orthonormalizes the 2 given vectors and returns a third mutually perpendicular unit vector.
/// Returns false if the 2 given vectors are parallel.
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
public static bool OrthoNormalize(ref Vec3d x, ref Vec3d y, out Vec3d z)
{
z = Vec3d.Cross(x, y);
double m = z.SquareLength;
if (m > 0.0)
{
x /= x.Length;
z /= Math.Sqrt(m);
y = Vec3d.Cross(z, x);
return(true);
}
return(false);
}
/// <summary>
/// Creates the rotation between v0 and v1
/// https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static OrthoBasis3d CreateFromTo(Vec3d from, Vec3d to)
{
if (!from.Unitize() || !to.Unitize())
{
return(Identity);
}
var v = Vec3d.Cross(from, to);
var c = Vec3d.Dot(from, to);
var k = 1.0 / (1.0 + c);
return(new OrthoBasis3d()
{
_x = new Vec3d(v.X * v.X * k + c, v.X * v.Y * k + v.Z, v.X * v.Z * k - v.Y),
_y = new Vec3d(v.Y * v.X * k - v.Z, v.Y * v.Y * k + c, v.Y * v.Z * k + v.X),
_z = new Vec3d(v.Z * v.X * k + v.Y, v.Z * v.Y * k - v.X, v.Z * v.Z * k + c)
});
}
/// <summary>
/// Creates the rotation between v0 and v1
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static OrthoBasis3d CreateFromTo(Vec3d from, Vec3d to)
{
// impl ref
// https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
if (!from.Unitize() || !to.Unitize())
{
return(Identity);
}
var v = Vec3d.Cross(from, to);
var c = Vec3d.Dot(from, to);
var k = 1.0 / (1.0 + c);
return(new OrthoBasis3d {
_x = new Vec3d(v.X * v.X * k + c, v.X * v.Y * k + v.Z, v.X * v.Z * k - v.Y),
_y = new Vec3d(v.Y * v.X * k - v.Z, v.Y * v.Y * k + c, v.Y * v.Z * k + v.X),
_z = new Vec3d(v.Z * v.X * k + v.Y, v.Z * v.Y * k - v.X, v.Z * v.Z * k + c)
});
}
/// <summary>
///
/// </summary>
/// <param name="points"></param>
/// <returns></returns>
public static double GetPolygonArea(IEnumerable <Vec3d> points, Vec3d unitNormal)
{
var itr = points.GetEnumerator();
itr.MoveNext();
var first = itr.Current;
var p0 = first;
var sum = Vec3d.Zero;
while (itr.MoveNext())
{
var p1 = itr.Current;
sum += Vec3d.Cross(p0, p1);
p0 = p1;
}
sum += Vec3d.Cross(p0, first);
return(Vec3d.Dot(sum, unitNormal) * 0.5);
}
/// <summary>
///
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static AxisAngle3d CreateFromTo(Vec3d from, Vec3d to)
{
if (!from.Unitize() || !to.Unitize())
{
return(Identity);
}
var ct = Vec3d.Dot(from, to);
// parallel check
if (1.0 - Math.Abs(ct) < SlurMath.ZeroTolerance)
{
// opposite check
if (ct < 0.0)
{
var perp = from.X < 1.0 ? from.CrossX() : from.CrossY();
return(new AxisAngle3d()
{
_axis = perp / perp.Length,
_angle = Math.PI,
_cosAngle = -1.0,
_sinAngle = 0.0
});
}
return(Identity);
}
// can assume axis is valid
var axis = Vec3d.Cross(from, to);
var st = axis.Length;
return(new AxisAngle3d()
{
_axis = axis / st,
_angle = Math.Acos(ct),
_sinAngle = st,
_cosAngle = ct
});
}
/// <summary>
/// Returns the aspect ratio of the tetrahedra defined by 4 given points.
/// This is defined as the longest edge / shortest altitude.
/// </summary>
/// <param name="points"></param>
/// <returns></returns>
public static double GetTetraAspect(Vec3d[] points)
{
double minEdge = 0.0;
double maxAlt = double.PositiveInfinity;
for (int i = 0; i < 4; i++)
{
Vec3d p0 = points[i];
Vec3d p1 = points[(i + 1) & 3];
Vec3d p2 = points[(i + 2) & 3];
Vec3d p3 = points[(i + 3) & 3];
Vec3d v0 = p1 - p0;
Vec3d v1 = p2 - p1;
Vec3d v2 = p3 - p2;
minEdge = Math.Max(minEdge, v0.SquareLength);
maxAlt = Math.Min(maxAlt, Vec3d.Project(v2, Vec3d.Cross(v0, v1)).SquareLength);
}
return(Math.Sqrt(minEdge) / Math.Sqrt(maxAlt));
}
/// <summary>
/// Returns the aspect ratio of the tetrahedra defined by 4 given points.
/// This is defined as the longest edge / shortest altitude.
/// </summary>
/// <param name="points"></param>
/// <returns></returns>
public static double GetTetraAspect(Vec3d p0, Vec3d p1, Vec3d p2, Vec3d p3)
{
double minEdge = 0.0;
double maxAlt = double.PositiveInfinity;
Vec3d v0 = p1 - p0;
Vec3d v1 = p2 - p1;
Vec3d v2 = p3 - p2;
Vec3d v3 = p0 - p3;
Sub(v0, v1, v2);
Sub(v1, v2, v3);
Sub(v2, v3, v0);
Sub(v3, v0, v1);
return(Math.Sqrt(minEdge) / Math.Sqrt(maxAlt));
void Sub(Vec3d a, Vec3d b, Vec3d c)
{
minEdge = Math.Max(minEdge, a.SquareLength);
maxAlt = Math.Min(maxAlt, Vec3d.Project(c, Vec3d.Cross(a, b)).SquareLength);
}
}
/// <summary>
/// Assumes the given vectors are orthonormal.
/// </summary>
/// <param name="y"></param>
/// <param name="z"></param>
private void SetYZ(Vec3d y, Vec3d z)
{
_x = Vec3d.Cross(y, z);
_y = y;
_z = z;
}
/// <summary>
/// Assumes the given vectors are orthonormal.
/// </summary>
/// <param name="z"></param>
/// <param name="x"></param>
internal void SetZX(Vec3d z, Vec3d x)
{
_x = x;
_y = Vec3d.Cross(z, x);
_z = z;
}
/// <summary>
/// Assumes the given vectors are orthonormal.
/// </summary>
/// <param name="y"></param>
/// <param name="z"></param>
internal void SetYZ(Vec3d y, Vec3d z)
{
_x = Vec3d.Cross(y, z);
_y = y;
_z = z;
}
/// <summary>
/// Assumes the given vectors are orthonormal.
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
internal void SetXY(Vec3d x, Vec3d y)
{
_x = x;
_y = y;
_z = Vec3d.Cross(x, y);
}
/// <summary>
///
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="right"></param>
/// <param name="left"></param>
/// <returns></returns>
public static double GetDihedralAngle(Vec3d start, Vec3d end, Vec3d left, Vec3d right)
{
var d = end - start;
return(GetDihedralAngle(d.Unit, Vec3d.Cross(d, left - end), Vec3d.Cross(d, start - right)));
}
/// <summary>
///
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public Vec3d RotateInverse(Vec3d vector)
{
return(vector * _cosAngle - Vec3d.Cross(_axis, vector) * _sinAngle + _axis * Vec3d.Dot(_axis, vector) * (1.0 - _cosAngle));
}
/// <summary>
/// Assumes the given vectors are orthonormal.
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
private void SetXY(Vec3d x, Vec3d y)
{
_x = x;
_y = y;
_z = Vec3d.Cross(x, y);
}
/// <summary>
/// http://www.block.arch.ethz.ch/brg/files/2013-ijss-vanmele-shaping-tension-structures-with-actively-bent-linear-elements_1386929572.pdf
/// </summary>
/// <param name="v0"></param>
/// <param name="v1"></param>
/// <returns></returns>
public static Vec3d GetCurvatureVector(Vec3d v0, Vec3d v1)
{
Vec3d v2 = Vec3d.Cross(v0, v1);
return(Vec3d.Cross((v0.SquareLength * v1 - v1.SquareLength * v0), v2) / (2.0 * v2.SquareLength));
}
/// <summary>
/// Assumes the given vectors are orthonormal.
/// </summary>
/// <param name="z"></param>
/// <param name="x"></param>
private void SetZX(Vec3d z, Vec3d x)
{
_x = x;
_y = Vec3d.Cross(z, x);
_z = z;
}
/// <summary>
/// Applies this rotation to the given vector.
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public Vec3d Apply(Vec3d vector)
{
return(_cosAngle * vector + _sinAngle * Vec3d.Cross(_axis, vector) + Vec3d.Dot(_axis, vector) * (1.0 - _cosAngle) * _axis);
}