MODULE VarKamSpring;

(* Last Version: 18-11-2000 *)

IMPORT LR4, Triangulation, Fmt, Stdio, Wr, Thread, Octf, Math;
  
FROM Triangulation IMPORT Topology, OrgV, Pneg;
FROM Energy IMPORT Coords, Gradient;
FROM Stdio IMPORT stderr;
FROM Octf IMPORT Pair, Clock, Fnext;

CONST inf = 10.0E30;

TYPE
  BOOL = BOOLEAN; BOOLS = ARRAY OF BOOL;
  LONG = LONGREAL; LONGS = ARRAY OF LONG;
  NAT = CARDINAL; NATS = ARRAY OF NAT;
  DistanceMatrix = ARRAY OF ARRAY OF REAL;
 
REVEAL
  T = Public BRANDED OBJECT
      top: Topology;             (* The topology *)
      termVar: REF BOOLS;        (* TRUE if vertex is variable & existing *)
      m : REF DistanceMatrix;   (* Matrix of initial distances *)
      eDdif: REF ARRAY OF LONGS; (* (Work) Gradient of "e" rel. to "dif"  *)
      L : LONG;              (* length of spring *)
                                 
    OVERRIDES
      init := Init;
      defTop := DefTop;
      defVar := DefVar;
      eval := Eval;
      name := Name;
    END;

PROCEDURE Init(erg: T): T =
  BEGIN
    RETURN erg
  END Init;

PROCEDURE DefTop(erg: T; READONLY top: Topology) =
  VAR dmax : LONG;
  BEGIN
    WITH
      NV = top.NV
    DO
      erg.m := ComputeDistanceMatrix(top);
      (*PrintHalfMatrix(erg.m, NV);*)
      ShortestPath(erg.m, NV);
      (* PrintHalfMatrix(erg.m, NV); *)
      dmax :=  FLOAT(FindMaxDistance(erg.m, NV), LONG);
      erg.L := FLOAT(erg.length,LONG)/dmax;
      erg.top := top;
      erg.termVar := NEW(REF BOOLS, NV);
      erg.eDdif := NEW(REF ARRAY OF LONGS, NV, NV);
      (*PrintHalfMatrix(erg.k, NV);*)
      
      (* Just in case the client forgets to call "defVar": *)
      FOR i := 0 TO top.NV-1 DO erg.termVar^[i] := FALSE END;
    END
  END DefTop;
 
PROCEDURE DefVar(erg: T; READONLY variable: BOOLS) =
  BEGIN
    (*
      Decide which vertices are relevant to kamada energy. A vertex is relevant
      iff it is variable.
    *)
    WITH
      NV = erg.top.NV,
      termVar = erg.termVar^
    DO
      (* Find the relevant vertices: *)
      <* ASSERT NUMBER(variable) = NV *>
      FOR v := 0 TO NV-1 DO
        termVar[v] := variable[v];
      END
    END
  END DefVar;

PROCEDURE ComputeDistanceMatrix(
  READONLY top : Triangulation.Topology;
  ) : REF DistanceMatrix =
  VAR
    m: REF DistanceMatrix;
  BEGIN
    m := NEW(REF ARRAY OF ARRAY OF REAL, top.NV, top.NV);
    FOR i := 0 TO top.NV-1 DO
      FOR j := 0 TO top.NV-1 DO
        m[i,j] := inf;
      END;
      m[i,i] := 0.0;
    END;
    
    WITH
      cdg = ComputeCorrectedDegrees(top)^
    DO
      FOR i := 0 TO top.NE-1 DO      
        WITH
          a = top.edge[i].pa,
          b = Clock(a),
          i  = OrgV(a).num,
          j  = OrgV(b).num,
          ni = FLOAT(cdg[i], LONG),
          nj = FLOAT(cdg[j], LONG),
          lij = FLOAT(Math.sqrt(ni/nj + nj/ni), REAL)
        DO
          m[i,j] := lij;
          m[j,i] := lij;
        END
      END
    END;
    RETURN m;
  END ComputeDistanceMatrix;
  
