Skip to content

Commit

Permalink
Remove static member function evaluateTimeToCollisionCondition
Browse files Browse the repository at this point in the history 
Signed-off-by: yamacir-kit <[email protected]>
  • Loading branch information
@yamacir-kit
yamacir-kit committed Dec 12, 2024
1 parent 3e20e28 commit b2a1b1e
Show file tree
Hide file tree
Showing 3 changed files with 12 additions and 214 deletions.
145 changes: 0 additions & 145 deletions openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -620,151 +620,6 @@ class SimulatorCore
return std::numeric_limits<value_type>::quiet_NaN();
}
}

static auto evaluateTimeToCollisionCondition(const Entity & from, const Entity & to)
{
/*
This function uses the Separating Axis Theorem (SAT) to find the
Time-To-Collision (TTC) of two entities in the world coordinate
system.
First, the expected time of collision is obtained independently for
each separating axis. Then, we check whether the polygons of the
entities actually collide after that time. If no collision occurs, the
expected collision time is rejected. The lowest value of the expected
collision times obtained for each separate axis is then returned as
the TTC.
*/

if (const auto entity_a = core->getEntity(from.name())) {
if (const auto entity_b = core->getEntity(to.name())) {
struct OrientedBoundingBox
{
Eigen::Vector3d position;

Eigen::Matrix3d rotation;

Eigen::Vector3d size; // [width, height, depth]

explicit OrientedBoundingBox(
const geometry_msgs::msg::Pose & pose,
const traffic_simulator_msgs::msg::BoundingBox & box)
: position(
pose.position.x + box.center.x, pose.position.y + box.center.y,
pose.position.z + box.center.z),
rotation(math::geometry::getRotationMatrix(pose.orientation)),
size(box.dimensions.y / 2, box.dimensions.z / 2, box.dimensions.x / 2)
{
}

auto axis(std::size_t index) const -> Eigen::Vector3d { return rotation.col(index); }
};

const Eigen::Vector3d v = evaluateRelativeSpeed(from, to);

const auto a = OrientedBoundingBox(entity_a->getMapPose(), entity_a->getBoundingBox());
const auto b = OrientedBoundingBox(entity_b->getMapPose(), entity_b->getBoundingBox());

const auto axes = std::array<Eigen::Vector3d, 15>{
a.axis(0),
a.axis(1),
a.axis(2),

b.axis(0),
b.axis(1),
b.axis(2),

a.axis(0).cross(b.axis(0)),
a.axis(0).cross(b.axis(1)),
a.axis(0).cross(b.axis(2)),

a.axis(1).cross(b.axis(0)),
a.axis(1).cross(b.axis(1)),
a.axis(1).cross(b.axis(2)),

a.axis(2).cross(b.axis(0)),
a.axis(2).cross(b.axis(1)),
a.axis(2).cross(b.axis(2)),
};

Eigen::Vector3d d = b.position - a.position;

auto t_min = std::numeric_limits<double>::infinity();

auto projection = [&](const auto & box, const auto & axis) {
return std::abs(axis.dot(box.axis(0)) * box.size(0)) +
std::abs(axis.dot(box.axis(1)) * box.size(1)) +
std::abs(axis.dot(box.axis(2)) * box.size(2));
};

for (const auto & axis : axes) {
/*
The variable `separation` means how far apart two Oriented
Bounding Boxes (OBBs) are on the separation axis. When it is
positive, it means they are separated by `separation`, i.e. the
OBBs do not overlap on the separation axis. When it is less than
zero, it means the OBBs overlap on the separation axis.
d.dot(axis) is the projection of the center distance between the
two OBBs onto the separation axis, and projection(a, axis) and
projection(b, axis) are half the projected range of the boxes
when the two OBBs are projected onto the separation axis.
*/
if (const auto separation =
std::abs(d.dot(axis)) - projection(a, axis) - projection(b, axis);
0 < separation) {
/*
v.dot(axis) is the projection of the relative distance onto
the separation axis. In other words, we want to find the TTC
for each separation axis. The relative velocity is the
velocity vector of the observed object measured in the
observer's rest frame, so a negative value means that the two
OBBs are approaching each other.
*/
if (const auto relative_speed = v.dot(axis); relative_speed < 0) {
/*
Since this t is only a Time-To-Collision on this separation
axis, it is necessary to check that a and b are not
separated on all separation axes after t seconds.
*/
const auto t = separation / -relative_speed;

const auto a_t = a;

const auto b_t = [&]() {
/*
Since t is the exact time when contact occurs, it may not
be possible to say that a and b are in collision after t
seconds. Therefore, 0.01 seconds is added to t so that
collision can be expected to occur with certainty. This
value "0.01" has no technical basis and is an arbitrary
value.
*/
constexpr auto tolerance = 0.01;
auto copy = b;
copy.position += v * (t + tolerance);
return copy;
}();

Eigen::Vector3d d_t = b_t.position - a_t.position;

if (std::all_of(axes.begin(), axes.end(), [&](const auto & axis) {
const auto separation_t =
std::abs(d_t.dot(axis)) - projection(a_t, axis) - projection(b_t, axis);
return separation_t <= 0;
})) {
t_min = std::min(t_min, t);
}
}
}
}

return t_min;
}
}

return std::numeric_limits<double>::quiet_NaN();
}
};

