ViewportPointSmoother.hpp

#pragma once
#include <NDEVR/GLComputeShader.h>
#include "RasterPointScanner/Headers/ViewPortSmoothShaderLogic.h"
#include "PointScanner/Headers/PointContainer.h"
#include "Base/Headers/Vertex.hpp"
#include "Base/Headers/Matrix.hpp"
#include "Base/Headers/MatrixFunctions.h"
#include "Base/Headers/Time.h"
#include "Base/Headers/RGBColor.h"
#include "Base/Headers/RWLock.h"
#include "Base/Headers/Line.hpp"
#include "Base/Headers/TimeSpan.h"
class QOpenGLShaderProgram;
namespace NDEVR
{
    static constexpr fltp04 FastInvSqrt(fltp04 number)
    {
        union { fltp04 f = 0.0f; uint04 i; } conv;
        const fltp04 threehalfs = 1.5f;
        fltp04 x2 = number * 0.5f;
        conv.f = number;
        conv.i = 0x5f3759df - (conv.i >> 1);
        conv.f = conv.f * (threehalfs - (x2 * conv.f * conv.f));
        return conv.f;
    }
    static Vertex<3, fltp04> AverageValue(Vertex<3, fltp04> a, fltp04 wa, Vertex<3, fltp04> b, fltp04 wb)
    {
        return ((a * wa + b * wb) * (1.0f / (wa + wb)));
    }
    
    struct Intrinsics
    {
        Vector<2, fltp04> size_f = Constant<Vector<2, fltp04>>::Invalid;
        Vector<2, fltp04> size_fr = Constant<Vector<2, fltp04>>::Invalid;
        Vector<2, uint04> center_i = Constant<Vector<2, uint04>>::Invalid;
        Vector<2, uint04> size_i = Constant<Vector<2, uint04>>::Invalid;
        Vector<2, fltp04> pp = Constant<Vector<2, fltp04>>::Invalid;
        Vector<2, fltp04> f = Constant<Vector<2, fltp04>>::Invalid;
        Vector<2, fltp04> inv_f = Constant<Vector<2, fltp04>>::Invalid;
        uint04 size_max_distance = Constant<uint04>::Invalid;
        Vector<5, fltp04> coeffs;
 
        void setScreenSize(const Vector<2, uint04>& view_size)
        {
            size_i = view_size;
            center_i = view_size / 2U;
            size_max_distance = view_size[X] * view_size[Y];
            size_f = size_i.as<2, fltp04>();
            size_fr = (size_i + 10U).as<2, fltp04>();
        }
        void setFx(Vector<2, fltp04> fov)
        {
            f = fov;
            inv_f = 1.0f / fov;
        }
    };
 
    struct ViewPortSmootherInformation
    {
        uint04 point_count = 0;
        fltp08 average_weight = 0.0;
        fltp08 average_distance = 0.0;
    };
 
    static constexpr uint04 s_discard_weight = 8U;
    static constexpr uint04 s_record_weight = 4096U;
    static constexpr uint04 s_max_weight = 1073741823U;
 
    template<class t_point_type>
    class ViewportPointSmoother
    {
    public:
        ViewportPointSmoother() 
        {}
 
        static constexpr uint04 RecordWeight() { return s_record_weight; }
        static constexpr uint04 MaxWeight() { return s_max_weight; }
        static constexpr uint04 DiscardWeight() { return s_discard_weight; }
        void setMaxDistance(fltp04 max_distance)
        {
            m_max_distance = max_distance;
        }
 
        void updateCameraLocationBounds()
        {
            m_camera_location_bounds = Constant<Bounds<2, fltp04>>::Min;
            fltp04 max_dist = m_max_distance;
            m_camera_location_bounds.addToBounds(pixelToPoint(Vector<3, fltp04>(0.0f, m_intrinsics.size_f[Y] / 2, max_dist)).template as<2, fltp04>());
            m_camera_location_bounds.addToBounds(pixelToPoint(Vector<3, fltp04>(m_intrinsics.size_f[X] / 2.0f, 0.0f, max_dist)).template as<2, fltp04>());
            m_camera_location_bounds.addToBounds(pixelToPoint(Vector<3, fltp04>(m_intrinsics.size_f[X] / 2.0f, m_intrinsics.size_f[Y], max_dist)).template as<2, fltp04>());
            m_camera_location_bounds.addToBounds(pixelToPoint(Vector<3, fltp04>(m_intrinsics.size_f[X], m_intrinsics.size_f[Y] / 2, max_dist)).template as<2, fltp04>());
            m_camera_location_bounds.addToBounds(Vector<2, fltp04>(0.0f));
            m_camera_location_bounds = m_camera_location_bounds.scale(1.0 / max_dist);
        }
 
