Commit 9d444112 authored by Leon's avatar Leon

First physics baby steps

parent a3bf8ade
......@@ -15,11 +15,13 @@
#include "Scene.hpp"
#include "Vector2D.hpp"
using namespace duodim;
constexpr auto ESC_KEY = 27;
constexpr auto MAX_POLY_COUNT = 32;
duodim::Engine* e = nullptr;
duodim::Scene* s = nullptr;
Engine* e = nullptr;
Scene* s = nullptr;
void checkArgs(int argc, char** argv)
{
......@@ -36,6 +38,7 @@ void checkArgs(int argc, char** argv)
e->toggleSkipCollisions();
}
}
is = NULL;
}
void RenderString(int x, int y, const char* string)
......@@ -60,25 +63,12 @@ void Mouse(int button, int state, int x, int y)
{
case GLUT_LEFT_BUTTON:
{
unsigned int count = (unsigned int)rand(3.f, (float)MAX_POLY_COUNT);
vector<duodim::Vector2D> vertices;
float random = rand(5.f, 10.f);
for (unsigned int i = 0; i < count; ++i)
{
vertices.push_back(duodim::Vector2D(rand(-random, random), rand(-random, random)));
}
duodim::ConvexPolygon* cp = new duodim::ConvexPolygon(vertices);
duodim::Texture* tex = duodim::Texture::red();
duodim::Body* b = new duodim::Body((float) x, (float) y, duodim::Material::wood(), cp, tex);
s->bodies.push_back(b);
s->bodies.push_back(new Body(x, y, Material::wood(), ConvexPolygon::random(MAX_POLY_COUNT), Texture::red(), true));
}
break;
case GLUT_RIGHT_BUTTON:
{
duodim::Circle* cs = new duodim::Circle(rand(1.f, 3.f));
duodim::Texture* tex = duodim::Texture::green();
duodim::Body* c = new duodim::Body((float) x, (float) y, duodim::Material::bouncyBall(), cs, tex);
s->bodies.push_back(c);
s->bodies.push_back(new Body(x, y, Material::bouncyBall(), Circle::random(), Texture::green(), true));
}
break;
}
......@@ -104,9 +94,11 @@ void Keyboard(unsigned char key, int x, int y)
void PhysicsLoop(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
RenderString(1, 2, "DuoDim Engine");
RenderString(1, 2, "DuoDim 2D physics engine");
RenderString(1, 4, "Left click to spawn a polygon");
RenderString(1, 6, "Right click to spawn a circle");
RenderString(1, 8, "\"F\" to pause, SPACE to resume");
RenderString(1, 10, "ESC to exit");
e->iterate();
glutSwapBuffers();
}
......@@ -137,8 +129,8 @@ int initGui(int argc, char** argv)
*/
int main(int argc, char** argv)
{
e = duodim::Engine::getInstance(); // fire up engine
s = duodim::Scene::getInstance();
e = Engine::getInstance(); // fire up engine
s = Scene::getInstance();
s->sceneHeight = 800;
s->sceneWidth = 600;
checkArgs(argc, argv);
......@@ -149,17 +141,8 @@ int main(int argc, char** argv)
if (!e->skipCollisions)
{
duodim::ConvexPolygon* poly = new duodim::ConvexPolygon();
poly->setBox(30.0f, 1.0f);
duodim::Texture* tex = duodim::Texture::blue();
duodim::Body* b = new duodim::Body(40.f, 55.f, duodim::Material::wood(), poly, tex);
b->setStatic();
s->bodies.push_back(b);
/* Circle* cs = new Circle(rand(1.f, 3.f));
Texture ctex = Texture::green();
Body* c = new Body(75.f, 20.f, &Material::bouncyBall(), cs, &ctex);
s->bodies.push_back(c);*/
s->bodies.push_back(new Body(40, 55, Material::wood(), ConvexPolygon::box(30.f, 1.f), Texture::blue(), false));
s->bodies.push_back(new Body(75, 20, Material::bouncyBall(), Circle::random(), Texture::green(), true));
}
srand(1);
......
......@@ -20,13 +20,13 @@ namespace duodim
float angularVelocity = 0.f;
float torque = 0.f;
float mass;
float inertia;
float mass = 0.f;
float inertia = 0.f;
float inv_mass;
float inv_inertia;
float inv_mass = 0.f;
float inv_inertia = 0.f;
Body(float posX, float posY, Material* material, Shape* shape, Texture* texture);
Body(int x, int y, Material* material, Shape* shape, Texture* texture, bool asStatic);
~Body();
bool isStatic();
......
......@@ -6,6 +6,7 @@ namespace duodim
class Circle : public Shape
{
public:
static Circle* random();
float radius;
Circle();
......
......@@ -13,13 +13,13 @@ namespace duodim
public:
vector<Vector2D> contactPoints;
Vector2D normal;
Vector2D tangent;
float penetration = 0.f;
unsigned int contactCount = 0u;
Collision(Body* a, Body* b);
~Collision();
bool solve();
void addContactPosition(const Vector2D p);
void applyImpulse();
bool applyImpulse();
void addContactPosition(Vector2D p);
void correctPositions();
private:
Body* a;
......
......@@ -12,9 +12,13 @@ namespace duodim
class ConvexPolygon : public Shape
{
public:
static ConvexPolygon* random(const int maxPolyCount);
static ConvexPolygon* box(const float hw, const float hh);
vector<Vector2D> vertices;
vector<Vector2D> normals;
ConvexPolygon();
ConvexPolygon(const float x, const float y);
ConvexPolygon(vector<Vector2D> vertices);
~ConvexPolygon();
......
......@@ -17,7 +17,7 @@ inline bool BiasGreaterThan(float a, float b)
return a >= (b * k_biasRelative) + (a * k_biasAbsolute);
}
inline float Clamp(float min, float max, float a)
inline double Clamp(double min, double max, double a)
{
if (a < min)
{
......
......@@ -9,14 +9,14 @@ namespace duodim
class NarrowPhase
{
public:
static void twoCircles(Collision* c, Body* a, Body* b);
static void circlePolygon(Collision* c, Body* a, Body* b);
static void polygonCircle(Collision* c, Body* a, Body* b);
static void twoPolygons(Collision* c, Body* a, Body* b);
static bool twoCircles(Collision* c, Body* a, Body* b);
static bool circlePolygon(Collision* c, Body* a, Body* b);
static bool polygonCircle(Collision* c, Body* a, Body* b);
static bool twoPolygons(Collision* c, Body* a, Body* b);
private:
static float findAxisLeastPenetration(unsigned int* faceIdx, Body* a, ConvexPolygon* cpa, Body* b, ConvexPolygon* cpb);
static float findAxisLeastPenetration(unsigned int* faceIdx, ConvexPolygon* cpa, ConvexPolygon* cpb);
static int clip(Vector2D n, float c, Vector2D* face);
static void findIncidentFace(Vector2D* v, ConvexPolygon* refShape, Body* refBody, ConvexPolygon* incShape, Body* incBody, unsigned int refIdx);
static void findIncidentFace(Vector2D* v, ConvexPolygon* refShape, ConvexPolygon* incShape, unsigned int refIdx);
};
}
#endif
\ No newline at end of file
......@@ -8,11 +8,12 @@ namespace duodim
class Scene
{
public:
std::vector<Collision> contacts = {};
std::vector<Collision*> contacts = {};
std::vector<Body*> bodies = {};
static Scene* getInstance();
const float gravityConst;
const float GRAVITY_CONST;
const unsigned int IMPULSE_ITERATIONS = 10u;
int sceneHeight = 0;
int sceneWidth = 0;
......@@ -21,7 +22,7 @@ namespace duodim
void render();
private:
static Scene* _sceneInstance;
Scene() : gravityConst(9.81f) {}
Scene() : GRAVITY_CONST(9.81f), IMPULSE_ITERATIONS(10u) {}
Scene(const Scene&);
~Scene() {}
Scene* operator=(Scene* o) = delete;
......
......@@ -7,6 +7,8 @@ namespace duodim
class Transform
{
public:
static Transform* atPosition(int x, int y);
Vector2D position;
Matrix2 rotMatrix;
float rotation;
......
......@@ -10,26 +10,47 @@ namespace duodim
static Vector2D min(const Vector2D a, const Vector2D b);
static Vector2D max(const Vector2D a, const Vector2D b);
/**
Returns a Vector2D (1, 1).
*/
static Vector2D one();
/**
Returns a Vector2D (0, -1).
*/
static Vector2D up();
/**
Returns a Vector2D (0, 1).
*/
static Vector2D down();
/**
Returns a Vector2D (-1, 0).
*/
static Vector2D left();
/**
Returns a Vector2D (1, 0).
*/
static Vector2D right();
/**
Returns a Vector2D (0., 9.81).
*/
static Vector2D gravity();
Vector2D(const double x, const double y);
Vector2D(const float x, const float y);
Vector2D(const int x, const int y);
Vector2D(const Vector2D& a);
Vector2D();
~Vector2D();
Vector2D crossMult(const float a);
Vector2D iCrossMult(const float a);
Vector2D normalized();
Vector2D operator +(const Vector2D b);
Vector2D operator -(const Vector2D b);
Vector2D operator *(const float a);
Vector2D operator /(const float a);
Vector2D operator +=(const Vector2D b);
Vector2D operator -=(const Vector2D b);
Vector2D operator *=(const float a);
Vector2D operator /=(const float a);
Vector2D crossMult(const float a);
Vector2D iCrossMult(const float a);
Vector2D normalized();
Vector2D operator -();
float operator *(const Vector2D b);
......@@ -37,7 +58,12 @@ namespace duodim
float squaredMagnitude();
float crossMult(const Vector2D b);
bool operator ==(Vector2D b);
bool operator ==(const Vector2D b);
void operator +=(const Vector2D b);
void operator -=(const Vector2D b);
void operator *=(const float a);
void operator /=(const float a);
};
}
#endif
\ No newline at end of file
......@@ -37,11 +37,11 @@ float AxisAlignedBoundingBox::getPerimeter()
bool AxisAlignedBoundingBox::intersects(AxisAlignedBoundingBox* o)
{
if (max.x < o->min.x || min.x > o->max.x)
if (max.x < o->min.x || min.x > o->max.x) // first: this is left of o; second: this is right of o
{
return false;
}
else if (max.y < o->min.y || min.y > o->max.y)
else if (max.y < o->min.y || min.y > o->max.y) // first: this is above o; second: this is below o
{
return false;
}
......
......@@ -9,11 +9,14 @@
using namespace duodim;
Body::Body(float posX, float posY, Material* material, Shape* shape, Texture* texture)
: transform(new Transform(Vector2D(posX, posY))), material(material), shape(shape), texture(texture)
Body::Body(int x, int y, Material* material, Shape* shape, Texture* texture, bool dynamic)
: transform(Transform::atPosition(x, y)), material(material), shape(shape), texture(texture)
{
shape->body = this;
unsetStatic();
if (dynamic)
{
unsetStatic();
}
}
Body::~Body()
......@@ -41,7 +44,7 @@ void Body::draw()
void Body::addForce(Vector2D f)
{
force = force + f;
force += f;
}
void Body::addForce(Vector2D f, Vector2D pos)
......@@ -52,7 +55,7 @@ void Body::addForce(Vector2D f, Vector2D pos)
void Body::addImpulse(Vector2D i, Vector2D pos)
{
velocity = velocity + (i * inv_mass);
velocity += (i * inv_mass);
angularVelocity = pos.crossMult(i) * inv_inertia;
}
......@@ -69,7 +72,7 @@ void Body::integrateForce()
}
float dt = Engine::getInstance()->dt;
Vector2D grav = Vector2D::gravity();
velocity = velocity + (((force * inv_mass) + grav) * (dt / 2.f));
velocity += (((force * inv_mass) + grav) * (dt / 2.f));
angularVelocity += ((torque * inv_inertia) * (dt / 2.f));
}
......@@ -80,7 +83,7 @@ void Body::integrateVelocity()
return;
}
float dt = Engine::getInstance()->dt;
transform->position = transform->position + (velocity * dt);
transform->position += (velocity * dt);
transform->addRotation(angularVelocity * dt);
integrateForce();
force = Vector2D();
......
......@@ -9,9 +9,9 @@ using namespace std;
vector<CollisionPair> BroadPhase::getNarrowPhaseCandidates(vector<Body*> bodies)
{
vector<CollisionPair> pairs;
for (auto i = 0; i < bodies.size(); i++)
for (unsigned int i = 0; i < bodies.size(); i++)
{
for (auto j = i + 1; j != bodies.size(); j++)
for (unsigned int j = i + 1; j != bodies.size(); j++)
{
Body* a = bodies[i];
Body* b = bodies[j];
......@@ -25,7 +25,7 @@ vector<CollisionPair> BroadPhase::getNarrowPhaseCandidates(vector<Body*> bodies)
AxisAlignedBoundingBox* aabbB = b->shape->getBounds();
if (aabbA->intersects(aabbB))
{
pairs.push_back({ a, b });
pairs.push_back({a, b});
}
delete aabbA;
delete aabbB;
......
......@@ -4,11 +4,17 @@
#include "AxisAlignedBoundingBox.hpp"
#include "Body.hpp"
#include "Circle.hpp"
#include "MathUtil.hpp"
#include "glut.h"
using namespace duodim;
using namespace std;
Circle* Circle::random()
{
return new Circle(rand(1.f, 3.f));
}
Circle::Circle(): radius(0.f)
{
updateName();
......@@ -29,7 +35,7 @@ AxisAlignedBoundingBox* Circle::getBounds()
MassAndInertia Circle::getMassAndInertia()
{
float sqRad = (float) pow(radius, 2);
float mass = body->material->density * M_PI * sqRad;
float mass = (float) body->material->density * M_PI * sqRad;
float inertia = .5f * mass * sqRad;
return {mass, inertia};
}
......@@ -54,7 +60,7 @@ void Circle::draw()
Vector2D position = body->transform->position;
float rotation = body->transform->rotation;
float theta = rotation;
float inc = M_PI * 2.0f / (float) kSegments;
float inc = (float) M_PI * 2.0f / kSegments;
for(int i = 0; i < kSegments; ++i)
{
theta += inc;
......@@ -65,8 +71,8 @@ void Circle::draw()
// Render line within circle so orientation is visible
glBegin(GL_LINE_STRIP);
float c = cos(rotation);
float s = sin(rotation);
float c = (float) cos(rotation);
float s = (float) sin(rotation);
Vector2D r = (Vector2D(s, c) * radius) + body->transform->position;
glVertex2f(body->transform->position.x, body->transform->position.y);
glVertex2f(r.x, r.y);
......
......@@ -8,10 +8,15 @@ using namespace duodim;
Collision::Collision(Body* a, Body* b) : a(a), b(b) {}
Collision::~Collision()
{
contactPoints.clear();
}
bool Collision::solve()
{
performNarrowPhase();
if (this->initialized)
if (initialized)
{
Material* am = a->material;
Material* bm = b->material;
......@@ -19,7 +24,7 @@ bool Collision::solve()
averageRestitution = min(am->restitution, bm->restitution);
mixedStaticFriction = (float) sqrt((double) am->staticFriction * bm->staticFriction);
mixedDynamicFriction = (float) sqrt((double) am->dynamicFriction * bm->dynamicFriction);
for (auto cPoint : contactPoints)
for (Vector2D cPoint : contactPoints)
{
Vector2D ra = cPoint - a->transform->position;
Vector2D rb = cPoint - b->transform->position;
......@@ -30,64 +35,62 @@ bool Collision::solve()
averageRestitution = 0.f;
}
}
am = NULL;
bm = NULL;
}
return initialized;
}
void Collision::addContactPosition(const Vector2D p)
void Collision::addContactPosition(Vector2D p)
{
contactPoints.push_back(p);
contactCount++;
}
void Collision::applyImpulse()
bool Collision::applyImpulse()
{
if (a->inv_mass + b->inv_mass == 0.f)
{
a->nullifyVelocity();
b->nullifyVelocity();
return;
return false;
}
Transform* at = a->transform;
Transform* bt = a->transform;
for (auto cPoint : contactPoints)
Transform* bt = b->transform;
for (Vector2D cPoint : contactPoints)
{
// Radii from COM to contact
Vector2D ra = cPoint - at->position;
Vector2D rb = cPoint - bt->position;
// Relative velocity
Vector2D rv = ((b->velocity + rb.crossMult(b->angularVelocity)) - (a->velocity - ra.crossMult(a->angularVelocity)));
float contactVelocity = rv * normal;
Vector2D relativeVelocity = b->velocity + rb.crossMult(b->angularVelocity) - a->velocity - ra.crossMult(a->angularVelocity);
float contactVelocity = relativeVelocity * normal;
if (contactVelocity > 0) // velocities must not separate
{
return;
return false;
}
float raCrossN = ra.crossMult(normal);
float rbCrossN = rb.crossMult(normal);
float invMassSum = (a->inv_mass + b->inv_mass + (float) pow(raCrossN, 2)) * (a->inv_inertia + (float) pow(rbCrossN, 2)) * b->inv_inertia;
float invMassSum = a->inv_mass + b->inv_mass + (float) pow(raCrossN, 2) * a->inv_inertia + (float) pow(rbCrossN, 2) * b->inv_inertia;
// Impulse scalar
float j = ((contactVelocity * -(1.f + averageRestitution)) / invMassSum) / (float) contactCount;
// Apply impulse
Vector2D impulse = normal * j;
a->addImpulse(impulse * -1.f, ra);
a->addImpulse(-impulse, ra);
b->addImpulse(impulse, rb);
//Recalculate relative velocity for friction impulse
rv = ((b->velocity + rb.crossMult(b->angularVelocity)) - (a->velocity - ra.crossMult(a->angularVelocity)));
// Friction impulse direction
Vector2D fiDirection = (rv - (normal * (normal * rv))).normalized();
Vector2D fiDirection = (relativeVelocity - (normal * (normal * relativeVelocity))).normalized();
// Friction impulse scalar
float jt = (-(rv * fiDirection) / invMassSum) / (float) contactCount;
float jt = (-(relativeVelocity * fiDirection) / invMassSum) / (float) contactCount;
if (jt == 0.f) // Friction impuls too tiny, abort
{
return;
return false;
}
// Applying Coulumb's law
......@@ -96,9 +99,12 @@ void Collision::applyImpulse()
: fiDirection * -j * mixedDynamicFriction
;
a->addImpulse(tImpulse * -1.f, ra);
a->addImpulse(-tImpulse, ra);
b->addImpulse(tImpulse, rb);
}
at = NULL;
bt = NULL;
return true;
}
void Collision::correctPositions()
......@@ -106,17 +112,17 @@ void Collision::correctPositions()
const float kSlop = .05f; // Penetration allowance
const float percent = .4f; // Penetration correction percentage
Vector2D correction = (normal * percent * (max((penetration - kSlop), 0.f) / (a->inv_mass + b->inv_mass))) * a->inv_mass;
a->transform->position = a->transform->position - correction;
b->transform->position = b->transform->position + correction;
a->transform->position -= correction;
b->transform->position += correction;
}
void Collision::performNarrowPhase()
{
typedef void (*CollisionCallback)(Collision* c, Body* a, Body* b);
typedef bool (*CollisionCallback)(Collision* c, Body* a, Body* b);
CollisionCallback performNarrowPhaseByType[(unsigned long long) ShapeType::ECOUNT][(unsigned long long) ShapeType::ECOUNT] =
{
{ NarrowPhase::twoCircles, NarrowPhase::circlePolygon },
{ NarrowPhase::polygonCircle, NarrowPhase::twoPolygons }
};
performNarrowPhaseByType[(unsigned long long) a->shape->getType()][(unsigned long long) b->shape->getType()](this, a, b);
initialized = performNarrowPhaseByType[(unsigned long long) a->shape->getType()][(unsigned long long) b->shape->getType()](this, a, b);
}
\ No newline at end of file
......@@ -3,6 +3,7 @@
#include "glut.h"
#include "Body.hpp"
#include "ConvexPolygon.hpp"
#include "MathUtil.hpp"
#include "Texture.hpp"
#include "Transform.hpp"
#include "Vector2D.hpp"
......@@ -10,11 +11,34 @@
using namespace duodim;
using namespace std;
ConvexPolygon* ConvexPolygon::random(const int maxPolyCount)
{
unsigned int count = (unsigned int)rand(3.f, (float) maxPolyCount);
vector<Vector2D> vertices;
float random = rand(5.f, 10.f);
for (unsigned int i = 0; i < count; ++i)
{
vertices.push_back(Vector2D(rand(-random, random), rand(-random, random)));
}
return new ConvexPolygon(vertices);
}
ConvexPolygon* ConvexPolygon::box(const float hw, const float hh)
{
return new ConvexPolygon(hw, hh);
}
ConvexPolygon::ConvexPolygon() : vertices({}), normals({})
{
updateName();
}
ConvexPolygon::ConvexPolygon(const float x, const float y) : vertices({}), normals({})
{
setBox(x, y);
updateName();
}
ConvexPolygon::ConvexPolygon(vector<Vector2D> vertices) : vertices(vertices)
{
Vector2D centroid;
......@@ -43,7 +67,7 @@ ConvexPolygon::~ConvexPolygon() {}
AxisAlignedBoundingBox* ConvexPolygon::getBounds()
{
Vector2D min = Vector2D::one() * INFINITY;
Vector2D max = Vector2D::one() * (-1.f) * INFINITY;
Vector2D max = -Vector2D::one() * INFINITY;
for (auto v : vertices)
{
Vector2D vertex = body->transform->getWorldPosition(v);
......@@ -108,10 +132,10 @@ void ConvexPolygon::setBox(float hw, float hh)
vertices.push_back(Vector2D(hw, -hh));
vertices.push_back(Vector2D(hw, hh));
vertices.push_back(Vector2D(-hw, hh));
normals.push_back(Vector2D(0.0f, -1.0f));
normals.push_back(Vector2D(1.0f, 0.0f));
normals.push_back(Vector2D(0.0f, 1.0f));
normals.push_back(Vector2D(-1.0f, 0.0f));
normals.push_back(Vector2D::up());
normals.push_back(Vector2D::right());
normals.push_back(Vector2D::down());
normals.push_back(Vector2D::left());
}
void ConvexPolygon::draw()
......@@ -148,8 +172,5 @@ void ConvexPolygon::calculateNormals()
void ConvexPolygon::updateName()
{
bool vempty = vertices.empty();
bool nempty = normals.empty();
name = vempty && nempty
? "ConvexPolygon (empty)"
: "ConvexPolygon (" + (vempty ? "no" : to_string(vertices.size())) + " polygons, " + (nempty ? "no" : to_string(normals.size())) + " normals)";
name = vempty ? "ConvexPolygon (empty)" : "ConvexPolygon (" + to_string(vertices.size()) + " polygons)";
}
\ No newline at end of file
......@@ -17,7 +17,7 @@ Material* Material::superBall()
}
Material::Material() : dynamicFriction(.4f), staticFriction(.4f), restitution(.4f), density(.4f) {}
Material::Material(float dynamicfriction, float staticFriction, float restitution, float density)
Material::Material(float dynamicFriction, float staticFriction, float restitution, float density)
: dynamicFriction(dynamicFriction), staticFriction(staticFriction), restitution(restitution), density(density) {}
Material::~Material() {}
\ No newline at end of file
......@@ -7,31 +7,25 @@ Matrix2 Matrix2::fromRadians(const float a)
{
float c = cos(a);
float s = sin(a);
return Matrix2(c, s * (-1), s, c);
return Matrix2(c, -s, s, c);
}