geometry3d.h

/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License. You may obtain a copy
 * of the License at http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software distributed under
 * the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#pragma once
 
#include <lagrange/MeshTrait.h>
#include <Eigen/Dense>
 
#include <limits>
 
namespace lagrange {

template <typename Scalar, int _Rows, int _Cols>
Scalar cos_angle_between(
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v1,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v2)
{
    static_assert((_Rows == 1 && _Cols == 3) || (_Rows == 3 && _Cols == 1), "");
    Scalar dot = v1.dot(v2);
    if (dot < -1) return Scalar(-1.0);
    if (dot > 1) return Scalar(1.0);
    return dot;
}

template <typename Scalar, int _Rows, int _Cols>
Scalar angle_between(
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v1,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v2)
{
    static_assert((_Rows == 1 && _Cols == 3) || (_Rows == 3 && _Cols == 1), "");
    return std::atan2(v1.cross(v2).norm(), v1.dot(v2));
}

template <typename Scalar, int _Rows, int _Cols>
Eigen::Matrix<Scalar, _Rows, _Cols> project_on_line(
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v1,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v2)
{
    static_assert((_Rows == 1 && _Cols == 3) || (_Rows == 3 && _Cols == 1), "");
    return v1.dot(v2) * v2;
}

template <typename Scalar, int _Rows, int _Cols>
Eigen::Matrix<Scalar, _Rows, _Cols> project_on_plane(
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& n)
{
    static_assert((_Rows == 1 && _Cols == 3) || (_Rows == 3 && _Cols == 1), "");
    return v - project_on_line(v, n);
}

template <typename Scalar, int _Rows, int _Cols>
Scalar projected_cos_angle_between(
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v1,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v2,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& n)
{
    static_assert((_Rows == 1 && _Cols == 3) || (_Rows == 3 && _Cols == 1), "");
    const Eigen::Matrix<Scalar, _Rows, _Cols> proj1 = project_on_plane(v1, n).stableNormalized();
    const Eigen::Matrix<Scalar, _Rows, _Cols> proj2 = project_on_plane(v2, n).stableNormalized();
    return cos_angle_between(proj1, proj2);
}

template <typename Scalar, int _Rows, int _Cols>
Scalar projected_angle_between(
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v1,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& v2,
    const Eigen::Matrix<Scalar, _Rows, _Cols>& n)
{
    static_assert((_Rows == 1 && _Cols == 3) || (_Rows == 3 && _Cols == 1), "");
    const Eigen::Matrix<Scalar, _Rows, _Cols> proj1 = project_on_plane(v1, n);
    const Eigen::Matrix<Scalar, _Rows, _Cols> proj2 = project_on_plane(v2, n);
    return std::atan2(proj1.cross(proj2).norm(), proj1.dot(proj2));
}

template <typename MeshType>
auto vector_between(const MeshType& mesh, typename MeshType::Index v1, typename MeshType::Index v2)
{
    static_assert(MeshTrait<MeshType>::is_mesh(), "MeshType is not a mesh");
    return mesh.get_vertices().row(v2) - mesh.get_vertices().row(v1);
}

template <typename Scalar>
void orthogonal_frame(
    const Eigen::Matrix<Scalar, 3, 1>& x,
    Eigen::Matrix<Scalar, 3, 1>& y,
    Eigen::Matrix<Scalar, 3, 1>& z)
{
    constexpr Scalar one(1);
    const Eigen::Matrix<Scalar, 3, 1> n = x.stableNormalized();
    const Scalar sign = std::copysign(one, n.z());
    const Scalar a = -one / (sign + n.z());
    const Scalar b = n.x() * n.y() * a;
    y = Eigen::Matrix<Scalar, 3, 1>(one + sign * n.x() * n.x() * a, sign * b, -sign * n.x());
    z = Eigen::Matrix<Scalar, 3, 1>(b, sign + n.y() * n.y() * a, -n.y());
}

template <typename Scalar>
Eigen::Vector3<Scalar> triangle_circumcenter_3d(
    const Eigen::Vector3<Scalar>& p1,
    const Eigen::Vector3<Scalar>& p2,
    const Eigen::Vector3<Scalar>& p3)
{
    // Edge vectors
    Eigen::Matrix<Scalar, 3, 1> a = p2 - p1;
    Eigen::Matrix<Scalar, 3, 1> b = p3 - p1;
 
    // Normal of the triangle plane
    Eigen::Matrix<Scalar, 3, 1> n = a.cross(b);
    Scalar n_norm2 = n.squaredNorm();
 
    // Degenerate case: points are collinear or nearly so
    if (n_norm2 < std::numeric_limits<Scalar>::epsilon()) return (p1 + p2 + p3) / Scalar(3);
 
    // Compute circumcenter in 3D
    // Formula: p1 + [ (|a|^2 * (b × n) + |b|^2 * (n × a)) ] / (2 * |n|^2)
    Eigen::Matrix<Scalar, 3, 1> term1 = a.squaredNorm() * (b.cross(n));
    Eigen::Matrix<Scalar, 3, 1> term2 = b.squaredNorm() * (n.cross(a));
    Eigen::Matrix<Scalar, 3, 1> circumcenter = p1 + (term1 + term2) / (Scalar(2) * n_norm2);
 
    return circumcenter;
}
 
 
} // namespace lagrange