码迷,mamicode.com
首页 > 其他好文 > 详细

利用RK4更新四元數並轉換成歐拉角及 OpenGL 的 Viewer

时间:2014-11-28 12:47:13      阅读:311      评论:0      收藏:0      [点我收藏+]

标签:des   style   http   io   ar   color   os   sp   for   

開發平台 :

Odroid-U2

bubuko.com,布布扣

四元數更新 :

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 :

bubuko.com,布布扣

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

标签:des   style   http   io   ar   color   os   sp   for   

原文地址:http://my.oschina.net/u/1170736/blog/349874

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!