Scan.h

/*--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants
Licensee a limited, non-exclusive, non-transferable, royalty-free license
(without the right to sublicense) to use the API solely for the purpose of
Licensee's internal development efforts to develop applications for which
the API was provided.
 
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Design
File: Scan
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/Model.h>
#if NDEVR_SCAN
#include <NDEVR/ScanMode.h>
#include <NDEVR/MaterialRaster.h>
#include <NDEVR/Angle.h>
#include <NDEVR/Vertex.h>
#include <NDEVR/Buffer.h>
#include <NDEVR/Bounds.h>
#include <NDEVR/VectorFunctions.h>
#include <NDEVR/MatrixFunctions.h>
 
namespace NDEVR
{
    class Scan;
    typedef AlocatingAlignedBuffer<Scan, 64> ScanBuffer;
    struct RasterInfo;
    class ImageData;
    struct ZippedRadialMeshOptions;
    NDEVR_DESIGN_T_API template class NDEVR_DESIGN_API StringStream<ScanMode>;

    struct ScanSetupSettings
    {
        ScanMode scan_mode = ScanMode::e_undefined;                             
        Vector<2, Angle<fltp08>> angle_of_separation = Vector<2, Angle<fltp08>>(Angle<fltp08>(DEGREES, 2.0));   
        Bounds<2, Angle<fltp08>> scan_bounds = Constant<Bounds<2, Angle<fltp08>>>::Max; 
        uint01 scan_mesh_reach = 3;                                                 
        bool is_continuous = false;                                                 
        bool generate_mesh = true;                                                  
        bool records_color = false;                                                 
        bool records_distance = true;                                               
        bool records_polar = true;                                                  
        bool records_intensity = true;                                              
        bool records_timestamp = true;                                              
        bool is_child_of_station = true;                                            
        RasterInfo raster_info;                                                     
    };
    enum ScanTargetBitFlags
    {
          e_cloud               
        , e_mesh                
        , e_calibration_a       
        , e_calibration_b       
        , e_color_raster        
    };

    template<class t_type>
    struct PolarScanRecord
    {
        Vertex<3, t_type> position = Vector<3, t_type>(0.0);        
        Vector<4, t_type> color = Vector<4, t_type>(0.0);           
        Vector<2, Angle<t_type>> coord;                             
        Vector<2, Angle<t_type>> raw_coord;                         
        t_type distance = 0.0;                                      
        t_type intensity = 0.0;                                     
        t_type total_weight = 0.0;                                  
        t_type max_distance = Constant<t_type>::Min;                
        t_type min_distance = Constant<t_type>::Max;                
        t_type scan_time = 0.0;                                     
        t_type scan_sweep = 0.0;                                    
        BitFlag target_bit_flags = BitFlag(0);                      

        PolarScanRecord()
        {}

        PolarScanRecord(Angle<t_type> roll, Angle<t_type> pitch)
            : coord(roll, pitch)
        {}

        PolarScanRecord(const Vector<2, Angle<t_type>>& roll_pitch)
            : coord(roll_pitch)
        {}

        void updateWeight(t_type weight)
        {
            t_type weight_conv = weight / total_weight;
            position *= weight_conv;
            distance *= weight_conv;
            intensity *= weight_conv;
            scan_time *= weight_conv;
            color *= weight_conv;
            total_weight = weight;
        }

        template<class t_other_type>
        void updateAverage(const PolarScanRecord<t_other_type>& record)
        {
            if (IsValid(record.position))
            {
                position += (record.total_weight * record.position).template as<3, t_type>();
                distance += cast<t_type>(record.total_weight * record.distance);
                intensity += cast<t_type>(record.total_weight * record.intensity);
                scan_time += cast<t_type>(record.total_weight * record.scan_time);
                scan_sweep += cast<t_type>(record.total_weight * record.scan_sweep);
                color += (record.total_weight * record.color).template as<4, t_type>();
                total_weight += cast<t_type>(record.total_weight);
            }
            if (IsValid(record.distance))
            {
                max_distance = getMax(max_distance, cast<t_type>(record.distance));
                min_distance = getMin(min_distance, cast<t_type>(record.distance));
            }
        }

        void clearAverage()
        {
            color = 0.0;
            distance = 0.0;
            intensity = 0.0;
            position = Vector<3, t_type>(0.0);
            total_weight = 0.0;
            max_distance = Constant<t_type>::Min;
            min_distance = Constant<t_type>::Max;
            scan_time = 0.0;
            scan_sweep = 0.0;
        }

        void autoCalcPositionH(Angle<t_type> roll = Angle<t_type>(DEGREES, 0.0f))
        {
            Matrix<fltp08> mat = Matrix<fltp08>::RotationMatrix(Vector<3, Angle<t_type>>(roll, coord[PITCH], coord[ROLL]));
            position = (mat * Vertex<3, fltp08>(0.0, 0.0, distance)).as<3, fltp04>();
        }

        void autoCalcPositionR(Angle<t_type> yaw = Angle<t_type>(DEGREES, 0.0f))
        {
            Matrix<fltp08> mat = Matrix<fltp08>::RotationMatrix(Vector<3, Angle<t_type>>(coord, yaw));
            position = (mat * Vertex<3, fltp08>(0.0, 0.0, distance)).as<3, fltp04>();
        }

        constexpr fltp08 operator[](uint01 dim) const
        {
            return position[dim];
        }
    };

    template<>
    struct ObjectInfo<PolarScanRecord<fltp04>, false, false>
    {
        static const uint01 Dimensions = 0;     
        static const bool Vector = false;           
        static const bool Buffer = false;           
        static const bool Primitive = true;         
        static const bool Pointer = false;          
        static const bool Unsigned = false;         
        static const bool Float = false;            
        static const bool Integer = false;          
        static const bool Number = false;           
        static const bool Enum = false;             
        static const bool String = false;           
        static const bool Color = false;            
        static const bool Boolean = false;          

        static constexpr ObjectInfo<PolarScanRecord<fltp04>, false, false> VectorSub() { return ObjectInfo<PolarScanRecord<fltp04>, false, false>(); }
    };
 
    class MaterialRasterBase;
    class NDEVR_DESIGN_API Scan : public Model
    {
    public:

        struct ScanCalibration
        {
            fltp08 head_offset = 0.0;                                   
            fltp08 emitter_offset = -0.1335;                            
            fltp08 lens_offset = 0.0;                                   
            Angle<fltp08> beam_angle = Angle<fltp08>(0);                
            Angle<fltp08> head_tilt_angle = Angle<fltp08>(0);           
            Angle<fltp08> laser_tilt_angle = Angle<fltp08>(0);          
        };
    public:
        Scan();//Invalid

        explicit Scan(const Model& model);

        Scan(const Model& model, const ScanSetupSettings& settings);

        bool needsMeshUpdate(const void* lock);

        void clearNeedsMeshUpdate(const void* lock);

        void updateScanProperties(bool scan_ended = false);

        void updateScanModel(bool scan_ended, bool force_calculate_mesh, const void* lock);

        void cacheChannels();

        Model scanMesh()  const;

        Model scanCloud() const;

        bool hasCalibrationRing(uint04 index) const;

        Scan& createCalibrationRing(uint04 index);

        Scan& calibrationRing(uint04 index);

        const Scan& calibrationRing(uint04 index) const;

        void reserveRecordSpace(uint04 space);

        void addToRecordQueue(const PolarScanRecord<fltp04>& record);

        void splitScan(const Vector<3, fltp08>& distance, const void* lock);

        void finishMoveScan(const void* lock_ptr);

        void addSweep(const void* lock_ptr = nullptr);

        void moveScan(const Vector<3, fltp08>& distance, fltp08 vertex_epsilon, fltp08 motion_only_max_difference, bool filter_by_direction, const void* lock_ptr);

        void setupMesh(bool add_new_structure, const void* lock_ptr);

        void setScannerOrientation(const Vector<3, Angle<fltp08>>& orientation);

        void setDeploymentOrientation(const Vector<3, Angle<fltp08>>& orientation);

        void setScannerInfo(const TranslatedString& scanner_info);

        TranslatedString scannerInfo() const;

        void createImageRaster(const RasterInfo& info);

        void addImageToRaster(const ImageData& data, const Vector<2, Angle<sint04>>& fov, const Matrix<fltp08>& camera_matrix);

        bool scanCalibrated() const;

        void removeShadowTriangles(fltp04 cuttoff_ratio);

        Vector<3, Angle<fltp08>> scannerOrientation() const;

        Vector<3, Angle<fltp08>> deploymentOrientation() const;

        Angle<fltp08> calculateCalibrationConstant(bool global_yaw, const void* lock) const;

        void calibrateScan(bool global_yaw, InterpolationValues value = InterpolationValues::e_linear, const void* lock = nullptr);

        void calibrateScan(Angle<fltp08> angle, bool global_yaw, InterpolationValues value = InterpolationValues::e_linear, const void* lock = nullptr);

        void decalibrateScan();

        void ensureModelFormat();//For older scans

        void updateOlderAngles();//Need to convert older models degrees to internal units

        Model pointsLayer() const;

        Model meshLayer() const;

        Model circleScanLayer() const;

        void addRecords(const Buffer<Vertex<3, fltp08>>& positions, const Vertex<3, fltp08>& scan_center = Vertex<3, fltp08>(0.0), bool add_to_cloud = true, bool add_to_mesh = true);

        void createMeshFromScan(const Vertex<3, fltp08>& scan_center = Vertex<3, fltp08>(0.0), const void* lock = nullptr);

        Bounds<2, Angle<fltp08>> scanBounds() const;

        Angle<fltp08> primaryAngleOfSeparation() const;

        Vector<2, Angle<fltp08>> anglesOfSeparation() const;

        void setAnglesOfSeperation(Vector<2, Angle<fltp08>>& angle);

        void setScanBounds(const Bounds<2, Angle<fltp08>>& bounds);

        Vector<3, Angle<fltp08>> angleCalibrationOffset() const;

        void setCalibration(const ScanCalibration& calibration, const void* lock = nullptr);

        ScanCalibration getCalibration() const;

        ScanMode scanMode() const;

        static constexpr StringView TypeName() { return "scan"; }

        static constexpr StringView CalibrationRingTypeName() { return "calibration_ring"; }

        TimeSpan scanDuration() const;

        void setScanDuration(const TimeSpan& span);

        void setIsDynamic(bool is_dynamic);

        void setMotionOnlyReference();

        bool isScanValid() const;

        void setValid(bool is_valid);

        Buffer<PolarScanRecord<fltp04>> generatePolarRecords(const Vertex<3, fltp08>& scan_center = Vertex<3, fltp08>(0.0), const void* lock = nullptr);
    protected:
        void setupScanRecordsFromExisting(const Vertex<3, fltp08>& scan_center = Vertex<3, fltp08>(0.0), const void* lock = nullptr);

        void addMeshRasterRecord(const PolarScanRecord<fltp04>& record, sint04 range);

        void addMeshRasterRecord(const ZippedRadialMeshOptions& options, Vector<2, Angle<fltp08>> delta, const PolarScanRecord<fltp04>& record);

        void blurMeshRecords(sint04 range);

        void addCloudRecord(const PolarScanRecord<fltp04>& record);

        void init();

        void updateScanColors();

        void initMaterial();

        [[nodiscard]] TranslatedString createDescription(bool is_finished);

        static Angle<fltp08> GetBestRotation(Angle<fltp08> increment, const Buffer<Vertex<3, fltp08>>& v_a, const Buffer<Vertex<3, fltp08>>& v_b, const Matrix<fltp08>& mat, const Vector<3, fltp08>& up, InfoPipe* pipe = nullptr);

        static Angle<fltp08> GetBestRotation_2DAffine(const Buffer<Vertex<3, fltp08>>& v_a, const Buffer<Vertex<3, fltp08>>& v_b, const Matrix<fltp08>& mat);
    protected:
        mutable Vector<3, Angle<fltp08>> m_scanner_orientation = Constant<Vector<3, Angle<fltp08>>>::Invalid;   
        mutable Vector<3, Angle<fltp08>> m_deployment_orientation = Constant<Vector<3, Angle<fltp08>>>::Invalid;    
        MaterialRasterBase* m_image_raster = nullptr;                   
        Bounds<1, uint04> m_mesh_write_update_bounds = Constant<Bounds<1, uint04>>::Min;    
        Bounds<3, fltp04> m_scan_record_bounds = Constant<Bounds<3, fltp04>>::Min;          
        PolarScanRecord<fltp08> m_total_avg;                            
        Buffer<PolarScanRecord<fltp04>> m_cloud_write_queue;            
        Buffer<PolarScanRecord<fltp04>> m_mesh_write_queue;             
        Buffer<uint04> m_mesh_locations;                                    
        mutable ScanBuffer m_reference_scans;                           
        uint04 m_cloud_intensity_channel = Constant<uint04>::Invalid;   
        uint04 m_cloud_roll_channel = Constant<uint04>::Invalid;        
        uint04 m_cloud_pitch_channel = Constant<uint04>::Invalid;       
        uint04 m_cloud_distance_channel = Constant<uint04>::Invalid;    
        uint04 m_scan_time_channel = Constant<uint04>::Invalid;         
        uint04 m_scan_sweep_channel = Constant<uint04>::Invalid;        
        uint04 m_scan_original_channel = Constant<uint04>::Invalid;     
        uint04 m_scan_sweeps = Constant<uint04>::Invalid;               
        uint04 m_mesh_write_offset = 0;                                 
        Vertex<3, fltp04> m_split_offset = Vertex<3, fltp04>(0.0);      
        Vertex<3, fltp04> m_last_min_mid_max;                           
        bool m_is_dynamic_scan = false;                                 
        bool m_needs_raster_update = false;                             
        bool m_needs_channel_update = true;                             
        mutable ScanSetupSettings m_settings;                           
 
    protected:
        static const uint04 s_scan_mesh = 0;                            
        static const uint04 s_scan_cloud = 1;                           
        static const uint04 s_calibration_circle_start_ring = 2;        
    };
 
}
#endif