Collision2D.cs 149 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666266726682669267026712672267326742675267626772678267926802681268226832684268526862687268826892690269126922693269426952696269726982699270027012702270327042705270627072708270927102711271227132714271527162717271827192720272127222723272427252726272727282729273027312732273327342735273627372738273927402741274227432744274527462747274827492750275127522753275427552756275727582759276027612762276327642765276627672768276927702771277227732774277527762777277827792780278127822783278427852786278727882789279027912792279327942795279627972798279928002801280228032804280528062807280828092810281128122813281428152816281728182819282028212822282328242825282628272828282928302831283228332834283528362837283828392840284128422843284428452846284728482849285028512852285328542855285628572858285928602861286228632864286528662867286828692870287128722873287428752876287728782879288028812882288328842885288628872888288928902891289228932894289528962897289828992900290129022903290429052906290729082909291029112912291329142915291629172918291929202921292229232924292529262927292829292930293129322933293429352936293729382939294029412942294329442945
  1. // Copyright (c) Craftwork Games. All rights reserved.
  2. // Licensed under the MIT license.
  3. // See LICENSE file in the project root for full license information.
  4. using System;
  5. using System.Runtime.CompilerServices;
  6. using Microsoft.Xna.Framework;
  7. namespace MonoGame.Extended
  8. {
  9. /// <summary>
  10. /// Provides low-level, allocation-free geometric queries and helper routines for 2D collision detection.
  11. /// </summary>
  12. /// <remarks>
  13. /// This class contains stateless, static methods for intersection tests, containment classification, distance
  14. /// queries, interval clipping, and projection operations between common 2D primitives such as points, line
  15. /// segments, rays, circles, axis-aligned bounding boxes (AABBs), oriented bounding boxes (OBBs), capsules,
  16. /// and convex polygons.
  17. ///
  18. /// The APIs are intended to be used as shared building blocks by higher-level geometric types (for example,
  19. /// bounding volumes and shapes) rather than consumed directly in most application code. Methods are designed
  20. /// to be deterministic, side-effect free, and suitable for performance-critical use, including tight update
  21. /// loops and broad-phase or narrow-phase collision detection.
  22. ///
  23. /// Many algorithms implemented here are standard results from computational geometry and collision detection
  24. /// literature, including distance-based tests and separating-axis tests (SAT). Where appropriate, individual
  25. /// methods include in-code attribution to published sources such as Christer Ericson’s
  26. /// <em>Real-Time Collision Detection</em>.
  27. ///
  28. /// This class is not thread-affine and contains no shared mutable state; all methods are thread-safe provided
  29. /// the supplied arguments are not mutated concurrently by the caller.
  30. /// </remarks>
  31. public static class Collision2D
  32. {
  33. #region Constants
  34. /// <summary>
  35. /// A small tolerance value used to account for floating-point imprecision in geometric computations.
  36. /// </summary>
  37. /// <remarks>
  38. /// This constant is used to stabilize comparisons involving distances, projections, and dot products where
  39. /// exact equality cannot be reliably assumed due to floating-point rounding behavior.
  40. /// </remarks>
  41. public const float Epsilon = 1e-6f;
  42. /// <summary>
  43. /// The square of <see cref="Epsilon"/>.
  44. /// </summary>
  45. /// <remarks>
  46. /// This value is provided for efficiency when comparing squared distances, avoiding unnecessary square root
  47. /// operations while maintaining consistent tolerance behavior.
  48. /// </remarks>
  49. public const float EpsilonSq = Epsilon * Epsilon;
  50. #endregion
  51. #region Helpers
  52. /// <summary>
  53. /// Determines whether the specified convex polygon data is structurally valid for use by collision routines.
  54. /// </summary>
  55. /// <param name="vertices">
  56. /// The polygon vertices in 2D space. A valid polygon requires at least three vertices.
  57. /// </param>
  58. /// <param name="normals">
  59. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  60. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  61. /// </param>
  62. /// <returns>
  63. /// <see langword="true"/> if <paramref name="vertices"/> and <paramref name="normals"/> are non-<see langword="null"/>,
  64. /// <paramref name="vertices"/> contains at least three elements, and both arrays have the same length;
  65. /// otherwise, <see langword="false"/>.
  66. /// </returns>
  67. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  68. public static bool IsValidPolygon(Vector2[] vertices, Vector2[] normals)
  69. {
  70. if (vertices == null || normals == null)
  71. return false;
  72. int count = vertices.Length;
  73. return count >= 3 && normals.Length == count;
  74. }
  75. /// <summary>
  76. /// Intersects a parametric interval with a clipping interval and updates the result in place.
  77. /// </summary>
  78. /// <param name="tMin">
  79. /// On input, the lower bound of the parametric interval. On output, the lower bound of the
  80. /// intersected interval.
  81. /// </param>
  82. /// <param name="tMax">
  83. /// On input, the upper bound of the parametric interval. On output, the upper bound of the
  84. /// intersected interval.
  85. /// </param>
  86. /// <param name="clipMin">The lower bound of the clipping interval.</param>
  87. /// <param name="clipMax">The upper bound of the clipping interval.</param>
  88. /// <returns>
  89. /// <see langword="true"/> if the two intervals overlap after clipping; otherwise, <see langword="false"/>.
  90. /// </returns>
  91. /// <remarks>
  92. /// This method is commonly used in parametric line, ray, and segment intersection algorithms,
  93. /// where <c>t</c> represents a scalar parameter along a direction vector and successive calls
  94. /// progressively restrict the valid range.
  95. /// </remarks>
  96. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  97. public static bool ClipInterval(ref float tMin, ref float tMax, float clipMin, float clipMax)
  98. {
  99. if (clipMin > tMin) tMin = clipMin;
  100. if (clipMax < tMax) tMax = clipMax;
  101. return tMin <= tMax;
  102. }
  103. /// <summary>
  104. /// Computes the intersection of two parametric intervals and returns the clipped result.
  105. /// </summary>
  106. /// <param name="tEnter">The lower bound of the first parametric interval.</param>
  107. /// <param name="tExit">The upper bound of the first parametric interval.</param>
  108. /// <param name="tLower">The lower bound of the clipping interval.</param>
  109. /// <param name="tUpper">The upper bound of the clipping interval.</param>
  110. /// <param name="clippedEnter">
  111. /// When this method returns, contains the lower bound of the intersected interval.
  112. /// </param>
  113. /// <param name="clippedExit">
  114. /// When this method returns, contains the upper bound of the intersected interval.
  115. /// </param>
  116. /// <returns>
  117. /// <see langword="true"/> if the intervals overlap and the resulting interval is non-empty;
  118. /// otherwise, <see langword="false"/>.
  119. /// </returns>
  120. /// <remarks>
  121. /// This overload performs interval clipping without modifying the input parameters and is suitable
  122. /// for algorithms that require preservation of the original bounds, such as line, ray, or segment
  123. /// intersection tests.
  124. /// </remarks>
  125. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  126. public static bool ClipInterval(float tEnter, float tExit, float tLower, float tUpper, out float clippedEnter, out float clippedExit)
  127. {
  128. clippedEnter = MathF.Max(tEnter, tLower);
  129. clippedExit = MathF.Min(tExit, tUpper);
  130. return clippedEnter <= clippedExit;
  131. }
  132. /// <summary>
  133. /// Determines whether two scalar intervals overlap or touch within a numeric tolerance.
  134. /// </summary>
  135. /// <param name="minA">The lower bound of the first interval.</param>
  136. /// <param name="maxA">The upper bound of the first interval.</param>
  137. /// <param name="minB">The lower bound of the second interval.</param>
  138. /// <param name="maxB">The upper bound of the second interval.</param>
  139. /// <returns>
  140. /// <see langword="true"/> if the intervals overlap or touch;otherwise, <see langword="false"/>.
  141. /// </returns>
  142. /// <remarks>
  143. /// Intervals that touch at a boundary are considered overlapping, which is required
  144. /// for separating-axis tests and projection-based intersection checks.
  145. /// </remarks>
  146. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  147. public static bool IntervalsOverlap(float minA, float maxA, float minB, float maxB)
  148. {
  149. // touch counts as overlap
  150. if (maxA < minB - Epsilon) return false;
  151. if (maxB < minA - Epsilon) return false;
  152. return true;
  153. }
  154. #endregion
  155. #region Containment
  156. #region Containment (AABB Contains)
  157. /// <summary>
  158. /// Determines whether a point is contained within an axis-aligned bounding box (AABB).
  159. /// </summary>
  160. /// <param name="point">The point to test.</param>
  161. /// <param name="min">The minimum corner of the AABB.</param>
  162. /// <param name="max">The maximum corner of the AABB.</param>
  163. /// <returns>
  164. /// <see cref="ContainmentType.Contains"/> if the point lies inside the bounding box or on its
  165. /// boundary; otherwise, <see cref="ContainmentType.Disjoint"/>.
  166. /// </returns>
  167. public static ContainmentType ContainsAabbPoint(Vector2 point, Vector2 min, Vector2 max)
  168. {
  169. if (point.X < min.X - Epsilon) return ContainmentType.Disjoint;
  170. if (point.X > max.X + Epsilon) return ContainmentType.Disjoint;
  171. if (point.Y < min.Y - Epsilon) return ContainmentType.Disjoint;
  172. if (point.Y > max.Y + Epsilon) return ContainmentType.Disjoint;
  173. return ContainmentType.Contains;
  174. }
  175. /// <summary>
  176. /// Determines the containment relationship between two axis-aligned bounding boxes (AABBs).
  177. /// </summary>
  178. /// <param name="aMin">The minimum corner of the reference AABB.</param>
  179. /// <param name="aMax">The maximum corner of the reference AABB.</param>
  180. /// <param name="bMin">The minimum corner of the AABB being tested.</param>
  181. /// <param name="bMax">The maximum corner of the AABB being tested.</param>
  182. /// <returns>
  183. /// <see cref="ContainmentType.Disjoint"/> if the boxes do not overlap; <see cref="ContainmentType.Contains"/>
  184. /// if the second box is fully contained within the first; otherwise, <see cref="ContainmentType.Intersects"/>.
  185. /// </returns>
  186. /// <remarks>
  187. /// Boundary contact is treated as overlap rather than disjoint.
  188. /// </remarks>
  189. public static ContainmentType ContainsAabbAabb(Vector2 aMin, Vector2 aMax, Vector2 bMin, Vector2 bMax)
  190. {
  191. // Disjoint (touch counts as NOT disjoint)
  192. if (bMax.X < aMin.X - Epsilon ||
  193. bMin.X > aMax.X + Epsilon ||
  194. bMax.Y < aMin.Y - Epsilon ||
  195. bMin.Y > aMax.Y + Epsilon)
  196. return ContainmentType.Disjoint;
  197. // Contains (inclusive)
  198. if (bMin.X >= aMin.X - Epsilon &&
  199. bMax.X <= aMax.X + Epsilon &&
  200. bMin.Y >= aMin.Y - Epsilon &&
  201. bMax.Y <= aMax.Y + Epsilon)
  202. return ContainmentType.Contains;
  203. // Otherwise, overlap/touch but not fully contained
  204. return ContainmentType.Intersects;
  205. }
  206. /// <summary>
  207. /// Determines the containment relationship between an axis-aligned bounding box (AABB) and a circle.
  208. /// </summary>
  209. /// <param name="boxMin">The minimum corner of the AABB.</param>
  210. /// <param name="boxMax">The maximum corner of the AABB.</param>
  211. /// <param name="circleCenter">The center of the circle.</param>
  212. /// <param name="circleRadius">The radius of the circle. Must be non-negative.</param>
  213. /// <returns>
  214. /// <see cref="ContainmentType.Disjoint"/> if the circle lies entirely outside the box;
  215. /// <see cref="ContainmentType.Contains"/> if the circle is fully contained within the box;
  216. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  217. /// </returns>
  218. /// <remarks>
  219. /// Boundary contact is treated as intersection rather than disjoint.
  220. /// </remarks>
  221. public static ContainmentType ContainsAabbCircle(Vector2 boxMin, Vector2 boxMax, Vector2 circleCenter, float circleRadius)
  222. {
  223. if (circleRadius < 0.0f)
  224. return ContainmentType.Disjoint;
  225. // Disjoint check: circle completely outside
  226. if (circleCenter.X - circleRadius > boxMax.X + Epsilon ||
  227. circleCenter.X + circleRadius < boxMin.X - Epsilon ||
  228. circleCenter.Y - circleRadius > boxMax.Y + Epsilon ||
  229. circleCenter.Y + circleRadius < boxMin.Y - Epsilon)
  230. return ContainmentType.Disjoint;
  231. // Contains (inclusive)
  232. if (circleCenter.X - circleRadius >= boxMin.X - Epsilon &&
  233. circleCenter.X + circleRadius <= boxMax.X + Epsilon &&
  234. circleCenter.Y - circleRadius >= boxMin.Y - Epsilon &&
  235. circleCenter.Y + circleRadius <= boxMax.Y + Epsilon)
  236. return ContainmentType.Contains;
  237. // Otherwise, overlap/touch, but not fully contains
  238. return ContainmentType.Intersects;
  239. }
  240. /// <summary>
  241. /// Determines the containment relationship between an axis-aligned bounding box (AABB) and an oriented bounding box (OBB).
  242. /// </summary>
  243. /// <param name="aabbMin">The minimum corner of the AABB.</param>
  244. /// <param name="aabbMax">The maximum corner of the AABB.</param>
  245. /// <param name="obbCenter">The center of the OBB.</param>
  246. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  247. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  248. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  249. /// <returns>
  250. /// <see cref="ContainmentType.Disjoint"/> if the volumes do not overlap; <see cref="ContainmentType.Contains"/> if the
  251. /// OBB is fully contained within the AABB; otherwise, <see cref="ContainmentType.Intersects"/>.
  252. /// </returns>
  253. /// <remarks>
  254. /// The overlap test is performed first. If the volumes overlap, containment is determined by testing whether all four OBB
  255. /// corners lie inside the AABB (inclusive).
  256. /// </remarks>
  257. public static ContainmentType ContainsAabbObb(Vector2 aabbMin, Vector2 aabbMax, Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents)
  258. {
  259. Vector2 aabbCenter = (aabbMin + aabbMax) * 0.5f;
  260. Vector2 aabbHalf = (aabbMax - aabbMin) * 0.5f;
  261. // Disjoint check
  262. if (!IntersectsAabbObb(aabbCenter, aabbHalf, obbCenter, obbAxisX, obbAxisY, obbHalfExtents))
  263. return ContainmentType.Disjoint;
  264. // Contains: all 4 OBB corners inside AABB
  265. Vector2 ex = obbAxisX * obbHalfExtents.X;
  266. Vector2 ey = obbAxisY * obbHalfExtents.Y;
  267. Vector2 c0 = obbCenter - ex - ey;
  268. Vector2 c1 = obbCenter + ex - ey;
  269. Vector2 c2 = obbCenter + ex + ey;
  270. Vector2 c3 = obbCenter - ex + ey;
  271. if (ContainsAabbPoint(c0, aabbMin, aabbMax) == ContainmentType.Contains &&
  272. ContainsAabbPoint(c1, aabbMin, aabbMax) == ContainmentType.Contains &&
  273. ContainsAabbPoint(c2, aabbMin, aabbMax) == ContainmentType.Contains &&
  274. ContainsAabbPoint(c3, aabbMin, aabbMax) == ContainmentType.Contains)
  275. return ContainmentType.Contains;
  276. return ContainmentType.Intersects;
  277. }
  278. /// <summary>
  279. /// Determines the containment relationship between an axis-aligned bounding box (AABB) and a capsule.
  280. /// </summary>
  281. /// <param name="boxMin">The minimum corner of the AABB.</param>
  282. /// <param name="boxMax">The maximum corner of the AABB.</param>
  283. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  284. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  285. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  286. /// <returns>
  287. /// <see cref="ContainmentType.Disjoint"/> if the capsule lies entirely outside the box;
  288. /// <see cref="ContainmentType.Contains"/> if the capsule is fully contained within the box;
  289. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  290. /// </returns>
  291. /// <remarks>
  292. /// This method first performs an intersection test. If the shapes overlap, full containment is determined by
  293. /// contracting the AABB by <paramref name="capsuleRadius"/> on all sides and testing whether both capsule
  294. /// endpoints lie inside the contracted box (inclusive). This corresponds to the condition that the capsule's
  295. /// swept disk about its segment is contained within the original AABB.
  296. /// </remarks>
  297. public static ContainmentType ContainsAabbCapsule(Vector2 boxMin, Vector2 boxMax, Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius)
  298. {
  299. if (capsuleRadius < 0.0f)
  300. return ContainmentType.Disjoint;
  301. // If they don't even intersect, containment is impossible
  302. if (!IntersectsAabbCapsule(boxMin, boxMax, capsuleA, capsuleB, capsuleRadius))
  303. return ContainmentType.Disjoint;
  304. // For full containment, the capsule's segment must lie inside the AABb
  305. // contracted (shrunk) by the capsule radius
  306. Vector2 innerMin = new Vector2(boxMin.X + capsuleRadius, boxMin.Y + capsuleRadius);
  307. Vector2 innerMax = new Vector2(boxMax.X - capsuleRadius, boxMax.Y - capsuleRadius);
  308. // If the contracted AABB is invalid, it can't contain any capsule with this radius
  309. if (innerMin.X > innerMax.X + Epsilon || innerMin.Y > innerMax.Y + Epsilon)
  310. return ContainmentType.Disjoint;
  311. // Segment inside convex set
  312. if (ContainsAabbPoint(capsuleA, innerMin, innerMax) == ContainmentType.Contains &&
  313. ContainsAabbPoint(capsuleB, innerMin, innerMax) == ContainmentType.Contains)
  314. return ContainmentType.Contains;
  315. return ContainmentType.Intersects;
  316. }
  317. /// <summary>
  318. /// Determines the containment relationship between an axis-aligned bounding box (AABB) and a convex polygon.
  319. /// </summary>
  320. /// <param name="boxMin">The minimum corner of the AABB.</param>
  321. /// <param name="boxMax">The maximum corner of the AABB.</param>
  322. /// <param name="vertices">
  323. /// The polygon vertices in winding order. A valid polygon requires at least three vertices.
  324. /// </param>
  325. /// <param name="normals">
  326. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  327. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  328. /// </param>
  329. /// <returns>
  330. /// <see cref="ContainmentType.Disjoint"/> if the polygon lies entirely outside the box;
  331. /// <see cref="ContainmentType.Contains"/> if the polygon is fully contained within the box;
  332. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  333. /// </returns>
  334. /// <remarks>
  335. /// This method first performs an intersection test. If the shapes overlap, containment is determined by verifying
  336. /// that every polygon vertex lies inside the AABB (inclusive). For a convex polygon, all vertices contained
  337. /// within a convex set implies the entire polygon is contained.
  338. /// </remarks>
  339. public static ContainmentType ContainsAabbConvexPolygon(Vector2 boxMin, Vector2 boxMax, Vector2[] vertices, Vector2[] normals)
  340. {
  341. if (!IsValidPolygon(vertices, normals))
  342. return ContainmentType.Disjoint;
  343. // If they don't intersect at all, containment is impossible
  344. if (!IntersectsAabbConvexPolygon(boxMin, boxMax, vertices, normals))
  345. return ContainmentType.Disjoint;
  346. // Fast containment check: if all polygon vertices are inside the AABB,
  347. // then the convex polygon is fully contained
  348. for (int i = 0; i < vertices.Length; i++)
  349. {
  350. if (ContainsAabbPoint(vertices[i], boxMin, boxMax) == ContainmentType.Disjoint)
  351. return ContainmentType.Intersects;
  352. }
  353. return ContainmentType.Contains;
  354. }
  355. #endregion
  356. #region Containment (Circle Contains)
  357. /// <summary>
  358. /// Determines whether a point is contained within a circle.
  359. /// </summary>
  360. /// <param name="point">The point to test.</param>
  361. /// <param name="center">The center of the circle.</param>
  362. /// <param name="radius">The circle radius. Must be non-negative.</param>
  363. /// <returns>
  364. /// <see cref="ContainmentType.Contains"/> if the point lies inside the circle or on its boundary;
  365. /// otherwise, <see cref="ContainmentType.Disjoint"/>.
  366. /// </returns>
  367. public static ContainmentType ContainsCirclePoint(Vector2 point, Vector2 center, float radius)
  368. {
  369. if (radius < 0.0f)
  370. return ContainmentType.Disjoint;
  371. // Expand radius by epsilon to improve tests when point lies near boundary.
  372. float r = radius + Epsilon;
  373. return Vector2.DistanceSquared(point, center) <= r * r ? ContainmentType.Contains : ContainmentType.Disjoint;
  374. }
  375. /// <summary>
  376. /// Determines the containment relationship between a circle and an axis-aligned bounding box (AABB).
  377. /// </summary>
  378. /// <param name="circleCenter">The center of the circle.</param>
  379. /// <param name="circleRadius">The circle radius. Must be non-negative.</param>
  380. /// <param name="aabbMin">The minimum corner of the AABB.</param>
  381. /// <param name="aabbMax">The maximum corner of the AABB.</param>
  382. /// <returns>
  383. /// <see cref="ContainmentType.Disjoint"/> if the box lies entirely outside the circle;
  384. /// <see cref="ContainmentType.Contains"/> if the box is fully contained within the circle;
  385. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  386. /// </returns>
  387. /// <remarks>
  388. /// This method first performs an intersection test. If the shapes overlap, containment is determined by testing
  389. /// whether all four AABB corners lie inside the circle (inclusive).
  390. /// </remarks>
  391. public static ContainmentType ContainsCircleAabb(Vector2 circleCenter, float circleRadius, Vector2 aabbMin, Vector2 aabbMax)
  392. {
  393. if (circleRadius < 0f)
  394. return ContainmentType.Disjoint;
  395. if (!IntersectsCircleAabb(circleCenter, circleRadius, aabbMin, aabbMax))
  396. return ContainmentType.Disjoint;
  397. // Contains if all 4 AABB corners are inside circle
  398. Vector2 c0 = new Vector2(aabbMin.X, aabbMin.Y);
  399. Vector2 c1 = new Vector2(aabbMax.X, aabbMin.Y);
  400. Vector2 c2 = new Vector2(aabbMax.X, aabbMax.Y);
  401. Vector2 c3 = new Vector2(aabbMin.X, aabbMax.Y);
  402. if (ContainsCirclePoint(c0, circleCenter, circleRadius) == ContainmentType.Contains &&
  403. ContainsCirclePoint(c1, circleCenter, circleRadius) == ContainmentType.Contains &&
  404. ContainsCirclePoint(c2, circleCenter, circleRadius) == ContainmentType.Contains &&
  405. ContainsCirclePoint(c3, circleCenter, circleRadius) == ContainmentType.Contains)
  406. return ContainmentType.Contains;
  407. return ContainmentType.Intersects;
  408. }
  409. /// <summary>
  410. /// Determines the containment relationship between two circles.
  411. /// </summary>
  412. /// <param name="aCenter">The center of the reference circle.</param>
  413. /// <param name="aRadius">The radius of the reference circle. Must be non-negative.</param>
  414. /// <param name="bCenter">The center of the circle being tested.</param>
  415. /// <param name="bRadius">The radius of the circle being tested. Must be non-negative.</param>
  416. /// <returns>
  417. /// <see cref="ContainmentType.Disjoint"/> if the circles do not overlap;
  418. /// <see cref="ContainmentType.Contains"/> if the second circle is fully contained within the first;
  419. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  420. /// </returns>
  421. /// <remarks>
  422. /// The test is performed using squared distances to avoid a square root. Boundary contact is treated as intersection,
  423. /// and containment tests are inclusive.
  424. /// </remarks>
  425. public static ContainmentType ContainsCircleCircle(Vector2 aCenter, float aRadius, Vector2 bCenter, float bRadius)
  426. {
  427. if (aRadius < 0.0f || bRadius < 0.0f)
  428. return ContainmentType.Disjoint;
  429. Vector2 d = bCenter - aCenter;
  430. float d2 = d.LengthSquared();
  431. // Disjoint if centers are father apart than sum of radii
  432. float sum = aRadius + bRadius + Epsilon;
  433. if (d2 > sum * sum)
  434. return ContainmentType.Disjoint;
  435. // Contains if B fits entirely with A (inclusive)
  436. float rhs = (aRadius - bRadius) + Epsilon;
  437. if (rhs >= 0.0f && d2 <= rhs * rhs)
  438. return ContainmentType.Contains;
  439. return ContainmentType.Intersects;
  440. }
  441. /// <summary>
  442. /// Determines the containment relationship between a circle and an oriented bounding box (OBB).
  443. /// </summary>
  444. /// <param name="circleCenter">The center of the circle.</param>
  445. /// <param name="circleRadius">The circle radius. Must be non-negative.</param>
  446. /// <param name="obbCenter">The center of the OBB.</param>
  447. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  448. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  449. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  450. /// <returns>
  451. /// <see cref="ContainmentType.Disjoint"/> if the OBB lies entirely outside the circle;
  452. /// <see cref="ContainmentType.Contains"/> if the OBB is fully contained within the circle;
  453. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  454. /// </returns>
  455. /// <remarks>
  456. /// This method first performs an intersection test. If the shapes overlap, containment is determined by testing
  457. /// whether all four OBB corners lie inside the circle (inclusive).
  458. /// </remarks>
  459. public static ContainmentType ContainsCircleObb(Vector2 circleCenter, float circleRadius, Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents)
  460. {
  461. if (circleRadius < 0f)
  462. return ContainmentType.Disjoint;
  463. // Disjoint check first (touch counts as NOT disjoint)
  464. if (!IntersectsCircleObb(circleCenter, circleRadius, obbCenter, obbAxisX, obbAxisY, obbHalfExtents))
  465. return ContainmentType.Disjoint;
  466. // Contains if all 4 OBB corners are inside circle
  467. Vector2 ex = obbAxisX * obbHalfExtents.X;
  468. Vector2 ey = obbAxisY * obbHalfExtents.Y;
  469. Vector2 c0 = obbCenter - ex - ey;
  470. Vector2 c1 = obbCenter + ex - ey;
  471. Vector2 c2 = obbCenter + ex + ey;
  472. Vector2 c3 = obbCenter - ex + ey;
  473. if (ContainsCirclePoint(c0, circleCenter, circleRadius) == ContainmentType.Contains &&
  474. ContainsCirclePoint(c1, circleCenter, circleRadius) == ContainmentType.Contains &&
  475. ContainsCirclePoint(c2, circleCenter, circleRadius) == ContainmentType.Contains &&
  476. ContainsCirclePoint(c3, circleCenter, circleRadius) == ContainmentType.Contains)
  477. return ContainmentType.Contains;
  478. return ContainmentType.Intersects;
  479. }
  480. /// <summary>
  481. /// Determines the containment relationship between a circle and a capsule.
  482. /// </summary>
  483. /// <param name="circleCenter">The center of the circle.</param>
  484. /// <param name="circleRadius">The circle radius. Must be non-negative.</param>
  485. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  486. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  487. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  488. /// <returns>
  489. /// <see cref="ContainmentType.Disjoint"/> if the capsule lies entirely outside the circle;
  490. /// <see cref="ContainmentType.Contains"/> if the capsule is fully contained within the circle;
  491. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  492. /// </returns>
  493. /// <remarks>
  494. /// The capsule is the Minkowski sum of its segment and a disk of radius <paramref name="capsuleRadius"/>.
  495. /// After confirming the shapes intersect, containment is determined by checking whether the capsule segment lies
  496. /// within a circle of radius <c>circleRadius - capsuleRadius</c> (inclusive). This is equivalent to requiring
  497. /// the maximum distance from <paramref name="circleCenter"/> to the segment to be less than or equal to
  498. /// <c>circleRadius - capsuleRadius</c>.
  499. /// </remarks>
  500. public static ContainmentType ContainsCircleCapsule(Vector2 circleCenter, float circleRadius, Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius)
  501. {
  502. if (circleRadius < 0f || capsuleRadius < 0f)
  503. return ContainmentType.Disjoint;
  504. if (!IntersectsCircleCapsule(circleCenter, circleRadius, capsuleA, capsuleB, capsuleRadius))
  505. return ContainmentType.Disjoint;
  506. float inner = (circleRadius - capsuleRadius) + Epsilon;
  507. if (inner < 0f)
  508. return ContainmentType.Intersects;
  509. float inner2 = inner * inner;
  510. // For the capsule to be fully contained, both endpoints must be within the inner circle
  511. if (Vector2.DistanceSquared(circleCenter, capsuleA) <= inner2 &&
  512. Vector2.DistanceSquared(circleCenter, capsuleB) <= inner2)
  513. return ContainmentType.Contains;
  514. return ContainmentType.Intersects;
  515. }
  516. /// <summary>
  517. /// Determines the containment relationship between a circle and a convex polygon.
  518. /// </summary>
  519. /// <param name="circleCenter">The center of the circle.</param>
  520. /// <param name="circleRadius">The circle radius. Must be non-negative.</param>
  521. /// <param name="vertices">
  522. /// The polygon vertices in winding order. A valid polygon requires at least three vertices.
  523. /// </param>
  524. /// <param name="normals">
  525. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  526. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  527. /// </param>
  528. /// <returns>
  529. /// <see cref="ContainmentType.Disjoint"/> if the polygon lies entirely outside the circle;
  530. /// <see cref="ContainmentType.Contains"/> if the polygon is fully contained within the circle;
  531. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  532. /// </returns>
  533. /// <remarks>
  534. /// This method first performs an intersection test. If the shapes overlap, containment is determined by verifying
  535. /// that every polygon vertex lies inside the circle (inclusive). For a convex polygon, all vertices contained
  536. /// within a convex set implies the entire polygon is contained.
  537. /// </remarks>
  538. public static ContainmentType ContainsCircleConvexPolygon(Vector2 circleCenter, float circleRadius, Vector2[] vertices, Vector2[] normals)
  539. {
  540. if (circleRadius < 0f)
  541. return ContainmentType.Disjoint;
  542. if (!IsValidPolygon(vertices, normals))
  543. return ContainmentType.Disjoint;
  544. if (!IntersectsCircleConvexPolygon(circleCenter, circleRadius, vertices, normals))
  545. return ContainmentType.Disjoint;
  546. // Contains: all polygon vertices inside circle
  547. for (int i = 0; i < vertices.Length; i++)
  548. {
  549. if (ContainsCirclePoint(vertices[i], circleCenter, circleRadius) == ContainmentType.Disjoint)
  550. return ContainmentType.Intersects;
  551. }
  552. return ContainmentType.Contains;
  553. }
  554. #endregion
  555. #region Containment (OBB Contains)
  556. /// <summary>
  557. /// Determines whether a point is contained within an oriented bounding box (OBB).
  558. /// </summary>
  559. /// <param name="point">The point to test.</param>
  560. /// <param name="center">The center of the OBB.</param>
  561. /// <param name="axisX">The OBB local X axis direction.</param>
  562. /// <param name="axisY">The OBB local Y axis direction.</param>
  563. /// <param name="halfExtents">The half-widths of the OBB along <paramref name="axisX"/> and <paramref name="axisY"/>.</param>
  564. /// <returns>
  565. /// <see cref="ContainmentType.Contains"/> if the point lies inside the box or on its boundary;
  566. /// otherwise, <see cref="ContainmentType.Disjoint"/>.
  567. /// </returns>
  568. /// <remarks>
  569. /// The test projects <c>(point - center)</c> onto the OBB axes and compares the absolute projected distances against
  570. /// the corresponding half extents. The axes are expected to be orthonormal for the extents to represent distances.
  571. /// </remarks>
  572. public static ContainmentType ContainsObbPoint(Vector2 point, Vector2 center, Vector2 axisX, Vector2 axisY, Vector2 halfExtents)
  573. {
  574. // Transform point into OBB local space by projecting it onto axes
  575. Vector2 d = point - center;
  576. float distX = Vector2.Dot(d, axisX);
  577. if (MathF.Abs(distX) > halfExtents.X + Epsilon)
  578. return ContainmentType.Disjoint;
  579. float distY = Vector2.Dot(d, axisY);
  580. if (MathF.Abs(distY) > halfExtents.Y + Epsilon)
  581. return ContainmentType.Disjoint;
  582. return ContainmentType.Contains;
  583. }
  584. /// <summary>
  585. /// Determines the containment relationship between an oriented bounding box (OBB) and an axis-aligned bounding box (AABB).
  586. /// </summary>
  587. /// <param name="obbCenter">The center of the OBB.</param>
  588. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  589. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  590. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  591. /// <param name="aabbMin">The minimum corner of the AABB.</param>
  592. /// <param name="aabbMax">The maximum corner of the AABB.</param>
  593. /// <returns>
  594. /// <see cref="ContainmentType.Disjoint"/> if the volumes do not overlap; <see cref="ContainmentType.Contains"/> if the
  595. /// AABB is fully contained within the OBB; otherwise, <see cref="ContainmentType.Intersects"/>.
  596. /// </returns>
  597. /// <remarks>
  598. /// The overlap test is performed first. If the volumes overlap, containment is determined by testing whether all four
  599. /// AABB corners lie inside the OBB (inclusive).
  600. /// </remarks>
  601. public static ContainmentType ContainsObbAabb(Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents, Vector2 aabbMin, Vector2 aabbMax)
  602. {
  603. Vector2 aabbCenter = (aabbMin + aabbMax) * 0.5f;
  604. Vector2 aabbHalf = (aabbMax - aabbMin) * 0.5f;
  605. // Disjoint check
  606. if (!IntersectsAabbObb(aabbCenter, aabbHalf, obbCenter, obbAxisX, obbAxisY, obbHalfExtents))
  607. return ContainmentType.Disjoint;
  608. // Contains: all 4 AABB corners inside OBB
  609. Vector2 c0 = new Vector2(aabbMin.X, aabbMin.Y);
  610. Vector2 c1 = new Vector2(aabbMax.X, aabbMin.Y);
  611. Vector2 c2 = new Vector2(aabbMax.X, aabbMax.Y);
  612. Vector2 c3 = new Vector2(aabbMin.X, aabbMax.Y);
  613. if (ContainsObbPoint(c0, obbCenter, obbAxisX, obbAxisY, obbHalfExtents) == ContainmentType.Contains &&
  614. ContainsObbPoint(c1, obbCenter, obbAxisX, obbAxisY, obbHalfExtents) == ContainmentType.Contains &&
  615. ContainsObbPoint(c2, obbCenter, obbAxisX, obbAxisY, obbHalfExtents) == ContainmentType.Contains &&
  616. ContainsObbPoint(c3, obbCenter, obbAxisX, obbAxisY, obbHalfExtents) == ContainmentType.Contains)
  617. return ContainmentType.Contains;
  618. return ContainmentType.Intersects;
  619. }
  620. /// <summary>
  621. /// Determines the containment relationship between an oriented bounding box (OBB) and a circle.
  622. /// </summary>
  623. /// <param name="boxCenter">The center of the OBB.</param>
  624. /// <param name="axisX">The OBB local X axis direction.</param>
  625. /// <param name="axisY">The OBB local Y axis direction.</param>
  626. /// <param name="halfExtents">The half-widths of the OBB along <paramref name="axisX"/> and <paramref name="axisY"/>.</param>
  627. /// <param name="circleCenter">The center of the circle.</param>
  628. /// <param name="circleRadius">The circle radius. Must be non-negative.</param>
  629. /// <returns>
  630. /// <see cref="ContainmentType.Disjoint"/> if the circle lies entirely outside the box;
  631. /// <see cref="ContainmentType.Contains"/> if the circle is fully contained within the box;
  632. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  633. /// </returns>
  634. /// <remarks>
  635. /// The circle center is projected into OBB local space. The circle is contained when its local-space extent along each
  636. /// axis fits within the corresponding half extent, i.e. <c>|dot(d, axisX)| + r &lt;= halfExtents.X</c> and
  637. /// <c>|dot(d, axisY)| + r &lt;= halfExtents.Y</c>.
  638. /// </remarks>
  639. public static ContainmentType ContainsObbCircle(Vector2 boxCenter, Vector2 axisX, Vector2 axisY, Vector2 halfExtents, Vector2 circleCenter, float circleRadius)
  640. {
  641. if (circleRadius < 0.0f)
  642. return ContainmentType.Disjoint;
  643. // Disjoint check
  644. if (!IntersectsCircleObb(circleCenter, circleRadius, boxCenter, axisX, axisY, halfExtents))
  645. return ContainmentType.Disjoint;
  646. // Containment: circle must fit within OBB extents in local space
  647. Vector2 d = circleCenter - boxCenter;
  648. float localX = Vector2.Dot(d, axisX);
  649. float localY = Vector2.Dot(d, axisY);
  650. float r = circleRadius + Epsilon;
  651. if (MathF.Abs(localX) + r <= halfExtents.X &&
  652. MathF.Abs(localY) + r <= halfExtents.Y)
  653. return ContainmentType.Contains;
  654. return ContainmentType.Intersects;
  655. }
  656. /// <summary>
  657. /// Determines the containment relationship between two oriented bounding boxes (OBBs).
  658. /// </summary>
  659. /// <param name="aCenter">The center of the reference OBB.</param>
  660. /// <param name="aAxisX">The reference OBB local X axis direction.</param>
  661. /// <param name="aAxisY">The reference OBB local Y axis direction.</param>
  662. /// <param name="aHalf">The half-widths of the reference OBB along <paramref name="aAxisX"/> and <paramref name="aAxisY"/>.</param>
  663. /// <param name="bCenter">The center of the OBB being tested.</param>
  664. /// <param name="bAxisX">The tested OBB local X axis direction.</param>
  665. /// <param name="bAxisY">The tested OBB local Y axis direction.</param>
  666. /// <param name="bHalf">The half-widths of the tested OBB along <paramref name="bAxisX"/> and <paramref name="bAxisY"/>.</param>
  667. /// <returns>
  668. /// <see cref="ContainmentType.Disjoint"/> if the boxes do not overlap; <see cref="ContainmentType.Contains"/> if the
  669. /// second box is fully contained within the first; otherwise, <see cref="ContainmentType.Intersects"/>.
  670. /// </returns>
  671. /// <remarks>
  672. /// This method first performs an overlap test. If the boxes overlap, containment is determined by testing whether all
  673. /// four corners of the second OBB lie inside the first OBB.
  674. /// </remarks>
  675. public static ContainmentType ContainsObbObb(Vector2 aCenter, Vector2 aAxisX, Vector2 aAxisY, Vector2 aHalf, Vector2 bCenter, Vector2 bAxisX, Vector2 bAxisY, Vector2 bHalf)
  676. {
  677. if (!IntersectsObbObb(aCenter, aAxisX, aAxisY, aHalf, bCenter, bAxisX, bAxisY, bHalf))
  678. return ContainmentType.Disjoint;
  679. // Contains all 4 B corners inside A
  680. Vector2 bx = bAxisX * bHalf.X;
  681. Vector2 by = bAxisY * bHalf.Y;
  682. Vector2 c0 = bCenter - bx - by;
  683. Vector2 c1 = bCenter + bx - by;
  684. Vector2 c2 = bCenter + bx + by;
  685. Vector2 c3 = bCenter - bx + by;
  686. if (ContainsObbPoint(c0, aCenter, aAxisX, aAxisY, aHalf) == ContainmentType.Contains &&
  687. ContainsObbPoint(c1, aCenter, aAxisX, aAxisY, aHalf) == ContainmentType.Contains &&
  688. ContainsObbPoint(c2, aCenter, aAxisX, aAxisY, aHalf) == ContainmentType.Contains &&
  689. ContainsObbPoint(c3, aCenter, aAxisX, aAxisY, aHalf) == ContainmentType.Contains)
  690. return ContainmentType.Contains;
  691. return ContainmentType.Intersects;
  692. }
  693. /// <summary>
  694. /// Determines the containment relationship between an oriented bounding box (OBB) and a capsule.
  695. /// </summary>
  696. /// <param name="boxCenter">The center of the OBB.</param>
  697. /// <param name="axisX">The OBB local X axis direction.</param>
  698. /// <param name="axisY">The OBB local Y axis direction.</param>
  699. /// <param name="halfExtents">The half-widths of the OBB along <paramref name="axisX"/> and <paramref name="axisY"/>.</param>
  700. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  701. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  702. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  703. /// <returns>
  704. /// <see cref="ContainmentType.Disjoint"/> if the capsule lies entirely outside the box;
  705. /// <see cref="ContainmentType.Contains"/> if the capsule is fully contained within the box;
  706. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  707. /// </returns>
  708. /// <remarks>
  709. /// This method first performs an intersection test. If the shapes overlap, containment is determined by contracting
  710. /// the OBB half extents by <paramref name="capsuleRadius"/> and testing whether both capsule endpoints lie inside the
  711. /// contracted OBB.
  712. /// </remarks>
  713. public static ContainmentType ContainsObbCapsule(Vector2 boxCenter, Vector2 axisX, Vector2 axisY, Vector2 halfExtents, Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius)
  714. {
  715. if (capsuleRadius < 0.0f)
  716. return ContainmentType.Disjoint;
  717. if (!IntersectsObbCapsule(boxCenter, axisX, axisY, halfExtents, capsuleA, capsuleB, capsuleRadius))
  718. return ContainmentType.Disjoint;
  719. Vector2 innerHalf = new Vector2(halfExtents.X - capsuleRadius, halfExtents.Y - capsuleRadius);
  720. // If the contracted OBB is invalid, it can't contain a capsule of this radius
  721. if (innerHalf.X < -Epsilon || innerHalf.Y < -Epsilon)
  722. return ContainmentType.Intersects;
  723. if (ContainsObbPoint(capsuleA, boxCenter, axisX, axisY, innerHalf) == ContainmentType.Contains &&
  724. ContainsObbPoint(capsuleB, boxCenter, axisX, axisY, innerHalf) == ContainmentType.Contains)
  725. return ContainmentType.Contains;
  726. return ContainmentType.Intersects;
  727. }
  728. /// <summary>
  729. /// Determines the containment relationship between an oriented bounding box (OBB) and a convex polygon.
  730. /// </summary>
  731. /// <param name="boxCenter">The center of the OBB.</param>
  732. /// <param name="axisX">The OBB local X axis direction.</param>
  733. /// <param name="axisY">The OBB local Y axis direction.</param>
  734. /// <param name="halfExtents">The half-widths of the OBB along <paramref name="axisX"/> and <paramref name="axisY"/>.</param>
  735. /// <param name="vertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  736. /// <param name="normals">
  737. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  738. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  739. /// </param>
  740. /// <returns>
  741. /// <see cref="ContainmentType.Disjoint"/> if the polygon lies entirely outside the box;
  742. /// <see cref="ContainmentType.Contains"/> if the polygon is fully contained within the box;
  743. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  744. /// </returns>
  745. /// <remarks>
  746. /// This method first performs an intersection test. If the shapes overlap, containment is determined by verifying
  747. /// that every polygon vertex lies inside the OBB.
  748. /// </remarks>
  749. public static ContainmentType ContainsObbConvexPolygon(Vector2 boxCenter, Vector2 axisX, Vector2 axisY, Vector2 halfExtents, Vector2[] vertices, Vector2[] normals)
  750. {
  751. if (!IsValidPolygon(vertices, normals))
  752. return ContainmentType.Disjoint;
  753. if (!IntersectsObbConvexPolygon(boxCenter, axisX, axisY, halfExtents, vertices, normals))
  754. return ContainmentType.Disjoint;
  755. for (int i = 0; i < vertices.Length; i++)
  756. {
  757. if (ContainsObbPoint(vertices[i], boxCenter, axisX, axisY, halfExtents) == ContainmentType.Disjoint)
  758. return ContainmentType.Intersects;
  759. }
  760. return ContainmentType.Contains;
  761. }
  762. #endregion
  763. #region Containment (Capsule Contains)
  764. /// <summary>
  765. /// Determines whether a point is contained within a capsule.
  766. /// </summary>
  767. /// <param name="point">The point to test.</param>
  768. /// <param name="a">The first endpoint of the capsule line segment.</param>
  769. /// <param name="b">The second endpoint of the capsule line segment.</param>
  770. /// <param name="radius">The capsule radius. Must be non-negative.</param>
  771. /// <returns>
  772. /// <see cref="ContainmentType.Contains"/> if the point lies within the capsule or on its boundary; otherwise,
  773. /// <see cref="ContainmentType.Disjoint"/>.
  774. /// </returns>
  775. /// <remarks>
  776. /// A capsule is the set of points whose distance to the segment <c>[a, b]</c> is less than or equal to
  777. /// <paramref name="radius"/>. This method compares the squared distance from the point to the segment against
  778. /// <c>radius^2</c> to avoid a square root.
  779. /// </remarks>
  780. public static ContainmentType ContainsCapsulePoint(Vector2 point, Vector2 a, Vector2 b, float radius)
  781. {
  782. if (radius < 0.0f)
  783. return ContainmentType.Disjoint;
  784. float r = radius + Epsilon;
  785. float rr = r * r;
  786. float d2 = DistanceSquaredPointSegment(point, a, b, out _, out _);
  787. return d2 <= rr ? ContainmentType.Contains : ContainmentType.Disjoint;
  788. }
  789. /// <summary>
  790. /// Determines the containment relationship between a capsule and an axis-aligned bounding box (AABB).
  791. /// </summary>
  792. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  793. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  794. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  795. /// <param name="aabbMin">The minimum corner of the AABB.</param>
  796. /// <param name="aabbMax">The maximum corner of the AABB.</param>
  797. /// <returns>
  798. /// <see cref="ContainmentType.Disjoint"/> if the box lies entirely outside the capsule;
  799. /// <see cref="ContainmentType.Contains"/> if the box is fully contained within the capsule;
  800. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  801. /// </returns>
  802. /// <remarks>
  803. /// This method first performs an intersection test. If the shapes overlap, containment is determined by testing
  804. /// whether all four AABB corners lie inside the capsule.
  805. /// </remarks>
  806. public static ContainmentType ContainsCapsuleAabb(Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius, Vector2 aabbMin, Vector2 aabbMax)
  807. {
  808. if (capsuleRadius < 0f)
  809. return ContainmentType.Disjoint;
  810. if (!IntersectsAabbCapsule(aabbMin, aabbMax, capsuleA, capsuleB, capsuleRadius))
  811. return ContainmentType.Disjoint;
  812. Vector2 c0 = new Vector2(aabbMin.X, aabbMin.Y);
  813. Vector2 c1 = new Vector2(aabbMax.X, aabbMin.Y);
  814. Vector2 c2 = new Vector2(aabbMax.X, aabbMax.Y);
  815. Vector2 c3 = new Vector2(aabbMin.X, aabbMax.Y);
  816. if (ContainsCapsulePoint(c0, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains &&
  817. ContainsCapsulePoint(c1, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains &&
  818. ContainsCapsulePoint(c2, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains &&
  819. ContainsCapsulePoint(c3, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains)
  820. return ContainmentType.Contains;
  821. return ContainmentType.Intersects;
  822. }
  823. /// <summary>
  824. /// Determines the containment relationship between a capsule and a circle.
  825. /// </summary>
  826. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  827. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  828. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  829. /// <param name="circleCenter">The center of the circle.</param>
  830. /// <param name="circleRadius">The circle radius. Must be non-negative.</param>
  831. /// <returns>
  832. /// <see cref="ContainmentType.Disjoint"/> if the circle lies entirely outside the capsule;
  833. /// <see cref="ContainmentType.Contains"/> if the circle is fully contained within the capsule;
  834. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  835. /// </returns>
  836. /// <remarks>
  837. /// This method first performs an intersection test. If the shapes overlap, the circle is contained when its center is
  838. /// within a distance of <c>capsuleRadius - circleRadius</c> from the capsule segment <c>[capsuleA, capsuleB]</c>.
  839. /// The test is performed using squared distance to avoid a square root.
  840. /// </remarks>
  841. public static ContainmentType ContainsCapsuleCircle(Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius, Vector2 circleCenter, float circleRadius)
  842. {
  843. if (capsuleRadius < 0f || circleRadius < 0f)
  844. return ContainmentType.Disjoint;
  845. // Disjoint check first (touch counts as NOT disjoint)
  846. if (!IntersectsCircleCapsule(circleCenter, circleRadius, capsuleA, capsuleB, capsuleRadius))
  847. return ContainmentType.Disjoint;
  848. // Contains if the circle fits fully inside the capsule
  849. float inner = (capsuleRadius - circleRadius) + Epsilon;
  850. if (inner < 0f)
  851. return ContainmentType.Intersects;
  852. float inner2 = inner * inner;
  853. float d2 = DistanceSquaredPointSegment(circleCenter, capsuleA, capsuleB, out _, out _);
  854. return d2 <= inner2 ? ContainmentType.Contains : ContainmentType.Intersects;
  855. }
  856. /// <summary>
  857. /// Determines the containment relationship between a capsule and an oriented bounding box (OBB).
  858. /// </summary>
  859. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  860. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  861. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  862. /// <param name="obbCenter">The center of the OBB.</param>
  863. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  864. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  865. /// <param name="obbHalf">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  866. /// <returns>
  867. /// <see cref="ContainmentType.Disjoint"/> if the OBB lies entirely outside the capsule;
  868. /// <see cref="ContainmentType.Contains"/> if the OBB is fully contained within the capsule;
  869. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  870. /// </returns>
  871. /// <remarks>
  872. /// This method first performs an intersection test. If the shapes overlap, containment is determined by testing
  873. /// whether all four OBB corners lie inside the capsule.
  874. /// </remarks>
  875. public static ContainmentType ContainsCapsuleObb(Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius, Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalf)
  876. {
  877. if (capsuleRadius < 0f)
  878. return ContainmentType.Disjoint;
  879. if (!IntersectsObbCapsule(obbCenter, obbAxisX, obbAxisY, obbHalf, capsuleA, capsuleB, capsuleRadius))
  880. return ContainmentType.Disjoint;
  881. Vector2 ex = obbAxisX * obbHalf.X;
  882. Vector2 ey = obbAxisY * obbHalf.Y;
  883. Vector2 c0 = obbCenter - ex - ey;
  884. Vector2 c1 = obbCenter + ex - ey;
  885. Vector2 c2 = obbCenter + ex + ey;
  886. Vector2 c3 = obbCenter - ex + ey;
  887. if (ContainsCapsulePoint(c0, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains &&
  888. ContainsCapsulePoint(c1, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains &&
  889. ContainsCapsulePoint(c2, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains &&
  890. ContainsCapsulePoint(c3, capsuleA, capsuleB, capsuleRadius) == ContainmentType.Contains)
  891. return ContainmentType.Contains;
  892. return ContainmentType.Intersects;
  893. }
  894. /// <summary>
  895. /// Determines the containment relationship between two capsules.
  896. /// </summary>
  897. /// <param name="a0">The first endpoint of the reference capsule segment.</param>
  898. /// <param name="a1">The second endpoint of the reference capsule segment.</param>
  899. /// <param name="aRadius">The reference capsule radius. Must be non-negative.</param>
  900. /// <param name="b0">The first endpoint of the capsule segment being tested.</param>
  901. /// <param name="b1">The second endpoint of the capsule segment being tested.</param>
  902. /// <param name="bRadius">The tested capsule radius. Must be non-negative.</param>
  903. /// <returns>
  904. /// <see cref="ContainmentType.Disjoint"/> if the capsules do not overlap;
  905. /// <see cref="ContainmentType.Contains"/> if the second capsule is fully contained within the first;
  906. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  907. /// </returns>
  908. /// <remarks>
  909. /// This method first performs an intersection test. If the capsules overlap, containment requires the segment
  910. /// endpoints of the second capsule to lie within a distance of <c>aRadius - bRadius</c> from the reference capsule
  911. /// segment <c>[a0, a1]</c>. The test is performed using squared distances to avoid a square root.
  912. /// </remarks>
  913. public static ContainmentType ContainsCapsuleCapsule(Vector2 a0, Vector2 a1, float aRadius, Vector2 b0, Vector2 b1, float bRadius)
  914. {
  915. if (aRadius < 0f || bRadius < 0f)
  916. return ContainmentType.Disjoint;
  917. if (!IntersectsCapsuleCapsule(a0, a1, aRadius, b0, b1, bRadius))
  918. return ContainmentType.Disjoint;
  919. // If B is larger, A can't contain it, but they do intersect
  920. float inner = (aRadius - bRadius) + Epsilon;
  921. if (inner < 0f)
  922. return ContainmentType.Intersects;
  923. float inner2 = inner * inner;
  924. float d20 = DistanceSquaredPointSegment(b0, a0, a1, out _, out _);
  925. if (d20 > inner2)
  926. return ContainmentType.Intersects;
  927. float d21 = DistanceSquaredPointSegment(b1, a0, a1, out _, out _);
  928. if (d21 > inner2)
  929. return ContainmentType.Intersects;
  930. return ContainmentType.Contains;
  931. }
  932. /// <summary>
  933. /// Determines the containment relationship between a capsule and a convex polygon.
  934. /// </summary>
  935. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  936. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  937. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  938. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  939. /// <param name="pNormals">
  940. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  941. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  942. /// </param>
  943. /// <returns>
  944. /// <see cref="ContainmentType.Disjoint"/> if the polygon lies entirely outside the capsule;
  945. /// <see cref="ContainmentType.Contains"/> if the polygon is fully contained within the capsule;
  946. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  947. /// </returns>
  948. /// <remarks>
  949. /// This method first performs an intersection test. If the shapes overlap, containment is determined by verifying
  950. /// that every polygon vertex lies inside the capsule.
  951. /// </remarks>
  952. public static ContainmentType ContainsCapsuleConvexPolygon(Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius, Vector2[] pVertices, Vector2[] pNormals)
  953. {
  954. if (capsuleRadius < 0f)
  955. return ContainmentType.Disjoint;
  956. if (!IsValidPolygon(pVertices, pNormals))
  957. return ContainmentType.Disjoint;
  958. if (!IntersectsCapsuleConvexPolygon(capsuleA, capsuleB, capsuleRadius, pVertices, pNormals))
  959. return ContainmentType.Disjoint;
  960. for (int i = 0; i < pVertices.Length; i++)
  961. {
  962. if (ContainsCapsulePoint(pVertices[i], capsuleA, capsuleB, capsuleRadius) == ContainmentType.Disjoint)
  963. return ContainmentType.Intersects;
  964. }
  965. return ContainmentType.Contains;
  966. }
  967. #endregion
  968. #region Containment (Convex Polygon Contains)
  969. /// <summary>
  970. /// Determines whether a point is contained within a convex polygon.
  971. /// </summary>
  972. /// <param name="point">The point to test.</param>
  973. /// <param name="vertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  974. /// <param name="normals">
  975. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  976. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  977. /// </param>
  978. /// <returns>
  979. /// <see cref="ContainmentType.Contains"/> if the point lies inside the polygon or on its boundary; otherwise,
  980. /// <see cref="ContainmentType.Disjoint"/>.
  981. /// </returns>
  982. /// <remarks>
  983. /// The polygon is treated as the intersection of half-spaces defined by its edges. For each edge normal <c>n</c>,
  984. /// the point is inside the polygon when <c>dot(n, point) &lt;= dot(n, vertices[i])</c>.
  985. /// </remarks>
  986. public static ContainmentType ContainsConvexPolygonPoint(Vector2 point, Vector2[] vertices, Vector2[] normals)
  987. {
  988. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  989. // Section 5.4.1 "Testing Point in Polygon" (half-space tests; adapted to convex polygon with outward normals)
  990. if (!IsValidPolygon(vertices, normals))
  991. return ContainmentType.Disjoint;
  992. for (int i = 0; i < vertices.Length; i++)
  993. {
  994. Vector2 n = normals[i];
  995. float planeD = Vector2.Dot(n, vertices[i]);
  996. // Inside halfspace if Dot(n, p) <= planD
  997. if (Vector2.Dot(n, point) > planeD + Epsilon)
  998. return ContainmentType.Disjoint;
  999. }
  1000. return ContainmentType.Contains;
  1001. }
  1002. /// <summary>
  1003. /// Determines the containment relationship between a convex polygon and an axis-aligned bounding box (AABB).
  1004. /// </summary>
  1005. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1006. /// <param name="pNormals">
  1007. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  1008. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  1009. /// </param>
  1010. /// <param name="aabbMin">The minimum corner of the AABB.</param>
  1011. /// <param name="aabbMax">The maximum corner of the AABB.</param>
  1012. /// <returns>
  1013. /// <see cref="ContainmentType.Disjoint"/> if the box lies entirely outside the polygon;
  1014. /// <see cref="ContainmentType.Contains"/> if the box is fully contained within the polygon;
  1015. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  1016. /// </returns>
  1017. /// <remarks>
  1018. /// This method first performs an intersection test. If the shapes overlap, containment is determined by testing
  1019. /// whether all four AABB corners lie inside the polygon.
  1020. /// </remarks>
  1021. public static ContainmentType ContainsConvexPolygonAabb(Vector2[] pVertices, Vector2[] pNormals, Vector2 aabbMin, Vector2 aabbMax)
  1022. {
  1023. if (!IsValidPolygon(pVertices, pNormals))
  1024. return ContainmentType.Disjoint;
  1025. Vector2 aabbCenter = (aabbMin + aabbMax) * 0.5f;
  1026. Vector2 aabbHalf = (aabbMax - aabbMin) * 0.5f;
  1027. if (!IntersectsAabbConvexPolygon(aabbCenter, aabbHalf, pVertices, pNormals))
  1028. return ContainmentType.Disjoint;
  1029. // Contains: all AABB corners inside polygon
  1030. Vector2 c0 = new Vector2(aabbMin.X, aabbMin.Y);
  1031. Vector2 c1 = new Vector2(aabbMax.X, aabbMin.Y);
  1032. Vector2 c2 = new Vector2(aabbMax.X, aabbMax.Y);
  1033. Vector2 c3 = new Vector2(aabbMin.X, aabbMax.Y);
  1034. if (ContainsConvexPolygonPoint(c0, pVertices, pNormals) == ContainmentType.Contains &&
  1035. ContainsConvexPolygonPoint(c1, pVertices, pNormals) == ContainmentType.Contains &&
  1036. ContainsConvexPolygonPoint(c2, pVertices, pNormals) == ContainmentType.Contains &&
  1037. ContainsConvexPolygonPoint(c3, pVertices, pNormals) == ContainmentType.Contains)
  1038. return ContainmentType.Contains;
  1039. return ContainmentType.Intersects;
  1040. }
  1041. /// <summary>
  1042. /// Determines the containment relationship between a convex polygon and a circle.
  1043. /// </summary>
  1044. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1045. /// <param name="pNormals">
  1046. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  1047. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  1048. /// </param>
  1049. /// <param name="circleCenter">The center of the circle.</param>
  1050. /// <param name="circleRadius">The circle radius. Must be non-negative.</param>
  1051. /// <returns>
  1052. /// <see cref="ContainmentType.Disjoint"/> if the circle lies entirely outside the polygon;
  1053. /// <see cref="ContainmentType.Contains"/> if the circle is fully contained within the polygon;
  1054. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  1055. /// </returns>
  1056. /// <remarks>
  1057. /// After confirming the shapes overlap, containment is tested against each polygon half-space. The circle is contained
  1058. /// when, for every edge normal <c>n</c>, <c>dot(n, circleCenter) + circleRadius &lt;= dot(n, pVertices[i])</c>.
  1059. /// This requires <paramref name="pNormals"/> to be outward-facing unit normals.
  1060. /// </remarks>
  1061. public static ContainmentType ContainsConvexPolygonCircle(Vector2[] pVertices, Vector2[] pNormals, Vector2 circleCenter, float circleRadius)
  1062. {
  1063. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1064. // Section 5.2.2 "Testing Sphere Against Plane" (2D reduction: circle vs line)
  1065. // Related: Section 5.2.8 "Testing Sphere Against Polygon" (apply plane test to all polygon edge half-spaces)
  1066. if (circleRadius < 0.0f)
  1067. return ContainmentType.Disjoint;
  1068. if (!IsValidPolygon(pVertices, pNormals))
  1069. return ContainmentType.Disjoint;
  1070. // If they don't intersect at all, containment is impossible
  1071. if (!IntersectsCircleConvexPolygon(circleCenter, circleRadius, pVertices, pNormals))
  1072. return ContainmentType.Disjoint;
  1073. // Contains if the circle is fully inside every polygon halfspace:
  1074. // Dot(n, center) + radius <= planeD
  1075. //
  1076. // NOTE: this assumes normals are outward facing unit normals
  1077. float r = circleRadius + Epsilon;
  1078. for (int i = 0; i < pVertices.Length; i++)
  1079. {
  1080. Vector2 n = pNormals[i];
  1081. float planeD = Vector2.Dot(n, pVertices[i]);
  1082. if (Vector2.Dot(n, circleCenter) + r > planeD)
  1083. return ContainmentType.Intersects;
  1084. }
  1085. return ContainmentType.Contains;
  1086. }
  1087. /// <summary>
  1088. /// Determines the containment relationship between a convex polygon and a capsule.
  1089. /// </summary>
  1090. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1091. /// <param name="pNormals">
  1092. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  1093. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  1094. /// </param>
  1095. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  1096. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  1097. /// <param name="capsuleRadius">The capsule radius. Must be non-negative.</param>
  1098. /// <returns>
  1099. /// <see cref="ContainmentType.Disjoint"/> if the capsule lies entirely outside the polygon;
  1100. /// <see cref="ContainmentType.Contains"/> if the capsule is fully contained within the polygon;
  1101. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  1102. /// </returns>
  1103. /// <remarks>
  1104. /// After confirming the shapes overlap, containment is tested against each polygon half-space. The capsule is
  1105. /// contained when, for every edge normal <c>n</c>, <c>max(dot(n, capsuleA), dot(n, capsuleB)) + capsuleRadius</c>
  1106. /// is less than or equal to <c>dot(n, pVertices[i])</c>. This requires <paramref name="pNormals"/> to be
  1107. /// outward-facing unit normals.
  1108. /// </remarks>
  1109. public static ContainmentType ContainsConvexPolygonCapsule(Vector2[] pVertices, Vector2[] pNormals, Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius)
  1110. {
  1111. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1112. // Section 5.2.2 "Testing Sphere Against Plane" (2D reduction: circle vs line)
  1113. if (capsuleRadius < 0f)
  1114. return ContainmentType.Disjoint;
  1115. if (!IsValidPolygon(pVertices, pNormals))
  1116. return ContainmentType.Disjoint;
  1117. if (!IntersectsCapsuleConvexPolygon(capsuleA, capsuleB, capsuleRadius, pVertices, pNormals))
  1118. return ContainmentType.Disjoint;
  1119. float r = capsuleRadius + Epsilon;
  1120. for (int i = 0; i < pVertices.Length; i++)
  1121. {
  1122. Vector2 n = pNormals[i];
  1123. float planeD = Vector2.Dot(n, pVertices[i]);
  1124. float da = Vector2.Dot(n, capsuleA);
  1125. float db = Vector2.Dot(n, capsuleB);
  1126. float max = da > db ? da : db;
  1127. if (max + r > planeD)
  1128. return ContainmentType.Intersects;
  1129. }
  1130. return ContainmentType.Contains;
  1131. }
  1132. /// <summary>
  1133. /// Determines the containment relationship between a convex polygon and an oriented bounding box (OBB).
  1134. /// </summary>
  1135. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1136. /// <param name="pNormals">
  1137. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  1138. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  1139. /// </param>
  1140. /// <param name="obbCenter">The center of the OBB.</param>
  1141. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  1142. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  1143. /// <param name="obbHalf">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  1144. /// <returns>
  1145. /// <see cref="ContainmentType.Disjoint"/> if the box lies entirely outside the polygon;
  1146. /// <see cref="ContainmentType.Contains"/> if the box is fully contained within the polygon;
  1147. /// otherwise, <see cref="ContainmentType.Intersects"/>.
  1148. /// </returns>
  1149. /// <remarks>
  1150. /// This method first performs an intersection test. If the shapes overlap, containment is determined by testing
  1151. /// whether all four OBB corners lie inside the polygon.
  1152. /// </remarks>
  1153. public static ContainmentType ContainsConvexPolygonObb(Vector2[] pVertices, Vector2[] pNormals, Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalf)
  1154. {
  1155. if (!IsValidPolygon(pVertices, pNormals))
  1156. return ContainmentType.Disjoint;
  1157. if (!IntersectsObbConvexPolygon(obbCenter, obbAxisX, obbAxisY, obbHalf, pVertices, pNormals))
  1158. return ContainmentType.Disjoint;
  1159. // Contains: all 4 OBB corners inside polygon
  1160. Vector2 ex = obbAxisX * obbHalf.X;
  1161. Vector2 ey = obbAxisY * obbHalf.Y;
  1162. Vector2 c0 = obbCenter - ex - ey;
  1163. Vector2 c1 = obbCenter + ex - ey;
  1164. Vector2 c2 = obbCenter + ex + ey;
  1165. Vector2 c3 = obbCenter - ex + ey;
  1166. if (ContainsConvexPolygonPoint(c0, pVertices, pNormals) == ContainmentType.Contains &&
  1167. ContainsConvexPolygonPoint(c1, pVertices, pNormals) == ContainmentType.Contains &&
  1168. ContainsConvexPolygonPoint(c2, pVertices, pNormals) == ContainmentType.Contains &&
  1169. ContainsConvexPolygonPoint(c3, pVertices, pNormals) == ContainmentType.Contains)
  1170. return ContainmentType.Contains;
  1171. return ContainmentType.Intersects;
  1172. }
  1173. /// <summary>
  1174. /// Determines the containment relationship between two convex polygons.
  1175. /// </summary>
  1176. /// <param name="aVertices">The reference polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1177. /// <param name="aNormals">
  1178. /// The per-edge outward unit normals for the reference polygon. The array length must match <paramref name="aVertices"/> so that
  1179. /// <c>aNormals[i]</c> corresponds to the edge from <c>aVertices[i]</c> to <c>aVertices[(i + 1) % aVertices.Length]</c>.
  1180. /// </param>
  1181. /// <param name="bVertices">The tested polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1182. /// <param name="bNormals">
  1183. /// The per-edge outward unit normals for the tested polygon. The array length must match <paramref name="bVertices"/> so that
  1184. /// <c>bNormals[i]</c> corresponds to the edge from <c>bVertices[i]</c> to <c>bVertices[(i + 1) % bVertices.Length]</c>.
  1185. /// </param>
  1186. /// <returns>
  1187. /// <see cref="ContainmentType.Disjoint"/> if the polygons do not overlap; <see cref="ContainmentType.Contains"/> if the
  1188. /// second polygon is fully contained within the first; otherwise, <see cref="ContainmentType.Intersects"/>.
  1189. /// </returns>
  1190. /// <remarks>
  1191. /// This method first performs an intersection test. If the polygons overlap, containment is determined by verifying
  1192. /// that every vertex of the second polygon lies inside the first polygon.
  1193. /// </remarks>
  1194. public static ContainmentType ContainsConvexPolygonConvexPolygon(Vector2[] aVertices, Vector2[] aNormals, Vector2[] bVertices, Vector2[] bNormals)
  1195. {
  1196. if (!IsValidPolygon(aVertices, aNormals) || !IsValidPolygon(bVertices, bNormals))
  1197. return ContainmentType.Disjoint;
  1198. // If they don't intersect at all, containment is impossible
  1199. if (!IntersectsConvexPolygonConvexPolygon(aVertices, aNormals, bVertices, bNormals))
  1200. return ContainmentType.Disjoint;
  1201. // A contains B if all of B's vertices are inside A
  1202. for (int i = 0; i < bVertices.Length; i++)
  1203. {
  1204. if (ContainsConvexPolygonPoint(bVertices[i], aVertices, aNormals) == ContainmentType.Disjoint)
  1205. return ContainmentType.Intersects;
  1206. }
  1207. return ContainmentType.Contains;
  1208. }
  1209. #endregion
  1210. #endregion
  1211. #region Projection
  1212. /// <summary>
  1213. /// Projects a set of points onto an axis and returns the resulting scalar interval.
  1214. /// </summary>
  1215. /// <param name="vertices">The points to project.</param>
  1216. /// <param name="axis">The axis onto which the points are projected.</param>
  1217. /// <param name="min">When this method returns, contains the minimum projection value.</param>
  1218. /// <param name="max">When this method returns, contains the maximum projection value.</param>
  1219. /// <remarks>
  1220. /// The projection of a point <c>v</c> onto <paramref name="axis"/> is computed as <c>dot(v, axis)</c>.
  1221. /// </remarks>
  1222. public static void ProjectOntoAxis(Vector2[] vertices, Vector2 axis, out float min, out float max)
  1223. {
  1224. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1225. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  1226. min = float.MaxValue;
  1227. max = float.MinValue;
  1228. for (int i = 0; i < vertices.Length; i++)
  1229. {
  1230. float p = Vector2.Dot(vertices[i], axis);
  1231. if (p < min) min = p;
  1232. if (p > max) max = p;
  1233. }
  1234. }
  1235. /// <summary>
  1236. /// Projects an axis-aligned bounding box (AABB) onto an axis and returns the resulting scalar interval.
  1237. /// </summary>
  1238. /// <param name="center">The center of the AABB.</param>
  1239. /// <param name="halfExtents">The half-extents of the AABB along the world X and Y axes.</param>
  1240. /// <param name="axis">The axis onto which the AABB is projected.</param>
  1241. /// <param name="min">When this method returns, contains the minimum projection value.</param>
  1242. /// <param name="max">When this method returns, contains the maximum projection value.</param>
  1243. /// <remarks>
  1244. /// The interval is computed as <c>[dot(center, axis) - r, dot(center, axis) + r]</c>, where
  1245. /// <c>r = halfExtents.X * |axis.X| + halfExtents.Y * |axis.Y|</c>.
  1246. /// </remarks>
  1247. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  1248. public static void ProjectAabbOntoAxis(Vector2 center, Vector2 halfExtents, Vector2 axis, out float min, out float max)
  1249. {
  1250. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1251. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  1252. // Axis does not need to be normalized as long as both shapes use the same axis
  1253. float c = Vector2.Dot(center, axis);
  1254. float r = halfExtents.X * MathF.Abs(axis.X) + halfExtents.Y * MathF.Abs(axis.Y);
  1255. min = c - r;
  1256. max = c + r;
  1257. }
  1258. /// <summary>
  1259. /// Projects an oriented bounding box (OBB) onto an axis and returns the resulting scalar interval.
  1260. /// </summary>
  1261. /// <param name="center">The center of the OBB.</param>
  1262. /// <param name="axisX">The OBB local X axis direction.</param>
  1263. /// <param name="axisY">The OBB local Y axis direction.</param>
  1264. /// <param name="halfExtents">The half-extents of the OBB along <paramref name="axisX"/> and <paramref name="axisY"/>.</param>
  1265. /// <param name="axis">The axis onto which the OBB is projected.</param>
  1266. /// <param name="min">When this method returns, contains the minimum projection value.</param>
  1267. /// <param name="max">When this method returns, contains the maximum projection value.</param>
  1268. /// <remarks>
  1269. /// The interval is computed as <c>[dot(center, axis) - r, dot(center, axis) + r]</c>, where
  1270. /// <c>r = halfExtents.X * |dot(axisX, axis)| + halfExtents.Y * |dot(axisY, axis)|</c>.
  1271. /// </remarks>
  1272. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  1273. public static void ProjectObbOntoAxis(Vector2 center, Vector2 axisX, Vector2 axisY, Vector2 halfExtents, Vector2 axis, out float min, out float max)
  1274. {
  1275. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1276. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  1277. float c = Vector2.Dot(center, axis);
  1278. // Radius is sum of projected half-extents onto the axis
  1279. float r = halfExtents.X * MathF.Abs(Vector2.Dot(axisX, axis)) +
  1280. halfExtents.Y * MathF.Abs(Vector2.Dot(axisY, axis));
  1281. min = c - r;
  1282. max = c + r;
  1283. }
  1284. /// <summary>
  1285. /// Determines whether the projections of two point sets overlap on an axis.
  1286. /// </summary>
  1287. /// <param name="aVerts">The vertices of the first shape.</param>
  1288. /// <param name="bVerts">The vertices of the second shape.</param>
  1289. /// <param name="axis">The axis onto which both point sets are projected.</param>
  1290. /// <returns><see langword="true"/> if the projection intervals overlap; otherwise, <see langword="false"/>.</returns>
  1291. public static bool OverlapOnAxis(Vector2[] aVerts, Vector2[] bVerts, Vector2 axis)
  1292. {
  1293. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1294. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  1295. ProjectOntoAxis(aVerts, axis, out float minA, out float maxA);
  1296. ProjectOntoAxis(bVerts, axis, out float minB, out float maxB);
  1297. return IntervalsOverlap(minA, maxA, minB, maxB);
  1298. }
  1299. /// <summary>
  1300. /// Determines whether the projections of an axis-aligned bounding box (AABB) and a point set overlap on an axis.
  1301. /// </summary>
  1302. /// <param name="aabbCenter">The center of the AABB.</param>
  1303. /// <param name="aabbHalfExtents">The half-extents of the AABB along the world X and Y axes.</param>
  1304. /// <param name="polygonVertices">The vertices of the shape being tested.</param>
  1305. /// <param name="axis">The axis onto which both shapes are projected.</param>
  1306. /// <returns><see langword="true"/> if the projection intervals overlap; otherwise, <see langword="false"/>.</returns>
  1307. public static bool OverlapOnAxis(Vector2 aabbCenter, Vector2 aabbHalfExtents, Vector2[] polygonVertices, Vector2 axis)
  1308. {
  1309. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1310. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  1311. ProjectAabbOntoAxis(aabbCenter, aabbHalfExtents, axis, out float minA, out float maxA);
  1312. ProjectOntoAxis(polygonVertices, axis, out float minB, out float maxB);
  1313. return IntervalsOverlap(minA, maxA, minB, maxB);
  1314. }
  1315. #endregion
  1316. #region Distance / Closest Point
  1317. /// <summary>
  1318. /// Computes the closest points between a ray and a line segment and returns the corresponding parameters and squared distance.
  1319. /// </summary>
  1320. /// <param name="rayOrigin">The ray origin.</param>
  1321. /// <param name="rayDirection">
  1322. /// The ray direction vector. The ray is parameterized as <c>rayOrigin + s * rayDirection</c> with <c>s &gt;= 0</c>.
  1323. /// </param>
  1324. /// <param name="segA">The first endpoint of the segment.</param>
  1325. /// <param name="segB">The second endpoint of the segment.</param>
  1326. /// <param name="sRay">
  1327. /// When this method returns, contains the ray parameter <c>s</c> of the closest point on the ray. This value is clamped to
  1328. /// <c>s &gt;= 0</c>.
  1329. /// </param>
  1330. /// <param name="tSeg">
  1331. /// When this method returns, contains the segment parameter <c>t</c> of the closest point on the segment. The segment is
  1332. /// parameterized as <c>segA + t * (segB - segA)</c> with <c>0 &lt;= t &lt;= 1</c>.
  1333. /// </param>
  1334. /// <param name="distanceSquared">When this method returns, contains the squared distance between the closest points.</param>
  1335. public static void ClosestPointRaySegment(Vector2 rayOrigin, Vector2 rayDirection, Vector2 segA, Vector2 segB, out float sRay, out float tSeg, out float distanceSquared)
  1336. {
  1337. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1338. // Section 5.1.9 "Closest Points of Two Line Segments" (ray vs segment adaptation)
  1339. // Solve closest points on the supporting lines, then clamp to ray/segment parameter domains.
  1340. Vector2 d1 = rayDirection;
  1341. Vector2 d2 = segB - segA;
  1342. Vector2 r = rayOrigin - segA;
  1343. float a = Vector2.Dot(d1, d1);
  1344. float e = Vector2.Dot(d2, d2);
  1345. // Degenerate ray (treat as point)
  1346. if (a <= Epsilon)
  1347. {
  1348. // Ray is a point at rayOrigin, clamp to segment
  1349. if (e <= Epsilon)
  1350. {
  1351. tSeg = 0.0f;
  1352. }
  1353. else
  1354. {
  1355. tSeg = MathHelper.Clamp(Vector2.Dot(-r, d2) / e, 0.0f, 1.0f);
  1356. }
  1357. Vector2 q = segA + tSeg * d2;
  1358. sRay = 0.0f;
  1359. distanceSquared = Vector2.DistanceSquared(rayOrigin, q);
  1360. return;
  1361. }
  1362. // Degenerate segment (treat as point)
  1363. if (e <= Epsilon)
  1364. {
  1365. // Segment is a point at segA, clamp ray to s >= 0
  1366. float c = Vector2.Dot(d1, r);
  1367. sRay = MathF.Max(-c / a, 0.0f);
  1368. tSeg = 0.0f;
  1369. Vector2 p = rayOrigin + sRay * d1;
  1370. distanceSquared = Vector2.DistanceSquared(p, segA);
  1371. return;
  1372. }
  1373. float b = Vector2.Dot(d1, d2);
  1374. float c2 = Vector2.Dot(d1, r);
  1375. float f = Vector2.Dot(d2, r);
  1376. float denom = a * e - b * b;
  1377. float s = 0.0f;
  1378. float t = 0.0f;
  1379. // If not parallel, compute closest point on infinite lines first
  1380. if (MathF.Abs(denom) > Epsilon)
  1381. {
  1382. s = (b * f - c2 * e) / denom;
  1383. }
  1384. else
  1385. {
  1386. // Parallel-ish, pick s=0 initially, we'll clamp below
  1387. s = 0.0f;
  1388. }
  1389. // Clamp s to ray domain [0, +inf]
  1390. if (s < 0.0f)
  1391. {
  1392. s = 0.0f;
  1393. t = MathHelper.Clamp(f / e, 0.0f, 1.0f);
  1394. }
  1395. else
  1396. {
  1397. // Compute t from s
  1398. t = (b * s + f) / e;
  1399. // Clamp t to segment [0,1] and recompute s if needed
  1400. if (t < 0.0f)
  1401. {
  1402. t = 0.0f;
  1403. s = MathF.Max(-c2 / a, 0.0f);
  1404. }
  1405. else if (t > 1.0f)
  1406. {
  1407. t = 1.0f;
  1408. s = MathF.Max((b - c2) / a, 0.0f);
  1409. }
  1410. }
  1411. Vector2 pClosest = rayOrigin + s * d1;
  1412. Vector2 qClosest = segA + t * d2;
  1413. sRay = s;
  1414. tSeg = t;
  1415. distanceSquared = Vector2.DistanceSquared(pClosest, qClosest);
  1416. }
  1417. /// <summary>
  1418. /// Computes the squared distance from a point to a line segment and returns the closest point on the segment.
  1419. /// </summary>
  1420. /// <param name="point">The point to measure from.</param>
  1421. /// <param name="a">The first endpoint of the segment.</param>
  1422. /// <param name="b">The second endpoint of the segment.</param>
  1423. /// <param name="t">
  1424. /// When this method returns, contains the segment parameter of the closest point. The segment is parameterized as
  1425. /// <c>a + t * (b - a)</c> with <c>0 &lt;= t &lt;= 1</c>.
  1426. /// </param>
  1427. /// <param name="closestPoint">When this method returns, contains the closest point on the segment to <paramref name="point"/>.</param>
  1428. /// <returns>The squared distance between <paramref name="point"/> and the segment <c>[a, b]</c>.</returns>
  1429. /// <remarks>
  1430. /// The parameter is computed by projecting <c>(point - a)</c> onto <c>(b - a)</c> and clamping to the segment domain:
  1431. /// <c>t = clamp(dot(point - a, b - a) / dot(b - a, b - a), 0, 1)</c>. Squared distance is returned to avoid a square root.
  1432. /// </remarks>
  1433. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  1434. public static float DistanceSquaredPointSegment(Vector2 point, Vector2 a, Vector2 b, out float t, out Vector2 closestPoint)
  1435. {
  1436. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1437. // Section 5.1.2 "Closest Point on Line Segment to Point"
  1438. Vector2 ab = b - a;
  1439. float denom = Vector2.Dot(ab, ab);
  1440. // Check if segment is degenerate
  1441. if (denom <= EpsilonSq)
  1442. {
  1443. // Segment is degenerate, treat it as a point
  1444. t = 0.0f;
  1445. closestPoint = a;
  1446. Vector2 d = point - a;
  1447. return d.X * d.X + d.Y * d.Y;
  1448. }
  1449. t = Vector2.Dot(point - a, ab) / denom;
  1450. t = MathHelper.Clamp(t, 0.0f, 1.0f);
  1451. closestPoint = a + t * ab;
  1452. Vector2 diff = point - closestPoint;
  1453. return diff.LengthSquared();
  1454. }
  1455. /// <summary>
  1456. /// Computes the squared distance between two line segments and returns the closest points on each segment.
  1457. /// </summary>
  1458. /// <param name="p1">The first endpoint of the first segment.</param>
  1459. /// <param name="q1">The second endpoint of the first segment.</param>
  1460. /// <param name="p2">The first endpoint of the second segment.</param>
  1461. /// <param name="q2">The second endpoint of the second segment.</param>
  1462. /// <param name="s">
  1463. /// When this method returns, contains the parameter of the closest point on the first segment. The first segment is
  1464. /// parameterized as <c>p1 + s * (q1 - p1)</c> with <c>0 &lt;= s &lt;= 1</c>.
  1465. /// </param>
  1466. /// <param name="t">
  1467. /// When this method returns, contains the parameter of the closest point on the second segment. The second segment is
  1468. /// parameterized as <c>p2 + t * (q2 - p2)</c> with <c>0 &lt;= t &lt;= 1</c>.
  1469. /// </param>
  1470. /// <param name="c1">When this method returns, contains the closest point on the first segment.</param>
  1471. /// <param name="c2">When this method returns, contains the closest point on the second segment.</param>
  1472. /// <returns>The squared distance between the two segments.</returns>
  1473. /// <remarks>
  1474. /// Based on the closest-point computation for two segments described in:
  1475. /// <para>
  1476. /// C. Ericson, <i>Real-Time Collision Detection</i>, Morgan Kaufmann, 2005.
  1477. /// </para>
  1478. /// The method solves for the closest points on the infinite lines, then clamps the parameters to the segment domains.
  1479. /// Degenerate segments (zero length) are treated as points. Squared distance is returned to avoid a square root.
  1480. /// </remarks>
  1481. public static float DistanceSquaredSegmentSegment(Vector2 p1, Vector2 q1, Vector2 p2, Vector2 q2, out float s, out float t, out Vector2 c1, out Vector2 c2)
  1482. {
  1483. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1484. // Derived from Section 5.1.9 "Closest Points of Two Line Segments" (ray/segment domain adaptation)
  1485. Vector2 d1 = q1 - p1; // Direction vector of segment S1
  1486. Vector2 d2 = q2 - p2; // Direction vector of segment S2
  1487. Vector2 r = p1 - p2;
  1488. float a = Vector2.Dot(d1, d1); // Squared length of segment S1
  1489. float e = Vector2.Dot(d2, d2); // Squared length of segment S2
  1490. float f = Vector2.Dot(d2, r);
  1491. // Default outputs
  1492. s = 0.0f;
  1493. t = 0.0f;
  1494. // Check if either or both segments degenerate into points
  1495. if (a <= Epsilon && e <= Epsilon)
  1496. {
  1497. c1 = p1;
  1498. c2 = p2;
  1499. Vector2 d = c1 - c2;
  1500. return d.LengthSquared();
  1501. }
  1502. if (a <= Epsilon)
  1503. {
  1504. // First segment degenerates into a point
  1505. s = 0.0f;
  1506. t = f / e;
  1507. t = MathHelper.Clamp(t, 0.0f, 1.0f);
  1508. }
  1509. else
  1510. {
  1511. float c = Vector2.Dot(d1, r);
  1512. if (e <= Epsilon)
  1513. {
  1514. // Second segment degenerates into a point
  1515. t = 0.0f;
  1516. s = MathHelper.Clamp(-c / a, 0.0f, 1.0f);
  1517. }
  1518. else
  1519. {
  1520. // General nondegenerate case starts here
  1521. float b = Vector2.Dot(d1, d2);
  1522. float denom = a * e - b * b; // Always nonnegative
  1523. // If segments not parallel, compute closest point on L1, to L2 and
  1524. // clamp to segment s! Else pick arbitrary s (here 0)
  1525. if (MathF.Abs(denom) > Epsilon)
  1526. {
  1527. s = MathHelper.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
  1528. }
  1529. else
  1530. {
  1531. s = 0.0f;
  1532. }
  1533. // Compute point on L2 closest to S1(s) using
  1534. // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
  1535. t = (b * s + f) / e;
  1536. // If t in [0,1] do nothing. Else clamp t, recompute s for
  1537. // the new value of t using
  1538. // s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1) = (t*b - c) / a
  1539. // and clamp s to [0,1]
  1540. if (t < 0.0f)
  1541. {
  1542. t = 0.0f;
  1543. s = MathHelper.Clamp(-c / a, 0.0f, 1.0f);
  1544. }
  1545. else if (t > 1.0f)
  1546. {
  1547. t = 1.0f;
  1548. s = MathHelper.Clamp((b - c) / a, 0.0f, 1.0f);
  1549. }
  1550. }
  1551. }
  1552. c1 = p1 + d1 * s;
  1553. c2 = p2 + d2 * t;
  1554. Vector2 diff = c1 - c2;
  1555. return diff.LengthSquared();
  1556. }
  1557. /// <summary>
  1558. /// Computes the squared distance from a point to an axis-aligned bounding box (AABB).
  1559. /// </summary>
  1560. /// <param name="point">The point to measure from.</param>
  1561. /// <param name="min">The minimum corner of the AABB.</param>
  1562. /// <param name="max">The maximum corner of the AABB.</param>
  1563. /// <returns>The squared distance between <paramref name="point"/> and the AABB.</returns>
  1564. /// <remarks>
  1565. /// The closest point on the AABB is found by clamping each coordinate of <paramref name="point"/> to the range
  1566. /// defined by <paramref name="min"/> and <paramref name="max"/>, then computing the squared distance to that point.
  1567. /// Squared distance is returned to avoid a square root.
  1568. /// </remarks>
  1569. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  1570. public static float DistanceSquaredPointAabb(Vector2 point, Vector2 min, Vector2 max)
  1571. {
  1572. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1573. // Section 5.1.3 "Closest Point on AABB to Point"
  1574. float cx = MathHelper.Clamp(point.X, min.X, max.X);
  1575. float cy = MathHelper.Clamp(point.Y, min.Y, max.Y);
  1576. float dx = point.X - cx;
  1577. float dy = point.Y - cy;
  1578. return dx * dx + dy * dy;
  1579. }
  1580. /// <summary>
  1581. /// Computes the squared distance from a point to an oriented bounding box (OBB).
  1582. /// </summary>
  1583. /// <param name="point">The point to measure from.</param>
  1584. /// <param name="obbCenter">The center of the OBB.</param>
  1585. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  1586. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  1587. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  1588. /// <returns>The squared distance between <paramref name="point"/> and the OBB.</returns>
  1589. /// <remarks>
  1590. /// The point is projected into the OBB's local space using dot products with the box axes. The closest point in local
  1591. /// space is obtained by clamping the local coordinates to <c>[-halfExtents, +halfExtents]</c>, and the squared
  1592. /// distance is computed in that local space. Squared distance is returned to avoid a square root.
  1593. /// </remarks>
  1594. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  1595. public static float DistanceSquaredPointObb(Vector2 point, Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents)
  1596. {
  1597. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1598. // Section 5.1.4 "Closest Point on OBB to Point"
  1599. Vector2 d = point - obbCenter;
  1600. // Point in local space
  1601. float x = Vector2.Dot(d, obbAxisX);
  1602. float y = Vector2.Dot(d, obbAxisY);
  1603. float cx = MathHelper.Clamp(x, -obbHalfExtents.X, obbHalfExtents.X);
  1604. float cy = MathHelper.Clamp(y, -obbHalfExtents.Y, obbHalfExtents.Y);
  1605. float dx = x - cx;
  1606. float dy = y - cy;
  1607. return dx * dx + dy * dy;
  1608. }
  1609. /// <summary>
  1610. /// Computes the squared distance from a point to a convex polygon.
  1611. /// </summary>
  1612. /// <param name="p">The point to measure from.</param>
  1613. /// <param name="vertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1614. /// <param name="normals">
  1615. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  1616. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  1617. /// </param>
  1618. /// <returns>The squared distance between <paramref name="p"/> and the polygon. Returns <c>0</c> when the point is inside.</returns>
  1619. /// <remarks>
  1620. /// If the point lies inside the polygon, the distance is zero. Otherwise, the distance is the minimum squared distance
  1621. /// to any polygon edge segment. Squared distance is returned to avoid a square root.
  1622. /// </remarks>
  1623. public static float DistanceSquaredPointConvexPolygon(Vector2 p, Vector2[] vertices, Vector2[] normals)
  1624. {
  1625. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1626. // Derived from:
  1627. // - Section 5.2.1 "Separating-axis Test" (convex point containment)
  1628. // - Section 5.1.2 "Closest Point on Line Segment to Point"
  1629. // 2D reduction of convex polyhedron distance computation
  1630. if (ContainsConvexPolygonPoint(p, vertices, normals) == ContainmentType.Contains)
  1631. return 0.0f;
  1632. float best = float.MaxValue;
  1633. for (int i = 0; i < vertices.Length; i++)
  1634. {
  1635. int j = (i + 1) % vertices.Length;
  1636. float d2 = DistanceSquaredPointSegment(p, vertices[i], vertices[j], out _, out _);
  1637. if (d2 < best) best = d2;
  1638. }
  1639. return best;
  1640. }
  1641. /// <summary>
  1642. /// Computes the squared distance between a line segment and an axis-aligned bounding box (AABB).
  1643. /// </summary>
  1644. /// <param name="a">The first endpoint of the segment.</param>
  1645. /// <param name="b">The second endpoint of the segment.</param>
  1646. /// <param name="min">The minimum corner of the AABB.</param>
  1647. /// <param name="max">The maximum corner of the AABB.</param>
  1648. /// <returns>The squared distance between the segment <c>[a, b]</c> and the AABB. Returns <c>0</c> when they intersect.</returns>
  1649. /// <remarks>
  1650. /// If the segment intersects the AABB, the distance is zero. Otherwise, the distance is the minimum of:
  1651. /// <list type="bullet">
  1652. /// <item><description>The squared distance from each segment endpoint to the AABB.</description></item>
  1653. /// <item><description>The squared distance between the segment and each of the four AABB edge segments.</description></item>
  1654. /// </list>
  1655. /// Squared distance is returned to avoid a square root.
  1656. /// </remarks>
  1657. public static float DistanceSquaredSegmentAabb(Vector2 a, Vector2 b, Vector2 min, Vector2 max)
  1658. {
  1659. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1660. // Derived from:
  1661. // - Section 5.1.3 "Closest Point on AABB to Point" (endpoint distance)
  1662. // - Section 5.1.9 "Closest Points of Two Line Segments" (segment-edge distance)
  1663. // - Parametric clipping against convex regions (segment/AABB intersection test; 2D reduction)
  1664. Vector2 d = b - a;
  1665. float dd = Vector2.Dot(d, d);
  1666. // Check for degenerate segment
  1667. if (dd < EpsilonSq)
  1668. {
  1669. // Segment is degenerate, treat as point
  1670. return DistanceSquaredPointAabb(a, min, max);
  1671. }
  1672. // If the segment intersects the AABB, distance is zero.
  1673. if (ClipLineToAabb(a, d, min, max, 0.0f, 1.0f, out _, out _))
  1674. return 0.0f;
  1675. // Otherwise, minimum of:
  1676. // - endpoints to AABB
  1677. // - segment to each AABB edge segment
  1678. float best = DistanceSquaredPointAabb(a, min, max);
  1679. float db = DistanceSquaredPointAabb(b, min, max);
  1680. if (db < best) best = db;
  1681. // AABB corners
  1682. Vector2 c0 = new Vector2(min.X, min.Y);
  1683. Vector2 c1 = new Vector2(max.X, min.Y);
  1684. Vector2 c2 = new Vector2(max.X, max.Y);
  1685. Vector2 c3 = new Vector2(min.X, max.Y);
  1686. // Distance segment-to-edge
  1687. float dist;
  1688. dist = DistanceSquaredSegmentSegment(a, b, c0, c1, out _, out _, out _, out _);
  1689. if (dist < best) best = dist;
  1690. dist = DistanceSquaredSegmentSegment(a, b, c1, c2, out _, out _, out _, out _);
  1691. if (dist < best) best = dist;
  1692. dist = DistanceSquaredSegmentSegment(a, b, c2, c3, out _, out _, out _, out _);
  1693. if (dist < best) best = dist;
  1694. dist = DistanceSquaredSegmentSegment(a, b, c3, c0, out _, out _, out _, out _);
  1695. if (dist < best) best = dist;
  1696. return best;
  1697. }
  1698. /// <summary>
  1699. /// Computes the squared distance between a line segment and a convex polygon.
  1700. /// </summary>
  1701. /// <param name="a">The first endpoint of the segment.</param>
  1702. /// <param name="b">The second endpoint of the segment.</param>
  1703. /// <param name="vertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1704. /// <param name="normals">
  1705. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  1706. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  1707. /// </param>
  1708. /// <returns>
  1709. /// The squared distance between the segment <c>[a, b]</c> and the polygon. Returns <c>0</c> when they intersect.
  1710. /// If the polygon data is invalid, returns <see cref="float.MaxValue"/>.
  1711. /// </returns>
  1712. /// <remarks>
  1713. /// If the segment intersects the polygon, the distance is zero. Otherwise, the distance is the minimum squared distance
  1714. /// between the segment and any polygon edge segment. Squared distance is returned to avoid a square root.
  1715. /// </remarks>
  1716. public static float DistanceSquaredSegmentConvexPolygon(Vector2 a, Vector2 b, Vector2[] vertices, Vector2[] normals)
  1717. {
  1718. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1719. // Derived from:
  1720. // - Section 5.2.1 "Separating Axis Test" (convex polygon half-space representation and clipping)
  1721. // - Section 5.1.9 "Closest Points of Two Line Segments" (segment–edge distance)
  1722. // - Section 5.1.2 "Closest Point on Line Segment to Point" (degenerate segment handling)
  1723. if (!IsValidPolygon(vertices, normals))
  1724. return float.MaxValue;
  1725. Vector2 d = b - a;
  1726. float dd = Vector2.Dot(d, d);
  1727. // Check for degenerate segment
  1728. if (dd <= EpsilonSq)
  1729. {
  1730. // Treat segment as point
  1731. return DistanceSquaredPointConvexPolygon(a, vertices, normals);
  1732. }
  1733. // If segment intersects polygon, distance is zero
  1734. if (ClipLineToConvexPolygon(a, d, vertices, normals, 0.0f, 1.0f, out _, out _))
  1735. return 0.0f;
  1736. // Otherwise min distance to edges
  1737. float best = float.MaxValue;
  1738. for (int i = 0; i < vertices.Length; i++)
  1739. {
  1740. int j = (i + 1) % vertices.Length;
  1741. float d2 = DistanceSquaredSegmentSegment(a, b, vertices[i], vertices[j], out _, out _, out _, out _);
  1742. if (d2 < best) best = d2;
  1743. }
  1744. return best;
  1745. }
  1746. #endregion
  1747. #region Parametric Solvers
  1748. /// <summary>
  1749. /// Solves the parametric intersection between a line in implicit form and a parametric line, ray, or segment.
  1750. /// </summary>
  1751. /// <param name="lineNormal">The normal vector of the implicit line.</param>
  1752. /// <param name="lineDistance">The line offset <c>d</c> in the implicit equation <c>dot(lineNormal, x) = d</c>.</param>
  1753. /// <param name="origin">The origin of the parametric line.</param>
  1754. /// <param name="direction">The direction vector of the parametric line.</param>
  1755. /// <param name="t">
  1756. /// When this method returns, contains the parameter <c>t</c> such that <c>origin + t * direction</c> lies on the
  1757. /// implicit line.
  1758. /// </param>
  1759. /// <returns>
  1760. /// <see langword="true"/> if a unique solution exists; otherwise, <see langword="false"/> if the parametric line is
  1761. /// parallel to the implicit line.
  1762. /// </returns>
  1763. /// <remarks>
  1764. /// This method solves <c>dot(lineNormal, origin + t * direction) = lineDistance</c>. A unique intersection exists
  1765. /// when <c>dot(lineNormal, direction) != 0</c>.
  1766. /// </remarks>
  1767. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  1768. public static bool SolveParametricIntersectionWithImplicitLine(Vector2 lineNormal, float lineDistance, Vector2 origin, Vector2 direction, out float t)
  1769. {
  1770. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1771. // Derived from Section 5.3.1 "Intersecting Segment Against Plane" (2D reduction)
  1772. //
  1773. // Solves the intersection between:
  1774. // Implicit line: dot(n, x) = d
  1775. // Parametric line: x = origin + t * direction
  1776. // yielding:
  1777. // t = (d - dot(n, origin)) / dot(n, direction)
  1778. float denom = Vector2.Dot(lineNormal, direction);
  1779. if (MathF.Abs(denom) < Epsilon)
  1780. {
  1781. t = default;
  1782. return false;
  1783. }
  1784. t = (lineDistance - Vector2.Dot(lineNormal, origin)) / denom;
  1785. return true;
  1786. }
  1787. /// <summary>
  1788. /// Solves the parametric intersection of two 2D lines in point-direction form.
  1789. /// </summary>
  1790. /// <param name="origin1">Origin of the first line.</param>
  1791. /// <param name="direction1">Direction of the first line.</param>
  1792. /// <param name="origin2">Origin of the second line.</param>
  1793. /// <param name="direction2">Direction of the second line.</param>
  1794. /// <param name="t1">
  1795. /// When this method returns <see langword="true"/>, contains the parametric value on the first line.
  1796. /// </param>
  1797. /// <param name="t2">
  1798. /// When this method returns <see langword="true"/>, contains the parametric value on the second line.
  1799. /// </param>
  1800. /// <returns>
  1801. /// <see langword="true"/> if the directions are not parallel; otherwise, <see langword="false"/>.
  1802. /// Parallel and collinear cases are treated as no single-point intersection.
  1803. /// </returns>
  1804. [MethodImpl(MethodImplOptions.AggressiveInlining)]
  1805. public static bool SolveParametricIntersection2D(Vector2 origin1, Vector2 direction1, Vector2 origin2, Vector2 direction2, out float t1, out float t2)
  1806. {
  1807. // Standard 2D line intersection in point-direction form.
  1808. // Uses 2D cross product (perp-dot): cross(a,b) = perpDot(a,b).
  1809. // Solve: o1 + t1*d1 = o2 + t2*d2
  1810. // => t1 = cross(o2 - o1, d2) / cross(d1, d2)
  1811. // => t2 = cross(o2 - o1, d1) / cross(d1, d2)
  1812. float cross = Vector2Extensions.PerpDot(direction1, direction2);
  1813. // Check if parallel
  1814. if (MathF.Abs(cross) < Epsilon)
  1815. {
  1816. // Parallel or coincident, no intersection
  1817. t1 = default;
  1818. t2 = default;
  1819. return false;
  1820. }
  1821. Vector2 diff = origin2 - origin1;
  1822. // Standard 2D line intersection parameters using perp-dot.
  1823. t1 = Vector2Extensions.PerpDot(diff, direction2) / cross;
  1824. t2 = Vector2Extensions.PerpDot(diff, direction1) / cross;
  1825. return true;
  1826. }
  1827. #endregion
  1828. #region Clipping & Ray Intervals
  1829. /// <summary>
  1830. /// Clips a parametric line <c>P(t) = origin + t * direction</c> against an axis-aligned bounding box
  1831. /// defined by <paramref name="min"/> and <paramref name="max"/>, restricting the result to the parametric
  1832. /// interval [<paramref name="tLower"/>, <paramref name="tUpper"/>].
  1833. /// </summary>
  1834. /// <param name="origin">The parametric line origin.</param>
  1835. /// <param name="direction">The parametric line direction (not required to be unit length).</param>
  1836. /// <param name="min">Minimum corner of the AABB.</param>
  1837. /// <param name="max">Maximum corner of the AABB.</param>
  1838. /// <param name="tLower">Lower bound for the allowed parameter range (e.g. 0 for rays/segments, -infinity for lines).</param>
  1839. /// <param name="tUpper">Upper bound for the allowed parameter range (e.g. 1 for segments, +infinity for rays/lines).</param>
  1840. /// <param name="tEnter">
  1841. /// When this method returns <see langword="true"/>, contains the entering parameter of the clipped interval,
  1842. /// clamped to [<paramref name="tLower"/>, <paramref name="tUpper"/>].
  1843. /// </param>
  1844. /// <param name="tExit">
  1845. /// When this method returns <see langword="true"/>, contains the exiting parameter of the clipped interval,
  1846. /// clamped to [<paramref name="tLower"/>, <paramref name="tUpper"/>].
  1847. /// </param>
  1848. /// <returns>
  1849. /// <see langword="true"/> if the parametric line overlaps the AABB over any interval within
  1850. /// [<paramref name="tLower"/>, <paramref name="tUpper"/>]; otherwise, <see langword="false"/>.
  1851. /// </returns>
  1852. /// <remarks>
  1853. /// This is the 2D slab test (Ericson-style). Using different [tLower, tUpper] produces:
  1854. /// <list type="bullet">
  1855. /// <item><description>Infinite line: (-∞, +∞)</description></item>
  1856. /// <item><description>Ray: [0, +∞)</description></item>
  1857. /// <item><description>Segment: [0, 1]</description></item>
  1858. /// </list>
  1859. /// </remarks>
  1860. public static bool ClipLineToAabb(Vector2 origin, Vector2 direction, Vector2 min, Vector2 max, float tLower, float tUpper, out float tEnter, out float tExit)
  1861. {
  1862. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1863. // Section 5.3.3 "Intersecting Ray or Segment Against Box" (slab method / parametric interval clipping).
  1864. // Note: This is equivalent in form to Liang–Barsky style parametric line clipping against axis-aligned boundaries.
  1865. float tMin = tLower;
  1866. float tMax = tUpper;
  1867. // X Slab
  1868. if (MathF.Abs(direction.X) < Epsilon)
  1869. {
  1870. // parallel to X planes; must be within slab
  1871. if (origin.X < min.X - Epsilon || origin.X > max.X + Epsilon)
  1872. {
  1873. tEnter = tExit = default;
  1874. return false;
  1875. }
  1876. }
  1877. else
  1878. {
  1879. float inv = 1.0f / direction.X;
  1880. float t1 = (min.X - origin.X) * inv;
  1881. float t2 = (max.X - origin.X) * inv;
  1882. if (t1 > t2) (t1, t2) = (t2, t1);
  1883. if (!ClipInterval(ref tMin, ref tMax, t1, t2))
  1884. {
  1885. tEnter = tExit = default;
  1886. return false;
  1887. }
  1888. }
  1889. // Y slab
  1890. if (MathF.Abs(direction.Y) < Epsilon)
  1891. {
  1892. // Parallel to Y planes, must be within slab
  1893. if (origin.Y < min.Y - Epsilon || origin.Y > max.Y + Epsilon)
  1894. {
  1895. tEnter = tExit = default;
  1896. return false;
  1897. }
  1898. }
  1899. else
  1900. {
  1901. float inv = 1.0f / direction.Y;
  1902. float t1 = (min.Y - origin.Y) * inv;
  1903. float t2 = (max.Y - origin.Y) * inv;
  1904. if (t1 > t2) (t1, t2) = (t2, t1);
  1905. if (!ClipInterval(ref tMin, ref tMax, t1, t2))
  1906. {
  1907. tEnter = tExit = default;
  1908. return false;
  1909. }
  1910. }
  1911. tEnter = tMin;
  1912. tExit = tMax;
  1913. return true;
  1914. }
  1915. /// <summary>
  1916. /// Clips a parametric line against a convex polygon expressed as an intersection of half-spaces.
  1917. /// </summary>
  1918. /// <param name="origin">The line origin.</param>
  1919. /// <param name="direction">The line direction vector.</param>
  1920. /// <param name="vertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  1921. /// <param name="normals">
  1922. /// The per-edge outward unit normals. The array length must match <paramref name="vertices"/> so that
  1923. /// <c>normals[i]</c> corresponds to the edge from <c>vertices[i]</c> to <c>vertices[(i + 1) % vertices.Length]</c>.
  1924. /// </param>
  1925. /// <param name="tLower">The lower bound of the line parameter interval to clip.</param>
  1926. /// <param name="tUpper">The upper bound of the line parameter interval to clip.</param>
  1927. /// <param name="tEnter">When this method returns, contains the entering parameter of the clipped interval.</param>
  1928. /// <param name="tExit">When this method returns, contains the exiting parameter of the clipped interval.</param>
  1929. /// <returns>
  1930. /// <see langword="true"/> if the line intersects the polygon within the parameter range <c>[tLower, tUpper]</c>;
  1931. /// otherwise, <see langword="false"/>.
  1932. /// </returns>
  1933. /// <remarks>
  1934. /// This method incrementally intersects the parameter interval <c>[tLower, tUpper]</c> with the constraints induced by
  1935. /// each polygon edge half-space <c>dot(n, X) &lt;= dot(n, v)</c>. For a segment or ray, pass an appropriate parameter
  1936. /// range (for example, <c>[0, 1]</c> for a segment and <c>[0, +inf)</c> for a ray).
  1937. /// </remarks>
  1938. public static bool ClipLineToConvexPolygon(Vector2 origin, Vector2 direction, Vector2[] vertices, Vector2[] normals, float tLower, float tUpper, out float tEnter, out float tExit)
  1939. {
  1940. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  1941. // Derived from Section 5.3.8 "Intersecting Ray or Segment Against Convex Polyhedron" (2D reduction).
  1942. // Also known as Cyrus–Beck clipping (parametric line clipping against a convex polygon via half-space constraints).
  1943. // Half-space: dot(n, X) <= dot(n, v)
  1944. // Parametric: X(t) = origin + t * direction
  1945. if (!IsValidPolygon(vertices, normals))
  1946. {
  1947. tEnter = tExit = default;
  1948. return false;
  1949. }
  1950. float tMin = tLower;
  1951. float tMax = tUpper;
  1952. for (int i = 0; i < vertices.Length; i++)
  1953. {
  1954. Vector2 n = normals[i];
  1955. Vector2 v = vertices[i];
  1956. // Half space: Dot(n, X) <= Dot(n, v)
  1957. float planeD = Vector2.Dot(n, v);
  1958. float dist = planeD - Vector2.Dot(n, origin);
  1959. float denom = Vector2.Dot(n, direction);
  1960. // Parallel to plane
  1961. if (MathF.Abs(denom) < Epsilon)
  1962. {
  1963. // If outside (dist < 0) no intersection with the convex region
  1964. if (dist < -Epsilon)
  1965. {
  1966. tEnter = tExit = default;
  1967. return false;
  1968. }
  1969. // Otherwise, line is inside/on this half-space; no constraint from edge
  1970. continue;
  1971. }
  1972. float t = dist / denom;
  1973. // With outward normals:
  1974. // denom > 0 => moving toward increasing Dot(n, X) => exiting the inside half-space
  1975. // denom < 0 => moving inward => entering
  1976. if (denom > 0.0f)
  1977. {
  1978. if (t < tMax)
  1979. tMax = t;
  1980. }
  1981. else
  1982. {
  1983. if (t > tMin)
  1984. tMin = t;
  1985. }
  1986. if (tMin > tMax)
  1987. {
  1988. tEnter = tExit = default;
  1989. return false;
  1990. }
  1991. }
  1992. tEnter = tMin;
  1993. tExit = tMax;
  1994. return true;
  1995. }
  1996. /// <summary>
  1997. /// Computes the parametric intersection interval between a ray and a circle.
  1998. /// </summary>
  1999. /// <param name="origin">The ray origin.</param>
  2000. /// <param name="direction">
  2001. /// The ray direction vector. The ray is parameterized as <c>origin + t * direction</c> with <c>t &gt;= 0</c>.
  2002. /// </param>
  2003. /// <param name="center">The center of the circle.</param>
  2004. /// <param name="radius">The circle radius.</param>
  2005. /// <param name="tMin">
  2006. /// When this method returns, contains the smaller intersection parameter along the supporting line of the ray.
  2007. /// </param>
  2008. /// <param name="tMax">
  2009. /// When this method returns, contains the larger intersection parameter along the supporting line of the ray.
  2010. /// </param>
  2011. /// <returns>
  2012. /// <see langword="true"/> if the supporting line intersects the circle; otherwise, <see langword="false"/>.
  2013. /// </returns>
  2014. /// <remarks>
  2015. /// Based on solving the quadratic equation obtained by substituting the ray equation
  2016. /// <c>x = origin + t * direction</c> into the circle equation <c>|x - center|^2 = radius^2</c>.
  2017. /// The returned parameters are ordered such that <paramref name="tMin"/> is less than or equal to
  2018. /// <paramref name="tMax"/>.
  2019. /// </remarks>
  2020. public static bool RayCircleIntersectionInterval(Vector2 origin, Vector2 direction, Vector2 center, float radius, out float tMin, out float tMax)
  2021. {
  2022. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  2023. // Section 5.3.2 "Intersecting Ray or Segment Against Sphere" (2D reduction).
  2024. // Solves the quadratic |origin + t*direction - center|^2 = radius^2.
  2025. Vector2 m = origin - center;
  2026. float a = Vector2.Dot(direction, direction);
  2027. if (a <= EpsilonSq)
  2028. {
  2029. tMin = tMax = default;
  2030. return false;
  2031. }
  2032. float b = Vector2.Dot(m, direction);
  2033. float c = Vector2.Dot(m, m) - radius * radius;
  2034. // Ray origin outside circle (c > 0)
  2035. // and ray pointing away from circle (b > 0)
  2036. if (c > 0.0f && b > 0.0f)
  2037. {
  2038. tMin = tMax = default;
  2039. return false;
  2040. }
  2041. float discriminant = b * b - a * c;
  2042. // Negative discriminant means ray misses circle
  2043. if (discriminant < 0.0f)
  2044. {
  2045. tMin = tMax = default;
  2046. return false;
  2047. }
  2048. float sqrtD = MathF.Sqrt(discriminant);
  2049. float invA = 1.0f / a;
  2050. tMin = (-b - sqrtD) * invA;
  2051. tMax = (-b + sqrtD) * invA;
  2052. if (tMin > tMax)
  2053. (tMin, tMax) = (tMax, tMin);
  2054. // Clamp tMin to 0 for ray semantics
  2055. if (tMin < 0.0f)
  2056. tMin = 0.0f;
  2057. return true;
  2058. }
  2059. /// <summary>
  2060. /// Computes the parametric intersection interval between a ray and a capsule.
  2061. /// </summary>
  2062. /// <param name="rayOrigin">The ray origin.</param>
  2063. /// <param name="rayDirection">
  2064. /// The ray direction vector. The ray is parameterized as <c>rayOrigin + t * rayDirection</c> with <c>t &gt;= 0</c>.
  2065. /// </param>
  2066. /// <param name="segA">The first endpoint of the capsule line segment.</param>
  2067. /// <param name="segB">The second endpoint of the capsule line segment.</param>
  2068. /// <param name="radius">The capsule radius.</param>
  2069. /// <param name="tMin">When this method returns, contains the smaller intersection parameter along the supporting line of the ray.</param>
  2070. /// <param name="tMax">When this method returns, contains the larger intersection parameter along the supporting line of the ray.</param>
  2071. /// <returns>
  2072. /// <see langword="true"/> if the supporting line intersects the capsule; otherwise, <see langword="false"/>.
  2073. /// </returns>
  2074. /// <remarks>
  2075. /// The capsule is treated as the Minkowski sum of the segment <c>[segA, segB]</c> and a disk of radius <paramref name="radius"/>.
  2076. /// The returned interval is computed by finding the closest approach between the ray and the capsule's medial segment and
  2077. /// expanding along the ray by the available radial slack. Degenerate inputs are handled by reducing to point/segment or
  2078. /// ray/circle cases.
  2079. /// </remarks>
  2080. public static bool RayCapsuleIntersectionInterval(Vector2 rayOrigin, Vector2 rayDirection, Vector2 segA, Vector2 segB, float radius, out float tMin, out float tMax)
  2081. {
  2082. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  2083. // Capsule as swept sphere / Minkowski sum of segment and disk (concept).
  2084. // Built from these Ericson primitives (2D reductions / domain adaptations):
  2085. // - Section 5.3.2 "Intersecting Ray or Segment Against Sphere" (ray-circle interval)
  2086. // - Section 5.1.2 "Closest Point on Line Segment to Point" (point-segment distance; degenerate ray)
  2087. // - Section 5.1.9 "Closest Points of Two Line Segments" (adapted to closest points of ray vs segment)
  2088. // The returned interval is formed by expanding around the closest-approach parameter using the available radial slack.
  2089. float a = Vector2.Dot(rayDirection, rayDirection);
  2090. float radiusSq = radius * radius;
  2091. // Check for degenerate ray
  2092. if (a <= EpsilonSq)
  2093. {
  2094. // Ray is degenerate, treat as a point
  2095. tMin = tMax = 0.0f;
  2096. float distSq = DistanceSquaredPointSegment(rayOrigin, segA, segB, out _, out _);
  2097. return distSq <= radiusSq;
  2098. }
  2099. Vector2 segDir = segB - segA;
  2100. // Check for degenerate capsule
  2101. if (segDir.LengthSquared() <= EpsilonSq)
  2102. {
  2103. // Capsule is degenerate, treat as a circle
  2104. return RayCircleIntersectionInterval(rayOrigin, rayDirection, segA, radius, out tMin, out tMax);
  2105. }
  2106. // Check if parallel or colinear with capsule medial line segment
  2107. float cross = Vector2Extensions.PerpDot(rayDirection, segDir);
  2108. if (MathF.Abs(cross) <= Epsilon)
  2109. {
  2110. // Parallel/colinear case: build interval by projecting segment endpoints
  2111. float invA = 1.0f / a;
  2112. Vector2 toA = segA - rayOrigin;
  2113. float tA = Vector2.Dot(toA, rayDirection) * invA;
  2114. float tB = Vector2.Dot(segB - rayOrigin, rayDirection) * invA;
  2115. // Perpendicular distance from segA to ray line
  2116. Vector2 perp = toA - (Vector2.Dot(toA, rayDirection) * invA) * rayDirection;
  2117. float perpDistSq = Vector2.Dot(perp, perp);
  2118. if (perpDistSq > radiusSq)
  2119. {
  2120. tMin = tMax = default;
  2121. return false;
  2122. }
  2123. float minProj = MathF.Min(tA, tB);
  2124. float maxProj = MathF.Max(tA, tB);
  2125. // Expand interval by how far we can move along the ray while staying within radius
  2126. // For non-unit direction: delta = sqrt((R^2 - perp^2) / Dot(d,d))
  2127. float delta = MathF.Sqrt((radiusSq - perpDistSq) * invA);
  2128. tMin = minProj - delta;
  2129. tMax = maxProj + delta;
  2130. // Clamp tMin to 0 for ray semantics
  2131. if (tMin < 0.0f)
  2132. tMin = 0.0f;
  2133. return true;
  2134. }
  2135. // General case: closest approach to capsule medial line segment
  2136. ClosestPointRaySegment(rayOrigin, rayDirection, segA, segB, out float sRay, out _, out float distSqToAxis);
  2137. if (distSqToAxis > radiusSq)
  2138. {
  2139. tMin = tMax = default;
  2140. return false;
  2141. }
  2142. // Convert "radial slack" into parameter offset along the ray
  2143. float offset = MathF.Sqrt((radiusSq - distSqToAxis) / a);
  2144. tMin = sRay - offset;
  2145. tMax = sRay + offset;
  2146. if (tMin > tMax)
  2147. {
  2148. (tMin, tMax) = (tMax, tMin);
  2149. }
  2150. // Clamp tMin to 0 for ray semantics
  2151. if (tMin < 0.0f)
  2152. tMin = 0.0f;
  2153. return true;
  2154. }
  2155. #endregion
  2156. #region Intersections
  2157. /// <summary>
  2158. /// Determines whether two axis-aligned bounding boxes (AABBs) intersect.
  2159. /// </summary>
  2160. /// <param name="aMin">The minimum corner of the first AABB.</param>
  2161. /// <param name="aMax">The maximum corner of the first AABB.</param>
  2162. /// <param name="bMin">The minimum corner of the second AABB.</param>
  2163. /// <param name="bMax">The maximum corner of the second AABB.</param>
  2164. /// <returns>
  2165. /// <see langword="true"/> if the AABBs overlap or touch; otherwise, <see langword="false"/>.
  2166. /// </returns>
  2167. public static bool IntersectsAabbAabb(Vector2 aMin, Vector2 aMax, Vector2 bMin, Vector2 bMax)
  2168. {
  2169. // Exit with no intersection if separated along any axis
  2170. if (aMax.X < bMin.X || aMin.X > bMax.X) return false;
  2171. if (aMax.Y < bMin.Y || aMin.Y > bMax.Y) return false;
  2172. return true;
  2173. }
  2174. /// <summary>
  2175. /// Determines whether an axis-aligned bounding box (AABB) intersects a capsule.
  2176. /// </summary>
  2177. /// <param name="boxMin">The minimum corner of the AABB.</param>
  2178. /// <param name="boxMax">The maximum corner of the AABB.</param>
  2179. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  2180. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  2181. /// <param name="capsuleRadius">The capsule radius.</param>
  2182. /// <returns>
  2183. /// <see langword="true"/> if the capsule overlaps or touches the AABB; otherwise, <see langword="false"/>.
  2184. /// </returns>
  2185. /// <remarks>
  2186. /// The capsule intersects the AABB when the squared distance between the capsule segment <c>[capsuleA, capsuleB]</c>
  2187. /// and the AABB is less than or equal to <c>capsuleRadius^2</c>.
  2188. /// </remarks>
  2189. public static bool IntersectsAabbCapsule(Vector2 boxMin, Vector2 boxMax, Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius)
  2190. {
  2191. float radiusSq = capsuleRadius * capsuleRadius;
  2192. float distSq = DistanceSquaredSegmentAabb(capsuleA, capsuleB, boxMin, boxMax);
  2193. return distSq <= radiusSq;
  2194. }
  2195. /// <summary>
  2196. /// Determines whether an axis-aligned bounding box (AABB) intersects a convex polygon.
  2197. /// </summary>
  2198. /// <param name="aabbCenter">The center of the AABB.</param>
  2199. /// <param name="aabbHalfExtents">The half-widths of the AABB along the world X and Y axes.</param>
  2200. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  2201. /// <param name="pNormals">
  2202. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  2203. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  2204. /// </param>
  2205. /// <returns>
  2206. /// <see langword="true"/> if the shapes overlap or touch; otherwise, <see langword="false"/>.
  2207. /// </returns>
  2208. /// <remarks>
  2209. /// Separating-axis test (SAT) is performed using all polygon edge normals and the two AABB axes. If the projections are
  2210. /// disjoint on any tested axis, the shapes do not intersect.
  2211. /// </remarks>
  2212. public static bool IntersectsAabbConvexPolygon(Vector2 aabbCenter, Vector2 aabbHalfExtents, Vector2[] pVertices, Vector2[] pNormals)
  2213. {
  2214. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  2215. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  2216. if (!IsValidPolygon(pVertices, pNormals))
  2217. return false;
  2218. // Test polygon edge normals
  2219. for (int i = 0; i < pNormals.Length; i++)
  2220. {
  2221. if (!OverlapOnAxis(aabbCenter, aabbHalfExtents, pVertices, pNormals[i]))
  2222. return false;
  2223. }
  2224. // Test AABB axes
  2225. if (!OverlapOnAxis(aabbCenter, aabbHalfExtents, pVertices, Vector2.UnitX))
  2226. return false;
  2227. if (!OverlapOnAxis(aabbCenter, aabbHalfExtents, pVertices, Vector2.UnitY))
  2228. return false;
  2229. return true;
  2230. }
  2231. /// <summary>
  2232. /// Determines whether an axis-aligned bounding box (AABB) intersects an oriented bounding box (OBB).
  2233. /// </summary>
  2234. /// <param name="aabbCenter">The center of the AABB.</param>
  2235. /// <param name="aabbHalfExtents">The half-widths of the AABB along the world X and Y axes.</param>
  2236. /// <param name="obbCenter">The center of the OBB.</param>
  2237. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  2238. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  2239. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  2240. /// <returns>
  2241. /// <see langword="true"/> if the boxes overlap or touch; otherwise, <see langword="false"/>.
  2242. /// </returns>
  2243. /// <remarks>
  2244. /// Separating-axis test (SAT) is performed using the two AABB axes and the two OBB axes. If the projections are
  2245. /// disjoint on any tested axis, the boxes do not intersect.
  2246. /// </remarks>
  2247. public static bool IntersectsAabbObb(Vector2 aabbCenter, Vector2 aabbHalfExtents, Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents)
  2248. {
  2249. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  2250. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  2251. // AABB axis x
  2252. ProjectAabbOntoAxis(aabbCenter, aabbHalfExtents, Vector2.UnitX, out float minA, out float maxA);
  2253. ProjectObbOntoAxis(obbCenter, obbAxisX, obbAxisY, obbHalfExtents, Vector2.UnitX, out float minB, out float maxB);
  2254. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2255. // AABB axis y
  2256. ProjectAabbOntoAxis(aabbCenter, aabbHalfExtents, Vector2.UnitY, out minA, out maxA);
  2257. ProjectObbOntoAxis(obbCenter, obbAxisX, obbAxisY, obbHalfExtents, Vector2.UnitY, out minB, out maxB);
  2258. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2259. // OBB axis X
  2260. ProjectAabbOntoAxis(aabbCenter, aabbHalfExtents, obbAxisX, out minA, out maxA);
  2261. ProjectObbOntoAxis(obbCenter, obbAxisX, obbAxisY, obbHalfExtents, obbAxisX, out minB, out maxB);
  2262. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2263. // OBB axis Y
  2264. ProjectAabbOntoAxis(aabbCenter, aabbHalfExtents, obbAxisY, out minA, out maxA);
  2265. ProjectObbOntoAxis(obbCenter, obbAxisX, obbAxisY, obbHalfExtents, obbAxisY, out minB, out maxB);
  2266. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2267. return true;
  2268. }
  2269. /// <summary>
  2270. /// Determines whether two oriented bounding boxes (OBBs) intersect.
  2271. /// </summary>
  2272. /// <param name="aCenter">The center of the first OBB.</param>
  2273. /// <param name="aAxisX">The first OBB local X axis direction.</param>
  2274. /// <param name="aAxisY">The first OBB local Y axis direction.</param>
  2275. /// <param name="aHalf">The half-widths of the first OBB along <paramref name="aAxisX"/> and <paramref name="aAxisY"/>.</param>
  2276. /// <param name="bCenter">The center of the second OBB.</param>
  2277. /// <param name="bAxisX">The second OBB local X axis direction.</param>
  2278. /// <param name="bAxisY">The second OBB local Y axis direction.</param>
  2279. /// <param name="bHalf">The half-widths of the second OBB along <paramref name="bAxisX"/> and <paramref name="bAxisY"/>.</param>
  2280. /// <returns>
  2281. /// <see langword="true"/> if the boxes overlap or touch; otherwise, <see langword="false"/>.
  2282. /// </returns>
  2283. /// <remarks>
  2284. /// Separating-axis test (SAT) is performed using the two axes from each box. If the projections are disjoint on any
  2285. /// tested axis, the boxes are disjoint.
  2286. /// </remarks>
  2287. public static bool IntersectsObbObb(Vector2 aCenter, Vector2 aAxisX, Vector2 aAxisY, Vector2 aHalf, Vector2 bCenter, Vector2 bAxisX, Vector2 bAxisY, Vector2 bHalf)
  2288. {
  2289. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  2290. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  2291. // A axis X
  2292. ProjectObbOntoAxis(aCenter, aAxisX, aAxisY, aHalf, aAxisX, out float minA, out float maxA);
  2293. ProjectObbOntoAxis(bCenter, bAxisX, bAxisY, bHalf, aAxisX, out float minB, out float maxB);
  2294. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2295. // A axis Y
  2296. ProjectObbOntoAxis(aCenter, aAxisX, aAxisY, aHalf, aAxisY, out minA, out maxA);
  2297. ProjectObbOntoAxis(bCenter, bAxisX, bAxisY, bHalf, aAxisY, out minB, out maxB);
  2298. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2299. // B axis X
  2300. ProjectObbOntoAxis(aCenter, aAxisX, aAxisY, aHalf, bAxisX, out minA, out maxA);
  2301. ProjectObbOntoAxis(bCenter, bAxisX, bAxisY, bHalf, bAxisX, out minB, out maxB);
  2302. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2303. // B axis Y
  2304. ProjectObbOntoAxis(aCenter, aAxisX, aAxisY, aHalf, bAxisY, out minA, out maxA);
  2305. ProjectObbOntoAxis(bCenter, bAxisX, bAxisY, bHalf, bAxisY, out minB, out maxB);
  2306. if (!IntervalsOverlap(minA, maxA, minB, maxB)) return false;
  2307. return true;
  2308. }
  2309. /// <summary>
  2310. /// Determines whether an oriented bounding box (OBB) intersects a capsule.
  2311. /// </summary>
  2312. /// <param name="obbCenter">The center of the OBB.</param>
  2313. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  2314. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  2315. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  2316. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  2317. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  2318. /// <param name="capsuleRadius">The capsule radius.</param>
  2319. /// <returns>
  2320. /// <see langword="true"/> if the capsule overlaps or touches the OBB; otherwise, <see langword="false"/>.
  2321. /// </returns>
  2322. /// <remarks>
  2323. /// The capsule segment endpoints are transformed into the OBB's local space so that the OBB becomes an axis-aligned
  2324. /// box with extents <c>[-halfExtents, +halfExtents]</c>. The capsule intersects the OBB when the squared distance
  2325. /// between the transformed segment and that box is less than or equal to <c>capsuleRadius^2</c>.
  2326. /// </remarks>
  2327. public static bool IntersectsObbCapsule(Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents, Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius)
  2328. {
  2329. float radiusSq = capsuleRadius * capsuleRadius;
  2330. // Transform capsule segment into OBB local space (OBB becomes AABB)
  2331. Vector2 a = capsuleA - obbCenter;
  2332. Vector2 b = capsuleB - obbCenter;
  2333. Vector2 localA = new Vector2(Vector2.Dot(a, obbAxisX), Vector2.Dot(a, obbAxisY));
  2334. Vector2 localB = new Vector2(Vector2.Dot(b, obbAxisX), Vector2.Dot(b, obbAxisY));
  2335. Vector2 min = -obbHalfExtents;
  2336. Vector2 max = obbHalfExtents;
  2337. float distSq = DistanceSquaredSegmentAabb(localA, localB, min, max);
  2338. return distSq <= radiusSq;
  2339. }
  2340. /// <summary>
  2341. /// Determines whether an oriented bounding box (OBB) intersects a convex polygon.
  2342. /// </summary>
  2343. /// <param name="obbCenter">The center of the OBB.</param>
  2344. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  2345. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  2346. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  2347. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  2348. /// <param name="pNormals">
  2349. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  2350. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  2351. /// </param>
  2352. /// <returns>
  2353. /// <see langword="true"/> if the shapes overlap or touch; otherwise, <see langword="false"/>.
  2354. /// </returns>
  2355. /// <remarks>
  2356. /// Separating-axis test (SAT) is performed using all polygon edge normals and the two OBB axes. If the projections are
  2357. /// disjoint on any tested axis, the shapes are disjoint.
  2358. /// </remarks>
  2359. public static bool IntersectsObbConvexPolygon(Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents, Vector2[] pVertices, Vector2[] pNormals)
  2360. {
  2361. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  2362. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  2363. if (!IsValidPolygon(pVertices, pNormals))
  2364. return false;
  2365. float minP, maxP, minB, maxB;
  2366. // Test polygon edge normals
  2367. for (int i = 0; i < pVertices.Length; i++)
  2368. {
  2369. Vector2 axis = pNormals[i];
  2370. ProjectOntoAxis(pVertices, axis, out minP, out maxP);
  2371. ProjectObbOntoAxis(obbCenter, obbAxisX, obbAxisY, obbHalfExtents, axis, out minB, out maxB);
  2372. if (!IntervalsOverlap(minP, maxP, minB, maxB)) return false;
  2373. }
  2374. // OBB X Axis
  2375. ProjectOntoAxis(pVertices, obbAxisX, out minP, out maxP);
  2376. ProjectObbOntoAxis(obbCenter, obbAxisX, obbAxisY, obbHalfExtents, obbAxisX, out minB, out maxB);
  2377. if (!IntervalsOverlap(minP, maxP, minB, maxB)) return false;
  2378. // Obb Y Axis
  2379. ProjectOntoAxis(pVertices, obbAxisY, out minP, out maxP);
  2380. ProjectObbOntoAxis(obbCenter, obbAxisX, obbAxisY, obbHalfExtents, obbAxisY, out minB, out maxB);
  2381. if (!IntervalsOverlap(minP, maxP, minB, maxB)) return false;
  2382. return true;
  2383. }
  2384. /// <summary>
  2385. /// Determines whether a capsule intersects a convex polygon.
  2386. /// </summary>
  2387. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  2388. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  2389. /// <param name="capsuleRadius">The capsule radius.</param>
  2390. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  2391. /// <param name="pNormals">
  2392. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  2393. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  2394. /// </param>
  2395. /// <returns>
  2396. /// <see langword="true"/> if the capsule overlaps or touches the polygon; otherwise, <see langword="false"/>.
  2397. /// </returns>
  2398. /// <remarks>
  2399. /// The capsule intersects the polygon when the squared distance between the capsule segment <c>[capsuleA, capsuleB]</c>
  2400. /// and the polygon is less than or equal to <c>capsuleRadius^2</c>.
  2401. /// </remarks>
  2402. public static bool IntersectsCapsuleConvexPolygon(Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius, Vector2[] pVertices, Vector2[] pNormals)
  2403. {
  2404. float radiusSq = capsuleRadius * capsuleRadius;
  2405. float d2 = DistanceSquaredSegmentConvexPolygon(capsuleA, capsuleB, pVertices, pNormals);
  2406. return d2 <= radiusSq;
  2407. }
  2408. /// <summary>
  2409. /// Determines whether two circles intersect.
  2410. /// </summary>
  2411. /// <param name="aCenter">The center of the first circle.</param>
  2412. /// <param name="aRadius">The radius of the first circle.</param>
  2413. /// <param name="bCenter">The center of the second circle.</param>
  2414. /// <param name="bRadius">The radius of the second circle.</param>
  2415. /// <returns>
  2416. /// <see langword="true"/> if the circles overlap or touch; otherwise, <see langword="false"/>.
  2417. /// </returns>
  2418. public static bool IntersectsCircleCircle(Vector2 aCenter, float aRadius, Vector2 bCenter, float bRadius)
  2419. {
  2420. float r = aRadius + bRadius;
  2421. float sqDist = Vector2.DistanceSquared(aCenter, bCenter);
  2422. return sqDist <= r * r;
  2423. }
  2424. /// <summary>
  2425. /// Determines whether a circle intersects an axis-aligned bounding box (AABB).
  2426. /// </summary>
  2427. /// <param name="cCenter">The center of the circle.</param>
  2428. /// <param name="cRadius">The radius of the circle.</param>
  2429. /// <param name="boxMin">The minimum corner of the AABB.</param>
  2430. /// <param name="boxMax">The maximum corner of the AABB.</param>
  2431. /// <returns>
  2432. /// <see langword="true"/> if the circle overlaps or touches the AABB; otherwise, <see langword="false"/>.
  2433. /// </returns>
  2434. /// <remarks>
  2435. /// The circle intersects the AABB when the squared distance from the circle center to the box is less than or equal to
  2436. /// <c>cRadius^2</c>.
  2437. /// </remarks>
  2438. public static bool IntersectsCircleAabb(Vector2 cCenter, float cRadius, Vector2 boxMin, Vector2 boxMax)
  2439. {
  2440. float d2 = DistanceSquaredPointAabb(cCenter, boxMin, boxMax);
  2441. return d2 <= cRadius * cRadius;
  2442. }
  2443. /// <summary>
  2444. /// Determines whether a circle intersects an oriented bounding box (OBB).
  2445. /// </summary>
  2446. /// <param name="cCenter">The center of the circle.</param>
  2447. /// <param name="cRadius">The radius of the circle.</param>
  2448. /// <param name="obbCenter">The center of the OBB.</param>
  2449. /// <param name="obbAxisX">The OBB local X axis direction.</param>
  2450. /// <param name="obbAxisY">The OBB local Y axis direction.</param>
  2451. /// <param name="obbHalfExtents">The half-widths of the OBB along <paramref name="obbAxisX"/> and <paramref name="obbAxisY"/>.</param>
  2452. /// <returns>
  2453. /// <see langword="true"/> if the circle overlaps or touches the OBB; otherwise, <see langword="false"/>.
  2454. /// </returns>
  2455. /// <remarks>
  2456. /// The circle intersects the OBB when the squared distance from the circle center to the box is less than or equal to
  2457. /// <c>cRadius^2</c>.
  2458. /// </remarks>
  2459. public static bool IntersectsCircleObb(Vector2 cCenter, float cRadius, Vector2 obbCenter, Vector2 obbAxisX, Vector2 obbAxisY, Vector2 obbHalfExtents)
  2460. {
  2461. float d2 = DistanceSquaredPointObb(cCenter, obbCenter, obbAxisX, obbAxisY, obbHalfExtents);
  2462. return d2 <= cRadius * cRadius;
  2463. }
  2464. /// <summary>
  2465. /// Determines whether a circle intersects a convex polygon.
  2466. /// </summary>
  2467. /// <param name="cCenter">The center of the circle.</param>
  2468. /// <param name="cRadius">The radius of the circle.</param>
  2469. /// <param name="pVertices">The polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  2470. /// <param name="pNormals">
  2471. /// The per-edge outward unit normals. The array length must match <paramref name="pVertices"/> so that
  2472. /// <c>pNormals[i]</c> corresponds to the edge from <c>pVertices[i]</c> to <c>pVertices[(i + 1) % pVertices.Length]</c>.
  2473. /// </param>
  2474. /// <returns>
  2475. /// <see langword="true"/> if the circle overlaps or touches the polygon; otherwise, <see langword="false"/>.
  2476. /// </returns>
  2477. /// <remarks>
  2478. /// The circle intersects the polygon when the squared distance from the circle center to the polygon is less than or
  2479. /// equal to <c>cRadius^2</c>.
  2480. /// </remarks>
  2481. public static bool IntersectsCircleConvexPolygon(Vector2 cCenter, float cRadius, Vector2[] pVertices, Vector2[] pNormals)
  2482. {
  2483. if (!IsValidPolygon(pVertices, pNormals))
  2484. return false;
  2485. float d2 = DistanceSquaredPointConvexPolygon(cCenter, pVertices, pNormals);
  2486. return d2 <= cRadius * cRadius;
  2487. }
  2488. /// <summary>
  2489. /// Determines whether a circle intersects a capsule.
  2490. /// </summary>
  2491. /// <param name="circleCenter">The center of the circle.</param>
  2492. /// <param name="circleRadius">The radius of the circle.</param>
  2493. /// <param name="capsuleA">The first endpoint of the capsule line segment.</param>
  2494. /// <param name="capsuleB">The second endpoint of the capsule line segment.</param>
  2495. /// <param name="capsuleRadius">The capsule radius.</param>
  2496. /// <returns>
  2497. /// <see langword="true"/> if the circle overlaps or touches the capsule; otherwise, <see langword="false"/>.
  2498. /// </returns>
  2499. /// <remarks>
  2500. /// The circle intersects the capsule when the squared distance from the circle center to the capsule segment
  2501. /// <c>[capsuleA, capsuleB]</c> is less than or equal to <c>(circleRadius + capsuleRadius)^2</c>.
  2502. /// </remarks>
  2503. public static bool IntersectsCircleCapsule(Vector2 circleCenter, float circleRadius, Vector2 capsuleA, Vector2 capsuleB, float capsuleRadius)
  2504. {
  2505. float r = circleRadius + capsuleRadius;
  2506. float d2 = DistanceSquaredPointSegment(circleCenter, capsuleA, capsuleB, out _, out _);
  2507. return d2 <= r * r;
  2508. }
  2509. /// <summary>
  2510. /// Determines whether two capsules intersect.
  2511. /// </summary>
  2512. /// <param name="a0">The first endpoint of the first capsule line segment.</param>
  2513. /// <param name="a1">The second endpoint of the first capsule line segment.</param>
  2514. /// <param name="aRadius">The radius of the first capsule.</param>
  2515. /// <param name="b0">The first endpoint of the second capsule line segment.</param>
  2516. /// <param name="b1">The second endpoint of the second capsule line segment.</param>
  2517. /// <param name="bRadius">The radius of the second capsule.</param>
  2518. /// <returns>
  2519. /// <see langword="true"/> if the capsules overlap or touch; otherwise, <see langword="false"/>.
  2520. /// </returns>
  2521. /// <remarks>
  2522. /// Two capsules intersect when the squared distance between their medial segments is less than or equal to
  2523. /// <c>(aRadius + bRadius)^2</c>.
  2524. /// </remarks>
  2525. public static bool IntersectsCapsuleCapsule(Vector2 a0, Vector2 a1, float aRadius, Vector2 b0, Vector2 b1, float bRadius)
  2526. {
  2527. float r = aRadius + bRadius;
  2528. float rr = r * r;
  2529. float d2 = DistanceSquaredSegmentSegment(a0, a1, b0, b1, out _, out _, out _, out _);
  2530. return d2 <= rr;
  2531. }
  2532. /// <summary>
  2533. /// Determines whether two convex polygons intersect.
  2534. /// </summary>
  2535. /// <param name="aVertices">The first polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  2536. /// <param name="aNormals">
  2537. /// The per-edge outward unit normals for the first polygon. The array length must match <paramref name="aVertices"/> so that
  2538. /// <c>aNormals[i]</c> corresponds to the edge from <c>aVertices[i]</c> to <c>aVertices[(i + 1) % aVertices.Length]</c>.
  2539. /// </param>
  2540. /// <param name="bVertices">The second polygon vertices in winding order. A valid polygon requires at least three vertices.</param>
  2541. /// <param name="bNormals">
  2542. /// The per-edge outward unit normals for the second polygon. The array length must match <paramref name="bVertices"/> so that
  2543. /// <c>bNormals[i]</c> corresponds to the edge from <c>bVertices[i]</c> to <c>bVertices[(i + 1) % bVertices.Length]</c>.
  2544. /// </param>
  2545. /// <returns>
  2546. /// <see langword="true"/> if the polygons overlap or touch; otherwise, <see langword="false"/>.
  2547. /// </returns>
  2548. /// <remarks>
  2549. /// Separating-axis test (SAT) is performed using the edge normals of both polygons. If the projections are disjoint on
  2550. /// any tested axis, the polygons are disjoint.
  2551. /// </remarks>
  2552. public static bool IntersectsConvexPolygonConvexPolygon(Vector2[] aVertices, Vector2[] aNormals, Vector2[] bVertices, Vector2[] bNormals)
  2553. {
  2554. // C. Ericson, Real-Time Collision Detection, Morgan Kaufmann, 2005
  2555. // Section 5.2.1 "Separating-axis Test" (2D adaptation)
  2556. if (!IsValidPolygon(aVertices, aNormals) || !IsValidPolygon(bVertices, bNormals))
  2557. return false;
  2558. // Test A's axis
  2559. for (int i = 0; i < aVertices.Length; i++)
  2560. {
  2561. if (!OverlapOnAxis(aVertices, bVertices, aNormals[i]))
  2562. return false;
  2563. }
  2564. // Test B's axis
  2565. for (int i = 0; i < bVertices.Length; i++)
  2566. {
  2567. if (!OverlapOnAxis(aVertices, bVertices, bNormals[i]))
  2568. return false;
  2569. }
  2570. return true;
  2571. }
  2572. #endregion
  2573. }
  2574. }