using System; using System.Collections.Generic; using System.Text; using UnityEngine; namespace Thousandto.Core.Base { /// /// 2D的数学函数集合 /// public static class Math2d { public static float VectorDot( ref Vector2 a, ref Vector2 b ) { return a.x * b.x + a.y * b.y; } public struct LineSegment { public Vector2 Start; public Vector2 End; } public struct Circle { public Vector2 Center; public float Radius; public Circle( Vector2 center, float radius ) { this.Center = center; this.Radius = radius; } } public struct Sector { public Vector2 Center; public float Radius; public Vector2 SideDir1; public Vector2 SideDir2; public Vector2 Dir; public float CosHalfAngle; public Circle ToCircle() { return new Circle( Center, Radius ); } static void RotateDir( Vector2 centerDir, float cos_a, float sin_a, out Vector2 sideDir1, out Vector2 sideDir2 ) { float cos_na = cos_a; float sin_na = -sin_a; sideDir1 = new Vector2( centerDir.x * cos_a - centerDir.y * sin_a, centerDir.x * sin_a + centerDir.y * cos_a ); sideDir2 = new Vector2( centerDir.x * cos_na - centerDir.y * sin_na, centerDir.x * sin_na + centerDir.y * cos_na ); } static void RotateDir( Vector2 centerDir, float halfAngle, out Vector2 sideDir1, out Vector2 sideDir2 ) { float cos_a = Mathf.Cos( halfAngle ); float sin_a = Mathf.Sin( halfAngle ); float cos_na = cos_a; float sin_na = -sin_a; sideDir1 = new Vector2( centerDir.x * cos_a - centerDir.y * sin_a, centerDir.x * sin_a + centerDir.y * cos_a ); sideDir2 = new Vector2( centerDir.x * cos_na - centerDir.y * sin_na, centerDir.x * sin_na + centerDir.y * cos_na ); } public Sector( Vector2 center, float radius, Vector2 dir, float halfAngleRad ) { this.Center = center; this.Radius = radius; this.Dir = dir; float cos_a = Mathf.Cos( halfAngleRad ); float sin_a = Mathf.Sin( halfAngleRad ); RotateDir( dir, cos_a, sin_a, out this.SideDir1, out this.SideDir2 ); CosHalfAngle = cos_a; } } public struct Obb { public Vector2 Center; public Vector2 Exterts; public Vector2 Axis; // along local y public Obb( Vector2 center, Vector2 axisDir, float width, float height ) { this.Axis = axisDir; this.Center = center; this.Exterts = new Vector2( width * 0.5f, height * 0.5f ); } public Obb( Vector2 sideCenter, Vector2 axisDir, float halfWidth, float length, int unused ) { var sideCenter2 = sideCenter + axisDir * length; this.Center = ( sideCenter + sideCenter2 ) * 0.5f; this.Axis = axisDir; this.Exterts = new Vector2( halfWidth, length * 0.5f ); } } public static bool CollisionTest( ref Circle circle1, ref Circle circle2 ) { var axis = circle1.Center - circle2.Center; var minDisSq = circle1.Radius + circle2.Radius; minDisSq *= minDisSq; return axis.sqrMagnitude <= minDisSq; } public static bool CollisionTest( ref Circle circle, ref LineSegment line ) { Vector2 pt; Closest_LineSegment_Circle( circle, line, out pt ); Vector2 toPoint = circle.Center - pt; if ( toPoint.sqrMagnitude <= circle.Radius * circle.Radius ) { // There is a collision return true; } return false; } public static bool CollisionTest( ref Sector sector, ref Circle circle ) { var sectorCircle = sector.ToCircle(); // check if it is intersecting the whole cirlce. var minDisSq = sectorCircle.Radius + circle.Radius; minDisSq *= minDisSq; var axis = circle.Center - sectorCircle.Center; var lenSq = axis.sqrMagnitude; if ( lenSq > minDisSq ) { // target circle not collides with sector's circle return false; } var distance = Mathf.Sqrt( lenSq ); // normalize direction from to sector center to target's center var invLen = 1.0f / distance; axis.x *= invLen; axis.y *= invLen; // check if the angle between the circle centres lies in the angular range of the segment var cosAngle = VectorDot( ref sector.Dir, ref axis ); if ( cosAngle >= sector.CosHalfAngle ) { return true; } else { // check if it is intersecting either of the straight segment lines. LineSegment edge; edge.Start = sector.Center; edge.End = sector.Center + sector.SideDir1 * sector.Radius; if ( CollisionTest( ref circle, ref edge ) ) { return true; } edge.End = sector.Center + sector.SideDir2 * sector.Radius; if ( CollisionTest( ref circle, ref edge ) ) { return true; } } return false; } public static Vector2 ClosestPointOnObb( Obb obb, Vector2 pt ) { Vector2 retPt; Vector2 d = pt - obb.Center; retPt = obb.Center; float dist = VectorDot( ref d, ref obb.Axis ); if ( dist > obb.Exterts.y ) { dist = obb.Exterts.y; } if ( dist < -obb.Exterts.y ) { dist = -obb.Exterts.y; } retPt += dist * obb.Axis; var naxis = new Vector2( -obb.Axis.y, obb.Axis.x ); dist = VectorDot( ref d, ref naxis ); if ( dist > obb.Exterts.x ) { dist = obb.Exterts.x; } if ( dist < -obb.Exterts.x ) { dist = -obb.Exterts.x; } retPt += dist * naxis; return retPt; } public static bool CollisionTest( Obb obb, Circle circle ) { var pt = ClosestPointOnObb( obb, circle.Center ); var v = pt - circle.Center; return v.sqrMagnitude <= circle.Radius * circle.Radius; } public static bool IsPointOnSegment( Vector2 point, Vector2 start, Vector2 end ) { var dir = end - start; var idir = point - start; if ( Vector2.Dot( idir, dir ) >= 0 ) { var normal = new Vector2( -dir.y, dir.x ); var dis = VectorDot( ref idir, ref normal ); if ( Mathf.Abs( dis ) < Vector2.kEpsilon ) { if ( idir.sqrMagnitude <= dir.sqrMagnitude ) { return true; } } } return false; } static List SOLVES_HOLDER = new List( 2 ); public static Vector2[] Intersect_LineSegment_Circle( Circle circle, LineSegment line ) { Vector2[] ret = null; var dir = line.End - line.Start; var lenSq = dir.sqrMagnitude; var rr = circle.Radius * circle.Radius; // check if line segment is a point if ( lenSq > float.Epsilon ) { var normal = new Vector2( -dir.y, dir.x ); normal.Normalize(); var sc = line.Start - circle.Center; // distance from circle center to line var dis = VectorDot( ref sc, ref normal ); if ( dis < circle.Radius ) { var dr = Mathf.Sqrt( rr - dis * dis ); var s = circle.Center + dis * normal; // create normalize dir from its normalize normal vector dir.x = normal.y; dir.y = -normal.x; var p1 = s - dr * dir; // near point var p2 = s + dr * dir; // far point var ds1 = line.Start - p1; var de1 = line.End - p1; List xx = SOLVES_HOLDER; xx.Clear(); // check if solves inside line segment if ( VectorDot( ref ds1, ref de1 ) <= 0 ) { var diff1 = ( p1 - line.Start ).sqrMagnitude; if ( diff1 <= lenSq ) { xx.Add( p1 ); } } var ds2 = line.Start - p2; var de2 = line.End - p2; if ( VectorDot( ref ds2, ref de2 ) <= 0 ) { var diff2 = ( p2 - line.Start ).sqrMagnitude; if ( diff2 <= lenSq ) { xx.Add( p2 ); } } ret = xx.ToArray(); } else if ( dis == circle.Radius ) { var p1 = circle.Center + circle.Radius * normal; var ds1 = line.Start - p1; var de1 = line.End - p1; if ( Vector2.Dot( ds1, de1 ) <= 0 ) { ret = new Vector2[1]; ret[0] = p1; } } } else { // point to circle var disSq = ( line.Start - circle.Center ).sqrMagnitude; if ( disSq < rr ) { ret = new Vector2[1]; ret[0] = line.Start; } } return ret; } public static Vector2[] Intersect_Line_Circle( Circle circle, LineSegment _line ) { Vector2[] ret = null; LineSegment line = _line; var start = line.Start - circle.Center; var end = line.End - circle.Center; var dx = end.x - start.x; var rr = circle.Radius * circle.Radius; if ( Mathf.Abs( dx ) > float.Epsilon ) { var dy = end.y - start.y; var k = dy / dx; var b = start.y - k * start.x; var bb = b * b; var A = 1 + k * k; var B = 2 * k * b; var C = bb - rr; var q = B * B - 4 * A * C; if ( q < 0 ) { return null; } var _inv_2A = 0.5f * A; var _sqrtQ = q > 0 ? Mathf.Sqrt( q ) : 0; var sx1 = ( -b + _sqrtQ ) * _inv_2A; if ( q == 0 ) { // one solve ret = new Vector2[1]; ret[0].x = circle.Center.x + sx1; ret[0].y = circle.Center.y + k * sx1 + b; return ret; } // two solves var sx2 = ( -b - _sqrtQ ) * _inv_2A; var dis1 = sx1 - start.x; var dis2 = sx2 - start.x; if ( dis1 > dis2 ) { // sort solves var temp = sx1; sx1 = sx2; sx2 = temp; } ret = new Vector2[2]; ret[0].x = circle.Center.x + sx1; ret[0].y = circle.Center.y + k * sx1 + b; ret[1].x = circle.Center.x + sx2; ret[1].y = circle.Center.y + k * sx2 + b; return ret; } else { // xx + yy = rr var q = rr - start.x * start.x; if ( q < 0 ) { return null; } var sy1 = Mathf.Sqrt( q ); if ( q > 0 ) { var sy2 = -sy1; var dis1 = sy1 - start.y; var dis2 = sy2 - start.y; if ( dis1 > dis2 ) { var temp = sy1; sy1 = sy2; sy2 = temp; } ret = new Vector2[2]; ret[0].x = _line.Start.x; ret[0].y = circle.Center.y + sy1; ret[1].x = _line.Start.x; ret[1].y = circle.Center.y + sy2; return ret; } else if ( q == 0 ) { ret = new Vector2[1]; ret[0].x = _line.Start.x; ret[0].y = circle.Center.y + sy1; return ret; } } return ret; } public static void Closest_LineSegment_Circle( Circle circle, LineSegment line, out Vector2 point ) { Vector2 dir = line.End - line.Start; Vector2 dirs = circle.Center - line.Start; Vector2 dire = circle.Center - line.End; float projs = VectorDot( ref dirs, ref dir ); float proje = VectorDot( ref dire, ref dir ); if ( projs * proje < 0 ) { // projection on segment point = Vector2.zero; float sqLen = dir.sqrMagnitude; // normalize float rlen = 1.0f / Mathf.Sqrt( sqLen ); dir *= rlen; // real length of projection projs *= rlen; point = line.Start + dir * projs; } else { // out of range if ( projs <= 0 ) { point = line.Start; } else if ( proje >= 0 ) { point = line.End; } else { point = line.Start; } } } } }