(物理)3D振り子を実装したい

[Dim!]

3D空間上で振り子を実装したいと考えています。
最初に初期値として支点(定位置)と重りの位置を与え、重りから手を離したと仮定した状態から振り子運動が始まります。
糸の長さは初期値が与えられた時に自動で計算され、縮むことも伸びることもありません。
糸と言うよりは棒のようなものとして考えて、曲がったり湾曲したりしません。
上から見て、円形や楕円形の軌道は描きません。
また、支点の位置よりも重りの位置が上になることはありません。(重りの位置の方が上になった時の処理は後から入れる予定です)

このような前提のもとで、こちらのサイト様を参考に3D空間上での2D振り子(Z軸方向には揺れない振り子)までは実装することが出来ました。
http://hakuhin.jp/as/move.html#MOVE_06
Z成分を0にした3Dベクトルを使って実装しました。

そこでZ軸への揺れも考慮した3D振り子の実装に手をつけたのですが、思ったような挙動をしません。
XY面に回転してから処理して回転しただけ戻す方法を最初に試したのですが、処理がスマートではありません(挙動も荒ぶりました)。
強引に2D振り子の処理を3D仕様にしようとしたのですが、X軸とZ軸が絡んだ回転の処理につまづいてしまいました。

単振り子や二重振り子といった資料はたくさんあるのですが、3D空間での振り子の資料は少なく非常に苦難しております。
3D振り子を実装するのに、もっとスマートな考え方はないものでしょうか。
その考え方と方法を是非ご教授願いたく、質問しました。
当方、C#とXNA3.1を使って開発しています。

(一部誤植訂正しました。もうしわけありません)
以上、よろしくお願いします。

[GRAM]

説明するのが難しいですが、物理的に考えればいいと思います
たるんだりする方が自分にとっては都合がいいので（というか昔作ったのがそうなっているので）、そう考えますね。

まず、速度ベクトルが紐が作る球の接線方向に近い場合、
これは次の微小時間も球の接線方向に運動するように運動を修正します。
修正方法としては、速度ベクトルを球の接線方向と、球の中心方向に分解し、
中心方向の成分を０に、接線方向の速度を現在の速度と同じになるように調整します。

次に速度ベクトルが、球の接線方向から大きく外れている場合、
この場合は、中心方向の速度ベクトルに反発係数をかけ、さらに反転させてやればいい感じになると思います。

接線方向から外れているかどうかは、そこそこ正確に判断できますが、簡単にするには
球との衝突時間を計算し、衝突点で速度を分解して中心方向の成分を見てやればいいと思います。

・・・とまぁこんな感じになるでしょうが、本質的なロジックは２Dでも3Dでも何も変わらないかなぁと思います。
一応昔作ったプログラムを一部修正し、コアとなるコードを載せてみます・・・が、
実はこのコードも２Dでも３Dでも両方で動作します。

► スポイラーを表示

コード:

//全部コードを載せると長くなるので要点だけ抑えます
/*
	Entityクラス　位置、質量、速度をパラメータに持つクラス
	AddForce関数（引数は力の大きさ/力を加える位置）でEntityに力を加える。
	Entityは毎フレーム運動方程式から加速度を求め、さらにその加速度から差分法で速度を求めながら運動する。
	つまりEntityの運動に関与するすべてのクラスは、基本的に（位置の強制修正を除いて）AddForceによりEntityを制御する

	Stringクラス  紐本体
	Environmentクラス　1フレームの時間を取得したり、体積力（重力等）を管理したりするクラス
	Vectorクラス　ベクトル。operator*　は内積を表す。（スカラーとの乗算は係数倍）
	これらのクラスを用いています
*/


//String.h
class String:public Entity{
public:
	String(
		double l, //紐の長さ
		double e, //反発係数
		std::shared_ptr<Entity> e1, const Linear::Vector<double>& s1,	//対象物１と、紐の接続位置
		std::shared_ptr<Entity> e2, const Linear::Vector<double>& s2	//対象物２と、紐の接続位置
	)
		:Entity(
			Linear::Vector<double>(),
			double(0.0),
			Linear::Vector<double>()
		),
		l_(l),e_(e),
		e1_(e1), s1_(s1),
		e2_(e2), s2_(s2)
	{
	}
	void Update();
	void Draw();
private:
	double l_;
	std::weak_ptr<Entity> e1_;
	const Linear::Vector<double>& s1_;
	std::weak_ptr<Entity> e2_;
	const Linear::Vector<double>& s2_;
	double e_;
};


//String.cpp
//固定点と質点を与えると糸に引っ張られた質点の運動を計算する
void FixedAndMaterial(			
	shared_ptr<Entity> mp,					//質点のポインタ
	const Vector<double >& v,				//質点の速度
	double l,								//糸の長さ
	double e,								//反発係数
	Vector<double>& d,						//現在の固定点から質点への相対位置ベクトル
	const Vector<double>& sfp,				//固定点の位置ベクトル
	const Vector<double>& smp				//質点の位置ベクトル
);

