Rewrote some hard to read linear algebra code to use Eigen

12 files changed, 75 insertions, 125 deletions

diff --git a/src/CGALEvaluator.cc b/src/CGALEvaluator.cc
index 72d1f17..10160ae 100644
--- a/src/CGALEvaluator.cc
+++ b/src/CGALEvaluator.cc
@@ -206,8 +206,8 @@ Response CGALEvaluator::visit(State &state, const TransformNode &node)
 				// tesselate it and create a new CGAL_Nef_polyhedron2 from it.. What a hack!
 				
 				CGAL_Aff_transformation2 t(
-					node.matrix[0], node.matrix[4], node.matrix[12],
-					node.matrix[1], node.matrix[5], node.matrix[13], node.matrix[15]);
+					node.matrix(0,0), node.matrix(0,1), node.matrix(0,3),
+					node.matrix(1,0), node.matrix(1,1), node.matrix(1,3), node.matrix(3,3));
 				
 				DxfData *dd = N.convertToDxfData();
 				for (size_t i=0; i < dd->points.size(); i++) {
@@ -226,9 +226,9 @@ Response CGALEvaluator::visit(State &state, const TransformNode &node)
 			}
 			else if (N.dim == 3) {
 				CGAL_Aff_transformation t(
-					node.matrix[0], node.matrix[4], node.matrix[ 8], node.matrix[12],
-					node.matrix[1], node.matrix[5], node.matrix[ 9], node.matrix[13],
-					node.matrix[2], node.matrix[6], node.matrix[10], node.matrix[14], node.matrix[15]);
+					node.matrix(0,0), node.matrix(0,1), node.matrix(0,2), node.matrix(0,3),
+					node.matrix(1,0), node.matrix(1,1), node.matrix(1,2), node.matrix(1,3),
+					node.matrix(2,0), node.matrix(2,1), node.matrix(2,2), node.matrix(2,3), node.matrix(3,3));
 				N.p3->transform(t);
 			}
 			CGALCache::instance()->insert(this->tree.getIdString(node), N);
