/*
 * The following routines transform points and planes from one space
 * to another.  Points and planes are represented by their
 * homogeneous coordinates, stored in variables of type Point3.
 */
#include <u.h>
#include <libc.h>
#include <draw.h>
#include <geometry.h>
/*
 * Transform point p.
 */
Point3 xformpoint(Point3 p, Space *to, Space *from){
	Point3 q, r;
	register double *m;
	if(from){
		m=&from->t[0][0];
		q.x=*m++*p.x; q.x+=*m++*p.y; q.x+=*m++*p.z; q.x+=*m++*p.w;
		q.y=*m++*p.x; q.y+=*m++*p.y; q.y+=*m++*p.z; q.y+=*m++*p.w;
		q.z=*m++*p.x; q.z+=*m++*p.y; q.z+=*m++*p.z; q.z+=*m++*p.w;
		q.w=*m++*p.x; q.w+=*m++*p.y; q.w+=*m++*p.z; q.w+=*m  *p.w;
	}
	else
		q=p;
	if(to){
		m=&to->tinv[0][0];
		r.x=*m++*q.x; r.x+=*m++*q.y; r.x+=*m++*q.z; r.x+=*m++*q.w;
		r.y=*m++*q.x; r.y+=*m++*q.y; r.y+=*m++*q.z; r.y+=*m++*q.w;
		r.z=*m++*q.x; r.z+=*m++*q.y; r.z+=*m++*q.z; r.z+=*m++*q.w;
		r.w=*m++*q.x; r.w+=*m++*q.y; r.w+=*m++*q.z; r.w+=*m  *q.w;
	}
	else
		r=q;
	return r;
}
/*
 * Transform point p with perspective division.
 */
Point3 xformpointd(Point3 p, Space *to, Space *from){
	p=xformpoint(p, to, from);
	if(p.w!=0){
		p.x/=p.w;
		p.y/=p.w;
		p.z/=p.w;
		p.w=1;
	}
	return p;
}
/*
 * Transform plane p -- same as xformpoint, except multiply on the
 * other side by the inverse matrix.
 */
Point3 xformplane(Point3 p, Space *to, Space *from){
	Point3 q, r;
	register double *m;
	if(from){
		m=&from->tinv[0][0];
		q.x =*m++*p.x; q.y =*m++*p.x; q.z =*m++*p.x; q.w =*m++*p.x;
		q.x+=*m++*p.y; q.y+=*m++*p.y; q.z+=*m++*p.y; q.w+=*m++*p.y;
		q.x+=*m++*p.z; q.y+=*m++*p.z; q.z+=*m++*p.z; q.w+=*m++*p.z;
		q.x+=*m++*p.w; q.y+=*m++*p.w; q.z+=*m++*p.w; q.w+=*m  *p.w;
	}
	else
		q=p;
	if(to){
		m=&to->t[0][0];
		r.x =*m++*q.x; r.y =*m++*q.x; r.z =*m++*q.x; r.w =*m++*q.x;
		r.x+=*m++*q.y; r.y+=*m++*q.y; r.z+=*m++*q.y; r.w+=*m++*q.y;
		r.x+=*m++*q.z; r.y+=*m++*q.z; r.z+=*m++*q.z; r.w+=*m++*q.z;
		r.x+=*m++*q.w; r.y+=*m++*q.w; r.z+=*m++*q.w; r.w+=*m  *q.w;
	}
	else
		r=q;
	return r;
}