diff options
Diffstat (limited to 'tools/plot_lighthouse/glutil.c')
-rw-r--r-- | tools/plot_lighthouse/glutil.c | 75 |
1 files changed, 75 insertions, 0 deletions
diff --git a/tools/plot_lighthouse/glutil.c b/tools/plot_lighthouse/glutil.c new file mode 100644 index 0000000..dd022a0 --- /dev/null +++ b/tools/plot_lighthouse/glutil.c @@ -0,0 +1,75 @@ +// +// glutil.c +// +// +// Created by user on 2/4/17. +// +// + +#include "glutil.h" + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ) +{ + float x,y,z; + + glBegin(GL_LINES); + + // X grid stripes + for (y=minY; y<maxY; y+=stepY) { + for (z=minZ; z<maxZ; z+=stepZ) { + glVertex3f(minX, y, z); + glVertex3f(maxX, y, z); + } + } + + // Y grid stripes + for (x=minX; x<maxX; x+=stepX) { + for (z=minZ; z<maxZ; z+=stepZ) { + glVertex3f(x, minY, z); + glVertex3f(x, maxY, z); + } + } + + // Z grid stripes + for (y=minY; y<maxY; y+=stepY) { + for (x=minX; x<maxX; x+=stepX) { + glVertex3f(x, y, minZ); + glVertex3f(x, y, maxZ); + } + } + + glEnd(); +} + + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr) +{ + Quaternion i0,j0,k0; + Quaternion i, j, k; + Quaternion q; + + // Calculate the i, j, and k vectors + QuaternionSet(i0, 1, 0, 0, 0); + QuaternionSet(j0, 0, 1, 0, 0); + QuaternionSet(k0, 0, 0, 1, 0); + QuaternionSet(q, qx, qy, qz, qr); + QuaternionRot(i, q, i0); + QuaternionRot(j, q, j0); + QuaternionRot(k, q, k0); + + // Draw the coordinate system i red, j green, k blue + glBegin(GL_LINES); + glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i.i,z+i.k,y+i.j); + glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j.i,z+j.k,y+j.j); + glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k.i,z+k.k,y+k.j); + glEnd(); +} + + + |