measures3d.c

Go to the documentation of this file.

/**********************************************************************
 *
 * PostGIS - Spatial Types for PostgreSQL
 * http://postgis.net
 *
 * PostGIS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 2 of the License, or
 * (at your option) any later version.
 *
 * PostGIS is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with PostGIS.  If not, see <http://www.gnu.org/licenses/>.
 *
 **********************************************************************
 *
 * Copyright 2011 Nicklas Avén
 * Copyright 2019 Darafei Praliaskouski
 *
 **********************************************************************/
 
#include <string.h>
#include <stdlib.h>
 
#include "measures3d.h"
#include "lwrandom.h"
#include "lwgeom_log.h"
 
static inline int
get_3dvector_from_points(const POINT3DZ *p1, const POINT3DZ *p2, VECTOR3D *v)
{
        v->x = p2->x - p1->x;
        v->y = p2->y - p1->y;
        v->z = p2->z - p1->z;
 
        return (!FP_IS_ZERO(v->x) || !FP_IS_ZERO(v->y) || !FP_IS_ZERO(v->z));
}
 
static inline int
get_3dcross_product(const VECTOR3D *v1, const VECTOR3D *v2, VECTOR3D *v)
{
        v->x = (v1->y * v2->z) - (v1->z * v2->y);
        v->y = (v1->z * v2->x) - (v1->x * v2->z);
        v->z = (v1->x * v2->y) - (v1->y * v2->x);
 
        return (!FP_IS_ZERO(v->x) || !FP_IS_ZERO(v->y) || !FP_IS_ZERO(v->z));
}
 
static double
project_point_on_plane(const POINT3DZ *p, PLANE3D *pl, POINT3DZ *p0)
{
        /*In our plane definition we have a point on the plane and a normal vector (pl.pv), perpendicular to the plane
        this vector will be parallel to the line between our inputted point above the plane and the point we are
        searching for on the plane. So, we already have a direction from p to find p0, but we don't know the distance.
        */
 
        VECTOR3D v1;
        double f;
 
        if (!get_3dvector_from_points(&(pl->pop), p, &v1))
                return 0.0;
 
        f = DOT(pl->pv, v1);
        if (FP_IS_ZERO(f))
        {
                /* Point is in the plane */
                *p0 = *p;
                return 0;
        }
 
        f = -f / DOT(pl->pv, pl->pv);
 
        p0->x = p->x + pl->pv.x * f;
        p0->y = p->y + pl->pv.y * f;
        p0->z = p->z + pl->pv.z * f;
 
        return f;
}
 
static LWGEOM *
create_v_line(const LWGEOM *lwgeom, double x, double y, int32_t srid)
{
 
        LWPOINT *lwpoints[2];
        GBOX gbox;
        int rv = lwgeom_calculate_gbox(lwgeom, &gbox);
 
        if (rv == LW_FAILURE)
                return NULL;
 
        lwpoints[0] = lwpoint_make3dz(srid, x, y, gbox.zmin);
        lwpoints[1] = lwpoint_make3dz(srid, x, y, gbox.zmax);
 
        return (LWGEOM *)lwline_from_ptarray(srid, 2, lwpoints);
}
 
LWGEOM *
lwgeom_closest_line_3d(const LWGEOM *lw1, const LWGEOM *lw2)
{
        return lw_dist3d_distanceline(lw1, lw2, lw1->srid, DIST_MIN);
}
 
LWGEOM *
lwgeom_furthest_line_3d(LWGEOM *lw1, LWGEOM *lw2)
{
        return lw_dist3d_distanceline(lw1, lw2, lw1->srid, DIST_MAX);
}
 
LWGEOM *
lwgeom_closest_point_3d(const LWGEOM *lw1, const LWGEOM *lw2)
{
        return lw_dist3d_distancepoint(lw1, lw2, lw1->srid, DIST_MIN);
}
 
