private void DrawMarkerSquare()
{
if (null != spatial_system_.JumpPointFinder)
{
int ct = spatial_system_.JumpPointFinder.VisitedCells.Count;
for (int i = 0; i < ct; ++i)
{
CellPos cell = spatial_system_.JumpPointFinder.VisitedCells[i];
Vector3 pos = cell_manager_.GetCellCenter(cell.row, cell.col);
DrawMarker((int)pos.x, (int)(map_height_ - pos.z), (int)cell_width_);
}
}
if (null != triangles)
{
for (int i = 0; i < triangles.Count; ++i)
{
TriangleNode tri = triangles[i];
Vector3 pt1 = tri.Points[0];
Vector3 pt2 = tri.Points[1];
Vector3 pt3 = tri.Points[2];
Point p1 = new Point((int)pt1.x, (int)(map_height_ - pt1.z));
Point p2 = new Point((int)pt2.x, (int)(map_height_ - pt2.z));
Point p3 = new Point((int)pt3.x, (int)(map_height_ - pt3.z));
graphics_.Graphics.DrawLine(triangle_pen, p1, p2);
graphics_.Graphics.DrawLine(triangle_pen, p2, p3);
graphics_.Graphics.DrawLine(triangle_pen, p3, p1);
graphics_.Graphics.DrawString((i + 1).ToString(), SystemFonts.DefaultFont, triangle_brush, (int)tri.Position.x, (int)(map_height_ - tri.Position.z));
//graphics_.Graphics.FillPolygon(triangle_brush, new Point[] { p1, p2, p3, p1 });
}
}
}
// Compare parent and children, if only one is relevant, return it, if both are revelant choose maximum
public static TriangleNode ChooseSuitableChild(Triangle triangle, int i, int j)
{
var currentParent = FindParent(triangle, i, j);
var currentLeftChild = FindLeftChild(triangle, i, j);
var currentRightChild = FindRightChild(triangle, i, j);
TriangleNode result = null;
if (IsValidTraversal(currentParent, currentLeftChild))
{
result = currentLeftChild;
}
if (IsValidTraversal(currentParent, currentRightChild))
{
if (result != null)
{
result = currentRightChild.NodeSum > currentLeftChild.NodeSum ? currentRightChild : currentLeftChild;
}
else
{
result = currentRightChild;
}
}
return(result);
}
internal TnsPathNode(TriangleNode node)
{
triangle_node = node;
prev = null;
cost = 0;
from_start_cost = 0;
}
static void Main()
{
TriangleNode Root = LoadTree();
int MaxSum = GetMaxSum(Root);
Console.Write("The best path sums up to {0}. Press enter to exit...", MaxSum);
Console.Read();
}
private float CalcHeuristic(TriangleNode triNode)
{
double xDelta = Math.Abs(m_EndNode.Position.x - triNode.Position.x);
double yDelta = Math.Abs(m_EndNode.Position.z - triNode.Position.z);
return((float)Math.Sqrt(xDelta * xDelta + yDelta * yDelta));
//return Math.Abs(m_EndNode.Position.X - triNode.Position.X) + Math.Abs(m_EndNode.Position.Z - triNode.Position.Z);
}
private static TriangleNode MaxOf(TriangleNode a, TriangleNode b)
{
if (null != a && null != b)
{
return(a.Value > b.Value ? a : b);
}
else
{
return(a ?? b);
}
}
private static bool IsValidTraversal(TriangleNode parent, TriangleNode child)
{
if (parent.NodeValue % 2 == child.NodeValue % 2)
{
return(false);
}
else
{
return(true);
}
}
private static TriangleNode[] LineToNodes(string line, TriangleNode[] lineBelow = null)
{
int[] vals = ParseLine(line);
TriangleNode[] newNodes = new TriangleNode[vals.Length];
for (int i = 0; i < vals.Length; i++)
{
var left = lineBelow?[i];
var right = lineBelow?[i + 1];
newNodes[i] = new TriangleNode(vals[i], left, right);
}
return(newNodes);
}
/// <summary>
/// Key part of the algorithm. Flips edges if they need to be flipped,
/// and recurses.
///
/// pi is the newly inserted point, creating a new triangle ti0.
/// The adjacent triangle opposite pi in ti0 is ti1. The edge separating
/// the two triangles is li0 and li1.
///
/// Checks if the (li0, li1) edge needs to be flipped. If it does,
/// creates two new triangles, and recurses to check if the newly
/// created triangles need flipping.
/// <summary>
void LegalizeEdge(int ti0, int ti1, int pi, int li0, int li1)
{
// ti1 might not be a leaf node (ti0 is guaranteed to be, it was
// just created), so find the current correct leaf.
ti1 = LeafWithEdge(ti1, li0, li1);
var t0 = triangles[ti0];
var t1 = triangles[ti1];
var qi = t1.OtherPoint(li0, li1);
if (!LegalEdge(pi, qi, li0, li1))
{
var ti2 = triangles.Count;
var ti3 = ti2 + 1;
var t2 = new TriangleNode(pi, li0, qi);
var t3 = new TriangleNode(pi, qi, li1);
t2.A0 = t1.Opposite(li1);
t2.A1 = ti3;
t2.A2 = t0.Opposite(li1);
t3.A0 = t1.Opposite(li0);
t3.A1 = t0.Opposite(li0);
t3.A2 = ti2;
triangles.Add(t2);
triangles.Add(t3);
var nt0 = triangles[ti0];
var nt1 = triangles[ti1];
nt0.C0 = ti2;
nt0.C1 = ti3;
nt1.C0 = ti2;
nt1.C1 = ti3;
triangles[ti0] = nt0;
triangles[ti1] = nt1;
if (t2.A0 != -1)
{
LegalizeEdge(ti2, t2.A0, pi, li0, qi);
}
if (t3.A0 != -1)
{
LegalizeEdge(ti3, t3.A0, pi, qi, li1);
}
}
}
private void FindNeighbors(TnsPathNode node)
{
m_Neighbors.Clear();
TriangleNode triNode = node.triangle_node;
for (int i = 0; i < 3; ++i)
{
TriangleNode neighbor = triNode.Neighbors[i];
if (null != neighbor)
{
m_Neighbors.Add(neighbor);
}
}
}
private int GetSideIndex(TriangleNode cur, TriangleNode other)
{
int ret = -1;
for (int ix = 0; ix < 3; ++ix)
{
if (cur.Neighbors[ix] == other)
{
ret = ix;
break;
}
}
return(ret);
}
// Navigates the tree and returns the maximum sum
static int GetMaxSum(TriangleNode Curr)
{
bool LeafNode = Curr.Left == null && Curr.Right == null;
bool Memoized = Curr.MaxSum != -1;
if (LeafNode)
{
Curr.MaxSum = Curr.Value;
}
else if (!Memoized)
{
Curr.MaxSum = Max(GetMaxSum(Curr.Left), GetMaxSum(Curr.Right)) + Curr.Value;
}
return(Curr.MaxSum);
}
private void IdentifySuccessors(TnsPathNode node)
{
FindNeighbors(node);
int ct = m_Neighbors.Count;
for (int i = 0; i < ct; ++i)
{
TriangleNode triNode = m_Neighbors[i];
if (m_CloseSet.ContainsKey(triNode))
{
continue;
}
Vector3 pos1 = node.triangle_node.Position;
Vector3 pos2 = triNode.Position;
float d = Geometry.Distance(pos1, pos2);
float ng = node.from_start_cost + d;
bool isOpened = true;
TnsPathNode nextNode;
if (!m_OpenNodes.TryGetValue(triNode, out nextNode))
{
nextNode = new TnsPathNode(triNode);
isOpened = false;
}
if (!isOpened || ng < nextNode.from_start_cost)
{
nextNode.from_start_cost = ng;
nextNode.cost = nextNode.from_start_cost + CalcHeuristic(triNode);
nextNode.prev = node;
if (!isOpened)
{
m_OpenQueue.Push(nextNode);
m_OpenNodes.Add(triNode, nextNode);
}
else
{
int index = m_OpenQueue.IndexOf(nextNode);
m_OpenQueue.Update(index, nextNode);
}
}
}
}
private bool IsLineAcross(Vector3 sp, Vector3 ep, TriangleNode start, TriangleNode end)
{
bool across = true;
TriangleNode current = start;
List <int> cacheList = new List <int>();
do
{
int acrossIndex = -1;
do
{
TriangleNode assit = current.Neighbors[0];
if (null != assit && !cacheList.Contains(assit.Id) && IntersectLine(sp, ep, assit))
{
acrossIndex = 0;
break;
}
assit = current.Neighbors[1];
if (null != assit && !cacheList.Contains(assit.Id) && IntersectLine(sp, ep, assit))
{
acrossIndex = 1;
break;
}
assit = current.Neighbors[2];
if (null != assit && !cacheList.Contains(assit.Id) && IntersectLine(sp, ep, assit))
{
acrossIndex = 2;
break;
}
} while (false);
if (acrossIndex < 0)
{
across = false;
break;
}
else
{
cacheList.Add(current.Id);
current = current.Neighbors[acrossIndex];
}
} while (current.Id != end.Id);
return(across);
}
private static TriangleNode BuildNodeStructure(List <string> data)
{
TriangleNode[] lineBelow = LineToNodes(data.Last(), null);
TriangleNode topNode = null;
for (int lineIdx = data.Count() - 2; lineIdx >= 0; lineIdx--)
{
TriangleNode[] thisLine = LineToNodes(data[lineIdx], lineBelow);
if (lineIdx == 0)
{
topNode = thisLine[0];
}
else
{
lineBelow = thisLine;
}
}
return(topNode);
}
public TriangleNode(int value, TriangleNode child1, TriangleNode child2)
{
Value = value;
LeftChild = child1;
RightChild = child2;
int maxChildVal = 0;
if (null != LeftChild && null != RightChild)
{
maxChildVal = LeftChild.MaxPathValue > RightChild.MaxPathValue
? LeftChild.MaxPathValue : RightChild.MaxPathValue;
}
else
{
maxChildVal = LeftChild?.MaxPathValue ?? RightChild?.MaxPathValue ?? 0;
}
MaxPathValue = Value + maxChildVal;
}
// Loads the tree from the file in the specified directory
static TriangleNode LoadTree()
{
TriangleNode Root = new TriangleNode(-1);
List <TriangleNode> PrevNodes = new List <TriangleNode>(); // each line of nodes needs to know the line before it
foreach (string Line in File.ReadAllLines(ProgramDataDir))
{
int[] NodeValues = Array.ConvertAll(Line.Split(' '), int.Parse);
if (Root.Value == -1)
{
Root.Value = NodeValues[0];
PrevNodes.Add(Root);
}
else
{
List <TriangleNode> TempPrevNodes = new List <TriangleNode>();
for (int i = 0; i < PrevNodes.Count; i++)
{
TriangleNode ParentNode = PrevNodes[i];
TriangleNode ChildNodeLeft = null;
if (i == 0)
{
ChildNodeLeft = new TriangleNode(NodeValues[0]);
TempPrevNodes.Add(ChildNodeLeft);
}
else
{
ChildNodeLeft = TempPrevNodes[i];
}
TriangleNode ChildNodeRight = new TriangleNode(NodeValues[i + 1]);
ParentNode.Left = ChildNodeLeft;
ParentNode.Right = ChildNodeRight;
TempPrevNodes.Add(ChildNodeRight);
}
PrevNodes = TempPrevNodes;
}
}
return(Root);
}
public void TriangleNode()
{
var tc = new TestCore(new Configuration());
tc.Init();
var triangle = new TriangleNode()
{
Color = new Color(255, 0, 0),
Point1 = new Vector2F(100f, 100f),
Point2 = new Vector2F(200f, 200f),
Point3 = new Vector2F(100f, 200f),
Position = new Vector2F(100, 100),
};
Engine.AddNode(triangle);
tc.LoopBody((c) =>
{
triangle.Point2 += new Vector2F(1, 1);
}, null);
tc.End();
}
// Populate the triangle structure nodes from text file, return empty array if exception
public Triangle PopulateFromFile(string fileName)
{
if (string.IsNullOrEmpty(fileName))
{
throw new ArgumentException("Error! File Name is incorrect.");
}
List <TriangleNode[]> nodes = new List <TriangleNode[]>();
using (StreamReader reader = new StreamReader(fileName))
{
var line = "";
while ((line = reader.ReadLine()) != null)
{
var valuesList = line.Split(" ");
TriangleNode[] lineNodes = new TriangleNode[valuesList.Length];
for (var i = 0; i < valuesList.Length; i++)
{
try
{
var numberValue = int.Parse(valuesList[i]);
lineNodes[i] = new TriangleNode(numberValue);
}
catch (FormatException ex)
{
Console.WriteLine("Error! Wrong file format. Error: " + ex.Message);
nodes.Clear();
return(new Triangle(nodes.ToArray()));
}
}
nodes.Add(lineNodes);
}
}
return(new Triangle(nodes.ToArray()));
}
public bool CanPass(Vector3 start, Vector3 target)
{
bool pass = false;
TriangleNode stNode = null, edNode = null;
m_KdTree.Query(start, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
{
if (0 <= Geometry.PointInTriangle(start, node.Points[0], node.Points[1], node.Points[2]))
{
stNode = node;
return(false);
}
return(true);
});
m_KdTree.Query(target, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
{
if (0 <= Geometry.PointInTriangle(target, node.Points[0], node.Points[1], node.Points[2]))
{
edNode = node;
return(false);
}
return(true);
});
if (null != stNode && null != edNode)
{
if (stNode == edNode)
{
pass = true;
}
else
{
pass = IsLineAcross(start, target, stNode, edNode);
}
}
return(pass);
}
static void Main(string[] args)
{
// Altseed2 を初期化します。
Engine.Initialize("ShapeNode", 640, 480);
// 円を描画するノード
var circle = new CircleNode()
{
Color = new Color(255, 100, 100),
Radius = 30f,
Position = new Vector2F(100f, 300f),
VertNum = 30
};
// 円弧を描画するノード
var arc = new ArcNode()
{
Color = new Color(100, 255, 100),
Radius = 25f,
Position = new Vector2F(300f, 100f),
StartDegree = 30f,
EndDegree = 150f,
VertNum = 30
};
// 直線を描画するノード
var line = new LineNode()
{
Color = new Color(100, 100, 255),
Point1 = new Vector2F(200f, 150f),
Point2 = new Vector2F(400f, 350f),
Thickness = 5f
};
// 短形を描画するノード
var rectangle = new RectangleNode()
{
Color = new Color(255, 255, 100),
Position = new Vector2F(300f, 400f),
RectangleSize = new Vector2F(50f, 50f)
};
// 三角形を描画するノード
var triangle = new TriangleNode()
{
Color = new Color(255, 100, 255),
Point1 = new Vector2F(50f, 50f),
Point2 = new Vector2F(100f, 50f),
Point3 = new Vector2F(50f, 100f),
};
// エンジンにノードを追加します。
Engine.AddNode(circle);
Engine.AddNode(arc);
Engine.AddNode(line);
Engine.AddNode(rectangle);
Engine.AddNode(triangle);
// メインループ。
// Altseed のウインドウが閉じられると終了します。
while (Engine.DoEvents())
{
// Altseed を更新します。
Engine.Update();
}
// Altseed の終了処理をします。
Engine.Terminate();
}
public static IList <TriangleNode> BuildTriangleFromFile(string sFilePath, bool useFileScale, out float width, out float height)
{
width = 0;
height = 0;
string sFileName = sFilePath.Replace("obs", "navmesh");
if (!File.Exists(sFileName))
{
return(null);
}
Dictionary <int, TriangleNode> TriangleNodeMap = new Dictionary <int, TriangleNode>();
List <TriangleNode> TriangleNodeList = new List <TriangleNode>();
try
{
using (FileStream fs = new FileStream(sFileName, FileMode.Open))
{
using (BinaryReader binReader = new BinaryReader(fs))
{
// 读取版本号
string fileVersion = new string(binReader.ReadChars(NAVMESH_VERSION.Length));
if (fileVersion != NAVMESH_VERSION)
{
return(null);
}
width = binReader.ReadInt32();
height = binReader.ReadInt32();
float scale = binReader.ReadSingle();
if (!useFileScale)
{
scale = 1.0f;
}
// 读取导航三角形数量
int navCount = binReader.ReadInt32();
for (int i = 0; i < navCount; i++)
{
TriangleNode node = new TriangleNode();
node.Read(binReader, scale);
TriangleNodeList.Add(node);
TriangleNodeMap.Add(node.Id, node);
}
binReader.Close();
}
fs.Close();
}
//build Neighbors node//顺时针
int ct = TriangleNodeList.Count;
for (int i = 0; i < ct; ++i)
{
TriangleNode node = TriangleNodeList[i];
TriangleNode outnode = null;
if (TriangleNodeMap.TryGetValue(node.NeighborsId[0], out outnode))
{
node.Neighbors[2] = outnode;
}
if (TriangleNodeMap.TryGetValue(node.NeighborsId[1], out outnode))
{
node.Neighbors[1] = outnode;
}
if (TriangleNodeMap.TryGetValue(node.NeighborsId[2], out outnode))
{
node.Neighbors[0] = outnode;
}
node.CalcGeometryInfo();
}
}
catch
{
}
finally
{
}
if (TriangleNodeList.Count == 0)
{
return(null);
}
//排序//
TriangleNodeList.Sort((TriangleNode node1, TriangleNode node2) =>
{
if (node1.Position.x < node2.Position.x)
{
return(-1); //左值小于右值，返回-1，为升序，如果返回1，就是降序
}
else if (node1.Position.x == node2.Position.x)
{//左值等于右值，返回0
if (node1.Position.z == node2.Position.z)
{
return(0);
}
else if (node1.Position.z < node2.Position.z)
{
return(-1);
}
else
{
return(1);
}
}
else
{
return(1);//左值大于右值，返回1，为升序，如果返回-1，就是降序
}
});
IList <TriangleNode> ret = TriangleNodeList.ToArray();
TriangleNodeList.Clear();
TriangleNodeMap.Clear();
return(ret);
}
public List <Vector3> FindPath(Vector3 start, Vector3 target, Vector3[] addToFirst, Vector3[] addToLast)
{
m_OpenQueue.Clear();
m_OpenNodes.Clear();
m_CloseSet.Clear();
m_VisitedNodes.Clear();
TriangleNode stNode = null, edNode = null;
m_CacheNodesForStart.Clear();
m_CacheNodesForEnd.Clear();
m_KdTree.Query(start, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
{
if (0 <= Geometry.PointInTriangle(start, node.Points[0], node.Points[1], node.Points[2]))
{
stNode = node;
return(false);
}
else
{
m_CacheNodesForStart.Add(node);
}
return(true);
});
m_KdTree.Query(target, m_SparseDistance * 2, (float distSqr, TriangleNode node) =>
{
if (0 <= Geometry.PointInTriangle(target, node.Points[0], node.Points[1], node.Points[2]))
{
edNode = node;
return(false);
}
else
{
m_CacheNodesForEnd.Add(node);
}
return(true);
});
Vector3 nearstStartPt = new Vector3();
bool useNearstStart = false;
if (null == stNode)
{
TriangleNode nearstNode = null;
float nearstDistSqr = float.MaxValue;
for (int ix = 0; ix < m_CacheNodesForStart.Count; ++ix)
{
TriangleNode node = m_CacheNodesForStart[ix];
for (int i = 0; i < 3; ++i)
{
if (node.Neighbors[i] == null)
{
Vector3 pt;
float dSqr = Geometry.PointToLineSegmentDistanceSquare(start, node.Points[i], node.Points[(i + 1) % 3], out pt);
if (dSqr < nearstDistSqr)
{
nearstNode = node;
nearstDistSqr = dSqr;
nearstStartPt = pt;
}
}
}
}
if (null != nearstNode)
{
stNode = nearstNode;
useNearstStart = true;
}
}
Vector3 nearstEndPt = new Vector3();
bool useNearstEnd = false;
if (null == edNode)
{
TriangleNode nearstNode = null;
float nearstDistSqr = float.MaxValue;
for (int ix = 0; ix < m_CacheNodesForEnd.Count; ++ix)
{
TriangleNode node = m_CacheNodesForEnd[ix];
for (int i = 0; i < 3; ++i)
{
if (node.Neighbors[i] == null)
{
Vector3 pt;
float dSqr = Geometry.PointToLineSegmentDistanceSquare(target, node.Points[i], node.Points[(i + 1) % 3], out pt);
if (dSqr < nearstDistSqr)
{
nearstNode = node;
nearstDistSqr = dSqr;
nearstEndPt = pt;
}
}
}
}
if (null != nearstNode)
{
edNode = nearstNode;
useNearstEnd = true;
}
}
if (null == stNode || null == edNode)
{
return(new List <Vector3>());
}
m_StartNode = stNode;
m_EndNode = edNode;
TnsPathNode startNode = new TnsPathNode(stNode);
startNode.cost = 0;
startNode.from_start_cost = 0;
m_OpenQueue.Push(startNode);
m_OpenNodes.Add(startNode.triangle_node, startNode);
TnsPathNode curNode = null;
while (m_OpenQueue.Count > 0)
{
curNode = m_OpenQueue.Pop();
if (m_RecordVisitedNodes)
{
m_VisitedNodes.Add(curNode.triangle_node);
//LogSystem.Debug("cost:{0} from_start_cost:{1}", curNode.cost, curNode.from_start_cost);
}
m_OpenNodes.Remove(curNode.triangle_node);
m_CloseSet.Add(curNode.triangle_node, true);
if (curNode.triangle_node == edNode)
{
break;
}
IdentifySuccessors(curNode);
}
List <Vector3> path = new List <Vector3>();
if (curNode.triangle_node == edNode)
{
//建立路径邻接信息与路点三角形的穿出边信息
TnsPathNode next = null;
while (curNode != null)
{
TnsPathNode prev = curNode.prev;
if (null != prev)
{
int ptIx = GetSideIndex(curNode.triangle_node, prev.triangle_node);
if (ptIx >= 0)
{
ptIx = (ptIx + 2) % 3;
curNode.new_point_index = ptIx;
}
if (null != next)
{
int sideIx = GetSideIndex(curNode.triangle_node, next.triangle_node);
if (sideIx == ptIx)
{
curNode.edge_flags = 0x01;
}
else
{
curNode.edge_flags = 0x02;
}
}
prev.next = curNode;
}
next = curNode;
curNode = prev;
}
//拐点法化简与平滑路径
if (null != addToFirst && addToFirst.Length > 0)
{
path.AddRange(addToFirst);
}
path.Add(start);
Vector3 curPnt;
Vector3 targetPnt;
if (useNearstStart)
{
path.Add(nearstStartPt);
curPnt = nearstStartPt;
}
else
{
curPnt = start;
}
if (useNearstEnd)
{
targetPnt = nearstEndPt;
}
else
{
targetPnt = target;
}
curNode = startNode;
while (null != curNode)
{
TnsPathNode nextNode;
curPnt = GetNextWayPoint(curPnt, targetPnt, curNode, out nextNode);
int ct = path.Count;
if (Geometry.DistanceSquare(path[ct - 1], curPnt) > m_SparseDistance * m_SparseDistance)
{
path.Add(curPnt);
}
curNode = nextNode;
}
if (useNearstEnd)
{
path.Add(nearstEndPt);
}
/// path.Add(target);
if (null != addToLast && addToLast.Length > 0)
{
path.AddRange(addToLast);
}
}
return(path);
}
private bool IntersectLine(Vector3 start, Vector3 target, TriangleNode node)
{
return(null != node && Geometry.IntersectLine(start, target, node.Points[0], node.Points[1], node.Points[2]));
}
/// <summary>
/// Run the algorithm
/// </summary>
void RunBowyerWatson()
{
// For each point, find the containing triangle, split it into three
// new triangles, call LegalizeEdge on all edges opposite the newly
// inserted points
for (int i = 0; i < verts.Count; i++)
{
var pi = i;
if (pi == highest)
{
continue;
}
// Index of the containing triangle
var ti = FindTriangleNode(pi);
var t = triangles[ti];
// The points of the containing triangle in CCW order
var p0 = t.P0;
var p1 = t.P1;
var p2 = t.P2;
// Indices of the newly created triangles.
var nti0 = triangles.Count;
var nti1 = nti0 + 1;
var nti2 = nti0 + 2;
// The new triangles! All in CCW order
var nt0 = new TriangleNode(pi, p0, p1);
var nt1 = new TriangleNode(pi, p1, p2);
var nt2 = new TriangleNode(pi, p2, p0);
// Setting the adjacency triangle references. Only way to make
// sure you do this right is by drawing the triangles up on a
// piece of paper.
nt0.A0 = t.A2;
nt1.A0 = t.A0;
nt2.A0 = t.A1;
nt0.A1 = nti1;
nt1.A1 = nti2;
nt2.A1 = nti0;
nt0.A2 = nti2;
nt1.A2 = nti0;
nt2.A2 = nti1;
// The new triangles are the children of the old one.
t.C0 = nti0;
t.C1 = nti1;
t.C2 = nti2;
triangles[ti] = t;
triangles.Add(nt0);
triangles.Add(nt1);
triangles.Add(nt2);
if (nt0.A0 != -1)
{
LegalizeEdge(nti0, nt0.A0, pi, p0, p1);
}
if (nt1.A0 != -1)
{
LegalizeEdge(nti1, nt1.A0, pi, p1, p2);
}
if (nt2.A0 != -1)
{
LegalizeEdge(nti2, nt2.A0, pi, p2, p0);
}
}
}
private void MergeMemory(TriangleNode parent, TriangleNode child)
{
parent.NodeMemory.AddRange(child.NodeMemory);
}
public void Basic()
{
Engine.Initialize("Basic", 800, 600, new Configuration());
var world = new World(new RectF(-100, -100, 1000, 1000), new Vector2F(0, 10));
var floor = new RectangleNode();
floor.RectangleSize = new Vector2F(800, 50);
Engine.AddNode(floor);
var floorCollider = new PhysicsRectangleColliderNode(world);
floorCollider.Position = new Vector2F(0.0f, 550.0f);
floorCollider.RectangleSize = floor.RectangleSize;
floorCollider.PhysicsColliderType = PhysicsColliderType.Static;
floorCollider.Restitution = 0.2f;
floor.AddChildNode(floorCollider);
var sprite = new RectangleNode();
sprite.Position = new Vector2F(400, 80);
sprite.RectangleSize = new Vector2F(50, 50);
sprite.CenterPosition = sprite.RectangleSize / 2;
sprite.Angle = 40;
Engine.AddNode(sprite);
var collider = new PhysicsRectangleColliderNode(world);
collider.CenterPosition = sprite.CenterPosition;
collider.Position = sprite.Position;
collider.Angle = sprite.Angle;
collider.RectangleSize = sprite.RectangleSize;
collider.PhysicsColliderType = PhysicsColliderType.Dynamic;
collider.Restitution = 0.2f;
sprite.AddChildNode(collider);
var text = new TextNode();
text.Font = Font.LoadDynamicFont("../TestData/Font/mplus-1m-regular.ttf", 50);
Engine.AddNode(text);
int count = 0;
while (Engine.DoEvents())
{
if (count % 30 == 0)
{
var sprite1 = new RectangleNode();
sprite1.Position = new Vector2F(400, 80);
sprite1.RectangleSize = new Vector2F(50, 50);
sprite1.CenterPosition = sprite1.RectangleSize / 2;
sprite1.Angle = 40;
Engine.AddNode(sprite1);
var collider1 = new PhysicsRectangleColliderNode(world);
collider1.CenterPosition = sprite1.CenterPosition;
collider1.Position = sprite1.Position;
collider1.Angle = sprite1.Angle;
collider1.RectangleSize = sprite1.RectangleSize;
collider1.PhysicsColliderType = PhysicsColliderType.Dynamic;
collider1.Restitution = 1f;
sprite1.AddChildNode(collider1);
var triangle = new TriangleNode();
triangle.Position = new Vector2F(300, 80);
triangle.Point1 = new Vector2F(50, 50);
triangle.Point2 = new Vector2F(100, 50);
triangle.Point3 = new Vector2F(0, 0);
triangle.Angle = 40;
Engine.AddNode(triangle);
var collider2 = new PhysicsPolygonColliderNode(world);
collider2.Position = triangle.Position;
collider2.Angle = triangle.Angle;
collider2.AddVertex(new Vector2F(50, 50));
collider2.AddVertex(new Vector2F(0, 0));
collider2.AddVertex(new Vector2F(100, 50));
collider2.PhysicsColliderType = PhysicsColliderType.Dynamic;
collider2.Restitution = 0f;
triangle.AddChildNode(collider2);
}
if (count++ > 4000)
{
break;
}
//text.Text = collider.Velocity.ToString();
world.Update();
Engine.Update();
}
Engine.Terminate();
}
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();
}
/// <summary>
/// Key part of the algorithm. Flips edges if they need to be flipped,
/// and recurses.
///
/// pi is the newly inserted point, creating a new triangle ti0.
/// The adjacent triangle opposite pi in ti0 is ti1. The edge separating
/// the two triangles is li0 and li1.
///
/// Checks if the (li0, li1) edge needs to be flipped. If it does,
/// creates two new triangles, and recurses to check if the newly
/// created triangles need flipping.
/// <summary>
void LegalizeEdge(int ti0, int ti1, int pi, int li0, int li1)
{
// ti1 might not be a leaf node (ti0 is guaranteed to be, it was
// just created), so find the current correct leaf.
ti1 = LeafWithEdge(ti1, li0, li1);
var t0 = triangles[ti0];
var t1 = triangles[ti1];
var qi = t1.OtherPoint(li0, li1);
Debug.Assert(t0.HasEdge(li0, li1));
Debug.Assert(t1.HasEdge(li0, li1));
Debug.Assert(t0.IsLeaf);
Debug.Assert(t1.IsLeaf);
Debug.Assert(t0.P0 == pi || t0.P1 == pi || t0.P2 == pi);
Debug.Assert(t1.P0 == qi || t1.P1 == qi || t1.P2 == qi);
//var p = points[pi];
//var q = points[qi];
//var l0 = points[li0];
//var l1 = points[li1];
if (!LegalEdge(pi, qi, li0, li1))
{
var ti2 = triangles.Count;
var ti3 = ti2 + 1;
var t2 = new TriangleNode(pi, li0, qi);
var t3 = new TriangleNode(pi, qi, li1);
t2.A0 = t1.Opposite(li1);
t2.A1 = ti3;
t2.A2 = t0.Opposite(li1);
t3.A0 = t1.Opposite(li0);
t3.A1 = t0.Opposite(li0);
t3.A2 = ti2;
triangles.Add(t2);
triangles.Add(t3);
var nt0 = triangles[ti0];
var nt1 = triangles[ti1];
nt0.C0 = ti2;
nt0.C1 = ti3;
nt1.C0 = ti2;
nt1.C1 = ti3;
triangles[ti0] = nt0;
triangles[ti1] = nt1;
if (t2.A0 != -1)
{
LegalizeEdge(ti2, t2.A0, pi, li0, qi);
}
if (t3.A0 != -1)
{
LegalizeEdge(ti3, t3.A0, pi, qi, li1);
}
}
}
