diff --git a/miscmaths.cc b/miscmaths.cc index 3650d619b13907a8de65226c16300e00ba4da13f..9649634333eab13434438e7cfa0422e1c5617954 100644 --- a/miscmaths.cc +++ b/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