LWGEOM *
lw_dist3d_distanceline(const LWGEOM *lw1, const LWGEOM *lw2, int32_t srid, int mode)
{
        LWDEBUG(2, "lw_dist3d_distanceline is called");
        double x1, x2, y1, y2, z1, z2, x, y;
        double initdistance = (mode == DIST_MIN ? DBL_MAX : -1.0);
        DISTPTS3D thedl;
        LWPOINT *lwpoints[2];
        LWGEOM *result;
 
        thedl.mode = mode;
        thedl.distance = initdistance;
        thedl.tolerance = 0.0;
 
        /* Check if we really have 3D geometries
         * If not, send it to 2D-calculations which will give the same result
         * as an infinite z-value at one or two of the geometries */
        if (!lwgeom_has_z(lw1) || !lwgeom_has_z(lw2))
        {
 
                lwnotice(
                    "One or both of the geometries is missing z-value. The unknown z-value will be regarded as \"any value\"");
 
                if (!lwgeom_has_z(lw1) && !lwgeom_has_z(lw2))
                        return lw_dist2d_distanceline(lw1, lw2, srid, mode);
 
                DISTPTS thedl2d;
                thedl2d.mode = mode;
                thedl2d.distance = initdistance;
                thedl2d.tolerance = 0.0;
                if (!lw_dist2d_comp(lw1, lw2, &thedl2d))
                {
                        /* should never get here. all cases ought to be error handled earlier */
                        lwerror("Some unspecified error.");
                        result = (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                }
                LWGEOM *vertical_line;
                if (!lwgeom_has_z(lw1))
                {
                        x = thedl2d.p1.x;
                        y = thedl2d.p1.y;
 
                        vertical_line = create_v_line(lw2, x, y, srid);
                        if (!lw_dist3d_recursive(vertical_line, lw2, &thedl))
                        {
                                /* should never get here. all cases ought to be error handled earlier */
                                lwfree(vertical_line);
                                lwerror("Some unspecified error.");
                                result = (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                        }
                        lwfree(vertical_line);
                }
                if (!lwgeom_has_z(lw2))
                {
                        x = thedl2d.p2.x;
                        y = thedl2d.p2.y;
 
                        vertical_line = create_v_line(lw1, x, y, srid);
                        if (!lw_dist3d_recursive(lw1, vertical_line, &thedl))
                        {
                                /* should never get here. all cases ought to be error handled earlier */
                                lwfree(vertical_line);
                                lwerror("Some unspecified error.");
                                return (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                        }
                        lwfree(vertical_line);
                }
        }
        else
        {
                if (!lw_dist3d_recursive(lw1, lw2, &thedl))
                {
                        /* should never get here. all cases ought to be error handled earlier */
                        lwerror("Some unspecified error.");
                        result = (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                }
        }
        /*if thedl.distance is unchanged there where only empty geometries input*/
        if (thedl.distance == initdistance)
        {
                LWDEBUG(3, "didn't find geometries to measure between, returning null");
                result = (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
        }
        else
        {
                x1 = thedl.p1.x;
                y1 = thedl.p1.y;
                z1 = thedl.p1.z;
                x2 = thedl.p2.x;
                y2 = thedl.p2.y;
                z2 = thedl.p2.z;
 
                lwpoints[0] = lwpoint_make3dz(srid, x1, y1, z1);
                lwpoints[1] = lwpoint_make3dz(srid, x2, y2, z2);
 
                result = (LWGEOM *)lwline_from_ptarray(srid, 2, lwpoints);
        }
 
        return result;
}
 
LWGEOM *
lw_dist3d_distancepoint(const LWGEOM *lw1, const LWGEOM *lw2, int32_t srid, int mode)
{
 
        double x, y, z;
        DISTPTS3D thedl;
        double initdistance = DBL_MAX;
        LWGEOM *result;
 
        thedl.mode = mode;
        thedl.distance = initdistance;
        thedl.tolerance = 0;
 
        LWDEBUG(2, "lw_dist3d_distancepoint is called");
 
        /* Check if we really have 3D geometries
         * If not, send it to 2D-calculations which will give the same result
         * as an infinite z-value at one or two of the geometries
         */
        if (!lwgeom_has_z(lw1) || !lwgeom_has_z(lw2))
        {
                lwnotice(
                    "One or both of the geometries is missing z-value. The unknown z-value will be regarded as \"any value\"");
 
                if (!lwgeom_has_z(lw1) && !lwgeom_has_z(lw2))
                        return lw_dist2d_distancepoint(lw1, lw2, srid, mode);
 
                DISTPTS thedl2d;
                thedl2d.mode = mode;
                thedl2d.distance = initdistance;
                thedl2d.tolerance = 0.0;
                if (!lw_dist2d_comp(lw1, lw2, &thedl2d))
                {
                        /* should never get here. all cases ought to be error handled earlier */
                        lwerror("Some unspecified error.");
                        return (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                }
 
                LWGEOM *vertical_line;
                if (!lwgeom_has_z(lw1))
                {
                        x = thedl2d.p1.x;
                        y = thedl2d.p1.y;
 
                        vertical_line = create_v_line(lw2, x, y, srid);
                        if (!lw_dist3d_recursive(vertical_line, lw2, &thedl))
                        {
                                /* should never get here. all cases ought to be error handled earlier */
                                lwfree(vertical_line);
                                lwerror("Some unspecified error.");
                                return (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                        }
                        lwfree(vertical_line);
                }
 
                if (!lwgeom_has_z(lw2))
                {
                        x = thedl2d.p2.x;
                        y = thedl2d.p2.y;
 
                        vertical_line = create_v_line(lw1, x, y, srid);
                        if (!lw_dist3d_recursive(lw1, vertical_line, &thedl))
                        {
                                /* should never get here. all cases ought to be error handled earlier */
                                lwfree(vertical_line);
                                lwerror("Some unspecified error.");
                                result = (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                        }
                        lwfree(vertical_line);
                }
        }
        else
        {
                if (!lw_dist3d_recursive(lw1, lw2, &thedl))
                {
                        /* should never get here. all cases ought to be error handled earlier */
                        lwerror("Some unspecified error.");
                        result = (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
                }
        }
        if (thedl.distance == initdistance)
        {
                LWDEBUG(3, "didn't find geometries to measure between, returning null");
                result = (LWGEOM *)lwcollection_construct_empty(COLLECTIONTYPE, srid, 0, 0);
        }
        else
        {
                x = thedl.p1.x;
                y = thedl.p1.y;
                z = thedl.p1.z;
                result = (LWGEOM *)lwpoint_make3dz(srid, x, y, z);
        }
 
        return result;
}
 
double
lwgeom_maxdistance3d(const LWGEOM *lw1, const LWGEOM *lw2)
{
        return lwgeom_maxdistance3d_tolerance(lw1, lw2, 0.0);
}
 
double
lwgeom_maxdistance3d_tolerance(const LWGEOM *lw1, const LWGEOM *lw2, double tolerance)
{
        if (!lwgeom_has_z(lw1) || !lwgeom_has_z(lw2))
        {
                lwnotice(
                    "One or both of the geometries is missing z-value. The unknown z-value will be regarded as \"any value\"");
                return lwgeom_maxdistance2d_tolerance(lw1, lw2, tolerance);
        }
        DISTPTS3D thedl;
        LWDEBUG(2, "lwgeom_maxdistance3d_tolerance is called");
        thedl.mode = DIST_MAX;
        thedl.distance = -1;
        thedl.tolerance = tolerance;
        if (lw_dist3d_recursive(lw1, lw2, &thedl))
                return thedl.distance;
 
        /* should never get here. all cases ought to be error handled earlier */
        lwerror("Some unspecified error.");
        return -1;
}
 
double
lwgeom_mindistance3d(const LWGEOM *lw1, const LWGEOM *lw2)
{
        return lwgeom_mindistance3d_tolerance(lw1, lw2, 0.0);
}
 
static inline int
gbox_contains_3d(const GBOX *g1, const GBOX *g2)
{
        return (g2->xmin >= g1->xmin) && (g2->xmax <= g1->xmax) && (g2->ymin >= g1->ymin) && (g2->ymax <= g1->ymax) &&
               (g2->zmin >= g1->zmin) && (g2->zmax <= g1->zmax);
}
 
static inline int
lwgeom_solid_contains_lwgeom(const LWGEOM *solid, const LWGEOM *g)
{
        const GBOX *b1;
        const GBOX *b2;
 
        /* If solid isn't solid it can't contain anything */
        if (!FLAGS_GET_SOLID(solid->flags))
                return LW_FALSE;
 
        b1 = lwgeom_get_bbox(solid);
        b2 = lwgeom_get_bbox(g);
 
        /* If box won't contain box, shape won't contain shape */
        if (!gbox_contains_3d(b1, b2))
                return LW_FALSE;
        else /* Raycast upwards in Z */
        {
                POINT4D pt;
                /* We'll skew copies if we're not lucky */
                LWGEOM *solid_copy = lwgeom_clone_deep(solid);
                LWGEOM *g_copy = lwgeom_clone_deep(g);
 
                while (LW_TRUE)
                {
                        uint8_t is_boundary = LW_FALSE;
                        uint8_t is_inside = LW_FALSE;
 
                        /* take first point */
                        if (!lwgeom_startpoint(g_copy, &pt))
                                return LW_FALSE;
 
                        /* get part of solid that is upwards */
                        LWCOLLECTION *c = lwgeom_clip_to_ordinate_range(solid_copy, 'Z', pt.z, DBL_MAX, 0);
 
                        for (uint32_t i = 0; i < c->ngeoms; i++)
                        {
                                if (c->geoms[i]->type == POLYGONTYPE)
                                {
                                        /* 3D raycast along Z is 2D point in polygon */
                                        int t = lwpoly_contains_point((LWPOLY *)c->geoms[i], (POINT2D *)&pt);
 
                                        if (t == LW_INSIDE)
                                                is_inside = !is_inside;
                                        else if (t == LW_BOUNDARY)
                                        {
                                                is_boundary = LW_TRUE;
                                                break;
                                        }
                                }
                                else if (c->geoms[i]->type == TRIANGLETYPE)
                                {
                                        /* 3D raycast along Z is 2D point in polygon */
                                        LWTRIANGLE *tri = (LWTRIANGLE *)c->geoms[i];
                                        int t = ptarray_contains_point(tri->points, (POINT2D *)&pt);
 
                                        if (t == LW_INSIDE)
                                                is_inside = !is_inside;
                                        else if (t == LW_BOUNDARY)
                                        {
                                                is_boundary = LW_TRUE;
                                                break;
                                        }
                                }
                        }
 
                        lwcollection_free(c);
                        lwgeom_free(solid_copy);
                        lwgeom_free(g_copy);
 
                        if (is_boundary)
                        {
                                /* randomly skew a bit and restart*/
                                double cx = lwrandom_uniform() - 0.5;
                                double cy = lwrandom_uniform() - 0.5;
                                AFFINE aff = {1, 0, cx, 0, 1, cy, 0, 0, 1, 0, 0, 0};
 
                                solid_copy = lwgeom_clone_deep(solid);
                                lwgeom_affine(solid_copy, &aff);
 
                                g_copy = lwgeom_clone_deep(g);
                                lwgeom_affine(g_copy, &aff);
 
                                continue;
                        }
                        return is_inside;
                }
        }
}
 
double
lwgeom_mindistance3d_tolerance(const LWGEOM *lw1, const LWGEOM *lw2, double tolerance)
{
        assert(tolerance >= 0);
        if (!lwgeom_has_z(lw1) || !lwgeom_has_z(lw2))
        {
                lwnotice(
                    "One or both of the geometries is missing z-value. The unknown z-value will be regarded as \"any value\"");
 
                return lwgeom_mindistance2d_tolerance(lw1, lw2, tolerance);
        }
        DISTPTS3D thedl;
        thedl.mode = DIST_MIN;
        thedl.distance = DBL_MAX;
        thedl.tolerance = tolerance;
 
        if (lw_dist3d_recursive(lw1, lw2, &thedl))
        {
                if (thedl.distance <= tolerance)
                        return thedl.distance;
                if (lwgeom_solid_contains_lwgeom(lw1, lw2) || lwgeom_solid_contains_lwgeom(lw2, lw1))
                        return 0;
 
                return thedl.distance;
        }
 
        /* should never get here. all cases ought to be error handled earlier */
        lwerror("Some unspecified error.");
        return DBL_MAX;
}
 
/*------------------------------------------------------------------------------------------------------------
End of Initializing functions
--------------------------------------------------------------------------------------------------------------*/
 
/*------------------------------------------------------------------------------------------------------------
Preprocessing functions
Functions preparing geometries for distance-calculations
--------------------------------------------------------------------------------------------------------------*/
 
int
lw_dist3d_recursive(const LWGEOM *lwg1, const LWGEOM *lwg2, DISTPTS3D *dl)
{
        int i, j;
        int n1 = 1;
        int n2 = 1;
        LWGEOM *g1 = NULL;
        LWGEOM *g2 = NULL;
        LWCOLLECTION *c1 = NULL;
        LWCOLLECTION *c2 = NULL;
 
        LWDEBUGF(2, "lw_dist3d_recursive is called with type1=%d, type2=%d", lwg1->type, lwg2->type);
 
        if (lwgeom_is_collection(lwg1))
        {
                LWDEBUG(3, "First geometry is collection");
                c1 = lwgeom_as_lwcollection(lwg1);
                n1 = c1->ngeoms;
        }
        if (lwgeom_is_collection(lwg2))
        {
                LWDEBUG(3, "Second geometry is collection");
                c2 = lwgeom_as_lwcollection(lwg2);
                n2 = c2->ngeoms;
        }
 
        for (i = 0; i < n1; i++)
        {
                if (lwgeom_is_collection(lwg1))
                        g1 = c1->geoms[i];
                else
                        g1 = (LWGEOM *)lwg1;
 
                if (lwgeom_is_empty(g1))
                        continue;
 
                if (lwgeom_is_collection(g1))
                {
                        LWDEBUG(3, "Found collection inside first geometry collection, recursing");
                        if (!lw_dist3d_recursive(g1, lwg2, dl))
                                return LW_FALSE;
                        continue;
                }
                for (j = 0; j < n2; j++)
                {
                        if (lwgeom_is_collection(lwg2))
                                g2 = c2->geoms[j];
                        else
                                g2 = (LWGEOM *)lwg2;
 
                        if (lwgeom_is_empty(g2))
                                continue;
 
                        if (lwgeom_is_collection(g2))
                        {
                                LWDEBUG(3, "Found collection inside second geometry collection, recursing");
                                if (!lw_dist3d_recursive(g1, g2, dl))
                                        return LW_FALSE;
                                continue;
                        }
 
                        /*If one of geometries is empty, return. True here only means continue searching. False would
                         * have stopped the process*/
                        if (lwgeom_is_empty(g1) || lwgeom_is_empty(g2))
                                return LW_TRUE;
 
                        if (!lw_dist3d_distribute_bruteforce(g1, g2, dl))
                                return LW_FALSE;
                        if (dl->distance <= dl->tolerance && dl->mode == DIST_MIN)
                                return LW_TRUE; /*just a check if the answer is already given*/
                }
        }
        return LW_TRUE;
}
 
int
lw_dist3d_distribute_bruteforce(const LWGEOM *lwg1, const LWGEOM *lwg2, DISTPTS3D *dl)
{
        int t1 = lwg1->type;
        int t2 = lwg2->type;
 
        LWDEBUGF(2, "lw_dist3d_distribute_bruteforce is called with typ1=%d, type2=%d", lwg1->type, lwg2->type);
 
        if (t1 == POINTTYPE)
        {
                if (t2 == POINTTYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_point_point((LWPOINT *)lwg1, (LWPOINT *)lwg2, dl);
                }
                else if (t2 == LINETYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_point_line((LWPOINT *)lwg1, (LWLINE *)lwg2, dl);
                }
                else if (t2 == POLYGONTYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_point_poly((LWPOINT *)lwg1, (LWPOLY *)lwg2, dl);
                }
                else if (t2 == TRIANGLETYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_point_tri((LWPOINT *)lwg1, (LWTRIANGLE *)lwg2, dl);
                }
                else
                {
                        lwerror("%s: Unsupported geometry type: %s", __func__, lwtype_name(t2));
                        return LW_FALSE;
                }
        }
        else if (t1 == LINETYPE)
        {
                if (t2 == POINTTYPE)
                {
                        dl->twisted = -1;
                        return lw_dist3d_point_line((LWPOINT *)lwg2, (LWLINE *)lwg1, dl);
                }
                else if (t2 == LINETYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_line_line((LWLINE *)lwg1, (LWLINE *)lwg2, dl);
                }
                else if (t2 == POLYGONTYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_line_poly((LWLINE *)lwg1, (LWPOLY *)lwg2, dl);
                }
                else if (t2 == TRIANGLETYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_line_tri((LWLINE *)lwg1, (LWTRIANGLE *)lwg2, dl);
                }
                else
                {
                        lwerror("%s: Unsupported geometry type: %s", __func__, lwtype_name(t2));
                        return LW_FALSE;
                }
        }
        else if (t1 == POLYGONTYPE)
        {
                if (t2 == POLYGONTYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_poly_poly((LWPOLY *)lwg1, (LWPOLY *)lwg2, dl);
                }
                else if (t2 == POINTTYPE)
                {
                        dl->twisted = -1;
                        return lw_dist3d_point_poly((LWPOINT *)lwg2, (LWPOLY *)lwg1, dl);
                }
                else if (t2 == LINETYPE)
                {
                        dl->twisted = -1;
                        return lw_dist3d_line_poly((LWLINE *)lwg2, (LWPOLY *)lwg1, dl);
                }
                else if (t2 == TRIANGLETYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_poly_tri((LWPOLY *)lwg1, (LWTRIANGLE *)lwg2, dl);
                }
                else
                {
                        lwerror("%s: Unsupported geometry type: %s", __func__, lwtype_name(t2));
                        return LW_FALSE;
                }
        }
        else if (t1 == TRIANGLETYPE)
        {
                if (t2 == POLYGONTYPE)
                {
                        dl->twisted = -1;
                        return lw_dist3d_poly_tri((LWPOLY *)lwg2, (LWTRIANGLE *)lwg1, dl);
                }
                else if (t2 == POINTTYPE)
                {
                        dl->twisted = -1;
                        return lw_dist3d_point_tri((LWPOINT *)lwg2, (LWTRIANGLE *)lwg1, dl);
                }
                else if (t2 == LINETYPE)
                {
                        dl->twisted = -1;
                        return lw_dist3d_line_tri((LWLINE *)lwg2, (LWTRIANGLE *)lwg1, dl);
                }
                else if (t2 == TRIANGLETYPE)
                {
                        dl->twisted = 1;
                        return lw_dist3d_tri_tri((LWTRIANGLE *)lwg1, (LWTRIANGLE *)lwg2, dl);
                }
                else
                {
                        lwerror("%s: Unsupported geometry type: %s", __func__, lwtype_name(t2));
                        return LW_FALSE;
                }
        }
 
        else
        {
                lwerror("%s: Unsupported geometry type: %s", __func__, lwtype_name(t1));
                return LW_FALSE;
        }
}
 
/*------------------------------------------------------------------------------------------------------------
End of Preprocessing functions
--------------------------------------------------------------------------------------------------------------*/
 
/*------------------------------------------------------------------------------------------------------------
Brute force functions
So far the only way to do 3D-calculations
--------------------------------------------------------------------------------------------------------------*/
 
int
lw_dist3d_point_point(const LWPOINT *point1, const LWPOINT *point2, DISTPTS3D *dl)
{
        POINT3DZ p1;
        POINT3DZ p2;
        LWDEBUG(2, "lw_dist3d_point_point is called");
 
        getPoint3dz_p(point1->point, 0, &p1);
        getPoint3dz_p(point2->point, 0, &p2);
 
        return lw_dist3d_pt_pt(&p1, &p2, dl);
}
int
lw_dist3d_point_line(const LWPOINT *point, const LWLINE *line, DISTPTS3D *dl)
{
        POINT3DZ p;
        POINTARRAY *pa = line->points;
        LWDEBUG(2, "lw_dist3d_point_line is called");
 
        getPoint3dz_p(point->point, 0, &p);
        return lw_dist3d_pt_ptarray(&p, pa, dl);
}
 
int
lw_dist3d_point_poly(const LWPOINT *point, const LWPOLY *poly, DISTPTS3D *dl)
{
        POINT3DZ p, projp; /*projp is "point projected on plane"*/
        PLANE3D plane;
        LWDEBUG(2, "lw_dist3d_point_poly is called");
        getPoint3dz_p(point->point, 0, &p);
 
        /* If we are looking for max distance, longestline or dfullywithin */
        if (dl->mode == DIST_MAX)
                return lw_dist3d_pt_ptarray(&p, poly->rings[0], dl);
 
        /* Find the plane of the polygon, the "holes" have to be on the same plane. so we only care about the boundary */
        if (!define_plane(poly->rings[0], &plane))
        {
                /* Polygon does not define a plane: Return distance point -> line */
                return lw_dist3d_pt_ptarray(&p, poly->rings[0], dl);
        }
 
        /* Get our point projected on the plane of the polygon */
        project_point_on_plane(&p, &plane, &projp);
 
        return lw_dist3d_pt_poly(&p, poly, &plane, &projp, dl);
}
 
/* point to triangle calculation */
int
lw_dist3d_point_tri(const LWPOINT *point, const LWTRIANGLE *tri, DISTPTS3D *dl)
{
        POINT3DZ p, projp; /*projp is "point projected on plane"*/
        PLANE3D plane;
        getPoint3dz_p(point->point, 0, &p);
 
        /* If we are looking for max distance, longestline or dfullywithin */
        if (dl->mode == DIST_MAX)
                return lw_dist3d_pt_ptarray(&p, tri->points, dl);
 
        /* If triangle does not define a plane, treat it as a line */
        if (!define_plane(tri->points, &plane))
                return lw_dist3d_pt_ptarray(&p, tri->points, dl);
 
        /* Get our point projected on the plane of triangle */
        project_point_on_plane(&p, &plane, &projp);
 
        return lw_dist3d_pt_tri(&p, tri, &plane, &projp, dl);
}
 
int
lw_dist3d_line_line(const LWLINE *line1, const LWLINE *line2, DISTPTS3D *dl)
{
        POINTARRAY *pa1 = line1->points;
        POINTARRAY *pa2 = line2->points;
        LWDEBUG(2, "lw_dist3d_line_line is called");
 
        return lw_dist3d_ptarray_ptarray(pa1, pa2, dl);
}
 
int
lw_dist3d_line_poly(const LWLINE *line, const LWPOLY *poly, DISTPTS3D *dl)
{
        PLANE3D plane;
        LWDEBUG(2, "lw_dist3d_line_poly is called");
 
        if (dl->mode == DIST_MAX)
                return lw_dist3d_ptarray_ptarray(line->points, poly->rings[0], dl);
 
        /* if polygon does not define a plane: Return distance line to line */
        if (!define_plane(poly->rings[0], &plane))
                return lw_dist3d_ptarray_ptarray(line->points, poly->rings[0], dl);
 
        return lw_dist3d_ptarray_poly(line->points, poly, &plane, dl);
}
 
int
lw_dist3d_line_tri(const LWLINE *line, const LWTRIANGLE *tri, DISTPTS3D *dl)
{
        PLANE3D plane;
 
        if (dl->mode == DIST_MAX)
                return lw_dist3d_ptarray_ptarray(line->points, tri->points, dl);
 
        /* if triangle does not define a plane: Return distance line to line */
        if (!define_plane(tri->points, &plane))
                return lw_dist3d_ptarray_ptarray(line->points, tri->points, dl);
 
        return lw_dist3d_ptarray_tri(line->points, tri, &plane, dl);
}
 
int
lw_dist3d_poly_poly(const LWPOLY *poly1, const LWPOLY *poly2, DISTPTS3D *dl)
{
        PLANE3D plane1, plane2;
        int planedef1, planedef2;
        LWDEBUG(2, "lw_dist3d_poly_poly is called");
        if (dl->mode == DIST_MAX)
                return lw_dist3d_ptarray_ptarray(poly1->rings[0], poly2->rings[0], dl);
 
        planedef1 = define_plane(poly1->rings[0], &plane1);
        planedef2 = define_plane(poly2->rings[0], &plane2);
 
        if (!planedef1 || !planedef2)
        {
                /* Neither polygon define a plane: Return distance line to line */
                if (!planedef1 && !planedef2)
                        return lw_dist3d_ptarray_ptarray(poly1->rings[0], poly2->rings[0], dl);
 
                /* Only poly2 defines a plane: Return distance from line (poly1) to poly2 */
                else if (!planedef1)
                        return lw_dist3d_ptarray_poly(poly1->rings[0], poly2, &plane2, dl);
 
                /* Only poly1 defines a plane: Return distance from line (poly2) to poly1 */
                else
                        return lw_dist3d_ptarray_poly(poly2->rings[0], poly1, &plane1, dl);
        }
 
        /* What we do here is to compare the boundary of one polygon with the other polygon
        and then take the second boundary comparing with the first polygon */
        dl->twisted = 1;
        if (!lw_dist3d_ptarray_poly(poly1->rings[0], poly2, &plane2, dl))
                return LW_FALSE;
        if (dl->distance < dl->tolerance) /* Just check if the answer already is given*/
                return LW_TRUE;
 
        dl->twisted = -1; /* because we switch the order of geometries we switch "twisted" to -1 which will give the
                             right order of points in shortest line. */
        return lw_dist3d_ptarray_poly(poly2->rings[0], poly1, &plane1, dl);
}
 
int
lw_dist3d_poly_tri(const LWPOLY *poly, const LWTRIANGLE *tri, DISTPTS3D *dl)
{
        PLANE3D plane1, plane2;
        int planedef1, planedef2;
 
        if (dl->mode == DIST_MAX)
                return lw_dist3d_ptarray_ptarray(poly->rings[0], tri->points, dl);
 
        planedef1 = define_plane(poly->rings[0], &plane1);
        planedef2 = define_plane(tri->points, &plane2);
 
        if (!planedef1 || !planedef2)
        {
                /* Neither define a plane: Return distance line to line */
                if (!planedef1 && !planedef2)
                        return lw_dist3d_ptarray_ptarray(poly->rings[0], tri->points, dl);
 
                /* Only tri defines a plane: Return distance from line (poly) to tri */
                else if (!planedef1)
                        return lw_dist3d_ptarray_tri(poly->rings[0], tri, &plane2, dl);
 
                /* Only poly defines a plane: Return distance from line (tri) to poly */
                else
                        return lw_dist3d_ptarray_poly(tri->points, poly, &plane1, dl);
        }
 
        /* What we do here is to compare the boundary of one polygon with the other polygon
        and then take the second boundary comparing with the first polygon */
        dl->twisted = 1;
        if (!lw_dist3d_ptarray_tri(poly->rings[0], tri, &plane2, dl))
                return LW_FALSE;
        if (dl->distance < dl->tolerance) /* Just check if the answer already is given*/
                return LW_TRUE;
 
        dl->twisted = -1; /* because we switch the order of geometries we switch "twisted" to -1 which will give the
                             right order of points in shortest line. */
        return lw_dist3d_ptarray_poly(tri->points, poly, &plane1, dl);
}
 
int
lw_dist3d_tri_tri(const LWTRIANGLE *tri1, const LWTRIANGLE *tri2, DISTPTS3D *dl)
{
        PLANE3D plane1, plane2;
        int planedef1, planedef2;
 
        if (dl->mode == DIST_MAX)
                return lw_dist3d_ptarray_ptarray(tri1->points, tri2->points, dl);
 
        planedef1 = define_plane(tri1->points, &plane1);
        planedef2 = define_plane(tri2->points, &plane2);
 
        if (!planedef1 || !planedef2)
        {
                /* Neither define a plane: Return distance line to line */
                if (!planedef1 && !planedef2)
                        return lw_dist3d_ptarray_ptarray(tri1->points, tri2->points, dl);
 
                /* Only tri defines a plane: Return distance from line (tri1) to tri2 */
                else if (!planedef1)
                        return lw_dist3d_ptarray_tri(tri1->points, tri2, &plane2, dl);
 
                /* Only poly defines a plane: Return distance from line (tri2) to tri1 */
                else
                        return lw_dist3d_ptarray_tri(tri2->points, tri1, &plane1, dl);
        }
 
        /* What we do here is to compare the boundary of one polygon with the other polygon
        and then take the second boundary comparing with the first polygon */
        dl->twisted = 1;
        if (!lw_dist3d_ptarray_tri(tri1->points, tri2, &plane2, dl))
                return LW_FALSE;
        if (dl->distance < dl->tolerance) /* Just check if the answer already is given*/
                return LW_TRUE;
 
        dl->twisted = -1; /* because we switch the order of geometries we switch "twisted" to -1 which will give the
                             right order of points in shortest line. */
        return lw_dist3d_ptarray_tri(tri2->points, tri1, &plane1, dl);
}
 
int
lw_dist3d_pt_ptarray(const POINT3DZ *p, const POINTARRAY *pa, DISTPTS3D *dl)
{
        uint32_t t;
        POINT3DZ start, end;
        int twist = dl->twisted;
        if (!pa)
                return LW_FALSE;
 
        getPoint3dz_p(pa, 0, &start);
 
        for (t = 1; t < pa->npoints; t++)
        {
                dl->twisted = twist;
                getPoint3dz_p(pa, t, &end);
                if (!lw_dist3d_pt_seg(p, &start, &end, dl))
                        return LW_FALSE;
 
                if (dl->distance <= dl->tolerance && dl->mode == DIST_MIN)
                        return LW_TRUE; /*just a check if  the answer is already given*/
                start = end;
        }
 
        return LW_TRUE;
}
 
int
lw_dist3d_pt_seg(const POINT3DZ *p, const POINT3DZ *A, const POINT3DZ *B, DISTPTS3D *dl)
{
        POINT3DZ c;
        double r;
        /*if start==end, then use pt distance */
        if ((A->x == B->x) && (A->y == B->y) && (A->z == B->z))
        {
                return lw_dist3d_pt_pt(p, A, dl);
        }
 
        r = ((p->x - A->x) * (B->x - A->x) + (p->y - A->y) * (B->y - A->y) + (p->z - A->z) * (B->z - A->z)) /
            ((B->x - A->x) * (B->x - A->x) + (B->y - A->y) * (B->y - A->y) + (B->z - A->z) * (B->z - A->z));
 
        /*This is for finding the 3Dmaxdistance.
        the maxdistance have to be between two vertices,
        compared to mindistance which can be between
        two vertices vertex.*/
        if (dl->mode == DIST_MAX)
        {
                if (r >= 0.5)
                        return lw_dist3d_pt_pt(p, A, dl);
                if (r < 0.5)
                        return lw_dist3d_pt_pt(p, B, dl);
        }
 
        if (r <= 0) /*If the first vertex A is closest to the point p*/
                return lw_dist3d_pt_pt(p, A, dl);
        if (r >= 1) /*If the second vertex B is closest to the point p*/
                return lw_dist3d_pt_pt(p, B, dl);
 
        /*else if the point p is closer to some point between a and b
        then we find that point and send it to lw_dist3d_pt_pt*/
        c.x = A->x + r * (B->x - A->x);
        c.y = A->y + r * (B->y - A->y);
        c.z = A->z + r * (B->z - A->z);
 
        return lw_dist3d_pt_pt(p, &c, dl);
}
 
double
distance3d_pt_pt(const POINT3D *p1, const POINT3D *p2)
{
        double dx = p2->x - p1->x;
        double dy = p2->y - p1->y;
        double dz = p2->z - p1->z;
        return sqrt(dx * dx + dy * dy + dz * dz);
}
 
int
lw_dist3d_pt_pt(const POINT3DZ *thep1, const POINT3DZ *thep2, DISTPTS3D *dl)
{
        double dx = thep2->x - thep1->x;
        double dy = thep2->y - thep1->y;
        double dz = thep2->z - thep1->z;
        double dist = sqrt(dx * dx + dy * dy + dz * dz);
        LWDEBUGF(2,
                 "lw_dist3d_pt_pt called (with points: p1.x=%f, p1.y=%f,p1.z=%f,p2.x=%f, p2.y=%f,p2.z=%f)",
                 thep1->x,
                 thep1->y,
                 thep1->z,
                 thep2->x,
                 thep2->y,
                 thep2->z);
 
        if (((dl->distance - dist) * (dl->mode)) >
            0) /*multiplication with mode to handle mindistance (mode=1)  and maxdistance (mode = (-1)*/
        {
                dl->distance = dist;
 
                if (dl->twisted > 0) /*To get the points in right order. twisted is updated between 1 and (-1) every
                                        time the order is changed earlier in the chain*/
                {
                        dl->p1 = *thep1;
                        dl->p2 = *thep2;
                }
                else
                {
                        dl->p1 = *thep2;
                        dl->p2 = *thep1;
                }
        }
        return LW_TRUE;
}
 
int
lw_dist3d_ptarray_ptarray(const POINTARRAY *l1, const POINTARRAY *l2, DISTPTS3D *dl)
{
        uint32_t t, u;
        POINT3DZ start, end;
        POINT3DZ start2, end2;
        int twist = dl->twisted;
        LWDEBUGF(2, "lw_dist3d_ptarray_ptarray called (points: %d-%d)", l1->npoints, l2->npoints);
 
        if (dl->mode == DIST_MAX) /*If we are searching for maxdistance we go straight to point-point calculation since
                                     the maxdistance have to be between two vertices*/
        {
                for (t = 0; t < l1->npoints; t++) /*for each segment in L1 */
                {
                        getPoint3dz_p(l1, t, &start);
                        for (u = 0; u < l2->npoints; u++) /*for each segment in L2 */
                        {
                                getPoint3dz_p(l2, u, &start2);
                                lw_dist3d_pt_pt(&start, &start2, dl);
                        }
                }
        }
        else
        {
                getPoint3dz_p(l1, 0, &start);
                for (t = 1; t < l1->npoints; t++) /*for each segment in L1 */
                {
                        getPoint3dz_p(l1, t, &end);
                        getPoint3dz_p(l2, 0, &start2);
                        for (u = 1; u < l2->npoints; u++) /*for each segment in L2 */
                        {
                                getPoint3dz_p(l2, u, &end2);
                                dl->twisted = twist;
                                lw_dist3d_seg_seg(&start, &end, &start2, &end2, dl);
                                LWDEBUGF(
                                    4, "mindist_ptarray_ptarray; seg %i * seg %i, dist = %g\n", t, u, dl->distance);
                                LWDEBUGF(3, " seg%d-seg%d dist: %f, mindist: %f", t, u, dl->distance, dl->tolerance);
                                if (dl->distance <= dl->tolerance && dl->mode == DIST_MIN)
                                        return LW_TRUE; /*just a check if the answer is already given*/
                                start2 = end2;
                        }
                        start = end;
                }
        }
        return LW_TRUE;
}
 
int
lw_dist3d_seg_seg(const POINT3DZ *s1p1, const POINT3DZ *s1p2, const POINT3DZ *s2p1, const POINT3DZ *s2p2, DISTPTS3D *dl)
{
        VECTOR3D v1, v2, vl;
        double s1k, s2k; /*two variables representing where on Line 1 (s1k) and where on Line 2 (s2k) a connecting line
                            between the two lines is perpendicular to both lines*/
        POINT3DZ p1, p2;
        double a, b, c, d, e, D;
 
        /*s1p1 and s1p2 are the same point */
        if (p3dz_same(s1p1, s1p2))
        {
                return lw_dist3d_pt_seg(s1p1, s2p1, s2p2, dl);
        }
        /*s2p1 and s2p2 are the same point */
        if (p3dz_same(s2p1, s2p2))
        {
                dl->twisted = ((dl->twisted) * (-1));
                return lw_dist3d_pt_seg(s2p1, s1p1, s1p2, dl);
        }
        /*s2p1 and s1p1 are the same point */
        if (p3dz_same(s2p1, s1p1))
        {
                dl->distance = 0.0;
                dl->p1 = dl->p2 = *s2p1;
                return LW_TRUE;
        }
        /*
                Here we use algorithm from softsurfer.com
                that can be found here
                http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
        */
 
        if (!get_3dvector_from_points(s1p1, s1p2, &v1))
                return LW_FALSE;
 
        if (!get_3dvector_from_points(s2p1, s2p2, &v2))
                return LW_FALSE;
 
        if (!get_3dvector_from_points(s2p1, s1p1, &vl))
                return LW_FALSE;
 
        a = DOT(v1, v1);
        b = DOT(v1, v2);
        c = DOT(v2, v2);
        d = DOT(v1, vl);
        e = DOT(v2, vl);
        D = a * c - b * b;
 
        if (D < 0.000000001)
        { /* the lines are almost parallel*/
                s1k =
                    0.0;   /*If the lines are parallel we try by using the startpoint of first segment. If that gives a
                              projected point on the second line outside segment 2 it will be found that s2k is >1 or <0.*/
                if (b > c) /* use the largest denominator*/
                        s2k = d / b;
                else
                        s2k = e / c;
        }
        else
        {
                s1k = (b * e - c * d) / D;
                s2k = (a * e - b * d) / D;
        }
 
        /* Now we check if the projected closest point on the infinite lines is outside our segments. If so the
         * combinations with start and end points will be tested*/
 
        if (s1k <= 0.0 || s1k >= 1.0 || s2k <= 0.0 || s2k >= 1.0)
        {
                if (s1k <= 0.0)
                {
                        if (!lw_dist3d_pt_seg(s1p1, s2p1, s2p2, dl))
                                return LW_FALSE;
                }
                if (s1k >= 1.0)
                {
                        if (!lw_dist3d_pt_seg(s1p2, s2p1, s2p2, dl))
                                return LW_FALSE;
                }
                if (s2k <= 0.0)
                {
                        dl->twisted = ((dl->twisted) * (-1));
                        if (!lw_dist3d_pt_seg(s2p1, s1p1, s1p2, dl))
                                return LW_FALSE;
                }
                if (s2k >= 1.0)
                {
                        dl->twisted = ((dl->twisted) * (-1));
                        if (!lw_dist3d_pt_seg(s2p2, s1p1, s1p2, dl))
                                return LW_FALSE;
                }
        }
        else
        { /*Find the closest point on the edges of both segments*/
                p1.x = s1p1->x + s1k * (s1p2->x - s1p1->x);
                p1.y = s1p1->y + s1k * (s1p2->y - s1p1->y);
                p1.z = s1p1->z + s1k * (s1p2->z - s1p1->z);
 
                p2.x = s2p1->x + s2k * (s2p2->x - s2p1->x);
                p2.y = s2p1->y + s2k * (s2p2->y - s2p1->y);
                p2.z = s2p1->z + s2k * (s2p2->z - s2p1->z);
 
                if (!lw_dist3d_pt_pt(&p1, &p2, dl)) /* Send the closest points to point-point calculation*/
                {
                        return LW_FALSE;
                }
        }
        return LW_TRUE;
}
 
int
lw_dist3d_pt_poly(const POINT3DZ *p, const LWPOLY *poly, PLANE3D *plane, POINT3DZ *projp, DISTPTS3D *dl)
{
        uint32_t i;
 
        if (pt_in_ring_3d(projp, poly->rings[0], plane))
        {
                for (i = 1; i < poly->nrings; i++)
                {
                        /* Inside a hole. Distance = pt -> ring */
                        if (pt_in_ring_3d(projp, poly->rings[i], plane))
                                return lw_dist3d_pt_ptarray(p, poly->rings[i], dl);
                }
 
                /* if the projected point is inside the polygon the shortest distance is between that point and the
                 * input point */
                return lw_dist3d_pt_pt(p, projp, dl);
        }
        else
                /* if the projected point is outside the polygon we search for the closest distance against the boundary
                 * instead */
                return lw_dist3d_pt_ptarray(p, poly->rings[0], dl);
 
        return LW_TRUE;
}
 
int
lw_dist3d_pt_tri(const POINT3DZ *p, const LWTRIANGLE *tri, PLANE3D *plane, POINT3DZ *projp, DISTPTS3D *dl)
{
        if (pt_in_ring_3d(projp, tri->points, plane))
                /* if the projected point is inside the polygon the shortest distance is between that point and the
                 * input point */
                return lw_dist3d_pt_pt(p, projp, dl);
        else
                /* if the projected point is outside the polygon we search for the closest distance against the boundary
                 * instead */
                return lw_dist3d_pt_ptarray(p, tri->points, dl);
 
        return LW_TRUE;
}
 
int
lw_dist3d_ptarray_poly(const POINTARRAY *pa, const LWPOLY *poly, PLANE3D *plane, DISTPTS3D *dl)
{
        uint32_t i, j, k;
        double f, s1, s2;
        VECTOR3D projp1_projp2;
        POINT3DZ p1, p2, projp1, projp2, intersectionp;
 
        getPoint3dz_p(pa, 0, &p1);
 
        /* the sign of s1 tells us on which side of the plane the point is. */
        s1 = project_point_on_plane(&p1, plane, &projp1);
        lw_dist3d_pt_poly(&p1, poly, plane, &projp1, dl);
        if ((s1 == 0.0) && (dl->distance < dl->tolerance))
                return LW_TRUE;
 
        for (i = 1; i < pa->npoints; i++)
        {
                int intersects;
                getPoint3dz_p(pa, i, &p2);
                s2 = project_point_on_plane(&p2, plane, &projp2);
                lw_dist3d_pt_poly(&p2, poly, plane, &projp2, dl);
                if ((s2 == 0.0) && (dl->distance < dl->tolerance))
                        return LW_TRUE;
 
                /* If s1 and s2 has different signs that means they are on different sides of the plane of the polygon.
                 * That means that the edge between the points crosses the plane and might intersect with the polygon */
                if ((s1 * s2) < 0)
                {
                        /* The size of s1 and s2 is the distance from the point to the plane. */
                        f = fabs(s1) / (fabs(s1) + fabs(s2));
                        get_3dvector_from_points(&projp1, &projp2, &projp1_projp2);
 
                        /* Get the point where the line segment crosses the plane */
                        intersectionp.x = projp1.x + f * projp1_projp2.x;
                        intersectionp.y = projp1.y + f * projp1_projp2.y;
                        intersectionp.z = projp1.z + f * projp1_projp2.z;
 
                        /* We set intersects to true until the opposite is proved */
                        intersects = LW_TRUE;
 
                        if (pt_in_ring_3d(&intersectionp, poly->rings[0], plane)) /*Inside outer ring*/
                        {
                                for (k = 1; k < poly->nrings; k++)
                                {
                                        /* Inside a hole, so no intersection with the polygon*/
                                        if (pt_in_ring_3d(&intersectionp, poly->rings[k], plane))
                                        {
                                                intersects = LW_FALSE;
                                                break;
                                        }
                                }
                                if (intersects)
                                {
                                        dl->distance = 0.0;
                                        dl->p1.x = intersectionp.x;
                                        dl->p1.y = intersectionp.y;
                                        dl->p1.z = intersectionp.z;
 
                                        dl->p2.x = intersectionp.x;
                                        dl->p2.y = intersectionp.y;
                                        dl->p2.z = intersectionp.z;
                                        return LW_TRUE;
                                }
                        }
                }
 
                projp1 = projp2;
                s1 = s2;
                p1 = p2;
        }
 
        /* check our pointarray against boundary and inner boundaries of the polygon */
        for (j = 0; j < poly->nrings; j++)
                lw_dist3d_ptarray_ptarray(pa, poly->rings[j], dl);
 
        return LW_TRUE;
}
 
int
lw_dist3d_ptarray_tri(const POINTARRAY *pa, const LWTRIANGLE *tri, PLANE3D *plane, DISTPTS3D *dl)
{
        uint32_t i;
        double f, s1, s2;
        VECTOR3D projp1_projp2;
        POINT3DZ p1, p2, projp1, projp2, intersectionp;
 
        getPoint3dz_p(pa, 0, &p1);
 
        /* the sign of s1 tells us on which side of the plane the point is. */
        s1 = project_point_on_plane(&p1, plane, &projp1);
        lw_dist3d_pt_tri(&p1, tri, plane, &projp1, dl);
        if ((s1 == 0.0) && (dl->distance < dl->tolerance))
                return LW_TRUE;
 
        for (i = 1; i < pa->npoints; i++)
        {
                int intersects;
                getPoint3dz_p(pa, i, &p2);
                s2 = project_point_on_plane(&p2, plane, &projp2);
                lw_dist3d_pt_tri(&p2, tri, plane, &projp2, dl);
                if ((s2 == 0.0) && (dl->distance < dl->tolerance))
                        return LW_TRUE;
 
                /* If s1 and s2 has different signs that means they are on different sides of the plane of the triangle.
                 * That means that the edge between the points crosses the plane and might intersect with the triangle
                 */
                if ((s1 * s2) < 0)
                {
                        /* The size of s1 and s2 is the distance from the point to the plane. */
                        f = fabs(s1) / (fabs(s1) + fabs(s2));
                        get_3dvector_from_points(&projp1, &projp2, &projp1_projp2);
 
                        /* Get the point where the line segment crosses the plane */
                        intersectionp.x = projp1.x + f * projp1_projp2.x;
                        intersectionp.y = projp1.y + f * projp1_projp2.y;
                        intersectionp.z = projp1.z + f * projp1_projp2.z;
 
                        /* We set intersects to true until the opposite is proved */
                        intersects = LW_TRUE;
 
                        if (pt_in_ring_3d(&intersectionp, tri->points, plane)) /*Inside outer ring*/
                        {
                                if (intersects)
                                {
                                        dl->distance = 0.0;
                                        dl->p1.x = intersectionp.x;
                                        dl->p1.y = intersectionp.y;
                                        dl->p1.z = intersectionp.z;
 
                                        dl->p2.x = intersectionp.x;
                                        dl->p2.y = intersectionp.y;
                                        dl->p2.z = intersectionp.z;
                                        return LW_TRUE;
                                }
                        }
                }
 
                projp1 = projp2;
                s1 = s2;
                p1 = p2;
        }
 
        /* check our pointarray against triangle contour */
        lw_dist3d_ptarray_ptarray(pa, tri->points, dl);
        return LW_TRUE;
}
 
/* Here we define the plane of a polygon (boundary pointarray of a polygon)
 * the plane is stored as a point in plane (plane.pop) and a normal vector (plane.pv)
 *
 * We are calculating the average point and using it to break the polygon into
 * POL_BREAKS (3) parts. Then we calculate the normal of those 3 vectors and
 * use its average to define the normal of the plane.This is done to minimize
 * the error introduced by FP precision
 * We use [3] breaks to contemplate the special case of 3d triangles
 */
int
define_plane(const POINTARRAY *pa, PLANE3D *pl)
{
        const uint32_t POL_BREAKS = 3;
 
        assert(pa);
        assert(pa->npoints > 3);
        if (!pa)
                return LW_FALSE;
 
        uint32_t unique_points = pa->npoints - 1;
        uint32_t i;
 
        if (pa->npoints < 3)
                return LW_FALSE;
 
        /* Calculate the average point */
        pl->pop.x = 0.0;
        pl->pop.y = 0.0;
        pl->pop.z = 0.0;
        for (i = 0; i < unique_points; i++)
        {
                POINT3DZ p;
                getPoint3dz_p(pa, i, &p);
                pl->pop.x += p.x;
                pl->pop.y += p.y;
                pl->pop.z += p.z;
        }
 
        pl->pop.x /= unique_points;
        pl->pop.y /= unique_points;
        pl->pop.z /= unique_points;
 
        pl->pv.x = 0.0;
        pl->pv.y = 0.0;
        pl->pv.z = 0.0;
        for (i = 0; i < POL_BREAKS; i++)
        {
                POINT3DZ point1, point2;
                VECTOR3D v1, v2, vp;
                uint32_t n1, n2;
                n1 = i * unique_points / POL_BREAKS;
                n2 = n1 + unique_points / POL_BREAKS; /* May overflow back to the first / last point */
 
                if (n1 == n2)
                        continue;
 
                getPoint3dz_p(pa, n1, &point1);
                if (!get_3dvector_from_points(&pl->pop, &point1, &v1))
                        continue;
 
                getPoint3dz_p(pa, n2, &point2);
                if (!get_3dvector_from_points(&pl->pop, &point2, &v2))
                        continue;
 
                if (get_3dcross_product(&v1, &v2, &vp))
                {
                        /* If the cross product isn't zero these 3 points aren't collinear
                         * We divide by its lengthsq to normalize the additions */
                        double vl = vp.x * vp.x + vp.y * vp.y + vp.z * vp.z;
                        pl->pv.x += vp.x / vl;
                        pl->pv.y += vp.y / vl;
                        pl->pv.z += vp.z / vl;
                }
        }
 
        return (!FP_IS_ZERO(pl->pv.x) || !FP_IS_ZERO(pl->pv.y) || !FP_IS_ZERO(pl->pv.z));
}
 
 
int
pt_in_ring_3d(const POINT3DZ *p, const POINTARRAY *ring, PLANE3D *plane)
{
 
        uint32_t cn = 0; /* the crossing number counter */
        uint32_t i;
        POINT3DZ v1, v2;
 
        POINT3DZ first, last;
 
        getPoint3dz_p(ring, 0, &first);
        getPoint3dz_p(ring, ring->npoints - 1, &last);
        if (memcmp(&first, &last, sizeof(POINT3DZ)))
        {
                lwerror("pt_in_ring_3d: V[n] != V[0] (%g %g %g!= %g %g %g)",
                        first.x,
                        first.y,
                        first.z,
                        last.x,
                        last.y,
                        last.z);
                return LW_FALSE;
        }
 
        LWDEBUGF(2, "pt_in_ring_3d called with point: %g %g %g", p->x, p->y, p->z);
        /* printPA(ring); */
 
        /* loop through all edges of the polygon */
        getPoint3dz_p(ring, 0, &v1);
 
        if (fabs(plane->pv.z) >= fabs(plane->pv.x) &&
            fabs(plane->pv.z) >= fabs(plane->pv.y)) /*If the z vector of the normal vector to the plane is larger than x
                                                       and y vector we project the ring to the xy-plane*/
        {
                for (i = 0; i < ring->npoints - 1; i++)
                {
                        double vt;
                        getPoint3dz_p(ring, i + 1, &v2);
 
                        /* edge from vertex i to vertex i+1 */
                        if (
                            /* an upward crossing */
                            ((v1.y <= p->y) && (v2.y > p->y))
                            /* a downward crossing */
                            || ((v1.y > p->y) && (v2.y <= p->y)))
                        {
 
                                vt = (double)(p->y - v1.y) / (v2.y - v1.y);
 
                                /* P.x <intersect */
                                if (p->x < v1.x + vt * (v2.x - v1.x))
                                {
                                        /* a valid crossing of y=p.y right of p.x */
                                        ++cn;
                                }
                        }
                        v1 = v2;
                }
        }
        else if (fabs(plane->pv.y) >= fabs(plane->pv.x) &&
                 fabs(plane->pv.y) >= fabs(plane->pv.z)) /*If the y vector of the normal vector to the plane is larger
                                                            than x and z vector we project the ring to the xz-plane*/
        {
                for (i = 0; i < ring->npoints - 1; i++)
                {
                        double vt;
                        getPoint3dz_p(ring, i + 1, &v2);
 
                        /* edge from vertex i to vertex i+1 */
                        if (
                            /* an upward crossing */
                            ((v1.z <= p->z) && (v2.z > p->z))
                            /* a downward crossing */
                            || ((v1.z > p->z) && (v2.z <= p->z)))
                        {
 
                                vt = (double)(p->z - v1.z) / (v2.z - v1.z);
 
                                /* P.x <intersect */
                                if (p->x < v1.x + vt * (v2.x - v1.x))
                                {
                                        /* a valid crossing of y=p.y right of p.x */
                                        ++cn;
                                }
                        }
                        v1 = v2;
                }
        }
        else /*Hopefully we only have the cases where x part of the normal vector is largest left*/
        {
                for (i = 0; i < ring->npoints - 1; i++)
                {
                        double vt;
                        getPoint3dz_p(ring, i + 1, &v2);
 
                        /* edge from vertex i to vertex i+1 */
                        if (
                            /* an upward crossing */
                            ((v1.z <= p->z) && (v2.z > p->z))
                            /* a downward crossing */
                            || ((v1.z > p->z) && (v2.z <= p->z)))
                        {
 
                                vt = (double)(p->z - v1.z) / (v2.z - v1.z);
 
                                /* P.x <intersect */
                                if (p->y < v1.y + vt * (v2.y - v1.y))
                                {
                                        /* a valid crossing of y=p.y right of p.x */
                                        ++cn;
                                }
                        }
                        v1 = v2;
                }
        }
        LWDEBUGF(3, "pt_in_ring_3d returning %d", cn & 1);
 
        return (cn & 1); /* 0 if even (out), and 1 if odd (in) */
}
 
/*------------------------------------------------------------------------------------------------------------
End of Brute force functions
--------------------------------------------------------------------------------------------------------------*/