Navigation Script 5/5 (1)

posted in: Control, Navigation, Operations, Scripts | 2

I know that with the implementation of the GPS function in the game, this script might seem useless. However, it does offer something that the in-game GPS does not: in conjunction with the autopilot script, it will actually take you to your desired coordinates.


// Naming a block with "Pos:" at the end will display 
// your current position 
 
// To enable navigation, you need to create three blocks. 
// The names, and reason for having them is below: 
// "Ship Radix", "Ship XAxis" and "Ship YAxis"  
// 
// They should be positioned as named with Radix in the corner,   
// XAXis to the right of Radix, and YAxis in front of Radix. 
// 
// All this orientation, is in relation to your cockpit/control 
// station. Example: 
// 
// Assume your ship is a flat 5x5 2 dimensional plane. Viewed 
// from above: 
//  
// C = your cockpit (facing the direction of the arrow; "up" in  
//     this diagram 
// R = the block named "Ship Radix" 
// X = the block named "Ship XAxis" 
// Y = the block named "Ship YAxis" 
//               ^ 
//               | 
//     +---+---+---+---+---+ 
//     | Y |   | C |   |   | 
//     +---+---+---+---+---+ 
//     |   |   |   |   |   | 
//     +---+---+---+---+---+ 
//     |   |   |   |   |   | 
//     +---+---+---+---+---+ 
//     |   |   |   |   |   | 
//     +---+---+---+---+---+ 
//     | R |   |   |   | X | 
//     +---+---+---+---+---+ 
// 
// What these blocks do is tell the script the orientation of  
// the ship. If you draw a line from "R" to "Y", that defines  
// "forward". If you draw a line from "R" to "X", that defines 
// "right". With this information, the script can figure out 
// which direction the "front" of your ship is pointing. 
//  
// With that information, and the information of where you 
// want to go, the script can tell you which direction you 
// need to turn so that you are pointing in the correct 
// direction to get to your destination. 
// 
// Without these three blocks it is impossible for the script 
// to determine which direction you are pointing, so navigation 
// will not work. 
// 
// To receive directions to your destination, you have to 
// create a block with the name "NAV:" at the end. If this 
// block is found by the script, it will automatically change 
// the name of the block and include directions that you 
// should follow to reach your destination. 
// 
// As you pitch/yaw/roll your ship, these instructions  
// update to show you the new vector to your destination. 
// 
// All this talk about navigation and destination... but 
// how do I actually tell the ship where to go? 
// 
// Create another block named: 
// 
//     "Target: X: Y: Z:" 
// 
// For example: 
//     You can have a beacon named: "Target X:100 Y:19 Z:131" 
//     You can have a gyro named: "Target X:-237 Y:1131 Z:622" 
// 
// Any block that allows you to change it's name can be used 
// (the same is true of all the other blocks discussed above). 
// 
// I tend to choose blocks that are not near one another to  
// avoid having the information displayed by the script falling 
// on top of each other. And I also like to use beacons, because 
// they automatically show up on the HUD. If you use any other  
// kind of block, make sure you set "Show on HUD" for the block 
// so you can see the information (if you want). 
   
void Main()    
{    
	try {
		// Define all the block lists we are going to need 
		List navigation = new List();    
		List shipOrientation = new List();     
		List gyros = new List();     
		List work = new List();     

		// Populate the navigation list with the "Pos:" and "NAV:" 
		// blocks 
		try {
			GridTerminalSystem.SearchBlocksOfName("Pos:" , work);   
			navigation.AddRange(work);    
			GridTerminalSystem.SearchBlocksOfName("NAV:" , work);   
			navigation.AddRange(work);    
		}
		catch (Exception ex) 
		{
			throw new Exception("Couldn't find Pos and/or NAV blocks");
		}

		try {
			// Populate the shipOrientation list with the blocks needed 
			// to determine ship orientation and the "Target:" block 
			GridTerminalSystem.SearchBlocksOfName("Ship Radix" , work);
			shipOrientation.AddRange(work);   
			GridTerminalSystem.SearchBlocksOfName("Ship XAxis" , work);   
			shipOrientation.AddRange(work);    
			GridTerminalSystem.SearchBlocksOfName("Ship YAxis" , work);   
			shipOrientation.AddRange(work);    
		}
		catch (Exception ex) 
		{
			throw new Exception("Couldn't find one or more ship orientation blocks");
		}
		
		try {
			GridTerminalSystem.SearchBlocksOfName("Target:" , work);     
			shipOrientation.AddRange(work);    
		}
		catch (Exception ex) 
		{
			throw new Exception("Couldn't find Target block");
		}
	  
		try {
			// Populate the gyros list with all blocks named "Control Gyro" 
			GridTerminalSystem.SearchBlocksOfName("Control Gyro" , gyros);     
		}
		catch (Exception ex) 
		{
			throw new Exception("Couldn't find any Control Gyro blocks");
		}
		  
		// The three arrays below are going to hold the coordinates 
		// for the three blocks used for ship orientation (Radix, XAxis  
		// and YAxis)	 
		double[] radixPosition = new double[3]; 
		double[] xAxisPosition = new double[3]; 
		double[] yAxisPosition = new double[3]; 
		 
		// The array below will store the coordinates of our target 
		// location extracted from the block named "Target:..." 
		double[] targetPosition = new double[3]; 
		 
		// Unlike the arrays above, this one has 4 elements. 
		// It is used to store the distance to the target, the 
		// ship pitch adjustment needed to point in the  
		// right direction, the ship yaw adjustment needed to 
		// point in the right direction and an indicator as 
		// to whether the current orientation of the ship puts 
		// us in a position where the target is behind us. 
		double[] targetData = new double[4];   
	   
		// The booleans below are used to indicate whether we found 
		// the blocks that we need for this thing to work 
		bool foundRadix = false; 
		bool foundXAxis = false; 
		bool foundYAxis = false; 
		 
		// This last boolean is used to indicate whether there is 
		// a "Target:" block on the ship so we know whether a  
		// destination has been entered 
		bool targetCheck = false;   
		String navigationBlockName = "NAV:"; 
		String targetBlockName = "Target:";   
		String navigationBlockBaseName = "NAV:"; 
		String targetBlockBaseName = "Target:";   
	   
		// Loop through the list of shipOrientation blocks to load 
		// their positions, if they are found
		for (int shipOrientationIndex=0; shipOrientationIndex < shipOrientation.Count; shipOrientationIndex++)   
		{   
			// If this block is the "Ship Radix" block
			if (shipOrientation[shipOrientationIndex].CustomName.Contains("Ship Radix"))    
			{    
				// We set this flag to know that we found the Radix block 
				foundRadix = true; 
	 
				// Load the position of the found block in the radixPosition array			 
				radixPosition = loadPosition(shipOrientation[shipOrientationIndex] as IMyFunctionalBlock); 
			}   
	   
			// If this block is the "Ship XAxis" block
			if (shipOrientation[shipOrientationIndex].CustomName.Contains("Ship XAxis"))    
			{    
				// We set this flag to know that we found the XAxis block 
				foundXAxis = true;   

				// Load the position of the found block in the xAxisPosition array			 
				xAxisPosition = loadPosition(shipOrientation[shipOrientationIndex] as IMyFunctionalBlock); 
			}   
	   
			// If this block is the "Ship YAxis" block
			if (shipOrientation[shipOrientationIndex].CustomName.Contains("Ship YAxis"))    
			{    
				// We set this flag to know that we found the YAxis block 
				foundYAxis = true; 
				
				// Load the position of the found block in the YAxisPosition array			 
				yAxisPosition = loadPosition(shipOrientation[shipOrientationIndex] as IMyFunctionalBlock); 
			}   
	   
			// If this block has the "targetBlockBaseName" string as part of it's name
			// and also has "X:", "Y:" and "Z:" as part of it's name, then we use the
			// information from this block for our destination		
			if (shipOrientation[shipOrientationIndex].CustomName.Contains(targetBlockBaseName) &&  
				shipOrientation[shipOrientationIndex].CustomName.Contains("X:") &&  
				shipOrientation[shipOrientationIndex].CustomName.Contains("Y:") &&  
				shipOrientation[shipOrientationIndex].CustomName.Contains("Z:"))   
			{    
				// We set this flag to know that we found the block with the target
				// information
				targetCheck = true;   
				
				// Extract the X/Y/Z coordinates from the name of the target block
				targetPosition = extractTargetPosition(shipOrientation[shipOrientationIndex] as IMyFunctionalBlock); 
			}   
	   
			// If we found the three blocks necessary for triangulation of the ship's
			// orientation
			if (foundRadix && foundXAxis && foundYAxis)   
			{   
				// If we found a target block, then we can do the navigation
				// calculations
				if(targetCheck)   
				{   
					// Calculate the navigation data and store it in the 
					// targetData array
					targetData = calculateNavigationData(radixPosition, xAxisPosition, yAxisPosition, targetPosition);   
					
					// Set adjustment to 0. If our destination is behind us, we
					// are going to add 180 degrees to our pitch adjustment, so 
					// the pilot knows the target is behind him
					int adjust = 0; 
					
					// targetData[3] will equal 0, if our target is behind us
					if (targetData[3] == 0)  
					{ 
						// Set adjustment to 180
						adjust = 180; 
					}  

					// Copy targetData[0] (distance to target) to our workDist 
					// variable
					double workDist = targetData[0];
					// Initially, we are going to show distance in meters
					String unit = "m";
					
					// If distance is greater that 9999
					if (workDist > 9999)
					{
					   // Convert distance to target from meters to kilometers
					   workDist = workDist / 1000;
					   // Set unit to "km"
					   unit = "km";
					}
					
					// Set our distanceString to the rounded value of our
					// distance to target
					String distanceString = "" + Math.Round(workDist,0); 

					// Set our pitchAdjustmentString to our calculated pitch adjustment
					// plus our adjustment factor (for if the target is behind us)
					String pitchAdjustmentString = "" + Math.Abs(Math.Round(targetData[1],0) + adjust); 

					// Set our yawAdjustmentString to our calculated yaw adjustment
					String yawAdjustmentString = "" + Math.Abs(Math.Round(targetData[2],0));
					
					// If the calculated pitch adjustment is positive
					if (targetData[1] > 0)   
					{     
						// Add "Up " to the pitchAdjustmentString
						pitchAdjustmentString = "Up " + pitchAdjustmentString; 
					}   
					else    
					{     
						// Add "Down " to the pitchAdjustmentString
						pitchAdjustmentString = "Down " + pitchAdjustmentString;    
					}   

					// If the calculated yaw adjustment is positive
					if (targetData[2] > 0)   
					{   
						// Add "Right " to the yawAdjustmentString
						yawAdjustmentString = "Right " + yawAdjustmentString;   
					}   
					else    
					{   
						// Add "Left " to the yawAdjustmentString
						yawAdjustmentString = "Left " + yawAdjustmentString;   
					}       

					// Set the name that we are going to use for the navigation block
					// (the one that has "NAV:" in the name)
					navigationBlockName = navigationBlockBaseName + distanceString + unit 
										  + "\r\nPitch: " + pitchAdjustmentString 
										  + "\r\nYaw: " + yawAdjustmentString + "\r\n\r\n";   
				}   
				else   
				{   
					// If there was no target information, change the name we are
					// going to use on the navigation block to tell the pilot that
					// a target is not set.
					navigationBlockName = navigationBlockBaseName + " No Target Data";   
				}   
			}   
			else   
			{   
				// If all of the ship orientation blocks (Radix, XAxis and YAxis) were
				// not found, then we can't do navigation. So set the navigation block
				// name to a message telling the pilot how many ship orientation blocks
				// are missing
				navigationBlockName = navigationBlockBaseName + " " + (4 - shipOrientation.Count) + " Ship Orientation Blocks Missing";   
			}   
		}   
	   
		// Loop through the blocks in the navigation list
		for (int i = 0; i < navigation.Count; i++)    
		{    
	   
			// If the block is the "Pos:" block   
			if (navigation[i].CustomName.Contains("Pos:"))    
			{    
				try {
					// Find the "Pos:" string in the block name
					int baseNameIndex = navigation[i].CustomName.IndexOf("Pos:");  
					
					// Strip everything from the "Pos:" forward (including the
					// "Pos:") out of the block name. What remains is the base
					// name of the block which we want to preserve.
					string baseName = navigation[i].CustomName.Remove(baseNameIndex);   
		   
					// Get the coordinates of the "Pos:" block 
					string x = " " + Math.Round(navigation[i].GetPosition().GetDim(0),0);   
					string y = " " + Math.Round(navigation[i].GetPosition().GetDim(1),0);   
					string z = " " + Math.Round(navigation[i].GetPosition().GetDim(2),0);   

					// Set the name of the "Pos:" block
					navigation[i].SetCustomName(baseName + "Pos: [X:" + x.Trim() + " Y:" + y.Trim() + " Z:" + z.Trim() + "]");   
					
					// Make sure the "Pos:" block is showing on the HUD
					navigation[i].RequestShowOnHUD(true);   
				} 
				catch (Exception ex)
				{
					throw new Exception("Couldn't set Pos block name");
				}
			}    
	   
			// If the block is the navigation block   
			if (navigation[i].CustomName.Contains(navigationBlockBaseName))    
			{    		
				try {
					// Find the "navigationBlockBaseName" string in the block name
					int baseNameIndex = navigation[i].CustomName.IndexOf(navigationBlockBaseName);   

					// Strip everything from the "navigationBlockBaseName" forward 
					// (including the "navigationBlockBaseName") out of the block 
					// name. What remains is the base name of the block which we 
					// want to preserve.
					string baseName = navigation[i].CustomName.Remove(baseNameIndex);   

					// Set the name of the navigation block
					navigation[i].SetCustomName(baseName + navigationBlockName);   

					// Make sure the "Pos:" block is showing on the HUD
					navigation[i].RequestShowOnHUD(true);   
				} 
				catch (Exception ex)
				{
					throw new Exception("Couldn't set " + navigationBlockBaseName + " block name");
				}
			}   
		}    
    }
	catch (Exception ex)
	{
		throw new Exception("Exception " + ex.Message + ". " + ex.StackTrace);
	}
}   
 
