# ======================================================================== #
#
# 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/>.
#
# ======================================================================== #

# ------------------------------------------------------------------------ #
# This file contains procedures for arithmetic of dual quaternions,
# polynomials over dual quaternions and bonds. It is used for
# computing the examples presented in the paper
#
# Gabor Hegedus, Josef Schicho, Hans-Peter Schroecker:
# The Theory of Bonds: A New Method for the Analysis of Linkages
# ------------------------------------------------------------------------ #

e := epsilon:                           # shorthand notation for dual unit

Smul := proc(k,a)
  # product of dual scalar with dual quaternion
  local raw, T;
  raw := [k*a[1], k*a[2], k*a[3], k*a[4]];
  return map(T->rem(expand(T), e^2, e), raw);
end proc;

Sinv := proc(k)
  # inverse of dual scalar
  local kp, kd;
  kp := coeff(k, e, 0);
  kd := coeff(k, e, 1);
  return 1 / kp - e * kd / kp^2;
end proc;

get43 := proc(h1, h2)
  # compute h3 and h4 such that (h1,h2,Cj(h4),Cj(h3)) is a Bennett quadruple
  local x, xc, h3c, h4c;
  x := h2 - Cj(h1);
  xc := Cj(x);
  h3c := Smul(Sinv(Mul(x, xc)[1]), Mul(Mul(xc, h1), x));
  h4c := h1 + h2 - h3c;
  return h4c, h3c;
end proc;

Cj := proc(a)
  # conjugate dual quaternion
  [ a[1],-a[2],-a[3],-a[4] ];
end proc;

Mul := proc(a,b)
  # product of dual quaternions
  local raw,T;
  raw:=
  [ a[1]*b[1]-a[2]*b[2]-a[3]*b[3]-a[4]*b[4],
    a[1]*b[2]+a[2]*b[1]+a[3]*b[4]-a[4]*b[3],
    a[1]*b[3]-a[2]*b[4]+a[3]*b[1]+a[4]*b[2],
    a[1]*b[4]+a[2]*b[3]-a[3]*b[2]+a[4]*b[1]
  ];
  map(T->rem(expand(T),e^2,e),raw);
end proc;

Mulall := proc()
  # product of all dual quaternions in a list
  local res,i;
  res := [1,0,0,0];
  for i from 1 to nargs do
    res := Mul(res,args[i]);
  od;
  res;
end proc;

RVec := proc(h)
  # the 8 real coordinates of a dual quaternion
  local pp,dp;
  pp := map(T->coeff(T,e,0),h);
  dp := map(T->coeff(T,e,1),h);
  [op(pp),op(dp)];
end proc;

cc := proc(k, n)
  # Returns the integer k-d*n in [1,...,n] where d is an integer as
  # well (for convenient display only).
  if (k > n) then: return cc(k-n, n): end if:
  if (k < 1) then: return cc(k+n, n): end if:
  return k:
end proc:

vanishingOrder := proc(f, t, t0)
  # Vanishing order of f(t) at t = t0.
  local v, d:
  if (f = 0) then: return infinity: end if:
  d := f:
  v := 0:
  while simplify(subs(t = t0, d) = 0) do
    v := v + 1:
    d := diff(d, t):
  od:
  return v:
end proc:

pNorm := proc(k)
  # Returns primal part of norm of k.
  local p:
  p := subs(epsilon = 0, k):
  return p[1]^2 + p[2]^2 + p[3]^2 + p[4]^2:
end proc:

connectionMatrix := proc(M)
  # Returns the connection matrix to a given local distance matrix M.
  local K, i, j, n, im, jm:
  n := LinearAlgebra[RowDimension](M):
  K := Matrix(n, n, shape=symmetric):
  for i from 1 to n do
    for j from i+1 to n do
      if i = 1 then im := n: else im := i - 1: end if:
      if j = 1 then jm := n: else jm := j - 1: end if:
      K[i,j] := M[i,j] + M[im,jm] - M[im,j] - M[i,jm]:
    od:
  od:
  return K:
end proc: