Develop Reference

Find the possible values for two equations

2x + 4y + 6z = 1200
x + y + z = 300
how can I find the possible x, y, z integer values in a c# method?, I am trying to find a better solution instead of using brute force nested for loops since it is not a good solution.
public List<Tuple<int, int, int>> Calculate()
{
var result = new List<Tuple<int, int, int>>();
int maxValue = 300;
for(int i = 0; i< maxValue; i++)
for (int j = 0; j < maxValue; j++)
for (int k = 0; k < maxValue; k++)
if (i + j + k == maxValue && 2 * i + 4 * j + 6 * k == 1200)
result.Add(new Tuple<int, int, int>(i, j, k));
return result;
}
Thank you in advance.

Well, having
2x + 4y + 6z = 1200
x + y + z = 300
you can put it as
x + 2y + 3z = 600
x + y + z = 300
subtract 2nd from the 1st and you get
y + 2z = 300
or
y = 300 - 2z
Since x = 300 - y - z we can put it as
x = 300 - y - z =
= 300 - (300 - 2z) - z =
= 300 - 300 + 2z - z =
= z
Finally, for the arbitrary z (which is free variable)
x = z
y = 300 - 2 * z;
Possible c# code:
private static (int x, int y, int z) Solution(int x) => (x, 300 - 2 * x, x);
Demo:
string solutions = string.Join(Environment.NewLine, Enumerable
.Range(0, 10)
.Select(x => Solution(x)));
...
// 10 solutions for x = 0..9
string solutions = string.Join(Environment.NewLine, Enumerable
.Range(0, 10)
.Select(x => Solution(x)));
Console.Write(solutions);
Outcome:
(0, 300, 0)
(1, 298, 1)
(2, 296, 2)
(3, 294, 3)
(4, 292, 4)
(5, 290, 5)
(6, 288, 6)
(7, 286, 7)
(8, 284, 8)
(9, 282, 9)
If you are looking for non-negative solutions only (you've mentioned probabilities in the comments for the code), then use x in [0..150] range:
(0, 300, 0)
(1, 298, 1)
(2, 296, 2)
...
(148, 4, 148)
(149, 2, 149)
(150, 0, 150)
Edit: your Calculate() method improved:
public static List<Tuple<int, int, int>> Calculate() {
var result = new List<Tuple<int, int, int>>();
const int maxValue = 300;
int start = Math.Max(150 - maxValue / 2, 0);
for (int x = start; ; ++x) {
int y = 300 - 2 * x;
int z = x;
if (y < 0 || x > maxValue)
break;
result.Add(new Tuple<int, int, int>(x, y, z));
}
return result;
}

Math Theory
If you want a general method you should use some Linear Algebra theory.
Let's rewrite the equations in matrix form as A(x,y) + bz = c,
where:
A: is the matrix that contains the coefficents of the x,y coordinates
A = (2,4,1,1)
b = (6,1) (transposed) is the vector containing the z coefficents.
c = (1200, 300) (transposed) is the vector containing the constant terms.
then (x,y) = AInverse * (c - bz). That is a function of z (Let's say f(z)). So for every z you obtain a valid solution: (f(z),z). f(z) is a vector of 2 components.
Note
If A is not invertible because the equations (rstricted to x and y) are linearly dependent (i.e. the 2 equations are the "same" equation in x and y), this method fails.
Code
We can code that like that:
Step1
We must code the Matrix2x2. I do it from scratch now, but you maybe want to use some framework if you really need in production:
public record struct Matrix2x2 (float A00, float A01, float A10, float A11)
{
public float Determinant => A00 * A11 - A01 * A10;
public Matrix2x2 Invert() => Determinant != 0
? 1 / Determinant * new Matrix2x2() { A11 = A00, A00 = A11, A01 = -A01, A10 = -A10 }
: throw new InvalidOperationException($"Cannot Invert this matrix");
public static Matrix2x2 operator *(Matrix2x2 a, float number) => new()
{
A00 = a.A00 * number,
A01 = a.A01 * number,
A10 = a.A10 * number,
A11 = a.A11 * number,
};
public static Matrix2x2 operator *(float number, Matrix2x2 a) => a * number;
public static Vector2 operator *(Matrix2x2 a, Vector2 vector)
{
var x = a.A00 * vector.X + a.A01 * vector.Y;
var y = a.A10 * vector.X + a.A11 * vector.Y;
return new(x, y);
}
}
Step2
We must encode the equation. We need the Matrix A, the z coefficents and b, and the vector c of constant terms:
public delegate Vector3 SolutionSpace(float z);
public static class EquationHelper
{
public static SolutionSpace SolveEquation(Matrix2x2 equationMatrix, Vector2 b, Vector2 c) => z =>
{
var v = equationMatrix.Invert() * (c - z * b);
return new Vector3(v.X, v.Y, z);
};
}
Note: The object returned from this function is the solution space, that depends from 1 parameter (the z value). So we could see it as a function that maps (float z) => (x(z), y(z), z) that in c# is a delegate that returns a Vector3 and takes in input a float.
Usage
var A = new Matrix2x2(2, 4, 1, 1);
var b = new Vector2(6, 1);
var c = new Vector2(1200, 300);
var solution = EquationHelper.SolveEquation(A, b, c);
foreach(var z in Enumerable.Range(-10, 21))
Console.WriteLine(solution(z));
Output

Segment Segment intersection in 3D

Let's suppose I have a triangle defined by three coordinates A, B and C and a line segment defined by two coordinates D and E in 3D euclidean space.
Let's further suppose that the segment is intersecting the triangle.
Now, I need to find out whether the line segment intersects the triangle at its "border".
So my idea was to take each edge of the triangle (AB, BC and CA) and test whether one of them intersects with the segment DE. The problem is that I haven't found a solution for segment/segment intersection. I have found this one but it doesn't work. Help very appreciated.
Edit
Based on MBo's answer, I have implemented a C# method:
// return: 0 -> segment parallel to plane
// return: 1 -> segment intersects an edge of the face
// return: -1 -> segments intersects the interior or the triangle or not at all
public static int SegmentTriangleIntersection(Vector3D a, Vector3D b, Vector3D c, Vector3D d, Vector3D e)
{
var ed = e - d;
var ba = b - a;
var ca = c - a;
var ad = a - d;
var det = (ed.X*-ba.Y*-ca.Z) + (-ba.X*-ca.Y*ed.Z) + (-ca.X*ed.Y*-ba.Z) - (ed.Z*-ba.Y*-ca.X) - (-ba.Z*-ca.Y*ed.X) -
(-ca.Z*ed.Y*-ba.X);
if (Vector3D.IsNearlyEqual(det, 0)) // segment parallel to triangle
return 0;
var det_t = (ad.X * -ba.Y * -ca.Z) + (-ba.X * -ca.Y * ad.Z) + (-ca.X * ad.Y * -ba.Z) - (ad.Z * -ba.Y * -ca.X) - (-ba.Z * -ca.Y * ad.X) -
(-ca.Z * ad.Y * -ba.X);
var t = det_t/det;
if (t >= 0 & t <= 1) // segment intersects plane of triangle
{
var det_u = (ed.X*ad.Y*-ca.Z) + (ad.X*-ca.Y*ed.Z) + (-ca.X*ed.Y*ad.Z) - (ed.Z*ad.Y*-ca.X) - (ad.Z*-ca.Y*ed.X) -
(-ca.Z*ed.Y*ad.X);
var u = det_u/det;
var det_v = (ed.X*-ba.Y*ad.Z) + (-ba.X*ad.Y*ed.Z) + (ad.X*ed.Y*-ba.Z) - (ed.Z*-ba.Y*ad.X) -
(-ba.Z*ad.Y*ed.X)-(ad.Z*ed.Y*-ba.X);
var v = det_v/det;
if (Vector3D.IsNearlyEqual(u, 0) && v >= 0 && v <= 1)
return 1;
if (Vector3D.IsNearlyEqual(v, 0) && u >= 0 && u <= 1)
return 1;
if (Vector3D.IsNearlyEqual(v + u, 1) && u >= 0 && v >= 0)
return 1;
}
return -1;
}

Let's use parametric equations for coordinates (vectors in bold):
Segment DE:
sDE(t) = D + t * (E - D) = D + t * ED
//ED = E - D
Segment AB:
sAB(u) = A + u * (B - A) = A + u * BA
Segment AC:
sAC(v) = A + v * (C - A) = A + u * CA
Plane of ABC triangle is described by bi-parametric equation:
pABC(u,v) = A + u * BA + v * CA
Point in the plane with coordinates (u,v) lies inside the triangle, if
u in range [0..1]
and
v in range [0..1]
and
u+v is in range [0..1]
Point with coordinates (u,v) lies at the triangle edge, if [edge condition]
u = 0 and v in range [0..1] //at AC edge
or
v = 0 and u in range [0..1] //at AC edge
or
v + u = 1 and u in range (0..1) and v in range (0..1) //at BC edge
Now let's write equation for intersection of DE segment and ABC plane
sDE(t) = pABC(u,v)
D + t * ED = A + u * BA + v * CA
We can solve the last equation (system of 3 linear equations for every coordinate) and find parameters t,u,v
D.X + t * ED.X = A.X + u * BA.X + v * CA.X
D.Y + t * ED.Y = A.Y + u * BA.Y + v * CA.Y
D.Z + t * ED.Z = A.Z + u * BA.Z + v * CA.Z
t * ED.X - u * BA.X - v * CA.X = A.X - D.X
t * ED.Y - u * BA.Y - v * CA.Y = A.Y - D.Y
t * ED.Z - u * BA.Z - v * CA.Z = A.Z - D.Z
At first find determinant of this system - if it is zero, segment is parallel to the plane.
If not, check for parameter t. Segment intersects the plane if t is in range [0..1]
If yes, check whether u, v parameters comply to edge condition above

Finding the point of intersection between a line and a cubic spline

I need a way to programmatically find the point of intersection between a line f(x), which originates from the origin, and a cubic spline defined by 4 points. The line is guaranteed to intersect the center segment of the spline, between X1 and X2.
I have tried a number of approaches but cannot seem to get the expected result. I suspect my problem lies somewhere in my handling of complex numbers.
Can anyone find the problem with my code, or else suggest a different approach?
private Vector2 CubicInterpolatedIntersection(float y0, float y1,
float y2, float y3, float lineSlope, lineYOffset)
{
// f(x) = lineSlope * x + lineYOffset
// g(x) = (a0 * x^3) + (a1 * x^2) + (a2 * x) + a3
// Calculate Catmull-Rom coefficients for g(x) equation as found
// in reference (1). These
double a0, a1, a2, a3;
a0 = -0.5 * y0 + 1.5 * y1 - 1.5 * y2 + 0.5 * y3;
a1 = y0 - 2.5 * y1 + 2 * y2 - 0.5 * y3;
a2 = -0.5 * y0 + 0.5 * y2;
a3 = y1;
// To find POI, let 'g(x) - f(x) = 0'. Simplified equation is:
// (a0 * x^3) + (a1 * x^2) + ((a2 - lineSlope) * x)
// + (a3 - lineYOffset) = 0
// Put in standard form 'ax^3 + bx^2 + cx + d = 0'
double a, b, c, d;
a = a0;
b = a1;
c = a2 - lineSlope;
d = a3 - lineYOffset;
// Solve for roots using cubic equation as found in reference (2).
// x = {q + [q^2 + (r-p^2)^3]^(1/2)}^(1/3)
// + {q - [q^2 + (r-p^2)^3]^(1/2)}^(1/3) + p
// Where...
double p, q, r;
p = -b / (3 * a);
q = p * p * p + (b * c - 3 * a * d) / (6 * a * a);
r = c / (3 * a);
//Solve the equation starting from the center.
double x, x2;
x = r - p * p;
x = x * x * x + q * q;
// Here's where I'm not sure. The cubic equation contains a square
// root. So if x is negative at this point, then we need to proceed
// with complex numbers.
if (x >= 0)
{
x = Math.Sqrt(x);
x = CubicRoot(q + x) + CubicRoot(q - x) + p;
}
else
{
x = Math.Sqrt(Math.Abs(x));
// x now represents the imaginary component of
// a complex number 'a + b*i'
// We need to take the cubic root of 'q + x' and 'q - x'
// Since x is imaginary, we have two complex numbers in
// standard form. Therefore, we take the cubic root of
// the magnitude of these complex numbers
x = CubicRoot(Math.Sqrt(q * q + x * x)) +
CubicRoot(Math.Sqrt(q * q + -x * -x)) + p;
}
// At this point, x should hold the x-value of
// the point of intersection.
// Now use it to solve g(x).
x2 = x * x;
return new Vector2((float)Math.Abs(x),
(float)Math.Abs(a0 * x * x2 + a1 * x2 + a2 * x + a3));
}
References:
http://paulbourke.net/miscellaneous/interpolation/
http://www.math.vanderbilt.edu/~schectex/courses/cubic/

The code
if (x >= 0)
{ // One real root and two imaginaries.
x = Math.Sqrt(x);
x = CubicRoot(q + x) + CubicRoot(q - x) + p;
}
else
{ // Three real roots.
x = Math.Sqrt(Math.Abs(x));
x_1 = Math.Sign(q)*2*(q*q + x*x)^(1/6)*Math.Cos(Math.Atan(x/q)/3) + p;
x_2 = Math.Sign(q)*2*(q*q + x*x)^(1/6)*Math.Cos(Math.Atan(x/q)/3 + Math.PI*2/3) + p;
x_3 = Math.Sign(q)*2*(q*q + x*x)^(1/6)*Math.Cos(Math.Atan(x/q)/3 + Math.PI*4/3) + p;
}
You can compute ( )^(1/6) with Math.Pow( , 1/6) or your Math.Sqrt(CubicRoot( )) or Math.Sqrt(Cbrt( )). See the following thread on Microsoft forum.
Be careful with q = 0. ( Atan(x/0) = pi/2 radians. Cos(Atan(x/0)/3) = Sqrt(3)/2 )

For a quadratic equation there exists atleast 1 single real root.
Use this method for finding the roots http://en.wikipedia.org/wiki/Cubic_function#General_formula_of_roots

C# solving system of quadratic equations

How do you solve this type of equation?
a *X + b * Y + c *Z = q
d *X + e * Y + f *Z = w
X *X + Y * Y + Z *Z = z
We are looking for X,Y,Z. If not the squares in the last row this could be solved as a typical linear equation, for example using Linear Equations from Dot Numerics, or writing Gauss Elimination.
But how do I solve this one? Also, do you know any libraries in .NET that solves that equation?

This may be viewed as a set of equations for 2 planes and a sphere. The solution finds the intersection of the 2 planes (a line) and then the intersection of that line with the sphere.
There may be 0, 1, or 2 unique solutions.
The code is C, but I assume OP can readily translate to c#
// Eq1: a *X + b * Y + c *Z = q
// Eq2: d *X + e * Y + f *Z = w
// Eq3: X *X + Y * Y + Z *Z = z
typedef struct {
double x,y,z,s;
} plane_t;
typedef struct {
double x,y,z;
} point_t;
int Interection_PlanePlaneSphere(point_t XYZ[2], const plane_t *abc, const plane_t *def, double radius) {
// Find intersection of 2 planes
// V = abc cross def
point_t V; // This is really 3D vector, not a point
V.x = abc->y*def->z - abc->z*def->y;
V.y = abc->z*def->x - abc->x*def->z;
V.z = abc->x*def->y - abc->y*def->x;
// printf("V (%12g, %12g, %12g)\n", V.x, V.y, V.z);
// Assume both planes go through z plane, e.g. z = 0 and V.z != 0
// Code could be adapted to not depend on this assumption.
// abc->x*P.x + abc->y*P.y = abc->s
// def->x*P.x + def->y*P.y = def->s
double det = abc->x*def->y - abc->y*def->x;
// if Planes are parallel ...
// Code could be adapted to deal with special case where planes are coincident.
if (det == 0.0) return 0; //
point_t P;
P.x = ( abc->s*def->y - def->s*abc->y)/det;
P.y = (-abc->s*def->x + def->s*abc->x)/det;
P.z = 0.0;
// L(t) = P + V*t = intersection of the 2 planes
// printf("p (%12g, %12g, %12g)\n", P.x, P.y, P.z);
if (radius < 0) return 0; // bad sphere
// Find where L(t) is on the sphere, or |L(t)| = radius^2
// (P.x - V.x*t)^2 + (P.y - V.y*t)^2 + (P.z - V.z*t)^2 = radius^2
// (V.x^2 + V.y^2 + V.z^2)*t^2 - 2*(P.x*V.x + P.y*V.y + P.z*V.z) + (P.x^2 + P.y^2 + P.z^2) = radius^2;
// Solve quadratic
double a, b, c;
a = V.x*V.x + V.y*V.y + V.z*V.z;
b = -2*(P.x*V.x + P.y*V.y + P.z*V.z);
c = P.x*P.x + P.y*P.y + P.z*P.z - radius*radius;
// printf("abc (%12g, %12g, %12g)\n", a, b, c);
det = b*b - 4*a*c;
if (det < 0) return 0; // no solutions
det = sqrt(det);
double t;
t = (-b + det)/(2*a);
XYZ[0].x = P.x + t*V.x;
XYZ[0].y = P.y + t*V.y;
XYZ[0].z = P.z + t*V.z;
if (det == 0.0) return 1;
t = (-b - det)/(2*a);
XYZ[1].x = P.x + t*V.x;
XYZ[1].y = P.y + t*V.y;
XYZ[1].z = P.z + t*V.z;
return 2;
}
void Test() {
plane_t abcq = {2, -1, 1, 5};
plane_t defw = {1, 1, -1, 1};
double z = 100;
point_t XYZ[2];
int result = Interection_PlanePlaneSphere(XYZ, &abcq, &defw, sqrt(z));
printf("Result %d\n", result);
int i=0;
for (i=0; i<result; i++) {
printf("XYZ[%d] (%12g, %12g, %12g)\n", i, XYZ[i].x, XYZ[i].y, XYZ[i].z);
}
// Result 2
// XYZ[0] ( 2, 5.41014, 6.41014)
// XYZ[1] ( 2, -8.41014, -7.41014)
}

Curve fitting points in 3D space

Trying to find functions that will assist us to draw a 3D line through a series of points.
For each point we know: Date&Time, Latitude, Longitude, Altitude, Speed and Heading.
Data might be recorded every 10 seconds and we would like to be able to guestimate the points in between and increase granularity to 1 second. Thus creating a virtual flight path in 3D space.
I have found a number of curve fitting algorithms that will approximate a line through a series of points but they do not guarantee that the points are intersected. They also do not take into account speed and heading to determine the most likely path taken by the object to reach the next point.

From a physics viewpoint:
You have to assume something about the acceleration in your intermediate points to get the interpolation.
If your physical system is relatively well-behaved (as a car or a plane), as opposed to for example a bouncing ball, you may go ahead supposing an acceleration varying linearly with time between your points.
The vector equation for a constant varying accelerated movement is:
x''[t] = a t + b
where all magnitudes except t are vectors.
For each segment you already know v(t=t0) x(t=t0) tfinal and x(tfinal) v(tfinal)
By solving the differential equation you get:
Eq 1:
x[t_] := (3 b t^2 Tf + a t^3 Tf - 3 b t Tf^2 - a t Tf^3 - 6 t X0 + 6 Tf X0 + 6 t Xf)/(6 Tf)
And imposing the initial and final contraints for position and velocity you get:
Eqs 2:
a -> (6 (Tf^2 V0 - 2 T0 Tf Vf + Tf^2 Vf - 2 T0 X0 + 2 Tf X0 +
2 T0 Xf - 2 Tf Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2))
b -> (2 (-2 Tf^3 V0 + 3 T0^2 Tf Vf - Tf^3 Vf + 3 T0^2 X0 -
3 Tf^2 X0 - 3 T0^2 Xf + 3 Tf^2 Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2))}}
So inserting the values for eqs 2 into eq 1 you get the temporal interpolation for your points, based on the initial and final position and velocities.
HTH!
Edit
A few examples with abrupt velocity change in two dimensions (in 3D is exactly the same). If the initial and final speeds are similar, you'll get "straighter" paths.
Suppose:
X0 = {0, 0}; Xf = {1, 1};
T0 = 0; Tf = 1;
If
V0 = {0, 1}; Vf = {-1, 3};
V0 = {0, 1}; Vf = {-1, 5};
V0 = {0, 1}; Vf = {1, 3};
Here is an animation where you may see the speed changing from V0 = {0, 1} to Vf = {1, 5}:
Here you may see an accelerating body in 3D with positions taken at equal intervals:
Edit
A full problem:
For convenience, I'll work in Cartesian coordinates. If you want to convert from lat/log/alt to Cartesian just do:
x = rho sin(theta) cos(phi)
y = rho sin(theta) sin(phi)
z = rho cos(theta)
Where phi is the longitude, theta is the latitude, and rho is your altitude plus the radius of the Earth.
So suppose we start our segment at:
t=0 with coordinates (0,0,0) and velocity (1,0,0)
and end at
t=10 with coordinates (10,10,10) and velocity (0,0,1)
I clearly made a change in the origin of coordinates to set the origin at my start point. That is just for getting nice round numbers ...
So we replace those numbers in the formulas for a and b and get:
a = {-(3/50), -(3/25), -(3/50)} b = {1/5, 3/5, 2/5}
With those we go to eq 1, and the position of the object is given by:
p[t] = {1/60 (60 t + 6 t^2 - (3 t^3)/5),
1/60 (18 t^2 - (6 t^3)/5),
1/60 (12 t^2 - (3 t^3)/5)}
And that is it. You get the position from 1 to 10 secs replacing t by its valus in the equation above.
The animation runs:
Edit 2
If you don't want to mess with the vertical acceleration (perhaps because your "speedometer" doesn't read it), you could just assign a constant speed to the z axis (there is a very minor error for considering it parallel to the Rho axis), equal to (Zfinal - Zinit)/(Tf-T0), and then solve the problem in the plane forgetting the altitude.

