sketcherMinimizerMaths.h

/*
 *  sketcherMinimizerMaths.h
 *
 *
 *  Created by Nicola Zonta on 15/3/2011.
 *  Copyright Schrodinger, LLC. All rights reserved
 *
 */

#ifndef sketcherMINIMIZERMATHS_H
#define sketcherMINIMIZERMATHS_H

#include <cassert>
#include <cmath>
#include <iostream>
#include <cmath>
#include <vector>

#define MACROCYCLE 9 // smallest MACROCYCLE

#define BONDLENGTH 50
#define FRACTION_OF_BONDLENGTH_FOR_CLASH 0.25

#define SKETCHER_EPSILON 0.0001f

#ifndef M_PI
#define M_PI 3.1415926535897931
#endif

inline float roundToTwoDecimalDigits(float f)
{
    return static_cast<float>(floor(f * 100 + 0.5) * 0.01);
}

inline float roundToPrecision(float f, int precision)
{
    return static_cast<float>(floor(f * pow(10.f, precision) + 0.5) *
                              pow(0.1, precision));
}

/* class to represent a point or vector in 2d */
class sketcherMinimizerPointF
{
  public:
    sketcherMinimizerPointF() = default;

    sketcherMinimizerPointF(const sketcherMinimizerPointF& p)
        : xp(p.x()), yp(p.y())
    {
    }

    sketcherMinimizerPointF(float xpos, float ypos) : xp(xpos), yp(ypos) {}

    sketcherMinimizerPointF& operator=(const sketcherMinimizerPointF& p)
    {
        if (this != &p) {
            xp = p.x();
            yp = p.y();
        }
        return *this;
    }

    inline float x() const { return xp; }
    inline float y() const { return yp; }
    inline float& rx() { return xp; }
    inline float& ry() { return yp; }
    void setX(float x) { xp = x; }
    void setY(float y) { yp = y; }
    float squareLength() const
    {
        float dd = x() * x() + y() * y();
        return dd;
    }

    /* return the length of the vector */
    float length() const
    {
        float dd = squareLength();
        if (dd > SKETCHER_EPSILON) {
            return sqrt(dd);
        } else {
            return 0;
        }
    }

    /* normalize the vector */
    void normalize()
    {
        float q = length();
        if (q > SKETCHER_EPSILON) {
            xp /= q;
            yp /= q;
        }
    }

    /* rotate the vector by the angle with given sine and cosine */
    void rotate(float s, float c)
    {
        float x = xp;
        float y = yp;
        xp = x * c + y * s;
        yp = -x * s + y * c;
    }

    /* parallel component of this along the give axis */
    sketcherMinimizerPointF
    parallelComponent(const sketcherMinimizerPointF& axis)
    {
        float dotProduct = x() * axis.x() + y() * axis.y();
        return axis * dotProduct / axis.squareLength();
    }

    /* round the coordinates to the given number of decimal figures */
    void round(int precision = 2)
    {
        if (precision == 2) {
            xp = roundToTwoDecimalDigits(xp);
            yp = roundToTwoDecimalDigits(yp);
        } else {
            xp = roundToPrecision(xp, precision);
            yp = roundToPrecision(yp, precision);
        }
    }

    sketcherMinimizerPointF& operator+=(const sketcherMinimizerPointF& p)
    {
        xp += p.xp;
        yp += p.yp;
        return *this;
    }
    sketcherMinimizerPointF& operator-=(const sketcherMinimizerPointF& p)
    {
        xp -= p.xp;
        yp -= p.yp;
        return *this;
    }
    template <typename T> sketcherMinimizerPointF& operator*=(T c)
    {
        xp *= static_cast<float>(c);
        yp *= static_cast<float>(c);
        return *this;
    }
    template <typename T> sketcherMinimizerPointF& operator/=(T c)
    {
        xp /= static_cast<float>(c);
        yp /= static_cast<float>(c);
        return *this;
    }

    // friend inline bool operator==(const sketcherMinimizerPointF &p1, const
    // sketcherMinimizerPointF &p2) ;
    //    friend inline bool operator!=(const sketcherMinimizerPointF &, const
    //    sketcherMinimizerPointF &);

    inline friend std::ostream& operator<<(std::ostream& out,
                                           sketcherMinimizerPointF& point)
    {
        out << "(" << point.xp << ", " << point.yp << ")";
        return out;
    }

    friend inline const sketcherMinimizerPointF
    operator+(const sketcherMinimizerPointF& p1,
              const sketcherMinimizerPointF& p2)
    {
        return sketcherMinimizerPointF(p1.xp + p2.xp, p1.yp + p2.yp);
    }
    friend inline const sketcherMinimizerPointF
    operator-(const sketcherMinimizerPointF& p1,
              const sketcherMinimizerPointF& p2)
    {
        return sketcherMinimizerPointF(p1.xp - p2.xp, p1.yp - p2.yp);
    }
    friend inline const sketcherMinimizerPointF
    operator*(float c, const sketcherMinimizerPointF& p1)
    {
        return sketcherMinimizerPointF(p1.xp * c, p1.yp * c);
    }
    template <typename T>
    friend inline const sketcherMinimizerPointF
    operator*(const sketcherMinimizerPointF& p1, T c)
    {
        auto cf = static_cast<float>(c);
        return sketcherMinimizerPointF(p1.xp * cf, p1.yp * cf);
    }
    template <typename T>
    friend inline const sketcherMinimizerPointF
    operator/(const sketcherMinimizerPointF& p1, T c)
    {
        auto cf = static_cast<float>(c);
        return sketcherMinimizerPointF(p1.xp / cf, p1.yp / cf);
    }

    friend inline const sketcherMinimizerPointF
    operator-(const sketcherMinimizerPointF& p1)
    {
        return sketcherMinimizerPointF(-p1.xp, -p1.yp);
    }
    //     friend inline const sketcherMinimizerPointF operator/(const
    //     sketcherMinimizerPointF &, float);

  private:
    float xp{0.f};
    float yp{0.f};
};

/* return true if the two segments intersect and if a result pointer was given,
 * set it to the intersection point */
struct sketcherMinimizerMaths {
    static bool
    intersectionOfSegments(const sketcherMinimizerPointF& s1p1,
                           const sketcherMinimizerPointF& s1p2,
                           const sketcherMinimizerPointF& s2p1,
                           const sketcherMinimizerPointF& s2p2,
                           sketcherMinimizerPointF* result = nullptr)
    {
        /*
                Suppose the two line segments run from p to p + r and from q to
           q + s. Then any point on the first line is representable as p + t r
           (for a scalar parameter t) and any point on the second line as q + u
           s (for a scalar parameter u).

                The two lines intersect if we can find t and u such that:

                    p + t r = q + u s

                        Cross both sides with s, getting

                        (p + t r) × s = (q + u s) × s

                        And since s × s = 0, this means

                        t (r × s) = (q − p) × s

                        And therefore, solving for t:

                            t = (q − p) × s / (r × s)

                            In the same way, we can solve for u:

                                (p + t r) × r = (q + u s) × r

                                u (s × r) = (p − q) × r

                                u = (p − q) × r / (s × r)

                                To reduce the number of computation steps, it's
           convenient to rewrite this as follows (remembering that s × r = − r ×
           s):

                                u = (q − p) × r / (r × s)

                                Now there are five cases:

                                If r × s = 0 and (q − p) × r = 0, then the two
           lines are collinear. If in addition, either 0 ≤ (q − p) · r ≤ r · r
           or 0 ≤ (p − q) · s ≤ s · s, then the two lines are overlapping.

                                If r × s = 0 and (q − p) × r = 0, but neither 0
           ≤ (q − p) · r ≤ r · r nor 0 ≤ (p − q) · s ≤ s · s, then the two lines
           are collinear but disjoint.

                                If r × s = 0 and (q − p) × r ≠ 0, then the two
           lines are parallel and non-intersecting.

                                If r × s ≠ 0 and 0 ≤ t ≤ 1 and 0 ≤ u ≤ 1, the
           two line segments meet at the point p + t r = q + u s.

                                Otherwise, the two line segments are not
           parallel but do not intersect.
        */

        const sketcherMinimizerPointF& p = s1p1;
        sketcherMinimizerPointF r = s1p2 - s1p1;
        const sketcherMinimizerPointF& q = s2p1;
        sketcherMinimizerPointF s = s2p2 - s2p1;
        float rxs = crossProduct(r, s);
        if (rxs > -SKETCHER_EPSILON &&
            rxs < SKETCHER_EPSILON) { // parallel lines
            return false;
        }
        sketcherMinimizerPointF qminusp = q - p;
        float t = crossProduct(qminusp, s) / rxs;
        if (t < 0 || t > 1) {
            return false;
        }
        float u = crossProduct(qminusp, r) / rxs;
        if (u < 0 || u > 1) {
            return false;
        }
        if (result) {
            *result = p + t * r;
        }
        return true;
    }

    /* signed angle between p1p2 and p2p3 */
    static float signedAngle(const sketcherMinimizerPointF& p1,
                             const sketcherMinimizerPointF& p2,
                             const sketcherMinimizerPointF& p3)
    {
        sketcherMinimizerPointF v1 = p1 - p2;
        sketcherMinimizerPointF v2 = p3 - p2;
        return float(atan2(v1.x() * v2.y() - v1.y() * v2.x(),
                           v1.x() * v2.x() + v1.y() * v2.y()) *
                     180 / M_PI);
    }

    /* unsigned angle between p1p2 and p2p3 */
    static float unsignedAngle(const sketcherMinimizerPointF& p1,
                               const sketcherMinimizerPointF& p2,
                               const sketcherMinimizerPointF& p3)
    {
        float x1 = p1.x();
        float y1 = p1.y();
        float x2 = p2.x();
        float y2 = p2.y();
        float x3 = p3.x();
        float y3 = p3.y();
        float v1x = x1 - x2;
        float v1y = y1 - y2;
        float v2x = x3 - x2;
        float v2y = y3 - y2;

        float d = sqrt(v1x * v1x + v1y * v1y) * sqrt(v2x * v2x + v2y * v2y);
        if (d < SKETCHER_EPSILON) {
            d = SKETCHER_EPSILON;
        }
        float cosine = (v1x * v2x + v1y * v2y) / d;

        if (cosine < -1) {
            cosine = -1;
        } else if (cosine > 1) {
            cosine = 1;
        }
        return float((acos(cosine)) * 180 / M_PI);
    }

    /* return true if the two points are very close in space */
    static bool pointsCoincide(const sketcherMinimizerPointF& p1,
                               const sketcherMinimizerPointF& p2)
    {
        return ((p1 - p2).squareLength() < SKETCHER_EPSILON * SKETCHER_EPSILON);
    }

    /* return true if p1 and p2 are in the same semiplane defined by the given
     * segment */
    static bool sameSide(const sketcherMinimizerPointF& p1,
                         const sketcherMinimizerPointF& p2,
                         const sketcherMinimizerPointF& lineP1,
                         const sketcherMinimizerPointF& lineP2)
    {
        float x = lineP2.x() - lineP1.x();
        float y = lineP2.y() - lineP1.y();
        //    ///cerr << "("<<p1.x()<<","<<p1.y()<<") ("<<p2.x
        //    ()<<","<<p2.y()<<")  ("<<lineP1.x()<<","<<lineP1.y()<<")
        //    ("<<lineP2.x ()<<","<<lineP2.y()<<")"<<endl;
        if (fabs(float(x)) > fabs((y))) { // what about q?
            float m = y / x;

            float d1 = p1.y() - lineP1.y() - m * (p1.x() - lineP1.x());
            float d2 = p2.y() - lineP1.y() - m * (p2.x() - lineP1.x());
            return (d2 * d1 > 0);
        } else {
            float m = x / y;
            float d1 = p1.x() - lineP1.x() - m * (p1.y() - lineP1.y());
            float d2 = p2.x() - lineP1.x() - m * (p2.y() - lineP1.y());
            return (d2 * d1 > 0);
        }
    }

