label-3d-points/public/js/radar.js

import * as THREE from './lib/three.module.js';
import { PCDLoader } from './lib/PCDLoader.js';
import { matmul, euler_angle_to_rotate_matrix_3by3} from "./util.js"

function Radar(sceneMeta, world, frameInfo, radarName){
    this.world = world;
    this.frameInfo = frameInfo;    
    this.name = radarName;
    this.sceneMeta = sceneMeta;
    this.coordinatesOffset = world.coordinatesOffset;

    this.showPointsOnly = false;
    this.showRadarBoxFlag = false;
    this.cssStyleSelector = this.sceneMeta.calib.radar[this.name].cssstyleselector;
    this.color = this.sceneMeta.calib.radar[this.name].color;
    this.velocityScale = 0.3;

    if (!this.color){
        this.color = [1.0, 0.0, 0.0];
    }

    this._radar_points_raw = null;  // read from file, centered at 0
    this.elements = null;   // geometry points

    this.preloaded = false;
    this.loaded = false;


    this.go_cmd_received = false;
    this.webglScene = null;
    this.on_go_finished = null;
    this.go = function(webglScene, on_go_finished){
        this.webglScene = webglScene;

        if (this.preloaded){
            if (this.elements){
                this.webglScene.add(this.elements.points);

                if (!this.showPointsOnly)
                    this.elements.arrows.forEach(a=>this.webglScene.add(a));

                if (this.showRadarBoxFlag)
                    this.webglScene.add(this.radar_box);
            }

            this.loaded = true;
            if (on_go_finished)
                on_go_finished();
        }
        
        //anyway we save go cmd 
        {
            this.go_cmd_received = true;
            this.on_go_finished = on_go_finished;
        }
    };

    this.showRadarBox = function(){
        this.showRadarBoxFlag = true;
        this.webglScene.add(this.radar_box);
    };

    this.hideRadarBox = function(){
        this.showRadarBoxFlag = false;
        this.webglScene.remove(this.radar_box);
    };

    this.get_unoffset_radar_points = function(){
        if (this.elements){
            let pts = this.elements.points.geometry.getAttribute("position").array;
            return pts.map((p,i)=>p-this.world.coordinatesOffset[i %3]);
        }
        else{
            return [];
        }
    };

    // todo: what if it's not preloaded yet
    this.unload = function(keep_box){
        if (this.elements){
            this.webglScene.remove(this.elements.points);
            if (!this.showPointsOnly)
                this.elements.arrows.forEach(a=>this.webglScene.remove(a));
            
            if (!keep_box)
                this.webglScene.remove(this.radar_box);
        }
        this.loaded = false;
    };

    // todo: its possible to remove points before preloading,
    this.deleteAll = function(keep_box){
        if (this.loaded){
            this.unload();
        }

        if (this.elements){
            //this.scene.remove(this.points);
            this.world.data.dbg.free();

            if (this.elements.points)
            {
                this.elements.points.geometry.dispose();
                this.elements.points.material.dispose();
            }

            if (this.elements.arrows)
            {
                this.elements.arrows.forEach(a=>{
                    this.world.data.dbg.free();
                    a.geometry.dispose();
                    a.material.dispose();
                })
            }

            this.elements = null;
        }

        if (!keep_box && this.radar_box){
            this.world.data.dbg.free();
            this.radar_box.geometry.dispose();
            this.radar_box.material.dispose();
            this.radar_box = null;
        }
    };


    this.preload = function(on_preload_finished){
        var loader = new PCDLoader();

        var _self = this;
        loader.load( this.frameInfo.get_radar_path(this.name), 
            //ok
            function ( pcd ) {
                var position = pcd.position;
                //var velocity = pcd.velocity;
                // velocity is a vector anchored at position, 
                // we translate them into position of the vector head
                var velocity = position.map((p,i)=>pcd.velocity[i]+pcd.position[i]);

                // scale velocity
                // velocity = velocity.map(v=>v*_self.velocityScale);

                //_self.points_parse_time = new Date().getTime();
                //console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "parse pionts ", _self.points_parse_time - _self.create_time, "ms");
                _self._radar_points_raw = position;
                _self._radar_velocity_raw = velocity;

                // add one box to calibrate radar with lidar
                _self.radar_box = _self.createRadarBox();

                // install callback for box changing
                _self.radar_box.on_box_changed = ()=>{
                    _self.move_radar(_self.radar_box);
                };

                //position = _self.transformPointsByOffset(position);
                position = _self.move_radar_points(_self.radar_box);
                velocity = _self.move_radar_velocity(_self.radar_box);
                let elements = _self.buildRadarGeometry(position, velocity);
                
                _self.elements = elements;
                //_self.points_backup = mesh;

                _self._afterPreload();

            },

            // on progress,
            function(){},

            // on error
            function(){
                //error
                console.log("load radar failed.");
                _self._afterPreload();
            },

            // on file loaded
            function(){
                //_self.points_readfile_time = new Date().getTime();
                //console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "read file ", _self.points_readfile_time - _self.create_time, "ms");
            }
        );
    };

    // internal funcs below
    this._afterPreload = function(){
        this.preloaded = true;
        console.log(`radar ${this.radarname} preloaded`);
        if (this.on_preload_finished){
            this.on_preload_finished();
        }                
        if (this.go_cmd_received){
            this.go(this.webglScene, this.on_go_finished);
        }
    };

    this.createRadarBox = function(){
        if (this.sceneMeta.calib.radar && this.sceneMeta.calib.radar[this.name]){
            return this.world.annotation.createCuboid(
                {
                    x: this.sceneMeta.calib.radar[this.name].translation[0] + this.coordinatesOffset[0],
                    y: this.sceneMeta.calib.radar[this.name].translation[1] + this.coordinatesOffset[1],
                    z: this.sceneMeta.calib.radar[this.name].translation[2] + this.coordinatesOffset[2],
                }, 
                {x:1,y:1, z:1}, 
                {
                    x: this.sceneMeta.calib.radar[this.name].rotation[0],
                    y: this.sceneMeta.calib.radar[this.name].rotation[1],
                    z: this.sceneMeta.calib.radar[this.name].rotation[2],
                }, 
                "radar", 
                this.name);
        
        }else {
            return this.world.annotation.createCuboid(
                {x: this.coordinatesOffset[0],
                 y: this.coordinatesOffset[1],
                 z: this.coordinatesOffset[2]}, 
                {x:1,y:1, z:1}, 
                {x:0,y:0,z:0}, 
                "radar", 
                this.name);
        }
    };

    this.buildPoints = function(position){
        // build geometry
        this.world.data.dbg.alloc();
        let geometry = new THREE.BufferGeometry();
        if ( position.length > 0 ) 
            geometry.addAttribute( 'position', new THREE.Float32BufferAttribute( position, 3 ) );
        
        
        let pointColor = this.color;
        let color=[];
        for (var i =0; i< position.length; i+=3){

            color.push(pointColor[0]);
            color.push(pointColor[1]);
            color.push(pointColor[2]);
        }

        geometry.addAttribute( 'color', new THREE.Float32BufferAttribute(color, 3 ) );

        geometry.computeBoundingSphere();
        
        // build material
        let pointSize = this.sceneMeta.calib.radar[this.name].point_size;
        if (!pointSize)
            pointSize = 2;

        let material = new THREE.PointsMaterial( { size: pointSize, vertexColors: THREE.VertexColors } );
        //material.size = 2;
        material.sizeAttenuation = false;

        // build mesh
        let mesh = new THREE.Points( geometry, material );                        
        mesh.name = "radar";

        return mesh;
    };

    this.buildArrow = function(position, velocity){
        var h = 0.5;
        
        let p=position;
        let v=velocity;

        var body = [
            p[0],p[1],p[2],
            v[0],v[1],v[2],
        ];
        

        this.world.data.dbg.alloc();
        var geo = new THREE.BufferGeometry();
        geo.addAttribute( 'position', new THREE.Float32BufferAttribute(body, 3 ) );
        

        let color = this.color.map(c=>Math.round(c*255)).reduce((a,b)=>a*256+b, 0);

        var material = new THREE.LineBasicMaterial( { color: color, linewidth: 1, opacity: 1, transparent: true } );
        var arrow = new THREE.LineSegments( geo, material );
        return arrow;
    }

    this.buildRadarGeometry = function(position, velocity){
        let points = this.buildPoints(position);

        let arrows = [];
        
        if (!this.showPointsOnly)
        {
            for (let i = 0; i<position.length/3; i++)
            {
                let arr = this.buildArrow(position.slice(i*3, i*3+3), velocity.slice(i*3, i*3+3));
                arrows.push(arr);
            }
        }
        
        return {
            points: points, 
            arrows: arrows
        };
    };

    this.move_points = function(points, box){
        let trans = euler_angle_to_rotate_matrix_3by3(box.rotation);        
        let rotated_points = matmul(trans, points, 3);
        let translation=[box.position.x, box.position.y, box.position.z];
        let translated_points = rotated_points.map((p,i)=>{
            return p + translation[i % 3];
        });
        return translated_points;
    };

    this.move_radar_points = function(box){        
        return this.move_points(this._radar_points_raw, box);
    };

    this.move_radar_velocity = function(box){
        return this.move_points(this._radar_velocity_raw, box);
    }

    this.move_radar= function(box){

        let translated_points = this.move_radar_points(box);
        let translated_velocity = this.move_radar_velocity(box);

        let elements = this.buildRadarGeometry(translated_points, translated_velocity);
        
        // remove old points
        this.unload(true);
        this.deleteAll(true);

        this.elements = elements;
        //_self.points_backup = mesh;
        if (this.go_cmd_received)  // this should be always true
        {
            this.webglScene.add(this.elements.points);
            if (!this.showPointsOnly)
                this.elements.arrows.forEach(a=>this.webglScene.add(a));
        }
    };
}

