/// <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>
///
/// </summary>
private static void LineLineClosestPoints(Vec3d u, Vec3d v, Vec3d w, out double tu, out double tv)
{
double uu = u.SquareLength;
double uv = Vec3d.Dot(u, v);
double vv = v.SquareLength;
double uw = Vec3d.Dot(u, w);
double vw = Vec3d.Dot(v, w);
double denom = 1.0 / (uu * vv - uv * uv);
tu = (uv * vw - vv * uw) * denom;
tv = (uu * vw - uv * uw) * denom;
}
/// <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>
/// 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>
/// Applies the inverse of this rotation to the given vector.
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public Vec3d ApplyInverse(Vec3d vector)
{
return(new Vec3d(Vec3d.Dot(vector, _x), Vec3d.Dot(vector, _y), Vec3d.Dot(vector, _z)));
}
/// <summary>
/// Assumes the given axes are orthonormal.
/// </summary>
/// <param name="vector"></param>
/// <param name="xAxis"></param>
/// <param name="yAxis"></param>
/// <returns></returns>
public static double GetAngleInPlane(Vec3d vector, Vec3d xAxis, Vec3d yAxis)
{
double t = Math.Atan2(Vec3d.Dot(vector, xAxis), Vec3d.Dot(vector, yAxis));
return((t < 0.0) ? t + SlurMath.TwoPI : t); // shift discontinuity to 0
}
/// <summary>
/// Returns the projection of the given point along the given direction onto this plane.
/// </summary>
/// <param name="point"></param>
/// <param name="direction"></param>
/// <returns></returns>
public Vec3d ProjectTo(Vec3d point, Vec3d direction)
{
return(point - direction * (DistanceTo(point) / Vec3d.Dot(direction, _normal)));
}
/// <summary>
/// Returns the signed distance from this plane to the given point.
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public double DistanceTo(Vec3d point)
{
return(Vec3d.Dot(point, _normal) - _distance);
}
/// <summary>
/// Sets the distance of this plane such that it passes through the given point.
/// </summary>
/// <param name="point"></param>
public void MakePassThrough(Vec3d point)
{
_distance = Vec3d.Dot(point, _normal);
}
/// <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>
/// Returns the angle of the vector in the given basis.
/// Assumes the given axes are orthonormal.
/// </summary>
/// <param name="vector"></param>
/// <param name="xAxis"></param>
/// <param name="yAxis"></param>
/// <returns></returns>
public static double GetPolarAngle(Vec3d vector, Vec3d xAxis, Vec3d yAxis)
{
return(Math.Atan2(Vec3d.Dot(vector, yAxis), Vec3d.Dot(vector, xAxis)));
}
/// <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);
}