/
Search.cs
105 lines (83 loc) · 2.74 KB
/
Search.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
using PathFinder;
using System.Collections.Generic;
using System.Security.Cryptography.X509Certificates;
namespace GOAP
{
public class GoapGraph : ISearchSpace
{
private ActionPlanner planner;
public GoapGraph(ActionPlanner planner)
{
this.planner = planner;
}
// This is our heuristic: estimate for remaining distance is the nr of mismatched atoms that matter.
public static float PlannerHeuristic(INode from, INode to)
{
var wsFrom = ((WorldStateNode)from).State;
var wsTo = ((WorldStateNode)to).State;
var mask = wsTo.Mask;
var diff = ((wsFrom.Values & mask) ^ (wsTo.Values & mask));
var distance = 0;
while (diff > 0)
{
// right most bit is 1
if (diff % 2 == 1)
{
distance++;
}
diff >>= 1;
}
return distance;
}
public IEnumerable<NodeConnection> GetPossibleTransitions(INode from)
{
var wsFrom = ((WorldStateNode)from).State;
var result = new List<NodeConnection>();
foreach (var action in planner.Actions)
{
var pre = action.Preconditions;
var mask = pre.Mask;
var met = (pre.Values & mask) == (wsFrom.Values & mask);
if (met)
{
var neighbour = planner.PerformAction(action, wsFrom);
if (wsFrom.Match(neighbour))
{
continue;
}
var connection = new NodeConnection();
connection.From = from;
connection.To = new WorldStateNode(this, neighbour, action);
connection.Cost = action.Cost;
result.Add(connection);
}
}
return result;
}
}
public class WorldStateNode : INode
{
public readonly GoapGraph Graph;
public WorldState State;
public ActionPlanner.Action Action;
public WorldStateNode(GoapGraph graph, WorldState state, ActionPlanner.Action action)
{
Graph = graph;
State = state;
Action = action;
}
public float Weight { get { return 1; } }
public IEnumerable<NodeConnection> Connections
{
get
{
return Graph.GetPossibleTransitions(this);
}
}
public bool Equals(INode other)
{
var wsOther = ((WorldStateNode)other).State;
return State.Match(wsOther);
}
}
}