# ======================================================================== #
#
# Copyright 2012, Gabor Hegedus, Josef Schicho, and Hans-Peter Schroecker
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
# ======================================================================== #

# Maple implementation of Example 3 of Hegedus, Schicho, and
# Schroecker: Factorization of Rational Curves in the Study Quadric.

read "qf.mpl":

# Choose three rotation quaternions.
h1 := [1,   1,  e,   e];
h2 := [1, 1-e,  1, 1+e];
h3 := [1,   2,2*e,   1];

# Compute P = (t-h1)*(t-h2)*(t-h3), its conjugate polynomial Pb, and
# multiply the two. The result is a real polynomial f of degree six
# with three irreducible quadratic factors M1, M2, M3 that can be
# computed as minimal polynomials of h1, h2, and h3, respectively.
P := PolMult(PolMult([-h1], [-h2]), [-h3]);
Pb := map(Cj, P);       # conjugate quaternion
PPb := PolMult(P, Pb);  # a real polynomial
f := t^6:
for i from 1 to nops(PPb) do
  f := f + t^(6-i) * op(i, PPb)[1]:
od:
f := factor(f);

# Computing f is actually not necessary. We can compute the factors
# directly.
M[1] := MinPol(h1);
M[2] := MinPol(h2);
M[3] := MinPol(h3);

# Permutations of three elements.
p := [[1, 2, 3], [1, 3, 2], [2, 1, 3],
      [2, 3, 1], [3, 1, 2], [3, 2, 1]]:

# Compute open 3R chains that generate the motion parametrized by P.
i := 1:
for q in p do
  H3[i] := Solution(P, M[q[1]]):       # first factor
  Q := PolQuot(P, [-H3[i]]):           # divide by (t-H3)
  H2[i] := Solution(Q, M[q[2]]):       # second factor
  H1[i] := -PolQuot(Q, [-H2[i]])[1]:   # divide by (t-H2)
                                       # --> last factor

  # print solution and ...
  print(H1[i], H2[i], H3[i]):
  # ... check result
  print(PolMult(PolMult([-H1[i]], [-H2[i]]), [-H3[i]]) - P):
  i := i + 1:
od:

# ==================================================
# Overconstrained 6R-chains
# ==================================================

# We compute the new overconstrained 6R-chains contained in the
# complete linkage. First, we check the closure condition for every t.
# Then we compute distances, offsets and cosine of angles.

# Pairings that give new overconstrained 6R-chains:
pairs := [[1, 6], [2, 4], [3, 5]]:
for p in pairs do
  i := p[1]: j := p[2]:
  L := [H3[i], H2[i], H1[i],
        Cj(H1[j]), Cj(H2[j]), Cj(H3[j])]:
  print("----------------------------------------"):
  print(L);                  # linkage
  print(Mulall(op(L)));      # should be real number
  print(ldists(L)):          # distances
  print(offsets(L)):         # offsets
  print(map(cos, langs(L))): # angles
od: