Snap to Road with direction in concern

I'm trying to add a snap-to-road function in my all custom made GPS software. There's a few thousand of roads with hundreds of thousands of nodes (lat-lon points). I have read all those snap to road question in SO with "closest point on a line" as a good solution. However, what I am doing is to snap to a nearest road with the same direction as the car is driving. I'm开发者_运维问答 so confused with how to achieve it. A simple procedure would help me a lot. (Please also note hundreds of thousands or nodes in concern as performance).

EDIT: Guys? c'mon, gimmie a clue.

---

first, find the heading of the road you want to test i.e how many degrees from north it is by using trigonometry: find the two defining lat-lon points for that section and call them point1 and point2: (in pseudo code)

double roadDrctn = Math.Atan2(point2.y - point2.y, point2.x - point1.x)

then get the orientation of your car/ GPS and see if it's the same or whatever you want to do to it. Remember to also test it + 180 degrees (or Math.Pi if it uses radians) as well because you can go both ways along a road! My only concern about this technique is that trig functions are quite expensive performance wise.

Hope that answers your question.