Vtgyjfy

my problem appears to be a very simple one, but I just can't seem to figure it out, maybe I am just overlooking something…

The problem is defined as the following (refer to the figure for better understanding): I have a square of side L and I have a variable point O inside the square. The distances d1, d2, d3, and d4 are known and the angle between d1 and the horizontal, which I call $alpha$, is also known. The line segments d1, d3 and d2, d4 make the lines AB and CD respectively, which are perpendicular to each other. My aim is to find the coordinates of the point O when the origin of the 2d plane is the bottom left corner of the square (denoted as P) and the X axis is defined by the bottom side of the square, and the Y axis is defined by the left side of the square. In the figure 1, this can be calculated very easily: the coordinate $X=L-d1*cos(alpha)$ and $Y=L-d4*cos(alpha)$.

The issue arises when I try and find a general equation to give the coordinates of O. For example in figure 2: $X$ is no longer $L-d1*cos(alpha)$ but rather is $X=d4*cos(alpha)$ and $Y$ is no longer $L-d4*cos(alpha)$ but is $Y=L- d1*sin(alpha)$. In other words, the expressions for X and Y keep changing based on certain conditions. Ideally, I would want to have a general expression for X and Y regardless of the situation, but I am okay with having a few conditions.

To put this math problem into context, my point O is actually a robotic car, the square represents a boundary that the car moves about in and the distances d1,d2,d3 and d4 are distances to the boundary that are measured from distance sensors that are attached to the car, the angle is measured using an onboard accelerometer and gyro. And I am trying to use these sensor inputs to position the robot on a 2d cartesian plane.

Update: I added another figure (figure 3) to clarify certain doubts.

We are going to introduce a new coordinate system $x'Oy'$ like shown in Fig. 1. If we are able to calculate coordinates of point $P(x'_P, y'_P)$ in that coordinate systen - we are done because:

$$x_O=-x'_P, y_O=-y'_Ptag{1}$$

Step 1:

Find coordinates of points $A,B,C,D$ in $x'Oy'$ coordinate system:

$$A: x'_1=d_1cosalpha, y'_1=d_1sinalpha$$

$$D: x'_2=d_4cos(alpha+90^circ), y'_2=d_4sin(alpha+90^circ)$$

$$B: x'_3=d_3cos(alpha+180^circ), y'_3=d_3sin(alpha+180^circ)$$

$$C: x'_4=d_2cos(alpha+270^circ), y'_4=d_2sin(alpha+270^circ)$$

Step 2:

Calculate the following values:

$$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)$$

Step 3:

Find the bottom left corner $P'(x'_{P'}, y'_{P'})$ of the bounding rectangle (shown in blue color) and calculate the width $w$ and height $h$ of the bounding rectangle:

$$x'_{P'}=x'_min$$
$$y'_{P'}=y'_min$$
$$w=x'_max-x'_min$$
$$h=y'_max-y'_min$$

Step 4:

If $h=w=L$ we are done (see Fig. 2) because the blue rectangle is actually our bounding square of length $L$, the points $P$ and $P'$ are identical and $x'_{P}=x'_{P'}$, $y'_{P}=y'_{P'}$. Just apply (1) and you have $x_O, y_O$.

Step 5:

Needed only if $w<L$. In this case your blue rectangle is narrower than the square and it is possible that points $P$ and $P'$ are not identical. In other words, you have to expand the blue rectangle either to the left or to the right to match the original square.

If the blue rectangle has to be expanded to the right, it won't impact the position of point $P'$ and in this case:

$$x'_P=x'_{P'}$$

The point $P'$ moves only if you have to expand your rectangle to the left. Such situation is presented in Fig. 3. Obviously:

$$x'_P=x'_{P'}-(L-w)$$

Now apply (1) and you have $x_O$.

How to recognize that expansion to the left is needed? Take a look at Fig. 3 again. You cannot move the right side of the blue rectangle more to the right because that side has points $A$ and $C$ on it and that points are not corners. If you moved that side to the right, endpoints of $d_1$ and $d_2$ would be no longer "attached" to the right side. On the contrary, there is no non-corner point on the left side of the blue rectangle so you can freely move the left side further to the left and still have endpoints $A,B,C,D$ "attached" to sides of the square.

You can detect if there is a point $(x_i, y_i)$ on the left side of the blue rectangle that is blocking expansion to the left with the following condition:

$$x_i=x_min land y_i ne y_min land y_i ne y_max$$

Expansion to the left is not possible if this condition is satisfied for any $i=1,2,3,4$

Step 6:

Needed only if $h<L$. In this case your blue rectangle is shorter than the square and it is possible that points $P$ and $P'$ are not identical. In other words, you have to expand the blue rectangle either upwards or downwards to match the original square.

If the blue rectangle has to be expanded upwards, it won't impact the position of point $P'$:

$$y'_P=y'_{P'}$$

The point $P'$ moves only if you have to expand your rectangle downwards. Such situation is presented in Fig. 1. Obviously:

$$y'_P=y'_{P'}-(L-h)$$

Now apply (1) and you have $y_O$.

How to recognize that expansion downwards is needed? Take a look at Fig. 1 again. You cannot move the top side of the blue rectangle upwards because that side has point $D$ on it and that point is not a corner. If you moved that side upwards, endpoint of $d_4$ would be no longer "attached" to the top side. On the contrary, there is no non-corner point on the bottom side of the blue rectangle so you can freely move the bottom side further down and still have endpoints $A,B,C,D$ "attached" to sides of the square.

You can detect if there is a point $(x_i, y_i)$ on the bottom side that is blocking expansion downwards with the following condition:

$$y_i=y_min land x_i ne x_min land x_i ne x_max$$

Expansion downwards is not possible if this condition is satisfied for any $i=1,2,3,4$

Important note

Steps shown above can be transformed into a fairly simple and efficient software code. However, have in mind that your sensors have limited precision so something that is strictly equal in mathematics (like $L=w$) is just approximately equal in the real world. In the real world, $L$ will be always different from $w,h$ because we calculate the last two values from sensor readings.

So if you want to check if $L=w$, your code should actually test for:

$$|L-w|<delta$$

...where $delta$ is some small value adjusted for the precision of your sensors.