3D Mouse bug fix

VIZ.Framework.Core/Core/Helper/ByteHelper.cs

+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace VIZ.Framework.Core
+{
+    /// <summary>
+    /// 位数据辅助类
+    /// </summary>
+    public static class ByteHelper
+    {
+        /// <summary>
+        /// 根据字符串获取byte数据组
+        /// 例如: 58 A0 01 00 00 00
+        /// </summary>
+        /// <param name="str">输入byte字符串</param>
+        /// <returns>byte数组</returns>
+        public static byte[] FromString(string str)
+        {
+            string[] args = str.Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
+            byte[] result = new byte[args.Length];
+
+            for (int i = 0; i < args.Length; ++i)
+            {
+                result[i] = byte.Parse(args[i], System.Globalization.NumberStyles.HexNumber);
+            }
+
+            return result;
+        }
+    }
+}

VIZ.Framework.Core/Core/Helper/FPSHelper.cs

+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace VIZ.Framework.Core
+{
+    /// <summary>
+    /// FPS计算辅助类
+    /// </summary>
+    public class FPSHelper : ModelBase, IDisposable
+    {
+        /// <summary>
+        /// 所有的辅助类
+        /// </summary>
+        private readonly static List<FPSHelper> allHelpers = new List<FPSHelper>();
+
+        public FPSHelper()
+        {
+            lock (allHelpers)
+            {
+                allHelpers.Add(this);
+            }
+        }
+
+        #region Duration -- 持续时间（单位：秒）
+
+        private int duration = 5;
+        /// <summary>
+        /// 持续时间（单位：秒）
+        /// </summary>
+        public int Duration
+        {
+            get { return duration; }
+            set { duration = value; this.RaisePropertyChanged(nameof(Duration)); }
+        }
+
+        #endregion
+
+        #region FPS -- 当前FPS
+
+        private int fps;
+        /// <summary>
+        /// 当前FPS
+        /// </summary>
+        public int FPS
+        {
+            get { return fps; }
+            set { fps = value; this.RaisePropertySaveChanged(nameof(FPS)); }
+        }
+
+        #endregion
+
+        /// <summary>
+        /// 帧数
+        /// </summary>
+        private int frameCount;
+
+        /// <summary>
+        /// 帧总数
+        /// </summary>
+        private int frameCountHistTotal;
+
+        /// <summary>
+        /// 计时器
+        /// </summary>
+        private Stopwatch stopwatch = new Stopwatch();
+
+        /// <summary>
+        /// 最后一次帧时间
+        /// </summary>
+        private long lastFrameTime;
+
+        /// <summary>
+        /// 帧数量列表
+        /// </summary>
+        private Queue<int> frameCountHist = new Queue<int>();
+
+        /// <summary>
+        /// 开始计算
+        /// </summary>
+        public void Start()
+        {
+            this.stopwatch.Start();
+        }
+
+        /// <summary>
+        /// 停止计算
+        /// </summary>
+        public void Stop()
+        {
+            this.stopwatch.Stop();
+            this.frameCount = 0;
+            this.frameCountHistTotal = 0;
+            this.lastFrameTime = 0;
+            this.frameCountHist.Clear();
+        }
+
+        /// <summary>
+        /// 计算FPS
+        /// </summary>
+        public void CalcFps()
+        {
+            ++this.frameCount;
+            if (this.stopwatch.ElapsedMilliseconds - this.lastFrameTime < 1000)
+                return;
+
+            this.frameCountHist.Enqueue(this.frameCount);
+            this.frameCountHistTotal += this.frameCount;
+            if (frameCountHist.Count > this.Duration)
+            {
+                frameCountHistTotal -= frameCountHist.Dequeue();
+            }
+
+            this.FPS = frameCountHistTotal / frameCountHist.Count;
+
+            this.frameCount = 0;
+            this.lastFrameTime = this.stopwatch.ElapsedMilliseconds;
+        }
+
+        /// <summary>
+        /// 销毁
+        /// </summary>
+        public void Dispose()
+        {
+            this.Stop();
+        }
+
+        /// <summary>
+        /// 销毁所有的辅助类
+        /// </summary>
+        public static void DisposeAll()
+        {
+            lock (allHelpers)
+            {
+                foreach (FPSHelper helper in allHelpers)
+                {
+                    helper.Dispose();
+                }
+
+                allHelpers.Clear();
+            }
+        }
+    }
+}

