X-Git-Url: https://git.sven.stormbind.net/?a=blobdiff_plain;f=src%2Fgeometry.cpp;fp=src%2Fgeometry.cpp;h=ffc53bc2c8a44526bc5f96c3060281abee7a279f;hb=d483bd8e6523c23c6f1d8908a2e0611c2bc9ff4f;hp=0000000000000000000000000000000000000000;hpb=7dfa3fe589d1722d49681f42cdb0bf1e6efb5223;p=sven%2Fvym.git

diff --git a/src/geometry.cpp b/src/geometry.cpp
new file mode 100644
index 0000000..ffc53bc
--- /dev/null
+++ b/src/geometry.cpp
@@ -0,0 +1,358 @@
+#include "geometry.h"
+
+#include "misc.h"
+#include <math.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);
+
+*/