14 #ifndef _OM_TRANSFORMATION_SOLVER_HXX_
15 #define _OM_TRANSFORMATION_SOLVER_HXX_
27 template <
class Vector>
31 m_matrixLength = matrixLength;
32 m_resolution = resolution;
33 m_pixelSize = m_resolution/m_matrixLength;
34 ScalarT halfSize = m_resolution/2-0.5;
37 m_modelTo2D = Eigen::Translation<ScalarT, 3>(-origin[0], -origin[1], -origin[2]);
45 m_modelTo2D = AngleAxis<ScalarT>(-angle, VectorEigen::UnitZ()) * m_modelTo2D;
48 aux = transformTo2DLinear(normal);
51 angle = vectorSignedAngle(
VectorT(0.0, 0.0, 1.0), aux,
VectorT(0.0, 1.0, 0.0));
52 m_modelTo2D = AngleAxis<ScalarT>(-angle, VectorEigen::UnitY()) * m_modelTo2D;
55 aux = transformTo2DLinear(direction);
57 angle = vectorSignedAngle(
VectorT(1.0, 0.0, 0.0), aux,
VectorT(0.0, 0.0, 1.0));
58 m_modelTo2D = AngleAxis<ScalarT>(-angle, VectorEigen::UnitZ()) * m_modelTo2D;
61 m_modelTo2D = Scaling<ScalarT, 3>(m_pixelSize, m_pixelSize, m_pixelSize)* m_modelTo2D;
64 m_modelTo2D = Translation<ScalarT, 3>(halfSize, halfSize, 0.0) *m_modelTo2D;
67 m_2DToModel = m_modelTo2D.inverse();
75 template <
class Vector>
80 ScalarT *matrixData = m_modelTo2D.data();
81 aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8] + matrixData[12];
82 aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9] + matrixData[13];
83 aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10] + matrixData[14];
92 template <
class Vector>
97 ScalarT *matrixData = m_2DToModel.data();
98 aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8] + matrixData[12];
99 aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9] + matrixData[13];
100 aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10] + matrixData[14];
109 template <
class Vector>
114 ScalarT *matrixData = m_modelTo2D.data();
115 aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8];
116 aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9];
117 aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10];
126 template <
class Vector>
131 ScalarT *matrixData = m_2DToModel.data();
132 aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8];
133 aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9];
134 aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10];
145 template <
class Vector>
149 ScalarT vec1Length = vec1.norm();
150 ScalarT vec2Length = vec2.norm();
153 if ( (vec1Length != 0.0) && (vec2Length != 0.0) )
160 ScalarT sign = ((normal|reference) < 0) ? -1 : 1;
162 return acos( (vec1|vec2) / (vec1Length * vec2Length) ) * sign;
174 template <
class Vector>
178 ScalarT vec1Length = vec1.norm();
179 ScalarT vec2Length = vec2.norm();
182 if ( (vec1Length != 0.0) && (vec2Length != 0.0) )
183 return acos( (vec1|vec2) / (vec1Length * vec2Length) );
192 template <
class Vector>
195 return VectorT(m_resolution-1, m_resolution-1, 0.0);
201 template <
class Vector>