寫個2D剛體模拟器，麻雀雖小五髒俱全

#ifndef RIGID_SOLVER_2D_H
#define RIGID_SOLVER_2D_H

#include <cassert>
#include <cstdint>
#include <iostream>
#include <limits>
#include <vector>
#include <map>

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include <glut.h>

namespace RIGID_SOLVER_2D
{

//-------------------------------------------------MATH
typedef double Scalar;
typedef Eigen::Vector2d Vec2;
typedef Eigen::Vector3d Vec3;
typedef Eigen::Vector4d Vec4;
typedef Eigen::VectorXd Vec;
typedef Eigen::Matrix2d Mat2;
static inline Scalar inf(){static Scalar val=std::numeric_limits<Scalar>::infinity();return val;}
static inline Vec2 infV(){static Vec2 val(inf(),inf());return val;}
static inline Scalar eps(){return 1E-9f;}
static inline Mat2 rot(Scalar theta){
	Scalar c=cos(theta),s=sin(theta);
	Mat2 ret;ret << c,-s,s,c;
	return ret;
}
static inline Vec3 invT(const Vec3& tran){
	Vec3 ret;
	ret[2]=-tran[2];
	ret.block<2,1>(0,0)=-rot(-tran[2])*tran.block<2,1>(0,0);
	return ret;
}
static inline Vec3 mulT(const Vec3& tranA,const Vec3& tranB){
	Vec3 ret;
	ret.block<2,1>(0,0)=rot(tranB[2])*tranA.block<2,1>(0,0)+tranB.block<2,1>(0,0);
	ret[2]=tranA[2]+tranB[2];
	return ret;
}
static inline Vec3 transP(const Vec3& plane,const Vec3& tran){
	Vec3 ret;
	ret[2]=tran.block<2,1>(0,0).dot(plane.block<2,1>(0,0))+plane[2];
	ret.block<2,1>(0,0)=rot(tran[2]).transpose()*plane.block<2,1>(0,0);
	return ret;
}
static inline Vec2 transPt(const Vec2& pt,const Vec3& tran){return rot(tran[2])*pt+tran.block<2,1>(0,0);}
static inline Scalar angTo(const Vec2& nA,const Vec2& nB){
	Scalar c=nA.dot(nB);
	Scalar s=nA[0]*nB[1]-nA[1]*nB[0];
	return atan2(s,c);
}
static inline bool interBB(const Vec4& bbA,const Vec4& bbB){
	return bbA[2] >= bbB[0] && bbA[3] >= bbB[1] &&
		   bbB[2] >= bbA[0] && bbB[3] >= bbA[1];
}
//-------------------------------------------------SHAPE
struct Shape
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	Shape():_M(inf()),_IB(inf()),_C(Vec2::Zero()){}
	virtual ~Shape(){}
	virtual bool inside(const Vec2& pt) const=0;
	virtual void CDPoint(const Vec2& pt,const Vec2& dir,Scalar& t,Vec2& n,Scalar& dist,Scalar margin,bool CCD) const=0;
	virtual void cut(const Vec3& plane) =0;
	virtual bool BB(Vec4& bb,const Vec3& tran) const=0;	//return infinite or not
	virtual Scalar rho() const=0;
	virtual void draw(const Vec3& tran=Vec3::Zero(),bool drawBB=false) const=0;
	virtual void verts(std::vector<Vec2,Eigen::aligned_allocator<Vec2> >& verts,const Vec3& trans=Vec3::Zero()) const=0;
	Scalar mass() const{return _M;}
	Vec2 center() const{return _C;}
	Scalar IB() const{return _IB;}
	Scalar IBC() const{return _IB-_C.squaredNorm()*_M;}
	Scalar IB(const Vec3& trans){
		Scalar ret=_IB;
		ret+=_M*trans.block<2,1>(0,0).squaredNorm();
		ret+=_M*_C.dot(rot(-trans[2])*trans.block<2,1>(0,0))*2.0f;
		return ret;
	}
	void debugIB(Scalar delta) const{
		Vec4 bb;
		if(BB(bb,Vec3::Zero())){
			Scalar MN=0.0f,IBN=0.0f;
			Vec2 CN=Vec2::Zero();
			for(Scalar x=bb[0];x<=bb[2];x+=delta)
			for(Scalar y=bb[1];y<=bb[3];y+=delta){
				if(inside(Vec2(x,y))){
					MN+=delta*delta;
					CN+=Vec2(x,y)*delta*delta;
					IBN+=Vec2(x,y).squaredNorm();
				}
			}
			std::cout << "NIB: " << IBN*rho()*delta*delta << " AIB: " << _IB << std::endl;
			std::cout << "NC: " << CN[0]/MN << " " << CN[1]/MN << " AC: " << center()[0] << " " << center()[1] << std::endl;
		}
	}
protected:
	Scalar _M,_IB;
	Vec2 _C;
};
struct ConvexShape : public Shape
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	ConvexShape(Scalar rho=1.0f):_rho(rho){}
	virtual bool inside(const Vec2& pt) const{
		if(_borders.empty())return _M != 0.0f;
		for(size_t i=0;i<_borders.size();i++)
		if(_borders[i].block<2,1>(0,0).dot(pt)+_borders[i][2] > 0.0f)return false;
		return true;
	}
	virtual void CDPoint(const Vec2& pt,const Vec2& dir,Scalar& t,Vec2& n,Scalar& dist,Scalar margin,bool CCD) const{
		if(_borders.empty()){t=inf();return;}
		t=0.0f;
		Scalar minD=-inf(),tb,ta;
		Vec2 dirB,VLI=_vertices.back()+_ns.back()*margin,VI;
		for(size_t i=0,li=_borders.size()-1;i<_borders.size();i++,li=i){
			VI=_vertices[i]+_ns[i]*margin;
			dist=_borders[i].block<2,1>(0,0).dot(pt)+_borders[i][2]-margin;
			if(dist < 0.0f){
				if(dist > minD){
					n=_borders[i].block<2,1>(0,0);
					minD=dist;
				}
			}else if(CCD){
				dist=0.0f;
				t=inf();
				tb=_borders[i].block<2,1>(0,0).dot(dir);
				if(tb < -eps() && (tb=-dist/tb) < 1.0f){
					n=_borders[i].block<2,1>(0,0);
					if(VLI[0] == inf() && VI[0] == inf()){
						t=tb;return;
					}else if(_vertices[i][0] == inf()){
						dirB=Vec2(-_borders[i][1],_borders[i][0]);
						if(dirB.dot(pt+dir*tb-VLI) > 0.0f){t=tb;return;}
					}else if(_vertices[li][0] == inf()){
						dirB=Vec2(_borders[i][1],-_borders[i][0]);
						if(dirB.dot(pt+dir*tb-VI) > 0.0f){t=tb;return;}
					}else{
						dirB=VI-VLI;
						ta=dirB.dot(pt+dir*tb-VLI);
						if(ta >= 0.0f && ta <= dirB.squaredNorm()){t=tb;return;}
					}
				}else{t=inf();return;}
			}else{
				t=inf();dist=inf();return;
			}
			VLI=VI;
		}
		dist=minD;
	}
	virtual void cut(const Vec3& plane){
		Vec2 v;
		Vec3 planeN=plane/plane.block<2,1>(0,0).norm();
		if(_borders.empty()){	//half plane
			_borders.push_back(planeN);
			_vertices.push_back(infV());
			_ns.push_back(Vec2::Zero());
		}else{	//cut current shape
			std::vector<Vec3,Eigen::aligned_allocator<Vec3> > bordersN;
			std::vector<Vec2,Eigen::aligned_allocator<Vec2> > verticesN;
			bool flag,flag0;
			flag0=flag=isInside(planeN,_borders[0],_vertices.back(),_vertices[0]);
			for(size_t i=0,li;i<_vertices.size();i++){
				li=(i+_vertices.size()-1)%_vertices.size();
				if(interP(planeN,_borders[i],_vertices[li],_vertices[i],v)){
					if(flag == flag0){
						if(flag0){
							bordersN.push_back(_borders[i]);
							verticesN.push_back(v);
							if(i == _vertices.size()-1){
								bordersN.push_back(planeN);
								verticesN.push_back(_vertices[i]);
							}
						}else{
							bordersN.push_back(planeN);
							verticesN.push_back(v);
							bordersN.push_back(_borders[i]);
							verticesN.push_back(_vertices[i]);
						}flag=!flag;
					}else{
						if(flag0){
							bordersN.push_back(planeN);
							verticesN.push_back(v);
							bordersN.push_back(_borders[i]);
							verticesN.push_back(_vertices[i]);
						}else{
							bordersN.push_back(_borders[i]);
							verticesN.push_back(v);
						}flag=!flag;
					}
				}else if(flag){
					bordersN.push_back(_borders[i]);
					verticesN.push_back(_vertices[i]);
				}
			}
			_borders=bordersN;
			_vertices=verticesN;
			reset();
		}
	}
	virtual bool BB(Vec4& bb,const Vec3& tran) const{
		bb.block<2,1>(0,0)=infV();
		bb.block<2,1>(2,0)=-infV();
		if(_vertices.empty())return false;
		Mat2 R=rot(tran[2]);
		for(size_t i=0;i<_vertices.size();i++){
			if(_vertices[i][0] == inf())return false;
			bb.block<2,1>(0,0)=bb.block<2,1>(0,0).cwiseMin(R*_vertices[i]+tran.block<2,1>(0,0));
			bb.block<2,1>(2,0)=bb.block<2,1>(2,0).cwiseMax(R*_vertices[i]+tran.block<2,1>(0,0));
		}return true;
	}
	virtual Scalar rho() const{return _rho;}
	virtual void draw(const Vec3& tran=Vec3::Zero(),bool drawBB=false) const{
#define EXT 10000.0f
		Mat2 R=rot(tran[2]);
		glBegin(GL_LINES);
		for(size_t i=0;i<_vertices.size();i++)
		{
			Vec2 a=_vertices[(i+_vertices.size()-1)%_vertices.size()];
			Vec2 b=_vertices[i];
			if(a[0] == inf() && b[0] == inf()){
				Vec2 v0=_borders[i].block<2,1>(0,0)*
						-_borders[i][2]/_borders[i].block<2,1>(0,0).squaredNorm();
				a=v0+Vec2(_borders[i][1],-_borders[i][0])*EXT;
				b=v0-Vec2(_borders[i][1],-_borders[i][0])*EXT;
			}else if(a[0] == inf())a=b+Vec2(_borders[i][1],-_borders[i][0])*EXT;
			else if(b[0] == inf())b=a+Vec2(-_borders[i][1],_borders[i][0])*EXT;
			a=R*a+tran.block<2,1>(0,0);
			b=R*b+tran.block<2,1>(0,0);
			glColor3d(1.0f,0.0f,0.0f);glVertex2d(a[0],a[1]);
			glColor3d(0.0f,0.0f,1.0f);glVertex2d(b[0],b[1]);

			if(b[0] != inf()){
				glColor3d(1.0f,1.0f,1.0f);
				glVertex2d(b[0],b[1]);
				b+=R*_ns[i]*1E-3f;
				glVertex2d(b[0],b[1]);
			}
		}
		glEnd();
#undef EXT

		Vec4 bb;
		if(drawBB && BB(bb,tran))
		{
			glColor3d(0.0f,1.0f,0.0f);
			glBegin(GL_LINE_LOOP);
			glVertex2d(bb[0],bb[1]);
			glVertex2d(bb[2],bb[1]);
			glVertex2d(bb[2],bb[3]);
			glVertex2d(bb[0],bb[3]);
			glEnd();
		}
	}
	virtual void verts(std::vector<Vec2,Eigen::aligned_allocator<Vec2> >& verts,const Vec3& tran=Vec3::Zero()) const{
		for(size_t i=0;i<_vertices.size();i++)
			verts.push_back(transPt(_vertices[i],tran));
	}
	void debug() const{
		assert(_borders.size() == _vertices.size());
		for(size_t i=0;i<_vertices.size();i++)
		{
			if(_vertices[i][0] != inf())
				assert(std::abs(_borders[i].block<2,1>(0,0).dot(_vertices[i])+_borders[i][2]) <= eps());
			size_t ni=(i+_vertices.size()-1)%_vertices.size();
			if(_vertices[ni][0] != inf())
				assert(std::abs(_borders[i].block<2,1>(0,0).dot(_vertices[ni])+_borders[i][2]) <= eps());
		}
		//counter clockwise
		for(size_t i=0;i<_borders.size();i++)
			if(_vertices[i][0] != inf())
			assert(angTo(_borders[i].block<2,1>(0,0),_borders[(i+1)%_vertices.size()].block<2,1>(0,0)) >= 0.0f);
	}
