How can I find the closest points that form a perpendicular?

Question

I have three points $A, B, C$ and I would like to find the three closest points $A', B', C'$ that

1. Cause $\vec{A'B'}$ to be perpendicular to $\vec{B'C'}$ and
2. Minimize the sum-square error between corresponding points $\epsilon = \lvert A' - A\rvert^2 + \lvert B' - B\rvert^2 + \lvert C' - C\rvert^2$

Since I know(?) that the best points will be in the same plane as the original points, I approached the formulas for the best points by using the original points as origins and bases:

$A' = A + t_{AA}(A - B) + t_{AC}(C - B)$

$B' = B + t_{BA}(A - B) + t_{BC}(C - B)$

$C' = C + t_{CA}(A - B) + t_{CC}(C - B)$

If I assume I know $A', B', t_{CC}$, I can compute $t_{CA}$ by applying the perpendicularity constraint via a zero-valued dot product:

$0 = (A' - B') \cdot (C' - B')$

...which leads to (if I figure correctly):

$t_{CA} = -\frac{(A' - B') \cdot (C - B') + t_{CC} (A' - B') \cdot (C - B)}{(A' - B') \cdot (A - B)}$

Since only the $\lvert C' - C\rvert^2$ part of $\epsilon$ is affected by by $t_{CC}$, I should be able to compute $t_{CC}$ by minimizing error:

$\frac{\partial}{\partial t_{CC}}[\lvert C' - C\rvert^2] = 0$

I can sort of get a result from solving the above using $\lvert C' - C\rvert^2 = (C' - C) \cdot (C' - C)$, but my Mathematica-foo is not strong enough to get a concise expression for $t_{CA}$ that eliminates $t_{CC}$. Then, I figure I would need to repeat that process 4 more times to find one of the $t_{xx}$ scalars as a function of only the knowns $A, B, C$.

Is there a better way to compute $A', B', C'$ and/or is someone better able to use Mathematica than me? I want to find expressions for $A', B', C'$ that only use the knowns $A, B, C$.

Answer 1

Take a point $A'$ lying on a circle of radius $r_1$ centered at $A$, such that the radius vector $AA'$ makes an angle $\theta_1$ with the positive direction of the $x$ axis. And take a point $C'$ lying on a circle of $r_2$ centered at $C$ such that the radius vector $CC'$ makes an angle $\theta_2$ with the positive direction of $x$ axis. Then we have

$ A' = A + r_1 (\cos \theta_1, \sin \theta_1 ) $

$ C' = C + r_2 (\cos \theta_2, \sin \theta_2 ) $

Since we $A'B'$ to be perpendicular to $B'C'$, then this means that $B'$ lies on a circle whose center is the midpoint of $A'C'$ and whose radius is $\dfrac{1}{2} \overline{A'C'} $. Further, we want $B'$ to be closest to $B$

Hence, if we let

$ M = \dfrac{1}{2} (A' + C') $

And

$ \theta_3 = \text{atan2}( B_x - M_x, B_y - M_y ) $

(Note that the order of the parameters for $atan2$ is reversed in some computer languages. The convention I am using, is $ \phi = \text{atan2}(u, v)$ retruns an angle $\phi$ such that $\tan \phi = \dfrac{v}{u} $.)

And finally,

$ R = \dfrac{1}{2} \| A' C' \| $

Then

$B' = M + R ( \cos \theta_3, \sin \theta_3 ) $

Note that the distance $r_3 = \| B B ' \| = \| B M \| - R $

Now the function we want to minimize is

$ f = r_1^2 + r_2^2 + r_3^2 $

And we four variables $r_1, \theta_1, r_2 , \theta_2 $. To carry out the minimization, I resorted to numerical methods, in particular the simple and very basic algorithm named steepest descent. This algorithm requires the gradient vector $\nabla f = [ \dfrac{\partial f }{\partial r_1} , \dfrac{\partial f }{\partial \theta_1} ,\dfrac{\partial f }{\partial r_2} , \dfrac{ \partial f}{\partial \theta_2} ]^T$.

This can be obtained by numerical differentiation.

The code I've developed is listed below, followed by the converged triangle $A'B'C'$. The points $A,B,C$ are the vertices of a 13-14-15 triangle.

Public Sub mse_min_sum_squared_distances_perpendicular()
Dim a(3), b(3), c(3) As Double, ap(3), bp(3), cp(3) As Double

Dim x(4), y(4) As Double

Dim pts(4, 2), pts2(4, 2) As Double

Dim x_(4), y_(4), dx(4), dy(4) As Double


a(1) = 0
a(2) = 0

b(1) = 14
b(2) = 0

c(1) = 5
c(2) = 12

x(1) = 0
x(2) = 0
x(3) = 0
x(4) = 0

success = 0

For ic = 1 To 2000

' find the gradient

Call find_y_minsumsq(a, b, c, x, y, ap, bp, cp)


k = 10
narcs = 0
ipass = 2


For i = 1 To 2
pts(1, i) = a(i)
pts(2, i) = b(i)
pts(3, i) = c(i)
pts(4, i) = a(i)

pts2(1, i) = ap(i)
pts2(2, i) = bp(i)
pts2(3, i) = cp(i)
pts2(4, i) = ap(i)
Next i

Call draw_points(2, 1, 4, pts, 1)
Call draw_points(2, 1, 4, pts2, 3)

Call plot_curve(0, 10, 0, 0, 0, "", "")

MsgBox ("")


If norm(y, 4) < 0.000001 Then
MsgBox (norm(y, 4))

success = 1
Exit For

End If

If ic = 1 Then

 gamma = 0.01

Else

For i = 1 To 4
dx(i) = x(i) - x_(i)
dy(i) = y(i) - y_(i)
    
Next i

If norm(dx, 4) < 0.000001 Then
   success = 1
   MsgBox ("converged in " + Str(ic - 1))
   Exit For
End If

dynormsq = norm(dy, 4) ^ 2

gamma = Abs(dotn(dx, dy, 4)) / dynormsq

End If


For i = 1 To 4
x_(i) = x(i)
x(i) = x(i) - gamma * y(i)
y_(i) = y(i)
Next i

Next ic

If success = 0 Then
MsgBox ("did not converge")
Exit Sub
End If


End Sub

---

Public Function find_f_minsumsq(a, b, c, x, ap, bp, cp) As Double
Dim m(2) As Double    

r1 = x(1)
th1 = x(2)
r2 = x(3)
th2 = x(4)

ap(1) = a(1) + r1 * Cos(th1)
ap(2) = a(2) + r1 * Sin(th1)

cp(1) = c(1) + r2 * Cos(th2)
cp(2) = c(2) + r2 * Sin(th2)

For i = 1 To 2
m(i) = 0.5 * (ap(i) + cp(i))
u1(i) = 0.5 * (ap(i) - cp(i))
Next i

r = norm(u1, 2)

th_3 = WorksheetFunction.Atan2( b(1) - m(1), b(2) - m(2) )

bp(1) = m(1) + r * cos(th_3)
bp(2) = m(2) + r * sin(th_3)

For i = 1 To 2
u1(i) = a(i) - ap(i)
u2(i) = b(i) - bp(i)
u3(i) = c(i) - cp(i)
Next i

find_f_minsumsq = dotn(u1, u1, 2) + dotn(u2, u2, 2) + dotn(u3, u3, 2)

End Function

---

Public Sub find_y_minsumsq(a, b, c, x, y, ap, bp, cp)

Dim u(4) As Double
Dim ap2(2), bp2(2), cp2(2) As Double

Dim i, j As Integer
Dim f1, f2 As Double

f1 = find_f_minsumsq(a, b, c, x, ap, bp, cp)

For j = 1 To 4

For i = 1 To 4
u(i) = x(i)
Next i

u(j) = u(j) + 0.01

f2 = find_f_minsumsq(a, b, c, u, ap, bp, cp)

y(j) = 100 * (f2 - f1)

Next j

End Sub

---