#include "geometry.h"
#include <math.h>
#include "misc.h"
#include <QString>
#include <iostream>
using namespace std;
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;
}
QSize addBBoxSize (QSize s1, QSize s2)
{
// Find the minimimum smallest QSize which could include 2 given sizes
QSize s=s1;
if (s1.width() <= s2.width() )
s.setWidth (s2.width() );
if (s1.height() <= s2.height() )
s.setHeight(s2.height() );
return s;
}
bool isInBox(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;
}
qreal Geometry::distance (const QPointF &p, const QPointF &q)
{
return sqrt (p.x()*q.x() + p.y()*q.y());
}
Vector::Vector ():QPointF ()
{
}
Vector::Vector (const QPointF &p):QPointF (p)
{
}
Vector::Vector (qreal x, qreal y):QPointF (x,y)
{
}
Vector::~Vector ()
{
}
//! Check if length is 0
bool Vector::isNull()
{
if (x()==0 && y()==0)
return true;
return false;
}
//! Normalize vector
void Vector::normalize ()
{
if (isNull() ) return;
qreal l=sqrt ( x()*x() + y()*y() );
setX (x()/l);
setY (y()/l);
}
//! Dot product of two vectors
qreal Vector::dotProduct (const QPointF &b)
{
return x()*b.x() + y()*b.y();
}
void Vector::scale (const qreal &f)
{
setX (x()*f);
setY (y()*f);
}
void Vector::invert ()
{
setX (-x());
setY (-y());
}
QPointF Vector::toQPointF ()
{
return QPointF (x(),y());
}
/*! Calculate the projection of a polygon on an axis
and returns it as a [min, max] interval */
ConvexPolygon::ConvexPolygon ()
{
}
ConvexPolygon::ConvexPolygon (QPolygonF p):QPolygonF (p)
{
}
ConvexPolygon::~ConvexPolygon ()
{
}
void ConvexPolygon::calcCentroid()
{
// Calculate area and centroid
// http://en.wikipedia.org/wiki/Centroid
qreal cx,cy,p;
cx=cy=0;
_area=0;
append (at(0));
for (int i=0;i<size()-1;i++)
{
p=at(i).x() * at(i+1).y() - at(i+1).x() * at(i).y();
_area+=p;
cx+=(at(i).x()+at(i+1).x()) * p;
cy+=(at(i).y()+at(i+1).y()) * p;
}
pop_back();
// area is negative if vertices ordered counterclockwise
// (in mirrored graphicsview!)
_area=_area/2;
p=_area*6;
_centroid.setX (cx/p);
_centroid.setY (cy/p);
}
QPointF ConvexPolygon::centroid() const
{
return _centroid;
}
qreal ConvexPolygon::weight() const
{
return _area;
}
std::string ConvexPolygon::toStdString()
{
QString s ("(");
for (int i=0;i<size();++i)
{
s+=QString("(%1,%2)").arg(at(i).x()).arg(at(i).y());
if (i<size()-1) s+=",";
}
s+=")";
return s.toStdString();
}
Vector ConvexPolygon::at(const int &i) const
{
return Vector (QPolygonF::at(i).x(),QPolygonF::at(i).y());
}
void ConvexPolygon::translate ( const Vector & offset )
{ translate (offset.x(),offset.y());}
void ConvexPolygon::translate ( qreal dx, qreal dy )
{
QPolygonF::translate (dx,dy);
_centroid=_centroid+QPointF (dx,dy);
}
void projectPolygon(Vector axis, ConvexPolygon polygon, qreal &min, qreal &max)
{
// To project a point on an axis use the dot product
//qDebug() << "Projecting on "<< axis;
qreal d = axis.dotProduct(polygon.at(0));
min = d;
max = d;
for (int i = 0; i < polygon.size(); i++)
{
d= polygon.at(i).dotProduct (axis);
if (d < min)
min = d;
else
if (d> max) max = d;
// qDebug() << "p="<<polygon.at(i)<<" d="<<d<<" (min, max)=("<<min<<","<<max<<")";
}
}
// 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(ConvexPolygon polygonA,
ConvexPolygon polygonB, Vector velocity)
{
PolygonCollisionResult result;
result.intersect = true;
result.willIntersect = true;
int edgeCountA = polygonA.size();
int edgeCountB = polygonB.size();
qreal minIntervalDistance = 1000000000;
QPointF translationAxis;
QPointF edge;
/*
qDebug() << "A: ";
for (int k=0; k<edgeCountA;k++)
qDebug() <<polygonA.at(k);
qDebug() << "B: ";
for (int k=0; k<edgeCountB;k++)
qDebug() <<polygonB.at(k);
qDebug() ;
*/
// Loop through all the edges of both polygons
for (int i=0;i<edgeCountA + edgeCountB;i++)
{
if (i< edgeCountA)
{
// Loop through polygon A
if (i<edgeCountA-1)
edge = QPointF (
polygonA.at(i+1).x()-polygonA.at(i).x(),
polygonA.at(i+1).y()-polygonA.at(i).y());
else
edge = QPointF (
polygonA.at(0).x()-polygonA.at(i).x(),
polygonA.at(0).y()-polygonA.at(i).y());
} else
{
// Loop through polygon B
if (i < edgeCountA +edgeCountB -1 )
edge = QPointF (
polygonB.at(i-edgeCountA+1).x() - polygonB.at(i-edgeCountA).x(),
polygonB.at(i-edgeCountA+1).y() - polygonB.at(i-edgeCountA).y());
else
edge = QPointF (
polygonB.at(0).x() - polygonB.at(i-edgeCountA).x(),
polygonB.at(0).y() - polygonB.at(i-edgeCountA).y());
}
// ===== 1. Find if the polygons are currently intersecting =====
// Find the axis perpendicular to the current edge
Vector axis (-edge.y(), edge.x());
axis.normalize();
// 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
qreal d = intervalDistance(minA, maxA, minB, maxB);
if (d > 0) result.intersect = false;
// ===== 2. Now find if the polygons *will* intersect =====
// Project the velocity on the current axis
qreal velocityProjection = axis.dotProduct(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
// d = intervalDistance(minA, maxA, minB, maxB);
//if (d > 0) result.willIntersect = false;
/*
qDebug() <<" ";
qDebug() << "edge="<<edge<<" ";
qDebug() <<"axis="<<axis<<" ";
qDebug() <<"dA=("<<minA<<","<<maxA<<") dB=("<<minB<<","<<maxB<<")";
qDebug() <<" d="<<d<<" ";
//qDebug() <<"minD="<<minIntervalDistance<<" ";
qDebug() <<"int="<<result.intersect<<" ";
//qDebug() <<"wint="<<result.willIntersect<<" ";
//qDebug() <<"velProj="<<velocityProjection<<" ";
qDebug() ;
*/
if (result.intersect )// || result.willIntersect)
{
// 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;
//qDebug() << "tAxix="<<translationAxis;
//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.minTranslation =
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.minTranslation;
else
// Just move the polygon by its velocity
polygonATranslation = velocity;
polygonA.Offset(polygonATranslation);
*/
