Files
Main/Assets/Plugins/References/FuncellBase/Math/Math2d.cs
2025-01-25 04:38:09 +08:00

367 lines
14 KiB
C#

using System;
using System.Collections.Generic;
using System.Text;
using UnityEngine;
namespace Thousandto.Core.Base
{
/// <summary>
/// 2D的数学函数集合
/// </summary>
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<Vector2> SOLVES_HOLDER = new List<Vector2>( 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<Vector2> 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;
}
}
}
}
}