Linear program

[dkal3]

Hi, I study the code from the ORCA algorithm. I try to understand the functions linearProgram1,linearProgram2 and linearProgram3 from ORCAAgent.cpp. I understand the brief description but when I want to deal with it in detail, I am confusing. In linearProgram1, you calculate
const float denominator = det(lines[lineNo]._direction, lines[i]._direction); const float numerator = det(lines[i]._direction, lines[lineNo]._point - lines[i]._point);

Which variables we put inside det. For example why numerator could not be like const float numerator = det(lines[i]._direction, lines[lineNo]._point);

Below in the code, there is a condition if (optVelocity * lines[lineNo]._direction > 0.0f). Why we put this inside the condition

I am sorry for asking so many things. I would be appreciate if you could give me a mathematical type that you follow for the linear Programs , to understand it better. Thank you in advance!!!

[curds01]

You have my deepest sympathies. The author of that code is incredibly gifted -- his gift is in taking math and rigorously reducing it to efficient code. However efficient code is not necessarily synonymous with easily understood. :)

This weekend I'll walk through the code and see if I can't give you an explanation of what you're seeing there. Sorry for the slight delay.

[curds01]

Turns out, I have the time now. :)

I've taken the code out of the source and put a copied-and-pasted version down below with line numbers for reference. But first the overview.

In this case, we're optimizing constraints around the current velocity. So, when linearProgram1 is applying the circular constraint, it is a circle around the origin.

The simple way to think about this function is that we have a line that we're going to clip with a circle and a set of halfspaces. The line to be clipped is drawn from lines and is at index lineNo. We first clip it with a circle, and then clip it with all of the other lines in lines before lineNo. Once we've clipped, we'll either do a simple directional optimization or a nearest point optimization -- based on the provided optVelocity.

So, with that overview in mind:

Some notation:

• the line to clip is given by the parameterized equation: L(t) = p + dt (where p is a point on the line and d is the unit-length direction of the line. (Note: when we interpret other lines as halfspaces, the direction will implicitly define which side of the line is the halfspace).
• All of the previous halfspaces are represented by lines and are notated as Lᵢ(t) = pᵢ + t⋅dᵢ (note the subscript i.)
• The circle radius is 'r'.
• Given vector v = <xᵥ, yᵥ>, we define v⟂ = <-yᵥ, xᵥ> as a vector perpendicular to v such that v⋅v⟂ = 0.

Algorithm overivew

• Lines [1-5]: Determine if the circular constraint doesn't intersect the line at all (at which point the entire line gets clipped away). We use the equation ‖p + dt‖₂ = r and look for the value of t. Alternatively, ‖p + dt‖₂² = r² is satisfied for the same values of t.
  • Ultimately, this is a quadratic equation in t: d⋅d⋅t² + 2⋅p⋅d⋅t + p⋅p - r² = 0, however, given that d is unit length, it simplifies to: t² + 2⋅p⋅d⋅t + p⋅p - r² = 0
  • We apply the quadratic equation and know that if the descriminate is negative, there is no real value. So: b² - 4ac = 4(p⋅d)² - 4(p⋅p - r²) >= 0 --> (p⋅d)² - p⋅p + r² >= 0. This is the test we see on line 3.
• Lines [6-8]: clips the line based on an actual intersection between line and circle spanned by [tₗ, tᵣ].
• Line [9-28]: The for loop for iterating through all previous lines.
  • Lines [10-11]: determines the intersection of the two lines. Essentially, line L intersects Lᵢ at t iff dᵢ⟂⋅(p + dt) - dᵢ⟂⋅ pᵢ = 0. With some algebraic manipulation we get: t = (dᵢ⟂⋅(pᵢ - p)) / (dᵢ⟂⋅d).
    • This equation represents the denominator on line 10 and the numerator on line 11. Calling det(u, v) is equivalent to evaluating u⟂⋅v, (The code definitions flip some terms to negate denominator and numerator, but it is an equivalent expression.)
  • Lines [12-18]: If the denominator is zero, the lines are parallel. So, we test the numerator to determine if the line lies completely outside the halfspace (line 13) or if it lies within (line 16 -- no clipping necessary).
  • Lines [19-24]: The lines are not parallel, so they do intersect. Compute t from the equation above. The denominator tells us which side of Lᵢ is "inside" and we clip the portion of line L that is outside.
  • Lines [25-27]: Determines if we've clipped the line to nothing.
• Lines [29-44]: Determines the result of the optimization.
  • Lines [30-34]: We want the end point most in the direction of optVelocity
  • Lines [35-44]: We want the nearest point to the optVelocity, which may lie on the interior of the clipped segment.

1.  const float dotProduct = lines[lineNo]._point * lines[lineNo]._direction;
2.    const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo]._point);
3.    if (discriminant < 0.0f) {
4.      return false;
5.    }
6.    const float sqrtDiscriminant = std::sqrt(discriminant);
7.    float tLeft = -dotProduct - sqrtDiscriminant;
8.    float tRight = -dotProduct + sqrtDiscriminant;
9.    for (size_t i = 0; i < lineNo; ++i) {
10.     const float denominator = det(lines[lineNo]._direction, lines[i]._direction);
11.      const float numerator = det(lines[i]._direction, lines[lineNo]._point - lines[i]._point);
12.      if (std::fabs(denominator) <= Menge::EPS) {
13.        if (numerator < 0.0f) {
14.          return false;
15.        } else {
16.          continue;
17.        }
18.      }
19.      const float t = numerator / denominator;
20.      if (denominator >= 0.0f) {
21.        tRight = std::min(tRight, t);
22.      } else {
23.        tLeft = std::max(tLeft, t);
24.      }
25.      if (tLeft > tRight) {
26.        return false;
27.      }
28.    }
29.    if (directionOpt) {
30.      if (optVelocity * lines[lineNo]._direction > 0.0f) {
31.        result = lines[lineNo]._point + tRight * lines[lineNo]._direction;
32.      } else {
33.        result = lines[lineNo]._point + tLeft * lines[lineNo]._direction;
34.      }
35.    } else {
36.      const float t = lines[lineNo]._direction * (optVelocity - lines[lineNo]._point);
37.      if (t < tLeft) {
38.        result = lines[lineNo]._point + tLeft * lines[lineNo]._direction;
39.      } else if (t > tRight) {
40.        result = lines[lineNo]._point + tRight * lines[lineNo]._direction;
41.      } else {
42.        result = lines[lineNo]._point + t * lines[lineNo]._direction;
43.      }
44.    }
45.    return true;

