Develop Reference

Why is my angle of 2 vectors function return NaN even though i follow the formula

I'm making a function that calculates the angle between 2 given vectors for my unity game using the dot product formula:
vector(a)*vector(b)=|vector(a)|*|vector(b)|*cos(the angle)
so I figured that the angle would equals
acos((vector(a)*vector(b))/(|vector(a)|*|vector(b)|))
Anyway here's my code:
float rotateAngle(Vector2 a,Vector2 b)
{
return Mathf.Acos((a.x * b.x + a.y * b.y) / ((Mathf.Sqrt(a.x * a.x + a.y * a.y)) * (Mathf.Sqrt(b.x * b.x + b.y * b.y)))) * (180 / Mathf.PI);
}
But when i played it the console showed NaN. I've tried and reviewed the code and the formula but returned empty-handed.
Can someone help me? Thank you in advanced!!

float.NaN is the result of undefined (for real numbers) mathematical operations such as 0 / 0 (note from the docs that x / 0 where x != 0 rather returns positive or negative infinity) or the square root of a negative value. As soon as one operant in an operation already is NaN then also the entire operation returns again NaN.
The second (square root of a negative value) can not happen here since you are using squared values so most probably your vectors have a magnitude of 0.
If you look at the Vector2 source code you will find their implementation of Vector2.Angle or Vector2.SignedAngle (which you should rather use btw as they are tested and way more efficient).
public static float Angle(Vector2 from, Vector2 to)
{
// sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers
float denominator = (float)Math.Sqrt(from.sqrMagnitude * to.sqrMagnitude);
if (denominator < kEpsilonNormalSqrt)
return 0F;
float dot = Mathf.Clamp(Dot(from, to) / denominator, -1F, 1F);
return (float)Math.Acos(dot) * Mathf.Rad2Deg;
}
// Returns the signed angle in degrees between /from/ and /to/. Always returns the smallest possible angle
public static float SignedAngle(Vector2 from, Vector2 to)
{
float unsigned_angle = Angle(from, to);
float sign = Mathf.Sign(from.x * to.y - from.y * to.x);
return unsigned_angle * sign;
}
There you will find that the first thing they check is
float denominator = (float)Math.Sqrt(from.sqrMagnitude * to.sqrMagnitude);
if (denominator < kEpsilonNormalSqrt)
return 0F;
which basically makes exactly sure that both given vectors have a "big enough" magnitude, in particular one that is not 0 ;)
Long story short: Don't reinvent the wheel and rather use already built-in Vector2.Angle or Vector2.SignedAngle

NaN are typically the result of invalid mathematical operations on floating point numbers. A common source is division by zero, so my guess would be that the vector is 0,0.
I would also recommend using the built in functions for computing the normalization, Length/Magnitude, Dot etc. that will make the code much easier to read, and the compiler should be fairly good at optimizing that kind of code. If you need to do any additional optimization, only do so after you have done some measurements.

GPS lap & Segment timer

I've been searching for a while but haven't found exactly what I'm looking for.
I'm working on an app that will go in a race car. It will give the driver the ability to press a button to mark a Start/Finish line. It will also have a button to allow a driver to set segment times.
Keep in mind a track can be an oval which I'm working on first. It could be a road course or it could be an auto cross where the start and finish line aren't the exact same location. They could be with 50 feet of each other or so but the car never crosses where it starts.
I have my gps data coming in and I convert the NMea messages to my classes and I store Lat, Lon, Speed, Course etc. In my research I've ran across this which is interesting. The GPS will be mounted outside the roof for better signal. It generates 10 hits per second. (Garmin Glo)
http://www.drdobbs.com/windows/gps-programming-net/184405690?pgno=1
It's old but it talks about UTM and the Cartesian coordinate system. So using the DecDeg2UTM, I convert Lat & Lon to X & coordinates as well.
I've also been trying to use the Intersect formula I found Here I took the intersect and tried to convert it to C# which I'll post at the end. However, feeding coordinates of an oval track, it doesn't seem to be working. Also, I'm not sure exactly what it's supposed to be doing. But the coordinates it returns when it does somethign like -35.xxx & 98.xxxx which out in an ocean somewhere 1000's of miles from where the track is.
I looking for answers to the following.
I assume I need to take the location recorded when a button is pressed for Start/Finish or Segment and calculate a line perpendicular to the direction the car in able to be able to do some sort of Line Intersection calculation. The Cartesian coordinates seems to calculate the bearing fairly well. But the question here is how do you get the "left and right coordinates". Also, keep in mind, an oval track may be 60 feet wide. But as mentioned an auto cross track may only be 20 ft wide and part of the track may be with 50 ft. Note I'm fine with indicating to set the points, the car needs to be going slow or stopped at the points to get an accurate coordinate. Some tracks they will have to be set while walking the track.
Based on this, should I be trying to use decimal lat lon or would utilizing the Cartesian coordinate system based on UTM be a more accurate method for what I'm trying to do?
Either one is there a .Net library or C based library with source code that has methods for making these calculations?
How can this be accurately handled. (Not that great with Math, links to code samples would help tremendously.)
Next, after I have the lines or whatever is needed for start/finish and segments, as I get GPS sign from the car racing, I need to figure out the most accurate way to tell when a car has crossed those segments. again if I'm lucky I'll get 10 hits per second but it will probably be lower. Then the vehicle speeds could vary significantly depending on the type of track and vehicle. So the GPS hit could be many feet "left or right" of a segment. Also, it could be many feet before or after a segment.
Again, if there is a GIS library out there I can feed coordinates and all this is calculated, that's would work as well as long as it's performant. If not again I'm trying to decide if it's best to break down coordinates to X Y or some geometry formulas for coordinates in decimal format. Mods, I assume there is hard data to support an answer of either way and this isn't responses aren't fully subjective to opinions.
Here is the C# code I came up with from the Script page above. I'm starting to feel UTM and the Cartesian Coordinate system would be better for accuracy and performance. But again I'm open to evidence to the contrary if it exists.
Thanks
P.S. Note GeoCoordinate is from the .Net System.Device.Location assemble. GpsData is just a class I use to convert NMEA messages into Lat, Lon, Course, NumSats, DateTime etc.
The degree Radian methods are extensions as as follows.
public static double DegreeToRadians(this double angle)
{
return Math.PI * angle / 180.0;
}
public static double RadianToDegree(this double angle)
{
return angle * (180.0 / Math.PI);
}
}
public static GeoCoordinate CalculateIntersection(GpsData p1, double brng1, GpsData p2, double brng2)
{
// see http://williams.best.vwh.net/avform.htm#Intersection
// Not sure I need to use Cosine
double _p1LatRadians = p1.Latitude.DegreeToRadians();
double _p1LonToRadians = p1.Longitude.DegreeToRadians();
double _p2LatToRadians = p2.Latitude.DegreeToRadians();
double _p2LonToRadians = p2.Longitude.DegreeToRadians();
double _brng1ToRadians = brng1.DegreeToRadians();
double _brng2ToRadians = brng2.DegreeToRadians();
double _deltaLat = _p2LatToRadians - _p1LatRadians;
double _deltaLon = _p2LonToRadians - _p1LonToRadians;
var _var1 = 2 * Math.Asin(Math.Sqrt(Math.Sin(_deltaLat / 2) * Math.Sin(_deltaLat / 2)
+ Math.Cos(_p1LatRadians) * Math.Cos(_p2LatToRadians) * Math.Sin(_deltaLon / 2) * Math.Sin(_deltaLon / 2)));
if (_var1 == 0) return null;
// initial/final bearings between points
var _finalBrng = Math.Acos((Math.Sin(_p2LatToRadians) - Math.Sin(_p1LatRadians) * Math.Cos(_var1)) / (Math.Sin(_var1) * Math.Cos(_p1LatRadians)));
//if (isNaN(θa)) θa = 0; // protect against rounding
var θb = Math.Acos((Math.Sin(_p1LatRadians) - Math.Sin(_p2LatToRadians) * Math.Cos(_var1)) / (Math.Sin(_var1) * Math.Cos(_p2LatToRadians)));
var θ12 = Math.Sin(_p2LonToRadians - _p1LonToRadians) > 0 ? _finalBrng : 2 * Math.PI - _finalBrng;
var θ21 = Math.Sin(_p2LonToRadians - _p1LonToRadians) > 0 ? 2 * Math.PI - θb : θb;
var α1 = (_brng1ToRadians - θ12 + Math.PI) % (2 * Math.PI) - Math.PI; // angle 2-1-3
var α2 = (θ21 - _brng2ToRadians + Math.PI) % (2 * Math.PI) - Math.PI; // angle 1-2-3
if (Math.Sin(α1) == 0 && Math.Sin(α2) == 0) return null; // infinite intersections
if (Math.Sin(α1) * Math.Sin(α2) < 0) return null; // ambiguous intersection
α1 = Math.Abs(α1);
α2 = Math.Abs(α2);
// ... Ed Williams takes abs of α1/α2, but seems to break calculation?
var α3 = Math.Acos(-Math.Cos(α1) * Math.Cos(α2) + Math.Sin(α1) * Math.Sin(α2) * Math.Cos(_var1));
var δ13 = Math.Atan2(Math.Sin(_var1) * Math.Sin(α1) * Math.Sin(α2), Math.Cos(α2) + Math.Cos(α1) * Math.Cos(α3));
var _finalLatRadians = Math.Asin(Math.Sin(_p1LatRadians) * Math.Cos(δ13) + Math.Cos(_p1LatRadians) * Math.Sin(δ13) * Math.Cos(_brng1ToRadians));
var _lonBearing = Math.Atan2(Math.Sin(_brng1ToRadians) * Math.Sin(δ13) * Math.Cos(_p1LatRadians), Math.Cos(δ13) - Math.Sin(_p1LatRadians) * Math.Sin(_finalLatRadians));
var _finalLon = _p1LonToRadians + _lonBearing;
var _returnLat = _finalLatRadians.RadianToDegree();
var _latToDegree = _finalLon.RadianToDegree();
var _returnLon = ( _latToDegree + 540) % 360 - 180;
return new GeoCoordinate(_returnLat, _returnLon);
//return new LatLon(φ3.toDegrees(), (λ3.toDegrees() + 540) % 360 - 180); // normalise to −180..+180°
}

