toys/gps/source/geometry.cpp

/**
 *  @file geometry.cpp
 *  @author donkey (anjingyu_ws@foxmail.com)
 *  @brief Implementation of basic geometry functions in \c ns::geo
 *  @version 0.1.0
 *  @date 2020-05-15
 *
 *  @copyright Copyright (c) 2020- donkey <anjingyu_ws@foxmail.com>.
 *
 */

#include "geo/geometry.h"

#include <cmath>    /**< std::sqrt std::fabs */

namespace ns {
namespace geo {

bool Geometry::perpendicularDistanceOn(double ptX, double ptY, double startX, double startY, double endX, double endY, double *distance)
{
    double dot = (ptX - endX) * (startX - endX) + (ptY - endY) * (startY - endY);
    double d2 = (startX - endX) * (startX - endX) + (startY - endY) * (startY - endY);
    double r = dot / d2;
    bool on = (r <= 1.0 && r >= 0.0);

    if (distance == nullptr)
    {
        return on;
    }

    if (on)
    {
        // Out of range
        if (r < 0.0 || r > 1.0)
        {
            *distance = 0.0;
        }
        else
        {
            if (startX == endX)
            {
                *distance = std::fabs(ptX - startX);
            }
            else
            {
                double lineK = (endY - startY) / (endX - startX);
                double lineC = (endX * startY - startX * endY) / (endX - startX);
                *distance = std::fabs(lineK * ptX - ptY + lineC) / (std::sqrt(lineK * lineK + 1));
            }

            // Get the foot of prependicular
            // double u = ((ptX - startX) * (startX - endX) + (ptY - startY) * (startY - endY)) / d2;
            // double footX = startX + u * (startX - endX);
            // double footY = startY + u * (startY - endY);
        }
    }
    else
    {
        *distance = 0.0;
    }

    return on;
}

bool Geometry::prependicularDistanceOn(double ptX, double ptY, double ptZ,
                                    double startX, double startY, double startZ,
                                    double endX, double endY, double endZ,
                                    double *distance)
{
    double dot = (ptX - endX) * (startX - endX) + (ptY - endY) * (startY - endY) + (ptZ - endZ) * (startZ - endZ);
    double d2 = (startX - endX) * (startX - endX) + (startY - endY) * (startY - endY) + (startZ - endZ) * (startZ - endZ);
    double r = dot / d2;
    bool on = (r <= 1.0 && r >= 0.0);
    
    if (distance == nullptr)
    {
        return on;
    }

    if (on)
    {
        // d = (AB x AC)/|AB| S = (AB X AC) / 2 = distance * |AB| / 2
        // TODO
    }
    else
    {
        distance = 0.0;
    }

    return as*sin_A; 
}

// Dot product of vector1(x1, y1) and vector2(x2, y2)
// result = x1 * x2 + y1 * y2
static inline double _dot(double x1, double y1, double x2, double y2)
{
    return x1 * x2 + y1 * y2
}


// Cross product of vector1(x1, y1) and vector2(x2, y2)
// result = x1 * y2 - x2 * y1
static inline double _cross(double x1, double y1, double x2, double y2)
{
    return x1 * y2 - x2 * y1
}


// Calculate the distance of point to line.
// 
// vector1 (endX - startX, endY - startY)
// vector2 (x - startX, y - startY)
// 
// result = |vector1 X vector2| / |vector1|
double Geometry::distancePoint2Line(x, y, startX, startY, endX, endY)
{
    double vec1x = endX - startX;
    double vec1y = endY - startY;

    double vec2x = x - startX;
    double vec2y = y - startY;

    double dis = std::abs(_cross(vec1x, vec1y, vec2x,
                      vec2y)) / std::sqrt(vec1x * vec1x + vec1y * vec1y)

    return dis
}

static int _getLineIntersection(float p0_x, float p0_y, float p1_x, float p1_y, 
    float p2_x, float p2_y, float p3_x, float p3_y, float *i_x, float *i_y)
{
    float s02_x, s02_y, s10_x, s10_y, s32_x, s32_y, s_numer, t_numer, denom, t;
    s10_x = p1_x - p0_x;
    s10_y = p1_y - p0_y;
    s32_x = p3_x - p2_x;
    s32_y = p3_y - p2_y;

    denom = s10_x * s32_y - s32_x * s10_y;
    if (denom == 0)
        return 0; // Collinear
    bool denomPositive = denom > 0;

    s02_x = p0_x - p2_x;
    s02_y = p0_y - p2_y;
    s_numer = s10_x * s02_y - s10_y * s02_x;
    if ((s_numer < 0) == denomPositive)
        return 0; // No collision

    t_numer = s32_x * s02_y - s32_y * s02_x;
    if ((t_numer < 0) == denomPositive)
        return 0; // No collision

    if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive))
        return 0; // No collision
    // Collision detected
    t = t_numer / denom;
    if (i_x != NULL)
        *i_x = p0_x + (t * s10_x);
    if (i_y != NULL)
        *i_y = p0_y + (t * s10_y);

    return 1;
}

//find intersect
//return value : 
//1(one intersection) 0 (parallel,no intersection) 2(same line)
//@param point: if return value is one, point is the intersection point.
static int _findLine2LineIntersection2D(
    const VectorD2D& p0, const VectorD2D& p1, 
    const VectorD2D& p2, const VectorD2D& p3, VectorD2D& point)
{
    const double epsilon = 1e-7;

    VectorD2D d0 = p1 - p0;
    VectorD2D d1 = p3 - p2;
    VectorD2D diff = p2 - p0;

    double krossd0d1    = d0.x()*d1.y() - d1.x()*d0.y();
    double sqrkrossd0d1 = krossd0d1*krossd0d1;
    double sqrlen0 = d0.length2();
    double sqrlen1 = d1.length2();

    if (sqrkrossd0d1 > epsilon * sqrlen0 * sqrlen1)
    {
        double s = diff.x() * d1.y() - d1.x() * diff.y();
        point = p0 + s * d0;
    }

    double krossdiffd0 = diff.x()*d0.y() - d0.x()*diff.y();
    double sqrkrossdiffd0 = krossdiffd0*krossdiffd0;
    double sqrlendiff = diff.length2();
    if (sqrkrossdiffd0 > epsilon * sqrlen0 * sqrlendiff)
    {
        return 0;
    }

    return 2;
}

//test intersect
//return value : 
//1(one intersection) 0 (parallel,no intersection) 2(same line)
int  testLine2LineIntersection2D(
    const VectorD2D& p0, const VectorD2D& p1, 
    const VectorD2D& p2, const VectorD2D& p3)
{
    const double epsilon = 1e-7;

    //one intersection
    VectorD2D d0 = p1 - p0;
    VectorD2D d1 = p3 - p2;
    double krossd0d1 = d0.x()*d1.y() - d1.x()*d0.y();
    double sqrKrossd0d1 = krossd0d1 * krossd0d1;
    double sqrL0 = d0.length2();
    double sqrL1 = d1.length2();
    if (sqrKrossd0d1 > epsilon * sqrL0 * sqrL1)
    {
        return 1;
    }

    //parallel
    Vec2d diff = p2 - p0;
    double sqrdiff = diff.length2();
    double krossdiffd0 = diff.x()*d0.y() - d0.x()*diff.y();
    double sqrkrossDiffd0 = krossdiffd0 * krossdiffd0;
    if (sqrkrossDiffd0 > epsilon * sqrdiff * sqrL0)
    {
        return 0;
    }

    //same line
    return 2;
}

} // namespace geo
} // namespace ns