ボールの衝突をFunctional Reactive Programmingで表現する（１１）

１１．プログラム

プログラムが完成したので、まず、動いているところから、観察してみよう。最初の動画は、大きさも質量も同じ二つのボールがビリヤード台の上を、物理法則に従って、動作する様子を示したものである。

次の動画は、大きさの比が1:2で、質量の比が1:8の二つのボールの動作を示したものである。

また、シミュレーション速度を高速にすると、ボールの動きが躍動的になる。

プログラムの全ては、以下にアップロードしてあるので、コピーして利用して欲しい。github.com

ここでは、衝突のプログラムだけ説明する。このプログラムは以下のようになっている。

{-# LANGUAGE Arrows #-}
module Collision (collision) where

import Prelude hiding ((.),)
import Control.Wire
import Control.Monad.IO.Class()
import FRP.Netwire()
import Linear.V2
import Linear.Matrix
import Configure


collision :: (HasTime t s) => Wire s () IO (Ball, Ball) NextS
collision = proc (b1@(Ball _ _ r1 (V2 vx1 vy1) (V2 x1 y1) (V2 dt1 dt1')), b2@(Ball _ _ r2 (V2 vx2 vy2) (V2 x2 y2) (V2 dt2 dt2'))) -> do
    let nx1 = x1 + vx1 * dt1
        nx2 = x2 + vx2 * dt2
        ny1 = y1 + vy1 * dt1'
        ny2 = y2 + vy2 * dt2'
        v1  = nx1 - r1 < (- wallV) || nx1 + r1 > wallV
        v2  = nx2 - r2 < (- wallV) || nx2 + r2 > wallV
        h1  = ny1 - r1 < (- wallH) || ny1 + r1 > wallH
        h2  = ny2 - r2 < (- wallH) || ny2 + r2 > wallH
        bC = collision1 b1 b2
    returnA -< NextS bC b1 b2 (WallC v1 v2 h1 h2)

collision1 :: Ball -> Ball -> BallC
collision1 (Ball n1 m1 r1 v1@(V2 vx1 vy1) p1@(V2 px1 py1) step1@(V2 dt1 dt1')) (Ball n2 m2 r2 v2@(V2 vx2 vy2) p2@(V2 px2 py2) step2@(V2 dt2 dt2')) = 
  if dx*dx + dy*dy > r*r then BallC False n1 v1 nxt1 n2 v2 nxt2 else BallC True n1 v1' p1' n2 v2' p2'
  where
      m          = m1 + m2
      invm       = 1 / m
      r          = r1 + r2
      invr       = 1 / r
-- Position and velocity of the gravity center
      p          = (V2 (V2 px1 px2) (V2 py1 py2) !* V2 m1 m2) * V2 invm invm
      v          = (V2 (V2 vx1 vx2) (V2 vy1 vy2) !* V2 m1 m2) * V2 invm invm
-- Transportation matrix for the rotation coordinate system
      V2 dvx dvy = v1 - v2
      dv         = sqrt $ dvx * dvx + dvy * dvy
      V2 cost sint = if dvy > 0 then V2 (dvx / dv) (dvy / dv) else V2 (-dvx / dv) (-dvy / dv)
      l          = abs $ cost * (py1 - py2) - sint * (px1 - px2)
      lr         = l * invr
      sqlr       = sqrt (r * r - l * l) * invr

      nxt@(V2 nxtx nxty) = nxt1 - nxtp
      cur@(V2 curx cury) = p1 - p
      V2 cosp sinp 
        | curx*nxty - nxtx*cury < 0 = V2 (lr * cost - sqlr * sint)  (sqlr * cost + lr * sint)    -- clockwise
        | otherwise                 = V2 (lr * cost + sqlr * sint)  (- sqlr * cost + lr * sint)

      mat        = V2 (V2 cosp sinp) (V2 (-sinp) cosp)

      V2 a1 b1   = mat !* (p1 - p)
      V2 a2 b2   = mat !* (p2 - p)
      V2 ua1 ub1 = mat !* (v1 - v)
      V2 ua2 ub2 = mat !* (v2 - v)
-- Position and velocity immediately after the collision on the rotation coordinate

      d          = (- m1 * r1 + m2 * r2) / m
      (s1',s2')
        | b1 - b2 > 0 = (V2 (a1 + dt1 * ua1) ( 2*(d + r1) - (b1 + dt1' * ub1)), V2 (a2 + dt2 * ua2) ( 2*(d - r2) - (b2 + dt2' * ub2)))
        | otherwise   = (V2 (a1 + dt1 * ua1) (-2*(d + r1) - (b1 + dt1' * ub1)), V2 (a2 + dt2 * ua2) (-2*(d - r2) - (b2 + dt2' * ub2)))
      (u1',u2')     = (V2 ua1 (-ub1), V2 ua2 (-ub2))

-- Transform to the billiard coodinate system
      invmat     = V2 (V2 cosp (-sinp)) (V2 sinp cosp)
      p1'        = invmat !* s1' + p
      p2'        = invmat !* s2' + p
      v1'        = invmat !* u1' + v
      v2'        = invmat !* u2' + v
-- Difference between two balls at next step      
      nxt1@(V2 nxtx1 nxty1) = p1 + v1 * step1
      nxt2@(V2 nxtx2 nxty2) = p2 + v2 * step2
      V2 dx dy              = nxt1 - nxt2
      nxtp                  = (V2 (V2 nxtx1 nxtx2) (V2 nxty1 nxty2) !* V2 m1 m2) * V2 invm invm

このプログラムで、collisionの関数が、ボール同士の衝突と上下左右の壁との接触を検出する。壁との衝突では、次のセッションまでに生じるかどうかを真理値で返す。ボール同士の衝突では、起きるかどうかをやはり真理値で返すとともに、さらに、次のセッションでのそれぞれのボールの位置と速度も返す。

関数collision1は、二つのボールが次のセッションに移るまでに衝突するかどうかを判定する。衝突の判定は、衝突が起きなかったものとして、次のセッションでボールがどこまで移動したかをえて、二つのボールの中心間の距離が、それらの半径の和よりも小さくなるときに衝突が起きると見なす。

衝突が起きる場合には、次のセッションでの位置と速度が変わるので、前の記事で書いた原理を利用して、正しい位置と速度を計算し、それを返す。

プログラムの中で、pとvが重心の座標と速度である。また、costとsintは、前の記事での\({\rm cos} \theta\)と\({\rm sin} \theta\)である。lrとsqlrが\(\frac{l}{q}\)と\(\frac{\sqrt{r^2-l^2}}{q}\)である。

また、curx*nxty - nxtx*cury によって、重心座標系でのそれぞれの速度が重心に対して時計回りであるかどうか判定している(これは次のセッションでの位置が、現在の位置に対して、重心から見たときに、時計回りであるかどうかで判定する。すなわち、重心から現在の位置までのベクトルと、重心から次のセッションでの位置までのベクトルを求め、この二つのベクトルのベクトル積をとって判定する）。もし、この値が負の時は、時計回りである。cospとsinpは、\({\rm cos} \varphi\)と\({\rm sin} \varphi\)である。

なお、二つのボールが真正面から衝突する場合について、前の記事で紹介したが、これらの判定は、これまでの判定の中に含まれている。

ボール1の現在の位置p1と速度v1は、衝突がなかったとすると、次のセッションでは回転座標系では(a1 b1) (ua1 ub1)になる。ボール2についても同じである。実際には衝突が起きているので、これをただしたのが、ボール1についてはs1'とu1'である。回転座標系のものをビリヤード座標系に直したのがp1',v1'である。ボール2についても同じである。

これで、衝突したときの次のセッションでの位置と速度が得られ、シミュレーションは次のセッションへと移ることができる。