![enter image description here]()
Vamos a introducir un nuevo sistema de coordenadas $x'Oy'$ , como se muestra en la Fig. 1. Si somos capaces de calcular las coordenadas del punto $P(x'_P, y'_P)$ en que coordinar systen - hemos terminado porque:
$$x_O=-x'_P, \ \ y_O=-y'_P\tag{1}$$
Paso 1:
Encontrar las coordenadas de los puntos de $A,B,C,D$ en $x'Oy'$ sistema de coordenadas:
$$A: x'_1=d_1\cos\alpha, \ y'_1=d_1\sin\alpha$$
$$D: x'_2=d_4\cos(\alpha+90^\circ), \ y'_2=d_4\sin(\alpha+90^\circ)$$
$$B: x'_3=d_3\cos(\alpha+180^\circ), \ y'_3=d_3\sin(\alpha+180^\circ)$$
$$C: x'_4=d_2\cos(\alpha+270^\circ), \ y'_4=d_2\sin(\alpha+270^\circ)$$
Paso 2:
Calcular los siguientes valores:
$$x'_{\min}=\min(x_1, x_2, x_3, x_4)$$
$$x'_{\max}=\max(x_1, x_2, x_3, x_4)$$
$$y'_{\min}=\min(y_1, y_2, y_3, y_4)$$
$$y'_{\max}=\max(y_1, y_2, y_3, y_4)$$
Paso 3:
Encontrar la esquina inferior izquierda $P'(x'_{P'}, y'_{P'})$ del rectángulo delimitador (se muestra en color azul) y calcular la anchura $w$ y la altura de la $h$ del rectángulo delimitador:
$$x'_{P'}=x'_\min$$
$$y'_{P'}=y'_\min$$
$$w=x'_\max-x'_\min$$
$$h=y'_\max-y'_\min$$
Paso 4:
Si $h=w=L$ hemos terminado (ver Fig. 2) porque el rectángulo azul es en realidad nuestro delimitador cuadrado de la longitud de la $L$, los puntos de $P$ e $P'$ son idénticas y $x'_{P}=x'_{P'}$, $y'_{P}=y'_{P'}$. Sólo se aplica (1) y $x_O, y_O$.
Paso 5:
Solo es necesario si $w<L$. En este caso, el rectángulo azul es más estrecha que la de la plaza y es posible que los puntos de $P$ e $P'$ no son idénticos. En otras palabras, usted tiene que ampliar el rectángulo azul, ya sea a la izquierda o a la derecha para que coincida con el original de la plaza.
Si el rectángulo azul ha de ser ampliada a la derecha, que no afecte a la posición de punto de $P'$ y en este caso:
$$x'_P=x'_{P'}$$
El punto de $P'$ se mueve sólo si tienes que expandir tu rectángulo a la izquierda. Tal situación se presenta en la Fig. 3. Obviamente:
$$x'_P=x'_{P'}-(L-w)$$
Se aplican ahora (1) y $x_O$.
Cómo reconocer que la expansión a la izquierda es necesaria? Echa un vistazo a la Fig. 3 de nuevo. Usted no puede mover el lado derecho del rectángulo azul más a la derecha, debido a que el lado que tiene los puntos de $A$ e $C$ en él y que los puntos no están en las esquinas. Si mueve de lado a la derecha, los extremos de $d_1$ e $d_2$ ya no sería "conectado" a la derecha. Por el contrario, no hay no-de un punto de esquina en el lado izquierdo del rectángulo azul así que usted puede mover libremente la izquierda más a la izquierda y todavía tiene los extremos de $A,B,C,D$ "conectados" a los lados de la plaza.
Usted puede detectar si hay un punto de $(x_i, y_i)$ en el lado izquierdo del rectángulo azul que es el bloqueo de la expansión a la izquierda con la siguiente condición:
$$x_i=x_\min \ \land \ y_i \ne y_\min \ \land \ y_i \ \ne y_\max$$
Expansión a la izquierda no es posible si esta condición se cumple para cualquier $i=1,2,3,4$
Paso 6:
Solo es necesario si $h<L$. En este caso, el rectángulo azul es más corta que la de la plaza y es posible que los puntos de $P$ e $P'$ no son idénticos. En otras palabras, usted tiene que ampliar el rectángulo azul hacia arriba o hacia abajo para que coincida con el original de la plaza.
Si el rectángulo azul tiene que ser ampliado hacia arriba, esto no afectará a la posición de punto de $P'$:
$$y'_P=y'_{P'}$$
El punto de $P'$ se mueve sólo si tienes que expandir tu rectángulo hacia abajo. Tal situación se presenta en la Fig. 1. Obviamente:
$$y'_P=y'_{P'}-(L-h)$$
Se aplican ahora (1) y $y_O$.
Cómo reconocer que la expansión de la baja es necesario? Echa un vistazo a la Fig. 1 de nuevo. Usted no puede mover el lado superior del rectángulo azul hacia arriba debido a que lado tiene punto de $D$ y eso no es una esquina. Si mueve de ese lado hacia arriba, extremo de $d_4$ ya no sería "conectado" en la parte superior. Por el contrario, no hay no-de un punto de esquina en el lado inferior del rectángulo azul, así que usted puede moverse libremente la parte de abajo más abajo y todavía tiene los extremos de $A,B,C,D$ "conectados" a los lados de la plaza.
Usted puede detectar si hay un punto de $(x_i, y_i)$ en la parte de abajo que es el bloqueo de la expansión hacia abajo con la siguiente condición:
$$y_i=y_\min \ \land \ x_i \ne x_\min \ \land \ x_i \ \ne x_\max$$
La expansión hacia abajo no es posible si esta condición se cumple para cualquier $i=1,2,3,4$
Nota importante
Pasos mostrados anteriormente se puede transformar en una forma bastante simple y eficiente de software de código. Sin embargo, tenga en mente que sus sensores tiene precisión limitada por lo tanto, algo que es exactamente igual en matemáticas (como $L=w$) es aproximadamente igual en el mundo real. En el mundo real, $L$ será siempre diferente de $w,h$ porque calculamos los dos últimos valores de las lecturas del sensor.
Así que si usted quiere comprobar si $L=w$, el código debe probar:
$$|L-w|<\delta$$
...donde $\delta$ es de algún pequeño valor ajustado por la precisión de sus sensores.
