// This file is part of the AliceVision project.

// Copyright (c) 2017 AliceVision contributors.

// This Source Code Form is subject to the terms of the Mozilla Public License,

// v. 2.0. If a copy of the MPL was not distributed with this file,

// You can obtain one at https://mozilla.org/MPL/2.0/.

#include "geometry.hpp"

#include <aliceVision/mvsData/geometryTriTri.hpp>

#include <cfloat>

namespace aliceVision {

double pointLineDistance3D(const Point3d& point, const Point3d& linePoint, const Point3d& lineVectNormalized)

{

return cross(lineVectNormalized, linePoint - point).size();

}

Point3d closestPointToLine3D(const Point3d* point, const Point3d* linePoint, const Point3d* lineVectNormalized)

{

return (*linePoint) + (*lineVectNormalized) * dot((*lineVectNormalized), (*point) - (*linePoint));

}

double pointPlaneDistance(const Point3d& point, const Point3d& planePoint, const Point3d& planeNormal)

{

return fabs(dot(point, planeNormal) - dot(planePoint, planeNormal)) / sqrt(dot(planeNormal, planeNormal));

}

double orientedPointPlaneDistance(const Point3d& point, const Point3d& planePoint, const Point3d& planeNormal)

{

return (dot(point, planeNormal) - dot(planePoint, planeNormal)) / std::sqrt(dot(planeNormal, planeNormal));

}

void computeRotCS(Point3d* xax, Point3d* yax, const Point3d* n)

{

xax->x = -n->y + n->z; // get any cross product

xax->y = +n->x + n->z;

xax->z = -n->x - n->y;

if(fabs(xax->x) < 0.0000001 && fabs(xax->y) < 0.0000001 && fabs(xax->z) < 0.0000001)

{

xax->x = -n->y - n->z; // get any cross product (complementar)

xax->y = +n->x - n->z;

xax->z = +n->x + n->y;

}

*xax = xax->normalize();

*yax = cross(*n, *xax);

}

bool lineLineIntersect(double* k, double* l, Point3d* llis, Point3d* lli1, Point3d* lli2, const Point3d& p1,

const Point3d& p2, const Point3d& p3, const Point3d& p4)

{

/*

% [pa, pb, mua, mub] = LineLineIntersect(p1,p2,p3,p4)

%

% Calculates the line segment pa_pb that is the shortest route

% between two lines p1_p2 and p3_p4. Calculates also the values of

% mua and mub where

% pa = p1 + mua (p2 - p1)

% pb = p3 + mub (p4 - p3)

%

% Returns a MATLAB error if no solution exists.

%

% This a simple conversion to MATLAB of the C code posted by Paul

% Bourke at

% http://astronomy.swin.edu.au/~pbourke/geometry/lineline3d/. The

% author of this all too imperfect translation is Cristian Dima

% (csd@cmu.edu)

*/

double d1343, d4321, d1321, d4343, d2121, denom, numer, p13[3], p43[3], p21[3], pa[3], pb[3], muab[2];

p13[0] = p1.x - p3.x;

p13[1] = p1.y - p3.y;

p13[2] = p1.z - p3.z;

p43[0] = p4.x - p3.x;

p43[1] = p4.y - p3.y;

p43[2] = p4.z - p3.z;

if((fabs(p43[0]) < FLT_EPSILON) && (fabs(p43[1]) < FLT_EPSILON) && (fabs(p43[2]) < FLT_EPSILON))

{

return false;

}

p21[0] = p2.x - p1.x;

p21[1] = p2.y - p1.y;

p21[2] = p2.z - p1.z;

if((fabs(p21[0]) < FLT_EPSILON) && (fabs(p21[1]) < FLT_EPSILON) && (fabs(p21[2]) < FLT_EPSILON))

{

return false;

}

d1343 = p13[0] * p43[0] + p13[1] * p43[1] + p13[2] * p43[2];

d4321 = p43[0] * p21[0] + p43[1] * p21[1] + p43[2] * p21[2];

d1321 = p13[0] * p21[0] + p13[1] * p21[1] + p13[2] * p21[2];

d4343 = p43[0] * p43[0] + p43[1] * p43[1] + p43[2] * p43[2];

d2121 = p21[0] * p21[0] + p21[1] * p21[1] + p21[2] * p21[2];

denom = d2121 * d4343 - d4321 * d4321;

if(fabs(denom) < FLT_EPSILON)

{

return false;

}

numer = d1343 * d4321 - d1321 * d4343;

muab[0] = numer / denom;

muab[1] = (d1343 + d4321 * muab[0]) / d4343;

pa[0] = p1.x + muab[0] * p21[0];

pa[1] = p1.y + muab[0] * p21[1];

pa[2] = p1.z + muab[0] * p21[2];

pb[0] = p3.x + muab[1] * p43[0];

pb[1] = p3.y + muab[1] * p43[1];

pb[2] = p3.z + muab[1] * p43[2];

Point3d S;

llis->x = (pa[0] + pb[0]) / 2.0;

llis->y = (pa[1] + pb[1]) / 2.0;

llis->z = (pa[2] + pb[2]) / 2.0;

*k = muab[0];

*l = muab[1];

lli1->x = pa[0];

lli1->y = pa[1];

lli1->z = pa[2];

lli2->x = pb[0];

lli2->y = pb[1];

lli2->z = pb[2];

return true;

}

bool lineLineIntersect(Point3d& out, const Point3d& p1, const Point3d& v1, const Point3d& p2, const Point3d& v2)

{

double k, l;

Point3d lli1, lli2;

Point3d p1v1 = p1 + v1;

Point3d p2v2 = p2 + v2;

return lineLineIntersect(&k, &l, &out, &lli1, &lli2, p1, p1v1, p2, p2v2);

}

Point3d linePlaneIntersect(const Point3d& linePoint, const Point3d& lineVect, const Point3d& planePoint,

const Point3d& planeNormal)

{

const double planePoint_x = planePoint.x;

const double planePoint_y = planePoint.y;

const double planePoint_z = planePoint.z;

const double planeNormal_x = planeNormal.x;

const double planeNormal_y = planeNormal.y;

const double planeNormal_z = planeNormal.z;

const double linePoint_x = linePoint.x;

const double linePoint_y = linePoint.y;

const double linePoint_z = linePoint.z;

const double lineVect_x = lineVect.x;

const double lineVect_y = lineVect.y;

const double lineVect_z = lineVect.z;

const double k = ((planePoint_x * planeNormal_x + planePoint_y * planeNormal_y + planePoint_z * planeNormal_z) - (planeNormal_x * linePoint_x + planeNormal_y * linePoint_y + planeNormal_z * linePoint_z)) / (planeNormal_x * lineVect_x + planeNormal_y * lineVect_y + planeNormal_z * lineVect_z);

return linePoint + lineVect * k;

//---KP---double k = (dot(planePoint, planeNormal) - dot(planeNormal, linePoint)) / dot(planeNormal, lineVect);

//---KP---return linePoint + lineVect * k;

}

void linePlaneIntersect(Point3d* out, const Point3d* linePoint, const Point3d* lineVect, const Point3d* planePoint,

const Point3d* planeNormal)

{

const double planePoint_x = planePoint->x;

const double planePoint_y = planePoint->y;

const double planePoint_z = planePoint->z;

const double planeNormal_x = planeNormal->x;

const double planeNormal_y = planeNormal->y;

const double planeNormal_z = planeNormal->z;

const double linePoint_x = linePoint->x;

const double linePoint_y = linePoint->y;

const double linePoint_z = linePoint->z;

const double lineVect_x = lineVect->x;

const double lineVect_y = lineVect->y;

const double lineVect_z = lineVect->z;

const double k = ((planePoint_x * planeNormal_x + planePoint_y * planeNormal_y + planePoint_z * planeNormal_z) - (planeNormal_x * linePoint_x + planeNormal_y * linePoint_y + planeNormal_z * linePoint_z)) / (planeNormal_x * lineVect_x + planeNormal_y * lineVect_y + planeNormal_z * lineVect_z);

out->x = linePoint_x + (lineVect_x) * k;

out->y = linePoint_y + (lineVect_y) * k;

out->z = linePoint_z + (lineVect_z) * k;

//---KP---double k = (dot(*planePoint, *planeNormal) - dot(*planeNormal, *linePoint)) / dot(*planeNormal, *lineVect);

//---KP---*out = *linePoint + (*lineVect) * k;

}

// this angle is always between 0 and 180

double angleBetwV1andV2(const Point3d& iV1, const Point3d& iV2)

{

Point3d V1, V2;

V1 = iV1.normalize();

V2 = iV2.normalize();

const double a = acos((double)(V1.x * V2.x + V1.y * V2.y + V1.z * V2.z));

if(std::isnan(a))

return 0.0;

return fabs(a / (M_PI / 180.0));

}

double angleBetwABandAC(const Point3d& A, const Point3d& B, const Point3d& C)

{

Point3d V1 = B - A;

Point3d V2 = C - A;

return angleBetwV1andV2(V1, V2);

}

void rotPointAroundVect(double* out, const double* X, const double* vect, const double angle)

{

double ux, uy, uz, vx, vy, vz, wx, wy, wz, sa, ca, x, y, z, u, v, w;

double sizeX = sqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2]);

