367 lines
14 KiB
C#
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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|