        Vertex<3, fltp04> closestPoint(const Vertex<3, fltp04>& point) const
        {
            fltp04 min_dist = Constant<fltp04>::Max;
            Vertex<3, fltp04> closest_point = Constant<Vertex<3, fltp04>>::Invalid;
            for (uint04 i = 0; i < m_active.locations.size(); i++)
            {
                Vertex<3, fltp04> p0 = m_active.locations[i];
                fltp04 current_dist = distanceSquared(p0, point);
                if (current_dist < min_dist) { min_dist = current_dist; closest_point = p0; }
            }
            return closest_point;
        }
        Vertex<3, fltp04> closestPoint(const LineSegment<3, fltp04>& line) const
        {
            fltp04 min_dist = Constant<fltp04>::Max;
            Vertex<3, fltp04> closest_point = Constant<Vertex<3, fltp04>>::Invalid;
            for (uint04 i = 0; i < m_active.locations.size(); i++)
            {
                Vertex<3, fltp04> p0 = m_active.locations[i];
                fltp04 current_dist = distanceSquared(p0, line);
                if (current_dist < min_dist) { min_dist = current_dist; closest_point = p0; }
            }
            return closest_point;
        }
 
        void calcHistoricPixelLocations(uint04 record_weight, uint04 discard_weight, uint04 max_weight, uint04 weight_reduction)
        {
            if (m_point_smooth_logic == nullptr)
                m_point_smooth_logic = new ViewPortSmoothShaderLogic();
            if (m_point_smooth_logic)
            {
                calcHistoricPixelLocationsGL(record_weight, discard_weight, max_weight, weight_reduction);
                return;
            }
            m_pixel_buffer.setSize(m_intrinsics.size_i.product());
            m_pixel_buffer.setAllToValue(Constant<uint04>::Invalid);
            uint04 nan_index = m_active.locations.size();
            for (uint04 i = 0; i < nan_index;)
            {
                uint04 pix_idx = Constant<uint04>::Invalid;
                bool is_filtered = false;
                if (!m_lock_weights || m_active.weights[i] <= weight_reduction)
                    is_filtered = m_active.weights[i] <= getMax(weight_reduction, discard_weight);
                if (!is_filtered)
                {
                    Vector<2, fltp04> location = screenLocation(m_active.locations[i]);
                    bool is_out_of_range = !isInActiveScreen(location);
                    if (is_out_of_range && !isInScreen(location)) is_filtered = true;
                    fltp04 d = 0.0f;
                    if (!is_out_of_range)
                    {
                        Vertex<3, fltp04> value_dir(m_active.locations[i] - m_camera_location);
                        d = value_dir.magnitudeSquared();
                        is_out_of_range = d > m_max_distance;
                        if (is_out_of_range && d > 1.2f * m_max_distance) is_filtered = true;
                        else
                        {
                            if constexpr (t_point_type::HasNormal())
                            {
                                if (!is_out_of_range)
                                {
                                    Vector<3, fltp04> normal = m_active.data[i].normal();
                                    fltp08 dot_t = dot(normal, value_dir);
                                    if (dot_t > 0) is_out_of_range = true;
                                    if (dot_t > 0.25f) is_filtered = true;
                                }
                            }
                        }
                    }
                    if (!is_out_of_range)
                    {
                        if (m_active.weights[i] > max_weight) m_active.weights[i] = max_weight;
                        else if (m_active.weights[i] > weight_reduction) m_active.weights[i] -= weight_reduction;
                        else { m_active.weights[i] = 0U; is_out_of_range = true; }
                    }
                    if (!is_out_of_range)
                    {
                        Vector<2, uint04> ipix = location.as<2, uint04>();
                        pix_idx = screenIndex(ipix);
                        uint04 existing_idx = m_pixel_buffer[pix_idx];
                        if (IsValid(existing_idx))
                        {
                            if (m_filter_matching_points)
                            {
                                if (m_active.weights[existing_idx] < record_weight)
                                {
                                    updateWeightedAverage(existing_idx, m_active.locations[i], m_active.data[i], m_active.weights[i], m_max_error_percent_sqrd * d);
                                }
                                m_active.weights[i] = 0U;
                                is_filtered = true;
                            }
                            else
                            {
                                m_pixel_buffer[pix_idx] = i;
                            }
                        }
                        else
                        {
                            m_pixel_buffer[pix_idx] = i;
                        }
                    }
                }
                if (is_filtered)
                {
                    if (m_active.weights[i] >= record_weight)
                    {
                        m_inactive.locations.add(m_active.locations[i]);
                        m_inactive.data.add(m_active.data[i]);
                        m_inactive.weights.add(m_active.weights[i]);
                    }
                    --nan_index;
                    m_active.locations.swapIndices(i, nan_index);
                    m_active.data.swapIndices(i, nan_index);
                    m_active.weights.swapIndices(i, nan_index);
                }
                else
                {
                    i++;
                }
            }
            m_active.locations.setSize(nan_index);
            m_active.data.setSize(nan_index);
            m_active.weights.setSize(nan_index);
        }
        const Intrinsics& intrinsics() const { return m_intrinsics; }
        void updateAverage(uint04 index, Vertex<3, fltp04> view_point, t_point_type view_color, uint04 weight)
        {
            m_active.locations[index] = AverageValue(m_active.locations[index], m_active.weights[index], view_point, weight);
            m_active.data[index] = combine(m_active.data[index], m_active.weights[index], view_color, weight);
            m_active.weights[index] += weight;
        }
 
