class CObstacle2{
public:
	char type;
	void rebound( ogVector2 &position, ogVector2 &velocity, ogLine2 &plane, double restitutionCoefficient, double frictionCoefficient ){
		double depth = plane.signedDistance( position );
		double k = velocity * plane.normal;
		if( depth < 0.0 && k < 0.0 ){
			ogVector2 velocityX, velocityY, velocityX2;
			velocityY.projection( velocity, plane.normal );
			velocityX.subtract( velocity, velocityY );
			double dv = frictionCoefficient * (1+restitutionCoefficient) * velocityY.abs();
			if(velocityX.abs() > dv ) velocity = velocityX * (1.0 - dv/velocityX.abs() );
			else velocity.zero();
			ogVector2 v;
			velocity -= (v = restitutionCoefficient * velocityY);
			position += (v = -depth*(1.0+restitutionCoefficient) * plane.normal);
		//	ogVector2 invasion = ( depth/k ) * velocity;
		//	invasion -= (v = depth * plane.normal);
		// simple way for debug
		//	if( k < 0.0 ){
		//		velocity.y = 0.0;
		//		position -= depth * plane.normal;
		//	}
		}
	}
	virtual void rebound( ogVector2 &position, ogVector2 &velocity, double restitutionCoefficient, double frictionCoefficient )=0;
	virtual void draw( unsigned char red, unsigned char green, unsigned char blue ){ }
	virtual void draw(){ }
	virtual void move( ogVector2 &center ){ }
	virtual void rebound( class CRigidRoom2 *room ){ }
};
class CObstacle{
public:
	char type;
	void rebound( ogVector &position, ogVector &velocity, ogPlane &plane, double restitutionCoefficient, double frictionCoefficient ){
		double depth = plane.signedDistance( position );
		double k = velocity * plane.normal;
		if( depth < 0.0 && k < 0.0 ){
			ogVector velocityX, velocityY, velocityX2;
			velocityY.projection( velocity, plane.normal );
			velocityX.subtract( velocity, velocityY );
			double dv = frictionCoefficient * (1+restitutionCoefficient) * velocityY.abs();
			if(velocityX.abs() > dv ) velocity = velocityX * (1.0 - dv/velocityX.abs() );
			else velocity.zero();
			ogVector v;
			velocity -= (v = restitutionCoefficient * velocityY);
			position += (v = -depth*(1.0+restitutionCoefficient) * plane.normal);
		}
	}
	virtual void rebound( ogVector &position, ogVector &velocity, double restitutionCoefficient, double frictionCoefficient )=0;
	virtual void draw( unsigned char red, unsigned char green, unsigned char blue ){ }
	virtual void draw(){ }
	virtual void move( ogVector &center ){ }
	virtual void move( ogVector2 &center ){ }
	virtual void rebound( class CRigidRoom *room ){ }
};

class CRigidRoom2:public CObstacle2{
public:
	ogVector2 min;
	ogVector2 max;
	ogLine2 floor[4];

	CRigidRoom2(){ type = 'r'; }
	CRigidRoom2( double minX, double minY, double maxX, double maxY ){ this->construct( minX, minY, maxX, maxY ); type = 'r'; }
	void construct( double minX, double minY, double maxX, double maxY ){
		min.x = minX; min.y = minY; max.x = maxX; max.y = maxY;
		floor[0].set(  1,  0, minX, 0 );
		floor[1].set( -1,  0, maxX, 0 );
		floor[2].set(  0,  1, 0, minY );
		floor[3].set(  0, -1, 0, maxY );
	}
	void rebound( ogVector2 &position, ogVector2 &velocity, double restitutionCoefficient, double frictionCoefficient ){
		for(int i=0; i<4; i++) CObstacle2::rebound( position, velocity, floor[i], restitutionCoefficient, frictionCoefficient );
	}

};
class CRigidRoom:public CObstacle{
public:
	ogVector min;
	ogVector max;
	ogPlane floor[6];

	CRigidRoom(){ type = 'r'; }
	CRigidRoom( double minX, double minY, double minZ, double maxX, double maxY, double maxZ ){ this->construct( minX, minY, minZ, maxX, maxY, maxZ ); type = 'r'; }
	void construct( double minX, double minY, double minZ, double maxX, double maxY, double maxZ ){
		min.x = minX; min.y = minY; min.z = minZ; max.x = maxX; max.y = maxY; max.z = maxZ;
		floor[0].set(  1,  0,  0, minX, 0, 0 );
		floor[1].set( -1,  0,  0, maxX, 0, 0 );
		floor[2].set(  0,  1,  0, 0, minY, 0 );
		floor[3].set(  0, -1,  0, 0, maxY, 0 );
		floor[4].set(  0,  0,  1, 0, 0, minZ );
		floor[5].set(  0,  0, -1, 0, 0, maxZ );
	}
	void rebound( ogVector &position, ogVector &velocity, double restitutionCoefficient, double frictionCoefficient ){
		for(int i=0; i<6; i++) CObstacle::rebound( position, velocity, floor[i], restitutionCoefficient, frictionCoefficient );
	}

};

class CRigidCylinder2:public CObstacle2{
public:
	ogVector2 center;
	double   radius;
	double   radiusXradius;

	CRigidCylinder2(){ type = 'c'; }
	CRigidCylinder2( double centerX, double centerY, double radius ){ this->construct( centerX, centerY, radius ); type = 'c'; }
	void construct( double centerX, double centerY, double radius ){ center.x = centerX; center.y = centerY; this->radius = radius; radiusXradius = radius*radius; }
	int contactPlane( ogLine2 &plane, ogVector2 &position ){
		if( (position.x -center.x)*(position.x -center.x) + (position.y -center.y)*(position.y -center.y) < radius*radius ){
			ogVector2 n( position.x-center.x, position.y-center.y );
			n.normalize();
			plane.set( n.x, n.y, -n.x*(center.x+n.x*radius)-n.y*(center.y+n.y*radius) );
			return 1;
		}else{
			return 0;
		}
	}
	void draw( unsigned char red, unsigned char green, unsigned char blue ){ og.DrawCircle( center.x, center.y, radius, red, green, blue ); }
	void draw(){ this->draw(255,255,255); }
	void rebound( ogVector2 &position, ogVector2 &velocity, double restitutionCoefficient, double frictionCoefficient ){
		ogLine2 plane;
		if( this->contactPlane( plane, position ) ){
			CObstacle2::rebound( position, velocity, plane, restitutionCoefficient, frictionCoefficient );
		}
	}
	//for manipulator
	void move( ogVector2 &center ){ this->center = center; }
	void move( double x, double y ){ ogVector2 center(x,y); this->move(center); }
	void translate( double dx, double dy ){ this->center.x += dx; this->center.y += dy; }
	void rebound( CRigidRoom2 *room ){
		if( center.x < room->min.x+radius ) center.x = room->min.x+radius;
		if( center.x > room->max.x-radius ) center.x = room->max.x-radius;
		if( center.y < room->min.y+radius ) center.y = room->min.y+radius;
		if( center.y > room->max.y-radius ) center.y = room->max.y-radius;
	}

};
class CRigidCylinder:public CObstacle{
public:
	ogVector center;
	double   radius;
	double   radiusXradius;
	double   length;

	CRigidCylinder(){ type = 'c'; }
	CRigidCylinder( double centerX, double centerY, double centerZ, double radius, double length ){ this->construct( centerX, centerY, centerZ, radius, length ); type = 'c'; }
	void construct( double centerX, double centerY, double centerZ, double radius, double length ){ center.x = centerX; center.y = centerY; center.z = centerZ; this->radius = radius; radiusXradius = radius*radius; this->length = length; }
	int contactPlane( ogPlane &plane, ogVector &position ){
		if( (position.x -center.x)*(position.x -center.x) + (position.z -center.z)*(position.z -center.z) < radius*radius && fabs( position.y-center.y ) < length / 2.0 ){
			ogVector n( position.x-center.x, 0.0, position.z-center.z );
			n.normalize();
			plane.set( n.x, n.y, n.z, -n.x*(center.x+n.x*radius)-n.y*(center.y+n.y*radius)-n.z*(center.z+n.z*radius) );
				return 1;
		}else{
			return 0;
		}
	}
	void draw( unsigned char red, unsigned char green, unsigned char blue ){ og.DrawCylinder( center.x, center.y-length/2, center.z, center.x, center.y+length/2, center.z, radius, red, green, blue ); }
	void draw(){ this->draw(255,255,255); }
	void rebound( ogVector &position, ogVector &velocity, double restitutionCoefficient, double frictionCoefficient ){
		ogPlane plane;
		if( this->contactPlane( plane, position ) ){
			CObstacle::rebound( position, velocity, plane, restitutionCoefficient, frictionCoefficient );
		}
	}
	//for manipulator
	void move( ogVector2 &center ){ this->center.x = center.x; this->center.z = center.y; }
	void move( ogVector &center ){ this->center = center; }
	void move( double x, double y, double z ){ ogVector center(x,y,z); this->move(center); }
	void translate( double dx, double dy, double dz=0.0 ){ this->center.x += dx; this->center.y += dy; this->center.z += dz; }
	void rebound( CRigidRoom *room ){
		if( center.x < room->min.x+radius ) center.x = room->min.x+radius;
		if( center.x > room->max.x-radius ) center.x = room->max.x-radius;
		if( center.y < room->min.y+radius ) center.y = room->min.y+radius;
		if( center.y > room->max.y-radius ) center.y = room->max.y-radius;
		if( center.z < room->min.z+radius ) center.z = room->min.z+radius;
		if( center.z > room->max.z-radius ) center.z = room->max.z-radius;
	}

};
//------------------------------------------------------------------------------------------------
/*class CDualManipulator2{
public:
	CManipulator2 manipulator[2];
	ogVector2 center;
	double   distance;

	CDualManipulator2( double centerX, double centerY, double radius, double distance ){ center.set( centerX, centerY ); this->distance = distance; manipulator[0].construct( centerX-distance/2.0, centerY, radius ); manipulator[1].construct( centerX+distance/2.0, centerY, radius ); }
	void draw(){ for( int i=0; i<2; i++ ) manipulator[i].draw(); }
	void move( ogVector2 &center ){ this->center = center; ogVector2 delta( distance/2.0, 0.0 ); ogVector2 v; manipulator[0].move( v = center - delta ); manipulator[1].move( v = center + delta ); }
	void translate( double dx, double dy ){ this->center.x += dx; this->center.y += dy; ogVector2 delta( distance/2.0, 0.0 ); ogVector2 v; manipulator[0].move( v = center - delta ); manipulator[1].move( v = center + delta ); }
	void widen( double delta ){ distance += delta; if( distance < manipulator[0].radius+manipulator[1].radius ) distance = manipulator[0].radius+manipulator[1].radius; move( center ); }
	void narrow( double delta ){ distance -= delta; if( distance < manipulator[0].radius+manipulator[1].radius ) distance = manipulator[0].radius+manipulator[1].radius; move( center ); }
};

class CDualManipulator{
public:
	CManipulator manipulator[2];
	ogVector center;
	double   distance;

	CDualManipulator( double centerX, double centerY, double centerZ, double radius, double length, double distance ){ center.set( centerX, centerY, centerZ ); this->distance = distance; manipulator[0].construct( centerX-distance/2.0, centerY, centerZ, radius, length ); manipulator[1].construct( centerX+distance/2.0, centerY, centerZ, radius, length ); }
	void draw(){ for( int i=0; i<2; i++ ) manipulator[i].draw(); }
	void translate( double dx, double dy ){ this->center.x += dx; this->center.y += dy; ogVector delta( distance/2.0, 0.0, 0.0 ); ogVector v; manipulator[0].move( v = center - delta ); manipulator[1].move( v = center + delta ); }
	void move( ogVector center ){ this->center = center; ogVector delta( distance/2.0, 0.0, 0.0 ); ogVector v; manipulator[0].move( v = center - delta ); manipulator[1].move( v = center + delta ); }
	void widen( double delta ){ distance += delta; if( distance < manipulator[0].radius+manipulator[1].radius ) distance = manipulator[0].radius+manipulator[1].radius; move( center ); }
	void narrow( double delta ){ distance -= delta; if( distance < manipulator[0].radius+manipulator[1].radius ) distance = manipulator[0].radius+manipulator[1].radius; move( center ); }
};*/