How determine if a point is in scan arc of a robot?

Question

I have a robot with a scan arc starting at $p1$ and ending at $p2$. The radius of this arc is $r$ and the angular aperture is $\phi$. The rotation angle of the robot is $\theta$. How can I determine if a point $c$ is in the scan arc of the robot.

Answer 1

Obviously, the distance to $c$ must be less than or equal to $r$. For testing the angle, since you don’t care about which side of the robot the object is on, using a dot product is convenient. Recall that $\mathbf v\cdot\mathbf w = \|\mathbf v\|\,\|\mathbf w\|\cos\alpha$, where $\alpha$ is the angle between the vectors. If you take $\mathbf v=(\cos\theta,\sin\theta)$, the unit vector in the direction of the robot’s facing and $\mathbf w=c=(x_c,y_c)$, the angle $\alpha$ is then the relative bearing of the object. For the object to be in the field of vision, you must have $\cos\alpha \ge \cos\frac\phi2$. Putting this all together, the object is visible iff $${x_c\cos\theta+y_c\sin\theta \over \sqrt{x_c^2+y_c^2}}\ge\cos\frac\phi2$$ and $\sqrt{x_c^2+y_c^2}\le r$.

Answer 2

Calling $\theta$ the robot pose then the span limits are given by the directions

$$ v_{min}= e^{i(\theta + \phi_{min})}\\ v_{max} = e^{i(\theta + \phi_{max})} $$

then the condition for $c$ scanned is negative. If

$$ c = \lambda_1 v_{min}+\lambda_2 v_{max} $$

for $\lambda_1 \ge 0, \lambda_2 \ge 0 $ then $c$ is not scanned or resuming, giving $c = (x,y)$

$$ x = \lambda_1\cos(\theta + \phi_{min})+\lambda_2\cos(\theta + \phi_{max})\\ y = \lambda_1\sin(\theta + \phi_{min})+\lambda_2\sin(\theta + \phi_{max}) $$

the point is scanned if the solution for $\lambda_1,\lambda_2$ gives any sign result distinct from

$$ \lambda_1 \ge 0, \lambda_2 \ge 0 $$

and of course $\sqrt{x^2+y^2} \le r$

Answer 3

Assuming we have the $(x, y)$-coordinates of our robot's position (call this $\mathbb{a}$), we start by checking if our robot is close enough (distance less than $r$). To do this, we compute a vector between them, then check if its magnitude is less than $r$. Let the vector between them be

$$\vec{b} = \begin{bmatrix} \mathbb{c}.x - \mathbb{a}.x \\ \mathbb{c}.y - \mathbb{a}.y \end{bmatrix}$$

To compute the magnitude: $$\|\vec{b}\| = \sqrt{\vec{b}.x^2 + \vec{b}.y^2}$$

After, we can check if $r < \|\vec{b}\|$.

Now to determine whether or not $\mathbb{c}$ is within our 'field of view'. To decide that this is the case, we need the angle from $\mathbb{c}$ and our robot's direction vector $<cos(\theta), sin(\theta)>$ to be greater than the angle between $p1$ and our direction vector and to be less than the angle between $p2$ and our direction vector. To compute those angles, use the following general formula (note the reuse of symbols):

$$\theta = \text{arccos}(\frac{\vec{a} \cdot \vec{b}}{\|\vec{a}\| * \|\vec{b}\|})$$