        uint08 updateWeightedAverage(uint04 index, Vertex<3, fltp04> view_point, t_point_type view_color, uint04 weight, fltp04 max_slice_distance)
        {
            fltp04 d2 = distanceSquared(m_active.locations[index], view_point);
            if (d2 > max_slice_distance)
            {
                if (!m_lock_weights) m_active.weights[index] -= getMin(m_active.weights[index], weight);
                return s_max_weight;
            }
            else
            {
                m_active.locations[index] = AverageValue(m_active.locations[index], m_active.weights[index], view_point, weight);
                m_active.data[index] = combine(m_active.data[index], m_active.weights[index], view_color, weight);
                m_active.weights[index] += weight;
                if (m_active.weights[index] < s_max_weight) return s_max_weight - m_active.weights[index];
                else return 0U;
            }
        }
 
        void setLockWeights(bool lock_weights) 
        { 
            m_lock_weights = lock_weights; 
        }
 
        fltp04 update(const Matrix<fltp04>& view_matrix, const Buffer<Vertex<3, fltp04>>& view_points, const Buffer<t_point_type>& view_data)
        {
            Time current_time = Time::SystemTime();
            uint04 weight_mult = 10;
            if (IsValid(m_last_update_time))
            {
                TimeSpan span = current_time - m_last_update_time;
                weight_mult = cast<uint04>(clip(300.0 * span.elapsedSeconds(), 1.0, 300.0));
            }
            m_last_update_time = current_time;
            return update(view_matrix, view_points, view_data, Buffer<uint04>());
        }
 