Calculate a specific curve with specific rotation, c#

I am trying to code for a game I am working on a specific curve with a specific rotation. I am not a great mathematician... At all... Tried searching for solutions for a few hours, but I'm affraid I do not find any solution.
So, a small picture to illustrate first:
This is an eighth of a circle, radius of 9, beggining is (0,0)
The end is now at about 6.364, -2.636. But I need this same curve, with a 45° direction at the end, but ending at aexactly 6.0,-3.0.
Could any of you show me how to do this? I need to be able to calculate precisly any point on this curve & its exact length. I would suppose using some kind of eliptical math could be a solution? I admit my math class are reaaaly far now and have now good clue for now...
Thank for any possible help

I think I found a quadratic curve which sastisfies your requirement:
f(x) = -1/12 x^2 + 9
Copy the following into https://www.desmos.com/calculator to see it:
-\frac{1}{12}x^2+9
f'(x) would be -1/6x, so when x=6, the derivative would be -1, which corresponds to a -45° inclination. There are probably infinite curves that satisfy your requirement but if my calculus isn't too rusty this is one of them.
I tried to fit an ellipse with foci starting at y=6 here and starting at y=9 here to your points but the slope doesn't look like 45°.
Also starting at any height k, here doesn't seem to work.

I don't think you've fully understood the question I asked in the comments about the "inclination" angle. So I will give a general case solution, where you have an explicit tangent vector for the end of the curve. (You can calculate this using the inclination angle; if we clarify what you mean by it then I will be happy to edit with a formula to calculate the tangent vector if necessary)
Let's draw a diagram of how the setup can look:
(Not 100% accurate)
A and B are your fixed points. T is the unit tangent vector. r and C are the radius and center of the arc we need to calculate.
The angle θ is given by the angle between BA and T minus π/2 radians (90 degrees). We can calculate it using the dot product:
The (signed) distance from the center of AB to C is given by:
Note that this is negative for the case on the right, and positive for the left. The radius is given by:
(You can simplify by substituting and using a cosine addition rule, but I prefer to keep things in terms of variables in the diagram). To obtain the point C, we need the perpendicular vector to AB (call it n):
Now that we have the radius and center of the circular arc, we still need to determine which direction we are moving in, i.e. whether we are moving clockwise or anti-clockwise when going from A to B. This is a simple test, using the cross-product:
If this is negative, then T is as in the diagram, and we need to move clockwise, and vice versa. The length of the arc l, and the angular displacement γ when we move by a distance x along the arc:
Nearly there! Just one more step - we need to work out how to rotate the point A by angle γ around point C, to get the point we want (call it D):
(Adapted from this Wikipedia page)
Now for some code, in case the above was confusing (it probably was!):
public Vector2 getPointOnArc(Vector2 A, Vector2 B, Vector2 T, double x)
{
// calculate preliminaries
Vector2 BA = B - A;
double d = BA.Length();
double theta = Math.Acos(Vector2.DotProduct(BA, T) / d) - Math.PI * 0.5;
// calculate radius
double r = d / (2.0 * Math.Cos(theta));
// calculate center
Vector2 n = new Vector2(BA.y, -BA.x);
Vector2 C = 0.5 * (A + B + n * Math.Tan(theta));
// calculate displacement angle from point A
double l = (Math.PI - 2.0 * theta) * r;
double gamma = (2.0 * Math.PI * x) / l;
// sign change as discussed
double cross = T.x * BA.y - T.y * BA.x;
if (cross < 0.0) gamma = -gamma;
// finally return the point we want
Vector2 disp = A - C;
double c_g = Math.Cos(gamma), s_g = Math.Sin(gamma);
return new Vector2(disp.X * c_g + disp.Y * s_g + C.X,
disp.Y * c_g - disp.X * s_g + C.Y);
}

How to find the distance between a point and a parabola in code

I am trying to find the closest point on a parabola to an arbitrary point in 2d, for a DirectX pixel shader.
A great amount of googling has revealed to me that this is a common pre-calculus homework problem. Unfortunately, the hundreds of relevant answers all say things like "Once you have this equation, use your graphing calculator's minimum function and it will tell you the answer is 6."
I confess that I recall nothing of pre-calculus. I recognize that the equation I seek is probably sitting right there on wikipedia, but I can't figure out how to convert these greek symbols into an HLSL function. A solution in C, C++, C#, or any other language would also be greatly appreciated.
edit: Per a request to see the format of the input curve:
//Equation of parabola being y = ax^2 + bx + c
//p is the arbitrary point we're trying to find the closest point on the parabola for.
float2 GetClosestPointOnParabola(float a, float b, float c, float2 p)
{
//Something involving the distance formula...
//Something involving "minimization"...
return float2(x, y);
}

You could make use of this:
Pmin = (xmin, ymin) ~ point on a parabola
P = (px, py) ~ point in 2d
y = a*x^2 + bx + c ~ parabola
P(x) = (x-px)^2 + (y-py)^2 = (x-px)^2 + (a*x^2 + bx + c - py)^2
You need to calculate the P(x) derivative, it's not that difficult. E.g.
If you get: P(x) = x^4 + 4x^2 - 3x + 10 the derivative would be:
P'(x) = 4x^3 + 8x - 3
I think you get how to calculate that. Then compare P'(x) to zero to find where it crossess an X-axis. You find an xmin from that and then you have ymin from:
y = a*x^2 + bx + c
That's it.

I assume what you want is the point on a parabola that is closest to another point in the plane. Let's assume the parabola is given by y = a * x^2 + b * x + c and that you want to find the point on it closest to the point A(xa, ya).
I would propose you use hill climbing. It finds a local minimum in a function with a logarithmic complexity. I will write example c++ code assuming there is a function h(x) that calculates the distance from A to the point with the point with x coordinate equal to x on the parabola.
double minDist() {
const double epsylon = 1e-9; // used to avoid double prescision errors
double current = 0.0;
double step = 1e+6;
while (step > 1e-5) { // change this with the accuracy you need
double left_neighbour = current - step;
double right_neighbour = current + step;
double cval = h(current);
double lval = h(left_neighbour);
double rval = h(right_neighbour);
if (cval < rval + epsylon && cval < lval + epsylon) {
step *= 0.5;
continue;
}
if (lval < rval) {
current = left_neighbour;
} else {
current = right_neighbour;
}
}
return current;
}
In most case you will have a single local mimimum that is the answer you need, but maybe there are cases where you have two(I believe they can not be more then 2). In these cases you need to start the function twice with different initial points.
Hope this helps.

Optimization of a distance calculation function

In my code I have to do a lot of distance calculation between pairs of lat/long values.
the code looks like this:
double result = Math.Acos(Math.Sin(lat2rad) * Math.Sin(lat1rad)
+ Math.Cos(lat2rad) * Math.Cos(lat1rad) * Math.Cos(lon2rad - lon1rad));
(lat2rad e.g. is latitude converted to radians).
I have identified this function as the performance bottleneck of my application. Is there any way to improve this?
(I cannot use look-up tables since the coordinates are varying). I have also looked at this question where a lookup scheme like a grid is suggested, which might be a possibility.
Thanks for your time! ;-)

If your goal is to rank (compare) distances, then approximations (sin and cos table lookups) could drastically reduce your amount of computations required (implement quick reject.)
Your goal is to only proceed with the actual trigonometric computation if the difference between the approximated distances (to be ranked or compared) falls below a certain threshold.
E.g. using lookup tables with 1000 samples (i.e. sin and cos sampled every 2*pi/1000), the lookup uncertainty is at most 0.006284. Using uncertainty calculation for the parameter to ACos, the cumulated uncertainty, also be the threshold uncertainty, will be at most 0.018731.
So, if evaluating Math.Sin(lat2rad) * Math.Sin(lat1rad)
+ Math.Cos(lat2rad) * Math.Cos(lat1rad) * Math.Cos(lon2rad - lon1rad) using sin and cos lookup tables for two coordinate-set pairs (distances) yields a certain ranking (one distance appears greater than the other based on the approximation), and the difference's modulus is greater than the threshold above, then the approximation is valid. Otherwise proceed with the actual trigonometric calculation.