VIZ.Framework.Core/Expand/ThreeDMouse/Navigation3DCamera.cs

@@ -18,7 +18,7 @@ namespace VIZ.Framework.Core
         public Matrix Matrix { get; internal set; } = new Matrix(1, 0, 0, 0,
                                                                  0, 1, 0, 0,
                                                                  0, 0, 1, 0,
-                                                                 0, 0, 30, 1);
+                                                                 0, 0, 5550, 1);
 
         /// <summary>
         /// 上方向
@@ -33,6 +33,6 @@ namespace VIZ.Framework.Core
         /// <summary>
         /// 位置
         /// </summary>
-        public Point Position { get; internal set; } = new Point(0, 0, 30);
+        public Point Position { get; internal set; } = new Point(0, 0, 5550);
     }
 }

VIZ.Framework.Core/Expand/ThreeDMouse/Navigation3DManager.cs

 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
+using System.Windows.Media;
 using TDx.SpaceMouse.Navigation3D;
+using log4net;
 
 namespace VIZ.Framework.Core
 {
@@ -13,6 +16,11 @@ namespace VIZ.Framework.Core
     public static class Navigation3DManager
     {
         /// <summary>
+        /// 日志
+        /// </summary>
+        private static readonly ILog log = LogManager.GetLogger(typeof(Navigation3DManager));
+
+        /// <summary>
         /// 3D鼠标模型
         /// </summary>
         public static Navigation3DModel Navigation3DModel { get; private set; }
@@ -23,6 +31,16 @@ namespace VIZ.Framework.Core
         public static Navigation3D Navigation3D { get; private set; }
 
         /// <summary>
+        /// 计时器
+        /// </summary>
+        public static Stopwatch Stopwatch { get; private set; } = new Stopwatch();
+
+        /// <summary>
+        /// 渲染时间
+        /// </summary>
+        public static long RenderTime { get; private set; }
+
+        /// <summary>
         /// 初始化
         /// </summary>
         /// <param name="profileName">3D鼠标配置名称</param>
@@ -33,8 +51,43 @@ namespace VIZ.Framework.Core
             Navigation3DModel.MaxX = maxX;
 
             Navigation3D = new Navigation3D(Navigation3DModel);
-            Navigation3D.FrameTiming = Navigation3D.TimingSource.SpaceMouse;
+            Navigation3D.FrameTiming = Navigation3D.TimingSource.Application;
             Navigation3D.Open3DMouse(profileName);
+
+            Stopwatch.Restart();
+        }
+
+        /// <summary>
+        /// 更新渲染时间
+        /// </summary>
+        /// <returns>时间差</returns>
+        public static long UpdateRenderTime()
+        {
+            try
+            {
+                if (Stopwatch == null)
+                    return 0;
+
+                RenderTime = Stopwatch.ElapsedMilliseconds;
+
+                if (Navigation3D == null)
+                    return 0;
+
+                long diff = RenderTime - (long)Navigation3D.FrameTime;
+
+                if (Navigation3D.FrameTime >= RenderTime)
+                    return 0;
+
+                Navigation3D.FrameTime = RenderTime;
+
+                return diff;
+            }
+            catch (Exception ex)
+            {
+                log.Error(ex);
+
+                return 0;
+            }
         }
 
         /// <summary>
@@ -42,9 +95,15 @@ namespace VIZ.Framework.Core
         /// </summary>
         public static void Dispose()
         {
-            Navigation3D?.Close();
-            Navigation3DModel = null;
-            Navigation3D = null;
+            // ------------------------------------
+            // 由于关闭系统时释放Navigation3D后会导致异常，所以该处不需要处理
+            // ------------------------------------
+            //Navigation3D?.Close();
+            //Navigation3DModel = null;
+            //Navigation3D = null;
+            // ------------------------------------
+            Stopwatch?.Stop();
+            Stopwatch = null;
         }
     }
 }

