/// <summary>
/// L2 norm of matrix column elemnts including start and excluding end.
/// </summary>
private static double ColNorm2(
this double[] matrix, long m0, long mx, long my, long col, long start, long end)
{
m0 += col * mx;
var norm = 0.0;
for (long mi = m0 + start * my, me = m0 + end * my; mi != me; mi += my)
{
norm += matrix[mi] * matrix[mi];
}
return(Fun.Sqrt(norm));
}
/// <summary>
/// L2 norm of matrix row elemnts including start and excluding end.
/// </summary>
private static double RowNorm2(
this double[] matrix, long m0, long mx, long my, long row, long start, long end)
{
m0 += row * my;
var norm = 0.0;
for (long mi = m0 + start * mx, me = m0 + end * mx; mi != me; mi += mx)
{
norm += matrix[mi] * matrix[mi];
}
return(Fun.Sqrt(norm));
}
/// <summary>
/// Creates a quaternion from a rotation matrix
/// </summary>
/// <param name="m"></param>
/// <param name="epsilon"></param>
public static __rot3t__ From__m33t__(__m33t__ m, __ft__ epsilon = (__ft__)1e-6)
{
if (!m.IsOrthonormal(epsilon))
{
throw new ArgumentException("Matrix is not orthonormal.");
}
var t = 1 + m.M00 + m.M11 + m.M22;
if (t > epsilon)
{
__ft__ s = t.Sqrt() * 2;
__ft__ x = (m.M21 - m.M12) / s;
__ft__ y = (m.M02 - m.M20) / s;
__ft__ z = (m.M10 - m.M01) / s;
__ft__ w = s / 4;
return(new __rot3t__(w, x, y, z).Normalized);
}
else
{
if (m.M00 > m.M11 && m.M00 > m.M22)
{
__ft__ s = Fun.Sqrt(1 + m.M00 - m.M11 - m.M22) * 2;
__ft__ x = s / 4;
__ft__ y = (m.M01 + m.M10) / s;
__ft__ z = (m.M02 + m.M20) / s;
__ft__ w = (m.M21 - m.M12) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
else if (m.M11 > m.M22)
{
__ft__ s = Fun.Sqrt(1 + m.M11 - m.M00 - m.M22) * 2;
__ft__ x = (m.M01 + m.M10) / s;
__ft__ y = s / 4;
__ft__ z = (m.M12 + m.M21) / s;
__ft__ w = (m.M20 - m.M02) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
else
{
__ft__ s = Fun.Sqrt(1 + m.M22 - m.M00 - m.M11) * 2;
__ft__ x = (m.M20 + m.M02) / s;
__ft__ y = (m.M12 + m.M21) / s;
__ft__ z = s / 4;
__ft__ w = (m.M01 - m.M10) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
}
}
public static List <int[]> ComputeNonConcaveSubPolygons(
this Polygon3d polygon, double absoluteEpsilon)
{
V3d normal = polygon.ComputeDoubleAreaNormal();
double len2 = normal.LengthSquared;
if (len2 < absoluteEpsilon * absoluteEpsilon)
{
return(new int[polygon.PointCount].SetByIndex(i => i).IntoList());
}
M44d.NormalFrame(V3d.Zero, normal * (1.0 / Fun.Sqrt(len2)), out M44d local2global, out M44d global2local);
var polygon2d = polygon.ToPolygon2d(p => global2local.TransformPos(p).XY);
return(polygon2d.ComputeNonConcaveSubPolygons(absoluteEpsilon));
}
/// <summary>
/// Create from Rodrigues axis-angle vactor
/// </summary>
public static __rot3t__ FromAngleAxis(__v3t__ angleAxis)
{
__ft__ theta2 = angleAxis.LengthSquared;
if (theta2 > Constant <__ft__> .PositiveTinyValue)
{
var theta = Fun.Sqrt(theta2);
var thetaHalf = theta / 2;
var k = Fun.Sin(thetaHalf) / theta;
return(new __rot3t__(Fun.Cos(thetaHalf), k * angleAxis));
}
else
{
return(new __rot3t__(1, 0, 0, 0));
}
}
/// <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);
}
}
public static bool IsTrueFor(long value)
{
if (value < 0)
{
return(false);
}
if (value < isPrimeArray.Length)
{
return(isPrimeArray[value]);
}
int root = (int)Fun.Sqrt(value);
int directMaxPrime = Fun.Min(primeArray[primeCount - 1], root);
int pi;
for (pi = 0; primeArray[pi] <= directMaxPrime; pi++)
{
if (value % primeArray[pi] == 0)
{
return(false);
}
}
if (directMaxPrime == root)
{
return(true);
}
while (true)
{
int p = Prime.WithIndex(pi);
if (p > root)
{
break;
}
if (value % p == 0)
{
return(false);
}
pi++;
}
return(true);
}
/// <summary>
/// Return real roots of: x^4 + c3 x^3 + c2 x^2 + c1 x + c0 = 0.
/// Double and triple solutions are returned as replicated values.
/// Imaginary and non existing solutions are returned as NaNs.
/// </summary>
public static Quad <double> RealRootsOfNormed(
double c3, double c2, double c1, double c0)
{
// eliminate cubic term (x = y - c3/4): x^4 + p x^2 + q x + r = 0
double e = c3 * c3;
double p = -3 / 8.0 * e + c2;
double q = (1 / 8.0 * e - 1 / 2.0 * c2) * c3 + c1;
double r = (1 / 16.0 * c2 - 3 / 256.0 * e) * e - 1 / 4.0 * c3 * c1 + c0;
if (Fun.IsTiny(r)) // ---- no absolute term: y (y^3 + p y + q) = 0
{
return(MergeSortedAndShift(
RealRootsOfDepressed(p, q), 0.0, -1 / 4.0 * c3));
}
// ----------------------- take one root of the resolvent cubic...
double z = OneRealRootOfNormed(
-1 / 2.0 * p, -r, 1 / 2.0 * r * p - 1 / 8.0 * q * q);
// --------------------------- ...to build two quadratic equations
double u = z * z - r;
double v = 2.0 * z - p;
if (u < Constant <double> .PositiveTinyValue) // +tiny instead of 0
{
u = 0.0; // improves unique
}
else // root accuracy by a
{
u = Fun.Sqrt(u); // factor of 10^5!
}
if (v < Constant <double> .PositiveTinyValue) // values greater than
{
v = 0.0; // +tiny == 4 * eps
}
else // do not seem to
{
v = Fun.Sqrt(v); // improve accuraccy!
}
double q1 = q < 0 ? -v : v;
return(MergeSortedAndShift(RealRootsOfNormed(q1, z - u),
RealRootsOfNormed(-q1, z + u),
-1 / 4.0 * c3));
}
/// <summary>
/// One real root of the equation: x^3 + c2 x^2 + c1 x + c0 = 0.
/// </summary>
public static double OneRealRootOfNormed(
double c2, double c1, double c0
)
{
// ------ eliminate quadric term (x = y - c2/3): x^3 + p x + q = 0
double d = c2 * c2;
double p3 = 1 / 3.0 * /* p */ (-1 / 3.0 * d + c1);
double q2 = 1 / 2.0 * /* q */ ((2 / 27.0 * d - 1 / 3.0 * c1) * c2 + c0);
double p3c = p3 * p3 * p3;
d = q2 * q2 + p3c;
if (d < 0) // -------------- casus irreducibilis: three real roots
{
return(2 * Fun.Sqrt(-p3) * Fun.Cos(1 / 3.0
* Fun.Acos(-q2 / Fun.Sqrt(-p3c))) - 1 / 3.0 * c2);
}
d = Fun.Sqrt(d); // one triple root or a single and a double root
return(Fun.Cbrt(d - q2) - Fun.Cbrt(d + q2) - 1 / 3.0 * c2);
}
/// <summary>
/// Return real roots of the equation x^2 + px + q = 0.
/// Double roots are returned as a pair of identical values.
/// If no real roots exists, then both entries are NaN.
/// </summary>
public static Pair <double> RealRootsOfNormed(double p, double q)
{
double p2 = p / 2.0;
double d = p2 * p2 - q;
if (d < 0)
{
return(Pair.Create(double.NaN, double.NaN));
}
if (p2 > 0.0) // prevent cancellation
{
double r = -(p2 + Fun.Sqrt(d));
return(Pair.Create(r, q / r));
}
else
{
double r = Fun.Sqrt(d) - p2;
return(Pair.Create(q / r, r));
}
}
public void ReportValue(int verbosity, string name)
{
using (Report.Job(verbosity, name))
{
if ((m_options & StatsOptions.Count) != 0)
{
Report.Value(verbosity, "count", Count);
}
if ((m_options & StatsOptions.Sum) != 0)
{
Report.Value(verbosity, "sum", Sum);
}
if ((m_options & StatsOptions.Min) != 0)
{
Report.Value(verbosity, "minimum", Min);
}
if ((m_options & StatsOptions.Max) != 0)
{
Report.Value(verbosity, "maximum", Max);
}
if ((m_options & StatsOptions.Mean) != 0)
{
Report.Value(verbosity, "mean", Mean);
}
if ((m_options & StatsOptions.NeedsSumOfSquares) != 0)
{
double variance = Variance;
if ((m_options & StatsOptions.Variance) != 0)
{
Report.Value(verbosity, "variance", variance);
}
if ((m_options & StatsOptions.Variance) != 0)
{
Report.Value(verbosity, "standard deviation", Fun.Sqrt(variance));
}
}
}
}
/// <summary>
/// Return real roots of the equation: x^3 + p x + q = 0
/// Double and triple solutions are returned as replicated values.
/// Imaginary and non existing solutions are returned as NaNs.
/// </summary>
public static Triple <double> RealRootsOfDepressed(
double p, double q)
{
double p3 = 1 / 3.0 * p, q2 = 1 / 2.0 * q;
double p3c = p3 * p3 * p3, d = q2 * q2 + p3c;
if (d < 0) // ---------- casus irreducibilis: three real solutions
{
double phi = 1 / 3.0 * Fun.Acos(-q2 / Fun.Sqrt(-p3c));
double t = 2 * Fun.Sqrt(-p3);
double r0 = t * Fun.Cos(phi);
double r1 = -t *Fun.Cos(phi + Constant.Pi / 3.0);
double r2 = -t *Fun.Cos(phi - Constant.Pi / 3.0);
return(Triple.CreateAscending(r0, r1, r2));
}
d = Fun.Sqrt(d); // one triple root or a single and a double root
double s0 = Fun.Cbrt(d - q2) - Fun.Cbrt(d + q2);
double s1 = -0.5 * s0;
return(s0 < s1?Triple.Create(s0, s1, s1)
: Triple.Create(s1, s1, s0));
}
/// <summary>
/// Returns the Euclidean (or 2-) distance between two matrices.
/// </summary>
public static __ctype__ Distance2(__nmtype__ a, __nmtype__ b)
{
return /*# if (ctype != "double") {*/ ((__ctype__)/*# } */ Fun.Sqrt(/*# n.ForEach(i => { m.ForEach(j => { */
Fun.Square(b.M__i____j__ - a.M__i____j__) /*# }, add); }, add); */));
}
public static float Dist2(this Vector <float> v0, Vector <float> v1) => Fun.Sqrt(v0.Dist2Squared(v1));
public static double Dist2(this Vector <double> v0, Vector <double> v1) => Fun.Sqrt(v0.Dist2Squared(v1));
public static __ft__ Dist2(this Vector <__ft__> v0, Vector <__ft__> v1) => Fun.Sqrt(v0.Dist2Squared(v1));
public bool HitsCylinder(Cylinder3d cylinder,
double tmin, double tmax,
ref RayHit3d hit)
{
var axisDir = cylinder.Axis.Direction.Normalized;
// Vector Cyl.P0 -> Ray.Origin
var op = Origin - cylinder.P0;
// normal RayDirection - CylinderAxis
var normal = Direction.Cross(axisDir);
var unitNormal = normal.Normalized;
// normal (Vec Cyl.P0 -> Ray.Origin) - CylinderAxis
var normal2 = op.Cross(axisDir);
var t = -normal2.Dot(unitNormal) / normal.Length;
var radius = cylinder.Radius;
if (cylinder.DistanceScale != 0)
{ // cylinder gets bigger, the further away it is
var pnt = GetPointOnRay(t);
var dis = V3d.Distance(pnt, this.Origin);
radius = ((cylinder.Radius / cylinder.DistanceScale) * dis) * 2;
}
// between enitre rays (caps are ignored)
var shortestDistance = Fun.Abs(op.Dot(unitNormal));
if (shortestDistance <= radius)
{
var s = Fun.Abs(Fun.Sqrt(radius.Square() - shortestDistance.Square()) / Direction.Length);
var t1 = t - s; // first hit of Cylinder shell
var t2 = t + s; // second hit of Cylinder shell
if (t1 > tmin && t1 < tmax)
{
tmin = t1;
}
if (t2 < tmax && t2 > tmin)
{
tmax = t2;
}
hit.T = t1;
hit.Point = GetPointOnRay(t1);
// check if found point is outside of Cylinder Caps
var bottomPlane = new Plane3d(cylinder.Circle0.Normal, cylinder.Circle0.Center);
var topPlane = new Plane3d(cylinder.Circle1.Normal, cylinder.Circle1.Center);
var heightBottom = bottomPlane.Height(hit.Point);
var heightTop = topPlane.Height(hit.Point);
// t1 lies outside of caps => find closest cap hit
if (heightBottom > 0 || heightTop > 0)
{
hit.T = tmax;
// intersect with bottom Cylinder Cap
var bottomHit = HitsPlane(bottomPlane, tmin, tmax, ref hit);
// intersect with top Cylinder Cap
var topHit = HitsPlane(topPlane, tmin, tmax, ref hit);
// hit still close enough to cylinder axis?
var distance = cylinder.Axis.Ray3d.GetMinimalDistanceTo(hit.Point);
if (distance <= radius && (bottomHit || topHit))
{
return(true);
}
}
else
{
return(true);
}
}
hit.T = tmax;
hit.Point = V3d.NaN;
return(false);
}