十進BASIC 第2掲示板 過去ログ

LET cEps=1e-13 !精度

DIM t(2) !作業用

SET bitmap SIZE 600,600
SET WINDOW -5,5,-5,5
DRAW grid

LET iter=200 !繰り返し回数

LET M=3 !ｍ角形　※３以上の整数

DIM Pw(M,4) !壁面の端
!DATA  3,-4,  3, 4 !正方形　始点(x1,y1)、終点(x2,y2)
!DATA  3, 4, -3, 4
!DATA -3, 4, -3,-4
!DATA -3,-4,  3,-4
!MAT READ Pw
!MAT PRINT Pw;

LET R=4 !外接円の半径
FOR i=1 TO M !Ｘ軸上の点(R,0)から反時計まわりに頂点を得る
   LET th=2*PI*(i-1)/M !始点(x1,y1)
   LET Pw(i,1)=R*COS(th)
   LET Pw(i,2)=R*SIN(th)
   LET th=2*PI*i/M !終点(x2,y2)
   LET Pw(i,3)=R*COS(th)
   LET Pw(i,4)=R*SIN(th)
NEXT i
MAT PRINT Pw;

DIM w(M,2) !壁面の法線ベクトル
FOR i=1 TO M !線分の方向ベクトルから算出する
   LET t(1)=-(Pw(i,4)-Pw(i,2)) !Ｘ方向
   LET t(2)=  Pw(i,3)-Pw(i,1)  !Ｙ方向
   CALL Vec2Normalize(t,t) !正規化
   MAT PRINT t;
   LET w(i,1)=t(1)
   LET w(i,2)=t(2)
NEXT i

FOR i=1 TO M !壁面を表示する
   PLOT LINES: Pw(i,1),Pw(i,2); Pw(i,3),Pw(i,4)
NEXT i


!ボールの軌道　直線p(t)=Pa+t*a
DIM a(2) !ボールの移動方向ベクトル
DATA 1,2
MAT READ a
CALL Vec2Normalize(a,a) !|a|=1
MAT PRINT a;

DIM Pa(2) !ボールの発射位置
DATA 1,0
MAT READ Pa


DIM Pc(2) !衝突位置
DIM aa(2) !反射方向ベクトル
FOR k=1 TO iter !バウンドさせる
   CALL CalcCollision(Pc,aa)
   MAT PRINT Pc; !debug
   PLOT LINES: Pa(1),Pa(2); Pc(1),Pc(2) !軌跡を描く

   MAT Pa=Pc !次へ
   MAT a=aa
NEXT k


!衝突する平面の法線ベクトルをn、入射方向ベクトルをaとする。
!反射方向ベクトルbは、b=a-2*(a・n)*n
!
!また、平面上の任意の点をPs、入射方向ベクトルaの始点をPaとすると、
!衝突位置Pcは、Pc=Pa+{n・(Ps-Pa)/(a・n)}*a

SUB CalcReflection(a(),n(), b()) !反射ベクトルを計算する
   MAT t=(2*DOT(a,n))*n !ベクトルaをベクトルnに射影して、その２倍のベクトル
   MAT b=a-t
   MAT PRINT b; !debug
END SUB

SUB CalcCollision(Pc(),b()) !壁面との衝突位置と反射方向ベクトルを計算する
   DIM n(2),Ps(2),Pe(2)
   FOR i=1 TO M
      CALL Vec2Set(w(i,1),w(i,2), n) !法線ベクトル
      CALL Vec2set(Pw(i,1),Pw(i,2), Ps) !平面上の任意の点
      PRINT "壁面=";i !debug

      LET an=DOT(a,n) !ベクトルａとベクトルｎとのなす角を得る　|a||n|cosθ<0
      IF an<0 THEN !衝突する壁面の表裏判定
         MAT t=Ps-Pa
         LET nT=DOT(n,t)
         IF nT<0 THEN !壁面との位置関係から
            MAT t=(nT/an)*a
            MAT Pc=Pa+t !交点

            CALL Vec2Set(Pw(i,3),Pw(i,4), Pe) !直線の終点
            LET v=InRange(Pc, Ps,Pe)
            IF v>=0 AND v<=1 THEN !線分上なら
               MAT PRINT Pc; !debug

               CALL CalcReflection(a,n, b) !反射方向ベクトル

               EXIT SUB !１つ見つかれば
            ELSE
               PRINT "壁面の延長上で衝突する"
               MAT PRINT Pc; !debug
            END IF
         ELSEIF nT=0 THEN
            PRINT "衝突中"
            MAT PRINT Pa; !debug

            MAT Pc=Pa
            CALL CalcReflection(a,n, b) !反射方向ベクトル

            EXIT SUB !１つ見つかれば
         ELSE
            PRINT "衝突しない"
         END IF

      ELSE
         PRINT "衝突しない..."
         IF i=M THEN
            PRINT "すべての壁面と衝突しません。"
            STOP
         END IF
      END IF
   NEXT i
END SUB

!PsとPeを結ぶ線分を延長した直線上の点Pcと線分との位置関係
!0≦t≦1なら、線分上
FUNCTION InRange(Pc(), Ps(),Pe())
   DIM t1(2),t2(2)
   MAT t1=Pe-Ps
   MAT t2=Pc-Ps
   IF ABS(t1(1))>=cEps THEN !t1(1)<>0
      LET v=t2(1)/t1(1) !Ｘ方向の比
   ELSE !垂直線
      IF ABS(t1(2))>=cEps THEN !t1(2)<>0
         LET v=t2(2)/t1(2) !Ｙ方向の比
      ELSE !１点
         LET v=0
      END IF
   END IF
   LET InRange=v
END FUNCTION


END


!ベクトル
EXTERNAL FUNCTION Vec2Length(a()) !長さ |a|
LET Vec2Length=SQR(a(1)*a(1)+a(2)*a(2))
END FUNCTION

EXTERNAL SUB Vec2Normalize(a(),n()) !正規化 n=a/|a|
LET L=Vec2Length(a)
IF L>0 THEN MAT n=(1/L)*a
END SUB

!その他
EXTERNAL SUB Vec2Set(x,y, v())
LET v(1)=x
LET v(2)=y
END SUB