   Logic to keep vehicle on course.
  Hi, If I have a vehicle going from A -> B -> C at a constant speed, (S).My 'default' rate of turn is 3 degree per second.As I want to be able to remain on course, I need to start my turn, (from A->B B->C), to ensure that I will not 'overshoot' or 'undershoot' my turn.I need 2 functions, 1- When on course from A->B, Calculate when to start turning so I can join the course B -> C at the right heading. This assumes that I will not cross point B on the route.2- When on course from A->B, a function that would help me turn in such a way that I will cross point B on the right heading to C, (my guess is that I would first need to turn away from B while on the A->B course).Any suggestion on function a could use?I am codding in C++, but I could convert the logic in any code.Many thanks in advance.FFMG   
  just a quick thought.You can compute the circle passing through A, B and C, and follow that circle to get a nice smooth motion.When you have two points (A, B), and you want to find a circle passing through the points, the centre of the circle will be at the equidistance from the two points. Meaning, the centre of the circle will be on the line perpendicular to the segment (A, B), and passing through the middle of (A, B) (the perpendicular bisector). call it perp(A, B).if you have three points, then the centre of the circle passing through the three points will be at the intersection between the perp(A, B) and perp(B, C) and consequently also on perp(A, C).That will give you a path to follow. say you have your car pat position 'P'. You want the car to move towards 'B', then 'C'. - find the centre ('O') of circle (P, B, C), using perp(P, C), perp(B, C)'s point of intersection.- the direction to follow will be the tangent of that circle at point 'P', aka the perpendicular of (P - O), aka N = normalisedVector(P.y - O.y, O.x - P.x).- your current vehicle direction is vector 'D'. The steering angle would be atan(crossProduct(N, D), dotProduct(N, D)), or something like that. Basically the angle between your current moving direction and the optimum direction to follow the circle passing through (P, B, C). Special case, if (P, B, C) nearly aligned, steering angle = 0.[Edited by - oliii on June 15, 2009 1:35:38 PM];  Let's see if I can help. Get paper, pencil, ruler and compass.1)If I understand you correctly, the vehicle starts at A, is supposed to head towards B along AB first, but then to arrive at C from the direction BC. That means we first head straight towards B, but shortly before we reach it we have to steer towards C, following an arc of a circle for a while till we head towards C, then straight again, till we are at C. Said circle has a given radius r (our turning radius) and must be tangent to both AB and BC.Its center M must lie on the ray bisecting the angle alpha=ABC, and the distance between AB and M (and BC and M as well) must equal r. Some trigonometry gives me that |BM|=r/cos(alpha/2). That should be enough to construct the circle and more importantly its tangent point to AB, at which you start turning.2) Yes, you'll have to turn twice. The second turning circle with center M2 is tangent to BC, and goes through B. The first one with center M! is tangent to AB in a point T1, lies on the other side of AB than C and is tangent to the second one in P2. M2 can be easily found: BM2 is orthogonal on BC, and has length r. M1 lies r away from AB on the other side of C, and is 2r away from M2. That should be enough to construct both circles, P1 where you start turning away from C, and P2=(M1+M2)/2, where you turn towards C again.Neither gives you a simple function from position to steering angle, but state transistion criteria instead: when to switch from heading straight at B from turning left/right, when to turn the steering wheel the other way around, when to head straight towards C. Sounds a lot like a finite state machine to me.   ;  First of all you need to determine what is your minimum turn radius. If you can turn at 3 degrees per second at speed S you get:R = v / w = v / (TORAD(3))Now take points B and C. Construct the vector from B to C. Take the perpendicular to it (cross product with Up vector) and multiply this vector by the Radius. Now you have the center point of a circle that is tangent to BC of your minimum turning radius. You now need to find the point where you have to enter this turn - the point where a line from A is tangent to this circle (center X, radius R). You can find a good method (I'm using it myself) Here.Now you have 2 states: moving and turning. You move in a straight line to the turn entry point. When you reach it you start turning at the maximum turning rate. You'll get to B on a direction pointing straight to C.Instead of using C as the next point, apply the algorithm for finding the turn entry to points C and D. Then calculate the initial tangent using this point so you get a turn that leaves you pointing straight to this future turn entry point.It gets (a lot) more complicated if you have acceleration on turning speed (instead of instantaneous change).   ;  Hi, Thanks for the replies, I think I understand what I need to do.For function (1) I need to calculate the point 'X' and 'Y' they are the tangent of my circle defined by my rate of turn.I am not sure I understand about the circle having to pass through A, B and C.A, B and C are only the path that the vehicle must/should follow.I have come up with a small formula that should work for the first problem.H1 = Heading from A->BH2 = Heading from B->CS = my current speed.R = my rate of turn.x = y = ((H2-H1) / R) * S.For example.H1 = 90H2 = 135S = 60KmpH, (= 0.0167KmpS)R = 3 degree/sec((H2-H1) / R)  = number of seconds to complete turn(45/3) = 15 seconds.15 * 0.0167 = 0.250So I should start my turn 250m before 'B' and I will complete my turn 250 past 'B'.Does that sound right?FFMG;  My solution was something like this:It ensures you hit all your waypoints.   ;  Folwoing on my previous postMy next problem is to make sure by the time I cross 'B' I am already heading in the right direction.So, (again using my previous post), I calculated that I should start turning 250m before B,But now I was thinking of turning 750m before 'B'First turn toward 'x', turn -45 degree, then +45 degree to be parallel to my current heading.Finally turn 45 as with my previous function and I should cross 'B' on the correct course.Does that seem right?My drawings seem a bit out of scale, but it looks like I will be on the right course 250m _before_ 'B' and not exactly as I cross 'B' so I must be doing something wrong.Any input/suggestions?ThanksFFMG   ;Quote:Original post by IliciMy solution was something like this:It ensures you hit all your waypoints.Ah, I see, what you mean now.But that would mean that the vehicle is never really on the right course, (from the beginning).In my case this is a problem as the speed is only constant at the start of the turn, on the course itself, the speed can change, so I will never really know what course to take, (from the beginning).FFMG   