I have a data set of x,y co-ordinates, starting from origin, recorded each second. I can detect distance, speed,acceleration, modulus of displacement . Is there any algorithm to detect whether a left or right turn ?
I am currently calculating distance and modulus of displacement for every 10 seconds, if the displacement is approximately equal to distance, then the vehicle is on straight path, but of the values change then there is a turn involved.
IS there an algorithm to decide whether the turn was left or right ? My data looks like this
Time x y
0 0 0
1 -0.2 -0.1
2 -0.7 0.9
3 -0.8 0.9
4 -1 0.8
5 -1.1 0.8
6 -1.2 0.7
7 -1.4 0.7
8 -1.9 1.7
9 -2 1.7
10 -2.2 1.6
11 -2.3 1.6
12 -2.5 1.5
13 -2.6 1.5
14 -2.7 1.5
15 -2.9 1.4
16 -3.6 1.2
17 -4.1 -0.1
18 -4.7 -1.5
19 -4.7 -2.6
20 -4.3 -3.7
21 -4.3 -3.7
22 -4.7 -3.8
23 -6.2 -3.1
24 -9.9 -1.9
25 -13.7 -1.9
26 -17.9 -2
27 -21.8 -0.8
28 -25.1 -0.6
29 -28.6 1.8
解决方案
Looking at 3 points p0, p1 and p2, you can look at the relative orientation of the two vectors p1 - p0 and p2 - p1. An easy way to do this is to calculate the cross product between the two vectors. The x- and y-components of the cross product are 0 because both vectors are in the xy-plane. So only the z-component of the cross product needs to be calculated.
If the z-component of the cross product is positive, you know that the second vector points left relative to the first one, because the first vector, second vector, and a vector in the positive z-direction are right handed. If the cross product is negative, the second vector points to the right relative to the first one.
I used my mad Python skills (I use Python about once a year...) to put this into the code below. There's a little logic so that the Left/Right designation can be printed at the middle point, even though it can only be calculated after the next point was read. To enable that, a couple of previous lines are saved away, with their printing delayed. The actual calculation is in the calcDir() function.
import sys
fileName = sys.argv[1]
dataFile = open(fileName, 'r')
def calcDir(p0, p1, p2):
v1x = float(p1[0]) - float(p0[0])
v1y = float(p1[1]) - float(p0[1])
v2x = float(p2[0]) - float(p1[0])
v2y = float(p2[1]) - float(p1[1])
if v1x * v2y - v1y * v2x > 0.0:
return 'Left'
else:
return 'Right'
lineIdx = 0
for line in dataFile:
line = line.rstrip()
lineIdx += 1
if lineIdx == 1:
print line
elif lineIdx == 2:
line0 = line
print line0
elif lineIdx == 3:
line1 = line
else:
line2 = line
dir = calcDir(line0.split()[1:], line1.split()[1:], line2.split()[1:])
print line1 + ' ' + dir
line0 = line1
line1 = line2
print line2