x = X[0] / sizeX;

y = X[1] / sizeX;

z = X[2] / sizeX;

u = vect[0];

v = vect[1];

w = vect[2];

/*Rotate the point (x,y,z) around the vector (u,v,w)*/

ux = u * x;

uy = u * y;

uz = u * z;

vx = v * x;

vy = v * y;

vz = v * z;

wx = w * x;

wy = w * y;

wz = w * z;

sa = sin((double)angle * (M_PI / 180.0));

ca = cos((double)angle * (M_PI / 180.0));

x = u * (ux + vy + wz) + (x * (v * v + w * w) - u * (vy + wz)) * ca + (-wy + vz) * sa;

y = v * (ux + vy + wz) + (y * (u * u + w * w) - v * (ux + wz)) * ca + (wx - uz) * sa;

z = w * (ux + vy + wz) + (z * (u * u + v * v) - w * (ux + vy)) * ca + (-vx + uy) * sa;

u = sqrt(x * x + y * y + z * z);

x /= u;

y /= u;

z /= u;

out[0] = x * sizeX;

out[1] = y * sizeX;

out[2] = z * sizeX;

}

void rotPointAroundVect(Point3d* out, const Point3d* X, const Point3d* vect, const double angle)

{

double o[3];

double dX[3] = {(double)X->x, (double)X->y, (double)X->z};

double dvect[3] = {(double)vect->x, (double)vect->y, (double)vect->z};

rotPointAroundVect(o, dX, dvect, angle);

out->x = o[0];

out->y = o[1];

out->z = o[2];

}

Point2d getLineTriangleIntersectBarycCoords(Point3d* P, const Point3d* A, const Point3d* B, const Point3d* C,

const Point3d* linePoint, const Point3d* lineVect)

{

// flat code instead of all functions calls to improve performances

const double A_x = A->x;

const double A_y = A->y;

const double A_z = A->z;

const double linePoint_x = linePoint->x;

const double linePoint_y = linePoint->y;

const double linePoint_z = linePoint->z;

const double lineVect_x = lineVect->x;

const double lineVect_y = lineVect->y;

const double lineVect_z = lineVect->z;

const double v0_x = C->x - A_x;

const double v0_y = C->y - A_y;

const double v0_z = C->z - A_z;

const double v1_x = B->x - A_x;

const double v1_y = B->y - A_y;

const double v1_z = B->z - A_z;

const double _n_x = v0_y * v1_z - v0_z * v1_y;

const double _n_y = v0_z * v1_x - v0_x * v1_z;

const double _n_z = v0_x * v1_y - v0_y * v1_x;

const double k = ((A_x * _n_x + A_y * _n_y + A_z * _n_z) - (_n_x * linePoint_x + _n_y * linePoint_y + _n_z * linePoint_z)) / (_n_x * lineVect_x + _n_y * lineVect_y + _n_z * lineVect_z);

const double P_x = linePoint_x + lineVect_x * k;

const double P_y = linePoint_y + lineVect_y * k;

const double P_z = linePoint_z + lineVect_z * k;

// Compute vectors

const double v2_x = P_x - A_x;

const double v2_y = P_y - A_y;

const double v2_z = P_z - A_z;

// Compute dot products

const double dot00 = (v0_x * v0_x + v0_y * v0_y + v0_z * v0_z);

const double dot01 = (v0_x * v1_x + v0_y * v1_y + v0_z * v1_z);

const double dot02 = (v0_x * v2_x + v0_y * v2_y + v0_z * v2_z);

const double dot11 = (v1_x * v1_x + v1_y * v1_y + v1_z * v1_z);

const double dot12 = (v1_x * v2_x + v1_y * v2_y + v1_z * v2_z);

// Compute barycentric coordinates

const double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);

