#include "geometry.h"

#include <math.h>

QRectF addBBox(QRectF r1, QRectF r2)

{	

	// Find smallest QRectF containing given rectangles

	QRectF n;

	// Set left border

	if (r1.left() <= r2.left() )

		n.setLeft(r1.left() );

	else

		n.setLeft(r2.left() );

	// Set top border		

	if (r1.top() <= r2.top() )

		n.setTop(r1.top() );

	else

		n.setTop(r2.top() );

	// Set right border

	if (r1.right() <= r2.right() )

		n.setRight(r2.right() );

	else

		n.setRight(r1.right() );

	// Set bottom 

	if (r1.bottom() <= r2.bottom() )

		n.setBottom(r2.bottom() );

	else

		n.setBottom(r1.bottom() );

	return n;

}

bool inBox(const QPointF &p, const QRectF &box)

{

    if (p.x() >= box.left() && p.x() <= box.right()  

	&& p.y() <= box.bottom() && p.y() >= box.top() )

		return true;

    return false;	

}

QPointF normalize (const QPointF &p)

{

	qreal n=sqrt ( p.x()*p.x() + p.y()*p.y() );

	return QPointF (p.x()/n,p.y()/n);

}

// Dot product of two vectors

qreal dotProduct (const QPointF &a, const QPointF &b)

{

	return a.x()*b.x() + a.y()*b.y();

}

// Structure that stores the results of the PolygonCollision function

class PolygonCollisionResult {

public:

    // Are the polygons going to intersect forward in time?

    bool WillIntersect;

    // Are the polygons currently intersecting?

    bool Intersect;

    // The translation to apply to the first polygon to push the polygons apart.

    QPointF MinimumTranslationVector;

};

/* Calculate the projection of a polygon on an axis

   and returns it as a [min, max] interval

*/

void ProjectPolygon(QPointF axis, QPolygonF polygon, qreal &min, qreal &max) 

{

    // To project a point on an axis use the dot product

    qreal d = dotProduct(axis,polygon.at(0));

    min = d;

    max = d;

    for (int i = 0; i < polygon.size(); i++) {

        d= dotProduct (polygon.at(i),axis);

        if (d < min) 

            min = d;

        else 

		{

            if (d> max) max = d;

        }

    }

}

/* Calculate the signed distance between [minA, maxA] and [minB, maxB]

   The distance will be negative if the intervals overlap

*/

qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {

    if (minA < minB) {

        return minB - maxA;

    } else {

        return minA - maxB;

    }

}

/*

 Check if polygon A is going to collide with polygon B.

 The last parameter is the *relative* velocity 

 of the polygons (i.e. velocityA - velocityB)

*/

PolygonCollisionResult PolygonCollision(QPolygonF polygonA, 

                              QPolygonF polygonB, QPointF velocity) {

    PolygonCollisionResult result;

    result.Intersect = true;

    result.WillIntersect = true;

    int edgeCountA = polygonA.size();

    int edgeCountB = polygonB.size();

    qreal minIntervalDistance = 1000000000;

    QPointF translationAxis;

    QPointF edge;

    // Loop through all the edges of both polygons

    for (int i=0; i < edgeCountA + edgeCountB; i++) 

	{

        if (i< edgeCountA) 

            edge = polygonA.at(i);

        else 

            edge = polygonB.at(i - edgeCountA);

        // ===== 1. Find if the polygons are currently intersecting =====

        // Find the axis perpendicular to the current edge

        QPointF axis (-edge.y(), edge.x());

        normalize(axis);

        // Find the projection of the polygon on the current axis

        qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;

        ProjectPolygon(axis, polygonA, minA, maxA);

        ProjectPolygon(axis, polygonB, minB, maxB);

        // Check if the polygon projections are currentlty intersecting

        if (intervalDistance(minA, maxA, minB, maxB) > 0)\

            result.Intersect = false;

        // ===== 2. Now find if the polygons *will* intersect =====

        // Project the velocity on the current axis

        qreal velocityProjection = dotProduct(axis,velocity);

        // Get the projection of polygon A during the movement

        if (velocityProjection < 0) {

            minA += velocityProjection;

        } else {

            maxA += velocityProjection;

        }

        // Do the same test as above for the new projection

        qreal d = intervalDistance(minA, maxA, minB, maxB);

        if (d > 0) result.WillIntersect = false;

        // If the polygons are not intersecting and won't intersect, exit the loop

        if (!result.Intersect && !result.WillIntersect) break;

        // Check if the current interval distance is the minimum one. If so store

        // the interval distance and the current distance.

        // This will be used to calculate the minimum translation vector

        if (d<0) d=-d;

        if (d < minIntervalDistance) {

            minIntervalDistance = d;

            translationAxis = axis;

            //QPointF t = polygonA.Center - polygonB.Center;

            QPointF t = polygonA.at(0) - polygonB.at(0);

            if (dotProduct(t,translationAxis) < 0)

                translationAxis = -translationAxis;

        }

    }

    // The minimum translation vector

    // can be used to push the polygons appart.

    if (result.WillIntersect)

        result.MinimumTranslationVector = 

               translationAxis * minIntervalDistance;

    return result;

}

/* The function can be used this way: 

   QPointF polygonATranslation = new QPointF();

*/   

/*

PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);

if (r.WillIntersect) 

  // Move the polygon by its velocity, then move

  // the polygons appart using the Minimum Translation Vector

  polygonATranslation = velocity + r.MinimumTranslationVector;

else 

  // Just move the polygon by its velocity

  polygonATranslation = velocity;

polygonA.Offset(polygonATranslation);

*/