void String::Update(){
	shared_ptr<Entity> p1, p2;
	if(! ((p1 = e1_.lock()) && (p2 = e2_.lock())) ) return;

	Vector<double> d = s2_- s1_;
	double		   x = d.Length();
	if( x < l_) return;

	//紐の接続先の質量を求める
	double m1 = p1->GetMass(), m2 = p2->GetMass();

	//どちらかが固定点（＝質量無限大）なら固定点周りの振り子運動
	if(m1 == std::numeric_limits<double>::infinity()){
		FixedAndMaterial(p2,p2->GetVelocity(),l_,e_,d,s1_,s2_);
	}else if(m2 == std::numeric_limits<double>::infinity() ){
		FixedAndMaterial(p1,p1->GetVelocity(),l_,e_,-d,s2_,s1_);

	//そうでないのなら重心系を用いて計算する
	}else{
		double dx = x - l_;
		d.Normalize();
		double SumOfMass(m1+m2);
		//重心を求める
		Vector<double> Sg = (m1*s1_+m2*s2_)/(SumOfMass);
		//重心の運動量を求める
		Vector<double> Vg = (m1*p1->GetVelocity()+m2*p2->GetVelocity())/SumOfMass;
		
		Vector<double> V1g = p1->GetVelocity() - Vg;
		Vector<double> V2g = p2->GetVelocity() - Vg;
		d = s1_ - Sg;
		FixedAndMaterial(p1,V1g,(l_*m2/SumOfMass),e_,d,Sg,s1_);

		d = s2_ - Sg;
		FixedAndMaterial(p2,V2g,(l_*m1/SumOfMass),e_,d,Sg,s2_);

	}
}

void FixedAndMaterial(
	shared_ptr<Entity> mp,
	const Vector<double>& v,
	double l,
	double e,
	Vector<double>& d,
	const Vector<double>& sfp,
	const Vector<double>& smp
){
	//質点に速度がなければ、位置を補正して戻る。（重力等は別のクラスが質点に加える）
	if( (v*v) < VTOL ){
		d.Normalize();
		mp->SetPosition(sfp + d*l);
		return;
	};

	//過去の衝突時間の算出（紐が作る球領域及び、現在の質点の位置と速度ベクトルから質点が球領域を通過する時刻を求める）
	//二次方程式になるため、判別式Dをまず求め場合分けする
	double D = (
			(v*smp-v*sfp)*(v*smp-v*sfp)-(v*v)*(smp*smp+sfp*sfp-(sfp*smp)*2.0-l*l)
		);
	double t(0.0);

	//衝突しない場合（計算誤差によると考えられるが、通常は球の接線方向かってにいるはずなので反発係数をかけないで軌道上に戻す）
	if( D < 0.0){
		Vector<double> de = d;
		de.Normalize();

		//ポジションを固定点の位置ベクトル＋紐の長さ＊相対位置ベクトルの正規ベクトルによって修正
		mp->SetPosition(sfp + de*l);
		
		//GetComponent関数は、第一引数の第二引数ベクトル成分を求める
		//まず速度の中心方向成分を求め
		Vector<double> vc = GetComponent( v,d );
		//接線方向成分を算出する。
		Vector<double> vl = v-vc;

		//完全に（エネルギーロス０で）滑ると考えるので、接線方向の運動量を保存し、
		vl *= ( v.Length()/vl.Length() );

		//中心方向の運動量を０にするだけの力を加える（力は運動量（すなわち力積）/微小時間（1フレームにかかる時間）で求める）
		Vector<double> vd = vl-v;
		mp->AddForce(vd*mp->GetMass()/Environment::Instance().GetTimeDelta(), smp);
		return;
	}else{

		//判別式が正なので衝突し、
		t = (v*(smp-sfp)- sqrt(D) )/(v*v);
		//衝突点を求める
		Vector<double> q = smp - (v*t);

		//衝突点から中心への相対位置ベクトル
		Vector<double> dCol = q - sfp;

		//中心方向と、接線方向の速度を導出し、
		Vector<double > vc = GetComponent( v,dCol );
		Vector<double> vl = v-vc;

		//現在いるべき座標を求める。（中心方向に跳ね返りが生じていると仮定し、その位置を考える）
		Vector<double> sTemp = q + ((vl - vc/((double)e+1.0))*t);
		
		//現在円の内側にいる場合（すなわち中心方向の速度は十分に大きく、紐の張力により跳ね返りが生じている場合）
		if( (sTemp-sfp)*(sTemp-sfp) < l*l ) {
			mp->SetPosition(sTemp);

			Vector<double> vd = -vc*((double)e+1.0);
			mp->AddForce(vd*mp->GetMass()/Environment::Instance().GetTimeDelta(),smp);
			return;
		}

		//現在円の外側にいる場合（中心方向の速度は十分に小さく、1フレーム前の接線方向に1フレーム分進んだために、円の外に出てしまっている場合）
		//後はD<0と同様
		Vector<double> de = d;
		de.Normalize();

		mp->SetPosition(sfp + de*l);
		
		vc = GetComponent(v,de);
		vl = v-vc;
		vl *= ( v.Length()/vl.Length() );

		Vector<double> vd = vl-v;
		mp->AddForce(vd*mp->GetMass()/Environment::Instance().GetTimeDelta(),smp);
		return;
	}
}