// Test whether two line segments intersect. If so, calculate the intersection point.
// p Vector to the start point of p.</param>
// p2 Vector to the end point of p.</param>
// q Vector to the start point of q.</param>
// q2 Vector to the end point of q.</param>
// intersection The point of intersection, if any.
// considerOverlapAsIntersect Do we consider overlapping lines as intersecting?
// Returns True if an intersection point was found.
public static bool IntersectSegementSegment(Vector2F p, Vector2F p2, Vector2F q, Vector2F q2, out Vector2F intersection, bool considerCollinearOverlapAsIntersect = false)
{
intersection = new Vector2F();
Vector2F r = p2 - p;
Vector2F s = q2 - q;
float rxs = r.Cross(s);
float qpxr = (q - p).Cross(r);
// If r x s = 0 and (q - p) x r = 0, then the two lines are collinear.
if (IsZeroFloat(rxs) && IsZeroFloat(qpxr))
{
// 1. If either 0 <= (q - p) * r <= r * r or 0 <= (p - q) * s <= * s
// then the two lines are overlapping,
if (considerCollinearOverlapAsIntersect)
{
if ((0 <= (q - p) * r && (q - p) * r <= r * r) || (0 <= (p - q) * s && (p - q) * s <= s * s))
{
return(true);
}
}
// 2. If neither 0 <= (q - p) * r = r * r nor 0 <= (p - q) * s <= s * s
// then the two lines are collinear but disjoint.
// No need to implement this expression, as it follows from the expression above.
return(false);
}
// 3. If r x s = 0 and (q - p) x r != 0, then the two lines are parallel and non-intersecting.
if (IsZeroFloat(rxs) && !IsZeroFloat(qpxr))
{
return(false);
}
// t = (q - p) x s / (r x s)
var t = (q - p).Cross(s) / rxs;
// u = (q - p) x r / (r x s)
var u = (q - p).Cross(r) / rxs;
// 4. If r x s != 0 and 0 <= t <= 1 and 0 <= u <= 1
// the two line segments meet at the point p + t r = q + u s.
if (!IsZeroFloat(rxs) && (0 <= t && t <= 1) && (0 <= u && u <= 1))
{
// We can calculate the intersection point using either t or u.
intersection = p + t * r;
// An intersection was found.
return(true);
}
// 5. Otherwise, the two line segments are not parallel but do not intersect.
return(false);
}
bool IsContainAtTriangle(Vector2F vertex1, Vector2F vertex2, Vector2F vertex3, Vector2F vector)
{
float c1 = Vector2F.Cross(vertex2 - vertex1, vector - vertex2);
float c2 = Vector2F.Cross(vertex3 - vertex2, vector - vertex3);
float c3 = Vector2F.Cross(vertex1 - vertex3, vector - vertex1);
if (c1 > 0 && c2 > 0 && c3 > 0 || c1 < 0 && c2 < 0 && c3 < 0)
{
return(true);
}
return(false);
}
List <PolygonDef> DivideToTriangles(List <Vector2F> argVertexes)
{
if (argVertexes.Count < 3)
{
return(null);
}
List <PolygonDef> result = new List <PolygonDef>();
if (argVertexes.Count == 3)
{
result.Add(CreatePolygonShape(argVertexes[0], argVertexes[1], argVertexes[2]));
return(result);
}
Vector2F root = new Vector2F();
foreach (var item in argVertexes)
{
if (root.Length < item.Length)
{
root = item;
}
}
Vector2F next1, next2;
next1 = argVertexes.IndexOf(root) != argVertexes.Count - 1 ? argVertexes[argVertexes.IndexOf(root) + 1] : argVertexes[0];
next2 = argVertexes.IndexOf(root) != 0 ? argVertexes[argVertexes.IndexOf(root) - 1] : argVertexes[argVertexes.Count - 1];
float cross = Vector2F.Cross(next1 - root, next2 - root);
while (true)
{
bool isDivideble = true;
foreach (var item in argVertexes)
{
if (IsContainAtTriangle(root, next1, next2, item))
{
isDivideble = false;
}
}
if (!isDivideble)
{
do
{
root = argVertexes.IndexOf(root) != argVertexes.Count - 1 ? argVertexes[argVertexes.IndexOf(root) + 1] : argVertexes[0];
next1 = argVertexes.IndexOf(next1) != argVertexes.Count - 1 ? argVertexes[argVertexes.IndexOf(next1) + 1] : argVertexes[0];
next2 = argVertexes.IndexOf(next2) != argVertexes.Count - 1 ? argVertexes[argVertexes.IndexOf(next2) + 1] : argVertexes[0];
} while (Math.Sign(cross) != Math.Sign(Vector2F.Cross(next1 - root, next2 - root)));
}
else
{
break;
}
}
result.Add(CreatePolygonShape(root, next1, next2));
List <Vector2F> remain = new List <Vector2F>(argVertexes);
remain.Remove(root);
result.AddRange(DivideToTriangles(remain));
return(result);
}
static void Main(string[] args)
{
Engine.Initialize("Phys2D", 960, 720);
float gravity = 9.8f;
Vector2F speed = new Vector2F(0f, 0f);
Color intersect = new Color(200, 0, 0);
Color normalColor = new Color(100, 130, 180);
List <CircleNode> collision = new List <CircleNode>();
//プレイヤー
var player = new CircleNode();
player.Radius = 70;
player.Position = new Vector2F(300, 100);
player.Color = normalColor;
player.VertNum = 100;
//circle.ZOrder = 1;
Engine.AddNode(player);
//円の相手
var other = new CircleNode();
other.Radius = 50;
other.Position = new Vector2F(350, 300);
other.Color = normalColor;
other.VertNum = 100;
Engine.AddNode(other);
var other2 = new CircleNode();
other2.Radius = 50;
other2.Position = new Vector2F(300, 50);
other2.Color = normalColor;
other2.VertNum = 100;
Engine.AddNode(other2);
//三角形の相手
var triangle = new TriangleNode();
triangle.Point1 = new Vector2F(400, 100);
triangle.Point2 = new Vector2F(450, 200);
triangle.Point3 = new Vector2F(500, 100);
//triangle.Position = new Vector2F((triangle.Point1.X + triangle.Point2.X + triangle.Point3.X) / 3, (triangle.Point1.Y + triangle.Point2.Y + triangle.Point3.Y) / 3);
//triangle.Position = new Vector2F((400 + 450 + 500) / 3, (100 + 200 + 100) / 3);
triangle.Color = normalColor;
//Engine.AddNode(triangle);
var line = new RectangleNode();
line.RectangleSize = new Vector2F(960, 2);
line.Position = new Vector2F(0, 400);
line.Color = new Color(255, 30, 30);
Engine.AddNode(line);
var lineL = new RectangleNode();
lineL.RectangleSize = new Vector2F(2, 720 - 320);
lineL.Position = new Vector2F(200, 0);
lineL.Color = new Color(255, 30, 30);
Engine.AddNode(lineL);
var lineR = new RectangleNode();
lineR.RectangleSize = new Vector2F(2, 720 - 320);
lineR.Position = new Vector2F(500, 0);
lineR.Color = new Color(255, 30, 30);
Engine.AddNode(lineR);
collision.Add(player);
collision.Add(other);
collision.Add(other2);
while (Engine.DoEvents())
{
Engine.Update();
//speed += new Vector2F(0f, gravity/100);
//player.Position += new Vector2F(0f, 1f);
for (int i = 0; i < collision.Count; i++)
{
for (int j = 0; j < collision.Count; j++)
{
if (collision[i] == collision[j])
{
continue;
}
float length = (collision[i].Position - collision[j].Position).Length;
float distance = collision[i].Radius + collision[j].Radius;
if (length <= distance)
{
//collision[i].Color = intersect;
//collision[j].Color = intersect;
//簡単な衝突応答
var dir = (collision[j].Position - collision[i].Position);
//中心間を結んだベクトルの方向に、めり込んだ分だけ移動
collision[i].Position -= dir.Normal * (distance - dir.Length);
collision[j].Position += dir.Normal * (distance - dir.Length);
}
else
{
collision[i].Color = normalColor;
collision[j].Color = normalColor;
}
}
CircleNode obj = collision[i];
obj.Position += new Vector2F(0f, 2f);
if (obj.Position.Y + obj.Radius > 400f)
{
obj.Position = new Vector2F(obj.Position.X, 400f - obj.Radius);
}
if (obj.Position.X - obj.Radius < 200f)
{
obj.Position = new Vector2F(200f + obj.Radius, obj.Position.Y);
}
if (obj.Position.X + obj.Radius > 500f)
{
obj.Position = new Vector2F(500f - obj.Radius, obj.Position.Y);
}
}
//円と三角形の衝突判定
if ((triangle.Point1 - player.Position).Length < player.Radius ||
(triangle.Point2 - player.Position).Length < player.Radius ||
(triangle.Point3 - player.Position).Length < player.Radius)
{
player.Color = intersect;
}
var AO = player.Position - triangle.Point1;
var BO = player.Position - triangle.Point3;
var CO = player.Position - triangle.Point2;
var AB = triangle.Point3 - triangle.Point1;
var BC = triangle.Point2 - triangle.Point3;
var CA = triangle.Point1 - triangle.Point2;
var center = new Vector2F((triangle.Point1.X + triangle.Point2.X + triangle.Point3.X) / 3, (triangle.Point1.Y + triangle.Point2.Y + triangle.Point3.Y) / 3);
var direction = center - player.Position;
var dist1 = Math.Abs(Vector2F.Cross(AO, AB)) / AB.Length;
var dist2 = Math.Abs(Vector2F.Cross(BO, BC)) / BC.Length;
var dist3 = Math.Abs(Vector2F.Cross(CO, CA)) / CA.Length;
if (dist1 <= player.Radius && Dot(AO, AB) * Dot(BO, AB) <= 0)
{
player.Color = intersect;
triangle.Point1 += direction.Normal * (player.Radius - dist1);
triangle.Point2 += direction.Normal * (player.Radius - dist1);
triangle.Point3 += direction.Normal * (player.Radius - dist1);
}
if (dist2 <= player.Radius && Dot(BO, BC) * Dot(CO, BC) <= 0)
{
player.Color = intersect;
triangle.Point1 += direction.Normal * (player.Radius - dist2);
triangle.Point2 += direction.Normal * (player.Radius - dist2);
triangle.Point3 += direction.Normal * (player.Radius - dist2);
}
if (dist3 <= player.Radius && Dot(CO, CA) * Dot(AO, CA) <= 0)
{
player.Color = intersect;
triangle.Point1 += direction.Normal * (player.Radius - dist3);
triangle.Point2 += direction.Normal * (player.Radius - dist3);
triangle.Point3 += direction.Normal * (player.Radius - dist3);
}
if (triangle.Point1.Y < 400 || triangle.Point2.Y < 400 || triangle.Point2.Y < 400)
{
triangle.Point1 += new Vector2F(0f, 1f);
triangle.Point2 += new Vector2F(0f, 1f);
triangle.Point3 += new Vector2F(0f, 1f);
}
//if (Vector2F.Cross(AO, AB) <= 0 && Vector2F.Cross(BO, BC) <= 0 && Vector2F.Cross(CO, CA) <= 0) player.Color = intersect;
// 移動
if (Engine.Keyboard.GetKeyState(Key.Right) == ButtonState.Hold)
{
player.Position += new Vector2F(5f, 0);
}
if (Engine.Keyboard.GetKeyState(Key.Left) == ButtonState.Hold)
{
player.Position += new Vector2F(-5f, 0);
}
if (Engine.Keyboard.GetKeyState(Key.Up) == ButtonState.Hold)
{
player.Position += new Vector2F(0, -5f);
}
if (Engine.Keyboard.GetKeyState(Key.Down) == ButtonState.Hold)
{
player.Position += new Vector2F(0, 5f);
}
//終了
if (Engine.Keyboard.GetKeyState(Key.Escape) == ButtonState.Push)
{
break;
}
}
Engine.Terminate();
}
