C++ QQuaternion函数代码示例

AHRS::AHRS()
{
	foundZero = false;
	zeroSamples = 0;
	zx = zy = zz = 0.0f;

	quat = QQuaternion(1.0f, 0.0f, 0.0f, 0.0f);
	err[0] = err[1] = err[2] = 0.0f;
}

void GLWidget::mouseReleaseEvent(QMouseEvent *e){
    if ( m_mouseClick){
        float x = 2.0 * (float(e->x()) / w) - 1.0;
        float y = 1.0 - 2.0 * (float(e->y()) / h);
        QPointF p(x, y);
        m_trackBalls.release( p, QQuaternion());
        m_mouseClick = 0;
    }
}

void OpenGLWidget::mouseDoubleClickEvent(QMouseEvent *)
{
    m_angularSpeed = 0;
    m_rotation = QQuaternion();

    resetCamera();

    updateGL();
}

QQuaternion M4toQuat(QMatrix4x4 mat) {
	double trace = mat(0,0) + mat(1,1) + mat(2,2) + 1;
	double s = 0.5 / sqrt(trace);
	double w = 0.25 / s;
	double x = (mat(2, 1) - mat(1, 2)) * s;
	double y = (mat(0, 2) - mat(2, 0)) * s;
	double z = (mat(1, 0) - mat(0, 1)) * s;
	return QQuaternion(w, x, y, z);
}

QVRRenderContext::QVRRenderContext() :
    _processIndex(-1),
    _windowIndex(-1),
    _windowGeometry(),
    _screenGeometry(),
    _navigationPosition(0.0f, 0.0f, 0.0f),
    _navigationOrientation(0.0f, 0.0f, 0.0f, 0.0f),
    _screenWall { QVector3D(), QVector3D(), QVector3D() },
    _outputMode(QVR_Output_Center),
    _viewPasses(0),
    _eye { QVR_Eye_Center, QVR_Eye_Center },
    _trackingPosition { QVector3D(0.0f, 0.0f, 0.0f), QVector3D(0.0f, 0.0f, 0.0f) },
    _trackingOrientation { QQuaternion(0.0f, 0.0f, 0.0f, 0.0f), QQuaternion(0.0f, 0.0f, 0.0f, 0.0f) },
    _frustum { { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f } },
    _viewMatrix { QMatrix4x4(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f),
                  QMatrix4x4(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f) }
{
}

void MainWidget::mouseReleaseEvent(QMouseEvent *event)
{

    if(event->button() == Qt::LeftButton)
    {
        this->trackball.release(pixelPosToViewPos(event->pos()),QQuaternion());
        event->accept();
    }

}

TrackBall::TrackBall(TrackMode mode)
    : m_angularVelocity(0)
    , m_paused(false)
    , m_pressed(false)
    , m_mode(mode)
{
    m_axis = QVector3D(0, 1, 0);
    m_rotation = QQuaternion();
    m_lastTime = QTime::currentTime();
}

TrackBall::TrackBall(float angularVelocity, const QVector3D& axis, TrackMode mode)
    : m_axis(axis)
    , m_angularVelocity(angularVelocity)
    , m_paused(false)
    , m_pressed(false)
    , m_mode(mode)
{
    m_rotation = QQuaternion();
    m_lastTime = QTime::currentTime();
}

Camera::Camera()
{
	quaternion = QQuaternion(1.0, 0.0, 0.0, 0.0);	
	target = QVector3D(0.0, 0.0, 0.0);
	xAxis = QVector3D(1.0, 0.0, 0.0);
	yAxis = QVector3D(0.0, 1.0, 0.0);
	zAxis = QVector3D(0.0, 0.0, 1.0);
	zoomOffset = 0.0;
	zoomSpeed = 0.005;
	rotationSpeed = 0.3;	
	viewMatrix.loadIdentity();		
}

/*!
    Creates a normalized quaternion that corresponds to rotating through
    \a angle degrees about the specified 3D \a axis.
*/
QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D& axis, qreal angle)
{
    // Algorithm from:
    // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
    // We normalize the result just in case the values are close
    // to zero, as suggested in the above FAQ.
    qreal a = (angle / 2.0f) * M_PI / 180.0f;
    qreal s = qSin(a);
    qreal c = qCos(a);
    QVector3D ax = axis.normalized();
    return QQuaternion(c, ax.x() * s, ax.y() * s, ax.z() * s).normalized();
}

// Test getting and setting quaternion properties via the metaobject system.
void tst_QQuaternion::properties()
{
    tst_QQuaternionProperties obj;

    obj.setQuaternion(QQuaternion(6.0f, 7.0f, 8.0f, 9.0f));

    QQuaternion q = qVariantValue<QQuaternion>(obj.property("quaternion"));
    QCOMPARE(q.scalar(), (qreal)6.0f);
    QCOMPARE(q.x(), (qreal)7.0f);
    QCOMPARE(q.y(), (qreal)8.0f);
    QCOMPARE(q.z(), (qreal)9.0f);

    obj.setProperty("quaternion",
                    qVariantFromValue(QQuaternion(-6.0f, -7.0f, -8.0f, -9.0f)));

    q = qVariantValue<QQuaternion>(obj.property("quaternion"));
    QCOMPARE(q.scalar(), (qreal)-6.0f);
    QCOMPARE(q.x(), (qreal)-7.0f);
    QCOMPARE(q.y(), (qreal)-8.0f);
    QCOMPARE(q.z(), (qreal)-9.0f);
}

