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

Source Code for Module geometry

 1  ################################################################################## 
 2  # Copyright (c) 2012, 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   
41 -def distance2D(a, b):
42 "2D distance between two points (x1, y1) and (x2, y2)" 43 xdiff = b[0] - a[0] 44 ydiff = b[1] - a[1] 45 diagSquared = xdiff * xdiff + ydiff * ydiff 46 return sqrt(diagSquared)
47
48 -def orientation2D(a, b):
49 """ 50 Orientation of the vector b-a. 51 The results is between -180 and 180 degrees 52 """ 53 # TODO: replace by a vector class? 54 xdiff = b[0] - a[0] 55 ydiff = b[1] - a[1] 56 return atan2(ydiff, xdiff) / pi * 180
57
58 -def computeShortestTurn(fromAng, toAng):
59 """ 60 Computes that shortest angle (positive or negative) 61 between 2 angles. 62 """ 63 # TODO: replace by a vector class? 64 turn = toAng - fromAng 65 candidate1 = turn 66 candidate2 = turn - 360 67 candidate3 = turn + 360 68 absTurn2OrigTurn = { abs(candidate1) : candidate1, 69 abs(candidate2) : candidate2, 70 abs(candidate3) : candidate3 } 71 return absTurn2OrigTurn[ min([abs(candidate1), abs(candidate2), abs(candidate3)]) ]
72