MinimalBasis.h

Go to the documentation of this file.

//
//      ██╗██████╗     ██╗     ██╗██████╗ ███████╗
//      ██║██╔══██╗    ██║     ██║██╔══██╗██╔════╝      ** JPLSpatial **
//      ██║██████╔╝    ██║     ██║██████╔╝███████╗
// ██   ██║██╔═══╝     ██║     ██║██╔══██╗╚════██║      https://github.com/Jaytheway/JPLSpatial
// ╚█████╔╝██║         ███████╗██║██████╔╝███████║
//  ╚════╝ ╚═╝         ╚══════╝╚═╝╚═════╝ ╚══════╝
//
//   Copyright 2024 Jaroslav Pevno, JPLSpatial is offered under the terms of the ISC license:
//
//   Permission to use, copy, modify, and/or distribute this software for any purpose with or
//   without fee is hereby granted, provided that the above copyright notice and this permission
//   notice appear in all copies. THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
//   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
//   AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
//   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
//   WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
//   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
#pragma once
 
#include "JPLSpatial/Core.h"
#include "JPLSpatial/Math/Math.h"
#include "JPLSpatial/Math/Vec3Math.h"
#include "JPLSpatial/Math/Vec3Traits.h"
#include "JPLSpatial/Math/SIMD.h"
 
#include <cmath>
 
namespace JPL
{
    template<CVec3 Vec3>
    struct Basis
    {
        using Float = Internal::FloatOf<Vec3>;
 
        Vec3 X, Y, Z;
 
        [[nodiscard]] static const Basis& Identity() noexcept
        {
            static constexpr Basis sIdentity(
                Vec3{ Float(1), Float(0), Float(0) },
                Vec3{ Float(0), Float(1), Float(0) },
                Vec3{ Float(0), Float(0), Float(1) }
            );
            return sIdentity;
        }
 
        [[nodiscard]] JPL_INLINE static Basis FromForward(const Vec3& forward) noexcept
        {
            Basis basis{ .Z = forward };
            Math::CreateOrthonormalBasis(forward, basis.X, basis.Y);
            return basis;
        }
 
        [[nodiscard]] inline static Basis FromUpAndForward(const Vec3& up, const Vec3& forward) noexcept
        {
            Vec3 Y = up;
            const Vec3 Z = forward;
            if (Math::IsNearlyEqual(Math::Abs<Float>(DotProduct(Y, Z)), Float(1.0)))
                Y = (Math::Abs(GetZ(Z)) < Float(0.999) ? Vec3(0, 0, 1) : Vec3(1, 0, 0));
 
            const Vec3 X = Normalized(CrossProduct(Y, Z));
            Y = CrossProduct(Z, X);
 
            return Basis{ X, Y, Z };
        }
 
        [[nodiscard]] inline static Basis Rotation(const Vec3& axis, Float angleRad) noexcept
        {
            const auto [s, c] = Math::SinCos(angleRad);
            const Float t = Float(1) - c;
 
            const Vec3 taxyz = t * axis;
            const Vec3 sAxis = s * axis;
 
            // Outer-product columns: (a ⊗ a) columns are ax*a, ay*a, az*a
            const Vec3 t_ax_a = axis * taxyz.X;
            const Vec3 t_ay_a = axis * taxyz.Y;
            const Vec3 t_az_a = axis * taxyz.Z;
 
            return Basis{
                .X = t_ax_a + Vec3{ c, sAxis.Z, -sAxis.Y },
                .Y = t_ay_a + Vec3{ -sAxis.Z, c, sAxis.X },
                .Z = t_az_a + Vec3{ sAxis.Y, -sAxis.X, c }
            };
        }
 
        [[nodiscard]] JPL_INLINE Vec3 InverseTransform(const Vec3& pWorld) const noexcept
        {
            return Vec3{
                DotProduct(pWorld, X),  // component along local X
                DotProduct(pWorld, Y),  // along local Y
                DotProduct(pWorld, Z)   // along local Z
            };
        }
 
        [[nodiscard]] JPL_INLINE Vec3 Transform(const Vec3& vector) const noexcept
        {
            // 3 FMAs, or 9 mul + 6 add
            return GetX(vector) * X + GetY(vector) * Y + GetZ(vector) * Z;
        }
 
        JPL_INLINE void Transform(simd& inOutX, simd& inOutY, simd& inOutZ) const noexcept
        {
            // Take a copy of the original input values
            // because we modify them as we compute next component
            const simd inX = inOutX;
            const simd inY = inOutY;
            const simd inZ = inOutZ;
 
            inOutX = Math::FMA(simd(GetX(X)), inX, Math::FMA(simd(GetX(Y)), inY, simd(GetX(Z)) * inZ));
            inOutY = Math::FMA(simd(GetY(X)), inX, Math::FMA(simd(GetY(Y)), inY, simd(GetY(Z)) * inZ));
            inOutZ = Math::FMA(simd(GetZ(X)), inX, Math::FMA(simd(GetZ(Y)), inY, simd(GetZ(Z)) * inZ));
        }
 
        [[nodiscard]] JPL_INLINE Basis Transform(const Basis& otherBasis) const noexcept
        {
            return Basis{ // R * R_other
                .X = Transform(otherBasis.X),
                .Y = Transform(otherBasis.Y),
                .Z = Transform(otherBasis.Z)
            };
        }
 
        [[nodiscard]] JPL_INLINE Basis InverseTransform(const Basis& otherBasis) const noexcept
        {
            return Basis{ // RT * R_other
                .X = InverseTransform(otherBasis.X),
                .Y = InverseTransform(otherBasis.Y),
                .Z = InverseTransform(otherBasis.Z)
            };
        }
 
    };
 
    namespace Math
    {
        template<CVec3 Vec3>
        [[nodiscard]] JPL_INLINE static Basis<Vec3> MakeBasis(const Vec3& forward) noexcept
        {
            return Basis<Vec3>::MakeFrom(forward);
        }
 
        template<CVec3 Vec3>
        [[nodiscard]] JPL_INLINE static Basis<Vec3> MakeBasis(const Vec3& forward, const Vec3& up) noexcept
        {
            return Basis<Vec3>::FromUpAndForward(up, forward);
        }
    }
} // namespace JPL