public Pnt(LasFile lasFile, LasFile.PDRF1 rec)
 {
     LasFileRef = lasFile;
     X          = ((double)rec.X * (double)lasFile.Header.XScaleFactor) + (double)lasFile.Header.XOffset;
     Y          = ((double)rec.Y * (double)lasFile.Header.YScaleFactor) + (double)lasFile.Header.YOffset;
     Z          = ((double)rec.Z * (double)lasFile.Header.ZScaleFactor) + (double)lasFile.Header.ZOffset;
     Intensity  = (float)rec.Intensity;
     Time       = GetFromGps(Convert.ToDouble("1" + rec.GPSTime));
     //Console.WriteLine();
 }
        private void Button_Colorize_Click(object sender, EventArgs e)
        {
            ValidateForm();
            Cursor = Cursors.WaitCursor;
            if (button_Colorize.Enabled == true)
            {
                int      gpsWeek = GetWeek(dateTime_ScanDate.Value);
                DateTime date    = dateTime_ScanDate.Value;
                double   adjAng  = (double)Properties.Settings.Default.setting_CamAngle;

                //Get the SBET data
                tool_Status.Text    = "Loading Sbet Data";
                tool_Status.Visible = true;
                this.Refresh();
                SbetRecsList ledger;
                if (!string.IsNullOrEmpty(text_CamEventsFile.Text))
                {
                    //Use the camera timed events to only load relevant data in the ledger
                    ledger = new SbetRecsList(text_SbetInput.Text, text_CamEventsFile.Text, (int)numeric_LeapSecs.Value, gpsWeek, combo_CoordSys.Text);
                }
                else
                {
                    //Load everything
                    ledger = new SbetRecsList(text_SbetInput.Text, (int)numeric_LeapSecs.Value, gpsWeek, combo_CoordSys.Text);
                }

                //Ledger loaded, get the pics
                tool_Status.Text = "Loading Panoramic files";
                this.Refresh();

                bool picsError = false;
                Parallel.ForEach(Directory.GetFiles(text_PicsInputFolder.Text, "*.png", SearchOption.TopDirectoryOnly), (f, state) =>
                {
                    if (picsError == false)
                    {
                        PanoPic newPic     = new PanoPic(f, (int)numeric_TimeOffset.Value, (double)numeric_TimeScaleFact.Value);
                        newPic.SbetRecord  = ledger.GetClosestRecord(newPic.UtcTime);
                        Matrix3D yawC      = NewRotateAroundZ(newPic.SbetRecord.yaw + DegreeToRadian((double)numeric_YawOffset.Value));
                        Matrix3D pitchC    = NewRotateAroundX(newPic.SbetRecord.pitch + DegreeToRadian((double)numeric_PitchOffset.Value));
                        Matrix3D rollC     = NewRotateAroundY(newPic.SbetRecord.roll + DegreeToRadian((double)numeric_RollOffset.Value));
                        Matrix3D newMatrix = yawC * rollC * pitchC;
                        Point3D oSet       = new Point3D((double)numeric_XOffset.Value, (double)numeric_YOffset.Value, (double)numeric_ZOffset.Value);
                        Point3D test       = Point3D.Multiply(oSet, newMatrix);
                        newPic.CamCentre   = new Point3D(newPic.SbetRecord.XYZ.X + test.X, newPic.SbetRecord.XYZ.Y + test.Y, newPic.SbetRecord.XYZ.Z + test.Z);

                        lock (pics)
                        {
                            try
                            {
                                pics.Add(newPic.UtcTime, newPic);
                            }
                            catch (Exception)
                            {
                                pics.TryGetValue(newPic.UtcTime, out PanoPic res);
                                MessageBox.Show("Time data is the same for " + Path.GetFileName(newPic.PicPath) + " and " + Path.GetFileName(res.PicPath) + "Processing will stop", "Duplicate data file", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                                picsError = true;
                                state.Break();
                            }
                        }
                    }
                });

                if (picsError == false)
                {
                    //Start preparing the points batch processing
                    List <string> files = text_PtsInput.Lines.ToList();
                    files.RemoveAll(str => String.IsNullOrEmpty(str));

                    tool_Progress.Maximum = files.Count;
                    tool_Progress.Minimum = 0;
                    tool_Progress.Value   = 0;
                    tool_Progress.Visible = true;
                    this.Refresh();

                    foreach (string pointfile in files)
                    {
                        tool_Status.Text = "processing " + Path.GetFileName(pointfile);
                        this.Refresh();
                        //init the point list
                        ConcurrentQueue <Pnt> points = new ConcurrentQueue <Pnt>();

                        if (Path.GetExtension(pointfile).ToUpper() == ".LAS")
                        {
                            //Read the las file
                            LasFile lasFile = new LasFile(pointfile);
                            Parallel.ForEach(lasFile.Records, (s) => { points.Enqueue(new Pnt(lasFile, s)); });
                        }
                        else
                        {
                            List <string> ptStr = new List <string>();
                            //Read all the points
                            using (System.IO.StreamReader sr = new System.IO.StreamReader(pointfile))
                            {
                                string line;
                                while ((line = sr.ReadLine()) != null)
                                {
                                    ptStr.Add(line);
                                }
                            }
                            Parallel.ForEach(ptStr, (s) => { points.Enqueue(new Pnt(s, date)); });
                        }

                        string picError = "false";

                        //For each point, find the closest picture in time
                        Parallel.ForEach(points, (pt, state) =>
                        {
                            if (picError == "true")
                            {
                                state.Break();
                            }
                            else
                            {
                                PanoPic refpic = GetClosestKey(pt.Time, pics).Value;
                                if (refpic == null)
                                {
                                    lock (picError) { picError = "true"; }
                                    //missing picture
                                    state.Break();
                                    Console.WriteLine("failed to find a picture for " + pt.Time.ToLongDateString());
                                }
                                else
                                {
                                    //Add the point to the closest image in time
                                    lock (refpic) { refpic.PicPoints.Add(pt); }
                                    pt.RefPic = refpic;
                                }
                            }
                        });

                        if (picError == "true")
                        {
                            MessageBox.Show("At least one part of the dataset doesn't have picture data" + Environment.NewLine + "Processing will now stop", "Insufficient Picture data", MessageBoxButtons.OK, MessageBoxIcon.Error);
                        }
                        else
                        {
                            //Colorize per pic
                            foreach (var p in pics)
                            {
                                if (p.Value.PicPoints.Count > 0)
                                {
                                    p.Value.InitBitmap();
                                    int pWid = p.Value.bit.Width;
                                    int pHei = p.Value.bit.Height;

                                    Matrix3D yawM   = NewRotateAroundZ(p.Value.SbetRecord.yaw + DegreeToRadian(adjAng) + (double)numeric_YawOffset.Value);
                                    Matrix3D pitchM = NewRotateAroundX(((-1) * p.Value.SbetRecord.pitch) + (double)numeric_PitchOffset.Value);
                                    Matrix3D rollM  = NewRotateAroundY(((-1) * p.Value.SbetRecord.roll) + (double)numeric_RollOffset.Value);

                                    Matrix3D combined = yawM * rollM * pitchM;

                                    _ = Parallel.ForEach(p.Value.PicPoints, (pt) =>
                                    {
                                        Point3D newPoint = new Point3D(pt.X - p.Value.CamCentre.X, pt.Y - p.Value.CamCentre.Y, pt.Z - p.Value.CamCentre.Z);
                                        Point3D trPoint  = Point3D.Multiply(newPoint, combined);

                                        double m, n;
                                        double hWid = (double)pWid / 2;
                                        double hPi  = Math.PI / 2;

                                        if (trPoint.X > 0)
                                        {
                                            double temp = (hPi - Math.Atan(trPoint.Y / trPoint.X)) / (Math.PI * 2);
                                            m           = hWid + (temp * ((double)pWid - 1));
                                        }
                                        else
                                        {
                                            double temp = (hPi + Math.Atan(trPoint.Y / trPoint.X)) / (Math.PI * 2);
                                            m           = hWid - (temp * ((double)pWid - 1));
                                        }
                                        double tmpAtan = Math.Atan(trPoint.Z / Math.Sqrt(Math.Pow(trPoint.X, 2) + Math.Pow(trPoint.Y, 2)));
                                        n = (pHei - 1) * (0.5 - (tmpAtan / Math.PI));

                                        int xP = Convert.ToInt32(m);
                                        int yP = (Convert.ToInt32(n));

                                        //check if the point is in a shadow area
                                        if (polyPts.Count > 3)
                                        {
                                            if (IsPointInPolygon(new PointF((float)xP / (float)pWid, (float)yP / (float)pHei), polyPts.ToArray()))
                                            {
                                                //Move the point to the next picture
                                                int index = pics.IndexOfKey(p.Key) + 1;
                                                if (index > pics.Count - 1)
                                                {
                                                    index -= 1;
                                                    lock (p.Value.bit) { pt.Color = p.Value.bit.GetPixel(xP, yP); }
                                                }
                                                else
                                                {
                                                    PanoPic nextPic = pics.Values[index];
                                                    lock (nextPic) { nextPic.PicPoints.Add(pt); }
                                                    pt.RefPic = nextPic;
                                                }
                                            }
                                            else
                                            {
                                                lock (p.Value.bit) { pt.Color = p.Value.bit.GetPixel(xP, yP); }
                                            }
                                        }
                                        else
                                        {
                                            lock (p.Value.bit) { pt.Color = p.Value.bit.GetPixel(xP, yP); }
                                        }
                                    });
                                }
                            }

                            tool_Status.Text = "processing complete for" + Path.GetFileName(pointfile) + ", writing output";
                            this.Refresh();
                            bool          intLast  = (bool)Properties.Settings.Default.setting_XYZRGBI;
                            List <string> contents = new List <string>();
                            Parallel.ForEach(pics, (p) =>
                            {
                                if (p.Value.PicPoints.Count > 0)
                                {
                                    List <string> tmpContents = new List <string>();
                                    foreach (Pnt pt in p.Value.PicPoints)
                                    {
                                        if (pt.RefPic == p.Value)
                                        {
                                            if (intLast == true)
                                            {
                                                lock (contents) { tmpContents.Add(pt.X.ToString() + " " + pt.Y.ToString() + " " + pt.Z.ToString() + " " + pt.Color.R + " " + pt.Color.G + " " + pt.Color.B + " " + pt.Intensity.ToString()); }
                                            }
                                            else
                                            {
                                                lock (contents) { tmpContents.Add(pt.X.ToString() + " " + pt.Y.ToString() + " " + pt.Z.ToString() + " " + pt.Intensity.ToString() + " " + pt.Color.R + " " + pt.Color.G + " " + pt.Color.B); }
                                            }
                                        }
                                    }

                                    if (check_PerPic.Checked)
                                    {
                                        File.WriteAllLines(text_OutputFolder.Text + @"\" + Path.GetFileNameWithoutExtension(pointfile) + "-" + Path.GetFileNameWithoutExtension(p.Value.PicPath) + ".pts", tmpContents);
                                    }
                                    else
                                    {
                                        lock (contents)
                                        {
                                            contents.AddRange(tmpContents);
                                        }
                                    }
                                    List <Pnt> swapQueue = new List <Pnt>();
                                    p.Value.PicPoints    = swapQueue;
                                    tmpContents.Clear();
                                }

                                if (p.Value.bit != null)
                                {
                                    p.Value.bit.Dispose();
                                }
                            });

                            if (!check_PerPic.Checked)
                            {
                                File.WriteAllLines(text_OutputFolder.Text + @"\" + Path.GetFileNameWithoutExtension(pointfile) + ".pts", contents.ToArray());
                                contents.Clear();
                            }
                            tool_Progress.Value += 1;
                        }
                    }
                }

                tool_Progress.Visible = false;
                tool_Progress.Value   = 0;
                tool_Status.Text      = "Processing complete!";
                MessageBox.Show("Processing complete", "Done", MessageBoxButtons.OK, MessageBoxIcon.Information);
                this.Refresh();
                OpenFolder(text_OutputFolder.Text);
                Application.Exit();
            }
            else
            {
                MessageBox.Show("Errors found, please review the parameters", "Processing aborted", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }

            Cursor = Cursors.Default;
        }