TI-Nspire & Lua / 常微分方程式の数値解法 / 応用例 / 2 重振子 / 古典的 Runge-Kutta 法

参考: 工業力学 (機械工学基礎講座), pp.229-231

DoublePend = class()
function DoublePend:init(funcs, t0, inits, h, numOfDiv, len1, radius1, len2, radius2)
   self.len1    = len1
   self.radius1 = radius1

   self.len2    = len2
   self.radius2 = radius2
   
   self.historicData = {{},{}}
  
   self.Unpack = unpack or table.unpack
   self.Sin    = math.sin
   self.Cos    = math.cos

   self.funcs = funcs
   self.t0    = t0
   self.inits = inits

   self.dim      = #funcs
   self.numOfDiv = numOfDiv or 1
   self.h        = h / self.numOfDiv

   self.f    = {{}, {}, {}, {}}
   self.temp = {{}, {}, {}}
   
   self.storeData = function(x, y, list, numOfPairs)
                       list[#list+1] = x
                       list[#list+1] = y
                       if #list > numOfPairs * 2 then
                          table.remove(list, 1)
                          table.remove(list, 1)
                       end
                    end
end
function DoublePend:updateRK()
   for i = 1, self.numOfDiv do   
      for j = 1, self.dim do self.f[1][j] = self.funcs[j](self.t0           , self.Unpack(self.inits))   ; self.temp[1][j] = self.inits[j] + self.h/2 * self.f[1][j] end
      for j = 1, self.dim do self.f[2][j] = self.funcs[j](self.t0 + self.h/2, self.Unpack(self.temp[1])) ; self.temp[2][j] = self.inits[j] + self.h/2 * self.f[2][j] end
      for j = 1, self.dim do self.f[3][j] = self.funcs[j](self.t0 + self.h/2, self.Unpack(self.temp[2])) ; self.temp[3][j] = self.inits[j] + self.h   * self.f[3][j] end
      for j = 1, self.dim do self.f[4][j] = self.funcs[j](self.t0 + self.h  , self.Unpack(self.temp[3]))                                                             end

      for j = 1, self.dim do self.inits[j] = self.inits[j] + self.h/6 * (self.f[1][j] + 2 * (self.f[2][j] + self.f[3][j]) + self.f[4][j]) end 

      self.t0 = self.t0 + self.h
   end
   return self
end
function DoublePend:print()
   print(self.t0, "{"..table.concat(self.inits, ", ").."}")
   return self
end
function DoublePend:drawTime(gc, x, y, fontSize, color)
   local x, y     = x or 10, y or 10
   local fontSize = fontSize or 10 
   gc:setColorRGB(color or 0x000000)
   gc:setFont("sansserif", "r", fontSize or 10)
   gc:drawString(string.format("t = %.1f", self.t0), x, y)
   
   return self
end
function DoublePend:drawPend(gc, cx, cy, unit, color)
   local len1OnScr    = unit * self.len1
   local radius1OnScr = unit * self.radius1
   local dia1OnScr    = 2 * radius1OnScr

   local len2OnScr    = unit * self.len2
   local radius2OnScr = unit * self.radius2
   local dia2OnScr    = 2 * radius2OnScr
   
   self.sx    = {}
   self.sy    = {}
   self.sx[1] = cx         + len1OnScr * self.Sin(self.inits[1])
   self.sy[1] = cy         + len1OnScr * self.Cos(self.inits[1])
   self.sx[2] = self.sx[1] + len2OnScr * self.Sin(self.inits[3])
   self.sy[2] = self.sy[1] + len2OnScr * self.Cos(self.inits[3])

   gc:setColorRGB(color or 0x000000)
   
   gc:drawArc(self.sx[1] - radius1OnScr, self.sy[1] - radius1OnScr, dia1OnScr, dia1OnScr, 0, 360)
   gc:drawLine(cx, cy, self.sx[1], self.sy[1])
   
   gc:drawArc(self.sx[2] - radius2OnScr, self.sy[2] - radius2OnScr, dia2OnScr, dia2OnScr, 0, 360)
   gc:drawLine(self.sx[1], self.sy[1], self.sx[2], self.sy[2])
   
   return self
end
function DoublePend:drawLocus(gc, which, numOfPairs, color)
   self.storeData(self.sx[which], self.sy[which], self.historicData[which], numOfPairs)
   gc:setColorRGB(color or 0x000000)
   gc:drawPolyLine(self.historicData[which])
   
   return self
end



--- 確かめる。
do
local isTicking = false

local W, H, CX, CY, UNIT, FONT_SIZE
function on.resize()
   W, H      = platform.window:width(), platform.window:height()
   CX, CY    = W/2, H/2
   UNIT      = H/4.5
   FONT_SIZE = H/16
end

-- この聯立微分方程式を解く。
local l1, l2             = 1  , 1
local m1, m2             = 1  , 1
local radius1, radius2   = 0.2, 0.2
local rad1init, rad2init = 3  , 3
local vel1init, vel2init = 0  , 0
local Sin = math.sin  
local Cos = math.cos
local function rad1Dot(t, rad1, vel1, rad2, vel2) return vel1 end
local function vel1Dot(t, rad1, vel1, rad2, vel2) return -(9.80665*(m1*Sin(rad1)-m2*(Cos(rad1-rad2)*Sin(rad2)-Sin(rad1)))+(l1*vel1^2*Cos(rad1-rad2)+l2*vel2^2)*m2*Sin(rad1-rad2))/(l1*(m1+m2*(Sin(rad1-rad2))^2)) end
local function rad2Dot(t, rad1, vel1, rad2, vel2) return vel2 end
local function vel2Dot(t, rad1, vel1, rad2, vel2) return (9.80665*(m1+m2)*(Sin(rad1)*Cos(rad1-rad2)-Sin(rad2))+(l1*(m1+m2)*vel1^2+l2*m2*vel2^2*Cos(rad1-rad2))*Sin(rad1-rad2))/(l2*(m1+m2*(Sin(rad1-rad2))^2)) end


-- 1 個だけインスタンス化する。
local pend
local function reset()
         local funcs = {rad1Dot , vel1Dot , rad2Dot , vel2Dot }
         local inits = {rad1init, vel1init, rad2init, vel2init}
         pend = DoublePend(funcs, 0, inits, 1/64, _, l1, radius1, l2, radius2) -- (函数リスト, t0, 初期値リスト, 刻み幅 [, 内部分割数], 糸1の長さ, 分銅1の半径, 糸2の長さ, 分銅2の半径)
      end


function on.construction()
   reset()
end
function on.enterKey()
   if     isTicking == false then timer.start(pend.h) ; isTicking = true
   elseif isTicking == true  then timer.stop()        ; isTicking = false
   end
end
function on.escapeKey()
   reset()
end
function on.timer()
   pend:updateRK()
       --:print()
   platform.window:invalidate()
end
function on.paint(gc)
   pend:drawPend(gc, CX, CY, UNIT, _)
       :drawTime(gc, 10, 0, FONT_SIZE, _)
       --:drawLocus(gc, 1, 100, 0x555555) -- 質点 1 のスクリーン座標を過去 100 ペアぶん描く。
       :drawLocus(gc, 2, 100, 0xaaaaaa)
end

end
広告を非表示にする