From 91d68cd2772aafd2172fab772c4d2d967bbc15b2 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Thu, 26 Jan 2017 17:42:30 -0500
Subject: [PATCH] tmp fix of spherical coord. bug

---
 include/roscontroller.h |  5 ++++-
 src/roscontroller.cpp   | 36 +++++++++++++++++++++++++++---------
 src/test1.bzz           |  6 +++---
 3 files changed, 34 insertions(+), 13 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index f754be9..eb9d1b5 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -85,8 +85,11 @@ private:
 	void set_cur_pos(double latitude,
 			 double longitude,
 			 double altitude);
-	/*convert from catresian to spherical coordinate system callback */
+	/*convert from cartesian to spherical coordinate system callback */
 	double* cvt_spherical_coordinates(double neighbours_pos_payload []);
+
+	/*convert from spherical to cartesian coordinate system callback */
+	double* cvt_cartesian_coordinates(double neighbours_pos_payload []);
 	
 	/*battery status callback*/ 
 	void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index f103136..33d4fa6 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -246,16 +246,32 @@ namespace rosbzz_node{
 
 	}
 
-	/*convert from catresian to spherical coordinate system callback */
-	double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
-		double latitude,longitude,altitude;
+	/*convert from spherical to cartesian coordinate system callback */
+	double* roscontroller::cvt_cartesian_coordinates(double neighbours_pos_payload []){
+		double latitude, longitude, rho;
 		latitude=neighbours_pos_payload[0];
 		longitude = neighbours_pos_payload[1];
-		altitude=neighbours_pos_payload[2];
+		rho=neighbours_pos_payload[2]+6371000; //centered on Earth
 		try {
-       		neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
-		neighbours_pos_payload[1]=atan(longitude/latitude);
-		neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
+			neighbours_pos_payload[0] = cos(latitude) * cos(longitude) * rho;
+    			neighbours_pos_payload[1] = cos(latitude) * sin(longitude) * rho;
+    			neighbours_pos_payload[2] = sin(latitude) * rho; // z is 'up'
+   		} catch (std::overflow_error e) {
+        	std::cout << e.what() << " Error in convertion to cartesian coordinate system "<<endl;
+   		}
+		
+		return neighbours_pos_payload;
+	}
+	/*convert from cartesian to spherical coordinate system callback */
+	double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
+		double x, y, z;
+		x = neighbours_pos_payload[0];
+		y = neighbours_pos_payload[1];
+		z = neighbours_pos_payload[2];
+		try {
+       		neighbours_pos_payload[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
+		neighbours_pos_payload[1]=atan(y/x);
+		neighbours_pos_payload[2]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z);
    		} catch (std::overflow_error e) {
         	std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
    		}
@@ -303,9 +319,11 @@ namespace rosbzz_node{
 		memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
 		buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
 		//cout<<"obt lat ,long alt"<<neighbours_pos_payload[0]<<neighbours_pos_payload[1]<<neighbours_pos_payload[2];
-		/*Convert obtained position to relative position*/
+		/*Convert obtained position to relative CARTESIAN position*/
+		double* cartesian_neighbours_pos = cvt_cartesian_coordinates(neighbours_pos_payload);
+		double* cartesian_cur_pos = cvt_cartesian_coordinates(cur_pos);
 		for(i=0;i<3;i++){
-			neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
+			neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
 		}
 		double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
 		/*Extract robot id of the neighbour*/
diff --git a/src/test1.bzz b/src/test1.bzz
index bea7846..1d58cb8 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -2,11 +2,11 @@
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
 include "/home/ubuntu/buzz/src/include/vec2.bzz"
 
-TARGET_ALTITUDE = 10.0
+TARGET_ALTITUDE = 10.1
  
 # Lennard-Jones parameters
-TARGET     = 0.000000100005
-EPSILON    = 0.1
+TARGET     = 0.000001001
+EPSILON    = 0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {