diff options
Diffstat (limited to 'src/plugins/sensors/blackberry/bbutil.cpp')
-rw-r--r-- | src/plugins/sensors/blackberry/bbutil.cpp | 82 |
1 files changed, 82 insertions, 0 deletions
diff --git a/src/plugins/sensors/blackberry/bbutil.cpp b/src/plugins/sensors/blackberry/bbutil.cpp new file mode 100644 index 00000000..c01c6303 --- /dev/null +++ b/src/plugins/sensors/blackberry/bbutil.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** +** +** Copyright (C) 2012 Research In Motion +** Contact: http://www.qt-project.org/ +** +** This file is part of the QtSensors module of the Qt Toolkit. +** +** $QT_BEGIN_LICENSE:LGPL$ +** GNU Lesser General Public License Usage +** This file may be used under the terms of the GNU Lesser General Public +** License version 2.1 as published by the Free Software Foundation and +** appearing in the file LICENSE.LGPL included in the packaging of this +** file. Please review the following information to ensure the GNU Lesser +** General Public License version 2.1 requirements will be met: +** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html. +** +** In addition, as a special exception, Nokia gives you certain additional +** rights. These rights are described in the Nokia Qt LGPL Exception +** version 1.1, included in the file LGPL_EXCEPTION.txt in this package. +** +** GNU General Public License Usage +** Alternatively, this file may be used under the terms of the GNU General +** Public License version 3.0 as published by the Free Software Foundation +** and appearing in the file LICENSE.GPL included in the packaging of this +** file. Please review the following information to ensure the GNU General +** Public License version 3.0 requirements will be met: +** http://www.gnu.org/copyleft/gpl.html. +** +** Other Usage +** Alternatively, this file may be used in accordance with the terms and +** conditions contained in a signed written agreement between you and Nokia. +** +** +** +** +** +** +** $QT_END_LICENSE$ +** +****************************************************************************/ +#include "bbutil.h" + +#include <QtCore/qmath.h> + +namespace BbUtil { + +static float getMatrixElement(const float matrix[3*3], int index0, int index1) +{ + return matrix[index0 * 3 + index1]; +} + +void matrixToEulerZXY(const float matrix[3*3], + float &thetaX, float &thetaY, float& thetaZ) +{ + thetaX = asin( getMatrixElement(matrix, 2, 1)); + if ( thetaX < M_PI_2 ) { + if ( thetaX > -M_PI_2 ) { + thetaZ = atan2( -getMatrixElement(matrix, 0, 1), + getMatrixElement(matrix, 1, 1) ); + thetaY = atan2( -getMatrixElement(matrix, 2, 0), + getMatrixElement(matrix, 2, 2) ); + } else { + // Not a unique solution + thetaZ = -atan2( getMatrixElement(matrix, 0, 2), + getMatrixElement(matrix, 0, 0) ); + thetaY = 0.0; + } + } else { + // Not a unique solution + thetaZ = atan2( getMatrixElement(matrix, 0, 2), + getMatrixElement(matrix, 0, 0) ); + thetaY = 0.0; + } +} + +qreal radiansToDegrees(qreal radians) +{ + static const qreal radToDeg = 180.0f / M_PI; + return radians * radToDeg; +} + +} |