<s> import math <EOL> import numpy <EOL> import random <EOL> import operator <EOL> import types <EOL> import numpy as np <EOL> class Transform ( object ) : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self , matrix ) : <EOL> self . matrix = numpy . array ( matrix ) <EOL> self . matrix_inv = None <EOL> self . zRot = False <EOL> def inverse ( self ) : <EOL> """<STR_LIT>""" <EOL> if self . matrix_inv == None : <EOL> self . matrix_inv = numpy . linalg . inv ( self . matrix ) <EOL> return Transform ( self . matrix_inv ) <EOL> def __neg__ ( self ) : <EOL> return self . inverse ( ) <EOL> def compose ( self , trans ) : <EOL> """<STR_LIT>""" <EOL> tr = Transform ( numpy . dot ( self . matrix , trans . matrix ) ) <EOL> if self . zRot and trans . zRot : <EOL> return tr . pose ( ) <EOL> else : <EOL> return tr <EOL> def __mul__ ( self , other ) : <EOL> return self . compose ( other ) <EOL> def pose ( self , zthr = <NUM_LIT> , fail = True ) : <EOL> """<STR_LIT>""" <EOL> if abs ( <NUM_LIT:1> - self . matrix [ <NUM_LIT:2> ] [ <NUM_LIT:2> ] ) < zthr : <EOL> theta = math . atan2 ( self . matrix [ <NUM_LIT:1> ] [ <NUM_LIT:0> ] , self . matrix [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] ) <EOL> return Pose ( self . matrix [ <NUM_LIT:0> ] [ <NUM_LIT:3> ] , self . matrix [ <NUM_LIT:1> ] [ <NUM_LIT:3> ] , self . matrix [ <NUM_LIT:2> ] [ <NUM_LIT:3> ] , theta ) <EOL> elif fail : <EOL> print self . matrix <EOL> raise Exception , "<STR_LIT>" <EOL> else : <EOL> return None <EOL> def point ( self ) : <EOL> return self . pose ( ) . point ( ) <EOL> def applyToPoint ( self , point ) : <EOL> """<STR_LIT>""" <EOL> p = numpy . dot ( self . matrix , point . matrix ( ) ) <EOL> return Point ( p [ <NUM_LIT:0> ] , p [ <NUM_LIT:1> ] , p [ <NUM_LIT:2> ] , p [ <NUM_LIT:3> ] ) <EOL> def __call__ ( self , point ) : <EOL> return self . applyToPoint ( point ) <EOL> def __repr__ ( self ) : <EOL> return str ( self . matrix ) <EOL> def shortStr ( self , trim = False ) : <EOL> return self . __repr__ ( ) <EOL> __str__ = __repr__ <EOL> class Pose ( Transform ) : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self , x , y , z , theta ) : <EOL> self . x = x <EOL> """<STR_LIT>""" <EOL> self . y = y <EOL> """<STR_LIT>""" <EOL> self . z = z <EOL> """<STR_LIT>""" <EOL> self . theta = fixAngle02Pi ( theta ) <EOL> """<STR_LIT>""" <EOL> self . initTrans ( ) <EOL> self . zRot = True <EOL> def initTrans ( self ) : <EOL> cosTh = math . cos ( self . theta ) <EOL> sinTh = math . sin ( self . theta ) <EOL> self . reprString = None <EOL> Transform . __init__ ( self , [ [ cosTh , - sinTh , <NUM_LIT:0.0> , self . x ] , <EOL> [ sinTh , cosTh , <NUM_LIT:0.0> , self . y ] , <EOL> [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> , self . z ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ] ) <EOL> def setX ( self , x ) : <EOL> self . x = x <EOL> self . initTrans ( ) <EOL> def setY ( self , y ) : <EOL> self . y = y <EOL> self . initTrans ( ) <EOL> def setZ ( self , z ) : <EOL> self . z = z <EOL> self . initTrans ( ) <EOL> def setTheta ( self , theta ) : <EOL> self . theta = theta <EOL> self . initTrans ( ) <EOL> def average ( self , other , alpha ) : <EOL> """<STR_LIT>""" <EOL> return Pose ( alpha * self . x + ( <NUM_LIT:1> - alpha ) * other . x , <EOL> alpha * self . y + ( <NUM_LIT:1> - alpha ) * other . y , <EOL> alpha * self . z + ( <NUM_LIT:1> - alpha ) * other . z , <EOL> angleAverage ( self . theta , other . theta , alpha ) ) <EOL> def point ( self ) : <EOL> """<STR_LIT>""" <EOL> return Point ( self . x , self . y , self . z ) <EOL> def pose ( self , fail = False ) : <EOL> return self <EOL> def near ( self , pose , distEps , angleEps ) : <EOL> """<STR_LIT>""" <EOL> return self . point ( ) . isNear ( pose . point ( ) , distEps ) and nearAngle ( self . theta , pose . pose ( ) . theta , angleEps ) <EOL> def diff ( self , pose ) : <EOL> """<STR_LIT>""" <EOL> return Pose ( self . x - pose . x , <EOL> self . y - pose . y , <EOL> self . z - pose . z , <EOL> fixAnglePlusMinusPi ( self . theta - pose . theta ) ) <EOL> def distance ( self , pose ) : <EOL> """<STR_LIT>""" <EOL> return self . point ( ) . distance ( pose . point ( ) ) <EOL> def totalDist ( self , pose , angleScale = <NUM_LIT:1> ) : <EOL> return self . distance ( pose ) + abs ( fixAnglePlusMinusPi ( self . theta - pose . theta ) ) * angleScale <EOL> def inverse ( self ) : <EOL> """<STR_LIT>""" <EOL> return super ( Pose , self ) . inverse ( ) . pose ( ) <EOL> def xyztTuple ( self ) : <EOL> """<STR_LIT>""" <EOL> return ( self . x , self . y , self . z , self . theta ) <EOL> def corrupt ( self , e , eAng = None ) : <EOL> def corrupt ( x , e ) : <EOL> return x + random . uniform ( - e , e ) <EOL> eAng = eAng or e <EOL> return Pose ( corrupt ( self . x , e ) , corrupt ( self . y , e ) , corrupt ( self . z , e ) , <EOL> fixAnglePlusMinusPi ( corrupt ( self . theta , eAng ) ) ) <EOL> def corruptGauss ( self , mu , sigma , noZ = False ) : <EOL> def corrupt ( x ) : <EOL> return x + random . gauss ( mu , sigma ) <EOL> return Pose ( corrupt ( self . x ) , corrupt ( self . y ) , <EOL> self . z if noZ else corrupt ( self . z ) , <EOL> fixAnglePlusMinusPi ( corrupt ( self . theta ) ) ) <EOL> def __repr__ ( self ) : <EOL> if not self . reprString : <EOL> self . reprString = '<STR_LIT>' + prettyString ( self . x ) + '<STR_LIT:U+002CU+0020>' + prettyString ( self . y ) + '<STR_LIT:U+002CU+0020>' + prettyString ( self . z ) + '<STR_LIT:U+002CU+0020>' + ( prettyString ( self . theta ) if self . theta <= <NUM_LIT> else prettyString ( <NUM_LIT:0.0> ) ) + '<STR_LIT:]>' <EOL> return self . reprString <EOL> def shortStr ( self , trim = False ) : <EOL> return self . __repr__ ( ) <EOL> def __eq__ ( self , other ) : <EOL> return str ( self ) == str ( other ) <EOL> def __hash__ ( self ) : <EOL> return str ( self ) . __hash__ ( ) <EOL> __str__ = __repr__ <EOL> class Point : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self , x , y , z , w = <NUM_LIT:1.0> ) : <EOL> self . x = x <EOL> """<STR_LIT>""" <EOL> self . y = y <EOL> """<STR_LIT>""" <EOL> self . z = z <EOL> """<STR_LIT>""" <EOL> self . w = w <EOL> """<STR_LIT>""" <EOL> def matrix ( self ) : <EOL> return numpy . array ( [ self . x , self . y , self . z , self . w ] ) <EOL> def isNear ( self , point , distEps ) : <EOL> """<STR_LIT>""" <EOL> return self . distance ( point ) < distEps <EOL> def distance ( self , point ) : <EOL> """<STR_LIT>""" <EOL> dx = self . x - point . x <EOL> dy = self . y - point . y <EOL> dz = self . z - point . z <EOL> return math . sqrt ( dx * dx + dy * dy + dz * dz ) <EOL> def distanceXY ( self , point ) : <EOL> """<STR_LIT>""" <EOL> return math . sqrt ( ( self . x - point . x ) ** <NUM_LIT:2> + ( self . y - point . y ) ** <NUM_LIT:2> ) <EOL> def distanceSq ( self , point ) : <EOL> """<STR_LIT>""" <EOL> dx = self . x - point . x <EOL> dy = self . y - point . y <EOL> dz = self . z - point . z <EOL> return dx * dx + dy * dy + dz * dz <EOL> def distanceSqXY ( self , point ) : <EOL> """<STR_LIT>""" <EOL> dx = self . x - point . x <EOL> dy = self . y - point . y <EOL> return dx * dx + dy * dy <EOL> def magnitude ( self ) : <EOL> """<STR_LIT>""" <EOL> return math . sqrt ( self . x ** <NUM_LIT:2> + self . y ** <NUM_LIT:2> + self . z ** <NUM_LIT:2> ) <EOL> def xyzTuple ( self ) : <EOL> """<STR_LIT>""" <EOL> return ( self . x , self . y , self . z ) <EOL> def pose ( self , angle = <NUM_LIT:0.0> ) : <EOL> """<STR_LIT>""" <EOL> return Pose ( self . x , self . y , self . z , angle ) <EOL> def point ( self ) : <EOL> """<STR_LIT>""" <EOL> return self <EOL> def __repr__ ( self ) : <EOL> if self . w == <NUM_LIT:1> : <EOL> return '<STR_LIT>' + prettyString ( self . xyzTuple ( ) ) <EOL> if self . w == <NUM_LIT:0> : <EOL> return '<STR_LIT>' + prettyString ( self . xyzTuple ( ) ) <EOL> else : <EOL> return '<STR_LIT>' + prettyString ( self . xyzTuple ( ) + ( self . w , ) ) <EOL> def shortStr ( self , trim = False ) : <EOL> return self . __repr__ ( ) <EOL> def angleToXY ( self , p ) : <EOL> """<STR_LIT>""" <EOL> dx = p . x - self . x <EOL> dy = p . y - self . y <EOL> return math . atan2 ( dy , dx ) <EOL> def add ( self , point ) : <EOL> """<STR_LIT>""" <EOL> return Point ( self . x + point . x , self . y + point . y , self . z + point . z ) <EOL> def __add__ ( self , point ) : <EOL> return self . add ( point ) <EOL> def sub ( self , point ) : <EOL> """<STR_LIT>""" <EOL> return Point ( self . x - point . x , self . y - point . y , self . z - point . z ) <EOL> def __sub__ ( self , point ) : <EOL> return self . sub ( point ) <EOL> def scale ( self , s ) : <EOL> """<STR_LIT>""" <EOL> return Point ( self . x * s , self . y * s , self . z * s ) <EOL> def __rmul__ ( self , s ) : <EOL> return self . scale ( s ) <EOL> def dot ( self , p ) : <EOL> """<STR_LIT>""" <EOL> return self . x * p . x + self . y * p . y + self . z * p . z <EOL> class LineXY : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self , p1 , p2 ) : <EOL> """<STR_LIT>""" <EOL> self . theta = p1 . angleToXY ( p2 ) <EOL> """<STR_LIT>""" <EOL> self . nx = - math . sin ( self . theta ) <EOL> """<STR_LIT>""" <EOL> self . ny = math . cos ( self . theta ) <EOL> """<STR_LIT>""" <EOL> self . off = p1 . x * self . nx + p1 . y * self . ny <EOL> """<STR_LIT>""" <EOL> def pointOnLine ( self , p , eps ) : <EOL> """<STR_LIT>""" <EOL> dist = abs ( p . x * self . nx + p . y * self . ny - self . off ) <EOL> return dist < eps <EOL> def __repr__ ( self ) : <EOL> return '<STR_LIT>' + prettyString ( ( self . nx , self . ny , self . off ) ) <EOL> def shortStr ( self , trim = False ) : <EOL> return self . __repr__ ( ) <EOL> class LineSeg ( LineXY ) : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self , p1 , p2 ) : <EOL> """<STR_LIT>""" <EOL> self . B = p1 <EOL> """<STR_LIT>""" <EOL> self . C = p2 <EOL> """<STR_LIT>""" <EOL> self . M = p2 - p1 <EOL> """<STR_LIT>""" <EOL> LineXY . __init__ ( self , p1 , p2 ) <EOL> """<STR_LIT>""" <EOL> def closestPoint ( self , p ) : <EOL> """<STR_LIT>""" <EOL> t0 = self . M . dot ( p - self . B ) / self . M . dot ( self . M ) <EOL> if t0 <= <NUM_LIT:0> : <EOL> return self . B <EOL> elif t0 >= <NUM_LIT:1> : <EOL> return self . B + self . M <EOL> else : <EOL> return self . B + t0 * self . M <EOL> def distToPoint ( self , p ) : <EOL> """<STR_LIT>""" <EOL> return p . distance ( self . closestPoint ( p ) ) <EOL> def __repr__ ( self ) : <EOL> return '<STR_LIT>' + prettyString ( ( self . B , self . M ) ) <EOL> def localToGlobal ( pose , point ) : <EOL> return pose . transformPoint ( point ) <EOL> def localPoseToGlobalPose ( pose1 , pose2 ) : <EOL> return pose1 . compose ( pose2 ) <EOL> def globalToLocal ( pose , point ) : <EOL> return pose . inverse ( ) . transformPoint ( point ) <EOL> def globalPoseToLocalPose ( pose1 , pose2 ) : <EOL> return pose1 . inverse ( ) . compose ( pose2 ) <EOL> def sum ( items ) : <EOL> """<STR_LIT>""" <EOL> if len ( items ) == <NUM_LIT:0> : <EOL> return <NUM_LIT:0> <EOL> else : <EOL> result = items [ <NUM_LIT:0> ] <EOL> for item in items [ <NUM_LIT:1> : ] : <EOL> result += item <EOL> return result <EOL> def smash ( lists ) : <EOL> return [ item for sublist in lists for item in sublist ] <EOL> def within ( v1 , v2 , eps ) : <EOL> """<STR_LIT>""" <EOL> return abs ( v1 - v2 ) < eps <EOL> def nearAngle ( a1 , a2 , eps ) : <EOL> """<STR_LIT>""" <EOL> return abs ( fixAnglePlusMinusPi ( a1 - a2 ) ) < eps <EOL> def nearlyEqual ( x , y ) : <EOL> """<STR_LIT>""" <EOL> return abs ( x - y ) < <NUM_LIT> <EOL> def fixAnglePlusMinusPi ( a ) : <EOL> """<STR_LIT>""" <EOL> pi2 = <NUM_LIT> * math . pi <EOL> while abs ( a ) > math . pi : <EOL> if a > math . pi : <EOL> a = a - pi2 <EOL> elif a < - math . pi : <EOL> a = a + pi2 <EOL> return a <EOL> def fixAngle02Pi ( a ) : <EOL> """<STR_LIT>""" <EOL> pi2 = <NUM_LIT> * math . pi <EOL> while a < <NUM_LIT:0> or a > pi2 : <EOL> if a < <NUM_LIT:0> : <EOL> a = a + pi2 <EOL> elif a > pi2 : <EOL> a = a - pi2 <EOL> return a <EOL> def reverseCopy ( items ) : <EOL> """<STR_LIT>""" <EOL> itemCopy = items [ : ] <EOL> itemCopy . reverse ( ) <EOL> return itemCopy <EOL> def dotProd ( a , b ) : <EOL> """<STR_LIT>""" <EOL> return sum ( [ ai * bi for ( ai , bi ) in zip ( a , b ) ] ) <EOL> def argmax ( l , f ) : <EOL> """<STR_LIT>""" <EOL> vals = [ f ( x ) for x in l ] <EOL> return l [ vals . index ( max ( vals ) ) ] <EOL> def argmaxWithVal ( l , f ) : <EOL> """<STR_LIT>""" <EOL> best = l [ <NUM_LIT:0> ] ; bestScore = f ( best ) <EOL> for x in l : <EOL> xScore = f ( x ) <EOL> if xScore > bestScore : <EOL> best , bestScore = x , xScore <EOL> return ( best , bestScore ) <EOL> def argmaxIndex ( l , f = lambda x : x ) : <EOL> """<STR_LIT>""" <EOL> best = <NUM_LIT:0> ; bestScore = f ( l [ best ] ) <EOL> for i in range ( len ( l ) ) : <EOL> xScore = f ( l [ i ] ) <EOL> if xScore > bestScore : <EOL> best , bestScore = i , xScore <EOL> return ( best , bestScore ) <EOL> def argmaxIndexWithTies ( l , f = lambda x : x ) : <EOL> """<STR_LIT>""" <EOL> best = [ ] ; bestScore = f ( l [ <NUM_LIT:0> ] ) <EOL> for i in range ( len ( l ) ) : <EOL> xScore = f ( l [ i ] ) <EOL> if xScore > bestScore : <EOL> best , bestScore = [ i ] , xScore <EOL> elif xScore == bestScore : <EOL> best , bestScore = best + [ i ] , xScore <EOL> return ( best , bestScore ) <EOL> def randomMultinomial ( dist ) : <EOL> """<STR_LIT>""" <EOL> r = random . random ( ) <EOL> for i in range ( len ( dist ) ) : <EOL> r = r - dist [ i ] <EOL> if r < <NUM_LIT:0.0> : <EOL> return i <EOL> return "<STR_LIT>" <EOL> def clip ( v , vMin , vMax ) : <EOL> """<STR_LIT>""" <EOL> try : <EOL> return [ clip ( x , vMin , vMax ) for x in v ] <EOL> except TypeError : <EOL> if vMin == None : <EOL> if vMax == None : <EOL> return v <EOL> else : <EOL> return min ( v , vMax ) <EOL> else : <EOL> if vMax == None : <EOL> return max ( v , vMin ) <EOL> else : <EOL> return max ( min ( v , vMax ) , vMin ) <EOL> def flatten ( M ) : <EOL> """<STR_LIT>""" <EOL> return np . reshape ( M , ( M . shape [ <NUM_LIT:0> ] * M . shape [ <NUM_LIT:1> ] ) ) <EOL> def sign ( x ) : <EOL> """<STR_LIT>""" <EOL> if x > <NUM_LIT:0.0> : <EOL> return <NUM_LIT:1> <EOL> elif x == <NUM_LIT:0.0> : <EOL> return <NUM_LIT:0> <EOL> else : <EOL> return - <NUM_LIT:1> <EOL> def make2DArray ( dim1 , dim2 , initValue ) : <EOL> """<STR_LIT>""" <EOL> result = [ ] <EOL> for i in range ( dim1 ) : <EOL> result = result + [ makeVector ( dim2 , initValue ) ] <EOL> return result <EOL> def make2DArrayFill ( dim1 , dim2 , initFun ) : <EOL> """<STR_LIT>""" <EOL> result = [ ] <EOL> for i in range ( dim1 ) : <EOL> result = result + [ makeVectorFill ( dim2 , lambda j : initFun ( i , j ) ) ] <EOL> return result <EOL> def make3DArray ( dim1 , dim2 , dim3 , initValue ) : <EOL> """<STR_LIT>""" <EOL> result = [ ] <EOL> for i in range ( dim1 ) : <EOL> result = result + [ make2DArray ( dim2 , dim3 , initValue ) ] <EOL> return result <EOL> def mapArray3D ( array , f ) : <EOL> """<STR_LIT>""" <EOL> for i in range ( len ( array ) ) : <EOL> for j in range ( len ( array [ <NUM_LIT:0> ] ) ) : <EOL> for k in range ( len ( array [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] ) ) : <EOL> array [ i ] [ j ] [ k ] = f ( array [ i ] [ j ] [ k ] ) <EOL> def makeVector ( dim , initValue ) : <EOL> """<STR_LIT>""" <EOL> return [ initValue ] * dim <EOL> def makeVectorFill ( dim , initFun ) : <EOL> """<STR_LIT>""" <EOL> return [ initFun ( i ) for i in range ( dim ) ] <EOL> def prettyString ( struct ) : <EOL> """<STR_LIT>""" <EOL> if type ( struct ) == list : <EOL> return '<STR_LIT:[>' + '<STR_LIT:U+002CU+0020>' . join ( [ prettyString ( item ) for item in struct ] ) + '<STR_LIT:]>' <EOL> elif type ( struct ) == tuple : <EOL> return '<STR_LIT:(>' + '<STR_LIT:U+002CU+0020>' . join ( [ prettyString ( item ) for item in struct ] ) + '<STR_LIT:)>' <EOL> elif type ( struct ) == dict : <EOL> return '<STR_LIT:{>' + '<STR_LIT:U+002CU+0020>' . join ( [ str ( item ) + '<STR_LIT::>' + prettyString ( struct [ item ] ) for item in struct ] ) + '<STR_LIT:}>' <EOL> elif type ( struct ) == float or type ( struct ) == numpy . float64 : <EOL> struct = round ( struct , <NUM_LIT:3> ) <EOL> if struct == <NUM_LIT:0> : struct = <NUM_LIT:0> <EOL> return "<STR_LIT>" % struct <EOL> else : <EOL> return str ( struct ) <EOL> def swapRange ( x , y ) : <EOL> if x < y : <EOL> return range ( x , y ) <EOL> if x > y : <EOL> r = range ( y , x ) <EOL> r . reverse ( ) <EOL> return r <EOL> return [ x ] <EOL> def avg ( a , b ) : <EOL> if type ( a ) in ( types . TupleType , types . ListType ) and type ( b ) in ( types . TupleType , types . ListType ) and len ( a ) == len ( b ) : <EOL> return tuple ( [ avg ( a [ i ] , b [ i ] ) for i in range ( len ( a ) ) ] ) <EOL> else : <EOL> return ( a + b ) / <NUM_LIT> <EOL> def recoverPath ( volume , start , end ) : <EOL> if not volume : <EOL> return None <EOL> p = [ ] <EOL> current = start <EOL> while current != end : <EOL> p . append ( current ) <EOL> successors = [ [ current [ <NUM_LIT:0> ] + i , current [ <NUM_LIT:1> ] + j ] for ( i , j ) in [ ( <NUM_LIT:1> , <NUM_LIT:0> ) , ( <NUM_LIT:0> , <NUM_LIT:1> ) , ( - <NUM_LIT:1> , <NUM_LIT:0> ) , ( <NUM_LIT:0> , - <NUM_LIT:1> ) ] ] <EOL> for v in volume : <EOL> if list ( v ) in successors and not list ( v ) in p : <EOL> current = list ( v ) <EOL> continue <EOL> p . append ( end ) <EOL> return p <EOL> class SymbolGenerator : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self ) : <EOL> self . count = <NUM_LIT:0> <EOL> def gensym ( self , prefix = '<STR_LIT:i>' ) : <EOL> self . count += <NUM_LIT:1> <EOL> return prefix + '<STR_LIT:_>' + str ( self . count ) <EOL> gensym = SymbolGenerator ( ) . gensym <EOL> """<STR_LIT>""" <EOL> def logGaussian ( x , mu , sigma ) : <EOL> """<STR_LIT>""" <EOL> return - ( ( x - mu ) ** <NUM_LIT:2> / ( <NUM_LIT:2> * sigma ** <NUM_LIT:2> ) ) - math . log ( sigma * math . sqrt ( <NUM_LIT:2> * math . pi ) ) <EOL> def gaussian ( x , mu , sigma ) : <EOL> """<STR_LIT>""" <EOL> return math . exp ( - ( ( x - mu ) ** <NUM_LIT:2> / ( <NUM_LIT:2> * sigma ** <NUM_LIT:2> ) ) ) / ( sigma * math . sqrt ( <NUM_LIT:2> * math . pi ) ) <EOL> def lineIndices ( ( i0 , j0 ) , ( i1 , j1 ) ) : <EOL> """<STR_LIT>""" <EOL> ans = [ ( i0 , j0 ) ] <EOL> di = i1 - i0 <EOL> dj = j1 - j0 <EOL> t = <NUM_LIT:0.5> <EOL> if abs ( di ) > abs ( dj ) : <EOL> m = float ( dj ) / float ( di ) <EOL> t += j0 <EOL> if di < <NUM_LIT:0> : di = - <NUM_LIT:1> <EOL> else : di = <NUM_LIT:1> <EOL> m *= di <EOL> while ( i0 != i1 ) : <EOL> i0 += di <EOL> t += m <EOL> ans . append ( ( i0 , int ( t ) ) ) <EOL> else : <EOL> if dj != <NUM_LIT:0> : <EOL> m = float ( di ) / float ( dj ) <EOL> t += i0 <EOL> if dj < <NUM_LIT:0> : dj = - <NUM_LIT:1> <EOL> else : dj = <NUM_LIT:1> <EOL> m *= dj <EOL> while j0 != j1 : <EOL> j0 += dj <EOL> t += m <EOL> ans . append ( ( int ( t ) , j0 ) ) <EOL> return ans <EOL> def angleDiff ( x , y ) : <EOL> twoPi = <NUM_LIT:2> * math . pi <EOL> z = ( x - y ) % twoPi <EOL> if z > math . pi : <EOL> return z - twoPi <EOL> else : <EOL> return z <EOL> def inRange ( v , r ) : <EOL> return r [ <NUM_LIT:0> ] <= v <= r [ <NUM_LIT:1> ] <EOL> def rangeOverlap ( r1 , r2 ) : <EOL> return r2 [ <NUM_LIT:0> ] <= r1 [ <NUM_LIT:1> ] and r1 [ <NUM_LIT:0> ] <= r2 [ <NUM_LIT:1> ] <EOL> def rangeIntersect ( r1 , r2 ) : <EOL> return ( max ( r1 [ <NUM_LIT:0> ] , r2 [ <NUM_LIT:0> ] ) , min ( r1 [ <NUM_LIT:1> ] , r2 [ <NUM_LIT:1> ] ) ) <EOL> def average ( stuff ) : <EOL> return ( <NUM_LIT:1.> / float ( len ( stuff ) ) ) * sum ( stuff ) <EOL> def tuplify ( x ) : <EOL> if isIterable ( x ) : <EOL> return tuple ( [ tuplify ( y ) for y in x ] ) <EOL> else : <EOL> return x <EOL> def squash ( listOfLists ) : <EOL> return reduce ( operator . add , listOfLists ) <EOL> def angleAverage ( th1 , th2 , alpha ) : <EOL> return math . atan2 ( alpha * math . sin ( th1 ) + ( <NUM_LIT:1> - alpha ) * math . sin ( th2 ) , <EOL> alpha * math . cos ( th1 ) + ( <NUM_LIT:1> - alpha ) * math . cos ( th2 ) ) <EOL> def floatRange ( lo , hi , stepsize ) : <EOL> """<STR_LIT>""" <EOL> if stepsize == <NUM_LIT:0> : <EOL> print '<STR_LIT>' <EOL> result = [ ] <EOL> v = lo <EOL> while v <= hi : <EOL> result . append ( v ) <EOL> v += stepsize <EOL> return result <EOL> def euclideanDistance ( x , y ) : <EOL> return math . sqrt ( sum ( [ ( xi - yi ) ** <NUM_LIT:2> for ( xi , yi ) in zip ( x , y ) ] ) ) <EOL> def pop ( x ) : <EOL> if isinstance ( x , list ) : <EOL> if len ( x ) > <NUM_LIT:0> : <EOL> return x . pop ( <NUM_LIT:0> ) <EOL> else : <EOL> return None <EOL> else : <EOL> try : <EOL> return x . next ( ) <EOL> except StopIteration : <EOL> return None <EOL> def isIterable ( x ) : <EOL> if type ( x ) in ( str , unicode ) : <EOL> return False <EOL> try : <EOL> x_iter = iter ( x ) <EOL> return True <EOL> except : <EOL> return False <EOL> def tangentSpaceAdd ( a , b ) : <EOL> res = a + b <EOL> for i in range ( <NUM_LIT:3> , len ( res ) , <NUM_LIT:4> ) : <EOL> res [ i , <NUM_LIT:0> ] = fixAnglePlusMinusPi ( res [ i , <NUM_LIT:0> ] ) <EOL> return res <EOL> def scalarMult ( l , c ) : <EOL> return type ( l ) ( [ i * c for i in l ] ) <EOL> def componentAdd ( a , b ) : <EOL> return type ( a ) ( [ i + j for ( i , j ) in zip ( a , b ) ] ) <EOL> def componentSubtract ( a , b ) : <EOL> return componentAdd ( a , [ - <NUM_LIT:1> * i for i in b ] ) </s>
