From f5de855fbd4bf5b0df1cad950cbe9069e41369ca Mon Sep 17 00:00:00 2001
From: Daniel Karbach <daniel.karbach@localhorst.tv>
Date: Fri, 13 Nov 2015 16:14:19 +0100
Subject: [PATCH 1/1] geometry stuff

---
 src/ai/ai.cpp              |  4 ++--
 src/geometry/distance.hpp  | 22 +++++-----------------
 src/geometry/geometry.cpp  | 29 +++++++++++++++++++++--------
 src/geometry/primitive.hpp |  2 ++
 src/world/chunk.cpp        |  2 +-
 5 files changed, 31 insertions(+), 28 deletions(-)

diff --git a/src/ai/ai.cpp b/src/ai/ai.cpp
index 0d4c9e9..4b76856 100644
--- a/src/ai/ai.cpp
+++ b/src/ai/ai.cpp
@@ -281,7 +281,7 @@ glm::vec3 AIController::GetObstacleAvoidanceForce(const Entity &e, const EntityS
 	for (WorldCollision &c : col) {
 		// diff points from block to state
 		glm::vec3 diff = state.RelativePosition(c.ChunkPos()) - c.BlockCoords();
-		float dist = length_squared(diff);
+		float dist = length2(diff);
 		if (dist < distance) {
 			nearest = &c;
 			difference = diff;
@@ -530,7 +530,7 @@ void ChaseState::Update(AIController &ctrl, Entity &e, float dt) const {
 		return;
 	}
 	// halt if we're close enough, flee if we're too close
-	float dist_sq = length_squared(e.AbsoluteDifference(ctrl.GetPursuitTarget()));
+	float dist_sq = length2(e.AbsoluteDifference(ctrl.GetPursuitTarget()));
 	if (dist_sq < 8.0f) {
 		ctrl.SetFleeTarget(ctrl.GetPursuitTarget());
 		ctrl.SetState(flee);
diff --git a/src/geometry/distance.hpp b/src/geometry/distance.hpp
index b53fcd7..b6955af 100644
--- a/src/geometry/distance.hpp
+++ b/src/geometry/distance.hpp
@@ -4,37 +4,25 @@
 #include <algorithm>
 #include <limits>
 #include <glm/glm.hpp>
+#include <glm/gtx/component_wise.hpp>
+#include <glm/gtx/norm.hpp>
 
 
 namespace blank {
 
-inline float length_squared(const glm::vec3 &v) noexcept {
-	return dot(v, v);
-}
-
-inline float distance_squared(const glm::vec3 &a, const glm::vec3 &b) noexcept {
-	return length_squared(a - b);
-}
-
-inline float distance(const glm::vec3 &a, const glm::vec3 &b) noexcept {
-	return length(a - b);
-}
-
 template <class T>
 inline bool iszero(const T &v) noexcept {
-	return length_squared(v) < std::numeric_limits<typename T::value_type>::epsilon();
+	return length2(v) < std::numeric_limits<typename T::value_type>::epsilon();
 }
 
 template<class T>
 T manhattan_distance(const glm::tvec3<T> &a, const glm::tvec3<T> &b) noexcept {
-	glm::tvec3<T> diff(abs(a - b));
-	return diff.x + diff.y + diff.z;
+	return compAdd(abs(a - b));
 }
 
 template<class T>
 T manhattan_radius(const glm::tvec3<T> &v) noexcept {
-	glm::tvec3<T> a(abs(v));
-	return std::max(a.x, std::max(a.y, a.z));
+	return compMax(abs(v));
 }
 
 }
diff --git a/src/geometry/geometry.cpp b/src/geometry/geometry.cpp
index ff28d2e..090f787 100644
--- a/src/geometry/geometry.cpp
+++ b/src/geometry/geometry.cpp
@@ -32,7 +32,7 @@ glm::mat3 find_rotation(const glm::vec3 &a, const glm::vec3 &b) noexcept {
 			return glm::mat3(glm::rotate(PI, axis));
 		}
 	}
-	float mv = length_squared(v);
+	float mv = length2(v);
 	float c = dot(a, b);
 	float f = (1 - c) / mv;
 	glm::mat3 vx(matrixCross3(v));
@@ -127,13 +127,22 @@ bool Intersection(
 		glm::vec3(b_m * glm::vec4(b_box.max.x, b_box.max.y, b_box.max.z, 1)),
 	};
 
-	glm::vec3 axes[6] = {
-		glm::vec3(a_m * glm::vec4(1, 0, 0, 0)),
-		glm::vec3(a_m * glm::vec4(0, 1, 0, 0)),
-		glm::vec3(a_m * glm::vec4(0, 0, 1, 0)),
-		glm::vec3(b_m * glm::vec4(1, 0, 0, 0)),
-		glm::vec3(b_m * glm::vec4(0, 1, 0, 0)),
-		glm::vec3(b_m * glm::vec4(0, 0, 1, 0)),
+	glm::vec3 axes[15] = {
+		glm::vec3(a_m[0]),
+		glm::vec3(a_m[1]),
+		glm::vec3(a_m[2]),
+		glm::vec3(b_m[0]),
+		glm::vec3(b_m[1]),
+		glm::vec3(b_m[2]),
+		cross(glm::vec3(a_m[0]), glm::vec3(b_m[0])),
+		cross(glm::vec3(a_m[0]), glm::vec3(b_m[1])),
+		cross(glm::vec3(a_m[0]), glm::vec3(b_m[2])),
+		cross(glm::vec3(a_m[1]), glm::vec3(b_m[0])),
+		cross(glm::vec3(a_m[1]), glm::vec3(b_m[1])),
+		cross(glm::vec3(a_m[1]), glm::vec3(b_m[2])),
+		cross(glm::vec3(a_m[2]), glm::vec3(b_m[0])),
+		cross(glm::vec3(a_m[2]), glm::vec3(b_m[1])),
+		cross(glm::vec3(a_m[2]), glm::vec3(b_m[2])),
 	};
 
 	depth = std::numeric_limits<float>::infinity();
@@ -141,6 +150,10 @@ bool Intersection(
 
 	int cur_axis = 0;
 	for (const glm::vec3 &axis : axes) {
+		if (iszero(axis)) {
+			// can result from the cross products if A and B have parallel axes
+			continue;
+		}
 		float a_min = std::numeric_limits<float>::infinity();
 		float a_max = -std::numeric_limits<float>::infinity();
 		for (const glm::vec3 &corner : a_corners) {
diff --git a/src/geometry/primitive.hpp b/src/geometry/primitive.hpp
index 7da1096..cdeae86 100644
--- a/src/geometry/primitive.hpp
+++ b/src/geometry/primitive.hpp
@@ -40,6 +40,8 @@ bool Intersection(
 	float *dist = nullptr,
 	glm::vec3 *normal = nullptr) noexcept;
 
+/// matrices may translate and rotate, but must not scale/shear/etc
+/// (basically the first three columns must have unit length)
 bool Intersection(
 	const AABB &a_box,
 	const glm::mat4 &a_m,
diff --git a/src/world/chunk.cpp b/src/world/chunk.cpp
index fb43755..b275315 100644
--- a/src/world/chunk.cpp
+++ b/src/world/chunk.cpp
@@ -460,7 +460,7 @@ bool Chunk::Intersection(
 	const glm::vec3 entity_coords(Mentity[3] - Mchunk[3]);
 	const float ec_radius = entity.Radius() + Radius();
 
-	if (distance_squared(entity_coords, Center()) > ec_radius * ec_radius) {
+	if (distance2(entity_coords, Center()) > ec_radius * ec_radius) {
 		return false;
 	}
 
-- 
2.39.2