/*
	This file contains the code listings for the book
	Foundations of Game Engine Development, Volume 1: Mathematics
	by Eric Lengyel.

	It also contains some additional data structures that are
	similar to the code contained in the book, such as Vector4D.

	This code is copyrighted. If you own a copy of FGED1, then you
	have the author's express permission to use this code in whatever
	way you see fit as long as you don't republish it or pass it off
	as your own work.
*/


// Listing 1.1, plus parts from Listing 1.2 and Listing 1.3

struct Vector3D
{
	float		x, y, z;

	Vector3D() = default;

	Vector3D(float a, float b, float c)
	{
		x = a;
		y = b;
		z = c;
	}

	float& operator [](int i)
	{
		return ((&x)[i]);
	}

	const float& operator [](int i) const
	{
		return ((&x)[i]);
	}

	Vector3D& operator *=(float s)
	{
		x *= s;
		y *= s;
		z *= s;
		return (*this);
	}

	Vector3D& operator /=(float s)
	{
		s = 1.0F / s;
		x *= s;
		y *= s;
		z *= s;
		return (*this);
	}

	Vector3D& operator +=(const Vector3D& v)
	{
		x += v.x;
		y += v.y;
		z += v.z;
		return (*this);
	}

	Vector3D& operator -=(const Vector3D& v)
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;
		return (*this);
	}
};

// 4D version

struct Vector4D
{
	float		x, y, z, w;

	Vector4D() = default;

	Vector4D(float a, float b, float c, float d)
	{
		x = a;
		y = b;
		z = c;
		w = d;
	}

	float& operator [](int i)
	{
		return ((&x)[i]);
	}

	const float& operator [](int i) const
	{
		return ((&x)[i]);
	}

	Vector4D& operator *=(float s)
	{
		x *= s;
		y *= s;
		z *= s;
		w *= s;
		return (*this);
	}

	Vector4D& operator /=(float s)
	{
		s = 1.0F / s;
		x *= s;
		y *= s;
		z *= s;
		w *= s;
		return (*this);
	}

	Vector4D& operator +=(const Vector4D& v)
	{
		x += v.x;
		y += v.y;
		z += v.z;
		w += v.w;
		return (*this);
	}

	Vector4D& operator -=(const Vector4D& v)
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;
		w -= v.w;
		return (*this);
	}
};

// Listing 1.2

inline Vector3D operator *(const Vector3D& v, float s)
{
	return (Vector3D(v.x * s, v.y * s, v.z * s));
}

inline Vector3D operator /(const Vector3D& v, float s)
{
	s = 1.0F / s;
	return (Vector3D(v.x * s, v.y * s, v.z * s));
}

inline Vector3D operator -(const Vector3D& v)
{
	return (Vector3D(-v.x, -v.y, -v.z));
}

inline float Magnitude(const Vector3D& v)
{
	return (sqrt(v.x * v.x + v.y * v.y + v.z * v.z));
}

inline Vector3D Normalize(const Vector3D& v)
{
	return (v / Magnitude(v));
}

// Listing 1.3

inline Vector3D operator +(const Vector3D& a, const Vector3D& b)
{
	return (Vector3D(a.x + b.x, a.y + b.y, a.z + b.z));
}

inline Vector3D operator -(const Vector3D& a, const Vector3D& b)
{
	return (Vector3D(a.x - b.x, a.y - b.y, a.z - b.z));
}

// Listing 1.4

struct Matrix3D
{
	private:

		float		n[3][3];

	public:

		Matrix3D() = default;

		Matrix3D(float n00, float n01, float n02,
		         float n10, float n11, float n12,
		         float n20, float n21, float n22)
		{
			n[0][0] = n00; n[0][1] = n10; n[0][2] = n20;
			n[1][0] = n01; n[1][1] = n11; n[1][2] = n21;
			n[2][0] = n02; n[2][1] = n12; n[2][2] = n22;
		}

