Repository: AlabamaASRL/asset_asrl (language: python)

Issue:
New interp tables
Increased order of derivatives when calculating tangent values in InterpTable1D  and InterpTable2D , greatly improved accuracy.
Implemented InterpTable3D as well. This required updating the pybind11 submodule to a newer commit that supports Eigen Tensors.



Discussion / hints:

Fix patch:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 07645f1..fe05048 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.16)
-project(ASSET VERSION 0.1.0 LANGUAGES CXX)
+project(ASSET VERSION 0.2.0 LANGUAGES CXX)
 set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 
 set( CMAKE_MESSAGE_LOG_LEVEL "STATUS" )
diff --git a/dep/pybind11 b/dep/pybind11
index 2d59b43..3cc7e42 160000
--- a/dep/pybind11
+++ b/dep/pybind11
@@ -1,1 +1,1 @@
-Subproject commit 2d59b43cbf8793119fb92726ce8eb33441469e3e
+Subproject commit 3cc7e4258c15a6a19ba5e0b62a220b1a6196d4eb
diff --git a/doc/docguides/VectorFunctionGuide.py b/doc/docguides/VectorFunctionGuide.py
index 8baedec..bea0e23 100644
--- a/doc/docguides/VectorFunctionGuide.py
+++ b/doc/docguides/VectorFunctionGuide.py
@@ -808,14 +808,66 @@ def f(x,y):
 #print(Tab2D(-6.3,0))       # throws exception
 #############################################
 
-x,y,c= Args(3).tolist()
+xy,c= Args(3).tolist([(0,2),(2,1)])
+x,y = xy.tolist()
 
-TabSf = Tab2D(x,y)  # it is now an asset scalar function of x,y in this expression
+# Use it as scalar function inside a statement
+Tab2sf = Tab2D(xy)
+Tab2sf = Tab2D(x,y)             # Functionally same as above but slower
+Tab2sf = Tab2D(vf.stack([x,y])) # Functionally same as above but slower
 
-Func = TabSf + c   # Use it as a normal scalar function
+Func = Tab2sf + c   # Use it as a normal scalar function
 
 print(Func([np.pi/2,0,1.0]))  # prints [2.0]
 
+############ 3D Tables ##############################
+#####################################################
+def f(x,y,z):return np.cos(x)*np.cos(y)*np.cos(z)
+
+nx = 100
+ny = 100
+nz = 100
+
+xlim = np.pi
+ylim = np.pi
+zlim = np.pi
+
+xs = np.linspace(-xlim,xlim,nx)
+ys = np.linspace(-ylim,ylim,ny)
+zs = np.linspace(-zlim,zlim,nz)
+
+X,Y,Z = np.meshgrid(xs, ys,zs,indexing = 'ij')
+Fs    = f(X,Y,Z)    #Scalar data defined on 3-D meshgrid in ij format!!!
+
+kind = 'cubic' # or 'linear', defaults to 'cubic'
+cache = False # defaults to False
+#cache = True # will precalcuate and cache all interpolation coeffs
+
+Tab3D = vf.InterpTable3D(xs,ys,zs,Fs,kind=kind,cache=cache)
+
+print(Tab3D(0,0,0))  #prints 1.0 
+
+#############################################
+Tab3D.WarnOutOfBounds=True   # By default
+print(Tab3D(-10,0,0))        # prints a warning
+print(Tab3D(0,-10,0))        # prints a warning
+print(Tab3D(0,0,-10))        # prints a warning
+
+Tab3D.ThrowOutOfBounds=True
+#print(Tab3D(-10,0,0))       # throws exception
+#############################################
+
+xyz,c= Args(4).tolist([(0,3),(3,1)])
+x,y,z = xyz.tolist()
+
+# Use it as scalar function inside a statement
+Tab3sf = Tab3D(xyz)
+Tab3sf = Tab3D(x,y,z)             # Functionally same as above but slower
+Tab3sf = Tab3D(vf.stack([x,y,z])) # Functionally same as above but slower
+
+Func = Tab3sf + c
+
+print(Func([0,0,0,1]))  # prints [2.0]
 
 print("########## Binding Raw Python (DONT READ THIS) #################")
 
diff --git a/doc/tutorials/Install.rst b/doc/tutorials/Install.rst
index c24a7f0..15d0f30 100644
--- a/doc/tutorials/Install.rst
+++ b/doc/tutorials/Install.rst
@@ -28,7 +28,7 @@ Step-By-Step Guide
 	  Follow the VS installation prompts until you are prompted to select any additional packages to install with VS. 
 	  Here we will want to select **Desktop development with C++**; however, we are going to make one change to the defaults for the C++ install and **un-check** the option for "C++ Clang tools for Windows" (shown below).
 
-	  .. image:: _static/vsstudiodesktopc++.PNG
+	  .. image:: _static/vsstudiodesktopc++.png
 
 	  Also ensure that the "**C++ CMake Tools for Windows**" is checked.
 	  After doing this continue and finish the installation.
@@ -118,11 +118,11 @@ Step-By-Step Guide
 	  Now we are going to configure the ASSET CMake settings by going to the project dropdown menu and selecting "Configure ASSET" or "Configure Cache" (depending on the version of VS).
 	  Wait until the output message from CMake says that it is finished and proceed to the build step.
 
-	  .. image:: _static/config.PNG
+	  .. image:: _static/config.png
 
 	  The last step is to actually build ASSET! After the configuration step has completed navigate to the Build dropdown menu and choose "Build All" (or Ctrl + Shift + B). Keep your Python IDE closed until the build completes.
 
-	  .. image:: _static/build.PNG
+	  .. image:: _static/build.png
 
 #. With that you should have a successfully built ASSET Python library and are ready to get started with the rest of the tutorials.
 
@@ -238,7 +238,7 @@ If it is desired to use an IDE other than Visual Studio Code, it is still requir
 
 	  Now hit build to begin building ASSET
 
-	  .. image:: _static/vscodebuild.PNG
+	  .. image:: _static/vscodebuild.png
 
 #. With that you should have a successfully built ASSET Python library and are ready to get started with the rest of the tutorials.
 
diff --git a/doc/tutorials/PhaseGuide.rst b/doc/tutorials/PhaseGuide.rst
index 2bd5de4..21f4bf8 100644
--- a/doc/tutorials/PhaseGuide.rst
+++ b/doc/tutorials/PhaseGuide.rst
@@ -889,6 +889,29 @@ What happens if I add multiple objectives?
 A phase has no restriction of the number of objectives that may be added. If multiple objectives are added, the optimizer will implicitly sum all of their values.
 
 
+Bad Initial Guesses 
+-------------------
+
+By default we take whatever the supplied initial guess to a phase is and use the specified transcription scheme's interpolation method to generate the initial mesh.
+For compressed collocation methods, when the initial guess is very poor, this can induce osculations in the initial interpolated mesh that are not present in the initial guess
+. This can be avoided by instead interpolating the initial mesh linearly from the supplied initial guess. You can specify the method for the initial interpolation of a phase as shown below.
+If the user supplied initial guess is linear or constant, we recommend using the linear interpolation method.
+
+.. code-block:: python
+       
+
+       LerpIG = False
+       #Interpolate initial mesh using transcriptions interpolation method (same as default)
+       phase = ode.phase("LGL3",TrajIG,nsegs,LerpIG) 
+       phase = ode.phase("LGL3",TrajIG,nsegs) # Above is the same as this
+
+       LerpIG = True
+       #Interpolate initial mesh using linear interpolation
+       phase = ode.phase("LGL3",TrajIG,nsegs,LerpIG)
+       
+
+       
+
 Reintegrating Solutions
 -----------------------
 Reintegration of a phase's trajectory can be accomplished using an ODE's integrator and the tabular
diff --git a/doc/tutorials/VectorFunctionGuide.rst b/doc/tutorials/VectorFunctionGuide.rst
index 62e07c8..73be717 100644
--- a/doc/tutorials/VectorFunctionGuide.rst
+++ b/doc/tutorials/VectorFunctionGuide.rst
@@ -858,9 +858,18 @@ by providing a ScalarFunction argument to the call operator.
 
 2-D Interpolation
 -----------------
-Similarly, you can also interpret scalar data defined on a 2-D regular grid of coordinates as an ASSET ScalarFunction using the
+Similarly, you can also interpret scalar data defined on a 2-D rectilinear grid of coordinates as an ASSET ScalarFunction using the
 :code:`vf.InterpTable2D` class. The class may be constructed by supplying the grid coordinates as either python lists or numpy
-arrays along with the function values formatted like a numpy meshgrid.
+arrays along with the function values formatted like a numpy meshgrid. The function values MUST be formatted like an xy indexed meshgrid,
+which is the default behavior for np.meshgrid in 2 dimensions
+
+..  note:: 
+
+	 The function values MUST be formatted like an xy indexed meshgrid.If you are 
+	 using np.meshgrid to generate the function values, take note that it assumes xy 
+	 indexing by default. If you index the values wrong and your coordinate dimensions 
+	 are the same size, we will not be able to detect an error.
+
 
 .. code-block:: python
 
@@ -888,20 +897,96 @@ arrays along with the function values formatted like a numpy meshgrid.
 	#print(Tab2D(-6.3,0))       # throws exception
 
 Once constructed, :code:`vf.InterpTable2D` can be converted into an ASSET ScalarFunction by supplying
-the x and y coordinates as ASSET ScalarFunctions to the table's call operator.
+the x and y coordinates to the table's call operator as a singe VectorFunction or two separate ScalarFunctions .
 
 
 .. code-block:: python
 
-	x,y,c= Args(3).tolist()
+	xy,c= Args(3).tolist([(0,2),(2,1)])
+	x,y = xy.tolist()
 
-	TabSf = Tab2D(x,y)  # it is now an asset scalar function of x,y in this expression
+	# Use it as scalar function inside a statement
+	Tab2sf = Tab2D(xy)
+	Tab2sf = Tab2D(x,y)             # Or
+	Tab2sf = Tab2D(vf.stack([x,y])) # Or
 
-	Func = TabSf + c   # Use it as a normal scalar function
+	Func = Tab2sf + c   # Use it as a normal scalar function
 
 	print(Func([np.pi/2,0,1.0]))  # prints [2.0]
 
-	
+3-D Interpolation
+-----------------
+As of version 0.1.0, we also support interpolating scalar data defined on 3 dimensional rectilinear grids
+with the :code:`vf.InterpTable3D` class. To construct it, we supply the coordinate values as three python
+lists/numpy vectors along with the function values. 
+
+..  note:: 
+
+	 The function values MUST be formatted like an ij indexed meshgrid. If you are 
+	 using np.meshgrid to generate the function values, take note that it assumes xy indexing by default,
+	 and this should be changed to ij.If you index the values wrong and your coordinate dimensions are the same size, 
+	 we will not be able to detect an error.
+
+As with the 1D and 2D tables, you can also select between linear or cubic interpolation. Cubic interpolation should be 
+preferred for anything that will end up in an optimizer. Additionally, for cubic interpolation you can specify that
+you want to pre-calculate and cache all possible values of the interpolation coefficients in each voxel of the domain.
+This requires the up front calculation of  :code:`(nx-1)*(ny-1)*(nz-1)` matrix vector products of size (64x64)x(64x1)  and 8 times the storage of the non-cached
+algorithm. However, interpolation will be 10-20 times faster. So use this if the dimensions are small and the interpolator
+will be called a lot.
+
+.. code-block:: python
+
+	def f(x,y,z):return np.cos(x)*np.cos(y)*np.cos(z)
+
+	nx = 100
+	ny = 100
+	nz = 100
+
+	xlim = np.pi
+	ylim = np.pi
+	zlim = np.pi
+
+	xs = np.linspace(-xlim,xlim,nx)
+	ys = np.linspace(-ylim,ylim,ny)
+	zs = np.linspace(-zlim,zlim,nz)
+
+	X,Y,Z = np.meshgrid(xs, ys,zs,indexing = 'ij')
+	Fs    = f(X,Y,Z)    #Scalar data defined on 3-D meshgrid in ij format!!!
+
+	kind = 'cubic' # or 'linear', defaults to 'cubic'
+	cache = False # defaults to False
+	#cache = True # will precalculate and cache all interpolation coeffs
+
+	Tab3D = vf.InterpTable3D(xs,ys,zs,Fs,kind=kind,cache=cache)
+
+	print(Tab3D(0,0,0))  #prints 1.0 
+
+	Tab3D.WarnOutOfBounds=True   # By default
+	print(Tab3D(-10,0,0))        # prints a warning
+	print(Tab3D(0,-10,0))        # prints a warning
+	print(Tab3D(0,0,-10))        # prints a warning
+
+	Tab3D.ThrowOutOfBounds=True
+	#print(Tab3D(-10,0,0))       # throws exception
+
+
+
+Once constructed, :code:`vf.InterpTable3D` can be converted into an ASSET ScalarFunction by supplying
+the x, y, and z coordinates to the table's call operator as a singe VectorFunction or three separate ScalarFunctions.
+
+.. code-block:: python
+
+	xyz,c= Args(4).tolist([(0,3),(3,1)])
+	x,y,z = xyz.tolist()
+
+	# Use it as scalar function inside a statement
+	Tab3sf = Tab3D(xyz)
+	Tab3sf = Tab3D(x,y,z)             # Or
+	Tab3sf = Tab3D(vf.stack([x,y,z])) # Or
+
+	Func = Tab3sf + c
+
+	print(Func([0,0,0,1]))  # prints [2.0]
 
 
 
diff --git a/src/Astro/ASSET_Astro.cpp b/src/Astro/ASSET_Astro.cpp
index ec28c15..d1992c2 100644
--- a/src/Astro/ASSET_Astro.cpp
+++ b/src/Astro/ASSET_Astro.cpp
@@ -6,8 +6,7 @@ void ASSET::AstroBuild(FunctionRegistry& reg, py::module& m)
 
 	BuildKeplerMod(reg, mod);
 	KeplerUtilsBuild(reg, mod);
-	LambertSolverBuild(reg, mod);
-	LambertSolverBuildMultiRev(reg, mod);
+	LambertSolversBuild(reg, mod);
 
 
 	
diff --git a/src/Astro/ASSET_Astro.h b/src/Astro/ASSET_Astro.h
index fe883ea..ac1df0e 100644
--- a/src/Astro/ASSET_Astro.h
+++ b/src/Astro/ASSET_Astro.h
@@ -1,10 +1,9 @@
 #pragma once
 #include "VectorFunctions/ASSET_VectorFunctions.h"
 #include "KeplerPropagator.h"
-#include "LambertSolver.h"
+#include "LambertSolvers.h"
 #include "KeplerModel.h"
 #include "KeplerUtils.h"
-#include "LambertSolverMultiRevTest.h"
 #include "MEEDynamics.h"
 #include "J2.h"
 #include "ThrusterModels.h"
