public static V2d EvalD3(double t, V2d p0, V2d p1, V2d p2, V2d p3)
{
var m0 = (p2 - p0) * 0.5;
var m1 = (p3 - p1) * 0.5;
return(CubicHermite.EvalD3(t, p1, p2, m0, m1));
}
public static V2d EvalD3(double t,
V2d p0, V2d p1, V2d p2, V2d p3, double tension, double bias)
{
var tangents = Tangents(p0, p1, p2, p3, tension, bias);
return(CubicHermite.EvalD3(t, p1, p2, tangents.E0, tangents.E1));
}
/// <summary>
/// Compute the first of four possible hashcodes for hashing in a 2-D
/// unit grid. Add items with this function, retrieve with function
/// HashCode.Get4.
/// </summary>
public static int Get1of4(V2d point)
{
var xi = (long)Floor(point.X);
var yi = (long)Floor(point.Y);
return(Combine((int)(xi >> 1), (int)(yi >> 1)));
}
/// <summary>
/// Compute the Delaunay triangulation of the supplied x-sorted point
/// array and return a triangle index array with counter-clockwise
/// triangles.
/// </summary>
public static int[] Triangulate(this V2d[] xSortedPoints)
{
int vc = xSortedPoints.Length;
V2d[] pxy = new V2d[vc + 4].SetByIndexLong(0, vc, i => xSortedPoints[i]);
return(Triangulate2d(pxy));
}
public static bool IsParallelTo(this V2d u, V2d v, double epsilon = 1e-6)
{
var un = u.Normalized;
var vn = v.Normalized;
return((un - vn).Length < epsilon || (un + vn).Length < epsilon);
}
/// <summary>
/// Calculates the nomalized plane of this <see cref="Plane2d"/>.
/// </summary>
public void Normalize()
{
double scale = Normal.Length;
Normal /= scale;
Distance /= scale;
}
public RayHit3d(double tMax)
{
T = tMax;
Point = V3d.NaN;
Coord = V2d.NaN;
BackSide = false;
Part = 0;
}
public static V2d Multiply(Rot2d rot, V2d vec)
{
double a = (double)System.Math.Cos(rot.Angle);
double b = (double)System.Math.Sin(rot.Angle);
return(new V2d(a * vec.X + b * vec.Y,
-b * vec.X + a * vec.Y));
}
/// <summary>
/// Gets the t for a point p on this ray.
/// </summary>
public double GetT(V2d p)
{
var v = p - Origin;
return((Direction.X.Abs() > Direction.Y.Abs())
? (v.X / Direction.X)
: (v.Y / Direction.Y));
}
/// <summary>
/// Position (xy) in normalized device coordinates.
/// </summary>
public Ndc2d(PixelPosition p)
{
var np = p.NormalizedPosition;
Position = new V2d(
-1.0 + np.X * 2.0,
+1.0 - np.Y * 2.0
);
}
/// <summary>
/// Computes incoming (Item1) and outgoing (Item2) tangent.
/// </summary>
private static Pair <V2d> Tangents(
V2d p0, V2d p1, V2d p2, V2d p3, double tension, double bias)
{
var oneMinusTensionHalf = (1 - tension) * 0.5;
var x1 = oneMinusTensionHalf * (1 + bias);
var x2 = oneMinusTensionHalf * (1 - bias);
var s0 = p1 - p0; var s1 = p2 - p1; var s2 = p3 - p2;
return(new Pair <V2d>(x1 * s0 + x2 * s1, x1 * s1 + x2 * s2));
}
/// <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>
/// Calculates the sum for a given set of V2ds.
/// </summary>
public static V2d Sum(this V2d[] vectors)
{
V2d sum = V2d.Zero;
for (var i = 0; i < vectors.Length; i++)
{
sum += vectors[i];
}
return(sum);
}
/// <summary>
/// Returns true if the supplied point is inside a hull with
/// planes whose normal vectors point to the inside of the hull.
/// The optional offset parameter is measured in normal direction,
/// i.e. a positive offset makes the hull smaller.
/// </summary>
public static bool IsInsideInwardHull(this V2d point, Hull2d hull, double offset = 0.0)
{
for (int i = 0; i < hull.PlaneCount; i++)
{
if (hull.PlaneArray[i].Height(point) < offset)
{
return(false);
}
}
return(true);
}
/// <summary>
/// Calculates the centroid for a given set of V2ds.
/// </summary>
public static V2d ComputeCentroid(this V2d[] vectors, int[] indices)
{
V2d sum = V2d.Zero;
for (var i = 0; i < indices.Length; i++)
{
sum += vectors[indices[i]];
}
return(sum / (double)indices.Length);
}
/// <summary>
/// Calculates the centroid for a given set of V2ds.
/// </summary>
public static V2d ComputeCentroid(this V2d[] vectors)
{
V2d sum = V2d.Zero;
for (var i = 0; i < vectors.Length; i++)
{
sum += vectors[i];
}
return(sum / (double)vectors.Length);
}
/// <summary>
/// Calculates the sum for a given set of V2ds.
/// </summary>
public static V2d Sum(this IEnumerable <V2d> vectors)
{
V2d sum = V2d.Zero;
foreach (var e in vectors)
{
sum += e;
}
return(sum);
}
/// <summary>
/// Returns a version of the point array scaled by a factor of s about
/// the supplied center.
/// </summary>
public static V2d[] Scaled(this V2d[] pointArray, V2d center, double s)
{
long count = pointArray.LongLength;
var pa = new V2d[count];
for (long i = 0; i < count; i++)
{
pa[i] = center + (pointArray[i] - center) * s;
}
return(pa);
}
public static Polygon2d ToPolygon2d <T>(
this IPolygon <T> polygon, Func <T, int, V2d> point_index_copyFun)
{
var pc = polygon.PointCount;
var pointArray = new V2d[pc];
for (int pi = 0; pi < pc; pi++)
{
pointArray[pi] = point_index_copyFun(polygon[pi], pi);
}
return(new Polygon2d(pointArray));
}
/// <summary>
/// Scales clipping window by given factor.
/// </summary>
public void Zoom(V2d center, double factor)
{
if (factor <= 0.0)
{
throw new ArgumentException("Factor needs to be greater than 0.0, but is " + factor + ".", "factor");
}
m_box.Min.X = (m_box.Min.X - center.X) * factor + center.X;
m_box.Min.Y = (m_box.Min.Y - center.Y) * factor + center.Y;
m_box.Max.X = (m_box.Max.X - center.X) * factor + center.X;
m_box.Max.Y = (m_box.Max.Y - center.Y) * factor + center.Y;
UpdateProjectionTrafo();
}
/// <summary>
/// Returns:
/// 1 if the Polygon created by poly has no self-intersections
/// 0 if one point of the polygon lies close to a line (absoluteEpsilon)
/// -1 if the Polygon created by poly has a real self-intersection
/// </summary>
public static int HasSelfIntersections(
this Polygon2d poly, double absoluteEpsilon)
{
var pointCount = poly.PointCount;
if (absoluteEpsilon < 0.0)
{
throw new ArgumentOutOfRangeException();
}
int i = 0;
int u1 = 0;
int worst = 1;
V2d n0;
//Triangles cannot have self-intersections
if (pointCount == 3)
{
return(1);
}
Line2d line;
for (i = 0; i < pointCount - 1; i++)
{
//line between i and i+1
line = new Line2d(poly[i], poly[i + 1]);
n0 = line.Direction.Normalized;
n0 = new V2d(-n0.Y, n0.X);
for (int u = i + 2; u < pointCount && (u + 1) % pointCount != i; u++)
{
//Polygon not degenerated -> Line cannot intersect with line directly before and directly after
//All lines prior to (u,u1) have already been tested with (i,i+1)
u1 = (u + 1) % pointCount;
if (line.IntersectsLine(poly[u], poly[u1]))
{
//One Point of (u,u1) lies on line (within absoluteEpsilon)
if (n0.Dot(poly[u1] - line.P0).Abs() < absoluteEpsilon || n0.Dot(poly[u] - line.P0).Abs() < absoluteEpsilon)
{
worst = 0;
}
else
{
return(-1);
}
}
}
}
return(worst);
}
/// <summary>
/// Calculates the centroid for a given set of V2ds.
/// </summary>
public static V2d ComputeCentroid(this IEnumerable <V2d> vectors)
{
V2d sum = V2d.Zero;
int count = 0;
foreach (var e in vectors)
{
sum += e;
count++;
}
return(sum / (double)count);
}
/// <summary>
/// Calculates a weighted centroid for a given array of V2ds.
/// </summary>
public static V2d ComputeCentroid(this V2d[] vectors, double[] weights)
{
V2d sum = V2d.Zero;
double weightSum = 0;
for (int i = 0; i < vectors.Length; i++)
{
sum += weights[i] * vectors[i];
weightSum += weights[i];
}
return(sum / weightSum);
}
public readonly double P1, P2; // 2 tangential distortion coefficients
/// <summary>
/// Construct the camera instinsics.
/// </summary>
/// <param name="focalLength">measured in pixels</param>
/// <param name="principalPoint">measured in pixels from [0,0] at the left upper corner of the image</param>
/// <param name="skew"></param>
/// <param name="k1">radial distortion parameter coefficient of r^2</param>
/// <param name="k2">radial distortion parameter coefficient of r^4</param>
/// <param name="k3">radial distortion parameter coefficient of r^6</param>
/// <param name="p1">tagnential distortion parameter 1</param>
/// <param name="p2">tagnential distortion parameter 2</param>
public CameraIntrinsics(
V2d focalLength,
V2d principalPoint,
double skew,
double k1, double k2, double k3,
double p1, double p2)
{
FocalLength = focalLength;
PrincipalPoint = principalPoint;
Skew = skew;
K1 = k1; K2 = k2; K3 = k3;
P1 = p1; P2 = p2;
}
/// <summary>
/// Compute all four possible hashcodes for hashing in a 2-D unit grid.
/// Retrive all items with the four hashodes written into the supplied
/// array. Items need to be added just with the first of the four
/// hashcodes (also computed by function HashCodeGet1of4).
/// </summary>
public static void Get4(V2d point, int[] hca)
{
var xi = (long)Floor(point.X);
var yi = (long)Floor(point.Y);
int xh0 = (int)(xi >> 1), xh1 = xh0 - 1 + ((int)(xi & 1) << 1);
int yh0 = (int)(yi >> 1), yh1 = yh0 - 1 + ((int)(yi & 1) << 1);
hca[0] = Combine(xh0, yh0);
hca[1] = Combine(xh1, yh0);
hca[2] = Combine(xh0, yh1);
hca[3] = Combine(xh1, yh1);
}
/// <summary>
/// Vincenty Inverse Solution of Geodesics on the Ellipsoid (c) Chris Veness 2002-2012
/// from: Vincenty inverse formula - T Vincenty, "Direct and Inverse Solutions of Geodesics on the
/// Ellipsoid with application of nested equations", Survey Review, vol XXII no 176, 1975
/// http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
/// Calculates geodetic distance between two points on the WGS 84
/// ellipsoid specified by latitude/longitude using Vincenty
/// inverse formula for ellipsoids.
/// </summary>
public static double DistanceVincenty(
V2d lonLat0, V2d lonLat1, GeoEllipsoid ellipsoid)
{
double a = ellipsoid.A, b = ellipsoid.B, f = ellipsoid.F;
double L = (lonLat1.X - lonLat0.X) * Constant.RadiansPerDegree;
double U1 = Math.Atan((1 - f) * Math.Tan(lonLat0.Y * Constant.RadiansPerDegree));
double U2 = Math.Atan((1 - f) * Math.Tan(lonLat1.Y * Constant.RadiansPerDegree));
double sinU1 = Math.Sin(U1), cosU1 = Math.Cos(U1);
double sinU2 = Math.Sin(U2), cosU2 = Math.Cos(U2);
double lambda = L, lambdaP;
int iterLimit = 100;
double sinSigma, cosSigma, sigma, cosSqAlpha, cos2SigmaM;
do
{
double sinLambda = Math.Sin(lambda), cosLambda = Math.Cos(lambda);
sinSigma = Math.Sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
* (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
if (sinSigma == 0)
{
return(0); // co-incident points
}
cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
sigma = Math.Atan2(sinSigma, cosSigma);
double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
cosSqAlpha = 1 - sinAlpha * sinAlpha;
cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
if (double.IsNaN(cos2SigmaM))
{
cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6)
}
double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
lambdaP = lambda;
lambda = L + (1 - C) * f * sinAlpha *
(sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
}while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);
if (iterLimit == 0)
{
return(double.NaN); // formula failed to converge
}
double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
double s = b * A * (sigma - deltaSigma);
return(s);
}
/// <summary>
/// Scales clipping window by given factor.
/// </summary>
public static void Zoom(this ICameraProjection self, V2d center, double factor)
{
if (factor <= 0.0)
{
throw new ArgumentException("Factor needs to be greater than 0.0, but is " + factor + ".", "factor");
}
var box = self.ClippingWindow;
box.Min.X = (box.Min.X - center.X) * factor + center.X;
box.Min.Y = (box.Min.Y - center.Y) * factor + center.Y;
box.Max.X = (box.Max.X - center.X) * factor + center.X;
box.Max.Y = (box.Max.Y - center.Y) * factor + center.Y;
self.ClippingWindow = box;
}
/// <summary>
/// Creates circle from three points.
/// </summary>
public Circle2d(V2d a, V2d b, V2d c)
{
var l0 = new Plane2d((b - a).Normalized, (a + b) * 0.5);
var l1 = new Plane2d((c - b).Normalized, (b + c) * 0.5);
if (l0.Intersects(l1, out Center))
{
Radius = (a - Center).Length;
}
else
{
throw new Exception("Cannot construct circle because given points are collinear.");
}
}
/// <summary>
/// UnDistortion of pixel coordinates.
/// </summary>
public V2d UndistortPixel(V2d p2, V2i imageSize)
{
var maxSize = Fun.Max(imageSize.X, imageSize.Y);
var y = (p2.Y - PrincipalPoint.Y * imageSize.Y) / (FocalLength.Y * maxSize);
var x = (p2.X - PrincipalPoint.X * imageSize.X - Skew * maxSize * y) / (FocalLength.X * maxSize);
ComputeUndistortionParamsForCameraPoint(x, y, out double rd, out double tx, out double ty);
double xd = x * rd + tx;
double yd = y * rd + ty;
return(new V2d(
FocalLength.X * maxSize * xd + Skew * maxSize * yd + PrincipalPoint.X * imageSize.X,
FocalLength.Y * maxSize * yd + PrincipalPoint.Y * imageSize.Y));
}
/// <summary>
/// Calculates a weighted centroid for vectors and weights given by indices.
/// Sum(vectors[indices[i]] * weights[indices[i]]) / Sum(weights[indices[i]].
/// </summary>
public static V2d ComputeCentroid(this V2d[] vectors, double[] weights, int[] indices)
{
V2d sum = V2d.Zero;
double weightSum = 0;
for (int i = 0; i < indices.Length; i++)
{
var w = weights[indices[i]];
sum += w * vectors[indices[i]];
weightSum += w;
}
return(sum / weightSum);
}
