Arithmic Motion Mistake

testApp.cpp

#include "testApp.h"

#define DOT(a,b)		a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
#define LENGTH(p)		sqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]);
#define NORMALIZE(p)	{ 	float l = LENGTH(p); p[0] /= l; p[1] /= l; p[2] /= l; }
#define CROSS(a,b)		{	a[1] * b[2] - a[2] * b[1], \
						 	a[2] * b[0] - a[0] * b[2], \
						 	a[0] * b[1] - a[1] * b[0]};
#define SUBTRACT(a,b,d)	d[0] = a[0] - b[0]; d[1] = a[1] - b[1]; d[2] = a[2] - b[2];
#define SCALE(a, s)		{ NORMALIZE(a); a[0] *= s; a[1] *= s; a[2] *= s; }
#define	ADD(a,b, d)		d[0] = a[0] + b[0]; d[1] = a[1] + b[1]; d[2] = a[2] + b[2];
#define MULT(a,s)		a[0] *= s; a[1] *= s; a[2] *= s;
class Plane {
public:
	Plane(float pos[3], float s) 
	{
		float hs = s * 0.5;
		position[0] = pos[0];
		position[1] = pos[1];
		position[2] = pos[2];
	
		// bottom left
		vertices[0] = position[0]-hs; 
		vertices[1] = position[1]+hs;
		vertices[2] = position[2];
		
		// bottom right
		vertices[3] = position[0]+hs; 
		vertices[4] = position[1]+hs;
		vertices[5] = position[2];
		
		// top right
		vertices[6] = position[0]+hs; 
		vertices[7] = position[1]-hs;
		vertices[8] = position[2];
		
		// top left
		vertices[9] = position[0]-hs; 
		vertices[10] = position[1]-hs;
		vertices[11] = position[2];
	}
			
	void update() {
	}
	
	void draw(float* center) {

		float up[3] = {0,1,0};
		float dir[3] = {0,0,0};
		SUBTRACT(center,position, dir);
		NORMALIZE(dir);
		float right[3] = CROSS(dir, up);
		NORMALIZE(right);		
		float up_corrected[3] = CROSS(right, dir);
		NORMALIZE(up_corrected);
		MULT(dir, -1);

		mat[0] = right[0];
		mat[1] = right[1];
		mat[2] = right[2];
		
		mat[4] = up_corrected[0];
		mat[5] = up_corrected[1];
		mat[6] = up_corrected[2];
		
		mat[8] = dir[0];
		mat[9] = dir[1];
		mat[10] = dir[2];
		
		mat[12] = position[0];
		mat[13] = position[1];
		mat[14] = position[2];
		mat[12] = mat[13] = mat[14] = 0;
		
		mat[3] = mat[7] = mat[11] = 0;
		mat[15] = 1.0f;
		
		SCALE(right, 0.5);
		SCALE(up_corrected, 0.5);
		SCALE(dir, 0.5);
		
		
		glPushMatrix();
			glMatrixMode(GL_MODELVIEW);
			glMultMatrixf(mat);
			glColor3f(0.5, 0.0, 0.3);
			glBegin(GL_QUADS);
				glVertex3fv(vertices);
				glVertex3fv(vertices+3);
				glVertex3fv(vertices+6);
				glVertex3fv(vertices+9);
			glEnd();
			
			float end[3] = {0,0,0};
			glBegin(GL_LINES);
				ADD(position, right, end);
				glColor3f(1,0,0);
				glVertex3fv(position);
				glVertex3fv(end);
				
				ADD(position, up_corrected, end);
				glColor3f(0,1,0);
				glVertex3fv(position);
				glVertex3fv(end);
				
				ADD(position, dir, end);
				glColor3f(0,1,1);
				glVertex3fv(position);
				glVertex3fv(end);
			glEnd();
		glPushMatrix();
	}
	
	float colors[12];
	float vertices[12];
	float position[3];
	float size;
	float mat[16];
};


vector<Plane*> planes;

//--------------------------------------------------------------
void testApp::setup(){
	ofBackground(33);
	ofSetFrameRate(60);
	ofSetVerticalSync(true);
	
	for(int i = 0; i < 11; ++i) {
		for(int j = 0; j < 41; ++j) {
			float position[3] = {-5+(i*0.6), -5+(j*0.1), 0};
			Plane* p = new Plane(position, 0.5);
			planes.push_back(p);
		}
	}
}

//--------------------------------------------------------------
void testApp::update(){
}

//--------------------------------------------------------------
void testApp::draw(){
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45.0f, 4.0f/3.0f, 0.1, 100);
	
	float time = ofGetElapsedTimef() * 0.6;
	float x = cos(time*0.5)*5;
	float y = sin(time*0.5)*5;
	float z = -30;
	float angle = ((1.0f+cos(time))*0.5)*90; // angle 0-90 degrees

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(x,y,z);
	glRotatef(angle,1,0,0);
	
	float center[3] = {x,y,z};
	for(int i = 0; i < planes.size(); ++i) {
		planes[i]->draw(center);
	}
	
	#ifdef OGG
		ogg.addFrame();
	#endif
}

//--------------------------------------------------------------
void testApp::keyPressed(int key){
	#ifdef OGG
	if(key == ' ') {
		ogg.enableRecording();
	}
	#endif
}

//--------------------------------------------------------------
void testApp::keyReleased(int key){

}

//--------------------------------------------------------------
void testApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void testApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void testApp::dragEvent(ofDragInfo dragInfo){ 

}