		Matrix3D(const Vector3D& a, const Vector3D& b, const Vector3D& c)
		{
			n[0][0] = a.x; n[0][1] = a.y; n[0][2] = a.z;
			n[1][0] = b.x; n[1][1] = b.y; n[1][2] = b.z;
			n[2][0] = c.x; n[2][1] = c.y; n[2][2] = c.z;
		}

		float& operator ()(int i, int j)
		{
			return (n[j][i]);
		}

		const float& operator ()(int i, int j) const
		{
			return (n[j][i]);
		}

		Vector3D& operator [](int j)
		{
			return (*reinterpret_cast<Vector3D *>(n[j]));
		}

		const Vector3D& operator [](int j) const
		{
			return (*reinterpret_cast<const Vector3D *>(n[j]));
		}
};

// 4D version

struct Matrix4D
{
	protected:

		float		n[4][4];

	public:

		Matrix4D() = default;

		Matrix4D(float n00, float n01, float n02, float n03,
		         float n10, float n11, float n12, float n13,
		         float n20, float n21, float n22, float n23,
				 float n30, float n31, float n32, float n33)
		{
			n[0][0] = n00; n[0][1] = n10; n[0][2] = n20; n[0][3] = n30;
			n[1][0] = n01; n[1][1] = n11; n[1][2] = n21; n[1][3] = n31;
			n[2][0] = n02; n[2][1] = n12; n[2][2] = n22; n[2][3] = n32;
			n[3][0] = n03; n[3][1] = n13; n[3][2] = n23; n[3][3] = n33;
		}

		Matrix4D(const Vector4D& a, const Vector4D& b, const Vector4D& c, const Vector4D& d)
		{
			n[0][0] = a.x; n[0][1] = a.y; n[0][2] = a.z; n[0][3] = a.w;
			n[1][0] = b.x; n[1][1] = b.y; n[1][2] = b.z; n[1][3] = b.w;
			n[2][0] = c.x; n[2][1] = c.y; n[2][2] = c.z; n[2][3] = c.w;
			n[3][0] = d.x; n[3][1] = d.y; n[3][2] = d.z; n[3][3] = d.w;
		}

		float& operator ()(int i, int j)
		{
			return (n[j][i]);
		}

		const float& operator ()(int i, int j) const
		{
			return (n[j][i]);
		}

		Vector4D& operator [](int j)
		{
			return (*reinterpret_cast<Vector4D *>(n[j]));
		}

		const Vector4D& operator [](int j) const
		{
			return (*reinterpret_cast<const Vector4D *>(n[j]));
		}
};

// Listing 1.5

Matrix3D operator *(const Matrix3D& A, const Matrix3D& B)
{
	return (Matrix3D(A(0,0) * B(0,0) + A(0,1) * B(1,0) + A(0,2) * B(2,0),
	                 A(0,0) * B(0,1) + A(0,1) * B(1,1) + A(0,2) * B(2,1),
	                 A(0,0) * B(0,2) + A(0,1) * B(1,2) + A(0,2) * B(2,2),
	                 A(1,0) * B(0,0) + A(1,1) * B(1,0) + A(1,2) * B(2,0),
	                 A(1,0) * B(0,1) + A(1,1) * B(1,1) + A(1,2) * B(2,1),
	                 A(1,0) * B(0,2) + A(1,1) * B(1,2) + A(1,2) * B(2,2),
	                 A(2,0) * B(0,0) + A(2,1) * B(1,0) + A(2,2) * B(2,0),
	                 A(2,0) * B(0,1) + A(2,1) * B(1,1) + A(2,2) * B(2,1),
	                 A(2,0) * B(0,2) + A(2,1) * B(1,2) + A(2,2) * B(2,2)));
}

Vector3D operator *(const Matrix3D& M, const Vector3D& v)
{
	return (Vector3D(M(0,0) * v.x + M(0,1) * v.y + M(0,2) * v.z,
	                 M(1,0) * v.x + M(1,1) * v.y + M(1,2) * v.z,
	                 M(2,0) * v.x + M(2,1) * v.y + M(2,2) * v.z));
}

// Listing 1.6

inline float Dot(const Vector3D& a, const Vector3D& b)
{
	return (a.x * b.x + a.y * b.y + a.z * b.z);
}

// Listing 1.7

inline Vector3D Cross(const Vector3D& a, const Vector3D& b)
{
	return (Vector3D(a.y * b.z - a.z * b.y,
	                 a.z * b.x - a.x * b.z,
	                 a.x * b.y - a.y * b.x));
}

// Listing 1.8

inline Vector3D Project(const Vector3D& a, const Vector3D& b)
{
	return (b * (Dot(a, b) / Dot(b, b)));
}

inline Vector3D Reject(const Vector3D& a, const Vector3D& b)
{
	return (a - b * (Dot(a, b) / Dot(b, b)));
}

// Listing 1.9

float Determinant(const Matrix3D& 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)));
}

// Listing 1.10

Matrix3D Inverse(const Matrix3D& M)
{
	const Vector3D& a = M[0];
	const Vector3D& b = M[1];
	const Vector3D& c = M[2];

	Vector3D r0 = Cross(b, c);
	Vector3D r1 = Cross(c, a);
	Vector3D r2 = Cross(a, b);

	float invDet = 1.0F / Dot(r2, c);

	return (Matrix3D(r0.x * invDet, r0.y * invDet, r0.z * invDet,
	                 r1.x * invDet, r1.y * invDet, r1.z * invDet,
	                 r2.x * invDet, r2.y * invDet, r2.z * invDet));
}

// Listing 1.11

Matrix4D Inverse(const Matrix4D& M)
{
	const Vector3D& a = reinterpret_cast<const Vector3D&>(M[0]);
	const Vector3D& b = reinterpret_cast<const Vector3D&>(M[1]);
	const Vector3D& c = reinterpret_cast<const Vector3D&>(M[2]);
	const Vector3D& d = reinterpret_cast<const Vector3D&>(M[3]);

	const float& x = M(3,0);
	const float& y = M(3,1);
	const float& z = M(3,2);
	const float& w = M(3,3);

	Vector3D s = Cross(a, b);
	Vector3D t = Cross(c, d);
	Vector3D u = a * y - b * x;
	Vector3D v = c * w - d * z;

	float invDet = 1.0F / (Dot(s, v) + Dot(t, u));
	s *= invDet;
	t *= invDet;
	u *= invDet;
	v *= invDet;

	Vector3D r0 = Cross(b, v) + t * y;
	Vector3D r1 = Cross(v, a) - t * x;
	Vector3D r2 = Cross(d, u) + s * w;
	Vector3D r3 = Cross(u, c) - s * z;

	return (Matrix4D(r0.x, r0.y, r0.z, -Dot(b, t),
	                 r1.x, r1.y, r1.z,  Dot(a, t),
	                 r2.x, r2.y, r2.z, -Dot(d, s),
	                 r3.x, r3.y, r3.z,  Dot(c, s)));
}

// Listing 2.1

Matrix3D MakeRotationX(float t)
{
	float c = cos(t);
	float s = sin(t);

	return (Matrix3D(1.0F, 0.0F, 0.0F,
	                 0.0F,  c,   -s,
	                 0.0F,  s,    c  ));
}

Matrix3D MakeRotationY(float t)
{
	float c = cos(t);
	float s = sin(t);

	return (Matrix3D( c,   0.0F,  s,
	                 0.0F, 1.0F, 0.0F,
	                 -s,   0.0F,  c  ));
}

Matrix3D MakeRotationZ(float t)
{
	float c = cos(t);
	float s = sin(t);

	return (Matrix3D( c,   -s,   0.0F,
	                  s,    c,   0.0F,
	                 0.0F, 0.0F, 1.0F));
}

// Listing 2.2