diff --git a/src/Astro/LambertSolver.cpp b/src/Astro/LambertSolver.cpp
deleted file mode 100644
index d627979..0000000
--- a/src/Astro/LambertSolver.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "LambertSolver.h"
-
-void ASSET::LambertSolverBuild(FunctionRegistry& reg, py::module& m) {
-
-
-	m.def("lambert_izzo",
-		[](const Vector3<double>& R1t, const Vector3<double>& R2t, double tf, double mu, bool lw) {
-			return lambert_izzo(R1t, R2t, tf, mu, lw);
-		});
-
-
-
-	
-
-
-
-
-}
diff --git a/src/Astro/LambertSolver.h b/src/Astro/LambertSolver.h
deleted file mode 100644
index 181f374..0000000
--- a/src/Astro/LambertSolver.h
+++ /dev/null
@@ -1,252 +0,0 @@
-#pragma once
-#include "VectorFunctions/ASSET_VectorFunctions.h"
-
-namespace ASSET {
-
-
-	void LambertSolverBuild(FunctionRegistry& reg, py::module& m);
-
-
-
-	// I took this from Izzo
-
-	template<class Scalar>
-	void lambert_izzo_impl(const Scalar* r1_in, const Scalar* r2_in, Scalar t, const Scalar& mu, //INPUT
-		bool lw, //INPUT
-		Scalar* v1, Scalar* v2, Scalar& a, Scalar& p, Scalar& theta, int& iter)//OUTPUT
-	{
-
-		//using namespace std;
-
-
-		Scalar	V, T,
-			r2_mod = 0.0,    // R2 module
-			dot_prod = 0.0, // dot product
-			c,		        // non-dimensional chord
-			s,		        // non dimensional semi-perimeter
-			am,		        // minimum energy ellipse semi major axis
-			lambda,	        //lambda parameter defined in Battin's Book
-			x, x1, x2, y1, y2, x_new = 0, y_new, err, alfa, beta, psi, eta, eta2, sigma1, vr1, vt1, vt2, vr2, R = 0.0;
-		int i_count, i;
-		const Scalar tolerance = 1e-11;
-		Scalar r1[3], r2[3], r2_vers[3];
-		Scalar ih_dum[3], ih[3], dum[3];
-
-		auto x2tof = [](Scalar x, Scalar s, Scalar c, const int & lw) {
-
-			Scalar am, a, alfa, beta;
-
-			am = s / 2;
-			a = am / (1 - x * x);
-			if (x < 1)//ellpise
-			{
-				beta = 2 * asin(sqrt((s - c) / (2 * a)));
-				if (lw) beta = -beta;
-				alfa = 2 * acos(x);
-			}
-			else
-			{
-				
-				alfa = 2. * acosh(x);
-				beta = 2. * asinh(sqrt((s - c) / (-2 * a)));
-				if (lw) beta = -beta;
-			}
-
-			if (a > 0)
-			{
-				return (a * sqrt(a) * ((alfa - sin(alfa)) - (beta - sin(beta))));
-			}
-			else
-			{
-				return (-a * sqrt(-a) * ((sinh(alfa) - alfa) - (sinh(beta) - beta)));
-			}
-
-
-		};
-
-
-
-		auto vers = [](const Scalar* V_in, Scalar* Ver_out)
-		{
-			Scalar v_mod = 0;
-			int i;
-
-			for (i = 0; i < 3; i++)
-			{
-				v_mod += V_in[i] * V_in[i];
-			}
-
-			Scalar sqrtv_mod = sqrt(v_mod);
-
-			for (i = 0; i < 3; i++)
-			{
-				Ver_out[i] = V_in[i] / sqrtv_mod;
-			}
-		};
-
-		auto vett = [](const Scalar* vet1, const Scalar* vet2, Scalar* prod)
-		{
-			prod[0] = (vet1[1] * vet2[2] - vet1[2] * vet2[1]);
-			prod[1] = (vet1[2] * vet2[0] - vet1[0] * vet2[2]);
-			prod[2] = (vet1[0] * vet2[1] - vet1[1] * vet2[0]);
-		};
-
-
-
-		// Increasing the tolerance does not bring any advantage as the
-		// precision is usually greater anyway (due to the rectification of the tof
-		// graph) except near particular cases such as parabolas in which cases a
-		// lower precision allow for usual convergence.
-
-		if (t <= 0)
-		{
-			//pagmo_throw(value_error, "ERROR in Lambert Solver: Negative Time in input.");
-		}
-
-		for (i = 0; i < 3; i++)
-		{
-			r1[i] = r1_in[i];
-			r2[i] = r2_in[i];
-			R += r1[i] * r1[i];
-		}
-
-		R = sqrt(R);
-		V = sqrt(mu / R);
-		T = R / V;
-
-		// working with non-dimensional radii and time-of-flight
-		t /= T;
-		for (i = 0; i < 3; i++)  // r1 dimension is 3
-		{
-			r1[i] /= R;
-			r2[i] /= R;
-			r2_mod += r2[i] * r2[i];
-		}
-
-		// Evaluation of the relevant geometry parameters in non dimensional units
-		r2_mod = sqrt(r2_mod);
-
-		for (i = 0; i < 3; i++)
-			dot_prod += (r1[i] * r2[i]);
-
-		theta = acos(dot_prod / r2_mod);
-
-		if (lw)
-			theta = 2 * acos(-1.0) - theta;
-
-		c = sqrt(1 + r2_mod * (r2_mod - 2.0 * cos(theta)));
-		s = (1 + r2_mod + c) / 2.0;
-		am = s / 2.0;
-		lambda = sqrt(r2_mod) * cos(theta / 2.0) / s;
-
-		// We start finding the log(x+1) value of the solution conic:
-		// NO MULTI REV --> (1 SOL)
-		//	inn1=-.5233;    //first guess point
-		//  inn2=.5233;     //second guess point
-		x1 = log(0.4767);
-		x2 = log(1.5233);
-		y1 = log(x2tof(-.5233, s, c, lw)) - log(t);
-		y2 = log(x2tof(.5233, s, c, lw)) - log(t);
-
-		// Regula-falsi iterations
-		err = 1;
-		i_count = 0;
-		while ((err > tolerance) && (y1 != y2))
-		{
-			i_count++;
-			x_new = (x1 * y2 - y1 * x2) / (y2 - y1);
-			y_new = log(x2tof(exp(x_new) - 1, s, c, lw)) - log(t);
-			x1 = x2;
-			y1 = y2;
-			x2 = x_new;
-			y2 = y_new;
-			err = fabs(x1 - x_new);
-		}
-		iter = i_count;
-		x = exp(x_new) - 1;
-
-		// The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we
-		// now need the conic. As for transfer angles near to pi the lagrange
-		// coefficient technique goes singular (dg approaches a zero/zero that is
-		// numerically bad) we here use a different technique for those cases. When
-		// the transfer angle is exactly equal to pi, then the ih unit vector is not
-		// determined. The remaining equations, though, are still valid.
-
-		a = am / (1 - x * x);
-
-		// psi evaluation
-		if (x < 1)  // ellipse
-		{
-			beta = 2 * asin(sqrt((s - c) / (2 * a)));
-			if (lw) beta = -beta;
-			alfa = 2 * acos(x);
-			psi = (alfa - beta) / 2;
-			eta2 = 2 * a * pow(sin(psi), 2) / s;
-			eta = sqrt(eta2);
-		}
-		else       // hyperbola
-		{
-			beta = 2 * asinh(sqrt((c - s) / (2 * a)));
-			if (lw) beta = -beta;
-			alfa = 2 * acosh(x);
-			psi = (alfa - beta) / 2;
-			eta2 = -2 * a * pow(sinh(psi), 2) / s;
-			eta = sqrt(eta2);
-		}
-
-		// parameter of the solution
-		p = (r2_mod / (am * eta2)) * pow(sin(theta / 2), 2);
-		sigma1 = (1 / (eta * sqrt(am))) * (2 * lambda * am - (lambda + x * eta));
-		vett(r1, r2, ih_dum);
-		vers(ih_dum, ih);
-
-		if (lw)
-		{
-			for (i = 0; i < 3; i++)
-				ih[i] = -ih[i];
-		}
-		
-		vr1 = sigma1;
-		vt1 = sqrt(p);
-		vett(ih, r1, dum);
-
-		for (i = 0; i < 3; i++)
-			v1[i] = vr1 * r1[i] + vt1 * dum[i];
-
-		vt2 = vt1 / r2_mod;
-		vr2 = -vr1 + (vt1 - vt2) / tan(theta / 2);
-		vers(r2, r2_vers);
-		vett(ih, r2_vers, dum);
-		for (i = 0; i < 3; i++)
-			v2[i] = vr2 * r2[i] / r2_mod + vt2 * dum[i];
-
-		for (i = 0; i < 3; i++)
-		{
-			v1[i] *= V;
-			v2[i] *= V;
-		}
-		a *= R;
-		p *= R;
-	}
-
-	template<class Scalar>
-	std::array<Vector3<Scalar>, 2> lambert_izzo(const Vector3<Scalar>& R1t, const Vector3<Scalar>& R2t, Scalar tf, Scalar mu, bool lw) {
-		Vector3<Scalar> R1 = R1t;
-		Vector3<Scalar> R2 = R2t;
-
-		Vector3<Scalar> V1;
-		Vector3<Scalar> V2;
-
-		Scalar a;
-		Scalar p;
-		Scalar theta;
-		int iter;
-
-		lambert_izzo_impl(R1.data(), R2.data(), tf, mu, lw,
-			V1.data(), V2.data(), a, p, theta, iter);
-		return std::array<Vector3<Scalar>, 2>{ V1, V2 };
-	}
-
-
-	
- }
\ No newline at end of file
diff --git a/src/Astro/LambertSolverMultiRevTest.cpp b/src/Astro/LambertSolverMultiRevTest.cpp
deleted file mode 100644
index 247348d..0000000
--- a/src/Astro/LambertSolverMultiRevTest.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "LambertSolverMultiRevTest.h"
-
-void ASSET::LambertSolverBuildMultiRev(FunctionRegistry& reg, py::module& m) {
-
-
-	m.def("lambert_izzo_multirev",
-		[](const Vector3<double>& R1t, const Vector3<double>& R2t, double tf, double mu, bool lw, double N, bool right) {
-			return lambert_izzo_multirev(R1t, R2t, tf, mu, lw, N, right);
-		});
-
-
-
-	
-
-
-}
diff --git a/src/Astro/LambertSolverMultiRevTest.h b/src/Astro/LambertSolverMultiRevTest.h
deleted file mode 100644
index 75d3d37..0000000
--- a/src/Astro/LambertSolverMultiRevTest.h
+++ /dev/null
@@ -1,283 +0,0 @@
-#pragma once
-#include "VectorFunctions/ASSET_VectorFunctions.h"
-# define D_ALP 1.5
-namespace ASSET {
-
-
-	void LambertSolverBuildMultiRev(FunctionRegistry& reg, py::module& m);
-
-
-
-	// I took this from Izzo
-
-	template<class Scalar>
-	void lambert_izzo_impl(const Scalar* r1_in, const Scalar* r2_in, Scalar t, const Scalar& mu, //INPUT
-		bool lw, const Scalar& N, bool right, //INPUT
-		Scalar* v1, Scalar* v2, Scalar& a, Scalar& p, Scalar& theta, int& iter)//OUTPUT
-	{
-
-		//using namespace std;
-
-
-		Scalar	V, T,
-			r2_mod = 0.0,    // R2 module
-			dot_prod = 0.0, // dot product
-			c,		        // non-dimensional chord
-			s,		        // non dimensional semi-perimeter
-			am,		        // minimum energy ellipse semi major axis
-			lambda,	        //lambda parameter defined in Battin's Book
-			x, x1, x2, y1, y2, x_new = 0, y_new, err, alfa, beta, psi, eta, eta2, sigma1, vr1, vt1, vt2, vr2, R = 0.0;
-		int i_count, i;
-		const Scalar tolerance = 1e-11;
-		Scalar r1[3], r2[3], r2_vers[3];
-		Scalar ih_dum[3], ih[3], dum[3];
-
-		auto x2tof = [](Scalar x, Scalar s, Scalar c, const int & lw, Scalar N) {
-
-			Scalar am, a, alfa, beta;
-
-			am = s / 2;
-			a = am / (1 - x * x);
-			if (x < 1)//ellpise
-			{
-				beta = 2 * asin(sqrt((s - c) / (2 * a)));
-				if (lw) beta = -beta;
-				alfa = 2 * acos(x);
-			}
-			else
-			{
-				
-				alfa = 2. * acosh(x);
-				beta = 2. * asinh(sqrt((s - c) / (-2 * a)));
-				if (lw) beta = -beta;
-			}
-
-			if (a > 0)
-			{
-				return (a * sqrt(a) * ((alfa - sin(alfa)) - (beta - sin(beta)) + 2 * atan(1) * 4 * N));
-			}
-			else
-			{
-				return (-a * sqrt(-a) * ((sinh(alfa) - alfa) - (sinh(beta) - beta)));
-			}
-
-
-		};
-
-
-
-		auto vers = [](const Scalar* V_in, Scalar* Ver_out)
-		{
-			Scalar v_mod = 0;
-			int i;
-
-			for (i = 0; i < 3; i++)
-			{
-				v_mod += V_in[i] * V_in[i];
-			}
-
-			Scalar sqrtv_mod = sqrt(v_mod);
-
-			for (i = 0; i < 3; i++)
-			{
-				Ver_out[i] = V_in[i] / sqrtv_mod;
-			}
-		};
-
-		auto vett = [](const Scalar* vet1, const Scalar* vet2, Scalar* prod)
-		{
-			prod[0] = (vet1[1] * vet2[2] - vet1[2] * vet2[1]);
-			prod[1] = (vet1[2] * vet2[0] - vet1[0] * vet2[2]);
-			prod[2] = (vet1[0] * vet2[1] - vet1[1] * vet2[0]);
-		};
-
-
-
-		// Increasing the tolerance does not bring any advantage as the
-		// precision is usually greater anyway (due to the rectification of the tof
-		// graph) except near particular cases such as parabolas in which cases a
-		// lower precision allow for usual convergence.
-
-		if (t <= 0)
-		{
-			//pagmo_throw(value_error, "ERROR in Lambert Solver: Negative Time in input.");
-		}
-
-		for (i = 0; i < 3; i++)
-		{
-			r1[i] = r1_in[i];
-			r2[i] = r2_in[i];
-			R += r1[i] * r1[i];
-		}
-
-		R = sqrt(R);
-		V = sqrt(mu / R);
-		T = R / V;
-
-		// working with non-dimensional radii and time-of-flight
-		t /= T;
-		for (i = 0; i < 3; i++)  // r1 dimension is 3
-		{
-			r1[i] /= R;
-			r2[i] /= R;
-			r2_mod += r2[i] * r2[i];
-		}
-
-		// Evaluation of the relevant geometry parameters in non dimensional units
-		r2_mod = sqrt(r2_mod);
-
-		for (i = 0; i < 3; i++)
-			dot_prod += (r1[i] * r2[i]);
-
-		theta = acos(dot_prod / r2_mod);
-
-		if (lw)
-			theta = 2 * acos(-1.0) - theta;
-
-		c = sqrt(1 + r2_mod * (r2_mod - 2.0 * cos(theta)));
-		s = (1 + r2_mod + c) / 2.0;
-		am = s / 2.0;
-		lambda = sqrt(r2_mod) * cos(theta / 2.0) / s;
-
-		// We start finding the log(x+1) value of the solution conic:
-		// NO MULTI REV --> (1 SOL)
-		//	inn1=-.5233;    //first guess point
-		//  inn2=.5233;     //second guess point
-
-        if (N == 0)
-        {
-        		x1 = log(0.4767);
-        		x2 = log(1.5233);
-        		y1 = log(x2tof(-.5233, s, c, lw, N)) - log(t);
-        		y2 = log(x2tof(.5233, s, c, lw, N)) - log(t);
-        		// Regula-falsi iterations
-        		err = 1;
-        		i_count = 0;
-        		while ((err > tolerance) && (y1 != y2))
-        		{
-        			i_count++;
-        			x_new = (x1 * y2 - y1 * x2) / (y2 - y1);
-        			y_new = log(x2tof(exp(x_new) - 1, s, c, lw, N)) - log(t);
-        			x1 = x2;
-        			y1 = y2;
-        			x2 = x_new;
-        			y2 = y_new;
-        			err = fabs(x1 - x_new);
-        		}
-        		iter = i_count;
-        		x = exp(x_new) - 1;
-        }
-        else
-        {
-        // multi-rev case
-        // Regula-falsi iterations
-            x1 = -1.0;
-            x2 = -0.5;
-            if (right) { 
-                x1 = 1.0;
-                x2 = 0.5;}
-            err = 1;
-            i_count = 0;
-            y1 = log(x2tof((exp(x1 * D_ALP) - 1.0) / (exp(x1 * D_ALP) + 1.0), s, c, lw, N)) - log(t);
-            y2 = log(x2tof((exp(x2 * D_ALP) - 1.0) / (exp(x2 * D_ALP) + 1.0), s, c, lw, N)) - log(t);
-        		while ((err > tolerance) && (y1 != y2))
-        		{
-        		    i_count++;
-                    x_new = (x1 * y2 - y1 * x2) / (y2 - y1);
-        		    y_new = log(x2tof((exp(x_new * D_ALP) - 1.0) / (exp(x_new * D_ALP) + 1.0), s, c, lw, N)) - log(t);
-        			x1 = x2;
-        			y1 = y2;
-        			x2 = x_new;
-        			y2 = y_new;
-        			err = fabs(x1 - x_new);
-        		}
-        		iter = i_count;
-        		x = (exp(x_new * D_ALP) - 1.0) / (exp(x_new * D_ALP) + 1.0);
-        		
-        }
-  
-        	// The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we
-      		// now need the conic. As for transfer angles near to pi the lagrange
-      		// coefficient technique goes singular (dg approaches a zero/zero that is
-      		// numerically bad) we here use a different technique for those cases. When
-      		// the transfer angle is exactly equal to pi, then the ih unit vector is not
-      		// determined. The remaining equations, though, are still valid.
-      
-      		a = am / (1 - x * x);
-      
-      		// psi evaluation
-      		if (x < 1)  // ellipse
-      		{
-      			beta = 2 * asin(sqrt((s - c) / (2 * a)));
-      			if (lw) beta = -beta;
-      			alfa = 2 * acos(x);
-      			psi = (alfa - beta) / 2;
-      			eta2 = 2 * a * pow(sin(psi), 2) / s;
-      			eta = sqrt(eta2);
-      		}
-      		else       // hyperbola
-      		{
-      			beta = 2 * asinh(sqrt((c - s) / (2 * a)));
-      			if (lw) beta = -beta;
-      			alfa = 2 * acosh(x);
-      			psi = (alfa - beta) / 2;
-      			eta2 = -2 * a * pow(sinh(psi), 2) / s;
-      			eta = sqrt(eta2);
-      		}
-      
-      		// parameter of the solution
-      		p = (r2_mod / (am * eta2)) * pow(sin(theta / 2), 2);
-      		sigma1 = (1 / (eta * sqrt(am))) * (2 * lambda * am - (lambda + x * eta));
-      		vett(r1, r2, ih_dum);
-      		vers(ih_dum, ih);
-      
-      		if (lw)
-      		{
-      			for (i = 0; i < 3; i++)
-      				ih[i] = -ih[i];
-      		}
-      		
-      		vr1 = sigma1;
-      		vt1 = sqrt(p);
-      		vett(ih, r1, dum);
-      
-      		for (i = 0; i < 3; i++)
-      			v1[i] = vr1 * r1[i] + vt1 * dum[i];
-      
-      		vt2 = vt1 / r2_mod;
-      		vr2 = -vr1 + (vt1 - vt2) / tan(theta / 2);
-      		vers(r2, r2_vers);
-      		vett(ih, r2_vers, dum);
-      		for (i = 0; i < 3; i++)
-      			v2[i] = vr2 * r2[i] / r2_mod + vt2 * dum[i];
-      
-      		for (i = 0; i < 3; i++)
-      		{
-      			v1[i] *= V;
-      			v2[i] *= V;
-      		}
-      		a *= R;
-      		p *= R;
-  
-  }
-	template<class Scalar>
-	std::array<Vector3<Scalar>, 2> lambert_izzo_multirev(const Vector3<Scalar>& R1t, const Vector3<Scalar>& R2t, Scalar tf, Scalar mu, bool lw, Scalar N, bool right) {
-		Vector3<Scalar> R1 = R1t;
-		Vector3<Scalar> R2 = R2t;
-
-		Vector3<Scalar> V1;
-		Vector3<Scalar> V2;
-
-		Scalar a;
-		Scalar p;
-		Scalar theta;
-		int iter;
-
-		lambert_izzo_impl(R1.data(), R2.data(), tf, mu, lw, N, right,
-			V1.data(), V2.data(), a, p, theta, iter);
-		return std::array<Vector3<Scalar>, 2>{ V1, V2 };
-	}
-
-# undef D_ALP
-	
- }
\ No newline at end of file
diff --git a/src/Astro/LambertSolvers.cpp b/src/Astro/LambertSolvers.cpp
new file mode 100644
index 0000000..bc8665e
--- /dev/null
+++ b/src/Astro/LambertSolvers.cpp
@@ -0,0 +1,158 @@
+#include "LambertSolvers.h"
+
+
+
+
+void ASSET::LambertSolversBuild(FunctionRegistry& reg, py::module& m) {
+
+
+	m.def("lambert_izzo",
+		[](const Vector3<double>& R1, const Vector3<double>& R2, double dt, double mu, bool longway) {
+			return lambert_izzo(R1, R2, dt, mu, longway);
+		});
+
+	m.def("lambert_izzo",
+		[](const Vector3<double>& R1, const Vector3<double>& R2, double dt, double mu, bool longway,int Nrevs, bool rightbranch) {
+			return lambert_izzo(R1,R2,dt,mu,longway,Nrevs,rightbranch);
+		});
+
+	m.def("lambert_izzo_multirev",
+		[](const Vector3<double>& R1, const Vector3<double>& R2, double dt, double mu, bool longway, int Nrevs, bool rightbranch) {
+			return lambert_izzo(R1, R2, dt, mu, longway, Nrevs, rightbranch);
+		});
+
+
+
+	using NumpyMat = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;
+
+	
+	m.def("lambert_izzo",
+		[](ConstEigenRef<NumpyMat> R1s, ConstEigenRef<NumpyMat> R2s, ConstEigenRef<VectorX<double>> dts, double mu, const std::vector<bool> & longways,
+			EigenRef<NumpyMat> V1s, EigenRef<NumpyMat> V2s,
+			int axis,bool vectorize) {
+
+			using SuperScalar = Eigen::Array<double, 8, 1>;
+			constexpr int vsize = SuperScalar::SizeAtCompileTime;
+			int NumCalls;
+			if (axis == 0) {
+				NumCalls = R1s.cols();
+			}
+			else {
+				NumCalls = R1s.rows();
+			}
+			int Packs = vectorize ? NumCalls / vsize:0;
+
+			Eigen::VectorXi exitcodes(NumCalls);
+			Vector3<SuperScalar> R1ss;
+			Vector3<SuperScalar> R2ss;
+			std::array<bool, vsize> lwss;
+			std::array<int, vsize > ecodess;
+
+			Vector3<SuperScalar> V1ss;
+			Vector3<SuperScalar> V2ss;
+
+			for (int i = 0; i < Packs; i++) {
+				int V = i * vsize;
+				if (axis == 0) {
+					R1ss[0] = R1s.row(0).segment<vsize>(V, vsize).transpose();
+					R1ss[1] = R1s.row(1).segment<vsize>(V, vsize).transpose();
+					R1ss[2] = R1s.row(2).segment<vsize>(V, vsize).transpose();
+
+					R2ss[0] = R2s.row(0).segment<vsize>(V, vsize).transpose();
+					R2ss[1] = R2s.row(1).segment<vsize>(V, vsize).transpose();
+					R2ss[2] = R2s.row(2).segment<vsize>(V, vsize).transpose();
+				}
+				else {
+
+					R1ss[0] = R1s.col(0).segment<vsize>(V, vsize);
+					R1ss[1] = R1s.col(1).segment<vsize>(V, vsize);
+					R1ss[2] = R1s.col(2).segment<vsize>(V, vsize);
+
+					R2ss[0] = R2s.col(0).segment<vsize>(V, vsize);
+					R2ss[1] = R2s.col(1).segment<vsize>(V, vsize);
+					R2ss[2] = R2s.col(2).segment<vsize>(V, vsize);
+				}
+
+
+				
+
+				SuperScalar dtss = dts.segment<vsize>(V);
+				for (int j = 0; j < vsize; j++) {
+					lwss[j] = longways[V + j];
+				}
+
+				lambert_izzo_impl(R1ss, R2ss, dtss, mu,
+					lwss,
+					0, false,
+					V1ss, V2ss,
+					ecodess);
+
+				
+				if (axis == 0) {
+					V1s.row(0).segment<vsize>(V, vsize) = V1ss[0].transpose();
+					V1s.row(1).segment<vsize>(V, vsize) = V1ss[1].transpose();
+					V1s.row(2).segment<vsize>(V, vsize) = V1ss[2].transpose();
+
+					V2s.row(0).segment<vsize>(V, vsize) = V2ss[0].transpose();
+					V2s.row(1).segment<vsize>(V, vsize) = V2ss[1].transpose();
+					V2s.row(2).segment<vsize>(V, vsize) = V2ss[2].transpose();
+				}
+				else {
+					V1s.col(0).segment<vsize>(V, vsize) = V1ss[0];
+					V1s.col(1).segment<vsize>(V, vsize) = V1ss[1];
+					V1s.col(2).segment<vsize>(V, vsize) = V1ss[2];
+
+					V2s.col(0).segment<vsize>(V, vsize) = V2ss[0];
+					V2s.col(1).segment<vsize>(V, vsize) = V2ss[1];
+					V2s.col(2).segment<vsize>(V, vsize) = V2ss[2];
+				}
+			
+				for (int j = 0; j < vsize; j++) {
+					exitcodes[V + j] = ecodess[j];
+				}
+			}
+
+			Vector3<double> R1;
+			Vector3<double> R2;
+			Vector3<double> V1;
+			Vector3<double> V2;
+
+			for (int i = Packs*vsize; i < NumCalls; i++) {
+
+				if (axis == 0) {
+					R1 = R1s.col(i);
+					R2 = R2s.col(i);
+				}
+				else {
+					R1 = R1s.row(i);
+					R2 = R2s.row(i);
+				}
+
+				double dt = dts[i];
+				bool longway = longways[i];
+				int excode;
+
+				lambert_izzo_impl(R1, R2, dt, mu,
+					longway,
+					0, false,
+					V1, V2,
+					excode);
+
+				exitcodes[i] = excode;
+
+				if (axis == 0) {
+					V1s.col(i) = V1;
+					V2s.col(i) = V2;
+				}
+				else {
+					V1s.row(i) = V1.transpose();
+					V2s.row(i) = V2.transpose();
+				}
+
+			}
+
+			return exitcodes;
+		});
+
+		
+}
diff --git a/src/Astro/LambertSolvers.h b/src/Astro/LambertSolvers.h
new file mode 100644
index 0000000..55340da
--- /dev/null
+++ b/src/Astro/LambertSolvers.h
@@ -0,0 +1,511 @@
+#pragma once
+#include "VectorFunctions/ASSET_VectorFunctions.h"
+
+namespace ASSET {
+
+
+	void LambertSolversBuild(FunctionRegistry& reg, py::module& m);
+
+
+	template<class Scalar, class WayBool, class IntType, class BranchBool, class ExitInt>
+	void lambert_izzo_impl(const Vector3<Scalar>& R1dim, const Vector3<Scalar>& R2dim, Scalar dtdim, double mu,
+		WayBool longway, IntType Nin, BranchBool rightbranch, 
+		Vector3<Scalar>& V1, Vector3<Scalar>& V2, ExitInt & exint) {
+
+
+		/// <summary>
+		/// An implementation of Dario Izzo's Lambert algorithm vectorized with Eigen::Array types.  2x increase in throughput with array size of 4 or 8.
+		/// </summary>
+		/// <returns></returns>
+
+		constexpr bool RisScalar = std::is_floating_point<Scalar>::value;
+		constexpr bool WayisScalar = std::is_same<WayBool, bool>::value;
+		constexpr bool BranchisScalar = std::is_same<BranchBool, bool>::value;
+		constexpr bool NisScalar = std::is_same<IntType, int>::value;
+
+		constexpr double Pi = 3.14159265358979;
+		constexpr double tol = 1.0e-11;
+		constexpr int maxiters = 20;
+
+
+		// Using Eigen's array types as scalars causes annoying issues w/ the cross and norm methods, rewriting here
+		auto cross = [](const Vector3<Scalar>& x1, const Vector3<Scalar>& x2, Vector3<Scalar>& out) {
+			out[0] = (x1[1] * x2[2] - x1[2] * x2[1]);
+			out[1] = (x2[0] * x1[2] - x2[2] * x1[0]);
+			out[2] = (x1[0] * x2[1] - x1[1] * x2[0]);
+		};
+
+		auto norm = [](const Vector3<Scalar>& x1) {
+			return Scalar(sqrt(x1.dot(x1)));
+		};
+
+
+		Scalar lstar = norm(R1dim);
+		Scalar vstar = sqrt(Scalar(mu) / lstar);
+		Scalar tstar = lstar / vstar;
+
+		Vector3<Scalar> R1 = R1dim / lstar;
+		Vector3<Scalar> R2 = R2dim / lstar;
+		Scalar dt = dtdim / tstar;
+
+
+		Scalar r2 = norm(R2);
+		Scalar logdt = log(dt);
+		Scalar theta = acos(Scalar(R1.dot(R2)) / r2);
+
+		/////////////////////////////////////////////////////////////////////////////////////////
+
+		Scalar lwsign(1.0);
+		Scalar branchsign(1.0);
+		bool NisSame = false;
+
+
+		if constexpr (WayisScalar) {
+			if (longway) {
+				theta = Scalar(2 * Pi - theta);
+				lwsign = Scalar(-1.0);
+			}
+		}
+		else {
+			for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+				if (longway[i]) {
+					theta[i] = 2.0 * Pi - theta[i];
+					lwsign[i] = -1.0;
+				}
+			}
+		}
+		if constexpr (BranchisScalar) {
+			if (rightbranch)branchsign = Scalar(-1.0);
+		}
+		else {
+			for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+				if (rightbranch[i])branchsign[i] = -1.0;
+			}
+		}
+
+		Scalar c = sqrt(1.0 + r2 * (r2 - 2.0 * cos(theta)));
+		Scalar s = (c + 1.0 + r2) / 2.0;
+		Scalar am = s / 2.0;
+		Scalar lambda = sqrt(r2) * cos(theta / 2.0) / s;
+		Scalar Nmax = floor(sqrt(Scalar(2.0) / (s * s * s)) * dt / Pi);
+
+		
+
+
+		Scalar x1, x2, N;
+
+		if constexpr (NisScalar) {
+			N = Scalar(double(Nin));
+			if (int(Nin) == 0) {
+				x1 = -0.740867916771357;
+				x2 = 0.4208790341605184;
+			}
+			else {
+				//N = std::max(N, Nmax);
+				if constexpr (BranchisScalar) {
+					if (rightbranch) {
+						x1 = Scalar(tan(.7234 * Pi / 2.0));
+						x2 = Scalar(tan(.5234 * Pi / 2.0));
+					}
+					else {
+						x1 = Scalar(tan(-.5234 * Pi / 2.0));
+						x2 = Scalar(tan(-.2234 * Pi / 2.0));
+					}
+				}
+				else {
+					for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+						if (rightbranch[i]) {
+							x1[i] = tan(.7234 * Pi / 2.0);
+							x2[i] = tan(.5234 * Pi / 2.0);
+						}
+						else {
+							x1[i] = tan(-.5234 * Pi / 2.0);
+							x2[i] = tan(-.2234 * Pi / 2.0);
+						}
+					}
+				}
+			}
+		}
+		else {
+			for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+				N[i] = Nin[i];
+				if (Nin[i] == 0) {
+					x1 = -0.740867916771357;
+					x2 = 0.4208790341605184;
+				}
+				else {
+					if constexpr (BranchisScalar) {
+						if (rightbranch) {
+							x1[i] = tan(.7234 * Pi / 2.0);
+							x2[i] = tan(.5234 * Pi / 2.0);
+						}
+						else {
+							x1[i] = tan(-.5234 * Pi / 2.0);
+							x2[i] = tan(-.2234 * Pi / 2.0);
+						}
+					}
+					else {
+						if (rightbranch[i]) {
+							x1[i] = tan(.7234 * Pi / 2.0);
+							x2[i] = tan(.5234 * Pi / 2.0);
+						}
+						else {
+							x1[i] = tan(-.5234 * Pi / 2.0);
+							x2[i] = tan(-.2234 * Pi / 2.0);
+						}
+					}
+				}
+			}
+		}
+
+
+		auto tofcon = [&](Scalar xin) {
+			Scalar x;
+			if constexpr (NisScalar) {
+				if (Nin == 0) {
+					x = exp(xin) - 1.0;
+				}
+				else {
+					x = atan(xin) * 2.0 / Pi;
+				}
+			}
+			else {
+				if (NisSame) {
+					if (Nin[0] == 0) {
+						x = exp(xin) - 1.0;
+					}
+					else {
+						x = atan(xin) * 2.0 / Pi;
+					}
+				}
+				else {
+					for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+						if (Nin[i] == 0) {
+							x[i] = exp(xin[i]);
+						}
+						else {
+							x[i] = atan(xin[i]) * 2.0 / Pi;
+						}
+					}
+				}
+			}
+
+			Scalar a = am / (1 - x * x);
+			Scalar alpha, beta, tof;
+
+			if constexpr (RisScalar) {
+				if (x < 1) {
+					alpha = 2 * acos(x);
+					beta = lwsign * 2 * asin(sqrt((s - c) / (2 * a)));
+				}
+				else {
+					alpha = 2 * acosh(x);
+					beta = lwsign * 2 * asinh(sqrt((s - c) / (-2 * a)));
+				}
+			}
+			else {
+				if (x.maxCoeff() < 1.0) {
+					alpha = 2 * acos(x);
+					beta = lwsign * 2 * asin(sqrt((s - c) / (2 * a)));
+				}
+				else if (x.minCoeff() > 1.0) {
+					alpha = 2 * acosh(x);
+					beta = lwsign * 2 * asinh(sqrt((s - c) / (-2 * a)));
+				}
+				else {
+					for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+						if (x[i] < 1) {
+							alpha[i] = 2 * acos(x[i]);
+							beta[i] = lwsign[i] * 2 * asin(sqrt((s[i] - c[i]) / (2 * a[i])));
+						}
+						else {
+							alpha[i] = 2 * acosh(x[i]);
+							beta[i] = lwsign[i] * 2 * asinh(sqrt((s[i] - c[i]) / (-2 * a[i])));
+						}
+					}
+				}
+			}
+
+
+			if constexpr (RisScalar) {
+				if (a > 0) {
+					tof = a * sqrt(a) * ((alpha - sin(alpha)) - (beta - sin(beta)) + 2 * Pi * N);
+				}
+				else {
+					tof = -a * sqrt(-a) * ((sinh(alpha) - alpha) - (sinh(beta) - beta));
+				}
+			}
+			else {
+				if (a.minCoeff() > 0.0) {
+					tof = a * sqrt(a) * ((alpha - sin(alpha)) - (beta - sin(beta)) + 2 * Pi * N);
+				}
+				else if (a.maxCoeff() < 0.0) {
+					tof = -a * sqrt(-a) * ((sinh(alpha) - alpha) - (sinh(beta) - beta));
+				}
+				else {
+					for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+						if (a[i] > 0) {
+							tof[i] = a[i] * sqrt(a[i]) * ((alpha[i] - sin(alpha[i])) - (beta[i] - sin(beta[i])) + 2 * Pi * N[i]);
+						}
+						else {
+							tof[i] = -a[i] * sqrt(-a[i]) * ((sinh(alpha[i]) - alpha[i]) - (sinh(beta[i]) - beta[i]));
+						}
+					}
+				}
+			}
+			Scalar y;
+			if constexpr (NisScalar) {
+				if (Nin == 0) {
+					y = log(tof) - logdt;
+				}
+				else {
+					y = tof - dt;
+				}
+			}
+			else {
+				if (NisSame) {
+					if (Nin[0] == 0) {
+						y = log(tof) - logdt;
+					}
+					else {
+						y = tof - dt;
+					}
+				}
+				else {
+					for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+						if (Nin[i] == 0) {
+							y[i] = log(tof[i]) - logdt[i];
+						}
+						else {
+							y[i] = tof[i] - dt[i];
+						}
+					}
+				}
+			}
+
+		
+			return y;
+		};
+
+
+
+		Scalar y1 = tofcon(x1);
+		Scalar y2 = tofcon(x2);
+
+		Scalar xn, yn, err;
+		int it = 0;
+
+		while (true) {
+
+			
+
+			xn = (x1 * y2 - y1 * x2) / (y2 - y1);
+			yn = tofcon(xn);
+			x1 = x2;
+			x2 = xn;
+			y1 = y2;
+			y2 = yn;
+			if constexpr (!RisScalar) {
+				y1 += Scalar(1.0e-15);
+			}
+
+			err = abs(x1 - xn);
+			if constexpr (RisScalar) {
+				if (err < tol) {
+					break;
+				}
+			}
+			else {
+				if (err.maxCoeff() < tol) {
+					break;
+				}
+			}
+
+			it++;
+			if (it > maxiters) break;
+		}
+
+
+
+		
+		if constexpr (RisScalar) {
+			if (err < tol) {
+				exint = 0;
+			}
+			else {
+				exint = 1;
+			}
+		}
+		else {
+			for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+				if (err[i] < tol) {
+					exint[i] = 0;
+				}
+				else {
+					exint[i] = 1;
+				}
+			}
+		}
+		
+
+		Scalar x;
+		if constexpr (NisScalar) {
+			if (Nin == 0) {
+				x = exp(xn) - 1.0;
+			}
+			else {
+				x = atan(xn) * 2.0 / Pi;
+			}
+		}
+		else {
+			if (NisSame) {
+				if (Nin[0] == 0) {
+					x = exp(xn) - 1.0;
+				}
+				else {
+					x = atan(xn) * 2.0 / Pi;
+				}
+			}
+			else {
+				for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+					if (Nin[i] == 0) {
+						x[i] = exp(xn[i]) - 1.0;
+					}
+					else {
+						x[i] = atan(xn[i]) * 2.0 / Pi;
+					}
+				}
+			}
+		}
+
+		
+
+
+		Scalar a = am / (1.0 - x * x);
+		Scalar alpha, beta, psi, eta, eta2;
+
+		if constexpr (RisScalar) {
+			if (x < 1) {
+				alpha = 2.0 * acos(x);
+				beta = lwsign * 2.0 * asin(sqrt((s - c) / (2.0 * a)));
+				psi = (alpha - beta) / 2.0;
+				eta2 = 2.0 * a * pow(sin(psi), 2) / s;
+				eta = sqrt(eta2);
+			}
+			else {
+				alpha = 2.0 * acosh(x);
+				beta = lwsign * 2.0 * asinh(sqrt((s - c) / (-2.0 * a)));
+				psi = (alpha - beta) / 2.0;
+				eta2 = -2.0 * a * pow(sinh(psi), 2) / s;
+				eta = sqrt(eta2);
+			}
+		}
+		else {
+			if (x.maxCoeff() < 1.0) {
+				alpha = 2.0 * acos(x);
+				beta = lwsign * 2.0 * asin(sqrt((s - c) / (2 * a)));
+				psi = (alpha - beta) / 2.0;
+				eta2 = 2.0 * a * pow(sin(psi), 2) / s;
+				eta = sqrt(eta2);
+			}
+			else if (x.minCoeff() > 1.0) {
+				alpha = 2.0 * acosh(x);
+				beta = lwsign * 2.0 * asinh(sqrt((s - c) / (-2 * a)));
+				psi = (alpha - beta) / 2.0;
+				eta2 = -2.0 * a * pow(sinh(psi), 2) / s;
+				eta = sqrt(eta2);
+			}
+			else {
+				for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
+					if (x[i] < 1) {
+						alpha[i] = 2.0 * acos(x[i]);
+						beta[i] = lwsign[i] * 2.0 * asin(sqrt((s[i] - c[i]) / (2.0 * a[i])));
+						psi[i] = (alpha[i] - beta[i]) / 2.0;
+						eta2[i] = 2 * a[i] * pow(sin(psi[i]), 2) / s[i];
+						eta[i] = sqrt(eta2[i]);
+					}
+					else {
+						alpha[i] = 2.0 * acosh(x[i]);
+						beta[i] = lwsign[i] * 2.0 * asinh(sqrt((s[i] - c[i]) / (-2.0 * a[i])));
+						psi[i] = (alpha[i] - beta[i]) / 2.0;
+						eta2[i] = -2.0 * a[i] * pow(sinh(psi[i]), 2) / s[i];
+						eta[i] = sqrt(eta2[i]);
+					}
+				}
+			}
+		}
+
+		/////////////////////////////////////////////////////////////////////////////////////////
+
+		R2 = R2 / r2;
+
+		Vector3<Scalar> nhat;
+		Vector3<Scalar> that1;
+		Vector3<Scalar> that2;
+
+		cross(R1, R2, nhat);
+		nhat = (nhat / norm(nhat)) * lwsign;
+
+		cross(nhat, R1, that1);
+		cross(nhat, R2, that2);
+
+		Scalar Vr1 = (1.0 / (eta * sqrt(am))) * (2.0 * lambda * am - lambda - x * eta);
+		Scalar Vt1 = sqrt((r2 / (am * eta2)) * pow(sin(theta / 2.0), 2));
+
+		Scalar Vt2 = Vt1 / r2;
+		Scalar Vr2 = (Vt1 - Vt2) / tan(theta / 2.0) - Vr1;
+
+		Vr1 *= vstar;
+		Vt1 *= vstar;
+		Vr2 *= vstar;
+		Vt2 *= vstar;
+
+
+
+		V1 = (Vr1 * R1 + Vt1 * that1);
+		V2 = (Vr2 * R2 + Vt2 * that2);
+
+	}
+	
+
+
+
+	
+
+	template<class Scalar>
+	std::array<Vector3<Scalar>, 2> lambert_izzo(const Vector3<Scalar>& R1, const Vector3<Scalar>& R2,
+		Scalar dt, double mu, bool longway) {
+
+		Vector3<Scalar> V1;
+		Vector3<Scalar> V2;
+		int exitcode;
+		lambert_izzo_impl(R1, R2, dt, mu,
+			longway,
+			0, false,
+			V1, V2,
+			exitcode);
+
+		return std::array<Vector3<Scalar>, 2>{ V1, V2 };
+	}
+
+	template<class Scalar>
+	std::array<Vector3<Scalar>, 2> lambert_izzo(const Vector3<Scalar>& R1, const Vector3<Scalar>& R2,
+		Scalar dt, double mu, bool longway, int Nrevs, bool rightbranch) {
+
+		Vector3<Scalar> V1;
+		Vector3<Scalar> V2;
+		int exitcode;
+		lambert_izzo_impl(R1, R2, dt, mu,
+			longway,
+			Nrevs, rightbranch,
+			V1, V2,
+			exitcode);
+
+		return std::array<Vector3<Scalar>, 2>{ V1, V2 };
+	}
+	
+
+	
+
+	
+ }
\ No newline at end of file
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b9eade5..91b1296 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -112,10 +112,8 @@ add_library(astro STATIC
   Astro/KeplerUtils.cpp
   Astro/KeplerModel.h 
   Astro/KeplerModel.cpp
-  Astro/LambertSolver.h 
-  Astro/LambertSolver.cpp
-  Astro/LambertSolverMultiRevTest.h 
-  Astro/LambertSolverMultiRevTest.cpp
+  Astro/LambertSolvers.h
+  Astro/LambertSolvers.cpp
   )
 
 target_precompile_headers(astro REUSE_FROM pch)
