From fba0dbe8855d8c44569eb1e128b2f12922783891 Mon Sep 17 00:00:00 2001
From: JH159753 <JHL159753@gmail.com>
Date: Wed, 21 Sep 2022 02:04:57 -0700
Subject: [PATCH] add boid pathfinding

---
 Boid.pde | 63 +++++++++++++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 56 insertions(+), 7 deletions(-)

diff --git a/Boid.pde b/Boid.pde
index 3856023..744ba50 100644
--- a/Boid.pde
+++ b/Boid.pde
@@ -38,13 +38,63 @@ class Boid
      if (target != null)
      {  
         // TODO: Implement seek here
-        //println(kinematic.getHeading());
         
-        //float targetDirection = atan2(target.y, target.x);
-        float temp1 = atan2(kinematic.getPosition().y-target.y, kinematic.getPosition().x-target.x);
-        float temp2 = kinematic.getHeading();
-        kinematic.increaseSpeed(1,kinematic.getHeading() - temp1);
-        println(temp1 + " " + temp2);
+        
+        //This makes a vector with the direction our boid needs to go to
+        PVector direction = PVector.sub(target, kinematic.position);
+        
+        //atan2(direction.y, direction.x) will return the direction we need to go in radians
+        
+        //print direction we need to go and the direction we are facing right now
+        //println(atan2(direction.y, direction.x) + " " + normalize_angle_left_right(kinematic.getHeading()));
+        
+        float directionalThreshold = .1;
+        float angleToTarget = atan2(direction.y, direction.x) - normalize_angle_left_right(kinematic.getHeading());
+        float arrivalThreshold = 60.0;
+        
+        //This just draws a circle for visual debugging purposes
+        //circle(target.x, target.y, arrivalThreshold);
+        
+        //prints the angle to the target
+        //println(angleToTarget);
+        
+        //if the angle is larger than the threshold in the positive direction, rotate counterclockwise
+        if (angleToTarget > directionalThreshold) {
+          kinematic.increaseSpeed(0.0, +1);
+          
+        //if the angle is smaller than the threshold in the negative direction, rotate clockwise
+        } else if (angleToTarget < -directionalThreshold) {
+          kinematic.increaseSpeed(0.0, -1);
+          
+        //if the angle is within our threshold, stop our rotational velocity by rotating opposite
+        } else if (directionalThreshold > angleToTarget) {
+          
+          if (kinematic.getRotationalVelocity() > 0) {
+            kinematic.increaseSpeed(0.0, -1);
+          }
+          else if (kinematic.getRotationalVelocity() < 0) {
+            kinematic.increaseSpeed(0.0, 1); 
+          }
+        }
+        
+        
+        
+        //Slight flaw: since the arrival threshold is so big, the boid just won't move if its target is that close. 
+        
+        //if the target is outside its arrival threshold, accelerate. 
+        //if the target is inside its arrival threshold, accelerate backwards until the speed is 0.
+        if (direction.mag() > arrivalThreshold) {
+          kinematic.increaseSpeed(1,0);
+        } else if (direction.mag() < arrivalThreshold) {
+          if (kinematic.getSpeed() > 0) {
+            kinematic.increaseSpeed(-1,0);
+          } 
+        }
+        
+        
+        
+        //drawing a line for testing purposes
+        line(kinematic.position.x, kinematic.position.y, kinematic.position.x + direction.x, kinematic.position.y + direction.y);
         
         
         
@@ -54,7 +104,6 @@ class Boid
         
         
         
-        kinematic.increaseSpeed(1,-.001);
      }
      
      // place crumbs, do not change