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