Module geometry
[hide private]
[frames] | no frames]

Source Code for Module geometry

 1  ################################################################################## 
 2  # Copyright (c) 2010, 2011, 2012, 2013, Daniel Urieli, Peter Stone 
 3  # University of Texas at Austin 
 4  # All right reserved 
 5  #  
 6  # Based On: 
 7  #  
 8  # Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam 
 9  # All rights reserved. 
10  #  
11  # Redistribution and use in source and binary forms, with or without 
12  # modification, are permitted provided that the following conditions are met: 
13  #  
14  # 1. Redistributions of source code must retain the above copyright notice, this 
15  # list of conditions and the following disclaimer. 
16  #  
17  # 2. Redistributions in binary form must reproduce the above copyright notice, 
18  # this list of conditions and the following disclaimer in the documentation 
19  # and/or other materials provided with the distribution. 
20  #  
21  # 3. Neither the name of the University of Amsterdam nor the names of its 
22  # contributors may be used to endorse or promote products derived from this 
23  # software without specific prior written permission. 
24  #  
25  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
26  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
27  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
28  # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
29  # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
30  # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
31  # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
32  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
33  # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
34  # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
35  ################################################################################## 
36   
37   
38   
39  from math import * 
40  import numpy as np 
41   
42 -def distance2D(a, b):
43 "2D distance between two points (x1, y1) and (x2, y2)" 44 xdiff = b[0] - a[0] 45 ydiff = b[1] - a[1] 46 diagSquared = xdiff * xdiff + ydiff * ydiff 47 return sqrt(diagSquared)
48
49 -def orientation2D(a, b):
50 """ 51 Orientation of the vector b-a. 52 The results is between -180 and 180 degrees 53 """ 54 # TODO: replace by a vector class? 55 xdiff = b[0] - a[0] 56 ydiff = b[1] - a[1] 57 return atan2(ydiff, xdiff) / pi * 180
58
59 -def computeShortestTurn(fromAng, toAng):
60 """ 61 Computes that shortest angle (positive or negative) 62 between 2 angles. 63 """ 64 # TODO: replace by a vector class? 65 turn = toAng - fromAng 66 candidate1 = turn 67 candidate2 = turn - 360 68 candidate3 = turn + 360 69 absTurn2OrigTurn = { abs(candidate1) : candidate1, 70 abs(candidate2) : candidate2, 71 abs(candidate3) : candidate3 } 72 return absTurn2OrigTurn[ min([abs(candidate1), abs(candidate2), abs(candidate3)]) ]
73
74 -def averagePoint2D(pointList):
75 """ 76 return the average point in a list of (x,y) points 77 """ 78 totalX = 0.0 79 totalY = 0.0 80 for p in pointList: 81 totalX += p[0] 82 totalY += p[1] 83 avgX = totalX / len(pointList) 84 avgY = totalY / len(pointList) 85 return np.array([avgX, avgY])
86