    /* return the projection of p on the line defined by the given segment */
    static sketcherMinimizerPointF
    projectPointOnLine(const sketcherMinimizerPointF& p,
                       const sketcherMinimizerPointF& sp1,
                       const sketcherMinimizerPointF& sp2)
    {
        sketcherMinimizerPointF l1 = p - sp1;
        sketcherMinimizerPointF l3 = sp2 - sp1;

        float segmentl2 = l3.squareLength();
        if (segmentl2 < SKETCHER_EPSILON) {
            segmentl2 = SKETCHER_EPSILON;
        }
        float t = sketcherMinimizerMaths::dotProduct(l1, l3) / segmentl2;
        return sp1 + t * l3;
    }

    /* squared distance of the given point from the given segment */
    static float squaredDistancePointSegment(const sketcherMinimizerPointF& p,
                                             const sketcherMinimizerPointF& sp1,
                                             const sketcherMinimizerPointF& sp2,
                                             float* returnT = nullptr)
    {

        sketcherMinimizerPointF l1 = p - sp1;
        sketcherMinimizerPointF l2 = sp2 - p;
        sketcherMinimizerPointF l3 = sp2 - sp1;

        float segmentl2 = l3.x() * l3.x() + l3.y() * l3.y();
        // float l1l =  sqrt ( l1.x () * l1.x () + l1.y() * l1.y() );

        if (segmentl2 < SKETCHER_EPSILON) {
            segmentl2 = SKETCHER_EPSILON;
        }
        float t = (l1.x() * l3.x() + l1.y() * l3.y()) / segmentl2;
        if (returnT != nullptr) {
            if (t < 0) {
                *returnT = 0;
            } else if (t > 1) {
                *returnT = 1;
            } else {
                *returnT = t;
            }
        }
        float squaredistance = 0.f;
        if (t < 0.f) {
            squaredistance = l1.x() * l1.x() + l1.y() * l1.y();
        } else if (t > 1.f) {

            squaredistance = l2.x() * l2.x() + l2.y() * l2.y();

        } else {

            sketcherMinimizerPointF proj = sp1 + t * l3;
            sketcherMinimizerPointF l5 = p - proj;
            squaredistance = l5.x() * l5.x() + l5.y() * l5.y();
        }
        if (squaredistance < SKETCHER_EPSILON) {
            squaredistance = SKETCHER_EPSILON;
        }

        return squaredistance;
    }

    static float squaredDistance(const sketcherMinimizerPointF& p1,
                                 const sketcherMinimizerPointF& p2)
    {
        return (p1.x() - p2.x()) * (p1.x() - p2.x()) +
               (p1.y() - p2.y()) * (p1.y() - p2.y());
    }

    static std::vector<float> tridiagonalSolve(const std::vector<float>& a,
                                               const std::vector<float>& b,
                                               const std::vector<float>& c,
                                               const std::vector<float>& rhs)
    {
        assert(a.size() == b.size() && a.size() == c.size() &&
               a.size() == rhs.size());
        assert(b[0] != 0.f);

        auto n = static_cast<unsigned int>(rhs.size());
        std::vector<float> u(n);
        std::vector<float> gam(n);

        float bet = b[0];
        u[0] = rhs[0] / bet;
        for (unsigned int j = 1; j < n; j++) {
            gam[j] = c[j - 1] / bet;
            bet = b[j] - a[j] * gam[j];
            assert(bet != 0.f);
            u[j] = (rhs[j] - a[j] * u[j - 1]) / bet;
        }
        for (unsigned int j = 1; j < n; j++) {
            u[n - j - 1] -= gam[n - j] * u[n - j];
        }

        return u;
    }

