SJ changes for Cfdt

miscmaths.cc

@@ -1109,7 +1109,12 @@ float median(const ColumnVector& x)
   return m;
 }
 
-
+void sph2cart(const ColumnVector& dir, float& th, float& ph)
+{
+  dir(1) = cos(ph) * sin(th);
+  dir(2) = sin(ph) * sin(th);
+  dir(3) = cos(th);
+}
 
 
 void cart2sph(const ColumnVector& dir, float& th, float& ph)
@@ -1166,6 +1171,45 @@ void cart2sph(const Matrix& dir,ColumnVector& th,ColumnVector& ph)
   }
 }
 
+// added by SJ
+void cart2sph(const vector<ColumnVector>& dir,ColumnVector& th,ColumnVector& ph)
+{
+  if(th.Nrows()!=dir.size()){
+    th.ReSize(dir.size());
+  }
+
+  if(ph.Nrows()!=dir.size()){
+    ph.ReSize(dir.size());
+  }
+  //double _2pi=2*M_PI;
+  double _pi2=M_PI/2;
+
+  int j=1;
+  for (unsigned int i=0;i<dir.size();i++) {
+	float mag=std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2)+dir[i](3)*dir[i](3));
+    if(mag==0){
+      ph(j)=_pi2;
+      th(j)=_pi2;
+    }
+    else{
+      if(dir[i](1)==0 && dir[i](2)>=0) ph(j)=_pi2;
+      else if(dir[i](1)==0 && dir[i](2)<0) ph(j)=-_pi2;
+      else if(dir[i](1)>0) ph(j)=std::atan(dir[i](2)/dir[i](1));
+      else if(dir[i](2)>0) ph(j)=std::atan(dir[i](2)/dir[i](1))+M_PI;
+      else ph(j)=std::atan(dir[i](2)/dir[i](1))-M_PI;
+
+      //ph(j)=fmod(ph(j),_2pi);
+
+      if(dir[i](3)==0) th(j)=_pi2;
+      else if(dir[i](3)>0) th(j)=std::atan(std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2))/dir[i](3));
+      else th(j)=std::atan(std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2))/dir[i](3))+M_PI;
+      
+      //th(j)=fmod(th(j),M_PI);
+
+    }
+    j++;
+  }
+}
 
 // Added by CFB   --- Matlab style Matrix functions