Source code for utilityFunctions

"""
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))
[docs]def clipInformed(value, minValue, maxValue): """ Clips a value to a certain range. The clipped value will remain unchanged if it fell within the range, it will be equal to the minValue if it was smaller than the minValue, and it will be equal to the maxValue if it was larger than the maxValue. Args: value(float): The value to clip. minValue(float): The lower bound of the range. maxValue(float): The upper bound of the range. Return: Tuple containing the clipped value, and a Boolean that is True if the value was clipped, and false otherwise. """ if value < minValue: return minValue, True if value > maxValue: return maxValue, True return value, False
#quad and moveTowards are used to get one object to move towards another
[docs]def quadFormula(a,b,c): """ Implementation of the quadratic formula for solving quadratic equations. Solves equations of the form ax^2 + bx + c, and returns the value of x. Args: a (float): Value of a. b (float): Value of b. c (float): Value of c. Return: List [n, s] where n is the number of solutions, and s is a list of length n containing the solutions. """ d=sqrt(pow(b,2)-4*a*c) if d<0: #print("No solution") return [0,[]] elif d==0: #print("One solution") return [1,[-b/(2*a)]] else: #print("Two solutions") return [2,[(-b+d)/(2*a),(-b-d)/(2*a)]]
[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])
[docs]def printContactList(contactList): """ Prints all contacts in the contact list. Args: contactList(:class:`.~ContactList`): The contact list to print. """ while contactList: cont = contactList.Contact print("Touch:", cont.IsTouching(), "active:", cont.Enabled, "fixA:", cont.FixtureA.Body.UserData, cont.FixtureA.Body.UserData.tag, "fixB:", cont.FixtureB.Body.UserData, cont.FixtureB.Body.UserData.tag) contactList = contactList.Next
[docs]def disableContactList(contactList): """ Call on robot.frame.body.ContactList to invalidate all contacts after moving the robot to prevent unwanted rotations. Used to stop robot from spinning after it has been stopped. Look up portal object for guide on how to use it. Args: contactList(:class:`.~ContactList`): The contact list to disable. """ while contactList: #print("Is touching:", contactList.Contact.IsTouching(), "is enabled:", #contactList.Contact.Enabled) contactList.Contact.Enabled = False contactList = contactList.Next
[docs]def cleanContactList(contactList): """ Cleans the contact list by throwing away all contacts that are no longer touching. Args: contactList(:class:`.~ContactList`): The contact list to clean. Return: Returns a contact list with all non-touching contacts removed. """ return [(_, contact) for _, contact in contactList if contact.IsTouching()]