Exemple #1
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)
Exemple #2
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
Exemple #3
0
    def test_buildFromTwoPoints_simpleCase_3(self):
        p1 = (9, 6)
        p2 = (8, 6)

        vector = Vector.buildFromTwoPoints(p1, p2)

        self.assertEqual([-1, 0], vector)
Exemple #4
0
    def test_buildFromTwoPoints_simpleCase_5(self):
        p1 = (8, 5)
        p2 = (8, 6)

        vector = Vector.buildFromTwoPoints(p1, p2)

        self.assertEqual([0, 1], vector)
Exemple #5
0
    def test_buildFromTwoPoints_simpleCase(self):
        p1 = (0, 0)
        p2 = (1, 0)

        vector = Vector.buildFromTwoPoints(p1, p2)

        self.assertEqual([1, 0], vector)
Exemple #6
0
    def test_buildFromTwoPoints_complexCase_2(self):
        p1 = (-6, 8)
        p2 = (3, 2)

        vector = Vector.buildFromTwoPoints(p1, p2)

        self.assertEqual([9, -6], vector)
Exemple #7
0
    def test_buildFromTwoPoints_complexCase_1(self):
        p1 = (-1, -1)
        p2 = (1, 1)

        vector = Vector.buildFromTwoPoints(p1, p2)

        self.assertEqual([2, 2], vector)
Exemple #8
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
Exemple #9
0
    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])