Develop Reference

Implementing Cubic Spline Interpolation (PCHIP or MAKIMA) for 3D points in C#

I wrote an application in C# which controls a camera through a 3D environment.
At the moment the camera follows a path which is defined as an array of points, where each point is defined as: x, y, z & rotation for each axis (everything is a float)
The camera position between those points are computed by linear interpolation every frame.
Because this leads to very clunky movement, I want to implement cubic spline interpolation instead.
There are many different algorithms, but so far I set my eyes on the following, since those feel "more correct", which means they don't overshoot:
Piecewise Cubic Hermite Interpolating Polynomial (PCHIP)
Modified Akima piecewise cubic Hermite interpolation (MAKIMA)
My biggest problem is, that I am absolutely not a mathematician and I can't read and understand the gibberish found online for their definitions, so I need help translating it into code.
Constraints for the implementation
Algorithm should use either PCHIP or MAKIMA (I am open for other "correct" approaches though)
Interpolated values must be a function of time (t from 0 to {points.Length})
Must run for 3D points, not only 2D
Should not contain any external libraries if possible
Libraries
I have checked the following, but they either don't support 3D points, or interpolation by time (and aren't intuitive to use either... not even talking about the license here):
Math.NET Numerics
ALGLIB
Here is a small code stump which I came up with to demonstrate what I am looking for:
static float[,] points = new float[,] {
{10, 10, 10},
{35, 5, 25},
{15, 30, 15}
};
static int numberOfPoints = points.Length / 3; // dividing for each stored float value per point: only x, y, z for simplification
static void Main()
{
List<float[]> pointsWithInterpolation = new(); // holding both known and interpolated points
float stepSize = 0.1f; // step size for t
for (int i = 0; i < numberOfPoints - 1; i++)
{ // loop for each spline segment
pointsWithInterpolation.Add(new float[] { points[i, 0], points[i, 1], points[i, 2] }); // add an allready known point to the list
for (float t = stepSize; t < 1f; t += stepSize)
{ // should run (1 / {stepSize} - 2) times
float[] interpolatedPoint = Interpolate(i + t);
pointsWithInterpolation.Add(interpolatedPoint); // add an interpolated point to the list
}
if (i == numberOfPoints - 1)
{ // add the very last point if we have finished the last segment
pointsWithInterpolation.Add(new float[] { points[i + 1, 0], points[i + 1, 1], points[i + 1, 2] }); // add an allready known point to the list
}
}
}
private static float[] Interpolate(float t)
{
// point indexes
int p1 = (int)t;
int p2 = p1 + 1;
int p0 = p1 == 0 ? 0 : p1 - 1; // if there are no previous point, set it to the first one
int p3 = p2 == numberOfPoints - 1 ? p2 : p2 + 1; // if there are no following points, set it to the last one
float x0 = points[p0, 0];
float x1 = points[p1, 0];
float x2 = points[p2, 0];
float x3 = points[p3, 0];
float y0 = points[p0, 1];
float y1 = points[p1, 1];
float y2 = points[p2, 1];
float y3 = points[p3, 1];
float z0 = points[p0, 2];
float z1 = points[p1, 2];
float z2 = points[p2, 2];
float z3 = points[p3, 2];
float x, y, z;
/* black magic hocus pocus happening here
x = ???;
y = ???;
z = ???;
*/
float[] point = new float[] { x, y, z };
return point;
}
So the question is: what is the formula for solving for x/y/z (the formula should be the same for each variable, but they use different variables).
Yes I know, this results in a constant number of interpolated points between the "real" ones, even if the distance of segments widely varies - I will tackle the issue of constant speed separately.
If I am not mistaken, every interpolation between p1 and p2 needs 4 values, so I set p0 equal to p1 for the very first segment and p3 equal to p2 for the last one to make up for non-existant points.
I haven't performance optimized the code, so I (hopefully) make it more clear/easy to understand what I try to do.
Of course I would also be thankful for code in other languages, as long as it fulfils the implementation constraints and doesn't use functions which aren't available in C# (and also no assembler please lmao)

Rescaling Image in Windows Form

