// intersect.cpp (Backends to intersection functions)
//
//  The WorldForge Project
//  Copyright (C) 2002  The WorldForge Project
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//  For information about WorldForge and its authors, please contact
//  the Worldforge Web Site at http://www.worldforge.org.
//

// Author: Ron Steinke

#include "intersect.h"

#include <cmath>

#include <cassert>

namespace WFMath {


// The 2d implementation was inspired as a simplification of the 3d.
// It used the fact that two not-similarly-oriented rectangles a and b
// intersect each other if and only if a's bounding box in b's coordinate
// system intersects b, and vice versa.
//
// To see this, it is only necessary to consider the bounding box intersection
// => intersection side of the assertion, since if the rectangles intersect,
// clearly their bounding boxes will as well. Let B(a) be the bounding box of
// a in b's coordinate system, and A(b) be the bounding box of b in a's coordinate
// system. Let the symbol ^ denote intersection. The thing we need to prove is:
//
// a ^ A(b) && b ^ B(a) => a ^ b
//
// I will discuss the equivalent statement
//
// a ^ A(b) && !(a ^ b) => !(b ^ B(a))
//
// If a intersects b's bounding box, but does not intersect b,
// the intersection of a and A(b) is a rectangle which lies in
// one of the four corners of the region A(b) - b (the set of all
// points which lie in the bounding box, but not in b). Without
// loss of generality, let a ^ A(b) lie in the lower left corner
// of A(b) - b. This is a triangular region, two of whose sides
// are parts of edges of A(b), the third being a side of b.
// Construct a line parallel to the side of b, passing between
// b and a ^ A(b). This line never intersects b, since b is a rectangle.
// It also never intersects a, since it passes above and to the
// right of the upper right corner of a. It also never intersects
// B(a), since the upper right side of B(a) is parallel to the
// line, but passes through the upper right corner of a. Thus,
// this line separates b from B(a), and they do not intersect. QED.

template<>
bool Intersect<2>(const RotBox<2>& r, const AxisBox<2>& b, bool proper) {
	const AxisBox<2> b2 = r.boundingBox();
	if (!Intersect(b2, b, proper))
		return false;

	RotMatrix<2> m = r.m_orient.inverse();

	const AxisBox<2> b3 = RotBox<2>(Point<2>(b.m_low).rotate(m, r.m_corner0),
									b.m_high - b.m_low, m).boundingBox();
	const AxisBox<2> b4(r.m_corner0, r.m_corner0 + r.m_size);
	return Intersect(b3, b4, proper);
}

// The 3d implementation is based on the following theorem:
//
// Theorem:
//
// Two convex polyhedra do not intersect if and only if there exists a separating
// plane which is either parallel to the face of one polyhedron or which is
// parallel to at least one edge of each polyhedron.
//
// Found this in the abstract to the paper:
//
// Surface-to-surface intersection based on triangular parameter domain subdivision
// Ernst Huber
// Institute of Computer Graphics
// Vienna University of Technology
// A-1040 Vienna, Karlsplatz 13/186/1, Austria
//
// online postscript version of the abstract (where I got this from):
//
// http://www.cs.ubc.ca/conferences/CCCG/elec_proc/c48.ps.gz
//
// The paper gives a reference for the theorem (probably the one to really look at
// for proof/details):
//
// S. Gottschalk
// Separating Axis Theorem
// Technical Report TR96-024
// Department of Computer Science
// UNC Chapel Hill
// 1996

template<>
bool Intersect<3>(const RotBox<3>& r, const AxisBox<3>& b, bool proper) {
	// Checking intersection of each with the bounding box of
	// the other in the coordinate system of the first will take care
	// of the "plane parallel to face" case

	const AxisBox<3> b2 = r.boundingBox();
	if (!Intersect(b2, b, proper))
		return false;

	RotMatrix<3> minv = r.m_orient.inverse();
	Vector<3> b_size = b.m_high - b.m_low;

	const AxisBox<3> b3 = RotBox<3>(Point<3>(b.m_low).rotate(minv, r.m_corner0),
									b_size, minv).boundingBox();
	const AxisBox<3> b4(r.m_corner0, r.m_corner0 + r.m_size);
	if (!Intersect(b3, b4, proper))
		return false;

	// Now for the "plane parallel to at least one edge of each" case

	Vector<3> sep = b.m_low - r.m_corner0;
	const RotMatrix<3>& m = r.m_orient;

	// Generate normals to the 9 possible separating planes

	for (int i = 0; i < 3; ++i) {
		// Generate edge vectors for the RotBox, ignore size, only care about direction
// Just access m_orient directly below instead of using r_vec
//    Vector<3> r_vec = m.row(i);

		for (int j = 0; j < 3; ++j) {
			Vector<3> axis;

			// Cross product, ignore size of b since we only care about direction

			switch (j) {
				case 0:
					axis[0] = 0;
					axis[1] = -m.elem(i, 2);
					axis[2] = m.elem(i, 1);
					break;
				case 1:
					axis[0] = m.elem(i, 2);
					axis[1] = 0;
					axis[2] = -m.elem(i, 0);
					break;
				case 2:
					axis[0] = -m.elem(i, 1);
					axis[1] = m.elem(i, 0);
					axis[2] = 0;
					break;
				default:
					assert(false);
			}

			if (axis.sqrMag() < numeric_constants<CoordType>::epsilon() * numeric_constants<CoordType>::epsilon()) {
				// Parallel edges, this reduces to the 2d case above. We've already
				// checked the bounding box intersections, so we know they intersect.
				// We don't need to scale WFMATH_EPSILON, det(m_orient) = 1
				// essentially takes care of that.
				return true;
			}

			// Project both boxes on this axis, check for separating plane.

			// We only need to project two axes per box, the one parallel
			// to the plane doesn't contribute

			const int next[] = {1, 2, 0};
			CoordType val;
			CoordType b_low, b_high, r_low, r_high, dist;
			int k;

			// AxisBox projection

			k = next[j];

			val = axis[k] * b_size[k];

			if (val > 0) {
				b_high = val;
				b_low = 0;
			} else {
				b_low = val;
				b_high = 0;
			}

			k = next[k];

			val = axis[k] * b_size[k];

			if (val > 0)
				b_high += val;
			else
				b_low += val;

			// RotBox projection

			k = next[i];

			val = Dot(m.row(k), axis) * r.m_size[k];

			if (val > 0) {
				r_high = val;
				r_low = 0;
			} else {
				r_low = val;
				r_high = 0;
			}

			k = next[k];

			val = Dot(m.row(k), axis) * r.m_size[k];

			if (val > 0)
				r_high += val;
			else
				r_low += val;

			// Distance between basepoints for boxes along this axis

			dist = Dot(sep, axis);

			if (_Greater(r_low - dist, b_high, proper)
				|| _Less(r_high - dist, b_low, proper))
				return false;
		}
	}

	return true;
}

// force a bunch of instantiations

template bool Intersect<2>(const Point<2>&, const Point<2>&, bool);

template bool Intersect<3>(const Point<3>&, const Point<3>&, bool);

template bool Contains<2>(const Point<2>&, const Point<2>&, bool);

template bool Contains<3>(const Point<3>&, const Point<3>&, bool);

template bool Intersect<Point<2>, AxisBox<2> >(const Point<2>&, const AxisBox<2>&, bool);

template bool Intersect<Point<3>, AxisBox<3> >(const Point<3>&, const AxisBox<3>&, bool);

template bool Contains<2>(const Point<2>&, const AxisBox<2>&, bool);

template bool Contains<3>(const Point<3>&, const AxisBox<3>&, bool);

template bool Intersect<2>(const AxisBox<2>&, const Point<2>&, bool);

template bool Intersect<3>(const AxisBox<3>&, const Point<3>&, bool);

template bool Contains<2>(const AxisBox<2>&, const Point<2>&, bool);

template bool Contains<3>(const AxisBox<3>&, const Point<3>&, bool);

template bool Intersect<2>(const AxisBox<2>&, const AxisBox<2>&, bool);

template bool Intersect<3>(const AxisBox<3>&, const AxisBox<3>&, bool);

template bool Contains<2>(const AxisBox<2>&, const AxisBox<2>&, bool);

template bool Contains<3>(const AxisBox<3>&, const AxisBox<3>&, bool);

template bool Intersect<Point<2>, Ball<2> >(const Point<2>&, const Ball<2>&, bool);

template bool Intersect<Point<3>, Ball<3> >(const Point<3>&, const Ball<3>&, bool);

template bool Contains<2>(const Point<2>&, const Ball<2>&, bool);

template bool Contains<3>(const Point<3>&, const Ball<3>&, bool);

template bool Intersect<2>(const Ball<2>&, const Point<2>&, bool);

template bool Intersect<3>(const Ball<3>&, const Point<3>&, bool);

template bool Contains<2>(const Ball<2>&, const Point<2>&, bool);

template bool Contains<3>(const Ball<3>&, const Point<3>&, bool);

template bool Intersect<AxisBox<2>, Ball<2> >(const AxisBox<2>&, const Ball<2>&, bool);

template bool Intersect<AxisBox<3>, Ball<3> >(const AxisBox<3>&, const Ball<3>&, bool);

template bool Contains<2>(const AxisBox<2>&, const Ball<2>&, bool);

template bool Contains<3>(const AxisBox<3>&, const Ball<3>&, bool);

template bool Intersect<2>(const Ball<2>&, const AxisBox<2>&, bool);

template bool Intersect<3>(const Ball<3>&, const AxisBox<3>&, bool);

template bool Contains<2>(const Ball<2>&, const AxisBox<2>&, bool);

template bool Contains<3>(const Ball<3>&, const AxisBox<3>&, bool);

template bool Intersect<2>(const Ball<2>&, const Ball<2>&, bool);

template bool Intersect<3>(const Ball<3>&, const Ball<3>&, bool);

template bool Contains<2>(const Ball<2>&, const Ball<2>&, bool);

template bool Contains<3>(const Ball<3>&, const Ball<3>&, bool);

template bool Intersect<Point<2>, Segment<2> >(const Point<2>&, const Segment<2>&, bool);

template bool Intersect<Point<3>, Segment<3> >(const Point<3>&, const Segment<3>&, bool);

template bool Contains<2>(const Point<2>&, const Segment<2>&, bool);

template bool Contains<3>(const Point<3>&, const Segment<3>&, bool);

template bool Intersect<2>(const Segment<2>&, const Point<2>&, bool);

template bool Intersect<3>(const Segment<3>&, const Point<3>&, bool);

template bool Contains<2>(const Segment<2>&, const Point<2>&, bool);

template bool Contains<3>(const Segment<3>&, const Point<3>&, bool);

template bool Intersect<AxisBox<2>, Segment<2> >(const AxisBox<2>&, const Segment<2>&, bool);

template bool Intersect<AxisBox<3>, Segment<3> >(const AxisBox<3>&, const Segment<3>&, bool);

template bool Contains<2>(const AxisBox<2>&, const Segment<2>&, bool);

template bool Contains<3>(const AxisBox<3>&, const Segment<3>&, bool);

template bool Intersect<2>(const Segment<2>&, const AxisBox<2>&, bool);

template bool Intersect<3>(const Segment<3>&, const AxisBox<3>&, bool);

template bool Contains<2>(const Segment<2>&, const AxisBox<2>&, bool);

template bool Contains<3>(const Segment<3>&, const AxisBox<3>&, bool);

template bool Intersect<Ball<2>, Segment<2> >(const Ball<2>&, const Segment<2>&, bool);

template bool Intersect<Ball<3>, Segment<3> >(const Ball<3>&, const Segment<3>&, bool);

template bool Contains<2>(const Ball<2>&, const Segment<2>&, bool);

template bool Contains<3>(const Ball<3>&, const Segment<3>&, bool);

template bool Intersect<2>(const Segment<2>&, const Ball<2>&, bool);

template bool Intersect<3>(const Segment<3>&, const Ball<3>&, bool);

template bool Contains<2>(const Segment<2>&, const Ball<2>&, bool);

template bool Contains<3>(const Segment<3>&, const Ball<3>&, bool);

template bool Intersect<2>(const Segment<2>&, const Segment<2>&, bool);

template bool Intersect<3>(const Segment<3>&, const Segment<3>&, bool);

template bool Contains<2>(const Segment<2>&, const Segment<2>&, bool);

template bool Contains<3>(const Segment<3>&, const Segment<3>&, bool);

template bool Intersect<Point<2>, RotBox<2> >(const Point<2>&, const RotBox<2>&, bool);

template bool Intersect<Point<3>, RotBox<3> >(const Point<3>&, const RotBox<3>&, bool);

template bool Contains<2>(const Point<2>&, const RotBox<2>&, bool);

template bool Contains<3>(const Point<3>&, const RotBox<3>&, bool);

template bool Intersect<2>(const RotBox<2>&, const Point<2>&, bool);

template bool Intersect<3>(const RotBox<3>&, const Point<3>&, bool);

template bool Contains<2>(const RotBox<2>&, const Point<2>&, bool);

template bool Contains<3>(const RotBox<3>&, const Point<3>&, bool);

template bool Intersect<AxisBox<2>, RotBox<2> >(const AxisBox<2>&, const RotBox<2>&, bool);

template bool Intersect<AxisBox<3>, RotBox<3> >(const AxisBox<3>&, const RotBox<3>&, bool);

template bool Contains<2>(const AxisBox<2>&, const RotBox<2>&, bool);

template bool Contains<3>(const AxisBox<3>&, const RotBox<3>&, bool);

template bool Contains<2>(const RotBox<2>&, const AxisBox<2>&, bool);

template bool Contains<3>(const RotBox<3>&, const AxisBox<3>&, bool);

template bool Intersect<Ball<2>, RotBox<2> >(const Ball<2>&, const RotBox<2>&, bool);

template bool Intersect<Ball<3>, RotBox<3> >(const Ball<3>&, const RotBox<3>&, bool);

template bool Contains<2>(const Ball<2>&, const RotBox<2>&, bool);

template bool Contains<3>(const Ball<3>&, const RotBox<3>&, bool);

template bool Intersect<2>(const RotBox<2>&, const Ball<2>&, bool);

template bool Intersect<3>(const RotBox<3>&, const Ball<3>&, bool);

template bool Contains<2>(const RotBox<2>&, const Ball<2>&, bool);

template bool Contains<3>(const RotBox<3>&, const Ball<3>&, bool);

template bool Intersect<Segment<2>, RotBox<2> >(const Segment<2>&, const RotBox<2>&, bool);

template bool Intersect<Segment<3>, RotBox<3> >(const Segment<3>&, const RotBox<3>&, bool);

template bool Contains<2>(const Segment<2>&, const RotBox<2>&, bool);

template bool Contains<3>(const Segment<3>&, const RotBox<3>&, bool);

template bool Intersect<2>(const RotBox<2>&, const Segment<2>&, bool);

template bool Intersect<3>(const RotBox<3>&, const Segment<3>&, bool);

template bool Contains<2>(const RotBox<2>&, const Segment<2>&, bool);

template bool Contains<3>(const RotBox<3>&, const Segment<3>&, bool);

template bool Intersect<2>(const RotBox<2>&, const RotBox<2>&, bool);

template bool Intersect<3>(const RotBox<3>&, const RotBox<3>&, bool);

template bool Contains<2>(const RotBox<2>&, const RotBox<2>&, bool);

template bool Contains<3>(const RotBox<3>&, const RotBox<3>&, bool);


}
