#include <stdio.h>
#include <string.h>
#include <math.h>
#include <dist.h>

Frame3	iframe3[1] = {1.0, 0.0, 0.0,
		      0.0, 1.0, 0.0,
		      0.0, 0.0, 1.0,
		      0.0, 0.0, 0.0};

void
ftrans3(Frame3 *d, Point3 p)
{
	d->r[0][0] = d->r[1][1] = d->r[2][2] = 1.0;
	d->r[1][0] = d->r[0][1] = d->r[2][0] = 0.0;
	d->r[0][2] = d->r[1][2] = d->r[2][1] = 0.0;
	d->p = p;
}

void
frot3(Frame3 *d, Point3 p, double t)
{
	double norm, c, s, v;
	double x, y, z;

	x = p.x;
	y = p.y;
	z = p.z;

	norm = sqrt(x*x + y*y + z*z);
	if (norm == 0.0) {
		memcpy(d, iframe3, sizeof(iframe3));
		return;
	}
	
	x /= norm; y /= norm; z /= norm;

	c = cos(t);
	s = sin(t);
	v = 1 - c;

	d->r[0][0] = x*x*v + c;
	d->r[1][0] = x*y*v + z*s;
	d->r[2][0] = x*z*v - y*s;
	d->r[0][1] = x*y*v - z*s;
	d->r[1][1] = y*y*v + c;
	d->r[2][1] = y*z*v + x*s;
	d->r[0][2] = x*z*v + y*s;
	d->r[1][2] = y*z*v - x*s;
	d->r[2][2] = z*z*v + c;
	
	d->p.x = d->p.y = d->p.z = 0;
}

void
frame3(Frame3 *f, Point3 p, Point3 w, double t)
{
	frot3(f, w, t);

	f->p = p;
}

void
finv3(Frame3 *d, Frame3 *s)
{
	Frame3 f;

	/* transpe ration section */

	f.r[0][0] = s->r[0][0];
	f.r[1][1] = s->r[1][1];
	f.r[2][2] = s->r[2][2];
	f.r[1][0] = s->r[0][1];
	f.r[0][1] = s->r[1][0];
	f.r[2][0] = s->r[0][2];
	f.r[0][2] = s->r[2][0];
	f.r[2][1] = s->r[1][2];
	f.r[1][2] = s->r[2][1];

	/* translation portion is -(R sup t) * p */

	f.p.x = -(f.r[0][0]*s->p.x +
		      f.r[0][1]*s->p.y +
		      f.r[0][2]*s->p.z);
	f.p.y = -(f.r[1][0]*s->p.x +
		      f.r[1][1]*s->p.y +
		      f.r[1][2]*s->p.z);
	f.p.z = -(f.r[2][0]*s->p.x +
		      f.r[2][1]*s->p.y +
		      f.r[2][2]*s->p.z);

	memcpy(d, &f, sizeof(f));
}

void
fmul3(Frame3 *d, Frame3 *s1, Frame3 *s2)
{
	Frame3 f;
	int i;
	double *p;

	p = (double *)&f.p;

	for (i = 0; i < 3; ++i) {
		f.r[i][0] = s1->r[i][0]*s2->r[0][0] +
				s1->r[i][1]*s2->r[1][0] +
				s1->r[i][2]*s2->r[2][0];
		f.r[i][1] = s1->r[i][0]*s2->r[0][1] +
				s1->r[i][1]*s2->r[1][1] +
				s1->r[i][2]*s2->r[2][1];
		f.r[i][2] = s1->r[i][0]*s2->r[0][2] +
				s1->r[i][1]*s2->r[1][2] +
				s1->r[i][2]*s2->r[2][2];
		p[i] = s1->r[i][0]*s2->p.x +
			   s1->r[i][1]*s2->p.y +
			   s1->r[i][2]*s2->p.z;
	}

	p[0] += s1->p.x;
	p[1] += s1->p.y;
	p[2] += s1->p.z;

	memcpy(d, &f, sizeof(f));
}

Point3
fmap3(Frame3 *f, Point3 p)
{
	Point3 p3;

	p3.x = f->r[0][0]*p.x + f->r[0][1]*p.y + f->r[0][2]*p.z;
	p3.y = f->r[1][0]*p.x + f->r[1][1]*p.y + f->r[1][2]*p.z;
	p3.z = f->r[2][0]*p.x + f->r[2][1]*p.y + f->r[2][2]*p.z;
	
	p3.x += f->p.x;
	p3.y += f->p.y;
	p3.z += f->p.z;

	return p3;
}

Point3
fimap3(Frame3 *f, Point3 p)
{
	Point3 p3;
	
	p.x -= f->p.x;	p.y -= f->p.y;	p.z -= f->p.z;

	p3.x = f->r[0][0]*p.x + f->r[1][0]*p.y + f->r[2][0]*p.z;
	p3.y = f->r[0][1]*p.x + f->r[1][1]*p.y + f->r[2][1]*p.z;
	p3.z = f->r[0][2]*p.x + f->r[1][2]*p.y + f->r[2][2]*p.z;

	return p3;
}

void	
frand3(Frame3 *f)
{
	Point3 p, w;

	p = point3(frand(), frand(), frand());
	w = point3(frand(), frand(), frand());
	frame3(f, p, w, frand()*2*MYPI);
}