protected:
	static bool interP(const Vec3& plane,const Vec3& border,const Vec2& vA,const Vec2& vB,Vec2& v){
		Scalar d=plane.block<2,1>(0,0).dot(border.block<2,1>(0,0));
		if(std::abs(1.0f-d*d) < eps())	//almost parallel
			return false;
		if(vA[0] == inf() && vB[0] == inf()){
			Mat2 lhs;
			lhs.row(0)=plane.block<2,1>(0,0);
			lhs.row(1)=border.block<2,1>(0,0);
			Eigen::FullPivLU<Mat2> sol=lhs.fullPivLu();
			v=sol.solve(-Vec2(plane[2],border[2]));
			return true;
		}else if(vA[0] == inf()){
			Vec2 d=Vec2(border[1],-border[0]);
			Scalar rhs=-plane[2]-plane.block<2,1>(0,0).dot(vB);
			Scalar lhs=plane.block<2,1>(0,0).dot(d);
			v=vB+d*rhs/lhs;
			return std::abs(lhs) > eps() && rhs/lhs > eps();
		}else if(vB[0] == inf()){
			Vec2 d=Vec2(-border[1],border[0]);
			Scalar rhs=-plane[2]-plane.block<2,1>(0,0).dot(vA);
			Scalar lhs=plane.block<2,1>(0,0).dot(d);
			v=vA+d*rhs/lhs;
			return std::abs(lhs) > eps() && rhs/lhs > eps();
		}else{
			Vec2 d=vB-vA;
			Scalar rhs=-plane[2]-plane.block<2,1>(0,0).dot(vA);
			Scalar lhs=plane.block<2,1>(0,0).dot(d);
			rhs/=lhs;
			v=vA+d*rhs;
			return std::abs(lhs) > eps() && rhs >= eps() && rhs <= 1.0f-eps();
		}
	}
	static bool isInside(const Vec3& plane,const Vec3& border,const Vec2& vA,const Vec2& vB){
		Scalar d=plane.block<2,1>(0,0).dot(border.block<2,1>(0,0));
		if(std::abs(1.0f-d*d) < eps())	//almost parallel
			return plane[2] < (d<0.0f?-1.0f:1.0f)*border[2];
		else if(vA[0] == inf() && vB[0] == inf()) 
			return angTo(border.block<2,1>(0,0),plane.block<2,1>(0,0)) > 0.0f;
		else if(vA[0] == inf())
			return plane.block<2,1>(0,0).dot(Vec2(border[1],-border[0])) < 0.0f;
		else return plane.block<2,1>(0,0).dot(vA)+plane[2] < 0.0f;
	}
	void merge(Scalar thres){
		bool more=_borders.size() > 3;
		while(more){
			size_t minD=-1;
			Scalar minDV=inf(),v=inf();
			for(size_t i=0;i<_borders.size();i++){
				size_t ni=(i+_vertices.size()-1)%_vertices.size();
				if(_vertices[i][0] == inf() || _vertices[ni][0] == inf())continue;
				if((v=(_vertices[i]-_vertices[ni]).squaredNorm()) < minDV){minDV=v;minD=i;}
			}
			if(sqrt(minDV) < thres){
				Vec2 newV;
				size_t nminD=(minD+_vertices.size()-1)%_vertices.size();
				bool inter=interP(_borders[(minD+1)%_borders.size()],_borders[nminD],infV(),infV(),newV);
				if(inter){
					_vertices[minD]=newV;
					_vertices.erase(_vertices.begin()+nminD);
					_borders.erase(_borders.begin()+minD);
				}
				more=inter && _borders.size() > 3;
			}else more=false;
		}
	}
	void reset(){
		merge(1E-2f);
		_ns.resize(_vertices.size());
		_C=Vec2::Zero();
		Mat2 lhs;
		Scalar xx=0.0f,yy=0.0f,m=0.0f,v,xt,yt;
		for(size_t i=0,ni;i<_vertices.size();i++){
			if(_vertices[i][0] == inf()){
				_C=Vec2::Zero();
				_ns[i].setZero();
				_M=_IB=inf();
				return;
			}else{
				ni=(i+1)%_vertices.size();
				lhs.row(0)=_borders[i].block<2,1>(0,0);
				lhs.row(1)=_borders[ni].block<2,1>(0,0);
				_ns[i]=lhs.inverse()*Vec2(1.0f-_borders[i][2],1.0f-_borders[ni][2])-_vertices[i];
				const Vec2& v0=_vertices[i];
				const Vec2& v1=_vertices[ni];
				v=v0[0]*v1[1]-v1[0]*v0[1];
				m+=v;
				xt=v0[0]+v1[0];
				yt=v0[1]+v1[1];
				_C[0]+=v*xt;
				_C[1]+=v*yt;
				xx+=v*(v0[0]*v0[0]+v1[0]*v1[0]+xt*xt);
				yy+=v*(v0[1]*v0[1]+v1[1]*v1[1]+yt*yt);
			}
		}
		if(m == 0.0f){
			_C=Vec2::Zero();
			_M=0.0f;
			_IB=0.0f;
		}else{
			_C=_C/(3.0f*m);
			_M=m/2.0f*_rho;
			_IB=(xx/24.0+yy/24.0f)*_rho;
		}
	}
	std::vector<Vec3,Eigen::aligned_allocator<Vec3> > _borders;		//counter clockwise
	std::vector<Vec2,Eigen::aligned_allocator<Vec2> > _vertices;	//_vertices[0] border _border[0] and _border[1]
	std::vector<Vec2,Eigen::aligned_allocator<Vec2> > _ns;
	Scalar _rho;
};
struct ConcaveShape : public Shape
{
	struct Comp
	{
		Comp(Shape* comp,const Vec3& tran,const Vec3& invTran):_comp(comp),_tran(tran),_invTran(invTran){}
		Shape* _comp;
		Vec3 _tran,_invTran;
	};
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	ConcaveShape(){_M=_IB=0.0f;}
	ConcaveShape(const ConcaveShape& other){operator=(other);}
	virtual ~ConcaveShape(){
		for(size_t i=0;i<_comps.size();i++)
			delete _comps[i]._comp;
	}
	ConcaveShape& operator=(const ConcaveShape& other){
		Shape::operator=(other);
		_comps.clear();
		for(size_t i=0;i<other._comps.size();i++){
			_comps.push_back(other._comps[i]);
			ConcaveShape* cs=dynamic_cast<ConcaveShape*>(other._comps[i]._comp);
			if(cs){
				_comps.back()._comp=new ConcaveShape;
				*(ConcaveShape*)(_comps.back()._comp)=*cs;
			}else{
				_comps.back()._comp=new ConvexShape;
				*(ConvexShape*)(_comps.back()._comp)=*(ConvexShape*)(other._comps[i]._comp);
			}
		}
		return *this;
	}
	virtual bool inside(const Vec2& pt) const{
		for(size_t i=0;i<_comps.size();i++)
		if(_comps[i]._comp->inside(transPt(pt,_comps[i]._invTran)))return true;
		return false;
	}
	virtual void CDPoint(const Vec2& pt,const Vec2& dir,Scalar& t,Vec2& n,Scalar& dist,Scalar margin,bool CCD) const{
		t=inf();
		dist=inf();
		Scalar t0,dist0;
		Vec2 n0;
		Mat2 R;
		for(size_t i=0;i<_comps.size();i++){
			R=rot(_comps[i]._tran[2]);
			_comps[i]._comp->CDPoint(transPt(pt,_comps[i]._invTran),R.transpose()*dir,t0,n0,dist0,margin,CCD);
			if(t0 >= 0.0f && t0 < 1.0f && t0 < t){t=t0;n=R*n0;dist=dist0;}
		}
	}
	virtual void cut(const Vec3& plane){
		_M=0.0f;
		_C=Vec2::Zero();
		_IB=0.0f;
		for(size_t i=0;i<_comps.size();)
		{
			_comps[i]._comp->cut(transP(plane,_comps[i]._tran));
			if(_comps[i]._comp->mass() == 0.0f){
				delete _comps[i]._comp;
				_comps.erase(_comps.begin()+i);
			}else{
				_M+=_comps[i]._comp->mass();
				_C+=_comps[i]._comp->mass()*transPt(_comps[i]._comp->center(),_comps[i]._tran);
				_IB+=_comps[i]._comp->IB(_comps[i]._tran);
				i++;
			}
		}
		if(_M == inf())_C=Vec2::Zero();
		else _C/=_M;
	}
	virtual bool BB(Vec4& bb,const Vec3& tran) const{
		Vec4 bbC;
		bb.block<2,1>(0,0)=infV();
		bb.block<2,1>(2,0)=-infV();
		for(size_t i=0;i<_comps.size();i++){
			if(!_comps[i]._comp->BB(bbC,mulT(_comps[i]._tran,tran)))return false;
			bb.block<2,1>(0,0)=bb.block<2,1>(0,0).cwiseMin(bbC.block<2,1>(0,0));
			bb.block<2,1>(2,0)=bb.block<2,1>(2,0).cwiseMax(bbC.block<2,1>(2,0));
		}return true;
	}
	virtual Scalar rho() const{
		Scalar M=0.0f,A=0.0f;
		for(size_t i=0;i<_comps.size();i++){
			M+=_comps[i]._comp->mass();
			A+=_comps[i]._comp->mass()/_comps[i]._comp->rho();
		}return M/A;
	}
	virtual void draw(const Vec3& tran=Vec3::Zero(),bool drawBB=false) const
	{
		for(size_t i=0;i<_comps.size();i++)
			_comps[i]._comp->draw(mulT(_comps[i]._tran,tran),false);

		Vec4 bb;
		if(drawBB && BB(bb,tran))
		{
			glColor3d(1.0f,0.0f,1.0f);
			glBegin(GL_LINE_LOOP);
			glVertex2d(bb[0],bb[1]);
			glVertex2d(bb[2],bb[1]);
			glVertex2d(bb[2],bb[3]);
			glVertex2d(bb[0],bb[3]);
			glEnd();
		}
	}
	virtual void verts(std::vector<Vec2,Eigen::aligned_allocator<Vec2> >& verts,const Vec3& tran=Vec3::Zero()) const{
		for(size_t i=0;i<_comps.size();i++)
			_comps[i]._comp->verts(verts,mulT(_comps[i]._tran,tran));
	}
	void add(Shape* comp,const Vec3& tran)
	{
		_comps.push_back(Comp(comp,tran,invT(tran)));
		_C=_C*_M+comp->mass()*transPt(comp->center(),tran);
		_M+=comp->mass();
		if(_M == inf())_C=Vec2::Zero();
		else _C/=_M;
		_IB+=comp->IB(tran);
	}
protected:
	std::vector<Comp> _comps;
};
//-------------------------------------------------BODY
struct Body
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	Body(Shape* shape):_shape(shape),_CCD(false){
		_pos=Vec3::Zero();
		_vel=Vec3::Zero();
		_for=Vec3::Zero();
		_fri=0.0f;
		_res=0.0f;
		updateShape();
	}
	void cut(Vec3& plane){
		Vec2 offC=_shape->center();
		_shape->cut(transP(plane,tran()));
		if(_shape->mass() == 0.0f)return;
		offC=rot(_pos[2])*(_shape->center()-offC);
		_pos.block<2,1>(0,0)+=offC;
		_vel.block<2,1>(0,0)+=Vec2(-offC[1],offC[0])*_vel[2];
		updateShape();
	}
	bool BB(Vec4& bb) const{return _shape->BB(bb,tran());}
	void draw(bool drawBB=false) const{
		Vec3 T=tran();Vec2 P;
		_shape->draw(T,drawBB);
		glPointSize(2.0f);
		glColor3d(1.0f,1.0f,1.0f);
		glBegin(GL_POINTS);
		for(size_t i=0;i<_vert.size();i++){
			P=transPt(_vert[i],T);
			glVertex2d(P[0],P[1]);
		}
		glEnd();
	}
	Vec3 tran() const{
		Vec3 tran=_pos;
		tran.block<2,1>(0,0)-=rot(_pos[2])*_shape->center();
		return tran;
	}
	Vec3& pos(){return _pos;}
	const Vec3& pos() const{return _pos;}
	Vec3& vel(){return _vel;}
	const Vec3& vel() const{return _vel;}
	Vec2 vel(const Vec2& ptRG) const{return _vel.block<2,1>(0,0)+Vec2(-ptRG[1],ptRG[0])*_vel[2];}
	bool CCD() const{return _CCD;}
	void setCCD(bool CCD){_CCD=CCD;}
	const Shape& shape() const{return *_shape;}
	Scalar fri() const{return _fri;}
	void setFri(Scalar fri){_fri=fri;}
	Scalar res() const{return _res;}
	void setRes(Scalar res){_res=res;}
	const std::vector<Vec2,Eigen::aligned_allocator<Vec2> >& vert() const{return _vert;}	//for collision detection
	void force(const Vec2& F){force(F,transPt(_shape->center(),tran()));}
	void force(const Vec2& F,const Vec2& pt){
		Vec2 C=pt-_pos.block<2,1>(0,0);
		_for+=Vec3(F[0],F[1],C[0]*F[1]-C[1]*F[0]);
	}
	void impulse(const Vec2& F,const Vec2& pt){
		if(_shape->mass() > 0.0f && _shape->mass() != inf()){
			Vec2 C=pt-_pos.block<2,1>(0,0);
			Vec3 dI(F[0],F[1],C[0]*F[1]-C[1]*F[0]);
			dI.block<2,1>(0,0)/=_shape->mass();
			dI[2]/=_shape->IBC();
			_vel+=dI;
		}
	}
	Scalar infCoef(const Vec2& Sv,const Vec2& Sn,const Vec2& Dv,const Vec2& Dn) const{
		if(_shape->mass() > 0.0f && _shape->mass() != inf()){
			Vec3 P(Dn[0],Dn[1],Vec2(-Dv[1],Dv[0]).dot(Dn));
			Vec3 V(Sn[0],Sn[1],Vec2(-Sv[1],Sv[0]).dot(Sn));
			V.block<2,1>(0,0)/=_shape->mass();
			V[2]/=_shape->IBC();
			return P.dot(V);
		}else return 0.0f;
	}
	void advanceVel(Scalar dt){
		if(_shape->mass() > 0.0f && _shape->mass() != inf()){
			_for.block<2,1>(0,0)/=_shape->mass();
			_for[2]/=_shape->IBC();
			_vel+=_for*dt;
		}
		_for.setZero();
	}
	void advancePos(Scalar dt){
		_pos+=_vel*dt;
		while(_pos[2] < 0.0f)_pos[2]+=M_PI*2.0f;
		while(_pos[2] > M_PI*2.0f)_pos[2]-=M_PI*2.0f;
	}
protected:	//world space, about center of mass
	void updateShape(){
		_vert.clear();
		_shape->verts(_vert);
	}
	std::vector<Vec2,Eigen::aligned_allocator<Vec2> > _vert;
	Vec3 _pos,_vel,_for;
	Shape* _shape;
	bool _CCD;
	Scalar _fri,_res;
};
//-------------------------------------------------COLLISION
struct Collide
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	Scalar relVel() const{return Vec2(_A->vel(_ptAL)-_B->vel(_ptBL)).dot(_nBG);}
	Scalar relVelT() const{return Vec2(_A->vel(_ptAL)-_B->vel(_ptBL)).dot(_tBG);}
	Body *_A,*_B;
	Vec2 _ptAL,_ptBL,_nBG,_tBG;
	Scalar _TOI,_dist;
	int64_t _key;
	size_t _IA,_IB;
};
class Collision
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	Collision(Scalar cellSz,Scalar margin):_cellSz(cellSz),_margin(margin){}
	void add(Body* body){_bodies.push_back(body);_bbs.push_back(Vec4());}
	void del(Body* body){
		for(size_t i=0;i<_bodies.size();i++)
		if(_bodies[i]==body){
			_bodies[i]=_bodies.back();
			_bodies.pop_back();
			_bbs.pop_back();
		}
	}
	void clear(){_bodies.clear();_bbs.clear();}
	void detect(Scalar dt){
		buildHash(dt);	//broadphase
		Vec2 ptW,ptWB,ptR;	//narrowphase
		Vec3 tranA;
		Vec4 bb;
		Body* bodyB;
		int x0,x1,y0,y1,x,y;
		std::map<int64_t,std::pair<size_t,size_t> >::iterator iter;
		//detect collision
		_colls.clear();
		_TOICache.clear();
		for(size_t i=0;i<_bodies.size();i++){
			tranA=_bodies[i]->tran();
			const std::vector<Vec2,Eigen::aligned_allocator<Vec2> >& vert=_bodies[i]->vert();
			for(size_t v=0;v<vert.size();v++){
				ptW=transPt(vert[v],tranA);
				ptWB=ptW+_bodies[i]->vel().block<2,1>(0,0);
				ptR=ptW-_bodies[i]->pos().block<2,1>(0,0);
				bb.block<2,1>(0,0)=ptW.cwiseMin(ptWB);
				bb.block<2,1>(2,0)=ptW.cwiseMax(ptWB);
				//detect against finite bodies
				x0=(int)floor(bb[0]/_cellSz);
				y0=(int)floor(bb[1]/_cellSz);
				x1=(int)floor(bb[2]/_cellSz);
				y1=(int)floor(bb[3]/_cellSz);
				for(x=x0;x<=x1;x++)
				for(y=y0;y<=y1;y++){
					iter=_hashOff.find(hash(x,y));
					if(iter != _hashOff.end())
					for(size_t b=iter->second.first;b<iter->second.second;b++)
						if(_hash[b].second != i){
							if(!interBB(bb,_bbs[_hash[b].second]))continue;
							bodyB=_bodies[_hash[b].second];
							collide(i,_hash[b].second,ptR,ptW,_bodies[i],bodyB,dt);
						}
				}
				//detect against infinite bodies
				for(size_t b=0;b<_infs.size();b++){
					if(_infs[b] == i)continue;
					bodyB=_bodies[_infs[b]];
					collide(i,_infs[b],ptR,ptW,_bodies[i],bodyB,dt);
				}
			}
		}
		//filter collision
		for(size_t i=0;i<_colls.size();)
			if(_colls[i]._TOI == _TOICache[_colls[i]._key])i++;
			else{_colls[i]=_colls.back();_colls.pop_back();}
	}
	void draw(Scalar len,bool drawBB=false,bool drawColl=false) const{
		for(size_t i=0;i<_bodies.size();i++){
			_bodies[i]->draw(false);
			glColor3d(0.0f,1.0f,0.0f);
			if(drawBB && _bbs[i][0] != inf())
			{
				glBegin(GL_LINE_LOOP);
				glVertex2d(_bbs[i][0],_bbs[i][1]);
				glVertex2d(_bbs[i][2],_bbs[i][1]);
				glVertex2d(_bbs[i][2],_bbs[i][3]);
				glVertex2d(_bbs[i][0],_bbs[i][3]);
				glEnd();
			}
		}
		if(drawColl)
		for(size_t i=0;i<_colls.size();i++){
			/*Vec3 tmpPos=_colls[i]._A->pos();
			_colls[i]._A->pos().block<2,1>(0,0)+=
			_colls[i]._A->vel().block<2,1>(0,0)*_colls[i]._TOI;
			_colls[i]._A->draw(false);
			_colls[i]._A->pos()=tmpPos;

			tmpPos=_colls[i]._B->pos();
			_colls[i]._B->pos().block<2,1>(0,0)+=
			_colls[i]._B->vel().block<2,1>(0,0)*_colls[i]._TOI;
			_colls[i]._B->draw(false);
			_colls[i]._B->pos()=tmpPos;*/
			
			Vec2 v,vn;
			glBegin(GL_LINES);
			glColor3d(0.0f,1.0f,0.0f);
			v=_colls[i]._A->pos().block<2,1>(0,0)+_colls[i]._ptAL;
			vn=v-_colls[i]._nBG*len;
			glVertex2d(v[0],v[1]);
			glVertex2d(vn[0],vn[1]);
			
			v=_colls[i]._B->pos().block<2,1>(0,0)+_colls[i]._ptBL;
			vn=v+_colls[i]._nBG*len;
			glVertex2d(v[0],v[1]);
			glVertex2d(vn[0],vn[1]);
			glEnd();
		}
	}
	const std::vector<Collide,Eigen::aligned_allocator<Collide> >& coll() const{return _colls;}
	static bool LSS(const std::pair<int64_t,size_t>& a,const std::pair<int64_t,size_t>& b){return a.first < b.first;}
protected:
	virtual void collide(size_t IA,size_t IB,const Vec2& vR,const Vec2& pt,Body* bodyA,Body* bodyB,Scalar dt){
		Collide c;
		c._A=bodyA;
		c._B=bodyB;
		c._IA=IA;
		c._IB=IB;
		c._key=IA<IB ? hash(IA,IB) : hash(IB,IA);
		std::map<int64_t,Scalar>::iterator iter;
		if((iter=_TOICache.find(c._key)) == _TOICache.end()){
			_TOICache[c._key]=inf();
			iter=_TOICache.find(c._key);
		}
		Vec3 tranB=bodyB->tran();
		Vec2 pt0=transPt(pt,invT(tranB));
		Vec2 dir=rot(-tranB[2])*(bodyA->vel()-bodyB->vel()).block<2,1>(0,0)*dt;
		Scalar TOI,t;
		if(bodyA->CCD() || bodyB->CCD()){
			bodyB->shape().CDPoint(pt0,dir,t,c._nBG,c._dist,_margin,true);
			TOI=t*dt;
		}else{
			bodyB->shape().CDPoint(pt0+dir,Vec2::Zero(),t,c._nBG,c._dist,_margin,false);
			TOI=dt;
		}
		if(t >= 0.0f && t <= 1.0f && TOI <= iter->second){
			c._ptAL=(bodyA->pos()+bodyA->vel()*TOI).block<2,1>(0,0);
			c._ptBL=(bodyB->pos()+bodyB->vel()*TOI).block<2,1>(0,0);
			c._ptBL=vR+c._ptAL-c._ptBL;
			c._ptAL=vR;
			c._nBG=rot(tranB[2])*c._nBG;
			c._tBG=Vec2(-c._nBG[1],c._nBG[0]);
			c._TOI=TOI;
			_colls.push_back(c);
			iter->second=TOI;
		}
	}
	void buildHash(Scalar dt){
		int x0,x1,y0,y1,x,y;
		_hash.clear();	//all hash entry
		_infs.clear();
		for(size_t i=0;i<_bodies.size();i++){
			Vec4& bb=_bbs[i];
			if(!_bodies[i]->BB(bb)){
				bb.setConstant(inf());
				_infs.push_back(i);
			}else{
				if(_bodies[i]->vel()[0] < 0.0f)
					bb[0]+=_bodies[i]->vel()[0]*dt;
				else bb[2]+=_bodies[i]->vel()[0]*dt;
				if(_bodies[i]->vel()[1] < 0.0f)
					bb[1]+=_bodies[i]->vel()[1]*dt;
				else bb[3]+=_bodies[i]->vel()[1]*dt;
				bb.block<2,1>(0,0).array()-=_margin;
				bb.block<2,1>(2,0).array()+=_margin;
				
				x0=(int)floor(bb[0]/_cellSz);
				y0=(int)floor(bb[1]/_cellSz);
				x1=(int)floor(bb[2]/_cellSz);
				y1=(int)floor(bb[3]/_cellSz);
				for(x=x0;x<=x1;x++)
				for(y=y0;y<=y1;y++)
					_hash.push_back(std::pair<int64_t,size_t>(hash(x,y),i));
			}
		}
		_hashOff.clear();	//sort hash
		if(_hash.empty())return;
		std::sort(_hash.begin(),_hash.end(),LSS);
		for(size_t i=1;i<_hash.size();i++)
			if(_hash[i].first != _hash[i-1].first)
			{_hashOff[_hash[i-1].first].second=i;
			 _hashOff[_hash[i].first].first=i;}
		_hashOff[_hash.front().first].first=0;
		_hashOff[_hash.back().first].second=_hash.size();
	}
	static int64_t hash(int x,int y){return (((int64_t)x)<<32)+y;}
	Scalar _cellSz,_margin;
	std::vector<Body*> _bodies;	//bodies
	std::vector<Vec4,Eigen::aligned_allocator<Vec4> > _bbs;
	std::vector<Collide,Eigen::aligned_allocator<Collide> > _colls;	//result
	std::map<int64_t,std::pair<size_t,size_t> > _hashOff;	//broadphase
	std::vector<std::pair<int64_t,size_t> > _hash;
	std::vector<size_t> _infs;
	std::map<int64_t,Scalar> _TOICache;	//narrowphase
};
//-------------------------------------------------SOLVER
class Solver : public Collision
{
public:
	typedef std::pair<size_t,size_t> Coord;
	typedef std::pair<Coord,Scalar> Entry;
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	Solver(Scalar cellSz,Scalar margin,size_t nrIter)
	:Collision(cellSz,margin),_nrIter(nrIter),_g(Vec2::Zero()),_cntThres(0.01f){}
	void setG(const Vec2& g){_g=g;}
	void advance(Scalar dt,Scalar CFL=0.01f){
		Scalar t=0.0f,delta;
		while(t < dt){
			delta=std::min(dt-t,CFL);
			for(size_t i=0;i<_bodies.size();i++){	//advance velocity
				_bodies[i]->force(_g*_bodies[i]->shape().mass());
				_bodies[i]->advanceVel(delta);
			}
			//pass 1: collision
			detect(delta);	//detect collision
			resolveN(inf(),true);	//resolve normal collision
			applyImpulse();	//apply impulse
			for(size_t i=0;i<_bodies.size();i++)	//advance position
				_bodies[i]->advancePos(delta);
			//pass 2: contact
			resolveT();	//resolve tangential friction
			resolveN(0.0f,false);	//resolve normal collision
			applyImpulse();	//apply impulse
			t+=delta;
		}
	}
	void resolveN(Scalar maxRes,bool firstOrder){
		_mats.resize(_bodies.size());	//build problem
		_v0.resize(_colls.size());
		_impulses.resize(_colls.size());
		_order.resize(_colls.size());
		for(size_t i=0;i<_mats.size();i++)
			_mats[i].clear();
		for(size_t i=0;i<_colls.size();i++){
			_mats[_colls[i]._IA].push_back(i);
			_mats[_colls[i]._IB].push_back(i);
			_v0[i]=_colls[i].relVel();
			Scalar res=std::max(_colls[i]._A->res(),_colls[i]._B->res());
			if(_v0[i] < -_cntThres)	//contact/collision switch
				_v0[i]*=1.0f+std::min(res,maxRes);
			if(firstOrder && _colls[i]._dist < -_margin)	//interpenetration avoidance
				_v0[i]-=1E-2f;//*(-_margin-_colls[i]._dist);
			_order[i]=i;
		}
		_entries.clear();
		for(size_t i=0;i<_mats.size();i++){
			for(size_t r=0;r<_mats[i].size();r++)
			for(size_t c=0;c<_mats[i].size();c++){
				size_t iR=_mats[i][r];
				size_t iC=_mats[i][c];
				const Collide& cR=_colls[iR];
				const Collide& cC=_colls[iC];
				Scalar val;
				if(iR==iC){
					if(i == cR._IA){
						val=cC._A->infCoef(cC._ptAL, cC._nBG, cC._ptAL, cC._nBG)+
							cC._B->infCoef(cC._ptBL,-cC._nBG, cC._ptBL,-cC._nBG);
						_entries.push_back(Entry(Coord(iR,iC),val));
					}
				}else{
					if(i == cC._IA){
						if(i == cR._IA)
							 val=cC._A->infCoef(cC._ptAL,cC._nBG, cR._ptAL, cR._nBG);
						else val=cC._A->infCoef(cC._ptAL,cC._nBG, cR._ptBL,-cR._nBG);
					}else{
						if(i == cR._IA)
							 val=cC._B->infCoef(cC._ptBL,-cC._nBG, cR._ptAL, cR._nBG);
						else val=cC._B->infCoef(cC._ptBL,-cC._nBG, cR._ptBL,-cR._nBG);
					}_entries.push_back(Entry(Coord(iR,iC),val));
				}
			}
		}
		buildMatrix();
		//solve problem using PGS
		_impulses.setZero();
		for(size_t it=0;it<_nrIter;it++){
			for(size_t c=0;c<_colls.size();c++){
				size_t iC=_order[c];
				Scalar NX=-_v0[iC];
				Scalar M=inf();
				for(size_t off=_rowOff[iC];off<_rowOff[iC+1];off++){
					const Entry& e=_entries[off];
					if(e.first.second == iC)M=e.second;
					else NX-=e.second*_impulses[e.first.second];
				}
				assert(M != inf());
				if(M > 0.0f)
				_impulses[iC]=std::max<Scalar>(0.0f,NX/M);
			}
			std::random_shuffle(_order.begin(),_order.end());
		}
	}
	void resolveT(){
		for(size_t i=0;i<_colls.size();i++){
			const Collide& C=_colls[i];
			Scalar v0=C.relVelT();
			Scalar inf=C._A->infCoef(C._ptAL, C._tBG, C._ptAL, C._tBG)+
					   C._B->infCoef(C._ptBL,-C._tBG, C._ptBL,-C._tBG);
			if(std::abs(inf) > eps()){
				Scalar I=-v0/inf;
				Scalar maxI=std::abs(_impulses[i])*std::max(C._A->fri(),C._B->fri());
				if(std::abs(I) > maxI)I*=maxI/std::abs(I);
				C._A->impulse(I*C._tBG,C._ptAL+C._A->pos().block<2,1>(0,0));
				C._B->impulse(-I*C._tBG,C._ptBL+C._B->pos().block<2,1>(0,0));
			}
		}
	}
	void debugProb(bool rand=true){
		if(rand)_impulses.setRandom();
		else _impulses.setZero();
		Vec vNew=_v0;
		mul(_impulses,vNew);
		vNew+=_v0;

		applyImpulse();
		for(size_t i=0;i<_colls.size();i++)
			std::cout << vNew[i] << " " << _colls[i].relVel() << std::endl;
	}
	static bool LSSE(const Entry& a,const Entry& b){
		return a.first.first < b.first.first || (a.first.first == b.first.first && a.first.second < b.first.second);
	}
protected:
	void buildMatrix(){
		std::sort(_entries.begin(),_entries.end(),LSSE);
		_rowOff.resize(_colls.size()+1);
		_rowOff[0]=0;
		size_t r=1;
		for(size_t i=0;i<_entries.size();i++)
			while(r <= _entries[i].first.first)
			_rowOff[r++]=i;
		while(r<=_colls.size())
			_rowOff[r++]=_entries.size();
	}
	void applyImpulse(){
		for(size_t i=0;i<_colls.size();i++){
			_colls[i]._A->impulse( _impulses[i]*_colls[i]._nBG,_colls[i]._ptAL+_colls[i]._A->pos().block<2,1>(0,0));
			_colls[i]._B->impulse(-_impulses[i]*_colls[i]._nBG,_colls[i]._ptBL+_colls[i]._B->pos().block<2,1>(0,0));
		}
	}
	void mul(const Vec& x,Vec& b) const{
		b.setZero();
		for(size_t i=0;i<_entries.size();i++)
			b[_entries[i].first.first]+=_entries[i].second*x[_entries[i].first.second];
	}
	std::vector<std::vector<size_t> > _mats;
	std::vector<Entry> _entries;
	std::vector<size_t> _rowOff;
	std::vector<size_t> _order;
	Vec _v0,_impulses,_g;
	size_t _nrIter;
	Scalar _cntThres;
};

}

#endif           

程式就一個檔案，别忘鍊Eigen庫，然後是例子程式。

#include "RigidSolver2D.h"
#include <ctime>

using namespace RIGID_SOLVER_2D;

Solver sol(0.05f,0.005f,200);
Scalar dt=1.0f/60.0f;
clock_t curr=clock();

#define KEY_ESCAPE 27
typedef struct {
    int width;
	int height;
	char* title;
	float field_of_view_angle;
	float z_near;
	float z_far;
} glutWindow;
glutWindow win;

void display()
{
	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();
	//draw shape
	glLineWidth(3.0f);
	sol.draw(0.0f,false,true);
	glutSwapBuffers();
}
void idle(){
	clock_t newT=clock();
	if(newT-curr > dt*100.0f){
		curr=newT;
		sol.advance(dt,dt*0.1f);
	}
	display();
}
void initialize()
{
    glMatrixMode(GL_PROJECTION);
    glViewport(0,0,win.width,win.height);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
	gluOrtho2D(0.0f,1.0f,0.0f,1.0f);
    glMatrixMode(GL_MODELVIEW);
    glShadeModel(GL_SMOOTH);
    glClearDepth(1.0f);
    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LEQUAL);
    glHint(GL_PERSPECTIVE_CORRECTION_HINT,GL_NICEST);
	glClearColor(0.0f,0.0f,0.0f,1.0f);
}
void initializeSolver1(){
	//clear
	sol.clear();
	
	//add plane
	ConvexShape plane;
	plane.cut(Vec3(0.0f,1.0f,-0.05f));
	ConvexShape* p=new ConvexShape;*p=plane;
	Body* bp=new Body(p);
	bp->setRes(0.0f);
	bp->setFri(0.9f);
	sol.add(bp);

	//add ball
	ConvexShape box;
	box.cut(Vec3( 0.2f, 0.0f,-0.01f));
	box.cut(Vec3( 0.0f, 0.2f,-0.002f));
	box.cut(Vec3(-0.2f, 0.0f,-0.01f));
	box.cut(Vec3( 0.0f,-0.2f,-0.002f));
	//box.cut(Vec3( 0.2f, 0.2f,-0.015f));
	//box.cut(Vec3(-0.2f, 0.2f,-0.015f));
	//box.cut(Vec3(-0.2f,-0.2f,-0.015f));
	//box.cut(Vec3( 0.2f,-0.2f,-0.015f));
	p=new ConvexShape;*p=box;
	bp=new Body(p);
	bp->pos()=Vec3(0.5f,0.95f,M_PI/6.0f);
	sol.add(bp);

	p=new ConvexShape;*p=box;
	bp=new Body(p);
	bp->pos()=Vec3(0.5f,0.75f,M_PI/6.0f);
	sol.add(bp);
	
	//gravity
	sol.setG(Vec2(0.0f,-1.0f));
}
void initializeSolver2(){
	//clear
	sol.clear();
	
	//add plane
	ConvexShape plane;
	plane.cut(Vec3(0.0f,1.0f,-0.05f));
	ConvexShape* p=new ConvexShape;*p=plane;
	Body* bp=new Body(p);
	bp->setRes(0.0f);
	sol.add(bp);

	ConvexShape plane2;
	plane2.cut(Vec3(1.0f,0.0f,-0.05f));
	p=new ConvexShape;*p=plane2;
	bp=new Body(p);
	sol.add(bp);

	ConvexShape plane3;
	plane3.cut(Vec3(-1.0f,0.0f,0.95f));
	p=new ConvexShape;*p=plane3;
	bp=new Body(p);
	sol.add(bp);

	//add ball
	ConvexShape box;
	box.cut(Vec3( 0.2f, 0.0f,-0.01f));
	box.cut(Vec3( 0.0f, 0.2f,-0.01f));
	box.cut(Vec3(-0.2f, 0.0f,-0.01f));
	box.cut(Vec3( 0.0f,-0.2f,-0.01f));
	//box.cut(Vec3( 0.2f, 0.2f,-0.015f));
	//box.cut(Vec3(-0.2f, 0.2f,-0.015f));
	//box.cut(Vec3(-0.2f,-0.2f,-0.015f));
	//box.cut(Vec3( 0.2f,-0.2f,-0.015f));
	for(Scalar x=0.25f;x<0.75f;x+=0.16f)
	for(Scalar y=0.25f;y<0.75f;y+=0.16f){
		p=new ConvexShape;*p=box;
		bp=new Body(p);
		bp->setFri(0.9f);
		bp->pos()=Vec3(x,y,M_PI*rand()/(Scalar)RAND_MAX);
		sol.add(bp);
	}

	//add complex shape
	ConcaveShape boxes;
	ConvexShape* bb;
	bb=new ConvexShape;*bb=box;
	boxes.add(bb,Vec3(0.0f,0.0f,M_PI*rand()/(Scalar)RAND_MAX));
	bb=new ConvexShape;*bb=box;
	boxes.add(bb,Vec3(0.1f,0.0f,M_PI*rand()/(Scalar)RAND_MAX));
	bb=new ConvexShape;*bb=box;
	boxes.add(bb,Vec3(0.05f,0.1f,M_PI*rand()/(Scalar)RAND_MAX));
	ConcaveShape* cp=new ConcaveShape;*cp=boxes;
	bp=new Body(cp);
	bp->pos()=Vec3(0.65f,0.85f,M_PI*rand()/(Scalar)RAND_MAX);
	sol.add(bp);
	
	//gravity
	sol.setG(Vec2(0.0f,-1.0f));
}
void initializeSolver3(){
	//clear
	sol.clear();
	
	//add plane
	ConvexShape plane;
	plane.cut(Vec3(0.0f,1.0f,-0.05f));
	ConvexShape* p=new ConvexShape;*p=plane;
	Body* bp=new Body(p);
	bp->setRes(0.0f);
	bp->setFri(0.9f);
	sol.add(bp);

	//add ball
	ConvexShape box;
	box.cut(Vec3( 0.2f, 0.0f,-0.002f));
	box.cut(Vec3( 0.0f, 0.2f,-0.05f));
	box.cut(Vec3(-0.2f, 0.0f,-0.002f));
	box.cut(Vec3( 0.0f,-0.2f,-0.05f));
	p=new ConvexShape;*p=box;
	bp=new Body(p);
	bp->pos()=Vec3(0.6f,0.29f,-3.0f);
	sol.add(bp);

	p=new ConvexShape;*p=box;
	bp=new Body(p);
	bp->pos()=Vec3(0.4f,0.29f,+3.0f);
	sol.add(bp);
	
	//gravity
	sol.setG(Vec2(0.0f,-1.0f));
}
void keyboard(unsigned char key,int mousePositionX,int mousePositionY)		
{ 
	switch(key){
	case '1':
		initializeSolver1();
		break;
	case '2':
		initializeSolver2();
		break;
	case '3':
		initializeSolver3();
		break;
	case KEY_ESCAPE:        
		exit(0);
		break;
	default:
		break;
	}
}
void mouse(int button,int state,int mousePositionX,int mousePositionY){}
void motion(int mousePositionX,int mousePositionY){}
int main(int argc,char **argv) 
{
	win.width=1000;
	win.height=1000;
	win.title="Minimal Rigid Solver";
	win.field_of_view_angle=45;
	win.z_near=1.0f;
	win.z_far=500.0f;

	glutInit(&argc,argv);
	glutInitDisplayMode(GLUT_RGB|GLUT_DOUBLE|GLUT_DEPTH);
	glutInitWindowSize(win.width,win.height);
	glutCreateWindow(win.title);
	glutDisplayFunc(display);
	glutIdleFunc(idle);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
    glutKeyboardFunc(keyboard);
	initialize();
	//initializeSolver1();
	glutMainLoop();
	return 0;
}           

再貼個效果圖。