void SimilarityTransform :: computeMatrix()
{
m_matrix.create(3,3);
const double cost = cos(deg_to_rad(m_rotation));
const double sint = sin(deg_to_rad(m_rotation));
m_matrix(0,0) = m_scale * cost;
m_matrix(0,1) = -sint * m_scale;
m_matrix(0,2) = m_translation[0];
m_matrix(1,0) = sint * m_scale;
m_matrix(1,1) = m_scale*cost;
m_matrix(1,2) = m_translation[1];
m_matrix(2,0) = 0;
m_matrix(2,1) = 0;
m_matrix(2,2) = 1;
}
void ctMatrix4::Translate(QVector3D t_pos)
{
QMatrix4x4 tmp;
tmp.setToIdentity();
tmp(0, 3) = t_pos.x();
tmp(1, 3) = t_pos.y();
tmp(2, 3) = t_pos.z();
m_matrix.translate(t_pos.x(),t_pos.y(),t_pos.z());
//m_matrix = tmp * m_matrix;
qDebug() << m_matrix(0, 3);
}
MatrixFloodPlotData::MatrixFloodPlotData(const std::vector<double>& xVector,
const std::vector<double>& yVector,
const std::vector<double>& matrix)
: m_xVector(xVector.size()),
m_yVector(yVector.size()),
m_matrix(xVector.size(),yVector.size()),
m_interpMethod(NearestInterp)
{
std::copy(xVector.begin(),xVector.end(),m_xVector.begin());
std::copy(yVector.begin(),yVector.end(),m_yVector.begin());
for (unsigned j=0;j<yVector.size();j++)
for (unsigned i=0;i<xVector.size();i++)
m_matrix(i,j)=matrix[j*xVector.size()+i];
init();
}
Angle
RotationMatrix2<T>::GetAngle( ) const
{
return ArcTan( m_matrix( 1, 0 ), m_matrix( 0, 0 ) );
}
const T &
RotationMatrix2<T>::operator()( int row, int column ) const
{
return m_matrix( row, column );
}
/// Returns the coordinates at \p index.
Point3 Coordinates::position(int index) const
{
return Point3(m_matrix(index, 0),
m_matrix(index, 1),
m_matrix(index, 2));
}
const_array_ref<float> operator[](size_t j) const
{
return array_ref<float>(&m_matrix(0, j), m_matrix.rows());
}
EulerAngles
RotationMatrix3<T>::GetEulerAngles( EulerAngles::EOrder order )
{
Assert( order <= EulerAngles::YXZ );
static const int indices[6][3]
= { { 0, 1, 2 }, { 1, 2, 0 }, { 2, 0, 1 },
{ 0, 2, 1 }, { 2, 1, 0 }, { 1, 0, 2 } };
const int i0 = indices[ order ][ 0 ];
const int i1 = indices[ order ][ 1 ];
const int i2 = indices[ order ][ 2 ];
T c1 = std::sqrt( m_matrix( i0, i0 ) * m_matrix( i0, i0 )
+ m_matrix( i0, i1 ) * m_matrix( i0, i1 ) );
Angle angles[3];
if ( order <= EulerAngles::ZXY ) //even permutation
{
angles[1] = ArcTan( m_matrix( i0, i2 ), c1 );
if ( c1 > 16. * std::numeric_limits<T>::epsilon() )
{
angles[0] = ArcTan( - m_matrix( i1, i2 ), m_matrix( i2, i2 ) );
angles[2] = ArcTan( - m_matrix( i0, i1 ), m_matrix( i0, i0 ) );
}
else
{
if ( angles[1].Radians() > 0. )
//"gimble lock", only (angles[0] + angles[2]) determined
angles[0] = ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
else
//"gimble lock", only (angles[0] - angles[2]) determined
angles[0] = - ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
angles[2] = 0.;
}
}
else //odd permutation
{
angles[1] = ArcTan( - m_matrix( i0, i2 ), c1 );
if ( c1 > 16. * std::numeric_limits<T>::epsilon() )
{
angles[0] = ArcTan( m_matrix( i1, i2 ), m_matrix( i2, i2 ) );
angles[2] = ArcTan( m_matrix( i0, i1 ), m_matrix( i0, i0 ) );
}
else
{
if ( angles[1].Radians() > 0. )
//"gimble lock", only (angles[0] - angles[2]) determined
angles[0] = ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
else
//"gimble lock", only (angles[0] + angles[2]) determined
angles[0] = ArcTan( - m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
angles[2] = 0.;
}
}
return EulerAngles( angles );
}
AxisAngle<T>
RotationMatrix3<T>::GetAxisAngle( ) const
{
Angle angle = ArcCos( (Trace() - 1.) / 2. );
Vector3<T> axis;
if ( angle.Radians() <= 0. )
axis = Vector3<T>::UnitX; //arbitrary: there is no rotation.
else if ( angle.Radians() < M_PI )
{
axis.Set( m_matrix( 2, 1 ) - m_matrix( 1, 2 ),
m_matrix( 0, 2 ) - m_matrix( 2, 0 ),
m_matrix( 1, 0 ) - m_matrix( 0, 1 ) );
axis.Normalize( );
}
else
{
int i0 = 0;
int i1 = 1;
int i2 = 2;
T maxD = m_matrix( 0, 0 );
if ( m_matrix( 1, 1 ) > maxD )
{
i0 = 1;
i1 = 0;
maxD = m_matrix( 1, 1 );
}
if ( m_matrix( 2, 2 ) > maxD )
{
i0 = 2;
i1 = 0;
i2 = 1;
}
T axisComps[3];
axisComps[i0] = static_cast<T>( 0.5 ) * std::sqrt( m_matrix( i0, i0 )
- m_matrix( i1, i1 )
- m_matrix( i2, i2 )
+ static_cast<T>( 1. ) );
axisComps[i1] = m_matrix( i1, i0 )
/ (static_cast<T>( 2. ) * axisComps[i0]);
axisComps[i2] = m_matrix( i1, i0 )
/ (static_cast<T>( 2. ) * axisComps[i0]);
axis.Set( axisComps[0], axisComps[1], axisComps[2] );
}
return AxisAngle<T>( axis, angle );
}
