無駄な処理を直したいです。

[fides]

オイラー法を使ったプログラムを書いているのですが、初学者な為出力結果が遅かったりと無駄な処理が多いです。
より早く結果を表示する為にはどこを直すといいか、また下記の直したいと思っている所について解答お願いします。

このプログラムを書くにあたって参考にしたのは
「MATLAB/C++で学ぶ物理学のための数値法〈上〉常微分方程式・データ解析 」

です。
＜直したいところ＞
・本当はステップ数が500で割り切れる時毎にテキストファイルにその時の結果をファイルに書きたいのですが125行めのif文が機能していない。(ステップ数を大きくしてもプロット数は200点前後で収めたいので)
・128行目からのfor文は無駄な気がする(毎回０からファイルに書き込んでいる？)。しかし、for文を削除するとデバック時にエラーが出てしまう。。

すみませんがよろしくお願いします。
環境 　
OS ： Windows7 Professional
コンパイラ名 ： VC++ 2010 Ultimate

コード:

//qrotation.cpp
/*(3)式のクォータニオン表現からロールピッチヨー角を導出するプログラム*/
//q==a+bi+cj+dk==q[0]+q[1]i+q[2]j+q[3]kである。

#include <iostream>
#include <fstream>
using namespace std;

double PI = 3.14159265358979323846264338327950288;	//円周率の定義
double q[4];								//(3)式のクォータニオン表現の左辺
double qn[4];								//正規化用のクォータニオン
double q0[] = {1.0, 0.0, 0.0, 0.0};			//クォータニオンの初期値
double qdot[4];								//クォータニオン表現の右辺
double dt = 0.0001;							//ステップ
double OMEGA[] = {1.0, 0.0, 0.0};			//Ω
double roll;								//回転後のロール角
double pitch;								//回転後のピッチ角
double yaw;									//回転後のヨー角

/* 回転の変換行列 */
double r[16];								//クォータニオン変換行列

/* クォータニオンのよる自由軸回転 */
void qrot(double q[])
{
	double x2 = q[1] * q[1] * 2.0;
	double y2 = q[2] * q[2] * 2.0;
	double z2 = q[3] * q[3] * 2.0;
	double xy = q[1] * q[2] * 2.0;
	double yz = q[2] * q[3] * 2.0;
	double zx = q[3] * q[1] * 2.0;
	double xw = q[1] * q[0] * 2.0;
	double yw = q[2] * q[0] * 2.0;
	double zw = q[3] * q[0] * 2.0;
 
	r[0] = 1.0 - y2 - z2;
	r[1] = xy + zw;
	r[2] = zx - yw;
	r[3] = 0.0;
	r[4] = xy - zw; 
	r[5] = 1.0 - z2 - x2;
	r[6] = yz + xw;	
	r[7] = 0.0;
	r[8] = zx + yw;
	r[9] = yz - xw;
	r[10] = 1.0 - x2 - y2;
	r[11] = 0.0;
	r[12] = 0.0;
	r[13] = 0.0;
	r[14] = 0.0;
	r[15] = 1.0;
}

int main(int argc, char *argv[])
{
	
	q[0] = q0[0], q[1] = q0[1], q[2] = q0[2], q[3] = q0[3];	//初期値
	
	qdot[0] = (-q[1]*OMEGA[0] - q[2]*OMEGA[1] - q[3]*OMEGA[2] )/2;
	qdot[1] = ( q[0]*OMEGA[0] - q[3]*OMEGA[1] + q[2]*OMEGA[2] )/2;
	qdot[2] = ( q[3]*OMEGA[0] + q[0]*OMEGA[1] - q[1]*OMEGA[2] )/2;
	qdot[3] = (-q[2]*OMEGA[0] + q[1]*OMEGA[1] + q[0]*OMEGA[2] )/2;
	int maxStep = 10000;
	double *q0plot, *q1plot, *q2plot, *q3plot;
	q0plot = new double [maxStep];
	q1plot = new double [maxStep];
	q2plot = new double [maxStep];
	q3plot = new double [maxStep];
	double *rollplot, *pitchplot, *yawplot;
	rollplot  = new double [maxStep];
	pitchplot = new double [maxStep];
	yawplot   = new double [maxStep];
	//ファイルに出力
	ofstream q0plotOut("q0plot.txt"),
			 q1plotOut("q1plot.txt"),
			 q2plotOut("q2plot.txt"), 
			 q3plotOut("q3plot.txt"),
			 rollplotOut("rollplot.txt"),
			 pitchplotOut("pitchplot.txt"),
			 yawplotOut("yawplot.txt");

	for(int iStep=0; iStep<=maxStep; iStep++)
	{
		q0plot[iStep] = q[0];							//プロット用の軌道を記録
		q1plot[iStep] = q[1];
		q2plot[iStep] = q[2];
		q3plot[iStep] = q[3];

		q[0] = q[0] + qdot[0]*dt;
		q[1] = q[1] + qdot[1]*dt;
		q[2] = q[2] + qdot[2]*dt;
		q[3] = q[3] + qdot[3]*dt;
		
		//クォータニオンから回転行列を求める
		qrot(q);
		
		/* クォータニオン回転行列からロールピッチヨー角を求める */
		if((-PI/2)<=pitch && pitch<=(PI/2))
		{
			roll  = atan2( r[9], r[10] );
			pitch = atan2( -r[8], sqrt(r[9]*r[9]+r[10]*r[10]) );
			yaw   = atan2( r[4], r[0] );
		}
		//ピッチ角が(PI/2)<=pitch<=(3*PI/2)
		if((PI/2)<=pitch && pitch<=(3*PI/2))
		{
			roll  = atan2( -r[9], -r[10] );
			pitch = atan2( -r[8], -sqrt(r[9]*r[9]+r[10]*r[10]) );
			yaw   = atan2( -r[4], -r[0] );
		}

		//クォータニオンを正規化する
		qn[0] = q[0] /( sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) );
		qn[1] = q[1] /( sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) );
		qn[2] = q[2] /( sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) );
		qn[3] = q[3] /( sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) );
		//正規化したクォータニオンをもとの配列q[]に戻す
		q[0] = qn[0];
		q[1] = qn[1];
		q[2] = qn[2];
		q[3] = qn[3];
		
		
		 
		int i;
		if(iStep%500==0)
		{
			for( i=0; i<= iStep; i++ )
			{
				q0plotOut << q0plot[i] << endl;
				q1plotOut << q1plot[i] << endl;
				q2plotOut << q2plot[i] << endl;
				q3plotOut << q3plot[i] << endl;
				rollplotOut << rollplot[i] <<endl;
				pitchplotOut << pitchplot[i] << endl;
				yawplotOut << yawplot[i] << endl;
			}
			cout << iStep  << endl;
		}

	}
}