VIZ.Framework.Core/Expand/ThreeDMouse/Navigation3DModel.INavigation.cs

@@ -178,7 +178,7 @@ namespace VIZ.Framework.Core
 #if DEBUG
             Debug.WriteLine("SetCameraMatrix");
 #endif
-            matrix.M41 = MathHelper.Clip(-this.MaxX, this.MaxX, matrix.M41);
+            matrix.M41 = MathHelper.Clip(-this.MaxX, this.MaxX, (int)matrix.M41);
 
             this.Camera.Matrix = matrix;
             this.Camera.Position = new Point(matrix.M41, matrix.M42, matrix.M43);

VIZ.Framework.Core/Expand/ThreeDMouse/Navigation3DModel.cs

@@ -34,7 +34,7 @@ namespace VIZ.Framework.Core
         /// <summary>
         /// 摄像机X轴最大值
         /// </summary>
-        public double MaxX { get; set; } = 3;
+        public double MaxX { get; set; } = 555;
 
         /// <summary>
         /// 是否使用平滑
@@ -47,6 +47,11 @@ namespace VIZ.Framework.Core
         public bool IsReady { get; set; } = true;
 
         /// <summary>
+        /// 渲染时间
+        /// </summary>
+        public TimeSpan RenderTime { get; set; } = TimeSpan.Zero;
+
+        /// <summary>
         /// 平滑处理器
         /// </summary>
         private Navigation3DSmooth Smooth = new Navigation3DSmooth();
@@ -54,12 +59,13 @@ namespace VIZ.Framework.Core
         /// <summary>
         /// 更新调匀值
         /// </summary>
-        public void UpdateSmooth()
+        /// <param name="t_e">时间差</param>
+        public void UpdateSmooth(double t_e)
         {
             double x = -this.Camera.Position.X;
             if (this.IsUseSmooth)
             {
-                x = this.Smooth.Call(-this.Camera.Position.X);
+                x = this.Smooth.Call(-this.Camera.Position.X, t_e);
             }
 
             x = MathHelper.Clip(-this.MaxX, this.MaxX, x);
@@ -71,13 +77,13 @@ namespace VIZ.Framework.Core
         /// 重置X值
         /// </summary>
         /// <param name="x">X坐标</param>
-        public void Reset(double x)
+        public void Reset(int x)
         {
             this.Camera.Position = new Point(-x, 0, 0);
             this.Camera.Matrix = new Matrix(1, 0, 0, 0,
                                             0, 1, 0, 0,
                                             0, 0, 1, 0,
-                                            -x, 0, 30, 1);
+                                            -x, 0, 5550, 1);
 
             this.Smooth.Init(x);
         }

VIZ.Framework.Core/Expand/ThreeDMouse/Navigation3DSmooth.cs

 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