What you're asking is a general interpolation problem. My guess is your actual problem isn't due to the curve-fitting algorithm being used, but rather your application of it to all discrete values recorded by the system instead of the relevant set of values.
Let's decompose your problem. You're currently drawing a point in spherically-mapped 3D space, adjusting for linear and curved paths. If we discretize the operations performed by an object with six degrees of freedom (roll, pitch, and yaw), the only operations you're particularly interested in are linear paths and curved paths accounting for pitch and yaw in any direction. Accounting for acceleration and deceleration also possible given understanding of basic physics.
Dealing with the spherical mapping is easy. Simply unwrap your points relative to their position on a plane, adjusting for latitude, longitude, and altitude. This should allow you to flatten data that would otherwise exist along a curved path, though this may not strictly be necessary for the solutions to your problem (see below).
Linear interpolation is easy. Given an arbitrary number of points backwards in time that fit a line within n error as determined by your system,* construct the line and compute the distance in time between each point. From here, attempt to fit the time points to one of two cases: constant velocity or constant acceleration.
Curve interpolation is a little more difficult, but still plausible. For cases of pitch, yaw, or combined pitch+yaw, construct a plane containing an arbitrary number of points backwards in time, within m error for curved readouts from your system.* From these data, construct a planar curve and once again account for constant velocity or acceleration along the curve.
You can do better than this by attempting to predict the expected operations of a plane in flight as part of a decision tree or neural network relative to the flight path. I'll leave that as an exercise for the reader.
Best of luck designing your system.
--
* Both error readouts are expected to be from GPS data, given the description of the problem. Accounting and adjusting for errors in these data is a separate interesting problem.

