From 3946f9157c52be15503c232346a226d74970b593 Mon Sep 17 00:00:00 2001
From: rexy712 <rexerton.rexy@tutamail.com>
Date: Mon, 11 Jan 2021 16:38:10 -0800
Subject: [PATCH] Add point-rectangle collision check

---
 include/engine/base_types.hpp |   5 +-
 include/engine/collision.hpp  |  48 +++----
 include/math/vec.hpp          |   5 +-
 include/math/vec.tpp          |   8 +-
 src/engine/collision.cpp      | 244 ++++++++++++++++++++--------------
 5 files changed, 183 insertions(+), 127 deletions(-)

diff --git a/include/engine/base_types.hpp b/include/engine/base_types.hpp
index 6b713c8..b697219 100644
--- a/include/engine/base_types.hpp
+++ b/include/engine/base_types.hpp
@@ -28,18 +28,19 @@ namespace egn{
 	public:
 		using math::vec3<float>::vec3;
 	};
-	class line
+	class line_segment
 	{
 	public:
 		point point1;
 		point point2;
 	};
-	class plane
+	class rectangle
 	{
 	public:
 		point point1;
 		point point2;
 		point point3;
+		point point4;
 	};
 	class aabb
 	{
diff --git a/include/engine/collision.hpp b/include/engine/collision.hpp
index 72ad185..9e3c8c7 100644
--- a/include/engine/collision.hpp
+++ b/include/engine/collision.hpp
@@ -52,8 +52,8 @@ namespace egn{
 			using egn::point::point;
 			using egn::point::operator=;
 		};
-		class line : public egn::line, public collidable<line>{};
-		class plane : public egn::plane, public collidable<plane>{};
+		class line_segment : public egn::line_segment, public collidable<line_segment>{};
+		class rectangle : public egn::rectangle, public collidable<rectangle>{};
 		class aabb : public egn::aabb, public collidable<aabb>
 		{
 		public:
@@ -78,8 +78,8 @@ namespace egn{
 	public:
 		virtual void visit(const collision::aabb& a) = 0;
 		virtual void visit(const collision::sphere& a) = 0;
-		virtual void visit(const collision::line& a) = 0;
-		virtual void visit(const collision::plane& a) = 0;
+		virtual void visit(const collision::line_segment& a) = 0;
+		virtual void visit(const collision::rectangle& a) = 0;
 		virtual void visit(const collision::point& p) = 0;
 	};
 
@@ -94,8 +94,8 @@ namespace egn{
 		constexpr explicit collision_visitor(const T& l, float epsilon);
 		void visit(const collision::aabb& a)override;
 		void visit(const collision::sphere& a)override;
-		void visit(const collision::line& a)override;
-		void visit(const collision::plane& a)override;
+		void visit(const collision::line_segment& a)override;
+		void visit(const collision::rectangle& a)override;
 		void visit(const collision::point& p)override;
 		constexpr bool result()const;
 	};
@@ -103,29 +103,29 @@ namespace egn{
 	//bool check_collision(const collidable_iface& l, const collidable_iface& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const aabb& l, const aabb& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const aabb& l, const sphere& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const aabb& l, const line& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const aabb& l, const plane& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const aabb& l, const line_segment& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const aabb& l, const rectangle& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const aabb& l, const point& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const sphere& l, const sphere& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const sphere& l, const aabb& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const sphere& l, const line& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const sphere& l, const plane& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const sphere& l, const line_segment& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const sphere& l, const rectangle& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const sphere& l, const point& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const line& l, const line& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const line& l, const aabb& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const line& l, const sphere& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const line& l, const plane& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const line& l, const point& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const plane& l, const plane& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const plane& l, const aabb& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const plane& l, const sphere& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const plane& l, const line& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const plane& l, const point& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const line_segment& l, const line_segment& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const line_segment& l, const aabb& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const line_segment& l, const sphere& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const line_segment& l, const rectangle& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const line_segment& l, const point& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const rectangle& l, const rectangle& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const rectangle& l, const aabb& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const rectangle& l, const sphere& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const rectangle& l, const line_segment& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const rectangle& l, const point& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const point& l, const point& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const point& l, const aabb& r, float epsilon = default_collision_epsilon);
 	bool check_collision(const point& l, const sphere& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const point& l, const line& r, float epsilon = default_collision_epsilon);
-	bool check_collision(const point& l, const plane& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const point& l, const line_segment& r, float epsilon = default_collision_epsilon);
+	bool check_collision(const point& l, const rectangle& r, float epsilon = default_collision_epsilon);
 
 
 	template<typename Derived>
@@ -152,11 +152,11 @@ namespace egn{
 		m_result = check_collision(m_l, a, m_epsilon);
 	}
 	template<typename T>
-	void collision_visitor<T>::visit(const collision::line& a){
+	void collision_visitor<T>::visit(const collision::line_segment& a){
 		m_result = check_collision(m_l, a, m_epsilon);
 	}
 	template<typename T>
-	void collision_visitor<T>::visit(const collision::plane& a){
+	void collision_visitor<T>::visit(const collision::rectangle& a){
 		m_result = check_collision(m_l, a, m_epsilon);
 	}
 	template<typename T>
diff --git a/include/math/vec.hpp b/include/math/vec.hpp
index a73aca3..92ddf4a 100644
--- a/include/math/vec.hpp
+++ b/include/math/vec.hpp
@@ -71,7 +71,7 @@ namespace math{
 		template<typename U = T>
 		constexpr const_reference w()const;
 
-		value_type magnitude();
+		value_type magnitude()const;
 		vector normalize();
 	protected:
 		template<typename U, typename... Args>
@@ -85,6 +85,9 @@ namespace math{
 	template<typename T, typename U>
 	constexpr auto cross(const vector<T,3>& left, const vector<U,3>& right);
 
+	template<typename T, size_t R>
+	constexpr auto magnitude(const vector<T,R>& v);
+
 	template<typename T, typename U, size_t C, size_t R>
 	constexpr auto operator*(const matrix<U,R,C>& left, const vector<T,C>& right);
 	template<typename T, typename U, size_t R>
diff --git a/include/math/vec.tpp b/include/math/vec.tpp
index e74b908..1c7628f 100644
--- a/include/math/vec.tpp
+++ b/include/math/vec.tpp
@@ -103,7 +103,7 @@ namespace math{
 		return this->m_data[3];
 	}
 	template<typename T, size_t R>
-	auto vector<T,R>::magnitude() -> value_type{
+	auto vector<T,R>::magnitude()const -> value_type{
 		value_type sum = 0;
 		for(size_type i = 0;i < R;++i){
 			sum += (this->m_data[i] * this->m_data[i]);
@@ -130,6 +130,10 @@ namespace math{
 	                     (left[2] * right[0]) - (left[0] * right[2]),
 	                     (left[0] * right[1]) - (left[1] * right[0]));
 	}
+	template<typename T, size_t R>
+	constexpr auto magnitude(const vector<T,R>& v){
+		return v.magnitude();
+	}
 
 	template<typename T, typename U, size_t C, size_t R>
 	constexpr auto operator*(const matrix<U,R,C>& left, const vector<T,C>& right){
@@ -168,7 +172,7 @@ namespace math{
 		using res_t = decltype(std::declval<U>() * std::declval<T>());
 		vector<res_t,R> res(zero_initialize);
 		for(size_t i = 0; i < R; ++i){
-			res[i] = std::forward<U>(right) * left[i];
+			res[i] = std::forward<U>(left) * right[i];
 		}
 		return res;
 	}
diff --git a/src/engine/collision.cpp b/src/engine/collision.cpp
index 316a235..20a1f66 100644
--- a/src/engine/collision.cpp
+++ b/src/engine/collision.cpp
@@ -37,85 +37,7 @@ namespace egn{
 
 	}
 
-/*	bool check_collision(const collidable_iface& l, const collidable_iface& r, float epsilon){
-		return l.check_collision(r, epsilon);
-	}*/
-	bool check_collision(const aabb& l, const aabb& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const aabb& l, const sphere& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const aabb& l, const line& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const aabb& l, const plane& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const aabb& l, const point& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const sphere& l, const sphere& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const sphere& l, const aabb& r, float epsilon){
-		return check_collision(r, l, epsilon);
-	}
-	bool check_collision(const sphere& l, const line& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const sphere& l, const plane& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const sphere& l, const point& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const line& l, const line& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const line& l, const sphere& r, float epsilon){
-		return check_collision(r, l, epsilon);
-	}
-	bool check_collision(const line& l, const aabb& r, float epsilon){
-		return check_collision(r, l, epsilon);
-	}
-	bool check_collision(const line& l, const plane& r, float epsilon){
-		(void)l;
-		(void)r;
-		(void)epsilon;
-		return false;
-	}
-	bool check_collision(const line& l, const point& r, float epsilon){
+	static math::vec3<float> lineseg_point_proj(const line_segment& l, const math::vec3<float>& r){
 		/*
 		          C (the point)
 		          .
@@ -136,48 +58,174 @@ namespace egn{
 			u = ab / |ab|
 			dist = |C - L(b)|
 		*/
+
 		math::vec3<float> ab = (l.point2 - l.point1);
 		math::vec3<float> ac = (r - l.point1);
 
 		//check if point is within the line segment
 		//don't need to divide b by (ab * ab) to check if it's less than 0
 		float b = ac * ab;
-		if(b < (0 - epsilon))
-			return false;
+		if(b <= 0)
+			return l.point1;
 
 		//don't need to divide b by (ab * ab) to check if it's greater than (ab * ab)
 		float ab2 = ab * ab;
-		if(b > (ab2 + epsilon))
-			return false;
+		if(b >= ab2)
+			return l.point2;
 
 		//now divide once we know that the point is within this line segment
 		b = b / ab2;
 
-		//check if the point is zero(ish) distance from the line
+		//calculate the point using the parametric equation for the line
 		math::vec3<float> pb = l.point1 + (ab * b);
-		return math::fuzzy_eq(r - pb, math::vec3<float>(math::zero_initialize), epsilon);
+		return pb;
 	}
-	bool check_collision(const plane& l, const plane& r, float epsilon){
+	static math::vec3<float> rectangle_point_coplanar_proj(const rectangle& l, const math::vec3<float>& r){
+		//point1 = b
+		//point2 = c
+		//point3 = d
+		//point4 = e
+		math::vec3<float> cb = l.point2 - l.point1;
+		math::vec3<float> eb = l.point4 - l.point1;
+
+		float r1 = l.point1 * cb;
+		float r2 =        r * cb;
+		float r3 = l.point2 * cb;
+
+		float r4 = l.point1 * eb;
+		float r5 =        r * eb;
+		float r6 = l.point4 * eb;
+
+		if(r1 > r2 || r3 < r2 || r4 > r5 || r6 < r5){
+			//point is not in rectangle
+			math::vec3<float> cand1, cand2;
+			if(r1 > r2){
+				//consider be segment
+				cand1 = lineseg_point_proj(line_segment{l.point1, l.point4}, r);
+ 			}else{
+				//consider cd segment
+				cand1 = lineseg_point_proj(line_segment{l.point2, l.point3}, r);
+			}
+			if(r4 > r5){
+				//consider bc segment
+				cand2 = lineseg_point_proj(line_segment{l.point1, l.point2}, r);
+			}else{
+				//consider de segment
+				cand2 = lineseg_point_proj(line_segment{l.point3, l.point4}, r);
+			}
+			math::vec3<float> diff1 = r - cand1;
+			math::vec3<float> diff2 = r - cand2;
+			return (diff1 * diff1) > (diff2 * diff2) ? cand1 : cand2;
+		}
+		//point is inside rectangle
+		return r;
+	}
+/*	bool check_collision(const collidable_iface& l, const collidable_iface& r, float epsilon){
+		return l.check_collision(r, epsilon);
+	}*/
+	bool check_collision(const aabb& l, const aabb& r, float epsilon){
 		(void)l;
 		(void)r;
 		(void)epsilon;
 		return false;
 	}
-	bool check_collision(const plane& l, const aabb& r, float epsilon){
-		return check_collision(r, l, epsilon);
-	}
-	bool check_collision(const plane& l, const sphere& r, float epsilon){
-		return check_collision(r, l, epsilon);
-	}
-	bool check_collision(const plane& l, const line& r, float epsilon){
-		return check_collision(r, l, epsilon);
-	}
-	bool check_collision(const plane& l, const point& r, float epsilon){
+	bool check_collision(const aabb& l, const sphere& r, float epsilon){
 		(void)l;
 		(void)r;
 		(void)epsilon;
 		return false;
 	}
+	bool check_collision(const aabb& l, const line_segment& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const aabb& l, const rectangle& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const aabb& l, const point& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const sphere& l, const sphere& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const sphere& l, const aabb& r, float epsilon){
+		return check_collision(r, l, epsilon);
+	}
+	bool check_collision(const sphere& l, const line_segment& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const sphere& l, const rectangle& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const sphere& l, const point& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const line_segment& l, const line_segment& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const line_segment& l, const sphere& r, float epsilon){
+		return check_collision(r, l, epsilon);
+	}
+	bool check_collision(const line_segment& l, const aabb& r, float epsilon){
+		return check_collision(r, l, epsilon);
+	}
+	bool check_collision(const line_segment& l, const rectangle& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const line_segment& l, const point& r, float epsilon){
+		return math::fuzzy_eq(r - lineseg_point_proj(l, r), math::vec3<float>(math::zero_initialize), epsilon);
+	}
+	bool check_collision(const rectangle& l, const rectangle& r, float epsilon){
+		(void)l;
+		(void)r;
+		(void)epsilon;
+		return false;
+	}
+	bool check_collision(const rectangle& l, const aabb& r, float epsilon){
+		return check_collision(r, l, epsilon);
+	}
+	bool check_collision(const rectangle& l, const sphere& r, float epsilon){
+		return check_collision(r, l, epsilon);
+	}
+	bool check_collision(const rectangle& l, const line_segment& r, float epsilon){
+		return check_collision(r, l, epsilon);
+	}
+	bool check_collision(const rectangle& l, const point& r, float epsilon){
+		math::vec3<float> normal = math::cross(l.point2 - l.point1, l.point3 - l.point2);
+
+		float n2 = normal * normal;
+		math::vec3<float> d = r - l.point1;
+
+		math::vec3<float> projected_point = r - (((normal * d) / n2) * normal);
+
+		return math::magnitude(r - rectangle_point_coplanar_proj(l, projected_point)) <= epsilon;
+	}
 	bool check_collision(const point& l, const point& r, float epsilon){
 		return math::fuzzy_eq(l, r, epsilon);
 	}
@@ -187,10 +235,10 @@ namespace egn{
 	bool check_collision(const point& l, const sphere& r, float epsilon){
 		return check_collision(r, l, epsilon);
 	}
-	bool check_collision(const point& l, const line& r, float epsilon){
+	bool check_collision(const point& l, const line_segment& r, float epsilon){
 		return check_collision(r, l, epsilon);
 	}
-	bool check_collision(const point& l, const plane& r, float epsilon){
+	bool check_collision(const point& l, const rectangle& r, float epsilon){
 		return check_collision(r, l, epsilon);
 	}