From cabd4bd49792d0a6b00871ca17ed835b86309a47 Mon Sep 17 00:00:00 2001
From: kradchen <chenjuijun@realcan.cn>
Date: Thu, 18 May 2023 10:58:43 +0800
Subject: [PATCH] Add Bresengam and  DGradient(and test).

---
 .../reconstruction/buildMatrix/Bresenham.cpp  | 208 ++++++
 .../reconstruction/buildMatrix/Bresenham.h    |   8 +
 .../reconstruction/buildMatrix/DGradient.cpp  | 679 ++++++++++++++++++
 .../reconstruction/buildMatrix/DGradient.h    |   7 +
 test/Reconstruction_Test.cpp                  |  14 +
 5 files changed, 916 insertions(+)
 create mode 100644 src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.cpp
 create mode 100644 src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.h
 create mode 100644 src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.cpp
 create mode 100644 src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.h

diff --git a/src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.cpp b/src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.cpp
new file mode 100644
index 0000000..a36375c
--- /dev/null
+++ b/src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.cpp
@@ -0,0 +1,208 @@
+#include "Bresenham.h"
+#include <math.h> 
+#include <stdlib.h>
+#include <vector>
+
+float* b3dMex(float *startPoint, float *endPoint,size_t &outputSize ) {
+
+	/* Input arguments from MATLAB */
+	// float *startPoint;     // data
+    // float *endPoint;      // reference data
+    	  
+    /* create a pointer to the real data in the input array  */
+	// startPoint = (float *)mxGetPr(prhs[0]);
+    // endPoint = (float *)mxGetPr(prhs[1]);  
+          
+    std::vector<float> points; 
+    
+    float i, l, m, n, x_inc, y_inc, z_inc, err_1, err_2, dx2, dy2, dz2, x,y,z;
+           
+    x = startPoint[0];
+    y = startPoint[1];
+    z = startPoint[2];
+    
+    float dx = endPoint[0] - x;
+    float dy = endPoint[1] - y;
+    float dz = endPoint[2] - z;
+    
+    x_inc = (dx < 0) ? -1 : 1;
+    l = abs(dx);
+    y_inc = (dy < 0) ? -1 : 1;
+    m = abs(dy);
+    z_inc = (dz < 0) ? -1 : 1;
+    n = abs(dz);
+    dx2 = l*2;// << 1;
+    dy2 = m*2;// << 1;
+    dz2 = n*2;// << 1;
+    
+    if ((l >= m) && (l >= n)) {
+        err_1 = dy2 - l;
+        err_2 = dz2 - l;
+        for (i = 0; i < l; i++) {
+
+               points.push_back(x);
+               points.push_back(y);
+               points.push_back(z);
+               
+            if (err_1 > 0) {
+                y += y_inc;
+                err_1 -= dx2;
+            }
+            if (err_2 > 0) {
+                z += z_inc;
+                err_2 -= dx2;
+            }
+            err_1 += dy2;
+            err_2 += dz2;
+            x += x_inc;
+
+        }
+    } else if ((m >= l) && (m >= n)) {
+        err_1 = dx2 - m;
+        err_2 = dz2 - m;
+        for (i = 0; i < m; i++) {
+
+            points.push_back(x);
+            points.push_back(y);
+            points.push_back(z);
+            
+            if (err_1 > 0) {
+                x += x_inc;
+                err_1 -= dy2;
+            }
+            if (err_2 > 0) {
+                z += z_inc;
+                err_2 -= dy2;
+            }
+            err_1 += dx2;
+            err_2 += dz2;
+            y += y_inc;
+
+        }
+    } else {
+        err_1 = dy2 - n;
+        err_2 = dx2 - n;
+        for (i = 0; i < n; i++) {
+
+            points.push_back(x);
+            points.push_back(y);
+            points.push_back(z);
+
+            if (err_1 > 0) {
+                y += y_inc;
+                err_1 -= dz2;
+            }
+            if (err_2 > 0) {
+                x += x_inc;
+                err_2 -= dz2;
+            }
+            err_1 += dy2;
+            err_2 += dx2;
+            z += z_inc;
+
+        }
+    }
+
+	float* output= new float[points.size()]{0};
+    std::copy(points.begin(), points.end(), output);
+    outputSize = points.size();
+    return output;
+}
+
+double * b3dMexDouble(double *startPoint,double *endPoint, size_t& outputSize) {
+
+    std::vector<double> points; 
+    
+    double i, l, m, n, x_inc, y_inc, z_inc, err_1, err_2, dx2, dy2, dz2, x,y,z;
+           
+    x = startPoint[0];
+    y = startPoint[1];
+    z = startPoint[2];
+    
+    double dx = endPoint[0] - x;
+    double dy = endPoint[1] - y;
+    double dz = endPoint[2] - z;
+    
+    x_inc = (dx < 0) ? -1 : 1;
+    l = abs(dx);
+    y_inc = (dy < 0) ? -1 : 1;
+    m = abs(dy);
+    z_inc = (dz < 0) ? -1 : 1;
+    n = abs(dz);
+    dx2 = l*2;// << 1;
+    dy2 = m*2;// << 1;
+    dz2 = n*2;// << 1;
+    
+    if ((l >= m) && (l >= n)) {
+        err_1 = dy2 - l;
+        err_2 = dz2 - l;
+        for (i = 0; i < l; i++) {
+
+               points.push_back(x);
+               points.push_back(y);
+               points.push_back(z);
+               
+            if (err_1 > 0) {
+                y += y_inc;
+                err_1 -= dx2;
+            }
+            if (err_2 > 0) {
+                z += z_inc;
+                err_2 -= dx2;
+            }
+            err_1 += dy2;
+            err_2 += dz2;
+            x += x_inc;
+
+        }
+    } else if ((m >= l) && (m >= n)) {
+        err_1 = dx2 - m;
+        err_2 = dz2 - m;
+        for (i = 0; i < m; i++) {
+
+            points.push_back(x);
+            points.push_back(y);
+            points.push_back(z);
+            
+            if (err_1 > 0) {
+                x += x_inc;
+                err_1 -= dy2;
+            }
+            if (err_2 > 0) {
+                z += z_inc;
+                err_2 -= dy2;
+            }
+            err_1 += dx2;
+            err_2 += dz2;
+            y += y_inc;
+
+        }
+    } else {
+        err_1 = dy2 - n;
+        err_2 = dx2 - n;
+        for (i = 0; i < n; i++) {
+
+            points.push_back(x);
+            points.push_back(y);
+            points.push_back(z);
+
+            if (err_1 > 0) {
+                y += y_inc;
+                err_1 -= dz2;
+            }
+            if (err_2 > 0) {
+                x += x_inc;
+                err_2 -= dz2;
+            }
+            err_1 += dy2;
+            err_2 += dx2;
+            z += z_inc;
+
+        }
+    }
+
+	double* output = new double[points.size()];
+    std::copy(points.begin(), points.end(), output);
+    outputSize = points.size();
+    return output;
+}
diff --git a/src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.h b/src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.h
new file mode 100644
index 0000000..bd5fd2d
--- /dev/null
+++ b/src/transmissionReconstruction/reconstruction/buildMatrix/Bresenham.h
@@ -0,0 +1,8 @@
+#ifndef __B3DMEX_H__
+#define __B3DMEX_H__
+#include <cstddef>
+namespace Recon {
+    float* b3dMex(float *startPoint, float *endPoint,std::size_t &outputSize );
+    double * b3dMexDouble(double *startPoint,double *endPoint, size_t& outputSize);
+}
+#endif // __B3DMEX_H__
\ No newline at end of file
diff --git a/src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.cpp b/src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.cpp
new file mode 100644
index 0000000..f8d2b03
--- /dev/null
+++ b/src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.cpp
@@ -0,0 +1,679 @@
+#include "DGradient.h"
+
+#include <iostream>
+#include <stdlib.h>
+#include <float.h>
+#include <string.h>
+#include <math.h>
+
+#include "Matrix.h"
+#include "Function.h"
+#include "Function3D.h"
+
+
+
+#ifndef MWSIZE_MAX
+#define mwSize  int              // Defined in tmwtypes.h
+#define mwIndex int
+#define MWSIZE_MAX INT_FAST32_MAX
+#endif
+
+#define ERR_HEAD "*** DGradient[mex]: "
+#define ERR_ID   "JSimon:DGradient:"
+
+// Prototypes: -----------------------------------------------------------------
+void CoreDim1Space1(double *X, const mwSize M, const mwSize nDX, double Space,
+              double *Y);
+void CoreDimNSpace1(double *X, const mwSize step, const mwSize nX,
+              const mwSize nDX, double Space, double *Y);
+
+void WrapSpaceN(double *X, const mwSize Step, const mwSize nX,
+              const mwSize nDX, double *Factor, double *Y);
+void GetFactor(double *Space, mwSize nDX, double *Factor);
+void CoreDim1SpaceN(double *X, const mwSize M, const mwSize nDX,
+              double *Space, double *Y);
+// void CoreDimNSpaceN(double *X, const mwSize step, const mwSize nX,
+//            const mwSize nDX, double *Space, double *Y);
+void CoreDim1FactorN(double *X, const mwSize M, const mwSize nDX,
+              double *Factor, double *Y);
+void CoreDimNFactorN(double *X, const mwSize Step, const mwSize nX,
+              const mwSize nDX, double *Factor, double *Y);
+
+void WrapSpaceNOrder2(double *X, const mwSize Step, const mwSize nX,
+              const mwSize nDX, double *Space, double *Y);
+void GetFactorOrder2(double *Space, const mwSize nDX,
+              double *A, double *B, double *C);
+void CoreDim1SpaceNOrder2(double *X, const mwSize nX, const mwSize nDX,
+              double *Space, double *Y);
+void CoreDimNSpaceNOrder2(double *X, const mwSize Step, const mwSize nX,
+              const mwSize nDX, double *Space, double *Y);
+void CoreDim1FactorNOrder2(double *X, const mwSize nX, const mwSize nDX,
+              double *A, double *B, double *C, double *Y);
+void CoreDimNFactorNOrder2(double *X, const mwSize Step, const mwSize nX,
+              const mwSize nDX, double *A, double *B, double *C, double *Y);
+
+mwSize FirstNonSingeltonDim(const mwSize Xndim, const mwSize *Xdim);
+mwSize GetStep(const mwSize *Xdim, const mwSize N);
+
+// Main function ===============================================================
+Aurora::Matrix Recon::DGradient(const Aurora::Matrix aX,double aSpacing,double aDim)
+{
+  double *X, *Y, Nd, *Space, UnitSpace = 1.0;
+  mwSize nX, nDX, ndimX, N, Step, nSpace = 1;
+  const mwSize *dimX;
+  int    Order2 = 0;
+  
+  // Pointers and dimension to input array: ------------------------------------
+  X     = aX.getData();
+  nX    = aX.getDataSize();
+  ndimX = aX.getDims();
+  dimX  = new int[3]{aX.getDimSize(0), aX.getDimSize(1), aX.getDimSize(2)};
+  
+  // Return fast on empty input matrix:
+  if (nX == 0) {
+     return aX;
+  }
+  
+  // Get spacing, if defined: --------------------------------------------------
+
+    Space  = &aSpacing;
+    nSpace = 1;
+    if (nSpace == 0) {
+        nSpace = 1;
+        Space  = &UnitSpace;
+    }
+
+  
+  // Determine dimension to operate on: ----------------------------------------
+   // 3rd input used:
+
+           Nd = aDim;
+           N  = (mwSize) Nd - 1;
+           if (Nd < 1.0 || Nd != floor(Nd)) {
+              std::cerr<<ERR_ID <<"BadValueInput3"<<ERR_HEAD<<"Dimension must be a positive integer scalar."<<std::endl;
+           }
+           
+           if (N < ndimX) {
+              Step = GetStep(dimX, N);
+              nDX  = dimX[N];
+           } else {
+              // Treat imaginated trailing dimensions as singelton, as usual in
+              // Matlab:
+              Step = nX;
+              nDX  = 1;
+           }
+
+  
+  // Check matching sizes of X and Spacing:
+  if (nSpace != 1 && nSpace != nDX) {
+     std::cerr<<ERR_ID <<"BadSizeInput2"<<ERR_HEAD<<"2nd input [Spacing] does not match the dimensions.";
+  }
+  
+  
+
+
+
+  // Reply ZEROS, if the length of the processed dimension is 1:
+  if (nDX == 1) {
+     return Aurora::zeros(aX.getDimSize(0),aX.getDimSize(1), aX.getDimSize(2));
+  }
+    // Create output matrix: -----------------------------------------------------
+  Y = Aurora::malloc(nX);
+  // Calculate the gradient: ---------------------------------------------------
+  if (nSpace == 1) {         // Scalar spacing
+     if (Step == 1) {        // Operate on 1st dimension
+        CoreDim1Space1(X, nX, nDX, *Space, Y);
+     } else {                // Step >= 1, operate on any dimension
+        CoreDimNSpace1(X, Step, nX, nDX, *Space, Y);
+     }
+
+  } else if (Order2) {       // Spacing defined as vector, 2nd order method:
+     if (nX == nDX) {        // Single vector only - dynamic spacing factors:
+        CoreDim1SpaceNOrder2(X, nX, nDX, Space, Y);
+     } else {
+        WrapSpaceNOrder2(X, Step, nX, nDX, Space, Y);
+     }
+     
+  } else {                   // Spacing defined as vector, 1st order method:
+     if (nX == nDX) {        // Single vector only - dynamic spacing factors:
+        CoreDim1SpaceN(X, nX, nDX, Space, Y);
+     } else {
+        WrapSpaceN(X, Step, nX, nDX, Space, Y);
+     }
+  }
+
+  return Aurora::Matrix::fromRawData(Y, aX.getDimSize(0),aX.getDimSize(1), aX.getDimSize(2));
+}
+
+// Subroutines: ================================================================
+mwSize FirstNonSingeltonDim(const mwSize Xndim, const mwSize *Xdim)
+{
+  // Get first non-singelton dimension - zero based.
+  
+  mwSize N;
+  
+  for (N = 0; N < Xndim; N++) {
+     if (Xdim[N] != 1) {
+        return (N);
+     }
+  }
+  
+  return (0);  // Use the first dimension if all dims are 1
+}
+
+// =============================================================================
+mwSize GetStep(const mwSize *Xdim, const mwSize N)
+{
+  // Get step size between elements of a subvector in the N'th dimension.
+  // This is the product of the leading dimensions.
+  
+  const mwSize *XdimEnd, *XdimP;
+  mwSize       Step;
+  
+  Step    = 1;
+  XdimEnd = Xdim + N;
+  for (XdimP = Xdim; XdimP < XdimEnd; Step *= *XdimP++) ; // empty loop
+  
+  return (Step);
+}
+
+// =============================================================================
+void CoreDim1Space1(double *X, const mwSize nX, const mwSize nDX, double Space,
+                    double *Y)
+{
+  // Operate on first dimension, scalar spacing, 1st order method.
+  
+  double x0, x1, x2, *Xf, *Xc, fac1, fac2;
+  
+  // Multiplication is faster than division:
+  fac1 = 1.0 / Space;
+  fac2 = 1.0 / (2.0 * Space);
+
+  Xf = X + nX;                 // End of input array
+  while (X < Xf) {
+     Xc   = X + nDX;
+     
+     x0   = *X++;              // Forward difference:
+     x1   = *X++;
+     x2   = *X++;
+     *Y++ = (4 * x1 - x2 - 3 * x0) * fac2;
+     X--;
+     while (X < Xc) {          // Central differences:
+        x2   = *X++;
+        //X++;
+        *Y++ = (x2 - x0) * fac2;
+        x0   = x1;
+        x1   = x2;
+     }
+     //going back in time 
+     x0 = *(X-3);
+     x1 = *(X-2);
+     *Y++ = (-4*x1 + x0 + 3* x2) * fac2;  // Backward difference
+  }
+
+  return;
+}
+
+// =============================================================================
+void CoreDimNSpace1(double *X, const mwSize Step, const mwSize nX,
+                    const mwSize nDX, double Space, double *Y)
+{
+  // Operate on any dimension, scalar spacing, 1st order method.
+  // Column oriented approach: Process contiguous memory blocks of input and
+  // output.
+  
+  double *Xf, *X0, *X1, *X2, *X3, *Xc, fac1, fac2;
+  mwSize nDXStep;
+
+  // Multiplication is faster than division:
+  fac1 = 1.0 / Space;
+  fac2 = 1.0 / (2.0 * Space);
+  
+  // Distance between first and last element of X in specified dim:
+  nDXStep = nDX * Step;
+  
+  Xf = X + nX;          // End of the input array
+  while (X < Xf) {
+     X1 = X;            // Forward differences:
+     X2 = X1 + Step;
+     X3 = X2 + Step;
+     Xc = X2;
+     while (X1 < Xc) {
+        *Y++ = (4* *X2++ -3* *X1++ - *X3++) * fac2;
+     }
+     
+     X1 = X;            // Central differences:
+     Xc = X + nDXStep;
+     while (X2 < Xc) {
+        *Y++ = (*X2++ - *X1++) * fac2;
+     }
+
+     X2 = X1 + Step;    // Backward differences:
+     X0 = X1 - Step;
+     while (X2 < Xc) {
+        *Y++ = ( *X0++ + 3* *X2++ -4* *X1++) * fac2;
+     }
+     
+     X = Xc;            // Move input pointer to the next chunk
+  }
+  
+  return;
+}
+
+// =============================================================================
+void WrapSpaceN(double *X, const mwSize Step, const mwSize nX, const mwSize nDX,
+                double *Space, double *Y)
+{
+  // Call different methods depending of the dimensions ofthe input.
+  // X has more than 1 vector. Therefore precalculating the spacing factors is
+  // cheaper.
+  
+  double *Factor;
+  
+  // Precalculate spacing factors:
+  if ((Factor =Aurora::malloc(nDX )) == NULL) {
+     std::cerr<<ERR_ID<<"NoMemory"<<ERR_HEAD<<"No memory for Factor vector.";
+  }
+  GetFactor(Space, nDX, Factor);
+  
+  if (Step == 1) {   // Operate on 1st dimension:
+     CoreDim1FactorN(X, nX, nDX, Factor, Y);
+  } else {           // Operate on any dimension:
+     CoreDimNFactorN(X, Step, nX, nDX, Factor, Y);
+  }
+  
+  Aurora::free(Factor);
+     
+  return;
+}
+
+// =============================================================================
+void CoreDim1SpaceN(double *X, const mwSize nX, const mwSize nDX,
+                    double *Space, double *Y)
+{
+  // Operate on the first dimension, spacing is a vector, order 1 method.
+  // The spacing factors are calculated dynamically. This is efficient for
+  // a single vector, but slower for matrices. No temporary vector is needed.
+  
+  double x0, x1, x2, *Xf, *Xg, *Sp, s0, s1, s2;
+  
+  Xf = X + nX;
+  while (X < Xf) {
+     Xg   = X + nDX;    // Forward difference
+     x0   = *X++;
+     x1   = *X++;
+     Sp   = Space;
+     s0   = *Sp++;
+     s1   = *Sp++;
+     *Y++ = (x1 - x0) / (s1 - s0);
+     
+     while (X < Xg) {   // Central differences
+        x2   = *X++;
+        s2   = *Sp++;
+        *Y++ = (x2 - x0) / (s2 - s0);
+        x0   = x1;
+        x1   = x2;
+        s0   = s1;
+        s1   = s2;
+     }
+     
+     *Y++ = (x1 - x0) / (s1 - s0);  // Backward difference
+  }
+
+  return;
+}
+
+// =============================================================================
+void GetFactor(double *Space, mwSize nDX, double *F)
+{
+  // If more than one vector is processed, it is cheaper to calculate the spacing
+  // factors once. This needs the memory for a temporary vector.
+  // INPUT:
+  //   Space: Pointer to DOUBLE vector from the inputs.
+  //   nDX:   Length of the Space vector.
+  // OUTPUT:
+  //   F:     Factors, pointer to DOUBLE vector:
+  //          [ 1/(S[1]-S[0]), 1/(S[i+1]-S[i-1]), 1/(S[nDX-1]-S[nDX-2]) ]
+   
+  double s0, s1, s2, *Ff;
+  
+  Ff   = F + nDX - 1;
+  s0   = *Space++;
+  s1   = *Space++;
+  *F++ = 1.0 / (s1 - s0);
+  
+  while (F < Ff) {
+     s2   = *Space++;
+     *F++ = 1.0 / (s2 - s0);
+     s0   = s1;
+     s1   = s2;
+  }
+  
+  *F = 1.0 / (s1 - s0);
+  
+  return;
+}
+
+// =============================================================================
+void CoreDim1FactorN(double *X, const mwSize nX, const mwSize nDX,
+                     double *Factor, double *Y)
+{
+  // Operate on first dimension, spacing is a vector, 1st order method.
+  // The spacing factors are calculated before. This needs a temporary vector
+  // of nDX elements. It is faster than the dynamically calculated spacing
+  // factors.
+  
+  double x0, x1, x2, *Xf, *Xc, *Fp;
+  
+  Xf = X + nX;
+  while (X < Xf) {
+     Xc   = X + nDX;    // End of column
+     x0   = *X++;       // Forward difference
+     x1   = *X++;
+     Fp   = Factor;
+     *Y++ = (x1 - x0) * *Fp++;
+     
+     while (X < Xc) {   // Central differences
+        x2   = *X++;
+        *Y++ = (x2 - x0) * *Fp++;
+        x0   = x1;
+        x1   = x2;
+     }
+  
+     *Y++ = (x1 - x0) * *Fp;  // Backward difference
+  }
+
+  return;
+}
+
+// =============================================================================
+void CoreDimNFactorN(double *X, const mwSize Step, const mwSize nX,
+                     const mwSize nDX, double *Factor, double *Y)
+{
+  // Operate on any dimension, spacing is a vector, 1st order method.
+  // The spacing factors are calculated before. This needs a temporary vector
+  // of nDX elements. This is faster than the dynamically calculated spacing
+  // factors, if more than one vector is processed.
+  
+  double *Xf, *X1, *X2, *Xc, *Fp, *Ff;
+  
+  Ff = Factor + nDX - 1;  // Zero-based indexing!
+
+  Xf = X + nX;            // End of the array
+  while (X < Xf) {
+     X1 = X;              // Forward differences:
+     X2 = X1 + Step;      // Element of second column
+     Xc = X2;             // End of first column
+     Fp = Factor;
+     while (X1 < Xc) {
+        *Y++ = (*X2++ - *X1++) * *Fp;
+     }
+     
+     X1  = X;             // Central differences:
+     Xc += Step;
+     while (++Fp < Ff) {
+        Xc += Step;
+        while (X2 < Xc) {
+           *Y++ = (*X2++ - *X1++) * *Fp;
+        }
+     }
+        
+     X2 = X1 + Step;      // Backward differences:
+     while (X2 < Xc) {
+        *Y++ = (*X2++ - *X1++) * *Fp;
+     }
+     
+     X = Xc;              // Move input pointer to the next chunk
+  }
+  
+  return;
+}
+
+// =============================================================================
+void WrapSpaceNOrder2(double *X, const mwSize Step, const mwSize nX,
+                      const mwSize nDX, double *Space, double *Y)
+{
+  // Call different methods depending of the dimensions ofthe input.
+  // X has more than one vector. Therefore it is cheaper to calculate the
+  // spacing factors once only.
+  
+  double *A, *B, *C;
+
+  // Precalculate spacing factors:
+  A = Aurora::malloc(nDX);
+  B = Aurora::malloc(nDX);
+  C = Aurora::malloc(nDX);
+  
+  if (A == NULL || B == NULL || C == NULL) {
+     std::cerr<<ERR_ID<<"NoMemory"<<ERR_HEAD<<"No memory for Factor vectors.";
+  }
+
+  GetFactorOrder2(Space, nDX, A, B, C);
+  
+  if (Step == 1) {        // Operate on first dimension:
+     //CoreDim1FactorNOrder2(X, nX, nDX, A, B, C, Y);
+     CoreDim1SpaceNOrder2(X, nX, nDX, Space, Y);
+  } else {                // Operate on any dimension:
+     //CoreDimNFactorNOrder2(X, Step, nX, nDX, A, B, C, Y);
+     CoreDimNSpaceNOrder2(X, Step, nX, nDX, Space, Y);
+  }
+  
+  Aurora::free(A);
+  Aurora::free(B);
+  Aurora::free(C);
+  
+  return;
+}
+
+// =============================================================================
+void GetFactorOrder2(double *Space, const mwSize nDX,
+                     double *A, double *B, double *C)
+{
+  // Calculate spacing factors for 2nd order method.
+  
+  double s0, s1, s2, s10, s21, *Sf;
+  
+  Sf = Space + nDX;
+  
+  s0   = *Space++;
+  s1   = *Space++;
+  s10  = s1 - s0;
+  *A++ = 1.0 / s10;
+  B++;
+  C++;
+  
+  while (Space < Sf) {
+     s2   = *Space++;
+     s21  = s2 - s1;
+     *A++ = s10 / (s21 * (s2 - s0));
+     *B++ = 1.0 / s10 - 1.0 / s21;
+     *C++ = s21 / (s10 * (s2 - s0));
+     s0   = s1;
+     s1   = s2;
+     s10  = s21;
+  }
+  
+  *A = 1.0 / s10;
+  
+  return;
+}
+
+// =============================================================================
+void CoreDim1SpaceNOrder2(double *X, const mwSize nX, const mwSize nDX,
+                          double *Space, double *Y)
+{
+  // Operate on first dimension, spacing is a vector, 2nd order method.
+  // For unevenly spaced data this algorithm is 2nd order accurate.
+  // This is fast for a single vector, while for arrays with more dimensions
+  // is is cheaper to calculate the spacing factors once externally.
+  
+  double x0, x1, x2, *Xf, *Xc, *Sp, s0, s1, s2, s10, s21;
+  
+  Xf = X + nX;
+  while (X < Xf) {
+     Xc   = X + nDX;    // Forward difference (same as for evenly spaced X)
+     x0   = *X++;
+     x1   = *X++;
+     Sp   = Space;
+     s0   = *Sp++;
+     s1   = *Sp++;
+     s10  = s1 - s0;
+     *Y++ = (x1 - x0) / s10;
+     
+     while (X < Xc) {    // Central differences, 2nd order method
+        x2   = *X++;
+        s2   = *Sp++;
+        s21  = s2 - s1;
+        *Y++ = ((x2 * s10 / s21) - (x0 * s21 / s10)) / (s2 - s0) +
+                x1 * (1.0 / s10 - 1.0 / s21);
+        x0  = x1;
+        x1  = x2;
+        s0  = s1;
+        s1  = s2;
+        s10 = s21;
+     }
+              
+     *Y++ = (x1 - x0) / s10;  // Backward difference
+  }
+
+  return;
+}
+
+// =============================================================================
+void CoreDimNSpaceNOrder2(double *X, const mwSize Step, const mwSize nX,
+                          const mwSize nDX, double *Space, double *Y)
+{
+  // Operate on any dimension, spacing is a vector, 2nd order method.
+  // The spacing factors are calculated dynamically. This is about 50% slower
+  // than calculating the spacing factors at first. I assume the number of
+  // registers are exhausted. Therefore this method is useful only, if the
+  // memory is nearly full.
+
+  double *Xf, *X0, *X1, *X2, *Xc, *Sp, *Sb;
+  register double a, b, c;
+  double s0, s1, s2;
+
+  Sb = Space + nDX;
+  
+  Xf = X + nX;            // End of the array
+  while (X < Xf) {
+     X0  = X;             // Forward differences for first column:
+     X1  = X;
+     X2  = X0 + Step;     // Element of second column
+     Xc  = X2;            // End of first column
+     Sp  = Space;
+     s0  = *Sp++;
+     s1  = *Sp++;
+     c   = 1.0 / (s1 - s0);
+     while (X1 < Xc) {
+        *Y++ = (*X2++ - *X1++) * c;
+     }
+
+     Xc += Step;                 // Central differences:
+     while (Sp < Sb) {
+        s2  = *Sp++;
+        a   = (s1 - s0) / ((s2 - s1) * (s2 - s0));
+        b   = 1.0 / (s1 - s0) - 1.0 / (s2 - s1);
+        c   = (s2 - s1) / ((s1 - s0) * (s2 - s0));
+        Xc += Step;
+        while (X2 < Xc) {
+           *Y++ = *X2++ * a + *X1++ * b - *X0++ * c;
+        }
+        s0  = s1;
+        s1  = s2;
+     }
+     
+     c = 1.0 / (s1 - s0);       // Backward differences:
+     while (X1 < Xc) {
+        *Y++ = (*X1++ - *X0++) * c;
+     }
+
+     X = Xc;                    // Move input pointer to the next chunk
+  }
+
+  return;
+}
+
+// =============================================================================
+void CoreDim1FactorNOrder2(double *X, const mwSize nX, const mwSize nDX,
+                           double *A, double *B, double *C, double *Y)
+{
+  // Operate on first dimension, spacing is a vector, 2nd order method.
+  // For unevenly spaced data this algorithm is 2nd order accurate.
+  // This is fast for a single vector, while for arrays with more dimensions
+  // is is cheaper to calculate the spacing factors once externally.
+  
+  double x0, x1, x2, *Xf, *Xc, *a, *b, *c;
+
+  Xf = X + nX;
+  while (X < Xf) {
+     Xc = X + nDX;    // Forward difference (same as for evenly spaced X)
+     x0 = *X++;
+     x1 = *X++;
+     a  = A;
+     b  = B + 1;
+     c  = C + 1;
+     *Y++ = (x1 - x0) * *a++;
+     
+     while (X < Xc) {    // Central differences, 2nd order method
+        x2   = *X++;
+        *Y++ = x2 * *a++ + x1 * *b++ - x0 * *c++;
+        x0   = x1;
+        x1   = x2;
+     }
+              
+     *Y++ = (x1 - x0) * *a;  // Backward difference
+  }
+
+  return;
+}
+
+// =============================================================================
+void CoreDimNFactorNOrder2(double *X, const mwSize Step, const mwSize nX,
+                          const mwSize nDX, double *A, double *B, double *C,
+                          double *Y)
+{
+  // Operate on any dimension, spacing is a vector, 2nd order method.
+  // The spacing factors are calculated externally once only.
+
+  double *Xf, *X0, *X1, *X2, *Xc, *Af, *a, *b, *c;
+  register double a_, b_, c_;
+  
+  Af = A + nDX - 1;
+  
+  Xf = X + nX;            // End of the array
+  while (X < Xf) {
+     X0  = X;             // Forward differences for first column:
+     X1  = X;
+     X2  = X0 + Step;     // Element of second column
+     Xc  = X2;            // End of first column
+     a   = A;
+     b   = B + 1;
+     c   = C + 1;
+     a_  = *a;
+     while (X1 < Xc) {
+        *Y++ = (*X2++ - *X1++) * a_;
+     }
+     
+     Xc += Step;          // Central differences:
+     a++;
+     while (a < Af) {
+        Xc += Step;
+        a_  = *a++;
+        b_  = *b++;
+        c_  = *c++;
+        while (X2 < Xc) {
+           *Y++ = *X2++ * a_ + *X1++ * b_ - *X0++ * c_;
+        }
+     }
+     
+     a_ = *a;            // Backward differences:
+     while (X1 < Xc) {
+        *Y++ = (*X1++ - *X0++) * a_;
+     }
+
+     X = Xc;              // Move input pointer to the next chunk
+  }
+
+  return;
+}
diff --git a/src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.h b/src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.h
new file mode 100644
index 0000000..e9faca5
--- /dev/null
+++ b/src/transmissionReconstruction/reconstruction/buildMatrix/DGradient.h
@@ -0,0 +1,7 @@
+#ifndef __DGRADIENT_H__
+#define __DGRADIENT_H__
+#include "Matrix.h"
+namespace Recon {
+    Aurora::Matrix DGradient(const Aurora::Matrix aX,double aSpacing,double aDim);
+}
+#endif // __DGRADIENT_H__
\ No newline at end of file
diff --git a/test/Reconstruction_Test.cpp b/test/Reconstruction_Test.cpp
index fec7724..9fce757 100644
--- a/test/Reconstruction_Test.cpp
+++ b/test/Reconstruction_Test.cpp
@@ -4,6 +4,7 @@
 #include "Function1D.h"
 #include "MatlabReader.h"
 #include "Matrix.h"
+#include "transmissionReconstruction/reconstruction/buildMatrix/DGradient.h"
 #include "transmissionReconstruction/reconstruction/reconstruction.h"
 
 
@@ -71,3 +72,16 @@ TEST_F(Reconstruction_Test, discretizePositions) {
     EXPECT_DOUBLE_AE(1,result.senderCoordList[1]);
 }
 
+TEST_F(Reconstruction_Test, DGradient) {
+    MatlabReader m("/home/krad/TestData/DGradient.mat");
+
+    auto x = m.read("x");
+    auto y = m.read("y");
+    // x.forceReshape(x.getDataSize(), 1, 1);
+    auto result = Recon::DGradient(x,1,2);
+    for (size_t i = 0; i < result.getDataSize(); i++)
+    {
+        EXPECT_DOUBLE_AE(result.getData()[i],y.getData()[i])<<"index:"<<i;
+    }
+}
+