/// <summary>
///
/// </summary>
private static bool LineLineClosestPoints(Vector3d u, Vector3d v, Vector3d w, out double tu, out double tv)
{
// impl ref
// http://geomalgorithms.com/a07-_distance.html
return(SolveSymmetric(u.SquareLength, -Vector3d.Dot(u, v), v.SquareLength, -Vector3d.Dot(w, u), Vector3d.Dot(w, v), out tu, out tv));
}
/// <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(Vector3d vector, Vector3d xAxis, Vector3d yAxis)
{
return(Math.Atan2(Vector3d.Dot(vector, yAxis), Vector3d.Dot(vector, xAxis)));
}
/// <summary>
/// Returns the barycentric coordinates for the given point with respect to triangle a, b, c
/// </summary>
/// <param name="point"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <param name="result"></param>
/// <returns></returns>
public static bool GetBarycentric(Vector3d point, Vector3d a, Vector3d b, Vector3d c, out Vector3d result)
{
// impl ref
// http://realtimecollisiondetection.net/
var u = b - a;
var v = c - a;
var w = point - a;
if (SolveSymmetric(u.SquareLength, Vector3d.Dot(u, v), v.SquareLength, Vector3d.Dot(w, u), Vector3d.Dot(w, v), out result.Y, out result.Z))
{
result.X = 1.0 - result.Y - result.Z;
return(true);
}
// no unique solution
result = Vector3d.Zero;
return(false);
}
/// <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 Vector3d ProjectTo(Vector3d point, Vector3d direction)
{
return(point - direction * (DistanceTo(point) / Vector3d.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(Vector3d point)
{
return(Vector3d.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(Vector3d point)
{
_distance = Vector3d.Dot(point, _normal);
}
/// <summary>
/// Applies this rotation to the given vector.
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public Vector3d Apply(Vector3d vector)
{
return(_cosAngle * vector + _sinAngle * Vector3d.Cross(_axis, vector) + Vector3d.Dot(_axis, vector) * (1.0 - _cosAngle) * _axis);
}
/// <summary>
/// Applies the inverse of this rotation to the given vector.
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public Vector3d ApplyInverse(Vector3d vector)
{
return(new Vector3d(Vector3d.Dot(vector, _x), Vector3d.Dot(vector, _y), Vector3d.Dot(vector, _z)));
}
