guided_kriging_kriging_as_drift.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 206,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "import pandas as pd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 207,
   "metadata": {},
   "outputs": [],
   "source": [
    "def squared_euclidean_distance(x_1,x_2):\n",
    "    sqd = np.sqrt(np.reshape(np.sum(x_1**2,1),newshape =(x_1.shape[0],1))+\\\n",
    "    np.reshape(np.sum(x_2**2,1),newshape =(1,x_2.shape[0]))-\\\n",
    "    2*(x_1@x_2.T))\n",
    "    return sqd\n",
    "\n",
    "def cartesian_dist(x_1,x_2):\n",
    "    return np.concatenate([\n",
    "        np.tile(x_1[:,0] - np.reshape(x_2[:,0],[x_2[:,0].shape[0],1]),[1,3]),\n",
    "        np.tile(x_1[:,1] - np.reshape(x_2[:,1],[x_2[:,1].shape[0],1]),[1,3]),\n",
    "        np.tile(x_1[:,2] - np.reshape(x_2[:,2],[x_2[:,2].shape[0],1]),[1,3])],axis = 0) \n",
    "\n",
    "def cov_gradients(dist_tiled,h_u,h_v,a_T,perpendicularity_matrix):\n",
    "    c_o_T = a_T**2/14/3\n",
    "    condition1 = 0\n",
    "    a = (h_u*h_v)\n",
    "    b = dist_tiled**2\n",
    "\n",
    "    t1 =  np.divide(a, b, out=np.zeros_like(a),casting='unsafe', where=b!=0)\n",
    "    t2 = np.where(dist_tiled < a_T,(-c_o_T*((-14/a_T**2)+\n",
    "                 105/4*dist_tiled/a_T**3 -\n",
    "                 35/2 * dist_tiled**3 / a_T **5 +\n",
    "                 21 /4 * dist_tiled**5/a_T**7)),0)+\\\n",
    "          np.where(dist_tiled < a_T,(c_o_T * 7 * (9 * dist_tiled ** 5 -\n",
    "                      20 * a_T ** 2 * dist_tiled ** 3 +\n",
    "                      15 * a_T ** 4 * dist_tiled -\n",
    "                      4 * a_T ** 5) / (2 * a_T ** 7)),0)\n",
    "\n",
    "    # when we do the covariance of Gx with Gx, Gy with Gy and so on, quation B9 in Gempy paper\n",
    "    t3 = perpendicularity_matrix * \\\n",
    "          np.where(dist_tiled < a_T,(c_o_T * ((-14 / a_T ** 2) + 105 / 4 * dist_tiled / a_T ** 3 -\n",
    "                   35 / 2 * dist_tiled ** 3 / a_T ** 5 +\n",
    "                   21 / 4 * dist_tiled ** 5 / a_T ** 7)),0)\n",
    "    t4 = 1/3*np.eye(dist_tiled.shape[0])\n",
    "\n",
    "    condition2 = t1 * t2 - t3 + t4\n",
    "\n",
    "    C_G = np.where(dist_tiled==0, condition1, condition2) ## adding nugget effect\n",
    "    return C_G\n",
    "\n",
    "    C_G = np.where(dist_tiled==0, condition1, condition2) ## adding nugget effect\n",
    "    return C_G\n",
    "\n",
    "def set_rest_ref_matrix(number_of_points_per_surface,layer1):\n",
    "    ref_layer_points = np.repeat(np.stack([layer1[-1]],axis = 0),repeats=number_of_points_per_surface-1,axis = 0)\n",
    "    rest_layer_points = np.concatenate([layer1[0:-1]],axis = 0)\n",
    "    return ref_layer_points,rest_layer_points\n",
    "\n",
    "def cov_interface(ref_layer_points,rest_layer_points,a_T):\n",
    "    sed_rest_rest = squared_euclidean_distance(rest_layer_points,rest_layer_points)\n",
    "    sed_ref_rest = squared_euclidean_distance(ref_layer_points,rest_layer_points)\n",
    "    sed_rest_ref = squared_euclidean_distance(rest_layer_points,ref_layer_points)\n",
    "    sed_ref_ref = squared_euclidean_distance(ref_layer_points,ref_layer_points)\n",
    "    c_o_T = a_T**2/14/3\n",
    "    C_I = c_o_T*(\\\n",
    "                (np.where(sed_rest_rest < a_T, (1 - 7 * (sed_rest_rest / a_T) ** 2 +\\\n",
    "                 35 / 4 * (sed_rest_rest / a_T) ** 3 -\\\n",
    "                 7 / 2 * (sed_rest_rest / a_T) ** 5 +\\\n",
    "                 3 / 4 * (sed_rest_rest / a_T) ** 7),0)) -\\\n",
    "                (np.where(sed_ref_rest < a_T, (1 - 7 * (sed_ref_rest / a_T) ** 2 +\\\n",
    "                  35 / 4 * (sed_ref_rest / a_T) ** 3 -\\\n",
    "                  7 / 2 * (sed_ref_rest / a_T) ** 5 +\\\n",
    "                  3 / 4 * (sed_ref_rest / a_T) ** 7),0)) -\\\n",
    "                (np.where(sed_rest_ref < a_T, (1 - 7 * (sed_rest_ref / a_T) ** 2 +\\\n",
    "                  35 / 4 * (sed_rest_ref / a_T) ** 3 -\\\n",
    "                  7 / 2 * (sed_rest_ref / a_T) ** 5 +\\\n",
    "                  3 / 4 * (sed_rest_ref / a_T) ** 7),0)) +\\\n",
    "                 (np.where(sed_ref_ref < a_T, (1 - 7 * (sed_ref_ref / a_T) ** 2 +\\\n",
    "                  35 / 4 * (sed_ref_ref / a_T) ** 3 -\\\n",
    "                  7 / 2 * (sed_ref_ref / a_T) ** 5 +\\\n",
    "                  3 / 4 * (sed_ref_ref / a_T) ** 7),0)))\n",
    "    \n",
    "    return C_I\n",
    "\n",
    "def cartesian_dist_no_tile(x_1,x_2):\n",
    "    return np.concatenate([\n",
    "        np.transpose((x_1[:,0] - np.reshape(x_2[:,0],[x_2.shape[0],1]))),\n",
    "        np.transpose((x_1[:,1] - np.reshape(x_2[:,1],[x_2.shape[0],1]))),\n",
    "        np.transpose((x_1[:,2] - np.reshape(x_2[:,2],[x_2.shape[0],1])))],axis = 0) \n",
    "\n",
    "def cov_interface_gradients(hu_rest,hu_ref,sed_dips_rest,sed_dips_ref,a_T):\n",
    "    c_o_T = a_T**2/14/3\n",
    "    C_GI = hu_rest*np.where(sed_dips_rest < a_T,(- c_o_T * ((-14 / a_T ** 2) + 105 / 4 * sed_dips_rest / a_T ** 3 -\n",
    "                                  35 / 2 * sed_dips_rest ** 3 / a_T ** 5 +\n",
    "                                  21 / 4 * sed_dips_rest ** 5 / a_T ** 7)),0)-\\\n",
    "    hu_ref*np.where(sed_dips_ref < a_T,(-c_o_T * ((-14 / a_T ** 2) + 105 / 4 * sed_dips_ref / a_T ** 3 -\n",
    "                                  35 / 2 * sed_dips_ref ** 3 / a_T ** 5 +\n",
    "                                  21 / 4 * sed_dips_ref ** 5 / a_T ** 7)),0)\n",
    "    return C_GI\n",
    "\n",
    "def safe_arange(start, stop, step):\n",
    "    return step * np.arange(start / step, stop / step)\n",
    "\n",
    "def perpendicularity(G_1):\n",
    "    a = np.concatenate([np.ones([G_1.shape[0],G_1.shape[0]]),np.zeros([G_1.shape[0],G_1.shape[0]]),np.zeros([G_1.shape[0],G_1.shape[0]])],axis = 1)\n",
    "    b = np.concatenate([np.zeros([G_1.shape[0],G_1.shape[0]]),np.ones([G_1.shape[0],G_1.shape[0]]),np.zeros([G_1.shape[0],G_1.shape[0]])],axis = 1)\n",
    "    c = np.concatenate([np.zeros([G_1.shape[0],G_1.shape[0]]),np.zeros([G_1.shape[0],G_1.shape[0]]),np.ones([G_1.shape[0],G_1.shape[0]])],axis = 1)\n",
    "    return np.concatenate([a,b,c],axis = 0)    \n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 208,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-0.09639682  0.20843053  0.87282635]\n",
      "[ 0.3142771   0.98720912  1.73124139 ... 18.15833261 20.12180441\n",
      " 22.22597296]\n"
     ]
    }
   ],
   "source": [
    "G_t = np.array([[0,10,0],[13,3,0]])\n",
    "G_t_o = np.array([[0,1,0],[-1,1.7,0]])\n",
    "r_t = 20\n",
    "layer_t = np.array([[2,8,0],[6,7,0],[13,6,0],[18,8,0]])\n",
    "\n",
    "\n",
    "\n",
    "G_d = np.array([[0,0,10],[0,13,3]])\n",
    "G_d_o = np.array([[0,0,1],[0,-1,-1.7]])\n",
    "r_d = 20\n",
    "layer_d = np.array([[0,8,2],[0,7,6],[0,6,13],[0,8,18]])\n",
    "\n",
    "G_t = G_t\n",
    "\n",
    "G_t_o = G_t_o\n",
    "\n",
    "## defining interface data\n",
    "layer_t = layer_t\n",
    "\n",
    "## deining range\n",
    "a_T_t = r_t\n",
    "\n",
    "c_o_T = a_T_t**2/14/3\n",
    "\n",
    "G_t_tiled = np.tile(G_t,[3,1])\n",
    "\n",
    "h_u_t = cartesian_dist(G_t,G_t)\n",
    "h_v_t = h_u_t.T\n",
    "\n",
    "perpendicularity_matrix_t = perpendicularity(G_t)\n",
    "\n",
    "dist_tiled_t = squared_euclidean_distance(G_t_tiled,G_t_tiled)\n",
    "\n",
    "dist_tiled_t = dist_tiled_t + np.eye(dist_tiled_t.shape[0])\n",
    "\n",
    "C_G_t = cov_gradients(dist_tiled_t,h_u_t,h_v_t,a_T_t,perpendicularity_matrix_t)\n",
    "\n",
    "number_of_points_per_surface_t = np.array([layer_t.shape[0]])\n",
    "\n",
    "ref_layer_points_t,rest_layer_points_t = set_rest_ref_matrix(number_of_points_per_surface_t,layer_t)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "# input data\n",
    "\n",
    "## defining the dips position\n",
    "G_d = G_d\n",
    "\n",
    "G_d_o = G_d_o\n",
    "\n",
    "## defining interface data\n",
    "layer_d = layer_d\n",
    "\n",
    "## deining range\n",
    "a_T_d = r_d\n",
    "\n",
    "c_o_T = a_T_d**2/14/3\n",
    "\n",
    "G_d_tiled = np.tile(G_d,[3,1])\n",
    "\n",
    "h_u_d = cartesian_dist(G_d,G_d)\n",
    "h_v_d = h_u_d.T\n",
    "\n",
    "perpendicularity_matrix_d = perpendicularity(G_d)\n",
    "\n",
    "dist_tiled_d = squared_euclidean_distance(G_d_tiled,G_d_tiled)\n",
    "\n",
    "dist_tiled_d = dist_tiled_d + np.eye(dist_tiled_d.shape[0])\n",
    "\n",
    "C_G_d = cov_gradients(dist_tiled_d,h_u_d,h_v_d,a_T_d,perpendicularity_matrix_d)\n",
    "\n",
    "number_of_points_per_surface_d = np.array([layer_d.shape[0]])\n",
    "\n",
    "ref_layer_points_d,rest_layer_points_d = set_rest_ref_matrix(number_of_points_per_surface_d,layer_d)\n",
    "\n",
    "sed_rest_rest_d = squared_euclidean_distance(rest_layer_points_d,rest_layer_points_d)\n",
    "sed_ref_rest_d = squared_euclidean_distance(ref_layer_points_d,rest_layer_points_d)\n",
    "sed_rest_ref_d = squared_euclidean_distance(rest_layer_points_d,ref_layer_points_d)\n",
    "sed_ref_ref_d = squared_euclidean_distance(ref_layer_points_d,ref_layer_points_d)\n",
    "\n",
    "C_I_d = cov_interface(ref_layer_points_d,rest_layer_points_d,a_T_d)\n",
    "\n",
    "sed_dips_rest_d = squared_euclidean_distance(G_d_tiled,rest_layer_points_d)\n",
    "sed_dips_ref_d = squared_euclidean_distance(G_d_tiled,ref_layer_points_d)\n",
    "\n",
    "hu_rest_d = cartesian_dist_no_tile(G_d,rest_layer_points_d)\n",
    "hu_ref_d = cartesian_dist_no_tile(G_d,ref_layer_points_d)\n",
    "\n",
    "C_GI_d = cov_interface_gradients(hu_rest_d,hu_ref_d,sed_dips_rest_d,sed_dips_ref_d,a_T_d)\n",
    "C_IG_d = C_GI_d.T\n",
    "\n",
    "xx = safe_arange(0,20,1)\n",
    "yy = safe_arange(0,20,1)\n",
    "zz = safe_arange(0,20,1)\n",
    "XX,YY,ZZ = np.meshgrid(xx,yy,zz)\n",
    "X = (np.reshape(XX,[-1])).T\n",
    "Y = (np.reshape(YY,[-1])).T\n",
    "Z = (np.reshape(ZZ,[-1])).T\n",
    "X = np.zeros(X.shape)\n",
    "Y = np.zeros(Y.shape)\n",
    "grid_d = np.stack([X,Y,Z],axis = 1)\n",
    "\n",
    "\n",
    "hu_Simpoints_d = cartesian_dist_no_tile(G_d,grid_d)\n",
    "sed_dips_SimPoint_d = squared_euclidean_distance(G_d_tiled,grid_d)\n",
    "sed_rest_SimPoint_d = squared_euclidean_distance(rest_layer_points_d,grid_d)\n",
    "sed_ref_SimPoint_d = squared_euclidean_distance(ref_layer_points_d,grid_d)\n",
    "\n",
    "\n",
    "K_d = np.concatenate([np.concatenate([C_G_d,C_GI_d],axis = 1),np.concatenate([C_IG_d,C_I_d],axis = 1)],axis = 0)\n",
    "\n",
    "a1 = np.concatenate((np.ones(G_d.shape[0]),np.zeros(G_d.shape[0]),np.zeros(G_d.shape[0])))\n",
    "a2 = np.concatenate((np.zeros(G_d.shape[0]),np.ones(G_d.shape[0]),np.zeros(G_d.shape[0])))\n",
    "a3 = np.concatenate((np.zeros(G_d.shape[0]),np.zeros(G_d.shape[0]),np.ones(G_d.shape[0])))\n",
    "a3 = np.vstack([a1,a2,a3])\n",
    "\n",
    "a4 = np.stack([ref_layer_points_d[:,0] - rest_layer_points_d[:,0],\\\n",
    "                ref_layer_points_d[:,1] - rest_layer_points_d[:,1],\\\n",
    "                ref_layer_points_d[:,2] - rest_layer_points_d[:,2]])\n",
    "\n",
    "\n",
    "a51 = np.concatenate((2*G_d[:,0],np.zeros(G_d.shape[0]),np.zeros(G_d.shape[0])))\n",
    "\n",
    "a52 = np.concatenate((np.zeros(G_d.shape[0]),2*G_d[:,1],np.zeros(G_d.shape[0])))\n",
    "\n",
    "a53 = np.concatenate((np.zeros(G_d.shape[0]),np.zeros(G_d.shape[0]),2*G_d[:,2]))\n",
    "\n",
    "a54 = np.concatenate((G_d[:,1],G_d[:,0],np.zeros(G_d.shape[0])))\n",
    "\n",
    "a55 = np.concatenate((G_d[:,2],np.zeros(G_d.shape[0]),G_d[:,0]))\n",
    "\n",
    "a56 = np.concatenate((np.zeros(G_d.shape[0]),G_d[:,2],G_d[:,1]))\n",
    "\n",
    "a5 = np.vstack([a51,a52,a53,a54,a55,a56])\n",
    "\n",
    "a6 = np.stack([ref_layer_points_d[:,0]*ref_layer_points_d[:,0].T - rest_layer_points_d[:,0]*rest_layer_points_d[:,0].T , \\\n",
    "            ref_layer_points_d[:,1]*ref_layer_points_d[:,1].T - rest_layer_points_d[:,1]*rest_layer_points_d[:,1].T , \\\n",
    "            ref_layer_points_d[:,2]*ref_layer_points_d[:,2].T - rest_layer_points_d[:,2]*rest_layer_points_d[:,2].T, \\\n",
    "            ref_layer_points_d[:,0]*ref_layer_points_d[:,1] - rest_layer_points_d[:,0]*rest_layer_points_d[:,1],\\\n",
    "            ref_layer_points_d[:,0]*ref_layer_points_d[:,2] - rest_layer_points_d[:,0]*rest_layer_points_d[:,2],\\\n",
    "            ref_layer_points_d[:,1]*ref_layer_points_d[:,2] - rest_layer_points_d[:,1]*rest_layer_points_d[:,2]])\n",
    "\n",
    "\n",
    "\n",
    "D_d = np.concatenate([np.concatenate([a3,a4],axis=1),np.concatenate([a5,a6],axis=1)], axis=0)\n",
    "D_d_T = D_d.T\n",
    "\n",
    "zero_matrix_d = np.zeros([D_d_T.shape[1],D_d_T.shape[1]])\n",
    "K_D_d = np.concatenate([np.concatenate([K_d,D_d_T],axis = 1),np.concatenate([D_d,zero_matrix_d],axis = 1)],axis = 0)\n",
    "\n",
    "# build right side matrix of cokriging system\n",
    "bk_d = np.concatenate([G_d_o[:,0],G_d_o[:,1],G_d_o[:,2],np.zeros(K_D_d.shape[0]-G_d.shape[0]*3)],axis = 0)\n",
    "bk_d = np.reshape(bk_d,newshape = [bk_d.shape[0],1])\n",
    "\n",
    "# solve kriging weight\n",
    "w_d = np.linalg.lstsq(K_D_d,bk_d)[0]\n",
    "\n",
    "#######################################################\n",
    "grid_r=ref_layer_points_t\n",
    "# grid_r[:,0] = np.zeros(grid_r[:,0].shape)\n",
    "# grid_r[:,1] = np.zeros(grid_r[:,0].shape)\n",
    "grid_s=rest_layer_points_t\n",
    "# grid_s[:,0] = np.zeros(grid_s[:,0].shape)\n",
    "# grid_s[:,1] = np.zeros(grid_s[:,0].shape)\n",
    "\n",
    "hu_Simpoints_s = cartesian_dist_no_tile(G_d,grid_s)\n",
    "sed_dips_SimPoint_s = squared_euclidean_distance(G_d_tiled,grid_s)\n",
    "sed_rest_SimPoint_s = squared_euclidean_distance(rest_layer_points_d,grid_s)\n",
    "sed_ref_SimPoint_s = squared_euclidean_distance(ref_layer_points_d,grid_s)\n",
    "\n",
    "hu_Simpoints_r = cartesian_dist_no_tile(G_d,grid_r)\n",
    "sed_dips_SimPoint_r = squared_euclidean_distance(G_d_tiled,grid_r)\n",
    "sed_rest_SimPoint_r = squared_euclidean_distance(rest_layer_points_d,grid_r)\n",
    "sed_ref_SimPoint_r = squared_euclidean_distance(ref_layer_points_d,grid_r)\n",
    "\n",
    "# gradient contribution\n",
    "sigma_0_grad_r = w_d[:G_d.shape[0]*3] * (-hu_Simpoints_r*(sed_dips_SimPoint_r < a_T_d)*(- c_o_T * ((-14 / a_T_d ** 2) + \n",
    "                    105 / 4 * sed_dips_SimPoint_r / a_T_d ** 3 - \n",
    "                    35 / 2 * sed_dips_SimPoint_r ** 3 / a_T_d ** 5 + \n",
    "                    21 / 4 * sed_dips_SimPoint_r ** 5 / a_T_d ** 7)))\n",
    "\n",
    "sigma_0_grad_r = np.sum(sigma_0_grad_r,axis=0)\n",
    "# surface point contribution\n",
    "sigma_0_interf_r = -w_d[G_d.shape[0]*3:-9]*(c_o_T  * ((sed_rest_SimPoint_r < a_T_d)*(1 - 7 * (sed_rest_SimPoint_r / a_T_d) ** 2 +\n",
    "            35 / 4 * (sed_rest_SimPoint_r / a_T_d) ** 3 -\n",
    "            7 / 2 * (sed_rest_SimPoint_r / a_T_d) ** 5 +\n",
    "            3 / 4 * (sed_rest_SimPoint_r / a_T_d) ** 7) -\n",
    "            (sed_ref_SimPoint_r < a_T_d)*(1 - 7 * (sed_ref_SimPoint_r / a_T_d) ** 2 +\n",
    "            35 / 4 * (sed_ref_SimPoint_r / a_T_d) ** 3 -\n",
    "            7 / 2 * (sed_ref_SimPoint_r / a_T_d) ** 5 +\n",
    "            3 / 4 * (sed_ref_SimPoint_r / a_T_d) ** 7)))\n",
    "sigma_0_interf_r = np.sum(sigma_0_interf_r,axis = 0)\n",
    "\n",
    "# 2nd order drift contribution\n",
    "sigma_0_2nd_drift_1 = np.sum(grid_r* (w_d[-9:-6]).T,axis = 1)\n",
    "\n",
    "sigma_0_2nd_drift_xx = grid_r[:,0]*grid_r[:,0]* (w_d[-6]).T\n",
    "sigma_0_2nd_drift_yy = grid_r[:,1]*grid_r[:,1]* (w_d[-5]).T\n",
    "sigma_0_2nd_drift_zz = grid_r[:,2]*grid_r[:,2]* (w_d[-4]).T\n",
    "\n",
    "sigma_0_2nd_drift_xy = grid_r[:,0]*grid_r[:,1]* (w_d[-3]).T\n",
    "sigma_0_2nd_drift_xz = grid_r[:,0]*grid_r[:,2]* (w_d[-2]).T\n",
    "sigma_0_2nd_drift_yz = grid_r[:,1]*grid_r[:,2]* (w_d[-1]).T\n",
    "\n",
    "sigma_0_2nd_drift_r = sigma_0_2nd_drift_1 + sigma_0_2nd_drift_xx + sigma_0_2nd_drift_yy + sigma_0_2nd_drift_zz + sigma_0_2nd_drift_xy + sigma_0_2nd_drift_xz + sigma_0_2nd_drift_yz\n",
    "\n",
    "interpolate_result_r = sigma_0_grad_r+sigma_0_interf_r+sigma_0_2nd_drift_r\n",
    "\n",
    "# gradient contribution\n",
    "sigma_0_grad_s = w_d[:G_d.shape[0]*3] * (-hu_Simpoints_s*(sed_dips_SimPoint_s < a_T_d)*(- c_o_T * ((-14 / a_T_d ** 2) + \n",
    "                    105 / 4 * sed_dips_SimPoint_s / a_T_d ** 3 - \n",
    "                    35 / 2 * sed_dips_SimPoint_s ** 3 / a_T_d ** 5 + \n",
    "                    21 / 4 * sed_dips_SimPoint_s ** 5 / a_T_d ** 7)))\n",
    "\n",
    "sigma_0_grad_s = np.sum(sigma_0_grad_s,axis=0)\n",
    "# surface point contribution\n",
    "sigma_0_interf_s = -w_d[G_d.shape[0]*3:-9]*(c_o_T  * ((sed_rest_SimPoint_s < a_T_d)*(1 - 7 * (sed_rest_SimPoint_s / a_T_d) ** 2 +\n",
    "            35 / 4 * (sed_rest_SimPoint_s / a_T_d) ** 3 -\n",
    "            7 / 2 * (sed_rest_SimPoint_s / a_T_d) ** 5 +\n",
    "            3 / 4 * (sed_rest_SimPoint_s / a_T_d) ** 7) -\n",
    "            (sed_ref_SimPoint_s < a_T_d)*(1 - 7 * (sed_ref_SimPoint_s / a_T_d) ** 2 +\n",
    "            35 / 4 * (sed_ref_SimPoint_s / a_T_d) ** 3 -\n",
    "            7 / 2 * (sed_ref_SimPoint_s / a_T_d) ** 5 +\n",
    "            3 / 4 * (sed_ref_SimPoint_s / a_T_d) ** 7)))\n",
    "sigma_0_interf_s = np.sum(sigma_0_interf_s,axis = 0)\n",
    "\n",
    "# 2nd order drift contribution\n",
    "sigma_0_2nd_drift_1 = np.sum(grid_s* (w_d[-9:-6]).T,axis = 1)\n",
    "\n",
    "sigma_0_2nd_drift_xx = grid_s[:,0]*grid_s[:,0]* (w_d[-6]).T\n",
    "sigma_0_2nd_drift_yy = grid_s[:,1]*grid_s[:,1]* (w_d[-5]).T\n",
    "sigma_0_2nd_drift_zz = grid_s[:,2]*grid_s[:,2]* (w_d[-4]).T\n",
    "\n",
    "sigma_0_2nd_drift_xy = grid_s[:,0]*grid_s[:,1]* (w_d[-3]).T\n",
    "sigma_0_2nd_drift_xz = grid_s[:,0]*grid_s[:,2]* (w_d[-2]).T\n",
    "sigma_0_2nd_drift_yz = grid_s[:,1]*grid_s[:,2]* (w_d[-1]).T\n",
    "\n",
    "sigma_0_2nd_drift_s = sigma_0_2nd_drift_1 + sigma_0_2nd_drift_xx + sigma_0_2nd_drift_yy + sigma_0_2nd_drift_zz + sigma_0_2nd_drift_xy + sigma_0_2nd_drift_xz + sigma_0_2nd_drift_yz\n",
    "\n",
    "interpolate_result_s = sigma_0_grad_s+sigma_0_interf_s+sigma_0_2nd_drift_s\n",
    "\n",
    "final_result = interpolate_result_r - interpolate_result_s\n",
    "print(final_result)\n",
    "#######################################################\n",
    "\n",
    "\n",
    "# gradient contribution\n",
    "sigma_0_grad_d = w_d[:G_d.shape[0]*3] * (-hu_Simpoints_d*(sed_dips_SimPoint_d < a_T_d)*(- c_o_T * ((-14 / a_T_d ** 2) + 105 / 4 * sed_dips_SimPoint_d / a_T_d ** 3 - 35 / 2 * sed_dips_SimPoint_d ** 3 / a_T_d ** 5 + 21 / 4 * sed_dips_SimPoint_d ** 5 / a_T_d ** 7)))\n",
    "\n",
    "sigma_0_grad_d = np.sum(sigma_0_grad_d,axis=0)\n",
    "# surface point contribution\n",
    "sigma_0_interf_d = -w_d[G_d.shape[0]*3:-9]*(c_o_T  * ((sed_rest_SimPoint_d < a_T_d)*(1 - 7 * (sed_rest_SimPoint_d / a_T_d) ** 2 +\n",
    "            35 / 4 * (sed_rest_SimPoint_d / a_T_d) ** 3 -\n",
    "            7 / 2 * (sed_rest_SimPoint_d / a_T_d) ** 5 +\n",
    "            3 / 4 * (sed_rest_SimPoint_d / a_T_d) ** 7) -\n",
    "            (sed_ref_SimPoint_d < a_T_d)*(1 - 7 * (sed_ref_SimPoint_d / a_T_d) ** 2 +\n",
    "            35 / 4 * (sed_ref_SimPoint_d / a_T_d) ** 3 -\n",
    "            7 / 2 * (sed_ref_SimPoint_d / a_T_d) ** 5 +\n",
    "            3 / 4 * (sed_ref_SimPoint_d / a_T_d) ** 7)))\n",
    "sigma_0_interf_d = np.sum(sigma_0_interf_d,axis = 0)\n",
    "\n",
    "# 2nd order drift contribution\n",
    "grid_d[:,0] = np.zeros(grid_d[:,0].shape)\n",
    "grid_d[:,1] = np.zeros(grid_d[:,0].shape)\n",
    "\n",
    "sigma_0_2nd_drift_1 = np.sum(grid_d* (w_d[-9:-6]).T,axis = 1)\n",
    "\n",
    "sigma_0_2nd_drift_xx = grid_d[:,0]*grid_d[:,0]* (w_d[-6]).T\n",
    "sigma_0_2nd_drift_yy = grid_d[:,1]*grid_d[:,1]* (w_d[-5]).T\n",
    "sigma_0_2nd_drift_zz = grid_d[:,2]*grid_d[:,2]* (w_d[-4]).T\n",
    "\n",
    "sigma_0_2nd_drift_xy = grid_d[:,0]*grid_d[:,1]* (w_d[-3]).T\n",
    "sigma_0_2nd_drift_xz = grid_d[:,0]*grid_d[:,2]* (w_d[-2]).T\n",
    "sigma_0_2nd_drift_yz = grid_d[:,1]*grid_d[:,2]* (w_d[-1]).T\n",
    "\n",
    "sigma_0_2nd_drift_d = sigma_0_2nd_drift_1 + sigma_0_2nd_drift_xx + sigma_0_2nd_drift_yy + sigma_0_2nd_drift_zz + sigma_0_2nd_drift_xy + sigma_0_2nd_drift_xz + sigma_0_2nd_drift_yz\n",
    "\n",
    "\n",
    "interpolate_result_drift_d = sigma_0_grad_d+sigma_0_interf_d+sigma_0_2nd_drift_d\n",
    "print(interpolate_result_drift_d)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 209,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.        ]\n",
      " [ 0.        ]\n",
      " [ 0.        ]\n",
      " [ 0.        ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]\n",
      " [-0.09639682]\n",
      " [ 0.20843053]\n",
      " [ 0.87282635]]\n"
     ]
    }
   ],
   "source": [
    "# G_1 = np.array([[0,10,0],[13,3,0]])\n",
    "\n",
    "# G_1_o = np.array([[0,1,0],[-1,1.7,0]])\n",
    "\n",
    "# ## deining range\n",
    "# a_T = 20\n",
    "\n",
    "# layer1 = np.array([[2,8,0],[6,7,0],[13,6,0],[18,8,0]])\n",
    "\n",
    "# c_o_T = a_T**2/14/3\n",
    "\n",
    "# G_1_tiled = np.tile(G_1,[3,1])\n",
    "\n",
    "# h_u = cartesian_dist(G_1,G_1)\n",
    "# h_v = h_u.T\n",
    "\n",
    "# perpendicularity_matrix = perpendicularity(G_1)\n",
    "\n",
    "# dist_tiled = squared_euclidean_distance(G_1_tiled,G_1_tiled)\n",
    "\n",
    "# dist_tiled = dist_tiled + np.eye(dist_tiled.shape[0])\n",
    "\n",
    "# C_G = cov_gradients(dist_tiled)\n",
    "\n",
    "# number_of_layer = 1\n",
    "\n",
    "# number_of_points_per_surface = np.array([layer1.shape[0]])\n",
    "\n",
    "# ref_layer_points,rest_layer_points = set_rest_ref_matrix(number_of_points_per_surface)\n",
    "\n",
    "sed_rest_rest_t = squared_euclidean_distance(rest_layer_points_t,rest_layer_points_t)\n",
    "sed_ref_rest_t = squared_euclidean_distance(ref_layer_points_t,rest_layer_points_t)\n",
    "sed_rest_ref_t = squared_euclidean_distance(rest_layer_points_t,ref_layer_points_t)\n",
    "sed_ref_ref_t = squared_euclidean_distance(ref_layer_points_t,ref_layer_points_t)\n",
    "\n",
    "C_I_t = cov_interface(ref_layer_points_t,rest_layer_points_t,a_T_t)\n",
    "\n",
    "sed_dips_rest_t = squared_euclidean_distance(G_t_tiled,rest_layer_points_t)\n",
    "sed_dips_ref_t = squared_euclidean_distance(G_t_tiled,ref_layer_points_t)\n",
    "\n",
    "hu_rest_t = cartesian_dist_no_tile(G_t,rest_layer_points_t)\n",
    "hu_ref_t = cartesian_dist_no_tile(G_t,ref_layer_points_t)\n",
    "\n",
    "\n",
    "C_GI_t = cov_interface_gradients(hu_rest_t,hu_ref_t,sed_dips_rest_t,sed_dips_ref_t,a_T_t)\n",
    "C_IG_t = C_GI_t.T\n",
    "\n",
    "xx = safe_arange(0,20,1)\n",
    "yy = safe_arange(0,20,1)\n",
    "zz = safe_arange(0,20,1)\n",
    "XX,YY,ZZ = np.meshgrid(xx,yy,zz)\n",
    "X = (np.reshape(XX,[-1])).T\n",
    "Y = (np.reshape(YY,[-1])).T\n",
    "Z = (np.reshape(ZZ,[-1])).T\n",
    "grid_t = np.stack([X,Y,Z],axis = 1)\n",
    "\n",
    "\n",
    "hu_Simpoints = cartesian_dist_no_tile(G_t,grid_t)\n",
    "sed_dips_SimPoint_t = squared_euclidean_distance(G_t_tiled,grid_t)\n",
    "sed_rest_SimPoint_t = squared_euclidean_distance(rest_layer_points_t,grid_t)\n",
    "sed_ref_SimPoint_t = squared_euclidean_distance(ref_layer_points_t,grid_t)\n",
    "\n",
    "\n",
    "K_t = np.concatenate([np.concatenate([C_G_t,C_GI_t],axis = 1),np.concatenate([C_IG_t,C_I_t],axis = 1)],axis = 0)\n",
    "\n",
    "###########################\n",
    "\n",
    "a1 = np.concatenate((np.ones(G_t.shape[0]),np.zeros(G_t.shape[0]),np.zeros(G_t.shape[0])))\n",
    "a2 = np.concatenate((np.zeros(G_t.shape[0]),np.ones(G_t.shape[0]),np.zeros(G_t.shape[0])))\n",
    "a3 = np.concatenate((np.zeros(G_t.shape[0]),np.zeros(G_t.shape[0]),np.zeros(G_t.shape[0])))\n",
    "a3 = np.vstack([a1,a2,a3])\n",
    "\n",
    "a4 = np.stack([ref_layer_points_t[:,0] - rest_layer_points_t[:,0],\\\n",
    "                ref_layer_points_t[:,1] - rest_layer_points_t[:,1],\\\n",
    "                np.zeros((ref_layer_points_t[:,2] - rest_layer_points_t[:,2]).shape[0])])\n",
    "\n",
    "\n",
    "a51 = np.concatenate((2*G_t[:,0],np.zeros(G_t.shape[0]),np.zeros(G_t.shape[0])))\n",
    "\n",
    "a52 = np.concatenate((np.zeros(G_t.shape[0]),2*G_t[:,1],np.zeros(G_t.shape[0])))\n",
    "\n",
    "a53 = np.concatenate((np.zeros(G_t.shape[0]),np.zeros(G_t.shape[0]),np.zeros(G_t.shape[0])))\n",
    "\n",
    "a54 = np.concatenate((G_t[:,1],G_t[:,0],np.zeros(G_t.shape[0])))\n",
    "\n",
    "a55 = np.concatenate((G_t[:,2],np.zeros(G_t.shape[0]),np.zeros(G_t.shape[0])))\n",
    "\n",
    "a56 = np.concatenate((np.zeros(G_t.shape[0]),G_t[:,2],np.zeros(G_t.shape[0])))\n",
    "\n",
    "a5 = np.vstack([a51,a52,a53,a54,a55,a56])\n",
    "\n",
    "a6 = np.stack([ref_layer_points_t[:,0]*ref_layer_points_t[:,0].T - rest_layer_points_t[:,0]*rest_layer_points_t[:,0].T , \\\n",
    "            ref_layer_points_t[:,1]*ref_layer_points_t[:,1].T - rest_layer_points_t[:,1]*rest_layer_points_t[:,1].T , \\\n",
    "            np.zeros((ref_layer_points_t[:,2]*ref_layer_points_t[:,2].T - rest_layer_points_t[:,2]*rest_layer_points_t[:,2].T).shape[0]), \\\n",
    "            ref_layer_points_t[:,0]*ref_layer_points_t[:,1] - rest_layer_points_t[:,0]*rest_layer_points_t[:,1],\\\n",
    "            np.zeros((ref_layer_points_t[:,0]*ref_layer_points_t[:,2] - rest_layer_points_t[:,0]*rest_layer_points_t[:,2]).shape[0]),\\\n",
    "            np.zeros((ref_layer_points_t[:,1]*ref_layer_points_t[:,2] - rest_layer_points_t[:,1]*rest_layer_points_t[:,2]).shape[0])])\n",
    "\n",
    "D_U = np.concatenate([np.concatenate([a3,a4],axis=1),np.concatenate([a5,a6],axis=1)], axis=0)\n",
    "\n",
    "D_U_T = D_U.T\n",
    "\n",
    "########################################\n",
    "\n",
    "D_X = G_d_o[0][0]*np.ones([G_d.shape[0],1])\n",
    "D_Y = G_d_o[0][1]*np.ones([G_d.shape[0],1])\n",
    "D_Z = G_d_o[0][2]*np.ones([G_d.shape[0],1])\n",
    "\n",
    "D_I = final_result.reshape((layer_t.shape[0]-1,1))\n",
    "\n",
    "D_E = np.concatenate([D_X , D_Y , D_Z , D_I],axis=0)\n",
    "print(D_E)\n",
    "D_E_T = D_E.T\n",
    "\n",
    "\n",
    "D = np.concatenate([D_U,D_E_T],axis=0)\n",
    "D_T = D.T \n",
    "\n",
    "# build zero matrix\n",
    "zero_matrix = np.zeros([D_T.shape[1],D_T.shape[1]])\n",
    "\n",
    "\n",
    "# concatenate drift matrix and kriging matrix\n",
    "K_D = np.concatenate([np.concatenate([K_t,D_T],axis = 1),np.concatenate([D,zero_matrix],axis = 1)],axis = 0)\n",
    "\n",
    "# build right side matrix of cokriging system\n",
    "bk = np.concatenate([G_t_o[:,0],G_t_o[:,1],G_t_o[:,2],np.zeros(K_D.shape[0]-G_t.shape[0]*3)],axis = 0)\n",
    "bk = np.reshape(bk,newshape = [bk.shape[0],1])\n",
    "\n",
    "# solve kriging weight\n",
    "w = np.linalg.lstsq(K_D,bk)[0]\n",
    "\n",
    "# gradient contribution\n",
    "sigma_0_grad = w[:G_t.shape[0]*3] * (-hu_Simpoints*(sed_dips_SimPoint_t < a_T_t)*(- c_o_T * ((-14 / a_T_t ** 2) + 105 / 4 * sed_dips_SimPoint_t / a_T_t ** 3 - 35 / 2 * sed_dips_SimPoint_t ** 3 / a_T_t ** 5 + 21 / 4 * sed_dips_SimPoint_t ** 5 / a_T_t ** 7)))\n",
    "\n",
    "sigma_0_grad = np.sum(sigma_0_grad,axis=0)\n",
    "# surface point contribution\n",
    "sigma_0_interf = -w[G_t.shape[0]*3:-10]*(c_o_T  * ((sed_rest_SimPoint_t < a_T_t)*(1 - 7 * (sed_rest_SimPoint_t / a_T_t) ** 2 +\n",
    "            35 / 4 * (sed_rest_SimPoint_t / a_T_t) ** 3 -\n",
    "            7 / 2 * (sed_rest_SimPoint_t / a_T_t) ** 5 +\n",
    "            3 / 4 * (sed_rest_SimPoint_t / a_T_t) ** 7) -\n",
    "            (sed_ref_SimPoint_t < a_T_t)*(1 - 7 * (sed_ref_SimPoint_t / a_T_t) ** 2 +\n",
    "            35 / 4 * (sed_ref_SimPoint_t / a_T_t) ** 3 -\n",
    "            7 / 2 * (sed_ref_SimPoint_t / a_T_t) ** 5 +\n",
    "            3 / 4 * (sed_ref_SimPoint_t / a_T_t) ** 7)))\n",
    "sigma_0_interf = np.sum(sigma_0_interf,axis = 0)\n",
    "\n",
    "# 2nd order drift contribution\n",
    "sigma_0_2nd_drift_1 = np.sum(grid_t* (w[-10:-7]).T,axis = 1)\n",
    "\n",
    "sigma_0_2nd_drift_xx = grid_t[:,0]*grid_t[:,0]* (w[-7]).T\n",
    "sigma_0_2nd_drift_yy = grid_t[:,1]*grid_t[:,1]* (w[-6]).T\n",
    "sigma_0_2nd_drift_zz = grid_t[:,2]*grid_t[:,2]* (w[-5]).T\n",
    "\n",
    "sigma_0_2nd_drift_xy = grid_t[:,0]*grid_t[:,1]* (w[-4]).T\n",
    "sigma_0_2nd_drift_xz = grid_t[:,0]*grid_t[:,2]* (w[-3]).T\n",
    "sigma_0_2nd_drift_yz = grid_t[:,1]*grid_t[:,2]* (w[-2]).T\n",
    "\n",
    "sigma_0_2nd_drift = sigma_0_2nd_drift_1 + sigma_0_2nd_drift_xx + sigma_0_2nd_drift_yy + sigma_0_2nd_drift_zz + sigma_0_2nd_drift_xy + sigma_0_2nd_drift_xz + sigma_0_2nd_drift_yz\n",
    "\n",
    "external_drift = interpolate_result_drift_d * (w[-1]).T\n",
    "\n",
    "#external_drift = (interpolate_result_drift_d + grid_t[:,1]+ grid_t[:,0]) * (w[-1]).T\n",
    "\n",
    "interpolate_result = 1*sigma_0_grad+1*sigma_0_interf +1*sigma_0_2nd_drift + 1 * external_drift\n",
    "\n",
    "intp = np.reshape(interpolate_result,[20,20,20]) # reshape the result to matrix shape\n",
    "intp_c = intp\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 210,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pyvista as pv\n",
    "import pyvistaqt as pvqt\n",
    "def plot_3D(grid,value,mode='block',surfaces_nr=10):\n",
    "    x = grid[:,0]\n",
    "    y = grid[:,1]\n",
    "    z = grid[:,2]\n",
    "\n",
    "    grid = pv.UniformGrid(dims=(20,20,20), spacing=(1,1,1),origin=(0,0,0))\n",
    "    p = pvqt.BackgroundPlotter()\n",
    "    layer1_ = layer_t\n",
    "    if mode == 'block':\n",
    "        p.add_mesh(grid,scalars=value.flatten(),show_edges=False,style='surface')\n",
    "        p.add_axes()\n",
    "        p.show()\n",
    "\n",
    "    elif mode == 'surfaces':\n",
    "        grid['vol'] = value.ravel()\n",
    "        lvl_t=value[np.where((XX==layer_t[0,0])&(YY==layer_t[0,1])&(ZZ==layer_t[0,2]))]\n",
    "        contours_1 = grid.contour(np.linspace(value.min(),value.max(),surfaces_nr))\n",
    "        contours_2 = grid.contour(lvl_t)\n",
    "        p.add_mesh(contours_1, show_scalar_bar=True, label='scalar_field',style='surface')\n",
    "        p.add_mesh(contours_2, show_scalar_bar=True, label='scalar_field',style='surface',color='red')\n",
    "\n",
    "    p.add_points(layer_t[:,[2,0,1]], render_points_as_spheres=True,\n",
    "                      point_size=20.0)\n",
    "    p.add_points(layer_d[:,[2,0,1]], render_points_as_spheres=True,\n",
    "                      point_size=20.0 ,color='black')\n",
    "    p.add_axes()\n",
    "    p.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 211,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_3D(grid_t,intp,mode='surfaces',surfaces_nr=10)"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0df33be97819e9b207f5c04bacfce8f16811435c04c7f96a541a7ffcc8bf3e5e"
  },
  "kernelspec": {
   "display_name": "Python 3.9.12 ('gempy')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}