I have a set of points I want to show on a 2000x2000 canvas. Here is an example: "61.86, 83 - 61.79, 82.91 - 61.77, 82.77 - 61.92, 82.76 - 61.75, 82.7 - 61.79, 82.58 - 61.85, 82.46 - 61.79, 82.17 - 61.72, 81.88 - 61.61, 81.61 - 61.51, 81.33 - 61.49, 81.02 - 61.33, 80.99 - 61.37, 80.83"
These points are from a 100x100 grid so the first one ought to be in the bottom right quarter of my 2000*2000 canvas.
To do this I have code that finds the biggest X and Y and then rescales.
List<double> MinAndMax(List<Node> spots)
{
List<double> retValues = new List<double>();
double xLowest = spots.Select(s => s.X).Min();
double xHighest = spots.Select(s => s.X).Max();
double xDifference = xHighest - xLowest;
double yLowest = spots.Select(s => s.Y).Min();
double yHighest = spots.Select(s => s.Y).Max();
double yDifference = yHighest - yLowest;
if (xLowest < yLowest)
retValues.Add(xLowest);
else
retValues.Add(yLowest);
if (xHighest < yHighest)
retValues.Add(yHighest);
else
retValues.Add(xHighest);
return retValues;
}
int Rescale(double oldValue, double oldMin, double oldMax, int newMin, int newMax)
{
return Convert.ToInt32(((oldValue - oldMin) * (newMax - newMin) / (oldMax - oldMin)) + newMin);
}
I call it like so:
double zoneMin, zoneMax;
int lowestCanvas = 150, highestCanvas = 1850;
List<Node> path = await PathMaker();
List<double> zoneMinMax = MinAndMax(path);
zoneMin = zoneMinMax[0];
zoneMax = zoneMinMax[1];
foreach (Node spot in path)
{
Point point = new Point();
point.X = Rescale(spot.X, zoneMin, zoneMax, lowestCanvas, highestCanvas);
point.Y = Rescale(spot.Y, zoneMin, zoneMax, lowestCanvas, highestCanvas);
NodeSpot dot = new NodeSpot()
{
Name = spot.Name,
Location = point,
IsInPath = true
};
drawingSurface1.Nodes.Add(dot);
}
drawingSurface1.Invalidate();
Instead of getting my little path nicely spread out, I get this odd clump in the bottom LEFT had quadrant.
I can't see where I am going wrong here. What do I need to do in order to have my 14 points spread out over the canvass?

Your issue is that you are returning a single min value and a single max value. You need separate min and max values for X and Y, as the ranges in each coordinate are different. In your sample data from the question the range of X is [-61.92, 61.86] and the range of Y is [80.83, 83]. Your approach will draw a frame covering [-61.92, -61.92] to [83, 83], with most of the points in one corner.
Your test fails to catch the problem as the X and Y values are the same in the test case. Create a test case where the X values are all negative and the Y values positive, this will show the issue.

let's take [[20, 20], [50, 50], [80, 80]] as an easy exemple.
min and max will be 20 and 80 and wanted scale is 0 to 2000.
for the point [50, 50]
(((oldValue - oldMin) * (newMax - newMin) / (oldMax - oldMin)) + newMin)
gives
((50 - 20) * (2000 - 0) / (80 - 20)) + 0
= 30*2000 / 60
= 1000 (which is half the size of the canvas)
seems coherent so the problem is not coming from the transform function
I suggest trying to debug it by printing the [X, Y] values of the "point" to be sure tho
Also print the min max of the oldScale to be sure this is not the problem

Use the Graphics.PageScale Property
See also Coordinate Systems and Transformations

Finding the true anomaly from state vectors

