プログラマ専用SNS ミクプラ

コード:

//全部コードを載せると長くなるので要点だけ抑えます
/*
	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;
	}
}