rotmatrix.cc

00001 /*
00002  *  This file is part of libcxxsupport.
00003  *
00004  *  libcxxsupport 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  *  libcxxsupport 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 libcxxsupport; if not, write to the Free Software
00016  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00017  */
00018 
00019 /*
00020  *  libcxxsupport is being developed at the Max-Planck-Institut fuer Astrophysik
00021  *  and financially supported by the Deutsches Zentrum fuer Luft- und Raumfahrt
00022  *  (DLR).
00023  */
00024 
00025 /*
00026  *  Class for rotation transforms in 3D space
00027  *
00028  *  Copyright (C) 2003-2011 Max-Planck-Society
00029  *  Author: Martin Reinecke
00030  */
00031 
00032 #include <algorithm>
00033 #include "rotmatrix.h"
00034 #include "lsconstants.h"
00035 
00036 using namespace std;
00037 
00038 rotmatrix::rotmatrix (const vec3 &a, const vec3 &b, const vec3 &c)
00039   {
00040   entry[0][0]=a.x; entry[0][1]=b.x; entry[0][2]=c.x;
00041   entry[1][0]=a.y; entry[1][1]=b.y; entry[1][2]=c.y;
00042   entry[2][0]=a.z; entry[2][1]=b.z; entry[2][2]=c.z;
00043   }
00044 
00045 void rotmatrix::SetToIdentity ()
00046   {
00047   entry[0][0] = entry[1][1] = entry[2][2] = 1.;
00048   entry[0][1] = entry[1][0] = entry[0][2] =
00049   entry[2][0] = entry[1][2] = entry[2][1] = 0.;
00050   }
00051 
00052 void rotmatrix::SetToZero ()
00053   {
00054   for (int m=0; m<3; ++m)
00055     entry[m][0] = entry[m][1] = entry[m][2] = 0;
00056   }
00057 
00058 void rotmatrix::Transpose ()
00059   {
00060   swap(entry[0][1], entry[1][0]);
00061   swap(entry[0][2], entry[2][0]);
00062   swap(entry[1][2], entry[2][1]);
00063   }
00064 
00065 void rotmatrix::toAxisAngle (vec3 &axis, double &angle) const
00066   {
00067   double c2 = entry[0][0] + entry[1][1] + entry[2][2] - 1;
00068   axis.x = entry[2][1] - entry[1][2];
00069   axis.y = entry[0][2] - entry[2][0];
00070   axis.z = entry[1][0] - entry[0][1];
00071 
00072   double s2 = axis.Length();
00073 
00074   if (s2>0)
00075     {
00076     angle = atan2(s2,c2);
00077     axis *= 1/s2;
00078     return;
00079     }
00080 
00081   if (c2>=2) // angle is 0
00082     {
00083     axis = vec3(1,0,0);
00084     angle = 0;
00085     return;
00086     }
00087 
00088   angle = pi;
00089 
00090   int choice = 0; // assume entry[0][0] is the largest
00091   if ((entry[1][1]>entry[0][0]) && (entry[1][1]>entry[2][2])) choice=1;
00092   if ((entry[2][2]>entry[0][0]) && (entry[2][2]>entry[1][1])) choice=2;
00093 
00094   if (choice==0)
00095     {
00096     axis.x = 0.5*sqrt(entry[0][0]-entry[1][1]-entry[2][2]+1);
00097     double half_inv = 0.5/axis.x;
00098     axis.y = half_inv*entry[0][1];
00099     axis.z = half_inv*entry[0][2];
00100     return;
00101     }
00102   if (choice==1)
00103     {
00104     axis.y = 0.5*sqrt(entry[1][1]-entry[0][0]-entry[2][2]+1);
00105     double half_inv = 0.5/axis.y;
00106     axis.x = half_inv*entry[0][1];
00107     axis.z = half_inv*entry[1][2];
00108     return;
00109     }
00110 
00111   axis.z = 0.5*sqrt(entry[2][2]-entry[0][0]-entry[1][1]+1);
00112   double half_inv = 0.5/axis.z;
00113   axis.x = half_inv*entry[0][2];
00114   axis.y = half_inv*entry[1][2];
00115   }
00116 
00117 void rotmatrix::Make_Axis_Rotation_Transform (const vec3 &axis, double angle)
00118   {
00119   double sa=sin(angle), ca=cos(angle);
00120   double ica=1-ca;
00121   entry[0][0] = axis.x*axis.x*ica + ca;
00122   entry[1][1] = axis.y*axis.y*ica + ca;
00123   entry[2][2] = axis.z*axis.z*ica + ca;
00124   double t1 = axis.x*axis.y*ica, t2 = axis.z*sa;
00125   entry[1][0] = t1 + t2;
00126   entry[0][1] = t1 - t2;
00127   t1 = axis.x*axis.z*ica; t2 = axis.y*sa;
00128   entry[2][0] = t1 - t2;
00129   entry[0][2] = t1 + t2;
00130   t1 = axis.y*axis.z*ica; t2 = axis.x*sa;
00131   entry[1][2] = t1 - t2;
00132   entry[2][1] = t1 + t2;
00133   }
00134 
00135 void rotmatrix::Make_CPAC_Euler_Matrix
00136   (double alpha, double beta, double gamma)
00137   {
00138   double ca=cos(alpha), cb=cos(beta), cg=cos(gamma);
00139   double sa=sin(alpha), sb=sin(beta), sg=sin(gamma);
00140 
00141   entry[0][0]= ca*cb*cg-sa*sg; entry[0][1]=-ca*cb*sg-sa*cg; entry[0][2]= ca*sb;
00142   entry[1][0]= sa*cb*cg+ca*sg; entry[1][1]=-sa*cb*sg+ca*cg; entry[1][2]= sa*sb;
00143   entry[2][0]=-sb*cg;          entry[2][1]= sb*sg;          entry[2][2]= cb;
00144   }
00145 
00146 void rotmatrix::Extract_CPAC_Euler_Angles
00147   (double &alpha, double &beta, double &gamma) const
00148   {
00149   double cb = entry[2][2];
00150   double sb = sqrt(entry[0][2]*entry[0][2] + entry[1][2]*entry[1][2]);
00151   beta=atan2(sb,cb);
00152   if (abs(sb)<=1e-6)
00153     {
00154     alpha=0;
00155     if (cb>0)
00156       gamma=atan2(entry[1][0],entry[0][0]);
00157     else
00158       gamma=atan2(entry[0][1],-entry[0][0]);
00159     }
00160   else
00161     {
00162     alpha=atan2(entry[1][2],entry[0][2]);
00163     gamma=atan2(entry[2][1],-entry[2][0]);
00164     }
00165   }
00166 
00167 rotmatrix operator* (const rotmatrix &a, const rotmatrix &b)
00168   {
00169   rotmatrix res;
00170   for (int i=0; i<3; ++i)
00171     for (int j=0; j<3; ++j)
00172       res.entry[i][j] = a.entry[i][0] * b.entry[0][j]
00173                       + a.entry[i][1] * b.entry[1][j]
00174                       + a.entry[i][2] * b.entry[2][j];
00175   return res;
00176   }
00177 
00178 void matmult (const rotmatrix &a, const rotmatrix &b, rotmatrix &res)
00179   {
00180   for (int i=0; i<3; ++i)
00181     for (int j=0; j<3; ++j)
00182       res.entry[i][j] = a.entry[i][0] * b.entry[0][j]
00183                       + a.entry[i][1] * b.entry[1][j]
00184                       + a.entry[i][2] * b.entry[2][j];
00185   }
00186 
00187 void TransposeTimes (const rotmatrix &a, const rotmatrix &b, rotmatrix &res)
00188   {
00189   for (int i=0; i<3; ++i)
00190     for (int j=0; j<3; ++j)
00191       res.entry[i][j] = a.entry[0][i] * b.entry[0][j]
00192                       + a.entry[1][i] * b.entry[1][j]
00193                       + a.entry[2][i] * b.entry[2][j];
00194   }
00195 
00196 ostream &operator<< (ostream &os, const rotmatrix &mat)
00197   {
00198   for (int i=0;i<3;++i)
00199     os << mat.entry[i][0] << ' '
00200        << mat.entry[i][1] << ' '
00201        << mat.entry[i][2] << endl;
00202   return os;
00203   }

Generated on Thu Oct 8 14:48:51 2015 for LevelS C++ support library