From 9a1f60e5af335d3c9f1d90ed347d8b578bd30374 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Mon, 20 Feb 2023 23:38:42 +0800
Subject: [PATCH] finish bio follower function (uncheck)

---
 class_model/include/class_model/follower.h |  1 +
 class_model/src/follower.cpp               | 54 +++++++++++++---------
 2 files changed, 32 insertions(+), 23 deletions(-)

diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h
index 1df97ce..0817564 100755
--- a/class_model/include/class_model/follower.h
+++ b/class_model/include/class_model/follower.h
@@ -45,6 +45,7 @@ private:
     void calculate_position(float k,float theta);
     void plane(global_location target, global_location leader, global_location follower);
     void find_ref_drone(global_location data[], size_t index, int leaderID);
+    int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member);
     void choose_leader();
     void follow();
     void get_location();
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index c58fea5..5134614 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -18,8 +18,8 @@ void FollowerClass::plane(global_location target, global_location leader, global
     
     //caculate drone is locate on R/L plane
     float slope,c,y;
-    slope = (target_location.lat-leader_location.lat)/(target_location.lon-leader_location.lon);
-    c = target_location.lat - slope*target_location.lon;
+    slope = (target.lat-leader.lat)/(target.lon-leader.lon);
+    c = target.lat - slope*target.lon;
     y = slope * follower.lon + c;
 
     if(follower.lat > y){
@@ -64,6 +64,27 @@ void FollowerClass::plane(global_location target, global_location leader, global
     
 }
 
+int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){
+
+    global_location vector_LT,vector_SM;
+    float m,n,k,cosTheta;
+    vector_LT.lon = leader.lon - target.lon;       //vector_LT (leader->target)
+    vector_LT.lat = leader.lat - target.lat;
+    vector_SM.lon = leader.lon - target.lon;       //vector_SM (self->member)
+    vector_SM.lat = leader.lat - target.lat;
+    k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat);     //k = vector_LT dot vector_SM
+    m = sqrt(pow(vector_LT.lon,2)+pow(vector_LT.lat,2));     //m = |vector_LT|
+    n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2));     //n = |vector_SM|
+    cosTheta = k / (m*n);
+
+    if(cosTheta > 0){
+        return 1;
+    }else{
+        return INFINITY;
+    }
+
+}
+
 void FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){
 
     for(int i=1 ; i<index ; i++){                //check leader drone
@@ -120,7 +141,7 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
     }
 
     for(int i=0;i<index-1;i++){
-        if(memberDrone[i].plane == self.ID){
+        if(memberDrone[i].plane == self.plane){    //load same plane member
             samePlane[k] = memberDrone[i];
             k++;
         }
@@ -129,35 +150,22 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
 
     int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
     float d[s_index];
-    int check_ID;
+    int dir,check_index,ref_ID;
 
-    for(int i=0 ; i<s_index ; i++){
+    for(int i=0 ; i<s_index ; i++){                 //caculate each same plane mamber's distance
 
-        d[i] = sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lat - samePlane[i].init_location.lat),2) );
+        dir = direct(data[0],leader_drone,self,samePlane[i]);
+        d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lat - samePlane[i].init_location.lat),2) );
 
         // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
     }
 
     std::vector<float> _d(d,d+s_index-1); 
     
-    check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
-    
-    // std::cout <<"Drone " <<  " is leader " << std::endl;
-
-
-
-
-
-
-
-
-    
+    check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
+    ref_ID = samePlane[check_index].ID;
 
-    
-
-
-
-    
+    // std::cout <<"Drone " <<  " is leader " << std::endl;    
 
 }