Added more 3d code (alignpoints)

CMakeLists.txt

@@ -229,6 +229,7 @@ set(project_headers
 	${PROJECT_SOURCE_DIR}/include/cif++/CifUtils.hpp
 	${PROJECT_SOURCE_DIR}/include/cif++/CifValidator.hpp
 	${PROJECT_SOURCE_DIR}/include/cif++/Compound.hpp
+	${PROJECT_SOURCE_DIR}/include/cif++/Matrix.hpp
 	${PROJECT_SOURCE_DIR}/include/cif++/PDB2Cif.hpp
 	${PROJECT_SOURCE_DIR}/include/cif++/PDB2CifRemark3.hpp
 	${PROJECT_SOURCE_DIR}/include/cif++/Point.hpp

include/cif++/CifUtils.hpp

@@ -26,13 +26,13 @@
 
 #pragma once
 
-#include <vector>
-#include <set>
 #include <cassert>
-#include <memory>
-#include <list>
-#include <iostream>
 #include <filesystem>
+#include <iostream>
+#include <list>
+#include <memory>
+#include <set>
+#include <vector>
 
 #ifndef STDOUT_FILENO
 #define STDOUT_FILENO 1
@@ -48,11 +48,11 @@
 #include "cif++/Cif++Export.hpp"
 
 #if _MSC_VER
-#	pragma warning (disable : 4996)	// unsafe function or variable	(strcpy e.g.)
-#	pragma warning (disable : 4068)	// unknown pragma
-#	pragma warning (disable : 4100) // unreferenced formal parameter
-#	pragma warning (disable : 4101) // unreferenced local variable
-#	define _SILENCE_CXX17_CODECVT_HEADER_DEPRECATION_WARNING 1
+#pragma warning(disable : 4996) // unsafe function or variable	(strcpy e.g.)
+#pragma warning(disable : 4068) // unknown pragma
+#pragma warning(disable : 4100) // unreferenced formal parameter
+#pragma warning(disable : 4101) // unreferenced local variable
+#define _SILENCE_CXX17_CODECVT_HEADER_DEPRECATION_WARNING 1
 #endif
 
 namespace cif
@@ -67,20 +67,20 @@ std::string get_version_nr();
 // some basic utilities: Since we're using ASCII input only, we define for optimisation
 // our own case conversion routines.
 
-bool iequals(const std::string& a, const std::string& b);
-int icompare(const std::string& a, const std::string& b);
+bool iequals(const std::string &a, const std::string &b);
+int icompare(const std::string &a, const std::string &b);
 
-bool iequals(const char* a, const char* b);
-int icompare(const char* a, const char* b);
+bool iequals(const char *a, const char *b);
+int icompare(const char *a, const char *b);
 
-void toLower(std::string& s);
-std::string toLowerCopy(const std::string& s);
+void toLower(std::string &s);
+std::string toLowerCopy(const std::string &s);
 
 // To make life easier, we also define iless and iset using iequals
 
 struct iless
 {
-	bool operator()(const std::string& a, const std::string& b) const
+	bool operator()(const std::string &a, const std::string &b) const
 	{
 		return icompare(a, b) < 0;
 	}
@@ -100,7 +100,7 @@ inline char tolower(int ch)
 
 // --------------------------------------------------------------------
 
-std::tuple<std::string,std::string> splitTagName(const std::string& tag);
+std::tuple<std::string, std::string> splitTagName(const std::string &tag);
 
 // --------------------------------------------------------------------
 // generate a cif name, mainly used to generate asym_id's
@@ -110,7 +110,7 @@ std::string cifIdForNumber(int number);
 // --------------------------------------------------------------------
 //	custom wordwrapping routine
 
-std::vector<std::string> wordWrap(const std::string& text, size_t width);
+std::vector<std::string> wordWrap(const std::string &text, size_t width);
 
 // --------------------------------------------------------------------
 //	Code helping with terminal i/o
@@ -125,26 +125,41 @@ std::string get_executable_path();
 // --------------------------------------------------------------------
 //	some manipulators to write coloured text to terminals
 
-enum StringColour {
-	scBLACK = 0, scRED, scGREEN, scYELLOW, scBLUE, scMAGENTA, scCYAN, scWHITE, scNONE = 9 };
+enum StringColour
+{
+	scBLACK = 0,
+	scRED,
+	scGREEN,
+	scYELLOW,
+	scBLUE,
+	scMAGENTA,
+	scCYAN,
+	scWHITE,
+	scNONE = 9
+};
 
-template<typename String, typename CharT>
+template <typename String, typename CharT>
 struct ColouredString
 {
 	static_assert(std::is_reference<String>::value or std::is_pointer<String>::value, "String type must be pointer or reference");
 
 	ColouredString(String s, StringColour fore, StringColour back, bool bold = true)
-		: m_s(s), m_fore(fore), m_back(back), m_bold(bold) {}
+		: m_s(s)
+		, m_fore(fore)
+		, m_back(back)
+		, m_bold(bold)
+	{
+	}
 
-	ColouredString& operator=(const ColouredString&) = delete;
+	ColouredString &operator=(const ColouredString &) = delete;
 
 	String m_s;
 	StringColour m_fore, m_back;
 	bool m_bold;
 };
 
-template<typename CharT, typename Traits>
-std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os, const ColouredString<const CharT*,CharT>& s)
+template <typename CharT, typename Traits>
+std::basic_ostream<CharT, Traits> &operator<<(std::basic_ostream<CharT, Traits> &os, const ColouredString<const CharT *, CharT> &s)
 {
 	if (isatty(STDOUT_FILENO))
 	{
@@ -159,8 +174,8 @@ std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>&
 		return os << s.m_s;
 }
 
-template<typename CharT, typename Traits, typename String>
-std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os, const ColouredString<String,CharT>& s)
+template <typename CharT, typename Traits, typename String>
+std::basic_ostream<CharT, Traits> &operator<<(std::basic_ostream<CharT, Traits> &os, const ColouredString<String, CharT> &s)
 {
 	if (isatty(STDOUT_FILENO))
 	{
@@ -175,20 +190,20 @@ std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>&
 		return os << s.m_s;
 }
 
-template<typename CharT>
-inline auto coloured(const CharT* s, StringColour fore = scWHITE, StringColour back = scRED, bool bold = true)
+template <typename CharT>
+inline auto coloured(const CharT *s, StringColour fore = scWHITE, StringColour back = scRED, bool bold = true)
 {
-	return ColouredString<const CharT*, CharT>(s, fore, back, bold);
+	return ColouredString<const CharT *, CharT>(s, fore, back, bold);
 }
 
-template<typename CharT, typename Traits, typename Alloc>
-inline auto coloured(const std::basic_string<CharT, Traits, Alloc>& s, StringColour fore = scWHITE, StringColour back = scRED, bool bold = true)
+template <typename CharT, typename Traits, typename Alloc>
+inline auto coloured(const std::basic_string<CharT, Traits, Alloc> &s, StringColour fore = scWHITE, StringColour back = scRED, bool bold = true)
 {
 	return ColouredString<const std::basic_string<CharT, Traits, Alloc>, CharT>(s, fore, back, bold);
 }
 
-template<typename CharT, typename Traits, typename Alloc>
-inline auto coloured(std::basic_string<CharT, Traits, Alloc>& s, StringColour fore = scWHITE, StringColour back = scRED, bool bold = true)
+template <typename CharT, typename Traits, typename Alloc>
+inline auto coloured(std::basic_string<CharT, Traits, Alloc> &s, StringColour fore = scWHITE, StringColour back = scRED, bool bold = true)
 {
 	return ColouredString<std::basic_string<CharT, Traits, Alloc>, CharT>(s, fore, back, bold);
 }
@@ -199,19 +214,19 @@ inline auto coloured(std::basic_string<CharT, Traits, Alloc>& s, StringColour fo
 class Progress
 {
   public:
-				Progress(int64_t inMax, const std::string& inAction);
+	Progress(int64_t inMax, const std::string &inAction);
 	virtual ~Progress();
 
 	void consumed(int64_t inConsumed); // consumed is relative
 	void progress(int64_t inProgress); // progress is absolute
 
-	void		message(const std::string& inMessage);
+	void message(const std::string &inMessage);
 
   private:
-				Progress(const Progress&) = delete;
-	Progress& operator=(const Progress&) = delete;
+	Progress(const Progress &) = delete;
+	Progress &operator=(const Progress &) = delete;
 
-	struct ProgressImpl*	mImpl;
+	struct ProgressImpl *mImpl;
 };
 
 // --------------------------------------------------------------------
@@ -221,4 +236,4 @@ std::unique_ptr<std::istream> loadResource(std::filesystem::path name);
 void addFileResource(const std::string &name, std::filesystem::path dataFile);
 void addDataDirectory(std::filesystem::path dataDir);
 
-}
+} // namespace cif

include/cif++/Matrix.hpp

+/*-
+ * SPDX-License-Identifier: BSD-2-Clause
+ * 
+ * Copyright Maarten L. Hekkelman, Radboud University 2008-2011.
+ * Copyright (c) 2021 NKI/AVL, Netherlands Cancer Institute
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// --------------------------------------------------------------------
+// uBlas compatible matrix types
+
+#pragma once
+
+#include <iostream>
+#include <vector>
+
+// matrix is m x n, addressing i,j is 0 <= i < m and 0 <= j < n
+// element m i,j is mapped to [i * n + j] and thus storage is row major
+
+template <typename T>
+class MatrixBase
+{
+  public:
+	using value_type = T;
+
+	virtual ~MatrixBase() {}
+
+	virtual uint32_t dim_m() const = 0;
+	virtual uint32_t dim_n() const = 0;
+
+	virtual value_type &operator()(uint32_t i, uint32_t j) { throw std::runtime_error("unimplemented method"); }
+	virtual value_type operator()(uint32_t i, uint32_t j) const = 0;
+
+	MatrixBase &operator*=(const value_type &rhs);
+
+	MatrixBase &operator-=(const value_type &rhs);
+};
+
+template <typename T>
+MatrixBase<T> &MatrixBase<T>::operator*=(const T &rhs)
+{
+	for (uint32_t i = 0; i < dim_m(); ++i)
+	{
+		for (uint32_t j = 0; j < dim_n(); ++j)
+		{
+			operator()(i, j) *= rhs;
+		}
+	}
+
+	return *this;
+}
+
+template <typename T>
+MatrixBase<T> &MatrixBase<T>::operator-=(const T &rhs)
+{
+	for (uint32_t i = 0; i < dim_m(); ++i)
+	{
+		for (uint32_t j = 0; j < dim_n(); ++j)
+		{
+			operator()(i, j) -= rhs;
+		}
+	}
+
+	return *this;
+}
+
+template <typename T>
+std::ostream &operator<<(std::ostream &lhs, const MatrixBase<T> &rhs)
+{
+	lhs << '[' << rhs.dim_m() << ',' << rhs.dim_n() << ']' << '(';
+	for (uint32_t i = 0; i < rhs.dim_m(); ++i)
+	{
+		lhs << '(';
+		for (uint32_t j = 0; j < rhs.dim_n(); ++j)
+		{
+			if (j > 0)
+				lhs << ',';
+			lhs << rhs(i, j);
+		}
+		lhs << ')';
+	}
+	lhs << ')';
+
+	return lhs;
+}
+
+template <typename T>
+class Matrix : public MatrixBase<T>
+{
+  public:
+	using value_type = T;
+
+	template <typename T2>
+	Matrix(const MatrixBase<T2> &m)
+		: m_m(m.dim_m())
+		, m_n(m.dim_n())
+	{
+		m_data = new value_type[m_m * m_n];
+		for (uint32_t i = 0; i < m_m; ++i)
+		{
+			for (uint32_t j = 0; j < m_n; ++j)
+				operator()(i, j) = m(i, j);
+		}
+	}
+
+	Matrix()
+		: m_data(nullptr)
+		, m_m(0)
+		, m_n(0)
+	{
+	}
+
+	Matrix(const Matrix &m)
+		: m_m(m.m_m)
+		, m_n(m.m_n)
+	{
+		m_data = new value_type[m_m * m_n];
+		std::copy(m.m_data, m.m_data + (m_m * m_n), m_data);
+	}
+
+	Matrix &operator=(const Matrix &m)
+	{
+		value_type *t = new value_type[m.m_m * m.m_n];
+		std::copy(m.m_data, m.m_data + (m.m_m * m.m_n), t);
+
+		delete[] m_data;
+		m_data = t;
+		m_m = m.m_m;
+		m_n = m.m_n;
+
+		return *this;
+	}
+
+	Matrix(uint32_t m, uint32_t n, T v = T())
+		: m_m(m)
+		, m_n(n)
+	{
+		m_data = new value_type[m_m * m_n];
+		std::fill(m_data, m_data + (m_m * m_n), v);
+	}
+
+	virtual ~Matrix()
+	{
+		delete[] m_data;
+	}
+
+	virtual uint32_t dim_m() const { return m_m; }
+	virtual uint32_t dim_n() const { return m_n; }
+
+	virtual value_type operator()(uint32_t i, uint32_t j) const
+	{
+		assert(i < m_m);
+		assert(j < m_n);
+		return m_data[i * m_n + j];
+	}
+
+	virtual value_type &operator()(uint32_t i, uint32_t j)
+	{
+		assert(i < m_m);
+		assert(j < m_n);
+		return m_data[i * m_n + j];
+	}
+
+	template <typename Func>
+	void each(Func f)
+	{
+		for (uint32_t i = 0; i < m_m * m_n; ++i)
+			f(m_data[i]);
+	}
+
+	template <typename U>
+	Matrix &operator/=(U v)
+	{
+		for (uint32_t i = 0; i < m_m * m_n; ++i)
+			m_data[i] /= v;
+
+		return *this;
+	}
+
+  private:
+	value_type *m_data;
+	uint32_t m_m, m_n;
+};
+
+// --------------------------------------------------------------------
+
+template <typename T>
+class SymmetricMatrix : public MatrixBase<T>
+{
+  public:
+	typedef typename MatrixBase<T>::value_type value_type;
+
+	SymmetricMatrix(uint32_t n, T v = T())
+		: m_owner(true)
+		, m_n(n)
+	{
+		uint32_t N = (m_n * (m_n + 1)) / 2;
+		m_data = new value_type[N];
+		std::fill(m_data, m_data + N, v);
+	}
+
+	SymmetricMatrix(const T *data, uint32_t n)
+		: m_owner(false)
+		, m_data(const_cast<T *>(data))
+		, m_n(n)
+	{
+	}
+
+	virtual ~SymmetricMatrix()
+	{
+		if (m_owner)
+			delete[] m_data;
+	}
+
+	virtual uint32_t dim_m() const { return m_n; }
+	virtual uint32_t dim_n() const { return m_n; }
+
+	T operator()(uint32_t i, uint32_t j) const;
+	virtual T &operator()(uint32_t i, uint32_t j);
+
+	// erase two rows, add one at the end (for neighbour joining)
+	void erase_2(uint32_t i, uint32_t j);
+
+	template <typename Func>
+	void each(Func f)
+	{
+		uint32_t N = (m_n * (m_n + 1)) / 2;
+
+		for (uint32_t i = 0; i < N; ++i)
+			f(m_data[i]);
+	}
+
+	template <typename U>
+	SymmetricMatrix &operator/=(U v)
+	{
+		uint32_t N = (m_n * (m_n + 1)) / 2;
+
+		for (uint32_t i = 0; i < N; ++i)
+			m_data[i] /= v;
+
+		return *this;
+	}
+
+  private:
+	bool m_owner;
+	value_type *m_data;
+	uint32_t m_n;
+};
+
+template <typename T>
+inline T SymmetricMatrix<T>::operator()(uint32_t i, uint32_t j) const
+{
+	return i < j
+	           ? m_data[(j * (j + 1)) / 2 + i]
+	           : m_data[(i * (i + 1)) / 2 + j];
+}
+
+template <typename T>
+inline T &SymmetricMatrix<T>::operator()(uint32_t i, uint32_t j)
+{
+	if (i > j)
+		std::swap(i, j);
+	assert(j < m_n);
+	return m_data[(j * (j + 1)) / 2 + i];
+}
+
+template <typename T>
+void SymmetricMatrix<T>::erase_2(uint32_t di, uint32_t dj)
+{
+	uint32_t s = 0, d = 0;
+	for (uint32_t i = 0; i < m_n; ++i)
+	{
+		for (uint32_t j = 0; j < i; ++j)
+		{
+			if (i != di and j != dj and i != dj and j != di)
+			{
+				if (s != d)
+					m_data[d] = m_data[s];
+				++d;
+			}
+
+			++s;
+		}
+	}
+
+	--m_n;
+}
+
+template <typename T>
+class IdentityMatrix : public MatrixBase<T>
+{
+  public:
+	typedef typename MatrixBase<T>::value_type value_type;
+
+	IdentityMatrix(uint32_t n)
+		: m_n(n)
+	{
+	}
+
+	virtual uint32_t dim_m() const { return m_n; }
+	virtual uint32_t dim_n() const { return m_n; }
+
+	virtual value_type operator()(uint32_t i, uint32_t j) const
+	{
+		value_type result = 0;
+		if (i == j)
+			result = 1;
+		return result;
+	}
+
+  private:
+	uint32_t m_n;
+};
+
+// --------------------------------------------------------------------
+// matrix functions
+
+template <typename T>
+Matrix<T> operator*(const MatrixBase<T> &lhs, const MatrixBase<T> &rhs)
+{
+	Matrix<T> result(std::min(lhs.dim_m(), rhs.dim_m()), std::min(lhs.dim_n(), rhs.dim_n()));
+
+	for (uint32_t i = 0; i < result.dim_m(); ++i)
+	{
+		for (uint32_t j = 0; j < result.dim_n(); ++j)
+		{
+			for (uint32_t li = 0, rj = 0; li < lhs.dim_m() and rj < rhs.dim_n(); ++li, ++rj)
+				result(i, j) += lhs(li, j) * rhs(i, rj);
+		}
+	}
+
+	return result;
+}
+
+template <typename T>
+Matrix<T> operator*(const MatrixBase<T> &lhs, T rhs)
+{
+	Matrix<T> result(lhs);
+	result *= rhs;
+
+	return result;
+}
+
+template <typename T>
+Matrix<T> operator-(const MatrixBase<T> &lhs, const MatrixBase<T> &rhs)
+{
+	Matrix<T> result(std::min(lhs.dim_m(), rhs.dim_m()), std::min(lhs.dim_n(), rhs.dim_n()));
+
+	for (uint32_t i = 0; i < result.dim_m(); ++i)
+	{
+		for (uint32_t j = 0; j < result.dim_n(); ++j)
+		{
+			result(i, j) = lhs(i, j) - rhs(i, j);
+		}
+	}
+
+	return result;
+}
+
+template <typename T>
+Matrix<T> operator-(const MatrixBase<T> &lhs, T rhs)
+{
+	Matrix<T> result(lhs.dim_m(), lhs.dim_n());
+	result -= rhs;
+	return result;
+}
+
+// template <typename T>
+// symmetric_matrix<T> hammingDistance(const MatrixBase<T> &lhs, T rhs);
+
+// template <typename T>
+// std::vector<T> sum(const MatrixBase<T> &m);

include/cif++/Point.hpp

@@ -363,7 +363,7 @@ PointF<F> Nudge(PointF<F> p, F offset);
 
 quaternion Normalize(quaternion q);
 
-//std::tuple<double,Point> QuaternionToAngleAxis(quaternion q);
+std::tuple<double,Point> QuaternionToAngleAxis(quaternion q);
 Point Centroid(std::vector<Point>& Points);
 Point CenterPoints(std::vector<Point>& Points);
 quaternion AlignPoints(const std::vector<Point>& a, const std::vector<Point>& b);

src/Point.cpp

@@ -28,6 +28,7 @@
 #include <valarray>
 
 #include "cif++/Point.hpp"
+#include "cif++/Matrix.hpp"
 
 namespace mmcif
 {
@@ -171,113 +172,113 @@ double LargestDepressedQuarticSolution(double a, double b, double c)
 	return t.max();
 }
 
-//quaternion AlignPoints(const vector<Point>& pa, const vector<Point>& pb)
-//{
-//	// First calculate M, a 3x3 matrix containing the sums of products of the coordinates of A and B
-//	matrix<double> M(3, 3, 0);
-//
-//	for (uint32_t i = 0; i < pa.size(); ++i)
-//	{
-//		const Point& a = pa[i];
-//		const Point& b = pb[i];
-//		
-//		M(0, 0) += a.mX * b.mX;	M(0, 1) += a.mX * b.mY;	M(0, 2) += a.mX * b.mZ;
-//		M(1, 0) += a.mY * b.mX;	M(1, 1) += a.mY * b.mY;	M(1, 2) += a.mY * b.mZ;
-//		M(2, 0) += a.mZ * b.mX;	M(2, 1) += a.mZ * b.mY;	M(2, 2) += a.mZ * b.mZ;
-//	}
-//	
-//	// Now calculate N, a symmetric 4x4 matrix
-//	symmetric_matrix<double> N(4);
-//	
-//	N(0, 0) =  M(0, 0) + M(1, 1) + M(2, 2);
-//	N(0, 1) =  M(1, 2) - M(2, 1);
-//	N(0, 2) =  M(2, 0) - M(0, 2);
-//	N(0, 3) =  M(0, 1) - M(1, 0);
-//	
-//	N(1, 1) =  M(0, 0) - M(1, 1) - M(2, 2);
-//	N(1, 2) =  M(0, 1) + M(1, 0);
-//	N(1, 3) =  M(0, 2) + M(2, 0);
-//	
-//	N(2, 2) = -M(0, 0) + M(1, 1) - M(2, 2);
-//	N(2, 3) =  M(1, 2) + M(2, 1);
-//	
-//	N(3, 3) = -M(0, 0) - M(1, 1) + M(2, 2);
-//
-//	// det(N - λI) = 0
-//	// find the largest λ (λm)
-//	//
-//	// Aλ4 + Bλ3 + Cλ2 + Dλ + E = 0
-//	// A = 1
-//	// B = 0
-//	// and so this is a so-called depressed quartic
-//	// solve it using Ferrari's algorithm
-//	
-//	double C = -2 * (
-//		M(0, 0) * M(0, 0) + M(0, 1) * M(0, 1) + M(0, 2) * M(0, 2) +
-//		M(1, 0) * M(1, 0) + M(1, 1) * M(1, 1) + M(1, 2) * M(1, 2) +
-//		M(2, 0) * M(2, 0) + M(2, 1) * M(2, 1) + M(2, 2) * M(2, 2));
-//	
-//	double D = 8 * (M(0, 0) * M(1, 2) * M(2, 1) +
-//					M(1, 1) * M(2, 0) * M(0, 2) +
-//					M(2, 2) * M(0, 1) * M(1, 0)) -
-//			   8 * (M(0, 0) * M(1, 1) * M(2, 2) +
-//					M(1, 2) * M(2, 0) * M(0, 1) +
-//					M(2, 1) * M(1, 0) * M(0, 2));
-//	
-//	double E = 
-//		(N(0,0) * N(1,1) - N(0,1) * N(0,1)) * (N(2,2) * N(3,3) - N(2,3) * N(2,3)) +
-//		(N(0,1) * N(0,2) - N(0,0) * N(2,1)) * (N(2,1) * N(3,3) - N(2,3) * N(1,3)) +
-//		(N(0,0) * N(1,3) - N(0,1) * N(0,3)) * (N(2,1) * N(2,3) - N(2,2) * N(1,3)) +
-//		(N(0,1) * N(2,1) - N(1,1) * N(0,2)) * (N(0,2) * N(3,3) - N(2,3) * N(0,3)) +
-//		(N(1,1) * N(0,3) - N(0,1) * N(1,3)) * (N(0,2) * N(2,3) - N(2,2) * N(0,3)) +
-//		(N(0,2) * N(1,3) - N(2,1) * N(0,3)) * (N(0,2) * N(1,3) - N(2,1) * N(0,3));
-//	
-//	// solve quartic
-//	double lm = LargestDepressedQuarticSolution(C, D, E);
-//	
-//	// calculate t = (N - λI)
-//	matrix<double> li = identity_matrix<double>(4) * lm;
-//	matrix<double> t = N - li;
-//	
-//	// calculate a matrix of cofactors for t
-//	matrix<double> cf(4, 4);
-//
-//	const uint32_t ixs[4][3] =
-//	{
-//		{ 1, 2, 3 },
-//		{ 0, 2, 3 },
-//		{ 0, 1, 3 },
-//		{ 0, 1, 2 }
-//	};
-//
-//	uint32_t maxR = 0;
-//	for (uint32_t r = 0; r < 4; ++r)
-//	{
-//		const uint32_t* ir = ixs[r];
-//		
-//		for (uint32_t c = 0; c < 4; ++c)
-//		{
-//			const uint32_t* ic = ixs[c];
-//
-//			cf(r, c) =
-//				t(ir[0], ic[0]) * t(ir[1], ic[1]) * t(ir[2], ic[2]) +
-//				t(ir[0], ic[1]) * t(ir[1], ic[2]) * t(ir[2], ic[0]) +
-//				t(ir[0], ic[2]) * t(ir[1], ic[0]) * t(ir[2], ic[1]) -
-//				t(ir[0], ic[2]) * t(ir[1], ic[1]) * t(ir[2], ic[0]) -
-//				t(ir[0], ic[1]) * t(ir[1], ic[0]) * t(ir[2], ic[2]) -
-//				t(ir[0], ic[0]) * t(ir[1], ic[2]) * t(ir[2], ic[1]);
-//		}
-//		
-//		if (r > maxR and cf(r, 0) > cf(maxR, 0))
-//			maxR = r;
-//	}
-//	
-//	// NOTE the negation of the y here, why? Maybe I swapped r/c above?
-//	quaternion q(cf(maxR, 0), cf(maxR, 1), -cf(maxR, 2), cf(maxR, 3));
-//	q = Normalize(q);
-//	
-//	return q;
-//}
+quaternion AlignPoints(const std::vector<Point>& pa, const std::vector<Point>& pb)
+{
+	// First calculate M, a 3x3 Matrix containing the sums of products of the coordinates of A and B
+	Matrix<double> M(3, 3, 0);
+
+	for (uint32_t i = 0; i < pa.size(); ++i)
+	{
+		const Point& a = pa[i];
+		const Point& b = pb[i];
+		
+		M(0, 0) += a.mX * b.mX;	M(0, 1) += a.mX * b.mY;	M(0, 2) += a.mX * b.mZ;
+		M(1, 0) += a.mY * b.mX;	M(1, 1) += a.mY * b.mY;	M(1, 2) += a.mY * b.mZ;
+		M(2, 0) += a.mZ * b.mX;	M(2, 1) += a.mZ * b.mY;	M(2, 2) += a.mZ * b.mZ;
+	}
+	
+	// Now calculate N, a symmetric 4x4 Matrix
+	SymmetricMatrix<double> N(4);
+	
+	N(0, 0) =  M(0, 0) + M(1, 1) + M(2, 2);
+	N(0, 1) =  M(1, 2) - M(2, 1);
+	N(0, 2) =  M(2, 0) - M(0, 2);
+	N(0, 3) =  M(0, 1) - M(1, 0);
+	
+	N(1, 1) =  M(0, 0) - M(1, 1) - M(2, 2);
+	N(1, 2) =  M(0, 1) + M(1, 0);
+	N(1, 3) =  M(0, 2) + M(2, 0);
+	
+	N(2, 2) = -M(0, 0) + M(1, 1) - M(2, 2);
+	N(2, 3) =  M(1, 2) + M(2, 1);
+	
+	N(3, 3) = -M(0, 0) - M(1, 1) + M(2, 2);
+
+	// det(N - λI) = 0
+	// find the largest λ (λm)
+	//
+	// Aλ4 + Bλ3 + Cλ2 + Dλ + E = 0
+	// A = 1
+	// B = 0
+	// and so this is a so-called depressed quartic
+	// solve it using Ferrari's algorithm
+	
+	double C = -2 * (
+		M(0, 0) * M(0, 0) + M(0, 1) * M(0, 1) + M(0, 2) * M(0, 2) +
+		M(1, 0) * M(1, 0) + M(1, 1) * M(1, 1) + M(1, 2) * M(1, 2) +
+		M(2, 0) * M(2, 0) + M(2, 1) * M(2, 1) + M(2, 2) * M(2, 2));
+	
+	double D = 8 * (M(0, 0) * M(1, 2) * M(2, 1) +
+					M(1, 1) * M(2, 0) * M(0, 2) +
+					M(2, 2) * M(0, 1) * M(1, 0)) -
+			   8 * (M(0, 0) * M(1, 1) * M(2, 2) +
+					M(1, 2) * M(2, 0) * M(0, 1) +
+					M(2, 1) * M(1, 0) * M(0, 2));
+	
+	double E = 
+		(N(0,0) * N(1,1) - N(0,1) * N(0,1)) * (N(2,2) * N(3,3) - N(2,3) * N(2,3)) +
+		(N(0,1) * N(0,2) - N(0,0) * N(2,1)) * (N(2,1) * N(3,3) - N(2,3) * N(1,3)) +
+		(N(0,0) * N(1,3) - N(0,1) * N(0,3)) * (N(2,1) * N(2,3) - N(2,2) * N(1,3)) +
+		(N(0,1) * N(2,1) - N(1,1) * N(0,2)) * (N(0,2) * N(3,3) - N(2,3) * N(0,3)) +
+		(N(1,1) * N(0,3) - N(0,1) * N(1,3)) * (N(0,2) * N(2,3) - N(2,2) * N(0,3)) +
+		(N(0,2) * N(1,3) - N(2,1) * N(0,3)) * (N(0,2) * N(1,3) - N(2,1) * N(0,3));
+	
+	// solve quartic
+	double lm = LargestDepressedQuarticSolution(C, D, E);
+	
+	// calculate t = (N - λI)
+	Matrix<double> li = IdentityMatrix<double>(4) * lm;
+	Matrix<double> t = N - li;
+	
+	// calculate a Matrix of cofactors for t
+	Matrix<double> cf(4, 4);
+
+	const uint32_t ixs[4][3] =
+	{
+		{ 1, 2, 3 },
+		{ 0, 2, 3 },
+		{ 0, 1, 3 },
+		{ 0, 1, 2 }
+	};
+
+	uint32_t maxR = 0;
+	for (uint32_t r = 0; r < 4; ++r)
+	{
+		const uint32_t* ir = ixs[r];
+		
+		for (uint32_t c = 0; c < 4; ++c)
+		{
+			const uint32_t* ic = ixs[c];
+
+			cf(r, c) =
+				t(ir[0], ic[0]) * t(ir[1], ic[1]) * t(ir[2], ic[2]) +
+				t(ir[0], ic[1]) * t(ir[1], ic[2]) * t(ir[2], ic[0]) +
+				t(ir[0], ic[2]) * t(ir[1], ic[0]) * t(ir[2], ic[1]) -
+				t(ir[0], ic[2]) * t(ir[1], ic[1]) * t(ir[2], ic[0]) -
+				t(ir[0], ic[1]) * t(ir[1], ic[0]) * t(ir[2], ic[2]) -
+				t(ir[0], ic[0]) * t(ir[1], ic[2]) * t(ir[2], ic[1]);
+		}
+		
+		if (r > maxR and cf(r, 0) > cf(maxR, 0))
+			maxR = r;
+	}
+	
+	// NOTE the negation of the y here, why? Maybe I swapped r/c above?
+	quaternion q(cf(maxR, 0), cf(maxR, 1), -cf(maxR, 2), cf(maxR, 3));
+	q = Normalize(q);
+	
+	return q;
+}
 
 // --------------------------------------------------------------------