1/*
2 * Agent.cpp
3 * RVO2-3D 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 * https://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 * <https://gamma.cs.unc.edu/RVO2/>
31 */
32
33#include "Agent3d.h"
34
35#include <cmath>
36#include <algorithm>
37
38#include "Definitions.h"
39#include "KdTree3d.h"
40
41namespace RVO3D {
42 /**
43 * \brief A sufficiently small positive number.
44 */
45 const float RVO3D_EPSILON = 0.00001f;
46
47 /**
48 * \brief Defines a directed line.
49 */
50 class Line3D {
51 public:
52 /**
53 * \brief The direction of the directed line.
54 */
55 Vector3 direction;
56
57 /**
58 * \brief A point on the directed line.
59 */
60 Vector3 point;
61 };
62
63 /**
64 * \brief Solves a one-dimensional linear program on a specified line subject to linear constraints defined by planes and a spherical constraint.
65 * \param planes Planes defining the linear constraints.
66 * \param planeNo The plane on which the line lies.
67 * \param line The line on which the 1-d linear program is solved
68 * \param radius The radius of the spherical constraint.
69 * \param optVelocity The optimization velocity.
70 * \param directionOpt True if the direction should be optimized.
71 * \param result A reference to the result of the linear program.
72 * \return True if successful.
73 */
74 bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line3D &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
75
76 /**
77 * \brief Solves a two-dimensional linear program on a specified plane subject to linear constraints defined by planes and a spherical constraint.
78 * \param planes Planes defining the linear constraints.
79 * \param planeNo The plane on which the 2-d linear program is solved
80 * \param radius The radius of the spherical constraint.
81 * \param optVelocity The optimization velocity.
82 * \param directionOpt True if the direction should be optimized.
83 * \param result A reference to the result of the linear program.
84 * \return True if successful.
85 */
86 bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
87
88 /**
89 * \brief Solves a three-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
90 * \param planes Planes defining the linear constraints.
91 * \param radius The radius of the spherical constraint.
92 * \param optVelocity The optimization velocity.
93 * \param directionOpt True if the direction should be optimized.
94 * \param result A reference to the result of the linear program.
95 * \return The number of the plane it fails on, and the number of planes if successful.
96 */
97 size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
98
99 /**
100 * \brief Solves a four-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
101 * \param planes Planes defining the linear constraints.
102 * \param beginPlane The plane on which the 3-d linear program failed.
103 * \param radius The radius of the spherical constraint.
104 * \param result A reference to the result of the linear program.
105 */
106 void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);
107
108 Agent3D::Agent3D() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) { }
109
110 void Agent3D::computeNeighbors(RVOSimulator3D *sim_)
111 {
112 agentNeighbors_.clear();
113
114 if (maxNeighbors_ > 0) {
115 sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
116 }
117 }
118
119 void Agent3D::computeNewVelocity(RVOSimulator3D *sim_)
120 {
121 orcaPlanes_.clear();
122
123 const float invTimeHorizon = 1.0f / timeHorizon_;
124
125 /* Create agent ORCA planes. */
126 for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
127 const Agent3D *const other = agentNeighbors_[i].second;
128
129 //const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f;
130 //const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod;
131
132 const Vector3 relativePosition = other->position_ - position_;
133 const Vector3 relativeVelocity = velocity_ - other->velocity_;
134 const float distSq = absSq(relativePosition);
135 const float combinedRadius = radius_ + other->radius_;
136 const float combinedRadiusSq = sqr(combinedRadius);
137
138 Plane plane;
139 Vector3 u;
140
141 if (distSq > combinedRadiusSq) {
142 /* No collision. */
143 const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition;
144 /* Vector from cutoff center to relative velocity. */
145 const float wLengthSq = absSq(w);
146
147 const float dotProduct = w * relativePosition;
148
149 if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq) {
150 /* Project on cut-off circle. */
151 const float wLength = std::sqrt(wLengthSq);
152 const Vector3 unitW = w / wLength;
153
154 plane.normal = unitW;
155 u = (combinedRadius * invTimeHorizon - wLength) * unitW;
156 }
157 else {
158 /* Project on cone. */
159 const float a = distSq;
160 const float b = relativePosition * relativeVelocity;
161 const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq);
162 const float t = (b + std::sqrt(sqr(b) - a * c)) / a;
163 const Vector3 w = relativeVelocity - t * relativePosition;
164 const float wLength = abs(w);
165 const Vector3 unitW = w / wLength;
166
167 plane.normal = unitW;
168 u = (combinedRadius * t - wLength) * unitW;
169 }
170 }
171 else {
172 /* Collision. */
173 const float invTimeStep = 1.0f / sim_->timeStep_;
174 const Vector3 w = relativeVelocity - invTimeStep * relativePosition;
175 const float wLength = abs(w);
176 const Vector3 unitW = w / wLength;
177
178 plane.normal = unitW;
179 u = (combinedRadius * invTimeStep - wLength) * unitW;
180 }
181
182 plane.point = velocity_ + 0.5f * u;
183 orcaPlanes_.push_back(plane);
184 }
185
186 const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_);
187
188 if (planeFail < orcaPlanes_.size()) {
189 linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
190 }
191 }
192
193 void Agent3D::insertAgentNeighbor(const Agent3D *agent, float &rangeSq)
194 {
195 // no point processing same agent
196 if (this == agent) {
197 return;
198 }
199 // ignore other agent if layers/mask bitmasks have no matching bit
200 if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
201 return;
202 }
203
204 if (avoidance_priority_ > agent->avoidance_priority_) {
205 return;
206 }
207
208 const float distSq = absSq(position_ - agent->position_);
209
210 if (distSq < rangeSq) {
211 if (agentNeighbors_.size() < maxNeighbors_) {
212 agentNeighbors_.push_back(std::make_pair(distSq, agent));
213 }
214
215 size_t i = agentNeighbors_.size() - 1;
216
217 while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
218 agentNeighbors_[i] = agentNeighbors_[i - 1];
219 --i;
220 }
221
222 agentNeighbors_[i] = std::make_pair(distSq, agent);
223
224 if (agentNeighbors_.size() == maxNeighbors_) {
225 rangeSq = agentNeighbors_.back().first;
226 }
227 }
228 }
229
230 void Agent3D::update(RVOSimulator3D *sim_)
231 {
232 velocity_ = newVelocity_;
233 position_ += velocity_ * sim_->timeStep_;
234 }
235
236 bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line3D &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
237 {
238 const float dotProduct = line.point * line.direction;
239 const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point);
240
241 if (discriminant < 0.0f) {
242 /* Max speed sphere fully invalidates line. */
243 return false;
244 }
245
246 const float sqrtDiscriminant = std::sqrt(discriminant);
247 float tLeft = -dotProduct - sqrtDiscriminant;
248 float tRight = -dotProduct + sqrtDiscriminant;
249
250 for (size_t i = 0; i < planeNo; ++i) {
251 const float numerator = (planes[i].point - line.point) * planes[i].normal;
252 const float denominator = line.direction * planes[i].normal;
253
254 if (sqr(denominator) <= RVO3D_EPSILON) {
255 /* Lines3D line is (almost) parallel to plane i. */
256 if (numerator > 0.0f) {
257 return false;
258 }
259 else {
260 continue;
261 }
262 }
263
264 const float t = numerator / denominator;
265
266 if (denominator >= 0.0f) {
267 /* Plane i bounds line on the left. */
268 tLeft = std::max(tLeft, t);
269 }
270 else {
271 /* Plane i bounds line on the right. */
272 tRight = std::min(tRight, t);
273 }
274
275 if (tLeft > tRight) {
276 return false;
277 }
278 }
279
280 if (directionOpt) {
281 /* Optimize direction. */
282 if (optVelocity * line.direction > 0.0f) {
283 /* Take right extreme. */
284 result = line.point + tRight * line.direction;
285 }
286 else {
287 /* Take left extreme. */
288 result = line.point + tLeft * line.direction;
289 }
290 }
291 else {
292 /* Optimize closest point. */
293 const float t = line.direction * (optVelocity - line.point);
294
295 if (t < tLeft) {
296 result = line.point + tLeft * line.direction;
297 }
298 else if (t > tRight) {
299 result = line.point + tRight * line.direction;
300 }
301 else {
302 result = line.point + t * line.direction;
303 }
304 }
305
306 return true;
307 }
308
309 bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
310 {
311 const float planeDist = planes[planeNo].point * planes[planeNo].normal;
312 const float planeDistSq = sqr(planeDist);
313 const float radiusSq = sqr(radius);
314
315 if (planeDistSq > radiusSq) {
316 /* Max speed sphere fully invalidates plane planeNo. */
317 return false;
318 }
319
320 const float planeRadiusSq = radiusSq - planeDistSq;
321
322 const Vector3 planeCenter = planeDist * planes[planeNo].normal;
323
324 if (directionOpt) {
325 /* Project direction optVelocity on plane planeNo. */
326 const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal;
327 const float planeOptVelocityLengthSq = absSq(planeOptVelocity);
328
329 if (planeOptVelocityLengthSq <= RVO3D_EPSILON) {
330 result = planeCenter;
331 }
332 else {
333 result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
334 }
335 }
336 else {
337 /* Project point optVelocity on plane planeNo. */
338 result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal;
339
340 /* If outside planeCircle, project on planeCircle. */
341 if (absSq(result) > radiusSq) {
342 const Vector3 planeResult = result - planeCenter;
343 const float planeResultLengthSq = absSq(planeResult);
344 result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult;
345 }
346 }
347
348 for (size_t i = 0; i < planeNo; ++i) {
349 if (planes[i].normal * (planes[i].point - result) > 0.0f) {
350 /* Result does not satisfy constraint i. Compute new optimal result. */
351 /* Compute intersection line of plane i and plane planeNo. */
352 Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal);
353
354 if (absSq(crossProduct) <= RVO3D_EPSILON) {
355 /* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */
356 return false;
357 }
358
359 Line3D line;
360 line.direction = normalize(crossProduct);
361 const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);
362 line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal;
363
364 if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result)) {
365 return false;
366 }
367 }
368 }
369
370 return true;
371 }
372
373 size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
374 {
375 if (directionOpt) {
376 /* Optimize direction. Note that the optimization velocity is of unit length in this case. */
377 result = optVelocity * radius;
378 }
379 else if (absSq(optVelocity) > sqr(radius)) {
380 /* Optimize closest point and outside circle. */
381 result = normalize(optVelocity) * radius;
382 }
383 else {
384 /* Optimize closest point and inside circle. */
385 result = optVelocity;
386 }
387
388 for (size_t i = 0; i < planes.size(); ++i) {
389 if (planes[i].normal * (planes[i].point - result) > 0.0f) {
390 /* Result does not satisfy constraint i. Compute new optimal result. */
391 const Vector3 tempResult = result;
392
393 if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result)) {
394 result = tempResult;
395 return i;
396 }
397 }
398 }
399
400 return planes.size();
401 }
402
403 void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result)
404 {
405 float distance = 0.0f;
406
407 for (size_t i = beginPlane; i < planes.size(); ++i) {
408 if (planes[i].normal * (planes[i].point - result) > distance) {
409 /* Result does not satisfy constraint of plane i. */
410 std::vector<Plane> projPlanes;
411
412 for (size_t j = 0; j < i; ++j) {
413 Plane plane;
414
415 const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
416
417 if (absSq(crossProduct) <= RVO3D_EPSILON) {
418 /* Plane i and plane j are (almost) parallel. */
419 if (planes[i].normal * planes[j].normal > 0.0f) {
420 /* Plane i and plane j point in the same direction. */
421 continue;
422 }
423 else {
424 /* Plane i and plane j point in opposite direction. */
425 plane.point = 0.5f * (planes[i].point + planes[j].point);
426 }
427 }
428 else {
429 /* Plane.point is point on line of intersection between plane i and plane j. */
430 const Vector3 lineNormal = cross(crossProduct, planes[i].normal);
431 plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal;
432 }
433
434 plane.normal = normalize(planes[j].normal - planes[i].normal);
435 projPlanes.push_back(plane);
436 }
437
438 const Vector3 tempResult = result;
439
440 if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size()) {
441 /* 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. */
442 result = tempResult;
443 }
444
445 distance = planes[i].normal * (planes[i].point - result);
446 }
447 }
448 }
449}
450