gogurt

def doesArmTouchObjects(armPosDist, objects, isGoal=False):
    """Determine whether the given arm links touch any obstacle or goal

        Args:
            armPosDist (list): start and end position and padding distance of all arm links [(start, end, distance)]
            objects (list): x-, y- coordinate and radius of object (obstacles or goals) [(x, y, r)]
            isGoal (bool): True if the object is a goal and False if the object is an obstacle.
                           When the object is an obstacle, consider padding distance.
                           When the object is a goal, no need to consider padding distance.
        Return:
            True if touched. False if not.
    """
    if isGoal == False:
        for obstacle in objects:
            for armL in armPosDist:
                startX = armL[0][0] - obstacle[0]
                startY = armL[0][1] - obstacle[1]
                endX = armL[1][0] - obstacle[0]
                endY = armL[1][1] - obstacle[1]
                # quadratic equation combining parametric eq of line and circle eq
                A = (endX - startX)**2 + (endY - startY)**2
                B = 2*(startX*(endX - startX) + startY*(endY - startY))
                C = startX**2 + startY**2 - (obstacle[2] + armL[2])**2
                # AX^2 + BX + C
                discriminant = (B**2) - (4*A*C)
                # no real roots b/c sqrt in quadratic formula will be sq root of negative
                # print(discriminant)
                if discriminant <= 0:
                    continue
                # now solve for parametric roots
                tIntersect1 = (-B + math.sqrt(discriminant))/(2*A)
                tIntersect2 = (-B - math.sqrt(discriminant))/(2*A)
                # now we will check if t is within the constraint values for the parametric line eq
                if (tIntersect1 >= 0 and tIntersect1 <= 1) or (tIntersect2 >= 0 and tIntersect2 <=1):
                    return True

    else:
        for goal in objects:
            for armL in armPosDist:
                startX = armL[0][0] - goal[0]
                startY = armL[0][1] - goal[1]
                endX = armL[1][0] - goal[0]
                endY = armL[1][1] - goal[1]
                # quadratic equation combining parametric eq of line and circle eq
                A = (endX - startX)**2 + (endY - startY)**2
                B = 2*(startX*(endX - startX) + startY*(endY - startY))
                C = startX**2 + startY**2 - (goal[2])**2
                # AX^2 + BX + C
                discriminant = (B**2) - (4*A*C)
                # no real roots b/c sqrt in quadratic formula will be sq root of negative
                # print(discriminant)
                if discriminant <= 0:
                    continue
                # now solve for parametric roots
                tIntersect1 = (-B + math.sqrt(discriminant))/(2*A)
                tIntersect2 = (-B - math.sqrt(discriminant))/(2*A)
                # now we will check if t is within the constraint values for the parametric line eq
                if (0 <=tIntersect1  and tIntersect1 <= 1) or (0 <= tIntersect2  and tIntersect2 <=1):
                    return True

    return False