function RadarManager(sceneMeta, world, frameInfo){
    this.radarList = [];

    if (world.data.cfg.enableRadar && sceneMeta.radar){
        let radars = [];
        
        for (let r in sceneMeta.calib.radar){
            if (!sceneMeta.calib.radar[r].disable)
                radars.push(r);
        }

        this.radarList = radars.map(name=>{
            return new Radar(sceneMeta, world, frameInfo, name);
        });
    }

    this.getAllBoxes = function()
    {
        if (this.showRadarBoxFlag)
        {
            return this.radarList.map(r=>r.radar_box);
        }
        else
        {
            return [];
        }
    };

    this.preloaded = function(){
        for (let r in this.radarList){
            if (!this.radarList[r].preloaded)
                return false;
        }
        return true;
    };

    this.go = function(webglScene, on_go_finished){
        this.radarList.forEach(r=>r.go(webglScene, on_go_finished));
    };

    this.preload = function(on_preload_finished){
        this.radarList.forEach(r=>r.preload(on_preload_finished));
    };

    this.unload = function(){
        this.radarList.forEach(r=>r.unload());
    };

    this.deleteAll = function(){
        this.radarList.forEach(r=>r.deleteAll());
    };

    this.getOperableObjects = function(){
        return this.radarList.flatMap(r=>r.getOperableObjects());
    };

    this.showRadarBoxFlag = false;
    this.showRadarBox = function(){
        this.showRadarBoxFlag = true;
        this.radarList.forEach(r=>r.showRadarBox());
    };

    this.hideRadarBox = function(){
        this.showRadarBoxFlag = false;
        this.radarList.forEach(r=>r.hideRadarBox());
    }
};


export {RadarManager}