private void LinearProgram3(IList <OrcaLine> lines, int numObstLines, int beginLine, float radius, Vector3 curVelocity, ref Vector3 result) { float distance = 0.0f; for (int i = beginLine; i < lines.Count; ++i) { if (VectorMath.det(lines[i].direction, lines[i].point - result) > distance) { /* Result does not satisfy constraint of line i. */ //std::vector<Line> projLines(lines.begin(), lines.begin() + numObstLines); IList <OrcaLine> projLines = new List <OrcaLine>(); for (int ii = 0; ii < numObstLines; ++ii) { projLines.Add(lines[ii]); } for (int j = numObstLines; j < i; ++j) { OrcaLine line; float determinant = VectorMath.det(lines[i].direction, lines[j].direction); if (VectorMath.fabs(determinant) <= VectorMath.EPSILON) { /* Line i and line j are parallel. */ if (VectorMath.dot(lines[i].direction, lines[j].direction) > 0.0f) { /* Line i and line j point in the same direction. */ continue; } else { /* Line i and line j point in opposite direction. */ line.point = 0.5f * (lines[i].point + lines[j].point); } } else { line.point = lines[i].point + (VectorMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; } line.direction = VectorMath.normalize(lines[j].direction - lines[i].direction); projLines.Add(line); } Vector3 tempResult = result; if (LinearProgram2(projLines, radius, new Vector3(-lines[i].direction.Z, 0, lines[i].direction.X), curVelocity, true, ref result) < projLines.Count) { /* This should in principle not happen. The result is by definition * already in the feasible region of this linear program. If it fails, * it is due to small floating point error, and the current result is * kept. */ result = tempResult; } distance = VectorMath.det(lines[i].direction, lines[i].point - result); } } }
private bool LinearProgram1(IList <OrcaLine> lines, int lineNo, float radius, Vector3 optVelocity, Vector3 curVelocity, bool directionOpt, ref Vector3 result) { float dotProduct = VectorMath.dot(lines[lineNo].point, lines[lineNo].direction); float discriminant = VectorMath.sqr(dotProduct) + VectorMath.sqr(radius) - VectorMath.absSq(lines[lineNo].point); if (discriminant < 0.0f) { /* Max speed circle fully invalidates line lineNo. */ return(false); } float sqrtDiscriminant = VectorMath.sqrt(discriminant); float tLeft = -dotProduct - sqrtDiscriminant; //最大速度圆与半平面分隔线的左交点(远离射线方向) float tRight = -dotProduct + sqrtDiscriminant; //最大速度圆与半平面分隔线的右交点(靠近射线方向) for (int i = 0; i < lineNo; ++i) { float denominator = VectorMath.det(lines[lineNo].direction, lines[i].direction); float numerator = VectorMath.det(lines[i].direction, lines[lineNo].point - lines[i].point); if (VectorMath.fabs(denominator) <= VectorMath.EPSILON) { /* Lines lineNo and i are (almost) parallel. */ if (numerator < 0.0f) { return(false);//此情形line i的半平面与line No半平面没有交集 } else { continue;//此情形line i的半平面完全包含line No半平面 } } float t = numerator / denominator;//t是line i半平面分隔线与line No分隔线的交点到line No参考点的距离(符号表示在参考点左边还是右边) if (denominator >= 0.0f) { /* Line i bounds line lineNo on the right. */ tRight = Math.Min(tRight, t); } else { /* Line i bounds line lineNo on the left. */ tLeft = Math.Max(tLeft, t); } if (tLeft > tRight) { return(false);//i之前的半平面无交集 } } if (directionOpt) { if (curVelocity.LengthSquared() > VectorMath.EPSILON) { if (VectorMath.dot(curVelocity, lines[lineNo].direction) > 0.0f) { result = lines[lineNo].point + tRight * lines[lineNo].direction; } else { result = lines[lineNo].point + tLeft * lines[lineNo].direction; } } else { /* Optimize direction. */ if (Math.Abs(tLeft) < VectorMath.EPSILON || VectorMath.dot(optVelocity, lines[lineNo].direction) > 0.0f && (Math.Abs(tRight) > radius * 0.5f || Math.Abs(tRight) > Math.Abs(tLeft))) { /* Take right extreme. */ result = lines[lineNo].point + tRight * lines[lineNo].direction; } else { /* Take left extreme. */ result = lines[lineNo].point + tLeft * lines[lineNo].direction; } } } else { /* Optimize closest point. */ float t = VectorMath.dot(lines[lineNo].direction, (optVelocity - lines[lineNo].point));//预选速度矢量与line No的交点到参考点的距离(带符号) if (t < tLeft) { result = lines[lineNo].point + tLeft * lines[lineNo].direction; } else if (t > tRight) { result = lines[lineNo].point + tRight * lines[lineNo].direction; } else { result = lines[lineNo].point + t * lines[lineNo].direction; } } return(true); }