#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);

 */