Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/TSDValueConverter.cs

@@ -0,0 +1,140 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Provides conversions between double and uint16 representations for
+/// truncated signed distance values and weights.
+/// </summary>
+public class TSDValueConverter
+{
+    private const double kMinWeight = 0.0;
+    private const ushort kUnknownTSDValue = 0;
+    private const ushort kUnknownWeightValue = 0;
+    private const ushort kUpdateMarker = (ushort)(1u << 15); // Highest bit
+
+    private readonly double _maxTSD;
+    private readonly double _minTSD;
+    private readonly double _maxWeight;
+    private readonly double _tsdResolution;
+    private readonly double _weightResolution;
+    private readonly double[] _valueToTSD;
+    private readonly double[] _valueToWeight;
+
+    public TSDValueConverter(double maxTSD, double maxWeight, ValueConversionTables conversionTables)
+    {
+        _maxTSD = maxTSD;
+        _minTSD = -maxTSD;
+        _maxWeight = maxWeight;
+        _tsdResolution = 32766.0 / (maxTSD - _minTSD);
+        _weightResolution = 32766.0 / (maxWeight - kMinWeight);
+
+        // Get conversion tables from ValueConversionTables
+        _valueToTSD = conversionTables.GetConversionTable(_minTSD, _minTSD, _maxTSD);
+        _valueToWeight = conversionTables.GetConversionTable(kMinWeight, kMinWeight, maxWeight);
+    }
+
+    /// <summary>
+    /// Converts a TSD to a ushort in the [1, 32767] range.
+    /// </summary>
+    public ushort TSDToValue(double tsd)
+    {
+        var clamped = ClampTSD(tsd);
+        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
+        // (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
+        var value = (int)Math.Round((clamped - _minTSD) * _tsdResolution, MidpointRounding.AwayFromZero) + 1;
+        return (ushort)Math.Clamp(value, 1, 32767);
+    }
+
+    /// <summary>
+    /// Converts a weight to a ushort in the [1, 32767] range.
+    /// </summary>
+    public ushort WeightToValue(double weight)
+    {
+        var clamped = ClampWeight(weight);
+        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
+        // (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
+        var value = (int)Math.Round((clamped - kMinWeight) * _weightResolution, MidpointRounding.AwayFromZero) + 1;
+        return (ushort)Math.Clamp(value, 1, 32767);
+    }
+
+    /// <summary>
+    /// Converts a ushort (which may or may not have the update marker set) to a
+    /// value in the range [min_tsd_, max_tsd_].
+    /// Match C++: return (*value_to_tsd_)[value];
+    /// Note: C++ conversion table has size 65536 (all possible ushort values),
+    /// and handles update marker by masking it out in PrecomputeValueToBoundedFloat.
+    /// </summary>
+    public double ValueToTSD(ushort value)
+    {
+        // Match C++: value_to_tsd_ has size 65536 (all possible ushort values)
+        // C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
+        // So we can safely access _valueToTSD[value] even if value has update marker set
+        if (value >= _valueToTSD.Length)
+        {
+            // Defensive check: should never happen if conversion table is correctly sized
+            return _minTSD;
+        }
+        return _valueToTSD[value];
+    }
+
+    /// <summary>
+    /// Converts a ushort (which may or may not have the update marker set) to a
+    /// value in the range [min_weight_, max_weight_].
+    /// Match C++: return (*value_to_weight_)[value];
+    /// Note: C++ conversion table has size 65536 (all possible ushort values),
+    /// and handles update marker by masking it out in PrecomputeValueToBoundedFloat.
+    /// </summary>
+    public double ValueToWeight(ushort value)
+    {
+        // Match C++: value_to_weight_ has size 65536 (all possible ushort values)
+        // C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
+        // So we can safely access _valueToWeight[value] even if value has update marker set
+        if (value >= _valueToWeight.Length)
+        {
+            // Defensive check: should never happen if conversion table is correctly sized
+            return kMinWeight;
+        }
+        return _valueToWeight[value];
+    }
+
+    public static ushort GetUnknownTSDValue() => kUnknownTSDValue;
+    public static ushort GetUnknownWeightValue() => kUnknownWeightValue;
+    public static ushort GetUpdateMarker() => kUpdateMarker;
+
+    public double GetMaxTSD() => _maxTSD;
+    public double GetMinTSD() => _minTSD;
+    public double GetMaxWeight() => _maxWeight;
+    public double GetMinWeight() => kMinWeight;
+
+    /// <summary>
+    /// Clamps TSD to be in the range [min_tsd_, max_tsd_].
+    /// </summary>
+    private double ClampTSD(double tsd)
+    {
+        return Math.Clamp(tsd, _minTSD, _maxTSD);
+    }
+
+    /// <summary>
+    /// Clamps weight to be in the range [min_weight_, max_weight_].
+    /// </summary>
+    private double ClampWeight(double weight)
+    {
+        return Math.Clamp(weight, kMinWeight, _maxWeight);
+    }
+}
+