const double u = (dot11 * dot02 - dot01 * dot12) * invDenom;

const double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

P->x = P_x;

P->y = P_y;

P->z = P_z;

/*

const Point3d v0 = *C - *A;

const Point3d v1 = *B - *A;

const Point3d n = cross(v0.normalize(), v1.normalize()).normalize();

linePlaneIntersect(P, linePoint, lineVect, A, &n);

// Compute vectors

const Point3d v2 = *P - *A;

// Compute dot products

const double dot00 = dot(v0, v0);

const double dot01 = dot(v0, v1);

const double dot02 = dot(v0, v2);

const double dot11 = dot(v1, v1);

const double dot12 = dot(v1, v2);

// Compute barycentric coordinates

const double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);

const double u = (dot11 * dot02 - dot01 * dot12) * invDenom;

const double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

*/

// Check if point is in triangle

return Point2d(u, v);

}

bool isLineInTriangle(Point3d* P, const Point3d* A, const Point3d* B, const Point3d* C, const Point3d* linePoint,

const Point3d* lineVect)

{

return isPointInTriangle(getLineTriangleIntersectBarycCoords(P, A, B, C, linePoint, lineVect));

}

bool isLineSegmentInTriangle(Point3d& lpi, const Point3d& A, const Point3d& B, const Point3d& C,

const Point3d& linePoint1, const Point3d& linePoint2)

{

Point3d v0 = C - A;

Point3d v1 = B - A;

Point3d n = cross(v0.normalize(), v1.normalize()).normalize();

Point3d lineVect = linePoint2 - linePoint1;

// linePlaneIntersect(&P, linePoint, lineVect, A, &n);

const double k = (dot(A, n) - dot(n, linePoint1)) / dot(n, lineVect);

lpi = linePoint1 + lineVect * k;

// Compute vectors

const Point3d v2 = lpi - A;

// Compute dot products

const double dot00 = dot(v0, v0);

const double dot01 = dot(v0, v1);

const double dot02 = dot(v0, v2);

const double dot11 = dot(v1, v1);

const double dot12 = dot(v1, v2);

// Compute barycentric coordinates

const double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);

const double u = (dot11 * dot02 - dot01 * dot12) * invDenom;

const double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

// Check if point is in triangle

return (k >= 0.0) && (k <= 1.0) && (u > 0.0) && (v > 0.0) && (u + v < 1.0);

}

// P = A + u * (C - A) + v * (B - A)

Point2d computeBarycentricCoordinates(const Point2d& A, const Point2d& B, const Point2d& C, const Point2d& P)

{

// http://www.blackpawn.com/texts/pointinpoly/default.html

const Point2d v0 = C - A;

const Point2d v1 = B - A;

const Point2d v2 = P - A;

// Compute dot products

const double dot00 = dot(v0, v0);

const double dot01 = dot(v0, v1);

const double dot02 = dot(v0, v2);

const double dot11 = dot(v1, v1);

const double dot12 = dot(v1, v2);

// Compute barycentric coordinates

const double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);

const double u = (dot11 * dot02 - dot01 * dot12) * invDenom;

