Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
    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
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
Archivo: circle.py Proyecto: spg/JDV
    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)]
Ejemplo n.º 5
0
 def test_length_negativeValues(self):
     self.assertEqual(math.sqrt(20), Vector.length([-2, -4]))
Ejemplo n.º 6
0
 def test_length_sqrt2(self):
     self.assertEqual(math.sqrt(2), Vector.length([1, 1]))
Ejemplo n.º 7
0
 def test_length_one(self):
     self.assertEqual(1, Vector.length([0, 1]))
Ejemplo n.º 8
0
 def test_length_zero(self):
     self.assertEqual(0, Vector.length([0, 0]))