(* ::Package:: *)

(* :Name: RigidityTools` *)

(* :Title: Rigidity Tools *)

(* :Author: Greg Clark *)

(* :Summary:
  This package provides functions related to rigidity theory.  It is a work
in progress.  Any suggestions are welcome.
*)

(* :Package Version: 0.1 *)

(* :Copyright: Copyright 2008, ... *)

(* :Keywords: Rigidity, Frameworks *)

(* :Mathematica Version: 6.0 *)

BeginPackage["RigidityTools`"];


RigidityMatrix::usage = "RigidityMatrix[{verticies, edges}] generates the 
rigidity matrix for a given framework in Euclidean space for a given 
dimension"


Unprotect[
  RigidityMatrix, RigidityMatrix3D, FirstOrderRigidQ, FirstOrderFlex,
  FirstOrderFlex3D, SphericalTo3DFramework, GraphFramework, 
  GraphFramework3D, GraphFrameworkSpherical, GraphFirstOrderFlex,
  GraphFirstOrderFlex3D, GraphFirstOrderFlexSpherical,
  GeneralPositionQ, GeneralPosition3DQ, GeneralPositionSphericalQ,
  GenericFrameworkQ
];

(* Consistent documentation makes the above unnecessary. *)

Begin["`Private`"];


RigidityMatrix[{verticies_,edges_},dimension_] := (
  Module[{matrix = {},row},
    For[r=1,r<Length[edges]+1,r++,
    row = {};
    For[c = 1, c < Length[verticies]+1, c++,
      For[xy = 1, xy < dimension+1, xy++,
        If[edges[[r,1]] == c,
          AppendTo[row, verticies[[c,xy]] - verticies[[edges[[r,2]], xy]]];
        ,
          If[edges[[r,2]] == c,
            AppendTo[row,verticies[[c,xy]] - verticies[[edges[[r,1]], xy]]]
          ,
            AppendTo[row, 0]
          ];
        ];
      ];
    ];
    AppendTo[matrix,row]
  ]; 
  matrix]);


RigidityMatrix[{verticies_,edges_}] := (
  RigidityMatrix[{verticies, edges}, Dimensions[verticies][[2]]]);


RigidityMatrix3D[{verticies_, edges_}] := (
  RigidityMatrix[{verticies, edges}, 3]);


FirstOrderRigidQ[{verticies_, edges_}] := (
  Dimensions[NullSpace[RigidityMatrix[{verticies, edges}]]][[1]] == 
    3*Dimensions[verticies][[2]]-3)


FirstOrderFlex[framework_, conditions_, dimension_] := (
  Module[{matrix, vel,vels, vars, zeros, mots, reformatedVs},
    matrix = RigidityMatrix[framework];
    If[FirstOrderRigidQ[framework],
      (*Print["This matrix is first order rigid."];
      Return[Null]*)
      Return["This matrix is first order rigid."]
    ,
      (*Print["This matrix is first order flexible."]*)
    ];

    vels = Array[vel, Dimensions[matrix][[2]]];

    (* Set the known velocities based on the conditions. *)
    For[c = 1, c < Length[conditions]+1, c++, 
      Switch[conditions[[c,1]],
      "fix",
        For[d = dimension-1, d > -1, d--,
          vel[conditions[[c,2]]*dimension-d] = 0;
        ],
      "x",
        vel[conditions[[c,2]]*dimension-dimension+1] = conditions[[c,3]],
      "y",
        vel[conditions[[c,2]]*dimension-dimension+2] = conditions[[c,3]],
      "z",
        (* check what dimension ? *)
        vel[conditions[[c,2]]*dimension] = conditions[[c,3]],
      _, 
        Throw["Bad condition."]
      ];
    ];

    vars = Select[vels, Not[NumericQ[#]]&];
    vels = Transpose[{vels}];

    zeros = {};
    For[r = 0, r <Dimensions[matrix][[1]], r++, AppendTo[zeros, {0}]];

    mots = Solve[matrix.vels == zeros, vars];
    vels = (vels /. mots)[[1]];

    (*
    Print[Row[{Opener[Dynamic[x]],
    PaneSelector[{False->"Velocities",True-> vels},Dynamic[x]]}]];
    *)

    reformatedVs = {};
    For[i = 1, i < Length[vels]/dimension + 1, i++,
    AppendTo[reformatedVs, Table[vels[[dimension*i-d,1]],
      {d, dimension-1,0,-1}]];
  ];
  reformatedVs])


FirstOrderFlex[{verticies_,edges_}, conditions_] := (
  FirstOrderFlex[{verticies,edges}, conditions,Dimensions[verticies][[2]]])


FirstOrderFlex3D[framework_, conditions_] := (
  FirstOrderFlex[framework, conditions, 3])


SphericalTo3DFramework[{polarverticies_,edges_}] := (
  Module[{verticies, newedges = edges},
    verticies = Map[PolarToEuclidean,polarverticies];
    AppendTo[verticies,{0,0,0}];
    For[v = 1, v < Length[verticies],v++,
      AppendTo[newedges,{v,Length[verticies]}];
    ];
    {verticies, newedges}])


PolarToEuclidean[{\[Theta]_,\[Phi]_ }] := ({Cos[\[Theta]]*Cos[ \[Phi]],Sin[\[Theta]]*Cos[\[Phi]],Sin[\[Phi]]})


GraphFramework[{verticies_,edges_}] := (
  Graphics[{PointSize[0.05],
    Point[Table[verticies[[t]],{t,1,Length[verticies],1}]],
    Thick,
    Line[Table[{verticies[[edges[[t,1]]]],verticies[[edges[[t,2]]]]}, 
      {t, 1,Length[edges],1}]]
  }])


GraphFramework3D[{verticies_,edges_}] := (
  Graphics3D[{PointSize[0.05],
    Point[Table[verticies[[t]],{t,1,Length[verticies],1}]],
    Thick,
    Line[Table[{verticies[[edges[[t,1]]]],verticies[[edges[[t,2]]]]}, 
      {t, 1,Length[edges],1}]]
  }])


GraphFrameworkSpherical[{verticies_,edges_}] := (
  Module[{newverticies, newedges},
    {newverticies, newedges} = SphericalTo3DFramework[{verticies, edges}];
    Graphics3D[{PointSize[0.05],
      Point[Table[newverticies[[t]],{t,1,Length[verticies],1}]],
      Blue,Opacity[0.4],Specularity[White,5],
      Sphere[{0,0,0},1],
      Thick,Black,Opacity[1],
      SphericalLine[Table[{newverticies[[edges[[t,1]]]],
        newverticies[[edges[[t,2]]]]}, {t, 1,Length[edges],1}]]
  }]])


(* This does not work well when the two points are close to opposite 
   each other on the sphere*)
SphericalLine[pairsin_] := (
  Module[{v,p, x1, y1, z1, x2, y2, z2, sx,sy,sz, pairs, pair, result = {}},
    If[Length[Dimensions[pairsin]]==2,
      pairs = {pairsin}
    ,
      pairs = pairsin
    ];
    sx[t_] := x1 * t + x2*(1-t);
    sy[t_] := y1 * t + y2*(1-t);
    sz[t_] := z1 * t + z2*(1-t);
    For[i = 1, i < Length[pairs]+1, i++,
      pair = pairs[[i]];
      {x1,y1,z1} = pair[[1]];
      {x2,y2,z2} = pair[[2]];
      v = Table[{sx[t],sy[t], sz[t]}/(Sqrt[sx[t]^2+sy[t]^2+sz[t]^2]),
        {t,0,1, 1/30}];
      p = Table[t, {t, 1,30+1}];
      AppendTo[result,GraphicsComplex[v,Line[p]]];
    ];
  result])


(*
GraphFrameworkSphericalOld[{verticies_,edges_}] := (
  Block[{newverticies, newedges},
    {newverticies, newedges} = SphericalTo3DFramework[{verticies, edges}];
    Graphics3D[{PointSize[0.05],
      Point[Table[newverticies[[t]],{t,1,Length[verticies],1}]],
      Blue,Opacity[0.4],Specularity[White,5],
      Sphere[{0,0,0},1],
      Thick,Black,Opacity[1],
      Line[Table[{newverticies[[edges[[t,1]]]],
        newverticies[[edges[[t,2]]]]}, {t, 1,Length[edges],1}]]
  }]])
*)


GraphFirstOrderFlex[{verticies_, edges_}, conditions_] := (
  Module[{motions = FirstOrderFlex[{verticies, edges}, conditions]},
    If[motions == Null, Return[Null]];
    Graphics[{PointSize[0.05],
      Thick,
      Line[Table[verticies[[edges[[t]]]], {t, 1,Length[edges],1}]],
      Red,Dashed,Arrowheads[Large],
      Table[Arrow[{verticies[[v]], verticies[[v]] + motions[[v]]}],
      {v, 1,Length[verticies],1}],
      Black,
      Point[Table[verticies[[t]],{t,1,Length[verticies],1}]]
  }]])


GraphFirstOrderFlex3D[{verticies_, edges_}, conditions_] := (
  Module[{motions = FirstOrderFlex3D[{verticies, edges}, conditions]},
    If[motions == Null, Return[Null]];
    Graphics3D[{PointSize[0.05],
      Thick,
      Line[Table[verticies[[edges[[t]]]], {t, 1,Length[edges],1}]],
      Red,(*Dashed,*)Arrowheads[Large],
      Table[Line[{verticies[[v]], verticies[[v]] + motions[[v]]}],
      {v, 1,Length[verticies],1}],
      Black,
      Point[Table[verticies[[t]],{t,1,Length[verticies],1}]]
  }]])


GraphFirstOrderFlexSpherical[{verticies_, edges_}, conditions_] := (
  Module[{newverticies, newedges,motions, newconditions = conditions},
    {newverticies, newedges} = SphericalTo3DFramework[{verticies, edges}];
    AppendTo[newconditions, {"fix", Length[newverticies]}];
    motions = FirstOrderFlex3D[{newverticies, newedges}, newconditions];
    If[motions == Null, Return[Null]];
    Graphics3D[{PointSize[0.05],
      Blue,Opacity[0.4],Specularity[White,5],
      Sphere[],
      Thick,Black,Opacity[1],
      (*Line[Table[newverticies[[edges[[t]]]], {t, 1,Length[edges],1}]],*)
      SphericalLine[Table[{newverticies[[edges[[t,1]]]],
        newverticies[[edges[[t,2]]]]}, {t, 1,Length[edges],1}]],
      Red,
      Table[Line[{newverticies[[v]], newverticies[[v]] + motions[[v]]}],
      {v, 1,Length[verticies],1}],
      Black,
      Point[Table[newverticies[[t]],{t,1,Length[verticies],1}]]
    }]])


GeneralPositionQ[{verticies_, edges_}] := (
  Module[{triples = Subsets[verticies,{3}]},
    For[t = 1, t < Length[triples]+1, t++,
      If[CollinearQ[triples[[t]]],Return[False]]
    ];
    True])


GeneralPosition3DQ[{verticies_, edges_}] := (
  Module[{quads = Subsets[verticies,{4}]},
    For[t = 1, t < Length[quads]+1, t++,
      If[CoplanarQ[quads[[t]]],Return[False]]
    ];
    True])


GeneralPositionSphericalQ[framework_] := (
  GeneralPosition3DQ[SphericalTo3DFramework[framework]])


CollinearQ[{p1_,p2_,p3_}] :=(MatrixRank[{{0,0},p2-p1,p3-p1}] < 2)


CoplanarQ[{p1_,p2_,p3_, p4_}] :=(
  MatrixRank[{{0,0,0},p2-p1,p3-p1, p4-p1}] < 3)


(* Is this ever used? *)
CoDimensional[points_] := (
  Module[{adjusted = {}},
    For[i = 1, i <Length[points]+1, i++, 
      AppendTo[adjusted,points[[i]] - points[[1]]]
    ];
    MatrixRank[adjusted] < Length[points]-1])


MaxRankForVerticies[number_] := (number*2-3)


GenericFrameworkQ[{verticies_, edges_}] := (
  Module[{rm,min, minors, zeros, abstractzeros , returnvalue = True},
    rm = CompleteRigidityMatrix[verticies];
    min= Min[Dimensions[rm]];
    abstractzeros = ZeroMinorsForAbstractFramework[Length[verticies]]; 

    For[d=1, d<min+1, d++,
      minors =Flatten[Minors[rm, d]];
      zeros = 0;
      For[m = 1, m < Dimensions[minors][[1]]+1, m++,
      If[Expand[minors[[m]]] == 0,zeros += 1];
    ];
    (*
    Print["Number of real zeros: ",zeros];
    Print["Number of abstract zeros: ",abstractzeros[[d]]];
    *)
    If[zeros > abstractzeros[[d]], 
      returnvalue = False (* This SHould return immediately. *)
    ,
      If[zeros < abstractzeros[[d]],
        Print["WARNING: The given framework has more zero minors than 
the abstract one.  This should never happen.  It may be because some of 
the coordinates of the verticies are defined using floating point number.  
Don't use floats."]];
      ];
    ];
    returnvalue])


ZeroMinorsForAbstractFramework[number_] := (
  Module[{x,y,rm, maxminordim,dminors, zeros, numberofzerominors = {}},
    rm =AbstractRigidityMatrix[number];
    maxminordim = Min[Dimensions[rm]];
    For[d=1, d<maxminordim+1, d++,
      dminors = Flatten[Minors[rm, d]];
      zeros = 0;
      For[m = 1, m < Dimensions[dminors][[1]]+1, m++,
        If[Expand[dminors[[m]]] == 0,zeros += 1];
      ];
      (*Print["zeros for ",d,": ", zeros];*)
      AppendTo[numberofzerominors, zeros];
    ];
    numberofzerominors])


AbstractRigidityMatrix[number_] :=(
  Module[{x,y,verticies ={}},
    For[n = 1, n < number +1, n++,AppendTo[verticies,{x[n],y[n]}]];
    CompleteRigidityMatrix[verticies]])


CompleteRigidityMatrix[verticies_] := (
  Module[{x,y,edges,nlist = {}},
    For[n = 1, n < Length[verticies] +1, n++, AppendTo[nlist,n]];
    edges = Subsets[nlist, {2}];
    RigidityMatrix[{verticies, edges}]])


End[]; (* End private context. *)
EndPackage[]; (* End package context. *)