diff --git a/PointCloudWeb.Server/PointCloudWeb.Server.Tests/Services/ScanConverterServiceTest.cs b/PointCloudWeb.Server/PointCloudWeb.Server.Tests/Services/ScanConverterServiceTest.cs index 24b975e..96493f8 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server.Tests/Services/ScanConverterServiceTest.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server.Tests/Services/ScanConverterServiceTest.cs @@ -10,34 +10,44 @@ namespace PointCloudWeb.Server.Tests.Services [Theory] [InlineData(1, 1, 1)] - //[InlineData(-1, 1, 1)] - //[InlineData(1, -1, 1)] + [InlineData(2, 2, 2)] + [InlineData(2, 1, 1)] + [InlineData(1, 2, 1)] + [InlineData(1, 1, 2)] + [InlineData(10, 15, 32)] + [InlineData(5, 12, 7)] + + [InlineData(-1, 1, 1)] + [InlineData(1, -1, 1)] + [InlineData(-1, -1, 1)] + [InlineData(-5, 12, 7)] + [InlineData(-5, -12, 7)] + [InlineData(-22, 13, 11)] + //[InlineData(1, 1, -1)] - //[InlineData(-1, -1, 1)] //[InlineData(-1, 1, -1)] //[InlineData(1, -1, -1)] - - - //[InlineData(10, 15, 32)] - //[InlineData(5, 12, 7)] - //[InlineData(-5, 12, 7)] - //[InlineData(5, -12, 7)] //[InlineData(5, 12, -7)] - //[InlineData(-5, -12, 7)] //[InlineData(-5, 12, -7)] //[InlineData(-5, -12, -7)] - //[InlineData(-22, 13, 11)] public static void ScanConverterTest(int x, int y, int z) { var scan = new ScanDataPoint { - RAX = 45,//(Math.Acos(y / Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2))) * 180 / Math.PI), - RAY = 45,//(Math.Acos(z / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))) * 180 / Math.PI), - //RAY = 360 - (Math.Acos(z / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))) * 180 / Math.PI), - //RAY = 180 - (Math.Acos(z / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))) * 180 / Math.PI), + RAX = Math.Acos(y / Math.Sqrt(Math.Pow(y, 2) + Math.Pow(z, 2))), + RAY = Math.Acos(x / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))), DistanceMM = (float)Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2)) }; + //from rad to degree + scan.RAX = scan.RAX * 180 / Math.PI; + scan.RAY = scan.RAY * 180 / Math.PI; + var expected = new Point(x, y, z); + + var service = new ScanConverterService(); + var point = service.Transform(scan); + Assert.Equal(expected, point); + //if (scan.RAX >= 0 && scan.RAX < 90) // scan.RAY = 360 - scan.RAY; //else if (scan.RAX >= 90 && scan.RAX < 180) @@ -55,18 +65,6 @@ namespace PointCloudWeb.Server.Tests.Services // sin *= -1; //} - var expected = new Point(x, y, z); - - var service = new ScanConverterService(); - var point = service.Transform(scan); - //Assert.Equal(expected, point); - Assert.True( - point.X >= expected.X - 1 && point.X <= expected.X + 1 - && point.Y >= expected.Y - 1 && point.Y <= expected.Y + 1 - && point.Z >= expected.Z - 1 && point.Z <= expected.Z + 1, - "expected: " + expected.ToString() + " \nactual: " + point.ToString() + - "\nAX: " + scan.RAX + "\n AY: " + scan.RAY - ); } } } diff --git a/PointCloudWeb.Server/PointCloudWeb.Server/Services/ScanConverterService.cs b/PointCloudWeb.Server/PointCloudWeb.Server/Services/ScanConverterService.cs index 57358e7..e165669 100644 --- a/PointCloudWeb.Server/PointCloudWeb.Server/Services/ScanConverterService.cs +++ b/PointCloudWeb.Server/PointCloudWeb.Server/Services/ScanConverterService.cs @@ -6,7 +6,6 @@ namespace PointCloudWeb.Server.Services { public class ScanConverterService { - private enum RotationAxis { X, Y, Z }; private static void CorrectQuadrants(double angle, ref float sin, ref float cos) { @@ -27,79 +26,42 @@ namespace PointCloudWeb.Server.Services } } - private static Matrix4x4 GetTransformationMatrix(ScanDataPoint scan, RotationAxis type) - { - double angle = type switch - { - RotationAxis.X => scan.RAX,//360 - 90 + scan.RAX, - RotationAxis.Y => -scan.RAY,//360 - scan.RAY, - RotationAxis.Z => 0,//360 - scan.RAY, - _ => throw new NotImplementedException(), - }; - - if (angle == 0) - return new Matrix4x4( - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 - ); - - angle %= 360; - if (angle < 0) - angle += 360; - - var angleInR = angle * (Math.PI / 180.0); - - var sin = (float)Math.Sin(angleInR); - var cos = (float)Math.Cos(angleInR); - - //CorrectQuadrants(angle, ref sin, ref cos); - - return type switch - { - RotationAxis.X => new Matrix4x4( - 1, 0, 0, 0, - 0, cos, -sin, 0, - 0, sin, cos, 0, - 0, 0, 0, 1 - ), - RotationAxis.Y => new Matrix4x4( - cos, 0, sin, 0, - 0, 1, 0, 0, - -sin, 0, cos, 0, - 0, 0, 0, 1 - ), - RotationAxis.Z => new Matrix4x4( - cos, -sin, 0, 0, - sin, cos, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 - ), - _ => throw new NotImplementedException(), - }; - } - - + private int Round(double value) => (int)Math.Round(value, 0, MidpointRounding.AwayFromZero); public Point Transform(ScanDataPoint scan) { - Vector3 v = new Vector3(0, 0, 0); + var degreeXA = scan.RAX; + var degreeXB = 180 - 90 - degreeXA; + var degreeYA = scan.RAY; + var degreeYB = 180 - 90 - degreeYA; - v.Z = scan.DistanceMM; + var radXA = degreeXA * Math.PI / 180; + var radXB = degreeXB * Math.PI / 180; + var radYA = degreeYA * Math.PI / 180; + var radYB = degreeYB * Math.PI / 180; - var matrixX = GetTransformationMatrix(scan, RotationAxis.X); - var matrixY = GetTransformationMatrix(scan, RotationAxis.Y); - var matrixZ = GetTransformationMatrix(scan, RotationAxis.Z); + double sinXA = Math.Sin(radXA); + double sinXB = Math.Sin(radXB); + double sinYA = Math.Sin(radYA); + double sinYB = Math.Sin(radYB); - v = Vector3.Transform(v, matrixX); - v = Vector3.Transform(v, matrixY); - v = Vector3.Transform(v, matrixZ); - - return new Point( - (int)Math.Round(v.X, 0, MidpointRounding.AwayFromZero), - (int)Math.Round(v.Y, 0, MidpointRounding.AwayFromZero), - (int)Math.Round(v.Z, 0, MidpointRounding.AwayFromZero) + var z = Math.Sqrt( + Math.Pow( + Math.Pow(sinXB, 2) / Math.Pow(sinXA, 2) + + Math.Pow(sinYB, 2) / Math.Pow(sinYA, 2) + + 1 + , -1) + * Math.Pow(scan.DistanceMM, 2) ); + + + var p = new Point() + { + X = Round(z * sinYB / sinYA), + Y = Round(z * sinXB / sinXA), + Z = Round(z) + }; + + return p; } } } \ No newline at end of file