#include <math.h>
#include "render.h"

void GramSchmidt(PG_BASIS *b)
{
float l;

	/* normalize Z */
	l=mysqrt(dot(b->z,b->z));
	b->z.x=b->z.x/l;
	b->z.y=b->z.y/l;
	b->z.z=b->z.z/l;
	
	/* orthogonalize y */
	l=dot(b->y,b->z);
	b->y.x-=b->z.x*l;
	b->y.y-=b->z.y*l;
	b->y.z-=b->z.z*l;

	/* normalize y */
	l=mysqrt(dot(b->y,b->y));
	b->y.x=b->y.x/l;
	b->y.y=b->y.y/l;
	b->y.z=b->y.z/l;
	
	/* orthogonalize x */
	l=dot(b->x,b->z);
	b->x.x-=b->z.x*l;
	b->x.y-=b->z.y*l;
	b->x.z-=b->z.z*l;

	l=dot(b->x,b->y);
	b->x.x-=b->y.x*l;
	b->x.y-=b->y.y*l;
	b->x.z-=b->y.z*l;

	/* normalize y */
	l=mysqrt(dot(b->x,b->x));
	b->x.x=b->x.x/l;
	b->x.y=b->x.y/l;
	b->x.z=b->x.z/l;
	
}

void RotVect(PG_VECTOR *v,PG_VECTOR *w,float cosx,float sinx)
{
PG_VECTOR out;
float dp;
	dp=dot(*w,*v);
	out.x=(1.0-cosx)*dp*w->x + v->x*cosx - (w->y*v->z - w->z*v->y)*sinx;
	out.y=(1.0-cosx)*dp*w->y + v->y*cosx - (w->z*v->x - w->x*v->z)*sinx;
	out.z=(1.0-cosx)*dp*w->z + v->z*cosx - (w->x*v->y - w->y*v->x)*sinx;
	v->x=out.x;
	v->y=out.y;
	v->z=out.z;
}

void Rotate(PG_BASIS *b,PG_VECTOR *w)
{
float l,cosx,sinx;
PG_VECTOR w2;
	l=mysqrt(dot(*w,*w));
	if (l<0.0001)
		return;
	w2.x=w->x/l;
	w2.y=w->y/l;
	w2.z=w->z/l;
	cosx=mycos(l);
	sinx=mysin(l);
	RotVect(&(b->x),&w2,cosx,sinx);
	RotVect(&(b->y),&w2,cosx,sinx);
	RotVect(&(b->z),&w2,cosx,sinx);
}