What you need (instead of modeling the physics) is to fit a spline through the data. I used a numerical recipies book (http://www.nrbook.com/a has free C and FORTRAN algorithms. Look into F77 section 3.3 to get the math needed). If you want to be simple then just fit lines through the points, but that will not result in a smooth flight path at all. Time will be your x value, and each parameter loged will have it's own cublic spline parameters.
Since we like long postings for this question here is the full code:
//driver program
static void Main(string[] args)
{
double[][] flight_data = new double[][] {
new double[] { 0, 10, 20, 30 }, // time in seconds
new double[] { 14500, 14750, 15000, 15125 }, //altitude in ft
new double[] { 440, 425, 415, 410 }, // speed in knots
};
CubicSpline altitude = new CubicSpline(flight_data[0], flight_data[1]);
CubicSpline speed = new CubicSpline(flight_data[0], flight_data[2]);
double t = 22; //Find values at t
double h = altitude.GetY(t);
double v = speed.GetY(t);
double ascent = altitude.GetYp(t); // ascent rate in ft/s
}
// Cubic spline definition
using System.Linq;
/// <summary>
/// Cubic spline interpolation for tabular data
/// </summary>
/// <remarks>
/// Adapted from numerical recipies for FORTRAN 77
/// (ISBN 0-521-43064-X), page 110, section 3.3.
/// Function spline(x,y,yp1,ypn,y2) converted to
/// C# by jalexiou, 27 November 2007.
/// Spline integration added also Nov 2007.
/// </remarks>
public class CubicSpline
{
double[] xi;
double[] yi;
double[] yp;
double[] ypp;
double[] yppp;
double[] iy;
#region Constructors
public CubicSpline(double x_min, double x_max, double[] y)
: this(Sequence(x_min, x_max, y.Length), y)
{ }
public CubicSpline(double x_min, double x_max, double[] y, double yp1, double ypn)
: this(Sequence(x_min, x_max, y.Length), y, yp1, ypn)
{ }
public CubicSpline(double[] x, double[] y)
: this(x, y, double.NaN, double.NaN)
{ }
public CubicSpline(double[] x, double[] y, double yp1, double ypn)
{
if( x.Length == y.Length )
{
int N = x.Length;
xi = new double[N];
yi = new double[N];
x.CopyTo(xi, 0);
y.CopyTo(yi, 0);
if( N > 0 )
{
double p, qn, sig, un;
ypp = new double[N];
double[] u = new double[N];
if( double.IsNaN(yp1) )
{
ypp[0] = 0;
u[0] = 0;
}
else
{
ypp[0] = -0.5;
u[0] = (3 / (xi[1] - xi[0])) *
((yi[1] - yi[0]) / (x[1] - x[0]) - yp1);
}
for (int i = 1; i < N-1; i++)
{
double hp = x[i] - x[i - 1];
double hn = x[i + 1] - x[i];
sig = hp / hn;
p = sig * ypp[i - 1] + 2.0;
ypp[i] = (sig - 1.0) / p;
u[i] = (6 * ((y[i + 1] - y[i]) / hn) - (y[i] - y[i - 1]) / hp)
/ (hp + hn) - sig * u[i - 1] / p;
}
if( double.IsNaN(ypn) )
{
qn = 0;
un = 0;
}
else
{
qn = 0.5;
un = (3 / (x[N - 1] - x[N - 2])) *
(ypn - (y[N - 1] - y[N - 2]) / (x[N - 1] - x[N - 2]));
}
ypp[N - 1] = (un - qn * u[N - 2]) / (qn * ypp[N - 2] + 1.0);
for (int k = N-2; k > 0; k--)
{
ypp[k] = ypp[k] * ypp[k + 1] + u[k];
}
// Calculate 1st derivatives
yp = new double[N];
double h;
for( int i = 0; i < N - 1; i++ )
{
h = xi[i + 1] - xi[i];
yp[i] = (yi[i + 1] - yi[i]) / h
- h / 6 * (ypp[i + 1] + 2 * ypp[i]);
}
h = xi[N - 1] - xi[N - 2];
yp[N - 1] = (yi[N - 1] - yi[N - 2]) / h
+ h / 6 * (2 * ypp[N - 1] + ypp[N - 2]);
// Calculate 3rd derivatives as average of dYpp/dx
yppp = new double[N];
double[] jerk_ij = new double[N - 1];
for( int i = 0; i < N - 1; i++ )
{
h = xi[i + 1] - xi[i];
jerk_ij[i] = (ypp[i + 1] - ypp[i]) / h;
}
Yppp = new double[N];
yppp[0] = jerk_ij[0];
for( int i = 1; i < N - 1; i++ )
{
yppp[i] = 0.5 * (jerk_ij[i - 1] + jerk_ij[i]);
}
yppp[N - 1] = jerk_ij[N - 2];
// Calculate Integral over areas
iy = new double[N];
yi[0] = 0; //Integration constant
for( int i = 0; i < N - 1; i++ )
{
h = xi[i + 1] - xi[i];
iy[i + 1] = h * (yi[i + 1] + yi[i]) / 2
- h * h * h / 24 * (ypp[i + 1] + ypp[i]);
}
}
else
{
yp = new double[0];
ypp = new double[0];
yppp = new double[0];
iy = new double[0];
}
}
else
throw new IndexOutOfRangeException();
}
#endregion
#region Actions/Functions
public int IndexOf(double x)
{
//Use bisection to find index
int i1 = -1;
int i2 = Xi.Length;
int im;
double x1 = Xi[0];
double xn = Xi[Xi.Length - 1];
bool ascending = (xn >= x1);
while( i2 - i1 > 1 )
{
im = (i1 + i2) / 2;
double xm = Xi[im];
if( ascending & (x >= Xi[im]) ) { i1 = im; } else { i2 = im; }
}
if( (ascending && (x <= x1)) || (!ascending & (x >= x1)) )
{
return 0;
}
else if( (ascending && (x >= xn)) || (!ascending && (x <= xn)) )
{
return Xi.Length - 1;
}
else
{
return i1;
}
}
public double GetIntY(double x)
{
int i = IndexOf(x);
double x1 = xi[i];
double x2 = xi[i + 1];
double y1 = yi[i];
double y2 = yi[i + 1];
double y1pp = ypp[i];
double y2pp = ypp[i + 1];
double h = x2 - x1;
double h2 = h * h;
double a = (x-x1)/h;
double a2 = a*a;
return h / 6 * (3 * a * (2 - a) * y1
+ 3 * a2 * y2 - a2 * h2 * (a2 - 4 * a + 4) / 4 * y1pp
+ a2 * h2 * (a2 - 2) / 4 * y2pp);
}
public double GetY(double x)
{
int i = IndexOf(x);
double x1 = xi[i];
double x2 = xi[i + 1];
double y1 = yi[i];
double y2 = yi[i + 1];
double y1pp = ypp[i];
double y2pp = ypp[i + 1];
double h = x2 - x1;
double h2 = h * h;
double A = 1 - (x - x1) / (x2 - x1);
double B = 1 - A;
return A * y1 + B * y2 + h2 / 6 * (A * (A * A - 1) * y1pp
+ B * (B * B - 1) * y2pp);
}
public double GetYp(double x)
{
int i = IndexOf(x);
double x1 = xi[i];
double x2 = xi[i + 1];
double y1 = yi[i];
double y2 = yi[i + 1];
double y1pp = ypp[i];
double y2pp = ypp[i + 1];
double h = x2 - x1;
double A = 1 - (x - x1) / (x2 - x1);
double B = 1 - A;
return (y2 - y1) / h + h / 6 * (y2pp * (3 * B * B - 1)
- y1pp * (3 * A * A - 1));
}
public double GetYpp(double x)
{
int i = IndexOf(x);
double x1 = xi[i];
double x2 = xi[i + 1];
double y1pp = ypp[i];
double y2pp = ypp[i + 1];
double h = x2 - x1;
double A = 1 - (x - x1) / (x2 - x1);
double B = 1 - A;
return A * y1pp + B * y2pp;
}
public double GetYppp(double x)
{
int i = IndexOf(x);
double x1 = xi[i];
double x2 = xi[i + 1];
double y1pp = ypp[i];
double y2pp = ypp[i + 1];
double h = x2 - x1;
return (y2pp - y1pp) / h;
}
public double Integrate(double from_x, double to_x)
{
if( to_x < from_x ) { return -Integrate(to_x, from_x); }
int i = IndexOf(from_x);
int j = IndexOf(to_x);
double x1 = xi[i];
double xn = xi[j];
double z = GetIntY(to_x) - GetIntY(from_x); // go to nearest nodes (k) (j)
for( int k = i + 1; k <= j; k++ )
{
z += iy[k]; // fill-in areas in-between
}
return z;
}
#endregion
#region Properties
public bool IsEmpty { get { return xi.Length == 0; } }
public double[] Xi { get { return xi; } set { xi = value; } }
public double[] Yi { get { return yi; } set { yi = value; } }
public double[] Yp { get { return yp; } set { yp = value; } }
public double[] Ypp { get { return ypp; } set { ypp = value; } }
public double[] Yppp { get { return yppp; } set { yppp = value; } }
public double[] IntY { get { return yp; } set { iy = value; } }
public int Count { get { return xi.Length; } }
public double X_min { get { return xi.Min(); } }
public double X_max { get { return xi.Max(); } }
public double Y_min { get { return yi.Min(); } }
public double Y_max { get { return yi.Max(); } }
#endregion
#region Helpers
static double[] Sequence(double x_min, double x_max, int double_of_points)
{
double[] res = new double[double_of_points];
for (int i = 0; i < double_of_points; i++)
{
res[i] = x_min + (double)i / (double)(double_of_points - 1) * (x_max - x_min);
}
return res;
}
#endregion
}

You can find an approximation of a line that intersects points in 3d and 2d space using a Hough Transformation algorithm. I am only familiar with it's uses in 2d however but it will still work for 3d spaces given that you know what kind of line you are looking for. There is a basic implementation description linked. You can Google for pre-mades and here is a link to a 2d C implementation CImg.
The algorithm process (roughly)... First you find equation of a line that you think will best approximate the shape of the line (in 2d parabolic, logarithmic, exponential, etc). You take that formula and solve for one of the parameters.
y = ax + b
becomes
b = y - ax
Next, for each point you are attempting to match, you plugin the points to the y and x values. With 3 points, you would have 3 separate functions of b with respect to a.
(2, 3) : b = 3 - 2a
(4, 1) : b = 1 - 4a
(10, -5): b = -5 - 10a
Next, the theory is that you find all possible lines which pass through each of the points, which is infinitely many for each individual point however when combined in an accumulator space only a few possible parameters best fit. In practice this is done by choosing a range space for the parameters (I chose -2 <= a <= 1, 1 <= b <= 6) and begin plugging in values for the variant parameter(s) and solving for the other. You tally up the number of intersections from each function in an accumulator. The points with the highest values give you your parameters.
Accumulator after processing b = 3 - 2a
a: -2 -1 0 1
b: 1
2
3 1
4
5 1
6
Accumulator after processing b = 1 - 4a
a: -2 -1 0 1
b: 1 1
2
3 1
4
4
5 2
6
Accumulator after processing b = -5 - 10a
a: -2 -1 0 1
b: 1 1
2
3 1
4
5 3
6
The parameter set with the highest accumulated value is (b a) = (5 -1) and the function best fit to the points given is y = 5 - x.
Best of luck.

My guess is that a serious application of this would use a http://en.wikipedia.org/wiki/Kalman_filter. By the way, that probably wouldn't guarantee that the reported points were intersected either, unless you fiddled with the parameters a bit. It would expect some degree of error in each data point given to it, so where it thinks the object is at time T would not necessarily be where it was at time T. Of course, you could set the error distribution to say that you were absolutely sure you knew where it was at time T.
Short of using a Kalman filter, I would try and turn it into an optimisation problem. Work at the 1s granularity and write down equations like
x_t' = x_t + (Vx_t + Vx_t')/2 + e,
Vx_t_reported = Vx_t + f,
Vx_t' = Vx_t + g
where e, f, and g represent the noise. Then create a penalty function such as e^2 + f^2 + g^2 +...
or some weighted version such as 1.5e^2 + 3f^2 + 2.6g^2 +... according to your idea of what the errors really are and how smooth you wnat the answer to be, and find the values that make the penalty function as small as possible - with least squares if the equations turn out nicely.