北航数值分析作业三

from  math import *

t_table = [0, 0.2, 0.4, 0.6, 0.8, 1.0]
th = 0.2
u_table = [0, 0.4, 0.8, 1.2, 1.6, 2]
uh = 0.4
z_table = [[-0.5, -0.34, 0.14, 0.94, 2.06, 3.5],
           [-0.42, -0.5, -0.26, 0.3, 1.18, 2.38],
           [-0.18, -0.5, -0.5, -0.18, 0.46, 1.42],
           [0.22, -0.34, -0.58, -0.5, -0.1, 0.62],
           [0.78, -0.02, -0.5, -0.66, -0.5, -0.02],
           [1.5, 0.46, -0.26, -0.66, -0.74, -0.5]
           ]
table_x = [0.08 * i for i in range(11)]
table_y = [0.5 + 0.05 * i for i in range(21)]
table_z = [[0 for i in range(len(table_y))] for j in range(len(table_x))]
epsilon = 1e-12
sigma_limit = 1e-7

# 向量的无穷范数
vector_norm = lambda *x: max([abs(x[i]) for i in range(len(x))])

# 列主元高斯消去法求解线性方程组
def gauss(a, b):
   for i in range(len(b)):
      maxrow = i
      for j in range(i + 1, len(b)):
         if abs(a[j][i]) > abs(a[maxrow][i]):
            maxrow = j
      # 换行
      a[maxrow], a[i] = a[i], a[maxrow]
      b[maxrow], b[i] = b[i], b[maxrow]
      for j in range(i + 1, len(b)):
         k = a[j][i] / a[i][i]
         for col in range(i, len(a[0])):
            a[j][col] -= k * a[i][col]
         b[j] -= k * b[i]
   # 回代过程
   for i in range(len(b) - 1, -1, -1):
      for col in range(i + 1, len(a[0])):
         b[i] -= a[i][col] * b[col]
      b[i] /= a[i][i]
   return b


# 矩阵转置
def transform(x):
   row_cnt, col_cnt = len(x), len(x[0])
   y = [[0 for i in range(row_cnt)] for j in range(col_cnt)]
   for i in range(row_cnt):
      for j in range(col_cnt):
         y[j][i] = x[i][j]
   return y


# 矩阵相乘
def matrix_mul_matrix(x, y):
   row_cnt, col_cnt = len(x), len(y[0])
   z = [[0 for i in range(col_cnt)] for j in range(row_cnt)]
   for i in range(row_cnt):
      for j in range(col_cnt):
         for k in range(len(x[0])):
            z[i][j] += x[i][k] * y[k][j]
   return z

#牛顿迭代法
def newton_iterate(t, u, v, w, b):
   f = [0.5 * cos(t) + u + v + w - b[0],
        t + 0.5 * sin(u) + v + w - b[1],
        0.5 * t + u + cos(v) + w - b[2],
        t + 0.5 * u + v + sin(w) - b[3]]
   f = [-1 * i for i in f]
   ff = [[-0.5 * sin(t), 1, 1, 1],
         [1, 0.5 * cos(u), 1, 1],
         [0.5, 1, -sin(v), 1],
         [1, 0.5, 1, cos(w)]]
   delta = gauss(ff, f)
   t, u, v, w = t + delta[0], u + delta[1], v + delta[2], w + delta[3]
   return t, u, v, w


# 迭代法求解非线性方程组
def solve(x, y):
   t, u, v, w = 1, 2, 1, 2
   b = [2.67 + x, 1.07 + y, 3.74 + x, 0.79 + y]
   while 1:
      tt, uu, vv, ww = newton_iterate(t, u, v, w, b)
      if vector_norm(tt - t, uu - u, vv - v, ww - w) / vector_norm(tt, uu, vv, ww) < epsilon:
         return tt, uu, vv, ww
      t, u, v, w = tt, uu, vv, ww