class NonStandardOperation : private CoordinateSystemConversion
Expand Down
33 changes: 12 additions & 21 deletions ...nario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,19 +141,18 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio
}
};

auto directional_dimension = [&]() {
switch (relative_distance_type) {
case RelativeDistanceType::longitudinal:
return std::optional<DirectionalDimension>(DirectionalDimension::longitudinal);
case RelativeDistanceType::lateral:
return std::optional<DirectionalDimension>(DirectionalDimension::lateral);
default:
case RelativeDistanceType::euclidianDistance:
return std::optional<DirectionalDimension>(std::nullopt);
};
};

auto speed = [&]() {
auto directional_dimension = [&]() {
switch (relative_distance_type) {
case RelativeDistanceType::longitudinal:
return std::optional<DirectionalDimension>(DirectionalDimension::longitudinal);
case RelativeDistanceType::lateral:
return std::optional<DirectionalDimension>(DirectionalDimension::lateral);
default:
case RelativeDistanceType::euclidianDistance:
return std::optional<DirectionalDimension>(std::nullopt);
};
};
if (time_to_collision_condition_target.is<Entity>()) {
return RelativeSpeedCondition::evaluate(
entities, triggering_entity, time_to_collision_condition_target.as<Entity>(),
Expand All @@ -163,15 +162,7 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio
}
};

if (
time_to_collision_condition_target.is<Entity>() and
coordinate_system == CoordinateSystem::entity and
relative_distance_type == RelativeDistanceType::euclidianDistance and freespace) {
return evaluateTimeToCollisionCondition(
triggering_entity, time_to_collision_condition_target.as<Entity>());
} else {
return distance() / std::max(speed(), 0.0);
}
return distance() / std::max(speed(), 0.0);
}

auto evaluate()
Expand Down
48 changes: 0 additions & 48 deletions ...ario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,6 @@ OpenSCENARIO:
name: ''
Properties:
Property: []
- name: vehicle_02
CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle }
ObjectController:
Controller:
name: ''
Properties:
Property: []
Storyboard:
Init:
Actions:
Expand Down Expand Up @@ -72,28 +65,6 @@ OpenSCENARIO:
Property:
- name: maxSpeed
value: '0.5'
- entityRef: vehicle_02
PrivateAction:
- TeleportAction:
Position:
LanePosition:
roadId: ''
laneId: '34576'
s: 0
offset: 0
Orientation:
type: relative
h: 0
p: 0
r: 0
- ControllerAction:
AssignControllerAction:
Controller:
name: ''
Properties:
Property:
- name: maxSpeed
value: '2' # If 2.5, collision will occur.
Story:
- name: ''
Act:
Expand Down Expand Up @@ -245,25 +216,6 @@ OpenSCENARIO:
TimeToCollisionConditionTarget:
EntityRef:
entityRef: vehicle_01
- Condition:
- name: ''
delay: 0
conditionEdge: none
ByEntityCondition:
TriggeringEntities:
triggeringEntitiesRule: any
EntityRef: [ entityRef: ego ]
EntityCondition:
TimeToCollisionCondition:
freespace: true
rule: lessThan
value: 0
relativeDistanceType: cartesianDistance
coordinateSystem: entity
routingAlgorithm: undefined
TimeToCollisionConditionTarget:
EntityRef:
entityRef: vehicle_02
Action:
- name: ''
UserDefinedAction:
Expand Down

0 comments on commit b2a1b1e

Please sign in to comment.