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

Rot3	irot3 = {{1.0, 0.0, 0.0},
		  {0.0, 1.0, 0.0},
		  {0.0, 0.0, 1.0}};

void
rinv3(Rot3 d, Rot3 s)
{
	Rot3 r;

	r[0][0] = s[0][0];
	r[1][1] = s[1][1];
	r[2][2] = s[2][2];
	r[1][0] = s[0][1];
	r[0][1] = s[1][0];
	r[2][0] = s[0][2];
	r[0][2] = s[2][0];
	r[2][1] = s[1][2];
	r[1][2] = s[2][1];

	memmove(d, r, sizeof(r));
}

Point3
rmap3(Rot3 r, Point3 p)
{
	Point3 p3;

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

	return p3;
}

void
rmul3(Rot3 d, Rot3 s1, Rot3 s2)
{
	Rot3 r;
	int i;

	for (i = 0; i < 3; ++i) {
		r[i][0] = s1[i][0]*s2[0][0] +
				s1[i][1]*s2[1][0] +
				s1[i][2]*s2[2][0];
		r[i][1] = s1[i][0]*s2[0][1] +
				s1[i][1]*s2[1][1] +
				s1[i][2]*s2[2][1];
		r[i][2] = s1[i][0]*s2[0][2] +
				s1[i][1]*s2[1][2] +
				s1[i][2]*s2[2][2];
	}

	memcpy(d, r, sizeof(Rot3));
}

void
rrot3(Rot3 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, irot3, sizeof(Rot3));
		return;
	}
	
	x /= norm; y /= norm; z /= norm;

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

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

void	
rrand3(Rot3 r)
{
	Point3 w;

	w = point3(frand(), frand(), frand());
	rrot3(r, w, frand()*2*MYPI);
}