1/*
2 * Agent2d.cpp
3 * RVO2 Library
4 *
5 * Copyright 2008 University of North Carolina at Chapel Hill
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 *
19 * Please send all bug reports to <geom@cs.unc.edu>.
20 *
21 * The authors may be contacted via:
22 *
23 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
24 * Dept. of Computer Science
25 * 201 S. Columbia St.
26 * Frederick P. Brooks, Jr. Computer Science Bldg.
27 * Chapel Hill, N.C. 27599-3175
28 * United States of America
29 *
30 * <http://gamma.cs.unc.edu/RVO2/>
31 */
32
33#include "Agent2d.h"
34
35#include "KdTree2d.h"
36#include "Obstacle2d.h"
37
38namespace RVO2D {
39 Agent2D::Agent2D() : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { }
40
41 void Agent2D::computeNeighbors(RVOSimulator2D *sim_)
42 {
43 obstacleNeighbors_.clear();
44 float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
45 sim_->kdTree_->computeObstacleNeighbors(this, rangeSq);
46
47 agentNeighbors_.clear();
48
49 if (maxNeighbors_ > 0) {
50 rangeSq = sqr(neighborDist_);
51 sim_->kdTree_->computeAgentNeighbors(this, rangeSq);
52 }
53 }
54
55 /* Search for the best new velocity. */
56 void Agent2D::computeNewVelocity(RVOSimulator2D *sim_)
57 {
58 orcaLines_.clear();
59
60 const float invTimeHorizonObst = 1.0f / timeHorizonObst_;
61
62 /* Create obstacle ORCA lines. */
63 for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) {
64
65 const Obstacle2D *obstacle1 = obstacleNeighbors_[i].second;
66 const Obstacle2D *obstacle2 = obstacle1->nextObstacle_;
67
68 const Vector2 relativePosition1 = obstacle1->point_ - position_;
69 const Vector2 relativePosition2 = obstacle2->point_ - position_;
70
71 /*
72 * Check if velocity obstacle of obstacle is already taken care of by
73 * previously constructed obstacle ORCA lines.
74 */
75 bool alreadyCovered = false;
76
77 for (size_t j = 0; j < orcaLines_.size(); ++j) {
78 if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) {
79 alreadyCovered = true;
80 break;
81 }
82 }
83
84 if (alreadyCovered) {
85 continue;
86 }
87
88 /* Not yet covered. Check for collisions. */
89
90 const float distSq1 = absSq(relativePosition1);
91 const float distSq2 = absSq(relativePosition2);
92
93 const float radiusSq = sqr(radius_);
94
95 const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_;
96 const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector);
97 const float distSqLine = absSq(-relativePosition1 - s * obstacleVector);
98
99 Line line;
100
101 if (s < 0.0f && distSq1 <= radiusSq) {
102 /* Collision with left vertex. Ignore if non-convex. */
103 if (obstacle1->isConvex_) {
104 line.point = Vector2(0.0f, 0.0f);
105 line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
106 orcaLines_.push_back(line);
107 }
108
109 continue;
110 }
111 else if (s > 1.0f && distSq2 <= radiusSq) {
112 /* Collision with right vertex. Ignore if non-convex
113 * or if it will be taken care of by neighoring obstace */
114 if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) {
115 line.point = Vector2(0.0f, 0.0f);
116 line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
117 orcaLines_.push_back(line);
118 }
119
120 continue;
121 }
122 else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) {
123 /* Collision with obstacle segment. */
124 line.point = Vector2(0.0f, 0.0f);
125 line.direction = -obstacle1->unitDir_;
126 orcaLines_.push_back(line);
127 continue;
128 }
129
130 /*
131 * No collision.
132 * Compute legs. When obliquely viewed, both legs can come from a single
133 * vertex. Legs extend cut-off line when nonconvex vertex.
134 */
135
136 Vector2 leftLegDirection, rightLegDirection;
137
138 if (s < 0.0f && distSqLine <= radiusSq) {
139 /*
140 * Obstacle viewed obliquely so that left vertex
141 * defines velocity obstacle.
142 */
143 if (!obstacle1->isConvex_) {
144 /* Ignore obstacle. */
145 continue;
146 }
147
148 obstacle2 = obstacle1;
149
150 const float leg1 = std::sqrt(distSq1 - radiusSq);
151 leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
152 rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
153 }
154 else if (s > 1.0f && distSqLine <= radiusSq) {
155 /*
156 * Obstacle viewed obliquely so that
157 * right vertex defines velocity obstacle.
158 */
159 if (!obstacle2->isConvex_) {
160 /* Ignore obstacle. */
161 continue;
162 }
163
164 obstacle1 = obstacle2;
165
166 const float leg2 = std::sqrt(distSq2 - radiusSq);
167 leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
168 rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
169 }
170 else {
171 /* Usual situation. */
172 if (obstacle1->isConvex_) {
173 const float leg1 = std::sqrt(distSq1 - radiusSq);
174 leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
175 }
176 else {
177 /* Left vertex non-convex; left leg extends cut-off line. */
178 leftLegDirection = -obstacle1->unitDir_;
179 }
180
181 if (obstacle2->isConvex_) {
182 const float leg2 = std::sqrt(distSq2 - radiusSq);
183 rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
184 }
185 else {
186 /* Right vertex non-convex; right leg extends cut-off line. */
187 rightLegDirection = obstacle1->unitDir_;
188 }
189 }
190
191 /*
192 * Legs can never point into neighboring edge when convex vertex,
193 * take cutoff-line of neighboring edge instead. If velocity projected on
194 * "foreign" leg, no constraint is added.
195 */
196
197 const Obstacle2D *const leftNeighbor = obstacle1->prevObstacle_;
198
199 bool isLeftLegForeign = false;
200 bool isRightLegForeign = false;
201
202 if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) {
203 /* Left leg points into obstacle. */
204 leftLegDirection = -leftNeighbor->unitDir_;
205 isLeftLegForeign = true;
206 }
207
208 if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) {
209 /* Right leg points into obstacle. */
210 rightLegDirection = obstacle2->unitDir_;
211 isRightLegForeign = true;
212 }
213
214 /* Compute cut-off centers. */
215 const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_);
216 const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_);
217 const Vector2 cutoffVec = rightCutoff - leftCutoff;
218
219 /* Project current velocity on velocity obstacle. */
220
221 /* Check if current velocity is projected on cutoff circles. */
222 const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec));
223 const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection);
224 const float tRight = ((velocity_ - rightCutoff) * rightLegDirection);
225
226 if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) {
227 /* Project on left cut-off circle. */
228 const Vector2 unitW = normalize(velocity_ - leftCutoff);
229
230 line.direction = Vector2(unitW.y(), -unitW.x());
231 line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW;
232 orcaLines_.push_back(line);
233 continue;
234 }
235 else if (t > 1.0f && tRight < 0.0f) {
236 /* Project on right cut-off circle. */
237 const Vector2 unitW = normalize(velocity_ - rightCutoff);
238
239 line.direction = Vector2(unitW.y(), -unitW.x());
240 line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW;
241 orcaLines_.push_back(line);
242 continue;
243 }
244
245 /*
246 * Project on left leg, right leg, or cut-off line, whichever is closest
247 * to velocity.
248 */
249 const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec)));
250 const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
251 const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));
252
253 if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
254 /* Project on cut-off line. */
255 line.direction = -obstacle1->unitDir_;
256 line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
257 orcaLines_.push_back(line);
258 continue;
259 }
260 else if (distSqLeft <= distSqRight) {
261 /* Project on left leg. */
262 if (isLeftLegForeign) {
263 continue;
264 }
265
266 line.direction = leftLegDirection;
267 line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
268 orcaLines_.push_back(line);
269 continue;
270 }
271 else {
272 /* Project on right leg. */
273 if (isRightLegForeign) {
274 continue;
275 }
276
277 line.direction = -rightLegDirection;
278 line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
279 orcaLines_.push_back(line);
280 continue;
281 }
282 }
283
284 const size_t numObstLines = orcaLines_.size();
285
286 const float invTimeHorizon = 1.0f / timeHorizon_;
287
288 /* Create agent ORCA lines. */
289 for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
290 const Agent2D *const other = agentNeighbors_[i].second;
291
292 //const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f;
293 //const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod;
294
295 const Vector2 relativePosition = other->position_ - position_;
296 const Vector2 relativeVelocity = velocity_ - other->velocity_;
297 const float distSq = absSq(relativePosition);
298 const float combinedRadius = radius_ + other->radius_;
299 const float combinedRadiusSq = sqr(combinedRadius);
300
301 Line line;
302 Vector2 u;
303
304 if (distSq > combinedRadiusSq) {
305 /* No collision. */
306 const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
307 /* Vector from cutoff center to relative velocity. */
308 const float wLengthSq = absSq(w);
309
310 const float dotProduct1 = w * relativePosition;
311
312 if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
313 /* Project on cut-off circle. */
314 const float wLength = std::sqrt(wLengthSq);
315 const Vector2 unitW = w / wLength;
316
317 line.direction = Vector2(unitW.y(), -unitW.x());
318 u = (combinedRadius * invTimeHorizon - wLength) * unitW;
319 }
320 else {
321 /* Project on legs. */
322 const float leg = std::sqrt(distSq - combinedRadiusSq);
323
324 if (det(relativePosition, w) > 0.0f) {
325 /* Project on left leg. */
326 line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
327 }
328 else {
329 /* Project on right leg. */
330 line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
331 }
332
333 const float dotProduct2 = relativeVelocity * line.direction;
334
335 u = dotProduct2 * line.direction - relativeVelocity;
336 }
337 }
338 else {
339 /* Collision. Project on cut-off circle of time timeStep. */
340 const float invTimeStep = 1.0f / sim_->timeStep_;
341
342 /* Vector from cutoff center to relative velocity. */
343 const Vector2 w = relativeVelocity - invTimeStep * relativePosition;
344
345 const float wLength = abs(w);
346 const Vector2 unitW = w / wLength;
347
348 line.direction = Vector2(unitW.y(), -unitW.x());
349 u = (combinedRadius * invTimeStep - wLength) * unitW;
350 }
351
352 line.point = velocity_ + 0.5f * u;
353 orcaLines_.push_back(line);
354 }
355
356 size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);
357
358 if (lineFail < orcaLines_.size()) {
359 linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
360 }
361 }
362
363 void Agent2D::insertAgentNeighbor(const Agent2D *agent, float &rangeSq)
364 {
365 // no point processing same agent
366 if (this == agent) {
367 return;
368 }
369 // ignore other agent if layers/mask bitmasks have no matching bit
370 if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
371 return;
372 }
373 // ignore other agent if this agent is below or above
374 if ((elevation_ > agent->elevation_ + agent->height_) || (elevation_ + height_ < agent->elevation_)) {
375 return;
376 }
377
378 if (avoidance_priority_ > agent->avoidance_priority_) {
379 return;
380 }
381
382 const float distSq = absSq(position_ - agent->position_);
383
384 if (distSq < rangeSq) {
385 if (agentNeighbors_.size() < maxNeighbors_) {
386 agentNeighbors_.push_back(std::make_pair(distSq, agent));
387 }
388
389 size_t i = agentNeighbors_.size() - 1;
390
391 while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
392 agentNeighbors_[i] = agentNeighbors_[i - 1];
393 --i;
394 }
395
396 agentNeighbors_[i] = std::make_pair(distSq, agent);
397
398 if (agentNeighbors_.size() == maxNeighbors_) {
399 rangeSq = agentNeighbors_.back().first;
400 }
401 }
402 }
403
404 void Agent2D::insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq)
405 {
406 const Obstacle2D *const nextObstacle = obstacle->nextObstacle_;
407
408 // ignore obstacle if no matching layer/mask
409 if ((avoidance_mask_ & nextObstacle->avoidance_layers_) == 0) {
410 return;
411 }
412 // ignore obstacle if below or above
413 if ((elevation_ > obstacle->elevation_ + obstacle->height_) || (elevation_ + height_ < obstacle->elevation_)) {
414 return;
415 }
416
417 const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_);
418
419 if (distSq < rangeSq) {
420 obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle));
421
422 size_t i = obstacleNeighbors_.size() - 1;
423
424 while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) {
425 obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
426 --i;
427 }
428
429 obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
430 }
431 //}
432 }
433
434 void Agent2D::update(RVOSimulator2D *sim_)
435 {
436 velocity_ = newVelocity_;
437 position_ += velocity_ * sim_->timeStep_;
438 }
439
440 bool linearProgram1(const std::vector<Line> &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
441 {
442 const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
443 const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point);
444
445 if (discriminant < 0.0f) {
446 /* Max speed circle fully invalidates line lineNo. */
447 return false;
448 }
449
450 const float sqrtDiscriminant = std::sqrt(discriminant);
451 float tLeft = -dotProduct - sqrtDiscriminant;
452 float tRight = -dotProduct + sqrtDiscriminant;
453
454 for (size_t i = 0; i < lineNo; ++i) {
455 const float denominator = det(lines[lineNo].direction, lines[i].direction);
456 const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);
457
458 if (std::fabs(denominator) <= RVO_EPSILON) {
459 /* Lines lineNo and i are (almost) parallel. */
460 if (numerator < 0.0f) {
461 return false;
462 }
463 else {
464 continue;
465 }
466 }
467
468 const float t = numerator / denominator;
469
470 if (denominator >= 0.0f) {
471 /* Line i bounds line lineNo on the right. */
472 tRight = std::min(tRight, t);
473 }
474 else {
475 /* Line i bounds line lineNo on the left. */
476 tLeft = std::max(tLeft, t);
477 }
478
479 if (tLeft > tRight) {
480 return false;
481 }
482 }
483
484 if (directionOpt) {
485 /* Optimize direction. */
486 if (optVelocity * lines[lineNo].direction > 0.0f) {
487 /* Take right extreme. */
488 result = lines[lineNo].point + tRight * lines[lineNo].direction;
489 }
490 else {
491 /* Take left extreme. */
492 result = lines[lineNo].point + tLeft * lines[lineNo].direction;
493 }
494 }
495 else {
496 /* Optimize closest point. */
497 const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);
498
499 if (t < tLeft) {
500 result = lines[lineNo].point + tLeft * lines[lineNo].direction;
501 }
502 else if (t > tRight) {
503 result = lines[lineNo].point + tRight * lines[lineNo].direction;
504 }
505 else {
506 result = lines[lineNo].point + t * lines[lineNo].direction;
507 }
508 }
509
510 return true;
511 }
512
513 size_t linearProgram2(const std::vector<Line> &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
514 {
515 if (directionOpt) {
516 /*
517 * Optimize direction. Note that the optimization velocity is of unit
518 * length in this case.
519 */
520 result = optVelocity * radius;
521 }
522 else if (absSq(optVelocity) > sqr(radius)) {
523 /* Optimize closest point and outside circle. */
524 result = normalize(optVelocity) * radius;
525 }
526 else {
527 /* Optimize closest point and inside circle. */
528 result = optVelocity;
529 }
530
531 for (size_t i = 0; i < lines.size(); ++i) {
532 if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
533 /* Result does not satisfy constraint i. Compute new optimal result. */
534 const Vector2 tempResult = result;
535
536 if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
537 result = tempResult;
538 return i;
539 }
540 }
541 }
542
543 return lines.size();
544 }
545
546 void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result)
547 {
548 float distance = 0.0f;
549
550 for (size_t i = beginLine; i < lines.size(); ++i) {
551 if (det(lines[i].direction, lines[i].point - result) > distance) {
552 /* Result does not satisfy constraint of line i. */
553 std::vector<Line> projLines(lines.begin(), lines.begin() + static_cast<ptrdiff_t>(numObstLines));
554
555 for (size_t j = numObstLines; j < i; ++j) {
556 Line line;
557
558 float determinant = det(lines[i].direction, lines[j].direction);
559
560 if (std::fabs(determinant) <= RVO_EPSILON) {
561 /* Line i and line j are parallel. */
562 if (lines[i].direction * lines[j].direction > 0.0f) {
563 /* Line i and line j point in the same direction. */
564 continue;
565 }
566 else {
567 /* Line i and line j point in opposite direction. */
568 line.point = 0.5f * (lines[i].point + lines[j].point);
569 }
570 }
571 else {
572 line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
573 }
574
575 line.direction = normalize(lines[j].direction - lines[i].direction);
576 projLines.push_back(line);
577 }
578
579 const Vector2 tempResult = result;
580
581 if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) {
582 /* This should in principle not happen. The result is by definition
583 * already in the feasible region of this linear program. If it fails,
584 * it is due to small floating point error, and the current result is
585 * kept.
586 */
587 result = tempResult;
588 }
589
590 distance = det(lines[i].direction, lines[i].point - result);
591 }
592 }
593 }
594}
595