Matrix3D MakeRotation(float t, const Vector3D& a)
{
	float c = cos(t);
	float s = sin(t);
	float d = 1.0F - c;

	float x = a.x * d;
	float y = a.y * d;
	float z = a.z * d;
	float axay = x * a.y;
	float axaz = x * a.z;
	float ayaz = y * a.z;

	return (Matrix3D(c + x * a.x, axay - s * a.z, axaz + s * a.y,
	                 axay + s * a.z, c + y * a.y, ayaz - s * a.x,
	                 axaz - s * a.y, ayaz + s * a.x, c + z * a.z));
}

// Listing 2.3

Matrix3D MakeReflection(const Vector3D& a)
{
	float x = a.x * -2.0F;
	float y = a.y * -2.0F;
	float z = a.z * -2.0F;
	float axay = x * a.y;
	float axaz = x * a.z;
	float ayaz = y * a.z;

	return (Matrix3D(x * a.x + 1.0F, axay, axaz,
	                 axay, y * a.y + 1.0F, ayaz,
	                 axaz, ayaz, z * a.z + 1.0F));
}

// Listing 2.4

Matrix3D MakeInvolution(const Vector3D& a)
{
	float x = a.x * 2.0F;
	float y = a.y * 2.0F;
	float z = a.z * 2.0F;
	float axay = x * a.y;
	float axaz = x * a.z;
	float ayaz = y * a.z;

	return (Matrix3D(x * a.x - 1.0F, axay, axaz,
	                 axay, y * a.y - 1.0F, ayaz,
	                 axaz, ayaz, z * a.z - 1.0F));
}

// Listing 2.5

Matrix3D MakeScale(float sx, float sy, float sz)
{
	return (Matrix3D(sx, 0.0F, 0.0F, 0.0F, sy, 0.0F, 0.0F, 0.0F, sz));
}

// Listing 2.6

Matrix3D MakeScale(float s, const Vector3D& a)
{
	s -= 1.0F;
	float x = a.x * s;
	float y = a.y * s;
	float z = a.z * s;
	float axay = x * a.y;
	float axaz = x * a.z;
	float ayaz = y * a.z;

	return (Matrix3D(x * a.x + 1.0F, axay, axaz,
	                 axay, y * a.y + 1.0F, ayaz,
	                 axaz, ayaz, z * a.z + 1.0F));
}

// Listing 2.7

Matrix3D MakeSkew(float t, const Vector3D& a, const Vector3D& b)
{
	t = tan(t);
	float x = a.x * t;
	float y = a.y * t;
	float z = a.z * t;

	return (Matrix3D(x * b.x + 1.0F, x * b.y, x * b.z,
	                 y * b.x, y * b.y + 1.0F, y * b.z,
	                 z * b.x, z * b.y, z * b.z + 1.0F));
}

// Listing 2.8

struct Point3D : Vector3D
{
	Point3D() = default;

	Point3D(float a, float b, float c) : Vector3D(a, b, c) {}

	Point3D& operator =(const Vector3D& v)
	{
		x = v.x;
		y = v.y;
		z = v.z;
		return (*this);
	}
};

inline Point3D operator +(const Point3D& a, const Vector3D& b)
{
	return (Point3D(a.x + b.x, a.y + b.y, a.z + b.z));
}

inline Point3D operator -(const Point3D& a, const Vector3D& b)
{
	return (Point3D(a.x - b.x, a.y - b.y, a.z - b.z));
}

inline Vector3D operator -(const Point3D& a, const Point3D& b)
{
	return (Vector3D(a.x - b.x, a.y - b.y, a.z - b.z));
}

// Listing 2.9

struct Transform4D : Matrix4D
{
	Transform4D() = default;

	Transform4D(float n00, float n01, float n02, float n03,
	            float n10, float n11, float n12, float n13,
	            float n20, float n21, float n22, float n23)
	{
		n[0][0] = n00; n[0][1] = n10; n[0][2] = n20;
		n[1][0] = n01; n[1][1] = n11; n[1][2] = n21;
		n[2][0] = n02; n[2][1] = n12; n[2][2] = n22;
		n[3][0] = n03; n[3][1] = n13; n[3][2] = n23;

		n[0][3] = n[1][3] = n[2][3] = 0.0F;
		n[3][3] = 1.0F;
	}

