CollisionDetector.java

package org.opentrafficsim.road.gtu.lane;

import java.util.Iterator;

import org.djunits.value.vdouble.scalar.Angle;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Speed;
import org.djutils.event.EventListenerMap;
import org.djutils.event.EventProducer;
import org.djutils.event.EventType;
import org.djutils.exceptions.Try;
import org.djutils.math.AngleUtil;
import org.djutils.metadata.MetaData;
import org.djutils.metadata.ObjectDescriptor;
import org.opentrafficsim.base.logger.Logger;
import org.opentrafficsim.core.gtu.Gtu;
import org.opentrafficsim.core.gtu.GtuException;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
import org.opentrafficsim.core.network.Network;
import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable.UnderlyingDistance;
import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
import org.opentrafficsim.road.gtu.lane.perception.object.PerceivedGtu;
import org.opentrafficsim.road.network.lane.object.LaneBasedObject;

/**
 * Checks for collisions.
 * <p>
 * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
 * </p>
 * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
 * @author <a href="https://github.com/peter-knoppers">Peter Knoppers</a>
 * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
 */
public class CollisionDetector extends AbstractLaneBasedMoveChecker implements EventProducer
{

    /** Collision event. */
    public static final EventType COLLISION = new EventType(new MetaData("COLLISION",
            "Event when a GTU collides into an object", new ObjectDescriptor("GTU", "Colliding GTU", LaneBasedGtu.class),
            new ObjectDescriptor("Object", "Object that is collided into", LaneBasedObject.class)));

    /** Listener map. */
    private final EventListenerMap listenerMap = new EventListenerMap();

    /**
     * Constructor.
     * @param network network
     */
    public CollisionDetector(final Network network)
    {
        super(network);
    }

    @Override
    public void checkMove(final LaneBasedGtu gtu) throws Exception
    {
        try
        {
            NeighborsPerception neighbors =
                    gtu.getTacticalPlanner().getPerception().getPerceptionCategory(NeighborsPerception.class);
            PerceptionCollectable<PerceivedGtu, LaneBasedGtu> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
            Iterator<UnderlyingDistance<LaneBasedGtu>> gtus = leaders.underlyingWithDistance();
            if (!gtus.hasNext())
            {
                return;
            }
            UnderlyingDistance<LaneBasedGtu> leader = gtus.next();
            if (leader.distance().lt0())
            {
                fireEvent(COLLISION, new Object[] {gtu, leader.object()});

                // throw new CollisionException("GTU " + gtu.getId() + " collided with GTU " + leader.object().getId());
            }
        }
        catch (OperationalPlanException exception)
        {
            throw new GtuException(exception);
        }
    }

    @Override
    public EventListenerMap getEventListenerMap()
    {
        return this.listenerMap;
    }

    /**
     * Logs collision with distance, speed difference and angle at info level.
     * @return this collision detector for method chaining
     */
    public CollisionDetector logCollisions()
    {
        Try.execute(() -> addListener((e) ->
        {
            Object[] payload = (Object[]) e.getContent();
            LaneBasedGtu gtu = (LaneBasedGtu) payload[0];
            LaneBasedObject object = (LaneBasedObject) payload[1];
            Speed objectSpeed = object instanceof Gtu otherGtu ? otherGtu.getSpeed() : Speed.ZERO;
            Speed dv = gtu.getSpeed().minus(objectSpeed);
            Length distance = Length.ofSI(gtu.getLocation().distance(object.getLocation()));
            Angle angle = Angle.ofSI(AngleUtil.normalizeAroundZero(object.getDirZ() - gtu.getDirZ()));
            Logger.ots().info("GTU " + gtu.getId() + " collided with " + object.getId() + " at a point distance of " + distance
                    + " with a speed difference of " + dv + " and and angle of " + angle + ".");
        }, COLLISION), "Unable to listen to collisions.");
        return this;
    }

    /**
     * Stops the GTU and the object if it is a GTU.
     * @return this collision detector for method chaining
     */
    public CollisionDetector stopCollided()
    {
        Try.execute(() -> addListener((e) ->
        {
            Object[] payload = (Object[]) e.getContent();
            LaneBasedGtu gtu = (LaneBasedGtu) payload[0];
            LaneBasedObject object = (LaneBasedObject) payload[1];
            gtu.stop();
            if (object instanceof LaneBasedGtu otherGtu)
            {
                otherGtu.stop();
            }
        }, COLLISION), "Unable to listen to collisions.");
        return this;
    }

    /**
     * Throws an exception upon a collision.
     * @return this collision detector for method chaining
     */
    public CollisionDetector throwException()
    {
        Try.execute(() -> addListener((e) ->
        {
            Object[] payload = (Object[]) e.getContent();
            LaneBasedGtu gtu = (LaneBasedGtu) payload[0];
            LaneBasedObject object = (LaneBasedObject) payload[1];
            throw new CollisionException("GTU " + gtu.getId() + " collided with " + object.getId());
        }, COLLISION), "Unable to listen to collisions.");
        return this;
    }

    /**
     * Destroys the GTU upon a collision.
     * @return this collision detector for method chaining
     */
    public CollisionDetector destroyGtu()
    {
        Try.execute(() -> addListener((e) -> ((LaneBasedGtu) ((Object[]) e.getContent())[0]).destroy(), COLLISION),
                "Unable to listen to collisions.");
        return this;
    }

}