直列多重振り子シミュレーション(改良版)
http://www.aihara.co.jp/~taiji/pendula-equations/present.html さんリスペクトです。
逆行列計算をLU分解にしました。コードはhttp://code.google.com/p/face-recognition-library-as3/source/browse/trunk/FaceRecognitionLib/src/com/karthik/math/lingalg/LUDecomposition.as?r=3を参考にさせて頂きました。
数が多くなっても軽い。
/**
* 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/5B5a
*/
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=5;
private var nl:Number=30;
private var r:Number=5;
private var mass:Number=1;
private var g:Number=9.8;
private var dt:Number=0.05;
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=80;
sli_n.minimum=2;
sli_n.labelPrecision = 0;
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=Math.round(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(); // inv_mat内でまたnewするため
}
//各スプライトの位置
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=Math.round(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 LU : Array = a;
var n : int = a.length;
var m : int = n;
var i : int = 0;
var k : int = 0;
var piv : Array = new Array(n);
for (i = 0; i < m; i++) {
piv[i] = i;
}
var pivsign : int = 1;
var LUrowi : Array;
var LUcolj : Array = new Array(n);
for (var j : int = 0; j < n; j++) {
for (i = 0; i < m; i++) {
LUcolj[i] = LU[i][j];
}
for (i = 0; i < m; i++) {
LUrowi = LU[i];
var kmax : int = Math.min(i, j);
var s : Number = 0.0;
for (k = 0; k < kmax; k++) {
s += LUrowi[k] * LUcolj[k];
}
LUrowi[j] = LUcolj[i] -= s;
}
var p : int = j;
for (i = j + 1; i < m; i++) {
if (Math.abs(LUcolj[i]) > Math.abs(LUcolj[p])) {
p = i;
}
}
if (p != j) {
for (k = 0; k < n; k++) {
var t : Number = LU[p][k]; LU[p][k] = LU[j][k]; LU[j][k] = t;
}
k = piv[p]; piv[p] = piv[j]; piv[j] = k;
pivsign = -pivsign;
}
if (j < m && LU[j][j] != 0.0) {
for (i = j + 1; i < m; i++) {
LU[i][j] /= LU[j][j];
}
}
}
var nx : int = n;
var X : Array = new Array(n);
for (i = 0; i < n; i++){
X[i] = new Array();
for (j = 0; j < n; j++){
if (i == j) X[i][j] = 1;
else X[i][j] = 0;
}
}
for (i = 0; i < n; i++)
{
c[i] = new Array(n);
for (j = 0; j < n; j++)
{
c[i][j] = X[piv[i]][j];
}
}
for (k = 0; k < n; k++) {
for (i = k + 1; i < n; i++) {
for (j = 0; j < nx; j++) {
c[i][j] -= c[k][j] * LU[i][k];
}
}
}
for (k = n - 1; k >= 0; k--) {
for (j = 0; j < nx; j++) {
c[k][j] /= LU[k][k];
}
for (i = 0; i < k; i++) {
for (j = 0; j < nx; j++) {
c[i][j] -= c[k][j] * LU[i][k];
}
}
}
}
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;
}
}
}