+using log4net;
 
 namespace VIZ.Framework.Core
 {
@@ -11,11 +13,18 @@ namespace VIZ.Framework.Core
     /// </summary>
     public class Navigation3DSmooth
     {
+        /// <summary>
+        /// 日志
+        /// </summary>
+        private static readonly ILog log = LogManager.GetLogger(typeof(Navigation3DSmooth));
+
         private double min_cutoff;
         private double beta;
         private double d_cutoff;
         private double x_prev;
         private double dx_prev;
+        //private Stopwatch stopwatch = new Stopwatch();
+        //private long last_time;
 
         /// <summary>
         /// 初始化
@@ -25,23 +34,33 @@ namespace VIZ.Framework.Core
         /// <param name="min_cutoff">终止值</param>
         /// <param name="beta"></param>
         /// <param name="d_cutoff"></param>
-        public void Init(double x0, double dx0 = 0, double min_cutoff = 0.015, double beta = 0, double d_cutoff = 1)
+        public void Init(double x0, double dx0 = 0, double min_cutoff = 0.02, double beta = 0, double d_cutoff = 1)
         {
             this.min_cutoff = min_cutoff;
             this.beta = beta;
             this.d_cutoff = d_cutoff;
             this.x_prev = x0;
             this.dx_prev = dx0;
+
+            //this.last_time = 0;
+            //this.stopwatch.Restart();
         }
 
         /// <summary>
         /// 根据实际值获取平滑值
         /// </summary>
         /// <param name="x">实际值</param>
+        /// <param name="t_e">时间差</param>
         /// <returns>平滑后的值</returns>
-        public double Call(double x)
+        public double Call(double x, double t_e)
         {
-            double t_e = 1;
+            if (t_e == 0)
+                return this.x_prev;
+
+            //long t = this.stopwatch.ElapsedMilliseconds;
+            //double t_e = (t - this.last_time) / 100d;
+            //this.last_time = t;
+
             double a_d = smoothing_factor(t_e, this.d_cutoff);
             double dx = (x - this.x_prev) / t_e;
             double dx_hat = exponential_smoothing(a_d, dx, this.x_prev);
@@ -67,4 +86,4 @@ namespace VIZ.Framework.Core
             return a * x + (1 - a) * x_prev;
         }
     }
-}
+}
\ No newline at end of file

VIZ.Framework.Core/Expand/ThreeDMouse/Navigation3DSmoothInner.cs

+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace VIZ.Framework.Core
+{
+    /// <summary>
+    /// 3D鼠标平滑 -- 1次
+    /// </summary>
+    public class Navigation3DSmoothInner
+    {
+        internal double min_cutoff;
+        internal double beta;
+        internal double d_cutoff;
+        internal double x_prev;
+        internal double dx_prev;
+
+        /// <summary>
+        /// 初始化
+        /// </summary>
+        /// <param name="x0">开始值</param>
+        /// <param name="dx0">之前的X值</param>
+        /// <param name="min_cutoff">终止值</param>
+        /// <param name="beta"></param>
+        /// <param name="d_cutoff"></param>
+        public void Init(double x0, double dx0 = 0, double min_cutoff = 1, double beta = 0, double d_cutoff = 1)
+        {
+            this.min_cutoff = min_cutoff;
+            this.beta = beta;
+            this.d_cutoff = d_cutoff;
+            this.x_prev = x0;
+            this.dx_prev = dx0;
+        }
+
+        /// <summary>
+        /// 根据实际值获取平滑值
+        /// </summary>
+        /// <param name="x">实际值</param>
+        /// <param name="t_e">时间差</param>
+        /// <returns>平滑后的值</returns>
+        public double Call(double x, double t_e)
+        {
+            double a_d = smoothing_factor(t_e, this.d_cutoff);
+            double dx = (x - this.x_prev) / t_e;
+            double dx_hat = exponential_smoothing(a_d, dx, this.x_prev);
+
+            double cutoff = this.min_cutoff + this.beta * Math.Abs(dx_hat);
+            double a = smoothing_factor(t_e, cutoff);
+            double x_hat = exponential_smoothing(a, x, this.x_prev);
+
+            this.x_prev = x_hat;
+            this.dx_prev = dx_hat;
+
+            return x_hat;
+        }
+
+        private double smoothing_factor(double t_e, double cutoff)
+        {
+            double r = 2 * Math.PI * cutoff * t_e;
+            return r / (r + 1);
+        }
+
+        private double exponential_smoothing(double a, double x, double x_prev)
+        {
+            return a * x + (1 - a) * x_prev;
+        }
+    }
+}
\ No newline at end of file

VIZ.Framework.Core/VIZ.Framework.Core.csproj

@@ -91,7 +91,9 @@
     <Compile Include="Core\Converter\ByteSizeConverter.cs" />
     <Compile Include="Core\Converter\Bool2SolidColorBrushConverter.cs" />
     <Compile Include="Core\Enum\EnumHelper.cs" />
