// a minimal 3d rigid-body simulator; // supports only boxes as objects // collisions are not treated efficiently; // heavy use of vector and matrix classes; to get reasonable performace, // compile with optimizations and inlining on (in VC, "release" mode) #define GLUT_API_VERSION 4 #include #include #include #ifdef _WIN32 #include #endif #include #include #include #include #ifndef M_PI #define M_PI 3.14159265358979323846 #endif #include #include "cvec2t.h" #include "cvec3t.h" #include "cvec4t.h" #include "hmatrix.h" typedef CVec2T Vec2f; typedef CVec3T Vec3f; typedef CVec3T Vec3i; typedef CVec4T Vec4f; typedef HMatrix HMatrixf; namespace WindowParams { static int WindowWidth = 800; static int WindowHeight = 600; static int MainWindow; }; namespace Params { const float WorldWidth = 1.33; const float WorldHeight = 1; const Vec3f BackgroundColor(0.5,0.5,0.5); const Vec3f Gravity(0,-0.1,0); const float Dumping = 0.005; // velocity and angular velocity are scaled by 1-Dumping const int SubSteps = 1; // number of substeps of physics calculations per frame const float CollisionTol = 0.01; // collision moment is resolved to this accuracy const float MaxSimStep = 0.1; // maximal step taken by simulator const float Restitution = 0.8; // restitution for collision; the magnitude of // the normal component of the relative velocity is multiplied by this }; float CurrentTime; // structure to store collision information: object numbers, collision point and normal vector class RigidObject; class Box; struct Collision { enum ColType { CEDGE,CVERTEX}; const RigidObject* A; const RigidObject* B; Vec3f p, n; Collision(const RigidObject* a, const RigidObject* b, const Vec3f& pc, const Vec3f& nc, const ColType tc): A(a), B(b), p(pc), n(nc), ctype(tc) {} ColType ctype; }; class RigidObject { public: // if other derived object types are created, entries should be added to this type, along with // appropriate collision functions enum ObjectType { OBJ_BOX }; float energy() const { return 0.5*( _velocity.dot()*_mass + ( _rotation*inertia()*_rotation.transpose()*Vec4f(_ang_velocity,0)).dot(Vec4f(_ang_velocity,0))); } // update for continuous motion virtual update(float dt, const Vec3f& force, const Vec3f& torque) { if(_movable) { _position += _velocity*dt; _velocity += force/_mass*dt; _ang_velocity += _rotation * _invinertia*_rotation.transpose()*Vec4f(torque,0)*dt; if(_ang_velocity.l2() >= FLT_EPSILON ) { _rotation = HMatrixf::Rotation(_ang_velocity.l2()*dt,_ang_velocity.dir() )*_rotation; _rotation.orthonormalize(); } } } // update for collisions virtual impulse_update(const Vec3f& impulse, const Vec3f& point) { if(_movable) { _velocity += impulse/_mass; _ang_velocity = _ang_velocity + _rotation*_invinertia*_rotation.transpose()*Vec4f(cross(point-_position,impulse),0); } } float mass() const { return _mass; } HMatrixf inertia() const { return _inertia; } const HMatrixf& invinertia() const { return _invinertia; } Vec3f position() const { return _position;} Vec3f velocity() const { return _velocity; } const HMatrixf& rotation() const { return _rotation; } Vec3f ang_velocity() const { return _ang_velocity;} bool movable() const { return _movable; } ObjectType type() const { return _type;} // velocity of a point p in the object, computed from velocity of c.m. and angular velocity Vec3f point_velocity(const Vec3f& p) const { Vec3f r = p - _position; return _velocity+ cross(_ang_velocity,r); } // abstract draw function, to force all derived classes to have it virtual void draw() = 0; // general collision dispatch virtual void collide(const RigidObject& ob, vector& collisions) const { // other object types can be added here switch( ob.type()) { case OBJ_BOX: collide( (const Box&)(ob), collisions); break; } } // abstract collision function used when the other object is also Box, to force derived classes to have it // if other object types are added specific functions like this need to be defined virtual void collide(const Box& box, vector& collisions) const =0; protected: float _mass; HMatrixf _inertia; HMatrixf _invinertia; // inverse inertia tensor in object coordinates Vec3f _position; Vec3f _velocity; HMatrixf _rotation; // rotation from axis-aligned orientation to current orientation Vec3f _ang_velocity; bool _movable; // is the object fixed in its initial position? useful for making static objects ObjectType _type; // object type, for collision dispatch }; // compute impulse for collision of two rigid objects (Restitution = 1 is perfectly elastic) Vec3f collisionImpulse(const RigidObject& oA, const RigidObject& oB, const Vec3f& p, const Vec3f& n) { Vec3f vA = oA.point_velocity(p); Vec3f vB = oB.point_velocity(p); Vec3f rA = p-oA.position(); Vec3f rB = p-oB.position(); Vec3f j = -(1+Params::Restitution)*(vA - vB).dot(n)*n/ ( 1/oA.mass() + 1/oB.mass() + n.dot(cross(Vec3f(oA.rotation()*oA.invinertia()*oA.rotation().transpose()*Vec4f(cross(rA,n),0)),rA)) + n.dot(cross(Vec3f(oB.rotation()*oB.invinertia()*oB.rotation().transpose()*Vec4f(cross(rB,n),0)),rB)) ); return j; } class Box: public RigidObject { public: Box ( const Vec3f& sz, // dimensions float m, // mass bool ismovable = true, // is the object allowed to move const Vec3f& init_position=Vec3f(0,0,0), const Vec3f& init_velocity=Vec3f(0,0,0), const HMatrixf init_rotation = HMatrixf::Identity(), const Vec3f& init_ang_velocity = Vec3f(0,0,0) ) { _mass = m; _size = sz; _position = init_position; _velocity = init_velocity; _rotation = init_rotation; _ang_velocity = init_ang_velocity; // store both inertia and its inverse _inertia.setIdentity(); _inertia(0,0) = (1/12.0)*(_size.y()*_size.y() + _size.z()*_size.z())*_mass; _inertia(1,1) = (1/12.0)*(_size.x()*_size.x() + _size.z()*_size.z())*_mass; _inertia(2,2) = (1/12.0)*(_size.x()*_size.x() + _size.y()*_size.y())*_mass; _invinertia.setInverse(_inertia); _movable = ismovable; _type = OBJ_BOX; } virtual void draw() { glPushMatrix(); glTranslatef(_position.x(),_position.y(),_position.z()); glMultMatrixf(_rotation); glPolygonMode(GL_FRONT_AND_BACK,GL_FILL); glScalef(_size.x(),_size.y(),_size.z()); glutSolidCube(1.0); glPopMatrix(); } // convert from world coordinates to the local coordinate system with origin at the center of the box // and axes aligned with the axes of the box Vec3f toLocal(const Vec3f& pworld) const { return _rotation.transpose()*Vec4f(pworld - _position,0); } // convert from local coordinates to world Vec3f toWorld(const Vec3f& plocal) const { return Vec3f(_rotation * Vec4f(plocal,0)) + _position; } // conversion for vectors (no translation applied) Vec3f toLocalVec(const Vec3f& pworld) const { return _rotation.transpose() * Vec4f(pworld,0); } // conversion for vectors (no translation applied) Vec3f toWorldVec(const Vec3f& plocal) const { return _rotation * Vec4f(plocal,0); } // the next five functions implement a simplistic algortihm for finding collision points // of two boxes; it was optimized for length of code first, then for robustness, and not much for speed // the most obvious improvement is to add overlap test for projections on 6 box axes, or the full 15-axis overlap test // (6 box axes + 9 cross products of box axes) to detect absence of collision early // check if point p is inside the box; if yes return the normal of the face to which it is close bool collidePoint(const Vec3f& p, Vec3f& normal) const { Vec3f plocal = toLocal(p); // check if there is a collision, i.e. p's coordinates in box's local cood system are in the right range bool collide = fabs(plocal.x()) <= 0.5*_size.x() && fabs(plocal.y()) <= 0.5*_size.y() && fabs(plocal.z()) <= 0.5*_size.z(); bool collide2d = fabs(plocal.x()) <= 0.5*_size.x() && fabs(plocal.y()) <= 0.5*_size.y(); if(collide) { float d[6]; // list of outward facing normals to sides static const Vec3f facenormal[6] = {Vec3f(-1,0,0),Vec3f(1,0,0), Vec3f(0,-1,0),Vec3f(0,1,0), Vec3f(0,0,-1),Vec3f(0,0,1)}; // distances to box sides d[0] = plocal.x()+0.5*_size.x(); d[1] = -plocal.x()+0.5*_size.x(); d[2] = plocal.y()+0.5*_size.y(); d[3] = -plocal.y()+0.5*_size.y(); d[4] = plocal.z()+0.5*_size.z(); d[5] = -plocal.z()+0.5*_size.z(); // find closest side int mindno = -1; float mind = FLT_MAX; for(int i = 0; i < 6; i++) { if (mind > d[i]) {mindno=i; mind = d[i]; } } // assign collision normal normal = toWorldVec(facenormal[mindno]); } return collide; } // check if edge intersects a face; if yes, find the closest point on the edge to // the nearest edge of the face // if one of the endpoints of the edge are inside the box, return no intersection bool collideEdge(const Vec3f& p1, const Vec3f& p2, Vec3f& pos, Vec3f& normal) const { // if an enpoint is a collision point, ignore Vec3f p1local = toLocal(p1); if( fabs(p1local.x()) <= 0.5*_size.x() && fabs(p1local.y()) <= 0.5*_size.y() && fabs(p1local.z()) <= 0.5*_size.z()) return false; Vec3f p2local = toLocal(p2); if( fabs(p2local.x()) <= 0.5*_size.x() && fabs(p2local.y()) <= 0.5*_size.y() && fabs(p2local.z()) <= 0.5*_size.z()) return false; Vec3f e1,e2; int i,s; bool collide = false; Vec3f pinter; // step 1: test for intersections with all faces for( i = 0; i < 3; i++) { for(s = -1; s <= 1; s += 2) { // are endpoints on different sides? if( (p1local(i) - 0.5*s*_size(i))*(p2local(i) - 0.5*s*_size(i)) < -FLT_EPSILON) { // line intersection point float tinter = (0.5*s*_size(i)-p1local(i))/(p2local(i)-p1local(i)); pinter = p2local*tinter + (1-tinter)*p1local; // is it in the interior? if( fabs(pinter( (i + 1) % 3)) < 0.5*_size( (i + 1) % 3 ) && fabs(pinter( (i + 2) % 3)) < 0.5*_size( (i + 2) % 3 )) { collide = true; break; } } } if(collide) break; } if(!collide) return false; // step 2: find closest edge of the intersected face float d[] = { pinter( (i + 1) % 3) + 0.5*_size( (i + 1) % 3 ), -pinter( (i + 1) % 3) + 0.5*_size( (i + 1) % 3 ), pinter( (i + 2) % 3) + 0.5*_size( (i + 2) % 3 ), -pinter( (i + 2) % 3) + 0.5*_size( (i + 2) % 3 )}; int mindno = -1; float mind = FLT_MAX; for(int j = 0; j < 4; j++) { if (mind > d[j]) {mindno=j; mind = d[j]; } } static const float e1coords[4][2] = { {-1,-1}, {1,-1}, {-1,-1}, {-1,1} }; static const float e2coords[4][2] = { {-1, 1}, {1, 1}, { 1,-1}, { 1,1} }; // closest edge endpoints coords e1 = 0.5*_size; e2 = 0.5*_size; e1(i) *= s; e1( (i + 1) % 3) *= e1coords[mindno][0]; e1( (i + 2) % 3) *= e1coords[mindno][1]; e2(i) *= s; e2( (i + 1) % 3) *= e2coords[mindno][0]; e2( (i + 2) % 3) *= e2coords[mindno][1]; // step 3: find the closest point on p1 p2 // normal is perpendicular to both edges normal = cross(e2-e1,p2local-p1local); // should point towards the exterior if(e1.dot(normal) < 0) normal = -normal; // point on p1-p2 closest to the line containing e1-e2 float tp = (e1-p1local).dot(cross(normal,e2-e1))/(p2local-p1local).dot(cross(normal,e2-e1)); tp = max(0.0f,min(1.0f,tp)); pos = p1local + tp*(p2local-p1local); pos = toWorld(pos); normal = toWorldVec(normal.dir()); return true; } // fina all corners of this box which are inside another box Box void collideAllCorners(const Box& box, vector& collisions) const { Vec3f corner, normal; int s1,s2,s3; for(s1 = -1; s1 <= 1; s1 += 2) for(s2 = -1; s2 <= 1; s2 += 2) for(s3 = -1; s3 <= 1; s3 += 2) { corner = toWorld(0.5*Vec3f(s1*_size.x(),s2*_size.y(), s3*_size.z())); if( box.collidePoint(corner,normal)) collisions.push_back(Collision(this,&box,corner,normal,Collision::CVERTEX)); } } // find all edges of this box which are pass through another box Box void collideAllEdges(const Box& box,vector& collisions) const { int i,j,si,sj; Vec3f pos, normal; for( i = 0; i < 3; i++) for( j = i+1; j < 3; j++) for( si = -1; si <= 1; si +=2 ) for( sj = -1; sj <= 1; sj +=2 ) { Vec3f p1=0.5*_size; Vec3f p2=0.5*_size; int last_coord = 3 - i - j; p1(i) *= si; p2(i) *= si; p1(j) *= sj; p2(j) *= sj; p1(last_coord) *=-1; if( box.collideEdge(toWorld(p1),toWorld(p2),pos,normal) ) { collisions.push_back(Collision(this,&box,pos,normal,Collision::CEDGE)); } } } virtual void collide(const Box& box, vector& collisions) const { // vertex-face collisions collideAllCorners(box,collisions); box.collideAllCorners(*this,collisions); // edge-edge collisions are reciprocal, no need to check both ways collideAllEdges(box,collisions); } Vec3f size() const { return _size;} private: Vec3f _size; }; void Reshape(int width, int height) { WindowParams::WindowWidth = width; WindowParams::WindowHeight = height; glViewport(0,0,width,height); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(20,1.33,1,30); } class Game { public: // list of all objects; can have different types derived from RigidObject vector objects; // collision list vector collisions; // this is used to visualize collisions only vector last_collisions; Game() { // create 4 boxes for(int i = 1; i <= 4; i++) { objects.push_back(new Box( Vec3f(0.1*(i+1),0.09,0.05*(i+1)), // dimensions i+1, // mass true, // does this box move? Vec3f(0,-0.4+0.05+0.1*(4-i),0) //initial position )); } // a small heavy box moving horizontally objects.push_back(new Box( Vec3f(0.05,0.05,0.05), // dimensions 10, // mass true, // does this box move? Vec3f(1,-0.3,0), //initial position Vec3f(-1,0,0) // initial velocity )); // very heavy box (one can also make an "infinite mass" box by setting the inverse mass to 0) objects.push_back(new Box(Vec3f(0.8,0.1,0.8),1e6,false, Vec3f(0,-0.45,0))); } ~Game() { for(int i = 0; i < objects.size(); i++) { delete objects[i]; } } void draw() { Vec3f col[] = {Vec3f(1,0,0),Vec3f(1,0.25,0),Vec3f(1,0.5,0),Vec3f(1,0.75,0),Vec3f(1,1,0)}; for(int i = 0; i < objects.size(); i++) { glColor3fv(col[i% 5]); objects[i]->draw(); } /* show collision points and normals glDisable(GL_LIGHTING); glDisable(GL_DEPTH_TEST); glColor3f(1,1,1); glLineWidth(3); glBegin(GL_LINES); for(int j = 0; j < last_collisions.size(); j++) { if(last_collisions[j].ctype == Collision::CVERTEX) glColor3f(0,1,0); else if(last_collisions[j].ctype == Collision::CEDGE) glColor3f(0,0,1); glVertex3fv(last_collisions[j].p); glVertex3fv(last_collisions[j].p+ 0.2*last_collisions[j].n); } glEnd(); */ } // find all intersecting objects A,B // store the list in collisions // this function checks all objects pairwise; // a considerable speedup can be obtained if only selected tests are performed // if certainl collisions can be safely ignored // a better way to do this is to use a spatial structure (a hierarchy or a grid) // to eliminate tests for remote objects void detectCollisions() { Vec3f p,n; collisions.clear(); // for all pairs of objects for(int i = 0; i < objects.size(); i++) { for(int j = i+1; j < objects.size(); j++) { objects[i]->collide(*(objects[j]),collisions); } } } // update all object positions; if there are forces other than gravity and dumping // they should be added here void update(float dt) { for(int i = 0; i < objects.size()-1; i++) { objects[i]->update(dt, objects[i]->mass()*(Params::Gravity -Params::Dumping*objects[i]->velocity()), objects[i]->inertia()*Vec4f((-Params::Dumping)*objects[i]->ang_velocity(),0) ); } } // advance simulation; check for collisions // and if there are any (there may be multipel per time step), try to resolve them by applying impulses. // Note that applying impulses in principle can create more // collisions so iterate the process // there is no guarantee that this iteration conerges // we do it a fixed number of times // if collisions are not resolved the effect // will be that object will fly through another with a chance // of getting stuck void processCollisions(float dt) { int i; int iter = 0; bool collision_found = false; do { collision_found = false; iter++; // go through the list of collisions and apply impulses // this does not remove overlaps, but makes them fly avay from each other for(int k = 0; k < collisions.size(); k++) { if( (collisions[k].A->point_velocity(collisions[k].p)- collisions[k].B->point_velocity(collisions[k].p) ).dot(collisions[k].n) <= 0 ){ collision_found = true; Vec3f j = collisionImpulse( *(collisions[k].A),*(collisions[k].B), collisions[k].p, collisions[k].n); ((RigidObject*)(collisions[k].A))->impulse_update(j,collisions[k].p); ((RigidObject*)(collisions[k].B))->impulse_update(-j,collisions[k].p); } } // check for collisions again; if now all objects are flying away from each other // there will be no collisions; if changing velocities made some objects collide, // this will force another iteration } while ( iter < 4 && collision_found ); if(collision_found) cout << "could not resolve collisions!!!" << endl; } // advance simulation time; if collisions are detected, refine time step to the specified accuracy void advance(float dt) { dt = dt/float(Params::SubSteps); for( int i = 0; i < Params::SubSteps; i++) { float t1 = 0; bool isfirst = true; while(t1 < dt) { float step = dt-t1; update(step); t1 += step; detectCollisions(); if(isfirst){ last_collisions = collisions; isfirst = false;} if(collisions.size() > 0) { update(-step); t1 -= step; // binary search for more precise collision time while(step > Params::CollisionTol) { step *= 0.5; update(step); t1 += step; detectCollisions(); if(collisions.size() > 0) { update(-step); t1 -= step; } } update(step); detectCollisions(); update(-step); processCollisions(step); update(step); t1 += step; } } } } }; Game* game; // gets called when a key is pressed down void Keyboard( unsigned char key, int, int) { } void Draw() { glClearColor( 0.5, 0.5,0.5,0.0); glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); // enable automatic rescaling of normals to unit length glEnable(GL_NORMALIZE); // enable two lights glEnable(GL_LIGHT0); glEnable(GL_LIGHT1); // directional lights (w=0) along z axis // flip normals for polygons facing away from the screen // this ensures the back facing polygins are lit glLightModelf(GL_LIGHT_MODEL_TWO_SIDE,1); glEnable(GL_LIGHTING); glEnable(GL_COLOR_MATERIAL); glColorMaterial(GL_FRONT_AND_BACK,GL_DIFFUSE); glLightfv(GL_LIGHT0,GL_DIFFUSE,Vec4f(1, 1, 1,1)); glLightfv(GL_LIGHT0,GL_POSITION, Vec4f(3, 2, 3,1)); glLightfv(GL_LIGHT1,GL_DIFFUSE,Vec4f(1, 1, 1,1)); glLightfv(GL_LIGHT1,GL_POSITION, Vec4f(3, -2, 3,1)); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(4,4,4, 0,0,0, 0,1,0); game->draw(); glutSwapBuffers(); } void AnimateVariable() { // allow up to 4 times sim step between screen updates float dt = min(4*Params::MaxSimStep, clock()/float(0.5*CLOCKS_PER_SEC)-CurrentTime); CurrentTime +=dt; float t = 0; while( t + Params::MaxSimStep < dt) { game->advance(Params::MaxSimStep); t += Params::MaxSimStep; } game->advance(dt-t); glutPostRedisplay(); } int main(int argc, char* argv[]) { glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGBA|GLUT_DOUBLE|GLUT_DEPTH); glutInitWindowSize(2*WindowParams::WindowWidth,2*WindowParams::WindowHeight); WindowParams::MainWindow = glutCreateWindow("Physics2D"); // register all callbacks glutDisplayFunc(Draw); glutReshapeFunc(Reshape); glutKeyboardFunc(Keyboard); CurrentTime = clock(); glutIdleFunc(AnimateVariable); game = new Game; glutMainLoop(); return 0; }