首页 > 代码库 > 利用RK4更新四元數並轉換成歐拉角及 OpenGL 的 Viewer
利用RK4更新四元數並轉換成歐拉角及 OpenGL 的 Viewer
開發平台 :
Odroid-U2
四元數更新 :
quat.c
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define M_PI 3.14159265358979323846
//#define M_PI 3.14
#define D2R(x) ((x) * M_PI/180)
#define R2D(x) ((x) * 180/M_PI)
#define NUM 4
void RK4(float X[NUM], float U[NUM], float dT);
void StateEq(float X[NUM], float U[NUM], float Xdot[NUM]);
/* run this program using the console pauser or add your own getch, system("pause") or input loop */
// gcc quat.c -o quat -lm -std=c99
int main(int argc, char *argv[]) {
float x[3] = {0};
float q[4] = {1,0,0,0};
float w[4] = {0,0,0,0};
//float r[4] = {0,0,0,0};
float m11, m12, m13, m23, m33;
float dt = 0.000001;
float norm;
printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));
// X
printf("---X\n");
w[1] = D2R(100), w[2] = 0, w[3] = 0;
for(int i=0; i < 10; i++) {
//r[0] = -w[1]*q[1] - w[2]*q[2] - w[3]*q[3];
//r[1] = +w[1]*q[0] - w[2]*q[3] + w[3]*q[2];
//r[2] = +w[1]*q[3] + w[2]*q[0] - w[3]*q[1];
//r[3] = -w[1]*q[2] + w[2]*q[1] + w[3]*q[0];
//q[0] = q[0] + (dt/2)* r[0];
//q[1] = q[1] + (dt/2)* r[1];
//q[2] = q[2] + (dt/2)* r[2];
//q[3] = q[3] + (dt/2)* r[3];
RK4(q,w,dt);
#if 1 // 不做的話會出現NAN
norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / norm;
q[1] = q[1] / norm;
q[2] = q[2] / norm;
q[3] = q[3] / norm;
#endif
m11 = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
m12 = 2.0 * (q[1]*q[2] + q[0]*q[3]);
m13 = 2.0 * (q[1]*q[3] - q[0]*q[2]);
m23 = 2.0 * (q[2]*q[3] + q[0]*q[1]);
m33 = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
x[0] = atanf(m23/m33);
x[1] = -asinf(m13);
x[2] = atanf(m12/m11);
printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));
}
// Y
printf("---Y\n");
w[1] = 0, w[2] = D2R(100), w[3] = 0;
for(int i=0; i < 10; i++) {
//r[0] = -w[1]*q[1] - w[2]*q[2] - w[3]*q[3];
//r[1] = +w[1]*q[0] - w[2]*q[3] + w[3]*q[2];
//r[2] = +w[1]*q[3] + w[2]*q[0] - w[3]*q[1];
//r[3] = -w[1]*q[2] + w[2]*q[1] + w[3]*q[0];
//q[0] = q[0] + (dt/2)* r[0];
//q[1] = q[1] + (dt/2)* r[1];
//q[2] = q[2] + (dt/2)* r[2];
//q[3] = q[3] + (dt/2)* r[3];
RK4(q,w,dt);
#if 1 // 不做的話會出現NAN
norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / norm;
q[1] = q[1] / norm;
q[2] = q[2] / norm;
q[3] = q[3] / norm;
#endif
m11 = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
m12 = 2.0 * (q[1]*q[2] + q[0]*q[3]);
m13 = 2.0 * (q[1]*q[3] - q[0]*q[2]);
m23 = 2.0 * (q[2]*q[3] + q[0]*q[1]);
m33 = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
x[0] = atanf(m23/m33);
x[1] = -asinf(m13);
x[2] = atanf(m12/m11);
printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));
}
// Z
printf("---Z\n");
w[1] = 0, w[2] = 0, w[3] = D2R(100);
for(int i=0; i < 10; i++) {
//r[0] = -w[1]*q[1] - w[2]*q[2] - w[3]*q[3];
//r[1] = +w[1]*q[0] - w[2]*q[3] + w[3]*q[2];
//r[2] = +w[1]*q[3] + w[2]*q[0] - w[3]*q[1];
//r[3] = -w[1]*q[2] + w[2]*q[1] + w[3]*q[0];
//q[0] = q[0] + (dt/2)* r[0];
//q[1] = q[1] + (dt/2)* r[1];
//q[2] = q[2] + (dt/2)* r[2];
//q[3] = q[3] + (dt/2)* r[3];
RK4(q,w,dt);
#if 1 // 不做的話會出現NAN
norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / norm;
q[1] = q[1] / norm;
q[2] = q[2] / norm;
q[3] = q[3] / norm;
#endif
m11 = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
m12 = 2.0 * (q[1]*q[2] + q[0]*q[3]);
m13 = 2.0 * (q[1]*q[3] - q[0]*q[2]);
m23 = 2.0 * (q[2]*q[3] + q[0]*q[1]);
m33 = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
x[0] = atanf(m23/m33);
x[1] = -asinf(m13);
x[2] = atanf(m12/m11);
printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));
}
return 0;
}
// ************* RungeKutta **********************
// Does a 4th order Runge Kutta numerical integration step
// Output, Xnew, is written over X
// NOTE the algorithm assumes time invariant state equations and
// constant inputs over integration step
// ************************************************
void RK4(float X[NUM], float U[NUM], float dT)
{
float dT2 = dT / 2, K1[NUM], K2[NUM], K3[NUM], K4[NUM], Xlast[NUM];
int i;
for (i = 0; i < NUM; i++)
Xlast[i] = X[i]; // make a working copy
StateEq(X, U, K1); // k1 = f(x,u)
for (i = 0; i < NUM; i++)
X[i] = Xlast[i] + dT2 * K1[i];
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
for (i = 0; i < NUM; i++)
X[i] = Xlast[i] + dT2 * K2[i];
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
for (i = 0; i < NUM; i++)
X[i] = Xlast[i] + dT * K3[i];
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i = 0; i < NUM; i++)
X[i] = Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] + K4[i]) / 6;
}
// *** StateEq ********
//
// State Variables = [Quaternion]
// Deterministic Inputs = [AngularVel]
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
//
// Notes:
// AngularVel in body frame
// Xdot is output of StateEq()
// ************************************************
void StateEq(float X[NUM], float U[NUM], float Xdot[NUM])
{
float wx, wy, wz, q0, q1, q2, q3;
wx = U[1];
wy = U[2];
wz = U[3];
q0 = X[0];
q1 = X[1];
q2 = X[2];
q3 = X[3];
// qdot = Q*w
Xdot[0] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
Xdot[1] = ( q0 * wx - q3 * wy + q2 * wz) / 2;
Xdot[2] = ( q3 * wx + q0 * wy - q1 * wz) / 2;
Xdot[3] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
}
Makefile
# # ‘make depend‘ uses makedepend to automatically generate dependencies # (dependencies are added to end of Makefile) # ‘make‘ build executable file ‘quat‘ # ‘make clean‘ removes all .o and executable files # # define the C compiler to use CC = gcc # define any compile-time flags CFLAGS = -Wall -g -std=c99 # define any directories containing header files other than /usr/include # INCLUDES = -Iinclude # define library paths in addition to /usr/lib # if I wanted to include libraries not in /usr/lib I‘d specify # their path using -Lpath, something like: LFLAGS = -Llib # define any libraries to link into executable: # if I want to link in libraries (libx.so or libx.a) I use the -llibname # option, something like (this will link in libmylib.so and libm.so: LIBS = -lm # define the C source files SRCS = quat.c # define the C object files # # This uses Suffix Replacement within a macro: # $(name:string1=string2) # For each word in ‘name‘ replace ‘string1‘ with ‘string2‘ # Below we are replacing the suffix .c of all words in the macro SRCS # with the .o suffix # OBJS = $(SRCS:.c=.o) # define the executable file TARGET = quat # # The following part of the makefile is generic; it can be used to # build any executable just by changing the definitions above and by # deleting dependencies appended to the file from ‘make depend‘ # .PHONY: depend clean all: $(TARGET) @echo Simple compiler named $(TARGET) has been compiled $(TARGET): $(OBJS) $(CC) $(CFLAGS) $(INCLUDES) -o $(TARGET) $(OBJS) $(LFLAGS) $(LIBS) # this is a suffix replacement rule for building .o‘s from .c‘s # it uses automatic variables $<: the name of the prerequisite of # the rule(a .c file) and $@: the name of the target of the rule (a .o file) # (see the gnu make manual section about automatic variables) .c.o: $(CC) $(CFLAGS) $(INCLUDES) -c $< -o $@ clean: $(RM) *.o *~ $(TARGET) depend: $(SRCS) makedepend $(INCLUDES) $^ # DO NOT DELETE THIS LINE -- make depend needs it
圖形介面 3D-Cube :
cube.c
/**** Traditional first 3D program: spinning cube
Written by Hugh Fisher ****/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <GL/glut.h>
#include "utility.h"
#include "glUtils.h"
#define Near 0.5
#define Far 20.0
#define ESC 27
#define cmdRed 1
#define cmdGreen 2
#define cmdBlue 3
#define cmdExit 99
static int WinWidth, WinHeight;
static RGBA CubeColor;
static int AppMenu;
static GLfloat Spin;
static GLfloat ViewPoint[3];
/**** Cube in points-polygons (polyhedron) form ****/
static GLfloat Verts[8][3] = {
{ -0.5, 0.5, -0.5 }, /* 0 left top rear */
{ 0.5, 0.5, -0.5 }, /* 1 right top rear */
{ 0.5, -0.5, -0.5 }, /* 2 right bottom rear */
{ -0.5, -0.5, -0.5 }, /* 3 left bottom rear */
{ -0.5, 0.5, 0.5 }, /* 4 left top front */
{ 0.5, 0.5, 0.5 }, /* 5 right top front */
{ 0.5, -0.5, 0.5 }, /* 6 right bottom front */
{ -0.5, -0.5, 0.5 } /* 7 left bottom front */
};
static GLuint Faces[] = {
4, 5, 6, 7, /* front */
5, 1, 2, 6, /* right */
0, 4, 7, 3, /* left */
4, 0, 1, 5, /* top */
7, 6, 2, 3, /* bottom */
1, 0, 3, 2 /* rear */
};
static void drawCube()
{
int i;
glPointSize(5.0f);
glLineWidth(5.0f);
/* Draw cube in traditional OpenGL style */
glBegin(GL_QUADS);
for (i = 0; i < 6 * 4; i++)
glVertex3fv(Verts[Faces[i]]);
glEnd();
}
#if 0
static void arrayCube()
{
/* Modern version using vertex arrays. Exactly the same effect
as above, but only 2 OpenGL calls instead of 26. */
/* Vertices are 3 floating point values each (XYZ), tightly
packed in array. Array size is not specified nor checked!
(Except by your program crashing if you get it wrong.) */
glVertexPointer(3, GL_FLOAT, 0, Verts);
/* Draw quads, using N vertices in total, in the order given
by an array of ints, from the vertex array specified earlier. */
glDrawElements(GL_QUADS, 6 * 4, GL_UNSIGNED_INT, Faces);
}
#endif
/**** Window events ****/
static void setProjection()
{
GLfloat aspect;
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
aspect = (float)WinWidth / (float)WinHeight;
gluPerspective(30.0, aspect, Near, Far); // 60.0
/* Back to normal */
glMatrixMode(GL_MODELVIEW);
}
static void setViewPoint()
{
glLoadIdentity();
gluLookAt(ViewPoint[0], ViewPoint[1], ViewPoint[2],
0.0, 0.0, 0.0,
0.0, 1.0, 0.0);
}
static void drawWorld()
{
glPushMatrix();
glRotatef(Spin, 1.0, 0.0, 0.0);
//glRotatef(Spin, 0.0, 1.0, 0.0);
//glRotatef(Spin, 0.0, 0.0, 1.0);
glScalef(0.5, 0.5, 0.5);
glColor3fv(CubeColor);
drawCube();
/* arrayCube() */
glPopMatrix();
}
static void display()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
setProjection();
setViewPoint();
drawWorld();
/* Check everything OK and update screen */
CheckGL();
glutSwapBuffers();
}
static void resize(int width, int height)
{
/* Save for event handlers */
WinWidth = width;
WinHeight = height;
/* Reset view in window. */
glViewport(0, 0, WinWidth, WinHeight);
}
/**** User events ****/
static void menuChoice(int item)
{
switch (item) {
case cmdRed:
SetColor(CubeColor, 1, 0, 0);
break;
case cmdGreen:
SetColor(CubeColor, 0, 1, 0);
break;
case cmdBlue:
SetColor(CubeColor, 0, 0, 1);
break;
case cmdExit:
exit(0);
break;
default:
break;
}
}
/* In most GUI systems we would write just one event handler
for all kinds of keystrokes. In GLUT, we need one for the
standard ASCII keys and one for special cursor or function
style keys that vary from system to system. Because the
GLUT special key code range overlaps with ASCII lowercase,
it isn‘t safe to use the same function for both. */
static void asciiKey(unsigned char key, int x, int y)
{
if (key == ESC)
menuChoice(cmdExit);
}
static void specialKey(int key, int x, int y)
{
/* Nothing yet */
}
/**** Startup ****/
static void initGraphics(void)
{
/* Black background */
glClearColor(0, 0, 0, 0);
/* Wireframe mode */
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
/* Needed for vertex arrays */
glEnableClientState(GL_VERTEX_ARRAY);
/* Popup menu attached to right mouse button */
AppMenu = glutCreateMenu(menuChoice);
glutSetMenu(AppMenu);
glutAddMenuEntry("R", cmdRed);
glutAddMenuEntry("G", cmdGreen);
glutAddMenuEntry("B", cmdBlue);
glutAddMenuEntry("----", 0);
glutAddMenuEntry("Exit", cmdExit);
glutAttachMenu(GLUT_RIGHT_BUTTON);
/* Start values */
Spin = 0.0;
ViewPoint[0] = 0.0;
ViewPoint[1] = 0.5;
ViewPoint[2] = 2.0;
menuChoice(cmdGreen);
}
/**** Main control ****/
static void spinDisplay(void)
{
Spin += 1;
if (Spin >= 360.0)
Spin -= 360.0;
glutPostRedisplay();
}
int main(int argc, char * argv[])
{
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGB);
glutInitWindowSize(500, 500);
glutInitWindowPosition(100, 75);
glutCreateWindow("Cube (CCTSAO1008)");
initGraphics();
glutDisplayFunc(display);
glutReshapeFunc(resize);
glutKeyboardFunc(asciiKey);
glutSpecialFunc(specialKey);
glutIdleFunc(spinDisplay);
glutMainLoop();
/* Should never get here, but keeps compiler happy */
return 0;
}
Makefile
## Linux CC = gcc CFLAGS = -I../util -DGL_GLEXT_PROTOTYPES -Wall LDFLAGS = # Older systems might need -lXi -lXmu GLIBS = -lglut -lGLU -lGL -lX11 -lm OBJS = ../util/utility.o ../util/glUtils.o TARGET = cube $(TARGET): $(TARGET).c $(OBJS) /bin/rm -f $@ $(CC) $(CFLAGS) -o $@ $(TARGET).c $(OBJS) $(LDFLAGS) $(GLIBS) clean: /bin/rm -f *.o $(TARGET) cube.o: cube.c
參考資料 :
1. OpenGL SuperBible http://www.openglsuperbible.com
2. 四元數更新 https://qcopter.googlecode.com/files/推導_四元數.pdf
利用RK4更新四元數並轉換成歐拉角及 OpenGL 的 Viewer
声明：以上内容来自用户投稿及互联网公开渠道收集整理发布，本网站不拥有所有权，未作人工编辑处理，也不承担相关法律责任，若内容有误或涉及侵权可进行投诉： 投诉/举报 工作人员会在5个工作日内联系你，一经查实，本站将立刻删除涉嫌侵权内容。