Должен сказать, что все оказалось намного сложнее, чем я ожидал.Я думаю, что у меня все это прямо, но дайте мне знать в комментариях о любых исправлениях, и я постараюсь исправить.
Прежде чем вы посмотрите на код ниже, ОБРАТИТЕ ВНИМАНИЕ!Предположения важны, и вы должны убедиться, что они верны в вашем случае.Есть десятки (по крайней мере!) Способов определения ориентации и местоположения в пространстве.Основными предположениями, которые вам необходимо убедиться в том, чтобы выровнять свое устройство, является пространственная структура, в которой мы работаем. Эта статья даст вам некоторое представление о том, почему это так важно!Наиболее очевидным является то, как мы маркируем свои оси, какой путь вверх (положительный Z, как я выбрал ниже, но если бы мы говорили о подводных лодках, например, мы могли бы выбрать отрицательный Z).
Исходные предположения: Представьте себе самолет (я знаю, что это не самолет, но его легче объяснить таким образом) с длинным стержнем, свисающим прямо вниз.Мы определим ось Z как вверх (положительная) и вниз (отрицательная).Ось X указывает вперед (положительно) и назад (отрицательно).Ось Y - это ось вращения вокруг крыльев, положительная от левого крыла и отрицательная от правого крыла - это « правосторонняя система координат ».Таким образом, оси пересекаются в середине самолета примерно там, где прикреплены крылья.Вращение определяется как против часовой стрелки вокруг оси для положительных углов и по часовой стрелке, будучи отрицательным.Итак ...
- "рыскание" представляет изменение абсолютного курса (поэтому, даже если вы находитесь в наклонном положении и накатываете, это направление, которое вы указываете относительно фактической земли.
- «тангаж» представляет угол вокруг крыльев - в основном, направлен ли нос вверх или вниз.
- «рулон» представляет крен самолета - таким образом, параллельна ли ось крыла поверхности земли или наклоненавокруг фюзеляжа.
Важно правильно все это понять, особенно знак (+/-), связанный с вашими углами - попробуйте наклонить его и повернуть на 30 градусов и убедиться, что результаты совпадаютс выводом - в противном случае измените знак угла. Для рыскания вам нужно будет изменить как курс, так и тангаж и крен, так как сам заголовок не повлияет на местоположение конца стержня, если он направлен вверх.и вниз. У вас есть данные, описывающие «самолет», это местоположение (три числа) в той же самой структуре XYZ, как описано выше, итри угла (в градусах, от -180 до 180), как описано выше.
- Допущения устройства: это вещи, которые вам, возможно, придется проверить у своего поставщика.Если числа малы относительно ожидаемой (или допустимой) ошибки GPS, это может не иметь большого значения.
- Код предполагает, что все оси встречаются прямо в нижней части устройства, и что стержень свисает вертикально вниз от этой точки.Например, если длина стержня составляет 2 метра, а оси на самом деле встречаются на три сантиметра выше точки соединения, вы должны отрегулировать длину стержня до 2,03 метра.Если стержень фактически прикреплен к точке, не совсем под пересекающимися осями, программное обеспечение необходимо немного изменить, чтобы учесть, что конец не находится непосредственно под ним.Опять же, несколько миллиметров могут не иметь значения для вас в общей схеме вещей.
- Код предполагает, что местоположение, в котором, как утверждается, находится устройство, фактически пересекается с осями.Если нет, вам нужно будет настроить местоположение в этой точке (или изменить программное обеспечение, чтобы учесть это).
- Вы должны указать длину стержня в тех же единицах, что и местоположение устройства.
- Другие предположения:
- Это НЕ относится к искривлению земли - если ваш стержень необычно длинный, это не должно иметь значения и не будет иметь значения, если выдержа его прямо (или почти так).
Код:
Я оставил в некоторых ненужных вещах (которые могут вам понадобиться на случай, если вам вообще понадобится реструктурировать), а также не пытался сделать его более эффективным (например, постоянный пересчет тех же грехов и косинусов), чтобы сделать его немного понятнее. Я оставил в закрывающем наборе компилятор и небольшую документацию, и на тот случай, если вы захотите минимизировать его позже. rodloc - это функция, которую вы хотите ...
function presentresult(location, length, yaw, pitch, roll) {
console.log("Starting point");
console.log(location);
console.log("Rod length = " + length);
console.log("Yaw = " + yaw + ", Pitch = " + pitch + ", Roll = " + roll);
console.log("Result:");
console.log(rodloc(location, length, yaw, pitch, roll));
}
presentresult([100, 100, 100], 2, 0, 0, 0); // Result: [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 30, 0, 0); // Result: [100, 100, 98] (3)
presentresult([100, 100, 100], 2, -30, 0, 0); // Result: [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 0, 30, 0); // Result: [99, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, -30, 0); // Result: [101, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, 30); // Result: [100, 101, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, -30); // Result: [100, 99, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 30, 30, 30); // Result: [98.75, 100.43301270189222, 98.5] (3)
presentresult([100, 100, 100], 2, -30, -30, -30); // Result: [100.25, 98.70096189432334, 98.5] (3)
presentresult([100, 100, 100], 2, -30, 30, -30); // Result: [98.75, 99.56698729810778, 98.5] (3)
/** @typedef {Array<number,number,number>} */ var Vector3D;
/** @typedef {Array<Vector3D,vector3D,Vector3D>} */ var Matrix3D;
/**
* @param {Vector3D} location - The location (3 coordinates) of the "plane"
* @param {number} length - The length of the rod
* @param {number} yaw - the yaw (heading) in degrees
* @param {number} pitch - the pitch in degrees
* @param {number} roll - the roll in degrees
* @returns {Vector3D} - the location of the end of the rod
*/
function rodloc(location, length, yaw, pitch, roll) {
let ryaw = yaw * Math.PI / 180.0; // Convert to radians
let rpitch = pitch * Math.PI / 180.0;
let rroll = roll * Math.PI / 180.0;
// This is where our axes start
let x = [1, 0, 0];
let y = [0, 1, 0];
let z = [0, 0, 1];
// NOTE: ORDER MATTERS - your data may mean different things (see
// assumptions in answer!
// Rotate axes around z by yaw
let yprime = rotatearound([0, 1, 0], [0, 0, 1], ryaw);
let xprime = rotatearound([1, 0, 0], [0, 0, 1], ryaw);
let zprime = z; // rotating around itself
// Next we need to rotate for pitch (around the Y axis...)
let x2prime = rotatearound(xprime, yprime, rpitch);
let y2prime = yprime; // dont need this
let z2prime = rotatearound(zprime, yprime, rpitch);
// Now we need to roll around the new x axis...
let x3prime = x2prime // dont need this
let y3prime = rotatearound(y2prime, x2prime, rroll); // dont need this
let z3prime = rotatearound(z2prime, x2prime, rroll);
// now take what started out as [0, 0, 1] and place the end of the rod
// (at what started out as [0, 0, -length])
let rotend = [0,1,2].map(n=>-length*z3prime[n]);
// now take that and add it to the original location of the plane
// and return it as the result
return [0,1,2].map(n=>location[n]+rotend[n]);
}
/** Multiply a vector times a matrix
* @param {Vector3D} offset - The vector of the offset
* @param {Matrix3D} rotate - The rotation vector
* @returns {Vector3D} - The new offset vector
*/
function vmmult(offset, rotate) {
return [0,1,2].map(x=>xmult(offset,rotate[x]));
}
/** dot product of two vectors
* @param {Vector3D} col
* @param {Vector3D} row
* @returns {number}
*/
function xmult(col, row) {
return [0,1,2].reduce((a,c)=>a+col[c]*row[c],0);
}
/** Rotate a point around a vector projecting from the origin
* @param {Vector3D} point - the we want to rotate
* @param {Vector3D} vec - the vector (from origin to here) to rotate around
* @param {number} angle - the angle (in radians) to rotate
* @returns {Vector3D} - the new point location
*/
function rotatearound(point, vec, angle) {
let rotmat = setuprotationmatrix(angle, vec);
return vmmult(point, rotmat);
}
/**
* Adapted from C courtesy of Bibek Subedi
* https://www.programming-techniques.com/2012/03/3d-rotation-algorithm-about-arbitrary.html
* @param {number} angle - the angle to rotate around the vector
* @param {Vector3D} vec - the vector around which to rotate
* @returns {Matrix3D} - the rotation matrix
*/
function setuprotationmatrix(angle, vec) {
// Leaving L in for reusability, but it should always be 1 in our case
let u = vec[0], v = vec[1], w = vec[2];
let L = (u*u + v * v + w * w);
let u2 = u * u;
let v2 = v * v;
let w2 = w * w;
let rotmat = [[],[],[]];
rotmat[0][0] = (u2 + (v2 + w2) * Math.cos(angle)) / L;
rotmat[0][1] = (u * v * (1 - Math.cos(angle)) - w * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[0][2] = (u * w * (1 - Math.cos(angle)) + v * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[1][0] = (u * v * (1 - Math.cos(angle)) + w * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[1][1] = (v2 + (u2 + w2) * Math.cos(angle)) / L;
rotmat[1][2] = (v * w * (1 - Math.cos(angle)) - u * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[2][0] = (u * w * (1 - Math.cos(angle)) - v * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[2][1] = (v * w * (1 - Math.cos(angle)) + u * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[2][2] = (w2 + (u2 + v2) * Math.cos(angle)) / L;
return rotmat;
}