// Function to extract the target coordinates from the name of the block
// passed as a parameter and return an array of doubles
double[] extractTargetPosition(IMyFunctionalBlock entity) 
{ 
    // Define retVal array
	double[] retVal = new double[3]; 
	
	try 
	{
		// Find the index of the "X:", "Y:" and "Z:" strings in the block name
		int xIndex = entity.CustomName.IndexOf("X:");   
		int yIndex = entity.CustomName.IndexOf("Y:");   
		int zIndex = entity.CustomName.IndexOf("Z:");   
	 
		// Extract the coordinates out of the block name string and convert
		// them to doubles (using TryParse)
		double.TryParse(entity.CustomName.Substring(xIndex + 2, yIndex - (xIndex + 2)), out retVal[0]);   
		double.TryParse(entity.CustomName.Substring(yIndex + 2, zIndex - (yIndex + 2)), out retVal[1]);   
		double.TryParse(entity.CustomName.Substring(zIndex + 2), out retVal[2]); 

		// Return the array	
		}
	catch (Exception ex)
	{
		throw new Exception("Unable to read target coordinates");
	}
    return retVal;	 
} 
 
// Function to extract the position of the block passed in as 
// a parameter and return it as an array of doubles.
double[] loadPosition(IMyFunctionalBlock entity) 
{ 
    // Define retVal array
    double[] retVal = new double[3]; 
	
	try
	{
		// Extract the three coordinates from the block
		for(int n=0; n < 3; n++)   
		{   
			// Assign the coordiante to the array element
			retVal[n] = entity.GetPosition().GetDim(n);   
		}  
	}
	catch (Exception ex)
	{
		throw new Exception("Unable to read coordinates from block " + entity.CustomName);
	}
	
    // Return the array	
	return retVal; 
} 
  
