Commit ccd3e5b4 authored by Leon's avatar Leon

Fix crucial circle-to-circle collision bug

parent 9d444112
......@@ -97,7 +97,7 @@ void PhysicsLoop(void)
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, 8, "\"F\" to toggle frame stepping, SPACE to step");
RenderString(1, 10, "ESC to exit");
e->iterate();
glutSwapBuffers();
......@@ -131,8 +131,8 @@ int main(int argc, char** argv)
{
e = Engine::getInstance(); // fire up engine
s = Scene::getInstance();
s->sceneHeight = 800;
s->sceneWidth = 600;
s->sceneHeight = 600;
s->sceneWidth = 800;
checkArgs(argc, argv);
if (initGui(argc, argv) == -1)
{
......@@ -142,7 +142,7 @@ int main(int argc, char** argv)
if (!e->skipCollisions)
{
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));
s->bodies.push_back(new Body(50, 20, Material::bouncyBall(), Circle::random(), Texture::green(), true));
}
srand(1);
......
......@@ -8,6 +8,7 @@ namespace duodim
{
public:
static Transform* atPosition(int x, int y);
static Vector2D toLocalPosition(Matrix2 rotationMatrix, Vector2D worldPosition, Vector2D actualPosition);
Vector2D position;
Matrix2 rotMatrix;
......@@ -18,9 +19,7 @@ namespace duodim
Transform(Vector2D position, float rotation);
Vector2D getWorldPosition(Vector2D localPosition);
Vector2D getWorldDirection(Vector2D localDirection);
Vector2D getLocalPosition(Vector2D worldPosition);
Vector2D getLocalDirection(Vector2D worldDirection);
void setRotation(const float r);
void addRotation(const float r);
private:
......
......@@ -31,7 +31,7 @@ namespace duodim
*/
static Vector2D right();
/**
Returns a Vector2D (0., 9.81).
Returns a Vector2D (0.f, 9.81f).
*/
static Vector2D gravity();
......@@ -64,6 +64,8 @@ namespace duodim
void operator -=(const Vector2D b);
void operator *=(const float a);
void operator /=(const float a);
private:
bool isZero;
};
}
#endif
\ No newline at end of file
......@@ -10,35 +10,32 @@ using namespace duodim;
bool NarrowPhase::twoCircles(Collision* c, Body* a, Body* b)
{
bool result = false;
Circle* sa = static_cast<Circle*>(a->shape);
Circle* sb = static_cast<Circle*>(b->shape);
Vector2D normal = a->transform->position - b->transform->position;
float sqDist = normal.squaredMagnitude();
float rSum = sa->radius + sb->radius;
if (sqDist >= pow(rSum, 2))
Vector2D betweenCenters = b->transform->position - a->transform->position;
float sqBetweenCentersDistance = betweenCenters.squaredMagnitude();
float sumRadii = sa->radius + sb->radius;
if (sqBetweenCentersDistance >= pow(sumRadii, 2))
{
c->contactPoints.clear();
sa = NULL;
sb = NULL;
return false;
}
float dist = (float) sqrt(sqDist);
if (dist == 0.f)
{
c->penetration = sa->radius;
c->normal = Vector2D::right();
c->addContactPosition(a->transform->position);
}
else
{
c->penetration = rSum - dist;
c->normal = normal / dist;
c->addContactPosition((c->normal * sa->radius + a->transform->position));
result = true;
float betweenCentersDistance = (float)sqrt(sqBetweenCentersDistance);
bool isDistanceZero = betweenCentersDistance == 0.f;
c->penetration = isDistanceZero ? sa->radius : sumRadii - betweenCentersDistance;
c->normal = isDistanceZero ? Vector2D::right() : betweenCenters / betweenCentersDistance;
c->addContactPosition(isDistanceZero ? a->transform->position : (c->normal * sa->radius + a->transform->position));
}
sa = NULL;
sb = NULL;
return true;
return result;
}
bool NarrowPhase::circlePolygon(Collision* c, Body* a, Body* b)
......@@ -58,14 +55,11 @@ bool NarrowPhase::circlePolygon(Collision* c, Body* a, Body* b)
float separation = -FLT_MAX;
unsigned int faceNormal = 0;
vector<Vector2D> bVertices = sb->vertices;
vector<Vector2D> bNormals = sb->normals;
// Find edge with minimum penetration
// Exact concept as using support points in Polygon vs Polygon
for (unsigned int i = 0; i < bVertices.size(); ++i)
for (unsigned int i = 0; i < sb->vertices.size(); ++i)
{
float s = bNormals[i] * (center - bVertices[i]);
float s = sb->normals[i] * (center - sb->vertices[i]);
if (s > sa->radius)
{
sa = NULL;
......@@ -82,15 +76,15 @@ bool NarrowPhase::circlePolygon(Collision* c, Body* a, Body* b)
}
// Grab face's vertices
Vector2D v1 = bVertices[faceNormal];
unsigned int i2 = ((__int64) faceNormal + 1 < bVertices.size() ? faceNormal + 1u: 0u);
Vector2D v2 = bVertices[i2];
Vector2D v1 = sb->vertices[faceNormal];
unsigned int i2 = ((__int64) faceNormal + 1 < sb->vertices.size() ? faceNormal + 1u: 0u);
Vector2D v2 = sb->vertices[i2];
// Check to see if center is within polygon
if (separation < EPSILON)
{
c->penetration = sa->radius;
c->normal = -(bt->rotMatrix * bNormals[faceNormal]);
c->normal = -(bt->rotMatrix * sb->normals[faceNormal]);
c->addContactPosition((c->normal * sa->radius + at->position));
sa = NULL;
sb = NULL;
......@@ -104,57 +98,41 @@ bool NarrowPhase::circlePolygon(Collision* c, Body* a, Body* b)
float dot2 = (center - v2) * (v1 - v2);
c->penetration = sa->radius - separation;
Vector2D dist;
bool voronoiResult = false;
if (dot1 <= 0.0f) // Closest to v1
{
dist = center - v1;
if ((dist * dist) > pow(sa->radius, 2))
if ((center - v1).squaredMagnitude() <= pow(sa->radius, 2))
{
sa = NULL;
sb = NULL;
at = NULL;
bt = NULL;
return false;
voronoiResult = true;
c->normal = (Transform::toLocalPosition(bt->rotMatrix, v1, center)).normalized();
c->addContactPosition(bt->getWorldPosition(v1));
}
c->normal = (bt->rotMatrix * (v1 - center)).normalized();
c->addContactPosition(bt->getWorldPosition(v1));
}
else if (dot2 <= 0.0f) // Closest to v2
{
dist = center - v2;
if ((dist * dist) > pow(sa->radius, 2))
if ((center - v2).squaredMagnitude() <= pow(sa->radius, 2))
{
sa = NULL;
sb = NULL;
at = NULL;
bt = NULL;
return false;
voronoiResult = true;
c->normal = (Transform::toLocalPosition(bt->rotMatrix, v2, center)).normalized();
c->addContactPosition(bt->getWorldPosition(v2));
}
c->normal = (bt->rotMatrix * (v2 - center)).normalized();
c->addContactPosition(bt->getWorldPosition(v2));
}
else // Closest to face
{
dist = bNormals[faceNormal];
if (((center - v1) * dist) > sa->radius)
Vector2D dist = sb->normals[faceNormal];
if (((center - v1) * dist) <= sa->radius)
{
sa = NULL;
sb = NULL;
at = NULL;
bt = NULL;
return false;
voronoiResult = true;
c->normal = -(bt->rotMatrix * dist);
c->addContactPosition((c->normal * sa->radius + at->position));
}
c->normal = -(bt->rotMatrix * dist);
c->addContactPosition((c->normal * sa->radius + at->position));
}
sa = NULL;
sb = NULL;
at = NULL;
bt = NULL;
return true;
return voronoiResult;
}
bool NarrowPhase::polygonCircle(Collision* c, Body* a, Body* b)
......
......@@ -8,6 +8,11 @@ Transform* Transform::atPosition(int x, int y)
return new Transform(Vector2D(x, y));
}
Vector2D Transform::toLocalPosition(Matrix2 rotationMatrix, Vector2D worldPosition, Vector2D actualPosition)
{
return rotationMatrix * (worldPosition - actualPosition);
}
Transform::Transform()
: position(Vector2D()), rotation(0.f), rotMatrix(Matrix2::fromRadians(0.f)) {}
Transform::Transform(Vector2D position)
......@@ -20,21 +25,11 @@ Vector2D Transform::getWorldPosition(Vector2D localPosition)
return rotMatrix * localPosition + position;
}
Vector2D Transform::getWorldDirection(Vector2D localDirection)
{
return rotMatrix * localDirection;
}
Vector2D Transform::getLocalPosition(Vector2D worldPosition)
{
return rotMatrix.transpose() * (worldPosition - position);
}
Vector2D Transform::getLocalDirection(Vector2D worldDirection)
{
return rotMatrix.transpose() * worldDirection;
}
void Transform::setRotation(const float r)
{
rotation = r;
......
......@@ -44,21 +44,21 @@ Vector2D Vector2D::right()
return Vector2D(1, 0);
}
Vector2D::Vector2D(const double x, const double y) : x((float) x), y((float) y) {}
Vector2D::Vector2D(const double x, const double y) : x((float) x), y((float) y), isZero(x + y == 0.0) {}
Vector2D::Vector2D(const float x, const float y) : x(x), y(y) {}
Vector2D::Vector2D(const float x, const float y) : x(x), y(y), isZero(x + y == 0.f) {}
Vector2D::Vector2D(const int x, const int y) : x((float) x), y((float) y) {}
Vector2D::Vector2D(const int x, const int y) : x((float) x), y((float) y), isZero(x + y == 0) {}
Vector2D::Vector2D(const Vector2D& a) : x(a.x), y(a.y) {}
Vector2D::Vector2D(const Vector2D& a) : x(a.x), y(a.y), isZero(a.isZero) {}
Vector2D::Vector2D() : x(0.f), y(0.f) {}
Vector2D::Vector2D() : x(0.f), y(0.f), isZero(true) {}
Vector2D::~Vector2D() {}
Vector2D Vector2D::operator +(const Vector2D b)
{
return Vector2D(x + b.x, y + b.y);
return isZero ? Vector2D(b) : Vector2D(x + b.x, y + b.y);
}
Vector2D Vector2D::operator -(const Vector2D b)
......@@ -68,12 +68,17 @@ Vector2D Vector2D::operator -(const Vector2D b)
Vector2D Vector2D::operator *(const float a)
{
return Vector2D(x * a, y * a);
return isZero ? Vector2D() : Vector2D(x * a, y * a);
}
Vector2D Vector2D::operator /(const float a)
{
if (a == 0.f) {
if (isZero)
{
return Vector2D();
}
else if (a == 0.f)
{
throw "Cannot divide by zero";
}
return Vector2D(x / a, y / a);
......@@ -86,40 +91,42 @@ Vector2D Vector2D::operator -()
Vector2D Vector2D::crossMult(const float a)
{
return Vector2D(a * y, a * (-1.f) * x);
return isZero ? Vector2D() : Vector2D(a * y, a * -x);
}
Vector2D Vector2D::iCrossMult(const float a)
{
return Vector2D(a * (-1.f) * y, a * x);
return isZero ? Vector2D() : Vector2D(a * -y, a * x);
}
Vector2D Vector2D::normalized()
{
float mag = magnitude();
float a = mag > 0.f ? 1 / mag : mag;
if (isZero)
{
return Vector2D::one();
}
float a = 1.f / magnitude();
return Vector2D(a, a);
}
float Vector2D::magnitude()
{
float sqMag = squaredMagnitude();
return sqMag == 0.f ? sqMag : (float) sqrt(sqMag);
return isZero ? 0.f : sqrt(squaredMagnitude());
}
float Vector2D::squaredMagnitude()
{
return (float) (pow(x, 2) + pow(y, 2));
return isZero ? 0.f : (float) (pow(x, 2) + pow(y, 2));
}
float Vector2D::crossMult(const Vector2D b)
{
return x * b.y - y * b.x;
return isZero ? 0.f : (x * b.y - y * b.x);
}
float Vector2D::operator *(const Vector2D b)
{
return x * b.x + y * b.y;
return isZero ? 0.f : (x * b.x + y * b.y);
}
bool Vector2D::operator ==(const Vector2D b)
......@@ -131,26 +138,36 @@ void Vector2D::operator +=(const Vector2D b)
{
x += b.x;
y += b.y;
isZero = x + y == 0.f;
}
void Vector2D::operator -=(const Vector2D b)
{
x -= b.x;
y -= b.y;
isZero = x + y == 0.f;
}
void Vector2D::operator *=(const float a)
{
x *= a;
y *= a;
if (!isZero)
{
x *= a;
y *= a;
isZero = x + y == 0.f;
}
}
void Vector2D::operator /=(const float a)
{
if (a == 0.f)
if (!isZero)
{
throw "Attempted division by zero.";
if (a == 0.f)
{
throw "Attempted division by zero.";
}
x /= a;
y /= a;
isZero = x + y == 0.f;
}
x /= a;
y /= a;
}
\ No newline at end of file
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment