CODE:

void RigidBody::AroundFixedPoint(){

	Phy dt = Environment::Instance().GetTimeDelta();
	//慣性テンソルを求める
	Matrix3x3> I
		(
			 CurrentIkk_.x,  CurrentIkj_.x,  -CurrentIkj_.z,
			-CurrentIkj_.x,  CurrentIkk_.y,	  CurrentIkj_.y,
			 CurrentIkj_.z, -CurrentIkj_.y,   CurrentIkk_.z
		);
	//角運動量を算出
	Vector> L  = I*o_;

	//オブジェクト空間における角速度の時間変化を算出
	Vector> dotO = Linear::GetSolutions3ByLU(I, N_-(o_^L));
	//角速度の更新
	o_ += dotO * dt;
	
	//角速度ベクトルから回転クォータニオンを導出
	Vector o = GetValue(o_);
	//角変位を求める
	double omega = o.Length()*dt.GetValue();
	if(omega (1.0,0.0,0.0);
		omega = 0.0;
	}else{
		o.Normalize();
	}

	//角変位分回転するクォータニオン、更新される座標系からは変位は逆になることに注意
	Quaternion dote = Linear::GetQuaternionByAxisAngle(-omega,o);

	//回転の合成
	e_ =  dote ^ e_ ;
	e_.Normalize();
	MaterialEntity::Update();
	N_ = Vector>();
}

CODE:

#pragma once

#include "MaterialEntity.h"
#include "EulerAngles.h"
#include 

class RigidBody :public MaterialEntity{
public:
	RigidBody(
		const Linear::Vector >&				s,	//重心位置
		Physics::Phy										m,	//質量
		const EulerAngles&											e,	//姿勢
		const Linear::Vector>&	Ikk,//重心周りの慣性モーメント
		const Linear::Vector>&	Ikj //重心周りの慣性乗積
	)
	:MaterialEntity(s,m),e_(Linear::GetQuaternionByEulerAngles(e).Reverse()),Ikk_(Ikk),Ikj_(Ikj),fixedPoint_(s),CurrentIkk_(Ikk),CurrentIkj_(Ikj)
	{}
	~RigidBody(){}

	//ワールド空間基準で力を加える
	virtual void AddForce(const Linear::Vector >&, const Linear::Vector >&);
	//オブジェクト空間基準で力を加える
	virtual void AddForceObject(const Linear::Vector >&, const Linear::Vector >&);

	//固定点の指定
	void SetFixedPoint(const Linear::Vector >&);
	//固定軸の指定
	void SetfixedAxis_(const Linear::Vector >&);

	//オブジェクト空間への変換クォータニオンを得る
	virtual Linear::Quaternion GetQuaternion(){return e_;}


	virtual void Update();
	virtual void Draw();
private:
	void CulcInertiaTensor();

	void AroundFixedPoint();
	void AroundFixedAxis();
private:
	Linear::Quaternion									e_;			//ワールド空間からオブジェクト空間への変換クォータニオン
	Linear::Vector>		o_;			//オブジェクト空間から見た角速度ベクトル


	Linear::Vector>	Ikk_;		//重心周りの慣性モーメント
	Linear::Vector>	Ikj_;		//重心周りの慣性乗積

	Linear::Vector>			N_;			//オブジェクト空間から見たオブジェクトにかかるトルク

	//拘束回転用変数
	Linear::Vector >				fixedPoint_;//現在の固定点（オブジェクト空間。なければ重心）
	Linear::Vector >		fixedAxis_; //現在の固定軸（オブジェクト空間。なければ0）
	
	Linear::Vector>	CurrentIkk_;//現在の固定点周りの慣性モーメント
	Linear::Vector>	CurrentIkj_;//現在の固定点周りの慣性乗積	
};

CODE:

//オイラー角の時間変化を算出
	EulerAngles> dotRad(0.0,0.0,0.0);
	double cosc = cos(e_.c);	
	double sinc = sin(e_.c);	
	double cosb = cos(e_.b);	
	double sinb = sin(e_.b);		
	dotRad.b = o_.x*cosc - o_.y*sinc;
	
	if(abs(cosb) > VTOL){
		dotRad.a = (cosc*o_.y+sinc*o_.x) / cosb;
	}else{
		dotRad.a = Phy(0.0);
	}
	dotRad.c = o_.z + dotRad.a*sinb;
	
	
	EulerAngles temp(dotRad.a*dt,dotRad.b*dt,dotRad.c*dt);

	//オイラー角の更新。
	e_ += temp;