MotionCal/visualize.c

#include "imuread.h"

magdata_t caldata[MAGBUFFSIZE];
magdata_t hard_iron = {0.0, 0.0, 0.0, 1};
float soft_iron[9] = {1.0, 0.0, 0.0,  0.0, 1.0, 0.0,  0.0, 0.0, 1.0};
quat_t current_orientation;



static void apply_calibration(const magdata_t *in, magdata_t *out)
{
	float x, y, z;

	x = in->x - hard_iron.x;
	y = in->y - hard_iron.y;
	z = in->z - hard_iron.z;
	out->x = x * soft_iron[0] + y * soft_iron[1] + z * soft_iron[2];
	out->y = x * soft_iron[3] + y * soft_iron[4] + z * soft_iron[5];
	out->z = x * soft_iron[6] + y * soft_iron[7] + z * soft_iron[8];
	out->valid = 1;
}

static void quad_to_rotation(const quat_t *quat, float *rmatrix)
{
	float qx = quat->x;
	float qy = quat->y;
	float qz = quat->z;
	float qw = quat->w;

	rmatrix[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
	rmatrix[1] = 2.0f * qx * qy - 2.0f * qz * qw;
	rmatrix[2] = 2.0f * qx * qz + 2.0f * qy * qw;
	rmatrix[3] = 2.0f * qx * qy + 2.0f * qz * qw;
	rmatrix[4] = 1.0f  - 2.0f * qx * qx - 2.0f * qz * qz;
	rmatrix[5] = 2.0f * qy * qz - 2.0f * qx * qw;
	rmatrix[6] = 2.0f * qx * qz - 2.0f * qy * qw;
	rmatrix[7] = 2.0f * qy * qz + 2.0f * qx * qw;
	rmatrix[8] = 1.0f  - 2.0f * qx * qx - 2.0f * qy * qy;
}

static void rotate(const magdata_t *in, magdata_t *out, const float *rmatrix)
{
	if (out == NULL) return;
	if (in == NULL || in->valid == 0) {
		out->valid = 0;
		out->x = out->y = out->z = 0;
		return;
	}
	out->x = in->x * rmatrix[0] + in->y * rmatrix[1] + in->z * rmatrix[2];
	out->y = in->x * rmatrix[3] + in->y * rmatrix[4] + in->z * rmatrix[5];
	out->z = in->x * rmatrix[6] + in->y * rmatrix[7] + in->z * rmatrix[8];
	out->valid = 1;
}


/*
typedef struct {
	float x;
	float y;
	float z;
	int valid;
} magdata_t;
magdata_t caldata[MAGBUFFSIZE];
*/

static GLuint spherelist;
static GLuint spherelowreslist;

void display_callback(void)
{
	static int updatenum=0;
	int i, count=0;
	float xscale, yscale, zscale;
	float xoff, yoff, zoff;
	float rotation[9];
	magdata_t point, draw;
	float magnitude[MAGBUFFSIZE];
	float latitude[MAGBUFFSIZE]; // 0 to PI
	float longitude[MAGBUFFSIZE]; // -PI to +PI
	float sumx=0.0, sumy=0.0, sumz=0.0, summag=0.0;

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glColor3f(1, 0, 0);	// set current color to red

	xscale = 0.06;
	yscale = 0.06;
	zscale = 0.06;
	xoff = 0.0;
	yoff = 0.0;
	zoff = -7.0;

	if (hard_iron.valid) {
		quad_to_rotation(&current_orientation, rotation);
		for (i=0; i < MAGBUFFSIZE; i++) {
			if (caldata[i].valid) {
				apply_calibration(&caldata[i], &point);
				sumx += point.x;
				sumy += point.y;
				sumz += point.z;
				magnitude[i] = sqrtf(point.x * point.x +
					point.y * point.y + point.z * point.z);
				summag += magnitude[i];
				longitude[i] = atan2f(point.y, point.x);
				latitude[i] = atan2f(sqrtf(point.x * point.x +
					point.y * point.y), point.z);
				count++;
				rotate(&point, &draw, rotation);
				glPushMatrix();
				glTranslatef(
					draw.x * xscale + xoff,
					draw.z * yscale + yoff,
					draw.y * zscale + zoff
				);
				if (draw.y >= 0.0f) {
					glCallList(spherelist);
				} else {
					glCallList(spherelowreslist);
				}
				glPopMatrix();
			}
		}
	}
#if 0
	if (updatenum++ == 500) {
		for (i=0; i < MAGBUFFSIZE; i++) {
			if (caldata[i].valid) {
				printf("long: %6.2f  lat: %5.2f\n",
					longitude[i] * 180.0 / M_PI,
					latitude[i] * 180.0 / M_PI);
			}
		}
		printf("count = %d\n", count);
		printf("sum = %.2f %.2f %.2f\n", sumx, sumy, sumz);
		printf("summag = %.2f  avg: %.2f\n", summag, summag / (float)count);
		printf("exit here\n");
		exit(1);
	}
#endif
}

void resize_callback(int width, int height)
{
	const float ar = (float) width / (float) height;

	glViewport(0, 0, width, height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glFrustum(-ar, ar, -1.0, 1.0, 2.0, 100.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity() ;
}


static const GLfloat light_ambient[]  = { 0.0f, 0.0f, 0.0f, 1.0f };
static const GLfloat light_diffuse[]  = { 1.0f, 1.0f, 1.0f, 1.0f };
static const GLfloat light_specular[] = { 1.0f, 1.0f, 1.0f, 1.0f };
static const GLfloat light_position[] = { 2.0f, 5.0f, 5.0f, 0.0f };

static const GLfloat mat_ambient[]    = { 0.7f, 0.7f, 0.7f, 1.0f };
static const GLfloat mat_diffuse[]    = { 0.8f, 0.8f, 0.8f, 1.0f };
static const GLfloat mat_specular[]   = { 1.0f, 1.0f, 1.0f, 1.0f };
static const GLfloat high_shininess[] = { 100.0f };

void visualize_init(void)
{
	GLUquadric *sphere;

	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_CULL_FACE);
	glCullFace(GL_BACK);
	//glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LESS);
	glEnable(GL_LIGHT0);
	//glEnable(GL_NORMALIZE);
	glEnable(GL_COLOR_MATERIAL);
	glEnable(GL_LIGHTING);
	glLightfv(GL_LIGHT0, GL_AMBIENT,  light_ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE,  light_diffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
	glLightfv(GL_LIGHT0, GL_POSITION, light_position);
	glMaterialfv(GL_FRONT, GL_AMBIENT,   mat_ambient);
	glMaterialfv(GL_FRONT, GL_DIFFUSE,   mat_diffuse);
	glMaterialfv(GL_FRONT, GL_SPECULAR,  mat_specular);
	glMaterialfv(GL_FRONT, GL_SHININESS, high_shininess);

	sphere = gluNewQuadric();
	gluQuadricDrawStyle(sphere, GLU_FILL);
	gluQuadricNormals(sphere, GLU_SMOOTH);
	spherelist = glGenLists(1);
	glNewList(spherelist, GL_COMPILE);
	gluSphere(sphere, 0.08, 16, 14);
	glEndList();
	spherelowreslist = glGenLists(1);
	glNewList(spherelowreslist, GL_COMPILE);
	gluSphere(sphere, 0.08, 12, 10);
	glEndList();
	gluDeleteQuadric(sphere);
}