From 2504792c976f8d4ff50e0516ba9758a3fe9783c4 Mon Sep 17 00:00:00 2001
From: Steve Streeting <steve@stevestreeting.com>
Date: Fri, 7 May 2021 16:18:34 +0100
Subject: [PATCH] Added StevesMathHelpers, with SphereOverlapCone to begin with

---
 ReadMe.md                                     | 32 ++++++++
 .../Public/StevesMathHelpers.h                | 82 +++++++++++++++++++
 2 files changed, 114 insertions(+)
 create mode 100644 Source/StevesUEHelpers/Public/StevesMathHelpers.h

diff --git a/ReadMe.md b/ReadMe.md
index d8735de..e6e53bb 100644
--- a/ReadMe.md
+++ b/ReadMe.md
@@ -120,3 +120,35 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of
 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
 
 THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+# 3rd Party Licenses
+
+Portions of this software include code from 3rd parties:
+
+Math routines from David Eberly, Geometric Tools, Redmond WA 98052
+Copyright (c) 1998-2021
+Distributed under the Boost Software License, Version 1.0.
+
+Boost Software License - Version 1.0 - August 17th, 2003
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
\ No newline at end of file
diff --git a/Source/StevesUEHelpers/Public/StevesMathHelpers.h b/Source/StevesUEHelpers/Public/StevesMathHelpers.h
new file mode 100644
index 0000000..f264298
--- /dev/null
+++ b/Source/StevesUEHelpers/Public/StevesMathHelpers.h
@@ -0,0 +1,82 @@
+#pragma once
+
+/// Helper maths routines that UE4 is missing, all static
+class StevesMathHelpers
+{
+public:
+
+	/**
+	* @brief Return whether a sphere overlaps a cone
+	* @param ConeOrigin Origin of the cone
+	* @param ConeDir Direction of the cone, must be normalised
+	* @param ConeHalfAngle Half-angle of the cone, in radians
+	* @param Distance Length of the cone
+	* @param SphereCentre Centre of the sphere
+	* @param SphereRadius Radius of the sphere
+	* @return True if any collider in the component overlaps the cone
+	*/
+	static bool SphereOverlapCone(const FVector& ConeOrigin, const FVector& ConeDir, float ConeHalfAngle, float Distance, const FVector& SphereCentre, float SphereRadius)
+	{
+		// Algorithm from https://www.geometrictools.com/GTE/Mathematics/IntrSphere3Cone3.h
+		
+		const float SinHalfAngle = FMath::Sin(ConeHalfAngle);
+		const float InvSinHalfAngle =  1.f/SinHalfAngle;
+
+		const FVector U = ConeOrigin - (SphereRadius * InvSinHalfAngle) * ConeDir;
+		const FVector CmU = SphereCentre - U;
+		const float AdCmU = FVector::DotProduct(ConeDir, CmU);
+		if (AdCmU > 0)
+		{
+			const float CosHalfAngle = FMath::Cos(ConeHalfAngle);
+			const float CosHalfAngleSq = CosHalfAngle * CosHalfAngle;
+
+			const float sqrLengthCmU = FVector::DotProduct(CmU, CmU);
+			if (AdCmU * AdCmU >= sqrLengthCmU * CosHalfAngleSq)
+			{
+				const FVector CmV = SphereCentre - ConeOrigin;
+				const float AdCmV = FVector::DotProduct(ConeDir, CmV);
+				if (AdCmV < -SphereRadius)
+				{
+					return false;
+				}
+
+				if (AdCmV > Distance + SphereRadius)
+				{
+					return false;
+				}
+
+				const float rSinAngle = SphereRadius * SinHalfAngle;
+				if (AdCmV >= -rSinAngle)
+				{
+					if (AdCmV <= Distance - rSinAngle)
+					{
+						return true;
+					}
+					else
+					{
+						const float TanHalfAngle = FMath::Tan(ConeHalfAngle);
+
+						const FVector barD = CmV - Distance * ConeDir;
+						const float lengthAxBarD = FVector::CrossProduct(ConeDir, barD).Size();
+						const float hmaxTanAngle = Distance * TanHalfAngle;
+						if (lengthAxBarD <= hmaxTanAngle)
+						{
+							return true;
+						}
+
+						const float AdBarD = AdCmV - Distance;
+						const float diff = lengthAxBarD - hmaxTanAngle;
+						const float sqrLengthCmBarK = AdBarD * AdBarD + diff * diff;
+						return sqrLengthCmBarK <= SphereRadius * SphereRadius;
+					}
+				}
+				else
+				{
+					const float sqrLengthCmV = FVector::DotProduct(CmV, CmV);
+					return sqrLengthCmV <= SphereRadius * SphereRadius;
+				}
+			}
+		}
+		return false;
+	}
+};
\ No newline at end of file