I'm attempting to convert from state vectors (position and speed) into Kepler elements, however I'm running into problems where a negative velocity or position will give me wrong results when trying to calculate true anomaly.
Here are the different ways I'm trying to calculate the True Anomaly:
/// <summary>
/// https://en.wikipedia.org/wiki/True_anomaly#From_state_vectors
/// </summary>
public static double TrueAnomaly(Vector4 eccentVector, Vector4 position, Vector4 velocity)
{
var dotEccPos = Vector4.Dot(eccentVector, position);
var talen = eccentVector.Length() * position.Length();
talen = dotEccPos / talen;
talen = GMath.Clamp(talen, -1, 1);
var trueAnomoly = Math.Acos(talen);
if (Vector4.Dot(position, velocity) < 0)
trueAnomoly = Math.PI * 2 - trueAnomoly;
return trueAnomoly;
}
//sgp = standard gravitational parameter
public static double TrueAnomaly(double sgp, Vector4 position, Vector4 velocity)
{
var H = Vector4.Cross(position, velocity).Length();
var R = position.Length();
var q = Vector4.Dot(position, velocity); // dot product of r*v
var TAx = H * H / (R * sgp) - 1;
var TAy = H * q / (R * sgp);
var TA = Math.Atan2(TAy, TAx);
return TA;
}
public static double TrueAnomalyFromEccentricAnomaly(double eccentricity, double eccentricAnomaly)
{
var x = Math.Sqrt(1 - Math.Pow(eccentricity, 2)) * Math.Sin(eccentricAnomaly);
var y = Math.Cos(eccentricAnomaly) - eccentricity;
return Math.Atan2(x, y);
}
public static double TrueAnomalyFromEccentricAnomaly2(double eccentricity, double eccentricAnomaly)
{
var x = Math.Cos(eccentricAnomaly) - eccentricity;
var y = 1 - eccentricity * Math.Cos(eccentricAnomaly);
return Math.Acos(x / y);
}
Edit: another way of doing it which Spectre pointed out:
public static double TrueAnomaly(Vector4 position, double loP)
{
return Math.Atan2(position.Y, position.X) - loP;
}
Positions are all relative to the parent body.
These functions all agree if position.x, position.y and velocity.y are all positive.
How do I fix these so that I get a consistent results when position and velocity are negitive?
Just to clarify: My angles appear to be sort of correct, just pointing in the wrong quadrant depending on the position and or velocity vectors.
Yeah so I was wrong, the above all do return the correct values after all.
So I found an edge case where most of the above calculations fail.
Given position and velocity:
pos = new Vector4() { X = -0.208994076275941, Y = 0.955838328099748 };
vel = new Vector4() { X = -2.1678187689294E-07, Y = -7.93096769486992E-08 };
I get some odd results, ie ~ -31.1 degrees, when I think it should return ` 31.1 (non negative). one of them returns ~ 328.8.
However testing with this position and velocity the results apear to be ok:
pos = new Vector4() { X = -0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(-25) };
See my answer for extra code on how I'm testing and the math I'm using for some of the other variables.
I'm going around in circles on this one. this is a result of a bug in my existing code that shows up under some conditions but not others.
I guess the real question now is WHY am I getting different results with position/velocity above that don't match to my expectations or each other?

Assuming 2D case... I am doing this differently:
compute radius of semi axises and rotation
so you need to remember whole orbit and find 2 most distant points on it that is major axis a. The minor axis b usually is 90 deg from major axis but to be sure just fins 2 perpendicularly most distant points on your orbit to major axis. So now you got both semi axises. The initial rotation is computed from the major axis by atan2.
compute true anomaly E
so if center is x0,y0 (intersection of a,b or center point of both) initial rotation is ang0 (angle of a) and your point on orbit is x,y then:
E = atan2(y-y0,x-x0) - ang0
However in order to match Newton/D'Alembert physics to Kepler orbital parameters you need to boost the integration precision like I did here:
Is it possible to make realistic n-body solar system simulation in matter of size and mass?
see the [Edit3] Improving Newton D'ALembert integration precision even more in there.
For more info and equations see:
Solving Kepler's equation
[Edit1] so you want to compute V I see it like this:
As you got your coordinates relative to parent you can assume they are already in focal point centered so no need for x0,y0 anymore. Of coarse if you want high precision and have more than 2 bodies (focal mass + object + proximity object(s) like moons) then the parent mass will no longer be in focal point of orbit but close to it ... and to remedy you need to use real focal point position so x0,y0 again... So how to do it:
compute center point (cx,cy) and a,b semi axises
so its the same as in previous text.
compute focal point (x0,y0) in orbit axis aligned coordinates
simple:
x0 = cx + sqrt( a^2 + b^2 );
y0 = cy;
initial angle ang0 of a
let xa,ya be the intersection of orbit and major axis a on the side with bigger speeds (near parent object focus). Then:
ang0 = atan2( ya-cy , xa-cx );
and finally the V fore any of yours x,y
V = atan2( y-y0 , x-x0 ) - ang0;

Ok so on further testing it appears my original calcs do all return the correct values, however when I was looking at the outputs I was not taking the LoP into account and basically not recognizing that 180 is essentially the same angle as -180.
(I was also looking at the output in radians and just didn't see what should have been obvious)
Long story short, I have a bug I thought was in this area of the code and got lost in the weeds.
Seems I was wrong above. see OP for edge case.
Here's some code I used to test these,
I used variations of the following inputs:
pos = new Vector4() { X = 0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(25) };
And tested them with the following
double parentMass = 1.989e30;
double objMass = 2.2e+15;
double sgp = GameConstants.Science.GravitationalConstant * (parentMass + objMass) / 3.347928976e33;
Vector4 ev = OrbitMath.EccentricityVector(sgp, pos, vel);
double e = ev.Length();
double specificOrbitalEnergy = Math.Pow(vel.Length(), 2) * 0.5 - sgp / pos.Length();
double a = -sgp / (2 * specificOrbitalEnergy);
double ae = e * a;
double aop = Math.Atan2(ev.Y, ev.X);
double eccentricAnomaly = OrbitMath.GetEccentricAnomalyFromStateVectors(pos, a, ae, aop);
double aopD = Angle.ToDegrees(aop);
double directAngle = Math.Atan2(pos.Y, pos.X);
var θ1 = OrbitMath.TrueAnomaly(sgp, pos, vel);
var θ2 = OrbitMath.TrueAnomaly(ev, pos, vel);
var θ3 = OrbitMath.TrueAnomalyFromEccentricAnomaly(e, eccentricAnomaly);
var θ4 = OrbitMath.TrueAnomalyFromEccentricAnomaly2(e, eccentricAnomaly);
var θ5 = OrbitMath.TrueAnomaly(pos, aop);
double angleΔ = 0.0000001; //this is the "acceptable" amount of error, really only the TrueAnomalyFromEccentricAnomaly() calcs needed this.
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ1), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ2), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ3), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ4), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ5), angleΔ);
and the following to compare the angles:
public static double DifferenceBetweenRadians(double a1, double a2)
{
return Math.PI - Math.Abs(Math.Abs(a1 - a2) - Math.PI);
}
And eccentricity Vector found thus:
public static Vector4 EccentricityVector(double sgp, Vector4 position, Vector4 velocity)
{
Vector4 angularMomentum = Vector4.Cross(position, velocity);
Vector4 foo1 = Vector4.Cross(velocity, angularMomentum) / sgp;
var foo2 = position / position.Length();
return foo1 - foo2;
}
And EccentricAnomaly:
public static double GetEccentricAnomalyFromStateVectors(Vector4 position, double a, double linierEccentricity, double aop)
{
var x = (position.X * Math.Cos(-aop)) - (position.Y * Math.Sin(-aop));
x = linierEccentricity + x;
double foo = GMath.Clamp(x / a, -1, 1); //because sometimes we were getting a floating point error that resulted in numbers infinatly smaller than -1
return Math.Acos(foo);
}
Thanks to Futurogogist and Spektre for their help.

I am assuming you are working in two dimensions?
Two dimensional vectors of position p and velocity v. The constant K is the the product of the gravitational constant and the mass of the gravity generating body. Calculate the eccentricity vector
eccVector = (dot(v, v)*p - dot(v, p)*v) / K - p / sqrt(dot(p, p));
eccentricity = sqrt(dot(eccVector, eccVector));
eccVector = eccVector / eccentricity;
b = { - eccVector.y, eccVector.x}; //unit vector perpendicular to eccVector
r = sqrt(dot(p, p));
cos_TA = dot(p, eccVector) / r; \\ cosine of true anomaly
sin_TA = dot(p, b) / r; \\ sine of true anomaly
if (sin_TA >= 0) {
trueAnomaly = arccos(cos_TA);
}
else if (sin_TA < 0){
trueAnomaly = 2*pi - arccos(cos_TA);
}

How do i find the middle point/s coordinates between two points?

I have this method:
private static List<PointF> ExtendPoints(PointF pt1, PointF pt3)
{
float x = (Math.Max(pt1.X, pt3.X) - Math.Min(pt1.X, pt3.X)) / 2 + Math.Min(pt1.X, pt3.X);
float y = (Math.Max(pt1.Y, pt3.Y) - Math.Min(pt1.Y, pt3.Y)) / 2 + Math.Min(pt1.Y, pt3.Y);
var pt2 = new PointF(x, y);
extendedPoints.Add(pt1);
extendedPoints.Add(pt2);
extendedPoints.Add(pt3);
return extendedPoints;
}
I give two points and it return a List with 3 points the new point pt2 is in the middle.
The calculation is just to find the excat coordinates between the distance of the two given points.
Now i want to make that it will return a List with 4 points.
pt1 pt2 pt3 pt4
Thr List format will be: pt1 pt2 pt3 pt4 and the new points are pt3 and pt4.
So now i need to divide it by 3 ? I need to find the exact two points between the given two points.
This is what i did but its not giving the right result:
private static List<PointF> ExtendPoints(PointF pt1, PointF pt4)
{
float x = (Math.Max(pt1.X, pt4.X) - Math.Min(pt1.X, pt4.X)) / 3 + Math.Min(pt1.X, pt4.X); // Could also doing: (pt1.X + pt3.X) / 2;
float y = (Math.Max(pt1.Y, pt4.Y) - Math.Min(pt1.Y, pt4.Y)) / 3 + Math.Min(pt1.Y, pt4.Y);
var pt2 = new PointF(x, y);
float a = (Math.Max(pt1.X, pt4.X) - Math.Min(pt1.X, pt4.X)) / 2 / 3 + Math.Min(pt1.X, pt4.X);
float b = (Math.Max(pt1.Y, pt4.Y) - Math.Min(pt1.Y, pt4.Y)) / 2 / 3 + Math.Min(pt1.Y, pt4.Y);
var pt3 = new PointF(a, b);
extendedPoints.Add(pt1);
extendedPoints.Add(pt2);
extendedPoints.Add(pt3);
extendedPoints.Add(pt4);
return extendedPoints;
}
The List return extendedPoints contain 4 indexs in each index there is x and y.
Then im doing in the construction:
point1 = new Point(80, 80);
point2 = new Point(280, 300);
extendedPoints = ExtendPoints(point1, point2);
Then in the paint event i want to display the List the points returned:
foreach (PointF pt in extendedPoints)
{
e.FillEllipse(Brushes.Red, pt.X, pt.Y, 4f, 4f);
}
But i see only 3 points not 4.
So where is the bug ? Something in the method with the calculations is wrong ?

Yes, dividing the distance travelled by 1/3 and 2/3 on each axis will give you the intermediate points on the straight line between the two endpoints. Basic geometry.

Calculate Minimum Bounding Rectangle Of 2D Shape By Coordinates

I have a solution that uses spatial data to represent a cluster of points on a map. I have the need to used the coordinates that represent the extents of a cluster to find the minimum bounding rectangle that can contain said cluster of points.
Does any simple algorithm exist to be able to calculate this or is there any built in functionality in C# to achieve this. I am aware of the NetTopologySuite but am not sure how/if I could use this to achieve the same goal. I have a list of coordinates so I would need to pass this list of strings into it and get the MBR out.

The easiest solution, and I assume the one you're most likely to be looking for, is to calculate the axis-aligned bounding box, which is simply a case of finding the min/max x & y values, then constructing a box from those.
I'll give you pseudo-code for that, given that you haven't posted the types that your geometry is expressed in...
type point { float x; float y; }
type box { point topleft; point topright; point bottomleft; point bottomright; }
function bounding_box(points)
{
xmin = min(points.x)
xmax = max(points.x)
ymin = min(points.y)
ymax = max(points.y)
return new box{
topleft = { x = xmin, y = ymax },
topright = { x = xmax, y = ymax },
bottomleft = { x = xmin, y = ymin },
bottomright = { x = xmax, y = ymin }
};
}
So given these:
point[] points = [[x = -2, y = 0], [x = 1, y = 2], [x = 1, y = 1], [x = -1, y = -2]];
box bounds = bounding_box(points);
All of the following will be true:
bounds.topleft == [x = -2, y = 2];
bounds.topright == [x = 1, y = 2];
bounds.bottomleft == [x = -2, y = -2];
bounds.bottomright == [x = -1, y = -2];
Of course, if the coordinate system has the lowest coordinates at the top (e.g. like a typical display) - then you have to invert the calculation; or calculate the result in object-space first and then translate to logical space afterwards.
Notice I've gone for a type for the box that expresses all four corners, in case you decide in the future to update to an arbitrarily aligned box in the future (although by the same token you could just use a point + 2 vectors for that).

One possible, though simple, way to do it could be like this:
public Rectangle Test(List<Point> points)
{
// Add checks here, if necessary, to make sure that points is not null,
// and that it contains at least one (or perhaps two?) elements
var minX = points.Min(p => p.X);
var minY = points.Min(p => p.Y);
var maxX = points.Max(p => p.X);
var maxY = points.Max(p => p.Y);
return new Rectangle(new Point(minX, minY), new Size(maxX-minX, maxY-minY));
}
This does of course assume that you're looking for a rectangle that is aligned vertically and horizontally. So if you're looking for the smallest possible rectangle, no matter how it is rotated, this is not for you.

Try G# at http://www.ceometric.com/products/g.html
It has minimum area and minimum perimeter enclosing rectangles and also minimum enclosing circles.