3D geometry

1 files changed, 75 insertions, 0 deletions

diff --git a/src/libgeometry/transform.c b/src/libgeometry/transform.c
new file mode 100644
index 00000000..a5924872
--- /dev/null
+++ b/src/libgeometry/transform.c
@@ -0,0 +1,75 @@
+/*
+ * 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;
+}