(* PlanarLinkages -- construction of planar linkages following a prescribed motion * Copyright (C) 2015 Christoph Koutschan * * 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, see http://www.gnu.org/licenses/ * * 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. *) If[\$Notebooks, CellPrint[Cell[TextData[{#1, StyleBox[#2, Small]}], "Text", FontColor -> Black, CellFrame -> 0.5, Background -> RGBColor[0.796887, 0.789075, 0.871107]]], Print[#1 <> #2]]&[ "PlanarLinkages \[LongDash] Copyright \[Copyright] 2015 Christoph Koutschan\n", "This program comes with absolutely no warranty; it is free software,\n" <> "and you are welcome to redistribute it and/or modify it under the\n" <> "terms of the GNU General Public License (http://www.gnu.org/licenses/)."]; (* Introduce a data type MP that represents (possibly constant) 2D motion polynomials: MP[x1, x2, x3, x4] encodes (x1 + I*x2) + eta*(x3 + I*x4). *) (* The symbol eta is automatically transformed into a motion polynomial. *) eta := MP[0, 1]; (* Define UpValues for MP. We want that addition and multiplication of MP objects are * automatically performed, unless the input is ambiguous concerning the order of factors: * if MP1*MP2 is input then Mathematica may reorder the factors, use MP1**MP2 instead. * We assume that in any input eta is on the left, i.e., I*eta is interpreted as eta*I. *) MP /: Plus[a_MP, b__] := AddMP[a, b]; MP /: Times[a_MP, b__ /; FreeQ[{b}, MP]] := MultMP[a, b]; MP /: NonCommutativeMultiply[b1___, a_MP, b2___] := MultMP[b1, a, b2]; (* How to display motion polynomials. The \[Eta] is always visible in order to enable the user * to distinguish MP objects from other Mathematica expressions, e.g., MP[1, 0] from 1. *) Format[MP[z_, w1_]] := Module[{w = w1, op}, op = If[MatchQ[MakeBoxes @@ {w}, RowBox[{"-", _}]], w = -w; "-", "+"]; Infix[{z, If[w === 1, \[Eta], CenterDot[\[Eta], w]]}, op]]; (* Introduce a data type FactoredMP that is used to keep a factorization of a motion polynomial. *) Format[FactoredMP[ms: (_MP)..]] := CenterDot[ms]; FactoredMP /: Expand[FactoredMP[ms: (_MP)..]] := MultMP[ms]; (* Interpret copy-and-paste input correctly. *) MakeExpression[InterpretationBox[RowBox[{a1_, op : ("+" | "-"), a2:("\[Eta]" | RowBox[{"\[Eta]", "\[CenterDot]", _}])}], _, Editable -> False], StandardForm] := With[{sp = If[a2 === "\[Eta]", 1, a2[[1,3]]]}, MakeExpression[RowBox[{"MP", "[", RowBox[{a1, ",", If[op === "+", sp, RowBox[{"-", sp}]]}], "]"}], StandardForm]]; MakeExpression[RowBox[{a1__, op : ("+" | "-"), a2:("\[Eta]" | RowBox[{"\[Eta]", "\[CenterDot]", _}])}], StandardForm] := With[{sp = If[a2 === "\[Eta]", 1, a2[[1,3]]]}, MakeExpression[RowBox[{"MP", "[", RowBox[{RowBox[{a1}], ",", If[op === "+", sp, RowBox[{"-", sp}]]}], "]"}], StandardForm]]; MakeExpression[RowBox[list_] /; Union[Take[list, {2, -1, 2}]] === {"\[CenterDot]"}, StandardForm] := MakeExpression[RowBox[{"FactoredMP", "[", RowBox[ Riffle[MakeBoxes[ReleaseHold[MakeExpression[#, StandardForm]]] & /@ Take[list, {1, -1, 2}], ","] ], "]"}], StandardForm]; (* Convert an (eta-free) expression to a motion polynomial + do some consistency checks. *) ToMP[a_] := With[{args = If[Head[a] === MP, List @@ a, {a, 0}]}, If[Length[args] === 2 && FreeQ[args, MP], MP @@ args, Throw["Cannot convert to motion polynomial."]]]; (* Simplify a motion polynomial. *) SimpMP[p_MP] := SimpMP[p, t]; SimpMP[MP[z_, w_], t_] := Collect[#, t]& /@ (MP[z, w] /. {Conjugate[t] -> t, Re[t] -> t, Im[t] -> 0}); (* Addition and Multiplication of motion polynomials. *) AddMP[] := MP[0, 0]; AddMP[ms: (_MP)..] := SimpMP[MP @@ Total[List @@@ {ms}]]; AddMP[ms__] := AddMP @@ (ToMP /@ {ms}); MultMP[] := MP[1, 0]; MultMP[MP[z1_, w1_], MP[z2_, w2_]] := SimpMP[MP @@ (Expand /@ {z1 * z2, Conjugate[z1] * w2 + w1 * z2})]; MultMP[ms: (_MP)..] := Fold[MultMP, First[{ms}], Rest[{ms}]]; MultMP[ms__] := MultMP @@ (ToMP /@ {ms}); (* The 3*3 matrix associated to a motion polynomial; it encodes the action * of that polynomial on P^3. *) MP2Mat[MP[z_, w_]] := Module[{e0, e12, e1, e2}, {e0, e12, e1, e2} = (# . t ^ (Range[Length[#]] - 1))& /@ Flatten[{Re[#], Im[#]}& /@ CoefficientList[{z, w}, t], 1]; {{ e0^2 + e12^2 , 0 , 0 }, {e0*e1 - e2*e12 , e0^2 - e12^2 , -2*e0*e12 }, {e0*e2 + e1*e12 , 2*e0*e12 , e0^2 - e12^2 }}]; (* Action of a motion polynomial on points in R^2 *) ActR2::usage = "ActR2[p, {x, y}] computes the result of applying the direct isometry given by p " <> "to the point {x, y}, where p is a (possibly constant) motion polynomial."; ActR2[p_MP, {x_, y_}] := With[{v = MP2Mat[p] . {1, x, y}}, Together[Rest[v] / First[v]]]; (* Given complex polynomials Z and W (by their lists of roots), find *) (* a permutation of the roots of R*Z (for some real polynomial R), *) (* such that R * (Z + eta * W) can be factored into linear factors. *) FindOrder[rtsZ_List, rtsW_List] := Module[{rts = rtsZ, alpha, albar, r, s, u, v, m, q = {}}, While[rts =!= {}, alpha = First[rts]; albar = ComplexExpand[Conjugate[alpha]]; (* Make always r_i >= s_i. *) If[Count[rts, alpha] <= Count[rts, albar], {alpha, albar} = {albar, alpha}]; {r, s} = Count[rts, #]& /@ {alpha, albar}; rts = DeleteCases[rts, alpha | albar]; {u, v} = Count[rtsW, #]& /@ {alpha, albar}; If[s * u * v > 0, Throw["Z and W have common real factors."]]; m = Min[s, u + v]; q = Join[q, Table[albar, {s - Min[s, v]}], Table[alpha, {r + s - m}], Table[albar, {s - Min[s, u]}]] ]; Return[q]; ]; (* Factorization of motion polynomials *) FactorMP::usage = "FactorMP[P] factors the given normed bounded motion polynomial P into linear factors. " <> "If necessary, P is multiplied by a real polynomial R and the factorization of R*P is computed; this " <> "situation is indicated by a message. With the option Order -> {r1, r2, ...} one can specify an order " <> "in which the roots of Z, the primal part of P, should appear in the factorization. Note that then R " <> "is not automatically determined; however, one can do so by giving more roots than just those of Z."; FactorMP::R = "Multiply the input with R = 1"; Options[FactorMP] = {Order -> Automatic}; FactorMP[P_MP, opts:(_Rule|_RuleDelayed)...] := FactorMP[P, t, opts]; (* Notation is the same as in the paper. *) FactorMP[MP[Z_, W_], t:Except[_Rule|_RuleDelayed], opts:(_Rule|_RuleDelayed)...] := Module[{error, rtsZ, rtsW, R, q, n, Qs, mat, ns, v, ws}, q = Order /. {opts} /. Options[FactorMP]; error = "FactorMP: input must be a bounded and normed motion polynomial."; If[Z === 0, Throw[error]]; {rtsZ, rtsW} = Function[poly, ComplexExpand[#[[1, 2]]]& /@ Solve[poly == 0, t]] /@ {Z, W}; (* Check normedness, i.e., deg(W) < deg(Z) and lc(Z) = 1. *) If[Length[rtsW] >= Length[rtsZ] || Coefficient[Z, t^Length[rtsZ]] =!= 1, Throw[error]]; (* Check boundedness, i.e., no real root. *) If[Or @@ (Element[#, Reals]& /@ rtsZ), Throw[error]]; q = If[q === Automatic, FindOrder[rtsZ, rtsW], ComplexExpand[q]]; R = ComplexExpand[Expand[Together[(Times @@ (t - # & /@ q)) / (Times @@ (t - # & /@ rtsZ))]]]; If[Not[PolynomialQ[R, t]], Throw["The given zeros must contain all zeros of Z (with multiplicity!)."]]; If[R =!= 1, Message[FactorMP::R, R]]; (* From this ansatz build a linear system and solve it. *) n = Length[q]; Qs = Table[Product[t - Conjugate[q[[l]]], {l, j - 1}] * Product[t - q[[l]], {l, j + 1, n}], {j, n}]; mat = Transpose[PadRight[CoefficientList[#, t]& /@ Append[Qs, R * W]]]; ns = NullSpace[mat]; (* The following should only happen when the user gives bad options. *) If[Select[ns, Last[#] =!= 0 &] === {}, Throw["No solution found."]]; (* Construct a parametric solution of the inhomogeneous system. *) v = First[SortBy[Select[ns, Last[#] =!= 0 &], ByteCount[Last[#]]&]]; v = Together[v / Last[v]]; ns = DeleteCases[Together[# - Last[#] * v]& /@ ns, {(0)..}]; ws = ComplexExpand[-Most[Total[ns * Array[C, Length[ns]]] + v]]; Return[FactoredMP @@ (MP[t - #1, #2]& @@@ Transpose[{q, ws}])]; ]; (* FixPoint of a revolution *) FixPoint::usage = "FixPoint[p] computes the fixpoint of a simple revolution given by " <> "the linear normed motion polynomial p, i.e., p is of the form t - z + \[Eta]*w where " <> "z and w are complex numbers with z being non-real."; FixPoint[m_MP] := FixPoint[m, t]; FixPoint[MP[z_, w_], t_] := Module[{}, If[Not[FreeQ[{z - t, w}, t]] || Im[z - t] === 0, Throw["FixPoint: Input is not a normed bounded motion polynomial of degree 1."]]; Return[{-Im[w], Re[w]} / (2 Im[z - t])]; ]; (* Compute the lengths of the links associated to a factored motion polynomial. *) Options[LinkLengths] = {Trace -> {0, 0}}; LinkLengths[FactoredMP[mps__], opts:((_Rule|_RuleDelayed)...)] := PowerExpand[Simplify[ Sqrt[#1^2 + #2^2]& @@@ Differences[Append[FixPoint /@ Reverse[{mps}], Trace /. {opts} /. Options[LinkLengths]]] ]]; (* Create Graphics primitives for trace (called by AnimateMP and ShowLinkage). *) TraceMP[m_MP, t_, p_, plotrange_, range_ : {-Pi/2 + 0.01, Pi/2 - 0.01}, dir1_ : {0, 0}, d3_ : False] := Module[{dir = dir1, b, d, x, dx, ps, pt, pts, step, r1, r2}, {r1, r2} = range; step = Max[10^(-6), Max[Differences /@ plotrange] / 100]; dx = 0.1; x = r1; ps = Union[{p - dir, p + dir}]; pts = {ActR2[m /. t -> Tan[x], #]& /@ ps}; While[x < r2, pt = ActR2[m /. t -> Tan[Min[x + dx, r2]], #]& /@ ps; d = Max[Norm /@ (pt - pts[[-1]])]; If[d < step || dx < 1/1000, AppendTo[pts, pt]; If[d < step/2 && dx < 0.1, dx *= 2]; x += dx; , dx /= 2; ]; ]; If[Max[Norm /@ (pts[[1]] - pts[[-1]])] < step, AppendTo[pts, pts[[1]]]]; If[d3 =!= False, pts = Map[Append[#, d3]&, pts, {2}]; AppendTo[dir, 0]; ]; Return[If[dir === -dir, Line[First /@ pts], {EdgeForm[], Polygon[#[[{1,2,4,3}]]& /@ (Join @@@ Partition[pts, 2, 1])]}]]; ]; (* Create Graphics primitives for MP and FactoredMP (called by AnimateMP). *) Options[ToGraphics] = {Style -> Automatic, Polygon -> {{0, 0}, {1, 0}, {0, 1}}}; ToGraphics::zerolen := "Component of length zero encountered."; ToGraphics[m_MP, opts:((_Rule|_RuleDelayed)...)] := ToGraphics[m, t, opts]; ToGraphics[m_MP, t:Except[_Rule|_RuleDelayed], opts:((_Rule|_RuleDelayed)...)] := Module[{z, pgon, trace, poly}, If[m[[1]] === 0, Return[Function[tt, Polygon[{{0, 0}, {0, 1}}]]]]; {pgon, trace} = {Polygon, Trace} /. {opts} /. Options[ToGraphics]; If[Not[MatchQ[trace, {_, _}]], trace = {0, 0}]; pgon = (# + trace)& /@ pgon; poly = m /. t -> z; Return[Function[tt, {Polygon[ActR2[poly /. z -> tt, #] & /@ pgon]}]]; ]; ToGraphics[fact_FactoredMP, opts:((_Rule|_RuleDelayed)...)] := ToGraphics[fact, t, opts]; ToGraphics[fact_FactoredMP, t:Except[_Rule|_RuleDelayed], opts:((_Rule|_RuleDelayed)...)] := Module[{a1, a2, z, style, pgon, trace, fps, rs, pts, polys}, {style, pgon, trace} = {Style, Polygon, Trace} /. {opts} /. Options[ToGraphics]; If[style === Automatic, style = "links"]; If[Not[MatchQ[trace, {_, _}]], trace = {0, 0}]; fps = Prepend[FixPoint /@ (List @@ fact), trace]; rs = Norm /@ Differences[fps]; If[MemberQ[rs, 0], Message[ToGraphics::zerolen]]; pts = Table[ {a1, a2} = fps[[i - 1]] - fps[[i]]; (fps[[i]] + #) & /@ {{0, 0}, {a1, a2}, {-a1, -a2}, {a2, -a1}, {-a2, a1}}, {i, 2, Length[fps]}]; polys = Reverse[FoldList[MultMP[#2, #1] &, Last[fact], Rest[Reverse[List @@ fact]]]] /. t -> z; Return[Function[tt, Module[{ps, r = {}}, Do[ ps = ActR2[polys[[i]] /. z -> tt, #] & /@ pts[[i]]; r = Join[r, Switch[style, "wheels", {Circle[ps[[1]], rs[[i]]], Line[{ps[[2]], ps[[3]]}], Line[{ps[[4]], ps[[5]]}]}, "links", {Line[{ps[[1]], ps[[2]]}]} ]]; , {i, Length[pts]}]; Join[{Gray, Opacity[0.5], Polygon[ActR2[polys[[1]] /. z -> tt, #] & /@ pgon], Black, Opacity[1]}, r]]]]; ]; (* Create animation of a motion. *) AnimateMP::usage = "AnimateMP[p] animates the motion described by the motion polynomial p. If p is given " <> "in factored form, then the decomposition of the motion into revolutions is shown. AnimateMP[p1, p2, ...] " <> "shows several motions at the same time. The following options can be given:\n" <> "Trace -> {x, y}: show the trace of the point {x, y} under the given motion.\n" <> "Style -> \"links\", Style -> \"wheels\": different visualizations of a decomposed motion.\n" <> "Polygon -> {{0, 0}, {1, 0}, {0, 1}}: specify the shape of a planar object that is displaced according to p.\n" <> "Moreover, all options of the Graphics command can be used."; Options[AnimateMP] = {Axes -> True, ImageSize -> {400, 300}, Trace -> {0, 0}}; (* + options in ToGraphics *) AnimateMP[args:(_MP|_FactoredMP).., opts:(_Rule|_RuleDelayed)...] := AnimateMP[args, t, opts]; AnimateMP[args:(_MP|_FactoredMP).., t:Except[_MP|_FactoredMP|_Rule|_RuleDelayed], opts:(_Rule|_RuleDelayed)...] := Module[{x, fs, c, pts, range, tr, opts1, pi2 = Pi/2 - 0.01}, fs = ToGraphics[#, opts]& /@ {args}; c = Quiet[Cases[Flatten[Table[#[Tan[x]]& /@ fs, {x, -pi2, pi2, pi2 / 25}]], _Line|_Polygon]]; pts = Select[Join @@ (First /@ c), FreeQ[#, Infinity | Indeterminate]&]; range = {{Min[#], Max[#]}&[First /@ pts], {Min[#], Max[#]}&[Last /@ pts]}; range += ((First[Differences[#]] / 8 * {-1, 1})& /@ range); tr = Trace /. {opts} /. Options[AnimateMP]; tr = Switch[tr, None, {}, {_, _}, Join[{Red}, TraceMP[#, t, tr, range]& /@ Union[Expand[{args}]], {Black}], _, Throw["Bad value for option Trace."]]; opts1 = Append[DeleteCases[Join[{opts}, Options[AnimateMP]], _[Style|Polygon|Trace, _]], PlotRange -> range]; Format[x] := ArcTan["t"]; Return[Animate[ TableForm[{"t = " <> ToString[Tan[x]], Graphics[Flatten[{tr, #[Tan[x]]& /@ fs}], Sequence @@ opts1]}], {x, -pi2, pi2}, AnimationRunning -> False]]; ]; (* The Flip procedure *) Flip::usage = "Flip[p1, p2] performs the flip procedure. Note that input and " <> "output are pairs of linear motion polynomials, of the form t - k, k in K, " <> "as opposed to the paper, where only the k's are given / returned."; Flip[MP[z1_, w1_], MP[z2_, w2_]] := Module[{w3, w4, den}, den = SimpMP[MP[z1 - Conjugate[z2], 0]][[1]]; If[den === 0, Throw["Flip: primal parts are conjugate to each other."]]; w3 = (w1 * 2*I*Im[z2] + w2 * Conjugate[z1 - z2]) / den; w4 = (w1 * (z1 - z2) + w2 * 2*I*Im[z1]) / den; Return[SimpMP /@ {MP[z2, w3], MP[z1, w4]}]; ]; (* Construct a linkage, in form of an enriched link graph, from a factored motion polynomial. *) (* This is described in Algorithm 4 ConstructStrongLinkage in the paper. *) ConstructLinkage::usage = "ConstructLinkage[mp, l1] constructs a linkage " <> "of mobility one that realizes the motion given by mp. The motion polynomial " <> "mp must be given in factored form, and l1 is a linear bounded motion polynomial. " <> "If l1 is omitted, a suitable such polynomial is taken instead. The output is given " <> "in the form as needed for ShowLinkage."; ConstructLinkage[mp_FactoredMP, l1_ : Automatic] := Module[{x, n = Length[mp], ks = List @@ mp, kts = {}, ls, kti, li1, eqns = {}, graph, len, case, pos}, (* These values have to be avoided for pp(l1). *) ls = Union[#, Conjugate[#]]&[t - First /@ ks]; If[l1 === Automatic, (* If l1 is not given, take one with suitable primal and generic secondary part. *) ls = {MP[t - I * First[Complement[Range[n + 1], ls / I]], x]}; , (* If l1 is given, test part of the IFM condition in advance. *) If[MemberQ[ls, t - l1[[1]]], Throw["ConstructLinkage: Condition IFM is not fulfilled."]]; ls = {l1}; ]; (* Compute the l_i and \tilde{k}_i. *) Do[ (* Test the remaining part of the IFM condition. *) AppendTo[eqns, FixPoint[ls[[i]]] - FixPoint[ks[[i]]]]; If[Last[eqns] === {0, 0}, Throw["ConstructLinkage: Condition IFM is not fulfilled."]]; {kti, li1} = Flip[ls[[i]], ks[[i]]]; AppendTo[kts, kti]; AppendTo[ls, li1]; , {i, n}]; (* For sp(l1) choose the smallest natural number such that IFM holds. *) If[l1 === Automatic, li1 = First[Complement[Range[0, n], Flatten[(x /. Solve[Thread[# == 0], x])& /@ eqns]]]; {ls, kts} = {ls, kts} /. x -> li1; ]; (* Note that we use a different labeling than in the paper (Algorithm 4). *) (* We start on the right with 1 (upper row) and n+2 (lower) and increase *) (* the labels while going to the left. This way, the base has label 1. *) graph = Join[ Table[{i, i+1, ks[[n-i+1]]}, {i, n}], Table[{n+i+1, n+i+2, kts[[n-i+1]]}, {i, n}], Table[{i, n+i+1, ls[[n-i+2]]}, {i, n+1}]]; (* (* Determine an order on the links such that each square of the link graph is collision-free. *) (* However, in practice, we get fewer collisions by just stacking each square on top of the previous one. *) len[k1_, k2_, k3_] := Norm[Differences[FixPoint[Last[First[Cases[graph, {#1, #2, _} | {#2, #1, _}]]]]& @@@ {{k1, k2}, {k2, k3}}]]; case = len[2, 1, n + 2] > len[1, n + 2, n + 3]; ls = {1, 2}; pos = 2; Do[ If[len[i, i - 1, i + n] > len[i + n + 1, i + n, i - 1], If[case, pos++]; ls = Insert[ls, i, pos]; , If[Not[case], pos++]; ls = Insert[ls, i, pos]; ]; , {i, 3, n + 1}]; Print[Flatten[If[case, {#, # + n + 1}, {# + n + 1, #}]& /@ ls]]; *) Return[graph]; ]; (* Graphics primitives for a link from {x1, y1} to {x2, y2} of radius r and color c. *) LineToGraphics[{{x1_, y1_}, {x2_, y2_}}, r_, c_] := Module[{vec, rect}, vec = Together[{x2 - x1, y2 - y1}]; If[vec === {0, 0}, Return[{}]]; vec = {vec[[2]], -vec[[1]]} / Sqrt[Together[vec[[1]]^2 + vec[[2]]^2]] * r; rect = Together[{{x1, y1} + vec, {x1, y1} - vec, {x2, y2} - vec, {x2, y2} + vec, {x1, y1} + vec}]; Return[{c, Polygon[rect], Black, Line[rect]}]; ]; (* Animate a linkage. *) ShowLinkage::usage = "ShowLinkage[graph, base] creates an animation of a linkage. " <> "The link graph must be given in the form {{i, j, mij}, ...} where i and j are the " <> "labels of two links and mij is a linear bounded motion polynomial describing the relative " <> "position between these two links. The second argument, base, is the label of the base link " <> "(the one that doesn't move). The following options can be given:\n" <> "ColorOutput -> {1 -> Yellow, 2 -> Green, ...}: to draw link 1 in yellow etc.\n" <> "Thickness -> 0.01: thickness of links. Thickness -> {0.01, 0.001}: thickness of links and lines.\n" <> "Trace -> {i, {x, y}}: trace the point {x, y} under the motion of link i.\n" <> "Direction -> {x, y}: draw the curve with a pen whose shape is a line from {0, 0} to {x, y}.\n" <> "Range -> {a, b}: range for the time ArcTan[t], i.e., [a, b] \[SubsetEqual] [-\[Pi]/2, \[Pi]/2].\n" <> "Links -> {i, j, ...}: specify an order in which the links should be drawn.\n" <> "Style -> \"CompleteGraph\", Style -> \"Star\": how to draw links that are connected with more than 2 joints.\n" <> "Return -> \"animation\": produces an animation of the linkage.\n" <> "Return -> {\"picture\", x}: produces an image showing the linkage at time x = ArcTan[t].\n" <> "Return -> {\"movie\", n}: creates a sequence of n image files that can be used to make a movie.\n" <> "Return -> {\"movie\", {t1, t2, ..., tn}}: as before, using time ti for frame i.\n" <> "Return -> \"graph\": draws the link graph.\n" <> "Return -> \"collisions\": returns a list of the form {{{i, k, j}, s, t}, ...} which means that " <> "the joint connecting i and j collides with link k at time t at position 0 <= s <= 1.\n" <> "Return works similarly with \"animation3D\", \"picture3D\", and \"movie3D\".\n" <> "Moreover, all options of the Graphics / Graphics3D commands can be used."; Options[ShowLinkage] = {ColorOutput -> {}, PlotRange -> All, Return -> "animation", Thickness -> Automatic, Trace -> None, Direction -> {0, 0}, Range -> {-Pi/2 + 0.001, Pi/2 - 0.001}, Links -> Automatic, Style -> "CompleteGraph"}; ShowLinkage[graph_List, opts:((_Rule|_RuleDelayed)...)] := ShowLinkage[graph, 1, opts]; ShowLinkage[graph1_List, base : Except[_Rule | _RuleDelayed], opts:((_Rule|_RuleDelayed)...)] := Module[{color, range, return, thick, trace, dir, r1, r2, draw3D, opts1, x, l1, l2, mp, graph, links, joints, lines, psj, js, gr, thk, traceZ, v, td, col}, (* Deal with options. *) {color, range, return, thick, trace, dir, {r1, r2}} = {ColorOutput, PlotRange, Return, Thickness, Trace, Direction, Range} /. {opts} /. Options[ShowLinkage]; opts1 = DeleteCases[{opts}, Rule[ColorOutput | PlotRange | Return | Thickness | Trace | Direction | Range | Links | Style, _]]; If[Not[MatchQ[range, All | {{_, _}, {_, _}} | {{_, _}, {_, _}, {_, _}}]], Throw["Option PlotRange must be of the form {{x1, x2}, {y1, y2}} or {{x1, x2}, {y1, y2}, {z1, z2}}."]]; If[Not[MatchQ[trace, None | {_, {_, _}}]], Throw["Option Trace must be of the form {l, {x, y}}."]]; draw3D = MatchQ[return, "animation3D" | {"picture3D" | "movie3D", _}]; (* For each edge (i,j) add the opposite edge (j,i) to the graph. *) (* Note that MP[z, w] ** MP[conj[z], -w] = a^2 + b^2 + 2*a*t + t^2. *) graph = Join[graph1, {#2, #1, If[Head[#3] === MP, MP[t + Conjugate[#3[[1]] - t], -#3[[2]]], #3]}& @@@ graph1]; (* For each link, compute a motion polynomial that describes its absolute motion. *) (* For each joint, compute its initial position (t = infinity). *) links = {{base, 1 + eta * 0}}; joints = psj = {}; While[graph =!= {}, {l1, l2, mp} = First[Select[graph, MemberQ[First /@ links, First[#]]&]]; If[Head[mp] === MP, v = FixPoint[mp], AppendTo[psj, {l1, l2}]; v = mp; mp = 1]; mp = mp ** First[Cases[links, {l1, a_} :> a]]; If[Not[MemberQ[First /@ links, l2]], AppendTo[links, {l2, mp}]]; AppendTo[joints, {l1, l2, Together[ActR2[mp, v]]}]; graph = DeleteCases[graph, {l1, l2, _} | {l2, l1, _}]; ]; If[trace =!= None, mp = First[Cases[links, {trace[[1]], a_} :> a]]; AppendTo[joints, {trace[[1]], trace[[1]], Together[ActR2[mp, trace[[2]]]]}]; ]; l1 = Links /. {opts} /. Options[ShowLinkage]; links = If[l1 === Automatic, SortBy[links, First], First[Cases[links, {#, _}]]& /@ l1]; If[return === "graph", {l1, l2} = {Take[##], Drop[##]}&[Sort[First /@ links], Ceiling[Length[links] / 2]]; If[MemberQ[l2, base], {l1, l2} = {l2, l1}]; v = Function[l, If[MemberQ[l1, l], {-Position[l1, l][[1,1]], 1}, {-Position[l2, l][[1,1]], 0}]]; thk = If[MatchQ[thick, {_, _}], thick[[2]], 0.002]; gr = Join[{Thickness -> thk}, Line[{v[#1], v[#2]}]& @@@ joints]; AppendTo[opts1, ImageSize -> 400]; thk = (ImageSize /. opts1) / 6 / Length[l1]; Do[ x = links[[i,1]]; col = If[x =!= (x /. color), x /. color, White]; gr = Join[gr, {col, Disk[v[x], 1/8], Black, Circle[v[x], 1/8], Text[Style[x, thk], v[x]]}]; , {i, Length[links]}]; Return[Graphics[gr, Sequence @@ opts1]]; ]; (* Find collisions. *) If[return === "collisions", col = Select[Subsets[First /@ links, {3}], MemberQ[joints, {#[[1]], #[[3]], _} | {#[[3]], #[[1]], _}]&]; (* {i1, i3, i2} means that link i3 and the joint between links i1 and i2 could collide. *) col = Flatten[Function[{i1, i3, i2}, js = Last[First[Cases[joints, {i1, i2, _} | {i2, i1, _}]]]; lines = Subsets[Last /@ Cases[joints, {i3, _, _} | {_, i3, _}], {2}]; {{i1, i3, i2}, (x * #[[1]] + (1 - x) * #[[2]] - js)}& /@ lines] @@@ col, 1]; col = Flatten[Function[{i123, sys}, Prepend[{x, t} /. #, i123]& /@ Join[ Solve[Thread[sys == 0], {x, t}], Append[#, t -> Infinity]& /@ Solve[Thread[Limit[sys, t -> Infinity] == 0], x]] ] @@@ col, 1]; col = Select[col, Element[#[[2]], Reals] &]; col = Select[col, 0 <= #[[2]] <= 1 && (Element[#[[3]], Reals] || #[[3]] === Infinity) &]; Return[col]; ]; (* Determine the plot range (if not given by option). *) If[range === All, js = With[{r = Pi/2 - 0.01}, Union @@ Table[(Last /@ joints) /. t -> Tan[x], {x, -r, r, r / 25}]]; range = {{Min[#], Max[#]}&[First /@ js], {Min[#], Max[#]}&[Last /@ js]}; range += ((First[Differences[#]] / 8 * {-1, 1})& /@ range); (* Avoid images of small width and large height. *) With[{dx = range[[1,2]] - range[[1,1]], dy = range[[2,2]] - range[[2,1]]}, If[dy > 2 * dx, range[[1]] = range[[1,1]] + dx / 2 + {-1, 1} * dy / 2]; ]; ]; (* Determine the thickness for drawing links and lines (if not given by option). *) thick = If[thick === Automatic, {Max[Differences /@ range] / 400, 0.002}, Flatten[{thick, 0.002}]]; thk = thick[[1]]; gr = {Thickness[thick[[2]]]}; (* Graphics primitives for the traced curve *) If[trace =!= None, mp = Cases[links, {trace[[1]], _}][[1,2]]; traceZ = If[draw3D, Position[links, {trace[[1]], _}][[1,1]] * 8 * thk, False]; (* They are generated separately in the case of a movie. *) If[Not[MatchQ[return, {"movie" | "movie3D", _}]], gr = Join[gr, {Red, Flatten[TraceMP[mp, t, trace[[2]], Take[range, 2], {r1, r2}, dir, traceZ]]}]; ]; ]; (* Graphics primitives for links and joints *) links = First /@ links; Do[ l1 = links[[i]]; js = Cases[joints, {l1, _, _} | {_, l1, _}]; (* Draw a link. *) lines = Switch[Style /. {opts} /. Options[ShowLinkage], "CompleteGraph", Subsets[Last /@ js, {2}], "Star", With[{pts = Last /@ js}, If[(l2 = Length[pts]) === 2, {pts}, {Total[pts] / l2, #}& /@ pts]] ]; col = If[l1 =!= (l1 /. color), l1 /. color, If[draw3D, Gray, LightGray]]; gr = Join[gr, Flatten[If[draw3D, (* 3D *) {col, EdgeForm[], Specularity[White, 30], Cylinder[Join[#, {{i}, {i}} * 8 * thk, 2], 2 * thk]}, (* 2D *) LineToGraphics[#, 3/2 * thk, col] ]& /@ lines]]; (* Draw only those joints which are not attached to a yet-to-be-drawn link (relevant only in 2D). *) js = Select[js, Intersection[Most[#], Drop[links, i]] === {} &]; If[draw3D, js = Function[j, pos = Append[j[[3]], #]& /@ (Sort[Position[links, #][[1,1]]& /@ Take[j, 2]] * 8 * thk); (* Hack for nice display of Z-joints *) (* If[MatchQ[Take[j, 2], {-4, 9} | {-3, 8}], pos = Drop[pos, 1]]; *) Which[ (* pen *) SameQ @@ pos && Length[pos] == 2, {RGBColor[0.99, 0, 0], Specularity[White, 30], Sphere[pos[[1]], 4 * thk]}, (* pseudo-joint (rigid) *) MemberQ[psj, Take[j, 2]], col = If[l1 =!= (l1 /. color), l1 /. color, Gray]; Join[{col, EdgeForm[], Specularity[White, 30], Cylinder[pos, 2 * thk]}, Sphere[#, 2 * thk]& /@ pos], (* Z-joint *) Length[pos] == 1, {Gray, EdgeForm[Thin], Specularity[White, 10], Cylinder[{pos[[1]] + {0, 0, -2 * thk}, pos[[1]] + {0, 0, 2 * thk}}, 3 * thk]}, (* joint *) True, pos = {{pos[[1]] - {0, 0, 1} * 3 * thk, Total[pos] / 2}, {Total[pos] / 2, pos[[2]] + {0, 0, 1} * 3 * thk}}; {Gray, EdgeForm[Thin], Specularity[White, 10], Cylinder[#, 3 * thk]& /@ pos} ] ] /@ js; , js = With[{pen = (trace =!= None && #1 === #2 === trace[[1]])}, {If[pen, RGBColor[0.99, 0, 0], LightGray], Disk[#3, 4 * thk], Black, Circle[#3, 4 * thk], If[pen, {}, Disk[#3, thk]]}]& @@@ js; ]; gr = Join[gr, Flatten[js]]; , {i, Length[links]}]; (* Options for Graphics *) If[draw3D, If[Length[range] === 2, range = Append[range, {0, Length[links] + 1} * 8 * thk]]; AppendTo[opts1, Lighting -> "Neutral"]; ]; opts1 = Sequence @@ Join[{PlotRange -> range}, opts1, {ImageSize -> 400}]; draw3D = If[draw3D, Graphics3D, Graphics]; Switch[return, "animation" | "animation3D", Format[x] := ArcTan["t"]; gr = (Function @@ {t, "cmnd"[gr, opts1]}) /. "cmnd" -> draw3D; Return[Animate[TableForm[{"t = " <> ToString[Tan[x]], gr[Tan[x]]}], {x, r1, r2}, AnimationRunning -> False]], {"picture" | "picture3D", _?NumericQ}, Return[draw3D[gr /. t -> Tan[return[[2]]], opts1]], {"movie" | "movie3D", _Integer | _List}, {l1, l2} = 2 * Round[ImageDimensions[draw3D[gr /. t -> 0, opts1]] / 4]; If[Head[td = return[[2]]] === Integer, td = Table[r1 + (r2-r1)*(i+1/2)/td, {i, 0, td - 1}]]; Do[ graph = If[trace === None, {}, Flatten[{Red, TraceMP[mp, t, trace[[2]], range, {td[[1]], td[[i]]}, dir, traceZ]}]]; Export[ "temp" <> (StringJoin @@ Table["0", {5 - StringLength[#]}]) <> # <> ".jpg" &[ToString[i-1]], ImageResize[draw3D[Join[{gr[[1]]}, graph, Rest[gr] /. t -> Tan[td[[i]]]], opts1], {l1, l2}], "CompressionLevel" -> 0.01]; , {i, Length[td]}]; Return["In the directory " <> Directory[] <> " execute the following command:\n" <> "ffmpeg -i temp%05d.jpg -r 25 -qscale 4 movie.mp4"], _, Throw["Option Return must be one of \"animation\", {\"picture\", t}, {\"movie\", frames}."]; ]; ];