Sunday, May 13, 2018
Thursday, January 25, 2018
Hex Puzzle 3d
DOWNLOAD TAR.GZ
https://drive.google.com/file/d/1qnczYoJ8EMXKv_cntfs1AlCOkkbPNpJ8/view?usp=sharing
https://github.com/pghauff55/hexpuzzle3d
HEXMAIN.CPP
#include <iostream>
#include <vector>
#include <stdio.h>
#include <termios.h>
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
// GL and related includes
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include "SOIL.h"
// Local includes
#include "hexplanet.h"
using namespace std;
using namespace cv;
//=========================================
// globals (mostly for dealing with glut/gui)
//=========================================
GLuint tex_2d;
float A1x=0.0,A1y=0.0,A1z=0.0;
//string cascadeName = "./data/haarcascades/haarcascade_frontalface_alt.xml";
//string nestedCascadeName = "./data/haarcascades/haarcascade_eye_tree_eyeglasses.xml";
//VideoCapture cap(0);
//CascadeClassifier cascade, nestedCascade;
double scale = 4.0;
int g_glutMainWin;
float g_aspect;
HexPlanet *m_planet;
GLfloat light_diffuse[] = {1.0, 1.0, 1.0, 1.0}; /* Red diffuse light. */
GLfloat light_position[] = {1.0, 15.0, 5.0, 0.0}; /* Infinite light location. */
int mouse_x=0,mouse_y=0;
GLfloat texcoord[][2] = {{0.5, 0.0},
{0.0669875,0.25},
{0.0669875, 0.75},
{0.5, 1.0},
{0.9330125, 0.75},
{0.9330125, 0.25}}; /* Red diffuse light. */
int tiles[300][2];
GLuint textile[5];
float anglex=0.0;
float angley=0.0;
float sz=1.0f;
float sx=0.0f;
float sy=0.0f;
float sX=0.0f,sY=0.0f,sZ=1.0f;
int current_axes=5;
int last_axes=5;
bool invert_y=false;
float theta2=atan(sz/sx);
Imath::V3f svec4,svec(0.0f,0.0f,1.0f),SVEC(0.0f,0.0f,1.0f);
Imath::V3f X(1.0f,0.0f,0.0f),Y(0.0f,1.0f,0.0f),Z(0.0f,0.0f,1.0f);
bool button_down=false;
bool flipped=false;
bool moving=false;
int select_hex=0;
int current_select_hex=0;
/*** drawing text on screen ***/
void drawString(char *string)
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
int w = glutGet( GLUT_WINDOW_WIDTH );
int h = glutGet( GLUT_WINDOW_HEIGHT );
glOrtho( 0, w, 0, h, -1, 1 );
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glDisable( GL_DEPTH_TEST );
glDisable( GL_LIGHTING );
glColor3f(1, 0, 0);
glRasterPos2i(20, 20);
void *font = GLUT_BITMAP_HELVETICA_18;
for (char* c=string; *c != '\0'; c++)
{
glutBitmapCharacter(font, *c);
}
glEnable( GL_LIGHTING );
glEnable (GL_DEPTH_TEST);
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
}
//=========================================
// glut Display Func
//=========================================
void glut_Display( void )
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
gluPerspective( 50.0f, 1.5f, 1.0f, 5000.0f );
float dt=0.000007f;
float dsx=0,dsy=0,dsz=0;
dsx=(mouse_x-800)*dt;
dsy=-(mouse_y-450.0)*dt;
if(abs(SVEC[1])>0.707f){
Imath::V3f x,y,z,Yt,Xt,Zt;
printf(" SVEC(%2.3f %2.3f %2.3f ) X(%2.3f %2.3f %2.3f ) Y(%2.3f %2.3f %2.3f ) Z(%2.3f %2.3f %2.3f )\n",SVEC[0],SVEC[1],SVEC[2],X[0],X[1],X[2],Y[0],Y[1],Y[2],Z[0],Z[1],Z[2]);
y[0]=-SVEC[0];
if(SVEC[1]>0.0f)
y[1]=sin(M_PI/2.0f-atan( abs(SVEC[1])/sqrt(SVEC[0]*SVEC[0]+SVEC[2]*SVEC[2]) ) );
else
y[1]=-sin(M_PI/2.0f-atan( abs(SVEC[1])/sqrt(SVEC[0]*SVEC[0]+SVEC[2]*SVEC[2]) ) );
y[2]=-SVEC[2];
y.normalize();
printf("(%2.3f %2.3f %2.3f ) ",y[0],y[1],y[2]);
z=SVEC;
z.normalize();
x=y%z;
x.normalize();
//y=z%x;
//y.normalize();
float square_angle=acos((y^z)/(sqrt(z^z)*sqrt(y^y)))*180.0f/M_PI;
printf(" %2.3f ",square_angle);
Yt[0]=y[0]*X[0]+y[1]*Y[0]+y[2]*Z[0];
Yt[1]=y[0]*X[1]+y[1]*Y[1]+y[2]*Z[1];
Yt[2]=y[0]*X[2]+y[1]*Y[2]+y[2]*Z[2];
Zt[0]=z[0]*X[0]+z[1]*Y[0]+z[2]*Z[0];
Zt[1]=z[0]*X[1]+z[1]*Y[1]+z[2]*Z[1];
Zt[2]=z[0]*X[2]+z[1]*Y[2]+z[2]*Z[2];
Xt[0]=x[0]*X[0]+x[1]*Y[0]+x[2]*Z[0];
Xt[1]=x[0]*X[1]+x[1]*Y[1]+x[2]*Z[1];
Xt[2]=x[0]*X[2]+x[1]*Y[2]+x[2]*Z[2];
X=Xt;Y=Yt;Z=Zt;
X.normalize();
Y.normalize();
Z.normalize();
printf(" SVEC(%2.3f %2.3f %2.3f ) X(%2.3f %2.3f %2.3f ) Y(%2.3f %2.3f %2.3f ) Z(%2.3f %2.3f %2.3f )\n",SVEC[0],SVEC[1],SVEC[2],X[0],X[1],X[2],Y[0],Y[1],Y[2],Z[0],Z[1],Z[2]);
float SY,SX,SZ;
SVEC[0]=0.0f;
SVEC[1]=0.0f;
SVEC[2]=1.0f;
SX=SVEC[0]*X[0]+SVEC[1]*Y[0]+SVEC[2]*Z[0];
SY=SVEC[0]*X[1]+SVEC[1]*Y[1]+SVEC[2]*Z[1];
SZ=SVEC[0]*X[2]+SVEC[1]*Y[2]+SVEC[2]*Z[2];
printf("SX SY SZ (%2.3f %2.3f %2.3f ) ",SX,SY,SZ);
getchar();
}
//up vector
Imath::V3f SVEC2;
SVEC2=SVEC-Imath::V3f(0.0f,1.0f,0.0f);
SVEC2.normalize();
Imath::V3f SVEC3=SVEC%SVEC2;
SVEC3.normalize();
Imath::V3f SVEC5;
SVEC5=SVEC%SVEC3;
SVEC5.normalize();
Imath::V3f SVEC_POINTER,svec_pointer;
SVEC_POINTER[0]=SVEC[0]+250.0f*dsx*SVEC3[0]+250.0f*dsy*SVEC5[0];
SVEC_POINTER[1]=SVEC[1]+250.0f*dsx*SVEC3[1]+250.0f*dsy*SVEC5[1];
SVEC_POINTER[2]=SVEC[2]+250.0f*dsx*SVEC3[2]+250.0f*dsy*SVEC5[2];
svec_pointer[0]=SVEC_POINTER[0]*X[0]+SVEC_POINTER[1]*Y[0]+SVEC_POINTER[2]*Z[0];
svec_pointer[1]=SVEC_POINTER[0]*X[1]+SVEC_POINTER[1]*Y[1]+SVEC_POINTER[2]*Z[1];
svec_pointer[2]=SVEC_POINTER[0]*X[2]+SVEC_POINTER[1]*Y[2]+SVEC_POINTER[2]*Z[2];
SVEC[0]+=dsx*SVEC3[0]+dsy*SVEC5[0];
SVEC[1]+=dsx*SVEC3[1]+dsy*SVEC5[1];
SVEC[2]+=dsx*SVEC3[2]+dsy*SVEC5[2];
SVEC.normalize();
sx=SVEC[0]*X[0]+SVEC[1]*Y[0]+SVEC[2]*Z[0];
sy=SVEC[0]*X[1]+SVEC[1]*Y[1]+SVEC[2]*Z[1];
sz=SVEC[0]*X[2]+SVEC[1]*Y[2]+SVEC[2]*Z[2];
Imath::V3f svec3;
svec3[0]=SVEC3[0]*X[0]+SVEC3[1]*Y[0]+SVEC3[2]*Z[0];
svec3[1]=SVEC3[0]*X[1]+SVEC3[1]*Y[1]+SVEC3[2]*Z[1];
svec3[2]=SVEC3[0]*X[2]+SVEC3[1]*Y[2]+SVEC3[2]*Z[2];
Imath::V3f svec5;
svec5[0]=SVEC5[0]*X[0]+SVEC5[1]*Y[0]+SVEC5[2]*Z[0];
svec5[1]=SVEC5[0]*X[1]+SVEC5[1]*Y[1]+SVEC5[2]*Z[1];
svec5[2]=SVEC5[0]*X[2]+SVEC5[1]*Y[2]+SVEC5[2]*Z[2];
Imath::V3f svec4(sx,sy,sz);
svec=svec4;
svec.normalize();
svec5.normalize();
char string1[100];
sprintf(string1," tile index: %d (%2.3f %2.3f %2.3f ) (%2.3f %2.3f %2.3f ) (%2.3f %2.3f %2.3f )",select_hex,SVEC[0],SVEC[1],SVEC[2],X[0],X[1],X[2],Y[0],Y[1],Y[2]);
drawString(string1);
gluLookAt(3.0f*svec[0], 3.0f*svec[1], 3.0f*svec[2], 0.0, 0.0, 0.0,svec5[0],svec5[1],svec5[2]);
light_position[0]=40.0f*svec[0];
light_position[1]=40.0f*svec[1];
light_position[2]=40.0f*svec[2];
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
glMatrixMode( GL_MODELVIEW );
glLoadIdentity();
glPushMatrix();
glScalef(1.3,1.3,1.3);
glDisable(GL_LIGHTING);
glBegin( GL_LINES );
glColor3f( 1.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( 1.0f, 0.0f, 0.0f );
glColor3f( 0.0f, 1.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 1.0f, 0.0f );
glColor3f( 0.0f, 0.0f, 1.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 1.0f );
glColor3f( 1.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( X[0], X[1], X[2] );
glColor3f( 0.0f, 1.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( Y[0], Y[1], Y[2] );
glColor3f( 0.0f, 0.0f, 1.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( Z[0], Z[1], Z[2] );
//glScalef(25.0f,25.0f,25.0f);
glColor3f( 0.0f, 1.0f, 1.0f );
glVertex3f( svec[0], svec[1], svec[2] );
glVertex3f( svec[0]+10.0f*dsy*svec5[0], svec[1]+10.0f*dsy*svec5[1], svec[2]+10.0f*dsy*svec5[2] );
glVertex3f( svec[0], svec[1], svec[2] );
glVertex3f( svec[0]+10.0f*dsx*svec3[0], svec[1]+10.0f*dsx*svec3[1], svec[2]+10.0f*dsx*svec3[2] );
glEnd();
glPopMatrix();
glEnable(GL_LIGHTING);
glScalef(0.1,0.1,0.1);
std::vector<Imath::V3f> Poly;
select_hex=m_planet->getHexIndexFromPoint(svec_pointer);
m_planet->getPolygon(m_planet->m_hexes[select_hex],Poly,1.1);
if(button_down){
tiles[select_hex][1]=(tiles[select_hex][1]+1)%Poly.size();
button_down=false;
}
glDisable(GL_TEXTURE_2D);
glDisable(GL_LIGHTING);
glColor3f(0,1,0);
glLineWidth(5.0);
glBegin(GL_LINE_LOOP);
for(int i=0;i<Poly.size();i++){
Imath::V3f p=Poly[i];
p.normalize();
p *= m_planet->kPlanetRadius + 1.3;
glVertex3f(p[0],p[1],p[2]);
}
glEnd();
glEnable(GL_TEXTURE_2D);
glEnable(GL_LIGHTING);
//printf("%d\n",m_planet->m_hexes.size());
for(int j=0;j<m_planet->m_hexes.size();j++){
glColor3f(1,1,0.5);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,textile[tiles[j][0]]);
//int j=16;
m_planet->getPolygon(m_planet->m_hexes[j],Poly,1.1);
Imath::V3f p2=m_planet->m_hexes[j].m_vertPos;
p2.normalize();
if( (p2[0]<0.3&&p2[0]>=0.0) && (p2[1]<0.3&&p2[1]>=0.0) && (p2[2]>0.8&&p2[2]<=1.0)){
//printf("*%d",j);
}
glBegin(GL_POLYGON);
glNormal3f(p2[0],p2[1],p2[2]);
//printf("-----------------\n %g %g %g",p2[0],p2[1],p2[2]);
for(int i=0;i<Poly.size();i++){
Imath::V3f p=Poly[i];
p.normalize();
p *= m_planet->kPlanetRadius + 1.1;
if(p[0]==p2[0] && p[1]==p2[1] && p[2]==p2[2] )printf("*");
glTexCoord2f(texcoord[(i+tiles[j][1])%Poly.size()][0],texcoord[(i+tiles[j][1])%Poly.size()][1]);
glVertex3f(p[0],p[1],p[2]);
//printf("%g %g %g\n",p[0],p[1],p[2]);
}
glEnd();
glDisable(GL_TEXTURE_2D);
glColor3f(1,0,0);
glLineWidth(5.0);
glBegin(GL_LINE_LOOP);
for(int i=0;i<Poly.size();i++){
Imath::V3f p=Poly[i];
p.normalize();
p *= m_planet->kPlanetRadius + 1.2;
glVertex3f(p[0],p[1],p[2]);
}
glEnd();
}
//GLUquadricObj *quadric;
//quadric = gluNewQuadric();
//gluQuadricDrawStyle(quadric, GLU_FILL );
//gluSphere( quadric , .5 , 36 , 18 );
glutSwapBuffers();
glutPostRedisplay();
}
void OnMouseClick(int button, int state, int x, int y)
{
if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
{
button_down=true;
}
}
void glut_Motion(int x, int y)
{
mouse_x = x;
mouse_y = y;
glutPostRedisplay();
}
//=========================================
// Main program
//=========================================
int main( int argc, char *argv[])
{
//if( !cascade.load( cascadeName ) )
// {
// printf("ERROR: Could not load classifier cascade");
// return -1;
/// }
//cap.set(CV_CAP_PROP_FRAME_WIDTH,1280);
//cap.set(CV_CAP_PROP_FRAME_HEIGHT,720);
// Todo: check args for fullscreen
m_planet = new HexPlanet( 3, 0.17,0.5 );
for(int j=0;j<m_planet->m_hexes.size();j++){
tiles[j][1]=rand() % 6 + 1;//direction
tiles[j][0]=rand() % 5;//tile type
}
// Initialize glut
glutInit(&argc, argv);
glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH );
glutInitWindowPosition( 0, 0 );
glutInitWindowSize( 800, 600 );
g_glutMainWin = glutCreateWindow( "Hex Planet Demo" );
glutFullScreen();
glutDisplayFunc( glut_Display );
glutPassiveMotionFunc( glut_Motion );
glutMouseFunc(OnMouseClick);
glEnable( GL_BLEND );
//glDisable( GL_BLEND );
// straight alpha
glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
// premultiplied alpha (remember to do the same in glColor!!)
//glBlendFunc( GL_ONE, GL_ONE_MINUS_SRC_ALPHA );
// do I want alpha thresholding?
glEnable( GL_ALPHA_TEST );
glAlphaFunc( GL_GREATER, 0.5f );
/* load an image file directly as a new OpenGL texture */
tex_2d = SOIL_load_OGL_texture
(
"shape_hexagon.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == tex_2d )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[0]= SOIL_load_OGL_texture
(
"shape_hexagon_empty.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[0] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[1]= SOIL_load_OGL_texture
(
"shape_hexagon2.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[1] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[2]= SOIL_load_OGL_texture
(
"shape_hexagon3.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[2] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[3]= SOIL_load_OGL_texture
(
"shape_hexagon4.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[3] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[4]= SOIL_load_OGL_texture
(
"shape_hexagon-straight.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[4] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
/* Enable a single OpenGL light. */
GLfloat light_ambient[] =
{0.2, 0.2, 0.2, 1.0};
GLfloat light_diffuse[] =
{1.0, 1.0, 1.0, 1.0};
GLfloat light_specular[] =
{1.0, 1.0, 1.0, 1.0};
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);
glEnable(GL_LIGHT0);
glDepthFunc(GL_LESS);
glEnable(GL_DEPTH_TEST);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
glEnable(GL_COLOR_MATERIAL);
glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT1, GL_POSITION, light_position);
glEnable(GL_LIGHT1);
glEnable(GL_LIGHTING);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
// Call glut main loop
glutMainLoop();
//SOIL_free_image_data(tex_2d);
return 1;
}
https://drive.google.com/file/d/1qnczYoJ8EMXKv_cntfs1AlCOkkbPNpJ8/view?usp=sharing
https://github.com/pghauff55/hexpuzzle3d
HEXMAIN.CPP
#include <iostream>
#include <vector>
#include <stdio.h>
#include <termios.h>
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
// GL and related includes
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include "SOIL.h"
// Local includes
#include "hexplanet.h"
using namespace std;
using namespace cv;
//=========================================
// globals (mostly for dealing with glut/gui)
//=========================================
GLuint tex_2d;
float A1x=0.0,A1y=0.0,A1z=0.0;
//string cascadeName = "./data/haarcascades/haarcascade_frontalface_alt.xml";
//string nestedCascadeName = "./data/haarcascades/haarcascade_eye_tree_eyeglasses.xml";
//VideoCapture cap(0);
//CascadeClassifier cascade, nestedCascade;
double scale = 4.0;
int g_glutMainWin;
float g_aspect;
HexPlanet *m_planet;
GLfloat light_diffuse[] = {1.0, 1.0, 1.0, 1.0}; /* Red diffuse light. */
GLfloat light_position[] = {1.0, 15.0, 5.0, 0.0}; /* Infinite light location. */
int mouse_x=0,mouse_y=0;
GLfloat texcoord[][2] = {{0.5, 0.0},
{0.0669875,0.25},
{0.0669875, 0.75},
{0.5, 1.0},
{0.9330125, 0.75},
{0.9330125, 0.25}}; /* Red diffuse light. */
int tiles[300][2];
GLuint textile[5];
float anglex=0.0;
float angley=0.0;
float sz=1.0f;
float sx=0.0f;
float sy=0.0f;
float sX=0.0f,sY=0.0f,sZ=1.0f;
int current_axes=5;
int last_axes=5;
bool invert_y=false;
float theta2=atan(sz/sx);
Imath::V3f svec4,svec(0.0f,0.0f,1.0f),SVEC(0.0f,0.0f,1.0f);
Imath::V3f X(1.0f,0.0f,0.0f),Y(0.0f,1.0f,0.0f),Z(0.0f,0.0f,1.0f);
bool button_down=false;
bool flipped=false;
bool moving=false;
int select_hex=0;
int current_select_hex=0;
/*** drawing text on screen ***/
void drawString(char *string)
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
int w = glutGet( GLUT_WINDOW_WIDTH );
int h = glutGet( GLUT_WINDOW_HEIGHT );
glOrtho( 0, w, 0, h, -1, 1 );
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glDisable( GL_DEPTH_TEST );
glDisable( GL_LIGHTING );
glColor3f(1, 0, 0);
glRasterPos2i(20, 20);
void *font = GLUT_BITMAP_HELVETICA_18;
for (char* c=string; *c != '\0'; c++)
{
glutBitmapCharacter(font, *c);
}
glEnable( GL_LIGHTING );
glEnable (GL_DEPTH_TEST);
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
}
//=========================================
// glut Display Func
//=========================================
void glut_Display( void )
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
gluPerspective( 50.0f, 1.5f, 1.0f, 5000.0f );
float dt=0.000007f;
float dsx=0,dsy=0,dsz=0;
dsx=(mouse_x-800)*dt;
dsy=-(mouse_y-450.0)*dt;
if(abs(SVEC[1])>0.707f){
Imath::V3f x,y,z,Yt,Xt,Zt;
printf(" SVEC(%2.3f %2.3f %2.3f ) X(%2.3f %2.3f %2.3f ) Y(%2.3f %2.3f %2.3f ) Z(%2.3f %2.3f %2.3f )\n",SVEC[0],SVEC[1],SVEC[2],X[0],X[1],X[2],Y[0],Y[1],Y[2],Z[0],Z[1],Z[2]);
y[0]=-SVEC[0];
if(SVEC[1]>0.0f)
y[1]=sin(M_PI/2.0f-atan( abs(SVEC[1])/sqrt(SVEC[0]*SVEC[0]+SVEC[2]*SVEC[2]) ) );
else
y[1]=-sin(M_PI/2.0f-atan( abs(SVEC[1])/sqrt(SVEC[0]*SVEC[0]+SVEC[2]*SVEC[2]) ) );
y[2]=-SVEC[2];
y.normalize();
printf("(%2.3f %2.3f %2.3f ) ",y[0],y[1],y[2]);
z=SVEC;
z.normalize();
x=y%z;
x.normalize();
//y=z%x;
//y.normalize();
float square_angle=acos((y^z)/(sqrt(z^z)*sqrt(y^y)))*180.0f/M_PI;
printf(" %2.3f ",square_angle);
Yt[0]=y[0]*X[0]+y[1]*Y[0]+y[2]*Z[0];
Yt[1]=y[0]*X[1]+y[1]*Y[1]+y[2]*Z[1];
Yt[2]=y[0]*X[2]+y[1]*Y[2]+y[2]*Z[2];
Zt[0]=z[0]*X[0]+z[1]*Y[0]+z[2]*Z[0];
Zt[1]=z[0]*X[1]+z[1]*Y[1]+z[2]*Z[1];
Zt[2]=z[0]*X[2]+z[1]*Y[2]+z[2]*Z[2];
Xt[0]=x[0]*X[0]+x[1]*Y[0]+x[2]*Z[0];
Xt[1]=x[0]*X[1]+x[1]*Y[1]+x[2]*Z[1];
Xt[2]=x[0]*X[2]+x[1]*Y[2]+x[2]*Z[2];
X=Xt;Y=Yt;Z=Zt;
X.normalize();
Y.normalize();
Z.normalize();
printf(" SVEC(%2.3f %2.3f %2.3f ) X(%2.3f %2.3f %2.3f ) Y(%2.3f %2.3f %2.3f ) Z(%2.3f %2.3f %2.3f )\n",SVEC[0],SVEC[1],SVEC[2],X[0],X[1],X[2],Y[0],Y[1],Y[2],Z[0],Z[1],Z[2]);
float SY,SX,SZ;
SVEC[0]=0.0f;
SVEC[1]=0.0f;
SVEC[2]=1.0f;
SX=SVEC[0]*X[0]+SVEC[1]*Y[0]+SVEC[2]*Z[0];
SY=SVEC[0]*X[1]+SVEC[1]*Y[1]+SVEC[2]*Z[1];
SZ=SVEC[0]*X[2]+SVEC[1]*Y[2]+SVEC[2]*Z[2];
printf("SX SY SZ (%2.3f %2.3f %2.3f ) ",SX,SY,SZ);
getchar();
}
//up vector
Imath::V3f SVEC2;
SVEC2=SVEC-Imath::V3f(0.0f,1.0f,0.0f);
SVEC2.normalize();
Imath::V3f SVEC3=SVEC%SVEC2;
SVEC3.normalize();
Imath::V3f SVEC5;
SVEC5=SVEC%SVEC3;
SVEC5.normalize();
Imath::V3f SVEC_POINTER,svec_pointer;
SVEC_POINTER[0]=SVEC[0]+250.0f*dsx*SVEC3[0]+250.0f*dsy*SVEC5[0];
SVEC_POINTER[1]=SVEC[1]+250.0f*dsx*SVEC3[1]+250.0f*dsy*SVEC5[1];
SVEC_POINTER[2]=SVEC[2]+250.0f*dsx*SVEC3[2]+250.0f*dsy*SVEC5[2];
svec_pointer[0]=SVEC_POINTER[0]*X[0]+SVEC_POINTER[1]*Y[0]+SVEC_POINTER[2]*Z[0];
svec_pointer[1]=SVEC_POINTER[0]*X[1]+SVEC_POINTER[1]*Y[1]+SVEC_POINTER[2]*Z[1];
svec_pointer[2]=SVEC_POINTER[0]*X[2]+SVEC_POINTER[1]*Y[2]+SVEC_POINTER[2]*Z[2];
SVEC[0]+=dsx*SVEC3[0]+dsy*SVEC5[0];
SVEC[1]+=dsx*SVEC3[1]+dsy*SVEC5[1];
SVEC[2]+=dsx*SVEC3[2]+dsy*SVEC5[2];
SVEC.normalize();
sx=SVEC[0]*X[0]+SVEC[1]*Y[0]+SVEC[2]*Z[0];
sy=SVEC[0]*X[1]+SVEC[1]*Y[1]+SVEC[2]*Z[1];
sz=SVEC[0]*X[2]+SVEC[1]*Y[2]+SVEC[2]*Z[2];
Imath::V3f svec3;
svec3[0]=SVEC3[0]*X[0]+SVEC3[1]*Y[0]+SVEC3[2]*Z[0];
svec3[1]=SVEC3[0]*X[1]+SVEC3[1]*Y[1]+SVEC3[2]*Z[1];
svec3[2]=SVEC3[0]*X[2]+SVEC3[1]*Y[2]+SVEC3[2]*Z[2];
Imath::V3f svec5;
svec5[0]=SVEC5[0]*X[0]+SVEC5[1]*Y[0]+SVEC5[2]*Z[0];
svec5[1]=SVEC5[0]*X[1]+SVEC5[1]*Y[1]+SVEC5[2]*Z[1];
svec5[2]=SVEC5[0]*X[2]+SVEC5[1]*Y[2]+SVEC5[2]*Z[2];
Imath::V3f svec4(sx,sy,sz);
svec=svec4;
svec.normalize();
svec5.normalize();
char string1[100];
sprintf(string1," tile index: %d (%2.3f %2.3f %2.3f ) (%2.3f %2.3f %2.3f ) (%2.3f %2.3f %2.3f )",select_hex,SVEC[0],SVEC[1],SVEC[2],X[0],X[1],X[2],Y[0],Y[1],Y[2]);
drawString(string1);
gluLookAt(3.0f*svec[0], 3.0f*svec[1], 3.0f*svec[2], 0.0, 0.0, 0.0,svec5[0],svec5[1],svec5[2]);
light_position[0]=40.0f*svec[0];
light_position[1]=40.0f*svec[1];
light_position[2]=40.0f*svec[2];
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
glMatrixMode( GL_MODELVIEW );
glLoadIdentity();
glPushMatrix();
glScalef(1.3,1.3,1.3);
glDisable(GL_LIGHTING);
glBegin( GL_LINES );
glColor3f( 1.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( 1.0f, 0.0f, 0.0f );
glColor3f( 0.0f, 1.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 1.0f, 0.0f );
glColor3f( 0.0f, 0.0f, 1.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 1.0f );
glColor3f( 1.0f, 0.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( X[0], X[1], X[2] );
glColor3f( 0.0f, 1.0f, 0.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( Y[0], Y[1], Y[2] );
glColor3f( 0.0f, 0.0f, 1.0f );
glVertex3f( 0.0f, 0.0f, 0.0f );
glVertex3f( Z[0], Z[1], Z[2] );
//glScalef(25.0f,25.0f,25.0f);
glColor3f( 0.0f, 1.0f, 1.0f );
glVertex3f( svec[0], svec[1], svec[2] );
glVertex3f( svec[0]+10.0f*dsy*svec5[0], svec[1]+10.0f*dsy*svec5[1], svec[2]+10.0f*dsy*svec5[2] );
glVertex3f( svec[0], svec[1], svec[2] );
glVertex3f( svec[0]+10.0f*dsx*svec3[0], svec[1]+10.0f*dsx*svec3[1], svec[2]+10.0f*dsx*svec3[2] );
glEnd();
glPopMatrix();
glEnable(GL_LIGHTING);
glScalef(0.1,0.1,0.1);
std::vector<Imath::V3f> Poly;
select_hex=m_planet->getHexIndexFromPoint(svec_pointer);
m_planet->getPolygon(m_planet->m_hexes[select_hex],Poly,1.1);
if(button_down){
tiles[select_hex][1]=(tiles[select_hex][1]+1)%Poly.size();
button_down=false;
}
glDisable(GL_TEXTURE_2D);
glDisable(GL_LIGHTING);
glColor3f(0,1,0);
glLineWidth(5.0);
glBegin(GL_LINE_LOOP);
for(int i=0;i<Poly.size();i++){
Imath::V3f p=Poly[i];
p.normalize();
p *= m_planet->kPlanetRadius + 1.3;
glVertex3f(p[0],p[1],p[2]);
}
glEnd();
glEnable(GL_TEXTURE_2D);
glEnable(GL_LIGHTING);
//printf("%d\n",m_planet->m_hexes.size());
for(int j=0;j<m_planet->m_hexes.size();j++){
glColor3f(1,1,0.5);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,textile[tiles[j][0]]);
//int j=16;
m_planet->getPolygon(m_planet->m_hexes[j],Poly,1.1);
Imath::V3f p2=m_planet->m_hexes[j].m_vertPos;
p2.normalize();
if( (p2[0]<0.3&&p2[0]>=0.0) && (p2[1]<0.3&&p2[1]>=0.0) && (p2[2]>0.8&&p2[2]<=1.0)){
//printf("*%d",j);
}
glBegin(GL_POLYGON);
glNormal3f(p2[0],p2[1],p2[2]);
//printf("-----------------\n %g %g %g",p2[0],p2[1],p2[2]);
for(int i=0;i<Poly.size();i++){
Imath::V3f p=Poly[i];
p.normalize();
p *= m_planet->kPlanetRadius + 1.1;
if(p[0]==p2[0] && p[1]==p2[1] && p[2]==p2[2] )printf("*");
glTexCoord2f(texcoord[(i+tiles[j][1])%Poly.size()][0],texcoord[(i+tiles[j][1])%Poly.size()][1]);
glVertex3f(p[0],p[1],p[2]);
//printf("%g %g %g\n",p[0],p[1],p[2]);
}
glEnd();
glDisable(GL_TEXTURE_2D);
glColor3f(1,0,0);
glLineWidth(5.0);
glBegin(GL_LINE_LOOP);
for(int i=0;i<Poly.size();i++){
Imath::V3f p=Poly[i];
p.normalize();
p *= m_planet->kPlanetRadius + 1.2;
glVertex3f(p[0],p[1],p[2]);
}
glEnd();
}
//GLUquadricObj *quadric;
//quadric = gluNewQuadric();
//gluQuadricDrawStyle(quadric, GLU_FILL );
//gluSphere( quadric , .5 , 36 , 18 );
glutSwapBuffers();
glutPostRedisplay();
}
void OnMouseClick(int button, int state, int x, int y)
{
if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
{
button_down=true;
}
}
void glut_Motion(int x, int y)
{
mouse_x = x;
mouse_y = y;
glutPostRedisplay();
}
//=========================================
// Main program
//=========================================
int main( int argc, char *argv[])
{
//if( !cascade.load( cascadeName ) )
// {
// printf("ERROR: Could not load classifier cascade");
// return -1;
/// }
//cap.set(CV_CAP_PROP_FRAME_WIDTH,1280);
//cap.set(CV_CAP_PROP_FRAME_HEIGHT,720);
// Todo: check args for fullscreen
m_planet = new HexPlanet( 3, 0.17,0.5 );
for(int j=0;j<m_planet->m_hexes.size();j++){
tiles[j][1]=rand() % 6 + 1;//direction
tiles[j][0]=rand() % 5;//tile type
}
// Initialize glut
glutInit(&argc, argv);
glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH );
glutInitWindowPosition( 0, 0 );
glutInitWindowSize( 800, 600 );
g_glutMainWin = glutCreateWindow( "Hex Planet Demo" );
glutFullScreen();
glutDisplayFunc( glut_Display );
glutPassiveMotionFunc( glut_Motion );
glutMouseFunc(OnMouseClick);
glEnable( GL_BLEND );
//glDisable( GL_BLEND );
// straight alpha
glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
// premultiplied alpha (remember to do the same in glColor!!)
//glBlendFunc( GL_ONE, GL_ONE_MINUS_SRC_ALPHA );
// do I want alpha thresholding?
glEnable( GL_ALPHA_TEST );
glAlphaFunc( GL_GREATER, 0.5f );
/* load an image file directly as a new OpenGL texture */
tex_2d = SOIL_load_OGL_texture
(
"shape_hexagon.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == tex_2d )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[0]= SOIL_load_OGL_texture
(
"shape_hexagon_empty.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[0] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[1]= SOIL_load_OGL_texture
(
"shape_hexagon2.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[1] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[2]= SOIL_load_OGL_texture
(
"shape_hexagon3.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[2] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[3]= SOIL_load_OGL_texture
(
"shape_hexagon4.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[3] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
textile[4]= SOIL_load_OGL_texture
(
"shape_hexagon-straight.png",
SOIL_LOAD_AUTO,
SOIL_CREATE_NEW_ID,
SOIL_FLAG_MIPMAPS | SOIL_FLAG_INVERT_Y | SOIL_FLAG_NTSC_SAFE_RGB | SOIL_FLAG_COMPRESS_TO_DXT
);
/* check for an error during the load process */
if( 0 == textile[4] )
{
printf( "SOIL loading error: '%s'\n", SOIL_last_result() );
}
else printf("LOADED TEXTURE\n");
/* Enable a single OpenGL light. */
GLfloat light_ambient[] =
{0.2, 0.2, 0.2, 1.0};
GLfloat light_diffuse[] =
{1.0, 1.0, 1.0, 1.0};
GLfloat light_specular[] =
{1.0, 1.0, 1.0, 1.0};
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);
glEnable(GL_LIGHT0);
glDepthFunc(GL_LESS);
glEnable(GL_DEPTH_TEST);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
glEnable(GL_COLOR_MATERIAL);
glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT1, GL_POSITION, light_position);
glEnable(GL_LIGHT1);
glEnable(GL_LIGHTING);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
// Call glut main loop
glutMainLoop();
//SOIL_free_image_data(tex_2d);
return 1;
}
x
Monday, November 13, 2017
Adaptive Filter Algorithm for Gryoscope MPU6050 and Arduino
To use this code FASTWIRE needs to be enabled in I2Cdev.h in the ./Arduino/libraries/I2Cdev/ folder.
This code Assumes the IMUZero program has been used to calibrate the MPU6050 before use here.
Arduino Nano attached to a MPU 6050.
#include <I2Cdev.h>
#include "I2Cdev.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include <Wire.h>
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro2(0x69); / <-- use for AD0 high
////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(115200);
// put your setup code here, to run once:
//while(!Serial.available());
Fastwire::setup(400, false);
accelgyro.initialize();
if(accelgyro.testConnection()){
}
else {
}
/*
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -1131
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -5595
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 3855
Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 30
Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 44
Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 27
Serial.print("\n");*/
// accelgyro.setXAccelOffset(0);
// accelgyro.setYAccelOffset(0);
// accelgyro.setZAccelOffset(0);
accelgyro.setXGyroOffset(27);
accelgyro.setYGyroOffset(27);
accelgyro.setZGyroOffset(78);
accelgyro.setFullScaleGyroRange(0); //0 = +/- 250 degrees/sec | 1 = +/- 500 degrees/sec | 2 = +/- 1000 degrees/sec | 3 = +/- 2000 degrees/sec
accelgyro.setFullScaleAccelRange(0); //0 = +/- 2g | 1 = +/- 4g | 2 = +/- 8g | 3 = +/- 16g
delay(1000);
}
///////////////////////////////////////////////////////////////////////
#define M 20
#define O 20
float A2x=0.0,A2y=0.0,A2z=0.0;
float A3x=0.0,A3y=0.0,A3z=0.0;
float A1x=0.0,A1y=0.0,A1z=0.0;
int counter_m=0,counter_o=0;
bool calc_A3=false;
unsigned long dt=0;
double fdt=0.0;
int counter=0;
int N=1000;
int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t BUFFER[14];
int state=0;
float angle1x=0.0;
float angle1y=0.0;
float angle1z=0.0;
float angle2x=0.0;
float angle2y=0.0;
float angle2z=0.0;
double fax=0.0,faz=0.0,fay=0.0,fd=0.0;
float theta;
#define DUTY_MAX 20
bool tripped=false;
bool flash=true;
unsigned int duty_counter=0,duty_cycleR=0,duty_cycleG=0,duty_cycleB=0;
///////////////////////////////////////////////////////////////////////////
void loop() {
// Serial.print(duty_counter%DUTY_MAX); Serial.print("\t");
// duty_counter++;
//////////////////////////////////////////////
fax=0.0;faz=0.0;fay=0.0;counter=0;
do{
tripped=false;
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
if(state>=0){
ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
if(!tripped){
counter++;
fax+=ax;
fay+=ay;
faz+=az;
}
// Serial.print(ax); Serial.print("\t");
// Serial.print(ay); Serial.print("\t");
// Serial.print(az); Serial.print("\t");
// Serial.print( sqrt(faz*faz+fax*fax+fay*fay)); Serial.print("\t\t");
}while(counter<N);
fax=fax/float(N);
fay=fay/float(N);
faz=faz/float(N);
angle1y=180.0/M_PI*atan(fay/faz);
angle1x=180.0/M_PI*atan(-fax/sqrt(fay*fay+faz*faz));
////////////////////////////
// Serial.print(fax); Serial.print("\t");
// Serial.print(fay); Serial.print("\t");
// Serial.print(faz); Serial.print("\t");
// Serial.print(sqrt(fax*fax+fay*fay+faz*faz)-16356.0); Serial.println("");
////////////////////////////////////////////////////////////////////////////////////////////////
do{
A2x=0.0;A2y=0.0;A2z=0.0;
counter_o=0.0;
do{
A1x=0.0;A1y=0.0;A1z=0.0;
counter_m=0;fdt=0.0;
do{
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
if(state>=0){
ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_YOUT_H,2, BUFFER);//
if(state>=0){
gy= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_XOUT_H,2, BUFFER);//
if(state>=0){
gx= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_ZOUT_H,2, BUFFER);//
if(state>=0){
gz= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
if(!tripped){
if(dt!=0)fdt+=(micros()-dt)/1000000.0;
else fdt=0.0;
dt=micros();
counter_m++;
A1x+=gx;
A1y+=gy;
A1z+=gz;
}
}while(counter_m<M);
A1x/=1.0*M;
A1y/=1.0*M;
A1z/=1.0*M;
if(calc_A3 && (abs(A1x-A3x)>15.0 || abs(A1y-A3y)>15.0 || abs(A1z-A3z)>50.0 )){
angle1x+=fdt*A1x/131.0;
angle1y+=fdt*A1y/131.0;
angle1z+=fdt*A1z/131.0;
// Serial.print("*");
//counter_o=0;A2x=0.0;A2y=0.0;A2z=0.0;
}
else{
A2x+=A1x;
A2y+=A1y;
A2z+=A1z;
counter_o++;
}
/// Serial.print(abs(A1x-A3x)); Serial.print("\t");
// Serial.print(abs(A1y-A3y)); Serial.print("\t");
// Serial.print(abs(A1z-A3z));Serial.print("\t");
// Serial.print(A3x); Serial.print("\t");
// Serial.print(A3y); Serial.print("\t");
// Serial.print(A3z);Serial.print("\t");
Serial.print(angle1x); Serial.print("\t");
Serial.print(angle1y); Serial.print("\t");
Serial.print(angle1z);Serial.print("\n");
// Serial.print(fdt*1000);Serial.println("");
}while(counter_o<O);
A3x=A2x/(1.0*O);
A3y=A2y/(1.0*O);
A3z=A2z/(1.0*O);
calc_A3=true;
}while(true);
}
Opengl gyro_cube.c
/* Copyright (c) Mark J. Kilgard, 1997. */
/* This program is freely distributable without licensing fees
and is provided without guarantee or warrantee expressed or
implied. This program is -not- in the public domain. */
/* This program was requested by Patrick Earl; hopefully someone else
will write the equivalent Direct3D immediate mode program. */
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <GL/glut.h>
GLfloat light_diffuse[] = {1.0, 0.0, 0.0, 1.0}; /* Red diffuse light. */
GLfloat light_position[] = {1.0, 5.0, -1.0, 0.0}; /* Infinite light location. */
GLfloat n[6][3] = { /* Normals for the 6 faces of a cube. */
{-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0},
{0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 0.0, -1.0} };
GLint faces[6][4] = { /* Vertex indices for the 6 faces of a cube. */
{0, 1, 2, 3}, {3, 2, 6, 7}, {7, 6, 5, 4},
{4, 5, 1, 0}, {5, 6, 2, 1}, {7, 4, 0, 3} };
GLfloat v[8][3]; /* Will be filled in with X,Y,Z vertexes. */
//serial descriptor
int fd;
char *portname = "/dev/ttyUSB0";
int
set_interface_attribs (int fd, int speed, int parity)
{
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
// error_message ("error %d from tcgetattr", errno);
return -1;
}
cfsetospeed (&tty, speed);
cfsetispeed (&tty, speed);
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
// disable IGNBRK for mismatched speed tests; otherwise receive break
// as \000 chars
tty.c_iflag &= ~IGNBRK; // disable break processing
tty.c_lflag = 0; // no signaling chars, no echo,
// no canonical processing
tty.c_oflag = 0; // no remapping, no delays
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
// enable reading
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
tty.c_cflag |= parity;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS;
if (tcsetattr (fd, TCSANOW, &tty) != 0)
{
//error_message ("error %d from tcsetattr", errno);
return -1;
}
return 0;
}
void
set_blocking (int fd, int should_block)
{
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
//error_message ("error %d from tggetattr", errno);
return;
}
tty.c_cc[VMIN] = should_block ? 1 : 0;
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tcsetattr (fd, TCSANOW, &tty);
}
void
drawBox(void)
{
int i;
for (i = 0; i < 6; i++) {
glBegin(GL_QUADS);
glNormal3fv(&n[i][0]);
glVertex3fv(&v[faces[i][0]][0]);
glVertex3fv(&v[faces[i][1]][0]);
glVertex3fv(&v[faces[i][2]][0]);
glVertex3fv(&v[faces[i][3]][0]);
glEnd();
}
}
///////////////////////////////////////////////////////////////////////////
void
display(void)
{
char buffer1[100],buffer2[100],buffer3[100];
float A1x=0.0,A1y=0.0,A1z=0.0;
char line[200];
char *pch;
char *pch1;
char *pch2;
char in;
ssize_t length;
int i=0;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
printf("START\n");
do{
length = read(fd, &in, 1);
//printf("%d",length);
if(length==1){
line[i]=in;
i++;
}
//printf("%d",i);
}while( in!='\n');
line[i]='\0';
//printf("%s\n",line);
pch=strchr(line,'\t');
strncpy(buffer1,line,pch-line+1);
A1x=atof(buffer1);
pch1=strchr(pch+1,'\t');
strncpy(buffer2,pch,pch1-pch+1);
A1y=atof(buffer2);
pch2=strchr(pch1+1,'\n');
strncpy(buffer3,pch1,pch2-pch1+1);
A1z=atof(buffer3);
printf("%g\t%g\t%g\n",A1x,A1y,A1z);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0.0, 0.0, 5.0, /* eye is at (0,0,5) */
0.0, 0.0, 0.0, /* center is at (0,0,0) */
0.0, 1.0, 0.); /* up is in positive Y direction */
//glTranslatef(0.0, 0.0, 1.0);
glRotatef(A1z, 0.0, 1.0, 0.0);
glRotatef(-A1x+90, 1.0, 0.0, 0.0);
drawBox();
glutSwapBuffers();
glutPostRedisplay();
}
////////////////////////////////////////////////////////////////////////////
void
init(void)
{
/* Setup cube vertex data. */
v[0][0] = v[1][0] = v[2][0] = v[3][0] = -1;
v[4][0] = v[5][0] = v[6][0] = v[7][0] = 1;
v[0][1] = v[1][1] = v[4][1] = v[5][1] = -1;
v[2][1] = v[3][1] = v[6][1] = v[7][1] = 1;
v[0][2] = v[3][2] = v[4][2] = v[7][2] = 1;
v[1][2] = v[2][2] = v[5][2] = v[6][2] = -1;
/* Enable a single OpenGL light. */
glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHTING);
/* Use depth buffering for hidden surface elimination. */
glEnable(GL_DEPTH_TEST);
/* Setup the view of the cube. */
glMatrixMode(GL_PROJECTION);
gluPerspective( /* field of view in degree */ 40.0,
/* aspect ratio */ 1.0,
/* Z near */ 1.0, /* Z far */ 10.0);
glMatrixMode(GL_MODELVIEW);
gluLookAt(0.0, 0.0, 5.0, /* eye is at (0,0,5) */
0.0, 0.0, 0.0, /* center is at (0,0,0) */
0.0, 1.0, 0.); /* up is in positive Y direction */
/* Adjust cube position to be asthetic angle. */
//glTranslatef(0.0, 0.0, -1.0);
//glRotatef(60, 1.0, 0.0, 0.0);
//glRotatef(-20, 0.0, 0.0, 1.0);
}
int
main(int argc, char **argv)
{
fd = open (portname, O_RDWR | O_NOCTTY | O_SYNC);
if (fd < 0)
{
printf ("error opening port\n");
return 1;
}
printf("port opened...");
set_interface_attribs (fd, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity)
set_blocking (fd, 0);
//printf("Writing port...\n");
//write (fd, "\n", 2); // send 7 character greeting
//usleep ((2) * 100);
printf("init glut...");
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
glutCreateWindow("red 3D lighted cube");
glutDisplayFunc(display);
init();
glutMainLoop();
return 0; /* ANSI C requires main to return int. */
}
This code Assumes the IMUZero program has been used to calibrate the MPU6050 before use here.
Arduino Nano attached to a MPU 6050.
#include <I2Cdev.h>
#include "I2Cdev.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include <Wire.h>
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro2(0x69); / <-- use for AD0 high
////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(115200);
// put your setup code here, to run once:
//while(!Serial.available());
Fastwire::setup(400, false);
accelgyro.initialize();
if(accelgyro.testConnection()){
}
else {
}
/*
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -1131
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -5595
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 3855
Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 30
Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 44
Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 27
Serial.print("\n");*/
// accelgyro.setXAccelOffset(0);
// accelgyro.setYAccelOffset(0);
// accelgyro.setZAccelOffset(0);
accelgyro.setXGyroOffset(27);
accelgyro.setYGyroOffset(27);
accelgyro.setZGyroOffset(78);
accelgyro.setFullScaleGyroRange(0); //0 = +/- 250 degrees/sec | 1 = +/- 500 degrees/sec | 2 = +/- 1000 degrees/sec | 3 = +/- 2000 degrees/sec
accelgyro.setFullScaleAccelRange(0); //0 = +/- 2g | 1 = +/- 4g | 2 = +/- 8g | 3 = +/- 16g
delay(1000);
}
///////////////////////////////////////////////////////////////////////
#define M 20
#define O 20
float A2x=0.0,A2y=0.0,A2z=0.0;
float A3x=0.0,A3y=0.0,A3z=0.0;
float A1x=0.0,A1y=0.0,A1z=0.0;
int counter_m=0,counter_o=0;
bool calc_A3=false;
unsigned long dt=0;
double fdt=0.0;
int counter=0;
int N=1000;
int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t BUFFER[14];
int state=0;
float angle1x=0.0;
float angle1y=0.0;
float angle1z=0.0;
float angle2x=0.0;
float angle2y=0.0;
float angle2z=0.0;
double fax=0.0,faz=0.0,fay=0.0,fd=0.0;
float theta;
#define DUTY_MAX 20
bool tripped=false;
bool flash=true;
unsigned int duty_counter=0,duty_cycleR=0,duty_cycleG=0,duty_cycleB=0;
///////////////////////////////////////////////////////////////////////////
void loop() {
// Serial.print(duty_counter%DUTY_MAX); Serial.print("\t");
// duty_counter++;
//////////////////////////////////////////////
fax=0.0;faz=0.0;fay=0.0;counter=0;
do{
tripped=false;
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
if(state>=0){
ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
if(!tripped){
counter++;
fax+=ax;
fay+=ay;
faz+=az;
}
// Serial.print(ax); Serial.print("\t");
// Serial.print(ay); Serial.print("\t");
// Serial.print(az); Serial.print("\t");
// Serial.print( sqrt(faz*faz+fax*fax+fay*fay)); Serial.print("\t\t");
}while(counter<N);
fax=fax/float(N);
fay=fay/float(N);
faz=faz/float(N);
angle1y=180.0/M_PI*atan(fay/faz);
angle1x=180.0/M_PI*atan(-fax/sqrt(fay*fay+faz*faz));
////////////////////////////
// Serial.print(fax); Serial.print("\t");
// Serial.print(fay); Serial.print("\t");
// Serial.print(faz); Serial.print("\t");
// Serial.print(sqrt(fax*fax+fay*fay+faz*faz)-16356.0); Serial.println("");
////////////////////////////////////////////////////////////////////////////////////////////////
do{
A2x=0.0;A2y=0.0;A2z=0.0;
counter_o=0.0;
do{
A1x=0.0;A1y=0.0;A1z=0.0;
counter_m=0;fdt=0.0;
do{
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);//
if(state>=0){
ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_YOUT_H,2, BUFFER);//
if(state>=0){
gy= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_XOUT_H,2, BUFFER);//
if(state>=0){
gx= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_ZOUT_H,2, BUFFER);//
if(state>=0){
gz= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
if(!tripped){
if(dt!=0)fdt+=(micros()-dt)/1000000.0;
else fdt=0.0;
dt=micros();
counter_m++;
A1x+=gx;
A1y+=gy;
A1z+=gz;
}
}while(counter_m<M);
A1x/=1.0*M;
A1y/=1.0*M;
A1z/=1.0*M;
if(calc_A3 && (abs(A1x-A3x)>15.0 || abs(A1y-A3y)>15.0 || abs(A1z-A3z)>50.0 )){
angle1x+=fdt*A1x/131.0;
angle1y+=fdt*A1y/131.0;
angle1z+=fdt*A1z/131.0;
// Serial.print("*");
//counter_o=0;A2x=0.0;A2y=0.0;A2z=0.0;
}
else{
A2x+=A1x;
A2y+=A1y;
A2z+=A1z;
counter_o++;
}
/// Serial.print(abs(A1x-A3x)); Serial.print("\t");
// Serial.print(abs(A1y-A3y)); Serial.print("\t");
// Serial.print(abs(A1z-A3z));Serial.print("\t");
// Serial.print(A3x); Serial.print("\t");
// Serial.print(A3y); Serial.print("\t");
// Serial.print(A3z);Serial.print("\t");
Serial.print(angle1x); Serial.print("\t");
Serial.print(angle1y); Serial.print("\t");
Serial.print(angle1z);Serial.print("\n");
// Serial.print(fdt*1000);Serial.println("");
}while(counter_o<O);
A3x=A2x/(1.0*O);
A3y=A2y/(1.0*O);
A3z=A2z/(1.0*O);
calc_A3=true;
}while(true);
}
Opengl gyro_cube.c
/* Copyright (c) Mark J. Kilgard, 1997. */
/* This program is freely distributable without licensing fees
and is provided without guarantee or warrantee expressed or
implied. This program is -not- in the public domain. */
/* This program was requested by Patrick Earl; hopefully someone else
will write the equivalent Direct3D immediate mode program. */
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <GL/glut.h>
GLfloat light_diffuse[] = {1.0, 0.0, 0.0, 1.0}; /* Red diffuse light. */
GLfloat light_position[] = {1.0, 5.0, -1.0, 0.0}; /* Infinite light location. */
GLfloat n[6][3] = { /* Normals for the 6 faces of a cube. */
{-1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0},
{0.0, -1.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 0.0, -1.0} };
GLint faces[6][4] = { /* Vertex indices for the 6 faces of a cube. */
{0, 1, 2, 3}, {3, 2, 6, 7}, {7, 6, 5, 4},
{4, 5, 1, 0}, {5, 6, 2, 1}, {7, 4, 0, 3} };
GLfloat v[8][3]; /* Will be filled in with X,Y,Z vertexes. */
//serial descriptor
int fd;
char *portname = "/dev/ttyUSB0";
int
set_interface_attribs (int fd, int speed, int parity)
{
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
// error_message ("error %d from tcgetattr", errno);
return -1;
}
cfsetospeed (&tty, speed);
cfsetispeed (&tty, speed);
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
// disable IGNBRK for mismatched speed tests; otherwise receive break
// as \000 chars
tty.c_iflag &= ~IGNBRK; // disable break processing
tty.c_lflag = 0; // no signaling chars, no echo,
// no canonical processing
tty.c_oflag = 0; // no remapping, no delays
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
// enable reading
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
tty.c_cflag |= parity;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS;
if (tcsetattr (fd, TCSANOW, &tty) != 0)
{
//error_message ("error %d from tcsetattr", errno);
return -1;
}
return 0;
}
void
set_blocking (int fd, int should_block)
{
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
//error_message ("error %d from tggetattr", errno);
return;
}
tty.c_cc[VMIN] = should_block ? 1 : 0;
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tcsetattr (fd, TCSANOW, &tty);
}
void
drawBox(void)
{
int i;
for (i = 0; i < 6; i++) {
glBegin(GL_QUADS);
glNormal3fv(&n[i][0]);
glVertex3fv(&v[faces[i][0]][0]);
glVertex3fv(&v[faces[i][1]][0]);
glVertex3fv(&v[faces[i][2]][0]);
glVertex3fv(&v[faces[i][3]][0]);
glEnd();
}
}
///////////////////////////////////////////////////////////////////////////
void
display(void)
{
char buffer1[100],buffer2[100],buffer3[100];
float A1x=0.0,A1y=0.0,A1z=0.0;
char line[200];
char *pch;
char *pch1;
char *pch2;
char in;
ssize_t length;
int i=0;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
printf("START\n");
do{
length = read(fd, &in, 1);
//printf("%d",length);
if(length==1){
line[i]=in;
i++;
}
//printf("%d",i);
}while( in!='\n');
line[i]='\0';
//printf("%s\n",line);
pch=strchr(line,'\t');
strncpy(buffer1,line,pch-line+1);
A1x=atof(buffer1);
pch1=strchr(pch+1,'\t');
strncpy(buffer2,pch,pch1-pch+1);
A1y=atof(buffer2);
pch2=strchr(pch1+1,'\n');
strncpy(buffer3,pch1,pch2-pch1+1);
A1z=atof(buffer3);
printf("%g\t%g\t%g\n",A1x,A1y,A1z);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0.0, 0.0, 5.0, /* eye is at (0,0,5) */
0.0, 0.0, 0.0, /* center is at (0,0,0) */
0.0, 1.0, 0.); /* up is in positive Y direction */
//glTranslatef(0.0, 0.0, 1.0);
glRotatef(A1z, 0.0, 1.0, 0.0);
glRotatef(-A1x+90, 1.0, 0.0, 0.0);
drawBox();
glutSwapBuffers();
glutPostRedisplay();
}
////////////////////////////////////////////////////////////////////////////
void
init(void)
{
/* Setup cube vertex data. */
v[0][0] = v[1][0] = v[2][0] = v[3][0] = -1;
v[4][0] = v[5][0] = v[6][0] = v[7][0] = 1;
v[0][1] = v[1][1] = v[4][1] = v[5][1] = -1;
v[2][1] = v[3][1] = v[6][1] = v[7][1] = 1;
v[0][2] = v[3][2] = v[4][2] = v[7][2] = 1;
v[1][2] = v[2][2] = v[5][2] = v[6][2] = -1;
/* Enable a single OpenGL light. */
glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHTING);
/* Use depth buffering for hidden surface elimination. */
glEnable(GL_DEPTH_TEST);
/* Setup the view of the cube. */
glMatrixMode(GL_PROJECTION);
gluPerspective( /* field of view in degree */ 40.0,
/* aspect ratio */ 1.0,
/* Z near */ 1.0, /* Z far */ 10.0);
glMatrixMode(GL_MODELVIEW);
gluLookAt(0.0, 0.0, 5.0, /* eye is at (0,0,5) */
0.0, 0.0, 0.0, /* center is at (0,0,0) */
0.0, 1.0, 0.); /* up is in positive Y direction */
/* Adjust cube position to be asthetic angle. */
//glTranslatef(0.0, 0.0, -1.0);
//glRotatef(60, 1.0, 0.0, 0.0);
//glRotatef(-20, 0.0, 0.0, 1.0);
}
int
main(int argc, char **argv)
{
fd = open (portname, O_RDWR | O_NOCTTY | O_SYNC);
if (fd < 0)
{
printf ("error opening port\n");
return 1;
}
printf("port opened...");
set_interface_attribs (fd, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity)
set_blocking (fd, 0);
//printf("Writing port...\n");
//write (fd, "\n", 2); // send 7 character greeting
//usleep ((2) * 100);
printf("init glut...");
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
glutCreateWindow("red 3D lighted cube");
glutDisplayFunc(display);
init();
glutMainLoop();
return 0; /* ANSI C requires main to return int. */
}
Subscribe to:
Posts (Atom)