+    <Compile Include="Core\Helper\ByteHelper.cs" />
     <Compile Include="Core\Helper\ProcessHelper.cs" />
+    <Compile Include="Core\Helper\FPSHelper.cs" />
     <Compile Include="Expand\GPI\GPIManager.cs" />
     <Compile Include="Expand\GPI\GPI_USB2GPI_DEVICE.cs" />
     <Compile Include="Expand\GPI\GPI_USB_DEVICE.cs" />
@@ -127,6 +129,7 @@
     <Compile Include="Expand\Monitor\System\Info\SystemMonitorInfo.cs" />
     <Compile Include="Expand\Monitor\System\SystemMonitorTask.cs" />
     <Compile Include="Core\Net\NetHelper.cs" />
+    <Compile Include="Expand\ThreeDMouse\Navigation3DSmoothInner.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
     <Compile Include="Expand\SharpDx\RawRectangleFExpand.cs" />
     <Compile Include="Expand\SharpDx\SharpDxColorHelper.cs" />

VIZ.Framework.Module/Setup/AppSetup.cs

@@ -63,6 +63,9 @@ namespace VIZ.Framework.Module
 
             // Step 5.  初始化循环
             appSetups.Add(new AppSetup_Loop());
+
+            // Step 6.  初始化辅助类
+            appSetups.Add(new AppSetup_Helper());
         }
 
         /// <summary>

VIZ.Framework.Module/Setup/Provider/Setup/AppSetup_Helper.cs

+using log4net;
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using System.Windows;
+using VIZ.Framework.Core;
+
+namespace VIZ.Framework.Module
+{
+    /// <summary>
+    /// 应用程序启动 -- 辅助类型
+    /// </summary>
+    public class AppSetup_Helper : AppSetupBase
+    {
+        /// <summary>
+        /// 日志
+        /// </summary>
+        private static ILog log = LogManager.GetLogger(typeof(AppSetup_Helper));
+
+        /// <summary>
+        /// 描述
+        /// </summary>
+        public override string Detail { get; } = "应用程序启动 -- 辅助类型";
+
+        /// <summary>
+        /// 执行启动
+        /// </summary>
+        /// <param name="context">应用程序启动上下文</param>
+        /// <returns>是否成功执行</returns>
+        public override bool Setup(AppSetupContext context)
+        {
+            return true;
+        }
+
+        /// <summary>
+        /// 执行关闭
+        /// </summary>
+        /// <param name="context">应用程序启动上下文</param>
+        public override void Shutdown(AppSetupContext context)
+        {
+            FPSHelper.DisposeAll();
+        }
+    }
+}

VIZ.Framework.Module/Setup/Provider/Setup/AppSetup_Navigation3D.cs

@@ -8,6 +8,7 @@ using VIZ.Framework.Core;
 using VIZ.Framework.Domain;
 using VIZ.Framework.Connection;
 using VIZ.Framework.Storage;
+using System.Windows.Media;
 
 namespace VIZ.Framework.Module
 {
@@ -47,8 +48,7 @@ namespace VIZ.Framework.Module
         /// <param name="context">应用程序启动上下文</param>
         public override void Shutdown(AppSetupContext context)
         {
-
+            Navigation3DManager.Dispose();
         }
-
     }
 }

VIZ.Framework.Module/VIZ.Framework.Module.csproj

@@ -98,6 +98,7 @@
     <Compile Include="Setup\AppSetupContext.cs" />
     <Compile Include="Setup\IAppSetup.cs" />
     <Compile Include="Setup\Message\AppShutDownMessage.cs" />
+    <Compile Include="Setup\Provider\Setup\AppSetup_Helper.cs" />
     <Compile Include="Setup\Provider\Setup\AppSetup_CatchUnhandledException.cs" />
     <Compile Include="Setup\Provider\Setup\AppSetup_Navigation3D.cs" />
     <Compile Include="Setup\Provider\Setup\AppSetup_Loop.cs" />

