Buscando el centro

En una de las aplicaciones anteriores, visualizo la cámara de infrarrojos para ofrecer una muestra visual de 'qué ve el robot'. Para que este pueda orientarse y saber hacia donde va el líder (recordad que los infrarrojos van montados en la parte trasera del primer robot, al que llamo mama-pato) necesitamos conocer el centro de la figura formada por los cuatro puntos. Con esta información, en principio, sabremos si debemos desplazarnos a la izquierda o a la derecha.

¿Cómo encontramos ese punto?, la figura que se forma con los cuatro puntos es un cuadrilátero. Conocemos los puntos que lo forman (A, B, C, D) y que las diagonales pasan por los puntos AC y BD.



Primero calculamos las ecuaciones de ambas diagonales a partir de la ecuación general de la recta:
y = m*x + n

Conocemos dos puntos, por lo que tenemos un sistema de dos ecuaciones:

Y_A = m*X_A + n
Y_C = m*X_C + n

De donde obtenemos m_1 y n_1, para m_2 y n_2 utilizamos las mismas ecuaciones con los puntos B y D

Ya tenemos las rectas de las diagonales, para saber donde se cruzan no tenemos mas que formar un nuevo sistema de ecuaciones con ambas rectas y despejar X e Y:

Y = m_1 * X + n_2
Y = m_2 * X + n_2

El nuevo punto (X, Y) representa un punto central que permitirá al robot saber hacia donde debe dirigirse para seguir a 'mama-pato'

En ocasiones creo que el universo está empeñado en llevarme la contraria. He repasado este razonamiento varias veces, lo he contrastado con artículos encontrado a lo largo de internet, he revisado muchas veces 'mis cuentas' y aún así, una vez implementado, el punto central no siempre aparece donde debe... como odio al universo...