robot.py 文件源码

def checkFront(self):
        "Update the front color sensor"

        # Get sensor position
        pos = delta(self.pos, vec2d(-self.radius, self.angle))

        # Sensor distance to edge of sketch
        sk = self.sketch
        if sk.weight:
            obj = sk
            prox = _distToWall(pos, self.angle, self.sensorWidth, *sk.size)
        else: obj = prox = None

        # Find closest object within sensor width
        u = vec2d(1, self.angle)
        sw = self.sensorWidth * DEG
        for gr in self.sensorObjects(sk):
            if gr is not self and gr.avgColor and hasattr(gr, "rect"):
                dr = delta(gr.rect.center, pos)
                d = hypot(*dr)
                r = gr.radius
                if r >= d:
                    prox = 0
                    obj = gr
                elif prox is None or d - r < prox:
                    minDot = cos(min(sw + asin(r/d), pi / 2))
                    x = (1 - sprod(u, dr) / d) / (1 - minDot)
                    if x < 1:
                        obj = gr
                        prox = (d - r) * (1 - x) + x * sqrt(d*d-r*r)

        # Save data
        self.closestObject = obj
        c = rgba(sk.border if obj is sk
            else obj.avgColor if obj else (0,0,0))
        self.sensorFront = noise(divAlpha(c), self.sensorNoise, 255)
        self.proximity = None if prox is None else round(prox)