写个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{//-------------------------------------------------MATHtypedef 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];}//-------------------------------------------------SHAPEstruct Shape{EIGEN_MAKE_ALIGNED_OPERATOR_NEWShape():_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 notvirtual 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_NEWConvexShape(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 shapestd::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.0fMat2 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 EXTVec4 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 clockwisefor(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 parallelreturn 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 parallelreturn 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 clockwisestd::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_NEWConcaveShape(){_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;};//-------------------------------------------------BODYstruct Body{EIGEN_MAKE_ALIGNED_OPERATOR_NEWBody(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 detectionvoid 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 massvoid 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;};//-------------------------------------------------COLLISIONstruct Collide{EIGEN_MAKE_ALIGNED_OPERATOR_NEWScalar 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_NEWCollision(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);//broadphaseVec2 ptW,ptWB,ptR;//narrowphaseVec3 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 bodiesx0=(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 bodiesfor(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 collisionfor(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 hashif(_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;//bodiesstd::vector<Vec4,Eigen::aligned_allocator<Vec4> > _bbs;std::vector<Collide,Eigen::aligned_allocator<Collide> > _colls;//resultstd::map<int64_t,std::pair<size_t,size_t> > _hashOff;//broadphasestd::vector<std::pair<int64_t,size_t> > _hash;std::vector<size_t> _infs;std::map<int64_t,Scalar> _TOICache;//narrowphase};//-------------------------------------------------SOLVERclass Solver : public Collision{public:typedef std::pair<size_t,size_t> Coord;typedef std::pair<Coord,Scalar> Entry;EIGEN_MAKE_ALIGNED_OPERATOR_NEWSolver(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: collisiondetect(delta);//detect collisionresolveN(inf(),true);//resolve normal collisionapplyImpulse();//apply impulsefor(size_t i=0;i<_bodies.size();i++)//advance position_bodies[i]->advancePos(delta);//pass 2: contactresolveT();//resolve tangential frictionresolveN(0.0f,false);//resolve normal collisionapplyImpulse();//apply impulset+=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 27typedef 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 shapeglLineWidth(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(){//clearsol.clear();//add planeConvexShape 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 ballConvexShape 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);//gravitysol.setG(Vec2(0.0f,-1.0f));}void initializeSolver2(){//clearsol.clear();//add planeConvexShape 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 ballConvexShape 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 shapeConcaveShape 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);//gravitysol.setG(Vec2(0.0f,-1.0f));}void initializeSolver3(){//clearsol.clear();//add planeConvexShape 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 ballConvexShape 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);//gravitysol.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;}

再贴个效果图。