/// <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(); }