        fltp04 update(const Matrix<fltp04>& view_matrix, const Buffer<Vertex<3, fltp04>>& view_points, const Buffer<t_point_type>& view_data, const Buffer<uint04>& weights)
        {
            Vertex<3, fltp04> new_camera_location = view_matrix.decomposeOffset();
            m_camera_location = new_camera_location;
            updateCameraLocationBounds();
            bool too_much_movement = false;
            if (!m_lock_weights) {}
 
            if (m_request_clear_all || IsInvalid(m_view_matrix) || m_active.locations.size() == 0 || too_much_movement)
            {
                m_last_update_time = Constant<Time>::Invalid;
                WLock lock(&m_active);
                m_active.locations = view_points;
                m_active.data = view_data;
                m_active.weights = weights;
                m_view_matrix = view_matrix;
                m_inactive.clear();
                m_inverse_view_matrix = view_matrix.invert();
                if (m_request_clear_all) clearInactiveLocations();
                m_request_clear_all = false;
                return 0.0f;
            }
 
            uint08 total_variablity = 0;
            m_view_matrix = view_matrix;
            m_inverse_view_matrix = view_matrix.invert();
            
            if (m_point_smooth_logic)
                m_point_smooth_logic->getInput(m_active, m_inactive, s_record_weight);
            uint04 original_size = m_active.size();
            m_active.locations.addAll(view_points);
            m_active.data.addAll(view_data);
            m_active.weights.addAll(weights);
            calcHistoricPixelLocations(s_record_weight, s_discard_weight, s_max_weight, 2 * m_frame_reduction);
            fltp04 percent = cast<fltp04>(1.0 - (cast<fltp08>(total_variablity) / (cast<fltp08>(view_points.size()) * cast<fltp08>(s_max_weight))));
            percent = clip(percent, 0.0f, 1.0f);
            m_active.setSize(original_size);//remove the newly added points. They just were to add to the buffer
            return percent;
        }
 
        uint04 getWeight(fltp04 camera_distance, Vertex<2, uint04> pixel_location)
        {
            uint04 distance_sqr = 0U;
            for (uint01 i = 0; i < 2; i++)
            {
                uint04 dt;
                if (pixel_location[i] > m_intrinsics.center_i[i]) dt = (pixel_location[i] - m_intrinsics.center_i[i]);
                else dt = (m_intrinsics.center_i[i] - pixel_location[i]);
                dt += 10U;
                distance_sqr += dt * dt;
            }
            return getMax(1U, cast<uint04>(FastInvSqrt(cast<fltp04>(distance_sqr)) * 65536.0f * camera_distance));
        }
        uint04 getWeight(Vertex<3, fltp04> location, Vertex<2, uint04> pixel_location, Vertex<3, fltp04> camera_location)
        {
            fltp04 d2 = distanceSquared(location, camera_location);
            return getWeight(FastInvSqrt(d2), pixel_location);
        }
 
        Vector<3, fltp04> locationAt(Vector<2, uint04> location, uint04 min_weight) const
        {
            uint04 idx = screenIndex(location);
            if (idx >= m_pixel_buffer.size()) return Constant<Vector<3, fltp04>>::Invalid;
            uint04 index = m_pixel_buffer[idx];
            if (index >= m_active.weights.size()) return Constant<Vector<3, fltp04>>::Invalid;
            if (m_active.weights[index] < min_weight) return Constant<Vector<3, fltp04>>::Invalid;
            return m_active.locations[index];
        }
 
        Vector<2, fltp04> screenLocation(Vertex<3, fltp04> location) const
        {
            location = m_inverse_view_matrix * location;
            if (location[Z] <= 0.01) return Constant<Vector<2, fltp04>>::Invalid;
            return pointToPixel(location);
        }
 
        uint04 screenIndex(Vector<2, uint04> location) const
        {
            return location[X] + location[Y] * m_intrinsics.size_i[X];
        }
 
        bool isInActiveScreen(Vertex<2, fltp04> pixel) const
        {
            return pixel[X] >= 10.0f && pixel[Y] >= 10.0f && pixel[X] < m_intrinsics.size_fr[X] && pixel[Y] < m_intrinsics.size_fr[Y];
        }
        bool isInScreen(Vertex<2, fltp04> pixel) const
        {
            return pixel[X] >= 0.0f && pixel[Y] >= 0.0f && pixel[X] < m_intrinsics.size_f[X] && pixel[Y] < m_intrinsics.size_f[Y];
        }
 
        template<class t_node_type>
        bool contains(Vertex<3, fltp04> value, const t_node_type& node) const
        {
            value = m_block_mat * value;
            auto pixel_value = screenLocation(value);
            if (!isInScreen(pixel_value) || distanceSquared(value, m_camera_location) > m_max_distance * m_max_distance) 
                return false;
            return true;
        }
        bool contains(Vertex<3, fltp04> value) const
        {
            if (m_pixel_buffer.size() == 0) return false;
            value = m_block_mat * value;
            auto pixel_value = screenLocation(value);
            if (!isInScreen(pixel_value) || distanceSquared(value, m_camera_location) > m_max_distance * m_max_distance) 
                return false;
            return true;
        }
 
