GravityIntersection.h

#pragma once
#include <NDEVR/DesignObjectLookup.h>
#include <NDEVR/AngleDefinitions.h>
#include <NDEVR/BinaryHeap.h>
namespace NDEVR
{
    class GravityIntersection
    {
    public:
        Matrix<fltp08> calculateIntersection() const
        {
            SelectionInfo info;
            if (m_query_points.size() < 3)
                return Matrix<fltp08>(1.0);
            MinHeap<fltp08, LineSegment<3, fltp08>> heap(4);
            RLock lock(m_lookup->readLock());
            info.model_filter = [](const Model& model)->bool
            {
                bool ignore = false;
                ignore |= !model.get<NDPO::allow_interactions>();
                //ignore |= model.get<bool>(NDPN::is_application_owned);
                ignore |= !model.get<NDPO::spacial_visible>();
                return !ignore;
            };
            Matrix<fltp08> inverted_mat = m_current_transform.invert();
            for (uint04 i = 0; i < m_query_points.size(); i++)
            {
                LineSegment<3, fltp08> seg;
                Vertex<3, fltp08> reference = m_current_transform * m_query_points[i];
                seg[A] = reference - m_gravity_vector * m_max_fall_height;
                seg[B] = reference + m_gravity_vector * m_max_fall_height;
                info.clearLastSelection();
                info.nearest_point.screen_distance = 0.0;
                info.min_screen_cutoff_distance = 0.0;
                info.nearest_line.screen_distance = info.nearest_point.screen_distance;
                info.nearest_solid.screen_distance = 0.0;
                info.is_exact = true;
                info.current_selection = SelectionArea<3, fltp08>(seg);
                info.current_selection.setTransform(Matrix<fltp08>(1.0));
                m_lookup->processSelection(info);
                if (IsValid(info.nearestScreenLocation()))
                {
                    fltp08 length = seg.lengthSquared();
                    seg[A] = reference;
                    seg[B] = info.nearestScreenLocation();
                    //if (heap.size() < 3 || heap.extremeComp() > length)
                    {
                        heap.insert(length, inverted_mat * seg);
                        //if (heap.size() > 3)
                            //heap.deleteExtreme();
                    }
                }
            }
            if(heap.size() < 3)
                return Matrix<fltp08>(1.0);
            Buffer<Vector<3, fltp08>> original_points(heap.size());
            Buffer<Vector<3, fltp08>> intersection_points(heap.size());
            for (uint04 i = 0; i < heap.size(); i++)
            {
                original_points.add(heap.getAll()[i].first[A]);
                intersection_points.add(heap.getAll()[i].first[B]);
            }
            MatrixFunctions::TransformSolveOptions options;
            Plane<3, fltp08> p1 = Plane<3, fltp08>::CreateBestFitPlane(original_points);
            Plane<3, fltp08> p2 = Plane<3, fltp08>::CreateBestFitPlane(intersection_points);
            Ray<3, fltp08> n0 = p1.normal;
            Ray<3, fltp08> n1 = p2.normal;
            //Ray<3, fltp08> average_fall = (p2.d * p2.normal) - (p1.d * p1.normal);
            if (IsInvalid(n0) || IsInvalid(n1))
                return Matrix<fltp08>(1.0);
            if (dot(n0, n1) < 0.0)
                n1 = -n1;
            Vector<3, fltp08> cross_product = cross(n0, n1).normalized();
            auto cos_theta = dot(n0, n1);
            double sin_theta = std::sqrt(1 - cos_theta * cos_theta);
 
            Matrix<fltp08> skew(1.0);
            skew[0][0] = 0;
            skew[0][1] = -cross_product[2];
            skew[0][2] = cross_product[1];
 
            skew[1][0] = cross_product[2];
            skew[1][1] = 0;
            skew[1][2] = -cross_product[0];
 
            skew[2][0] = -cross_product[1];
            skew[2][1] = cross_product[0];
            skew[2][2] = 0;
 
            Matrix<fltp08> axis_matrix(1.0);
            for (uint01 i = 0; i < 3; ++i)
            {
                for (uint01 j = 0; j < 3; ++j)
                    axis_matrix[i][j] = cross_product[i] * cross_product[j];
            }
            Matrix<fltp08> mat(1.0);
            // Rotation matrix formula: R = I + sin(theta) * skew(axis) + (1 - cos(theta)) * (axis * axis^T)
            for (uint01 i = 0; i < 3; ++i)
            {
                for (uint01 j = 0; j < 3; ++j)
                    mat[i][j] = mat[i][j] * cos_theta + sin_theta * skew[i][j] + (1 - cos_theta) * axis_matrix[i][j];
            }
            lib_assert(IsValid(mat), "Bad matrix");
            mat = mat.transpose();
            Ray<3, fltp08> average_fall(0.0);
            for (uint04 i = 0; i < heap.size(); i++)
            {
                average_fall += heap.getAll()[i].first[B] - heap.getAll()[i].first[A];
            }
            average_fall /= cast<fltp08>(heap.size());
            //n0 =  mat.transpose() * n0;
            return Matrix<fltp08>::OffsetMatrix(average_fall) * mat;
        }

