# -*- coding: utf-8 -*-
"""
Created on Sat Apr 11 17:39:16 2020

@author: L JL
"""
import numpy as np 
import matplotlib.pyplot as plt 
from mpl_toolkits.mplot3d import Axes3D

f = open(‘xuanze.txt‘,‘r‘)
point = f.read()
f.close()
data1 = point.replace(‘\n‘,‘ ‘)
data2 = data1.split(‘ ‘)
data2.pop()
n = len(data2)
data2 = list(map(float,data2))

mat1 = np.array(data2[0:n]) 
mat2 = mat1.reshape(int(n/3),3)

mat3 = []

for each in mat2:
    each_line = list(map(lambda x:float(x),each))
    mat3.append(each_line)
mat4=np.array(mat3)
x = [k[0] for k in mat4]
y = [k[1] for k in mat4]
z = [k[2] for k in mat4]

def D_radius(x,y,X,Y,N):
    ind_mat = np.zeros((N,2))
    for i in range(N):
        s = ((x-X[i])**2+(y-Y[i])**2)**0.5
        ind_mat[i][0] = s
        ind_mat[i][1] = i
    ind_mat = ind_mat[np.lexsort(ind_mat[:,::-1].T)]
    return ind_mat
    
def W_mat(d,x0,y0,x,y): 
    s=(((x-x0)**2+(y-y0)**2)**0.5)/d
    #print((x-x0)**2+(y-y0)**2)
    #print(s)
    if (s<=0.5):
        return (2/3)-4*s**2+4*s**3 
    elif(s<=1):
        return (4/3)-4*s+4*s**2-(4/3)*s**3 
    else: return 0

def A_mat(W,P,M,N):
    A = []
    for m in range(M):
        a = []
        for n in range(M):
            #pmn = p_mn(W,P,N,m,n)
            pmn = 0
            for i in range(N):
                pmn = pmn + W[i]*P[i,m]*P[i,n]
            a.append(float(pmn))
        A.append(a)
    return A

def B_mat(u,W,P,M,N):
    B = []
    for i in range(M):
        sumB = 0
        for j in range(N):
            sumB = sumB + W[j]*P[j,i]*u[j]
        B.append(float(sumB))
    return B

X = np.array(x)
Y = np.array(y)
Z = np.array(z)
Xmax = int(np.max(X))
Xmin = int(np.min(X))
Ymax = int(np.max(Y))
Ymin = int(np.min(Y))


N = len(X)
M = 3
P = np.mat(np.zeros((N,M)))
W = np.mat(np.zeros((N,1)))
A = np.mat(np.zeros((M,M)))
B = np.mat(np.zeros((M,N)))
u = np.mat(Z).T
a = np.mat(np.zeros((M,1)))
d_mat = np.mat(np.zeros((N,1)))
#dataZ = []

dataX = np.arange(Xmin,Xmax,0.5)
dataY = np.arange(Ymin,Ymax,0.5)

print(Xmin,Xmax)
print(Ymin,Ymax)

f2 = open("dataZ.txt","w")
for i in dataX:
    for j in dataY:
        #d = D_radius(i,j,X,Y,N)
        d = 2
        ind_mat = D_radius(i,j,X,Y,N)
        #print(ind_mat)
        if ind_mat[3,0] <= d:
            try:
                W = np.mat(np.zeros((N,1)))
                for n in range(0,N):
                    P[n,0] = 1 
                    P[n,1] = X[n]
                    P[n,2] = Y[n] 
                    W[n] = W_mat(d,X[n],Y[n],i,j)
                #print(W)
                A = A_mat(W,P,M,N)
                B = B_mat(u,W,P,M,N)
                c = np.linalg.solve(A,B) 
                #dataZ.append(c[0]+c[1]*i+c[2]*j)
                dataZ = c[0]+c[1]*i+c[2]*j
                print(‘A‘)
                #print(dataZ)
            except:
                ind_Zsum = 0
                for ind in range(0,4):
                    ind_Zsum += Z[int(ind_mat[ind,1])]
                #dataZ.append(ind_Zsum/4)
                dataZ = ind_Zsum/4
                print(‘B‘)
                #print(dataZ)
        else:
            ind_Zsum = 0
            for ind in range(0,4):
                ind_Zsum += Z[int(ind_mat[ind,1])]
            #dataZ.append(ind_Zsum/4)
            dataZ = ind_Zsum/4
            print(‘C‘)
            #print(dataZ)
        f2.write(str(i) + ‘,‘ + str(j) + ‘,‘ + str(dataZ) + ‘\n‘)          
        print(i,j)

f2.close()        

# 3D绘图示意
import numpy as np 
import matplotlib.pyplot as plt 
from mpl_toolkits.mplot3d import Axes3D

fig = plt.figure()
ax = Axes3D(fig)
Xmin = -82
Xmax = 23
Ymin = 0
Ymax = 42

x = np.arange(Xmin, Xmax, 0.5)
y = np.arange(Ymin, Ymax, 0.5)

y,x  = np.meshgrid(y, x)

f = open(‘dataZ.txt‘,‘r‘)
point = f.read()
f.close()
data1 = point.replace(‘\n‘,‘,‘)
data2 = data1.split(‘,‘)
data2.pop()
n = len(data2)
data2 = list(map(float,data2))

mat1 = np.array(data2[0:n])
mat2 = mat1.reshape(int(n/3),3)

z = mat2[:,2].reshape(x.shape)
‘‘‘
print("网格化后的X=",x)
print("X维度信息",x.shape)
print("网格化后的Y=",y)
print("Y维度信息", y.shape)
print("网格化后的Z=",z)
‘‘‘

ax.plot_surface(x, y, z, cmap=plt.cm.hot)      # 渐变颜色
#ax.contourf(x, y, z,cmap=plt.cm.hot)
ax.set_xlabel(‘X Label (m)‘)
ax.set_ylabel(‘Y Label (m)‘)
ax.set_zlabel(‘Z Label (m)‘)
ax.set_title(‘Point Cloud Surface Fitting‘)
plt.show()