Use FOV instead of angle difference check

Author: Timocop

PSMSVirtualDeviceManager/Classes/Tools/ClassMathUtils.vb

@@ -153,26 +153,36 @@ Public Class ClassMathUtils
     End Function
 
     Public Shared Function CalculateAngleDegreesDifference(q1 As Quaternion, q2 As Quaternion) As Single
-        ' Convert both quaternions to unit quaternions
         q1 = Quaternion.Normalize(q1)
         q2 = Quaternion.Normalize(q2)
 
-        ' Compute the dot product between the two quaternions
         Dim dotProduct = q1.W * q2.W + q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z
-
-        ' Ensure the dot product is within [-1, 1] to avoid floating point errors
         dotProduct = CSng(Math.Min(Math.Max(dotProduct, -1.0), 1.0))
 
-        ' Calculate the angle in radians (twice the angle because quaternions double-cover the rotation space)
         Dim angleRad = 2.0 * Math.Acos(Math.Abs(dotProduct))
-
-        ' Convert to degrees
         Dim angleDeg = angleRad * (180.0 / Math.PI)
 
-        ' Return the smallest angle (0-180 degrees)
         Return CSng(Math.Min(angleDeg, 360.0 - angleDeg))
     End Function
 
+    Public Shared Function CalculateAngleDegreesDifferenceFov(p1 As Vector3, q1 As Quaternion, p2 As Vector3, q2 As Quaternion) As Single
+        q1 = Quaternion.Normalize(q1)
+        q2 = Quaternion.Normalize(q2)
+
+        Dim directionToTarget As Vector3 = p2 - p1
+        If (directionToTarget.Length < Single.Epsilon) Then
+            Return 0.0F
+        End If
+
+        Dim forward = RotateVector(q1, Vector3.UnitZ)
+
+        directionToTarget = Vector3.Normalize(directionToTarget)
+
+        Dim angle As Single = CSng(Math.Acos(Vector3.Dot(forward, directionToTarget)) * (180.0F / Math.PI))
+
+        Return angle
+    End Function
+
     Public Shared Function QuaternionFromAngularVelocity(angularVelocity As Vector3, dt As Double) As Quaternion
         If angularVelocity.Length() < Single.Epsilon Then
             Return Quaternion.Identity

PSMSVirtualDeviceManager/StartPage/TroubleshootIssueTracker/ClassLogManageServiceDevices.vb

@@ -911,6 +911,11 @@ Public Class ClassLogManageServiceDevices
                 Single.Parse(mDeviceConfig.GetValue("pose\orientation", "z", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture),
                 Single.Parse(mDeviceConfig.GetValue("pose\orientation", "w", "1.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture)
             )
+            Dim mTrackerPosition As New Vector3(
+                Single.Parse(mDeviceConfig.GetValue("pose\position", "x", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture),
+                Single.Parse(mDeviceConfig.GetValue("pose\position", "y", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture),
+                Single.Parse(mDeviceConfig.GetValue("pose\position", "z", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture)
+            )
 
             For Each mDeviceOther In GetDevices()
                 If (mDeviceOther.iId < 0) Then
@@ -944,11 +949,18 @@ Public Class ClassLogManageServiceDevices
                     Single.Parse(mDeviceOtherConfig.GetValue("pose\orientation", "z", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture),
                     Single.Parse(mDeviceOtherConfig.GetValue("pose\orientation", "w", "1.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture)
                 )
+                Dim mOtherTrackerPosition As New Vector3(
+                    Single.Parse(mDeviceOtherConfig.GetValue("pose\position", "x", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture),
+                    Single.Parse(mDeviceOtherConfig.GetValue("pose\position", "y", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture),
+                    Single.Parse(mDeviceOtherConfig.GetValue("pose\position", "z", "0.0"), Globalization.NumberStyles.Float, Globalization.CultureInfo.InvariantCulture)
+                )
+
+                Dim fAngleDiff1 As Single = ClassMathUtils.CalculateAngleDegreesDifferenceFov(mTrackerPosition, mTrackerOrientation, mOtherTrackerPosition, mOtherTrackerOrientation)
+                Dim fAngleDiff2 As Single = ClassMathUtils.CalculateAngleDegreesDifferenceFov(mOtherTrackerPosition, mOtherTrackerOrientation, mTrackerPosition, mTrackerOrientation)
 
-                Dim fAngleDiff As Single = ClassMathUtils.CalculateAngleDegreesDifference(mTrackerOrientation, mOtherTrackerOrientation)
-                If (fAngleDiff > 180.0F - iAngleLimit) Then
+                If (Math.Abs(fAngleDiff1) < iAngleLimit AndAlso Math.Abs(fAngleDiff2) < iAngleLimit) Then
                     Dim mIssue As New STRUC_LOG_ISSUE(mTemplate)
-                    mIssue.sDescription = String.Format(mIssue.sDescription, mDevice.iId, mDeviceOther.iId, CInt(Math.Abs(fAngleDiff - 180.0F)))
+                    mIssue.sDescription = String.Format(mIssue.sDescription, mDevice.iId, mDeviceOther.iId, CInt((Math.Abs(fAngleDiff1) + Math.Abs(fAngleDiff2)) / 2))
                     mIssues.Add(mIssue)
                 End If
             Next