Position.h

//  trax track library
//  AD 2013 
//
//  "I saw a werewolve with a chinese menue in his hand"
//
//                              Warren Zevon
//
// Copyright (c) 2025 Trend Redaktions- und Verlagsgesellschaft mbH
// Copyright (c) 2019 Marc-Michael Horstmann
//
// Permission is hereby granted to any person obtaining a copy of this software 
// and associated source code (the "Software"), to use, view, and study the 
// Software for personal or internal business purposes, subject to the following 
// conditions:
//
// 1. Redistribution, modification, sublicensing, or commercial use of the 
// Software is NOT permitted without prior written consent from the copyright 
// holder.
//
// 2. The Software is provided "AS IS", without warranty of any kind, express 
// or implied.
//
// 3. All copies of the Software must retain this license notice.
//
// For further information, please contact: horstmann.marc@trendverlag.de
 
#pragma once
 
#include "common/Helpers.h"
 
 
namespace spat{
 
    template<typename> struct Vector;
    template<typename> struct PositionH;
    template<typename> struct Position2D;
    template<typename,typename> struct VectorBundle;
 

    template<typename Valtype>
    struct Position
    {
        typedef Valtype value_type;
 
        Valtype x;  
        Valtype y;  
        Valtype z;  
 

                 constexpr Position() noexcept = default;                       
                 constexpr Position( Valtype x, Valtype y, Valtype z ) noexcept;
        explicit           Position( const Vector<Valtype>& v ) = delete;       
        template<typename Valtype2>
        explicit constexpr Position( const VectorBundle<Valtype,Valtype2>& b ) noexcept;
        explicit constexpr Position( const Position2D<Valtype>& p ) noexcept;
        template<typename Valtype2>
        explicit constexpr Position( const PositionH<Valtype2>& pos ) noexcept;
        template<typename Valtype2>
        explicit           Position( const Valtype2* pVal ) noexcept;
 

        Position& operator=( const Position2D<Valtype>& p2d ) noexcept;
 
        template<typename Valtype2>
        Position& operator=( const Valtype2* pVal ) noexcept;
 

        const Valtype& operator[]( size_t index ) const;
 
        Valtype& operator[]( size_t index );
 

        Valtype* ptr() noexcept;
 

        const Valtype* ptr() const noexcept;
 

        Position& Init() noexcept;
 

        Position& Transport( const Vector<Valtype>& v ) noexcept;
 

        template<typename Valtype2>
        Position& Rotate( const VectorBundle<Valtype,Valtype2>& axis ) noexcept;
 

        template<typename Valtype2>
        Position& Rotate( const VectorBundle<Valtype,Valtype2>& axis, Valtype2 angle ) noexcept;
 

        constexpr bool Equals( const Position& p, Valtype epsilon = Valtype{0} ) const noexcept;
 

        Position& Round( int toDigit ) noexcept;
    };
 

    template<typename Valtype>
    constexpr Position<Valtype> Origin3D{Valtype{0},Valtype{0},Valtype{0}};

    template<typename Valtype,typename Valtype2>
    Position<Valtype>& spatial_cast( Position<Valtype2>& from ) noexcept;

