diff --git a/src/Menge/MengeCore/Orca/ORCAAgent.cpp b/src/Menge/MengeCore/Orca/ORCAAgent.cpp index 71a1d292..f726e42e 100644 --- a/src/Menge/MengeCore/Orca/ORCAAgent.cpp +++ b/src/Menge/MengeCore/Orca/ORCAAgent.cpp @@ -1,9 +1,9 @@ -/* +/* License Menge -Copyright © and trademark ™ 2012-14 University of North Carolina at Chapel Hill. +Copyright © and trademark â„¢ 2012-14 University of North Carolina at Chapel Hill. All rights reserved. Permission to use, copy, modify, and distribute this software and its documentation @@ -439,27 +439,53 @@ std::string Agent::getStringId() const { return "orca"; } bool linearProgram1(const std::vector& lines, size_t lineNo, float radius, const Vector2& optVelocity, bool directionOpt, Vector2& result) { + // First, evaluate the circular constraint; if there is no intersection between circle and line + // then the optimization is infeasible. + // + // If the line lines[lineNo] is L(t) = p + dt, then this line intersects a circle (centered at the + // origin with radius r) where ‖p + dt‖₂ = r or, equivalently, ‖p + dt‖₂² = r². This expands to + // the quadratic equation: dâ‹…dâ‹…t² + 2â‹…pâ‹…dâ‹…t + pâ‹…p - r² = 0. + // However, d is unit length, so it simplifies to: t² + 2â‹…pâ‹…dâ‹…t + pâ‹…p - r² = 0. + // For there to be *no* intersection, the descriminant must be < 0. I.e., + // b² - 4ac < 0 + // 4(pâ‹…d)² - 4(pâ‹…p - r²) < 0 + // (pâ‹…d)² - (pâ‹…p - r²) < 0 const float dotProduct = lines[lineNo]._point * lines[lineNo]._direction; const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo]._point); if (discriminant < 0.0f) { - /* Max speed circle fully invalidates line lineNo. */ + // No intersection; there is no solution. return false; } + // There is an intersection; find the extents of the intersection. const float sqrtDiscriminant = std::sqrt(discriminant); float tLeft = -dotProduct - sqrtDiscriminant; float tRight = -dotProduct + sqrtDiscriminant; + // For each *previous* line, find where it intersects the optimization line. Then, based on the + // direction of the constraint line, clip the optimization line as appropriate. + // First, some notation. + // L(t) = p + dt is the optimization line. + // Láµ¢(t) = páµ¢ + tâ‹…dáµ¢ is boundary of a constraining halfspace. + // Given vector v = , we define v⟂ = as a vector perpendicular to v such that + // vâ‹…v⟂ = 0. + // + // Line L intersects line Láµ¢ at t if and only if dᵢ⟂⋅(p + dt) - dᵢ⟂⋅ páµ¢ = 0. + // With some algebraic manipulation, that becomes: t = (dᵢ⟂⋅(páµ¢ - p)) / (dᵢ⟂⋅d). + // Calling the det(u, v) method is equivalent to evaluating uâ‹…v⟂ for (size_t i = 0; i < lineNo; ++i) { + // The denominator and nuemrator of t = (dᵢ⟂⋅(páµ¢ - p)) / (dᵢ⟂⋅d), respectively. const float denominator = det(lines[lineNo]._direction, lines[i]._direction); const float numerator = det(lines[i]._direction, lines[lineNo]._point - lines[i]._point); if (std::fabs(denominator) <= Menge::EPS) { - /* Lines lineNo and i are (almost) parallel. */ + // Lines L and i are Láµ¢ functionally parallel. if (numerator < 0.0f) { + // Line L lies completely outside the halfspace bounded by Láµ¢; problem infeasible. return false; } else { + // Line L lies completely inside the halfspace bounded by Láµ¢; no clipping required. continue; } } @@ -467,29 +493,30 @@ bool linearProgram1(const std::vector& lines, size_t lineNo, const float t = numerator / denominator; if (denominator >= 0.0f) { - /* Line i bounds line lineNo on the right. */ + // Line Láµ¢ bounds line L on the right. tRight = std::min(tRight, t); } else { - /* Line i bounds line lineNo on the left. */ + // Line Láµ¢ bounds line L on the left. tLeft = std::max(tLeft, t); } + // Test to see if there still a line segment left over. if (tLeft > tRight) { return false; } } if (directionOpt) { - /* Optimize direction. */ + // Optimize direction. if (optVelocity * lines[lineNo]._direction > 0.0f) { - /* Take right extreme. */ + // Take right extreme. result = lines[lineNo]._point + tRight * lines[lineNo]._direction; } else { - /* Take left extreme. */ + // Take left extreme. result = lines[lineNo]._point + tLeft * lines[lineNo]._direction; } } else { - /* Optimize closest point. */ + // Optimize closest point. const float t = lines[lineNo]._direction * (optVelocity - lines[lineNo]._point); if (t < tLeft) { diff --git a/src/Menge/MengeCore/Orca/ORCAAgent.h b/src/Menge/MengeCore/Orca/ORCAAgent.h index b7a827f7..53b133a6 100644 --- a/src/Menge/MengeCore/Orca/ORCAAgent.h +++ b/src/Menge/MengeCore/Orca/ORCAAgent.h @@ -1,4 +1,4 @@ -/* +/* Menge Crowd Simulation Framework Copyright and trademark 2012-17 University of North Carolina at Chapel Hill @@ -110,18 +110,33 @@ class MENGE_API Agent : public Menge::Agents::BaseAgent { void obstacleLine(size_t obstNbrID, const float invTau, bool flip); }; -/*! - @brief Solves a one-dimensional linear program on a specified line subject to linear - constraints defined by lines and a circular constraint. - - @param lines Lines defining the linear constraints. - @param lineNo The specified line constraint. - @param radius The radius of the circular constraint. - @param optVelocity The optimization velocity. - @param directionOpt True if the direction should be optimized. - @param result A reference to the result of the linear program. - @returns True if successful. - */ + /*! @brief Finds the optimal solution on a *line*, subject to a set of constraints, relative to + a given optimization velocity. + + THe line being optimized is given by index `lineNo`, drawn from the collection of `lines`. The + constraints include a circle, centered on the origin with the given `radius`. Furthermore, it + includes all half spaces represented by the `lines` index by `0 ≤ i < lineNo`. It is assumed that + the constraints (circle and previous half spaces) are consistent with respect to the optimization + point (`optVelocity`). + + Essentially, the line gets clipped by all of the constraints to a finite domain (it must be + finite, otherwise it hasn't actually intersected the circle). Once the clipped domain has been + calculated, one of two optimizations will be run (based on `directionOpt`). + + If `directionOpt` is true, the optimal solution is the end of the clipped segement most in the + same direction (relative to the line direction) as `optVelocity`. If `directionOpt` is false, + it returns the nearest point of the line segment to `optVelocity`. If there is no actual solution + (typically because the constraints are infeasible), the function returns false. + + @param lines The collection of lines defining the halfspace constraints and + optimizaiton line. + @param lineNo The index of the line to optimize on. All lines in the range [0, lineNo) + will serve as halfspace constraints. + @param radius The radius of the circular constraint. + @param optVelocity The optimization velocity. + @param directionOpt True if the direction should be optimized, false if nearest point. + @param result[out] The optimized result is returned here -- if one is found. + @returns True if successful (and `result` is meaningful). */ bool linearProgram1(const std::vector& lines, size_t lineNo, float radius, const Menge::Math::Vector2& optVelocity, bool directionOpt, Menge::Math::Vector2& result);