Would the CORDIC algorithm work for you (in regards to speed/accuracy)?

Using inspiration from #Brann I think you can reduce the calculation a bit (Warning its a long time since I did any of this and it will need to be verified). Some sort of lookup of precalculated values probably the fastest though
You have :
1: ACOS( SIN A SIN B + COS A COS B COS(A-B) )
but 2: COS(A-B) = SIN A SIN B + COS A COS B
which is rewritten as 3: SIN A SIN B = COS(A-B) - COS A COS B
replace SIN A SIN B in 1. you have :
4: ACOS( COS(A-B) - COS A COS B + COS A COS B COS(A-B) )
You pre-calculate X = COS(A-B) and Y = COS A COS B and you put the values into 4
to give:
ACOS( X - Y + XY )
4 trig calculations instead of 6 !

Change the way you store long/lat:
struct LongLat
{
float
long,
lat,
x,y,z;
}
When creating a long/lat, also compute the (x,y,z) 3D point that represents the equivalent position on a unit sphere centred at the origin.
Now, to determine if point B is nearer to point A than point C, do the following:
// is B nearer to A than C?
bool IsNearer (LongLat A, LongLat B, LongLat C)
{
return (A.x * B.x + A.y * B.y + A.z * B.z) < (A.x * C.x + A.y * C.y + A.z * C.z);
}
and to get the distance between two points:
float Distance (LongLat A, LongLat B)
{
// radius is the size of sphere your mapping long/lats onto
return radius * acos (A.x * B.x + A.y * B.y + A.z * B.z);
}
You could remove the 'radius' term, effectively normalising the distances.

Switching to lookup tables for sin/cos/acos. Will be faster, there are alot of c/c++ fixed point libraries that also include those.
Here is code from someone else on Memoization. Which might work if the actual values used are more clustered.
Here is an SO question on Fixed Point.

What is the bottle neck? Is the the sine/cosine function calls or the arcsine call?
If your sine/cosine calls are slow, you could use the following theorem to prevent so many calls:
1 = sin(x)^2 + cos(x)^2
cos(x) = sqrt(1 - sin(x)^2)
But I like the mapping idea so that you don't have to recompute values you've already computed. Although be careful as the map could get very large very quickly.

How exact do you need the values to be?
If you round your values a bit then you could store the result of all lookups and check if thay have been used befor each calculation?

Well, since lat and lon are garenteed to be within a certain range, you could try using some form of a lookup table for you Math.* method calls. Say, a Dictionary<double,double>

I would argue that you may want to re-examine how you found that function to be the bottleneck. (IE did you profile the application?)
The equation to me seems very light weight and shouldn't cause any trouble.
Granted, I don't know your application and you say you do a lot of these calculations.
Nevertheless it is something to consider.

As someone else pointed out, are you sure this is your bottleneck?
I've done some performance testing of a similar application I'm building where I call a simple method to return a distance between two points using standard trig. 20,000 calls to it shoves it right at the top of the profiling output, yet there's no way I can make it faster... It's just the shear # of calls.
In this case, I need to reduce the # calls to it... Not that this is the bottleneck.

I use a different algorithm for calculating distance between 2 lati/longi positions, it could be lighter than yours since it only does 1 Cos call and 1 Sqrt call.
public static double GetDistanceBetweenTwoPos(double lat1, double long1, double lat2, double long2)
{
double distance = 0;
double x = 0;
double y = 0;
x = 69.1 * (lat1 - lat2);
y = 69.1 * (long1 - long2) * System.Math.Cos(lat2 / 57.3);
//calculation base : Miles
distance = System.Math.Sqrt(x * x + y * y);
//Distance calculated in Kilometres
return distance * 1.609;
}

someone has already mentioned memoisation and this is a bit similar. if you comparing the same point to many other points then it is better to precalculate parts of that equation.
instead of
double result = Math.Acos(Math.Sin(lat2rad) * Math.Sin(lat1rad)
+ Math.Cos(lat2rad) * Math.Cos(lat1rad) * Math.Cos(lon2rad - lon1rad));
have:
double result = Math.Acos(lat2rad.sin * lat1rad.sin
+ lat2rad.cos * lat1rad.cos * (lon2rad.cos * lon1rad.cos + lon1rad.sin * lon2rad.sin));
and i think that's the same formula as someone else has posted because part of the equation will disappear when you expand the brackets:)