        IntersectionTypes classifyBlock(Bounds<3, fltp04> bounds) const
        {
            if ((m_block_mat * bounds).contains(m_camera_location)) return IntersectionTypes::e_mixed;
            bool has_object = contains({ bounds[MAX][X],bounds[MAX][Y],bounds[MAX][Z] });
            if (has_object != contains({ bounds[MAX][X],bounds[MAX][Y],bounds[MIN][Z] })) return IntersectionTypes::e_mixed;
            if (has_object != contains({ bounds[MAX][X],bounds[MIN][Y],bounds[MAX][Z] })) return IntersectionTypes::e_mixed;
            if (has_object != contains({ bounds[MAX][X],bounds[MIN][Y],bounds[MIN][Z] })) return IntersectionTypes::e_mixed;
            if (has_object != contains({ bounds[MIN][X],bounds[MAX][Y],bounds[MAX][Z] })) return IntersectionTypes::e_mixed;
            if (has_object != contains({ bounds[MIN][X],bounds[MAX][Y],bounds[MIN][Z] })) return IntersectionTypes::e_mixed;
            if (has_object != contains({ bounds[MIN][X],bounds[MIN][Y],bounds[MAX][Z] })) return IntersectionTypes::e_mixed;
            if (has_object != contains({ bounds[MIN][X],bounds[MIN][Y],bounds[MIN][Z] })) return IntersectionTypes::e_mixed;
            if (has_object) return IntersectionTypes::e_mixed; else return IntersectionTypes::e_outside;
        }
 
        Vector<2, uint04> viewSizeI() const { return m_intrinsics.size_i; }
        Vector<2, fltp04> viewSizeF() const { return m_intrinsics.size_f; }
 
        void setBlockTransform(const Matrix<fltp04>& mat)
        {
            m_block_mat = mat;
            m_block_camera_mat = m_inverse_view_matrix * m_block_mat;
        }
 
        void finish()
        {
            for (uint04 i = 0; i < m_active.weights.size(); i++)
            {
                if (m_active.weights[i] >= s_record_weight)
                {
                    m_inactive.locations.add(m_active.locations[i]);
                    m_inactive.data.add(m_active.data[i]);
                    m_inactive.weights.add(m_active.weights[i]);
                }
            }
            m_active.clear();
        }
 
        void setFilterMatchingPoints(bool filter_matching) { m_filter_matching_points = filter_matching; }
 
        void getActiveLocations(Buffer<Vertex<3, fltp04>>& locations, Buffer<RGBColor>& colors) const
        {
            locations.ensureCapacity(locations.size() + m_active.weights.size());
            colors.ensureCapacity(colors.size() + m_active.weights.size());
            for (uint04 i = 0; i < m_active.weights.size(); i++)
            {
                if (m_active.weights[i] >= s_record_weight)
                {
                    locations.add(m_active.locations[i]);
                    colors.add(m_active.data[i].color());
                }
            }
        }
 
        void getInactiveLocations(Buffer<Vertex<3, fltp04>>& locations, Buffer<RGBColor>& colors) const
        {
            uint04 offset = locations.size();
            locations.setSize(offset + m_inactive.size());
            colors.setSize(offset + m_inactive.size());
            memcpy(locations.begin(offset), m_inactive.locations.begin(), sizeof(Vertex<3, fltp04>) * m_inactive.size());
            for (uint04 i = 0; i < m_inactive.size(); i++) colors[i + offset] = m_inactive.data[i].color();
        }
 
        void getActiveNodes(Buffer<t_point_type>& nodes) const
        {
            nodes.ensureCapacity(m_active.weights.size());
            for (uint04 i = 0; i < m_active.weights.size(); i++)
                if (m_active.weights[i] >= s_record_weight) nodes.add(m_active.data[i]);
        }
 
        void getActiveWeights(Buffer<uint04>& weights) const
        {
            weights.ensureCapacity(m_active.weights.size());
            for (uint04 i = 0; i < m_active.weights.size(); i++)
                if (m_active.weights[i] >= s_record_weight) weights.add(m_active.weights[i]);
        }
 