diff --git a/src/OptimalControl/LGLInterpTable.cpp b/src/OptimalControl/LGLInterpTable.cpp
index efa4ea1..fa5acb7 100644
--- a/src/OptimalControl/LGLInterpTable.cpp
+++ b/src/OptimalControl/LGLInterpTable.cpp
@@ -1,7 +1,5 @@
 #include "LGLInterpTable.h"
-#include "LGLInterpTable.h"
-#include "LGLInterpTable.h"
-#include "LGLInterpTable.h"
+
 
 
 void ASSET::LGLInterpTable::setMethod(TranscriptionModes m)
diff --git a/src/OptimalControl/ODE.h b/src/OptimalControl/ODE.h
index dd8c219..0ef9dd1 100644
--- a/src/OptimalControl/ODE.h
+++ b/src/OptimalControl/ODE.h
@@ -143,7 +143,10 @@ struct GenericODE
         const std::vector<Eigen::VectorXd>& Traj, int numdef) {
             return std::make_shared<ODEPhase<Derived>>(od, Tmode, Traj, numdef);
         });
-
+    obj.def("phase", [](const Derived& od, std::string Tmode,
+        const std::vector<Eigen::VectorXd>& Traj, int numdef,bool LerpIG) {
+            return std::make_shared<ODEPhase<Derived>>(od, Tmode, Traj, numdef, LerpIG);
+        });
     
 
     py::implicitly_convertible<Derived, GenericFunction<-1, -1>>();
diff --git a/src/OptimalControl/ODEPhase.h b/src/OptimalControl/ODEPhase.h
index fadccfb..c8e9ca5 100644
--- a/src/OptimalControl/ODEPhase.h
+++ b/src/OptimalControl/ODEPhase.h
@@ -52,6 +52,11 @@ struct ODEPhase : ODEPhaseBase {
       : ODEPhase(ode, Tmode) {
     this->setTraj(Traj, numdef);
   }
+  ODEPhase(const DODE& ode, TranscriptionModes Tmode,
+      const std::vector<Eigen::VectorXd>& Traj, int numdef, bool LerpIG)
+      : ODEPhase(ode, Tmode) {
+      this->setTraj(Traj, numdef,LerpIG);
+  }
 
   ODEPhase(const DODE& ode, std::string Tmode)
       : ODEPhase(ode, strto_TranscriptionMode(Tmode)) {
@@ -62,6 +67,10 @@ struct ODEPhase : ODEPhaseBase {
       :ODEPhase(ode, strto_TranscriptionMode(Tmode),Traj,  numdef) {
      
   }
+  ODEPhase(const DODE& ode, std::string Tmode,
+      const std::vector<Eigen::VectorXd>& Traj, int numdef,bool LerpIG)
+      :ODEPhase(ode, strto_TranscriptionMode(Tmode), Traj, numdef,LerpIG) {
+  }
 
   virtual void setTranscriptionMode(TranscriptionModes m) {
     this->resetTranscription();
@@ -535,9 +544,14 @@ struct ODEPhase : ODEPhaseBase {
       phase.def(py::init<DODE, TranscriptionModes>());
       phase.def(py::init<DODE, TranscriptionModes,
           const std::vector<Eigen::VectorXd>&, int>());
+      phase.def(py::init<DODE, TranscriptionModes,
+          const std::vector<Eigen::VectorXd>&, int,bool>());
+
       phase.def(py::init<DODE, std::string>());
       phase.def(py::init<DODE, std::string,
           const std::vector<Eigen::VectorXd>&, int>());
+      phase.def(py::init<DODE, std::string,
+          const std::vector<Eigen::VectorXd>&, int,bool>());
   }
 
   static void Build(py::module& m) {
diff --git a/src/OptimalControl/ODEPhaseBase.cpp b/src/OptimalControl/ODEPhaseBase.cpp
index 1652c89..30bbf91 100644
--- a/src/OptimalControl/ODEPhaseBase.cpp
+++ b/src/OptimalControl/ODEPhaseBase.cpp
@@ -1,12 +1,4 @@
 #include "ODEPhaseBase.h"
-#include "ODEPhaseBase.h"
-#include "ODEPhaseBase.h"
-#include "ODEPhaseBase.h"
-#include "ODEPhaseBase.h"
-#include "ODEPhaseBase.h"
-#include "ODEPhaseBase.h"
-#include "ODEPhaseBase.h"
-
 #include "LGLControlSplines.h"
 #include "LGLIntegrals.h"
 #include "MeshSpacingConstraints.h"
@@ -489,7 +481,7 @@ std::vector<Eigen::VectorXd> ASSET::ODEPhaseBase::returnCostateTraj() const {
 }
 
 void ASSET::ODEPhaseBase::setTraj(const std::vector<Eigen::VectorXd>& mesh,
-                                  Eigen::VectorXd DBS, Eigen::VectorXi DPB) {
+                                  Eigen::VectorXd DBS, Eigen::VectorXi DPB, bool LerpTraj) {
 
   if (mesh.size() == 0) {
         throw std::invalid_argument("Input trajectory is empty");
@@ -516,12 +508,60 @@ void ASSET::ODEPhaseBase::setTraj(const std::vector<Eigen::VectorXd>& mesh,
   }
   for (auto& X : mesh) {
       if (X.hasNaN()) {
-          throw std::invalid_argument("NaN detetected in State Vector in ODEPhaseBase::setTraj");
+          throw std::invalid_argument("NaN detected in State Vector in ODEPhaseBase::setTraj");
       }
   }
 
+
+ 
   this->Table.loadUnevenData(DPB.sum(), mesh);
-  this->ActiveTraj = this->Table.NDdistribute(DBS, DPB);
+
+  if (LerpTraj) {
+
+      double t0 = mesh[0][this->TVar()];
+      double tf = mesh.back()[this->TVar()];
+      Eigen::VectorXd intime_nd(mesh.size());
+      for (int i = 0; i < mesh.size(); i++) {
+          intime_nd[i] = (mesh[i][this->TVar()] - t0) / (tf - t0);
+      }
+        
+      int nstates = (this->numTranCardStates - 1) * DPB.sum() + 1;
+      Eigen::VectorXd outtime_nd(nstates);
+      outtime_nd.setZero();
+
+      Eigen::VectorXd Tspacing = this->Table.Tspacing;
+      Eigen::VectorXd cvect(Tspacing.size());
+
+      this->ActiveTraj.resize(nstates);
+
+      for (int i = 0,start=0; i < DBS.size() - 1;i++) {
+          double bint0 = DBS[i];
+          double bintf = DBS[i+1];
+          double segdt = (bintf - bint0) / DPB[i];
+          for (int j = 0; j < DPB[i]; j++) {
+              cvect.setConstant(outtime_nd[start]);
+              outtime_nd.segment(start, Tspacing.size()) = cvect + Tspacing * segdt;
+              start += (this->numTranCardStates - 1);
+          }
+      }
+
+
+      for (int i = 0, elem=0; i < nstates; i++) {
+          double ti = outtime_nd[i];
+          auto it = std::upper_bound(intime_nd.cbegin() + elem, intime_nd.cend(), ti);
+          elem = int(it -intime_nd.cbegin()) - 1;
+          elem = std::clamp(elem, 0, int(intime_nd.size() - 2));
+          double helem = intime_nd[elem + 1] - intime_nd[elem];
+          double tndlocal = (ti - intime_nd[elem]) / (helem);
+          this->ActiveTraj[i] = mesh[elem] * (1.0 - tndlocal) + mesh[elem + 1] * tndlocal;
+      }
+
+
+  }
+  else {
+      this->ActiveTraj = this->Table.NDdistribute(DBS, DPB);
+  }
+
   this->DefBinSpacing = DBS;
   this->DefsPerBin = DPB;
   this->numDefects = DPB.sum();
@@ -529,6 +569,8 @@ void ASSET::ODEPhaseBase::setTraj(const std::vector<Eigen::VectorXd>& mesh,
   this->resetTranscription();
 }
 
+
+
 void ASSET::ODEPhaseBase::refineTrajManual(VectorXd DBS, VectorXi DPB) {
   if ((DBS.size() - 1) != DPB.size()) {
     std::cout << "User Input Error in function setInitTraj for ODE:"
@@ -1302,11 +1344,22 @@ void ASSET::ODEPhaseBase::Build(py::module& m) {
       py::overload_cast<const std::vector<Eigen::VectorXd>&, Eigen::VectorXd,
                         Eigen::VectorXi>(&ODEPhaseBase::setTraj),
       ODEPhaseBase_setTraj1);
+
+  obj.def(
+      "setTraj",
+      py::overload_cast<const std::vector<Eigen::VectorXd>&, Eigen::VectorXd,
+      Eigen::VectorXi,bool>(&ODEPhaseBase::setTraj));
+
   obj.def("setTraj",
           py::overload_cast<const std::vector<Eigen::VectorXd>&, int>(
               &ODEPhaseBase::setTraj),
           ODEPhaseBase_setTraj2);
 
+  obj.def("setTraj",
+      py::overload_cast<const std::vector<Eigen::VectorXd>&, int,bool>(
+          &ODEPhaseBase::setTraj));
+
+
   obj.def("switchTranscriptionMode",
           py::overload_cast<TranscriptionModes, VectorXd, VectorXi>(
               &ODEPhaseBase::switchTranscriptionMode),
diff --git a/src/OptimalControl/ODEPhaseBase.h b/src/OptimalControl/ODEPhaseBase.h
index 5680745..14e9efe 100644
--- a/src/OptimalControl/ODEPhaseBase.h
+++ b/src/OptimalControl/ODEPhaseBase.h
@@ -99,8 +99,8 @@ struct ODEPhaseBase : ODESize<-1, -1, -1>, OptimizationProblemBase {
   double RelSwitchTol = .3;
   double AbsSwitchTol = 1.0e-6;
 
-
   double MeshTol = 1.0e-6;
+
   std::string MeshErrorEstimator    = "deboor";
   std::string MeshErrorCriteria     = "max" ;  //"max,avg,geometric,endtoend"
   std::string MeshErrorDistributor  = "avg";  // "max,avg,geometric,endtoend"
@@ -712,16 +712,26 @@ struct ODEPhaseBase : ODESize<-1, -1, -1>, OptimizationProblemBase {
   /////////////////////////////////////////////////
 
   virtual void setTraj(const std::vector<Eigen::VectorXd>& mesh, Eigen::VectorXd DBS,
-               Eigen::VectorXi DPB);
+               Eigen::VectorXi DPB,bool LerpTraj);
+
+  void setTraj(const std::vector<Eigen::VectorXd>& mesh, Eigen::VectorXd DBS,
+      Eigen::VectorXi DPB) {
+      this->setTraj(mesh, DBS, DPB, false);
+  }
 
+  void setTraj(const std::vector<Eigen::VectorXd>& mesh, int ndef,bool LerpTraj) {
+      VectorXd dbs(2);
+      dbs[0] = 0.0;
+      dbs[1] = 1.0;
+      VectorXi dpb(1);
+      dpb[0] = ndef;
+      this->setTraj(mesh, dbs, dpb,LerpTraj);
+  }
   void setTraj(const std::vector<Eigen::VectorXd>& mesh, int ndef) {
-    VectorXd dbs(2);
-    dbs[0] = 0.0;
-    dbs[1] = 1.0;
-    VectorXi dpb(1);
-    dpb[0] = ndef;
-    this->setTraj(mesh, dbs, dpb);
+      this->setTraj(mesh, ndef,false);
   }
+
+  
   void refineTrajManual(VectorXd DBS, VectorXi DPB);
 
   void refineTrajManual(int ndef) {
diff --git a/src/OptimalControl/OptimalControlProblem.cpp b/src/OptimalControl/OptimalControlProblem.cpp
index d2700d4..d30315f 100644
--- a/src/OptimalControl/OptimalControlProblem.cpp
+++ b/src/OptimalControl/OptimalControlProblem.cpp
@@ -1,7 +1,4 @@
 #include "OptimalControlProblem.h"
-#include "OptimalControlProblem.h"
-#include "OptimalControlProblem.h"
-#include "OptimalControlProblem.h"
 
 #include "PyDocString/OptimalControl/OptimalControlProblem_doc.h"
 
diff --git a/src/VectorFunctions/ASSET_VectorFunctions.cpp b/src/VectorFunctions/ASSET_VectorFunctions.cpp
index fbc5762..2f21bae 100644
--- a/src/VectorFunctions/ASSET_VectorFunctions.cpp
+++ b/src/VectorFunctions/ASSET_VectorFunctions.cpp
@@ -4,6 +4,7 @@
 
 #include "CommonFunctions/InterpTable2D.h"
 #include "CommonFunctions/InterpTable1D.h"
+#include "CommonFunctions/InterpTable3D.h"
 
 namespace ASSET {
 
@@ -54,6 +55,7 @@ void ASSET::VectorFunctionBuild(FunctionRegistry& reg, py::module& m) {
 
   InterpTable1DBuild(mod);
   InterpTable2DBuild(mod);
+  InterpTable3DBuild(mod);
 
   mod.def("ScalarDynamicStackTest", [](const std::vector<GenericFunction<-1, 1>>& funcs) {
 
diff --git a/src/VectorFunctions/CommonFunctions/InterpTable1D.h b/src/VectorFunctions/CommonFunctions/InterpTable1D.h
index e49c33f..70ccdff 100644
--- a/src/VectorFunctions/CommonFunctions/InterpTable1D.h
+++ b/src/VectorFunctions/CommonFunctions/InterpTable1D.h
@@ -107,20 +107,20 @@ namespace ASSET {
 			vlen  = vs.rows();
 			ttotal = ts[tsize - 1] - ts[0];
 
-			if (tsize < 3) {
-				throw std::invalid_argument("t coordinates must be larger than 2");
+			if (tsize < 5) {
+				throw std::invalid_argument("t coordinates must be larger than 4");
 			}
 			if (tsize != vs.cols()) {
 				throw std::invalid_argument("Length of t coordinates must match length of interpolation axis");
 			}
-			for (int i = 0; i < ts.size() - 1; i++) {
+			for (int i = 0; i < tsize - 1; i++) {
 				if (ts[i + 1] < ts[i]) {
 					throw std::invalid_argument("t Coordinates must be in ascending order");
 				}
 			}
 
 			Eigen::VectorXd testt;
-			testt.setLinSpaced(ts.size(), ts[0], ts[ts.size() - 1]);
+			testt.setLinSpaced(ts.size(), ts[0], ts[tsize - 1]);
 			
 			double terr = (ts - testt).lpNorm<Eigen::Infinity>();
 
@@ -133,63 +133,54 @@ namespace ASSET {
 
 		}
 
-		void calc_derivs() {
-			
-			dvs_dts.resize(vlen, tsize);
-
-
-			Eigen::Matrix3d stens;
-			stens << 1, 1, 1,
-				-1, 0, 1,
-				-1, 0, 1;
-
-			Eigen::Vector3d rhs;
-			rhs << 0, 1, 0;
-
-			Eigen::Vector3d coeffs;
-
-			for (int i = 0; i < tsize; i++) {
-				
-					if (i == 0) {
-						double tstep1 = ts[i + 1] - ts[i];
-
-						if (this->teven) {
-							dvs_dts.col(i) = (-.5*vs.col(i + 2)+2.0*vs.col(i + 1) - 1.5*vs.col(i)) / tstep1;
-
-						}
-						else {
-							dvs_dts.col(i) = (vs.col(i + 1) - vs.col(i)) / tstep1;
-						}
-					}
-					else if (i == tsize - 1) {
-						double tstep1 = ts[i] - ts[i - 1];
-						if (this->teven) {
-							dvs_dts.col(i) = (1.5 * vs.col(i) - 2.0 * vs.col(i - 1) + .5 * vs.col(i - 2)) / tstep1;
-						}
-						else {
-							dvs_dts.col(i) = (vs.col(i) -  vs.col(i - 1) ) / tstep1;
-						}
-						
-					}
-					else {
-						double tstep1 = ts[i + 1] - ts[i];
 
-						if (this->teven) {
-							dvs_dts.col(i) = (vs.col(i + 1) - vs.col(i - 1)) / (2 * tstep1);
-						}
-						else {
-							double tstep2 = ts[i] - ts[i - 1];
-							double tn = -1.0 * tstep2 / tstep1;
-
-							stens(1, 0) = tn;
-							stens(2, 0) = tn * tn;
-							coeffs = stens.inverse() * rhs;
-
-							dvs_dts.col(i) = (coeffs[0] * vs.col(i - 1) + coeffs[1] * vs.col(i) + coeffs[2] * vs.col(i + 1)) / tstep1;
+		void calc_derivs() {
 
-						}
+			this->dvs_dts.resize(this->vlen, this->tsize);
+
+			Eigen::Matrix<double,5,5> stens;
+			stens.row(0).setOnes();
+			Eigen::Matrix<double, 5, 1> rhs;
+			rhs << 0,1,0,0,0;
+			Eigen::Matrix<double, 5, 1> times;
+			Eigen::Matrix<double, 5, 1> coeffs;
+
+			bool hitcent = false;
+			for (int i = 0; i < this->tsize; i++) {
+				int start = 0;
+				bool recalc = true;
+				if (i + 2 <= this->tsize - 1 && i - 2 >= 0) {
+					// central difference
+					if (this->teven && hitcent) {
+						recalc = false;
 					}
+					hitcent = true;
+					start = i - 2;
+				}
+				else if (i < this->tsize-1-i) {
+					// forward difference
+					start = 0;
+				}
+				else {
+					// backward difference
+					start = this->tsize - 5;
+				}
+				int stepdir = (i<this->tsize-1)? 1:-1;
+				double ti = this->ts[i];
+				double tstep = std::abs(this->ts[i + stepdir] - ti);
+				if (recalc) {
+					times = this->ts.segment(start, 5);
+					times -= Eigen::Matrix<double, 5, 1>::Constant(ti);
+					times /= tstep;
+					stens.row(1) = times.transpose();
+					stens.row(2) = stens.row(1).cwiseProduct(times.transpose());
+					stens.row(3) = stens.row(2).cwiseProduct(times.transpose());
+					stens.row(4) = stens.row(3).cwiseProduct(times.transpose());
+					coeffs = stens.inverse() * rhs;
+				}
 				
+				dvs_dts.col(i) = this->vs.middleCols(start, 5) * (coeffs / tstep);
+
 			}
 
 		}
@@ -208,10 +199,6 @@ namespace ASSET {
 				telem = int(it - ts.begin()) - 1;
 			}
 
-
-			double tstep = ts[telem + 1] - ts[telem];
-			double tnd = (t - ts[telem]) / tstep;
-
 			telem = std::min(telem, this->tsize - 2);
 			telem = std::max(telem, 0);
 			return telem;
@@ -224,7 +211,7 @@ namespace ASSET {
 				double eps = std::numeric_limits<double>::epsilon() * ttotal;
 				if (t<(ts[0]-eps) || t>(ts[ts.size() - 1]+eps)) {
 					fmt::print(fmt::fg(fmt::color::red),
-						"WARNING: t falls outside of InterpTable1D time range. Data is being extrapolated!!\n");
+						"WARNING: t= {0:} falls outside of InterpTable1D time range. Data is being extrapolated!!\n",t);
 					if (ThrowOutOfBounds) {
 						throw std::invalid_argument("");
 					}
diff --git a/src/VectorFunctions/CommonFunctions/InterpTable2D.h b/src/VectorFunctions/CommonFunctions/InterpTable2D.h
index 8313b6b..43d6718 100644
--- a/src/VectorFunctions/CommonFunctions/InterpTable2D.h
+++ b/src/VectorFunctions/CommonFunctions/InterpTable2D.h
@@ -1,6 +1,6 @@
 #pragma once
 #include "VectorFunction.h"
-
+#include "Utils/Timer.h"
 
 namespace ASSET {
 
@@ -21,6 +21,9 @@ namespace ASSET {
 		MatType dzys;
 		MatType dzys_dxs;
 
+		Eigen::Matrix<Eigen::Array4d, -1, -1, Eigen::RowMajor> all_dat;
+
+
 		bool WarnOutOfBounds = true;
 		bool ThrowOutOfBounds = false;
 
@@ -32,7 +35,6 @@ namespace ASSET {
 		int ysize;
 		double ytotal;
 
-
 		InterpTable2D(){}
 
 		InterpTable2D(const Eigen::VectorXd & Xs, const Eigen::VectorXd& Ys,
@@ -64,11 +66,11 @@ namespace ASSET {
 			xsize = xs.size();
 			ysize = ys.size();
 
-			if (xsize < 3) {
-				throw std::invalid_argument("X coordinates must be larger than 2");
+			if (xsize < 5) {
+				throw std::invalid_argument("X coordinates must be larger than 4");
 			}
-			if (ysize < 3) {
-				throw std::invalid_argument("Y  coordinates must be larger than 2");
+			if (ysize < 5) {
+				throw std::invalid_argument("Y  coordinates must be larger than 4");
 			}
 
 			if (xsize != zs.cols()) {
@@ -117,89 +119,96 @@ namespace ASSET {
 			dzxs.resize(ysize, xsize);
 			dzys.resize(ysize, xsize);
 			dzys_dxs.resize(ysize, xsize);
-			Eigen::Matrix<double, -1, -1, Eigen::RowMajor> dzys_dxs_tmp(ysize, xsize);
-
-
-			Eigen::Matrix3d stens;
-			stens << 1, 1, 1,
-				-1, 0, 1,
-				-1, 0, 1;
-
-			Eigen::Vector3d rhs;
-			rhs << 0, 1, 0;
-
-			Eigen::Vector3d coeffs;
+			all_dat.resize(ysize, xsize);
 
-			for (int i = 0; i < ysize; i++) {
-				for (int j = 0; j < xsize; j++) {
-					if (i == 0) {
-						double ystep1 = ys[i + 1] - ys[i];
-						dzys(i, j) = (zs(i + 1, j) - zs(i, j)) / ystep1;
-					}
-					else if (i == ysize - 1) {
-						double ystep1 = ys[i] - ys[i-1];
-						dzys(i, j) = (zs(i, j) - zs(i - 1, j)) / ystep1;
-					}
-					else {
-						double ystep1 = ys[i + 1] - ys[i];
-						
-						if (this->yeven) {
-							dzys(i, j) = (zs(i + 1, j) - zs(i - 1, j)) / (2 * ystep1);
-						}
-						else {
-							double ystep2 = ys[i] - ys[i - 1];
-							double yn = -1.0 * ystep2 / ystep1;
 
-							stens(1, 0) = yn;
-							stens(2, 0) = yn * yn;
-							coeffs = stens.inverse() * rhs;
+			Eigen::Matrix<double, 5, 5> stens;
+			stens.row(0).setOnes();
+			Eigen::Matrix<double, 5, 1> rhs;
+			rhs << 0, 1, 0, 0, 0;
+			Eigen::Matrix<double, 5, 1> times;
+			Eigen::Matrix<double, 5, 1> coeffs;
 
-							dzys(i, j) = (coeffs[0] * zs(i - 1, j) + coeffs[1] * zs(i, j) + coeffs[2] * zs(i + 1, j)) / ystep1;
 
-						}
+			bool hitcent = false;
+			for (int i = 0; i < this->ysize; i++) {
+				int start = 0;
+				bool recalc = true;
+				if (i + 2 <= this->ysize - 1 && i - 2 >= 0) {
+					// central difference
+					if (this->yeven && hitcent) {
+						recalc = false;
 					}
+					hitcent = true;
+					start = i - 2;
+				}
+				else if (i < this->ysize - 1 - i) {
+					// forward difference
+					start = 0;
 				}
+				else {
+					// backward difference
+					start = this->ysize - 5;
+				}
+				int stepdir = (i < this->ysize - 1) ? 1 : -1;
+				double yi = this->ys[i];
+				double ystep = std::abs(this->ys[i + stepdir] - yi);
+				if (recalc) {
+					times = this->ys.segment(start, 5);
+					times -= Eigen::Matrix<double, 5, 1>::Constant(yi);
+					times /= ystep;
+					stens.row(1) = times.transpose();
+					stens.row(2) = stens.row(1).cwiseProduct(times.transpose());
+					stens.row(3) = stens.row(2).cwiseProduct(times.transpose());
+					stens.row(4) = stens.row(3).cwiseProduct(times.transpose());
+					coeffs = stens.inverse() * rhs;
+				}
+				dzys.row(i) = (coeffs / ystep).transpose() * this->zs.middleRows(start, 5);
 			}
+			
 
-			for (int i = 0; i < ysize; i++) {
-				for (int j = 0; j < xsize; j++) {
-					if (j == 0) {
-						double xstep1 = xs[j + 1] - xs[j];
-
-						dzxs(i, j) = (zs(i, j + 1) - zs(i, j)) / xstep1;
-						dzys_dxs(i, j) = (dzys(i, j + 1) - dzys(i, j)) / xstep1;
-					}
-					else if (j == xsize - 1) {
-						double xstep1 = xs[j] - xs[j-1];
-
-						dzxs(i, j) = (zs(i, j) - zs(i, j - 1)) / xstep1;
-						dzys_dxs(i, j) = (dzys(i, j) - dzys(i, j - 1)) / xstep1;
+			hitcent = false;
+			for (int i = 0; i < this->xsize; i++) {
+				int start = 0;
+				bool recalc = true;
+				if (i + 2 <= this->xsize - 1 && i - 2 >= 0) {
+					// central difference
+					if (this->xeven && hitcent) {
+						recalc = false;
 					}
-					else {
-						double xstep1 = xs[j + 1] - xs[j];
-
-						if (this->xeven) {
-							dzxs(i, j) = (zs(i, j + 1) - zs(i, j - 1)) / (2 * xstep1);
-							dzys_dxs(i, j) = (dzys(i, j + 1) - dzys(i, j - 1)) / (2 * xstep1);
-						}
-						else {
-
-							double xstep2 = xs[j] - xs[j - 1];
-							double xn = -1.0 * xstep2 / xstep1;
-
-							stens(1, 0) = xn;
-							stens(2, 0) = xn * xn;
-							coeffs = stens.inverse() * rhs;
+					hitcent = true;
+					start = i - 2;
+				}
+				else if (i < this->xsize - 1 - i) {
+					// forward difference
+					start = 0;
+				}
+				else {
+					// backward difference
+					start = this->xsize - 5;
+				}
+				int stepdir = (i < this->xsize - 1) ? 1 : -1;
+				double xi = this->xs[i];
+				double xstep = std::abs(this->xs[i + stepdir] - xi);
+				if (recalc) {
+					times = this->xs.segment(start, 5);
+					times -= Eigen::Matrix<double, 5, 1>::Constant(xi);
+					times /= xstep;
+					stens.row(1) = times.transpose();
+					stens.row(2) = stens.row(1).cwiseProduct(times.transpose());
+					stens.row(3) = stens.row(2).cwiseProduct(times.transpose());
+					stens.row(4) = stens.row(3).cwiseProduct(times.transpose());
+					coeffs = stens.inverse() * rhs;
+				}
 
-							dzxs(i, j)    = (coeffs[0] * zs(i, j - 1)    + coeffs[1] * zs(i, j )   + coeffs[2] * zs(i, j + 1)) / (xstep1);
-							dzys_dxs(i, j) = (coeffs[0] * dzys(i, j - 1) + coeffs[1] * dzys(i, j)  + coeffs[2] * dzys(i, j + 1)) / (xstep1);
+				dzxs.col(i) = this->zs.middleCols(start, 5) * (coeffs / xstep);
+				dzys_dxs.col(i) = this->dzys.middleCols(start, 5) * (coeffs / xstep);
 
-						}
-					}
-				}
 			}
+
+			
 		}
-		
+
 
 		int find_elem(const Eigen::VectorXd& vs, double v) const {
 			int center = int(vs.size() / 2);
@@ -213,8 +222,8 @@ namespace ASSET {
 			int xelem, yelem;
 
 			if (this->xeven) {
-				double xlocal = x - xs[0];
-				double xstep = xs[1] - xs[0];
+				double xlocal = x - this->xs[0];
+				double xstep = this->xs[1] - this->xs[0];
 				xelem = std::min(int(xlocal / xstep), this->xsize - 2);
 			}
 			else {
@@ -260,12 +269,13 @@ namespace ASSET {
 
 			Eigen::Matrix4<double> Z;
 
+			
 			double z00 = zs(yelem, xelem);
-			double z10 = zs(yelem, xelem+1);
-			double z01 = zs(yelem+1, xelem);
-			double z11 = zs(yelem+1, xelem+1);
+			double z10 = zs(yelem, xelem + 1);
+			double z01 = zs(yelem + 1, xelem);
+			double z11 = zs(yelem + 1, xelem + 1);
 
-			double dz00_x = dzxs(yelem, xelem)*xstep;
+			double dz00_x = dzxs(yelem, xelem) * xstep;
 			double dz10_x = dzxs(yelem, xelem + 1) * xstep;
 			double dz01_x = dzxs(yelem + 1, xelem) * xstep;
 			double dz11_x = dzxs(yelem + 1, xelem + 1) * xstep;
@@ -287,6 +297,9 @@ namespace ASSET {
 				z10, z11, dz10_y, dz11_y,
 				dz00_x, dz01_x, dz00_xy, dz01_xy,
 				dz10_x, dz11_x, dz10_xy, dz11_xy;
+			
+
+			
 
 			a = L * Z * R;
 			return a;
@@ -301,7 +314,7 @@ namespace ASSET {
 			if (WarnOutOfBounds || ThrowOutOfBounds) {
 				double xeps = std::numeric_limits<double>::epsilon() * xtotal;
 				if (x<(xs[0] - xeps) || x>(xs[xs.size() - 1] + xeps)) {
-					fmt::print("{0}\n", x);
+					
 					fmt::print(fmt::fg(fmt::color::red),
 						"WARNING: x coordinate falls outside of InterpTable2D range. Data is being extrapolated!!\n");
 					if (ThrowOutOfBounds) {
@@ -453,7 +466,7 @@ namespace ASSET {
 
 		InterpFunction2D() {}
 		InterpFunction2D(std::shared_ptr<InterpTable2D> tab) :tab(tab) {
-			this->setIORows(1, 1);
+			this->setIORows(2, 1);
 		}
 
 
@@ -499,9 +512,6 @@ namespace ASSET {
 
 		}
 
-
-
-
 	};
 
 	
@@ -529,6 +539,8 @@ namespace ASSET {
 		obj.def("__call__", py::overload_cast<double,double>(&InterpTable2D::interp, py::const_), py::is_operator());
 		obj.def("__call__", py::overload_cast<const MatType&, const MatType&>(&InterpTable2D::interp, py::const_), py::is_operator());
 
+
+
 		obj.def("__call__", [](const InterpTable2D& self, const GenericFunction<-1, 1>& x, const GenericFunction<-1, 1>& y) {
 			return GenericFunction<-1, 1>(InterpFunction2D(std::make_shared<InterpTable2D>(self)).eval(stack(x,y)));
 			});
@@ -537,6 +549,15 @@ namespace ASSET {
 			return GenericFunction<-1, 1>(InterpFunction2D(std::make_shared<InterpTable2D>(self)).eval(stack(x, y)));
 			});
 
+		obj.def("__call__", [](const InterpTable2D& self, const Segment<-1, 2, -1>& xy) {
+			return GenericFunction<-1, 1>(InterpFunction2D(std::make_shared<InterpTable2D>(self)).eval(xy));
+			});
+
+		obj.def("__call__", [](const InterpTable2D& self, const GenericFunction<-1, -1>& xy) {
+			return GenericFunction<-1, 1>(InterpFunction2D(std::make_shared<InterpTable2D>(self)).eval(xy));
+			});
+
+
 		obj.def("sf", [](const InterpTable2D& self) {
 			return GenericFunction<-1, 1>(InterpFunction2D(std::make_shared<InterpTable2D>(self)));
 			});
@@ -545,7 +566,59 @@ namespace ASSET {
 			});
 
 
+		m.def("InterpTable2DSpeedTest", [](const GenericFunction<-1, 1> & tabf,double xl,double xu,double yl,double yu, int nsamps,bool lin) {
+
+			Eigen::ArrayXd xsamps;
+			xsamps.setRandom(nsamps);
+			xsamps +=1;
+			xsamps /= 2;
+			xsamps *= (xu - xl);
+			xsamps += xl;
 
+			Eigen::ArrayXd ysamps;
+			ysamps.setRandom(nsamps);
+			ysamps += 1;
+			ysamps /= 2;
+			ysamps *= (yu - yl);
+			ysamps += yl;
+
+			if (lin) {
+				xsamps.setLinSpaced(xl, xu);
+				ysamps.setLinSpaced(yl, yu);
+			}
+
+
+			Eigen::VectorXd xy(2);
+			Vector1<double> f;
+			f.setZero();
+
+			Utils::Timer Runtimer;
+			Runtimer.start();
+
+			double tmp = 0;
+			for (int i = 0; i < nsamps; i++) {
+
+				
+				xy[0] = xsamps[i];
+				xy[1] = ysamps[i];
+
+				tabf.compute(xy, f);
+				tmp += f[0] / double(i + 3);
+
+				//fmt::print("{0:} \n",f[0]);
+
+
+				f.setZero();
+
+
+			}
+			Runtimer.stop();
+			double tseconds = double(Runtimer.count<std::chrono::microseconds>()) / 1000000;
+			fmt::print("Total Time: {0:} ms \n", tseconds*1000);
+
+
+			return tmp;
+			});
 
 	}
 
diff --git a/src/VectorFunctions/CommonFunctions/InterpTable3D.h b/src/VectorFunctions/CommonFunctions/InterpTable3D.h
new file mode 100644
index 0000000..e20c3d1
--- /dev/null
+++ b/src/VectorFunctions/CommonFunctions/InterpTable3D.h
@@ -0,0 +1,831 @@
+#pragma once
+#include "VectorFunction.h"
+#include <unsupported/Eigen/CXX11/Tensor>
+#include <pybind11/eigen/tensor.h>
+namespace ASSET {
+
+	struct InterpTable3D {
+
+		enum class InterpType {
+			cubic_interp,
+			linear_interp
+		};
+
+		Eigen::VectorXd xs;
+		Eigen::VectorXd ys;
+		Eigen::VectorXd zs;
+
+        // numpy meshgrid ij format (x,y,z)
+        Eigen::Tensor<double, 3> fs;
+
+        Eigen::Tensor<double, 3> fs_dx;
+        Eigen::Tensor<double, 3> fs_dy;
+        Eigen::Tensor<double, 3> fs_dz;
+
+        Eigen::Tensor<double, 3> fs_dxdy;
+        Eigen::Tensor<double, 3> fs_dxdz;
+        Eigen::Tensor<double, 3> fs_dydz;
+
+        Eigen::Tensor<double, 3> fs_dxdydz;
+
+        Eigen::Tensor<Eigen::Matrix<double, 64, 1>, 3> alphavecs;
+
+		Eigen::MatrixXd Cmat;
+
+        InterpType interp_kind = InterpType::linear_interp;
+
+		bool xeven = true;
+		bool yeven = true;
+		bool zeven = true;
+
+		int xsize;
+		double xtotal;
+		int ysize;
+		double ytotal;
+		int zsize;
+		double ztotal;
+        bool cache_alpha=false;
+        int cache_threads = 1;
+
+        bool WarnOutOfBounds = true;
+        bool ThrowOutOfBounds = false;
+
+        InterpTable3D() {}
+
+        InterpTable3D(const Eigen::VectorXd& Xs, const Eigen::VectorXd& Ys, const Eigen::VectorXd& Zs,
+            const Eigen::Tensor<double, 3>& Fs, std::string kind,bool cache ) {
+
+            this->xs = Xs;
+            this->ys = Ys;
+            this->zs = Zs;
+            this->fs = Fs;
+            this->cache_alpha = cache;
+
+            /// <summary>
+            /// ///////////////////////////////////////////
+            /// </summary>
+            if (kind == "cubic" || kind == "Cubic") {
+                this->interp_kind = InterpType::cubic_interp;
+            }
+            else if (kind == "linear" || kind == "Linear") {
+                this->interp_kind = InterpType::linear_interp;
+            }
+            else {
+                throw std::invalid_argument("Unrecognized interpolation type");
+            }
+
+
+            xsize = xs.size();
+            ysize = ys.size();
+            zsize = zs.size();
+
+            if (xsize < 5) {
+                throw std::invalid_argument("X coordinates must be larger than 4");
+            }
+            if (ysize < 5) {
+                throw std::invalid_argument("Y  coordinates must be larger than 4");
+            }
+            if (zsize < 5) {
+                throw std::invalid_argument("Z  coordinates must be larger than 4");
+            }
+
+
+            if (xsize != fs.dimension(0)) {
+                throw std::invalid_argument("X coordinates must be first dimension of value tensor");
+            }
+            if (ysize != fs.dimension(1)) {
+                throw std::invalid_argument("Y coordinates must be second dimension of value tensor");
+            }
+            if (zsize != fs.dimension(2)) {
+                throw std::invalid_argument("Z coordinates must be third dimension of value tensor");
+            }
+            
+            
+            for (int i = 0; i < xsize - 1; i++) {
+                if (xs[i + 1] < xs[i]) {
+                    throw std::invalid_argument("X coordinates must be in ascending order");
+                }
+            }
+            for (int i = 0; i < ysize - 1; i++) {
+                if (ys[i + 1] < ys[i]) {
+                    throw std::invalid_argument("Y coordinates must be in ascending order");
+                }
+            }
+            for (int i = 0; i < zsize - 1; i++) {
+                if (zs[i + 1] < zs[i]) {
+                    throw std::invalid_argument("Z coordinates must be in ascending order");
+                }
+            }
+
+            xtotal = xs[xsize - 1] - xs[0];
+            ytotal = ys[ysize - 1] - ys[0];
+            ztotal = zs[zsize - 1] - zs[0];
+
+
+            Eigen::VectorXd testx;
+            testx.setLinSpaced(xsize, xs[0], xs[xsize - 1]);
+            Eigen::VectorXd testy;
+            testy.setLinSpaced(ysize, ys[0], ys[ysize - 1]);
+            Eigen::VectorXd testz;
+            testz.setLinSpaced(zsize, zs[0], zs[zsize - 1]);
+
+            double xerr = (xs - testx).lpNorm<Eigen::Infinity>();
+            double yerr = (ys - testy).lpNorm<Eigen::Infinity>();
+            double zerr = (zs - testz).lpNorm<Eigen::Infinity>();
+
+            if (xerr > abs(xtotal) * 1.0e-12) {
+                this->xeven = false;
+            }
+            if (yerr > abs(ytotal) * 1.0e-12) {
+                this->yeven = false;
+            }
+            if (zerr > abs(ztotal) * 1.0e-12) {
+                this->zeven = false;
+            }
+
+            if (this->interp_kind == InterpType::cubic_interp) {
+                this->fill_Cmat();
+                this->calc_derivs();
+                if (this->cache_alpha) {
+                    this->cache_alphavecs();
+                }
+            }
+        }
+
+
+
+		void fill_Cmat() {
+
+          
+
+            const int coeffs[64][64] =
+            { {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {-3, 3, 0, 0, 0, 0, 0, 0, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {2, -2, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {-3, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, -3, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {9, -9, -9, 9, 0, 0, 0, 0, 6, 3, -6, -3, 0, 0, 0, 0, 6, -6, 3, -3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 2, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {-6, 6, 6, -6, 0, 0, 0, 0, -3, -3, 3, 3, 0, 0, 0, 0, -4, 4, -2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, -2, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {2, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 2, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {-6, 6, 6, -6, 0, 0, 0, 0, -4, -2, 4, 2, 0, 0, 0, 0, -3, 3, -3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, -1, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {4, -4, -4, 4, 0, 0, 0, 0, 2, 2, -2, -2, 0, 0, 0, 0, 2, -2, 2, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 3, 0, 0, 0, 0, 0, 0, -2, -1, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, -2, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -1, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, -9, -9, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 3, -6, -3, 0, 0, 0, 0, 6, -6, 3, -3, 0, 0, 0, 0, 4, 2, 2, 1, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -6, 6, 6, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, -3, 3, 3, 0, 0, 0, 0, -4, 4, -2, 2, 0, 0, 0, 0, -2, -2, -1, -1, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -6, 6, 6, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -4, -2, 4, 2, 0, 0, 0, 0, -3, 3, -3, 3, 0, 0, 0, 0, -2, -1, -2, -1, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, -4, -4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, -2, -2, 0, 0, 0, 0, 2, -2, 2, -2, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0},
+             {-3, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, -3, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {9, -9, 0, 0, -9, 9, 0, 0, 6, 3, 0, 0, -6, -3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, -6, 0, 0, 3, -3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 2, 0, 0, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {-6, 6, 0, 0, 6, -6, 0, 0, -3, -3, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -4, 4, 0, 0, -2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, -2, 0, 0, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, 0, 0, -1, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, -9, 0, 0, -9, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 3, 0, 0, -6, -3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, -6, 0, 0, 3, -3, 0, 0, 4, 2, 0, 0, 2, 1, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -6, 6, 0, 0, 6, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, -3, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -4, 4, 0, 0, -2, 2, 0, 0, -2, -2, 0, 0, -1, -1, 0, 0},
+             {9, 0, -9, 0, -9, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 3, 0, -6, 0, -3, 0, 6, 0, -6, 0, 3, 0, -3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 2, 0, 2, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 9, 0, -9, 0, -9, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 3, 0, -6, 0, -3, 0, 6, 0, -6, 0, 3, 0, -3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 2, 0, 2, 0, 1, 0},
+             {-27, 27, 27, -27, 27, -27, -27, 27, -18, -9, 18, 9, 18, 9, -18, -9, -18, 18, -9, 9, 18, -18, 9, -9, -18, 18, 18, -18, -9, 9, 9, -9, -12, -6, -6, -3, 12, 6, 6, 3, -12, -6, 12, 6, -6, -3, 6, 3, -12, 12, -6, 6, -6, 6, -3, 3, -8, -4, -4, -2, -4, -2, -2, -1},
+             {18, -18, -18, 18, -18, 18, 18, -18, 9, 9, -9, -9, -9, -9, 9, 9, 12, -12, 6, -6, -12, 12, -6, 6, 12, -12, -12, 12, 6, -6, -6, 6, 6, 6, 3, 3, -6, -6, -3, -3, 6, 6, -6, -6, 3, 3, -3, -3, 8, -8, 4, -4, 4, -4, 2, -2, 4, 4, 2, 2, 2, 2, 1, 1},
+             {-6, 0, 6, 0, 6, 0, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 0, -3, 0, 3, 0, 3, 0, -4, 0, 4, 0, -2, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -2, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, -6, 0, 6, 0, 6, 0, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 0, -3, 0, 3, 0, 3, 0, -4, 0, 4, 0, -2, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -2, 0, -1, 0, -1, 0},
+             {18, -18, -18, 18, -18, 18, 18, -18, 12, 6, -12, -6, -12, -6, 12, 6, 9, -9, 9, -9, -9, 9, -9, 9, 12, -12, -12, 12, 6, -6, -6, 6, 6, 3, 6, 3, -6, -3, -6, -3, 8, 4, -8, -4, 4, 2, -4, -2, 6, -6, 6, -6, 3, -3, 3, -3, 4, 2, 4, 2, 2, 1, 2, 1},
+             {-12, 12, 12, -12, 12, -12, -12, 12, -6, -6, 6, 6, 6, 6, -6, -6, -6, 6, -6, 6, 6, -6, 6, -6, -8, 8, 8, -8, -4, 4, 4, -4, -3, -3, -3, -3, 3, 3, 3, 3, -4, -4, 4, 4, -2, -2, 2, 2, -4, 4, -4, 4, -2, 2, -2, 2, -2, -2, -2, -2, -1, -1, -1, -1},
+             {2, 0, 0, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {-6, 6, 0, 0, 6, -6, 0, 0, -4, -2, 0, 0, 4, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 3, 0, 0, -3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, -1, 0, 0, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {4, -4, 0, 0, -4, 4, 0, 0, 2, 2, 0, 0, -2, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, -2, 0, 0, 2, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -6, 6, 0, 0, 6, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -4, -2, 0, 0, 4, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3, 3, 0, 0, -3, 3, 0, 0, -2, -1, 0, 0, -2, -1, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, -4, 0, 0, -4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 0, 0, -2, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, -2, 0, 0, 2, -2, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0},
+             {-6, 0, 6, 0, 6, 0, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, -4, 0, -2, 0, 4, 0, 2, 0, -3, 0, 3, 0, -3, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -1, 0, -2, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, -6, 0, 6, 0, 6, 0, -6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -4, 0, -2, 0, 4, 0, 2, 0, -3, 0, 3, 0, -3, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2, 0, -1, 0, -2, 0, -1, 0},
+             {18, -18, -18, 18, -18, 18, 18, -18, 12, 6, -12, -6, -12, -6, 12, 6, 12, -12, 6, -6, -12, 12, -6, 6, 9, -9, -9, 9, 9, -9, -9, 9, 8, 4, 4, 2, -8, -4, -4, -2, 6, 3, -6, -3, 6, 3, -6, -3, 6, -6, 3, -3, 6, -6, 3, -3, 4, 2, 2, 1, 4, 2, 2, 1},
+             {-12, 12, 12, -12, 12, -12, -12, 12, -6, -6, 6, 6, 6, 6, -6, -6, -8, 8, -4, 4, 8, -8, 4, -4, -6, 6, 6, -6, -6, 6, 6, -6, -4, -4, -2, -2, 4, 4, 2, 2, -3, -3, 3, 3, -3, -3, 3, 3, -4, 4, -2, 2, -4, 4, -2, 2, -2, -2, -1, -1, -2, -2, -1, -1},
+             {4, 0, -4, 0, -4, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0, 2, 0, -2, 0, -2, 0, 2, 0, -2, 0, 2, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+             {0, 0, 0, 0, 0, 0, 0, 0, 4, 0, -4, 0, -4, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0, 2, 0, -2, 0, -2, 0, 2, 0, -2, 0, 2, 0, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0},
+             {-12, 12, 12, -12, 12, -12, -12, 12, -8, -4, 8, 4, 8, 4, -8, -4, -6, 6, -6, 6, 6, -6, 6, -6, -6, 6, 6, -6, -6, 6, 6, -6, -4, -2, -4, -2, 4, 2, 4, 2, -4, -2, 4, 2, -4, -2, 4, 2, -3, 3, -3, 3, -3, 3, -3, 3, -2, -1, -2, -1, -2, -1, -2, -1},
+             {8, -8, -8, 8, -8, 8, 8, -8, 4, 4, -4, -4, -4, -4, 4, 4, 4, -4, 4, -4, -4, 4, -4, 4, 4, -4, -4, 4, 4, -4, -4, 4, 2, 2, 2, 2, -2, -2, -2, -2, 2, 2, -2, -2, 2, 2, -2, -2, 2, -2, 2, -2, 2, -2, 2, -2, 1, 1, 1, 1, 1, 1, 1, 1} };
+
+            this->Cmat.resize(64, 64);
+            for (int i = 0; i < 64; i++)
+                for (int j = 0; j < 64; j++)
+                    this->Cmat(i, j) = coeffs[i][j];
+
+
+
+		}
+
+        void calc_derivs() {
+
+            fs_dx.resize(fs.dimension(0), fs.dimension(1), fs.dimension(2));
+            fs_dy.resize(fs.dimension(0), fs.dimension(1), fs.dimension(2));
+            fs_dz.resize(fs.dimension(0), fs.dimension(1), fs.dimension(2));
+            fs_dxdy.resize(fs.dimension(0), fs.dimension(1), fs.dimension(2));
+            fs_dxdz.resize(fs.dimension(0), fs.dimension(1), fs.dimension(2));
+            fs_dydz.resize(fs.dimension(0), fs.dimension(1), fs.dimension(2));
+            fs_dxdydz.resize(fs.dimension(0), fs.dimension(1), fs.dimension(2));
+
+
+ 
+            auto fdiffimpl = [&](int dir,bool even, const auto& ts, const auto & src,  auto & dest) {
+
+                Eigen::Matrix<double, 5, 5> stens;
+                stens.row(0).setOnes();
+                Eigen::Matrix<double, 5, 1> rhs;
+                rhs << 0, 1, 0, 0, 0;
+                Eigen::Matrix<double, 5, 1> times;
+                Eigen::Matrix<double, 5, 1> coeffs;
+
+                int tsize = ts.size();
+                bool hitcent = false;
+                for (int i = 0; i < tsize; i++) {
+                    int start = 0;
+                    bool recalc = true;
+                    if (i + 2 <= tsize - 1 && i - 2 >= 0) {
+                        // central difference
+                        if (even && hitcent) {
+                            recalc = false;
+                        }
+                        hitcent = true;
+                        start = i - 2;
+                    }
+                    else if (i < tsize - 1 - i) {
+                        // forward difference
+                        start = 0;
+                    }
+                    else {
+                        // backward difference
+                        start = tsize - 5;
+                    }
+                    int stepdir = (i < tsize - 1) ? 1 : -1;
+                    double ti = ts[i];
+                    double tstep = std::abs(ts[i + stepdir] - ti);
+                    if (recalc) {
+                        times = ts.segment(start, 5);
+                        times -= Eigen::Matrix<double, 5, 1>::Constant(ti);
+                        times /= tstep;
+                        stens.row(1) = times.transpose();
+                        stens.row(2) = (stens.row(1).cwiseProduct(times.transpose())).eval();
+                        stens.row(3) = (stens.row(2).cwiseProduct(times.transpose())).eval();
+                        stens.row(4) = (stens.row(3).cwiseProduct(times.transpose())).eval();
+                        
+                        coeffs = stens.inverse() * rhs;
+                    }
+                    dest.chip(i, dir) 
+                        = src.chip(start, dir) * (coeffs[0] / tstep)
+                        + src.chip(start + 1, dir) * (coeffs[1] / tstep)
+                        + src.chip(start + 2, dir) * (coeffs[2] / tstep)
+                        + src.chip(start + 3, dir) * (coeffs[3] / tstep)
+                        + src.chip(start + 4, dir) * (coeffs[4] / tstep);
+
+                }
+            };
+
+            fdiffimpl(0, this->xeven, this->xs, this->fs, this->fs_dx);
+            fdiffimpl(1, this->yeven, this->ys, this->fs, this->fs_dy);
+            fdiffimpl(2, this->zeven, this->zs, this->fs, this->fs_dz);
+            
+            fdiffimpl(1, this->yeven, this->ys, this->fs_dx, this->fs_dxdy);
+            fdiffimpl(2, this->zeven, this->zs, this->fs_dx, this->fs_dxdz);
+            fdiffimpl(2, this->zeven, this->zs, this->fs_dy, this->fs_dydz);
+
+            fdiffimpl(2, this->zeven, this->zs, this->fs_dxdy, this->fs_dxdydz);
+
+        }
+
+
+
+        void cache_alphavecs() {
+            this->alphavecs.resize(fs.dimension(0)-1, fs.dimension(1)-1, fs.dimension(2)-1);
+            for (int i = 0; i < zsize - 1; i++) {
+                for (int j = 0; j < ysize - 1; j++) {
+                    for (int k = 0; k < xsize - 1; k++) {
+                        this->alphavecs(k, j, i) = this->calc_alphavec(k, j, i);
+                    }
+                }
+            }
+        }
+
+        static int find_elem(const Eigen::VectorXd& ts, bool teven, double t)  {
+            int elem;
+            if (teven) {
+                double tlocal = t - ts[0];
+                double tstep = ts[1] - ts[0];
+                elem = int(tlocal / tstep);
+            }
+            else {
+                int center = int(ts.size() / 2);
+                int shift = (ts[center] > t) ? 0 : center;
+                auto it = std::upper_bound(ts.begin() + shift, ts.end(), t);
+                elem = int(it - ts.begin()) - 1;
+            }
+            elem = std::min(elem, int(ts.size() - 2));
+            elem = std::max(elem, 0);
+            return elem;
+        }
+
+        std::tuple<int, int,int> get_xyzelems(double x, double y,double z) const {
+            
+            int xelem = this->find_elem(this->xs, this->xeven, x);
+            int yelem = this->find_elem(this->ys, this->yeven, y);
+            int zelem = this->find_elem(this->zs, this->zeven, z);
+
+            return std::tuple{ xelem,yelem,zelem };
+        }
+
+        Eigen::Matrix<double, 64, 1> calc_alphavec(int xelem, int yelem, int zelem) const {
+
+            double xstep = xs[xelem + 1] - xs[xelem];
+            double ystep = ys[yelem + 1] - ys[yelem];
+            double zstep = zs[zelem + 1] - zs[zelem];
+
+            Eigen::Matrix<double, 64, 1> bvec;
+            Eigen::Matrix<double, 64, 1> alphavec;
+
+
+            auto fillop = [&](auto start, const auto & src) {
+                for (int i = 0; i < 8; i++) {
+                    bvec[start]   = src(xelem  , yelem, zelem);
+                    bvec[start+1] = src(xelem+1, yelem, zelem);
+                    bvec[start+2] = src(xelem, yelem+1, zelem);
+                    bvec[start+3] = src(xelem+1, yelem+1, zelem);
+
+                    bvec[start+4] = src(xelem, yelem, zelem+1);
+                    bvec[start+5] = src(xelem + 1, yelem, zelem+1);
+                    bvec[start+6] = src(xelem , yelem + 1, zelem+1);
+                    bvec[start+7] = src(xelem + 1, yelem + 1, zelem+1);
+                }
+            };
+            
+            fillop(0, this->fs);
+            fillop(8 , this->fs_dx);
+            fillop(16, this->fs_dy);
+            fillop(24, this->fs_dz);
+            fillop(32, this->fs_dxdy);
+            fillop(40, this->fs_dxdz);
+            fillop(48, this->fs_dydz);
+            fillop(56, this->fs_dxdydz);
+
+            bvec.segment(8, 8)  *= (xstep);
+            bvec.segment(16, 8) *= (ystep);
+            bvec.segment(24, 8) *= (zstep);
+            bvec.segment(32, 8) *= (xstep * ystep);
+            bvec.segment(40, 8) *= (xstep * zstep);
+            bvec.segment(48, 8) *= (ystep * zstep);
+            bvec.segment(56, 8) *= (xstep * ystep * zstep);
+
+
+            alphavec.noalias() = Cmat * bvec;
+
+            return alphavec;
+        }
+
+        Eigen::Matrix<double, 64, 1> get_alphavec(int xelem, int yelem, int zelem) const {
+            if (this->cache_alpha) {
+                return this->alphavecs(xelem, yelem, zelem);
+            }
+            else {
+                return this->calc_alphavec(xelem, yelem, zelem);
+            }
+        }
+
+
+        void interp_impl(double x, double y, double z,
+            int deriv,
+            double& fval,
+            Eigen::Vector3<double>& dfxyz,
+            Eigen::Matrix3<double>& d2fxyz) const {
+
+            if (WarnOutOfBounds || ThrowOutOfBounds) {
+                double xeps = std::numeric_limits<double>::epsilon() * xtotal;
+                if (x<(xs[0] - xeps) || x>(xs[xs.size() - 1] + xeps)) {
+                    
+                    fmt::print(fmt::fg(fmt::color::red),
+                        "WARNING: x coordinate falls outside of InterpTable3D range. Data is being extrapolated!!\n");
+                    if (ThrowOutOfBounds) {
+                        throw std::invalid_argument("");
+                    }
+                }
+                double yeps = std::numeric_limits<double>::epsilon() * ytotal;
+                if (y<(ys[0] - yeps) || y>(ys[ys.size() - 1]) + yeps) {
+                    fmt::print(fmt::fg(fmt::color::red),
+                        "WARNING: y coordinate falls outside of InterpTable3D range. Data is being extrapolated!!\n");
+                    if (ThrowOutOfBounds) {
+                        throw std::invalid_argument("");
+                    }
+                }
+                double zeps = std::numeric_limits<double>::epsilon() * ztotal;
+                if (z<(zs[0] - zeps) || z>(zs[zs.size() - 1]) + zeps) {
+                    fmt::print(fmt::fg(fmt::color::red),
+                        "WARNING: z coordinate falls outside of InterpTable3D range. Data is being extrapolated!!\n");
+                    if (ThrowOutOfBounds) {
+                        throw std::invalid_argument("");
+                    }
+                }
+
+            }
+
+
+            auto [xelem, yelem, zelem] = get_xyzelems(x, y, z);
+
+            double xstep = xs[xelem + 1] - xs[xelem];
+            double ystep = ys[yelem + 1] - ys[yelem];
+            double zstep = zs[zelem + 1] - zs[zelem];
+
+            
+
+            double xf = (x - xs[xelem]) / xstep;
+            double yf = (y - ys[yelem]) / ystep;
+            double zf = (z - zs[zelem]) / zstep;
+
+            if (this->interp_kind == InterpType::cubic_interp) {
+                Eigen::Matrix<double, 64, 1> alphavec = this->get_alphavec(xelem,yelem,zelem);
+                
+                double xf2 = xf * xf;
+                double xf3 = xf2 * xf;
+
+                double yf2 = yf * yf;
+                double yf3 = yf2 * yf;
+
+                double zf2 = zf * zf;
+                double zf3 = zf2 * zf;
+
+                Eigen::Vector4d yfs{ 1,yf,yf2,yf3 };
+                Eigen::Vector4d zfs{ 1,zf,zf2,zf3 };
+
+             
+                
+                fval = 0;
+
+                for (int i = 0,start=0; i < 4; i++) {
+                    for (int j = 0; j < 4; j++,start+=4) {
+                        fval += (yfs[j] * zfs[i]) * (alphavec[start] + xf * alphavec[start + 1] + xf2 * alphavec[start + 2] + xf3 * alphavec[start + 3]);
+                    }
+                }
+
+                if (deriv > 0) {
+
+                    
+                    dfxyz[0] = 0;
+                    dfxyz[1] = 0;
+                    dfxyz[2] = 0;
+
+                    
+                    for (int i = 0, start = 0; i < 4; i++) {
+                        for (int j = 0; j < 4; j++,start+=4) {
+                            double xterm = alphavec[start] + xf * alphavec[start + 1] + xf2 * alphavec[start + 2] + xf3 * alphavec[start + 3];
+                            double dxterm = alphavec[start + 1] + 2 * xf * alphavec[start + 2] + 3 * xf2 * alphavec[start + 3];
+                            dfxyz[0] += (yfs[j] * zfs[i]) * (dxterm);
+                            if (j > 0) {
+                                dfxyz[1] += (j*yfs[j-1] * zfs[i]) * (xterm);
+                            }
+                            if (i > 0) {
+                                dfxyz[2] += (yfs[j] * i*zfs[i-1]) * (xterm);
+                            }
+                            
+                        }
+                    }
+
+                    dfxyz[0]/= xstep;
+                    dfxyz[1]/= ystep;
+                    dfxyz[2]/= zstep;
+                   
+
+                    if (deriv > 1) {
+                        Eigen::DiagonalMatrix<double, 3> dmat(1.0 / xstep, 1.0 / ystep, 1.0 / zstep);
+
+                        d2fxyz.setZero();
+
+                        for (int i = 0, start=0; i < 4; i++) {
+                            for (int j = 0; j < 4; j++,start+=4) {
+
+                                double xterm = alphavec[start] + xf * alphavec[start + 1] + xf2 * alphavec[start + 2] + xf3 * alphavec[start + 3];
+                                double dxterm = alphavec[start + 1] + 2 * xf * alphavec[start + 2] + 3 * xf2 * alphavec[start + 3];
+
+                                // Hit first row of hessian, diffing this term
+                                //dfxyz[0] += (yfs[j] * zfs[i]) * (dxterm);
+                                //wrt.x
+                                d2fxyz(0, 0) += (yfs[j] * zfs[i]) * (2 * alphavec[start + 2] + 6 * xf * alphavec[start + 3]);
+                                //wrt.y
+                                if (j > 0) {
+                                    d2fxyz(0, 1) += (j * yfs[j - 1] * zfs[i]) * (dxterm);
+                                }
+                                //wrt.z
+                                if (i > 0) {
+                                    d2fxyz(0, 2) += (yfs[j] * i * zfs[i - 1]) * (dxterm);
+                                }
+
+                                if (j > 0) {
+                                    // Hit second row of hessian, diffing this term
+                                    //dfxyz[1] += (j * yfs[j - 1] * zfs[i]) * (xterm);
+                                    //wrt.y
+                                    if (j > 1) {
+                                        d2fxyz(1, 1) += (j * (j - 1) * yfs[j - 2] * zfs[i]) * (xterm);
+                                    }
+                                    //wrt.z
+                                    if (i > 0) {
+                                        d2fxyz(1, 2) += (j * yfs[j - 1] * i*zfs[i-1]) * (xterm);
+                                    }
+                                }
+                                if (i > 0) {
+                                    // Hit third row of hessian, diffing this term
+                                    //dfxyz[2] += (yfs[j] * i * zfs[i - 1]) * (xterm);
+                                    if (i > 1) {
+                                        d2fxyz(2, 2) += (yfs[j] * i*(i-1) * zfs[i - 2]) * (xterm);
+                                    }
+                                }
+                            }
+                        }
+
+                        //symmetric
+                        d2fxyz(1, 0) = d2fxyz(0, 1);
+                        d2fxyz(2, 0) = d2fxyz(0, 2);
+                        d2fxyz(2, 1) = d2fxyz(1, 2);
+                
+                     
+                        d2fxyz = (dmat * d2fxyz * dmat).eval();
+                    }
+                }
+
+            }
+            else {
+                double c000 = this->fs(xelem,yelem, zelem);
+                double c100 = this->fs(xelem + 1,yelem, zelem);
+
+                double c001 = this->fs(xelem, yelem, zelem + 1);
+                double c101 = this->fs(xelem + 1, yelem, zelem + 1);
+
+                double c010 = this->fs(xelem, yelem + 1, zelem);
+                double c110 = this->fs(xelem + 1, yelem + 1, zelem);
+
+                double c011 = this->fs(xelem, yelem + 1, zelem + 1);
+                double c111 = this->fs(xelem + 1, yelem + 1, zelem + 1);
+
+                double x0 = this->xs[xelem];
+                double x1 = this->xs[xelem + 1];
+
+                double y0 = this->ys[yelem];
+                double y1 = this->ys[yelem + 1];
+
+                double z0 = this->zs[zelem];
+                double z1 = this->zs[zelem + 1];
+
+                double scale = -1.0 / (xstep * ystep * zstep);
+
+
+                double a0 = (-c000 * x1 * y1 * z1 + c001 * x1 * y1 * z0 + c010 * x1 * y0 * z1 - c011 * x1 * y0 * z0
+                    + c100 * x0 * y1 * z1 - c101 * x0 * y1 * z0 - c110 * x0 * y0 * z1 + c111 * x0 * y0 * z0) * scale;
+
+                double a1 = (c000 * y1 * z1 - c001 * y1 * z0 - c010 * y0 * z1 + c011 * y0 * z0
+                    - c100 * y1 * z1 + c101 * y1 * z0 + c110 * y0 * z1 - c111 * y0 * z0) * scale;
+
+                double a2 = (c000 * x1 * z1 - c001 * x1 * z0 - c010 * x1 * z1 + c011 * x1 * z0
+                    - c100 * x0 * z1 + c101 * x0 * z0 + c110 * x0 * z1 - c111 * x0 * z0) * scale;
+
+                double a3 = (c000 * x1 * y1 - c001 * x1 * y1 - c010 * x1 * y0 + c011 * x1 * y0
+                    - c100 * x0 * y1 + c101 * x0 * y1 + c110 * x0 * y0 - c111 * x0 * y0) * scale;
+
+
+                double a4 = (-c000 * z1 + c001 * z0 + c010 * z1 - c011 * z0 + c100 * z1 - c101 * z0 - c110 * z1 + c111 * z0) * scale;
+                double a5 = (-c000 * y1 + c001 * y1 + c010 * y0 - c011 * y0 + c100 * y1 - c101 * y1 - c110 * y0 + c111 * y0) * scale;
+                double a6 = (-c000 * x1 + c001 * x1 + c010 * x1 - c011 * x1 + c100 * x0 - c101 * x0 - c110 * x0 + c111 * x0) * scale;
+
+                double a7 = (c000 - c001 - c010 + c011 - c100 + c101 + c110 - c111) * scale;
+
+                fval = a0 + a1 * x + a2 * y + a3 * z + a4 * x * y + a5 * x * z + a6 * y * z + a7 * x * y * z;
+
+                if (deriv > 0) {
+                    dfxyz[0] = a1 + a4 * y + a5 * z + a7 * y * z;
+                    dfxyz[1] = a2 + a4 * x + a6 * z + a7 * x * z;
+                    dfxyz[2] = a3 + a5 * x + a6 * y + a7 * x * y;
+
+                    if (deriv > 1) {
+                        d2fxyz.setZero();
+                        d2fxyz(1, 0) = a4 + a7 * z;
+                        d2fxyz(2, 0) = a5 + a7 * y;
+                        d2fxyz(2, 1) = a6 + a7 * x;
+                        d2fxyz(0, 1) = d2fxyz(1, 0);
+                        d2fxyz(0, 2) = d2fxyz(2, 0);
+                        d2fxyz(1, 2) = d2fxyz(2, 1);
+
+                    }
+                }
+
+
+            }
+        }
+
+        double interp(double x, double y,double z) const {
+            double f;
+            Eigen::Vector3<double> dfxyz;
+            Eigen::Matrix3<double> d2fxyz;
+            interp_impl(x,y,z, 0, f, dfxyz, d2fxyz);
+            return f;
+        }
+
+        std::tuple<double, Eigen::Vector3<double>> interp_deriv1(double x, double y, double z) const {
+            double f;
+            Eigen::Vector3<double> dfxyz;
+            Eigen::Matrix3<double> d2fxyz;
+            interp_impl(x, y, z, 1, f, dfxyz, d2fxyz);
+            return std::tuple{ f,dfxyz };
+        }
+        std::tuple<double, Eigen::Vector3<double>, Eigen::Matrix3<double>> interp_deriv2(double x, double y, double z) const {
+            double f;
+            Eigen::Vector3<double> dfxyz;
+            Eigen::Matrix3<double> d2fxyz;
+            interp_impl(x, y, z, 2, f, dfxyz, d2fxyz);
+            return std::tuple{ f,dfxyz,d2fxyz };
+        }
+	};
+
+
+    struct InterpFunction3D : VectorFunction<InterpFunction3D, 3, 1, Analytic, Analytic> {
+        using Base = VectorFunction<InterpFunction3D, 3, 1, Analytic, Analytic>;
+        DENSE_FUNCTION_BASE_TYPES(Base);
+
+        std::shared_ptr<InterpTable3D> tab;
+
+
+        InterpFunction3D() {}
+        InterpFunction3D(std::shared_ptr<InterpTable3D> tab) :tab(tab) {
+            this->setIORows(3, 1);
+        }
+
+
+        template <class InType, class OutType>
+        inline void compute_impl(ConstVectorBaseRef<InType> x,
+            ConstVectorBaseRef<OutType> fx_) const {
+            typedef typename InType::Scalar Scalar;
+            VectorBaseRef<OutType> fx = fx_.const_cast_derived();
+            fx[0] = this->tab->interp(x[0], x[1],x[2]);
+
+        }
+        template <class InType, class OutType, class JacType>
+        inline void compute_jacobian_impl(ConstVectorBaseRef<InType> x,
+            ConstVectorBaseRef<OutType> fx_,
+            ConstMatrixBaseRef<JacType> jx_) const {
+            typedef typename InType::Scalar Scalar;
+            VectorBaseRef<OutType> fx = fx_.const_cast_derived();
+            MatrixBaseRef<JacType> jx = jx_.const_cast_derived();
+
+            auto [z, dzdx] = this->tab->interp_deriv1(x[0],x[1],x[2]);
+            fx[0] = z;
+            jx = dzdx.transpose();
+
+        }
+        template <class InType, class OutType, class JacType, class AdjGradType,
+            class AdjHessType, class AdjVarType>
+            inline void compute_jacobian_adjointgradient_adjointhessian_impl(
+                ConstVectorBaseRef<InType> x, ConstVectorBaseRef<OutType> fx_,
+                ConstMatrixBaseRef<JacType> jx_, ConstVectorBaseRef<AdjGradType> adjgrad_,
+                ConstMatrixBaseRef<AdjHessType> adjhess_,
+                ConstVectorBaseRef<AdjVarType> adjvars) const {
+            typedef typename InType::Scalar Scalar;
+            VectorBaseRef<OutType> fx = fx_.const_cast_derived();
+            MatrixBaseRef<JacType> jx = jx_.const_cast_derived();
+            VectorBaseRef<AdjGradType> adjgrad = adjgrad_.const_cast_derived();
+            MatrixBaseRef<AdjHessType> adjhess = adjhess_.const_cast_derived();
+
+            auto [z, dzdx, d2zdx] = this->tab->interp_deriv2(x[0], x[1],x[2]);
+            fx[0] = z;
+            jx = dzdx.transpose();
+            adjgrad = adjvars[0] * dzdx;
+            adjhess = adjvars[0] * d2zdx;
+
+        }
+
+    };
+
+
+
+	static void InterpTable3DBuild(py::module& m) {
+
+        auto obj = py::class_<InterpTable3D, std::shared_ptr<InterpTable3D>>(m, "InterpTable3D");
+
+        obj.def(py::init<const Eigen::VectorXd& , const Eigen::VectorXd& , const Eigen::VectorXd& ,
+            const Eigen::Tensor<double, 3>& , std::string , bool  >(),
+            py::arg("xs"), py::arg("ys"), py::arg("zs"), py::arg("fs"), 
+            py::arg("kind") = std::string("cubic"), 
+            py::arg("cache")=false);
+
+
+        obj.def("interp", py::overload_cast<double, double,double>(&InterpTable3D::interp, py::const_));
+        obj.def("interp_deriv1", py::overload_cast<double, double, double>(&InterpTable3D::interp_deriv1, py::const_));
+        obj.def("interp_deriv2", py::overload_cast<double, double, double>(&InterpTable3D::interp_deriv2, py::const_));
+
+        obj.def_readwrite("WarnOutOfBounds", &InterpTable3D::WarnOutOfBounds);
+        obj.def_readwrite("ThrowOutOfBounds", &InterpTable3D::ThrowOutOfBounds);
+
+        obj.def("__call__", py::overload_cast<double,double,double>(&InterpTable3D::interp, py::const_), py::is_operator());
+
+        obj.def("__call__", [](const InterpTable3D& self, const GenericFunction<-1, 1>& x, const GenericFunction<-1, 1>& y, const GenericFunction<-1, 1>& z) {
+            return GenericFunction<-1, 1>(InterpFunction3D(std::make_shared<InterpTable3D>(self)).eval(stack(x, y,z)));
+            });
+
+        obj.def("__call__", [](const InterpTable3D& self, const Segment<-1, 1, -1>& x, const Segment<-1, 1, -1>& y, const Segment<-1, 1, -1>& z) {
+            return GenericFunction<-1, 1>(InterpFunction3D(std::make_shared<InterpTable3D>(self)).eval(stack(x, y,z)));
+            });
+
+        obj.def("__call__", [](const InterpTable3D& self, const Segment<-1, 3, -1>& xyz) {
+            return GenericFunction<-1, 1>(InterpFunction3D(std::make_shared<InterpTable3D>(self)).eval(xyz));
+            });
+
+        obj.def("__call__", [](const InterpTable3D& self, const GenericFunction<-1, -1>& xyz) {
+            return GenericFunction<-1, 1>(InterpFunction3D(std::make_shared<InterpTable3D>(self)).eval(xyz));
+            });
+
+        obj.def("sf", [](const InterpTable3D& self) {
+            return GenericFunction<-1, 1>(InterpFunction3D(std::make_shared<InterpTable3D>(self)));
+            });
+        obj.def("vf", [](const InterpTable3D& self) {
+            return GenericFunction<-1, -1>(InterpFunction3D(std::make_shared<InterpTable3D>(self)));
+            });
+
+
+
+        m.def("InterpTable3DSpeedTest", [](const GenericFunction<-1, 1>& tabf, 
+            double xl, double xu, double yl, double yu, double zl, double zu, 
+            int nsamps, bool lin) {
+
+            Eigen::ArrayXd xsamps;
+            xsamps.setRandom(nsamps);
+            xsamps += 1;
+            xsamps /= 2;
+            xsamps *= (xu - xl);
+            xsamps += xl;
+
+            Eigen::ArrayXd ysamps;
+            ysamps.setRandom(nsamps);
+            ysamps += 1;
+            ysamps /= 2;
+            ysamps *= (yu - yl);
+            ysamps += yl;
+
+            Eigen::ArrayXd zsamps;
+            zsamps.setRandom(nsamps);
+            zsamps += 1;
+            zsamps /= 2;
+            zsamps *= (zu - zl);
+            zsamps += zl;
+
+
+            if (lin) {
+                xsamps.setLinSpaced(xl, xu);
+                ysamps.setLinSpaced(yl, yu);
+                zsamps.setLinSpaced(zl, zu);
+
+            }
+
+
+            Eigen::VectorXd xyz(3);
+            Vector1<double> f;
+            f.setZero();
+
+            Utils::Timer Runtimer;
+            Runtimer.start();
+
+            double tmp = 0;
+            for (int i = 0; i < nsamps; i++) {
+
+                xyz[0] = xsamps[i];
+                xyz[1] = ysamps[i];
+                xyz[2] = zsamps[i];
+
+                tabf.compute(xyz, f);
+                tmp += f[0] / double(i + 3);
+
+                f.setZero();
+
+
+            }
+            Runtimer.stop();
+            double tseconds = double(Runtimer.count<std::chrono::microseconds>()) / 1000000;
+            fmt::print("Total Time: {0:} ms \n", tseconds * 1000);
+
+
+            return tmp;
+            });
+
+
+	}
+
+}
\ No newline at end of file


Test patch:
diff --git a/asset_asrl/test/test_VectorFunctions/test_InterpTable1D.py b/asset_asrl/test/test_VectorFunctions/test_InterpTable1D.py
deleted file mode 100644
index 28caca5..0000000
--- a/asset_asrl/test/test_VectorFunctions/test_InterpTable1D.py
+++ /dev/null
@@ -1,136 +0,0 @@
-import numpy as np
-import asset_asrl as ast
-
-import matplotlib.pyplot as plt
-from matplotlib import cm
-from matplotlib.ticker import LinearLocator
-
-from scipy import interpolate
-
-
-import unittest
-
-
-vf        = ast.VectorFunctions
-oc        = ast.OptimalControl
-Args      = vf.Arguments
-
-class test_InterpTable1D(unittest.TestCase):
-    
-    def Interp_test(self,n,Func,dFunc,d2Func,tstab,tscheck):
-        
-        
-        Vts0 =[]
-        Vts2 =[]
-        
-        for t in tstab:
-            Vt0=np.zeros((n+1))
-            Vt0[0]=t
-            Vt0[1:n+1]=Func(t)
-            Vts0.append(Vt0)
-            
-            Vt2=np.zeros((n+1))
-            Vt2[n]=t
-            Vt2[0:n]=Func(t)
-            Vts2.append(Vt2)
-            
-
-                
-        kind   = 'cubic'
-        #print(Func(tstab))
-        TabAx0 = vf.InterpTable1D(tstab,Func(tstab).T,axis=0,kind=kind)
-        TabAx1 = vf.InterpTable1D(tstab,Func(tstab),axis=1,kind=kind)
-        
-        TabVt0 = vf.InterpTable1D(Vts0,tvar=0,kind=kind)
-        TabVt2 = vf.InterpTable1D(Vts2,tvar=n,kind=kind)
-        
-        
-        RetAx0 = TabAx0(tscheck)
-        RetAx1 = TabAx1(tscheck)
-        RetVt0 = TabVt0(tscheck)
-        RetVt2 = TabVt2(tscheck)
-        
-        
-        
-        ErrAx1Ax0 = abs(RetAx1-RetAx0).max()
-        ErrVt0Ax0 = abs(RetVt0-RetAx0).max()
-        ErrVt2Ax0 = abs(RetVt2-RetAx0).max()
-        
-        
-        self.assertLess(ErrAx1Ax0, 1.0e-13)
-        self.assertLess(ErrVt0Ax0, 1.0e-13)
-        self.assertLess(ErrVt2Ax0, 1.0e-13)
-        
-        
-        Valtol   = 1.0e-6
-        dValtol  = 1.0e-2
-        d2Valtol = 5.0e-2
-        
-        
-        
-        
-        Tabs=[TabAx0,TabAx1,TabVt0,TabVt2]
-        
-        for Tab in Tabs:
-            TabFunc = Tab.vf()
-            L = range(1,n+1)
-            
-            for t in tscheck:
-                
-                Val   = Func(t)
-                dVal  = dFunc(t)
-                d2Val = d2Func(t)
-                
-                
-                Vali,dVali,d2Vali = Tab.interp_deriv2(t)
-                
-                #print(d2Vali,d2Val,t)
-
-                
-                Valerr = abs(Val-Vali).max()
-                dValerr = abs(dVal-dVali).max()
-                d2Valerr = abs(d2Val-d2Vali).max()
-                
-                self.assertLess(Valerr, Valtol)
-                self.assertLess(dValerr, dValtol)
-                self.assertLess(d2Valerr, d2Valtol)
-                
-                fx,jx,gx,hx = TabFunc.computeall([t],L)
-                
-                fxerr = abs(Vali-fx).max()
-                jxerr= abs(dVali-jx.T).max()
-                gxerr =abs(np.dot(L,dVali)-gx[0]).max()
-                hxerr =abs(np.dot(L,d2Vali)-hx[0,0]).max()
-                
-                self.assertLess(fxerr, 1.0e-13)
-                self.assertLess(jxerr, 1.0e-13)
-                self.assertLess(gxerr, 1.0e-13)
-                self.assertLess(hxerr, 1.0e-13)
-        
-        
-        
-    def test_EvenInterp(self):
-        
-        
-        def  Func(t) :return np.array([ np.cos(t), np.sin(t)])
-        def dFunc(t) :return np.array([-np.sin(t), np.cos(t)])
-        def d2Func(t):return np.array([-np.cos(t),-np.sin(t)])
-        
-        
-        tstab = list(np.linspace(0,2*np.pi,500))
-        
-        tscheck = np.linspace(0.01,2*np.pi,37)
-        
-        self.Interp_test(2,Func,dFunc,d2Func,tstab,tscheck)
-        
-        def  Func(t) :return np.array([ np.cos(t)])
-        def dFunc(t) :return np.array([-np.sin(t)])
-        def d2Func(t):return np.array([-np.cos(t)])
-
-        self.Interp_test(1,Func,dFunc,d2Func,tstab,tscheck)
-
-        
-        
-        
-if __name__ == "__main__":
-    unittest.main(exit=False)
\ No newline at end of file
diff --git a/asset_asrl/test/test_VectorFunctions/test_Tables.py b/asset_asrl/test/test_VectorFunctions/test_Tables.py
new file mode 100644
index 0000000..848c86e
--- /dev/null
+++ b/asset_asrl/test/test_VectorFunctions/test_Tables.py
@@ -0,0 +1,366 @@
+import numpy as np
+import asset_asrl as ast
+
+import matplotlib.pyplot as plt
+from matplotlib import cm
+from matplotlib.ticker import LinearLocator
+
+from scipy import interpolate
+
+
+import unittest
+
+
+vf        = ast.VectorFunctions
+oc        = ast.OptimalControl
+Args      = vf.Arguments
+
+class test_InterpTables(unittest.TestCase):
+    
+    def Interp1D_test(self,n,Func,dFunc,d2Func,tstab,tscheck):
+        
+        
+        Vts0 =[]
+        Vts2 =[]
+        
+        for t in tstab:
+            Vt0=np.zeros((n+1))
+            Vt0[0]=t
+            Vt0[1:n+1]=Func(t)
+            Vts0.append(Vt0)
+            
+            Vt2=np.zeros((n+1))
+            Vt2[n]=t
+            Vt2[0:n]=Func(t)
+            Vts2.append(Vt2)
+            
+
+                
+        kind   = 'cubic'
+        TabAx0 = vf.InterpTable1D(tstab,Func(tstab).T,axis=0,kind=kind)
+        TabAx1 = vf.InterpTable1D(tstab,Func(tstab),axis=1,kind=kind)
+        
+        TabVt0 = vf.InterpTable1D(Vts0,tvar=0,kind=kind)
+        TabVt2 = vf.InterpTable1D(Vts2,tvar=n,kind=kind)
+        
+        
+        RetAx0 = TabAx0(tscheck)
+        RetAx1 = TabAx1(tscheck)
+        RetVt0 = TabVt0(tscheck)
+        RetVt2 = TabVt2(tscheck)
+        
+        
+        
+        ErrAx1Ax0 = abs(RetAx1-RetAx0).max()
+        ErrVt0Ax0 = abs(RetVt0-RetAx0).max()
+        ErrVt2Ax0 = abs(RetVt2-RetAx0).max()
+        
+        
+        self.assertLess(ErrAx1Ax0, 1.0e-13)
+        self.assertLess(ErrVt0Ax0, 1.0e-13)
+        self.assertLess(ErrVt2Ax0, 1.0e-13)
+        
+        
+        Valtol   = 1.0e-6
+        dValtol  = 1.0e-4
+        d2Valtol = 1.0e-2
+        
+        
+        
+        
+        Tabs=[TabAx0,TabAx1,TabVt0,TabVt2]
+        
+        for Tab in Tabs:
+            TabFunc = Tab.vf()
+            L = range(1,n+1)
+            
+            for t in tscheck:
+                
+                Val   = Func(t)
+                dVal  = dFunc(t)
+                d2Val = d2Func(t)
+                
+                
+                Vali,dVali,d2Vali = Tab.interp_deriv2(t)
+                
+
+                
+                Valerr = abs(Val-Vali).max()
+                dValerr = abs(dVal-dVali).max()
+                d2Valerr = abs(d2Val-d2Vali).max()
+                
+                #print(Valerr,dValerr,d2Valerr)
+                
+                self.assertLess(Valerr, Valtol)
+                self.assertLess(dValerr, dValtol)
+                self.assertLess(d2Valerr, d2Valtol)
+                
+                fx,jx,gx,hx = TabFunc.computeall([t],L)
+                
+                fxerr = abs(Vali-fx).max()
+                jxerr= abs(dVali-jx.T).max()
+                gxerr =abs(np.dot(L,dVali)-gx[0]).max()
+                hxerr =abs(np.dot(L,d2Vali)-hx[0,0]).max()
+                
+                self.assertLess(fxerr, 1.0e-13)
+                self.assertLess(jxerr, 1.0e-13)
+                self.assertLess(gxerr, 1.0e-13)
+                self.assertLess(hxerr, 1.0e-13)
+        
+    def Interp2D_test(self,Func,dFunc,d2Func,xstab,ystab,xscheck,yscheck):
+        
+        X, Y = np.meshgrid(xstab, ystab)
+        
+        F = Func(X,Y)
+                
+        Tab = vf.InterpTable2D(xstab,ystab,F,kind='cubic')
+        
+        args = Args(2)
+        
+        sf1 = Tab(args)
+        sf2 = Tab(args[0],args[1])
+        sf3 = Tab(args[0]*1,args[1]*1)
+        sf4 = Tab.sf()
+        
+        sfs = [sf1,sf2,sf3,sf4]
+        lm = 2.0
+        
+        Valtol = 1.0e-6
+        dValtol = 1.0e-4
+        d2Valtol = 1.0e-2
+        
+        for i,x in enumerate(xscheck):
+            for j,y in enumerate(yscheck):
+                Val = Func(x,y)
+                dVal = dFunc(x,y)
+                d2Val = d2Func(x,y)
+                
+                for func in sfs:
+                    xin = [x,y]
+                    
+                    fx,jx,gx,hx = func.computeall(xin,[lm])
+                    fx1 = func.compute(xin)
+                    jx1 = func.jacobian(xin)
+                    
+                    fxerr = abs(Val-fx).max()
+                    jxerr= abs(dVal-jx.T).max()
+                    gxerr =abs(dVal*lm-gx).max()
+                    hxerr =abs(d2Val*lm-hx).max()
+                    selffxerr = abs(fx1-fx).max()
+                    selfjxerr= abs(jx1-jx).max()
+                    
+                    self.assertLess(fxerr, Valtol)
+                    self.assertLess(jxerr, dValtol)
+                    self.assertLess(gxerr, dValtol)
+                    self.assertLess(hxerr, d2Valtol)
+                    
+                    self.assertLess(selffxerr, 1.0e-12)
+                    self.assertLess(selfjxerr, 1.0e-12)
+                    
+    def Interp3D_test(self,Func,dFunc,d2Func,xstab,ystab,zstab,xscheck,yscheck,zscheck,cache = False,indexing = 'ij'):
+        
+        X,Y,Z = np.meshgrid(xstab, ystab,zstab,indexing='ij')
+        
+        F = Func(X,Y,Z)
+                
+        Tab = vf.InterpTable3D(xstab,ystab,zstab,F,kind='cubic',cache = cache)
+        args = Args(3)
+        
+        sf1 = Tab(args)
+        sf2 = Tab(args[0],args[1],args[2])
+        sf3 = Tab(args[0]*1,args[1]*1,args[2]*1)
+        sf4 = Tab.sf()
+        
+        sfs = [sf1,sf2,sf3,sf4]
+        sfs = [sf4]
+        lm = 2.0
+        
+        Valtol = 1.0e-6
+        dValtol = 1.0e-4
+        d2Valtol = 1.0e-2
+        
+        for i,x in enumerate(xscheck):
+            for j,y in enumerate(yscheck):
+                for k,z in enumerate(zscheck):
+                    Val = Func(x,y,z)
+                    dVal = dFunc(x,y,z)
+                    d2Val = d2Func(x,y,z)
+                    
+                    for func in sfs:
+                        xin = [x,y,z]
+                        
+                        fx1 = func.compute(xin)
+                        jx1 = func.jacobian(xin)
+                        
+                        fx,jx,gx,hx = func.computeall(xin,[lm])
+                       
+                        fxerr = abs(Val-fx).max()
+                        jxerr= abs(dVal-jx.T).max()
+                        gxerr =abs(dVal*lm-gx).max()
+                        hxerr =abs(d2Val*lm-hx).max()
+                        
+                        selffxerr = abs(fx1-fx).max()
+                        selfjxerr= abs(jx1-jx).max()
+                        
+                        self.assertLess(fxerr, Valtol)
+                        self.assertLess(jxerr, dValtol)
+                        self.assertLess(gxerr, dValtol)
+                        self.assertLess(hxerr, d2Valtol)  
+                        
+                        self.assertLess(selffxerr, 1.0e-12)
+                        self.assertLess(selfjxerr, 1.0e-12)  
+    
+    def test_Interp1D(self):
+    
+        def  Func(t) :return np.array([ np.cos(t), np.sin(t)])
+        def dFunc(t) :return np.array([-np.sin(t), np.cos(t)])
+        def d2Func(t):return np.array([-np.cos(t),-np.sin(t)])
+        
+        n = 100
+        
+        tstab  = list(np.linspace(0,2*np.pi,n))
+        tstabu = list(np.linspace(0,2*np.pi,n))
+        tstabu.pop(int(n/4))
+        tstabu.pop(int(n/3))
+        tstabu.pop(int(n/2))
+        tstabu.pop(int(2*n/3))
+        tstabu.pop(int(3*n/4))
+
+        tscheck = np.linspace(0.00,2*np.pi,37)
+        self.Interp1D_test(2,Func,dFunc,d2Func,tstab,tscheck)
+        self.Interp1D_test(2,Func,dFunc,d2Func,tstabu,tscheck)
+
+        
+        def  Func(t) :return np.array([ np.cos(t)])
+        def dFunc(t) :return np.array([-np.sin(t)])
+        def d2Func(t):return np.array([-np.cos(t)])
+
+        self.Interp1D_test(1,Func,dFunc,d2Func,tstab,tscheck)
+        self.Interp1D_test(1,Func,dFunc,d2Func,tstabu,tscheck)
+        
+    def test_Interp2D(self):
+        
+        def Func(x,y):
+            return np.cos(x)*np.cos(y)
+        def dFunc(x,y):
+            return - np.array([np.sin(x)*np.cos(y),
+                               np.cos(x)*np.sin(y)])
+        def d2Func(x,y):
+            row1 = - np.array([ np.cos(x)*np.cos(y),-np.sin(x)*np.sin(y)])
+            row2 = - np.array([-np.sin(x)*np.sin(y), np.cos(x)*np.cos(y)])
+            return  np.array([row1,row2])
+
+        nx = 103
+        ny = 111
+        
+        xs = np.linspace(-np.pi, np.pi*1.1,nx)
+        ys = np.linspace(-1.33*np.pi, np.pi*.9,ny)
+        
+        xsu = list(np.copy(xs))
+        xsu.pop(int(nx/4))
+        xsu.pop(int(nx/3))
+        xsu.pop(int(nx/2))
+        xsu.pop(int(2*nx/3))
+        xsu.pop(int(3*nx/4))
+        
+        
+        ysu = list(np.copy(ys))
+        ysu.pop(int(ny/5))
+        ysu.pop(int(4*ny/5))
+        
+        xscheck = np.linspace(xs[0],xs[-1],50)
+        yscheck = np.linspace(ys[0],ys[-1],60)
+        
+        
+        with self.subTest("Even on Even"):
+            self.Interp2D_test(Func,dFunc,d2Func,xs,ys,xscheck,yscheck)
+            
+        with self.subTest("Even on Uneven"):
+            self.Interp2D_test(Func,dFunc,d2Func,xs,ysu,xscheck,yscheck)
+            
+        with self.subTest("Uneven on Even"):
+            self.Interp2D_test(Func,dFunc,d2Func,xsu,ys,xscheck,yscheck)
+            
+        with self.subTest("Uneven on Uneven"):
+            self.Interp2D_test(Func,dFunc,d2Func,xsu,ysu,xscheck,yscheck)
+    
+    def test_Interp3D(self):
+        
+        def Func(x,y,z):
+            return np.cos(x)*np.cos(y)*np.cos(z)
+
+        def dFunc(x,y,z):
+            return - np.array([np.sin(x)*np.cos(y)*np.cos(z),
+                               np.cos(x)*np.sin(y)*np.cos(z),
+                               np.cos(x)*np.cos(y)*np.sin(z)])
+
+        def d2Func(x,y,z):
+            row1 = - np.array([ np.cos(x)*np.cos(y)*np.cos(z),-np.sin(x)*np.sin(y)*np.cos(z),-np.sin(x)*np.cos(y)*np.sin(z)])
+            row2 = - np.array([-np.sin(x)*np.sin(y)*np.cos(z), np.cos(x)*np.cos(y)*np.cos(z),-np.cos(x)*np.sin(y)*np.sin(z)])
+            row3 = - np.array([-np.sin(x)*np.cos(y)*np.sin(z),-np.cos(x)*np.sin(y)*np.sin(z), np.cos(x)*np.cos(y)*np.cos(z)])
+
+            return  np.array([row1,row2,row3])
+        
+        
+        nx = 103
+        ny = 111
+        nz = 101
+        
+        
+        xs = np.linspace(-np.pi, np.pi*1.1,nx)
+        ys = np.linspace(-1.33*np.pi, np.pi*.9,ny)
+        zs = np.linspace(-1.1*np.pi, np.pi*1.06,nz)
+
+
+        xsu = list(np.copy(xs))
+        xsu.pop(int(nx/4))
+        xsu.pop(int(nx/3))
+        xsu.pop(int(nx/2))
+        xsu.pop(int(2*nx/3))
+        xsu.pop(int(3*nx/4))
+        
+        
+        ysu = list(np.copy(ys))
+        ysu.pop(int(ny/5))
+        ysu.pop(int(4*ny/5))
+        
+        zsu = list(np.copy(zs))
+        zsu.pop(int(nz/7)+1)
+        zsu.pop(int(3*nz/5))
+        
+        
+        xscheck = np.linspace(xs[0],xs[-1],20)
+        yscheck = np.linspace(ys[0],ys[-1],23)
+        zscheck = np.linspace(zs[0],zs[-1],19)
+        
+       
+
+        with self.subTest("Even, Even, Even"):
+            self.Interp3D_test(Func,dFunc,d2Func,xs,ys,zs,xscheck,yscheck,zscheck)
+        
+        with self.subTest("Even, Uneven, Even"):
+            self.Interp3D_test(Func,dFunc,d2Func,xs,ysu,zs,xscheck,yscheck,zscheck)  
+            
+        with self.subTest("Uneven, Even, Uneven"):
+            self.Interp3D_test(Func,dFunc,d2Func,xsu,ys,zsu,xscheck,yscheck,zscheck)
+            
+        with self.subTest("Even, Even, Even: Cached"):
+            self.Interp3D_test(Func,dFunc,d2Func,xs,ys,zs,xscheck,yscheck,zscheck,cache=True)
+        
+        with self.subTest("Even, Uneven, Even: Cached"):
+            self.Interp3D_test(Func,dFunc,d2Func,xs,ysu,zs,xscheck,yscheck,zscheck,cache=True)  
+        
+        
+
+    
+        
+        
+
+
+        
+
+
+        
+        
+if __name__ == "__main__":
+    unittest.main(exit=False)
\ No newline at end of file