| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Buffers.Binary; |
| | | 4 | | using System.Runtime.InteropServices; |
| | | 5 | | using ReFlex.Core.Common.Components; |
| | | 6 | | using ReFlex.Core.Common.Util; |
| | | 7 | | |
| | | 8 | | namespace ReFlex.Core.Interactivity.Components; |
| | | 9 | | |
| | | 10 | | /// <summary>A request-local regular raster. Its geometry survives settings changes during an RPC.</summary> |
| | | 11 | | public sealed class RemoteDepthRaster |
| | | 12 | | { |
| | 436 | 13 | | public int SensorWidth { get; } |
| | 36 | 14 | | public int SensorHeight { get; } |
| | 878 | 15 | | public int Width { get; } |
| | 104 | 16 | | public int Height { get; } |
| | 830 | 17 | | public int SampleStep { get; } |
| | 404 | 18 | | public IReadOnlyList<float> Depths { get; } |
| | | 19 | | private readonly float[] _depths; |
| | | 20 | | |
| | 22 | 21 | | public RemoteDepthRaster(PointCloud3 cloud, int skip, bool completeDataSet) |
| | 22 | 22 | | { |
| | 22 | 23 | | if (cloud == null) throw new ArgumentNullException(nameof(cloud)); |
| | 26 | 24 | | if (skip < 0 || skip == int.MaxValue) throw new ArgumentOutOfRangeException(nameof(skip)); |
| | 18 | 25 | | SensorWidth = cloud.SizeX; |
| | 18 | 26 | | SensorHeight = cloud.SizeY; |
| | 18 | 27 | | if (SensorWidth <= 0 || SensorHeight <= 0) throw new ArgumentException("Empty point cloud.", nameof(cloud)); |
| | 18 | 28 | | SampleStep = skip + 1; |
| | 18 | 29 | | Width = (SensorWidth - 1) / SampleStep + 1; |
| | 18 | 30 | | Height = (SensorHeight - 1) / SampleStep + 1; |
| | 18 | 31 | | var values = new float[checked(Width * Height)]; |
| | 18 | 32 | | var points = cloud.AsArray(); |
| | 160 | 33 | | for (var y = 0; y < Height; y++) |
| | 700 | 34 | | for (var x = 0; x < Width; x++) |
| | 288 | 35 | | { |
| | 288 | 36 | | var point = points[y * SampleStep * SensorWidth + x * SampleStep]; |
| | 288 | 37 | | values[y * Width + x] = point != null && float.IsFinite(point.Z) && |
| | 288 | 38 | | (completeDataSet || (point.IsValid && !point.IsFiltered)) ? point.Z : float.NaN; |
| | 288 | 39 | | } |
| | 18 | 40 | | _depths = values; |
| | 18 | 41 | | Depths = Array.AsReadOnly(values); |
| | 18 | 42 | | } |
| | | 43 | | |
| | | 44 | | /// <summary>Own row-major IEEE754 float32 payload; NaN keeps its missing-sample meaning.</summary> |
| | | 45 | | public byte[] ToLittleEndianBytes() |
| | 4 | 46 | | { |
| | 4 | 47 | | if (BitConverter.IsLittleEndian) |
| | 4 | 48 | | return MemoryMarshal.AsBytes(_depths.AsSpan()).ToArray(); |
| | 0 | 49 | | var bytes = new byte[checked(_depths.Length * sizeof(float))]; |
| | 0 | 50 | | for (var index = 0; index < _depths.Length; index++) |
| | 0 | 51 | | BinaryPrimitives.WriteSingleLittleEndian(bytes.AsSpan(index * sizeof(float), sizeof(float)), _depths[index]) |
| | 0 | 52 | | return bytes; |
| | 4 | 53 | | } |
| | | 54 | | |
| | | 55 | | public bool TryGetSensorIndex(int index, out int sensorIndex) |
| | 114 | 56 | | { |
| | 114 | 57 | | sensorIndex = -1; |
| | 120 | 58 | | if (index < 0 || index >= Depths.Count) return false; |
| | 108 | 59 | | sensorIndex = (index / Width * SampleStep) * SensorWidth + index % Width * SampleStep; |
| | 108 | 60 | | return true; |
| | 114 | 61 | | } |
| | | 62 | | |
| | | 63 | | public Interaction CreateCandidate(int index, float depth, int touchId, long time) |
| | 12 | 64 | | { |
| | 12 | 65 | | if (!TryGetSensorIndex(index, out var sensorIndex) || !float.IsFinite(depth) || !float.IsFinite(Depths[index])) |
| | 10 | 66 | | throw new ArgumentException("Remote candidate has an invalid index, missing sample or non-finite depth."); |
| | 2 | 67 | | return new Interaction |
| | 2 | 68 | | { |
| | 2 | 69 | | TouchId = touchId, Time = time, Confidence = 1, Type = InteractionType.None, |
| | 2 | 70 | | // Observers require lateral array coordinates, not sensor world coordinates. |
| | 2 | 71 | | Position = new Point3(sensorIndex % SensorWidth, sensorIndex / SensorWidth, depth) |
| | 2 | 72 | | }; |
| | 2 | 73 | | } |
| | | 74 | | } |