        void clearInactiveLocations() { m_inactive.clear(); }
        void clearInactiveLocations(PointContainer<t_point_type>& points) { points.clear(); std::swap(m_inactive, points); }
 
        void addBlockPoints(const Buffer<Vertex<3, fltp04>>& locations, const Buffer<t_point_type>& nodes, const Buffer<uint04>& weights)
        {
            lib_assert(locations.size() == nodes.size(), "invalid size");
            lib_assert(nodes.size() == weights.size(), "invalid size");
            for (uint04 i = 0; i < locations.size(); i++)
            {
                if constexpr (t_point_type::HasOffset()) m_active.locations.add(m_block_mat * (locations[i] + nodes[i].offset()));
                else m_active.locations.add(m_block_mat * locations[i]);
                m_active.data.add(nodes[i]);
                m_active.weights.add(weights[i]);
            }
        }
 
        PointContainer<t_point_type>& inactive() { return m_inactive; }
        PointContainer<t_point_type>& active() { return m_active; }
 
        void postClearAll() { m_request_clear_all = true; }
        void setFrameReduction(uint04 frame_reduction) { m_frame_reduction = frame_reduction; }
        void setViewSize(const Vector<2, uint04>& view_size) { m_intrinsics.setScreenSize(view_size); }
        const Buffer<uint04>& pixelBuffer() const { return m_pixel_buffer; }
 
        [[nodiscard]] inline Vertex<3, fltp04> pixelToPoint(Vertex<3, fltp04> pixel) const
        {
            const auto& intr = m_intrinsics;
            const fltp04 invfx = intr.inv_f[X], invfy = intr.inv_f[Y];
            fltp04 x = (pixel[X] - intr.pp[X]) * invfx;
            fltp04 y = (pixel[Y] - intr.pp[Y]) * invfy;
            const fltp04 xo = x, yo = y;
            const fltp04 k1 = intr.coeffs[0], k2 = intr.coeffs[1], p1 = intr.coeffs[2], p2 = intr.coeffs[3], k3 = intr.coeffs[4];
            for (int i = 0; i < 3; ++i)
            {
                const fltp04 r2 = std::fma(x, x, y * y);
                const fltp04 icdist = fltp04(1) / (std::fma(std::fma(std::fma(k3, r2, k2), r2, k1), r2, fltp04(1)));
                const fltp04 xq = x * icdist;
                const fltp04 yq = y * icdist;
                const fltp04 xq2 = xq * xq;
                const fltp04 yq2 = yq * yq;
                const fltp04 xyq = xq * yq;
                const fltp04 dx = fltp04(2) * p1 * xyq + p2 * (r2 + fltp04(2) * xq2);
                const fltp04 dy = fltp04(2) * p2 * xyq + p1 * (r2 + fltp04(2) * yq2);
                x = (xo - dx) * icdist;
                y = (yo - dy) * icdist;
            }
            return { pixel[Z] * x,pixel[Z] * y,pixel[Z] };
        }
 
        Vertex<2, fltp04> pointToPixel(Vertex<3, fltp04> location) const
        {
            Vertex<2, fltp04> xy;
            const fltp04 iz = fltp04(1) / cast<fltp04>(location[Z]);
            xy[X] = cast<fltp04>(location[X]) * iz;
            xy[Y] = cast<fltp04>(location[Y]) * iz;
            if (!m_camera_location_bounds.contains(xy)) return Constant<Vector<2, fltp04>>::Invalid;
 
            const fltp04 x0 = xy[X];
            const fltp04 y0 = xy[Y];
            const fltp04 x2 = x0 * x0;
            const fltp04 y2 = y0 * y0;
            const fltp04 r2 = x2 + y2;
            const fltp04 k1 = m_intrinsics.coeffs[0];
            const fltp04 k2 = m_intrinsics.coeffs[1];
            const fltp04 p1 = m_intrinsics.coeffs[2];
            const fltp04 p2 = m_intrinsics.coeffs[3];
            const fltp04 k3 = m_intrinsics.coeffs[4];
 
            const fltp04 f = std::fma(std::fma(std::fma(k3, r2, k2), r2, k1), r2, fltp04(1));
            const fltp04 xr = x0 * f;
            const fltp04 yr = y0 * f;
 
            const fltp04 two_xy = fltp04(2) * x0 * y0;
            const fltp04 dx = xr + p1 * two_xy + p2 * (r2 + fltp04(2) * x2);
            const fltp04 dy = yr + p2 * two_xy + p1 * (r2 + fltp04(2) * y2);
 
            Vertex<2, fltp04> pixel;
            pixel[0] = std::fma(dx, m_intrinsics.f[X], m_intrinsics.pp[X]);
            pixel[1] = std::fma(dy, m_intrinsics.f[Y], m_intrinsics.pp[Y]);
            return pixel;
        }
 