    /* used by ClosedBezierControlPoints */
    static std::vector<float> cyclicSolve(const std::vector<float>& a,
                                          const std::vector<float>& b,
                                          const std::vector<float>& c,
                                          float alpha, float beta,
                                          const std::vector<float>& rhs)
    {
        assert(a.size() == b.size() && a.size() == c.size());
        auto n = static_cast<unsigned int>(b.size());
        assert(n > 2);
        float gamma = -b[0]; // Avoid subtraction error in forming bb[0].
        // Set up the diagonal of the modified tridiagonal system.
        std::vector<float> bb(n);
        bb[0] = b[0] - gamma;
        bb[n - 1] = b[n - 1] - alpha * beta / gamma;
        for (unsigned int i = 1; i < n - 1; i++) {
            bb[i] = b[i];
        }
        // Solve A · x = rhs.
        std::vector<float> solution = tridiagonalSolve(a, bb, c, rhs);
        std::vector<float> x = solution;

        // Set up the vector u.
        std::vector<float> u(n);
        u[0] = gamma;
        u[n - 1] = alpha;
        for (unsigned int i = 1; i < n - 1; i++) {
            u[i] = 0.f;
        }
        // Solve A · z = u.
        solution = tridiagonalSolve(a, bb, c, u);
        std::vector<float> z = solution;

        // Form v · x/(1 + v · z).
        double fact = (x[0] + beta * x[n - 1] / gamma) /
                      (1.f + z[0] + beta * z[n - 1] / gamma);

        // Now get the solution vector x.
        for (unsigned int i = 0; i < n; i++) {
            x[i] -= float(fact * z[i]);
        }
        return x;
    }

    static sketcherMinimizerPointF
    pointOnCubicBezier(const sketcherMinimizerPointF& p1,
                       const sketcherMinimizerPointF& cp1,
                       const sketcherMinimizerPointF& cp2,
                       const sketcherMinimizerPointF& p2, float t)
    {
        // using Casteljiau's algorithm
        auto v1 = (1 - t) * p1 + t * cp1;
        auto v2 = (1 - t) * cp1 + t * cp2;
        auto v3 = (1 - t) * cp2 + t * p2;
        auto v4 = (1 - t) * v1 + t * v2;
        auto v5 = (1 - t) * v2 + t * v3;
        return (1 - t) * v4 + t * v5;
    }

    /* find control points to a closed Bezier curve that passes through the
     * given points */
    static void ClosedBezierControlPoints(
        const std::vector<sketcherMinimizerPointF>& knots,
        std::vector<sketcherMinimizerPointF>& firstControlPoints,
        std::vector<sketcherMinimizerPointF>& secondControlPoints)
    {

        auto n = static_cast<unsigned int>(knots.size());
        if (n <= 2) {
            return;
        }

        // Calculate first Bezier control points

        std::vector<float> a(n), b(n), c(n);
        for (unsigned int i = 0; i < n; i++) {
            a[i] = 1;
            b[i] = 4;
            c[i] = 1;
        }

        std::vector<float> rhs(n);
        for (unsigned int i = 0; i < n; i++) {
            int j = i + 1;
            if (j > int(n - 1)) {
                j = 0;
            }
            rhs[i] = 4 * knots[i].x() + 2 * knots[j].x();
        }
        // Solve the system for X.
        std::vector<float> x = cyclicSolve(a, b, c, 1, 1, rhs);

        for (unsigned int i = 0; i < n; i++) {
            int j = i + 1;
            if (j > int(n - 1)) {
                j = 0;
            }
            rhs[i] = 4 * knots[i].y() + 2 * knots[j].y();
        }
        // Solve the system for Y.
        std::vector<float> y = cyclicSolve(a, b, c, 1, 1, rhs);

        // Fill output arrays.
        firstControlPoints.resize(n);
        secondControlPoints.resize(n);
        for (unsigned int i = 0; i < n; i++) {
            firstControlPoints[i] = sketcherMinimizerPointF(x[i], y[i]);
            secondControlPoints[i] = sketcherMinimizerPointF(
                2 * knots[i].x() - x[i], 2 * knots[i].y() - y[i]);
        }
    }

    /* return the mirror image of the given point wrt the given segment */
    static sketcherMinimizerPointF
    mirrorPoint(const sketcherMinimizerPointF& point,
                const sketcherMinimizerPointF& segmentPoint1,
                const sketcherMinimizerPointF& segmentPoint2)
    {
        sketcherMinimizerPointF segmentV = segmentPoint2 - segmentPoint1;
        sketcherMinimizerPointF v2 = point - segmentPoint1;
        sketcherMinimizerPointF parallelComponent =
            v2.parallelComponent(segmentV);
        sketcherMinimizerPointF normalComponent = v2 - parallelComponent;
        return segmentPoint1 + parallelComponent - normalComponent;
    }

