rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

Ground/ModelView: Fixed attitude rotation of the 3D model.

Browse Source 
Euler rotation has problems with Yaw, so now it's using quaternion rotation.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2064 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
muralha 2010-11-03 22:20:45 +00:00 committed by muralha
parent 48a3b6c569
commit 7a8484edfb
1 changed files with 31 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
ground/src/plugins/modelview/modelviewgadgetwidget.cpp
View File

@ -289,17 +289,37 @@ void ModelViewGadgetWidget::keyPressEvent(QKeyEvent * e) // switch between camer
//////////////////////////////////////////////////////////////////////
void ModelViewGadgetWidget::updateAttitude()
{
AttitudeActual::DataFields data = attActual->getData();
GLC_StructOccurence* rootObject= m_World.rootOccurence();
GLC_Matrix4x4 rootObjectPosition(0, 0, 0); // will be changed when the acceleration movement is tested
double pitch= glc::toRadian(-data.Pitch);
double roll= glc::toRadian(data.Roll);
double yaw= glc::toRadian(-data.Yaw);
GLC_Matrix4x4 rootObjectRotation(GLC_Matrix4x4().fromEuler(pitch, roll, yaw));
rootObject->structInstance()->setMatrix(rootObjectPosition * rootObjectRotation);
AttitudeActual::DataFields data = attActual->getData(); // get attitude data
GLC_StructOccurence* rootObject= m_World.rootOccurence(); // get the full 3D model
// create quaternions rotations for each axis
QQuaternion qX= QQuaternion::fromAxisAndAngle(QVector3D(1,0,0),data.Pitch);
QQuaternion qY= QQuaternion::fromAxisAndAngle(QVector3D(0,1,0),data.Roll);
QQuaternion qZ= QQuaternion::fromAxisAndAngle(QVector3D(0,0,1),data.Yaw);
QQuaternion quat= qX * qY * qZ; // create the quaternion of all the rotations
// pass values to simpler variables
double x= quat.vector().x();
double y= quat.vector().y();
double z= quat.vector().z();
double w= quat.scalar();
// create and gives the product of 2 4x4 matrices to get the rotation of the 3D model's matrix
QMatrix4x4 m1;
m1.setRow(0, QVector4D(w,z,-y,x));
m1.setRow(1, QVector4D(-z,w,x,y));
m1.setRow(2, QVector4D(y,-x,w,z));
m1.setRow(3, QVector4D(-x,-y,-z,w));
QMatrix4x4 m2;
m2.setRow(0, QVector4D(w,z,-y,-x));
m2.setRow(1, QVector4D(-z,w,x,-y));
m2.setRow(2, QVector4D(y,-x,w,-z));
m2.setRow(3, QVector4D(x,y,z,w));
QMatrix4x4 m0= m1 * m2;
// convert QMatrix4x4 to GLC_Matrix4x4
GLC_Matrix4x4 rootObjectRotation;
for (int i=0; i<16; i++){
rootObjectRotation.data()[i]= m0.data()[i];
}
// sets and updates the 3D model's matrix
rootObject->structInstance()->setMatrix(rootObjectRotation);
rootObject->updateChildrenAbsoluteMatrix();
updateGL();
}