Related
I have 3 very large signed integers.
long x = long.MaxValue;
long y = long.MaxValue - 1;
long z = long.MaxValue - 2;
I want to calculate their truncated average. Expected average value is long.MaxValue - 1, which is 9223372036854775806.
It is impossible to calculate it as:
long avg = (x + y + z) / 3; // 3074457345618258600
Note: I read all those questions about average of 2 numbers, but I don't see how that technique can be applied to average of 3 numbers.
It would be very easy with the usage of BigInteger, but let's assume I cannot use it.
BigInteger bx = new BigInteger(x);
BigInteger by = new BigInteger(y);
BigInteger bz = new BigInteger(z);
BigInteger bavg = (bx + by + bz) / 3; // 9223372036854775806
If I convert to double, then, of course, I lose precision:
double dx = x;
double dy = y;
double dz = z;
double davg = (dx + dy + dz) / 3; // 9223372036854780000
If I convert to decimal, it works, but also let's assume that I cannot use it.
decimal mx = x;
decimal my = y;
decimal mz = z;
decimal mavg = (mx + my + mz) / 3; // 9223372036854775806
Question: Is there a way to calculate the truncated average of 3 very large integers only with the usage of long type? Don't consider that question as C#-specific, just it is easier for me to provide samples in C#.
This code will work, but isn't that pretty.
It first divides all three values (it floors the values, so you 'lose' the remainder), and then divides the remainder:
long n = x / 3
+ y / 3
+ z / 3
+ ( x % 3
+ y % 3
+ z % 3
) / 3
Note that the above sample does not always work properly when having one or more negative values.
As discussed with Ulugbek, since the number of comments are exploding below, here is the current BEST solution for both positive and negative values.
Thanks to answers and comments of Ulugbek Umirov, James S, KevinZ, Marc van Leeuwen, gnasher729 this is the current solution:
static long CalculateAverage(long x, long y, long z)
{
return (x % 3 + y % 3 + z % 3 + 6) / 3 - 2
+ x / 3 + y / 3 + z / 3;
}
static long CalculateAverage(params long[] arr)
{
int count = arr.Length;
return (arr.Sum(n => n % count) + count * (count - 1)) / count - (count - 1)
+ arr.Sum(n => n / count);
}
NB - Patrick has already given a great answer. Expanding on this you could do a generic version for any number of integers like so:
long x = long.MaxValue;
long y = long.MaxValue - 1;
long z = long.MaxValue - 2;
long[] arr = { x, y, z };
var avg = arr.Select(i => i / arr.Length).Sum()
+ arr.Select(i => i % arr.Length).Sum() / arr.Length;
Patrick Hofman has posted a great solution. But if needed it can still be implemented in several other ways. Using the algorithm here I have another solution. If implemented carefully it may be faster than the multiple divisions in systems with slow hardware divisors. It can be further optimized by using divide by constants technique from hacker's delight
public class int128_t {
private int H;
private long L;
public int128_t(int h, long l)
{
H = h;
L = l;
}
public int128_t add(int128_t a)
{
int128_t s;
s.L = L + a.L;
s.H = H + a.H + (s.L < a.L);
return b;
}
private int128_t rshift2() // right shift 2
{
int128_t r;
r.H = H >> 2;
r.L = (L >> 2) | ((H & 0x03) << 62);
return r;
}
public int128_t divideby3()
{
int128_t sum = {0, 0}, num = new int128_t(H, L);
while (num.H || num.L > 3)
{
int128_t n_sar2 = num.rshift2();
sum = add(n_sar2, sum);
num = add(n_sar2, new int128_t(0, num.L & 3));
}
if (num.H == 0 && num.L == 3)
{
// sum = add(sum, 1);
sum.L++;
if (sum.L == 0) sum.H++;
}
return sum;
}
};
int128_t t = new int128_t(0, x);
t = t.add(new int128_t(0, y));
t = t.add(new int128_t(0, z));
t = t.divideby3();
long average = t.L;
In C/C++ on 64-bit platforms it's much easier with __int128
int64_t average = ((__int128)x + y + z)/3;
You can calculate the mean of numbers based on the differences between the numbers rather than using the sum.
Let's say x is the max, y is the median, z is the min (as you have). We will call them max, median and min.
Conditional checker added as per #UlugbekUmirov's comment:
long tmp = median + ((min - median) / 2); //Average of min 2 values
if (median > 0) tmp = median + ((max - median) / 2); //Average of max 2 values
long mean;
if (min > 0) {
mean = min + ((tmp - min) * (2.0 / 3)); //Average of all 3 values
} else if (median > 0) {
mean = min;
while (mean != tmp) {
mean += 2;
tmp--;
}
} else if (max > 0) {
mean = max;
while (mean != tmp) {
mean--;
tmp += 2;
}
} else {
mean = max + ((tmp - max) * (2.0 / 3));
}
Patching Patrick Hofman's solution with supercat's correction, I give you the following:
static Int64 Avg3 ( Int64 x, Int64 y, Int64 z )
{
UInt64 flag = 1ul << 63;
UInt64 x_ = flag ^ (UInt64) x;
UInt64 y_ = flag ^ (UInt64) y;
UInt64 z_ = flag ^ (UInt64) z;
UInt64 quotient = x_ / 3ul + y_ / 3ul + z_ / 3ul
+ ( x_ % 3ul + y_ % 3ul + z_ % 3ul ) / 3ul;
return (Int64) (quotient ^ flag);
}
And the N element case:
static Int64 AvgN ( params Int64 [ ] args )
{
UInt64 length = (UInt64) args.Length;
UInt64 flag = 1ul << 63;
UInt64 quotient_sum = 0;
UInt64 remainder_sum = 0;
foreach ( Int64 item in args )
{
UInt64 uitem = flag ^ (UInt64) item;
quotient_sum += uitem / length;
remainder_sum += uitem % length;
}
return (Int64) ( flag ^ ( quotient_sum + remainder_sum / length ) );
}
This always gives the floor() of the mean, and eliminates every possible edge case.
Because C uses floored division rather than Euclidian division, it may easier to compute a properly-rounded average of three unsigned values than three signed ones. Simply add 0x8000000000000000UL to each number before taking the unsigned average, subtract it after taking the result, and use an unchecked cast back to Int64 to get a signed average.
To compute the unsigned average, compute the sum of the top 32 bits of the three values. Then compute the sum of the bottom 32 bits of the three values, plus the sum from above, plus one [the plus one is to yield a rounded result]. The average will be 0x55555555 times the first sum, plus one third of the second.
Performance on 32-bit processors might be enhanced by producing three "sum" values each of which is 32 bits long, so that the final result is ((0x55555555UL * sumX)<<32) + 0x55555555UL * sumH + sumL/3; it might possibly be further enhanced by replacing sumL/3 with ((sumL * 0x55555556UL) >> 32), though the latter would depend upon the JIT optimizer [it might know how to replace a division by 3 with a multiply, and its code might actually be more efficient than an explicit multiply operation].
If you know you have N values, can you just divide each value by N and sum them together?
long GetAverage(long* arrayVals, int n)
{
long avg = 0;
long rem = 0;
for(int i=0; i<n; ++i)
{
avg += arrayVals[i] / n;
rem += arrayVals[i] % n;
}
return avg + (rem / n);
}
You could use the fact that you can write each of the numbers as y = ax + b, where x is a constant. Each a would be y / x (the integer part of that division). Each b would be y % x (the rest/modulo of that division). If you choose this constant in an intelligent way, for example by choosing the square root of the maximum number as a constant, you can get the average of x numbers without having problems with overflow.
The average of an arbitrary list of numbers can be found by finding:
( ( sum( all A's ) / length ) * constant ) +
( ( sum( all A's ) % length ) * constant / length) +
( ( sum( all B's ) / length )
where % denotes modulo and / denotes the 'whole' part of division.
The program would look something like:
class Program
{
static void Main()
{
List<long> list = new List<long>();
list.Add( long.MaxValue );
list.Add( long.MaxValue - 1 );
list.Add( long.MaxValue - 2 );
long sumA = 0, sumB = 0;
long res1, res2, res3;
//You should calculate the following dynamically
long constant = 1753413056;
foreach (long num in list)
{
sumA += num / constant;
sumB += num % constant;
}
res1 = (sumA / list.Count) * constant;
res2 = ((sumA % list.Count) * constant) / list.Count;
res3 = sumB / list.Count;
Console.WriteLine( res1 + res2 + res3 );
}
}
I also tried it and come up with a faster solution (although only by a factor about 3/4). It uses a single division
public static long avg(long a, long b, long c) {
final long quarterSum = (a>>2) + (b>>2) + (c>>2);
final long lowSum = (a&3) + (b&3) + (c&3);
final long twelfth = quarterSum / 3;
final long quarterRemainder = quarterSum - 3*twelfth;
final long adjustment = smallDiv3(lowSum + 4*quarterRemainder);
return 4*twelfth + adjustment;
}
where smallDiv3 is division by 3 using multipliation and working only for small arguments
private static long smallDiv3(long n) {
assert -30 <= n && n <= 30;
// Constants found rather experimentally.
return (64/3*n + 10) >> 6;
}
Here is the whole code including a test and a benchmark, the results are not that impressive.
This function computes the result in two divisions. It should generalize nicely to other divisors and word sizes.
It works by computing the double-word addition result, then working out the division.
Int64 average(Int64 a, Int64 b, Int64 c) {
// constants: 0x10000000000000000 div/mod 3
const Int64 hdiv3 = UInt64(-3) / 3 + 1;
const Int64 hmod3 = UInt64(-3) % 3;
// compute the signed double-word addition result in hi:lo
UInt64 lo = a; Int64 hi = a>=0 ? 0 : -1;
lo += b; hi += b>=0 ? lo<b : -(lo>=UInt64(b));
lo += c; hi += c>=0 ? lo<c : -(lo>=UInt64(c));
// divide, do a correction when high/low modulos add up
return hi>=0 ? lo/3 + hi*hdiv3 + (lo%3 + hi*hmod3)/3
: lo/3+1 + hi*hdiv3 + Int64(lo%3-3 + hi*hmod3)/3;
}
Math
(x + y + z) / 3 = x/3 + y/3 + z/3
(a[1] + a[2] + .. + a[k]) / k = a[1]/k + a[2]/k + .. + a[k]/k
Code
long calculateAverage (long a [])
{
double average = 0;
foreach (long x in a)
average += (Convert.ToDouble(x)/Convert.ToDouble(a.Length));
return Convert.ToInt64(Math.Round(average));
}
long calculateAverage_Safe (long a [])
{
double average = 0;
double b = 0;
foreach (long x in a)
{
b = (Convert.ToDouble(x)/Convert.ToDouble(a.Length));
if (b >= (Convert.ToDouble(long.MaxValue)-average))
throw new OverflowException ();
average += b;
}
return Convert.ToInt64(Math.Round(average));
}
Try this:
long n = Array.ConvertAll(new[]{x,y,z},v=>v/3).Sum()
+ (Array.ConvertAll(new[]{x,y,z},v=>v%3).Sum() / 3);
I am wondering if this could be some kind of associativity problem, because when I do the problem on paper, I get the correct answer, but when I run the code I keep getting 4 over and over. Here is the code. Why aren't these equal? What am I missing?
The whole problem (returns 4 on every iteration):
for (int x = 1; x <= stackCount; x++) {
temp = ((x - 1) / stackCount * uBound) + lBound + 1;
Base[x] = Top[x] = Convert.ToInt32(Math.Floor(temp));
}
Broken into pieces (runs correctly):
double temp, temp1, temp2, temp3, temp4;
for (int x = 1; x <= stackCount; x++) {
temp1 = (x - 1);
temp2 = temp1 / stackCount;
temp3 = temp2 * uBound;
temp4 = temp3 + lBound + 1;
Base[x] = Top[x] = Convert.ToInt32(Math.Floor(temp4));
}
Added:
Yes, I am sorry, I forgot about that declarations:
//the main memory for the application
private string[] Memory;
//arrays to keep track of the bottom and top of stacks
private int[] Base;
private int[] Top;
//keep track of the upper and lower bounds and usable size
private int LowerBound;
private int UpperBound;
private int usableSize;
I also think I had that backwards. I thought that if you used a double in a division operation with integers that the result would be a double, but it appears that is not the case. That makes sense! Thank you all!
Speculation: stackCount, uBound, and lBound are all integers or longs.
Result: The entire expression is computed as though you're doing integer arithmetic.
Solution: temp = ((double)(x -1) / stackCount * uBound) + lBound + 1;
You haven't given us the full code. In particular, the declarations for stackCount, uBound and lBound and temp have all been omitted. You've also omitted the values of the first 3.
If, as seems likely, all the bits involved in your expression
((x - 1) / stackCount * uBound) + lBound + 1;
are integral types, the result will also be an integral type since integer division is performed:
int x = 9 ;
int y = 4 ;
double z = x / y ;
yields the expected double precision value 2.0.
((5 - 1) / 9 * 11) + 3 + 1
The particular integral type that the expression resolves two is depends on the various types involved and whether or not they are signed, and whether or not they are all compatible.
I am trying to convert a C++ class to C# and in the process learn something of C++. I had never run into a vector<> before and my understanding is this is like a List<> function in C#. During the conversion of the class I re-wrote the code using List futures_price = New List(Convert.ToInt32(no_steps) + 1);. As soon as I run the code, I get a "Index was out of range" error.
Having looked around on SOF, I believe the issue is regarding the parameter being out of index range relating to this, but I do not see a simple solution to solve this with the below code.
In particular, this is the line that is triggering the error: futures_prices[0] = spot_price * Math.Pow(d, no_steps);
Below is the full code:
public double futures_option_price_call_american_binomial(double spot_price, double option_strike, double r, double sigma, double time, double no_steps)
{
//double spot_price, // price futures contract
//double option_strike, // exercise price
//double r, // interest rate
//double sigma, // volatility
//double time, // time to maturity
//int no_steps
List<double> futures_prices = new List<double>(Convert.ToInt32(no_steps) + 1);
//(no_steps+1);
//double call_values = (no_steps+1);
List<double> call_values = new List<double>(Convert.ToInt32(no_steps) + 1);
double t_delta = time/no_steps;
double Rinv = Math.Exp(-r*(t_delta));
double u = Math.Exp(sigma * Math.Sqrt(t_delta));
double d = 1.0/u;
double uu= u*u;
double pUp = (1-d)/(u-d); // note how probability is calculated
double pDown = 1.0 - pUp;
futures_prices[0] = spot_price * Math.Pow(d, no_steps);
int i;
for (i=1; i<=no_steps; ++i) futures_prices[i] = uu*futures_prices[i-1]; // terminal tree nodes
for (i=0; i<=no_steps; ++i) call_values[i] = Math.Max(0.0, (futures_prices[i]-option_strike));
for (int step = Convert.ToInt32(no_steps) - 1; step >= 0; --step)
{
for (i = 0; i <= step; ++i)
{
futures_prices[i] = d * futures_prices[i + 1];
call_values[i] = (pDown * call_values[i] + pUp * call_values[i + 1]) * Rinv;
call_values[i] = Math.Max(call_values[i], futures_prices[i] - option_strike); // check for exercise
};
};
return call_values[0];
}
Here is the original source in C++:
double futures_option_price_call_american_binomial(const double& F, // price futures contract
const double& K, // exercise price
const double& r, // interest rate
const double& sigma, // volatility
const double& time, // time to maturity
const int& no_steps) { // number of steps
vector<double> futures_prices(no_steps+1);
vector<double> call_values (no_steps+1);
double t_delta= time/no_steps;
double Rinv = exp(-r*(t_delta));
double u = exp(sigma*sqrt(t_delta));
double d = 1.0/u;
double uu= u*u;
double pUp = (1-d)/(u-d); // note how probability is calculated
double pDown = 1.0 - pUp;
futures_prices[0] = F*pow(d, no_steps);
int i;
for (i=1; i<=no_steps; ++i) futures_prices[i] = uu*futures_prices[i-1]; // terminal tree nodes
for (i=0; i<=no_steps; ++i) call_values[i] = max(0.0, (futures_prices[i]-K));
for (int step=no_steps-1; step>=0; --step) {
for (i=0; i<=step; ++i) {
futures_prices[i] = d*futures_prices[i+1];
call_values[i] = (pDown*call_values[i]+pUp*call_values[i+1])*Rinv;
call_values[i] = max(call_values[i], futures_prices[i]-K); // check for exercise
};
};
return call_values[0];
};
A List<double> starts out empty until you add items to it. (passing the constructor argument just sets the capacity, preventing costly resizes)
You can't access [0] until you Add() it.
To use it the way you are, use an array instead.
As SLaks says, it's better to use an Array in this situation. C# lists are filled with Add method and values are removed through Remove method... this would be more complicated and memory/performance expensive as you are also replacing values.
public Double FuturesOptionPriceCallAmericanBinomial(Double spotPrice, Double optionStrike, Double r, Double sigma, Double time, Double steps)
{
// Avoid calling Convert multiple times as it can be quite performance expensive.
Int32 stepsInteger = Convert.ToInt32(steps);
Double[] futurePrices = new Double[(stepsInteger + 1)];
Double[] callValues = new Double[(stepsInteger + 1)];
Double tDelta = time / steps;
Double rInv = Math.Exp(-r * (tDelta));
Double u = Math.Exp(sigma * Math.Sqrt(tDelta));
Double d = 1.0 / u;
Double uu = u * u;
Double pUp = (1 - d) / (u - d);
Double pDown = 1.0 - pUp;
futurePrices[0] = spotPrice * Math.Pow(d, steps);
for (Int32 i = 1; i <= steps; ++i)
futurePrices[i] = uu * futurePrices[(i - 1)];
for (Int32 i = 0; i <= steps; ++i)
callValues[i] = Math.Max(0.0, (futurePrices[i] - optionStrike));
for (Int32 step = stepsInteger - 1; step >= 0; --step)
{
for (Int32 i = 0; i <= step; ++i)
{
futurePrices[i] = d * futurePrices[(i + 1)];
callValues[i] = ((pDown * callValues[i]) + (pUp * callValues[i + 1])) * rInv;
callValues[i] = Math.Max(callValues[i], (futurePrices[i] - option_strike));
}
}
return callValues[0];
}
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.
screenshot of debug: http://img1.uploadscreenshot.com/images/orig/12/36121481470-orig.jpg
note how x, y have values (i have no idea why x and y stopped on 69 in the for loop - x should've went up to 86 and y to 183) yet r has no value at all. (the variable doesn't exist? what?) how should I fix this?
code if you want to read:
public float[] cht(int[,] matrix)
{
float[] xyrd = new float[4];
int xthreshold, ythreshold;
float slope;
double dir;
float zone;
int[] limitsStorage = new int[3] { matrix.GetLength(0), matrix.GetLength(1), matrix.GetLength(0) / 2 - 10 };
short[,,] accumulator = new short[limitsStorage[0]+1, limitsStorage[1]+1,limitsStorage[2]+1];
for (int x = 0; x < limitsStorage[0]; x++)
{ //first dimension loop of matrix 100
for (int y = 0; y < limitsStorage[1]; y++)
{ //second dimension loop of matrix 120
if (matrix[x, y] == 225)
{//the data at the for loop location is a 1 and not 0 hit.
xthreshold = x - limitsStorage[0] / 2;
ythreshold = y - limitsStorage[1] / 2;
//forget angle, search slope: float angle = xthreshold > 0 ? ((float)Math.Atan2(xthreshold, ythreshold)) : ((float)Math.Atan2(xthreshold, ythreshold) + 180);
slope = xthreshold / ythreshold;
//initiate if loops.
dir = 180 + Math.Round(Math.Atan2(ythreshold, xthreshold) * 57.2957 / 45, 0) * 45 + 45 * (Math.Round(((Math.Atan2(ythreshold, xthreshold) * 57.2957) % 45) / 45));
if (slope > .404 || slope < -.404)
{
if (slope < 2.3558 || slope > -2.3558)
{
if (xthreshold > 0)
{
if (ythreshold > 0)
{
//+x+y zone
zone = 45 + 180;
}
else
{
//+x-y zone
zone = 180 - 45;
}
}
else
{
if (ythreshold > 0)
{
//-x+y zone
zone = 360 - 45;
}
else
{
//-x-y zone
zone = 45;
}
}
}
else if (ythreshold > 0)
{
//+y zone
zone = 360 - 90;
}
else
{
//-y zone
zone = 90;
}
}
else if (xthreshold > 0)
{
//+x zone
zone = 180;
}
else
{
//-x zone
zone = 0;
}
for (int R = 6; R < limitsStorage[2]; R++)
{ //Radius loop for scan 44
float delta = (float)((1 / R) * 57.2957);
for (float Theta = zone - 25; Theta < zone + 25; Theta += delta)
{
accumulator[(int)(((R * Math.Cos(Theta / 57.2957)) < 0 || (R * Math.Cos(Theta / 57.2957)) > limitsStorage[0]) ? 0 : R * Math.Cos(Theta / 57.2957)), (int)(((R * Math.Sin(Theta / 57.2957)) < 0 || (R * Math.Sin(Theta / 57.2957)) > limitsStorage[1]) ? 0 : R * Math.Sin(Theta / 57.2957)),R]++;
//btw, 0,0,R is always the non-center area.
}
}
}
}
}
for (int x = 1; x < limitsStorage[0]; x++)
{
for (int y = 1; y < limitsStorage[1]; y++)
{
for (int r = 6; r < limitsStorage[2]; r++)
{
if (xyrd[3] > accumulator[x, y, r])
{
xyrd[0] = x;
xyrd[1] = y;
xyrd[2] = r;
xyrd[3] = accumulator[x, y, r];
}
}
}
}
if (accPrint)
{
//do something for debugging?
accPrint = false;
}
return xyrd;
}
I just noticed that the x and y have the little lock symbol under them indicating that you have private variables named x and y in the class in which this method is executing. Those are the x and y that you are seeing in the debugger.
r is appropriately out of scope as you've exited the loop in which it is declared.
By the way, x and y are ridiculously bad member variable names, and are ridiculously bad names for for loop variables of type int, especially if they are contained in a class with member variables named x and y.
The only place you declare r is in the for statement, right? That means r goes out of scope as soon as the loop ends. So naturally if you inspect variables at he end of the function, r won't be there.
Confessing I don't know why x and y are in scope based on the comments. They could be class variables, but the asker says not. That's the only explanation I can think of, though.
The behaviour is not weird -- you actually get exactly what you expect.
Please note that the watch window can only accurately show you values that are in scope at the breakpoint.
At the highlighted breakpoint, only accumulator[x, y, r] is in scope, and you see exactly the values you expected.