C++ & SFML Separating Axis Theorem - c++

Question

I'm trying to implement collision detection in sfml using the separating axis theorem but my function to get the min translation vector (getMtv()) is always returning that there is a collision (MTV::IsValid()). I was following a tutorial here
but I can't see where I went wrong.
#include <iostream>
#include <math.h>
#include <SFML/Graphics.hpp>
#include <gtest/gtest.h>
typedef sf::Vector2f Vector2;
typedef sf::Vector2f Axis;
typedef sf::Vector2f Projection;
typedef std::vector<Axis> AxesVec;
class MTV
{
private:
bool _valid;
Axis _axis;
float _overlap;
public:
MTV(Axis axis, float overlap, bool valid)
{
_valid = valid;
_overlap = overlap;
_axis = axis;
}
bool IsValid() const
{
return _valid;
}
};
Vector2 perpendicular(Vector2 v)
{
return Vector2(v.y, -v.x);
}
float dot(Vector2 vec1, Vector2 vec2)
{
return (vec1.x * vec2.x) + (vec1.y * vec2.y);
}
float magnitude(Vector2 v)
{
return std::sqrt(dot(v,v));
}
class Polygon : public sf::ConvexShape
{
public:
const AxesVec& GetAxes() const
{
return axes;
}
AxesVec axes;
void generateAxes()
{
for (int i = 0; i < getPointCount(); i++)
{
// get the current vertex
Vector2 p1 = getPoint(i); //shape.vertices[i];
// get the next vertex
Vector2 p2 = getPoint(i + 1 == getPointCount() ? 0 : i + 1);
// subtract the two to get the edge vector
Vector2 edge = p1 - p2;
// get either perpendicular vector
Vector2 normal = perpendicular(edge);
// the perp method is just (x, y) => (-y, x) or (y, -x)
axes.push_back(normal / magnitude(normal));
}
};
float cross(Vector2 vec1, Vector2 vec2)
{
return (vec1.x * vec2.y) - (vec1.y * vec2.x);
}
Vector2 project(Polygon p, Axis axis)
{
float min = dot(axis, p.getPoint(0)); //axis.dot(shape.vertices[0]);
float max = min;
for (int i = 1; i < p.getPointCount(); i++)
{
// NOTE: the axis must be normalized to get accurate projections
float prj = dot(axis, p.getPoint(i));//axis.dot(shape.vertices[i]);
if (prj < min)
{
min = prj;
}
else if (prj > max)
{
max = prj;
}
}
//Projection proj = new Projection(min, max);
return Projection(min, max);
}
class Collison
{
private:
Vector2 mtv;
Polygon a;
Polygon b;
};
bool overlap(Projection a, Projection b)
{
// x = min & y = max
return !(a.x > b.y || a.x > b.y);
}
float getOverlap(Projection a, Projection b)
{
// x = min & y = max
return (a.y < b.y) ? a.y - b.x : b.y - a.x;
}
MTV getMtv(Polygon a, Polygon b)
{
float overlapMax = std::numeric_limits<float>::infinity();// really large value;
float Overlap;
Axis smallest;// = null;
AxesVec axesA = a.GetAxes();
AxesVec axesB = b.GetAxes();
// loop over the axes1
for (auto&& axis : axesA) //for (int i = 0; i < axes1.length; i++)
{
//Axis axis = axes1[i];
// project both shapes onto the axis
Projection pA = project(a, axis);
Projection pB = project(b, axis);
// do the projections overlap?
if (!overlap(pA, pB)) //(!p1.overlap(p2))
{
// then we can guarantee that the shapes do not overlap
return MTV(smallest, 0, false);
}
else
{
// get the overlap
float o = getOverlap(pA, pB); //p1.getOverlap(p2);
// check for minimum
if (o < overlapMax)
{
// then set this one as the smallest
Overlap = o;
smallest = axis;
}
}
}
for (auto&& axis : axesB) //for (int i = 0; i < axes1.length; i++)
{
//Axis axis = axes1[i];
// project both shapes onto the axis
Projection pA = project(a, axis);
Projection pB = project(b, axis);
// do the projections overlap?
if (!overlap(pA, pB)) //(!p1.overlap(p2))
{
// then we can guarantee that the shapes do not overlap
return MTV(smallest, 0, false);
}
else
{
// get the overlap
double o = getOverlap(pA, pB); //p1.getOverlap(p2);
// check for minimum
if (o < overlapMax)
{
// then set this one as the smallest
Overlap = o;
smallest = axis;
}
}
}
//MTV mtv = new MTV(smallest, overlap);
// if we get here then we know that every axis had overlap on it
// so we can guarantee an intersection
return MTV(smallest, Overlap, true);
}
int main(int argc, char **argv)
{
Polygon polygon;
polygon.setPointCount(3);
polygon.setPoint(0, sf::Vector2f(500, 100));
polygon.setPoint(1, sf::Vector2f(250, 500));
polygon.setPoint(2, sf::Vector2f(750, 500));
polygon.setFillColor(sf::Color::Red);
polygon.generateAxes();
Polygon polygon2;
polygon2.setPointCount(3);
polygon2.setPoint(0, sf::Vector2f(100, 0));
polygon2.setPoint(1, sf::Vector2f(50, 150));
polygon2.setPoint(2, sf::Vector2f(150, 150));
polygon2.generateAxes();
//polygon2.setPoint(0, sf::Vector2f(100, 0));
//polygon2.setPoint(1, sf::Vector2f(500, 150));
//polygon2.setPoint(2, sf::Vector2f(250, 150));
polygon2.setFillColor(sf::Color::Green);
sf::RenderWindow window(sf::VideoMode(800, 600), "My window");
// run the program as long as the window is open
while (window.isOpen())
{
// check all the window's events that were triggered since the last iteration of the loop
sf::Event event;
while (window.pollEvent(event))
{
// "close requested" event: we close the window
if (event.type == sf::Event::Closed)
window.close();
}
// clear the window with black color
window.clear(sf::Color::Black);
// draw everything here...
window.draw(polygon);
window.draw(polygon2);
std::cout << getMtv(polygon, polygon2).IsValid() << std::endl;
// end the current frame
window.display();
}
return 0;
}