You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
483 lines
20 KiB
483 lines
20 KiB
var Math = {
|
|
#
|
|
# Authors: Nikolai V. Chr, Axel Paccalin.
|
|
#
|
|
# Version 2.02
|
|
#
|
|
# When doing euler coords. to cartesian: +x = forw, +y = left, +z = up.
|
|
# FG struct. coords: +x = back, +y = right, +z = up.
|
|
#
|
|
# If euler to cartesian (with inverted heading) then:
|
|
# cartesian vector will be x: north, y: west, z: skyward
|
|
#
|
|
# When doing euler angles (from pilots point of view): yaw = yaw left, pitch = rotate up, roll = roll right.
|
|
# FG rotations: heading = yaw right, pitch = rotate up, roll = roll right.
|
|
#
|
|
# Cartesian is right-handed coord system.
|
|
#
|
|
clamp: func(v, min, max) { v < min ? min : v > max ? max : v },
|
|
|
|
convertCoords: func (x,y,z) {
|
|
return [-x, -y, z];
|
|
},
|
|
|
|
convertAngles: func (heading,pitch,roll) {
|
|
return [-heading, pitch, roll];
|
|
},
|
|
|
|
# returns direction in geo coordinate system
|
|
vectorToGeoVector: func (a, coord) {
|
|
me.handp = me.cartesianToEuler(a);
|
|
me.end_dist_m = 100;# not too low for floating point precision. Not too high to get into earth curvature stuff.
|
|
me.tgt_coord = geo.Coord.new(coord);
|
|
if (me.handp[0] != nil) {
|
|
me.tgt_coord.apply_course_distance(me.handp[0],me.end_dist_m);
|
|
me.upamount = me.end_dist_m * math.tan(me.handp[1]*D2R);
|
|
} elsif (me.handp[1] == 90) {
|
|
me.upamount = me.end_dist_m;
|
|
} else {
|
|
me.upamount = -me.end_dist_m;
|
|
}
|
|
me.tgt_coord.set_alt(coord.alt()+me.upamount);
|
|
me.geoVeccy = me.product(me.magnitudeVector(a), me.normalize([me.tgt_coord.x()-coord.x(),me.tgt_coord.y()-coord.y(),me.tgt_coord.z()-coord.z()]));
|
|
return {"x":me.tgt_coord.x()-coord.x(), "y":me.tgt_coord.y()-coord.y(), "z":me.tgt_coord.z()-coord.z(), "vector": me.geoVeccy};
|
|
},
|
|
|
|
# When observing another MP aircraft the groundspeed velocity info is in body frame, this method will convert it to cartesian vector.
|
|
#
|
|
# Warning: If you input body velocities in fps the output will be in fps also. Likewise for mps.
|
|
getCartesianVelocity: func (yaw_deg, pitch_deg, roll_deg, uBody_fps, vBody_fps, wBody_fps) {
|
|
me.bodyVelocity = [uBody_fps, -vBody_fps, -wBody_fps];
|
|
return me.rollPitchYawVector(roll_deg, pitch_deg, yaw_deg, me.bodyVelocity);
|
|
},
|
|
|
|
# angle between 2 vectors. Returns 0-180 degrees.
|
|
angleBetweenVectors: func (a,b) {
|
|
a = me.normalize(a);
|
|
b = me.normalize(b);
|
|
me.value = me.clamp((me.dotProduct(a,b)/me.magnitudeVector(a))/me.magnitudeVector(b),-1,1);#just to be safe in case some floating point error makes it out of bounds
|
|
return R2D * math.acos(me.value);
|
|
},
|
|
|
|
# Rodrigues' rotation formula. Use unitVectorAxis as axis, and rotate around it.
|
|
rotateVectorAroundVector: func (a, unitVectorAxis, angle) {
|
|
return me.plus(
|
|
me.plus(
|
|
me.product(math.cos(angle*D2R),a),
|
|
me.product(math.sin(angle*D2R), me.crossProduct(unitVectorAxis,a))
|
|
),
|
|
me.product((1-math.cos(angle*D2R))*me.dotProduct(unitVectorAxis,a), unitVectorAxis)
|
|
);
|
|
},
|
|
|
|
#Rotate a certain amound of degrees towards 'towardsMe'.
|
|
rotateVectorTowardsVector: func (a, towardsMe, angle) {
|
|
return me.rotateVectorAroundVector(a, me.normalize(me.crossProduct(a, towardsMe)), angle);
|
|
},
|
|
|
|
# length of vector
|
|
magnitudeVector: func (a) {
|
|
me.mag = 1;
|
|
call(func {me.mag = math.sqrt(math.pow(a[0],2)+math.pow(a[1],2)+math.pow(a[2],2))},nil,nil,var err =[]);#strange bug in Nasal can sometimes mess up sqrt
|
|
return me.mag;
|
|
},
|
|
|
|
# dot product of 2 vectors
|
|
dotProduct: func (a,b) {
|
|
return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
|
|
},
|
|
|
|
# rotate a vector. Order: roll, pitch, yaw (same as aircraft)
|
|
#
|
|
# The coordinate system is fixed during all the rolls.
|
|
#
|
|
rollPitchYawVector: func (roll, pitch, yaw, vector) {
|
|
me.rollM = me.rollMatrix(roll);
|
|
me.pitchM = me.pitchMatrix(pitch);
|
|
me.yawM = me.yawMatrix(yaw);
|
|
me.rotation = me.multiplyMatrices(me.yawM, me.multiplyMatrices(me.pitchM, me.rollM));
|
|
return me.multiplyMatrixWithVector(me.rotation, vector);
|
|
},
|
|
|
|
# rotate a vector. Order: yaw, pitch, roll (reverse of aircraft)
|
|
yawPitchRollVector: func (yaw, pitch, roll, vector) {
|
|
me.rollM = me.rollMatrix(roll);
|
|
me.pitchM = me.pitchMatrix(pitch);
|
|
me.yawM = me.yawMatrix(yaw);
|
|
me.rotation = me.multiplyMatrices(me.rollM, me.multiplyMatrices(me.pitchM, me.yawM));
|
|
return me.multiplyMatrixWithVector(me.rotation, vector);
|
|
},
|
|
|
|
# rotate a vector. Order: yaw, pitch
|
|
yawPitchVector: func (yaw, pitch, vector) {
|
|
me.pitchM = me.pitchMatrix(pitch);
|
|
me.yawM = me.yawMatrix(yaw);
|
|
me.rotation = me.multiplyMatrices(me.pitchM, me.yawM);
|
|
return me.multiplyMatrixWithVector(me.rotation, vector);
|
|
},
|
|
|
|
# rotate a vector. Order: pitch, yaw
|
|
pitchYawVector: func (pitch, yaw, vector) {
|
|
me.pitchM = me.pitchMatrix(pitch);
|
|
me.yawM = me.yawMatrix(yaw);
|
|
me.rotation = me.multiplyMatrices(me.yawM, me.pitchM);
|
|
return me.multiplyMatrixWithVector(me.rotation, vector);
|
|
},
|
|
|
|
# rotate a vector. Order: yaw
|
|
yawVector: func (yaw, vector) {
|
|
me.yawM = me.yawMatrix(yaw);
|
|
return me.multiplyMatrixWithVector(me.yawM, vector);
|
|
},
|
|
|
|
# rotate a vector. Order: pitch
|
|
pitchVector: func (pitch, vector) {
|
|
me.pitchM = me.pitchMatrix(pitch);
|
|
return me.multiplyMatrixWithVector(me.pitchM, vector);
|
|
},
|
|
|
|
# multiply 3x3 matrix with vector
|
|
multiplyMatrixWithVector: func (matrix, vector) {
|
|
return [matrix[0]*vector[0]+matrix[1]*vector[1]+matrix[2]*vector[2],
|
|
matrix[3]*vector[0]+matrix[4]*vector[1]+matrix[5]*vector[2],
|
|
matrix[6]*vector[0]+matrix[7]*vector[1]+matrix[8]*vector[2]];
|
|
},
|
|
|
|
# multiply 2 3x3 matrices
|
|
multiplyMatrices: func (a,b) {
|
|
return [a[0]*b[0]+a[1]*b[3]+a[2]*b[6], a[0]*b[1]+a[1]*b[4]+a[2]*b[7], a[0]*b[2]+a[1]*b[5]+a[2]*b[8],
|
|
a[3]*b[0]+a[4]*b[3]+a[5]*b[6], a[3]*b[1]+a[4]*b[4]+a[5]*b[7], a[3]*b[2]+a[4]*b[5]+a[5]*b[8],
|
|
a[6]*b[0]+a[7]*b[3]+a[8]*b[6], a[6]*b[1]+a[7]*b[4]+a[8]*b[7], a[6]*b[2]+a[7]*b[5]+a[8]*b[8]];
|
|
},
|
|
|
|
# matrix for rolling
|
|
rollMatrix: func (roll) {
|
|
roll = roll * D2R;
|
|
return [1,0,0,
|
|
0,math.cos(roll),-math.sin(roll),
|
|
0,math.sin(roll), math.cos(roll)];
|
|
},
|
|
|
|
# matrix for pitching
|
|
pitchMatrix: func (pitch) {
|
|
pitch = -pitch * D2R;
|
|
return [math.cos(pitch),0,math.sin(pitch),
|
|
0,1,0,
|
|
-math.sin(pitch),0,math.cos(pitch)];
|
|
},
|
|
|
|
# matrix for yawing
|
|
yawMatrix: func (yaw) {
|
|
yaw = yaw * D2R;
|
|
return [math.cos(yaw),-math.sin(yaw),0,
|
|
math.sin(yaw),math.cos(yaw),0,
|
|
0,0,1];
|
|
},
|
|
|
|
# vector to heading/pitch
|
|
cartesianToEuler: func (vector) {
|
|
me.horz = math.sqrt(vector[0]*vector[0]+vector[1]*vector[1]);
|
|
if (me.horz != 0) {
|
|
me.pitch = math.atan2(vector[2],me.horz)*R2D;
|
|
me.div = math.clamp(-vector[1]/me.horz, -1, 1);
|
|
me.hdg = math.asin(me.div)*R2D;
|
|
|
|
if (vector[0] < 0) {
|
|
# south
|
|
if (me.hdg >= 0) {
|
|
me.hdg = 180-me.hdg;
|
|
} else {
|
|
me.hdg = -180-me.hdg;
|
|
}
|
|
}
|
|
me.hdg = geo.normdeg(me.hdg);
|
|
} else {
|
|
me.pitch = vector[2]>=0?90:-90;
|
|
me.hdg = nil;
|
|
}
|
|
return [me.hdg, me.pitch];
|
|
},
|
|
|
|
# vector to pitch
|
|
cartesianToEulerPitch: func (vector) {
|
|
me.horz = math.sqrt(vector[0]*vector[0]+vector[1]*vector[1]);
|
|
if (me.horz != 0) {
|
|
me.pitch = math.atan2(vector[2],me.horz)*R2D;
|
|
} else {
|
|
me.pitch = vector[2]>=0?90:-90;
|
|
}
|
|
return me.pitch;
|
|
},
|
|
|
|
# vector to heading
|
|
cartesianToEulerHeading: func (vector) {
|
|
me.horz = math.sqrt(vector[0]*vector[0]+vector[1]*vector[1]);
|
|
if (me.horz != 0) {
|
|
me.div = math.clamp(-vector[1]/me.horz, -1, 1);
|
|
me.hdg = math.asin(me.div)*R2D;
|
|
|
|
if (vector[0] < 0) {
|
|
# south
|
|
if (me.hdg >= 0) {
|
|
me.hdg = 180-me.hdg;
|
|
} else {
|
|
me.hdg = -180-me.hdg;
|
|
}
|
|
}
|
|
me.hdg = geo.normdeg(me.hdg);
|
|
} else {
|
|
me.hdg = 0;
|
|
}
|
|
return me.hdg;
|
|
},
|
|
|
|
# gives an vector that points up from fuselage
|
|
eulerToCartesian3Z: func (yaw_deg, pitch_deg, roll_deg) {
|
|
me.yaw = yaw_deg * D2R;
|
|
me.pitch = pitch_deg * D2R;
|
|
me.roll = roll_deg * D2R;
|
|
me.x = -math.cos(me.yaw)*math.sin(me.pitch)*math.cos(me.roll) + math.sin(me.yaw)*math.sin(me.roll);
|
|
me.y = -math.sin(me.yaw)*math.sin(me.pitch)*math.cos(me.roll) - math.cos(me.yaw)*math.sin(me.roll);
|
|
me.z = math.cos(me.pitch)*math.cos(me.roll);#roll changed from sin to cos, since the rotation matrix is wrong
|
|
return [me.x,me.y,me.z];
|
|
},
|
|
|
|
# gives an vector that points forward from fuselage
|
|
eulerToCartesian3X: func (yaw_deg, pitch_deg, roll_deg) {
|
|
me.yaw = yaw_deg * D2R;
|
|
me.pitch = pitch_deg * D2R;
|
|
me.roll = roll_deg * D2R;
|
|
me.x = math.cos(me.yaw)*math.cos(me.pitch);
|
|
me.y = math.sin(me.yaw)*math.cos(me.pitch);
|
|
me.z = math.sin(me.pitch);
|
|
return [me.x,me.y,me.z];
|
|
},
|
|
|
|
# gives an vector that points left from fuselage
|
|
eulerToCartesian3Y: func (yaw_deg, pitch_deg, roll_deg) {
|
|
me.yaw = yaw_deg * D2R;
|
|
me.pitch = pitch_deg * D2R;
|
|
me.roll = roll_deg * D2R;
|
|
me.x = -math.cos(me.yaw)*math.sin(me.pitch)*math.sin(me.roll) - math.sin(me.yaw)*math.cos(me.roll);
|
|
me.y = -math.sin(me.yaw)*math.sin(me.pitch)*math.sin(me.roll) + math.cos(me.yaw)*math.cos(me.roll);
|
|
me.z = math.cos(me.pitch)*math.sin(me.roll);
|
|
return [me.x,me.y,me.z];
|
|
},
|
|
|
|
# same as eulerToCartesian3X, except it needs no roll
|
|
eulerToCartesian2: func (yaw_deg, pitch_deg) {
|
|
me.yaw = yaw_deg * D2R;
|
|
me.pitch = pitch_deg * D2R;
|
|
me.x = math.cos(me.pitch) * math.cos(me.yaw);
|
|
me.y = math.cos(me.pitch) * math.sin(me.yaw);
|
|
me.z = math.sin(me.pitch);
|
|
return [me.x,me.y,me.z];
|
|
},
|
|
|
|
#pitch from coord1 to coord2 in degrees (takes curvature of earth into effect.)
|
|
getPitch: func (coord1, coord2) {
|
|
if (coord1.lat() == coord2.lat() and coord1.lon() == coord2.lon()) {
|
|
if (coord2.alt() > coord1.alt()) {
|
|
return 90;
|
|
} elsif (coord2.alt() < coord1.alt()) {
|
|
return -90;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
if (coord1.alt() != coord2.alt()) {
|
|
me.d12 = coord1.direct_distance_to(coord2);
|
|
if (me.d12 == 0) {
|
|
# on top of each other, maybe rounding error..
|
|
return 0;
|
|
}
|
|
me.coord3 = geo.Coord.new(coord1);
|
|
me.coord3.set_alt(coord1.alt()-me.d12*5);# this will increase the area of the triangle so that rounding errors dont get in the way. Changed to 5 May 2023, which gives more presision than 0.5 when c1 and c2 are very close.
|
|
me.d13 = coord1.alt()-me.coord3.alt();
|
|
me.d32 = me.coord3.direct_distance_to(coord2);
|
|
if (math.abs(me.d13)+me.d32 < me.d12) {
|
|
# rounding errors somewhere..one triangle side is longer than other 2 sides combined.
|
|
return 0;
|
|
}
|
|
# standard formula for a triangle where all 3 side lengths are known:
|
|
me.len = (math.pow(me.d12, 2)+math.pow(me.d13,2)-math.pow(me.d32, 2))/(2 * me.d12 * math.abs(me.d13));
|
|
if (me.len < -1 or me.len > 1) {
|
|
# something went wrong, maybe rounding error..
|
|
return 0;
|
|
}
|
|
me.angle = R2D * math.acos(me.len);
|
|
me.pitch = -1* (90 - me.angle);
|
|
#printf("d12 %.4f d32 %.4f d13 %.4f len %.4f pitch %.4f angle %.4f", me.d12, me.d32, me.d13, me.len, me.pitch, me.angle);
|
|
return me.pitch;
|
|
} else {
|
|
# same altitude
|
|
me.nc = geo.Coord.new();
|
|
me.nc.set_xyz(0,0,0); # center of earth
|
|
me.radiusEarth = coord1.direct_distance_to(me.nc);# current distance to earth center
|
|
me.d12 = coord1.direct_distance_to(coord2);
|
|
# standard formula for a triangle where all 3 side lengths are known:
|
|
me.len = (math.pow(me.d12, 2)+math.pow(me.radiusEarth,2)-math.pow(me.radiusEarth, 2))/(2 * me.d12 * me.radiusEarth);
|
|
if (me.len < -1 or me.len > 1) {
|
|
# something went wrong, maybe rounding error..
|
|
return 0;
|
|
}
|
|
me.angle = R2D * math.acos(me.len);
|
|
me.pitch = -1* (90 - me.angle);
|
|
return me.pitch;
|
|
}
|
|
},
|
|
|
|
# supply a normal to the plane, and a vector. The vector will be projected onto the plane, and that projection is returned as a vector.
|
|
projVectorOnPlane: func (planeNormal, vector) {
|
|
if (me.magnitudeVector(planeNormal) == 0) return [0,0,0];#safety
|
|
return me.minus(vector, me.product(me.dotProduct(vector,planeNormal)/math.pow(me.magnitudeVector(planeNormal),2), planeNormal));
|
|
},
|
|
|
|
# Project a onto ontoMe.
|
|
projVectorOnVector: func (a, ontoMe) {
|
|
if (me.magnitudeVector(ontoMe) == 0) return [0,0,0];#safety
|
|
return me.product(me.dotProduct(a,ontoMe)/me.dotProduct(ontoMe,ontoMe), ontoMe);
|
|
},
|
|
|
|
# Project a onto ontoMe and measure how long along ontoMe it goes, opposite will give negative number.
|
|
scalarProjVectorOnVector: func (a, ontoMe) {
|
|
me.ontoMeMag = me.magnitudeVector(ontoMe);
|
|
if (me.ontoMeMag == 0) return 0;
|
|
return me.dotProduct(a,ontoMe)/me.ontoMeMag;
|
|
},
|
|
|
|
# unary - vector
|
|
opposite: func (v){
|
|
# author: Paccalin
|
|
return [-v[0], -v[1], -v[2]];
|
|
},
|
|
|
|
# vector a - vector b
|
|
minus: func (a, b) {
|
|
return [a[0]-b[0], a[1]-b[1], a[2]-b[2]];
|
|
},
|
|
|
|
# vector a + vector b
|
|
plus: func (a, b) {
|
|
return [a[0]+b[0], a[1]+b[1], a[2]+b[2]];
|
|
},
|
|
|
|
# float * vector
|
|
product: func (scalar, vector) {
|
|
return [scalar*vector[0], scalar*vector[1], scalar*vector[2]]
|
|
},
|
|
|
|
# print vector to console
|
|
format: func (v) {
|
|
return sprintf("(%.2f, %.2f, %.2f)",v[0],v[1],v[2]);
|
|
},
|
|
|
|
# make vector length 1.0
|
|
normalize: func (v) {
|
|
me.mag = me.magnitudeVector(v);
|
|
return [v[0]/me.mag, v[1]/me.mag, v[2]/me.mag];
|
|
},
|
|
|
|
crossProduct: func (a,b) {
|
|
return [a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]];
|
|
},
|
|
|
|
distance_from_point_to_line: func (coordP, coordL1, coordL2) {
|
|
var P = [coordP.x(), coordP.y(), coordP.z()];
|
|
var L1 = [coordL1.x(), coordL1.y(), coordL1.z()];
|
|
var L2 = [coordL2.x(), coordL2.y(), coordL2.z()];
|
|
|
|
return me.magnitudeVector(me.crossProduct(me.minus(L2,L1), me.minus(L1,P)))/me.magnitudeVector(me.minus(L2,L1));
|
|
},
|
|
|
|
interpolateVector: func (start, end, fraction) {
|
|
me.xx = (start[0]*(1-fraction) +end[0]*fraction);
|
|
me.yy = (start[1]*(1-fraction) +end[1]*fraction);
|
|
me.zz = (start[2]*(1-fraction) +end[2]*fraction);
|
|
|
|
return [me.xx, me.yy, me.zz];
|
|
},
|
|
|
|
# move distance 'along' from start towards end. Total dist from start to end is dist.
|
|
alongVector: func (start, end, dist, along) {
|
|
me.fraction = along/dist;
|
|
return me.interpolateVector(start, end, me.fraction);
|
|
},
|
|
|
|
# Orthogonal projection of a vector `vec` onto another `ref` !!can throw an exception if the referential vector is null!!.
|
|
orthogonalProjection: func(vec, ref){
|
|
# author: Paccalin
|
|
me.op_refMag = me.magnitudeVector(ref);
|
|
if(me.op_refMag == 0)
|
|
die("Orthogonal projection on a null vector referential");
|
|
|
|
return me.dotProduct(vec, ref) / me.op_refMag;
|
|
},
|
|
|
|
# Time at which two particles will be at shortest distance !!can throw an exception if the relative speed is null!!
|
|
particleShortestDistTime: func (orig1, speed1, orig2, speed2) {
|
|
# author: Paccalin
|
|
# Compute the origin of the second particle in a referential positionally centered on the first particle.
|
|
me.psdt_tgtOrig = me.minus(orig2, orig1);
|
|
# Compute the speed of the second particle in a referential inertially based on the first particle.
|
|
me.psdt_tgtSpeed = me.minus(speed2, speed1);
|
|
|
|
# Project the origin of the particle1 referential onto the line supported by the particle2 trajectory in 1 unit of time.
|
|
# And divide the result by the magnitude of the speed to have it normalized relative to the time.
|
|
return me.orthogonalProjection(me.opposite(me.psdt_tgtOrig), me.psdt_tgtSpeed) / me.magnitudeVector(me.psdt_tgtSpeed);
|
|
},
|
|
|
|
# rotation matrices
|
|
#
|
|
#
|
|
#| 1 0 0 |
|
|
#| 0 cos(roll) -sin(roll) |
|
|
#| 0 sin(roll) cos(roll) |
|
|
#
|
|
#| cos(-pitch) 0 sin(-pitch) |
|
|
#| 0 1 0 |
|
|
#| -sin(-pitch) 0 cos(-pitch) |
|
|
#
|
|
#| cos(yaw) -sin(yaw) 0 |
|
|
#| sin(yaw) cos(yaw) 0 |
|
|
#| 0 0 1 |
|
|
#
|
|
# combined matrix from yaw, pitch, roll:
|
|
#
|
|
#| cos(yaw)cos(pitch) -cos(yaw)sin(pitch)sin(roll)-sin(yaw)cos(roll) -cos(yaw)sin(pitch)cos(roll)+sin(yaw)sin(roll)|
|
|
#| sin(yaw)cos(pitch) -sin(yaw)sin(pitch)sin(roll)+cos(yaw)cos(roll) -sin(yaw)sin(pitch)cos(roll)-cos(yaw)sin(roll)|
|
|
#| sin(pitch) cos(pitch)sin(roll) cos(pitch)cos(roll)|
|
|
#
|
|
#
|
|
|
|
};
|
|
|
|
|
|
var unitTest = {
|
|
start: func {
|
|
# 1: Simple test of rotation matrices and getting the input back in another way.
|
|
var localDir = Math.pitchYawVector(-40, -75, [1,0,0]);
|
|
me.eulerDir = Math.cartesianToEuler(localDir);
|
|
me.eulerDir[0] = me.eulerDir[0]==nil?0:geo.normdeg(me.eulerDir[0]);
|
|
printf("Looking out of aircraft window at heading 75 and 40 degs down: %.4f heading %.4f pitch.",me.eulerDir[0],me.eulerDir[1]);
|
|
# 2: Revert part of test #1
|
|
var forwardDir = Math.yawPitchVector(75, 0, localDir);
|
|
me.eulerDir = Math.cartesianToEuler(forwardDir);
|
|
me.eulerDir[0] = me.eulerDir[0]==nil?0:geo.normdeg(me.eulerDir[0]);
|
|
printf("Looking out of aircraft window at heading 0 and 40 degs down: %.4f heading %.4f pitch.",me.eulerDir[0],me.eulerDir[1]);
|
|
# 3: Tougher test, try it different places on earth since earth is not sphere. At KXTA runway it gives 0.03% error in pitch.
|
|
var someDir = Math.eulerToCartesian3X(-75, -40, 20);
|
|
var myCoord = geo.aircraft_position();
|
|
me.thousandVectorGeo = Math.vectorToGeoVector(someDir, myCoord).vector;# does not return magnitude with meaning, only its direction matters.
|
|
me.thousandVectorGeo = Math.normalize(me.thousandVectorGeo);
|
|
me.thousandVectorGeo = Math.product(100000, me.thousandVectorGeo);
|
|
me.lookAt = geo.Coord.new().set_xyz(myCoord.x()+me.thousandVectorGeo[0], myCoord.y()+me.thousandVectorGeo[1], myCoord.z()+me.thousandVectorGeo[2]);
|
|
printf("Looking out of aircraft window 100000m away heading 75 and 40 degs down: %.4f heading %.4f pitch %.4f meter.", myCoord.course_to(me.lookAt), Math.getPitch(myCoord, me.lookAt), myCoord.direct_distance_to(me.lookAt));
|
|
# 4:
|
|
var aircraft = Math.eulerToCartesian3X(-35, 72, 21);
|
|
var aircraft2 = Math.rollPitchYawVector(21, 72, -35, [1,0,0]);
|
|
printf("These two should be the same %s = %s",Math.format(aircraft),Math.format(aircraft2));
|
|
},
|
|
};
|
|
#unitTest.start();
|