dsasadasdasd
This commit is contained in:
parent
4692b4e25e
commit
40a06fae4d
|
|
@ -19,7 +19,7 @@ public interface ICollider2D : IBehaviour, IAssignableTransform
|
|||
IReadOnlyList<Vector2> Vertices { get; }
|
||||
|
||||
|
||||
bool CheckCollision(Vector2 point, ICollider2D otherCollider, out CollisionInformation collisionInformation);
|
||||
bool CheckCollision(Vector2 point, ICollider2D otherCollider, out CollisionInformation? collisionInformation);
|
||||
|
||||
void RecalculateVertices();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,8 +2,11 @@ using System;
|
|||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Diagnostics.CodeAnalysis;
|
||||
|
||||
using Microsoft.Xna.Framework;
|
||||
|
||||
using Pong;
|
||||
|
||||
using Syntriax.Engine.Core;
|
||||
using Syntriax.Engine.Core.Abstract;
|
||||
using Syntriax.Engine.Graphics.TwoDimensional;
|
||||
|
|
@ -11,6 +14,7 @@ using Syntriax.Engine.Physics2D.Abstract;
|
|||
|
||||
namespace Syntriax.Engine.Physics2D;
|
||||
|
||||
|
||||
public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, ICollider2D
|
||||
{
|
||||
private List<Triangle> triangles = new List<Triangle>(32);
|
||||
|
|
@ -49,19 +53,19 @@ public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, I
|
|||
|
||||
foreach (var triangle in triangles)
|
||||
{
|
||||
if (!isInside(point, triangle))
|
||||
if (!PhysicsMath.IsInTriangle(point, triangle))
|
||||
continue;
|
||||
|
||||
OnCollision?.Invoke(this, otherCollider);
|
||||
|
||||
Edge main = new() { A = otherCollider.Transform.Position, B = point };
|
||||
Line main = new(otherCollider.Transform.Position, point);
|
||||
|
||||
foreach (var edge in GetEdges(triangle))
|
||||
foreach (var line in PhysicsMath.GetLines([triangle.A, triangle.B, triangle.C]))
|
||||
{
|
||||
if (!DoIntersect(main, edge))
|
||||
if (!PhysicsMath.DoIntersect(main, line))
|
||||
continue;
|
||||
|
||||
Vector2 contactPoint = ClosestPointOnEdge(point, edge);
|
||||
Vector2 contactPoint = PhysicsMath.ClosestPointOnLine(point, line);
|
||||
|
||||
Vector2 normal = contactPoint - point;
|
||||
if (normal.LengthSquared() < 0.001f)
|
||||
|
|
@ -89,11 +93,11 @@ public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, I
|
|||
_vertices.Add(scaledPosition + Transform.Position);
|
||||
}
|
||||
|
||||
Triangle superTriangle = GetSuperTriangle(_vertices);
|
||||
Triangle superTriangle = PhysicsMath.GetSuperTriangle(_vertices);
|
||||
triangles.Add(superTriangle);
|
||||
|
||||
List<Triangle> badTriangles = new(32);
|
||||
List<Edge> polygon = new(32);
|
||||
List<Line> polygon = new(32);
|
||||
|
||||
foreach (var vertex in _vertices)
|
||||
{
|
||||
|
|
@ -102,30 +106,21 @@ public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, I
|
|||
|
||||
foreach (var triangle in triangles)
|
||||
{
|
||||
Circle circle = GetCircumCircle(triangle);
|
||||
Circle circle = PhysicsMath.GetCircumCircle(triangle);
|
||||
if (Vector2.DistanceSquared(circle.Center, vertex) <= circle.Radius * circle.Radius)
|
||||
badTriangles.Add(triangle);
|
||||
}
|
||||
|
||||
foreach (var triangle in badTriangles)
|
||||
foreach (var edge in GetEdges(triangle))
|
||||
{
|
||||
if (DoesEdgeExistInTriangles(edge, badTriangles))
|
||||
polygon.Add(edge);
|
||||
}
|
||||
foreach (var line in PhysicsMath.GetLines([triangle.A, triangle.B, triangle.C]))
|
||||
if (PhysicsMath.DoesLineExistInVertices(line, [triangle.A, triangle.B, triangle.C]))
|
||||
polygon.Add(line);
|
||||
|
||||
foreach (var triangle in badTriangles)
|
||||
triangles.Remove(triangle);
|
||||
|
||||
foreach (var edge in polygon)
|
||||
{
|
||||
triangles.Add(new()
|
||||
{
|
||||
A = edge.A,
|
||||
B = edge.B,
|
||||
C = vertex
|
||||
});
|
||||
}
|
||||
foreach (var line in polygon)
|
||||
triangles.Add(new(line.From, line.To, vertex));
|
||||
}
|
||||
|
||||
for (int i = triangles.Count - 1; i >= 0; i--)
|
||||
|
|
@ -150,15 +145,15 @@ public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, I
|
|||
for (int i = 0; i < triangles.Count; i++)
|
||||
{
|
||||
Triangle triangle = triangles[i];
|
||||
foreach (var edge in GetEdges(triangle))
|
||||
foreach (var line in PhysicsMath.GetLines([triangle.A, triangle.B, triangle.C]))
|
||||
{
|
||||
GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
|
||||
DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
|
||||
displayableSpriteBehaviour.Color = Color.Aqua;
|
||||
displayableSpriteBehaviour.Origin = new(0.5f, 1f);
|
||||
displayableSpriteBehaviour.Assign(Game1.spriteBox);
|
||||
gameObject.Transform.Position = edge.A;
|
||||
Vector2 vector2 = edge.B - edge.A;
|
||||
gameObject.Transform.Position = line.From;
|
||||
Vector2 vector2 = line.To - line.From;
|
||||
gameObject.Transform.Scale = new Vector2(2f, .0f) + Vector2.UnitY * vector2.Length();
|
||||
gameObject.Transform.Rotation = (float)Math.Atan2(vector2.X, vector2.Y);
|
||||
gameObjects.Add(gameObject);
|
||||
|
|
@ -167,7 +162,7 @@ public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, I
|
|||
displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
|
||||
displayableSpriteBehaviour.Color = Color.Crimson;
|
||||
displayableSpriteBehaviour.Assign(Game1.spriteBox);
|
||||
gameObject.Transform.Position = edge.B;
|
||||
gameObject.Transform.Position = line.To;
|
||||
gameObject.Transform.Scale = new Vector2(4f, 4f);
|
||||
gameObjects.Add(gameObject);
|
||||
}
|
||||
|
|
@ -187,200 +182,4 @@ public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, I
|
|||
|
||||
private List<IGameObject> gameObjects = new List<IGameObject>(32);
|
||||
|
||||
private Vector2 ClosestPointOnEdge(Vector2 point, Edge edge)
|
||||
{
|
||||
// Convert edge points to vectors
|
||||
var edgeVector = new Vector2(edge.B.X - edge.A.X, edge.B.Y - edge.A.Y);
|
||||
var pointVector = new Vector2(point.X - edge.A.X, point.Y - edge.A.Y);
|
||||
|
||||
// Calculate the projection of pointVector onto edgeVector
|
||||
double t = (pointVector.X * edgeVector.X + pointVector.Y * edgeVector.Y) / (edgeVector.X * edgeVector.X + edgeVector.Y * edgeVector.Y);
|
||||
|
||||
// Clamp t to the range [0, 1] to ensure the closest point is on the edge
|
||||
t = Math.Max(0, Math.Min(1, t));
|
||||
|
||||
// Calculate the closest point on the edge
|
||||
double closestX = edge.A.X + t * edgeVector.X;
|
||||
double closestY = edge.A.Y + t * edgeVector.Y;
|
||||
|
||||
return new Vector2((float)closestX, (float)closestY);
|
||||
}
|
||||
|
||||
private bool DoesEdgeExistInTriangles(Edge edge, List<Triangle> triangles)
|
||||
{
|
||||
foreach (var triangle in triangles)
|
||||
foreach (var edgeOther in GetEdges(triangle))
|
||||
if (edge.A == edgeOther.A && edge.B == edgeOther.B)
|
||||
return true;
|
||||
else if (edge.A == edgeOther.B && edge.B == edgeOther.A)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
private List<Edge> GetEdges(Triangle triangle)
|
||||
=> [
|
||||
new() { A = triangle.A, B = triangle.B },
|
||||
new() { A = triangle.B, B = triangle.C },
|
||||
new() { A = triangle.C, B = triangle.A }
|
||||
];
|
||||
|
||||
private Triangle GetSuperTriangle(IReadOnlyList<Vector2> vertices)
|
||||
{
|
||||
double minX = double.MaxValue, minY = double.MaxValue;
|
||||
double maxX = double.MinValue, maxY = double.MinValue;
|
||||
|
||||
foreach (Vector2 point in vertices)
|
||||
{
|
||||
minX = Math.Min(minX, point.X);
|
||||
minY = Math.Min(minY, point.Y);
|
||||
maxX = Math.Max(maxX, point.X);
|
||||
maxY = Math.Max(maxY, point.Y);
|
||||
}
|
||||
|
||||
double dx = maxX - minX;
|
||||
double dy = maxY - minY;
|
||||
double deltaMax = Math.Max(dx, dy);
|
||||
double midX = (minX + maxX) / 2;
|
||||
double midY = (minY + maxY) / 2;
|
||||
|
||||
Vector2 p1 = new Vector2((float)midX - 20f * (float)deltaMax, (float)midY - (float)deltaMax);
|
||||
Vector2 p2 = new Vector2((float)midX, (float)midY + 20 * (float)deltaMax);
|
||||
Vector2 p3 = new Vector2((float)midX + 20 * (float)deltaMax, (float)midY - (float)deltaMax);
|
||||
|
||||
return new Triangle() { A = p1, B = p2, C = p3 };
|
||||
}
|
||||
|
||||
private struct Triangle { public Vector2 A, B, C; }
|
||||
|
||||
private double GetArea(Triangle triangle)
|
||||
{
|
||||
return Math.Abs((triangle.A.X * (triangle.B.Y - triangle.C.Y) +
|
||||
triangle.B.X * (triangle.C.Y - triangle.A.Y) +
|
||||
triangle.C.X * (triangle.A.Y - triangle.B.Y)) / 2.0);
|
||||
}
|
||||
|
||||
/* A function to check whether point P(x, y) lies
|
||||
inside the triangle formed by A(x1, y1),
|
||||
B(x2, y2) and C(x3, y3) */
|
||||
private bool isInside(Vector2 point, Triangle triangle)
|
||||
{
|
||||
double A = GetArea(triangle);
|
||||
/* Calculate area of triangle ABC */
|
||||
// double A = area(x1, y1, x2, y2, x3, y3);
|
||||
|
||||
double A1 = GetArea(new() { A = point, B = triangle.B, C = triangle.C });
|
||||
/* Calculate area of triangle PBC */
|
||||
// double A1 = area(x, y, x2, y2, x3, y3);
|
||||
|
||||
/* Calculate area of triangle PAC */
|
||||
double A2 = GetArea(new() { A = triangle.A, B = point, C = triangle.C });
|
||||
// double A2 = area(x1, y1, x, y, x3, y3);
|
||||
|
||||
/* Calculate area of triangle PAB */
|
||||
double A3 = GetArea(new() { A = triangle.A, B = triangle.B, C = point });
|
||||
// double A3 = area(x1, y1, x2, y2, x, y);
|
||||
|
||||
/* Check if sum of A1, A2 and A3 is same as A */
|
||||
return A >= A1 + A2 + A3;
|
||||
}
|
||||
|
||||
private struct Edge
|
||||
{
|
||||
public Vector2 A;
|
||||
public Vector2 B;
|
||||
}
|
||||
|
||||
private struct Circle
|
||||
{
|
||||
public double Radius;
|
||||
public Vector2 Center;
|
||||
}
|
||||
|
||||
private Circle GetCircumCircle(Triangle triangle)
|
||||
{
|
||||
Circle result = new();
|
||||
|
||||
Vector2 midAB = (triangle.A + triangle.B) / 2;
|
||||
Vector2 midBC = (triangle.B + triangle.C) / 2;
|
||||
|
||||
double slopeAB = (triangle.B.Y - triangle.A.Y) / (triangle.B.X - triangle.A.X);
|
||||
double slopeBC = (triangle.C.Y - triangle.B.Y) / (triangle.C.X - triangle.B.X);
|
||||
|
||||
// Check if the slopes are not parallel
|
||||
if (Math.Abs(slopeAB - slopeBC) > double.Epsilon)
|
||||
{
|
||||
double x = (slopeAB * slopeBC * (triangle.A.Y - triangle.C.Y) + slopeBC * (triangle.A.X + triangle.B.X) - slopeAB * (triangle.B.X + triangle.C.X)) / (2 * (slopeBC - slopeAB));
|
||||
double y = -(x - (triangle.A.X + triangle.B.X) / 2) / slopeAB + (triangle.A.Y + triangle.B.Y) / 2;
|
||||
|
||||
result.Center = new Vector2((float)x, (float)y);
|
||||
result.Radius = Vector2.Distance(result.Center, triangle.A);
|
||||
}
|
||||
else
|
||||
{
|
||||
// If slopes are parallel, use the midpoints of the sides as the circumcenter
|
||||
result.Center = (midAB + midBC) / 2;
|
||||
result.Radius = Vector2.Distance(result.Center, triangle.A);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Given three collinear points p, q, r, the function checks if
|
||||
// point q lies on line segment 'pr'
|
||||
private bool OnSegment(Vector2 p, Vector2 q, Vector2 r)
|
||||
{
|
||||
if (q.X <= Math.Max(p.X, r.X) && q.X >= Math.Min(p.X, r.X) &&
|
||||
q.Y <= Math.Max(p.Y, r.Y) && q.Y >= Math.Min(p.Y, r.Y))
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// To find orientation of ordered triplet (p, q, r).
|
||||
// The function returns following values
|
||||
// 0 --> p, q and r are collinear
|
||||
// 1 --> Clockwise
|
||||
// 2 --> Counterclockwise
|
||||
private int Orientation(Vector2 p, Vector2 q, Vector2 r)
|
||||
{
|
||||
// See https://www.geeksforgeeks.org/orientation-3-ordered-points/
|
||||
// for details of below formula.
|
||||
double val = (q.Y - p.Y) * (r.X - q.X) -
|
||||
(q.X - p.X) * (r.Y - q.Y);
|
||||
|
||||
if (val == 0) return 0; // collinear
|
||||
|
||||
return (val > 0) ? 1 : 2; // clock or counterclock wise
|
||||
}
|
||||
|
||||
// The main function that returns true if line segment 'edge1.Aedge1.B'
|
||||
// and 'edge2.Aedge2.B' intersect.
|
||||
private bool DoIntersect(Edge edge1, Edge edge2)
|
||||
{
|
||||
// Find the four orientations needed for general and
|
||||
// special cases
|
||||
int o1 = Orientation(edge1.A, edge1.B, edge2.A);
|
||||
int o2 = Orientation(edge1.A, edge1.B, edge2.B);
|
||||
int o3 = Orientation(edge2.A, edge2.B, edge1.A);
|
||||
int o4 = Orientation(edge2.A, edge2.B, edge1.B);
|
||||
|
||||
// General case
|
||||
if (o1 != o2 && o3 != o4)
|
||||
return true;
|
||||
|
||||
// Special Cases
|
||||
// edge1.A, edge1.B and edge2.A are collinear and edge2.A lies on segment edge1.Aedge1.B
|
||||
if (o1 == 0 && OnSegment(edge1.A, edge2.A, edge1.B)) return true;
|
||||
|
||||
// edge1.A, edge1.B and edge2.B are collinear and edge2.B lies on segment edge1.Aedge1.B
|
||||
if (o2 == 0 && OnSegment(edge1.A, edge2.B, edge1.B)) return true;
|
||||
|
||||
// edge2.A, edge2.B and edge1.A are collinear and edge1.A lies on segment edge2.Aedge2.B
|
||||
if (o3 == 0 && OnSegment(edge2.A, edge1.A, edge2.B)) return true;
|
||||
|
||||
// edge2.A, edge2.B and edge1.B are collinear and edge1.B lies on segment edge2.Aedge2.B
|
||||
if (o4 == 0 && OnSegment(edge2.A, edge1.B, edge2.B)) return true;
|
||||
|
||||
return false; // Doesn't fall in any of the above cases
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -51,71 +51,6 @@ public class PhysicsEngine2D : IPhysicsEngine2D
|
|||
foreach (var collider in colliders)
|
||||
collider.RecalculateVertices();
|
||||
|
||||
for (int colliderIX = 0; colliderIX < colliders.Count; colliderIX++)
|
||||
{
|
||||
ICollider2D colliderX = colliders[colliderIX];
|
||||
for (int colliderIY = colliderIX + 1; colliderIY < colliders.Count; colliderIY++)
|
||||
for (int verticesIndex = 0; verticesIndex < colliderX.Vertices.Count; verticesIndex++)
|
||||
{
|
||||
ICollider2D colliderY = colliders[colliderIY];
|
||||
bool v = colliderY.CheckCollision(colliderX.Vertices[verticesIndex], colliderX, out var _);
|
||||
if (!colliderY.CheckCollision(colliderX.Vertices[verticesIndex], colliderX, out var collisionInformation))
|
||||
continue;
|
||||
|
||||
if (colliderX.BehaviourController.TryGetBehaviour(out IRigidBody2D? rigidX))
|
||||
{
|
||||
Vector2 xVertex = colliderX.Vertices[verticesIndex];
|
||||
Vector2 edgeDirection = new Vector2(collisionInformation.Normal.Y, collisionInformation.Normal.X);
|
||||
Vector2 p1 = xVertex - rigidX.Velocity * intervalDeltaTime;
|
||||
float t = IntersectionParameterT(collisionInformation.ContactPosition - edgeDirection * 100f, collisionInformation.ContactPosition + edgeDirection * 100f, xVertex, p1);
|
||||
Vector2 vertexNewPosition = Vector2.Lerp(xVertex, p1, t);
|
||||
rigidX.Velocity = Vector2.Reflect(rigidX.Velocity, collisionInformation.Normal);
|
||||
rigidX.Transform.Position -= xVertex - vertexNewPosition;
|
||||
|
||||
// {
|
||||
// GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
|
||||
// DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
|
||||
// displayableSpriteBehaviour.Assign(Game1.spriteBox);
|
||||
// gameObject.Transform.Position = collisionInformation.ContactPosition;
|
||||
// gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
|
||||
// gameObject.Transform.Rotation = (float)Math.Atan2(collisionInformation.Normal.Y, collisionInformation.Normal.X);
|
||||
// }
|
||||
// {
|
||||
// GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
|
||||
// DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
|
||||
// displayableSpriteBehaviour.Color = Color.Aqua;
|
||||
// displayableSpriteBehaviour.Assign(Game1.spriteBox);
|
||||
// gameObject.Transform.Position = collisionInformation.ContactPosition;
|
||||
// gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
|
||||
// gameObject.Transform.Rotation = (float)Math.Atan2(rigidX.Velocity.Y, rigidX.Velocity.X);
|
||||
// }
|
||||
// {
|
||||
// GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
|
||||
// DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
|
||||
// displayableSpriteBehaviour.Color = Color.Pink;
|
||||
// displayableSpriteBehaviour.Assign(Game1.spriteBox);
|
||||
// gameObject.Transform.Position = collisionInformation.ContactPosition;
|
||||
// gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
|
||||
// gameObject.Transform.Rotation = (float)Math.Atan2(edgeDirection.Y, edgeDirection.X);
|
||||
// }
|
||||
// {
|
||||
// GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
|
||||
// DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>();
|
||||
// displayableSpriteBehaviour.Color = Color.DarkGoldenrod;
|
||||
// displayableSpriteBehaviour.Assign(Game1.spriteBox);
|
||||
// gameObject.Transform.Position = collisionInformation.ContactPosition;
|
||||
// gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
|
||||
// gameObject.Transform.Rotation = (float)Math.Atan2((xVertex - p1).Y, (xVertex - p1).X);
|
||||
// }
|
||||
|
||||
StepRigidBody(rigidX, intervalDeltaTime * (1f - t));
|
||||
colliders[colliderIX].RecalculateVertices();
|
||||
verticesIndex--;
|
||||
}
|
||||
Console.WriteLine($"/////////////////////////////////////////////");
|
||||
// Console.WriteLine($"Collision");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -127,15 +62,6 @@ public class PhysicsEngine2D : IPhysicsEngine2D
|
|||
rigidBody.Transform.Position = nextPosition;
|
||||
}
|
||||
|
||||
private float IntersectionParameterT(Vector2 p0, Vector2 p1, Vector2 q0, Vector2 q1)
|
||||
{
|
||||
// Solve the linear interpolation equation for 't'
|
||||
float t = ((q0.X - p0.X) * (p1.Y - p0.Y) - (q0.Y - p0.Y) * (p1.X - p0.X)) /
|
||||
((q1.Y - q0.Y) * (p1.X - p0.X) - (q1.X - q0.X) * (p1.Y - p0.Y));
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
private void OnBehaviourAdded(IBehaviourController controller, IBehaviour behaviour)
|
||||
{
|
||||
if (behaviour is not ICollider2D collider2D)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,210 @@
|
|||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics.CodeAnalysis;
|
||||
|
||||
using Microsoft.Xna.Framework;
|
||||
|
||||
namespace Syntriax.Engine.Physics2D;
|
||||
|
||||
public record Line(Vector2 From, Vector2 To);
|
||||
public record Triangle(Vector2 A, Vector2 B, Vector2 C);
|
||||
public record Circle(Vector2 Center, double Radius);
|
||||
|
||||
public static class PhysicsMath
|
||||
{
|
||||
public static float IntersectionParameterT(Vector2 p0, Vector2 p1, Vector2 q0, Vector2 q1)
|
||||
=> ((q0.X - p0.X) * (p1.Y - p0.Y) - (q0.Y - p0.Y) * (p1.X - p0.X)) /
|
||||
((q1.Y - q0.Y) * (p1.X - p0.X) - (q1.X - q0.X) * (p1.Y - p0.Y));
|
||||
|
||||
public static float IntersectionParameterT(Line l1, Line l2)
|
||||
=> ((l2.From.X - l1.From.X) * (l1.To.Y - l1.From.Y) - (l2.From.Y - l1.From.Y) * (l1.To.X - l1.From.X)) /
|
||||
((l2.To.Y - l2.From.Y) * (l1.To.X - l1.From.X) - (l2.To.X - l2.From.X) * (l1.To.Y - l1.From.Y));
|
||||
|
||||
public static Vector2 GetIntersectionPoint(Line l1, Line l2)
|
||||
=> Vector2.Lerp(l1.From, l1.To, IntersectionParameterT(l1, l2));
|
||||
|
||||
public static Vector2 ClosestPointOnLine(Vector2 point, Line line)
|
||||
{
|
||||
// Convert edge points to vectors
|
||||
var edgeVector = new Vector2(line.To.X - line.From.X, line.To.Y - line.From.Y);
|
||||
var pointVector = new Vector2(point.X - line.From.X, point.Y - line.From.Y);
|
||||
|
||||
// Calculate the projection of pointVector onto edgeVector
|
||||
double t = (pointVector.X * edgeVector.X + pointVector.Y * edgeVector.Y) / (edgeVector.X * edgeVector.X + edgeVector.Y * edgeVector.Y);
|
||||
|
||||
// Clamp t to the range [0, 1] to ensure the closest point is on the edge
|
||||
t = Math.Max(0, Math.Min(1, t));
|
||||
|
||||
// Calculate the closest point on the edge
|
||||
double closestX = line.From.X + t * edgeVector.X;
|
||||
double closestY = line.From.Y + t * edgeVector.Y;
|
||||
|
||||
return new Vector2((float)closestX, (float)closestY);
|
||||
}
|
||||
|
||||
public static double GetArea(Triangle triangle)
|
||||
{
|
||||
return Math.Abs((triangle.A.X * (triangle.B.Y - triangle.C.Y) +
|
||||
triangle.B.X * (triangle.C.Y - triangle.A.Y) +
|
||||
triangle.C.X * (triangle.A.Y - triangle.B.Y)) * .5f);
|
||||
}
|
||||
|
||||
public static bool IsInTriangle(Vector2 point, Triangle triangle)
|
||||
{
|
||||
double A = GetArea(triangle);
|
||||
/* Calculate area of triangle ABC */
|
||||
// double A = area(x1, y1, x2, y2, x3, y3);
|
||||
|
||||
double A1 = GetArea(new Triangle(point, triangle.B, triangle.C));
|
||||
/* Calculate area of triangle PBC */
|
||||
// double A1 = area(x, y, x2, y2, x3, y3);
|
||||
|
||||
/* Calculate area of triangle PAC */
|
||||
double A2 = GetArea(new Triangle(triangle.A, point, triangle.C));
|
||||
// double A2 = area(x1, y1, x, y, x3, y3);
|
||||
|
||||
/* Calculate area of triangle PAB */
|
||||
double A3 = GetArea(new Triangle(triangle.A, triangle.B, point));
|
||||
// double A3 = area(x1, y1, x2, y2, x, y);
|
||||
|
||||
/* Check if sum of A1, A2 and A3 is same as A */
|
||||
return A >= A1 + A2 + A3;
|
||||
}
|
||||
|
||||
// Given three collinear points p, q, r, the function checks if
|
||||
// point q lies on line segment 'pr'
|
||||
public static bool OnSegment(Vector2 p, Vector2 q, Vector2 r)
|
||||
{
|
||||
if (q.X <= Math.Max(p.X, r.X) && q.X >= Math.Min(p.X, r.X) &&
|
||||
q.Y <= Math.Max(p.Y, r.Y) && q.Y >= Math.Min(p.Y, r.Y))
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// To find orientation of ordered triplet (p, q, r).
|
||||
// The function returns following values
|
||||
// 0 --> p, q and r are collinear
|
||||
// 1 --> Clockwise
|
||||
// 2 --> Counterclockwise
|
||||
public static int Orientation(Vector2 p, Vector2 q, Vector2 r)
|
||||
{
|
||||
// See https://www.geeksforgeeks.org/orientation-3-ordered-points/
|
||||
// for details of below formula.
|
||||
double val = (q.Y - p.Y) * (r.X - q.X) -
|
||||
(q.X - p.X) * (r.Y - q.Y);
|
||||
|
||||
if (val == 0) return 0; // collinear
|
||||
|
||||
return (val > 0) ? 1 : 2; // clock or counterclock wise
|
||||
}
|
||||
|
||||
public static bool DoIntersect(Line l1, Line l2)
|
||||
{
|
||||
int o1 = Orientation(l1.From, l1.To, l2.From);
|
||||
int o2 = Orientation(l1.From, l1.To, l2.To);
|
||||
int o3 = Orientation(l2.From, l2.To, l1.From);
|
||||
int o4 = Orientation(l2.From, l2.To, l1.To);
|
||||
|
||||
if (o1 != o2 && o3 != o4)
|
||||
return true;
|
||||
|
||||
if (o1 == 0 && OnSegment(l1.From, l2.From, l1.To)) return true;
|
||||
if (o2 == 0 && OnSegment(l1.From, l2.To, l1.To)) return true;
|
||||
if (o3 == 0 && OnSegment(l2.From, l1.From, l2.To)) return true;
|
||||
if (o4 == 0 && OnSegment(l2.From, l1.To, l2.To)) return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public static bool DoIntersect(Line l1, Line l2, [NotNullWhen(returnValue: true)] out Vector2? point)
|
||||
{
|
||||
point = null;
|
||||
|
||||
bool result = DoIntersect(l1, l2);
|
||||
|
||||
if (result)
|
||||
point = GetIntersectionPoint(l1, l2);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public static Circle GetCircumCircle(Triangle triangle)
|
||||
{
|
||||
Vector2 midAB = (triangle.A + triangle.B) / 2;
|
||||
Vector2 midBC = (triangle.B + triangle.C) / 2;
|
||||
|
||||
double slopeAB = (triangle.B.Y - triangle.A.Y) / (triangle.B.X - triangle.A.X);
|
||||
double slopeBC = (triangle.C.Y - triangle.B.Y) / (triangle.C.X - triangle.B.X);
|
||||
|
||||
Vector2 center;
|
||||
if (Math.Abs(slopeAB - slopeBC) > double.Epsilon)
|
||||
{
|
||||
double x = (slopeAB * slopeBC * (triangle.A.Y - triangle.C.Y) + slopeBC * (triangle.A.X + triangle.B.X) - slopeAB * (triangle.B.X + triangle.C.X)) / (2 * (slopeBC - slopeAB));
|
||||
double y = -(x - (triangle.A.X + triangle.B.X) / 2) / slopeAB + (triangle.A.Y + triangle.B.Y) / 2;
|
||||
center = new Vector2((float)x, (float)y);
|
||||
}
|
||||
else
|
||||
center = (midAB + midBC) * .5f;
|
||||
|
||||
return new(center, Vector2.Distance(center, triangle.A));
|
||||
}
|
||||
|
||||
public static Triangle GetSuperTriangle(IList<Vector2> vertices)
|
||||
{
|
||||
double minX = double.MaxValue, minY = double.MaxValue;
|
||||
double maxX = double.MinValue, maxY = double.MinValue;
|
||||
|
||||
foreach (Vector2 point in vertices)
|
||||
{
|
||||
minX = Math.Min(minX, point.X);
|
||||
minY = Math.Min(minY, point.Y);
|
||||
maxX = Math.Max(maxX, point.X);
|
||||
maxY = Math.Max(maxY, point.Y);
|
||||
}
|
||||
|
||||
double dx = maxX - minX;
|
||||
double dy = maxY - minY;
|
||||
double deltaMax = Math.Max(dx, dy);
|
||||
double midX = (minX + maxX) / 2;
|
||||
double midY = (minY + maxY) / 2;
|
||||
|
||||
Vector2 p1 = new Vector2((float)midX - 20f * (float)deltaMax, (float)midY - (float)deltaMax);
|
||||
Vector2 p2 = new Vector2((float)midX, (float)midY + 20 * (float)deltaMax);
|
||||
Vector2 p3 = new Vector2((float)midX + 20 * (float)deltaMax, (float)midY - (float)deltaMax);
|
||||
|
||||
return new Triangle(p1, p2, p3);
|
||||
}
|
||||
|
||||
public static List<Line> GetLines(IList<Vector2> vertices)
|
||||
{
|
||||
List<Line> lines = new List<Line>(vertices.Count - 1);
|
||||
GetLines(vertices, lines);
|
||||
return lines;
|
||||
}
|
||||
|
||||
public static void GetLines(IList<Vector2> vertices, IList<Line> lines)
|
||||
{
|
||||
lines.Clear();
|
||||
for (int i = 0; i < vertices.Count - 1; i++)
|
||||
lines.Add(new(vertices[i], vertices[i + 1]));
|
||||
lines.Add(new(vertices[^1], vertices[0]));
|
||||
}
|
||||
|
||||
public static bool DoesLineExistInVertices(Line lineToCheck, List<Vector2> vertices)
|
||||
{
|
||||
for (int i = 0; i < vertices.Count - 1; i++)
|
||||
{
|
||||
Vector2 vertexCurrent = vertices[i];
|
||||
Vector2 vertexNext = vertices[i];
|
||||
if (lineToCheck.From == vertexCurrent && lineToCheck.To == vertexNext) return true;
|
||||
if (lineToCheck.From == vertexNext && lineToCheck.To == vertexCurrent) return true;
|
||||
}
|
||||
|
||||
Vector2 vertexFirst = vertices[0];
|
||||
Vector2 vertexLast = vertices[^1];
|
||||
if (lineToCheck.From == vertexFirst && lineToCheck.To == vertexLast) return true;
|
||||
if (lineToCheck.From == vertexLast && lineToCheck.To == vertexFirst) return true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue