#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; }
再贴个效果图。