size_t linearProgram2(const std::vector<Line>& lines, float radius, const Vector2& optVelocity, bool directionOpt, Vector2& result) { if (directionOpt) { /* * Optimize direction. Note that the optimization velocity is of unit * length in this case. */ result = optVelocity * radius; } else if (absSq(optVelocity) > sqr(radius)) { /* Optimize closest point and outside circle. */ result = normalize(optVelocity) * radius; } else { /* Optimize closest point and inside circle. */ result = optVelocity; } for (size_t i = 0; i < lines.size(); ++i) { if (det(lines[i].direction, lines[i].point - result) > 0.0f) { /* Result does not satisfy constraint i. Compute new optimal result. */ const Vector2 tempResult = result; if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { result = tempResult; return i; } } } return lines.size(); }
bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) { const float planeDist = planes[planeNo].point * planes[planeNo].normal; const float planeDistSq = sqr(planeDist); const float radiusSq = sqr(radius); if (planeDistSq > radiusSq) { /* Max speed sphere fully invalidates plane planeNo. */ return false; } const float planeRadiusSq = radiusSq - planeDistSq; const Vector3 planeCenter = planeDist * planes[planeNo].normal; if (directionOpt) { /* Project direction optVelocity on plane planeNo. */ const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal; const float planeOptVelocityLengthSq = absSq(planeOptVelocity); if (planeOptVelocityLengthSq <= RVO_EPSILON) { result = planeCenter; } else { result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity; } } else { /* Project point optVelocity on plane planeNo. */ result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal; /* If outside planeCircle, project on planeCircle. */ if (absSq(result) > radiusSq) { const Vector3 planeResult = result - planeCenter; const float planeResultLengthSq = absSq(planeResult); result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult; } } for (size_t i = 0; i < planeNo; ++i) { if (planes[i].normal * (planes[i].point - result) > 0.0f) { /* Result does not satisfy constraint i. Compute new optimal result. */ /* Compute intersection line of plane i and plane planeNo. */ Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal); if (absSq(crossProduct) <= RVO_EPSILON) { /* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */ return false; } Line line; line.direction = normalize(crossProduct); const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal); line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal; if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result)) { return false; } } } return true; }