// Function to calculate the directions to the target
double[] calculateNavigationData(double[] radixPosition, 
                                 double[] xAxisPosition, 
                                 double[] yAxisPosition, 
								 double[] targetPosition)   
{   
	// Define return value array
	double[] targetDirections = new double[4];   

	try {
		// Define working arrays
		double[] distanceArray    = new double[4];   
		double[] vector1= new double[3];   
		double[] vector2= new double[3];   
		double[] vector3= new double[3];   
		double[] vector4= new double[3];   
		double[] vector5= new double[3];   
	   
		// Loop through the 3 coordinates and populate vector1-4
		// arrays
		for (int i=0; i < 3; i++)   
		{   
			vector1[i] = targetPosition[i] - radixPosition[i];   
			vector2[i] = xAxisPosition[i]  - radixPosition[i];   
			vector3[i] = yAxisPosition[i]  - radixPosition[i];   
			vector4[i] = targetPosition[i] - yAxisPosition[i];   
		}   
	   
		// Calculate vector5 data
		vector5[0] = (vector2[1] * vector3[2]) - (vector2[2] * vector3[1]);   
		vector5[1] = (vector2[2] * vector3[0]) - (vector2[0] * vector3[2]);   
		vector5[2] = (vector2[0] * vector3[1]) - (vector2[1] * vector3[0]);   
	   
		// Calculate distanceArray
		distanceArray[0] = Math.Sqrt( Math.Pow(vector1[0], 2) + Math.Pow(vector1[1], 2) + Math.Pow(vector1[2], 2));   
		distanceArray[1] = Math.Sqrt( Math.Pow(vector5[0], 2) + Math.Pow(vector5[1], 2) + Math.Pow(vector5[2], 2));   
		distanceArray[2] = Math.Sqrt( Math.Pow(vector2[0], 2) + Math.Pow(vector2[1], 2) + Math.Pow(vector2[2], 2));   
		distanceArray[3] = Math.Sqrt( Math.Pow(vector4[0], 2) + Math.Pow(vector4[1], 2) + Math.Pow(vector4[2], 2));   
		
		// Calculate targetDirections array	
		targetDirections[0] = distanceArray[0];   
		targetDirections[1] = -1 * (Math.Acos((vector5[0] * vector1[0] + vector5[1] * vector1[1] + vector5[2] * vector1[2]) 
								   / (distanceArray[1] * distanceArray[0])) * 180 / Math.PI - 90);   
		targetDirections[2] = -1 * (Math.Acos((vector2[0] * vector1[0] + vector2[1] * vector1[1] + vector2[2] * vector1[2]) 
								   / (distanceArray[2] * distanceArray[0])) * 180 / Math.PI - 90);   
		
		// The last element of the targetDirections array will indicate
		// whether we are pointing in the direction of the target or 
		// whether the target is behind us
		if(distanceArray[3] < distanceArray[0])   
		{   
			targetDirections[3] = 1;   
		}   
		else   
		{   
			targetDirections[3] = 0;   
		}   
    }
	catch (Exception ex)
	{
		throw new Exception("Error while calculating navigation data");
	}
    // return the targetDirections array
    return targetDirections;   
}

Please rate this post

2 Responses

  1. […] in conjunction with my Navigation Script, this script will pilot your ship to the target […]

  2. This is excellent, thank you!

Leave a Reply