MODULE Main;

IMPORT LR3, Simulator, KinematicConstraint;
FROM Simulator IMPORT AddConstraint, GetPos, GetVel;

CONST vertex = 3;

BEGIN
  Simulator.Init();
  WITH
    pos0 = Simulator.GetInitialPos(),
    vel0 = Simulator.GetInitialVel(),
    t0   = Simulator.GetInitialTime(),
    
    c1 = NEW(KinematicConstraint.T),
    c2 = NEW(KinematicConstraint.T),

    pa = GetPos(vertex),
    va = GetVel(vertex),
    pb = LR3.T{0.0D0, 16.0D0, 10.0D0},
    vb = LR3.T{0.0D0,  0.0D0,  0.0D0},
    pc = LR3.T{9.0D0,  6.0D0,  5.0D0},
    vc = LR3.T{0.0D0,  0.0D0,  0.0D0}
  DO
    c1.init(vertex, 1.0D0, 3.0D0, pa, pb, va, vb);
    c2.init(vertex, 3.00001D0, 5.0D0, pb, pc, vb, vc);
    c1.activate(pos0, vel0, t0);
    c2.activate(pos0, vel0, t0);
    AddConstraint(c1);
    AddConstraint(c2);
  END;
  Simulator.Go();
END Main.