Initial commit

srcs/RobotNet10/RobotApp/Communication/CeresSharp.Test/CostFunctionTests.cs

@@ -0,0 +1,189 @@
+using CeresSharp;
+using CeresSharp.Enums;
+
+namespace CeresSharp.Test;
+
+[TestFixture]
+public class CostFunctionTests
+{
+    [Test]
+    public void AutoDiffCostFunction_SimpleLinear_ShouldWork()
+    {
+        var costFunction = new AutoDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                // Simple linear: residual = x - 1.0
+                residuals[0] = parameters[0][0] - 1.0;
+                return true;
+            },
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void AutoDiffCostFunction_MultipleParameterBlocks_ShouldWork()
+    {
+        var costFunction = new AutoDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                // residual = x[0] * y[0] - 2.0
+                residuals[0] = parameters[0][0] * parameters[1][0] - 2.0;
+                return true;
+            },
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1, 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void AutoDiffCostFunction_MultipleResiduals_ShouldWork()
+    {
+        var costFunction = new AutoDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                // 2 residuals: [x - 1, x - 2]
+                residuals[0] = parameters[0][0] - 1.0;
+                residuals[1] = parameters[0][0] - 2.0;
+                return true;
+            },
+            numResiduals: 2,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void DynamicAutoDiffCostFunction_ShouldWork()
+    {
+        var costFunction = new DynamicAutoDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                residuals[0] = parameters[0][0] - 1.0;
+                return true;
+            },
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void DynamicAutoDiffCostFunction_MultipleBlocks_ShouldWork()
+    {
+        var costFunction = new DynamicAutoDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                residuals[0] = parameters[0][0] + parameters[1][0] - 3.0;
+                return true;
+            },
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1, 2 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void NumericDiffCostFunction_Central_ShouldWork()
+    {
+        var costFunction = new NumericDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                residuals[0] = parameters[0][0] * parameters[0][0] - 4.0;
+                return true;
+            },
+            method: NumericDiffMethod.Central,
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void NumericDiffCostFunction_Forward_ShouldWork()
+    {
+        var costFunction = new NumericDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                residuals[0] = parameters[0][0] - 1.0;
+                return true;
+            },
+            method: NumericDiffMethod.Forward,
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void NumericDiffCostFunction_Ridders_ShouldWork()
+    {
+        var costFunction = new NumericDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                residuals[0] = parameters[0][0] - 1.0;
+                return true;
+            },
+            method: NumericDiffMethod.Ridders,
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void DynamicNumericDiffCostFunction_ShouldWork()
+    {
+        var costFunction = new DynamicNumericDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                residuals[0] = parameters[0][0] - 1.0;
+                return true;
+            },
+            method: NumericDiffMethod.Central,
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void CostFunction_WithComplexCalculation_ShouldWork()
+    {
+        var costFunction = new AutoDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                var x = parameters[0][0];
+                var y = parameters[0][1];
+                // residual = x^2 + y^2 - 1 (circle constraint)
+                residuals[0] = x * x + y * y - 1.0;
+                return true;
+            },
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 2 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+
+    [Test]
+    public void CostFunction_ReturnFalse_ShouldIndicateFailure()
+    {
+        var costFunction = new AutoDiffCostFunction(
+            (parameters, residuals) =>
+            {
+                // Simulate failure condition
+                if (parameters[0][0] < 0)
+                {
+                    return false;
+                }
+                residuals[0] = parameters[0][0] - 1.0;
+                return true;
+            },
+            numResiduals: 1,
+            parameterBlockSizes: new[] { 1 });
+        
+        Assert.That(costFunction, Is.Not.Null);
+    }
+}