Merge pull request #168 from S010MON/Agent-Collision-RE

agent collision test

src/main/java/app/controller/GameEngine.java

@@ -2,11 +2,11 @@
 
 import app.controller.graphicsEngine.GraphicsEngine;
 import app.controller.graphicsEngine.RayTracing;
+import app.controller.linAlg.Intersection;
 import app.controller.linAlg.Vector;
 import app.model.agents.Agent;
 import app.model.boundary.Boundary;
 import app.model.Map;
-import app.view.simulation.Info;
 import app.view.simulation.Renderer;
 import javafx.animation.Animation;
 import javafx.animation.KeyFrame;
@@ -37,12 +37,15 @@ public void tick()
             Vector startPoint = a.getPosition();
             Vector endPoint = startPoint.add(a.move().getDeltaPos());
 
-            if (legalMove(startPoint, endPoint))
+            if (legalMove(startPoint, endPoint) &&
+                legalMove(a, endPoint) &&
+                legalMove(a, startPoint) && legalMove(a, startPoint,endPoint))
+            {
                 a.updateLocation(endPoint);
+            }
         }
 
         map.getAgents().forEach(a -> a.updateView(graphicsEngine.compute(map, a)));
-
         renderer.render();
     }
 
@@ -70,4 +73,28 @@ private boolean legalMove(Vector start, Vector end)
         }
         return true;
     }
+
+    private boolean legalMove(Agent currentAgent, Vector end)
+    {
+        for(Agent otherAgent: map.getAgents())
+        {
+            double dist = otherAgent.getPosition().dist(end);
+            if(!currentAgent.equals(otherAgent) && dist <= currentAgent.getRadius())
+                return false;
+        }
+        return true;
+    }
+
+    private boolean legalMove(Agent currentAgent, Vector start,Vector end)
+    {
+        double radius = currentAgent.getRadius();
+        for(Agent otherAgent: map.getAgents())
+        {
+            Vector positionOther = currentAgent.getPosition();
+            double radiusOther = otherAgent.getRadius();
+            if(!currentAgent.equals(otherAgent) && !Intersection.hasDirectionIntersect(start, end, radius, positionOther, radiusOther))
+                return false;
+        }
+        return true;
+    }
 }

src/main/java/app/controller/linAlg/Intersection.java

@@ -3,6 +3,9 @@
 import app.controller.graphicsEngine.Ray;
 import app.controller.soundEngine.SoundRay;
 
+import static java.lang.Math.atan2;
+import static java.lang.Math.toDegrees;
+
 public abstract class Intersection
 {
     public static Vector findIntersection(Vector p_1, Vector p_2, Vector p_3, Vector p_4)
@@ -86,4 +89,34 @@ public static boolean hasIntersection(SoundRay r1, SoundRay r2)
         return findIntersection(r1, r2) != null;
     }
 
+    public static boolean hasDirectionIntersect(Vector start,Vector end,double radius,Vector positionOther,double radiusOther){
+
+        Vector recCenter = start.add(end).scale(0.5);
+        // rotate all the positions to the degree 0
+        Vector start_rotate = recCenter.findPointOnCircle(start.dist(recCenter),180);
+        Vector end_rotate = recCenter.findPointOnCircle(end.dist(recCenter), 0);
+
+        Vector recBL_rotate = start_rotate.findPointOnCircle(radius,270);
+        Vector recTR_rotate = end_rotate.findPointOnCircle(radius,90);
+
+        Vector otherRotate = recCenter.findPointOnCircle(positionOther.dist(recCenter),0);
+
+        // check if circle is in the rectangle but has no intersect
+        if(otherRotate.getX() >= recBL_rotate.getX() &&
+                otherRotate.getX() <= recTR_rotate.getX() &&
+                otherRotate.getY() >= recBL_rotate.getY() &&
+                otherRotate.getY() <= recTR_rotate.getY()){
+            return true;
+        }
+
+        // find the nearest point to rec
+        double Xn = Math.max(recBL_rotate.getX(), Math.min(otherRotate.getX(), recTR_rotate.getX()));
+        double Yn = Math.max(recBL_rotate.getY(), Math.min(otherRotate.getY(), recTR_rotate.getY()));
+        double Dx = Xn - otherRotate.getX();
+        double Dy = Yn - otherRotate.getY();
+
+        return (Dx * Dx + Dy * Dy) <= radiusOther * radiusOther;
+    }
+
+
 }

src/main/java/app/controller/linAlg/Vector.java

@@ -90,4 +90,10 @@ public Integer vectorHashCode()
     {
         return this.hashCode();
     }
+
+    public Vector findPointOnCircle(double r, double theta)
+    {
+        return new Vector(this.x + (r * Math.cos(Math.toRadians(theta))), this.y + (r * Math.sin(Math.toRadians(theta))));
+    }
+
 }

src/main/java/app/model/agents/Agent.java

@@ -29,4 +29,7 @@ public interface Agent extends Boundary
     void updateView(ArrayList<Ray> view);
 
     double getHearing();
+
+    double getRadius();
+
 }

src/main/java/app/model/agents/AgentImp.java

@@ -17,7 +17,7 @@ public class AgentImp implements Agent
     @Getter @Setter protected double maxSprint = 10;
     @Getter protected Vector position;
     @Getter protected Vector direction;
-    protected double radius;
+    @Getter protected double radius;
     protected ArrayList<Ray> view;
 
     public AgentImp(Vector position, Vector direction, double radius)

src/test/java/testing/AgentCollisionTest.java

@@ -0,0 +1,35 @@
+package testing;
+
+import app.controller.linAlg.Intersection;
+import app.controller.linAlg.Vector;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+public class AgentCollisionTest
+{
+    @Test
+    void intersectOnePoint()
+    {
+        assertTrue(Intersection.hasDirectionIntersect(new Vector(0,0),new Vector(0,5),1,new Vector(0,0),0.5));
+    }
+
+    @Test
+    void intersectStatic()
+    {
+        assertFalse(Intersection.hasDirectionIntersect(new Vector(0,0),new Vector(0,0),5,new Vector(10.1,0),5));
+    }
+
+    @Test
+    void intersectRec()
+    {
+        assertTrue(Intersection.hasDirectionIntersect(new Vector(0,0),new Vector(10,10),1,new Vector(3,3),0.5));
+    }
+
+    @Test
+    void noIntersect()
+    {
+        assertFalse(Intersection.hasDirectionIntersect(new Vector(0, 0), new Vector(2, 2), 1, new Vector(3, 3), 0.5));
+    }
+}