MODULE OriKamSpring;

(* In this module non exists an Apropiate factor normalization for this energy,
   actually it is 0.5.

   Revisions:

   18-11-2000:  Added the constant value Epsilon to avoid possiblev infinite
                values (Nan).
*)


IMPORT LR4, Triangulation, Fmt, Math, Stdio, Wr, Thread;
  
FROM Triangulation IMPORT Topology, OrgV;
FROM Energy IMPORT Coords, Gradient;
FROM Stdio IMPORT stderr;

CONST Epsilon = 1.0d-10;

TYPE
  BOOLS = ARRAY OF BOOLEAN;
  LONGS = ARRAY OF LONGREAL;
  AdjMatrix = ARRAY OF ARRAY OF LONGREAL;

VAR inf : LONGREAL := Math.pow(10.0d0,10.0d0);  
  
REVEAL
  T = Public BRANDED OBJECT
      F: LONGREAL;               (* The energy normalization factor *)    
      top: Topology;             (* The topology *)
      termVar: REF BOOLS;        (* TRUE if vertex is variable & existing *)
      m:  REF AdjMatrix;         (* Matrix of initial distances *)
      eDdif: REF ARRAY OF LONGS; (* (Work) Gradient of "e" rel. to "dif"  *)
      L:  LONGREAL;              (* Kamada' constant *)
      l,k: REF AdjMatrix;        (* parameters: lij is the original length of
                                    the spring between pi and pj; kij is the
				    strength of  the spring  pi and pj. *)
                                 
    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 : LONGREAL;
  BEGIN
    WITH
      NV       = top.NV,
      length   = FLOAT(erg.length, LONGREAL),
      strength = FLOAT(erg.strength, LONGREAL) 
    DO
      erg.m := MakeAdjMatrix(top);
      ShortestPath(erg.m, NV);
      IF erg.detail THEN
        PrintHalfMatrix(erg.m, NV);
      END; 
      dmax    := FindMaxDistance(erg.m, NV);
      erg.L   := length/dmax;
      erg.F   := 0.50d0;  
      erg.top := top;
      erg.termVar := NEW(REF BOOLS, NV);
      erg.eDdif   := NEW(REF ARRAY OF LONGS, NV, NV);
      erg.l := CalculateLength (erg.m, NV, erg.L);
      erg.k := CalculateStrengh(erg.m, NV, strength);
      IF erg.detail THEN 
        PrintHalfMatrix(erg.k, NV);
      END;
      (* 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
    (* Decides 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 MakeAdjMatrix(
  READONLY top : Triangulation.Topology;
  ) : REF AdjMatrix =
  VAR
    m: REF ARRAY OF ARRAY OF LONGREAL;
    v: REF ARRAY OF INTEGER;
  BEGIN
    m := NEW(REF ARRAY OF ARRAY OF LONGREAL, 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
    END;

    FOR i := 0 TO top.NV-1 DO 
      WITH
        a = top.out[i],
        star = Triangulation.Neighbors(OrgV(a),top),
        nv = NUMBER(star^)
      DO
        m[i,i] := 0.0d0;
        v := NEW(REF ARRAY OF INTEGER, nv);
        FOR k := 0 TO nv-1 DO
          v[k] := star[k].num;
          m[i,v[k]] := 1.0d0;
          m[v[k],i] := 1.0d0;
        END
      END
    END;
    RETURN m;
  END MakeAdjMatrix;

PROCEDURE FindMaxDistance(
   READONLY m: REF AdjMatrix;
   n: INTEGER;
   ) : LONGREAL =
  VAR max := 0.0d0; 
  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 CalculateStrengh(
   READONLY m : REF AdjMatrix;
   n: INTEGER;
   K: LONGREAL;
   ) : REF AdjMatrix =
  VAR
    k : REF ARRAY OF ARRAY OF LONGREAL; 
  BEGIN
    k := NEW(REF ARRAY OF ARRAY OF LONGREAL, n, n);
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO i DO
        IF i = j
	  THEN  k[i,j] := 0.0d0;
	ELSE
          k[i,j] := K / (m[i,j]*m[i,j]);
	  k[j,i] := k[i,j];
	END  
      END
    END;
    RETURN k
  END CalculateStrengh;

PROCEDURE CalculateLength(
   READONLY m : REF AdjMatrix;
   n: INTEGER;
   L: LONGREAL;
   ) : REF AdjMatrix =
  VAR
    l: REF ARRAY OF ARRAY OF LONGREAL; 
  BEGIN
    l := NEW(REF ARRAY OF ARRAY OF LONGREAL, n, n);
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO i  DO
        l[i,j] := L * m[i,j];
	l[j,i] := l[i,j];
      END
    END;
    RETURN l;
  END CalculateLength;

<* UNUSED *>
PROCEDURE PrintAdjMatrix(READONLY m : REF AdjMatrix; 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.LongReal(m[i,j]) & " ");
        END;
       END;
       Wr.PutText(stderr, "\n");
    END;
  END PrintAdjMatrix;

PROCEDURE PrintHalfMatrix(READONLY m : REF AdjMatrix; 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.LongReal(m[i,j],Fmt.Style.Fix,prec:=3) & " ");
        END;
      END;
      Wr.PutText(stderr, "\n");
    END;
  END PrintHalfMatrix;

PROCEDURE ShortestPath(VAR m : REF AdjMatrix; n : INTEGER) =
  VAR s : LONGREAL;     
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO n-1 DO
        IF m[i,j] < 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: LONGREAL; 
    grad: BOOLEAN;
    VAR eDc: Gradient;
  ) =
  BEGIN
    WITH
      NV      = erg.top.NV,
      F       = erg.F,
      eDdif   = erg.eDdif^,
      termVar = erg.termVar,
      k       = erg.k^,
      l       = erg.l^
    DO
      PROCEDURE AccumTerm(READONLY u: CARDINAL) =
        (*
          Adds to "e" the energy term corresponding to a vertex "u".
	  Returns also the gradient "eDdif".
        *)
        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),
		luv = l[u,i],
		kuv = k[u,i],
                d   = dif - luv,
                d2  = d * d
              DO
                e := e + F * kuv * d2;
                IF grad THEN
                  eDdif[u,i] := 2.0d0 * F * kuv * d;
		  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: CARDINAL) =
        (* Distribute eDdif on endpoints "c[u]" and "c[v]" *)
        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
      "OriKamada(ilenth := " & Fmt.Real(erg.length,Fmt.Style.Fix,prec := 3) &
      " istrength := " & Fmt.Real(erg.strength,Fmt.Style.Fix,prec:= 3) & ")";
  END Name;

BEGIN
END OriKamSpring.
(* Last edited on 2001-05-21 02:29:25 by stolfi *)