From aa2649b20472135cb96f8d97fa4c2cb465f8364a Mon Sep 17 00:00:00 2001
From: sunwen <sunwen@realcan.com>
Date: Mon, 12 Jun 2023 11:40:24 +0800
Subject: [PATCH] Add meshgridInterp3.

---
 src/Function3D.cpp | 83 ++++++++++++++++++++++++++++++++++++++++++++++
 src/Function3D.h   |  1 +
 2 files changed, 84 insertions(+)

diff --git a/src/Function3D.cpp b/src/Function3D.cpp
index 62181ce..f2b4ae3 100644
--- a/src/Function3D.cpp
+++ b/src/Function3D.cpp
@@ -123,3 +123,86 @@ int Aurora::size(const Matrix &aMatrix,int dims)
 {
     return aMatrix.getDimSize(dims-1);
 }
+
+Matrix Aurora::meshgridInterp3(const Matrix& aX, const Matrix& aY, const Matrix& aZ, const Matrix& aV, const Matrix& aX1, const Matrix& aY1, const Matrix& aZ1,InterpnMethod aMethod, double aExtrapval)
+{
+    std::vector<Matrix> zTemps;
+    for(int rIndex=0; rIndex<aV.getDimSize(2); ++rIndex)
+    {
+        std::vector<Matrix> yTemps;
+        for(int xIndex=0; xIndex<aV.getDimSize(0); ++xIndex)
+        {
+            Matrix test = aV($, xIndex, rIndex).toMatrix();
+            Matrix a = interp1(aY, test, aY1, aMethod);
+            yTemps.push_back(a);
+        }
+
+        std::vector<Matrix> xTemps;
+        for(int yIndex=0; yIndex<yTemps[0].getDataSize(); ++yIndex)
+        {
+            double* xTempData = new double[yTemps.size()];
+            for(int i=0;i<yTemps.size();++i)
+            {
+                xTempData[i] =  yTemps[i][yIndex];
+            }
+            Matrix xTemp = Matrix::fromRawData(xTempData, yTemps.size());
+            Matrix b = interp1(aX, xTemp, aX1, aMethod);
+            xTemps.push_back(b);
+        }
+
+        Matrix result2D = xTemps[0];
+        for(int i=1; i<xTemps.size(); ++i)
+        {
+            result2D = horzcat(result2D, xTemps[i]);
+        }
+        result2D = transpose(result2D);
+        zTemps.push_back(result2D);
+    }
+
+    std::vector<Matrix>  resultVector;
+    for(int i=0; i<zTemps[0].getDataSize(); ++i)
+    {
+        double* resultTemp = new double[zTemps.size()]; 
+        for(int j=0; j<zTemps.size(); ++j)
+        {
+            resultTemp[j] = zTemps[j][i];
+        }
+        Matrix zTemp = Matrix::fromRawData(resultTemp, zTemps.size());
+        Matrix c = interp1(aZ, zTemp, aZ1, aMethod);
+        resultVector.push_back(c);
+    }
+
+    Matrix result = resultVector[0];
+    for(int i=1; i<resultVector.size(); ++i)
+    {
+        result = horzcat(result, resultVector[i]);
+    }
+    result = transpose(result);
+    result.forceReshape(aX1.getDataSize(), aY1.getDataSize(), aZ1.getDataSize());
+
+    double minXSOS = aX[0];
+    double maxXSOS = aX[aX.getDataSize() - 1];
+    double minYSOS = aY[0];
+    double maxYSOS = aY[aY.getDataSize() - 1];
+    double minZSOS = aZ[0];
+    double maxZSOS = aZ[aZ.getDataSize() - 1];
+    for(size_t zIndex=0; zIndex<aZ1.getDataSize(); ++zIndex)
+    {
+        for(size_t yIndex=0; yIndex<aY1.getDataSize(); ++yIndex)
+        {
+            for(size_t xIndex=0; xIndex<aX1.getDataSize(); ++xIndex)
+            {
+                size_t index = zIndex * aY1.getDataSize() * aX1.getDataSize() +
+                               yIndex * aX1.getDataSize() + xIndex;
+                if(aX1[xIndex] < minXSOS || aX1[xIndex] > maxXSOS ||
+                   aY1[yIndex] < minYSOS || aY1[yIndex] > maxYSOS ||
+                   aZ1[zIndex] < minZSOS || aZ1[zIndex] > maxZSOS)
+                   {
+                        result[index] = aExtrapval;
+                   }
+            }
+        }
+    }
+
+    return result;
+}
diff --git a/src/Function3D.h b/src/Function3D.h
index 17bc282..1a8a3f6 100644
--- a/src/Function3D.h
+++ b/src/Function3D.h
@@ -39,6 +39,7 @@ namespace Aurora {
      */
     Matrix zeros(int aSquareRow);
     Matrix interp3(const Matrix& aX, const Matrix& aY, const Matrix& aZ, const Matrix& aV, const Matrix& aX1, const Matrix& aY1, const Matrix& aZ1,InterpnMethod aMethod);
+    Matrix meshgridInterp3(const Matrix& aX, const Matrix& aY, const Matrix& aZ, const Matrix& aV, const Matrix& aX1, const Matrix& aY1, const Matrix& aZ1,InterpnMethod aMethod, double aExtrapval);
     Matrix interpn(const Matrix& aX, const Matrix& aY, const Matrix& aZ, const Matrix& aV, const Matrix& aX1, const Matrix& aY1, const Matrix& aZ1,InterpnMethod aMethod);
 
     Matrix size(const Matrix &aMatrix);