rotmatrix.cc
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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)
00082 {
00083 axis = vec3(1,0,0);
00084 angle = 0;
00085 return;
00086 }
00087
00088 angle = pi;
00089
00090 int choice = 0;
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 }