ConformalLabpp/code/include/p2_utility.hpp

#pragma once
// Copyright (c) 2024-2026 Tarik Moussa.
// SPDX-License-Identifier: MIT


// 2-D projective geometry utilities for the Euclidean signature.
// Ported from de.jreality.math.P2 and de.varylab.discreteconformal.math.P2Big.
//
// Points and lines are represented as homogeneous 3-vectors (x, y, w).
// In the Euclidean case a finite point (px, py) is stored as (px, py, 1).

#include <Eigen/Dense>
#include <cmath>

namespace conformallab {

// ── Point / line duality ──────────────────────────────────────────────────────

// Intersection of two lines l1, l2  (or  line through two points p1, p2)
// via the cross product.  Works for any P2 element.
// Corresponds to Java P2.pointFromLines / P2.lineFromPoints.
inline Eigen::Vector3d pointFromLines(const Eigen::Vector3d& l1,
                                      const Eigen::Vector3d& l2) {
    return l1.cross(l2);
}

// ── Euclidean perpendicular bisector ─────────────────────────────────────────

// Returns the homogeneous line coordinates (a, b, c) of the perpendicular
// bisector of the segment [p, q] in the Euclidean plane.
// Coordinates: ax + by + c = 0  (after dehomogenizing p and q).
//
// Corresponds to Java P2.perpendicularBisector(p, q, Pn.EUCLIDEAN).
inline Eigen::Vector3d perpendicularBisectorEuclidean(const Eigen::Vector3d& p_h,
                                                      const Eigen::Vector3d& q_h) {
    // Dehomogenize
    Eigen::Vector2d p = p_h.head<2>() / p_h(2);
    Eigen::Vector2d q = q_h.head<2>() / q_h(2);

    // Direction vector (p → direction, matching jReality sign convention)
    Eigen::Vector2d d = p - q;

    // Midpoint
    Eigen::Vector2d m = (p + q) * 0.5;

    // Line: d[0]*(x - m[0]) + d[1]*(y - m[1]) = 0
    //      = d[0]*x + d[1]*y - (d[0]*m[0] + d[1]*m[1])
    double c = -(d(0) * m(0) + d(1) * m(1));
    return {d(0), d(1), c};
}

// ── Euclidean distance between two P2 homogeneous points ─────────────────────

inline double euclideanDistanceP2(const Eigen::Vector3d& p_h,
                                  const Eigen::Vector3d& q_h) {
    Eigen::Vector2d p = p_h.head<2>() / p_h(2);
    Eigen::Vector2d q = q_h.head<2>() / q_h(2);
    return (p - q).norm();
}

// ── Direct Euclidean isometry from two point-frames ──────────────────────────

// Build the 3×3 projective matrix that represents the coordinate frame
// anchored at p0 with p1 defining the positive x-direction.
// Euclidean case: columns are [dehom(p0), unit_dir(p0→p1), perp_dir].
//
// Template parameter S allows float / double / long double.
template <typename S>
Eigen::Matrix<S, 3, 3> makeFrameMatrix(Eigen::Matrix<S, 3, 1> p0_h,
                                        Eigen::Matrix<S, 3, 1> p1_h) {
    // Dehomogenize
    Eigen::Matrix<S, 3, 1> p0 = p0_h / p0_h(2);          // (px, py, 1)
    Eigen::Matrix<S, 3, 1> p1_d = p1_h / p1_h(2);

    // Unit direction p0 → p1
    Eigen::Matrix<S, 2, 1> dir2 = (p1_d - p0).template head<2>();
    dir2.normalize();
    Eigen::Matrix<S, 3, 1> p1n(dir2(0), dir2(1), S(0));

    // Perpendicular direction
    Eigen::Matrix<S, 3, 1> p2(-dir2(1), dir2(0), S(0));

    Eigen::Matrix<S, 3, 3> M;
    M.col(0) = p0;
    M.col(1) = p1n;
    M.col(2) = p2;
    return M;
}

// Find the 3×3 Euclidean isometry (as a projective matrix) that maps
// the frame (s1, s2) to the frame (t1, t2).
//
// Corresponds to Java P2.makeDirectIsometryFromFrames(s1, s2, t1, t2, Pn.EUCLIDEAN)
// and P2Big.makeDirectIsometryFromFrames(...) (the BigDecimal / high-precision variant).
template <typename S>
Eigen::Matrix<S, 3, 3> makeDirectIsometryFromFramesEuclidean(
    Eigen::Matrix<S, 3, 1> s1, Eigen::Matrix<S, 3, 1> s2,
    Eigen::Matrix<S, 3, 1> t1, Eigen::Matrix<S, 3, 1> t2)
{
    auto toS = makeFrameMatrix<S>(s1, s2);
    auto toT = makeFrameMatrix<S>(t1, t2);
    return toT * toS.inverse();
}

} // namespace conformallab