    template<typename Valtype,typename Valtype2>
    const Position<Valtype>& spatial_cast( const Position<Valtype2>& from ) noexcept;
 

    
    template<typename Valtype> constexpr
    Position<Valtype> operator+( const Position<Valtype>&, const Position<Valtype>& ) = delete;
    template<typename Valtype> constexpr
    Position<Valtype> operator+( const Position<Valtype>& p, const Vector<Valtype>& v ) noexcept;
    template<typename Valtype> constexpr
    Position<Valtype> operator+( const Vector<Valtype>&, const Position<Valtype>& ) = delete;
    template<typename Valtype>
    Position<Valtype>& operator+=( Position<Valtype>&, const Position<Valtype>& ) = delete;
    template<typename Valtype>
    Position<Valtype>& operator+=( Position<Valtype>& p, const Vector<Valtype>& v ) noexcept;
    template<typename Valtype>
    Vector<Valtype>& operator+=( Vector<Valtype>&, const Position<Valtype>& ) = delete;
    template<typename Valtype> constexpr
    Position<Valtype> operator+( const Position<Valtype>& ) = delete;
    template<typename Valtype> constexpr
    Vector<Valtype> operator-( const Position<Valtype>& p1, const Position<Valtype>& p2 ) noexcept;
    template<typename Valtype> constexpr
    Position<Valtype> operator-( const Position<Valtype>& p, const Vector<Valtype>& v ) noexcept;
    template<typename Valtype> constexpr
    Position<Valtype> operator-( const Vector<Valtype>&, const Position<Valtype>& ) = delete;
    template<typename Valtype>
    Position<Valtype>& operator-=( Position<Valtype>&, const Position<Valtype>& ) = delete;
    template<typename Valtype>
    Position<Valtype>& operator-=( Position<Valtype>& p, const Vector<Valtype>& v ) noexcept;
    template<typename Valtype> constexpr
    Position<Valtype> operator-( const Position<Valtype>& ) = delete;
    template<typename Valtype> constexpr
    Position<Valtype> operator*( const Position<Valtype>& , const Position<Valtype>& ) = delete;
    template<typename Valtype> constexpr
    auto operator*( Valtype f, const Position<Valtype>& p ) noexcept -> Position<decltype(Valtype{}*Valtype{})>;
    template<typename Valtype> constexpr
    auto operator*( const Position<Valtype>& p, Valtype f ) noexcept -> Position<decltype(Valtype{}*Valtype{})>;
    template<typename Valtype,typename Valtype2> constexpr
    auto operator*( Valtype f, const Position<Valtype2>& p ) noexcept -> Position<decltype(Valtype{}*Valtype2{})>;
    template<typename Valtype,typename Valtype2> constexpr
    auto operator*( const Position<Valtype>& p, Valtype2 f ) noexcept -> Position<decltype(Valtype{}*Valtype2{})>;
    template<typename Valtype,typename Valtype2> 
    Position<Valtype>& operator*=( Position<Valtype>& p, Valtype2 f ) noexcept;
    template<typename Valtype> 
    Valtype& operator*=( Valtype&, const Position<Valtype>& ) = delete;
    template<typename Valtype> constexpr
    Position<Valtype> operator%( const Position<Valtype>& , const Position<Valtype>& ) = delete;
    template<typename Valtype>
    Position<Valtype>& operator%=( const Position<Valtype>& , const Position<Valtype>& ) = delete;
    template<typename Valtype> constexpr
    Valtype operator/( const Position<Valtype>& , const Position<Valtype>& ) = delete;
    template<typename Valtype,typename Valtype2> constexpr
    auto operator/( const Position<Valtype>& p, Valtype2 f ) noexcept -> Position<decltype(Valtype{}/Valtype2{})>;
    template<typename Valtype> constexpr
    Position<Valtype> operator/( Valtype f, const Position<Valtype>& p ) = delete;
    template<typename Valtype,typename Valtype2>
    Position<Valtype>& operator/=( Position<Valtype>& p, Valtype2 f ) noexcept;
    template<typename Valtype> constexpr
    bool operator==( const Position<Valtype>& p1, const Position<Valtype>& p2 ) noexcept;
    template<typename Valtype> constexpr
    bool operator!=( const Position<Valtype>& p1, const Position<Valtype>& p2 ) noexcept;
 
 
    template<typename Valtype>
    constexpr size_t size( const Position<Valtype> ) noexcept {
        return 3;
    }
 
 
template<typename Valtype>
inline constexpr Position<Valtype>::Position( Valtype x, Valtype y, Valtype z ) noexcept
    : x{x}, y{y}, z{z} 
{}
 
template<typename Valtype>
template<typename Valtype2>
inline constexpr Position<Valtype>::Position( const VectorBundle<Valtype,Valtype2>& b ) noexcept
    : x{b.P.x}, y{b.P.y}, z{b.P.z} 
{}
 
template<typename Valtype>
inline constexpr Position<Valtype>::Position( const Position2D<Valtype>& p ) noexcept
    : x{p.x}, y{p.y}, z{0} 
{}
 
template<typename Valtype>
template<typename Valtype2>
inline constexpr Position<Valtype>::Position( const PositionH<Valtype2>& p ) noexcept
    : x{p.x/p.w}, y{p.y/p.w}, z{p.z/p.w} 
{}
 
template<typename Valtype>
template<typename Valtype2>
inline Position<Valtype>::Position( const Valtype2* pVal ) noexcept
    : x{Valtype{static_cast<Valtype>(pVal[0])}}, 
      y{Valtype{static_cast<Valtype>(pVal[1])}}, 
      z{Valtype{static_cast<Valtype>(pVal[2])}}
{}
 
template<typename Valtype>
inline Position<Valtype>& Position<Valtype>::operator=( const Position2D<Valtype>& p2d ) noexcept{
    x = p2d.x; y = p2d.y;
    return *this;
}
 
template<typename Valtype>
template<typename Valtype2>
inline Position<Valtype>& Position<Valtype>::operator=( const Valtype2* pVal ) noexcept{
    x = Valtype{pVal[0]};
    y = Valtype{pVal[1]};
    z = Valtype{pVal[2]};
    return *this;
}
 
template<typename Valtype>
template<typename Valtype2>
inline Position<Valtype>& Position<Valtype>::Rotate( const VectorBundle<Valtype,Valtype2>& axis ) noexcept
{
    VectorBundle<Valtype,Valtype2> r(axis);
    Valtype2 angle = r.T.Normalize();
 
    return Rotate( r, angle );
}
 
template<typename Valtype>
template<typename Valtype2>
inline Position<Valtype>& Position<Valtype>::Rotate( const VectorBundle<Valtype,Valtype2>& axis, Valtype2 angle ) noexcept
{
    Vector<Valtype> D = (*this - axis.P);
    Vector<Valtype> Dp = D * axis.T * axis.T;
    Vector<Valtype> Do = D - Dp;
    *this = axis.P + Dp + cos(angle) * Do + axis.T % Do * sin(angle);
    return *this;
}
 
template<typename Valtype>
inline const Valtype& Position<Valtype>::operator[]( size_t index ) const{
    if( index < 3 )
        return (&x)[index];
 
    throw std::out_of_range( "invalid Position subscript" );
}
 
template<typename Valtype>
inline Valtype& Position<Valtype>::operator[]( size_t index ){
    if( index < 3 )
        return (&x)[index];
 
    throw std::out_of_range( "invalid Position subscript" );
}
 
template<typename Valtype>
inline Valtype* Position<Valtype>::ptr() noexcept{
    return &x;
}
 
template<typename Valtype>
inline const Valtype* Position<Valtype>::ptr() const noexcept{
    return &x;
}
 
template<typename Valtype>
inline Position<Valtype>& Position<Valtype>::Init() noexcept{
    *this = Origin3D<Valtype>;
    return *this;
}
 
template<typename Valtype> 
inline Position<Valtype>& Position<Valtype>::Transport( const Vector<Valtype>& v ) noexcept{
    x += v.dx;
    y += v.dy;
    z += v.dz;
    return *this;
}
 
template<typename Valtype> 
inline constexpr bool Position<Valtype>::Equals( const Position<Valtype>& p, Valtype epsilon_ ) const noexcept{
    const Vector<Valtype> d{ *this - p };
    return d*d <= epsilon_*epsilon_;
}
 
template<typename Valtype>
inline Position<Valtype>& Position<Valtype>::Round( int toDigit ) noexcept{
    x = common::Round(x,toDigit);
    y = common::Round(y,toDigit);
    z = common::Round(z,toDigit);
    return *this;
}
 
template<typename Valtype,typename Valtype2>
inline Position<Valtype>& spatial_cast( Position<Valtype2>& from ) noexcept{
    static_assert( sizeof(Valtype) == sizeof(Valtype2), "Can not spatial_cast with different memory layout" );
    return reinterpret_cast<Position<Valtype>&>(from);
}
 
template<typename Valtype,typename Valtype2>
inline const Position<Valtype>& spatial_cast( const Position<Valtype2>& from ) noexcept{
    static_assert( sizeof(Valtype) == sizeof(Valtype2), "Can not spatial_cast with different memory layout" );
    return reinterpret_cast<const Position<Valtype>&>(from);
}
 
template<typename Valtype> constexpr
inline Position<Valtype> operator+( const Position<Valtype>& p, const Vector<Valtype>& v ) noexcept{
    return { p.x + v.dx,
             p.y + v.dy,
             p.z + v.dz };
}
 
template<typename Valtype>
inline Position<Valtype>& operator+=( Position<Valtype>& p, const Vector<Valtype>& v ) noexcept{
    p.Transport( v );
    return p;
}
 
template<typename Valtype> constexpr
inline Vector<Valtype> operator-( const Position<Valtype>& p1, const Position<Valtype>& p2 ) noexcept{
    return { p1.x - p2.x,
             p1.y - p2.y,
             p1.z - p2.z };
}
 
template<typename Valtype> constexpr
inline Position<Valtype> operator-( const Position<Valtype>& p, const Vector<Valtype>& v ) noexcept{
    return { p.x - v.dx,
             p.y - v.dy,
             p.z - v.dz };
}
 
template<typename Valtype>
inline Position<Valtype>& operator-=( Position<Valtype>& p, const Vector<Valtype>& v ) noexcept{
    p.x -= v.dx;
    p.y -= v.dy;
    p.z -= v.dz;
    return p;
}
 
template<typename Valtype> constexpr
inline auto operator*( Valtype f, const Position<Valtype>& p ) noexcept -> Position<decltype(Valtype{}*Valtype{})>{
    return { p.x * f, 
             p.y * f, 
             p.z * f }; 
}
template<typename Valtype> constexpr
inline auto operator*( const Position<Valtype>& p, Valtype f ) noexcept -> Position<decltype(Valtype{}*Valtype{})>{
    return f * p;
}
 
template<typename Valtype,typename Valtype2> constexpr
inline auto operator*( Valtype f, const Position<Valtype2>& p ) noexcept -> Position<decltype(Valtype{}*Valtype2{})>{
    static_assert( !std::is_integral<Valtype>::value, "Possible data loss on integral values, proper implementation provided for int only!" );
    return { p.x * f, 
             p.y * f, 
             p.z * f }; 
}
 
template<typename Valtype,typename Valtype2> constexpr
inline auto operator*( const Position<Valtype>& p, Valtype2 f ) noexcept -> Position<decltype(Valtype{}*Valtype2{})>{
    static_assert( !std::is_integral<Valtype>::value, "Possible data loss on integral values, proper implementation provided for int only!" );
    return f * p;
}
 
template<typename Valtype> constexpr
inline Position<int> operator*( const Position<int>& p, Valtype f ) noexcept{
    return { static_cast<int>(static_cast<Valtype>(p.x) * f), 
             static_cast<int>(static_cast<Valtype>(p.y) * f), 
             static_cast<int>(static_cast<Valtype>(p.z) * f) };
}
 
template<typename Valtype> constexpr
inline Position<int> operator*( Valtype f, const Position<int>& p ) noexcept{
    return p * f;
}
 
template<typename Valtype,typename Valtype2>
inline Position<Valtype>& operator*=( Position<Valtype>& p, Valtype2 f ) noexcept{
    static_assert( !std::is_integral<Valtype>::value, "Possible data loss on integral values, proper implementation provided for int only!" );
    p.x *= f;
    p.y *= f;
    p.z *= f;
    return p;
}
 
template<typename Valtype>
inline Position<int>& operator*=( Position<int>& p, Valtype f ) noexcept{
    p.x = static_cast<int>(static_cast<Valtype>(p.x) * f);
    p.y = static_cast<int>(static_cast<Valtype>(p.y) * f);
    p.z = static_cast<int>(static_cast<Valtype>(p.z) * f);
    return p;
}
 
template<typename Valtype,typename Valtype2> constexpr
inline auto operator/( const Position<Valtype>& p, Valtype2 f ) noexcept -> Position<decltype(Valtype{}/Valtype2{})>{
    static_assert( !std::is_integral<Valtype>::value, "Possible data loss on integral values, proper implementation provided for int only!" );
    return { p.x / f,
             p.y / f,
             p.z / f };
}
 
template<typename Valtype> constexpr
inline Position<int> operator/( const Position<int>& p, Valtype f ) noexcept{
    return { static_cast<int>(static_cast<Valtype>(p.x) / f),
             static_cast<int>(static_cast<Valtype>(p.y) / f),
             static_cast<int>(static_cast<Valtype>(p.z) / f) };
}
 
template<typename Valtype,typename Valtype2>
inline Position<Valtype>& operator/=( Position<Valtype>& p, Valtype2 f ) noexcept{
    static_assert( !std::is_integral<Valtype>::value, "Possible data loss on integral values, proper implementation provided for int only!" );
    p.x /= f;
    p.y /= f;
    p.z /= f;
    return p;
}
 
template<typename Valtype> constexpr
inline Position<int>& operator/=( Position<int>& p, Valtype f ) noexcept{
    p.x = static_cast<int>(static_cast<Valtype>(p.x) / f);
    p.y = static_cast<int>(static_cast<Valtype>(p.y) / f);
    p.z = static_cast<int>(static_cast<Valtype>(p.z) / f);
    return p;
}
 
template<typename Valtype> constexpr
inline bool operator==( const Position<Valtype>& p1, const Position<Valtype>& p2 ) noexcept{
    return (p1.x == p2.x && p1.y == p2.y && p1.z == p2.z);
}
 
template<typename Valtype> constexpr
inline bool operator!=( const Position<Valtype>& p1, const Position<Valtype>& p2 ) noexcept{
    return !(p1 == p2);
}
 
} // namespace spat