PROCEDURE ComputeCorrectedDegrees(READONLY top: Topology): REF NATS = 
  (* 
    Computes the corrected degree of each vertex, defined as 
    the true degree for interior vertices, and the true degree
    plus the interior degree for border vertices. *)
  BEGIN
    WITH
      rd = NEW(REF NATS, top.NV), d = rd^,
      border = NEW(REF BOOLS, top.NV)^
    DO
      FOR i := 0 TO LAST(d) DO d[i] := 0; border[i] := FALSE END;
      (* Tally interior edges twice, border edegs once: *)
      FOR i := 0 TO top.NE-1 DO
        WITH
          a = top.edge[i].pa,
          b = Clock(a),
          un = OrgV(a).num,
          vn = OrgV(b).num
        DO
          IF EdgeIsBorder(a) THEN 
            border[un] := TRUE; border[vn] := TRUE;
            INC(d[un]); INC(d[vn])
          ELSE
            INC(d[un],2); INC(d[vn],2)
          END;
        END
      END;
      (* Correct count for non-border vertices: *)
      FOR i := 0 TO LAST(d) DO
        IF NOT border[i] THEN 
          <* ASSERT d[i] MOD 2 = 0 *>
          d[i] := d[i] DIV 2
        END
      END;
      RETURN rd
    END
  END ComputeCorrectedDegrees;
  
PROCEDURE EdgeIsBorder(a: Pair): BOOL =
  VAR b: Pair := a;
  BEGIN
    REPEAT
      IF Pneg(a) = NIL THEN RETURN TRUE END;
      b := Fnext(b);
    UNTIL b = a;
    RETURN FALSE 
  END EdgeIsBorder;
  
PROCEDURE FindMaxDistance(
   READONLY m: REF DistanceMatrix;
   n: INTEGER;
   ) : REAL =
  VAR max := 0.0; 
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO i DO
        IF m[i,j] > max  THEN
          max := m[i,j];
        END
      END
    END;
    RETURN max;
  END FindMaxDistance;
  
PROCEDURE CalculateStrength(dist: REAL) : LONG =
  BEGIN
    IF dist = 0.0 THEN
      RETURN 0.0d0
    ELSE
      WITH
        d = FLOAT(dist, LONG)
      DO
        RETURN 1.0d0/(d*d);
      END
    END
  END CalculateStrength;

PROCEDURE CalculateLength(dist : REAL) : LONG =
  BEGIN
    IF dist = 0.0 THEN
      RETURN 0.0d0
    ELSE
      WITH
        d = FLOAT(dist, LONG)
      DO
        RETURN d;
      END
    END
  END CalculateLength;

<* UNUSED *>
PROCEDURE PrintDistanceMatrix(READONLY m : REF DistanceMatrix; n: INTEGER) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO n-1 DO
        IF m[i,j] = inf THEN
          Wr.PutText(stderr, "# ");
        ELSE
          Wr.PutText(stderr, Fmt.Real(m[i,j], prec:=2, style:=Fmt.Style.Fix) & " ");
        END;
       END;
       Wr.PutText(stderr, "\n");
    END;
  END PrintDistanceMatrix;

<* UNUSED *>
PROCEDURE PrintHalfMatrix(READONLY m : REF DistanceMatrix; n: INTEGER) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO i DO
        IF m[i,j] = inf THEN
          Wr.PutText(stderr, "# ");
        ELSE
          Wr.PutText(stderr, Fmt.Real(m[i,j], prec:=2, style:=Fmt.Style.Fix) & " ");
        END;
       END;
       Wr.PutText(stderr, "\n");
    END;
  END PrintHalfMatrix;

PROCEDURE ShortestPath(VAR m : REF DistanceMatrix; n : INTEGER) =
  VAR s : REAL;     
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO n-1 DO
        IF m[j,i] < inf THEN
          FOR k := 0 TO n-1 DO
            IF m[i,k] < inf THEN
              s := m[j,i] + m[i,k];
              IF s < m[j,k] THEN m[j,k] := s END;
            END
          END
        END
      END
    END
   END ShortestPath;
    
PROCEDURE Eval(
    erg: T; 
    READONLY c: Coords; 
    VAR e: LONG; 
    grad: BOOL;
    VAR eDc: Gradient;
  ) =
  BEGIN
    WITH
      NV = erg.top.NV,
      eDdif = erg.eDdif^,
      termVar = erg.termVar,
      length   = erg.L,
      strength = FLOAT(erg.strength, LONG),
      m = erg.m^
    DO
      PROCEDURE AccumTerm(READONLY u: NAT) =
        (*
          Adds to "e" the energy term corresponding to a vertex "u".
	  Returns also the gradient "eDdif".
        *)
        CONST Epsilon = 1.0d-10; 
        BEGIN  
          WITH
	    cu = c[u]
	  DO    
	    FOR i := u+1 TO NV-1 DO  
	      WITH
	        cv  = c[i],
                n   = LR4.Sub(cu,cv),
                dif = LR4.Norm(n),
		d2  = dif * dif + Epsilon,
		duv = m[u,i],
		luv = length * CalculateLength(duv),
		kuv = strength * CalculateStrength(duv),
		l2  = luv * luv + Epsilon,
		d3  = d2 * dif  + Epsilon
              DO
                e := e + kuv * ( (d2/l2) + (l2/d2) - 2.0d0 );
                IF grad THEN
                  eDdif[u,i] := 2.0d0 * kuv * ( (dif/l2) - (l2/d3) );
		  eDdif[i,u] := eDdif[u,i];
                ELSE
                  eDdif[u,i] := 0.0d0;
		  eDdif[i,u] := eDdif[u,i];
                END
              END
	     END 		
          END
       END AccumTerm;

      PROCEDURE Distribute_eDdif(READONLY u: NAT) =
        (* Distribute eDdif on endpoints "c[u]" and "c[v]" *)
        CONST Epsilon = 1.0d-10;
        BEGIN
	  WITH
	    cu = c[u]
	  DO  
	    FOR i := u+1 TO NV-1 DO  
	      WITH
	        ci     = c[i],
                n      = LR4.Sub(cu,ci),
                dif    = LR4.Norm(n)+Epsilon,
                eDi    = eDc[i],
                eDu    = eDc[u],
                difDcu = LR4.Scale(1.0d0/dif, n),
                difDci = LR4.Scale(-1.0d0/dif,n),
                eDcu   = LR4.Scale(eDdif[u,i], difDcu),
                eDci   = LR4.Scale(eDdif[u,i], difDci)
              DO
                IF termVar[i] THEN
                  eDi := LR4.Add(eDi, eDci);
                END;
                IF termVar[u] THEN
                  eDu := LR4.Add(eDu, eDcu);
                END
              END
	    END   
          END;
        END Distribute_eDdif;
      
      BEGIN
        FOR i := 0 TO NV-1 DO eDc[i]:=LR4.T{0.0d0, 0.0d0, 0.0d0, 0.0d0} END;
        e := 0.0d0;
        FOR l := 0 TO NV-1 DO
          IF termVar[l] THEN
              AccumTerm(l);
              IF grad THEN
                Distribute_eDdif(l);
              END
            END
          END
        END
      END
    END Eval;

PROCEDURE Name(erg: T): TEXT =
  BEGIN
    RETURN
      "VarKamada(length := " & Fmt.Real(erg.length,Fmt.Style.Fix,prec := 3) &
      " strength := " & Fmt.Real(erg.strength,Fmt.Style.Fix,prec:= 3) & ")";
  END Name;

  
BEGIN
END VarKamSpring.
(* Last edited on 2001-05-21 02:36:15 by stolfi *)