[dkal3]

You are amazing!!! Thank you so much for your detailed answer!!!

[dkal3]

I don't have another question! We can close the issue! Thank you very much!

[curds01]

The nice thing is, I can use this to upgrade the documentation.

[dkal3]

Hi again!! You have done amazing job with the documentation! Can I ask something? I am a bit confused with the determinant and which parameters we put inside. The result of the determinant what does it show? Thank you in advance!!!

[curds01]

So, there's two things about det(). I'll see if I can't clarify it.

1. det() as a mathematical function:
   • Generally, this is the definition of the determinant of a 2x2 matrix:

     |a c|
     det|   | = ad - bc
     |b d|

     This is where the name comes from. The function is invoked as det(u, v) which implicitly computes:

   |u₁ v₁|
det|     | = u₁v₂ - u₂v₁
   |u₂ v₂|

2. det() as how it is typically used in Menge.
   • The first application is a sneaky mechanism for computing u⋅v⟂ what all you have is u and v. This works because if v = [v₁ v₂], then v⟂= [v₂ -v₁]. Therefore, u⋅v⟂ = u₁v₂ - u₂v₁ = det(u, v).
   • It can also be used to determine the sine of the angle between vectors u and v. This works by thinking of the 2D vector v = [v₁ v₂] as the 3D vector v = [v₁ v₂ 0]. Given u, v ∈ ℜ³, |u ⨯ v| = |u||v|sin(θ). If |u| = |v| = 1 then |u ⨯ v| = sin(θ). If you multiply out the cross product, you get the vector |[0, 0, u₁v₂ - u₂v₁]| = u₁v₂ - u₂v₁ = sin(θ). So, for unit normals in ℜ², det(u, v) will give you the sin(θ) between u and v which is enough to determine whether v lies clockwise or counter-clockwise of u. (And if you really care about the angle, you can call acos(det(u, v)).

I hope this clarifies det().

[dkal3]

Yes it is clear now!! For one more time, thank you a lot!!!