B样条曲面插值算法

 

前一阵子在搞曲面插值，网上的代码很少，自己就参考B样条曲面公式写了一个。代码如下：

矩阵类：

 

public class MatrixUtil { /// <summary> /// 构造函数 构造方阵 /// </summary> /// <param name="row">行列数相等矩阵</param> public MatrixUtil(int row) { m_data = new double[row, row]; } /// <summary> /// 构造函数 构造方阵 /// </summary> /// <param name="row">行数</param> /// <param name="col">列数</param> public MatrixUtil(int row, int col) { m_data = new double[row, col]; } /// <summary> /// 复制构造函数 /// </summary> /// <param name="m">需要被复制的矩阵</param> public MatrixUtil(MatrixUtil m) { int row = m.Row; int col = m.Col; m_data = new double[row, col]; for (int i = 0; i < row; i++) for (int j = 0; j < col; j++) m_data[i, j] = m[i, j]; } /// <summary> /// 对于已含有内存的矩阵，将清空数据 /// </summary> /// <param name="row">行</param> /// <param name="col">列</param> public void SetSize(int row,int col) { m_data = new double[row,col]; } /// <summary> /// unit MatrixUtil:设为单位阵 /// </summary> public void SetUnit() { for (int i = 0; i < m_data.GetLength(0); i++) for (int j = 0; j < m_data.GetLength(1); j++) m_data[i, j] = ((i == j) ? 1 : 0); } //设置元素值 public void SetValue(double d) { for (int i = 0; i < m_data.GetLength(0); i++) for (int j = 0; j < m_data.GetLength(1); j++) m_data[i, j] = d; } /// <summary> /// 返中行数 /// </summary> public int Row { get { return m_data.GetLength(0); } } /// <summary> /// 返回列数 /// </summary> public int Col { get { return m_data.GetLength(1); } } /// <summary> /// 重载索引,存取数据成员 /// </summary> /// <param name="row">行数</param> /// <param name="col">列数</param> /// <returns></returns> public double this[int row, int col] { get { return m_data[row, col]; } set { m_data[row, col] = value; } } /// <summary> /// 初等变换　对调两行：ri<-->rj /// </summary> /// <param name="i">对调行1</param> /// <param name="j">对调行2</param> /// <returns>变换后的矩阵</returns> public MatrixUtil Exchange(int i, int j) { double temp; for (int k = 0; k < Col; k++) { temp = m_data[i, k]; m_data[i, k] = m_data[j, k]; m_data[j, k] = temp; } return this; } /// <summary> /// 初等变换　第index 行乘以mul /// </summary> /// <param name="index">行索引</param> /// <param name="mul">数值</param> /// <returns>变换后的矩阵</returns> MatrixUtil Multiple(int index, double mul) { for (int j = 0; j < Col; j++) { m_data[index, j] *= mul; } return this; } /// <summary> /// 初等变换 第src行乘以mul加到第index行 /// </summary> /// <param name="index">加到第index行的行索引</param> /// <param name="src">第src行的行索引</param> /// <param name="mul">数值</param> /// <returns>变换后的矩阵</returns> MatrixUtil MultipleAdd(int index, int src, double mul) { for (int j = 0; j < Col; j++) { m_data[index, j] += m_data[src, j] * mul; } return this; } /// <summary> /// 矩阵 转置 /// </summary> /// <returns>转置后的矩阵</returns> public MatrixUtil Transpose() { MatrixUtil ret = new MatrixUtil(Col, Row); for (int i = 0; i < Row; i++) for (int j = 0; j < Col; j++) { ret[j, i] = m_data[i, j]; } return ret; } /// <summary> /// 矩阵 相加 /// </summary> /// <param name="lhs">矩阵1</param> /// <param name="rhs">矩阵2</param> /// <returns>相加后的矩阵</returns> public static MatrixUtil operator +(MatrixUtil lhs, MatrixUtil rhs) { if (lhs.Row != rhs.Row) //异常 { System.Exception e = new Exception("相加的两个矩阵的行数不等"); throw e; } if (lhs.Col != rhs.Col) //异常 { System.Exception e = new Exception("相加的两个矩阵的列数不等"); throw e; } int row = lhs.Row; int col = lhs.Col; MatrixUtil ret = new MatrixUtil(row, col); for (int i = 0; i < row; i++) for (int j = 0; j < col; j++) { double d = lhs[i, j] + rhs[i, j]; ret[i, j] = d; } return ret; } /// <summary> /// 矩阵 相减 /// </summary> /// <param name="lhs">矩阵1</param> /// <param name="rhs">矩阵2</param> /// <returns>相减后的矩阵</returns> public static MatrixUtil operator -(MatrixUtil lhs, MatrixUtil rhs) { if (lhs.Row != rhs.Row) //异常 { System.Exception e = new Exception("相减的两个矩阵的行数不等"); throw e; } if (lhs.Col != rhs.Col) //异常 { System.Exception e = new Exception("相减的两个矩阵的列数不等"); throw e; } int row = lhs.Row; int col = lhs.Col; MatrixUtil ret = new MatrixUtil(row, col); for (int i = 0; i < row; i++) for (int j = 0; j < col; j++) { double d = lhs[i, j] - rhs[i, j]; ret[i, j] = d; } return ret; } /// <summary> /// 矩阵 相乘 /// </summary> /// <param name="lhs">矩阵1</param> /// <param name="rhs">矩阵2</param> /// <returns>相乘后的矩阵</returns> public static MatrixUtil operator *(MatrixUtil lhs, MatrixUtil rhs) { //两个矩阵行列数不匹配时 if (lhs.Col != rhs.Row) { System.Exception e = new Exception("相乘的两个矩阵的行列数不匹配"); throw e; } MatrixUtil ret = new MatrixUtil(lhs.Row, rhs.Col); double temp; for (int i = 0; i < lhs.Row; i++) { for (int j = 0; j < rhs.Col; j++) { temp = 0; for (int k = 0; k < lhs.Col; k++) { temp += lhs[i, k] * rhs[k, j]; } ret[i, j] = temp; } } return ret; } /// <summary> /// 矩阵 相除 /// </summary> /// <param name="lhs">矩阵1</param> /// <param name="rhs">矩阵2</param> /// <returns>相除后的矩阵</returns> public static MatrixUtil operator /(MatrixUtil lhs, MatrixUtil rhs) { return lhs * rhs.Inverse(); } /// <summary> /// 单目加 /// </summary> /// <param name="m">矩阵</param> /// <returns>单目加后矩阵</returns> public static MatrixUtil operator +(MatrixUtil m) { MatrixUtil ret = new MatrixUtil(m); return ret; } /// <summary> /// 单目减 /// </summary> /// <param name="m">矩阵</param> /// <returns>单目减后矩阵</returns> public static MatrixUtil operator -(MatrixUtil m) { MatrixUtil ret = new MatrixUtil(m); for (int i = 0; i < ret.Row; i++) for (int j = 0; j < ret.Col; j++) { ret[i, j] = -ret[i, j]; } return ret; } /// <summary> /// 数乘 /// </summary> /// <param name="d">数值</param> /// <param name="m">矩阵</param> /// <returns>数乘后的矩阵</returns> public static MatrixUtil operator *(double d, MatrixUtil m) { MatrixUtil ret = new MatrixUtil(m); for (int i = 0; i < ret.Row; i++) for (int j = 0; j < ret.Col; j++) ret[i, j] *= d; return ret; } /// <summary> /// 数除 /// </summary> /// <param name="d">数值</param> /// <param name="m">矩阵</param> /// <returns>数除后的矩阵</returns> public static MatrixUtil operator /(double d, MatrixUtil m) { return d * m.Inverse(); } /// <summary> /// 返回列主元素的行号（在行号[row,Col)范围内查找第row列中绝对值最大的元素，返回所在行号） /// </summary> /// <param name="row">row为开始查找的行号</param> /// <returns></returns> int Pivot(int row) { int index = row; for (int i = row + 1; i < Row; i++) { if (m_data[i, row] > m_data[index, row]) index = i; } return index; } /// <summary> /// 逆阵：使用矩阵的初等变换，列主元素消去法 /// </summary> /// <returns>逆阵</returns> public MatrixUtil Inverse() { if (Row != Col) //异常,非方阵 { System.Exception e = new Exception("求逆的矩阵不是方阵"); throw e; } StreamWriter sw = new StreamWriter("..//annex//close_matrix.txt"); MatrixUtil tmp = new MatrixUtil(this); MatrixUtil ret = new MatrixUtil(Row); //单位阵 ret.SetUnit(); int maxIndex; double dMul; for (int i = 0; i < Row; i++) { maxIndex = tmp.Pivot(i); if (tmp.m_data[maxIndex, i] == 0) { System.Exception e = new Exception("求逆的矩阵的行列式的值等于0,"); throw e; } if (maxIndex != i) //下三角阵中此列的最大值不在当前行，交换 { tmp.Exchange(i, maxIndex); ret.Exchange(i, maxIndex); } ret.Multiple(i, 1 / tmp[i, i]); tmp.Multiple(i, 1 / tmp[i, i]); for (int j = i + 1; j < Row; j++) { dMul = -tmp[j, i] / tmp[i, i]; tmp.MultipleAdd(j, i, dMul); ret.MultipleAdd(j, i, dMul); } sw.WriteLine("tmp=/r/n" + tmp); sw.WriteLine("ret=/r/n" + ret); } sw.WriteLine("**=/r/n" + this * ret); for (int i = Row - 1; i > 0; i--) { for (int j = i - 1; j >= 0; j--) { dMul = -tmp[j, i] / tmp[i, i]; tmp.MultipleAdd(j, i, dMul); ret.MultipleAdd(j, i, dMul); } } sw.WriteLine("tmp=/r/n" + tmp); sw.WriteLine("ret=/r/n" + ret); sw.WriteLine("***=/r/n" + this * ret); sw.Close(); return ret; } /// <summary> /// 方阵 /// </summary> /// <returns></returns> public bool IsSquare() { return Row == Col; } /// <summary> /// 对称阵 /// </summary> /// <returns></returns> public bool IsSymmetric() { if (Row != Col) { return false; } for (int i = 0; i < Row; i++) { for (int j = i + 1; j < Col; j++) { if (m_data[i, j] != m_data[j, i]) { return false; } } } return true; } /// <summary> /// 一阶矩阵->实数 /// </summary> /// <returns></returns> public double ToDouble() { Trace.Assert(Row == 1 && Col == 1); return m_data[0, 0]; } /// <summary> /// conert to string /// </summary> /// <returns></returns> public override string ToString() { string s = ""; for (int i = 0; i < Row; i++) { for (int j = 0; j < Col; j++) s += string.Format("{0} ", m_data[i, j]); s += "/r/n"; } return s; } 

计算方法：

#region 计算曲面化数据 /// <summary> /// 保存初始X坐标 /// </summary> private double[] saveXPosition; /// <summary> /// 保存初始Y坐标 /// </summary> private double[] saveYPosition; /// <summary> /// 曲面密度 /// </summary> private int density = 3; /// <summary> /// 参数矩阵M数据 /// </summary> private double[] mArray = new double[] { -1, 3, -3, 1, 3, -6, 3, 0, -3, 0, 3, 0, 1, 4, 1, 0 }; /// <summary> /// 参数矩阵M /// </summary> private MatrixUtil m; /// <summary> /// 参数矩阵M的转置矩阵 /// </summary> private MatrixUtil mT; /// <summary> /// 参数矩阵U /// </summary> private MatrixUtil mU; /// <summary> /// 参数矩阵W /// </summary> private MatrixUtil mW; /// <summary> /// 计算曲面化数据 /// </summary> /// <param name="density">密度</param> /// <param name="data">数据数组</param> private float[][] CalSurfaceData(int density, float[][] data) { float[][] reArray = new float[data.GetLength(0) * density][]; for (int i = 0; i < data.GetLength(0) * density; i++) { reArray[i] = new float[data.GetLength(0) * density]; } List<double> ll = new List<double>(); //行 for (int i = 0; i < data.GetLength(0); i++) { if (i > data.GetLength(0) - 4) { break; } //列 for (int j = 0; j < data[i].GetLength(0); j++) { if (j > data[i].GetLength(0) - 4) { break; } //Z值矩阵 MatrixUtil mZ = new MatrixUtil(4); List<double> valList = new List<double>(); List<double[]> pointList = new List<double[]>(); List<double[]> insertPList = new List<double[]>(); List<int[]> indexList = new List<int[]>(); //从原始数据中，构造4*4矩阵数据 for (int m = 0; m < 4; m++) { for (int n = 0; n < 4; n++) { if (data[i + m][j + n] == CommonConst.POINT_LAND) { //被插值点位置集合 double[] array = new double[2]; array[0] = xPositionArray[i + m]; array[1] = yPositionArray[j + n]; insertPList.Add(array); indexList.Add(new int[] { m, n }); } else { mZ[m, n] = data[i + m][j + n]; valList.Add(data[i + m][j + n]); double[] array = new double[2]; array[0] = xPositionArray[i + m]; array[1] = yPositionArray[j + n]; pointList.Add(array); } } } //需要距离加权插值 if (insertPList.Count > 0) { if (insertPList.Count == 16) { for (int g = 0; g < mZ.Row; g++) { for (int h = 0; h < mZ.Col; h++) { mZ[g, h] = CommonConst.POINT_LAND; } } } else { double[] insertValArray = InsertValue(valList, pointList, insertPList); for (int h = 0; h < indexList.Count; h++) { mZ[indexList[h][0], indexList[h][1]] = insertValArray[h]; } } } //循环次数临时变量 int numa = 0; //计算曲面数据 for (double a = 0; a <= 1 + (density - 1) * (1.0 / density); a += (1.0 / density)) { int numb = 0; for (double b = 0; b <= 1 + (density - 1) * (1.0 / density); b += (1.0 / density)) { //初始化参数信息 InitParam(); InitParamUW(a, b); double z = CalculateSurface(mZ); reArray[density + i * density + numa][density + j * density + numb] = (float)z; numb++; } numa++; } } } return reArray; } /// <summary> /// 计算画面曲面坐标 /// </summary> /// <param name="density">经度</param> /// <returns></returns> private List<double[]> CalXYPosition(int density) { List<double[]> reList = new List<double[]>(); double xStep = (xPositionArray[xPositionArray.Length - 1] - xPositionArray[0]) / (xPositionArray.Length * density - 1); double yStep = (yPositionArray[0] - yPositionArray[yPositionArray.Length - 1]) / (xPositionArray.Length * density - 1); double[] xArray = new double[xPositionArray.Length * density]; double[] yArray = new double[xPositionArray.Length * density]; for (int i = 0; i < xPositionArray.Length * density; i++) { xArray[i] = xPositionArray[0] + i * xStep; yArray[i] = yPositionArray[0] - i * yStep; } reList.Add(xArray); reList.Add(yArray); return reList; } /// <summary> /// 初始化参数信息 /// </summary> private void InitParam() { //初始化参数m m = new MatrixUtil(4); for (int i = 0; i < m.Row; i++) { for (int j = 0; j < m.Col; j++) { m[i, j] = mArray[i * 4 + j]; } } m = 1.0 / 6.0 * m; //初始化参数mT mT = m.Transpose(); } /// <summary> /// 初始化参数U、W信息 /// </summary> /// <param name="paramU">参数U</param> /// <param name="paramW">参数W</param> private void InitParamUW(double paramU, double paramW) { //初始化参数矩阵U mU = new MatrixUtil(1, 4); for (int k = 0; k < mU.Col; k++) { if (k < 3) { mU[0, k] = Math.Pow(paramU, mU.Col - k - 1); } else { mU[0, k] = 1; } } //初始化参数矩阵W mW = new MatrixUtil(4, 1); for (int j = 0; j < mW.Row; j++) { if (j < 3) { mW[j, 0] = Math.Pow(paramW, mW.Row - j - 1); } else { mW[j, 0] = 1; } } } /// <summary> /// 计算曲面数据 /// </summary> /// <param name="mCoordinate">控制点矩阵 4*4 矩阵</param> private double CalculateSurface(MatrixUtil mCoordinate) { MatrixUtil k = mU * m * mCoordinate * mT * mW; double re = k[0, 0]; return re; } /// <summary> /// 计算边界数据 /// </summary> /// <param name="density">密度</param> private void CalBoundVal(int density) { for (int i = 0; i < dataArray.GetLength(0); i++) { for (int j = 0; j < dataArray[i].GetLength(0); j++) { //是边界位置 if (dataArray[i][j] == 0) { List<double> valList = new List<double>(); List<double[]> xyList = new List<double[]>(); int indexLeftTopX = i / density; int indexLeftTopY = j / density; if (indexLeftTopX == tmpData.GetLength(0) - 1) { indexLeftTopX -= 1; } if (indexLeftTopY == tmpData[0].GetLength(0) - 1) { indexLeftTopY -= 1; } int indexLeftBottomX = indexLeftTopX + 1; int indexRigthTopY = indexLeftTopY + 1; if (tmpData[indexLeftTopX][indexLeftTopY] != CommonConst.POINT_LAND) { //左上点值 valList.Add(tmpData[indexLeftTopX][indexLeftTopY]); //左上点坐标 xyList.Add(new double[] { saveXPosition[indexLeftTopX], saveYPosition[indexLeftTopY] }); } if (tmpData[indexLeftBottomX][indexLeftTopY] != CommonConst.POINT_LAND) { //左下点值 valList.Add(tmpData[indexLeftBottomX][indexLeftTopY]); //左下点坐标 xyList.Add(new double[] { saveXPosition[indexLeftBottomX], saveYPosition[indexLeftTopY] }); } if (tmpData[indexLeftTopX][indexRigthTopY] != CommonConst.POINT_LAND) { //右上点值 valList.Add(tmpData[indexLeftTopX][indexRigthTopY]); //右上点坐标 xyList.Add(new double[] { saveXPosition[indexLeftTopX], saveYPosition[indexRigthTopY] }); } if (tmpData[indexLeftBottomX][indexRigthTopY] != CommonConst.POINT_LAND) { //右下点值 valList.Add(tmpData[indexLeftBottomX][indexRigthTopY]); //右下点坐标 xyList.Add(new double[] { saveXPosition[indexLeftBottomX], saveYPosition[indexRigthTopY] }); } //被插值点坐标 List<double[]> insertPosition = new List<double[]>(); insertPosition.Add(new double[] { xPositionArray[i], yPositionArray[j] }); //全是陆地点 if (valList.Count == 0) { dataArray[i][j] = CommonConst.POINT_LAND; } //不全是陆地点 else { dataArray[i][j] = (float)InsertValue(valList, xyList, insertPosition)[0]; } } } } } /// <summary> /// 距离加权插值 /// </summary> /// <param name="_valueList">点的值集合</param> /// <param name="_positonList">点的位置集合</param> /// <param name="_insertPostion">被插值点的位置集合</param> /// <returns>插值后的值</returns> private double[] InsertValue(List<double> _valueList, List<double[]> _positonList, List<double[]> _insertPostion) { // 点的值= （p1/p1到点的距离+ p2/p2到点的距离+ p3/p3到点的距离+ p4/p4到点的距离）/ //（1/p1到点的距离+ 1/p2到点的距离+ 1/p3到点的距离+1/p4到点的距） //double re = 0f; double[] re = new double[_insertPostion.Count]; for (int k = 0; k < _insertPostion.Count; k++) { List<double> distanceList = new List<double>(); for (int i = 0; i < _valueList.Count; i++) { double p = Math.Sqrt( Math.Pow(_positonList[0][0] - _insertPostion[k][0], 2) + Math.Pow(_positonList[i][1] - _insertPostion[k][1], 2)); distanceList.Add(p); } double sum1 = 0; double sum2 = 0; for (int i = 0; i < distanceList.Count; i++) { sum1 += _valueList[i] / distanceList[i]; sum2 += 1 / distanceList[i]; } re[k] = sum1 / sum2; } return re; } #endregion 

注：计算曲面化数据方法CalSurfaceData为计算的主要方法。举例：

我传入一个61*61的二维数组float[][] data,密度density为3。曲面插值后的数据就应该为183*183。上面代码我只对Z值进行插值，因为是规则的矩形化网格，X坐标。Y坐标只进行了普通插值。下图为插值实现方式：

即由周围16个红点位置插值出灰色区域内的XYZ值。 所有插值完成之后 还要进行补插周围一圈的2*2矩阵值。CalBoundVal方法即为边界插值。因为我插值的数据是河道数据，可能包含陆地点，所以我在上述代码中加入了一些关于陆地点插值的判断。代码写的比较多，难免有疏漏，就是提供个参考吧。