[non]

＜直したいところ＞
・本当はステップ数が500で割り切れる時毎にテキストファイルにその時の結果をファイルに書きたいのですが125行めのif文が機能していない。(ステップ数を大きくしてもプロット数は200点前後で収めたいので)
・128行目からのfor文は無駄な気がする(毎回０からファイルに書き込んでいる？)。しかし、for文を削除するとデバック時にエラーが出てしまう。。

何をやりたいのかもう少し詳しく説明してください。
現在10000個の配列にデータを求め、500個おきに、配列を０～その数までをファイルに格納しているわですよね。
それで、500おきにどうしたいのですか？

[beatle]

最適化はとにかく計測ありきです．計測というのはプログラムの実行時間を測ることです．
Boost.Timerを使うと，簡単にソースコードの一部分の実行時間を計測できますから，これから最適化しようと思う部分の時間を測るように改造しましょう．
時間計測が出来るようになったら最適化に手をつけてOKです．
時間を測らずに，「ここを直したら速くなると思う」という，勘だよりの最適化は良くありません．

[non]

もしかしたら、500おきに出力したいということなのでしょうか？
10000÷500=200　ですからね。
それなら、

コード:

		if(iStep%500==0)
		{
			q0plotOut << q0plot[iStep] << endl;
			q1plotOut << q1plot[iStep] << endl;
			q2plotOut << q2plot[iStep] << endl;
			q3plotOut << q3plot[iStep] << endl;
			rollplotOut << rollplot[iStep] <<endl;
			pitchplotOut << pitchplot[iStep] << endl;
			yawplotOut << yawplot[iStep] << endl;
			cout << iStep  << endl;
		}

ですね。

for(int iStep=0; iStep<=maxStep; iStep++)
で、maxStepまで求めるのは配列の範囲を超えてませんか？

なお、プログラムがあっているかどうかは見ていません。

[fides]

お二方回答ありがとうございます。

何をやりたいのかもう少し詳しく説明してください。
現在10000個の配列にデータを求め、500個おきに、配列を０～その数までをファイルに格納しているわですよね。
それで、500おきにどうしたいのですか？

このプログラムは剛体回転のシミュレーションで、59-62行目の

コード:

qdot[0] = (-q[1]*OMEGA[0] - q[2]*OMEGA[1] - q[3]*OMEGA[2] )/2;
    qdot[1] = ( q[0]*OMEGA[0] - q[3]*OMEGA[1] + q[2]*OMEGA[2] )/2;
    qdot[2] = ( q[3]*OMEGA[0] + q[0]*OMEGA[1] - q[1]*OMEGA[2] )/2;
    qdot[3] = (-q[2]*OMEGA[0] + q[1]*OMEGA[1] + q[0]*OMEGA[2] )/2; 

の4つの1階状微分方程式をオイラー法を用いて数値解析をして、その値を使って
ロール角、ピッチ角、ヨー角をステップ毎に計算させるものです。
No.4の部分、iを直し忘れてたみたいです。
直したら無事動かすことが出来ました！

for(int iStep=0; iStep<=maxStep; iStep++)
で、maxStepまで求めるのは配列の範囲を超えてませんか？

配列の範囲というのはq0plotとかの事ですか・・？
特にエラーは出なかったのですがこうした方がいいのでしょうか？

コード:

q0plot = new double [maxStep+1];
	q1plot = new double [maxStep+1];
	q2plot = new double [maxStep+1];
	q3plot = new double [maxStep+1];
	double *rollplot, *pitchplot, *yawplot;
	rollplot  = new double [maxStep+1];
	pitchplot = new double [maxStep+1];
	yawplot   = new double [maxStep+1];

最適化はとにかく計測ありきです．計測というのはプログラムの実行時間を測ることです．
Boost.Timerを使うと，簡単にソースコードの一部分の実行時間を計測できますから，これから最適化しようと思う部分の時間を測るように改造しましょう．
時間計測が出来るようになったら最適化に手をつけてOKです．
時間を測らずに，「ここを直したら速くなると思う」という，勘だよりの最適化は良くありません．

初めて知りました。ありがとうございます。
時間がかかりましたが一応導入できました。
10000ステップだと1秒くらいになりました。(改良前は30秒くらい？)
これはまだ試作段階で、ステップ数はもっと増やしていきたいと思ってます。
使用するメモリが少なくなればさらに早くなるような気もするのですが他に改良点あればご教示願います。

[non]

それ（添付したプログラム）で、あってたみたいですね。
配列の確保もそれならOKです。

[fides]

解決しました。
ありがとうございました！