直列多重振り子シミュレーション
http://www.aihara.co.jp/~taiji/pendula-equations/present.html さんリスペクトです。
dfferetial time は0.2~0.4推奨です。
逆行列も本当はLU分解とかして求めた方が良いですよね…もっと勉強します…
/**
* Copyright Nos_lkSsvOhB ( http://wonderfl.net/user/Nos_lkSsvOhB )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/cWv7
*/
package {
import flash.display.*;
import flash.events.Event;
import com.bit101.components.*;
import flash.text.TextField;
public class Oscillator extends Sprite {
private var stage_x:int=200;
private var stage_y:int=80;
private var i:Number;
private var j:Number;
private var n:int=2;
private var nl:Number=50;
private var r:Number=10;
private var mass:Number=1;
private var g:Number=9.8;
private var dt:Number=0.4;
private var atten:Number=100;
private var m:Array=new Array();
private var summ:Array=new Array();
private var l:Array=new Array();
private var th0:Array=new Array();
private var th1:Array=new Array();
private var dth0:Array=new Array();
private var dth1:Array=new Array();
private var A:Array=new Array();
private var B:Array=new Array();
private var C:Array=new Array();
private var D:Array=new Array();
private var bar:Array=new Array();
private var b:Array=new Array();
private var pole:Sprite=new Sprite();
private var sli_n:HUISlider;
private var sli_dt:HUISlider;
private var sli_att:HUISlider;
private var sli_m:HUISlider;
//コンストラクタ
public function Oscillator() {
sli_n = new HUISlider(this, 10, 10, "Number", onChange);
sli_n.value=n;
sli_n.maximum=8;
sli_n.minimum=2;
sli_dt = new HUISlider(this, 10, 30, "differential time", onChange);
sli_dt.value=dt;
sli_dt.maximum=0.75;
sli_dt.minimum=0.01;
sli_att = new HUISlider(this, 200, 10, "attenuation", onChange);
sli_att.value=atten;
sli_att.maximum=1000;
sli_att.minimum=10;
sli_m = new HUISlider(this, 200, 30, "mass", onChange);
sli_m.value=mass;
sli_m.maximum=1;
sli_m.minimum=0.1;
init(n);
}
//初期化
private function init(nm:Number):void {
//ボールの数
n=nm;
//スプライトの深度用カウンタ
var c:int=0;
for(i=0;i<n;i++,c++){
if(i==1)
c++;
//i番目の質量
m[i]=mass;
//i番目の棒の長さ
l[i]=nl;
//棒の描画
bar[i] = new Sprite();
bar[i].graphics.lineStyle(2,0x000000,1,false);
bar[i].graphics.moveTo(-l[i]/2,0);
bar[i].graphics.lineTo(l[i]/2,0);
addChildAt(bar[i],c);
if(i==0){
//支点の描画
pole.graphics.beginFill(0xFF0000);
pole.graphics.drawCircle(0,0,r/2);
pole.graphics.endFill();
addChildAt(pole,1);
pole.x=stage_x;
pole.y=stage_y;
}
//ボール描画
b[i] = new Sprite();
b[i].graphics.beginFill(0xFFFFFF*Math.random());
b[i].graphics.drawCircle(0,0,r);
b[i].graphics.endFill();
addChildAt(b[i],i+n+1);
//初期角度をランダムにしてみたら面白いかな～って感じで
var thran:Number=Math.random()*Math.PI*2;
th0[i]=thran;
th1[i]=thran;
dth0[i]=0;
dth1[i]=0;
b[i].x=stage_x;
b[i].y=stage_y;
//行列操作用2次元配列
A[i]=new Array();
C[i]=new Array();
}
//各スプライトの位置
for(i=0;i<n;i++){
for(j=0;j<=i;j++){
b[i].x+=l[j]*Math.cos(th1[j]);
b[i].y+=l[j]*Math.sin(th1[j]);
}
bar[i].x=b[i].x-l[i]*Math.cos(th1[i])/2;
bar[i].y=b[i].y-l[i]*Math.sin(th1[i])/2;
//単に角度計算すると壊れる現象があるため
if(i==0)
bar[i].rotation=Math.atan2(b[i].y - stage_y, b[i].x - stage_x)*180/Math.PI;
else
bar[i].rotation=Math.atan2(b[i].y - b[i-1].y, b[i].x - b[i-1].x)*180/Math.PI;
//iからnまでの質量の合計を入れる配列の初期化
summ[i]=0;
for(j=i;j<n;j++){
summ[i]+=m[j];
}
}
//EnterFrameのイベントリスナ
addEventListener(Event.ENTER_FRAME,onEnter);
}
private function onEnter(event:Event):void {
for(i=0;i<n;i++){
B[i]=0;
for(j=0;j<n;j++){
//A[i][j]の計算
if(j==i)
A[i][j]=summ[i]*Math.pow(l[i],2);
else if(j<i)
A[i][j]=summ[i]*l[j]*l[i]*Math.cos(th0[j]-th0[i]);
else if(j>i)
A[i][j]=summ[j]*l[j]*l[i]*Math.cos(th0[j]-th0[i]);
//B[i]の計算
if(j==i) continue;
else if(j<i)
B[i]+=summ[i]*l[j]*l[i]*Math.pow(dth0[j],2)*Math.sin(th0[j]-th0[i]);
else if(j>i)
B[i]+=summ[j]*l[j]*l[i]*Math.pow(dth0[j],2)*Math.sin(th0[j]-th0[i]);
}
B[i]+=summ[i]*g*l[i]*Math.cos(th0[i])-atten*dth0[i];
}
//Aの逆行列をCに入れる
inv_mat(A,C);
//行列CとベクトルBの掛け算の結果をベクトルDに入れる
D=mat_mul(C,B);
for(i=0;i<n;i++){
//微分方程式を有限差分法で解く
dth1[i]=dth0[i]+D[i]*dt;
th1[i]=th0[i]+dth1[i]*dt;
}
//各スプライトを新しい位置に
for(i=0;i<n;i++){
b[i].x=stage_x;
b[i].y=stage_y;
for(j=0;j<=i;j++){
b[i].x+=l[j]*Math.cos(th1[j]);
b[i].y+=l[j]*Math.sin(th1[j]);
}
bar[i].x=b[i].x-l[i]*Math.cos(th1[i])/2;
bar[i].y=b[i].y-l[i]*Math.sin(th1[i])/2;
//単に角度計算すると壊れる現象があるため
if(i==0)
bar[i].rotation=Math.atan2(b[i].y - stage_y, b[i].x - stage_x)*180/Math.PI;
else
bar[i].rotation=Math.atan2(b[i].y - b[i-1].y, b[i].x - b[i-1].x)*180/Math.PI;
}
//漸化式（微小時間後に代入）
for(i=0;i<n;i++){
th0[i]=th1[i];
dth0[i]=dth1[i];
}
}
//スライドバーが変更されたときのメソッド
private function onChange(event:Event):void {
var _n:int=int(sli_n.value);
var _dt:Number=sli_dt.value;
var _att:Number=sli_att.value;
var _mass:Number=sli_m.value;
for(i=0;i<n;i++){
b[i].graphics.clear();
bar[i].graphics.clear();
A[i].splice(_n,n);
}
//配列の切り詰め
if(_n<n){
A.splice(_n,n);
B.splice(_n,n);
C.splice(_n,n);
D.splice(_n,n);
}
dt=_dt;
atten=_att;
mass=_mass;
init(_n);
}
private function inv_mat(a:Array,c:Array):void{
var d:Number=0;
var n:int=a.length,i:int,j:int;
for(i=0;i<n;i++){
for(j=0;j<n;j++){
c[j][i]=Math.pow(-1,i+j)*det(s_mat(a,i,j));
if(i==0)
d+=a[i][j]*c[j][i];
}
}
for(i=0;i<n;i++)
for(j=0;j<n;j++)
c[i][j]=c[i][j]/d;
}
private function det(a:Array):Number{
var n:int=a.length;
var d:Number=0;
if(n==3) return a[0][0]*a[1][1]*a[2][2]+a[0][1]*a[1][2]*a[2][0]+a[0][2]*a[1][0]*a[2][1]-a[0][2]*a[1][1]*a[2][0]-a[0][1]*a[1][0]*a[2][2]-a[0][0]*a[1][2]*a[2][1];
else if(n==2) return a[0][0]*a[1][1]-a[0][1]*a[1][0];
else if(n==1) return a[0];
for(var j:int=0;j<n;j++){
d+=Math.pow(-1,j)*a[0][j]*det(s_mat(a,0,j));
}
return d;
}
private function s_mat(a:Array,p:int,q:int):Array{
var n:int=a.length;
var k:int=0,l:int=0;
var c:Array=new Array();
for(var i:int=0;i<n;i++){
if(i!=p){
c[k]=new Array();
for(var j:int=0;j<n;j++){
if(j!=q){
var temp:Number=a[i][j];
c[k][l]=temp;
l++;
}
}
k++; l=0;
}
}
return c;
}
private function mat_mul(a:Array,b:Array):Array{
var n:int=a.length;
var c:Array=new Array();
for(var i:int=0;i<n;i++){
c[i]=0;
for(var k:int=0;k<n;k++){
c[i]+=a[i][k]*b[k];
}
}
return c;
}
}
}