const double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

return Point2d(u, v);

}

bool isPointInTriangle(const Point2d& barycUv)

{

// Check if point is in triangle

return (barycUv.x >= 0.0) && (barycUv.y >= 0.0) && (barycUv.x + barycUv.y <= 1.0);

}

bool isPointInTriangle(const Point2d& A, const Point2d& B, const Point2d& C, const Point2d& P)

{

// Check if point is in triangle

return isPointInTriangle(computeBarycentricCoordinates(A, B, C, P));

}

bool lineSegmentsIntersect2DTest(const Point2d& A, const Point2d& B, const Point2d& C, const Point2d& D)

{

const double r = ((A.y - C.y) * (D.x - C.x) - (A.x - C.x) * (D.y - C.y)) /

((B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x));

const double s = ((A.y - C.y) * (B.x - A.x) - (A.x - C.x) * (B.y - A.y)) /

((B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x));

return ((r >= 0.0) && (r <= 1.0) && (s >= 0.0) && (s <= 1.0));

}

bool lineSegmentsIntersect2DTest(Point2d& S, const Point2d& A, const Point2d& B, const Point2d& C, const Point2d& D)

{

const double r = ((A.y - C.y) * (D.x - C.x) - (A.x - C.x) * (D.y - C.y)) /

((B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x));

const double s = ((A.y - C.y) * (B.x - A.x) - (A.x - C.x) * (B.y - A.y)) /

((B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x));

S = A + (B - A) * r;

return ((r >= 0.0) && (r <= 1.0) && (s >= 0.0) && (s <= 1.0));

}

bool ccw_tri_tri_intersection_2d(const Point2d& p1, const Point2d& q1, const Point2d& r1, const Point2d& p2,

const Point2d& q2, const Point2d& r2)

{

if(ORIENT_2D(p2, q2, p1) > 0.0f)

{

if(ORIENT_2D(q2, r2, p1) > 0.0f)

{

if(ORIENT_2D(r2, p2, p1) > 0.0f)

return true;

else

INTERSECTION_TEST_EDGE(p1, q1, r1, p2, q2, r2)

}

else

{

if(ORIENT_2D(r2, p2, p1) > 0.0f)

INTERSECTION_TEST_EDGE(p1, q1, r1, r2, p2, q2)

else

INTERSECTION_TEST_VERTEX(p1, q1, r1, p2, q2, r2)

}

}

else

{

if(ORIENT_2D(q2, r2, p1) > 0.0f)

{

if(ORIENT_2D(r2, p2, p1) > 0.0f)

INTERSECTION_TEST_EDGE(p1, q1, r1, q2, r2, p2)

else

INTERSECTION_TEST_VERTEX(p1, q1, r1, q2, r2, p2)

}

else

INTERSECTION_TEST_VERTEX(p1, q1, r1, r2, p2, q2)

}

}

bool TrianglesOverlap(const Point2d* t1, const Point2d* t2)

{

if(ORIENT_2D(t1[0], t1[1], t1[2]) < 0.0f)

if(ORIENT_2D(t2[0], t2[1], t2[2]) < 0.0f)

return ccw_tri_tri_intersection_2d(t1[0], t1[2], t1[1], t2[0], t2[2], t2[1]);

else

return ccw_tri_tri_intersection_2d(t1[0], t1[2], t1[1], t2[0], t2[1], t2[2]);

else if(ORIENT_2D(t2[0], t2[1], t2[2]) < 0.0f)

return ccw_tri_tri_intersection_2d(t1[0], t1[1], t1[2], t2[0], t2[2], t2[1]);

else

return ccw_tri_tri_intersection_2d(t1[0], t1[1], t1[2], t2[0], t2[1], t2[2]);

}

bool interectsTriangleTriangle(const Point3d* tri1, const Point3d* tri2)

{

bool ok = (bool)tri_tri_intersect(tri1[0].m, tri1[1].m, tri1[2].m, tri2[0].m, tri2[1].m, tri2[2].m);

return ok;

}

} // namespace aliceVision