I'm facing the problem of computing values of a clothoid in C in real-time.
First I tried using the Matlab coder to obtain auto-generated C code for the quadgk-integrator for the Fresnel formulas. This essentially works great in my test scnearios. The only issue is that it runs incredibly slow (in Matlab as well as the auto-generated code).
Another option was interpolating a data-table of the unit clothoid connecting the sample points via straight lines (linear interpolation). I gave up after I found out that for only small changes in curvature (tiny steps along the clothoid) the results were obviously degrading to lines. What a surprise...
I know that circles may be plotted using a different formula but low changes in curvature are often encountered in real-world-scenarios and 30k sampling points in between the headings 0° and 360° didn't provide enough angular resolution for my problems.
Then I tried a Taylor approximation around the R = inf point hoping that there would be significant curvatures everywhere I wanted them to be. I soon realized I couldn't use more than 4 terms (power of 15) as the polynom otherwise quickly becomes unstable (probably due to numerical inaccuracies in double precision fp-computation). Thus obviously accuracy quickly degrades for large t values. And by "large t values" I'm talking about every point on the clothoid that represents a curve of more than 90° w.r.t. the zero curvature point.
For instance when evaluating a road that goes from R=150m to R=125m while making a 90° turn I'm way outside the region of valid approximation. Instead I'm in the range of 204.5° - 294.5° whereas my Taylor limit would be at around 90° of the unit clothoid.
I'm kinda done randomly trying out things now. I mean I could just try to spend time on the dozens of papers one finds on that topic. Or I could try to improve or combine some of the methods described above. Maybe there even exists an integrate function in Matlab that is compatible with the Coder and fast enough.
This problem is so fundamental it feels to me I shouldn't have that much trouble solving it. any suggetions?
about the 4 terms in Taylor series - you should be able to use much more. total theta of 2pi is certainly doable, with doubles.
you're probably calculating each term in isolation, according to the full formula, calculating full factorial and power values. that is the reason for losing precision extremely fast.
instead, calculate the terms progressively, the next one from the previous one. Find the formula for the ratio of the next term over the previous one in the series, and use it.
For increased precision, do not calculate in theta by rather in the distance, s (to not lose the precision on scaling).
your example is an extremely flat clothoid. if I made no mistake, it goes from (25/22) pi =~ 204.545° to (36/22) pi =~ 294.545° (why not include these details in your question?). Nevertheless it should be OK. Even 2 pi = 360°, the full circle (and twice that), should pose no problem.
given: r = 150 -> 125, 90 degrees turn :
r s = A^2 = 150 s = 125 (s+x)
=> 1+(x/s) = 150/125 = 1 + 25/125 x/s = 1/5
theta = s^2/2A^2 = s^2 / (300 s) = s / 300 ; = (pi/2) * (25/11) = 204.545°
theta2 = (s+x)^2/(300 s) = (6/5)^2 s / 300 ; = (pi/2) * (36/11) = 294.545°
theta2 - theta = ( 36/25 - 1 ) s / 300 == pi/2
=> s = 300 * (pi/2) * (25/11) = 1070.99749554 x = s/5 = 214.1994991
A^2 = 150 s = 150 * 300 * (pi/2) * (25/11)
a = sqrt (2 A^2) = 300 sqrt ( (pi/2) * (25/11) ) = 566.83264608
The reference point is at r = Infinity, where theta = 0.
we have x = a INT[u=0..(s/a)] cos(u^2) d(u) where a = sqrt(2 r s) and theta = (s/a)^2. write out the Taylor series for cos, and integrate it, term-by-term, to get your Taylor approximation for x as function of distance, s, along the curve, from the 0-point. that's all.
next you have to decide with what density to calculate your points along the clothoid. you can find it from a desired tolerance value above the chord, for your minimal radius of 125. these points will thus define the approximation of the curve by line segments, drawn between the consecutive points.
I am doing my thesis in the same area right now.
My approach is the following.
at each point on your clothoid, calculate the following (change in heading / distance traveled along your clothoid), by this formula you can calculate the curvature at each point by this simple equation.
you are going to plot each curvature value, your x-axis will be the distance along the clothoid, the y axis will be the curvature. By plotting this and applying very easy linear regression algorithm (search for Peuker algorithm implementation in your language of choice)
you can easily identify where are the curve sections with value of zero (Line has no curvature), or linearly increasing or decreasing (Euler spiral CCW/CW), or constant value != 0 (arc has constant curvature across all points on it).
I hope this will help you a little bit.
You can find my code on github. I implemented some algorithms for such problems like Peuker Algorithm.
Im trying to do a graph from evalued math function and this is last think I need to do. I have graph with limit coordinates -250:-250 left down and 250:250 right up. I have Y-limit function, which is defined as -10:10, but it could be user redefined and if it is redefined, I need to calculate new coordinates.
I have now field of y-coordinates with 20000 values and each of is multiply by:
ratioY = 25 / (fabs( up-limit - down-limit ) / 20) which will make coordinates adapt for new Y-limit (if limit is -5:5, graph looks 2x bigger), this works good, but now isnt graph exactly where it should be (see pictures). Simply 25 is multiplied for postscript coordinates and (up-limit - down-limit) / 20 is ratio for "zooming" Y coordinates. This works fine.
Now Im trying to "move coordinates" which will subtracted from revaluated value:
ycoor = (ycoor * ratioY) - move-coorY ;.
Now I have something like this:
move-coorY = 25* ( ( up-limit - down-limit) /2 );
and it doesnt work correctly. I need to do sin(0) start from 0.
This is a correct graph which is -10:10
(source: matematika.cz)
This is a bad graph which is -5:10
(source: matematika.cz)
Maybe its easier not to do this with fixed numbers (like your ratioY) but with two different coordinate systems. Physical coordinates are in your problem domain, i.e. they are the real values of your sine curves. Logical coordinates refer to the device, in your case they are the point values in Postscript, but they might be pixels on a HTML canvas or whatever.
I'll denote the physical coordinates of the first axis with a small x and the corresponding logical coordinate with a capital X. In each coordinate system we have:
Lower bound: x_min, X_max
Upper bound: x_max, X_max
Range: dx = x_max - x_min
dX = X_max - X_min
Then you can calculate your logical coordinates from the physical ones:
X(x) = X_min + (x - x_min) * dX / dx
This also works vice versa, which is not an issue for Postscript files, but max be useful for an intractive canvas where a mouse click should yield the physical coordinates.
x(X) = x_min + (X - X_min) * dx / dX
In your case, the ratio or scale factor is dX / dx, which you can calculate once for each axis. Let's plot the first point with y == 0 in your first graph:
y_min = -10
y_max = 10
dy = 20
Y_min = -250
Y_max = 250
dX = 500
Y(0) = -250 + (0 - (-10)) * 500 / 20
= -250 + 10 * 500 / 20
= 0
In the second graph, the logical coordinates are the same, but:
y_min = -5
y_max = 10
dy = 15
Y(0) = -250 + (0 - (-5)) * 500 / 15
= -250 + 5 * 500 / 15
= -83.3333
If you change the range of your graph, e.g. from (-10, 10) to (-5, 10), just adjust the physical coordinates. If you resize your graph, change the logical coordinates. (Also, calculating the point in the graph is the same as calculating the position of the tick mark for the axis. Strangely, you got the tick marks right, but not your graph. I think your problem was to account for the non-zero lower bound in both graph and curve data.)
Also, it's probably better to re-evaluate the logical coordinates when printing instead of transfroming them from a previous plot. You can do that on the fly, so that you only need to keep the physical data in an array.
(And lastly, I'll admit that I'm not entirely sure these two kinds of cooirdinates are called physical and logical. I know these terms are used, but it may be the other way round or they might even mean sonething different altogether.)
My friend did a well yob for me and programmed this...
double zeroPosition(double startY, double endY){
double range = endY - startY;
double topSize = endY / range;
return 250.0 - 500 * topSize;
}
This will calculate position of zero, which I just add to my Y position with ratio and It works exactly how I need!
But thanks M Oehm ;)
I am working on a project in which i have to rotate a tyre based on the value from the position sensor. Sensor provides values from 261(at -90 degree) to 395(at +90 degree). I am using TransformGroup so almost at 327 value from sensor the tyre should be straight and erect.
When value is 395 then tyre will totally lie along X axis similar is case for value 261. I am able to move the tyre but i am unable to calculate precise angle.
My way of calculation is like that i calculate the number of values btween the default value or zero value(327) for a specific angle(say 3). so i get number of values for that angle. Then i divide this number from the angle(3 degree). So i get angle for one value.
float tempangle = value from sensor-zero value which is 327;
tempangle = Math.Abs(tempangle);
tempangle /= float.Parse(angle for reference say 3 degree);
tempangle = 1 / tempangle;
Here i put some body of known angle under sensor(which have an agle of degree). When i want to find some angle for some value i just multiply this angle with the number of values between that value and default value(327). So i get the angle to rotate the body.
suppose i have got angle after calculation
angle for one sensor value =1.14286286
Suppose i calculate angle by calculator like
1.14286286 *2=2.2857
1.14286286 *3=3.42858
1.14286286 *4=4.57145
1.14286286 *5=5.714314
1.14286286 *6=6.85717
As you can see that angle is skipping some decimal values. Is there any way to co op with it?
thanks
No idea if I understand your problem, but a method that calcaluates an angle between -90 and 90 degree from an input value in the range 261 to 395 would simply look like this:
private static double SensorValueToAngle(double value)
{
const double sensorMin = 261;
const double sensorMax = 395;
const double angleMin = -90;
const double angleMax = 90;
return angleMin + (angleMax - angleMin)
* (value - sensorMin) / (sensorMax - sensorMin);
}
I have began working on an autonomous rc helicopter in c. I need help finding a way to calculate the bearing acuratley. I am using two sets of coordinates (latitude and longitude), one is the current location and the other is the destination point. I have converted the latitude and longitude into decimal format so....
40°58'19.86"N = 40.972183
74°14'52.74"W = 74.247983
Can anyone show me code in c to find the bearing or a formula i can use?
i have looked at: http://www.movable-type.co.uk/scripts/latlong.html and when i transfer it to c, the results make no sense.
This is what i have tried:
double x = Sin(Longitude2 - Longitude1) * Cos(Latitude2);
double y = Cos(Latitude1) * Sin(Latitude2) - Sin(Latitude1) * Cos(Latitude2) * Cos(Longitude2 - Longitude1);
double heading = (Atan2(x, y) % 2 * 3.14159265) (180/3.14159265);
Have you converted your coordinates from degrees to radians before calculations ?
angleRad = 3.14159265 * angleDeg / 180;
And
bearing = Atan2(y, x);
in that website.
The bearing should be converted from rad to deg in turn:
bearing = 180 * bearing / 3.14159265;
and in case of negative value eventually:
bearing = bearing + 360;
I don't know how to write this using the convention above (with "%").
Latitude1 and Longitude1 are coordinates of the observer
Latitude2 and Longitude2 are coordinates of destination point.
Formulas from that website work well.
For the very small area you are considering you can assume the world is flat.
If you also only need this to work locally you can pre-calculate the value of a deg lat/lon on the ground in metres (or ft)
Then simply convert the delta lat/lon into distance and do the normal trig.
To convert distance to lat/lon you can either use google earth to measure it or
How to convert latitude or longitude to meters?
I have not marked this question Answered yet.
The current accepted answer got accepted automatically because of the Bounty Time-Limit
With reference to this programming game I am currently building.
As you can see from the above link, I am currently building a game in where user-programmable robots fight autonomously in an arena.
Now, I need a way to detect if a robot has detected another robot in a particular angle (depending on where the turret may be facing):
alt text http://img21.imageshack.us/img21/7839/robotdetectionrg5.jpg
As you can see from the above image, I have drawn a kind of point-of-view of a tank in which I now need to emulate in my game, as to check each point in it to see if another robot is in view.
The bots are just canvases that are constantly translating on the Battle Arena (another canvas).
I know the heading the turret (the way it will be currently facing), and with that, I need to find if there are any bots in its path(and the path should be defined in kind of 'viewpoint' manner, depicted in the image above in the form of the red 'triangle'. I hope the image makes things more clear to what I am trying to convey.
I hope that someone can guide me to what math is involved in achieving this problem.
[UPDATE]
I have tried the calculations that you have told me, but it's not working properly, since as you can see from the image, bot1 shouldn't be able to see Bot2 . Here is an example :
alt text http://img12.imageshack.us/img12/7416/examplebattle2.png
In the above scenario, Bot 1 is checking if he can see Bot 2. Here are the details (according to Waylon Flinn's answer):
angleOfSight = 0.69813170079773179 //in radians (40 degrees)
orientation = 3.3 //Bot1's current heading (191 degrees)
x1 = 518 //Bot1's Center X
y1 = 277 //Bot1's Center Y
x2 = 276 //Bot2's Center X
y2 = 308 //Bot2's Center Y
cx = x2 - x1 = 276 - 518 = -242
cy = y2 - y1 = 308 - 277 = 31
azimuth = Math.Atan2(cy, cx) = 3.0141873380511295
canHit = (azimuth < orientation + angleOfSight/2) && (azimuth > orientation - angleOfSight/2)
= (3.0141873380511295 < 3.3 + 0.349065850398865895) && (3.0141873380511295 > 3.3 - 0.349065850398865895)
= true
According to the above calculations, Bot1 can see Bot2, but as you can see from the image, that is not possible, since they are facing different directions.
What am I doing wrong in the above calculations?
The angle between the robots is arctan(x-distance, y-distance) (most platforms provide this 2-argument arctan that does the angle adjustment for you. You then just have to check whether this angle is less than some number away from the current heading.
Edit 2020: Here's a much more complete analysis based on the updated example code in the question and a now-deleted imageshack image.
Atan2: The key function you need to find an angle between two points is atan2. This takes a Y-coordinate and X-coordinate of a vector and returns the angle between that vector and the positive X axis. The value will always be wrapped to lie between -Pi and Pi.
Heading vs Orientation: atan2, and in general all your math functions, work in the "mathematical standard coordinate system", which means an angle of "0" corresponds to directly east, and angles increase counterclockwise. Thus, an "mathematical angle" of Pi / 2 as given by atan2(1, 0) means an orientation of "90 degrees counterclockwise from due east", which matches the point (x=0, y=1). "Heading" is a navigational idea that expresses orientation is a clockwise angle from due north.
Analysis: In the now-deleted imageshack image, your "heading" of 191 degrees corresponded to a south-south-west direction. This actually an trigonometric "orientation" of -101 degrees, or -1.76. The first issue in the updated code is therefore conflating "heading" and "orientation". you can get the latter from the former by orientation_degrees = 90 - heading_degrees or orientation_radians = Math.PI / 2 - heading_radians, or alternatively you could specify input orientations in the mathematical coordinate system rather than the nautical heading coordinate system.
Checking that an angle lies between two others: Checking that an vector lies between two other vectors is not as simple as checking that the numeric angle value is between, because of the way the angles wrap at Pi/-Pi.
Analysis: in your example, the orientation is 3.3, the right edge of view is orientation 2.95, the left edge of view is 3.65. The calculated azimith is 3.0141873380511295, which happens to be correct (it does lie between). However, this would fail for azimuth values like -3, which should be calculated as "hit". See Calculating if an angle is between two angles for solutions.
Calculate the relative angle and distance of each robot relative to the current one. If the angle is within some threshold of the current heading and within the max view range, then it can see it.
The only tricky thing will be handling the boundary case where the angle goes from 2pi radians to 0.
Something like this within your bot's class (C# code):
/// <summary>
/// Check to see if another bot is visible from this bot's point of view.
/// </summary>
/// <param name="other">The other bot to look for.</param>
/// <returns>True iff <paramref name="other"/> is visible for this bot with the current turret angle.</returns>
private bool Sees(Bot other)
{
// Get the actual angle of the tangent between the bots.
var actualAngle = Math.Atan2(this.X - other.X, this.Y - other.Y) * 180/Math.PI + 360;
// Compare that angle to a the turret angle +/- the field of vision.
var minVisibleAngle = (actualAngle - (FOV_ANGLE / 2) + 360);
var maxVisibleAngle = (actualAngle + (FOV_ANGLE / 2) + 360);
if (this.TurretAngle >= minVisibleAngle && this.TurretAngle <= maxVisibleAngle)
{
return true;
}
return false;
}
Notes:
The +360's are there to force any negative angles to their corresponding positive values and to shift the boundary case of angle 0 to somewhere easier to range test.
This might be doable using only radian angles but I think they're dirty and hard to read :/
See the Math.Atan2 documentation for more details.
I highly recommend looking into the XNA Framework, as it's created with game design in mind. However, it doesn't use WPF.
This assumes that:
there are no obstacles to obstruct the view
Bot class has X and Y properties
The X and Y properties are at the center of the bot.
Bot class a TurretAngle property which denotes the turret's positive angle relative to the x-axis, counterclockwise.
Bot class has a static const angle called FOV_ANGLE denoting the turret's field of vision.
Disclaimer: This is not tested or even checked to compile, adapt it as necessary.
A couple of suggestions after implementing something similar (a long time ago!):
The following assumes that you are looping through all bots on the battlefield (not a particularly nice practice, but quick and easy to get something working!)
1) Its a lot easier to check if a bot is in range then if it can currently be seen within the FOV e.g.
int range = Math.sqrt( Math.abs(my.Location.X - bots.Location.X)^2 +
Math.abs(my.Location.Y - bots.Location.Y)^2 );
if (range < maxRange)
{
// check for FOV
}
This ensures that it can potentially short-cuircuit a lot of FOV checking and speed up the process of running the simulation. As a caveat, you could have some randomness here to make it more interesting, such that after a certain distance the chance to see is linearly proportional to the range of the bot.
2) This article seems to have the FOV calculation stuff on it.
3) As an AI graduate ... nave you tried Neural Networks, you could train them to recognise whether or not a robot is in range and a valid target. This would negate any horribly complex and convoluted maths! You could have a multi layer perceptron [1], [2] feed in the bots co-ordinates and the targets cordinates and recieve a nice fire/no-fire decision at the end. WARNING: I feel obliged to tell you that this methodology is not the easiest to achieve and can be horribly frustrating when it goes wrong. Due to the (simle) non-deterministic nature of this form of algorithm, debugging can be a pain. Plus you will need some form of learning either Back Propogation (with training cases) or a Genetic Algorithm (another complex process to perfect)! Given the choice I would use Number 3, but its no for everyone!
It can be quite easily achieved with the use of a concept in vector math called dot product.
http://en.wikipedia.org/wiki/Dot_product
It may look intimidating, but it's not that bad. This is the most correct way to deal with your FOV issue, and the beauty is that the same math works whether you are dealing with 2D or 3D (that's when you know the solution is correct).
(NOTE: If anything is not clear, just ask in the comment section and I will fill in the missing links.)
Steps:
1) You need two vectors, one is the heading vector of the main tank. Another vector you need is derived from the position of the tank in question and the main tank.
For our discussion, let's assume the heading vector for main tank is (ax, ay) and vector between main tank's position and target tank is (bx, by). For example, if main tank is at location (20, 30) and target tank is at (45, 62), then vector b = (45 - 20, 62 - 30) = (25, 32).
Again, for purpose of discussion, let's assume main tank's heading vector is (3,4).
The main goal here is to find the angle between these two vectors, and dot product can help you get that.
2) Dot product is defined as
a * b = |a||b| cos(angle)
read as a (dot product) b since a and b are not numbers, they are vectors.
3) or expressed another way (after some algebraic manipulation):
angle = acos((a * b) / |a||b|)
angle is the angle between the two vectors a and b, so this info alone can tell you whether one tank can see another or not.
|a| is the magnitude of the vector a, which according to the Pythagoras Theorem, is just sqrt(ax * ax + ay * ay), same goes for |b|.
Now the question comes, how do you find out a * b (a dot product b) in order to find the angle.
4) Here comes the rescue. Turns out that dot product can also be expressed as below:
a * b = ax * bx + ay * by
So angle = acos((ax * bx + ay * by) / |a||b|)
If the angle is less than half of your FOV, then the tank in question is in view. Otherwise it's not.
So using the example numbers above:
Based on our example numbers:
a = (3, 4)
b = (25, 32)
|a| = sqrt(3 * 3 + 4 * 4)
|b| = sqrt(25 * 25 + 32 * 32)
angle = acos((20 * 25 + 30 * 32) /|a||b|
(Be sure to convert the resulting angle to degree or radian as appropriate before comparing it to your FOV)
This will tell you if the center of canvas2 can be hit by canvas1. If you want to account for the width of canvas2 it gets a little more complicated. In a nutshell, you would have to do two checks, one for each of the relevant corners of canvas2, instead of one check on the center.
/// assumming canvas1 is firing on canvas2
// positions of canvas1 and canvas2, respectively
// (you're probably tracking these in your Tank objects)
int x1, y1, x2, y2;
// orientation of canvas1 (angle)
// (you're probably tracking this in your Tank objects, too)
double orientation;
// angle available for firing
// (ditto, Tank object)
double angleOfSight;
// vector from canvas1 to canvas2
int cx, cy;
// angle of vector between canvas1 and canvas2
double azimuth;
// can canvas1 hit the center of canvas2?
bool canHit;
// find the vector from canvas1 to canvas2
cx = x2 - x1;
cy = y2 - y1;
// calculate the angle of the vector
azimuth = Math.Atan2(cy, cx);
// correct for Atan range (-pi, pi)
if(azimuth < 0) azimuth += 2*Math.PI;
// determine if canvas1 can hit canvas2
// can eliminate the and (&&) with Math.Abs but this seems more instructive
canHit = (azimuth < orientation + angleOfSight) &&
(azimuth > orientation - angleOfSight);
Looking at both of your questions I'm thinking you can solve this problem using the math provided, you then have to solve many other issues around collision detection, firing bullets etc. These are non trivial to solve, especially if your bots aren't square. I'd recommend looking at physics engines - farseer on codeplex is a good WPF example, but this makes it into a project way bigger than a high school dev task.
Best advice I got for high marks, do something simple really well, don't part deliver something brilliant.
Does your turret really have that wide of a firing pattern? The path a bullet takes would be a straight line and it would not get bigger as it travels. You should have a simple vector in the direction of the the turret representing the turrets kill zone. Each tank would have a bounding circle representing their vulnerable area. Then you can proceed the way they do with ray tracing. A simple ray / circle intersection. Look at section 3 of the document Intersection of Linear and Circular Components in 2D.
Your updated problem seems to come from different "zero" directions of orientation and azimuth: an orientation of 0 seems to mean "straight up", but an azimuth of 0 "straight right".