/*
 * Copyright (c) 2021, Jeffrey Lee
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met: 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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 HOLDER 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.
 */
#ifndef PLANE2D_H
#define PLANE2D_H

template<typename T> class plane2d
{
private:
	T p;
	typename T::value_type d;

public:
	typedef typename T::value_type value_type;
	typedef T coord_type;

	inline plane2d() {}

	/* Construct plane from normal and distance */
	inline plane2d(coord_type const& n, value_type dist)
	{
		p = n;
		d = dist;
	}

	/* Construct plane from normal and a point on the plane */
	inline plane2d(coord_type const& n,coord_type const& a)
	{
		p = n;
		d = -dot(n,a);
	}

	/* Return plane normal */
	inline coord_type normal() const
	{
		return p;
	}

	/* Return distance from origin */
	inline value_type dist() const
	{
		return d;
	}

	/* Return signed distance from point, along plane normal vector */
	template<typename V>
	inline typename V::value_type dist(V const& a) const
	{
		return dot(a, normal()) + d;
	}

	/* Return signed distance from point, along the given direction vector */
	template<typename V>
	inline typename V::value_type dist(V const& a,V const& dir) const
	{
		typename V::value_type d1 = dist(a); /* Distance along plane normal vector */
		typename V::value_type d2 = dot(dir, normal()); /* Scale factor for result */
		return -d1*recp(d2);
	}

	/* Plane-line intersection (as fraction of distance along line)
	   (Any benefit over just using dist(v0,v1-v0)? slightly faster because scalar subtraction instead of vector subtraction?) */
	template<typename V>
	inline typename V::value_type intersection(V const& v0, V const& v1) const
	{
		typename V::value_type d0 = dist(v0);
		typename V::value_type d1 = dist(v1);
		return -d0*recp(d1-d0);
	}

	/* Plane-line intersection (as point) */
	template<typename V>
	inline V intersection_point(V const& v0, V const& v1) const
	{
		typename V::value_type dist = intersection(v0,v1);
		return v0 + (v1-v0)*dist;
	}

	/* Plane-plane intersection */
	inline coord_type intersection(plane2d const& b) const
	{
		coord_type v;
		v[1] = (d*b.p[0] - b.d*p[0]) * recp(b.p[1]*p[0] - p[1]*b.p[0]);
		if (abs(p[0]) > abs(b.p[0]))
		{
			v[0] = -(v[1]*p[1] + d)*recp(p[0]);
		}
		else
		{
			v[0] = -(v[1]*b.p[1] + b.d)*recp(b.p[0]);
		}
		return v;
	}

	/* Flip the plane */
	inline plane2d operator-() const
	{
		plane2d p2;
		p2.p = -p;
		p2.d = -d;
		return p2;
	}

	/* Move to a different position */
	inline void set_position(coord_type const& a)
	{
		d = -dot(normal(),a);
	}

	/* Project vector onto plane */
	template<typename V>
	inline V project(V const& v) const
	{
		return v - normal()*dot(v, normal());
	}
};

#endif
