/// <summary>
/// Returns spherical direction (phi, theta) from Cartesian direction.
/// </summary>
public static V2d SphericalFromCartesian(this V3d v)
{
return(new V2d(
Fun.Atan2(v.Y, v.X), // phi
Fun.Atan2(v.Z, v.XY.Length) // theta
));
}
/// <summary>
/// Construct ellipse from two conjugate diameters, and set
/// Axis0 to the major axis and Axis1 to the minor axis.
/// The algorithm was constructed from first principles.
/// Also computes the squared lengths of the major and minor
/// half axis.
/// </summary>
public static __et__ FromConjugateDiameters(__vt__ center, /*# if (d == 3) { */ __vt__ normal, /*# } */ __vt__ a, __vt__ b,
out double major2, out double minor2)
{
var ab = __vt__.Dot(a, b);
double a2 = a.LengthSquared, b2 = b.LengthSquared;
if (ab.IsTiny())
{
if (a2 >= b2)
{
major2 = a2; minor2 = b2; return(new __et__(center, /*# if (d == 3) { */ normal, /*# } */ a, b));
}
else
{
major2 = b2; minor2 = a2; return(new __et__(center, /*# if (d == 3) { */ normal, /*# } */ b, a));
}
}
else
{
var t = 0.5 * Fun.Atan2(2 * ab, a2 - b2);
double ct = Fun.Cos(t), st = Fun.Sin(t);
__vt__ v0 = a * ct + b * st, v1 = b * ct - a * st;
a2 = v0.LengthSquared; b2 = v1.LengthSquared;
if (a2 >= b2)
{
major2 = a2; minor2 = b2; return(new __et__(center, /*# if (d == 3) { */ normal, /*# } */ v0, v1));
}
else
{
major2 = b2; minor2 = a2; return(new __et__(center, /*# if (d == 3) { */ normal, /*# } */ v1, v0));
}
}
}
/// <summary>
/// Returns the Euler-Angles from the quatarnion.
/// </summary>
public V3d GetEulerAngles()
{
var test = W * Y - X * Z;
if (test > 0.49999) // singularity at north pole
{
return(new V3d(
-2 * Fun.Atan2(X, W),
Constant.PiHalf,
0));
}
if (test < -0.49999) // singularity at south pole
{
return(new V3d(
2 * Fun.Atan2(X, W),
-Constant.PiHalf,
0));
}
// From Wikipedia, conversion between quaternions and Euler angles.
return(new V3d(
Fun.Atan2(2 * (W * X + Y * Z),
1 - 2 * (X * X + Y * Y)),
Fun.AsinC(2 * test),
Fun.Atan2(2 * (W * Z + X * Y),
1 - 2 * (Y * Y + Z * Z))));
}
/// <summary>
/// Returns the minimal distance between the polygon and the non-
/// overlapping other supplied polygon. The minimal distance is
/// always computed as the distance between a line segment and a
/// point. The indices of the minimal distance configuration are
/// returned in the out parameter, as the indices of points on the
/// two polygons, and wether the line segement was on this or the
/// other polygon. O(n). The returned index of the line segment is
/// the lower point index (except in case of wraparound).
/// </summary>
public static double MinDistanceTo(
this Polygon2d polygon, Polygon2d polygon1,
out int pi0, out int pi1, out bool lineOnThis)
{
var p0a = polygon.m_pointArray;
var p1a = polygon1.m_pointArray;
var p0c = polygon.m_pointCount;
var p1c = polygon1.m_pointCount;
var e0a = polygon.GetEdgeArray();
var e1a = polygon1.GetEdgeArray();
int i0 = p0a.IndexOfMinY(p0c), i1 = p1a.IndexOfMaxY(p1c);
V2d p0 = p0a[i0], e0 = e0a[i0], p1 = p1a[i1], e1 = e1a[i1];
int start0 = i0, start1 = i1;
var dir = V2d.XAxis;
var d = V2d.Distance(p0, p1);
var bestValue = double.MaxValue;
int bpi0 = -1, bpi1 = -1;
var bLineOnThis = true;
do
{
var s0 = Fun.Atan2(e0.Dot90(dir), e0.Dot(dir));
var s1 = Fun.Atan2(e1.Dot270(dir), e1.Dot180(dir));
if (s0 <= s1)
{
dir = e0a[i0];
int i0n = (i0 + 1) % p0c; var p0n = p0a[i0];
var dn = V2d.Distance(p0n, p1);
var dist = DistanceToLine(p1, p0, p0n, d, dn);
if (dist < bestValue)
{
bestValue = dist;
bLineOnThis = true; bpi0 = i0; bpi1 = i1;
}
i0 = i0n; p0 = p0n; e0 = e0a[i0]; d = dn;
}
else
{
dir = e0a[i1].Rot180;
int i1n = (i1 + 1) % p1c; var p1n = p1a[i1];
var dn = V2d.Distance(p0, p1n);
var dist = DistanceToLine(p0, p1, p1n, d, dn);
if (dist < bestValue)
{
bestValue = dist;
bLineOnThis = false; bpi0 = i0; bpi1 = i1;
}
i1 = i1n; p1 = p1n; e1 = e1a[i1]; d = dn;
}
}while (i0 != start0 || i1 != start1);
lineOnThis = bLineOnThis; pi0 = bpi0; pi1 = bpi1;
return(bestValue);
}
/// <summary>
/// Returns the Rodrigues angle-axis vector of the quaternion.
/// </summary>
public __v3t__ ToAngleAxis()
{
var sinTheta2 = V.LengthSquared;
if (sinTheta2 > Constant <__ft__> .PositiveTinyValue)
{
__ft__ sinTheta = Fun.Sqrt(sinTheta2);
__ft__ cosTheta = W;
__ft__ twoTheta = 2 * (cosTheta < 0 ? Fun.Atan2(-sinTheta, -cosTheta)
: Fun.Atan2(sinTheta, cosTheta));
return(V * (twoTheta / sinTheta));
}
else
{
return(__v3t__.Zero);
}
}
/// <summary>
/// Computes the winding number of the polyon.
/// The winding number is positive for counter-
/// clockwise polygons, negative for clockwise polygons.
/// </summary>
public static int ComputeWindingNumber(this Polygon2d polygon)
{
int pc = polygon.PointCount;
V2d e = polygon[0] - polygon[pc - 1];
var a = Fun.Atan2(e.Y, e.X);
var a0 = a;
var radians = 0.0;
for (int pi = 0; pi < pc - 1; pi++)
{
V2d e1 = polygon[pi + 1] - polygon[pi];
var a1 = Fun.Atan2(e1.Y, e1.X);
var alpha = a1 - a0;
if (alpha >= Constant.Pi)
{
alpha -= Constant.PiTimesTwo;
}
else if (alpha < -Constant.Pi)
{
alpha += Constant.PiTimesTwo;
}
radians += alpha;
a0 = a1;
}
var alpha0 = a - a0;
if (alpha0 >= Constant.Pi)
{
alpha0 -= Constant.PiTimesTwo;
}
else if (alpha0 < -Constant.Pi)
{
alpha0 += Constant.PiTimesTwo;
}
radians += alpha0;
var winding = radians >= 0.0
? (int)((Constant.Pi + radians) / Constant.PiTimesTwo)
: -(int)((Constant.Pi - radians) / Constant.PiTimesTwo);
return(winding);
}
/// <summary>
/// Returns spherical direction (phi) from Cartesian direction.
/// </summary>
public static double SphericalFromCartesian(this V2d v)
{
return(Fun.Atan2(v.Y, v.X));
}