VIZ.Framework.UnitTest/Mouse3DTest.cs

+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using System;
+using System.Diagnostics;
+using VIZ.Framework.Core;
+
+namespace VIZ.Framework.UnitTest
+{
+    /// <summary>
+    /// 3D鼠标测试
+    /// </summary>
+    [TestClass]
+    public class Mouse3DTest
+    {
+        /// <summary>
+        /// 系统监视测试
+        /// </summary>
+        [TestMethod]
+        public void SmoothTest()
+        {
+
+        }
+    }
+}

VIZ.Framework.UnitTest/VIZ.Framework.UnitTest.csproj

@@ -73,6 +73,7 @@
     <Compile Include="MonitorTask.cs" />
     <Compile Include="NetTest.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="Mouse3DTest.cs" />
   </ItemGroup>
   <ItemGroup>
     <None Include="app.config" />

VIZ.Framework.WpfTest/MainWindow.xaml

@@ -8,6 +8,6 @@
         mc:Ignorable="d" WindowStartupLocation="CenterScreen" WindowState="Maximized"  WindowStyle="None"
         Title="MainWindow" Height="800" Width="1200">
     <Grid>
-        <local:OpenCVVideoTest></local:OpenCVVideoTest>
+        <local:Mouse3DTest></local:Mouse3DTest>
     </Grid>
 </Window>

VIZ.Framework.WpfTest/Mouse3DTest/Mouse3DTest.xaml

+<UserControl x:Class="VIZ.Framework.WpfTest.Mouse3DTest"
+             xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
+             xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
+             xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006" 
+             xmlns:d="http://schemas.microsoft.com/expression/blend/2008" 
+             xmlns:local="clr-namespace:VIZ.Framework.WpfTest"
+             mc:Ignorable="d" Background="White" 
+             d:DesignHeight="450" d:DesignWidth="800">
+    <Grid>
+        <Canvas Background="SkyBlue" Width="600" Height="300">
+            <Border x:Name="border" Canvas.Left="0" Width="100" Height="300" Background="Red"></Border>
+        </Canvas>
+    </Grid>
+</UserControl>

VIZ.Framework.WpfTest/Mouse3DTest/Mouse3DTest.xaml.cs

+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using System.Windows;
+using System.Windows.Controls;
+using System.Windows.Data;
+using System.Windows.Documents;
+using System.Windows.Input;
+using System.Windows.Media;
+using System.Windows.Media.Imaging;
+using System.Windows.Navigation;
+using System.Windows.Shapes;
+using VIZ.Framework.Core;
+
+namespace VIZ.Framework.WpfTest
+{
+    /// <summary>
+    /// Mouse3DTest.xaml 的交互逻辑
+    /// </summary>
+    public partial class Mouse3DTest : UserControl
+    {
+        public Mouse3DTest()
+        {
+            InitializeComponent();
+        }
+    }
+}

VIZ.Framework.WpfTest/VIZ.Framework.WpfTest.csproj

@@ -118,6 +118,10 @@
       <DependentUpon>MainWindow.xaml</DependentUpon>
       <SubType>Code</SubType>
     </Compile>
+    <Page Include="Mouse3DTest\Mouse3DTest.xaml">
+      <SubType>Designer</SubType>
+      <Generator>MSBuild:Compile</Generator>
+    </Page>
     <Page Include="NavigationTest\NavigationTest2.xaml">
       <Generator>MSBuild:Compile</Generator>
       <SubType>Designer</SubType>
@@ -144,6 +148,9 @@
     </Page>
   </ItemGroup>
   <ItemGroup>
+    <Compile Include="Mouse3DTest\Mouse3DTest.xaml.cs">
+      <DependentUpon>Mouse3DTest.xaml</DependentUpon>
+    </Compile>
     <Compile Include="NavigationTest\NavigationTest2.xaml.cs">
       <DependentUpon>NavigationTest2.xaml</DependentUpon>
     </Compile>