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

Frame2	iframe2[1] = {1.0, 0.0,
		      0.0, 1.0,
		      0.0, 0.0};

void
ftrans2(Frame2 *d, Point2 p)
{
	d->r[0][0] = d->r[1][1] = 1.0;
	d->r[1][0] = d->r[0][1] = 0.0;
	d->p = p;
}

void
frot2(Frame2 *d, double t)
{
	double c, s;

	c = cos(t);
	s = sin(t);

	d->r[0][0] = d->r[1][1] = c;
	d->r[1][0] = s;
	d->r[0][1] = -s;
	d->p = point2(0,0);
}

void
frame2(Frame2 *d, Point2 p, double t)
{
	double c, s;

	c = cos(t);
	s = sin(t);
	d->r[0][0] = d->r[1][1] = c;
	d->r[1][0] = s;
	d->r[0][1] = -s;
	d->p = p;
}

void
finv2(Frame2 *d, Frame2 *s)
{
	Frame2 f;

	/* transpe rotation section */

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

	/* 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.p.y = -(f.r[1][0]*s->p.x +
		      f.r[1][1]*s->p.y);

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

void
fmul2(Frame2 *d, Frame2 *s1, Frame2 *s2)
{
	Frame2 f;
	int i;
	double *p;

	p = (double *)&f.p;

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

	f.p.x += s1->p.x;
	f.p.y += s1->p.y;

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

Point2
fmap2(Frame2 *f, Point2 p)
{
	Point2 p2;

	p2.x = f->r[0][0]*p.x + f->r[0][1]*p.y;
	p2.y = f->r[1][0]*p.x + f->r[1][1]*p.y;
	
	p2.x += f->p.x;	p2.y += f->p.y;
	return p2;
}

Point2
fimap2(Frame2 *f, Point2 p)
{
	Point2 p2;

	p.x -= f->p.x;	p.y -= f->p.y;

	p2.x = f->r[0][0]*p.x + f->r[1][0]*p.y;
	p2.y = f->r[0][1]*p.x + f->r[1][1]*p.y;
	
	return p2;
}