00001 #ifndef DSRPDB_INTERNAL_TRANSFORM_H
00002 #define DSRPDB_INTERNAL_TRANSFORM_H
00003 #include <cmath>
00004
00005 namespace dsrpdb {
00007
00011 struct Transform {
00012 Transform(){
00013 for (unsigned int i=0; i< 3; ++i){
00014 rot_[i][0]=0;
00015 rot_[i][1]=0;
00016 rot_[i][2]=0;
00017 rot_[i][i]=1;
00018 trans_[i]=0;
00019 }
00020 }
00021
00023 template <class TA, class TB>
00024 Transform(TA rot, TB trans){
00025 for (unsigned int i=0; i< 3; ++i){
00026 trans_[i]=trans[i];
00027 for (unsigned int j=0; j< 3; ++j){
00028 rot_[i][j]=rot[i][j];
00029 }
00030 }
00031
00032 #ifndef NDEBUG
00033 double m01 = rot[0][0]*rot[1][1] - rot[1][0]*rot[0][1];
00034 double m02 = rot[0][0]*rot[2][1] - rot[2][0]*rot[0][1];
00035 double m12 = rot[1][0]*rot[2][1] - rot[2][0]*rot[1][1];
00036 double det = m01*rot[2][2] - m02*rot[1][2] + m12*rot[0][2];
00037 assert(det >0);
00038 assert(std::abs(1-det) < .25);
00039 #endif
00040 }
00041
00043 template <class Point>
00044 Point operator()(const Point &pt) const {
00045 double ix= pt.x();
00046 double iy= pt.y();
00047 double iz= pt.z();
00048 double x= ix*rot_[0][0] + iy*rot_[0][1] + iz*rot_[0][2] + trans_[0];
00049 double y= ix*rot_[1][0] + iy*rot_[1][1] + iz*rot_[1][2] + trans_[1];
00050 double z= ix*rot_[2][0] + iy*rot_[2][1] + iz*rot_[2][2] + trans_[2];
00051 return Point(x,y,z);
00052 }
00053
00054
00055
00057 template <class Pt>
00058 void set_translation(Pt tr){
00059 trans_[0]=tr.x();
00060 trans_[1]=tr.y();
00061 trans_[2]=tr.z();
00062 }
00063
00064 void write(std::ostream &out) const {
00065 for (unsigned int i=0; i< 3; ++i){
00066 out << rot_[i][0] << "\t" << rot_[i][1] << "\t" << rot_[i][2] << "\t" << trans_[i] << std::endl;
00067 }
00068 out << 0 << "\t" << 0 << "\t" << 0 << "\t" << 1 << std::endl;
00069
00070 }
00071
00072 double error(const Transform &o) const {
00073 double n=0;
00074 for (unsigned int i=0; i< 3; ++i){
00075 n += std::abs(o.trans_[i]- trans_[i]);
00076 for (unsigned int j=0; j< 3; ++j){
00077 n+= std::abs(o.rot_[i][j]- rot_[i][j]);
00078 }
00079 }
00080 return n;
00081 }
00082
00083 private:
00084 double rot_[3][3];
00085 double trans_[3];
00086 };
00087
00089
00096 inline std::ostream &operator<<(std::ostream &out, const Transform &t) {
00097 t.write(out);
00098 return out;
00099 }
00100
00101 };
00102
00103 #endif