rotmatrix.cc

00001 /*
00002  *  This file is part of Healpix_cxx.
00003  *
00004  *  Healpix_cxx is free software; you can redistribute it and/or modify
00005  *  it under the terms of the GNU General Public License as published by
00006  *  the Free Software Foundation; either version 2 of the License, or
00007  *  (at your option) any later version.
00008  *
00009  *  Healpix_cxx is distributed in the hope that it will be useful,
00010  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *  GNU General Public License for more details.
00013  *
00014  *  You should have received a copy of the GNU General Public License
00015  *  along with Healpix_cxx; if not, write to the Free Software
00016  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00017  *
00018  *  For more information about HEALPix, see http://healpix.jpl.nasa.gov
00019  */
00020 
00021 /*
00022  *  Healpix_cxx is being developed at the Max-Planck-Institut fuer Astrophysik
00023  *  and financially supported by the Deutsches Zentrum fuer Luft- und Raumfahrt
00024  *  (DLR).
00025  */
00026 
00027 /*
00028  *  Class for rotation transforms in 3D space
00029  *
00030  *  Copyright (C) 2003 Max-Planck-Society
00031  *  Author: Martin Reinecke
00032  */
00033 
00034 #include "rotmatrix.h"
00035 #include "vec3.h"
00036 #include "lsconstants.h"
00037 #include <algorithm>
00038 
00039 using namespace std;
00040 
00041 rotmatrix::rotmatrix (const vec3 &a, const vec3 &b, const vec3 &c)
00042   {
00043   entry[0][0]=a.x; entry[0][1]=b.x; entry[0][2]=c.x;
00044   entry[1][0]=a.y; entry[1][1]=b.y; entry[1][2]=c.y;
00045   entry[2][0]=a.z; entry[2][1]=b.z; entry[2][2]=c.z;
00046   }
00047 
00048 void rotmatrix::SetToIdentity ()
00049   {
00050   entry[0][0] = entry[1][1] = entry[2][2] = 1.;
00051   entry[0][1] = entry[1][0] = entry[0][2] =
00052   entry[2][0] = entry[1][2] = entry[2][1] = 0.;
00053   }
00054 
00055 void rotmatrix::SetToZero ()
00056   {
00057   for (int m=0; m<3; ++m)
00058     entry[m][0] = entry[m][1] = entry[m][2] = 0;
00059   }
00060 
00061 void rotmatrix::Transpose ()
00062   {
00063   swap(entry[0][1], entry[1][0]);
00064   swap(entry[0][2], entry[2][0]);
00065   swap(entry[1][2], entry[2][1]);
00066   }
00067 
00068 void rotmatrix::toAxisAngle (vec3 &axis, double &angle) const
00069   {
00070   double c2 = entry[0][0] + entry[1][1] + entry[2][2] - 1;
00071   axis.x = entry[2][1] - entry[1][2];
00072   axis.y = entry[0][2] - entry[2][0];
00073   axis.z = entry[1][0] - entry[0][1];
00074 
00075   double s2 = axis.Length();
00076 
00077   if (s2>0)
00078     {
00079     angle = atan2(s2,c2);
00080     axis *= 1/s2;
00081     return;
00082     }
00083 
00084   if (c2>=2) // angle is 0
00085     {
00086     axis = vec3(1,0,0);
00087     angle = 0;
00088     return;
00089     }
00090 
00091   angle = pi;
00092 
00093   int choice = 0; // assume entry[0][0] is the largest
00094   if ((entry[1][1]>entry[0][0]) && (entry[1][1]>entry[2][2])) choice=1;
00095   if ((entry[2][2]>entry[0][0]) && (entry[2][2]>entry[1][1])) choice=2;
00096 
00097   if (choice==0)
00098     {
00099     axis.x = 0.5*sqrt(entry[0][0]-entry[1][1]-entry[2][2]+1);
00100     double half_inv = 0.5/axis.x;
00101     axis.y = half_inv*entry[0][1];
00102     axis.z = half_inv*entry[0][2];
00103     return;
00104     }
00105   if (choice==1)
00106     {
00107     axis.y = 0.5*sqrt(entry[1][1]-entry[0][0]-entry[2][2]+1);
00108     double half_inv = 0.5/axis.y;
00109     axis.x = half_inv*entry[0][1];
00110     axis.z = half_inv*entry[1][2];
00111     return;
00112     }
00113 
00114   axis.z = 0.5*sqrt(entry[2][2]-entry[0][0]-entry[1][1]+1);
00115   double half_inv = 0.5/axis.z;
00116   axis.x = half_inv*entry[0][2];
00117   axis.y = half_inv*entry[1][2];
00118   }
00119 
00120 void rotmatrix::Make_CPAC_Euler_Matrix
00121   (double alpha, double beta, double gamma)
00122   {
00123   double ca=cos(alpha), cb=cos(beta), cg=cos(gamma);
00124   double sa=sin(alpha), sb=sin(beta), sg=sin(gamma);
00125 
00126   entry[0][0]= ca*cb*cg-sa*sg; entry[0][1]=-ca*cb*sg-sa*cg; entry[0][2]= ca*sb;
00127   entry[1][0]= sa*cb*cg+ca*sg; entry[1][1]=-sa*cb*sg+ca*cg; entry[1][2]= sa*sb;
00128   entry[2][0]=-sb*cg;          entry[2][1]= sb*sg;          entry[2][2]= cb;
00129   }
00130 
00131 void rotmatrix::Extract_CPAC_Euler_Angles
00132   (double &alpha, double &beta, double &gamma) const
00133   {
00134   double cb = entry[2][2];
00135   double sb = sqrt(entry[0][2]*entry[0][2] + entry[1][2]*entry[1][2]);
00136   beta=atan2(sb,cb);
00137   if (abs(sb)<=1e-6)
00138     {
00139     alpha=0;
00140     if (cb>0)
00141       gamma=atan2(entry[1][0],entry[0][0]);
00142     else
00143       gamma=atan2(entry[0][1],-entry[0][0]);
00144     }
00145   else
00146     {
00147     alpha=atan2(entry[1][2],entry[0][2]);
00148     gamma=atan2(entry[2][1],-entry[2][0]);
00149     }
00150   }
00151 
00152 rotmatrix operator* (const rotmatrix &a, const rotmatrix &b)
00153   {
00154   rotmatrix res;
00155   for (int i=0; i<3; ++i)
00156     for (int j=0; j<3; ++j)
00157       res.entry[i][j] = a.entry[i][0] * b.entry[0][j]
00158                       + a.entry[i][1] * b.entry[1][j]
00159                       + a.entry[i][2] * b.entry[2][j];
00160   return res;
00161   }
00162 
00163 void TransposeTimes (const rotmatrix &a, const rotmatrix &b, rotmatrix &res)
00164   {
00165   for (int i=0; i<3; ++i)
00166     for (int j=0; j<3; ++j)
00167       res.entry[i][j] = a.entry[0][i] * b.entry[0][j]
00168                       + a.entry[1][i] * b.entry[1][j]
00169                       + a.entry[2][i] * b.entry[2][j];
00170   }
00171 
00172 ostream &operator<< (ostream &os, const rotmatrix &mat)
00173   {
00174   for (int i=0;i<3;++i)
00175     os << mat.entry[i][0] << ' '
00176        << mat.entry[i][1] << ' '
00177        << mat.entry[i][2] << endl;
00178   return os;
00179   }