# 二次分片插值查表根据t,u插值求出z
def interpolate(t, u):
   ti = round(t / th)
   ui = round(u / uh)
   ti = min(len(t_table) - 2, max(1, ti))
   ui = min(len(u_table) - 2, max(1, ui))
   ans = 0
   for i in range(ti - 1, ti + 2):
      for j in range(ui - 1, ui + 2):
         l = 1
         for k in range(ti - 1, ti + 2):
            if k != i:
               l *= t - t_table[k]
               l /= t_table[i] - t_table[k]
         for k in range(ui - 1, ui + 2):
            if k != j:
               l *= u - u_table[k]
               l /= u_table[j] - u_table[k]
         ans += z_table[i][j] * l
   return ans

#根据x,y求z
def getZ():
   for xi, x in enumerate(table_x):
      for yi, y in enumerate(table_y):
         t, u, v, w = solve(x, y)
         z = interpolate(t, u)
         table_z[xi][yi] = z


# 列主元高斯消去法求逆矩阵
def inverse(x):
   n = len(x)
   I = [[int(i == j) for i in range(n)] for j in range(n)]
   # 增广矩阵
   ex = [x[i] + I[i] for i in range(n)]
   for i in range(n):
      max_row = i
      for j in range(i + 1, n):
         if abs(ex[j][i]) > abs(ex[max_row][i]):
            max_row = j
      ex[i], ex[max_row] = ex[max_row], ex[i]
      for j in range(i + 1, n):
         k = ex[j][i] / ex[i][i]
         for col in range(n * 2):
            ex[j][col] -= k * ex[i][col]
      # 将当前行第一个数字变为1
      k = ex[i][i]
      for j in range(i, 2 * n):
         ex[i][j] /= k
   # 开始回溯
   for i in range(n - 1, -1, -1):
      for j in range(i + 1, n):  # 倍数ex[i][j]
         for k in range(n, 2 * n):
            ex[i][k] -= ex[i][j] * ex[j][k]
         ex[i][j] = 0
   # 后半部分变为逆矩阵
   ans = [[ex[i][j + n] for j in range(n)] for i in range(n)]
   return ans


# 为了让矩阵乘法可读性更强
class Matrix:
   def __init__(self, matrix):
      self.matrix = matrix

   def mul(self, x):
      return Matrix(matrix_mul_matrix(self.matrix, x))


def get_sigma(k, C, cout=False):
   s = 0
   for xi, x in enumerate(table_x):
      for yi, y in enumerate(table_y):
         fxy = table_z[xi][yi]
         x_vector = [[table_x[xi] ** i for i in range(k + 1)]]  # 行向量
         y_vector = [[table_y[yi] ** i] for i in range(k + 1)]  # 列向量
         pxy = Matrix(x_vector).mul(C).mul(y_vector).matrix[0][0]
         s += (fxy - pxy) ** 2
         if cout:
            print(x, y, fxy, pxy)
   return s

#最小二乘法曲面拟合
def least_square():
   k = 1
   while 1:
      B = [[table_x[j] ** i for i in range(k + 1)] for j in range(len(table_x))]
      G = [[table_y[j] ** i for i in range(k + 1)] for j in range(len(table_y))]
      C = Matrix(inverse(matrix_mul_matrix(transform(B), B))) 
         .mul(transform(B)) 
         .mul(table_z) 
         .mul(G) 
         .mul(inverse(matrix_mul_matrix(transform(G), G))).matrix
      sigma = get_sigma(k, C)
      print(k, sigma)
      if sigma < sigma_limit:
         return k, sigma, C
      else:
         k += 1


getZ()
print("打印x,y,z")
for xi, x in enumerate(table_x):
   for yi, y in enumerate(table_y):
      print(x, y, (table_z[xi][yi]))
print("开始选择k并计算sigma")
ans_k, ans_sigma, ans_C = least_square()
print("最终选择的k,sigma,和系数矩阵为")
print(ans_k, ans_sigma, ans_C)
print("打印x,y,f(x,y),p(x,y)")
get_sigma(ans_k, ans_C, cout=True)