	Transform4D(const Vector3D& a, const Vector3D& b,
	            const Vector3D& c, const Point3D& p)
	{
		n[0][0] = a.x; n[0][1] = a.y; n[0][2] = a.z;
		n[1][0] = b.x; n[1][1] = b.y; n[1][2] = b.z;
		n[2][0] = c.x; n[2][1] = c.y; n[2][2] = c.z;
		n[3][0] = p.x; n[3][1] = p.y; n[3][2] = p.z;

		n[0][3] = n[1][3] = n[2][3] = 0.0F;
		n[3][3] = 1.0F;
	}

	Vector3D& operator [](int j)
	{
		return (*reinterpret_cast<Vector3D *>(n[j]));
	}

	const Vector3D& operator [](int j) const
	{
		return (*reinterpret_cast<const Vector3D *>(n[j]));
	}

	const Point3D& GetTranslation(void) const
	{
		return (*reinterpret_cast<const Point3D *>(n[3]));
	}

	void SetTranslation(const Point3D& p)
	{
		n[3][0] = p.x;
		n[3][1] = p.y;
		n[3][2] = p.z;
	}
};

Transform4D Inverse(const Transform4D& H)
{
	const Vector3D& a = H[0];
	const Vector3D& b = H[1];
	const Vector3D& c = H[2];
	const Vector3D& d = H[3];

	Vector3D s = Cross(a, b);
	Vector3D t = Cross(c, d);

	float invDet = 1.0F / Dot(s, c);

	s *= invDet;
	t *= invDet;
	Vector3D v = c * invDet;

	Vector3D r0 = Cross(b, v);
	Vector3D r1 = Cross(v, a);

	return (Transform4D(r0.x, r0.y, r0.z, -Dot(b, t),
	                    r1.x, r1.y, r1.z,  Dot(a, t),
	                     s.x,  s.y,  s.z, -Dot(d, s)));
}

Transform4D operator *(const Transform4D& A, const Transform4D& B)
{
	return (Transform4D(
		A(0,0) * B(0,0) + A(0,1) * B(1,0) + A(0,2) * B(2,0),
		A(0,0) * B(0,1) + A(0,1) * B(1,1) + A(0,2) * B(2,1),
		A(0,0) * B(0,2) + A(0,1) * B(1,2) + A(0,2) * B(2,2),
		A(0,0) * B(0,3) + A(0,1) * B(1,3) + A(0,2) * B(2,3) + A(0,3),
		A(1,0) * B(0,0) + A(1,1) * B(1,0) + A(1,2) * B(2,0),
		A(1,0) * B(0,1) + A(1,1) * B(1,1) + A(1,2) * B(2,1),
		A(1,0) * B(0,2) + A(1,1) * B(1,2) + A(1,2) * B(2,2),
		A(1,0) * B(0,3) + A(1,1) * B(1,3) + A(1,2) * B(2,3) + A(1,3),
		A(2,0) * B(0,0) + A(2,1) * B(1,0) + A(2,2) * B(2,0),
		A(2,0) * B(0,1) + A(2,1) * B(1,1) + A(2,2) * B(2,1),
		A(2,0) * B(0,2) + A(2,1) * B(1,2) + A(2,2) * B(2,2),
		A(2,0) * B(0,3) + A(2,1) * B(1,3) + A(2,2) * B(2,3) + A(2,3)));
}

Vector3D operator *(const Transform4D& H, const Vector3D& v)
{
	return (Vector3D(H(0,0) * v.x + H(0,1) * v.y + H(0,2) * v.z,
	                 H(1,0) * v.x + H(1,1) * v.y + H(1,2) * v.z,
	                 H(2,0) * v.x + H(2,1) * v.y + H(2,2) * v.z));
}

Point3D operator *(const Transform4D& H, const Point3D& p)
{
	return (Point3D(H(0,0) * p.x + H(0,1) * p.y + H(0,2) * p.z + H(0,3),
	                H(1,0) * p.x + H(1,1) * p.y + H(1,2) * p.z + H(1,3),
	                H(2,0) * p.x + H(2,1) * p.y + H(2,2) * p.z + H(2,3)));
}

// Listing 2.10

struct Quaternion
{
	float		x, y, z, w;

	Quaternion() = default;

	Quaternion(float a, float b, float c, float s)
	{
		x = a; y = b; z = c;
		w = s;
	}

	Quaternion(const Vector3D& v, float s)
	{
		x = v.x; y = v.y; z = v.z;
		w = s;
	}

	Vector3D& GetVectorPart(void)
	{
		return (reinterpret_cast<Vector3D&>(x));
	}

	const Vector3D& GetVectorPart(void) const
	{
		return (reinterpret_cast<const Vector3D&>(x));
	}

	Matrix3D GetRotationMatrix(void);
	void SetRotationMatrix(const Matrix3D& m);
};

Quaternion operator *(const Quaternion& q1, const Quaternion& q2)
{
	return (Quaternion(
		q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
		q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
		q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w,
		q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z));
}

// Listing 2.11

Vector3D Transform(const Vector3D& v, const Quaternion& q)
{
	const Vector3D& b = q.GetVectorPart();
	float b2 = b.x * b.x + b.y * b.y + b.z * b.z;
	return (v * (q.w * q.w - b2) + b * (Dot(v, b) * 2.0F)
		+ Cross(b, v) * (q.w * 2.0F));
}

// Listing 2.12

Matrix3D Quaternion::GetRotationMatrix(void)
{
	float x2 = x * x;
	float y2 = y * y;
	float z2 = z * z;
	float xy = x * y;
	float xz = x * z;
	float yz = y * z;
	float wx = w * x;
	float wy = w * y;
	float wz = w * z;

	return (Matrix3D(
		1.0F - 2.0F * (y2 + z2), 2.0F * (xy - wz), 2.0F * (xz + wy),
		2.0F * (xy + wz), 1.0F - 2.0F * (x2 + z2), 2.0F * (yz - wx),
		2.0F * (xz - wy), 2.0F * (yz + wx), 1.0F - 2.0F * (x2 + y2)));
}

// Listing 2.13

void Quaternion::SetRotationMatrix(const Matrix3D& m)
{
	float m00 = m(0,0);
	float m11 = m(1,1);
	float m22 = m(2,2);
	float sum = m00 + m11 + m22;

	if (sum > 0.0F)
	{
		w = sqrt(sum + 1.0F) * 0.5F;
		float f = 0.25F / w;

		x = (m(2,1) - m(1,2)) * f;
		y = (m(0,2) - m(2,0)) * f;
		z = (m(1,0) - m(0,1)) * f;
	}
	else if ((m00 > m11) && (m00 > m22))
	{
		x = sqrt(m00 - m11 - m22 + 1.0F) * 0.5F;
		float f = 0.25F / x;

		y = (m(1,0) + m(0,1)) * f;
		z = (m(0,2) + m(2,0)) * f;
		w = (m(2,1) - m(1,2)) * f;
	}
	else if (m11 > m22)
	{
		y = sqrt(m11 - m00 - m22 + 1.0F) * 0.5F;
		float f = 0.25F / y;

		x = (m(1,0) + m(0,1)) * f;
		z = (m(2,1) + m(1,2)) * f;
		w = (m(0,2) - m(2,0)) * f;
	}
	else
	{
		z = sqrt(m22 - m00 - m11 + 1.0F) * 0.5F;
		float f = 0.25F / z;

		x = (m(0,2) + m(2,0)) * f;
		y = (m(2,1) + m(1,2)) * f;
		w = (m(1,0) - m(0,1)) * f;
	}
}

// Listing 3.1

Vector3D operator *(const Vector3D& n, const Transform4D& H)
{
	return (Vector3D(n.x * H(0,0) + n.y * H(1,0) + n.z * H(2,0),
	                 n.x * H(0,1) + n.y * H(1,1) + n.z * H(2,1),
	                 n.x * H(0,2) + n.y * H(1,2) + n.z * H(2,2)));
}

// Listing 3.2

float DistPointLine(const Point3D& q, const Point3D& p, const Vector3D& v)
{
	Vector3D a = Cross(q - p, v);
	return (sqrt(Dot(a, a) / Dot(v, v)));
}

// Listing 3.3

float DistLineLine(const Point3D& p1, const Vector3D& v1,
                   const Point3D& p2, const Vector3D& v2)
{
	Vector3D dp = p2 - p1;

	float v12 = Dot(v1, v1);
	float v22 = Dot(v2, v2);
	float v1v2 = Dot(v1, v2);

	float det = v1v2 * v1v2 - v12 * v22;
	if (fabs(det) > FLT_MIN)
	{
		det = 1.0F / det;

		float dpv1 = Dot(dp, v1);
		float dpv2 = Dot(dp, v2);
		float t1 = (v1v2 * dpv2 - v22 * dpv1) * det;
		float t2 = (v12 * dpv2 - v1v2 * dpv1) * det;

		return (Magnitude(dp + v2 * t2 - v1 * t1));
	}

	// The lines are nearly parallel.

	Vector3D a = Cross(dp, v1);
	return (sqrt(Dot(a, a) / v12));
}

// Listing 3.4

struct Plane
{
	float		x, y, z, w;

	Plane() = default;

	Plane(float nx, float ny, float nz, float d)
	{
		x = nx;
		y = ny;
		z = nz;
		w = d;
	}

	Plane(const Vector3D& n, float d)
	{
		x = n.x;
		y = n.y;
		z = n.z;
		w = d;
	}

	const Vector3D& GetNormal(void) const
	{
		return (reinterpret_cast<const Vector3D&>(x));
	}
};

float Dot(const Plane& f, const Vector3D& v)
{
	return (f.x * v.x + f.y * v.y + f.z * v.z);
}

float Dot(const Plane& f, const Point3D& p)
{
	return (f.x * p.x + f.y * p.y + f.z * p.z + f.w);
}

// Listing 3.5

Transform4D MakeReflection(const Plane& f)
{
	float x = f.x * -2.0F;
	float y = f.y * -2.0F;
	float z = f.z * -2.0F;
	float nxny = x * f.y;
	float nxnz = x * f.z;
	float nynz = y * f.z;

	return (Transform4D(x * f.x + 1.0F, nxny, nxnz, x * f.w,
	                    nxny, y * f.y + 1.0F, nynz, y * f.w,
	                    nxnz, nynz, z * f.z + 1.0F, z * f.w));
}

// Listing 3.6

bool IntersectLinePlane(const Point3D& p, const Vector3D& v,
                        const Plane& f, Point3D *q)
{
	float fv = Dot(f, v);
	if (fabs(fv) > FLT_MIN)
	{
		*q = p - v * (Dot(f, p) / fv);
		return (true);
	}

	return (false);
}

// Listing 3.7

bool IntersectThreePlanes(const Plane& f1, const Plane& f2,
                          const Plane& f3, Point3D *p)
{
	const Vector3D& n1 = f1.GetNormal();
	const Vector3D& n2 = f2.GetNormal();
	const Vector3D& n3 = f3.GetNormal();

	Vector3D n1xn2 = Cross(n1, n2);
	float det = Dot(n1xn2, n3);
	if (fabs(det) > FLT_MIN)
	{
		*p = (Cross(n3, n2) * f1.w + Cross(n1, n3) * f2.w
		        - n1xn2 * f3.w) / det;
		return (true);
	}

	return (false);
}

// Listing 3.8