QQuaternion GLC_Matrix4x4::quaternion() const
{
	QQuaternion subject;
	GLC_Matrix4x4 rotMat= rotationMatrix();
	if ((this->type() != GLC_Matrix4x4::Identity) && (rotMat != GLC_Matrix4x4()))
	{
		const double matrixTrace= rotMat.trace();
		double s, w, x, y, z;

		if (matrixTrace > 0.0)
		{
			s= 0.5 / sqrt(matrixTrace);
			w= 0.25 / s;
			x= (rotMat.m_Matrix[9] - rotMat.m_Matrix[6]) * s;
			y= (rotMat.m_Matrix[2] - rotMat.m_Matrix[8]) * s;
			z= (rotMat.m_Matrix[4] - rotMat.m_Matrix[1]) * s;
		}
		else
		{
			if ((abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[5])) &&  (abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[15])))
			{	// column 0 greater
		        s= sqrt(1.0 + rotMat.m_Matrix[0] - rotMat.m_Matrix[5] - rotMat.m_Matrix[10]) * 2.0;

		        w= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9] ) / s;
		        x= 0.5 / s;
		        y= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4] ) / s;
		        z= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8] ) / s;
			}
			else if ((abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[0])) &&  (abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[15])))
			{	// column 1 greater
		        s= sqrt(1.0 + rotMat.m_Matrix[5] - rotMat.m_Matrix[0] - rotMat.m_Matrix[10]) * 2.0;

		        w= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
		        x= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
		        y= 0.5 / s;
		        z= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
			}
			else
			{	// column 3 greater
		        s= sqrt(1.0 + rotMat.m_Matrix[10] - rotMat.m_Matrix[0] - rotMat.m_Matrix[5]) * 2.0;

		        w = (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
		        x = (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
		        y = (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
		        z = 0.5 / s;
			}
		}
		subject= QQuaternion(w, x, y, z);
	}

	return subject;
}

void OpenGLWindow::initialize()
{
    m_program = new QOpenGLShaderProgram(this);
    // see declaration of sources on top
    m_program->addShaderFromSourceCode(QOpenGLShader::Vertex, vertexShaderSource);
    m_program->addShaderFromSourceCode(QOpenGLShader::Fragment, fragmentShaderSource);
    m_program->link();
    m_posAttr = m_program->attributeLocation("posAttr");
    m_colAttr = m_program->attributeLocation("colAttr");
    m_matrixUniform = m_program->uniformLocation("matrix");
    qrotation = QQuaternion(1, 0, 0, 0);
    rotationMode = 0; // 0 = simple rotation, 1 = trackball
}

void EditCameraDialog::setCamRot(Dim axis){
    switch (axis) {
    case X:
        parent->setCameraRot(QQuaternion::fromAxisAndAngle(0, 1, 0, 90));
        break;
    case Y:
        parent->setCameraRot(QQuaternion::fromAxisAndAngle(1, 0, 0, 90));
        break;
    case Z:
        parent->setCameraRot(QQuaternion());
        break;
    }
}

void  OpenGLWindow::mouseMoveEvent(QMouseEvent* event) {
    /*Change Model matrix according to mouse movement when left mouse button is pressed*/
    if (event->buttons() & Qt::LeftButton) {
        // check if position values are valid
        if (oldMousePosition.x() != -1 && oldMousePosition.y() != -1) {
            // quaterion q is used to store additional rotation
            QQuaternion q;
            if (rotationMode == 0) // simple rotation
            {
                //difference of new and old mouse position
                float xDiff = oldMousePosition.x() - event->x();
                float yDiff = oldMousePosition.y() - event->y();

                q = q.fromEulerAngles(QVector3D(yDiff, -xDiff, 0));
                //q2 = q2.fromEulerAngles(QVector3D(0, 0, -yDiff));
            }
            else if (rotationMode == 1) //trackball
            {
                // define center of window
                float centerX = float(this->width() / 2);
                float centerY = float(this->height() / 2);

                //difference of new and old mouse position
                float oldX = (oldMousePosition.x() - centerX) / centerX;
                float oldY = (oldMousePosition.y() - centerY) / centerY;
                float newX = (event->x() - centerX) / centerX;
                float newY = (event->y() - centerY) / centerY;

                //model.rotate(2, -xDiff, 0, -yDiff);
                float rotation[4];
                // get rotation as quaterion from trackball function
                gfs_gl_trackball(rotation, newX, newY, oldX, oldY);
                q = QQuaternion(QVector4D(rotation[0], rotation[1], rotation[2], rotation[3]));
            }
            // combine new rotation and current rotation
            qrotation = q  * qrotation;
            QMatrix4x4 m;
            m.rotate(qrotation);
            model.setToIdentity();
            // translate to center for rotation
            model.translate(center);
            model = model * m;
            //move back to former position
            model.translate(-center);
        }
        // set current mouse position as old
        oldMousePosition.setX(event->x());
        oldMousePosition.setY(event->y());
    }
}