MODULE SimpleSpring;

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

IMPORT LR4, Triangulation, Fmt;
  
FROM Triangulation IMPORT Topology, OrgV;
FROM Octf IMPORT Clock;
FROM Energy IMPORT Coords, Gradient;

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

REVEAL
  T = Public BRANDED OBJECT
      K: LONGREAL;               (* The energy normalization factor *)    
      top: Topology;             (* The topology *)
      termVar: REF BOOLS;        (* TRUE if vertex is variable & existing *)
      edgeRelevant: REF BOOLS;   (* TRUE if edge is relevant *)
      eDdif: REF LONGS           (* (Work) Gradient of "e" rel. to "dif" *)
    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) =
  BEGIN
    erg.K := 1.0d0;
    erg.top := top;
    erg.termVar := NEW(REF BOOLS, top.NV);
    erg.edgeRelevant := NEW(REF BOOLS, top.NE);
    erg.eDdif := NEW(REF LONGS, top.NE);
      
    (* Just in case the client forgets to call "defVar": *)
    FOR i := 0 TO top.NV-1 DO erg.termVar^[i] := FALSE END;
    FOR j := 0 TO top.NE-1 DO erg.edgeRelevant^[j] := FALSE END;
  END DefTop;
 
PROCEDURE DefVar(erg: T; READONLY variable: BOOLS) =
  BEGIN
    (*
      Decide which edges are relevant to spring energy. A edge is relevant
      iff it has at least one endpoint vertex that is variable.
    *)
    WITH
      NV      = erg.top.NV,
      NE      = erg.top.NE,
      termVar = erg.termVar^,
      edge    = erg.top.edge^,
      edgeRelevant = erg.edgeRelevant^
    DO
      <* ASSERT NUMBER(variable) = NV *>
      FOR v := 0 TO NV-1 DO
        termVar[v] := variable[v];
      END;
 
      (* Find the relevant edges : *)
      FOR k := 0 TO NE-1 DO edgeRelevant[k] := FALSE END;
      FOR k := 0 TO NE-1 DO
        WITH
          e = edge[k], 
          u = NARROW(OrgV(e.pa), Triangulation.Vertex),
          v = NARROW(OrgV(Clock(e.pa)), Triangulation.Vertex),         
          vvar = termVar[u.num] OR termVar[v.num]
        DO
          IF vvar THEN
            edgeRelevant[k] := TRUE;
          END
        END
      END
    END
  END DefVar;

PROCEDURE Eval(
    erg: T; 
    READONLY c: Coords; 
    VAR e: LONGREAL; 
    grad: BOOLEAN;
    VAR eDc: Gradient;
  ) =
  BEGIN
    WITH
      NV = erg.top.NV,
      NE = erg.top.NE,
      edge = erg.top.edge^,
      edgeRelevant = erg.edgeRelevant^,
      K = erg.K,
      L = FLOAT(erg.length, LONGREAL),
      eDdif = erg.eDdif^,
      termVar = erg.termVar
    DO
      PROCEDURE AccumTerm(READONLY cu,cv: LR4.T; VAR eDdif : LONGREAL) =
        (*
          Adds to "e" the energy term corresponding to a vertices "cv"
          and "cu". Returns also the gradient "eDdif".
        *)
        CONST Epsilon = 1.0d-10; 
        BEGIN
          WITH
            n   = LR4.Sub(cu,cv),
            dif = LR4.Norm(n),
            d2  = dif * dif + Epsilon,
            l2  = L * L     + Epsilon,
            d3  = d2 * dif  + Epsilon
          DO
            e := e + K * ((d2/l2) + (l2/d2) - 2.0d0);
            IF grad THEN
              eDdif := 2.0d0 * K * ((dif/l2)-(l2/d3) );
            ELSE
              eDdif := 0.0d0;
            END
          END
       END AccumTerm;


      PROCEDURE Distribute_eDdif(READONLY u,v: CARDINAL;
                                 READONLY eDdif: LONGREAL) =
        (* Distribute eDdif on endpoints "c[u]" and "c[v]" *)
        CONST Epsilon = 1.0d-10;
        BEGIN
          WITH
            cu  = c[u],
            cv  = c[v],
            n   = LR4.Sub(cu,cv),
            dif = LR4.Norm(n)+Epsilon,
            eDv = eDc[v],
            eDu = eDc[u]
          DO
            WITH
              difDcu = LR4.Scale(1.0d0/dif, n),
              difDcv = LR4.Scale(-1.0d0/dif,n),
              eDcu   = LR4.Scale(eDdif, difDcu),
              eDcv   = LR4.Scale(eDdif, difDcv)
            DO
              IF termVar[v] THEN
                eDv := LR4.Add(eDv, eDcv);
              END;
              IF termVar[u] THEN
                eDu := LR4.Add(eDu, eDcu);
              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 k := 0 TO NE-1 DO
          WITH
            e  = edge[k], 
            un = OrgV(e.pa).num, 
            vn = OrgV(Clock(e.pa)).num
          DO
            IF edgeRelevant[k] THEN
              AccumTerm(c[un], c[vn], eDdif[k]);
              IF grad THEN
                Distribute_eDdif(un, vn, eDdif[k]);
              END
            END
          END
        END
      END
    END
  END Eval;


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

BEGIN
END SimpleSpring.
(* Last edited on 2001-05-21 02:27:52 by stolfi *)