def planMovement(self, currentPose, nextNodes):
currentX = currentPose[0]
currentY = currentPose[1]
currentTheta = currentPose[2]
currentPoseVector = Vector.buildUnitaryVectorFromAngle(currentTheta)
moves = []
print "current pose in shuffle: " + str(currentPose)
print "path to be planned in shuffle: " + str(nextNodes)
for nextNode in nextNodes:
destinationVector = Vector.buildFromTwoPoints((currentX, currentY), (nextNode[0], nextNode[1]))
shuffleDistance = Vector.length(destinationVector)
shuffleAngle = Vector.angleBetween(currentPoseVector, destinationVector)
if shuffleAngle < 0:
shuffleAngle += 360
moves.append(Shuffle(shuffleDistance, shuffleAngle))
currentX = nextNode[0]
currentY = nextNode[1]
return moves
def determineCircle(self, angleBetweenP1AndP2, p1, p2):
distanceBetweenP1andP2 = Vector.length(Vector.buildFromTwoPoints(p1, p2))
radius = distanceBetweenP1andP2 / 2 / math.sin(math.radians(angleBetweenP1AndP2))
circle1 = Circle(p1, radius)
circle2 = Circle(p2, radius)
intersectionPoints = Circle.intersect(circle1, circle2)
center = intersectionPoints[0]
if len(intersectionPoints) == 2:
v1 = Vector.buildFromTwoPoints(intersectionPoints[0], p1)
v2 = Vector.buildFromTwoPoints(intersectionPoints[0], p2)
angle1 = Vector.angleBetween(v1, v2)
v1 = Vector.buildFromTwoPoints(intersectionPoints[1], p1)
v2 = Vector.buildFromTwoPoints(intersectionPoints[1], p2)
angle2 = Vector.angleBetween(v1, v2)
if angle1 > angle2:
center = intersectionPoints[0]
else:
center = intersectionPoints[1]
return Circle(center, radius)
def test_buildFromTwoPoints_complexCase_1(self):
p1 = (-1, -1)
p2 = (1, 1)
vector = Vector.buildFromTwoPoints(p1, p2)
self.assertEqual([2, 2], vector)
def test_buildFromTwoPoints_complexCase_2(self):
p1 = (-6, 8)
p2 = (3, 2)
vector = Vector.buildFromTwoPoints(p1, p2)
self.assertEqual([9, -6], vector)
def intersect(circle1, circle2):
x1 = circle1.center[0]
y1 = circle1.center[1]
r1 = circle1.radius
x2 = circle2.center[0]
y2 = circle2.center[1]
r2 = circle2.radius
distanceBetweenCenters = Vector.length((x2 - x1, y2 - y1))
if distanceBetweenCenters > r1 + r2 or distanceBetweenCenters < math.fabs(r1 - r2):
return []
elif distanceBetweenCenters == 0 and r1 == r2:
raise CircleException("The two circles are the same")
a = (math.pow(r1, 2) - math.pow(r2, 2) + math.pow(distanceBetweenCenters, 2)) / (2 * distanceBetweenCenters)
p2x = x1 + a * (x2 - x1) / distanceBetweenCenters
p2y = y1 + a * (y2 - y1) / distanceBetweenCenters
if a == r1:
return [(p2x, p2y)]
h = math.sqrt(math.pow(r1, 2) - math.pow(a, 2))
p3x = p2x + h*(y2 - y1)/distanceBetweenCenters
p3y = p2y - h*(x2 - x1)/distanceBetweenCenters
p4x = p2x - h*(y2 - y1)/distanceBetweenCenters
p4y = p2y + h*(x2 - x1)/distanceBetweenCenters
return [(p3x, p3y), (p4x, p4y)]
def test_buildFromTwoPoints_simpleCase(self):
p1 = (0, 0)
p2 = (1, 0)
vector = Vector.buildFromTwoPoints(p1, p2)
self.assertEqual([1, 0], vector)
def test_buildFromTwoPoints_simpleCase_3(self):
p1 = (9, 6)
p2 = (8, 6)
vector = Vector.buildFromTwoPoints(p1, p2)
self.assertEqual([-1, 0], vector)
def test_buildFromTwoPoints_simpleCase_5(self):
p1 = (8, 5)
p2 = (8, 6)
vector = Vector.buildFromTwoPoints(p1, p2)
self.assertEqual([0, 1], vector)
def test_buildFromTwoPoints_complexCase_3(self):
p1 = (26.3, -11.5)
p2 = (23.1, 2)
vector = Vector.buildFromTwoPoints(p1, p2)
self.assertAlmostEqual(-3.2, vector[0], delta=0.0000001)
self.assertEqual(13.5, vector[1])
def planMovement(self, currentPose, nextNodes, finalAbsoluteAngle):
currentX = currentPose[0]
currentY = currentPose[1]
currentTheta = currentPose[2]
moves = []
for nextNode in nextNodes:
currentPoseVector = Vector.buildUnitaryVectorFromAngle(currentTheta)
destinationVector = Vector.buildFromTwoPoints((currentX, currentY), (nextNode[0], nextNode[1]))
rotationAngle = Vector.angleBetween(currentPoseVector, destinationVector)
if rotationAngle:
moves.append(Rotate(rotationAngle))
distanceToAdvance = Vector.length(destinationVector)
if distanceToAdvance:
moves.append(Advance(distanceToAdvance))
currentX = nextNode[0]
currentY = nextNode[1]
currentTheta += rotationAngle
currentRobotVector = Vector.buildUnitaryVectorFromAngle(currentTheta)
finalRobotVector = Vector.buildUnitaryVectorFromAngle(finalAbsoluteAngle)
rotationAngle = Vector.angleBetween(currentRobotVector, finalRobotVector)
if math.fabs(rotationAngle) > 0.01:
moves.append(Rotate(rotationAngle))
return moves
def test_angleBetween_minusOneEightyDegrees(self):
self.assertEqual(180, Vector.angleBetween([-1, 0], [1, 0]))
def test_length_zero(self):
self.assertEqual(0, Vector.length([0, 0]))
def test_angleBetween_ninetyDegrees(self):
self.assertEqual(-90, Vector.angleBetween([1, 0], [0, 1]))
def test_length_one(self):
self.assertEqual(1, Vector.length([0, 1]))
def test_angleBetween_minusNinetyDegreesLargeRotation(self):
self.assertAlmostEqual(90,
Vector.angleBetween([0, -1],[-1, 0]),
delta=0.000000001)
def test_angleBetween_largeRotation(self):
expectedAngle = math.degrees(-1*math.acos(2/math.sqrt(1229)))
self.assertAlmostEqual(expectedAngle,
Vector.angleBetween([-1, 0], [-2, -35]),
delta=0.000000001)
def test_angleBetween_minusThirtyDegrees(self):
self.assertAlmostEqual(30,
Vector.angleBetween([-1 / 2, -math.sqrt(3) / 2], [-math.sqrt(3) / 2, -1 / 2]),
delta=0.000000001)
def test_angleBetween_thirtyDegrees(self):
expectedAngle = -30
self.assertAlmostEqual(expectedAngle,
Vector.angleBetween([-math.sqrt(3) / 2, -1 / 2], [-1 / 2, -math.sqrt(3) / 2]),
delta=0.000000001)
def test_angleBetween_zeroDegrees(self):
self.assertEqual(0, Vector.angleBetween([1, 1], [2, 2]))
def test_angleBetween_minusNinetyDegrees(self):
self.assertEqual(90, Vector.angleBetween([0, 1], [1, 0]))
def test_angleBetween_oneEightyDegrees(self):
self.assertEqual(-180, Vector.angleBetween([1, 0], [-1, 0]))
def test_buildUnitaryVectorFromAngle_ninety(self):
vector = Vector.buildUnitaryVectorFromAngle(90)
self.assertAlmostEqual(0, vector[0], delta=0.000001)
self.assertEqual(-1, vector[1])
def test_buildUnitaryVectorFromAngle_zero(self):
vector = Vector.buildUnitaryVectorFromAngle(0)
self.assertEqual([1, 0], vector)
def test_length_negativeValues(self):
self.assertEqual(math.sqrt(20), Vector.length([-2, -4]))
def test_length_sqrt2(self):
self.assertEqual(math.sqrt(2), Vector.length([1, 1]))
def test_buildUnitaryVectorFromAngle_210(self):
vector = Vector.buildUnitaryVectorFromAngle(-210)
self.assertAlmostEqual(-math.sqrt(3)/2, vector[0], delta=0.000001)
self.assertAlmostEqual(-0.5, vector[1], delta=0.0000001)
def test_angleBetween_minusFortyFiveDegrees(self):
expectedAngle = 45
self.assertEqual(expectedAngle, Vector.angleBetween([1, 1], [1, 0]))