bool IntersectTwoPlanes(const Plane& f1, const Plane& f2,
                        Point3D *p, Vector3D *v)
{
	const Vector3D& n1 = f1.GetNormal();
	const Vector3D& n2 = f2.GetNormal();

	*v = Cross(n1, n2);
	float det = Dot(*v, *v);
	if (fabs(det) > FLT_MIN)
	{
		*p = (Cross(*v, n2) * f1.w + Cross(n1, *v) * f2.w) / det;
		return (true);
	}

	return (false);
}

// Listing 3.9

Plane operator *(const Plane& f, const Transform4D& H)
{
	return (Plane(f.x * H(0,0) + f.y * H(1,0) + f.z * H(2,0),
	              f.x * H(0,1) + f.y * H(1,1) + f.z * H(2,1),
	              f.x * H(0,2) + f.y * H(1,2) + f.z * H(2,2),
	              f.x * H(0,3) + f.y * H(1,3) + f.z * H(2,3) + f.w));
}

// Listing 3.10

struct Line
{
	Vector3D	direction;
	Vector3D	moment;

	Line() = default;

	Line(float vx, float vy, float vz, float mx, float my, float mz) :
		direction(vx, vy, vz), moment(mx, my, mz)
	{
	}

	Line(const Vector3D& v, const Vector3D& m)
	{
		direction = v;
		moment = m;
	}
};

// Listing 3.11

Line Transform(const Line& line, const Transform4D& H)
{
	Matrix3D adj(Cross(H[1], H[2]), Cross(H[2], H[0]), Cross(H[0], H[1]));
	const Point3D& t = H.GetTranslation();

	Vector3D v = H * line.direction;
	Vector3D m = adj * line.moment + Cross(t, v);
	return (Line(v, m));
}

// Listing 4.1

inline Line operator ^(const Point3D& p, const Point3D& q)
{
	return (Line(q.x - p.x, q.y - p.y, q.z - p.z,
	  p.y * q.z - p.z * q.y, p.z * q.x - p.x * q.z, p.x * q.y - p.y * q.x));
}

inline Line operator ^(const Plane& f, const Plane& g)
{
	return (Line(f.z * g.y - f.y * g.z,
	             f.x * g.z - f.z * g.x,
	             f.y * g.x - f.x * g.y,
	             f.x * g.w - f.w * g.x,
	             f.y * g.w - f.w * g.y,
	             f.z * g.w - f.w * g.z));
}

inline Plane operator ^(const Line& L, const Point3D& p)
{
	return (Plane(L.direction.y * p.z - L.direction.z * p.y + L.moment.x,
	              L.direction.z * p.x - L.direction.x * p.z + L.moment.y,
	              L.direction.x * p.y - L.direction.y * p.x + L.moment.z,
	             -L.moment.x * p.x - L.moment.y * p.y - L.moment.z * p.z));
}

inline Plane operator ^(const Point3D& p, const Line& L)
{
	return (L ^ p);
}

inline Vector4D operator ^(const Line& L, const Plane& f)
{
	return (Vector4D(
	      L.moment.y * f.z - L.moment.z * f.y + L.direction.x * f.w,
	      L.moment.z * f.x - L.moment.x * f.z + L.direction.y * f.w,
	      L.moment.x * f.y - L.moment.y * f.x + L.direction.z * f.w,
	     -L.direction.x * f.x - L.direction.y * f.y - L.direction.z * f.z));
}

inline Vector4D operator ^(const Plane& f, const Line& L)
{
	return (L ^ f);
}

inline float operator ^(const Line& L1, const Line& L2)
{
	return (-(Dot(L1.direction, L2.moment) + Dot(L2.direction, L1.moment)));
}

inline float operator ^(const Point3D& p, const Plane& f)
{
	return (p.x * f.x + p.y * f.y + p.z * f.z + f.w);
}

inline float operator ^(const Plane& f, const Point3D& p)
{
	return (-(p ^ f));
}