"""
This module contains a collection of helper functions for math, obtaining shape
dimensions, and manipulating contact lists.
"""
import os
from math import sqrt,copysign,pi,radians,sin,cos,acos
import types
from Graphics import *
#Doc-string style guide:
#http://google-styleguide.googlecode.com/svn/trunk/pyguide.html#Comments
[docs]def getWidth(element):
"""
Returns the width of an element.
This function exists because the width and height of different Calico shapes
has to be calculated differently. For example, Rectangles do not have a
width attribute, but the width as to be calculated by taking the absolute
difference between the two point of the Rectangle.
Args:
element(:class:`.Shape`): The element for which to get the width.
Return:
The width of the provided element.
"""
width = 0
try:
width = abs(element.getP1().x - element.getP2().x)
return width
except (IndexError, AttributeError, SystemError):
try:
width = element.width
except AttributeError:
pass
return width
[docs]def getHeight(element):
"""
Returns the height of an element.
This function exists because the width and height of different Calico shapes
has to be calculated differently. For example, Rectangles do not have a
width attribute, but the width as to be calculated by taking the absolute
difference between the two point of the Rectangle.
Args:
element(:class:`.Shape`): The element for which to get the height.
Return:
The height of the provided element.
"""
height = 0
try:
return abs(element.getP1().y - element.getP2().y)
except (IndexError, AttributeError, SystemError):
pass
try:
return element.height
except AttributeError:
pass
try:
return element.radius*2
except AttributeError:
pass
try:
return element.shapes[0].height
except (IndexError, AttributeError, ValueError):
pass
return height
[docs]def fuzzyCompare(a, b, tol=0.000000001):
"""
Tests whether two floats are equal within a certain tolerance.
Args:
a(float): The first float.
b(float): The second float.
tol(float): The tolerance.
Return:
True if a and b are within tol of eachother, False otherwise.
"""
return abs(a - b) < tol
[docs]def getAngleSides(a,b,c):
"""
Given three sides of a triangle, returns the angle.
Args:
a (float): Length of side a.
b (float): Length of side b.
c (float): Length of side c.
Return:
The angle between sides a and b.
"""
return toGrad(acos((a**2 + b**2 - c**2) / (2*a*b)))
[docs]def getAngleVector(x,y):
"""
Given a vector (x, y), calculates the angle of that vector.
Args:
x(float): The x component of the vector.
y(float): The y component of the vector.
Return:
The angle of the vector.
"""
if x < 0:
return -toGrad(acos(y / sqrt(x*x + y*y)))
else:
return toGrad(acos(y / sqrt(x*x + y*y)))
[docs]def toGrad(rad):
"""
Converts radians to degrees.
Args:
rad(float): Angle in radians.
Return:
Angle in degrees.
"""
return rad*(360/(2*pi))
#quad and moveTowards are used to get one object to move towards another
[docs]def moveDelta(eX,eY,sX,sY,minDist=10,speed=10):
"""
Calculates the values to move from one position to another at fixed speed.
Returns the dx and dy needed to move from (startX,startY) to (endX,endY) at
some small increment. Returns (None,None) if the minDist is reached.
Args:
eX(float): X coordinate of the end position.
eY(float): Y coordinate of the end position.
sX(float): X coordinate of the start position.
sY(float): Y coordinate of the start position.
minDist(float): Distance at which the function returns (None, None).
speed(float): The length of the returned vector.
Return:
Tuple (dx, dy) describing a vector of length speed towards the end
position. Returns the vector (None, None) when the start and end
positions are within minDist from each other.
"""
#speed of move
s=speed
if eX-sX==0:
return [copysign(s,eX-sX),copysign(s,eY-sY)]
else:
#slope of line between robot and ghost
m=(eY-sY)/(eX-sX)
#distance between tgt and src
D=sqrt(pow(eX-sX,2) + pow(eY-sY,2))
if D<minDist:
return [None,None]
#distance from robot that ghost will be placed at
d=D-s
#solve the equation
#d^2=y^2 + x^2 where y and x are the distance to the
#point the ghost will be placed at
#ans should produce two values which will be
#the x (run) value
ans=quadFormula(pow(m,2)+1,0,-pow(d,2))
if ans[0]==2:
nX1=eX+ans[1][0]
nX2=eX+ans[1][1]
nY1=eY+ans[1][0]*m
nY2=eY+ans[1][1]*m
#there are two possible solutions to the equation
#we want the one closer to the ghost
pD1=sqrt(pow(sX-nX1,2)+pow(sY-nY1,2))
pD2=sqrt(pow(sX-nX2,2)+pow(sY-nY2,2))
if pD1<pD2:
return [nX1-sX,nY1-sY]
else:
return [nX2-sX,nY2-sY]
[docs]def curvedShape(origin,radius,startAngle,stopAngle,convex=True,edgeThickness=10):
"""
Quick and dirty function for drawing convex and convave curves.
Warning: this function is a quick and dirty. It is prone to crashing Calico
if ill-used. I encourage someone to make something a bit more robust. RV
::
self.addObstacle(self.curvedShape([350,350],250,180,360,False,250))
self.addObstacle(self.curvedShape([350,350],150,0,359))
self.addObstacle(self.curvedShape([350,350],250,0,90,False,250))
Ways to crash the function:
1. Start and stop angles are not in the right order.
::
self.addObstacle(self.curvedShape([350,350],250,180,90,False,250))
2. I think this crashes the function because of the point at angle = 360.
Either the point is repeated or creates a backwards polygon
::
self.addObstacle(self.curvedShape([350,350],150,0,360))
Args:
origin(tuple): The center point of the curve.
radius(float): The radius of the curve.
startAngle(float): The angle at which the curve starts.
stopAngle(float): The angle at which the curve terminates.
convex(bool): Whether or not this curve is convex or concave.
edgeThickness(float): The width of the curve if concave.
Return:
:class:`~.Polygon` object in the shape of the specified curve.
"""
curve=Polygon()
angle=0
step=(2*pi)/100
while(angle<2*pi):
#print(angle)
#print(radians(180))
if (angle>=radians(startAngle)) and (angle<=radians(stopAngle)):
#print("test")
#print((r*cos(angle)+orig[0],r*sin(angle)+orig[1]))
curve.append((radius*cos(angle)+origin[0],
radius*sin(angle)+origin[1]))
#print("test1")
angle+=step
if convex:
#print("convex")
curve.append(origin)
else:
#print("concave")
angle=2*pi
radius+=edgeThickness
while(angle>0):
if (angle>=radians(startAngle)) and (angle<=radians(stopAngle)):
#print((r*cos(angle)+orig[0],r*sin(angle)+orig[1]))
curve.append((radius*cos(angle)+origin[0],
radius*sin(angle)+origin[1]))
angle-=step
return curve
[docs]def curvedShape2(origin,radius,startAngle,stopAngle,convex=True,edgeThickness=10):
"""
Function for drawing convex and convave curves.
Effectively the same as :func:`~.curvedShape`, but with some extra checks
to avoid crashing calico.
::
self.addObstacle(self.curvedShape([350,350],250,180,360,False,250))
self.addObstacle(self.curvedShape([350,350],150,0,359))
self.addObstacle(self.curvedShape([350,350],250,0,90,False,250))
Args:
origin(tuple): The center point of the curve.
radius(float): The radius of the curve.
startAngle(float): The angle at which the curve starts.
stopAngle(float): The angle at which the curve terminates.
convex(bool): Whether or not this curve is convex or concave.
edgeThickness(float): The width of the curve if concave.
Return:
:class:`~.Polygon` object in the shape of the specified curve.
"""
try:
curve=Polygon()
angle=0
step=(2*pi)/100
if stopAngle<startAngle:
stopAngle=stopAngle+360
angle=radians(startAngle)
#print(angle,radians(startAngle),radians(stopAngle))
while(angle<radians(stopAngle)):
#print("Angle 1 is ",angle)
#print(angle,radius*cos(angle)+origin[0],radius*sin(angle)+origin[1])
curve.append((radius*cos(angle)+origin[0],
radius*sin(angle)+origin[1]))
angle+=step
if convex:
curve.append(origin)
else:
radius+=edgeThickness
angle=radians(stopAngle)
while(angle>radians(startAngle)):
curve.append((radius*cos(angle)+origin[0],
radius*sin(angle)+origin[1]))
angle-=step
return curve
except:
print("Error with curvedShape2 funciton in utilityFuntions.py")
return None
[docs]def makeList(objectOrList):
"""
Ensures that the returned object is a list.
If the passed object was already a list (or tuple), returns the object
unmodified. If the object was not a list, this function will return a list
of length one, containing that object.
Args:
objectOrList(any): The object that needs to be a list.
Return:
The original list if the passed object was a list, or a list containing
the passed object.
"""
if isinstance(objectOrList, str) or not hasattr(objectOrList, "__iter__"):
return [objectOrList]
return objectOrList
[docs]def dist(a,b):
"""
Calculates the euclidean distance between point a and point b.
Args:
a(tuple): Point a.
b(tuple): Point b.
Return:
The euclidean distance between points a and b.
"""
return sqrt( pow(a[0]-b[0],2)+pow(a[1]-b[1],2) )
[docs]def mag(v):
"""
Calculates the length of a vector.
Args:
v(tuple): The vector for which to calculate the length.
Return:
The length of vector v.
"""
a=0
for i in v:
a+=i*i
return sqrt(a)
[docs]def norm(v):
"""
Normalizes vector v.
Normalization simply means making the vector of length 1. Crashes if the
original vector has lenght 0.
Args:
v(tuple): The vector to normalize.
Return:
The normalized vector.
"""
a=[]
m=mag(v)
for i in v:
a.append(i/m)
return a
#Function that moves shape from start to end at a certain speed.
#Blocks until shape has reached end location
[docs]def moveToLocBlock(shape,end,speed=10):
"""
Function that moves shape from start to end at a certain speed.
Blocks until the shape has reached the end location. Function relies on the
physics simulator being active in the background, and it will never return
if the phsyics simulator is not active (or blocked), or if the shape is
unable to reach the end location because of intervening obstacles.
Args:
shape(:class:`~.Shape`): The shape to move.
end(tuple): The position to move the shape to.
speed(float): The speed with which to move the shape.
"""
atEnd=False
start=(shape.x,shape.y)
distance=[end[0]-start[0],end[1]-start[1]] #distance to end of x and y
direction=norm(distance) #normalize the distance to get a vector of the direction to go
while not atEnd:
if distance[0]==0: #avoid dividing by 0
if (shape.y-start[1])/distance[1] >= 1:
atEnd=True
elif distance[1]==0: #avoid divinding by 0
if (shape.x-start[0])/distance[0] >= 1:
atEnd=True
else:
if fabs((shape.x-start[0])/distance[0]) >=1 and fabs((shape.y-start[1])/distance[1]) >=1:
atEnd=True
if atEnd:
shape.body.LinearVelocity = Vector(0,0)
else:
shape.body.LinearVelocity = Vector(speed*direction[0], speed*direction[1])