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);
}
