Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
OCSR
UMANS
Commits
d30a8c6d
Commit
d30a8c6d
authored
Sep 08, 2020
by
VAN TOLL Wouter
Browse files
CostFunction: Improved correction for the agent radius in TTCA/DCA calculation.
parent
f51efee4
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/Engine/core/costFunction.cpp
View file @
d30a8c6d
...
...
@@ -324,16 +324,57 @@ float CostFunction::ComputeTimeToCollision_LineSegmentInterior(const Vector2D& p
return
std
::
max
(
0.0
f
,
timeToCollision
-
timeToSubtract
);
}
/*std::pair<float, float> CostFunction::ComputeTimeAndDistanceToClosestApproach(
const Vector2D& position1, const Vector2D& velocity1, const float radius1,
const Vector2D& position2, const Vector2D& velocity2, const float radius2) const
{
// dp = difference in position
Vector2D dp(position2 - position1);
// dv = different in velocity
Vector2D dv(velocity2 - velocity1);
// ttca = time to closest approach. Source: Dutra et al, "Gradient-based steering for vision-based crowd simulation algorithms", 2016.
float ttca = (dv.sqrMagnitude() == 0 ? 0 : -dp.dot(dv) / dv.sqrMagnitude());
// The "classical" definition of ttca does not take agent radii into account,
// and in case of a collision, it computes the time to the biggest overlap.
// This affects cost functions incorrectly. We use the following variant:
// if there is a future collision (i.e. ttc is defined), then ttca = tca. Otherwise, ttca = the classical version.
float ttc = ComputeTimeToCollision(position1, velocity1, radius1, position2, velocity2, radius2);
ttca = std::min(ttca, ttc);
float dca = (dp + dv * ttca).magnitude() - radius1 - radius2;
if (dca < 0) dca = 0;
return { ttca, dca };
}*/
std
::
pair
<
float
,
float
>
CostFunction
::
ComputeTimeAndDistanceToClosestApproach
(
const
Vector2D
&
position1
,
const
Vector2D
&
velocity1
,
const
float
radius1
,
const
Vector2D
&
position2
,
const
Vector2D
&
velocity2
,
const
float
radius2
)
const
{
const
Vector2D
dp
(
position2
-
position1
);
const
Vector2D
dv
(
velocity2
-
velocity1
);
const
Vector2D
dp_center
(
position2
-
position1
);
const
Vector2D
dv_center
(
velocity2
-
velocity1
);
// The standard TTCA definition works with these dp and dv immediately.
// We make some corrections to account for the radii of agents:
float
dpMag
=
dp_center
.
magnitude
();
const
Vector2D
&
dp
=
dp_center
/
dpMag
*
(
dpMag
-
radius1
-
radius2
);
Vector2D
dpNormal
(
-
dp_center
.
y
,
dp_center
.
x
);
const
Vector2D
dv
=
dv_center
+
dpNormal
*
dpNormal
.
dot
(
dv_center
);
// Source: Dutra et al, "Gradient-based steering for vision-based crowd simulation algorithms", 2016.
float
ttca
=
(
dv
.
sqrMagnitude
()
==
0
?
0
:
-
dp
.
dot
(
dv
)
/
dv
.
sqrMagnitude
());
float
dca
=
(
dp
+
dv
*
ttca
).
magnitude
()
-
radius1
-
radius2
;
// Prevent negative ttca?
// This means that if the agents are moving away from each other, it does not matter how strongly they do this
if
(
ttca
<
0
)
ttca
=
0
;
float
dca
=
(
dp
+
dv
*
ttca
).
magnitude
();
// subtracting the radii is not needed anymore, dp and dv already incorporate this
// Prevent negative dca?
// Combined with a non-negative ttca, this can only happen if agents are already colliding now.
if
(
dca
<
0
)
dca
=
0
;
return
{
ttca
,
dca
};
}
\ No newline at end of file
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment