/*

* This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).

* Copyright (c) 2022-2026 Jochen Meidow, Fraunhofer IOSB

*

* This program 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 3 of the License, or

* (at your option) any later version.

*

* This program 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 this program. If not, see <https://www.gnu.org/licenses/>.

*/

#include "matfun.h"

#include "udistance.h"

#include "upoint.h"

#include "usegment.h"

#include "ustraightline.h"

#include <Eigen/Core>

#include <cassert>

#include <cfloat>

#include <memory>

#include <utility>

using Eigen::Index;

using Eigen::Matrix3d;

using Eigen::VectorXi;

using Eigen::Vector3d;

using Eigen::VectorXd;

using Eigen::Vector;

namespace Uncertain {

using Geometry::skew;

using Matfun::sign;

//! Get uncertain endpoint x=S(l)*m in homogeneous coordinates

uPoint uStraightLineSegment::ux() const

{

const Matrix<double, 6,6> Sigma_lm = m_Cov_tt.topLeftCorner(6,6);

const Matrix3d Sl = skew( hl() );

Matrix<double,3,6> JJ;

JJ << -skew( hm() ), Sl;

// x = Sl*m:

return { Sl*hm(), JJ*Sigma_lm*JJ.adjoint() };

}

//! Get uncertain endpoint y=S(l)*n in homogeneous coordinates

uPoint uStraightLineSegment::uy() const

{

const Matrix3d Sl = skew( hl() );

Matrix<double,3,9> JJ;

JJ << -skew( hn() ), Matrix3d::Zero(), Sl;

return { Sl*hn(), JJ*Cov_tt()*JJ.adjoint() };

}

uStraightLineSegment::uStraightLineSegment( Vector9d t, Matrix9d Sigma_tt)

: m_t(std::move(t)), m_Cov_tt(std::move(Sigma_tt))

{

m_bounding_box = ux().bbox().united( uy().bbox() );

}

//! Construction of an uncertain straight line segment via two uncorrelated endpoints

uStraightLineSegment::uStraightLineSegment( const uPoint & ux,

const uPoint & uy)

: m_bounding_box( ux.bbox().united( uy.bbox()) )

{

static const Matrix3d CC = Vector3d(1,1,0).asDiagonal();

// (1) 9-parameter vector t = [l; m; n];

const Matrix3d Sx = skew( ux.v() );

const Matrix3d Sy = skew( uy.v() );

const Matrix3d UUx = Sx*CC*Sx;

const Matrix3d UUy = Sy*CC*Sy;

const Vector3d l = Sx*uy.v();

assert( l.norm() > FLT_EPSILON && "identical points.");

const double xh = ux.v(2);

const double yh = uy.v(2);

m_t.segment( 0, 3) = l;

m_t.segment( 3, 3) = +sign(yh)*UUx*uy.v();

m_t.segment( 6, 3) = -sign(xh)*UUy*ux.v();

// (2) 9x9 covariance matrix

//Matrix3d JJlx = -Sy;

//Matrix3d JJly = Sx;

const Matrix3d JJmx = -sign(yh) * (Sx*CC*Sy + skew((CC*l).eval()) );

const Matrix3d JJmy = +sign(yh) * UUx;

const Matrix3d JJnx = -sign(xh)*UUy;

const Matrix3d JJny = +sign(xh)*( Sy*CC*Sx -skew( (CC*l).eval() ));

/* JJ = [ ...

JJlx, JJly; ...

JJmx, JJmy; ...

JJnx, JJny]; */

Matrix<double,9,6> JJ;

JJ << -Sy, Sx,

JJmx, JJmy,

JJnx, JJny;

Matrix<double,6,6> Sigma_xy;

Sigma_xy.setZero();

Sigma_xy.topLeftCorner(3,3) = ux.Cov();

Sigma_xy.bottomRightCorner(3,3) = uy.Cov();

m_Cov_tt = JJ*Sigma_xy*JJ.transpose();

}

//! Check if the 'this' and the uncertain straight line segment 'ut' intersect (deterministic)

bool uStraightLineSegment::intersects( const uStraightLineSegment &ut) const

{

const double d1 = ut.hx().dot( hl() );

const double d2 = ut.hy().dot( hl() );

if ( d1*d2 >= 0 ) { // same sign?

return false;

}

const double d3 = hx().dot( ut.hl() );

const double d4 = hy().dot( ut.hl() );

return !(d3*d4 >=0 );

}

//! Check if one of the endpoints of the uncertain straight line segment 'other' is incident.

bool uStraightLineSegment::touches( const uStraightLineSegment & other,

const double T_dist,

const double T_in) const

{

if ( other.touchedBy( ux(), T_dist, T_in) ) {

return true;

}

if ( other.touchedBy( uy(), T_dist, T_in) ) {

return true;

}

return false;

}

//! Check if the uncertain point 'ux' is incident.

bool uStraightLineSegment::touchedBy( const uPoint & ux,

const double T_dist,

const double T_in) const

{

if ( !ux.isIncidentWith(ul(), T_in) ) { // dist(x,l)

return false;

}

const uDistance ud1 = ux.distanceAlgebraicTo( um() ); // dist(x,m)

if ( !ud1.isLessThanZero(T_dist) ) {

return false;

}

const uDistance ud2 = ux.distanceAlgebraicTo( un() ); // dist(x,n)

if ( !ud2.isGreaterThanZero(T_dist) ) {

return false;

}

return true;

}

//! Get the endpoints connecting uncertain straight line l = S(x)*y

uStraightLine uStraightLineSegment::ul() const

{

const Matrix3d Sigma_ll = m_Cov_tt.topLeftCorner(3,3);

return { m_t.head(3), Sigma_ll };

}

//! Get the delimiting uncertain straight line m in homogeneous coordinates.

uStraightLine uStraightLineSegment::um() const

{

const Matrix3d Sigma_mm = m_Cov_tt.block(3,3,3,3);

return { m_t.segment(3,3), Sigma_mm};

}

//! Get the delimiting uncertain straight line n in homogeneous coordinates.

uStraightLine uStraightLineSegment::un() const

{

const Matrix3d Sigma_nn = m_Cov_tt.bottomRightCorner(3,3);

return { m_t.tail(3), Sigma_nn};

}

//! Get the endpoint x in homogeneous coordinates

Vector3d uStraightLineSegment::hx() const

{

const Vector3d l = m_t.head(3);

const Vector3d m = m_t.segment(3,3);

return skew( l )*m;

}

//! Get the endpoint y in homogeneous coordinates

Vector3d uStraightLineSegment::hy() const

{

const Vector3d l = m_t.head(3);

const Vector3d n = m_t.tail(3);

return skew( l )*n;

}

//! Move endpoint x along l to intersection point of m and l.

bool uStraightLineSegment::move_x_to(const Vector3d &m)

{

const Vector3d l = hl().normalized();

const Vector3d z = l.cross( m.normalized() ); // intersection point z

if ( z.norm() < FLT_EPSILON ) {

return false; // l==m

}

const Vector2d dx = x() - z.head(2) / z(2);

const Matrix3d HH {

{ 1, 0, dx(0)},

{ 0, 1, dx(1)},

{ 0, 0, 1 } };

Matrix9d TT = Matrix9d::Identity();

TT.block(3,3,3,3) = HH.adjoint(); // m

transform( TT);

return true;

}

//! Move endpoint y along l to intersection point of n and l.

bool uStraightLineSegment::move_y_to( const Vector3d & n )

{

// intersection point z ............................

const Vector3d l = hl().normalized();

const Vector3d z = l.cross( n.normalized() );

if ( z.norm() < FLT_EPSILON) {

return false; // l==n

}

const Vector2d dx = y() - z.head(2) / z(2);

const Matrix3d HH {

{ 1, 0, dx(0)},

{ 0, 1, dx(1)},

{ 0, 0, 1 } };

Matrix9d TT = Matrix9d::Identity();

TT.block(6,6,3,3) = HH.adjoint(); // n

transform( TT);

return true;

}

//! Transform uncertain straight line segment via 9x9 transformation matrix for t = [l',m',n']'.

void uStraightLineSegment::transform( const Matrix9d & TT)

{

m_t = TT*m_t;

m_Cov_tt = TT*m_Cov_tt*TT.adjoint();

}

//! Create uncertain straight line segment (for deserialization)

std::shared_ptr<uStraightLineSegment>

uStraightLineSegment::create()

{

return std::shared_ptr<uStraightLineSegment>(

new uStraightLineSegment()

);

// return std::make_shared<uStraightLineSegment>();

// Note from

// https://en.cppreference.com/w/cpp/memory/shared_ptr/make_shared.html:

//

// std::shared_ptr<T>(new T(args...)) may call a non-public constructor

// of T if executed in context where it is accessible, while

// std::make_shared requires public access to the selected constructor.

}

} // namespace Uncertain