From 14f9716aa3567b472ec4c9ec14b62c543125c9af Mon Sep 17 00:00:00 2001
From: Ivan Mahonin <bh@icystar.com>
Date: Jan 15 2019 15:13:15 +0000
Subject: perspective: matrix


---

diff --git a/c++/perspective/src/SConstruct b/c++/perspective/src/SConstruct
index 3871bff..2b21240 100644
--- a/c++/perspective/src/SConstruct
+++ b/c++/perspective/src/SConstruct
@@ -19,6 +19,7 @@ target = 'perspective'
 sources = [
 	'generator.cpp',
 	'main.cpp',
+	'matrix.cpp',
 	'surface.cpp' ]
 
 
diff --git a/c++/perspective/src/matrix.cpp b/c++/perspective/src/matrix.cpp
new file mode 100644
index 0000000..da4b1ff
--- /dev/null
+++ b/c++/perspective/src/matrix.cpp
@@ -0,0 +1,58 @@
+#include "matrix.h"
+
+
+// remove from sequence 0, 1, 2, 3, two numbers, see following
+// example:
+//   r0 = 1, r1 = 2
+// initial sequence:
+//   0, 1, 2, 3
+// remove number at place r0 (1 for ex):
+//   0, 2, 3
+// remove number at place r1 (2 for ex):
+//   0, 2
+// so result for index0(1, 2) is 0
+//       and for index1(1, 2) is 2
+//
+// r1\r0 :  0  1  2  3
+// -------------------
+//     0 : 23 23 13 12 
+//     1 : 13 03 03 02 
+//     2 : 12 02 01 01
+static inline int index0(int r0, int r1)
+	{ return r1 ? (r0 ? 1 : 0) : (r0 > 1 ? 1 : 2); }
+static inline int index1(int r0, int r1)
+	{ return r1 < 2 ? (r0 < 3 ? 3 : 2) : (r0 > 1 ? 1 : 2); }
+
+static inline Real det2(const Matrix4 &m, int r, int c, int rr, int cc) {
+	const int r0 = index0(r, rr);
+	const int r1 = index1(r, rr);
+	const int c0 = index0(c, cc);
+	const int c1 = index1(c, cc);
+	return m[r0][c0]*m[r1][c1] - m[r0][c1]*m[r1][c0];
+}
+static inline Real det3(const Matrix4 &m, int r, int c)
+	{ return det2(m, r, c, 0, 0) - det2(m, r, c, 0, 1) + det2(m, r, c, 0, 2); }
+
+
+Real
+Matrix4::det() const
+	{ return det3(*this, 0, 0) - det3(*this, 0, 1) + det3(*this, 0, 2) - det3(*this, 0, 3); }
+
+Matrix4
+Matrix4::invert() const {
+	Vector4 dv(
+		 det3(*this, 0, 0),
+		-det3(*this, 0, 1),
+		 det3(*this, 0, 2),
+		-det3(*this, 0, 3) );
+	Real d = dv.x + dv.y + dv.z + dv.w;
+	if (fabs(d) <= realRrecision)
+		return zero();
+
+	d = 1.0/d;
+	return Matrix4(
+		Vector4(dv.x, -det3(*this, 1, 0), det3(*this, 2, 0), -det3(*this, 3, 0))*d,
+		Vector4(dv.y,  det3(*this, 1, 1),-det3(*this, 2, 1),  det3(*this, 3, 1))*d,
+		Vector4(dv.z, -det3(*this, 1, 2), det3(*this, 2, 2), -det3(*this, 3, 2))*d,
+		Vector4(dv.w,  det3(*this, 1, 3),-det3(*this, 2, 3),  det3(*this, 3, 3))*d );
+}
diff --git a/c++/perspective/src/matrix.h b/c++/perspective/src/matrix.h
new file mode 100644
index 0000000..75684dd
--- /dev/null
+++ b/c++/perspective/src/matrix.h
@@ -0,0 +1,68 @@
+#ifndef MATRIX_H
+#define MATRIX_H
+
+
+#include "vector.h"
+
+
+class Matrix4 {
+public:
+	union {
+		struct {
+			Real m00, m01, m02, m03,
+				 m10, m11, m12, m13,
+				 m20, m21, m22, m23,
+				 m30, m31, m32, m33;
+		};
+		struct { Real m[4][4]; };
+		struct { Real a[16]; };
+	};
+	
+	inline explicit Matrix4(
+		const Vector4 &x = Vector4(1, 0, 0, 0),
+		const Vector4 &y = Vector4(0, 1, 0, 0),
+		const Vector4 &z = Vector4(0, 0, 1, 0),
+		const Vector4 &w = Vector4(0, 0, 0, 1)
+	):
+		m00(x.x), m01(x.y), m02(x.z), m03(x.w),
+		m10(y.x), m11(y.y), m12(y.z), m13(y.w),
+		m20(z.x), m21(z.y), m22(z.z), m23(z.w),
+		m30(w.x), m31(w.y), m32(w.z), m33(w.w) { }
+	
+	inline Vector4& operator[] (int index) { return Vector4::cast(m[index]); }
+	inline const Vector4& operator[] (int index) const { return Vector4::cast(m[index]); }
+	
+	inline Vector4& row_x() { return (*this)[0]; }
+	inline Vector4& row_y() { return (*this)[1]; }
+	inline Vector4& row_z() { return (*this)[2]; }
+	inline Vector4& row_w() { return (*this)[3]; }
+
+	inline const Vector4& row_x() const { return (*this)[0]; }
+	inline const Vector4& row_y() const { return (*this)[1]; }
+	inline const Vector4& row_z() const { return (*this)[2]; }
+	inline const Vector4& row_w() const { return (*this)[3]; }
+	
+	Real det() const;
+	Matrix4 invert() const;
+	
+	inline Vector4 operator* (const Vector4 &v) const
+		{ return row_x()*v.x + row_y()*v.y + row_z()*v.z + row_w()*v.w; }
+	inline Matrix4 operator* (const Matrix4 &other) const {
+		return Matrix4( *this * other.row_x(),
+						*this * other.row_y(),
+						*this * other.row_z(),
+						*this * other.row_w() );
+	}
+	
+	inline Matrix4& operator*= (const Matrix4 &other)
+		{ return *this = *this * other; }
+	
+	static inline Matrix4 zero() { return Matrix4(Vector4(), Vector4(), Vector4(), Vector4()); }
+	static inline Matrix4 identity() { return Matrix4(); }
+};
+
+
+typedef Matrix4 Matrix;
+
+
+#endif
diff --git a/c++/perspective/src/vector.h b/c++/perspective/src/vector.h
index 59b217a..ee790d2 100644
--- a/c++/perspective/src/vector.h
+++ b/c++/perspective/src/vector.h
@@ -192,6 +192,11 @@ public:
 		{ return SelfType(*this) *= c; }
 	inline Coord square() const
 		{ return *this * *this; }
+		
+	inline static SelfType& cast(Coord *c)
+		{ return *(SelfType*)c; }
+	inline static const SelfType& cast(const Coord *c)
+		{ return *(const SelfType*)c; }
 };