Algorithm: Triangle with two constraints, each corner on a given line

Question

Some time ago I asked a question on math.stackexchange and got an answer. I have difficulties deriving an algorithm from that answer because my background is in design and hope some of you can help me.

The original question with visual sketch and possible answer are here: https://math.stackexchange.com/questions/667432/triangle-with-two-constraints-each-corner-on-a-given-line

The question was: Given 3 3-dimensional lines (a, b and c) that coincide in a common point S and a given Point B on b, I'm looking for a point A on a and a point C on c where AB and BC have the same length and the angle ABC is 90 degrees.

I will have to implement this algorithm in an imperative language, any code in C++, Java, imperative pseudo-code or similar is fine.

Also, different approaches to this problem are equally welcome. Plus: Thanks for any hints, if the complete solution is indeed too time-consuming!

Answer 1

The two key formulas are

(I've replied the derivation for the formulas in the mathematics stack exchange site)

Substituting the first in the second gives in the end a 4th degree equation that is quite annoying to solve with a closed form. I've therefore used instead a trivial numerical solver in Python:

# function to solve (we look for t such that f(t)=0)
def f(t):
    s = (t*cB - B2) / (t*ac - aB)
    return s*s - 2*s*aB - t*t + 2*t*cB

# given f and an interval to search generates all solutions in the range
def solutions(f, x0, x1, n=100, eps=1E-10):
    X = [x0 + i*(x1 - x0)/(n - 1) for i in xrange(n)]
    Y = map(f, X)
    for i in xrange(n-1):
        if (Y[i]<0 and Y[i+1]>=0 or Y[i+1]<0 and Y[i]>=0):
            xa, xb = X[i], X[i+1]
            ya, yb = Y[i], Y[i+1]
            if (xb - xa) < eps:
                # Linear interpolation
                # 0 = ya + (x - xa)*(yb - ya)/(xb - xa)
                yield xa - ya * (xb - xa) / (yb - ya)
            else:
                for x in solutions(f, xa, xb, n, eps):
                    yield x

The search algorithm samples the function in the interval and when it finds two adjacent samples that are crossing the f=0 line repeats the search recursively between those two samples (unless the interval size is below a specified limit, approximating the function with a line and computing the crossing point in that case).

I've tested the algorithm generating random problems and solving them with

from random import random as rnd

for test in xrange(1000):
    a = normalize((rnd()-0.5, rnd()-0.5, rnd()-0.5))
    b = normalize((rnd()-0.5, rnd()-0.5, rnd()-0.5))
    c = normalize((rnd()-0.5, rnd()-0.5, rnd()-0.5))

    L = rnd() * 100
    B = tuple(x*L for x in b)

    aB = dot(a, B)
    cB = dot(c, B)
    B2 = dot(B, B)
    ac = dot(a, c)

sols = list(solutions(f, -1000., 1000.))

And there are cases in which the solutions are 0, 1, 2, 3 or 4. For example the problem

a = (-0.5900900304960981, 0.4717596600172049, 0.6551614908475357)
b = (-0.9831451620384042, -0.10306322574446096, 0.15100848274062748)
c = (-0.6250439408232388, 0.49902426033920616, -0.6002456660677057)
B = (-33.62793897729328, -3.5252208930692497, 5.165162011403056)

has four distinct solutions:

s = 57.3895941365 , t = -16.6969433689
    A = (-33.865027354189415, 27.07409541837935, 37.59945205363035)
    C = (10.436323283003153, -8.332179814593692, 10.022267893763457)
    |A - B| = 44.5910029061
    |C - B| = 44.5910029061
    (A - B)·(C - B) = 1.70530256582e-13

s = 43.619078237 , t = 32.9673082734
    A = (-25.739183207076163, 20.5777215193455, 28.577540327140607)
    C = (-20.606016281518986, 16.45148662649085, -19.78848391300571)
    |A - B| = 34.5155582156
    |C - B| = 34.5155582156
    (A - B)·(C - B) = 1.13686837722e-13

s = -47.5886624358 , t = 83.8222109697
    A = (28.08159526800866, -22.450411211385674, -31.17825902887765)
    C = (-52.39256507303229, 41.82931682916268, -50.313918854788845)
    |A - B| = 74.0747844969
    |C - B| = 74.0747844969
    (A - B)·(C - B) = 4.54747350886e-13

s = 142.883074325 , t = 136.634726869
    A = (-84.31387768560096, 67.4064705656035, 93.61148799140805)
    C = (-85.40270813540043, 68.1840435123674, -82.01440263735996)
    |A - B| = 124.189861967
    |C - B| = 124.189861967
    (A - B)·(C - B) = -9.09494701773e-13

Answer 2

Write two quadratic equations for lambda, mu unknowns (just above matrix forms).
Solve this system with paper, pen and head, or with any mathematical software like Maple, Mathematica, Matlab, Derive etc. You will have 4th order equation. It has closed-form solution - apply Ferrari or Kardano method and get real roots, find mu, lambda, then point coordinates.