        void setup(DesignObjectLookup* lookup, const Matrix<fltp08>& mat, const Buffer<Vertex<3, fltp08>>& points)
        {
            m_lookup = lookup;
            m_current_transform = mat;
            m_query_points = points;
        }

        static Buffer<Vertex<3, fltp08>> FindGravityPoints(const Model& model, fltp08 delta, fltp08 max)
        {
            Set<Vertex<3, fltp08>> points;
            SelectionInfo info;
            info.use_interaction_flag = false;
            Matrix<fltp08> mat = model.getCompleteTransform();
            Bounds<3, fltp08> bounds = mat * model.getBoundsOfVisible();
            Matrix<fltp08> inverted_matrix = mat.invert();
            fltp08 d = bounds.span().dimensionalValue<MAX>() * delta;
            for (fltp08 x = bounds[MIN][X]; x < bounds[MAX][X]; x += d)
            {
                for (fltp08 y = bounds[MIN][Y]; y < bounds[MAX][Y]; y += d)
                {
                    info.clearLastSelection();
                    info.min_screen_cutoff_distance = max;
                    info.nearest_point.screen_distance = info.min_screen_cutoff_distance;
                    info.nearest_line.screen_distance  = info.min_screen_cutoff_distance;
                    info.nearest_solid.screen_distance = info.min_screen_cutoff_distance;
                    info.is_exact = true;
                    info.current_selection = SelectionArea<3, fltp08>(LineSegment<3, fltp08>({ x, y, bounds[MIN][Z] }, { x, y, bounds[MIN][Z] + max }));
                    info.current_selection.setTransform(Matrix<fltp08>(1.0));
                    info.parseAll(model);
                    if (IsValid(info.nearestScreenLocation()))
                        points.add(info.nearestScreenLocation());
                }
            }
            Buffer<Vertex<3, fltp08>> avg = FilterSamples(10 * d, points.values(), bounds.center());
            for (uint04 i = 0; i < avg.size(); i++)
                avg[i] = inverted_matrix * avg[i];
            return avg;
        }

        static Vertex<3, fltp08> LowestPoint(const Buffer<Vertex<3, fltp08>>& clustered_points)
        {
            Vertex<3, fltp08> lowest_point(Constant<fltp08>::Max);
            for (uint04 i = 0; i < clustered_points.size(); i++)
            {
                if (clustered_points[i][Z] < lowest_point[Z])
                {
                    lowest_point = clustered_points[i];
                }
            }
            return lowest_point;
        }

        static Buffer<Vertex<3, fltp08>> FilterSamples(fltp08 cluster_distance, Buffer<Vertex<3, fltp08>> clustered_points, Vertex<3, fltp08> center)
        {
            Buffer<Vertex<3, fltp08>> lowest_points;
            while(clustered_points.size() > 0)
            {
                Vertex<3, fltp08> lowest = LowestPoint(clustered_points);
                Vertex<3, fltp08> true_lowest = lowest;
                fltp08 d_from_center = distanceSquared(lowest, center);
                for (uint04 i = clustered_points.size() - 1; IsValid(i); i--)
                {
                    if (distanceSquared(clustered_points[i], lowest) < cluster_distance)
                    {
                        if (clustered_points[i][Z] == lowest[Z] && distanceSquared(clustered_points[i], center) < d_from_center)
                            lowest = clustered_points[i];
                        clustered_points.removeIndex(i);
                    }
                }
                lowest_points.add(lowest);
            }
            return lowest_points;
        }
    protected:
        Vector<3, fltp08> m_gravity_vector = Vector<3, fltp08>(0, 0, -1); 
        Buffer<Vertex<3, fltp08>> m_query_points; 
 
        fltp08 m_max_fall_height = 100.0; 
        Matrix<fltp08> m_current_transform; 
        DesignObjectLookup* m_lookup = nullptr; 
    };
}