//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

//
// Functions for computing reference frames.
//

#ifndef INCLUDED_IMATHFRAME_H
#define INCLUDED_IMATHFRAME_H

#include "ImathNamespace.h"

IMATH_INTERNAL_NAMESPACE_HEADER_ENTER

/// @cond Doxygen_Suppress
template <class T> class Vec3;
template <class T> class Matrix44;
/// @endcond

///
/// @{
/// @name Functions for computing reference frames
///
/// These methods compute a set of reference frames, defined by their
/// transformation matrix, along a curve. It is designed so that the
/// array of points and the array of matrices used to fetch these
/// routines don't need to be ordered as the curve.
///
/// A typical usage would be :
///
///      m[0] = IMATH_INTERNAL_NAMESPACE::firstFrame( p[0], p[1], p[2] );
///      for( int i = 1; i < n - 1; i++ )
///      {
///          m[i] = IMATH_INTERNAL_NAMESPACE::nextFrame( m[i-1], p[i-1], p[i], t[i-1], t[i] );
///      }
///      m[n-1] = IMATH_INTERNAL_NAMESPACE::lastFrame( m[n-2], p[n-2], p[n-1] );
///
///  See Graphics Gems I for the underlying algorithm.

template <class T>
Matrix44<T> constexpr firstFrame (
    const Vec3<T>&,                 // First point
    const Vec3<T>&,                 // Second point
    const Vec3<T>&) IMATH_NOEXCEPT; // Third point

template <class T>
Matrix44<T> constexpr nextFrame (
    const Matrix44<T>&,       // Previous matrix
    const Vec3<T>&,           // Previous point
    const Vec3<T>&,           // Current point
    Vec3<T>&,                 // Previous tangent
    Vec3<T>&) IMATH_NOEXCEPT; // Current tangent

template <class T>
Matrix44<T> constexpr lastFrame (
    const Matrix44<T>&,             // Previous matrix
    const Vec3<T>&,                 // Previous point
    const Vec3<T>&) IMATH_NOEXCEPT; // Last point

///
/// Compute the first reference frame along a curve.
///
/// This function returns the transformation matrix to the reference
/// frame defined by the three points `pi`, `pj` and `pk`. Note that
/// if the two vectors <`pi`,`pj`> and <`pi`,`pk`> are colinears, an
/// arbitrary twist value will be choosen.
///
/// Throw `std::domain_error` if `pi` and `pj` are equal.
///
/// @param pi
///      First point
/// @param pj
///      Second point
/// @param pk
///      Third point
///
template <class T>
Matrix44<T> constexpr firstFrame (
    const Vec3<T>& pi,                // first point
    const Vec3<T>& pj,                // secont point
    const Vec3<T>& pk) IMATH_NOEXCEPT // third point
{
    Vec3<T> t = pj - pi;
    t.normalizeExc ();

    Vec3<T> n = t.cross (pk - pi);
    n.normalize ();
    if (n.length () == 0.0f)
    {
        Vec3<T> v (0.0, 0.0, 0.0);

        int i = fabs (t.x) < fabs (t.y) ? 0 : 1;
        if (fabs (t.z) < fabs (t[i])) i = 2;

        v[i] = 1.0;
        n    = t.cross (v);
        n.normalize ();
    }

    Vec3<T> b = t.cross (n);

    Matrix44<T> M;

    M[0][0] = t.x;
    M[0][1] = t.y;
    M[0][2] = t.z;
    M[0][3] = 0.0;
    M[1][0] = n.x;
    M[1][1] = n.y;
    M[1][2] = n.z;
    M[1][3] = 0.0;
    M[2][0] = b.x;
    M[2][1] = b.y;
    M[2][2] = b.z;
    M[2][3] = 0.0;
    M[3][0] = pi.x;
    M[3][1] = pi.y;
    M[3][2] = pi.z;
    M[3][3] = 1.0;

    return M;
}

///
/// Compute the next reference frame along a curve.
///
/// This function returns the transformation matrix to the next reference
/// frame defined by the previously computed transformation matrix and the
/// new point and tangent vector along the curve.
///
/// @param Mi
///      The previous matrix
/// @param pi
///      The previous point
/// @param pj
///      The current point
/// @param ti
///      The previous tangent vector
/// @param tj
///      The current tangent vector

template <class T>
Matrix44<T> constexpr nextFrame (
    const Matrix44<T>& Mi,      // Previous matrix
    const Vec3<T>&     pi,      // Previous point
    const Vec3<T>&     pj,      // Current point
    Vec3<T>&           ti,      // Previous tangent vector
    Vec3<T>&           tj) IMATH_NOEXCEPT // Current tangent vector
{
    Vec3<T> a (0.0, 0.0, 0.0); /// Rotation axis.
    T       r = 0.0;           // Rotation angle.

    if (ti.length () != 0.0 && tj.length () != 0.0)
    {
        ti.normalize ();
        tj.normalize ();
        T dot = ti.dot (tj);

        //
        //  This is *really* necessary :
        //

        if (dot > 1.0)
            dot = 1.0;
        else if (dot < -1.0)
            dot = -1.0;

        r = acosf (dot);
        a = ti.cross (tj);
    }

    if (a.length () != 0.0 && r != 0.0)
    {
        Matrix44<T> R;
        R.setAxisAngle (a, r);
        Matrix44<T> Tj;
        Tj.translate (pj);
        Matrix44<T> Ti;
        Ti.translate (-pi);

        return Mi * Ti * R * Tj;
    }
    else
    {
        Matrix44<T> Tr;
        Tr.translate (pj - pi);

        return Mi * Tr;
    }
}

///
/// Compute the last reference frame along a curve.
///
/// This function returns the transformation matrix to the last reference
/// frame defined by the previously computed transformation matrix and the
/// last point along the curve.
///
/// @param Mi
///      The previous matrix
/// @param pi
///      The previous point
/// @param pj
///      The last point

template <class T>
Matrix44<T> constexpr lastFrame (
    const Matrix44<T>& Mi,            // Previous matrix
    const Vec3<T>&     pi,            // Previous point
    const Vec3<T>&     pj) IMATH_NOEXCEPT // Last point
{
    Matrix44<T> Tr;
    Tr.translate (pj - pi);

    return Mi * Tr;
}

/// @}

IMATH_INTERNAL_NAMESPACE_HEADER_EXIT

#endif // INCLUDED_IMATHFRAME_H