User:Necolatis/terrain detection from nasal

This page is deprecated. See Terrain Detection for how it ended up being implemented.

This is mostly a page for Richard and James to see that many use cases are easily solved in Nasal, with the new wrapper:

Check for terrain between 2 planes

This example is typically for radar use.

var myPos = geo.aircraft_position();
var otherPos = geo.aircraft_position().apply_course_distance(100, 20000);# another plane 20 Km out

# Check for terrain between own aircraft and other:
v = get_cart_ground_intersection(myPos.x(), myPos.y(), myPos.z(), otherPos.x()-myPos.x(), otherPos.y()-myPos.y(), otherPos.z()-myPos.z());
if (v == nil) {
 printf("No terrain, planes has clear view of each other");
} else {
 var terrain = geo.Coord.new();
 terrain.set_xyz(v[0],v[1],v[2]);
 var maxDist = myPos.direct_distance_to(otherPos);
 var terrainDist = myPos.direct_distance_to(terrain);
 if (terrainDist < maxDist) {
   print("terrain found between the planes");
 } else {
  print("The planes has clear view of each other");
 }
}

Find terrain from radar beam

In this example, we have a Mig21 with a forward down looking terrain detecting radar. The radar beam is fixed 15 degs down from the nose of the aircraft.

# First we need this helper method:
var getGPS = func(x, y, z) {
	#
	# get Coord from body structural position. x,y,z must be in meters.
	# derived from Vivian's code in AIModel/submodel.cxx.
	#
	var ac = geo.aircraft_position();

	if(x == 0 and y==0 and z==0) {
		return geo.Coord.new(ac);
	}

	var ac_roll = getprop("orientation/roll-deg");
	var ac_pitch = getprop("orientation/pitch-deg");
	var ac_hdg   = getprop("orientation/heading-deg");

	var in    = [0,0,0];
	var trans = [[0,0,0],[0,0,0],[0,0,0]];
	var out   = [0,0,0];

	in[0] =  -x * M2FT;
	in[1] =   y * M2FT;
	in[2] =   z * M2FT;
	# Pre-process trig functions:
	var cosRx = math.cos(-ac_roll * D2R);
	var sinRx = math.sin(-ac_roll * D2R);
	var cosRy = math.cos(-ac_pitch * D2R);
	var sinRy = math.sin(-ac_pitch * D2R);
	var cosRz = math.cos(ac_hdg * D2R);
	var sinRz = math.sin(ac_hdg * D2R);
	# Set up the transform matrix:
	trans[0][0] =  cosRy * cosRz;
	trans[0][1] =  -1 * cosRx * sinRz + sinRx * sinRy * cosRz ;
	trans[0][2] =  sinRx * sinRz + cosRx * sinRy * cosRz;
	trans[1][0] =  cosRy * sinRz;
	trans[1][1] =  cosRx * cosRz + sinRx * sinRy * sinRz;
	trans[1][2] =  -1 * sinRx * cosRx + cosRx * sinRy * sinRz;
	trans[2][0] =  -1 * sinRy;
	trans[2][1] =  sinRx * cosRy;
	trans[2][2] =  cosRx * cosRy;
	# Multiply the input and transform matrices:
	out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
	out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
	out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
	# Convert ft to degrees of latitude:
	out[0] = out[0] / (366468.96 - 3717.12 * math.cos(ac.lat() * D2R));
	# Convert ft to degrees of longitude:
	out[1] = out[1] / (365228.16 * math.cos(ac.lat() * D2R));
	# Set position:
	var mlat = ac.lat() + out[0];
	var mlon = ac.lon() + out[1];
	var malt = (ac.alt() * M2FT) + out[2];
	
	var c = geo.Coord.new();
	c.set_latlon(mlat, mlon, malt * FT2M);

	return c;
}

#in this example we will just assume the radar is at the center of aircraft, thats pretty trivial to change though.
var myPos = geo.aircraft_position();

#first find a coord in the radar beam in aircraft coordinates:
var beam_x = -5000*math.cos(-15*D2R);#15 deg down, 5Km out
var beam_y = 0;
var beam_z = 5000*math.sin(-15*D2R);
var beam = getGPS(beam_x, beam_y, beam_z);

#we now find the vector the beam is pointed in:
v = get_cart_ground_intersection(myPos.x(), myPos.y(), myPos.z(), beam.x()-myPos.x(), beam.y()-myPos.y(), beam.z()-myPos.z());
if (v == nil) {
 printf("No terrain");
} else {
 var terrain = geo.Coord.new();
 terrain.set_xyz(v[0],v[1],v[2]);
 var terrainDist = myPos.direct_distance_to(terrain);
 printf("terrain found %0.1f meters down the beam", terrainDist);
}