/// <summary>Runs the A* search algorithm algorithm on a graph.</summary>
/// <param name="start">The node to start at.</param>
/// <param name="neighbors">Step function for all neigbors of a given node.</param>
/// <param name="heuristic">Computes the heuristic value of a given node in a graph.</param>
/// <param name="cost">Computes the cost of moving from the current node to a specific neighbor.</param>
/// <param name="goal">Predicate for determining if we have reached the goal node.</param>
/// <returns>Stepper of the shortest path or null if no path exists.</returns>
public static Stepper <T> Astar(T start, Neighbors neighbors, Heuristic heuristic, Cost cost, Goal goal)
{
// using a heap (aka priority queue) to store nodes based on their computed A* f(n) value
Heap <Astar_Node> fringe = new HeapArray <Astar_Node>(
// NOTE: Typical A* implementations prioritize smaller values
(Astar_Node left, Astar_Node right) =>
{
Comparison comparison = Compute.Compare <Math>(right.Priority, left.Priority);
return(comparison);
});
// using a map (aka dictionary) to store costs from start to current nodes
Map <Math, Astar_Node> computed_costs = new MapHashArray <Math, Astar_Node>();
// construct the f(n) for this A* execution
Astar_function function = (T node, Astar_Node previous) =>
{
Math previousCost = computed_costs.Get(previous);
Math currentCost = cost(previous.Value, node);
Math costFromStart = Compute.Add <Math>(previousCost, currentCost);
Math hueristic = heuristic(node);
return(Compute.Add <Math>(costFromStart, hueristic));
};
// push starting node
Astar_Node start_node = new Astar_Node(null, start, default(Math));
fringe.Enqueue(start_node);
computed_costs.Add(start_node, default(Math));
// run the algorithm
while (fringe.Count != 0)
{
Astar_Node current = fringe.Dequeue();
if (goal(current.Value))
{
return(Astar_BuildPath(current));
}
else
{
neighbors(current.Value,
(T neighbor) =>
{
Astar_Node newNode = new Astar_Node(current, neighbor, function(neighbor, current));
Math costValue = Compute.Add <Math>(computed_costs.Get(current), cost(current.Value, neighbor));
computed_costs.Add(newNode, costValue);
fringe.Enqueue(newNode);
});
}
}
return(null); // goal node was not reached (no path exists)
}
///// <summary>Creates a quaternion from an axis and rotation.</summary>
///// <param name="axis">The to create the quaternion from.</param>
///// <param name="angle">The angle to create teh quaternion from.</param>
///// <returns>The newly created quaternion.</returns>
//public static Quaternion<T> FactoryFromAxisAngle(Vector axis, T angle)
//{
// throw new System.NotImplementedException();
// //if (axis.LengthSquared() == 0.0f)
// // return FactoryIdentity;
// //T sinAngleOverAxisLength = Calc.Sin(angle / 2) / axis.Length();
// //return Quaternion<T>.Normalize(new Quaternion<T>(
// // _multiply(axis.X, sinAngleOverAxisLength),
// // axis.Y * sinAngleOverAxisLength,
// // axis.Z * sinAngleOverAxisLength,
// // Calc.Cos(angle / 2)));
//}
public static Quaternion <T> Factory_Matrix3x3(Matrix <T> matrix)
{
if (matrix.Rows != 3 || matrix.Columns != 3)
{
throw new System.ArithmeticException("error converting matrix to quaternion. matrix is not 3x3.");
}
T w = Compute.Subtract(Compute.Add(Constant <T> .One, matrix[0, 0], matrix[1, 1], matrix[2, 2]), Compute.FromInt32 <T>(2));
return(new Quaternion <T>(
Compute.Divide(Compute.Subtract(matrix[2, 1], matrix[1, 2]), Compute.Multiply(Compute.FromInt32 <T>(4), w)),
Compute.Divide(Compute.Subtract(matrix[0, 2], matrix[2, 0]), Compute.Multiply(Compute.FromInt32 <T>(4), w)),
Compute.Divide(Compute.Subtract(matrix[1, 0], matrix[0, 1]), Compute.Multiply(Compute.FromInt32 <T>(4), w)),
w));
}
/// <summary>Converts a quaternion into a 3x3 matrix.</summary>
/// <param name="quaternion">The quaternion of the conversion.</param>
/// <returns>The resulting 3x3 matrix.</returns>
public static Matrix <T> ToMatrix3x3(Quaternion <T> quaternion)
{
return(new Matrix <T>(3, 3, (int x, int y) =>
{
switch (x)
{
case 0:
switch (y)
{
case 0: return Compute.Subtract(Compute.Subtract(Compute.Add(Compute.Multiply(quaternion.W, quaternion.W), Compute.Multiply(quaternion.X, quaternion.X)), Compute.Multiply(quaternion.Y, quaternion.Y)), Compute.Multiply(quaternion.Z, quaternion.Z));
case 1: return Compute.Subtract(Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.X), quaternion.Y), Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.W), quaternion.Z));
case 2: return Compute.Add(Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.X), quaternion.Z), Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.W), quaternion.Y));
default: throw new MathematicsException("BUG");
}
case 1:
switch (y)
{
case 0: return Compute.Add(Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.X), quaternion.Y), Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.W), quaternion.Z));
case 1: return Compute.Subtract(Compute.Add(Compute.Subtract(Compute.Multiply(quaternion.W, quaternion.W), Compute.Multiply(quaternion.X, quaternion.X)), Compute.Multiply(quaternion.Y, quaternion.Y)), Compute.Multiply(quaternion.Z, quaternion.Z));
case 2: return Compute.Add(Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.Y), quaternion.Z), Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.W), quaternion.X));
default: throw new MathematicsException("BUG");
}
case 2:
switch (y)
{
case 0: return Compute.Subtract(Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.X), quaternion.Z), Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.W), quaternion.Y));
case 1: return Compute.Subtract(Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.Y), quaternion.Z), Compute.Multiply(Compute.Multiply(Compute.FromInt32 <T>(2), quaternion.W), quaternion.X));
case 2: return Compute.Add(Compute.Subtract(Compute.Subtract(Compute.Multiply(quaternion.W, quaternion.W), Compute.Multiply(quaternion.X, quaternion.X)), Compute.Multiply(quaternion.Y, quaternion.Y)), Compute.Multiply(quaternion.Z, quaternion.Z));
default: throw new MathematicsException("BUG");
}
default:
throw new MathematicsException("BUG");
}
}));
}
public static void TestMath()
{
#region math stuffs
Console.WriteLine("Negate: " + (Compute <int> .Negate(7) == -7));
Console.WriteLine("Add: " + (Compute <int> .Add(7, 7) == 14));
Console.WriteLine("Subtract: " + (Compute <int> .Subtract(14, 7) == 7));
Console.WriteLine("Multiply: " + (Compute <int> .Multiply(7, 7) == 49));
Console.WriteLine("Divide: " + (Compute <int> .Divide(14, 7) == 2));
Console.WriteLine("AbsoluteValue: " + (Compute <int> .AbsoluteValue(7) == 7 && Compute <int> .AbsoluteValue(-7) == 7));
Console.WriteLine("Clamp: " + (Compute <int> .Clamp(7, 6, 8) == 7));
Console.WriteLine("Maximum: " + (Compute <int> .Maximum((Step <int> step) => { step(1); step(2); step(3); }) == 3));
Console.WriteLine("Minimum: " + (Compute <int> .Minimum((Step <int> step) => { step(1); step(2); step(3); }) == 1));
Console.WriteLine("LessThan: " + (Compute <int> .LessThan(1, 2) == true && Compute <int> .LessThan(2, 1) == false));
Console.WriteLine("GreaterThan: " + (Compute <int> .GreaterThan(2, 1) == true && Compute <int> .GreaterThan(1, 2) == false));
Console.WriteLine("Compare: " + (Compute <int> .Compare(2, 1) == Comparison.Greater && Compute <int> .Compare(1, 2) == Comparison.Less && Compute <int> .Compare(1, 1) == Comparison.Equal));
Console.WriteLine("Equate: " + (Compute <int> .Equate(2, 1) == false && Compute <int> .Equate(1, 1) == true));
Console.WriteLine("EqualsLeniency: " + (Compute <int> .Equals(2, 1, 1) == true && Compute <int> .Equals(2, 1, 0) == false && Compute <int> .Equals(1, 1, 0) == true));
#endregion
}
private static bool SphereDetection(Sphere <T> a, Sphere <T> b, out Vector <T> point, out Vector <T> normal, out T penetration)
{
Vector <T> b_minus_a = b.Position - a.Position;
T distance = b_minus_a.Magnitude;
T combinedRadius = Compute.Add(a.Radius, b.Radius);
if (Compute.LessThan(distance, combinedRadius))
{
penetration = Compute.Subtract(combinedRadius, distance);
normal = b_minus_a.Normalize();
point = (b_minus_a * Compute.Divide(Constant <T> .One, Constant <T> .Two)) + a.Position;
return(true);
}
else
{
penetration = default(T);
normal = null;
point = null;
return(false);
}
}
/// <summary>Runs the A* search algorithm algorithm on a graph.</summary>
/// <param name="start">The node to start at.</param>
/// <param name="neighbors">Step function for all neigbors of a given node.</param>
/// <param name="heuristic">Computes the heuristic value of a given node in a graph.</param>
/// <param name="cost">Computes the cost of moving from the current node to a specific neighbor.</param>
/// <param name="goal">Predicate for determining if we have reached the goal node.</param>
/// <returns>Stepper of the shortest path or null if no path exists.</returns>
public static Stepper <NODE> Graph <NODE, NUMERIC>(NODE start, Neighbors <NODE> neighbors, Heuristic <NODE, NUMERIC> heuristic, Cost <NODE, NUMERIC> cost, Goal <NODE> goal)
{
// using a heap (aka priority queue) to store nodes based on their computed A* f(n) value
IHeap <AstarNode <NODE, NUMERIC> > fringe = new HeapArray <AstarNode <NODE, NUMERIC> >(
// NOTE: Typical A* implementations prioritize smaller values
(a, b) => Compute.Compare(b.Priority, a.Priority));
// push starting node
fringe.Enqueue(
new AstarNode <NODE, NUMERIC>(
null,
start,
default(NUMERIC),
Constant <NUMERIC> .Zero));
// run the algorithm
while (fringe.Count != 0)
{
AstarNode <NODE, NUMERIC> current = fringe.Dequeue();
if (goal(current.Value))
{
return(BuildPath(current));
}
else
{
neighbors(current.Value,
(NODE neighbor) =>
{
NUMERIC costValue = Compute.Add(current.Cost, cost(current.Value, neighbor));
fringe.Enqueue(
new AstarNode <NODE, NUMERIC>(
current,
neighbor,
Compute.Add(heuristic(neighbor), costValue),
costValue));
});
}
}
return(null); // goal node was not reached (no path exists)
}
public static Quaternion <T> Factory_Matrix4x4(Matrix <T> matrix)
{
matrix = matrix.Transpose();
T w, x, y, z;
T diagonal = Compute.Add(matrix[0, 0], matrix[1, 1], matrix[2, 2]);
if (Compute.GreaterThan(diagonal, Constant <T> .Zero))
{
T w4 = Compute.Multiply(Compute.SquareRoot(Compute.Add(diagonal, Constant <T> .One)), Compute.FromInt32 <T>(2));
w = Compute.Divide(w4, Compute.FromInt32 <T>(4));
x = Compute.Divide(Compute.Subtract(matrix[2, 1], matrix[1, 2]), w4);
y = Compute.Divide(Compute.Subtract(matrix[0, 2], matrix[2, 0]), w4);
z = Compute.Divide(Compute.Subtract(matrix[1, 0], matrix[0, 1]), w4);
}
else if (Compute.GreaterThan(matrix[0, 0], matrix[1, 1]) && Compute.GreaterThan(matrix[0, 0], matrix[2, 2]))
{
T x4 = Compute.Multiply(Compute.SquareRoot(Compute.Subtract(Compute.Subtract(Compute.Add(Constant <T> .One, matrix[0, 0]), matrix[1, 1]), matrix[2, 2])), Compute.FromInt32 <T>(2));
w = Compute.Divide(Compute.Subtract(matrix[2, 1], matrix[1, 2]), x4);
x = Compute.Divide(x4, Compute.FromInt32 <T>(4));
y = Compute.Divide(Compute.Add(matrix[0, 1], matrix[1, 0]), x4);
z = Compute.Divide(Compute.Add(matrix[0, 2], matrix[2, 0]), x4);
}
else if (Compute.GreaterThan(matrix[1, 1], matrix[2, 2]))
{
T y4 = Compute.Multiply(Compute.SquareRoot(Compute.Subtract(Compute.Subtract(Compute.Add(Constant <T> .One, matrix[1, 1]), matrix[0, 0]), matrix[2, 2])), Compute.FromInt32 <T>(2));
w = Compute.Divide(Compute.Subtract(matrix[0, 2], matrix[2, 0]), y4);
x = Compute.Divide(Compute.Add(matrix[0, 1], matrix[1, 0]), y4);
y = Compute.Divide(y4, Compute.FromInt32 <T>(4));
z = Compute.Divide(Compute.Add(matrix[1, 2], matrix[2, 1]), y4);
}
else
{
T z4 = Compute.Multiply(Compute.SquareRoot(Compute.Subtract(Compute.Subtract(Compute.Add(Constant <T> .One, matrix[2, 2]), matrix[0, 0]), matrix[1, 1])), Compute.FromInt32 <T>(2));
w = Compute.Divide(Compute.Subtract(matrix[1, 0], matrix[0, 1]), z4);
x = Compute.Divide(Compute.Add(matrix[0, 2], matrix[2, 0]), z4);
y = Compute.Divide(Compute.Add(matrix[1, 2], matrix[2, 1]), z4);
z = Compute.Divide(z4, Compute.FromInt32 <T>(4));
}
return(new Quaternion <T>(x, y, z, w));
}
static void Main(string[] args)
{
Console.WriteLine("You are runnning the Algorithms example.");
Console.WriteLine("======================================================");
Console.WriteLine();
#region Sorting
{
// Note: these functions are not restricted to array types. You can use the
// overloads with "Get" and "Assign" delegates to use them on any int-indexed
// data structure.
Console.WriteLine(" Sorting Algorithms----------------------");
Console.WriteLine();
int[] dataSet = new int[] { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
Console.Write(" Data Set:" + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
// Shuffling (Randomizing)
Sort.Shuffle(dataSet);
Console.Write(" Shuffle (Randomizing): " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
// Bubble
Sort.Bubble(dataSet);
Console.Write(" Bubble: " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort.Shuffle(dataSet);
// Selection
Sort.Selection(dataSet);
Console.Write(" Selection: " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort.Shuffle(dataSet);
// Insertion
Sort.Insertion(dataSet);
Console.Write(" Insertion: " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort.Shuffle(dataSet);
// Quick
Sort.Quick(dataSet);
Console.Write(" Quick: " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort.Shuffle(dataSet);
// Merge
Sort.Merge(Compute.Compare, dataSet);
Console.Write(" Merge: " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort.Shuffle(dataSet);
// Heap
Sort.Heap(Compute.Compare, dataSet);
Console.Write(" Heap: " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort.Shuffle(dataSet);
// OddEven
Sort.OddEven(Compute.Compare, dataSet);
Console.Write(" OddEven: " + string.Join(", ", dataSet.Select(x => x.ToString())));
Console.WriteLine();
//Console.WriteLine(" shuffling dataSet...");
//Sort<int>.Shuffle(dataSet);
//// Slow
//Sort<int>.Slow(Logic.compare, get, set, 0, dataSet.Length);
//Console.Write("Slow: " + string.Join(", ", dataSet.Select(x => x.ToString())));
//Console.WriteLine();
//Console.WriteLine(" shuffling dataSet...");
//Sort<int>.Shuffle(dataSet);
// Bogo
//Sort<int>.Bogo(Logic.compare, get, set, 0, dataSet.Length);
Console.Write(" Bogo: Disabled (takes forever)"); //+ string.Join(", ", dataSet.Select(x => x.ToString())));
//Console.WriteLine();
Console.WriteLine();
Console.WriteLine();
}
#endregion
#region Graph Search (Using Graph Data Structure)
{
Console.WriteLine(" Graph Searching----------------------");
Console.WriteLine();
// make a graph
IGraph <int> graph = new GraphSetOmnitree <int>()
{
// add nodes
0,
1,
2,
3,
// add edges
{ 0, 1 },
{ 0, 2 },
{ 1, 3 },
{ 2, 3 }
};
// make a heuristic function
int heuristic(int node)
{
switch (node)
{
case 0:
return(3);
case 1:
return(6);
case 2:
return(1);
case 3:
return(0);
default:
throw new NotImplementedException();
}
}
// make a cost function
int cost(int from, int to)
{
if (from == 0 && to == 1)
{
return(1);
}
if (from == 0 && to == 2)
{
return(2);
}
if (from == 1 && to == 3)
{
return(5);
}
if (from == 2 && to == 3)
{
return(1);
}
if (from == 0 && to == 3)
{
return(99);
}
throw new Exception("invalid path cost computation");
}
// make a goal function
bool goal(int node)
{
if (node == 3)
{
return(true);
}
else
{
return(false);
}
}
// run A* the algorithm
Stepper <int> aStar_path = Search.Graph <int, int>(0, graph, heuristic, cost, goal);
Console.Write(" A* Path: ");
if (aStar_path != null)
{
aStar_path(i => Console.Write(i + " "));
}
else
{
Console.Write("none");
}
Console.WriteLine();
// run the Greedy algorithm
Stepper <int> greedy_path = Search.Graph <int, int>(0, graph, heuristic, goal);
Console.Write(" Greedy Path: ");
if (greedy_path != null)
{
greedy_path(i => Console.Write(i + " "));
}
else
{
Console.Write("none");
}
Console.WriteLine();
Console.WriteLine();
}
#endregion
#region Graph Search (Vector Game-Style Example)
{
Console.WriteLine(" Graph Searching (Vector Game-Style Example)-------------------");
Console.WriteLine();
Console.WriteLine(" Debug the code. The path is to large to write to the console.");
Console.WriteLine();
// Lets say you are coding enemy AI and you want the AI to find a path towards the player
// in order to attack them. Here are their starting positions:
Vector <float> enemyLocation = new Vector <float>(-100f, 0f, -50f);
Vector <float> playerLocation = new Vector <float>(200f, 0f, -50f);
float enemyAttackRange = 3f; // enemy has a melee attack with 3 range
// Lets say most of the terrain is open, but there is a big rock in between them that they
// must go around.
Vector <float> rockLocation = new Vector <float>(15f, 0f, -40f);
float rockRadius = 20f;
// Make sure we don't re-use locations (must be wiped after running the algorithm)
ISet <Vector <float> > alreadyUsed = new SetHashLinked <Vector <float> >();
Vector <float> validationVectorStorage = null; // storage to prevent a ton of vectors from being allocated
// So, we just need to validate movement locations (make sure the path finding algorithm
// ignores locations inside the rock)
bool validateMovementLocation(Vector <float> location)
{
// if the location is inside the rock, it is not a valid movement
location.Subtract(rockLocation, ref validationVectorStorage);
float magnitude = validationVectorStorage.Magnitude;
if (magnitude <= rockRadius)
{
return(false);
}
// NOTE: If you are running a physics engine, you might be able to just call it to validate a location.
// if the location was already used, then let's consider it invalid, because
// another path (which is faster) has already reached that location
if (alreadyUsed.Contains(location))
{
return(false);
}
return(true);
}
// Now we need the neighbor function (getting the neighbors of the current location).
void neighborFunction(Vector <float> currentLocation, Step <Vector <float> > neighbors)
{
// NOTES:
// - This neighbor function has a 90 degree per-node resolution (360 / 4 [north/south/east/west] = 90).
// - This neighbor function has a 1 unit per-node distance resolution, because we went 1 unit in each direction.
// RECOMMENDATIONS:
// - If the path finding is failing, you may need to increase the resolution.
// - If the algorithm is running too slow, you may need to reduce the resolution.
float distanceResolution = 1;
float x = currentLocation.X;
float y = currentLocation.Y;
float z = currentLocation.Z;
// Note: I'm using the X-axis and Z-axis here, but which axis you need to use
// depends on your environment. Your "north" could be along the Y-axis for example.
Vector <float> north = new Vector <float>(x + distanceResolution, y, z);
if (validateMovementLocation(north))
{
alreadyUsed.Add(north); // mark location as used
neighbors(north);
}
Vector <float> east = new Vector <float>(x, y, z + distanceResolution);
if (validateMovementLocation(east))
{
alreadyUsed.Add(east); // mark location as used
neighbors(east);
}
Vector <float> south = new Vector <float>(x - distanceResolution, y, z);
if (validateMovementLocation(south))
{
alreadyUsed.Add(south); // mark location as used
neighbors(south);
}
Vector <float> west = new Vector <float>(x, y, z - distanceResolution);
if (validateMovementLocation(west))
{
alreadyUsed.Add(west); // mark location as used
neighbors(west);
}
}
Vector <float> heuristicVectorStorage = null; // storage to prevent a ton of vectors from being allocated
// Heuristic function (how close are we to the goal)
float heuristicFunction(Vector <float> currentLocation)
{
// The goal is the player's location, so we just need our distance from the player.
currentLocation.Subtract(playerLocation, ref heuristicVectorStorage);
return(heuristicVectorStorage.Magnitude);
}
// Lets say there is a lot of mud around the rock, and the mud makes our player move at half their normal speed.
// Our path finding needs to find the fastest route to the player, whether it be through the mud or not.
Vector <float> mudLocation = new Vector <float>(15f, 0f, -70f);
float mudRadius = 30f;
Vector <float> costVectorStorage = null; // storage to prevent a ton of vectors from being allocated
// Cost function
float costFunction(Vector <float> from, Vector <float> to)
{
// If the location we are moving to is in the mud, lets adjust the
// cost because mud makes us move slower.
to.Subtract(mudLocation, ref costVectorStorage);
float magnitude = costVectorStorage.Magnitude;
if (magnitude <= mudRadius)
{
return(2f);
}
// neither location is in the mud, it is just a standard movement at normal speed.
return(1f);
}
Vector <float> goalVectorStorage = null; // storage to prevent a ton of vectors from being allocated
// Goal function
bool goalFunction(Vector <float> currentLocation)
{
// if the player is within the enemy's attack range WE FOUND A PATH! :)
currentLocation.Subtract(playerLocation, ref goalVectorStorage);
float magnitude = goalVectorStorage.Magnitude;
if (magnitude <= enemyAttackRange)
{
return(true);
}
// the enemy is not yet within attack range
return(false);
}
// We have all the necessary parameters. Run the pathfinding algorithms!
Stepper <Vector <float> > aStarPath =
Search.Graph(
enemyLocation,
neighborFunction,
heuristicFunction,
costFunction,
goalFunction);
// Flush the already used markers before running the Greedy algorithm.
// Normally you won't run two algorithms for the same graph/location, but
// we are running both algorithms in this example to demonstrate the
// differences between them.
alreadyUsed.Clear();
Stepper <Vector <float> > greedyPath =
Search.Graph(
enemyLocation,
neighborFunction,
heuristicFunction,
goalFunction);
// NOTE: If there is no valid path, then "Search.Graph" will return "null."
// For this example, I know that there will be a valid path so I did not
// include a null check.
// Lets convert the paths into arrays so you can look at them in the debugger. :)
Vector <float>[] aStarPathArray = aStarPath.ToArray();
Vector <float>[] greedyPathArray = greedyPath.ToArray();
// lets calculate the movement cost of each path to see how they compare
float astartTotalCost = Compute.Add <float>(step =>
{
for (int i = 0; i < aStarPathArray.Length - 1; i++)
{
step(costFunction(aStarPathArray[i], aStarPathArray[i + 1]));
}
});
float greedyTotalCost = Compute.Add <float>(step =>
{
for (int i = 0; i < greedyPathArray.Length - 1; i++)
{
step(costFunction(greedyPathArray[i], greedyPathArray[i + 1]));
}
});
// Notice that that the A* algorithm produces a less costly path than the Greedy,
// meaning that it is faster. The Greedy path went through the mud, but the A* path
// took the longer route around the other side of the rock, which ended up being faster
// than running through the mud.
}
#endregion
#region Random Generation
{
Console.WriteLine(" Random Generation---------------------");
Console.WriteLine();
int iterationsperrandom = 3;
void testrandom(Random random)
{
for (int i = 0; i < iterationsperrandom; i++)
{
Console.WriteLine(" " + i + ": " + random.Next());
}
Console.WriteLine();
}
Arbitrary mcg_2pow59_13pow13 = new Arbitrary.Algorithms.MultiplicativeCongruent_A();
Console.WriteLine(" mcg_2pow59_13pow13 randoms:");
testrandom(mcg_2pow59_13pow13);
Arbitrary mcg_2pow31m1_1132489760 = new Arbitrary.Algorithms.MultiplicativeCongruent_B();
Console.WriteLine(" mcg_2pow31m1_1132489760 randoms:");
testrandom(mcg_2pow31m1_1132489760);
Arbitrary mersenneTwister = new Arbitrary.Algorithms.MersenneTwister();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(mersenneTwister);
Arbitrary cmr32_c2_o3 = new Arbitrary.Algorithms.CombinedMultipleRecursive();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(cmr32_c2_o3);
Arbitrary wh1982cmcg = new Arbitrary.Algorithms.WichmannHills1982();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(wh1982cmcg);
Arbitrary wh2006cmcg = new Arbitrary.Algorithms.WichmannHills2006();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(wh2006cmcg);
Arbitrary mwcxorsg = new Arbitrary.Algorithms.MultiplyWithCarryXorshift();
Console.WriteLine(" mwcxorsg randoms:");
testrandom(mwcxorsg);
}
#endregion
Console.WriteLine();
Console.WriteLine("============================================");
Console.WriteLine("Example Complete...");
Console.ReadLine();
}
public static Desnity <T> Add(Desnity <T> a, Desnity <T> b)
{
Density.Units units = a.Units <= b.Units ? a.Units : b.Units;
return(new Desnity <T>(Compute.Add(a[units], b[units]), units));
}
static void Main(string[] args)
{
Console.WriteLine("You are runnning the Mathematics example.");
Console.WriteLine("==========================================");
Console.WriteLine();
#region Basic Operations
Console.WriteLine(" Basics----------------------------------------------");
Console.WriteLine();
// Negation
Console.WriteLine(" Compute<int>.Negate(7): " + Compute <int> .Negate(7));
// Addition
Console.WriteLine(" Compute<double>.Add(7, 7): " + Compute <decimal> .Add(7, 7));
// Subtraction
Console.WriteLine(" Compute<float>.Subtract(14, 7): " + Compute <float> .Subtract(14, 7));
// Multiplication
Console.WriteLine(" Compute<long>.Multiply(7, 7): " + Compute <long> .Multiply(7, 7));
// Division
Console.WriteLine(" Compute<short>.Divide(14, 7): " + Compute <short> .Divide((short)14, (short)7));
// Absolute Value
Console.WriteLine(" Compute<decimal>.AbsoluteValue(-7): " + Compute <double> .AbsoluteValue(-7));
// Clamp
Console.WriteLine(" Compute<Fraction>.Clamp(-123, 7, 14): " + Compute <Fraction> .Clamp(-123, 7, 14));
// Maximum
Console.WriteLine(" Compute<byte>.Maximum(1, 2, 3): " + Compute <byte> .Maximum((Step <byte> step) => { step(1); step(2); step(3); }));
// Minimum
Console.WriteLine(" Compute<Integer>.Minimum(1, 2, 3): " + Compute <Integer> .Minimum((Step <Integer> step) => { step(1); step(2); step(3); }));
// Less Than
Console.WriteLine(" Compute<Fraction128>.LessThan(1, 2): " + Compute <Fraction128> .LessThan(1, 2));
// Greater Than
Console.WriteLine(" Compute<Fraction64>.GreaterThan(1, 2): " + Compute <Fraction64> .GreaterThan(1, 2));
// Compare
Console.WriteLine(" Compute<Fraction32>.Compare(7, 7): " + Compute <Fraction32> .Compare(7, 7));
// Equate
Console.WriteLine(" Compute<int>.Equate(7, 6): " + Compute <int> .Equate(7, 6));
// EqualsLeniency
Console.WriteLine(" Compute<int>.EqualsLeniency(2, 1, 1): " + Compute <int> .Equals(2, 1, 1));
Console.WriteLine();
#endregion
#region Number Theory
Console.WriteLine(" Number Theory--------------------------------------");
Console.WriteLine();
// Prime Checking
int prime_check = random.Next(0, 100000);
Console.WriteLine(" IsPrime(" + prime_check + "): " + Compute <int> .IsPrime(prime_check));
// GCF Checking
int[] gcf = new int[] { random.Next(0, 500) * 2, random.Next(0, 500) * 2, random.Next(0, 500) * 2 };
Console.WriteLine(" GCF(" + gcf[0] + ", " + gcf[1] + ", " + gcf[2] + "): " + Compute <int> .GreatestCommonFactor(gcf.Stepper()));
// LCM Checking
int[] lcm = new int[] { random.Next(0, 500) * 2, random.Next(0, 500) * 2, random.Next(0, 500) * 2 };
Console.WriteLine(" LCM(" + lcm[0] + ", " + lcm[1] + ", " + lcm[2] + "): " + Compute <int> .LeastCommonMultiple(lcm.Stepper()));
Console.WriteLine();
#endregion
#region Range
Console.WriteLine(" Range---------------------------------------------");
Console.WriteLine();
Console.WriteLine(" 1D int");
{
Range <int> range1 = new Range <int>(1, 7);
Console.WriteLine(" range1: " + range1);
Range <int> range2 = new Range <int>(4, 10);
Console.WriteLine(" range2: " + range2);
Range <int>[] range3 = range1 ^ range2;
Console.WriteLine(" range1 ^ range2 (Complement): " + range3[0]);
Range <int>[] range4 = range1 | range2;
Console.WriteLine(" range1 | range2 (Union): " + range4[0]);
Range <int> range5 = range1 & range2;
Console.WriteLine(" range1 & range2 (Intersection): " + range5);
}
Console.WriteLine();
#endregion
#region Angles
Console.WriteLine(" Angles--------------------------------------");
Console.WriteLine();
Angle <double> angle1 = Angle <double> .Factory_Degrees(90d);
Console.WriteLine(" angle1 = " + angle1);
Angle <double> angle2 = Angle <double> .Factory_Turns(0.5d);
Console.WriteLine(" angle2 = " + angle2);
Console.WriteLine(" angle1 + angle2 = " + (angle1 + angle2));
Console.WriteLine(" angle2 - angle1 = " + (angle1 + angle2));
Console.WriteLine(" angle1 * 2 = " + (angle1 * 2));
Console.WriteLine(" angle1 / 2 = " + (angle1 / 2));
Console.WriteLine(" angle1 > angle2 = " + (angle1 > angle2));
Console.WriteLine(" angle1 == angle2 = " + (angle1 == angle2));
Console.WriteLine(" angle1 * 2 == angle2 = " + (angle1 * 2 == angle2));
Console.WriteLine(" angle1 != angle2 = " + (angle1 != angle2));
Console.WriteLine();
// examples of non-doubles
Angle <float> angle10 = Angle <float> .Factory_Degrees(90f);
Angle <Fraction> angle11 = Angle <Fraction> .Factory_Degrees(new Fraction("90/1"));
Angle <decimal> angle12 = Angle <decimal> .Factory_Degrees(90m);
#endregion
#region Fraction
//Console.WriteLine(" Fractions-----------------------------------");
//Console.WriteLine();
//Fraction128 fraction1 = new Fraction128(2.5);
//Console.WriteLine(" fraction1 = " + fraction1);
//Fraction128 fraction2 = new Fraction128(3.75);
//Console.WriteLine(" fraction2 = " + fraction2);
//Console.WriteLine(" fraction1 + fraction2 = " + fraction1 + fraction2);
//Console.WriteLine(" fraction2 - fraction1 = " + fraction1 + fraction2);
//Console.WriteLine(" fraction1 * 2 = " + fraction1 * 2);
//Console.WriteLine(" fraction1 / 2 = " + fraction1 / 2);
//Console.WriteLine(" fraction1 > fraction2 = " + (fraction1 > fraction2));
//Console.WriteLine(" fraction1 == fraction2 = " + (fraction1 == fraction2));
//Console.WriteLine(" fraction1 * 2 == fraction2 = " + (fraction1 * 2 == fraction2));
//Console.WriteLine(" fraction1 != fraction2 = " + (fraction1 != fraction2));
//Console.WriteLine();
#endregion
#region Trigonometry
Console.WriteLine(" Trigonometry -----------------------------------------");
Console.WriteLine();
Angle <double> testingAngle = Angle <double> .Factory_Degrees(90d);
Console.WriteLine(" Sin(90degrees) = " + Compute <double> .Sine(testingAngle));
#endregion
#region Statistics
Console.WriteLine(" Statistics-----------------------------------------");
Console.WriteLine();
// Makin some random data...
double mode_temp = random.NextDouble() * 100;
double[] statistics_data = new double[]
{
random.NextDouble() * 100,
mode_temp,
random.NextDouble() * 100,
random.NextDouble() * 100,
random.NextDouble() * 100,
random.NextDouble() * 100,
mode_temp
};
// Print the data to the console...
Console.WriteLine(" data: [" +
string.Format("{0:0.00}", statistics_data[0]) + ", " +
string.Format("{0:0.00}", statistics_data[1]) + ", " +
string.Format("{0:0.00}", statistics_data[2]) + ", " +
string.Format("{0:0.00}", statistics_data[3]) + ", " +
string.Format("{0:0.00}", statistics_data[4]) + ", " +
string.Format("{0:0.00}", statistics_data[5]) + ", " +
string.Format("{0:0.00}", statistics_data[6]) + "]");
Console.WriteLine();
// Mean
Console.WriteLine(" Mean(data): " + string.Format("{0:0.00}", Compute <double> .Mean(statistics_data.Stepper())));
// Median
Console.WriteLine(" Median(data): " + string.Format("{0:0.00}", Compute <double> .Median(statistics_data.Stepper())));
// Mode
Console.WriteLine(" Mode(data): ");
Heap <Link <double, int> > modes = Compute <double> .Mode(statistics_data.Stepper());
while (modes.Count > 0)
{
Link <double, int> link = modes.Dequeue();
Console.WriteLine(" Point: " + string.Format("{0:0.00}", link._1) + " Occurences: " + link._2);
}
Console.WriteLine();
// Geometric Mean
Console.WriteLine(" Geometric Mean(data): " + string.Format("{0:0.00}", Compute <double> .GeometricMean(statistics_data.Stepper())));
// Range
Range <double> range = Compute <double> .Range(statistics_data.Stepper());
Console.WriteLine(" Range(data): " + string.Format("{0:0.00}", range.Min) + "-" + string.Format("{0:0.00}", range.Max));
// Variance
Console.WriteLine(" Variance(data): " + string.Format("{0:0.00}", Compute <double> .Variance(statistics_data.Stepper())));
// Standard Deviation
Console.WriteLine(" Standard Deviation(data): " + string.Format("{0:0.00}", Compute <double> .StandardDeviation(statistics_data.Stepper())));
// Mean Deviation
Console.WriteLine(" Mean Deviation(data): " + string.Format("{0:0.00}", Compute <double> .MeanDeviation(statistics_data.Stepper())));
Console.WriteLine();
// Quantiles
//double[] quatiles = Statistics<double>.Quantiles(4, statistics_data.Stepper());
//Console.Write(" Quartiles(data):");
//foreach (double i in quatiles)
// Console.Write(string.Format(" {0:0.00}", i));
//Console.WriteLine();
//Console.WriteLine();
#endregion
#region Algebra
Console.WriteLine(" Algebra---------------------------------------------");
Console.WriteLine();
// Prime Factorization
int prime_factors = random.Next(0, 100000);
Console.Write(" Prime Factors(" + prime_factors + "): ");
Compute <int> .FactorPrimes(prime_factors, (int i) => { Console.Write(i + " "); });
Console.WriteLine();
Console.WriteLine();
// Logarithms
int log_1 = random.Next(0, 11), log_2 = random.Next(0, 100000);
Console.WriteLine(" log_" + log_1 + "(" + log_2 + "): " + string.Format("{0:0.00}", Compute <double> .Logarithm((double)log_1, (double)log_2)));
Console.WriteLine();
// Summation
double[] summation_values = new double[]
{
random.NextDouble(),
random.NextDouble(),
random.NextDouble(),
random.NextDouble(),
};
double summation = Compute <double> .Add(summation_values.Stepper());
Console.Write(" Σ (" + string.Format("{0:0.00}", summation_values[0]));
for (int i = 1; i < summation_values.Length; i++)
{
Console.Write(", " + string.Format("{0:0.00}", summation_values[i]));
}
Console.WriteLine(") = " + string.Format("{0:0.00}", summation));
Console.WriteLine();
#endregion
#region Combinatorics
Console.WriteLine(" Combinatorics--------------------------------------");
Console.WriteLine();
// Factorials
Console.WriteLine(" 7!: " + Compute <int> .Factorial(7));
Console.WriteLine();
// Combinations
Console.WriteLine(" 7! / (3! * 4!): " + Compute <int> .Combinations(7, new int[] { 3, 4 }));
Console.WriteLine();
// Choose
Console.WriteLine(" 7 choose 2: " + Compute <int> .Choose(7, 2));
Console.WriteLine();
#endregion
#region Linear Algebra
Console.WriteLine(" Linear Algebra------------------------------------");
Console.WriteLine();
// Vector Construction
Vector <double> V = new double[]
{
random.NextDouble(),
random.NextDouble(),
random.NextDouble(),
random.NextDouble(),
};
Console.WriteLine(" Vector<double> V: ");
ConsoleWrite(V);
Console.WriteLine(" Normalize(V): ");
ConsoleWrite(V.Normalize());
// Vctor Negation
Console.WriteLine(" -V: ");
ConsoleWrite(-V);
// Vector Addition
Console.WriteLine(" V + V (aka 2V): ");
ConsoleWrite(V + V);
// Vector Multiplication
Console.WriteLine(" V * 2: ");
ConsoleWrite(V * 2);
// Vector Division
Console.WriteLine(" V / 2: ");
ConsoleWrite(V / 2);
// Vector Dot Product
Console.WriteLine(" V dot V: " + Vector <double> .DotProduct(V, V));
Console.WriteLine();
// Vector Cross Product
Vector <double> V3 = new double[]
{
random.NextDouble(),
random.NextDouble(),
random.NextDouble(),
};
Console.WriteLine(" Vector<double> V3: ");
ConsoleWrite(V3);
Console.WriteLine(" V3 cross V3: ");
ConsoleWrite(Vector <double> .CrossProduct(V3, V3));
// Matrix Construction
Matrix <double> M = (Matrix <double>) new double[, ]
{
{ random.NextDouble(), random.NextDouble(), random.NextDouble(), random.NextDouble() },
{ random.NextDouble(), random.NextDouble(), random.NextDouble(), random.NextDouble() },
{ random.NextDouble(), random.NextDouble(), random.NextDouble(), random.NextDouble() },
{ random.NextDouble(), random.NextDouble(), random.NextDouble(), random.NextDouble() },
};
Console.WriteLine(" Matrix<double>.Identity(4, 4): ");
ConsoleWrite(Matrix <double> .FactoryIdentity(4, 4));
Console.WriteLine(" Matrix<double> M: ");
ConsoleWrite(M);
// Matrix Negation
Console.WriteLine(" -M: ");
ConsoleWrite(-M);
// Matrix Addition
Console.WriteLine(" M + M (aka 2M): ");
ConsoleWrite(M + M);
// Matrix Subtraction
Console.WriteLine(" M - M: ");
ConsoleWrite(M - M);
// Matrix Multiplication
Console.WriteLine(" M * M (aka M ^ 2): ");
ConsoleWrite(M * M);
// If you have a large matrix that you want to multi-thread the multiplication,
// use the function: "LinearAlgebra.Multiply_parallel". This function will
// automatically parrallel the multiplication to the number of cores on your
// personal computer.
// Matrix Power
Console.WriteLine(" M ^ 3: ");
ConsoleWrite(M ^ 3);
// Matrix Multiplication
Console.WriteLine(" minor(M, 1, 1): ");
ConsoleWrite(M.Minor(1, 1));
// Matrix Reduced Row Echelon
Console.WriteLine(" rref(M): ");
ConsoleWrite(Matrix <double> .ReducedEchelon(M));
// Matrix Determinant
Console.WriteLine(" determinent(M): " + string.Format("{0:0.00}", Matrix <double> .Determinent(M)));
Console.WriteLine();
// Matrix-Vector Multiplication
Console.WriteLine(" M * V: ");
ConsoleWrite(M * V);
// Matrix Lower-Upper Decomposition
Matrix <double> l, u;
Matrix <double> .DecomposeLU(M, out l, out u);
Console.WriteLine(" Lower-Upper Decomposition:");
Console.WriteLine();
Console.WriteLine(" lower(M):");
ConsoleWrite(l);
Console.WriteLine(" upper(M):");
ConsoleWrite(u);
// Quaternion Construction
Quaternion <double> Q = new Quaternion <double>(
random.NextDouble(),
random.NextDouble(),
random.NextDouble(),
1.0d);
Console.WriteLine(" Quaternion<double> Q: ");
ConsoleWrite(Q);
// Quaternion Addition
Console.WriteLine(" Q + Q (aka 2Q):");
ConsoleWrite(Q + Q);
// Quaternion-Vector Rotation
Console.WriteLine(" Q * V3 * Q':");
// Note: the vector should be normalized on the 4th component
// for a proper rotation. (I did not do that)
ConsoleWrite(V3.RotateBy(Q));
#endregion
#region Convex Optimization
//Console.WriteLine(" Convex Optimization-----------------------------------");
//Console.WriteLine();
//double[,] tableau = new double[,]
//{
// { 0.0, -0.5, -3.0, -1.0, -4.0, },
// { 40.0, 1.0, 1.0, 1.0, 1.0, },
// { 10.0, -2.0, -1.0, 1.0, 1.0, },
// { 10.0, 0.0, 1.0, 0.0, -1.0, },
//};
//Console.WriteLine(" tableau (double): ");
//ConsoleWrite(tableau); Console.WriteLine();
//Vector<double> simplex_result = LinearAlgebra.Simplex(ref tableau);
//Console.WriteLine(" simplex(tableau): ");
//ConsoleWrite(tableau); Console.WriteLine();
//Console.WriteLine(" resulting maximization: ");
//ConsoleWrite(simplex_result);
#endregion
#region Symbolics
Console.WriteLine(" Symbolics---------------------------------------");
Console.WriteLine();
Expression <Func <double, double> > expression1 = (x) => 2 * (x / 7);
var syntax1 = Symbolics <double> .Parse(expression1);
Console.WriteLine(" Expression 1: " + syntax1);
Console.WriteLine(" Simplified: " + syntax1.Simplify());
Console.WriteLine(" Plugin(5): " + syntax1.Assign("x", 5).Simplify());
Expression <Func <double, double> > expression2 = (x) => 2 * x / 7;
var syntax2 = Symbolics <double> .Parse(expression2);
Console.WriteLine(" Expression 2: " + syntax2);
Console.WriteLine(" Simplified: " + syntax2.Simplify());
Console.WriteLine(" Plugin(5): " + syntax2.Assign("x", 5).Simplify());
Expression <Func <double, double> > expression3 = (x) => 2 - x + 7;
var syntax3 = Symbolics <double> .Parse(expression3);
Console.WriteLine(" Expression 3: " + syntax3);
Console.WriteLine(" Simplified: " + syntax3.Simplify());
Console.WriteLine(" Plugin(5): " + syntax3.Assign("x", 5).Simplify());
Expression <Func <double, double> > expression4 = (x) => 2 + (x - 7);
var syntax4 = Symbolics <double> .Parse(expression4);
Console.WriteLine(" Expression 4: " + syntax4);
Console.WriteLine(" Simplified: " + syntax4.Simplify());
Console.WriteLine(" Plugin(5): " + syntax4.Assign("x", 5).Simplify());
Expression <Func <double, double, double, double> > expression5 = (x, y, z) => Compute <double> .Power(x, 3) + 2 * x * y * Compute <double> .Power(z, 2) - y * z + 1;
var syntax5 = Symbolics <double> .Parse(expression5);
Console.WriteLine(" Expression 5: " + syntax5);
Console.WriteLine(" Simplified: " + syntax5.Simplify());
Console.WriteLine(" Plugin(x = 5): " + syntax5.Assign("x", 5).Simplify());
#endregion
var test = Symbolics <double> .Parse("less(5, 10)");
Console.WriteLine();
Console.WriteLine(" Parse (string) Test: " + test);
var test2 = Symbolics <double> .Parse("less(add(5, 10), 10)");
Console.WriteLine();
Console.WriteLine(" Parse (string) Test: " + test2);
Console.WriteLine(" Parse (string) Test Simplify: " + test2.Simplify());
Console.WriteLine();
Console.WriteLine("=================================================");
Console.WriteLine("Example Complete...");
Console.ReadLine();
}
public static Angle <T> operator +(Angle <T> left, Angle <T> right)
{
return(new Angle <T>(Compute <T> .Add(left._radians, right._radians)));
}
private static bool XenoDetection(
XenoScan <T> a,
XenoScan <T> b,
int maxIterations,
out Vector <T> point,
out Vector <T> normal,
out T penetration)
{
point = null;
normal = null;
penetration = default(T);
Vector <T> minkowskiDifference = b.Position - a.Position;
normal = -minkowskiDifference;
if (NearlyZero(minkowskiDifference.MagnitudeSquared))
{
minkowskiDifference = new Vector <T>(
Compute.Divide(Constant <T> .One, Compute.FromInt32 <T>(100000)),
Constant <T> .Zero,
Constant <T> .Zero);
}
Vector <T> a_xenoScan1 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, minkowskiDifference)));
Vector <T> b_xenoScan1 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans1_subtract = b_xenoScan1 - a_xenoScan1;
if (Compute.LessThanOrEqual(Vector <T> .DotProduct(xenoScans1_subtract, normal), Constant <T> .Zero))
{
return(false);
}
Vector <T> crossOfXenoAndMD = Vector <T> .CrossProduct(xenoScans1_subtract, minkowskiDifference);
if (NearlyZero(crossOfXenoAndMD.MagnitudeSquared))
{
crossOfXenoAndMD = (xenoScans1_subtract - minkowskiDifference).Normalize();
point = (a_xenoScan1 + b_xenoScan1) * Compute.Divide(Constant <T> .One, Constant <T> .Two);
penetration = Vector <T> .DotProduct(xenoScans1_subtract, crossOfXenoAndMD);
return(true);
}
Vector <T> a_xenoScan2 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, -crossOfXenoAndMD)));
Vector <T> b_xenoScan2 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans2_subtract = b_xenoScan2 - a_xenoScan2;
if (Compute.LessThanOrEqual(Vector <T> .DotProduct(xenoScans2_subtract, normal), Constant <T> .Zero))
{
return(false);
}
Vector <T> crossOfXenoScans = Vector <T> .CrossProduct(xenoScans1_subtract - minkowskiDifference, xenoScans2_subtract - minkowskiDifference);
T distance = Vector <T> .DotProduct(crossOfXenoAndMD, minkowskiDifference);
if (Compute.LessThan(distance, Constant <T> .Zero))
{
Vector <T> temp;
temp = xenoScans1_subtract;
xenoScans1_subtract = xenoScans2_subtract;
xenoScans2_subtract = temp;
temp = a_xenoScan1;
a_xenoScan1 = a_xenoScan2;
a_xenoScan2 = temp;
temp = b_xenoScan1;
b_xenoScan1 = b_xenoScan2;
b_xenoScan2 = temp;
normal = -normal;
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
while (true)
{
if (phase1 > maxIterations)
{
return(false);
}
phase1++;
Vector <T> neg_normal = -normal;
Vector <T> a_xenoScan3 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, neg_normal)));
Vector <T> b_xenoScan3 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans3_subtract = b_xenoScan3 - a_xenoScan3;
if (Compute.LessThan(Vector <T> .DotProduct(xenoScans3_subtract, normal), Constant <T> .Zero))
{
return(false);
}
if (Compute.LessThan(Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans1_subtract, xenoScans3_subtract), minkowskiDifference), Constant <T> .Zero))
{
xenoScans2_subtract = xenoScans3_subtract;
a_xenoScan2 = a_xenoScan3;
b_xenoScan2 = b_xenoScan3;
normal = Vector <T> .CrossProduct(xenoScans1_subtract - minkowskiDifference, xenoScans3_subtract - minkowskiDifference);
continue;
}
if (Compute.LessThan(Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans3_subtract, xenoScans2_subtract), minkowskiDifference), Constant <T> .Zero))
{
xenoScans1_subtract = xenoScans3_subtract;
a_xenoScan1 = a_xenoScan3;
b_xenoScan1 = b_xenoScan3;
normal = Vector <T> .CrossProduct(xenoScans3_subtract - minkowskiDifference, xenoScans2_subtract - minkowskiDifference);
continue;
}
while (true)
{
phase2++;
normal = Vector <T> .CrossProduct(xenoScans2_subtract - xenoScans1_subtract, xenoScans3_subtract - xenoScans1_subtract);
// Ommited because appears to be an error
//if (NearlyZero(normal.Magnitude()))
// return true;
normal = normal.Normalize();
if (!hit && Compute.GreaterThanOrEqual(Vector <T> .DotProduct(normal, xenoScans1_subtract), Constant <T> .Zero))
{
hit = true;
}
neg_normal = -normal;
Vector <T> a_xenoScan4 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, neg_normal)));
Vector <T> b_xenoScan4 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans4_subtract = b_xenoScan4 - a_xenoScan4;
T delta = Vector <T> .DotProduct(xenoScans4_subtract - xenoScans3_subtract, normal);
penetration = Vector <T> .DotProduct(xenoScans4_subtract, normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (Compute.LessThanOrEqual(delta, CollideEpsilon) || Compute.LessThanOrEqual(penetration, Constant <T> .Zero) || phase2 > maxIterations)
{
if (hit)
{
T b0 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans1_subtract, xenoScans2_subtract), xenoScans3_subtract);
T b1 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans3_subtract, xenoScans2_subtract), minkowskiDifference);
T b2 = Vector <T> .DotProduct(Vector <T> .CrossProduct(minkowskiDifference, xenoScans1_subtract), xenoScans3_subtract);
T b3 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans2_subtract, xenoScans1_subtract), minkowskiDifference);
T sum = Compute.Add(b0, b1, b2, b3);
if (Compute.LessThanOrEqual(sum, Constant <T> .Zero))
{
b0 = Constant <T> .Zero;
b1 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans2_subtract, xenoScans3_subtract), normal);
b2 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans3_subtract, xenoScans1_subtract), normal);
b3 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans1_subtract, xenoScans2_subtract), normal);
sum = Compute.Add(b0, b1, b2, b3);
}
T inv = Compute.Divide(Constant <T> .One, sum);
point =
(a.Position * b0 +
a_xenoScan1 * b1 +
a_xenoScan2 * b2 +
a_xenoScan3 * b3 +
b.Position * b0 +
b_xenoScan1 * b1 +
b_xenoScan2 * b2 +
b_xenoScan3 * b3)
* inv;
}
return(hit);
}
Vector <T> xeno4CrossMD = Vector <T> .CrossProduct(xenoScans4_subtract, minkowskiDifference);
T dot = Vector <T> .DotProduct(xeno4CrossMD, xenoScans1_subtract);
if (Compute.GreaterThanOrEqual(dot, Constant <T> .Zero))
{
dot = Vector <T> .DotProduct(xeno4CrossMD, xenoScans2_subtract);
if (Compute.GreaterThanOrEqual(dot, Constant <T> .Zero))
{
xenoScans1_subtract = xenoScans4_subtract;
a_xenoScan1 = a_xenoScan4;
b_xenoScan1 = b_xenoScan4;
}
else
{
xenoScans3_subtract = xenoScans4_subtract;
a_xenoScan3 = a_xenoScan4;
b_xenoScan3 = b_xenoScan4;
}
}
else
{
dot = Vector <T> .DotProduct(xeno4CrossMD, xenoScans3_subtract);
if (Compute.GreaterThanOrEqual(dot, Constant <T> .Zero))
{
xenoScans2_subtract = xenoScans4_subtract;
a_xenoScan2 = a_xenoScan4;
b_xenoScan2 = b_xenoScan4;
}
else
{
xenoScans1_subtract = xenoScans4_subtract;
a_xenoScan1 = a_xenoScan4;
b_xenoScan1 = b_xenoScan4;
}
}
}
}
}
static void Main(string[] args)
{
Console.WriteLine("You are runnning the Algorithms tutorial.");
Console.WriteLine("======================================================");
Console.WriteLine();
#region Sorting
{
Console.WriteLine(" Sorting Algorithms----------------------");
Console.WriteLine();
int[] dataSet = new int[] { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
Console.Write(" Data Set:");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
// if you want to sort non-array types, see the overloads using Get<int> and Assign<int>
// Delegates
//Get<int> get = (int index) => { return dataSet[index]; };
//Assign<int> assign = (int index, int value) => { dataSet[index] = value; };
// Shuffling (Randomizing)
Sort <int> .Shuffle(dataSet);
Console.Write(" Shuffle (Randomizing): ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
// Bubble
Sort <int> .Bubble(dataSet);
Console.Write(" Bubble: ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort <int> .Shuffle(dataSet);
// Selection
Sort <int> .Selection(dataSet);
Console.Write(" Selection: ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort <int> .Shuffle(dataSet);
// Insertion
Sort <int> .Insertion(dataSet);
Console.Write(" Insertion: ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort <int> .Shuffle(dataSet);
// Quick
Sort <int> .Quick(dataSet);
Console.Write(" Quick: ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort <int> .Shuffle(dataSet);
// Merge
Sort <int> .Merge(Compute.Compare, dataSet);
Console.Write(" Merge: ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort <int> .Shuffle(dataSet);
// Heap
Sort <int> .Heap(Compute.Compare, dataSet);
Console.Write(" Heap: ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
Console.WriteLine(" shuffling dataSet...");
Sort <int> .Shuffle(dataSet);
// OddEven
Sort <int> .OddEven(Compute.Compare, dataSet);
Console.Write(" OddEven: ");
Console.Write(dataSet[0]);
for (int i = 1; i < dataSet.Length; i++)
{
Console.Write(", " + dataSet[i]);
}
Console.WriteLine();
//Sort<int>.Shuffle(get, set, 0, dataSet.Length);
//// Slow
//Sort<int>.Slow(Logic.compare, get, set, 0, dataSet.Length);
//Console.Write("Slow: ");
//Console.Write(dataSet[0]);
//for (int i = 1; i < dataSet.Length; i++)
// Console.Write(", " + dataSet[i]);
//Console.WriteLine();
Sort <int> .Shuffle(dataSet);
// Bogo
//Sort<int>.Bogo(Logic.compare, get, set, 0, dataSet.Length);
Console.Write(" Bogo: Disabled (takes forever)");
//Console.Write(dataSet[0]);
//for (int i = 1; i < dataSet.Length; i++)
// Console.Write(", " + dataSet[i]);
//Console.WriteLine();
Console.WriteLine();
Console.WriteLine();
}
#endregion
#region Graph Search
{
Console.WriteLine(" Graph Searching----------------------");
Console.WriteLine();
// make a graph
Graph <int> graph = new GraphSetOmnitree <int>(
Compare.Default,
Hash.Default);
// add nodes
graph.Add(0);
graph.Add(1);
graph.Add(2);
graph.Add(3);
// add edges
graph.Add(0, 1);
graph.Add(0, 2);
graph.Add(1, 3);
graph.Add(2, 3);
//// represent a graph
//// Note: can be any type (doesn't have to be int?[,])
//int?[,] adjacencyMatrix =
//{
// { null, 1, 2, null },
// { null, null, null, 5 },
// { null, null, null, 1 },
// { null, null, null, null }
//};
// make a delegate for finding neighbor nodes
Action <int, Step <int> > neighbors =
(int current, Step <int> step_function) =>
{
//for (int i = 0; i < 4; i++)
// if (adjacencyMatrix[current, i] != null)
// step(i);
graph.Neighbors(current, step_function);
};
// make a delegate for computing heuristics
Func <int, int> heuristic =
(int node) =>
{
switch (node)
{
case 0:
return(3);
case 1:
return(6);
case 2:
return(1);
case 3:
return(0);
default:
throw new NotImplementedException();
}
};
// make a delegate for computing costs
Func <int, int, int> cost =
(int from, int to) =>
{
if (from == 0 && to == 1)
{
return(1);
}
if (from == 0 && to == 2)
{
return(2);
}
if (from == 1 && to == 3)
{
return(5);
}
if (from == 2 && to == 3)
{
return(1);
}
if (from == 0 && to == 3)
{
return(99);
}
throw new Exception("invalid path cost computation");
};
// make a delegate for determining if the goal is reached
Func <int, bool> goal =
(int node) =>
{
if (node == 3)
{
return(true);
}
else
{
return(false);
}
};
// run A* the algorithm
Stepper <int> aStar_path = Search <int> .Graph <int> .Astar(
0,
graph,
new Search <int> .Graph <int> .Heuristic(heuristic),
new Search <int> .Graph <int> .Cost(cost),
new Search <int> .Graph <int> .Goal(goal));
// run the Greedy algorithm
Stepper <int> greedy_path = Search <int> .Graph <int> .Greedy(
0,
graph,
new Search <int> .Graph <int> .Heuristic(heuristic),
new Search <int> .Graph <int> .Goal(goal));
Console.Write(" A* Path: ");
if (aStar_path != null)
{
aStar_path((int i) => { System.Console.Write(i + " "); });
}
else
{
Console.Write(" none");
}
Console.WriteLine();
Console.Write(" Greedy Path: ");
if (greedy_path != null)
{
greedy_path((int i) => { System.Console.Write(i + " "); });
}
else
{
Console.Write(" none");
}
Console.WriteLine();
Console.WriteLine();
}
#endregion
#region Graph Search (Vector Game-Style Example)
// Lets say you are coding enemy AI and you want the AI to find a path towards the player
// in order to attack them. Here are their starting positions:
Vector <float> enemy_location = new Vector <float>(-100, 0, -50);
Vector <float> player_location = new Vector <float>(200, 0, -50);
float enemy_attack_range = 3; // enemy has a melee attack with 3 range
// Lets say most of the terrain is open, but there is a big rock in between them that they
// must go around.
Vector <float> rock_location = new Vector <float>(15, 0, -40);
float rock_radius = 20;
// So, we just need to validate movement locations (make sure the path finding algorithm
// ignores locations inside the rock)
Func <Vector <float>, bool> validateMovementLocation = location =>
{
float mag = (location - rock_location).Magnitude;
if (mag <= rock_radius)
{
return(false); // inside rock (not valid)
}
return(true); // not inside rock (valid)
// NOTE:
// This function will normally be handled by your physics engine if you are running one.
};
// Make sure we don't re-use locations (must be wiped after running the algorithm)
Set <Vector <float> > already_used = new SetHashList <Vector <float> >();
// Now we need the neighbor function (getting the neighbors of the current location).
Search <Vector <float> > .Graph <float> .Neighbors neighborFunction = (currentLocation, neighbors) =>
{
// lets make a simple neighbor function that returns 4 locations (directly north, south, east, and west)
// and the distance of each node in the graph will be 1
Vector <float>
north = new Vector <float>(currentLocation.X + 1, currentLocation.Y, currentLocation.Z),
east = new Vector <float>(currentLocation.X, currentLocation.Y, currentLocation.Z + 1),
south = new Vector <float>(currentLocation.X - 1, currentLocation.Y, currentLocation.Z),
west = new Vector <float>(currentLocation.X, currentLocation.Y, currentLocation.Z - 1);
// validate the locations (not inside the rock) and make sure we have not already traversed the location
if (validateMovementLocation(north) && !already_used.Contains(north))
{
already_used.Add(north); // mark for usage so we do not use this location again
neighbors(north);
}
if (validateMovementLocation(east) && !already_used.Contains(east))
{
already_used.Add(east); // mark for usage so we do not use this location again
neighbors(east);
}
if (validateMovementLocation(south) && !already_used.Contains(south))
{
already_used.Add(south); // mark for usage so we do not use this location again
neighbors(south);
}
if (validateMovementLocation(west) && !already_used.Contains(west))
{
already_used.Add(west); // mark for usage so we do not use this location again
neighbors(west);
}
// NOTES:
// - This neighbor function has a 90 degree per-node resolution (360 / 4 [north/south/east/west] = 90).
// - This neighbor function has a 1 unit per-node resolution, because we went 1 unit in each direction.
// RECOMMENDATIONS:
// - If the path finding is failing, you may need to increase the resolution.
// - If the algorithm is running too slow, you may need to reduce the resolution.
};
// Now we need the heuristic function (how close are we to the goal).
Search <Vector <float> > .Graph <float> .Heuristic heuristicFunction = currentLocation =>
{
// The goal is the player's location, so we just need our distance from the player.
return((currentLocation - player_location).Magnitude);
};
// Lets say there is a lot of mud around the rock, and the mud makes our player move at half their normal speed.
// Our path finding needs to find the fastest route to the player, whether it be through the mud or not.
Vector <float> mud_location = new Vector <float>(15, 0, -70);
float mud_radius = 30;
// Now we need the cost function
Search <Vector <float> > .Graph <float> .Cost costFunction = (location1, location2) =>
{
// If either locations are in the mud, lets increase the cost of moving to that spot.
float mag1 = (location1 - mud_location).Magnitude;
if (mag1 <= mud_radius)
{
return(2);
}
float mag2 = (location2 - mud_location).Magnitude;
if (mag2 <= mud_radius)
{
return(2);
}
// neither location is in the mud, it is just a standard movement at normal speed.
return(1);
};
// Now we need a goal function
Search <Vector <float> > .Graph <float> .Goal goalFunction = currentLocation =>
{
float mag = (currentLocation - player_location).Magnitude;
// if the player is within the enemy's attack range WE FOUND A PATH! :)
if (mag <= enemy_attack_range)
{
return(true);
}
// the enemy is not yet within attack range
return(false);
};
// We have all the necessary parameters. Run the pathfinding algorithms!
Stepper <Vector <float> > aStarPath = Search <Vector <float> > .Graph <float> .Astar(
enemy_location,
neighborFunction,
heuristicFunction,
costFunction,
goalFunction);
// NOTE:
// if the "Astar" function returns "null" there is no valid path. (in this example there
// are valid paths, so I didn't add a nul check)
// Here is the path converted to an array (easier to read while debugging)
Vector <float>[] aStarPath_array = aStarPath.ToArray();
// flush the duplicate locations checker before running the Greedy algorithm
already_used.Clear();
Stepper <Vector <float> > greedyPath = Search <Vector <float> > .Graph <float> .Greedy(
enemy_location,
neighborFunction,
heuristicFunction,
goalFunction);
// Here is the path converted to an array (easier to read while debugging)
Vector <float>[] greedyPath_array = greedyPath.ToArray();
// lets calculate the movement cost of each path
float total_cost_astar = Compute.Add <float>(step =>
{
for (int i = 0; i < aStarPath_array.Length - 1; i++)
{
float cost = costFunction(aStarPath_array[i], aStarPath_array[i + 1]);
step(cost);
}
});
float total_cost_greedy = Compute.Add <float>(step =>
{
for (int i = 0; i < greedyPath_array.Length - 1; i++)
{
float cost = costFunction(greedyPath_array[i], greedyPath_array[i + 1]);
step(cost);
}
});
// Notice that that the A* algorithm produces a less costly path than the Greedy,
// meaning that it is faster. The Greedy path went through the mud, but the A* path
// took the longer route around the other side of the rock, which ended up being faster
// than running through the mud.
#endregion
#region Random Generation
Console.WriteLine(" Random Generation---------------------");
Console.WriteLine();
int iterationsperrandom = 3;
Action <Random> testrandom = (Random random) =>
{
for (int i = 0; i < iterationsperrandom; i++)
{
Console.WriteLine(" " + i + ": " + random.Next());
}
Console.WriteLine();
};
Arbitrary mcg_2pow59_13pow13 = new Arbitrary.Algorithms.MultiplicativeCongruent_A();
Console.WriteLine(" mcg_2pow59_13pow13 randoms:");
testrandom(mcg_2pow59_13pow13);
Arbitrary mcg_2pow31m1_1132489760 = new Arbitrary.Algorithms.MultiplicativeCongruent_B();
Console.WriteLine(" mcg_2pow31m1_1132489760 randoms:");
testrandom(mcg_2pow31m1_1132489760);
Arbitrary mersenneTwister = new Arbitrary.Algorithms.MersenneTwister();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(mersenneTwister);
Arbitrary cmr32_c2_o3 = new Arbitrary.Algorithms.CombinedMultipleRecursive();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(cmr32_c2_o3);
Arbitrary wh1982cmcg = new Arbitrary.Algorithms.WichmannHills1982();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(wh1982cmcg);
Arbitrary wh2006cmcg = new Arbitrary.Algorithms.WichmannHills2006();
Console.WriteLine(" mersenneTwister randoms:");
testrandom(wh2006cmcg);
Arbitrary mwcxorsg = new Arbitrary.Algorithms.MultiplyWithCarryXorshift();
Console.WriteLine(" mwcxorsg randoms:");
testrandom(mwcxorsg);
#endregion
Console.WriteLine();
Console.WriteLine("============================================");
Console.WriteLine("Example Complete...");
Console.ReadLine();
}