CSUtils::ENUPoint CSUtils::ecef_to_enu(const CSUtils::ECEFPoint& _point, const CSUtils::GCSPoint& _origin)
{
double phi = _origin.latitude;
double lamda = _origin.longitude;
double h = _origin.altitude;
Eigen::Vector3d lccs_center_point;
lccs_center_point(0) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * cos(lamda);
lccs_center_point(1) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * sin(lamda);
lccs_center_point(2) = b * sin(atan(b * tan(phi) / a)) + h * sin(phi);
Eigen::Matrix3d C_1 = Eigen::Matrix3d::Zero();
C_1(0, 2) = -1.0; C_1(1, 1) = 1.0; C_1(2, 0) = 1.0;
Eigen::Matrix3d C_2 = Eigen::Matrix3d::Zero();
C_2(0, 0) = 1.0;
C_2(1, 1) = cos(phi); C_2(1, 2) = -sin(phi);
C_2(2, 1) = sin(phi); C_2(2, 2) = cos(phi);
Eigen::Matrix3d C_3 = Eigen::Matrix3d::Zero();
C_3(0, 0) = sin(lamda); C_3(0, 1) = cos(lamda);
C_3(1, 0) = -cos(lamda); C_3(1, 1) = sin(lamda);
C_3(2, 2) = 1.0;
Eigen::Matrix3d C = C_3 * C_2 * C_1;
C.transposeInPlace();
Eigen::Vector3d result = C * Eigen::Vector3d(_point.x, _point.y, _point.z) - C * lccs_center_point;
return CSUtils::ENUPoint(result(0), result(1), result(2));
}
Eigen::Matrix3d createRotationMatrix(double x, double y, double z, double w) {
KDL::Rotation kdl_rotaion = KDL::Rotation::Quaternion(x, y, z, w);
Eigen::Matrix3d result;
for(int i = 0; i < 9; i++) {
result.data()[i] = kdl_rotaion.data[i];
}
result.transposeInPlace();
return result;
}
Eigen::Matrix3d CEMatrixEditor::validateEditor()
{
// Editing fractional matrix is not allowed. The widget is
// disabled to ensure that this assertion passes.
Q_ASSERT(m_ext->matrixCartFrac() != Fractional);
// Clear selection, otherwise there is a crash on Qt 4.7.2.
QTextCursor tc = ui.edit_matrix->textCursor();
tc.clearSelection();
ui.edit_matrix->setTextCursor(tc);
QString text = ui.edit_matrix->document()->toPlainText();
QStringList lines = text.split("\n",
QString::SkipEmptyParts);
if (lines.size() != 3) {
emit invalidInput();
return Eigen::Matrix3d::Zero();
}
QList<QStringList> stringVecs;
Eigen::Matrix3d mat;
for (int row = 0; row < 3; ++row) {
stringVecs.append(lines.at(row).simplified()
.split(CE_PARSE_IGNORE_REGEXP,
QString::SkipEmptyParts));
QStringList &stringVec = stringVecs[row];
if (stringVec.size() != 3) {
emit invalidInput();
return Eigen::Matrix3d::Zero();
}
for (int col = 0; col < 3; ++col) {
bool ok;
double val = stringVec[col].toDouble(&ok);
if (!ok) {
emit invalidInput();
return Eigen::Matrix3d::Zero();
}
mat(row,col) = val;
}
}
// Transpose if needed
if (m_ext->matrixVectorStyle() == ColumnVectors) {
// Warning: mat = mat.transpose() will *not* work correctly with
// Eigen matrices. Use transposeInPlace() instead.
mat.transposeInPlace();
}
emit validInput();
return mat;
}
void Reader::draw_label( QGLPainter *painter, const QVector3D& position, const QColor4ub& color, const std::string& label )
{
if( label.empty() ) { return; }
painter->modelViewMatrix().push();
painter->modelViewMatrix().translate( position - m_offset ); // painter->modelViewMatrix().translate( position );
QMatrix4x4 world = painter->modelViewMatrix().top();
Eigen::Matrix3d R;
R << world( 0, 0 ) , world( 0, 1 ), world( 0, 2 ),
world( 1, 0 ) , world( 1, 1 ), world( 1, 2 ),
world( 2, 0 ) , world( 2, 1 ), world( 2, 2 );
R.transposeInPlace();
snark::rotation_matrix rotation( R );
Eigen::Quaterniond q = rotation.quaternion();
painter->modelViewMatrix().rotate( QQuaternion( q.w(), q.x(), q.y(), q.z() ) );
//painter->modelViewMatrix().translate( m_offset );
double scale = 1.0 / double( m_viewer.height() );
scale *= m_viewer.camera()->projectionType() == QGLCamera::Orthographic
? ( 0.25 * m_viewer.camera()->viewSize().width() )
: ( 0.2 * Eigen::Vector3d( world( 0, 3 ) , world( 1, 3 ), world( 2, 3 ) ).norm() );
painter->modelViewMatrix().scale( scale ); // TODO make size configurable ?
drawText( painter, &label[0], color );
painter->modelViewMatrix().pop();
}
