libgeometry revamp

1 files changed, 341 insertions, 99 deletions

diff --git a/sys/src/libgeometry/matrix.c b/sys/src/libgeometry/matrix.c
index cf7a6c152..d640403a7 100644
--- a/sys/src/libgeometry/matrix.c
+++ b/sys/src/libgeometry/matrix.c
@@ -1,106 +1,348 @@
-/*
- * ident(m)		store identity matrix in m
- * matmul(a, b)		matrix multiply a*=b
- * matmulr(a, b)	matrix multiply a=b*a
- * determinant(m)	returns det(m)
- * adjoint(m, minv)	minv=adj(m)
- * invertmat(m, minv)	invert matrix m, result in minv, returns det(m)
- *			if m is singular, minv=adj(m)
- */
 #include <u.h>
 #include <libc.h>
-#include <draw.h>
 #include <geometry.h>
-void ident(Matrix m){
-	register double *s=&m[0][0];
-	*s++=1;*s++=0;*s++=0;*s++=0;
-	*s++=0;*s++=1;*s++=0;*s++=0;
-	*s++=0;*s++=0;*s++=1;*s++=0;
-	*s++=0;*s++=0;*s++=0;*s=1;
-}
-void matmul(Matrix a, Matrix b){
-	register i, j, k;
-	double sum;
+
+/* 2D */
+
+void
+identity(Matrix m)
+{
+	memset(m, 0, 3*3*sizeof(double));
+	m[0][0] = m[1][1] = m[2][2] = 1;
+}
+
+void
+addm(Matrix a, Matrix b)
+{
+	int i, j;
+
+	for(i = 0; i < 3; i++)
+		for(j = 0; j < 3; j++)
+			a[i][j] += b[i][j];
+}
+
+void
+subm(Matrix a, Matrix b)
+{
+	int i, j;
+
+	for(i = 0; i < 3; i++)
+		for(j = 0; j < 3; j++)
+			a[i][j] -= b[i][j];
+}
+
+void
+mulm(Matrix a, Matrix b)
+{
+	int i, j, k;
 	Matrix tmp;
-	for(i=0;i!=4;i++) for(j=0;j!=4;j++){
-		sum=0;
-		for(k=0;k!=4;k++)
-			sum+=a[i][k]*b[k][j];
-		tmp[i][j]=sum;
-	}
-	for(i=0;i!=4;i++) for(j=0;j!=4;j++)
-		a[i][j]=tmp[i][j];
-}
-void matmulr(Matrix a, Matrix b){
-	register i, j, k;
-	double sum;
+
+	for(i = 0; i < 3; i++)
+		for(j = 0; j < 3; j++){
+			tmp[i][j] = 0;
+			for(k = 0; k < 3; k++)
+				tmp[i][j] += a[i][k]*b[k][j];
+		}
+	memmove(a, tmp, 3*3*sizeof(double));
+}
+
+void
+smulm(Matrix m, double s)
+{
+	int i, j;
+
+	for(i = 0; i < 3; i++)
+		for(j = 0; j < 3; j++)
+			m[i][j] *= s;
+}
+
+void
+transposem(Matrix m)
+{
+	int i, j;
+	double tmp;
+
+	for(i = 0; i < 3; i++)
+		for(j = i; j < 3; j++){
+			tmp = m[i][j];
+			m[i][j] = m[j][i];
+			m[j][i] = tmp;
+		}
+}
+
+double
+detm(Matrix m)
+{
+	return m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1])+
+	       m[0][1]*(m[1][2]*m[2][0] - m[1][0]*m[2][2])+
+	       m[0][2]*(m[1][0]*m[2][1] - m[1][1]*m[2][0]);
+}
+
+double
+tracem(Matrix m)
+{
+	return m[0][0] + m[1][1] + m[2][2];
+}
+
+void
+adjm(Matrix m)
+{
 	Matrix tmp;
-	for(i=0;i!=4;i++) for(j=0;j!=4;j++){
-		sum=0;
-		for(k=0;k!=4;k++)
-			sum+=b[i][k]*a[k][j];
-		tmp[i][j]=sum;
-	}
-	for(i=0;i!=4;i++) for(j=0;j!=4;j++)
-		a[i][j]=tmp[i][j];
-}
-/*
- * Return det(m)
- */
-double determinant(Matrix m){
-	return m[0][0]*(m[1][1]*(m[2][2]*m[3][3]-m[2][3]*m[3][2])+
-			m[1][2]*(m[2][3]*m[3][1]-m[2][1]*m[3][3])+
-			m[1][3]*(m[2][1]*m[3][2]-m[2][2]*m[3][1]))
-	      -m[0][1]*(m[1][0]*(m[2][2]*m[3][3]-m[2][3]*m[3][2])+
-			m[1][2]*(m[2][3]*m[3][0]-m[2][0]*m[3][3])+
-			m[1][3]*(m[2][0]*m[3][2]-m[2][2]*m[3][0]))
-	      +m[0][2]*(m[1][0]*(m[2][1]*m[3][3]-m[2][3]*m[3][1])+
-			m[1][1]*(m[2][3]*m[3][0]-m[2][0]*m[3][3])+
-			m[1][3]*(m[2][0]*m[3][1]-m[2][1]*m[3][0]))
-	      -m[0][3]*(m[1][0]*(m[2][1]*m[3][2]-m[2][2]*m[3][1])+
-			m[1][1]*(m[2][2]*m[3][0]-m[2][0]*m[3][2])+
-			m[1][2]*(m[2][0]*m[3][1]-m[2][1]*m[3][0]));
-}
-/*
- * Store the adjoint (matrix of cofactors) of m in madj.
- * Works fine even if m and madj are the same matrix.
- */
-void adjoint(Matrix m, Matrix madj){
-	double m00=m[0][0], m01=m[0][1], m02=m[0][2], m03=m[0][3];
-	double m10=m[1][0], m11=m[1][1], m12=m[1][2], m13=m[1][3];
-	double m20=m[2][0], m21=m[2][1], m22=m[2][2], m23=m[2][3];
-	double m30=m[3][0], m31=m[3][1], m32=m[3][2], m33=m[3][3];
-	madj[0][0]=m11*(m22*m33-m23*m32)+m21*(m13*m32-m12*m33)+m31*(m12*m23-m13*m22);
-	madj[0][1]=m01*(m23*m32-m22*m33)+m21*(m02*m33-m03*m32)+m31*(m03*m22-m02*m23);
-	madj[0][2]=m01*(m12*m33-m13*m32)+m11*(m03*m32-m02*m33)+m31*(m02*m13-m03*m12);
-	madj[0][3]=m01*(m13*m22-m12*m23)+m11*(m02*m23-m03*m22)+m21*(m03*m12-m02*m13);
-	madj[1][0]=m10*(m23*m32-m22*m33)+m20*(m12*m33-m13*m32)+m30*(m13*m22-m12*m23);
-	madj[1][1]=m00*(m22*m33-m23*m32)+m20*(m03*m32-m02*m33)+m30*(m02*m23-m03*m22);
-	madj[1][2]=m00*(m13*m32-m12*m33)+m10*(m02*m33-m03*m32)+m30*(m03*m12-m02*m13);
-	madj[1][3]=m00*(m12*m23-m13*m22)+m10*(m03*m22-m02*m23)+m20*(m02*m13-m03*m12);
-	madj[2][0]=m10*(m21*m33-m23*m31)+m20*(m13*m31-m11*m33)+m30*(m11*m23-m13*m21);
-	madj[2][1]=m00*(m23*m31-m21*m33)+m20*(m01*m33-m03*m31)+m30*(m03*m21-m01*m23);
-	madj[2][2]=m00*(m11*m33-m13*m31)+m10*(m03*m31-m01*m33)+m30*(m01*m13-m03*m11);
-	madj[2][3]=m00*(m13*m21-m11*m23)+m10*(m01*m23-m03*m21)+m20*(m03*m11-m01*m13);
-	madj[3][0]=m10*(m22*m31-m21*m32)+m20*(m11*m32-m12*m31)+m30*(m12*m21-m11*m22);
-	madj[3][1]=m00*(m21*m32-m22*m31)+m20*(m02*m31-m01*m32)+m30*(m01*m22-m02*m21);
-	madj[3][2]=m00*(m12*m31-m11*m32)+m10*(m01*m32-m02*m31)+m30*(m02*m11-m01*m12);
-	madj[3][3]=m00*(m11*m22-m12*m21)+m10*(m02*m21-m01*m22)+m20*(m01*m12-m02*m11);
-}
-/*
- * Store the inverse of m in minv.
- * If m is singular, minv is instead its adjoint.
- * Returns det(m).
- * Works fine even if m and minv are the same matrix.
- */
-double invertmat(Matrix m, Matrix minv){
-	double d, dinv;
+
+	tmp[0][0] =  m[1][1]*m[2][2] - m[1][2]*m[2][1];
+	tmp[0][1] = -m[0][1]*m[2][2] + m[0][2]*m[2][1];
+	tmp[0][2] =  m[0][1]*m[1][2] - m[0][2]*m[1][1];
+	tmp[1][0] = -m[1][0]*m[2][2] + m[1][2]*m[2][0];
+	tmp[1][1] =  m[0][0]*m[2][2] - m[0][2]*m[2][0];
+	tmp[1][2] = -m[0][0]*m[1][2] + m[0][2]*m[1][0];
+	tmp[2][0] =  m[1][0]*m[2][1] - m[1][1]*m[2][0];
+	tmp[2][1] = -m[0][0]*m[2][1] + m[0][1]*m[2][0];
+	tmp[2][2] =  m[0][0]*m[1][1] - m[0][1]*m[1][0];
+	memmove(m, tmp, 3*3*sizeof(double));
+}
+
+/* Cayley-Hamilton */
+//void
+//invertm(Matrix m)
+//{
+//	Matrix m², r;
+//	double det, trm, trm²;
+//
+//	det = detm(m);
+//	if(det == 0)
+//		return;
+//	trm = tracem(m);
+//	memmove(m², m, 3*3*sizeof(double));
+//	mulm(m², m²);
+//	trm² = tracem(m²);
+//	identity(r);
+//	smulm(r, (trm*trm - trm²)/2);
+//	smulm(m, trm);
+//	subm(r, m);
+//	addm(r, m²);
+//	smulm(r, 1/det);
+//	memmove(m, r, 3*3*sizeof(double));
+//}
+
+/* Cramer's */
+void
+invm(Matrix m)
+{
+	double det;
+
+	det = detm(m);
+	if(det == 0)
+		return; /* singular matrices are not invertible */
+	adjm(m);
+	smulm(m, 1/det);
+}
+
+Point2
+xform(Point2 p, Matrix m)
+{
+	return (Point2){
+		p.x*m[0][0] + p.y*m[0][1] + p.w*m[0][2],
+		p.x*m[1][0] + p.y*m[1][1] + p.w*m[1][2],
+		p.x*m[2][0] + p.y*m[2][1] + p.w*m[2][2]
+	};
+}
+
+/* 3D */
+
+void
+identity3(Matrix3 m)
+{
+	memset(m, 0, 4*4*sizeof(double));
+	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1;
+}
+
+void
+addm3(Matrix3 a, Matrix3 b)
+{
+	int i, j;
+
+	for(i = 0; i < 4; i++)
+		for(j = 0; j < 4; j++)
+			a[i][j] += b[i][j];
+}
+
+void
+subm3(Matrix3 a, Matrix3 b)
+{
+	int i, j;
+
+	for(i = 0; i < 4; i++)
+		for(j = 0; j < 4; j++)
+			a[i][j] -= b[i][j];
+}
+
+void
+mulm3(Matrix3 a, Matrix3 b)
+{
+	int i, j, k;
+	Matrix3 tmp;
+
+	for(i = 0; i < 4; i++)
+		for(j = 0; j < 4; j++){
+			tmp[i][j] = 0;
+			for(k = 0; k < 4; k++)
+				tmp[i][j] += a[i][k]*b[k][j];
+		}
+	memmove(a, tmp, 4*4*sizeof(double));
+}
+
+void
+smulm3(Matrix3 m, double s)
+{
 	int i, j;
-	d=determinant(m);
-	adjoint(m, minv);
-	if(d!=0.){
-		dinv=1./d;
-		for(i=0;i!=4;i++) for(j=0;j!=4;j++) minv[i][j]*=dinv;
-	}
-	return d;
+
+	for(i = 0; i < 4; i++)
+		for(j = 0; j < 4; j++)
+			m[i][j] *= s;
+}
+
+void
+transposem3(Matrix3 m)
+{
+	int i, j;
+	double tmp;
+
+	for(i = 0; i < 4; i++)
+		for(j = i; j < 4; j++){
+			tmp = m[i][j];
+			m[i][j] = m[j][i];
+			m[j][i] = tmp;
+		}
+}
+
+double
+detm3(Matrix3 m)
+{
+	return m[0][0]*(m[1][1]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+
+			m[1][2]*(m[2][3]*m[3][1] - m[2][1]*m[3][3])+
+			m[1][3]*(m[2][1]*m[3][2] - m[2][2]*m[3][1]))
+	      -m[0][1]*(m[1][0]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+
+			m[1][2]*(m[2][3]*m[3][0] - m[2][0]*m[3][3])+
+			m[1][3]*(m[2][0]*m[3][2] - m[2][2]*m[3][0]))
+	      +m[0][2]*(m[1][0]*(m[2][1]*m[3][3] - m[2][3]*m[3][1])+
+			m[1][1]*(m[2][3]*m[3][0] - m[2][0]*m[3][3])+
+			m[1][3]*(m[2][0]*m[3][1] - m[2][1]*m[3][0]))
+	      -m[0][3]*(m[1][0]*(m[2][1]*m[3][2] - m[2][2]*m[3][1])+
+			m[1][1]*(m[2][2]*m[3][0] - m[2][0]*m[3][2])+
+			m[1][2]*(m[2][0]*m[3][1] - m[2][1]*m[3][0]));
+}
+
+double
+tracem3(Matrix3 m)
+{
+	return m[0][0] + m[1][1] + m[2][2] + m[3][3];
+}
+
+void
+adjm3(Matrix3 m)
+{
+	Matrix3 tmp;
+
+	tmp[0][0]=m[1][1]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+
+		  m[2][1]*(m[1][3]*m[3][2] - m[1][2]*m[3][3])+
+		  m[3][1]*(m[1][2]*m[2][3] - m[1][3]*m[2][2]);
+	tmp[0][1]=m[0][1]*(m[2][3]*m[3][2] - m[2][2]*m[3][3])+
+		  m[2][1]*(m[0][2]*m[3][3] - m[0][3]*m[3][2])+
+		  m[3][1]*(m[0][3]*m[2][2] - m[0][2]*m[2][3]);
+	tmp[0][2]=m[0][1]*(m[1][2]*m[3][3] - m[1][3]*m[3][2])+
+		  m[1][1]*(m[0][3]*m[3][2] - m[0][2]*m[3][3])+
+		  m[3][1]*(m[0][2]*m[1][3] - m[0][3]*m[1][2]);
+	tmp[0][3]=m[0][1]*(m[1][3]*m[2][2] - m[1][2]*m[2][3])+
+		  m[1][1]*(m[0][2]*m[2][3] - m[0][3]*m[2][2])+
+		  m[2][1]*(m[0][3]*m[1][2] - m[0][2]*m[1][3]);
+	tmp[1][0]=m[1][0]*(m[2][3]*m[3][2] - m[2][2]*m[3][3])+
+		  m[2][0]*(m[1][2]*m[3][3] - m[1][3]*m[3][2])+
+		  m[3][0]*(m[1][3]*m[2][2] - m[1][2]*m[2][3]);
+	tmp[1][1]=m[0][0]*(m[2][2]*m[3][3] - m[2][3]*m[3][2])+
+		  m[2][0]*(m[0][3]*m[3][2] - m[0][2]*m[3][3])+
+		  m[3][0]*(m[0][2]*m[2][3] - m[0][3]*m[2][2]);
+	tmp[1][2]=m[0][0]*(m[1][3]*m[3][2] - m[1][2]*m[3][3])+
+		  m[1][0]*(m[0][2]*m[3][3] - m[0][3]*m[3][2])+
+		  m[3][0]*(m[0][3]*m[1][2] - m[0][2]*m[1][3]);
+	tmp[1][3]=m[0][0]*(m[1][2]*m[2][3] - m[1][3]*m[2][2])+
+		  m[1][0]*(m[0][3]*m[2][2] - m[0][2]*m[2][3])+
+		  m[2][0]*(m[0][2]*m[1][3] - m[0][3]*m[1][2]);
+	tmp[2][0]=m[1][0]*(m[2][1]*m[3][3] - m[2][3]*m[3][1])+
+		  m[2][0]*(m[1][3]*m[3][1] - m[1][1]*m[3][3])+
+		  m[3][0]*(m[1][1]*m[2][3] - m[1][3]*m[2][1]);
+	tmp[2][1]=m[0][0]*(m[2][3]*m[3][1] - m[2][1]*m[3][3])+
+		  m[2][0]*(m[0][1]*m[3][3] - m[0][3]*m[3][1])+
+		  m[3][0]*(m[0][3]*m[2][1] - m[0][1]*m[2][3]);
+	tmp[2][2]=m[0][0]*(m[1][1]*m[3][3] - m[1][3]*m[3][1])+
+		  m[1][0]*(m[0][3]*m[3][1] - m[0][1]*m[3][3])+
+		  m[3][0]*(m[0][1]*m[1][3] - m[0][3]*m[1][1]);
+	tmp[2][3]=m[0][0]*(m[1][3]*m[2][1] - m[1][1]*m[2][3])+
+		  m[1][0]*(m[0][1]*m[2][3] - m[0][3]*m[2][1])+
+		  m[2][0]*(m[0][3]*m[1][1] - m[0][1]*m[1][3]);
+	tmp[3][0]=m[1][0]*(m[2][2]*m[3][1] - m[2][1]*m[3][2])+
+		  m[2][0]*(m[1][1]*m[3][2] - m[1][2]*m[3][1])+
+		  m[3][0]*(m[1][2]*m[2][1] - m[1][1]*m[2][2]);
+	tmp[3][1]=m[0][0]*(m[2][1]*m[3][2] - m[2][2]*m[3][1])+
+		  m[2][0]*(m[0][2]*m[3][1] - m[0][1]*m[3][2])+
+		  m[3][0]*(m[0][1]*m[2][2] - m[0][2]*m[2][1]);
+	tmp[3][2]=m[0][0]*(m[1][2]*m[3][1] - m[1][1]*m[3][2])+
+		  m[1][0]*(m[0][1]*m[3][2] - m[0][2]*m[3][1])+
+		  m[3][0]*(m[0][2]*m[1][1] - m[0][1]*m[1][2]);
+	tmp[3][3]=m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1])+
+		  m[1][0]*(m[0][2]*m[2][1] - m[0][1]*m[2][2])+
+		  m[2][0]*(m[0][1]*m[1][2] - m[0][2]*m[1][1]);
+	memmove(m, tmp, 4*4*sizeof(double));
+}
+
+/* Cayley-Hamilton */
+//void
+//invertm3(Matrix3 m)
+//{
+//	Matrix3 m², m³, r;
+//	double det, trm, trm², trm³;
+//
+//	det = detm3(m);
+//	if(det == 0)
+//		return;
+//	trm = tracem3(m);
+//	memmove(m³, m, 4*4*sizeof(double));
+//	mulm(m³, m³);
+//	mulm(m³, m);
+//	trm³ = tracem3(m³);
+//	memmove(m², m, 4*4*sizeof(double));
+//	mulm(m², m²);
+//	trm² = tracem3(m²);
+//	identity3(r);
+//	smulm3(r, (trm*trm*trm - 3*trm*trm² + 2*trm³)/6);
+//	smulm3(m, (trm*trm - trm²)/2);
+//	smulm3(m², trm);
+//	subm(r, m);
+//	addm(r, m²);
+//	subm(r, m³);
+//	smulm(r, 1/det);
+//	memmove(m, r, 4*4*sizeof(double));
+//}
+
+/* Cramer's */
+void
+invm3(Matrix3 m)
+{
+	double det;
+
+	det = detm3(m);
+	if(det == 0)
+		return; /* singular matrices are not invertible */
+	adjm3(m);
+	smulm3(m, 1/det);
+}
+
+Point3
+xform3(Point3 p, Matrix3 m)
+{
+	return (Point3){
+		p.x*m[0][0] + p.y*m[0][1] + p.z*m[0][2] + p.w*m[0][3],
+		p.x*m[1][0] + p.y*m[1][1] + p.z*m[1][2] + p.w*m[1][3],
+		p.x*m[2][0] + p.y*m[2][1] + p.z*m[2][2] + p.w*m[2][3],
+		p.x*m[3][0] + p.y*m[3][1] + p.z*m[3][2] + p.w*m[3][3],
+	};
 }