diff --git a/src/CSGTermEvaluator.cc b/src/CSGTermEvaluator.cc
index 2467c05..d1af987 100644
--- a/src/CSGTermEvaluator.cc
+++ b/src/CSGTermEvaluator.cc
@@ -147,18 +147,7 @@ Response CSGTermEvaluator::visit(State &state, const CsgNode &node)
 Response CSGTermEvaluator::visit(State &state, const TransformNode &node)
 {
 	if (state.isPrefix()) {
-		double m[16];
-		
-		for (int i = 0; i < 16; i++)
-		{
-			int c_row = i%4;
-			int m_col = i/4;
-			m[i] = 0;
-			for (int j = 0; j < 4; j++) {
-				m[i] += state.matrix()[c_row + j*4] * node.matrix[m_col*4 + j];
-			}
-		}
-		state.setMatrix(m);
+		state.setMatrix(state.matrix() * node.matrix);
 	}
 	if (state.isPostfix()) {
 		applyToChildren(node, CSGT_UNION);
diff --git a/src/OpenCSGRenderer.cc b/src/OpenCSGRenderer.cc
index a030e6c..5d6b0da 100644
--- a/src/OpenCSGRenderer.cc
+++ b/src/OpenCSGRenderer.cc
@@ -39,11 +39,11 @@ public:
 	OpenCSGPrim(OpenCSG::Operation operation, unsigned int convexity) :
 			OpenCSG::Primitive(operation, convexity) { }
 	shared_ptr<PolySet> ps;
-	double *m;
+	Transform3d m;
 	int csgmode;
 	virtual void render() {
 		glPushMatrix();
-		glMultMatrixd(m);
+		glMultMatrixd(m.data());
 		ps->render_surface(PolySet::COLORMODE_NONE, PolySet::csgmode_e(csgmode), m);
 		glPopMatrix();
 	}
@@ -85,10 +85,10 @@ void OpenCSGRenderer::renderCSGChain(CSGChain *chain, GLint *shaderinfo,
 			}
 			if (shaderinfo) glUseProgram(shaderinfo[0]);
 			for (; j < i; j++) {
-				double *m = chain->matrices[j];
+				const Transform3d &m = chain->matrices[j];
 				double *c = chain->colors[j];
 				glPushMatrix();
-				glMultMatrixd(m);
+				glMultMatrixd(m.data());
 				int csgmode = chain->types[j] == CSGTerm::TYPE_DIFFERENCE ? PolySet::CSGMODE_DIFFERENCE : PolySet::CSGMODE_NORMAL;
 				if (highlight) {
 					chain->polysets[j]->render_surface(PolySet::COLORMODE_HIGHLIGHT, PolySet::csgmode_e(csgmode + 20), m, shaderinfo);
diff --git a/src/ThrownTogetherRenderer.cc b/src/ThrownTogetherRenderer.cc
index 7a92e7f..01c7513 100644
--- a/src/ThrownTogetherRenderer.cc
+++ b/src/ThrownTogetherRenderer.cc
@@ -60,14 +60,14 @@ void ThrownTogetherRenderer::renderCSGChain(CSGChain *chain, bool highlight,
 																						bool fberror) const
 {
 	glDepthFunc(GL_LEQUAL);
-	QHash<QPair<PolySet*,double*>,int> polySetVisitMark;
+	QHash<QPair<PolySet*,Transform3d*>,int> polySetVisitMark;
 	for (size_t i = 0; i < chain->polysets.size(); i++) {
-		if (polySetVisitMark[QPair<PolySet*,double*>(chain->polysets[i].get(), chain->matrices[i])]++ > 0)
+		if (polySetVisitMark[QPair<PolySet*,Transform3d*>(chain->polysets[i].get(), &chain->matrices[i])]++ > 0)
 			continue;
-		double *m = chain->matrices[i];
+		const Transform3d &m = chain->matrices[i];
 		double *c = chain->colors[i];
 		glPushMatrix();
-		glMultMatrixd(m);
+		glMultMatrixd(m.data());
 		int csgmode = chain->types[i] == CSGTerm::TYPE_DIFFERENCE ? PolySet::CSGMODE_DIFFERENCE : PolySet::CSGMODE_NORMAL;
 		if (highlight) {
 			chain->polysets[i]->render_surface(PolySet::COLORMODE_HIGHLIGHT, PolySet::csgmode_e(csgmode + 20), m);
diff --git a/src/csgterm.cc b/src/csgterm.cc
index 8306aaf..16ef75f 100644
--- a/src/csgterm.cc
+++ b/src/csgterm.cc
@@ -47,10 +47,10 @@
  */
 
 
-CSGTerm::CSGTerm(const shared_ptr<PolySet> &polyset, const double matrix[16], const double color[4], const std::string &label)
+CSGTerm::CSGTerm(const shared_ptr<PolySet> &polyset, const Transform3d &matrix, const double color[4], const std::string &label)
 	: type(TYPE_PRIMITIVE), polyset(polyset), label(label), left(NULL), right(NULL)
 {
-	for (int i = 0; i < 16; i++) this->m[i] = matrix[i];
+	this->m = matrix;
 	for (int i = 0; i < 4; i++) this->color[i] = color[i];
 	refcounter = 1;
 }
@@ -188,7 +188,7 @@ CSGChain::CSGChain()
 {
 }
 
-void CSGChain::add(const shared_ptr<PolySet> &polyset, double *m, double *color, CSGTerm::type_e type, std::string label)
+void CSGChain::add(const shared_ptr<PolySet> &polyset, const Transform3d &m, double *color, CSGTerm::type_e type, std::string label)
 {
 	polysets.push_back(polyset);
 	matrices.push_back(m);
@@ -236,11 +236,7 @@ BoundingBox CSGChain::getBoundingBox() const
 			if (!psbox.isNull()) {
 				Eigen::Transform3d t;
 				// Column-major vs. Row-major
-				t.matrix()	<< 
-					matrices[i][0], matrices[i][4], matrices[i][8], matrices[i][12],
-					matrices[i][1], matrices[i][5], matrices[i][9], matrices[i][13],
-					matrices[i][2], matrices[i][6], matrices[i][10], matrices[i][14],
-					matrices[i][3], matrices[i][7], matrices[i][11], matrices[i][15];
+				t = matrices[i];
 				bbox.extend(t * psbox.min());
 				bbox.extend(t * psbox.max());
 			}
diff --git a/src/csgterm.h b/src/csgterm.h
index 63d1240..1d9d9fd 100644
--- a/src/csgterm.h
+++ b/src/csgterm.h
@@ -23,11 +23,11 @@ public:
 	std::string label;
 	CSGTerm *left;
 	CSGTerm *right;
-	double m[16];
+	Transform3d m;
 	double color[4];
 	int refcounter;
 
-	CSGTerm(const shared_ptr<PolySet> &polyset, const double matrix[16], const double color[4], const std::string &label);
+	CSGTerm(const shared_ptr<PolySet> &polyset, const Transform3d &matrix, const double color[4], const std::string &label);
 	CSGTerm(type_e type, CSGTerm *left, CSGTerm *right);
 
 	CSGTerm *normalize();
@@ -42,14 +42,14 @@ class CSGChain
 {
 public:
 	std::vector<shared_ptr<PolySet> > polysets;
-	std::vector<double*> matrices;
+	std::vector<Transform3d> matrices;
 	std::vector<double*> colors;
 	std::vector<CSGTerm::type_e> types;
 	std::vector<std::string> labels;
 
 	CSGChain();
 
-	void add(const shared_ptr<PolySet> &polyset, double *m, double *color, CSGTerm::type_e type, std::string label);
+	void add(const shared_ptr<PolySet> &polyset, const Transform3d &m, double *color, CSGTerm::type_e type, std::string label);
 	void import(CSGTerm *term, CSGTerm::type_e type = CSGTerm::TYPE_UNION);
 	std::string dump();
 
diff --git a/src/linalg.h b/src/linalg.h
index 60a706f..06991cf 100644
--- a/src/linalg.h
+++ b/src/linalg.h
@@ -6,5 +6,8 @@
 
 using Eigen::Vector3d;
 typedef Eigen::AlignedBox<double, 3> BoundingBox;
+using Eigen::Matrix3f;
+using Eigen::Matrix3d;
+using Eigen::Transform3d;
 
 #endif
diff --git a/src/polyset.cc b/src/polyset.cc
index ec0cdf9..1d31005 100644
--- a/src/polyset.cc
+++ b/src/polyset.cc
@@ -31,7 +31,7 @@
 #include <CGAL/assertions_behaviour.h>
 #include <CGAL/exceptions.h>
 #endif
-#include <Eigen/Core>
+#include "linalg.h"
 #include <Eigen/LU>
 #include <QColor>
 
@@ -112,13 +112,9 @@ static void gl_draw_triangle(GLint *shaderinfo, const Vector3d &p0, const Vector
 	}
 }
 
-void PolySet::render_surface(colormode_e colormode, csgmode_e csgmode, double *m, GLint *shaderinfo) const
+void PolySet::render_surface(colormode_e colormode, csgmode_e csgmode, const Transform3d &m, GLint *shaderinfo) const
 {
-	Eigen::Matrix3f m3f;
-	m3f << m[0], m[4], m[8],
-		m[1], m[5], m[9],
-		m[2], m[6], m[10];
-	bool mirrored = m3f.determinant() < 0;
+	bool mirrored = m.matrix().determinant() < 0;
 
 	if (colormode == COLORMODE_MATERIAL) {
 // FIXME: Reenable/rewrite - don't be dependant on GUI
diff --git a/src/polyset.h b/src/polyset.h
index 2fe1c1f..57f5057 100644
--- a/src/polyset.h
+++ b/src/polyset.h
@@ -45,7 +45,7 @@ public:
 		CSGMODE_HIGHLIGHT_DIFFERENCE = 22
 	};
 
-	void render_surface(colormode_e colormode, csgmode_e csgmode, double *m, GLint *shaderinfo = NULL) const;
+	void render_surface(colormode_e colormode, csgmode_e csgmode, const Transform3d &m, GLint *shaderinfo = NULL) const;
 	void render_edges(colormode_e colormode, csgmode_e csgmode) const;
 };
 
diff --git a/src/state.h b/src/state.h
index 69aee87..5dc74df 100644
--- a/src/state.h
+++ b/src/state.h
@@ -2,13 +2,14 @@
 #define STATE_H_
 
 #include <cstring>
+#include "linalg.h"
 
 class State
 {
 public:
   State(const class AbstractNode *parent) 
     : parentnode(parent), isprefix(false), ispostfix(false), numchildren(0) {
-		for (int i=0;i<16;i++) this->m[i] = i % 5 == 0 ? 1.0 : 0.0;
+		m = Transform3d::Identity();
 		for (int i=0;i<4;i++) this->c[i] = -1.0;
 	}
   virtual ~State() {}
@@ -17,14 +18,14 @@ public:
   void setPostfix(bool on) { this->ispostfix = on; }
   void setNumChildren(unsigned int numc) { this->numchildren = numc; }
   void setParent(const AbstractNode *parent) { this->parentnode = parent; }
-	void setMatrix(const double m[16]) { memcpy(this->m, m, 16*sizeof(double)); }
+	void setMatrix(const Transform3d &m) { this->m = m; }
 	void setColor(const double c[4]) { memcpy(this->c, c, 4*sizeof(double)); }
 
   bool isPrefix() const { return this->isprefix; }
   bool isPostfix() const { return this->ispostfix; }
   unsigned int numChildren() const { return this->numchildren; }
   const AbstractNode *parent() const { return this->parentnode; }
-	const double *matrix() const { return this->m; }
+	const Transform3d &matrix() const { return this->m; }
 	const double *color() const { return this->c; }
 
 private:
@@ -34,7 +35,7 @@ private:
   unsigned int numchildren;
 
 	// Transformation matrix and color. FIXME: Generalize such state variables?
-	double m[16];
+	Transform3d m;
 	double c[4];
 };
 
diff --git a/src/transform.cc b/src/transform.cc
index 88f473f..eda69c2 100644
--- a/src/transform.cc
+++ b/src/transform.cc
@@ -55,8 +55,7 @@ AbstractNode *TransformModule::evaluate(const Context *ctx, const ModuleInstanti
 {
 	TransformNode *node = new TransformNode(inst);
 
-	for (int i = 0; i < 16; i++)
-		node->matrix[i] = i % 5 == 0 ? 1.0 : 0.0;
+	node->matrix = Transform3d::Identity();
 
 	std::vector<std::string> argnames;
 	std::vector<Expression*> argexpr;
@@ -86,82 +85,50 @@ AbstractNode *TransformModule::evaluate(const Context *ctx, const ModuleInstanti
 
 	if (this->type == SCALE)
 	{
+		Vector3d scalevec(1,1,1);
 		Value v = c.lookup_variable("v");
-		v.getnum(node->matrix[0]);
-		v.getnum(node->matrix[5]);
-		v.getnum(node->matrix[10]);
-		v.getv3(node->matrix[0], node->matrix[5], node->matrix[10]);
-		if (node->matrix[10] <= 0)
-			node->matrix[10] = 1;
+		v.getnum(scalevec[0]);
+		v.getnum(scalevec[1]);
+		v.getnum(scalevec[2]);
+		v.getv3(scalevec[0], scalevec[1], scalevec[2]);
+		if (scalevec[2] == 0) scalevec[2] = 1;
+		node->matrix.scale(scalevec);
 	}
 	else if (this->type == ROTATE)
 	{
 		Value val_a = c.lookup_variable("a");
 		if (val_a.type == Value::VECTOR)
 		{
-			for (size_t i = 0; i < 3 && i < val_a.vec.size(); i++) {
-				double a;
-				val_a.vec[i]->getnum(a);
-				double c = cos(a*M_PI/180.0);
-				double s = sin(a*M_PI/180.0);
-				double x = i == 0, y = i == 1, z = i == 2;
-				double mr[16] = {
-					x*x*(1-c)+c,
-					y*x*(1-c)+z*s,
-					z*x*(1-c)-y*s,
-					0,
-					x*y*(1-c)-z*s,
-					y*y*(1-c)+c,
-					z*y*(1-c)+x*s,
-					0,
-					x*z*(1-c)+y*s,
-					y*z*(1-c)-x*s,
-					z*z*(1-c)+c,
-					0,
-					0, 0, 0, 1
-				};
-				double m[16];
-				for (int x = 0; x < 4; x++)
-				for (int y = 0; y < 4; y++)
-				{
-					m[x+y*4] = 0;
-					for (int i = 0; i < 4; i++)
-						m[x+y*4] += node->matrix[i+y*4] * mr[x+i*4];
-				}
-				for (int i = 0; i < 16; i++)
-					node->matrix[i] = m[i];
+			Eigen::AngleAxisd rotx, roty, rotz;
+			double a;
+			if (val_a.vec.size() > 0) {
+				val_a.vec[0]->getnum(a);
+				rotx = Eigen::AngleAxisd(a*M_PI/180, Vector3d::UnitX());
 			}
+			if (val_a.vec.size() > 1) {
+				val_a.vec[1]->getnum(a);
+				roty = Eigen::AngleAxisd(a*M_PI/180, Vector3d::UnitY());
+			}
+			if (val_a.vec.size() > 2) {
+				val_a.vec[2]->getnum(a);
+				rotz = Eigen::AngleAxisd(a*M_PI/180, Vector3d::UnitZ());
+			}
+			node->matrix.rotate(rotz * roty * rotx);
 		}
 		else
 		{
 			Value val_v = c.lookup_variable("v");
-			double a = 0, x = 0, y = 0, z = 1;
+			double a = 0;
 
 			val_a.getnum(a);
 
-			if (val_v.getv3(x, y, z)) {
-				if (x != 0.0 || y != 0.0 || z != 0.0) {
-					double sn = 1.0 / sqrt(x*x + y*y + z*z);
-					x *= sn, y *= sn, z *= sn;
-				}
+			Vector3d axis(0,0,1);
+			if (val_v.getv3(axis[0], axis[1], axis[2])) {
+				if (axis.squaredNorm() > 0) axis.normalize();
 			}
 
-			if (x != 0.0 || y != 0.0 || z != 0.0)
-			{
-				double c = cos(a*M_PI/180.0);
-				double s = sin(a*M_PI/180.0);
-
-				node->matrix[ 0] = x*x*(1-c)+c;
-				node->matrix[ 1] = y*x*(1-c)+z*s;
-				node->matrix[ 2] = z*x*(1-c)-y*s;
-
-				node->matrix[ 4] = x*y*(1-c)-z*s;
-				node->matrix[ 5] = y*y*(1-c)+c;
-				node->matrix[ 6] = z*y*(1-c)+x*s;
-
-				node->matrix[ 8] = x*z*(1-c)+y*s;
-				node->matrix[ 9] = y*z*(1-c)-x*s;
-				node->matrix[10] = z*z*(1-c)+c;
+			if (axis.squaredNorm() > 0) {
+				node->matrix = Eigen::AngleAxisd(a*M_PI/180, axis);
 			}
 		}
 	}
@@ -179,23 +146,20 @@ AbstractNode *TransformModule::evaluate(const Context *ctx, const ModuleInstanti
 
 		if (x != 0.0 || y != 0.0 || z != 0.0)
 		{
-			node->matrix[ 0] = 1-2*x*x;
-			node->matrix[ 1] = -2*y*x;
-			node->matrix[ 2] = -2*z*x;
-
-			node->matrix[ 4] = -2*x*y;
-			node->matrix[ 5] = 1-2*y*y;
-			node->matrix[ 6] = -2*z*y;
-
-			node->matrix[ 8] = -2*x*z;
-			node->matrix[ 9] = -2*y*z;
-			node->matrix[10] = 1-2*z*z;
+			Eigen::Matrix4d m;
+			m << 1-2*x*x, -2*y*x, -2*z*x, 0,
+				-2*x*y, 1-2*y*y, -2*z*y, 0,
+				-2*x*z, -2*y*z, 1-2*z*z, 0,
+				0, 0, 0, 1;
+			node->matrix = m;
 		}
 	}
 	else if (this->type == TRANSLATE)
 	{
 		Value v = c.lookup_variable("v");
-		v.getv3(node->matrix[12], node->matrix[13], node->matrix[14]);
+		Vector3d translatevec(0,0,0);
+		v.getv3(translatevec[0], translatevec[1], translatevec[2]);
+		node->matrix.translate(translatevec);
 	}
 	else if (this->type == MULTMATRIX)
 	{
@@ -204,7 +168,7 @@ AbstractNode *TransformModule::evaluate(const Context *ctx, const ModuleInstanti
 			for (int i = 0; i < 16; i++) {
 				size_t x = i / 4, y = i % 4;
 				if (y < v.vec.size() && v.vec[y]->type == Value::VECTOR && x < v.vec[y]->vec.size())
-					v.vec[y]->vec[x]->getnum(node->matrix[i]);
+					v.vec[y]->vec[x]->getnum(node->matrix(y, x));
 			}
 		}
 	}
@@ -224,7 +188,7 @@ std::string TransformNode::toString() const
 		stream << "[";
 		for (int i=0;i<4;i++) {
 			// FIXME: The 0 test is to avoid a leading minus before a single 0 (cosmetics)
-			stream << ((this->matrix[i*4+j]==0)?0:this->matrix[i*4+j]);
+			stream << ((this->matrix(j, i)==0)?0:this->matrix(j, i));
 			if (i != 3) stream << ", ";
 		}
 		stream << "]";
diff --git a/src/transformnode.h b/src/transformnode.h
index 9d822cb..29c6d43 100644
--- a/src/transformnode.h
+++ b/src/transformnode.h
@@ -3,6 +3,7 @@
 
 #include "node.h"
 #include "visitor.h"
+#include "linalg.h"
 
 class TransformNode : public AbstractNode
 {
@@ -14,7 +15,7 @@ public:
 	virtual std::string toString() const;
 	virtual std::string name() const;
 
-	double matrix[16];
+	Transform3d matrix;
 };
 
 #endif