tf_conv.py 文件源码

def compute_distance_to_wall(a, r1, r2):
    """ Given a sighting that determines a distance of r1 to the wall, and then a rotation by angle *a*
        to the left and a sighting of distance r2, returns the angle that we are currently rotated by
        from the perpendicular to the wall and the current distance to the wall as a pair tuple. Angle
        is given in degrees before the rotation from r1 to r2. Rotating to the right by this angle
        should cause us to face the wall directly.
    """
    try:
        if r1 < r2:
            r = r1/r2
            i = 1.0
        elif r1 > r2:
            r = r2/r1
            i = -1.0
        else:
            return 0, r2
        d = radians(a)
        c = cos(d)
        s = sin(d)
        dt = sqrt(1 - c * c * r * r)
        x = c * c * r + s * dt
        return degrees(acos(x)) * i, r2 * x
    except ValueError:
        traceback.print_exc()
        return None, None