Teleport on agent done (#121)

* Teleport on agent done

* get check collision event

* Changed the padding position

src/sim.cpp

@@ -284,6 +284,19 @@ inline void movementSystem(Engine &e,
         }
     }
 
+    if(done.v)
+    {
+        // Case: Agent has not collided but is done. 
+        // This can only happen if the agent has reached goal or the episode has ended.
+        // In that case we teleport the agent. The agent will not collide with anything.
+        position = consts::kPaddingPosition;
+        velocity.linear.x = 0;
+        velocity.linear.y = 0;
+        velocity.linear.z = 0;
+        velocity.angular = Vector3::zero();
+        return;
+    }
+
     if (type == EntityType::Vehicle && controlledState.controlledState == ControlMode::BICYCLE)
     {
         // TODO: Handle the case when the agent is not valid. Currently, we are not doing anything.
@@ -527,23 +540,41 @@ inline void stepTrackerSystem(Engine &ctx,
 void collisionDetectionSystem(Engine &ctx,
                               const CandidateCollision &candidateCollision) {
 
-    auto isExpertAgentInInvalidState = [&](const Loc &candidate) -> bool
+    auto isInvalidExpertOrDone = [&](const Loc &candidate) -> bool
     {
         auto controlledState = ctx.getCheck<ControlledState>(candidate);
-        if (controlledState.valid() && controlledState.value().controlledState == ControlMode::EXPERT)
+        if (controlledState.valid())
         {
-            auto currStep = getCurrentStep(ctx.get<StepsRemaining>(candidate));
-            auto validState = ctx.get<Trajectory>(candidate).valids[currStep];
-            if (!validState)
+            if( controlledState.value().controlledState == ControlMode::EXPERT)
+            {
+                // Case: If an expert agent is in an invalid state, we need to ignore the collision detection for it.
+                auto currStep = getCurrentStep(ctx.get<StepsRemaining>(candidate));
+                auto validState = ctx.get<Trajectory>(candidate).valids[currStep];
+                if (!validState)
+                {
+                    return true;
+                }
+            }
+            else if (controlledState.value().controlledState == ControlMode::BICYCLE)
             {
-                return true;
+                // Case: If a controlled agent gets done, we teleport it to the padding position
+                // Hence we need to ignore the collision detection for it.
+                // The agent can also be done because it collided. 
+                // In that case, we dont want to ignore collision. Especially if AgentStop is set.
+                auto done = ctx.get<Done>(candidate);
+                auto collisionEvent = ctx.getCheck<CollisionDetectionEvent>(candidate);
+                if(done.v && collisionEvent.valid() && !collisionEvent.value().hasCollided.load_relaxed())
+                {
+                    return true;
+                }
             }
         }
         return false;
     };
 
-    if (isExpertAgentInInvalidState(candidateCollision.a) ||
-        isExpertAgentInInvalidState(candidateCollision.b)) {
+    if (isInvalidExpertOrDone(candidateCollision.a) || 
+        isInvalidExpertOrDone(candidateCollision.b)) {
+
         return;
     }