时间点:(2019.06.17)
在实现很多数学相关的功能的时候, 数学模型的选择尤为重要, 一个是在准确性上, 一个是在扩展性上.
比如最近我要计算一个射线跟一个线段的交点, 初中的时候就学过, 直线和直线外一点的交点怎样计算, 这里
直接上已经写完的两段代码.
简图
因为是计算机, 肯定是通过两点来确定直线方程了, 首先是用点斜式的计算方法( 原始方程 y = kx + b, 推导过程略 ):
////// 点斜式推理出的交点方程 /// /// /// /// /// /// [System.Obsolete("Incorrect", false)] public static bool IntersectRayAndLineSegment_XOZ_PointSlope(Ray ray, Vector3 p1, Vector3 p2, out Vector3 point) { var p3 = new Vector3(ray.origin.x, 0, ray.origin.z); float k1 = (p2.z - p1.z) / (p2.x - p1.x); float k2 = -1.0f / k1; float b1 = p1.z - (k1 * p1.x); float b2 = p3.z - (k2 * p3.x); float x = (b2 - b1) / (k1 - k2); float z = x * k1 + b1; point = new Vector3(x, 0, z); var rayDir = ray.direction; rayDir.y = 0; bool intersect = Vector3.Dot(point - p3, rayDir) > 0; if(intersect) { intersect = (x >= Mathf.Min(p1.x, p2.x) && x <= Mathf.Max(p1.x, p2.x)) && (z >= Mathf.Min(p1.z, p2.z) && z <= Mathf.Max(p1.z, p2.z)); } return intersect; }
然后是两点式的计算方法( 原始方程 (y-y1)/(x-x1) = (y-y2)/(x-x2), 推导过程略 ):
////// 两点式推出的交点方程 /// /// /// /// /// /// public static bool IntersectRayAndLineSegment_XOZ_TwoPoint(Ray ray, Vector3 p1, Vector3 p2, out Vector3 point) { point = Vector3.zero; Vector3 p3 = new Vector3(ray.origin.x, 0, ray.origin.z); Vector3 p4 = ray.GetPoint(1.0f); p4.y = 0; float a1, b1, c1; PointToLineEquation_2D_XOZ(p1, p2, out a1, out b1, out c1); float a2, b2, c2; PointToLineEquation_2D_XOZ(p3, p4, out a2, out b2, out c2); float D = a1 * b2 - a2 * b1; if(D == 0) { return false; } float D1 = -c1 * b2 + c2 * b1; float D2 = -c2 * a1 + a2 * c1; point = new Vector3(D1 / D, 0, D2 / D); var rayDir = ray.direction; rayDir.y = 0; bool intersect = Vector3.Dot(point - p3, rayDir) > 0; if(intersect) { intersect = (point.x >= Mathf.Min(p1.x, p2.x) && point.x <= Mathf.Max(p1.x, p2.x)) && (point.z >= Mathf.Min(p1.z, p2.z) && point.z <= Mathf.Max(p1.z, p2.z)); } return intersect; } public static void PointToLineEquation_2D_XOZ(Vector3 p1, Vector3 p2, out float a, out float b, out float c) { float x1 = p1.x; float y1 = p1.z; float x2 = p2.x; float y2 = p2.z; a = y2 - y1; b = x1 - x2; c = (y1 * x2) - (y2 * x1); }
在大部分情况下它们的结果是正确的, 可是在线段是平行于坐标轴的情况下, 点斜式的结果就不对了, 往回看计算过程:
var p3 = new Vector3(ray.origin.x, 0, ray.origin.z); float k1 = (p2.z - p1.z) / (p2.x - p1.x); float k2 = -1.0f / k1; float b1 = p1.z - (k1 * p1.x); float b2 = p3.z - (k2 * p3.x); float x = (b2 - b1) / (k1 - k2); float z = x * k1 + b1;
点斜式在刚开始就计算了斜率k, 然后k被作为了分母参与计算, 这样在平行于x轴的时候斜率就是0, 被除之后得到无穷大(或无穷小), 在平行y轴的时候( 两点的x相等 ), k直接就是无穷大(或无穷小).
所以点斜式在计算平行于坐标轴的情况就不行了. 点斜式的问题就在于过早进行了除法计算, 而且分母还可能为0, 这在使用计算机的系统里面天生就存在重大隐患.
然后是两点式, 它的过程是由两点式推算出一般方程的各个变量( 一般方程 ax + by + c = 0 ), 然后联立两个一般方程来进行求解, 它的稳定性就体现在这里:
a = y2 - y1; b = x1 - x2; c = (y1 * x2) - (y2 * x1);
它的初始计算完全不涉及除法, 第一步天生就是计算上稳定的.
然后是计算联立方程的方法, 这里直接用了二元一次线性方程组的求解:
float D = a1 * b2 - a2 * b1; if(D == 0) { return false; } float D1 = -c1 * b2 + c2 * b1; float D2 = -c2 * a1 + a2 * c1; point = new Vector3(D1 / D, 0, D2 / D);
使用行列式计算的好处是很容易判断出是否有解, D==0 时就是无解, 这也是计算稳定性的保证, 然后这里是只计算x, z平面的, 也就是2D的,
如果要计算3D的或更高阶的, 只需要把行列式和一般方程封装成任意元的多元方法即可, 例如一般方程为( ax + by + cz + d = 0 )甚至更高阶的( ax + by + cz + dw......+n = 0 ),
然后行列式求多元线性方程组的解就更简单了, 这就提供了很强大的扩展性, 不仅2维, 3维, 甚至N维都能提供解.
所以在做功能前仔细想好怎样做, 慎重选择数学模型是非常重要的. 虽然这些活看似初中生也能做, 大学生也能做, 差距还是一目了然的吧.
PS : 在射线和直线重合的时候, 这个方法也是无解的, 因为首先射线跟直线是平行的, 行列式无解, 还要做一次与判断才行(2019.06.19)
PS : 计算精度问题今天碰到了, 下图判断点是否在线段内的时候会出错... 后续会进行修改(2019.06.19)
PS : 忘了在射线与线段平行时也有交点的情况, 仍然需要计算交点.... 后续会进行修改(2019.06.20)
时间点:(2019.06.18)
补充一下上面说过的多元线性方程组的解, 分成行列式和线性方程组:
public class Determinant { public int order { get; private set; } public float[,] matrix { get; private set; } private int m_index = 0; public Determinant(int order) { if(order < 2) { throw new System.Exception("order >= 2 !!"); } this.order = order; CreateMatrix(); } #region Main Funcs public void SetColumn(int column, params float[] values) { if(column >= 0 && column < order && values != null) { for(int i = 0, imax = Mathf.Min(order, values.Length); i < imax; i++) { matrix[i, column] = values[i]; } } } public void SetRow(int row, params float[] values) { if(row >= 0 && row < order && values != null) { for(int i = 0, imax = Mathf.Min(order, values.Length); i < imax; i++) { matrix[row, i] = values[i]; } } } public void SetRow(params float[] values) { SetRow(m_index, values); m_index++; } public Determinant Clone() { Determinant tag = new Determinant(this.order); System.Array.Copy(matrix, tag.matrix, matrix.Length); return tag; } public Determinant GetSubDeterminant(int row, int column) { Determinant tag = new Determinant(this.order - 1); for(int i = 0, tagRow = 0; i < order; i++) { if(i == row) { continue; } for(int j = 0, tagColumn = 0; j < order; j++) { if(j == column) { continue; } tag.matrix[tagRow, tagColumn] = this.matrix[i, j]; tagColumn++; } tagRow++; } return tag; } public float GetValue() { if(order == 2) { float value = matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]; return value; } else { float value = 0.0f; int row = order - 1; for(int column = 0; column < order; column++) { var aij = matrix[row, column] * ((row + column) % 2 > 0 ? -1 : 1); var subMatrix = GetSubDeterminant(row, column); var mij = subMatrix.GetValue(); float tempValue = (aij * mij); value += tempValue; } return value; } } #endregion #region Help Funcs private void CreateMatrix() { matrix = new float[order, order]; } #endregion } public class DeterminantEquation { public Determinant determinant { get; private set; } public int order { get { return determinant != null ? determinant.order : 0; } } public DeterminantEquation(int order) { determinant = new Determinant(order); } #region Main Funcs public float[] CaculateVariables(params float[] values) { var D = determinant.GetValue(); if(D == 0) { Debug.LogWarning("This Determinant has no Result !!"); return null; } float[] variables = new float[order]; for(int i = 0; i < order; i++) { var subDeterminant = determinant.Clone(); subDeterminant.SetColumn(i, values); var Di = subDeterminant.GetValue(); var variable = Di / D; variables[i] = variable; } return variables; } #endregion }
因为多元方程组的一般方程就是 NxN 的等边矩阵, 所以行列数量是一样的, 用二维数组表示. 各个解通过克拉默法则计算得出.
而行列式值的计算就通过按行展开, 降阶通过代数余子式计算. 这样就可以计算任意阶的方程组了.
那么两点式的计算代码改一改, 就能看出扩展性了:
////// 两点式推出的交点方程 /// /// /// /// /// /// //public static bool IntersectRayAndLineSegment_XOZ_TwoPoint(Ray ray, Vector3 p1, Vector3 p2, out Vector3 point) public static bool IntersectRayAndLineSegment_XOZ_TwoPoint(Ray ray, Vector3 p1, Vector3 p2, out Vector3 point, out bool isParallel) { isParallel = false; point = Vector3.zero; Vector3 p3 = new Vector3(ray.origin.x, 0, ray.origin.z); Vector3 p4 = ray.GetPoint(1.0f); p4.y = 0; float a1, b1, c1; PointToLineEquation_2D_XOZ(p1, p2, out a1, out b1, out c1); float a2, b2, c2; PointToLineEquation_2D_XOZ(p3, p4, out a2, out b2, out c2); // 使用行列式计算线性方程组 DeterminantEquation determinantEquation = new DeterminantEquation(2); determinantEquation.determinant.SetRow(a1, b1); determinantEquation.determinant.SetRow(a2, b2); var variables = determinantEquation.CaculateVariables(-c1, -c2); if(variables == null) { // 平行线测试添加 (2019.06.19) float ea, eb; ea = a1 * c2 - a2 * c1; eb = b1 * c2 - b2 * c1; if((ea == 0) && (eb == 0)) { isParallel = true; Debug.Log("Determinant No Result, it is Parallel Line."); // 平行线未进行交点计算修改 (2019.06.20) if(IsPointInsideLine(ray.origin, p1, p2)) { point = ray.origin; return true; } else { var testDir = ((p1 + p2) * 0.5f) - ray.origin; testDir.y = 0.0f; if(Vector3.Dot(rayDir, testDir) > 0) { point = Vector3.SqrMagnitude(p1 - ray.origin) < Vector3.SqrMagnitude(p2 - ray.origin) ? p1 : p2; return true; } } Debug.Log("it is Parallel Line, and has no intersection"); } return false; } point = new Vector3(variables[0], 0, variables[1]); var rayDir = ray.direction; rayDir.y = 0; bool intersect = Vector3.Dot(point - p3, rayDir) > 0; if(intersect) { intersect = IsPointInsideLine(point, p1, p2); } return intersect; } /// /// 一般方程变量计算 /// /// /// /// /// /// public static void PointToLineEquation_2D_XOZ(Vector3 p1, Vector3 p2, out float a, out float b, out float c) { float x1 = p1.x; float y1 = p1.z; float x2 = p2.x; float y2 = p2.z; a = y2 - y1; b = x1 - x2; c = (y1 * x2) - (y2 * x1); } /// /// 检查是否相等, 解决计算机计算误差 /// /// /// /// public static bool IsTheSameValue(float a, float b) { const double Epsilon = 1e-5; var val = System.Math.Abs(a - b); return val <= Epsilon; } /// /// 测试点是否在线段上 /// /// /// /// /// public static bool IsPointInsideLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd) { bool xClamp = IsTheSameValue(point.x, lineStart.x) || IsTheSameValue(point.x, lineEnd.x) || (point.x >= Mathf.Min(lineStart.x, lineEnd.x) && point.x <= Mathf.Max(lineStart.x, lineEnd.x)); bool zClamp = IsTheSameValue(point.z, lineStart.z) || IsTheSameValue(point.z, lineEnd.z) || (point.z >= Mathf.Min(lineStart.z, lineEnd.z) && point.z <= Mathf.Max(lineStart.z, lineEnd.z)); return xClamp && zClamp; }
PS: 补充了射线与线段平行时的错误, 修改了精度错误测试.(2019.06.20)
把行列式的类( Determinant ) 改成结构体应该在效率上会高效一些.
PS: 数学模型这里有两个, 第一个是通过各个点获取联立方程, 第二个是怎样解联立方程
点斜式获取的是点斜式方程
y = k1*x + b1
y = k2*x + b2
两点式获取的是标准方程
a1*x + b1*y + c1 = 0
a2*x + b2*y + c2 = 0
这两个方程的计算差别上面已经说了.
解联立方程的方法也用了两种, 点斜式用的是一般推论, 就是普通的求解过程, 两点式使用了线性代数(注意点斜式的联立方程也能用线性方程解的, 只是作为对比),
可以预想到如果联立方程的维数增加到很多的时候一般求解过程的难度会大大增加,
比如
3元方程 -> 6个变量
4元方程 -> 24个变量
5元方程 -> 120个变量... 所以初中生最多就教到3元方程
而 DeterminantEquation 这个类就能很简单的计算出多元方程式的解. 所以在扩展性上胜出.
(2019.06.18)
昨天写的是用行列式解联立方程组得到的扩展性, 今天补充一下获取标准方程组特征值的泛用方法, 改进一下之前的函数, 提高扩展性.
因为这一步应该很少人去实现, 就把推导过程也加上吧.
先考虑2D和3D直线的一般方程: ax + by + C = 0 / ax + by + cz + D = 0
这里获取直线方程是通过两点获取的, 比如点斜式, 两点式, 都是基于几何的, 并且推导并不具有一般性.
两点 p1(x1,y1), p2(x2, y2) => (y-y1)/(x-x1) = (y2-y1)/(x2-x1) 这是根据斜率相等得到的, 一步步下来就得到
y(x1-x2) + x(y2-y1) + y1x2 - y2x1 = 0
于是
a = y2 - y1
b = x1 - x2
c = y1x2 - y2x1
这是 PointToLineEquation_2D_XOZ 函数做的计算, 可是如果是多元等式的话要怎样推算呢? 这里直接使用向量的概念, 这个向量可以是任意阶的向量.
向量 : vec = p2 - p1, 很好理解, 就是p1到p2的向量, 虽然可能是4阶, 5阶, 或者更高阶的. 假设f(p)是多元等式方程( ax + by + cz + dw .... + N = 0 ),
那么向量的值就是 ( v1, v2, v3, v4, ..... ), 向量的意义是什么呢? 向量就代表了增量, 哪个元的向量增量大, 它在哪个方向的增量就大,
得出的结论就是 : 在p1, p2构成的直线上(应该叫组成的多元等式上), p2点与p1点的差是向量vec, 任意一点p3与p1的差是向量vec的倍数,
而向量vec本来就是p1到p2的向量, 即 :
(v1) = (x2-x1) => (v1)/(x2-x1) = 1
(v2) = (y2-y1) => (v2)/(y2-y1) = 1
......
可得 (v1)/(x2-x1) = (v2)/(y2-y1) = (v3)/(z2-z1) ..... = 1
而任一点p3( x3, y3, z3, w3 .... )与p1的向量为vec的n倍, 可得 : (x3-x1)/(x2-x1) = (y3-y1)/(y2-y1) = (z3-z1)/(z2-z1) ..... = 1 * n 注意这里是多个等式
然后我们把等式变成一般式( ax + by + cz + dw .... + N = 0 )的形式, 要怎样变换呢? 很简单, 因为它们都相等, 用减法就行了, 比如上面的式子, 有A个元,
我们给它第一个等式乘一个(A-1)倍, 然后减去其它的元就行了 :
(x3-x1)/(x2-x1) = (y3-y1)/(y2-y1) = (z3-z1)/(z2-z1) ..... = 1 * n
变换可得 : (A-1)*(x3-x1)/(x2-x1) - (y3-y1)/(y2-y1) - (z3-z1)/(z2-z1) - ....... = 0
然后p3点代表任意点, 把它写成一般式的未展开形式 : (A-1)*(x-x1)/(v1) - (y-y1)/(v2) - (z-z1)/(v3) - ....... = 0
在这里我们按照计算原则, 不进行除法以便消除计算误差, 那么给等式两边乘以 (v1)*(v2)*(v3)......*(vA) 有A个元
得到没有除法的一般式 : (A-1)*(x-x1)*(v2)*(v3)......*(vA) - (y-y1)*(v1)*(v3)......*(vA) - (z-z1)*(v1)*(v2)......*(vA) - ....... = 0
最后抽出一般式的各个变量:
a = (A-1)*(v2)*(v3)......*(vA)
b = (-1)*(v1)*(v3)......*(vA)
c = (-1)*(v1)*(v2)......*(vA)
.......
N = (-x1)*(A-1)*(v2)*(v3)......*(vA) + (y1)*(v1)*(v3)......*(vA) + (z1)*(v1)*(v2)......*(vA) + ......
这样就可以得到一般式( ax + by + cz + dw .... + N = 0 )的各个变量了.
PS : 多元方程的一般式求解不仅限于2D直线3D直线这些, 到更多元的计算比如12维空间这些都能用的上, 并不是没有实际意义的.
下面是实现的代码, 很意外的非常简单:
public static class GeometryMath { ////// 求解多元线性方程的特征值 /// /// /// /// (aX + bY + cZ + dW + ..... + N = 0) => (a, b, c, d, .... N) public static void MultivariateLinearEquation(float[] p1, float[] p2, out float[] variables) { int maxLen = Mathf.Min(p1.Length, p2.Length); variables = new float[maxLen + 1]; float[] vector = new float[maxLen]; for(int i = 0; i < maxLen; i++) { vector[i] = p2[i] - p1[i]; } // caculate variables form vector variables[0] = (maxLen - 1) * ArrayValueMultiple(vector, 0); for(int i = 1; i < maxLen; i++) { variables[i] = (-1) * ArrayValueMultiple(vector, i); } // caculate last fixed value float N = (-1) * p1[0] * ArrayValueMultiple(vector, 0); for(int i = 1; i < p1.Length; i++) { float value = p1[i] * ArrayValueMultiple(vector, i); N += value; } variables[variables.Length - 1] = N; } /// /// float数组中去掉某个index的值的相乘 /// /// /// /// private static float ArrayValueMultiple(float[] array, int except) { float value = 1.0f; for(int i = 0; i < array.Length; i++) { if(i != except) { value *= array[i]; } } return value; } }
然后是改动后的两点式代码, 修正了计算误差:
////// 射线与线段相交性判断 /// /// /// /// /// /// /// public static bool IntersectRayAndLineSegment_XOZ_TwoPoint(Ray ray, Vector3 p1, Vector3 p2, out Vector3 point, out bool isParallel) { point = Vector3.zero; isParallel = false; Vector3 p3 = new Vector3(ray.origin.x, 0, ray.origin.z); Vector3 p4 = ray.GetPoint(1.0f); p4.y = 0; var rayDir = ray.direction; rayDir.y = 0; float a1, b1, c1; float[] variables1; GeometryMath.MultivariateLinearEquation(new float[2] { p1.x, p1.z }, new float[2] { p2.x, p2.z }, out variables1); a1 = variables1[0]; b1 = variables1[1]; c1 = variables1[2]; float a2, b2, c2; float[] variables2; GeometryMath.MultivariateLinearEquation(new float[2] { p3.x, p3.z }, new float[2] { p4.x, p4.z }, out variables2); a2 = variables2[0]; b2 = variables2[1]; c2 = variables2[2]; DeterminantEquation determinantEquation = new DeterminantEquation(2); determinantEquation.determinant.SetRow(a1, b1); determinantEquation.determinant.SetRow(a2, b2); var variables = determinantEquation.CaculateVariables(-c1, -c2); if(variables == null) { float ea, eb; ea = a1 * c2 - a2 * c1; eb = b1 * c2 - b2 * c1; if((ea == 0) && (eb == 0)) { isParallel = true; Debug.Log("Determinant No Result, it is Parallel Line."); if(IsPointInsideLine(ray.origin, p1, p2)) { point = ray.origin; return true; } else { var testDir = ((p1 + p2) * 0.5f) - ray.origin; testDir.y = 0.0f; if(Vector3.Dot(rayDir, testDir) > 0) { point = Vector3.SqrMagnitude(p1 - ray.origin) < Vector3.SqrMagnitude(p2 - ray.origin) ? p1 : p2; return true; } } Debug.Log("it is Parallel Line, and has no intersection"); } return false; } point = new Vector3(variables[0], 0, variables[1]); bool intersect = Vector3.Dot(point - p3, rayDir) > 0; if(intersect) { intersect = IsPointInsideLine(point, p1, p2); } return intersect; } /// /// 检查是否相等, 解决计算机计算误差 /// /// /// /// public static bool IsTheSameValue(float a, float b) { const double Epsilon = 1e-5; var val = System.Math.Abs(a - b); return val <= Epsilon; } /// /// 测试点是否在线段上 /// /// /// /// /// public static bool IsPointInsideLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd) { bool xClamp = IsTheSameValue(point.x, lineStart.x) || IsTheSameValue(point.x, lineEnd.x) || (point.x >= Mathf.Min(lineStart.x, lineEnd.x) && point.x <= Mathf.Max(lineStart.x, lineEnd.x)); bool zClamp = IsTheSameValue(point.z, lineStart.z) || IsTheSameValue(point.z, lineEnd.z) || (point.z >= Mathf.Min(lineStart.z, lineEnd.z) && point.z <= Mathf.Max(lineStart.z, lineEnd.z)); return xClamp && zClamp; }
看着真是牛了, 最终在计算上规避了所有除法公式造成的数据错误, 稳定性非常高, 然后在获取一般方程上扩展出了获取任意元的一般方程的方法, 以及计算任意阶线性方程组的方法.
唯一的遗憾就是判断point是否在线段上的精度误差.