    /* dot product of two vectors */
    static float dotProduct(const sketcherMinimizerPointF& a,
                            const sketcherMinimizerPointF& b)
    {

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

    /* cross product of two vectors */
    static float crossProduct(const sketcherMinimizerPointF& a,
                              const sketcherMinimizerPointF& b)
    {

        return (a.x() * b.y() - a.y() * b.x());
    }

    static float cannonBallDistance(float originX, float originY, float originZ,
                                    float directionX, float directionY,
                                    float directionZ, float targetX,
                                    float targetY, float targetZ, float ballR,
                                    float targetR, float cutOff = 4.f)
    // how far can a cannonball of radius ballR shot from origin travel befor
    // hitting a target ball of targetR radius
    {

        // assume that direction is normalized

        float targetdX = targetX - originX;
        float targetdY = targetY - originY;
        float targetdZ = targetZ - originZ;
        float rR = ballR + targetR;
        float d2 = (targetdX * targetdX) + (targetdY * targetdY) +
                   (targetdZ * targetdZ);

        if (d2 > (cutOff + rR) * (cutOff + rR)) {
            return cutOff;
        }

        if (d2 < rR * rR) {
            return 0;
        }

        float d = sqrt(d2);
        if (d > SKETCHER_EPSILON) {
            targetdX /= d;
            targetdY /= d;
            targetdZ /= d;
        }
        float cos = targetdX * directionX + targetdY * directionY +
                    targetdZ * directionZ;
        if (cos < 0) {
            return cutOff;
        }
        float sin = sqrt(1 - (cos * cos));
        float f = d * sin;
        if (f > rR) {
            return cutOff;
        }
        float result = sqrt(d2 - (f * f)) - sqrt((rR * rR) - (f * f));
        if (result > cutOff) {
            return cutOff;
        }
        return result;
    }

    /* length of a 3d vector */
    static float length3D(float x, float y, float z)
    {
        float m = x * x + y * y + z * z;
        if (m > SKETCHER_EPSILON) {
            m = sqrt(m);
        }
        return m;
    }

    /* dot product of two 3d vectors */
    static float dotProduct3D(float x1, float y1, float z1, float x2, float y2,
                              float z2)
    {
        return x1 * x2 + y1 * y2 + z1 * z2;
    }

    /* cross product of two 3d vectors */
    static void crossProduct3D(float x1, float y1, float z1, float x2, float y2,
                               float z2, float& xr, float& yr, float& zr)

    {
        xr = y1 * z2 - z1 * y2;
        yr = z1 * x2 - x1 * z2;
        zr = x1 * y2 - y1 * x2;
    }
    static float distance3D(float x1, float y1, float z1, float x2, float y2,
                            float z2)
    {
        return length3D(x2 - x1, y2 - y1, z2 - z1);
    }

    /* angle between two 3d vectors */
    static float angle3D(float x1, float y1, float z1, float x2, float y2,
                         float z2, float x3, float y3, float z3)
    {
        float xa = x1 - x2;
        float ya = y1 - y2;
        float za = z1 - z2;
        float xb = x3 - x2;
        float yb = y3 - y2;
        float zb = z3 - z2;
        float l1 = length3D(xa, ya, za);
        float l2 = length3D(xb, yb, zb);
        float dp = dotProduct3D(xa, ya, za, xb, yb, zb);
        return static_cast<float>(acos(dp / (l1 * l2)) * 180.f / M_PI);
    }

    /* dihedral angle defined by 4 3d points */
    static float dihedral3D(float x1, float y1, float z1, float x2, float y2,
                            float z2, float x3, float y3, float z3, float x4,
                            float y4, float z4)
    {
        float xa, ya, za;
        crossProduct3D(x1 - x2, y1 - y2, z1 - z2, x3 - x2, y3 - y2, z3 - z2, xa,
                       ya, za);
        float xb, yb, zb;
        crossProduct3D(x2 - x3, y2 - y3, z2 - z3, x4 - x3, y4 - y3, z4 - z3, xb,
                       yb, zb);
        return angle3D(xa, ya, za, 0, 0, 0, xb, yb, zb);
    }

}; // struct sketcherMinimizerMaths

#endif // sketcherMINIMIZERMATHS_H