    protected:
        void fillUBO(uint04 record_weight,
            uint04 discard_weight, uint04 max_weight, uint04 weight_reduction,
            ViewPortSmoothShaderLogic::UBO& u)
        {
            u.inv_view = m_inverse_view_matrix;
            u.pp = m_intrinsics.pp;
            u.f = m_intrinsics.f;
            u.inv_f = m_intrinsics.inv_f;
            u.size_f = m_intrinsics.size_f;
            u.size_fr = m_intrinsics.size_fr;
            u.size_i = m_intrinsics.size_i;
            u.k12p1p2[0] = m_intrinsics.coeffs[0];
            u.k12p1p2[1] = m_intrinsics.coeffs[1];
            u.k12p1p2[2] = m_intrinsics.coeffs[2];
            u.k12p1p2[3] = m_intrinsics.coeffs[3];
            u.k3 = m_intrinsics.coeffs[4];
            u.max_dist2 = m_max_distance * m_max_distance;
            u.max_dist2_relax = m_max_distance * 1.2f * m_max_distance * 1.2f;
            //u.filter_matching_points = 0U;// m_filter_matching_points ? 1u : 0u;
            for (uint01 i = 0; i < 3; i++)
                u.cam_pos4[i] = m_camera_location[i];
            u.cam_pos4[3] = 1.0f;
            u.max_error_percent_sqrd = m_max_error_percent_sqrd;
 
            u.weightsA[0] = record_weight;       // was s_record_weight
            u.weightsA[1] = discard_weight;      // was s_discard_weight
            u.weightsA[2] = max_weight;          // was s_max_weight
            u.weightsA[3] = weight_reduction;    // was m_frame_reduction*32
            //u.has_normal = 0u;// t_point_type::HasNormal() ? 1u : 0u;
            //u.lock_weights = m_lock_weights ? 1u : 0u;
            u.counts[0] = m_active.size();
        }
        void calcHistoricPixelLocationsGL(uint04 record_weight, uint04 discard_weight, uint04 max_weight, uint04 weight_reduction)
        {
            
            if (m_active.locations.size() == 0)
                return;
            m_point_smooth_logic->ensureCurrent();
            ViewPortSmoothShaderLogic::UBO u;
            fillUBO(record_weight, discard_weight, max_weight, weight_reduction, u);
            m_point_smooth_logic->setUBO(u);
            m_point_smooth_logic->uploadInputs(m_active, m_intrinsics.size_i.product());
            m_point_smooth_logic->runAll(m_active.size());
        }
 
    protected:
        ViewPortSmoothShaderLogic* m_point_smooth_logic = nullptr;
        Matrix<fltp04> m_view_matrix = Constant<Matrix<fltp04>>::Invalid;
        Matrix<fltp04> m_inverse_view_matrix = Constant<Matrix<fltp04>>::Invalid;
        Matrix<fltp04> m_block_camera_mat = Constant<Matrix<fltp04>>::Invalid;
        Matrix<fltp04> m_block_mat = Constant<Matrix<fltp04>>::Invalid;
        PointContainer<t_point_type> m_inactive;
        PointContainer<t_point_type> m_active;
        Intrinsics m_intrinsics;
        Buffer<uint04> m_pixel_buffer;
        Bounds<2, fltp04> m_camera_location_bounds;
        Time m_last_update_time = Constant<Time>::Invalid;
        Vertex<3, fltp04> m_camera_location;
        fltp04 m_max_distance = 0.0f;
        uint04 m_frame_reduction = 0U;
        fltp04 m_max_error_percent_sqrd = 0.20f * 0.20f;
        bool m_request_clear_all = false;
        bool m_filter_matching_points = false;
        bool m_lock_weights = false;
        
    };
}