rem Double Pendulum .... Rev 2.2
      rem A J Tooth // May 2004
      on error if (err=17) then quit
      proc_fullscreen(22)
      *FLOAT64
      
      rem Input Parameters
      proc_title
      proc_params
      
      rem E0 is the Initial Total Energy
      E0=fn_energy(P0,Q0,Pd0,Qd0)
      print tab(0,0);"Total Energy = ";E0
      print tab(0,2);"Delta Energy = ";
      Flp%=1
      rem Main Routine
      repeat
        for tim%=0 to stps%
          sys "GetCurrentProcess" to hprocess%
          sys "SetPriorityClass", hprocess%, &100
          
          rem Main calculations - Improved Euler Method
          dThdt2a=fn_dfunc2(P0,Q0,Pd0,Qd0,L1,L2) : Qd1a=Qd0 + dThdt2a
          dTh2a=Qd1a*dtim
          
          dThdt2b=fn_dfunc2(P0,Q0,Pd0,Qd1a,L1,L2) : Qd1b=Qd0 + dThdt2b
          dTh2b=Qd1b*dtim
          
          Thdt2=(dThdt2a + dThdt2b)/(2*dtim)
          
          dThdt1a=fn_dfunc1(P0,Q0,Pd0,Qd0,L1,L2,Thdt2) : Pd1a=Pd0 + dThdt1a
          dTh1a=Pd1a*dtim
          
          dThdt1b=fn_dfunc1(P0,Q0,Pd1a,Qd0,L1,L2,Thdt2) : Pd1b=Pd0 + dThdt1b
          dTh1b=Pd1b*dtim
          
          Q1=Q0 + ((dTh2a+dTh2b)/2) : P1=P0 + ((dTh1a+dTh1b)/2)
          Qd1=(Qd1a+Qd1b)/2 : Pd1=(Pd1a+Pd1b)/2
          
          rem Plot section
          proc_plot(P1,Q1,Flp%)
          rem Track Energy
          E=fn_energy(P1,Q1,Pd1,Qd1)
          Pc=(int(100*(E-E0)/E0))/100
          print tab(15,2);Pc;"%         "
          
          rem Resets current values for Phi and Theta at time T
          Q0=Q1 : P0=P1 : Pd0=Pd1 : Qd0=Qd1
          sys "GetCurrentProcess" to hprocess%
          sys "SetPriorityClass", hprocess%, &20
          
        next tim%
        a$=inkey$(1)
        if a$="t" then
          Flp%=-Flp%
          cls
          print tab(0,0);"Total Energy = ";E0
          print tab(0,2);"Delta Energy = ";
        endif
      until a$=" "
      
      a$=get$
      quit
      end
      rem End of Program +++++++++++++++++++++++++++++++++++
      
      rem Set up use of Full Screen
      def proc_fullscreen(N%)
      sys "GetSystemMetrics", 0 to xscreen%
      sys "GetSystemMetrics", 1 to yscreen%
      sys "SetWindowLong",@hwnd%,-16,&16000000
      sys "SetWindowPos",@hwnd%,-1,0,0,xscreen%,yscreen%,0
      mode N%
      mouse off : off : rem Turns off the Mouse Pointer and the Cursor
      endproc
      
      rem Plot Procedure
      def proc_plot(P,Q,Fl%)
      
      X1P=L1*sin(P) : Y1P=-L1*cos(P)
      sx%=sgn(X1P) : sy%=sgn(Y1P)
      x1%=sx%*int(abs(X1P))
      y1%=sy%*int(abs(Y1P))
      
      X2P=X1P+L2*sin(Q) : Y2P=Y1P-L2*cos(Q)
      sx%=sgn(X2P) : sy%=sgn(Y2P)
      x2%=sx%*int(abs(X2P))
      y2%=sy%*int(abs(Y2P))
      
      case Fl% of
        when 1 :
          *REFRESH OFF
          gcol 7:move 0,0 : draw x1%,y1%
          gcol 1:circle fill x1%,y1%,10
          gcol 7:move x1%,y1% : draw x2%,y2%
          gcol 2:circle fill x2%,y2%,10
          *REFRESH
          gcol 0
          move 0,0 : draw x1%,y1% : draw x2%,y2%
          circle fill x1%,y1%,10
          circle fill x2%,y2%,10
        when -1 :
          *REFRESH
          gcol 4 : move x2%,y2% : draw x2%,y2%
      endcase
      gcol 3: circle 0,0,30
      endproc
      
      rem Function 2
      def fn_dfunc2(p,q,pd,qd,l1,l2)
      local c,s,num,den
      
      s=sin(q-p) : c=cos(q-p)
      
      den=l2*(1- m*c*c)
      num=g*c*sin(p) -g*sin(q) -l1*pd*pd*s -m*l2*qd*qd*s*c
      =num*dtim/den
      
      rem Function 1
      def fn_dfunc1(p,q,pd,qd,l1,l2,thdt2)
      local c,s,num,den
      
      s=sin(q-p) : c=cos(q-p)
      
      den=l1*c
      num=-g*sin(q) -l1*pd*pd*s -l2*thdt2
      =num*dtim/den
      
      rem Input Parameters
      def proc_params
      colour 1:print:print " Press -t- to toggle between seeing the Pendulum or the path. Press -space- to QUIT."
      colour 3:print:print:input " Enter number of Degrees 1st Pendulum swings from (90)",Th1
      if Th1=0 then Th1=90
      input " Enter number of Degrees 2nd Pendulum swings from (90)",Th2
      if Th2=0 then Th2=90
      Thd1=0 : Thd2=0
      input " Enter length of 1st Pendulum rod (300)",L1
      if L1=0 then L1=300
      input " Enter length of 2nd Pendulum rod (400)",L2
      if L2=0 then L2=400
      
      input " How many steps for the Time parameter T (250)",stps%
      if stps%=0 then stps%=250
      dtim=1/stps%
      input " Enter Mass of 1st Pendulum Bob (25)",m1
      if m1=0 then m1=25
      input " Enter Mass of 2nd Pendulum Bob (10)",m2
      if m2=0 then m2=10
      m=m2/(m1+m2)
      g=9.81 : rem Gravitational acceleration
      cls
      
      xmax%=1024 : ymax%=768
      rem Place Origin and draw Axes
      setx%=xmax% : sety%=int(1.5*ymax%)
      origin setx%,sety%
      
      P0=rad(Th1) : Q0=rad(Th2)
      Pd0=Thd1 : Qd0=Thd2
      endproc
      
      
      rem TITLE SEQUENCE
      def proc_title
      Font$="Algerian,"
      colour 4
      for w%=0 to 30
        Fs%=40-int(30*(w%/30))
        cls
        command$="FONT "+Font$+str$(Fs%)
        oscli command$
        print tab(10+w%,10);"The Double Pendulum"
        
        a$=inkey$(4)
      next w%
      for u%=1 to 7
        Fs%=10+6*u%
        cls
        command$="FONT "+Font$+str$(Fs%)
        oscli command$
        print tab(5,5);"The Double Pendulum"
        
        a$=inkey$(4)
      next u%
      
      a$=inkey$(20)
      *FONT Georgia,12
      endproc
      
      rem Energy Tracking Routine
      def fn_energy(p,q,pd,qd)
      local T,V
      
      V=(m1+m2)*g*L1*(1-cos(p)) + m2*g*L2*(1-cos(q))
      T=(m1*L1*L1*pd*pd + m2*(L1*L1*pd*pd +L2*L2*qd*qd +2*L1*L2*pd*qd*cos(q-p)))/2
      =T+V