2020-10-29 07:29:46 +08:00
var Math = {
#
# Authors: Nikolai V. Chr, Axel Paccalin.
#
2023-09-26 07:50:04 +08:00
# Version 2.02
2020-10-29 07:29:46 +08:00
#
# 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.
#
2022-11-25 08:36:41 +08:00
# Cartesian is right-handed coord system.
#
2020-10-29 07:29:46 +08:00
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);
2022-11-25 08:36:41 +08:00
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};
2020-10-29 07:29:46 +08:00
},
# When observing another MP aircraft the groundspeed velocity info is in body frame, this method will convert it to cartesian vector.
2022-11-25 08:36:41 +08:00
#
# Warning: If you input body velocities in fps the output will be in fps also. Likewise for mps.
2020-10-29 07:29:46 +08:00
getCartesianVelocity: func (yaw_deg, pitch_deg, roll_deg, uBody_fps, vBody_fps, wBody_fps) {
me.bodyVelocity = [uBody_fps, -vBody_fps, -wBody_fps];
2022-11-25 08:36:41 +08:00
return me.rollPitchYawVector(roll_deg, pitch_deg, yaw_deg, me.bodyVelocity);
2020-10-29 07:29:46 +08:00
},
# 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);
},
2022-11-25 08:36:41 +08:00
# 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);
},
2020-10-29 07:29:46 +08:00
# length of vector
magnitudeVector: func (a) {
2022-11-25 08:36:41 +08:00
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;
2020-10-29 07:29:46 +08:00
},
# dot product of 2 vectors
dotProduct: func (a,b) {
return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
},
2022-11-25 08:36:41 +08:00
# rotate a vector. Order: roll, pitch, yaw (same as aircraft)
#
# The coordinate system is fixed during all the rolls.
#
2020-10-29 07:29:46 +08:00
rollPitchYawVector: func (roll, pitch, yaw, vector) {
me.rollM = me.rollMatrix(roll);
me.pitchM = me.pitchMatrix(pitch);
me.yawM = me.yawMatrix(yaw);
2022-11-25 08:36:41 +08:00
me.rotation = me.multiplyMatrices(me.yawM, me.multiplyMatrices(me.pitchM, me.rollM));
2020-10-29 07:29:46 +08:00
return me.multiplyMatrixWithVector(me.rotation, vector);
},
2022-11-25 08:36:41 +08:00
# rotate a vector. Order: yaw, pitch, roll (reverse of aircraft)
2020-10-29 07:29:46 +08:00
yawPitchRollVector: func (yaw, pitch, roll, vector) {
me.rollM = me.rollMatrix(roll);
me.pitchM = me.pitchMatrix(pitch);
me.yawM = me.yawMatrix(yaw);
2022-11-25 08:36:41 +08:00
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);
2020-10-29 07:29:46 +08:00
return me.multiplyMatrixWithVector(me.rotation, vector);
},
2022-11-25 08:36:41 +08:00
# 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);
},
2020-10-29 07:29:46 +08:00
# 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) {
2022-11-25 08:36:41 +08:00
pitch = -pitch * D2R;
return [math.cos(pitch),0,math.sin(pitch),
2020-10-29 07:29:46 +08:00
0,1,0,
2022-11-25 08:36:41 +08:00
-math.sin(pitch),0,math.cos(pitch)];
2020-10-29 07:29:46 +08:00
},
# 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;
2022-11-25 08:36:41 +08:00
me.div = math.clamp(-vector[1]/me.horz, -1, 1);
me.hdg = math.asin(me.div)*R2D;
2020-10-29 07:29:46 +08:00
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];
},
2022-11-25 08:36:41 +08:00
# 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;
},
2020-10-29 07:29:46 +08:00
# 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;
}
2023-09-26 07:50:04 +08:00
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();
2020-10-29 07:29:46 +08:00
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) {
2022-11-25 08:36:41 +08:00
if (me.magnitudeVector(planeNormal) == 0) return [0,0,0];#safety
2020-10-29 07:29:46 +08:00
return me.minus(vector, me.product(me.dotProduct(vector,planeNormal)/math.pow(me.magnitudeVector(planeNormal),2), planeNormal));
},
2022-11-25 08:36:41 +08:00
# 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;
},
2020-10-29 07:29:46 +08:00
# 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) {
2022-11-25 08:36:41 +08:00
return sprintf("(%.2f, %.2f, %.2f)",v[0],v[1],v[2]);
2020-10-29 07:29:46 +08:00
},
# 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));
},
2022-11-25 08:36:41 +08:00
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);
},
2020-10-29 07:29:46 +08:00
# 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) |
#
2022-11-25 08:36:41 +08:00
#| cos(-pitch) 0 sin(-pitch) |
#| 0 1 0 |
#| -sin(-pitch) 0 cos(-pitch) |
2020-10-29 07:29:46 +08:00
#
#| 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)|
#
#
};
2022-11-25 08:36:41 +08:00
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));
},
};
2023-09-26 07:50:04 +08:00
#unitTest.start();
