From 98fc0cf5b4949f7cf2ab831dd3bfa6ebedb0676b Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 31 May 2025 12:46:33 +0300 Subject: [PATCH] Add quaternion and vector chapter to book --- docs/book.css | 6 + docs/book.toml | 2 +- docs/book/SUMMARY.md | 1 + docs/book/geometry.md | 309 +++++++++++++++++++++++++++++++++++++ docs/book/js/rotation.js | 262 +++++++++++++++++++++++++++++++ docs/img/axes-rotation.svg | 94 +++++++++++ docs/img/left-axes.svg | 67 ++++++++ docs/img/right-axes.svg | 67 ++++++++ docs/rotation.css | 29 ++++ 9 files changed, 836 insertions(+), 1 deletion(-) create mode 100644 docs/book/geometry.md create mode 100644 docs/book/js/rotation.js create mode 100644 docs/img/axes-rotation.svg create mode 100644 docs/img/left-axes.svg create mode 100644 docs/img/right-axes.svg create mode 100644 docs/rotation.css diff --git a/docs/book.css b/docs/book.css index 7612e77..f724c32 100644 --- a/docs/book.css +++ b/docs/book.css @@ -53,6 +53,12 @@ footer a.telegram, footer a.github { border: 1px solid #c9c9c9; } +@media (max-width: 600px) { + .MathJax_Display { + overflow-x: auto; + } +} + .firmware { position: relative; margin: 20px 0; diff --git a/docs/book.toml b/docs/book.toml index d5c505b..de44bce 100644 --- a/docs/book.toml +++ b/docs/book.toml @@ -10,7 +10,7 @@ description = "Учебник по разработке полетного ко build-dir = "build" [output.html] -additional-css = ["book.css", "zoom.css"] +additional-css = ["book.css", "zoom.css", "rotation.css"] additional-js = ["zoom.js", "js.js"] edit-url-template = "https://github.com/okalachev/flix/blob/master/docs/{path}?plain=1" mathjax-support = true diff --git a/docs/book/SUMMARY.md b/docs/book/SUMMARY.md index e0280ab..249913e 100644 --- a/docs/book/SUMMARY.md +++ b/docs/book/SUMMARY.md @@ -11,6 +11,7 @@ * [Светодиод]() * [Моторы]() * [Радиоуправление]() +* [Вектор, кватернион](geometry.md) * [Гироскоп](gyro.md) * [Акселерометр]() * [Оценка состояния]() diff --git a/docs/book/geometry.md b/docs/book/geometry.md new file mode 100644 index 0000000..bb45721 --- /dev/null +++ b/docs/book/geometry.md @@ -0,0 +1,309 @@ +# Вектор, кватернион + +В алгоритме управления квадрокоптером широко применяются геометрические (и алгебраические) объекты, такие как **векторы** и **кватернионы**. Они позволяют упростить математические вычисления и улучшить читаемость кода. В этой главе мы рассмотрим именно те геометрические объекты, которые используются в алгоритме управления квадрокоптером Flix, причем акцент будет сделан на практических аспектах их использования. + +## Система координат + +### Оси координат + +Для работы с объектами в трехмерном пространстве необходимо определить *систему координат*. Как известно, система координат задается тремя взаимно перпендикулярными осями, которые обозначаются как *X*, *Y* и *Z*. Порядок обозначения этих осей зависит от того, какую систему координат мы выбрали — *левую* или *правую*: + +|Левая система координат|Правая система координат| +|-----------------------|------------------------| +|Левая система координат|Правая система координат| + +В Flix для всех математических расчетов используется **правая система координат**, что является стандартом в робототехнике и авиации. + +Также необходимо выбрать направление осей — в Flix они выбраны в соответствии со стандартом [REP-103](https://www.ros.org/reps/rep-0103.html). Для величин, заданных в подвижной системе координат, связанной с корпусом дрона, применяется порядок FLU: + +* ось X — направлена **вперед**; +* ось Y — направлена **влево**; +* ось Z — направлена **вверх**. + +Для величин, заданных в *мировой* системе координат (относительно фиксированной точки в пространстве) — ENU: + +* ось X — направлена на **восток** (условный); +* ось Y — направлена на **север** (условный); +* ось Z — направлена **вверх**. + +> [!NOTE] +> Для системы ENU важно только взаимное направление осей. Если доступен магнитометр, то используются реальные восток и север, но если нет — то произвольно выбранные. + +Углы и угловые скорости определяются в соответствии с правилами математики: значения увеличиваются против часовой стрелки, если смотреть в сторону начала координат. Общий вид системы координат: + +Система координат + +> [!TIP] +> Оси координат X, Y и Z часто обозначаются красными, зелеными и синими цветами соответственно. Запомнить это можно с помощью сокращения RGB. + +## Вектор + +
+ Файл прошивки: + vector.h.
+
+ +**Вектор** — простой геометрический объект, который содержит три значения, соответствующие координатам *X*, *Y* и *Z*. Эти значения называются *компонентами вектора*. Вектор может описывать точку в пространстве, направление или ось вращения, скорость, ускорение, угловые скорости и другие физические величины. В Flix векторы задаются объектами `Vector` из библиотеки `vector.h`: + +```cpp +Vector v(1, 2, 3); +v.x = 5; +v.y = 10; +v.z = 15; +``` + +> [!TIP] +> Не следует путать геометрический вектор — vector и динамический массив в стандартной библиотеке C++ — std::vector. + +В прошивке в виде векторов представлены, например: + +* `acc` — истинное ускорение с акселерометра. +* `gyro` — угловые скорости с гироскопа. +* `rates` — рассчитанная угловая скорость дрона. +* `accBias`, `accScale`, `gyroBias` — параметры калибровки IMU. + +### Операции с векторами + +**Длина вектора** рассчитывается при помощи теоремы Пифагора; в прошивке используется метод `norm()`: + +```cpp +Vector v(3, 4, 5); +float length = v.norm(); // 7.071 +``` + +Любой вектор можно привести к **единичному вектору** (сохранить направление, но сделать длину равной 1) при помощи метода `normalize()`: + +```cpp +Vector v(3, 4, 5); +v.normalize(); // 0.424, 0.566, 0.707 +``` + +**Сложение и вычитание** векторов реализуется через простое покомпонентное сложение и вычитание. Геометрически сумма векторов представляет собой вектор, который соединяет начало первого вектора с концом второго. Разность векторов представляет собой вектор, который соединяет конец первого вектора с концом второго. Это удобно для расчета относительных позиций, суммарных скоростей и решения других задач. В коде эти операции интуитивно понятны: + +```cpp +Vector a(1, 2, 3); +Vector b(4, 5, 6); +Vector sum = a + b; // 5, 7, 9 +Vector diff = a - b; // -3, -3, -3 +``` + +Операция **умножения на число** `n` увеличивает (или уменьшает) длину вектора в `n` раз (сохраняя направление): + +```cpp +Vector a(1, 2, 3); +Vector b = a * 2; // 2, 4, 6 +``` + +В некоторых случаях полезна операция **покомпонентного умножения** (или деления) векторов. Например, для применения коэффициентов калибровки к данным с IMU. В разных библиотеках эта операция обозначается по разному, но в библиотеке `vector.h` используется простые знаки `*` и `/`: + +```cpp +acc = acc / accScale; +``` + +**Угол между векторами** можно найти при помощи статического метода `Vector::angleBetween()`: + +```cpp +Vector a(1, 0, 0); +Vector b(0, 1, 0); +float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов) +``` + +#### Скалярное произведение + +Скалярное произведение векторов (*dot product*) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*. + +В Flix используется статический метод `Vector::dot()`: + +```cpp +Vector a(1, 2, 3); +Vector b(4, 5, 6); +float dotProduct = Vector::dot(a, b); // 32 +``` + +Операция скалярного произведения может помочь, например, при расчете проекции одного вектора на другой. + +### Векторное произведение + +Векторное произведение (*cross product*) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке — статический метод `Vector::cross()`: + +```cpp +Vector a(1, 2, 3); +Vector b(4, 5, 6); +Vector crossProduct = Vector::cross(a, b); // -3, 6, -3 +``` + +## Кватернион + +### Ориентация в трехмерном пространстве + +В отличие от позиции и скорости, у ориентации в трехмерном пространстве нет универсального для всех случаев способа представления. В зависимости от задачи ориентация может быть представлена в виде углов Эйлера, матрицы поворота, вектора вращения или кватерниона. Рассмотрим используемые в полетной прошивке способы представления ориентации. + +### Углы Эйлера + +**Углы Эйлера** — *крен*, *тангаж* и *рыскание* — это наиболее «естественный» для человека способ представления ориентации. Они описывают последовательные вращения объекта вокруг трех осей координат. + +В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, геометрически говоря, не являются вектором): + +* Угол по крену (*roll*) — `vector.x`. +* Угол по тангажу (*pitch*) — `vector.y`. +* Угол по рысканию (*yaw*) — `vector.z`. + +Особенности углов Эйлера: + +1. Углы Эйлера зависят от порядка применения вращений, то есть существует 6 типов углов Эйлера. Порядок вращений, принятый в Flix (и в роботехнике в целом) — рыскание, тангаж, крен (ZYX). +2. Для некоторых ориентаций углы Эйлера «вырождаются». Так, если объект «смотрит» строго вниз, то угол по рысканию и угол по крену становятся неразличимыми. Эта ситуация называется *gimbal lock* — потеря одной степени свободы. + +Ввиду этих особенности для углов Эйлера не существует общих формул для самых базовых задач с ориентациями, таких как применение одного вращения (ориентации) к другому, расчет разницы между ориентациями и подобных. Поэтому в основном углы Эйлера применяются в пользовательском интерфейсе, но редко используются в математических расчетах. + +> [!IMPORTANT] +> Для углов Эйлера не существует общих формул для самых базовых операций с ориентациями. + +### Axis-angle + +Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами: + +* **Ось вращения** (*axis*) — единичный вектор, определяющий ось вращения. +* **Угол поворота** (*angle* или *θ*) — угол, на который нужно повернуть объект вокруг этой оси. + +В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах: + +```cpp +// Вращение на 45 градусов вокруг оси (1, 2, 3) +Vector axis(1, 2, 3); +float angle = radians(45); +``` + +Этот способ более удобен для расчетов, чем углы Эйлера, но все еще не является оптимальным. + +### Вектор вращения + +Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** (*rotation vector*). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата. + +Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями. + +> [!IMPORTANT] +> Чтобы за единичное время прийти к заданной ориентации, собственные угловые скорости объекта должны быть равны компонентам вектора вращения. + +Вектора вращения в Flix представляются в виде объектов `Vector`: + +```cpp +// Вращение на 45 градусов вокруг оси (1, 2, 3) +Vector rotation = radians(45) * Vector(1, 2, 3); +``` + +### Кватернион + +
+ Файл прошивки: + quaternion.h.
+
+ +Вектор вращения удобен, но для математических расчетов еще удобнее использовать **кватернион**. В Flix кватернионы представляются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения (*axis*) и угла поворота (*θ*) по следующей формуле: + +\\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\] + +На практике оказывается, что **именно такое представление наиболее удобно для математических расчетов**. + +Проиллюстрируем кватернион и остальные описанные выше способы представления ориентации с помощью интерактивной визуализации. Изменяйте угол поворота *θ* с помощью ползунка (ось вращения константна) и изучите, как меняется ориентация объекта, вектор вращения и кватернион: + +
+

+ + +

+

+

+

+

+
+ + + + +> [!IMPORTANT] +> В контексте управляющих алгоритмов кватернион — это оптимизированный для расчетов аналог вектора вращения. + +Кватернион это наиболее часто используемый способ представления ориентации в алгоритмах. Кроме этого, у кватерниона есть большое значение в теории чисел и алгебре, как у расширения понятия комплексного числа, но рассмотрение этого аспекта выходит за рамки описания работы с вращениями с практической точки зрения. + +В прошивке в виде кватернионов представлены, например: + +* `attitude` — текущая ориентация квадрокоптера. +* `attitudeTarget` — целевая ориентация квадрокоптера. + +### Операции с кватернионами + +Кватернион создается напрямую из четырех его компонент: + +```cpp +// Кватернион, представляющий нулевую (исходную) ориентацию +Quaternion q(1, 0, 0, 0); +``` + +Кватернион можно создать из оси вращения и угла поворота, вектора вращения или углов Эйлера: + +```cpp +Quaternion q1 = Quaternion::fromAxisAngle(axis, angle); +Quaternion q2 = Quaternion::fromRotationVector(rotation); +Quaternion q3 = Quaternion::fromEuler(Vector(roll, pitch, yaw)); +``` + +И наоборот: + +```cpp +q1.toAxisAngle(axis, angle); +Vector rotation = q2.toRotationVector(); +Vector euler = q3.toEuler(); +``` + +Возможно рассчитать вращение между двумя обычными векторами: + +```cpp +Quaternion q = Quaternion::fromBetweenVectors(v1, v2); // в виде кватерниона +Vector rotation = Vector::rotationVectorBetween(v1, v2); // в виде вектора вращения +``` + +Шорткаты для работы с вращением по рысканию (удобно для алгоритмов управления полетом): + +```cpp +float yaw = q.getYaw(); +q.setYaw(yaw); +``` + +#### Применения вращений + +Чтобы применить вращение, выраженное в кватернионе, к другому кватерниону, в математике используется операция **умножения кватернионов**. При использовании этой операции, необходимо учитывать, что она не является коммутативной, то есть порядок операндов имеет значение. Формула умножения кватернионов выглядит так: + +\\[ q_1 \times q_2 = \left( \begin{array}{c} w_1 \\\\ x_1 \\\\ y_1 \\\\ z_1 \end{array} \right) \times \left( \begin{array}{c} w_2 \\\\ x_2 \\\\ y_2 \\\\ z_2 \end{array} \right) = \left( \begin{array}{c} w_1 w_2 - x_1 x_2 - y_1 y_2 - z_1 z_2 \\\\ w_1 x_2 + x_1 w_2 + y_1 z_2 - z_1 y_2 \\\\ w_1 y_2 - x_1 z_2 + y_1 w_2 + z_1 x_2 \\\\ w_1 z_2 + x_1 y_2 - y_1 x_2 + z_1 w_2 \end{array} \right) \\] + +В библиотеке `quaternion.h` для этой операции используется статический метод `Quaternion::rotate()`: + +```cpp +// Композиция вращений q1 и q2 +Quaternion result = Quaternion::rotate(q1, q2); +``` + +Также полезной является операция применения вращения к вектору, которая делается похожим образом: + +```cpp +// Вращение вектора v кватернионом q +Vector result = Quaternion::rotateVector(v, q); +``` + +Для расчета разницы между двумя ориентациями используется метод `Quaternion::between()`: + +```cpp +// Расчет вращения от q1 к q2 +Quaternion q = Quaternion::between(q1, q2); +``` + +## Дополнительные материалы + +* [Интерактивный учебник по кватернионам](https://eater.net/quaternions). +* [Визуализация вращения вектора с помощью кватернионов](https://quaternions.online). diff --git a/docs/book/js/rotation.js b/docs/book/js/rotation.js new file mode 100644 index 0000000..a092b2c --- /dev/null +++ b/docs/book/js/rotation.js @@ -0,0 +1,262 @@ +import * as THREE from 'three'; +import { SVGRenderer, SVGObject } from 'three/addons/renderers/SVGRenderer.js'; +import { OrbitControls } from 'three/addons/controls/OrbitControls.js'; + +const diagramEl = document.getElementById('rotation-diagram'); + +const scene = new THREE.Scene(); +scene.background = new THREE.Color(0xffffff); + +const camera = new THREE.OrthographicCamera(); + +camera.position.set(9, 26, 20); +camera.up.set(0, 0, 1); +camera.lookAt(0, 0, 0); + +const renderer = new SVGRenderer(); +diagramEl.prepend(renderer.domElement); + +const controls = new OrbitControls(camera, renderer.domElement); +controls.enableZoom = false; + +const LINE_WIDTH = 4; + +function createLabel(text, x, y, z, min = false) { + const label = document.createElementNS('http://www.w3.org/2000/svg', 'text'); + label.setAttribute('class', 'label' + (min ? ' min' : '')); + label.textContent = text; + label.setAttribute('y', -15); + const object = new SVGObject(label); + object.position.x = x; + object.position.y = y; + object.position.z = z; + return object; +} + +function createLine(x1, y1, z1, x2, y2, z2, color) { + const geometry = new THREE.BufferGeometry().setFromPoints([ + new THREE.Vector3(x1, y1, z1), + new THREE.Vector3(x2, y2, z2) + ]); + const material = new THREE.LineBasicMaterial({ color: color, linewidth: LINE_WIDTH, transparent: true, opacity: 0.8 }); + const line = new THREE.Line(geometry, material); + scene.add(line); + return line; +} + +function changeLine(line, x1, y1, z1, x2, y2, z2) { + line.geometry.setFromPoints([new THREE.Vector3(x1, y1, z1), new THREE.Vector3(x2, y2, z2)]); + return line; +} + +function createVector(x1, y1, z1, x2, y2, z2, color, label = '') { + const HEAD_LENGTH = 1; + const HEAD_WIDTH = 0.2; + + const group = new THREE.Group(); + const direction = new THREE.Vector3(x2 - x1, y2 - y1, z2 - z1).normalize(); + const norm = new THREE.Vector3(x2 - x1, y2 - y1, z2 - z1).length(); + let end = new THREE.Vector3(x2, y2, z2); + + if (norm > HEAD_LENGTH) { + end = new THREE.Vector3(x2 - direction.x * HEAD_LENGTH / 2, y2 - direction.y * HEAD_LENGTH / 2, z2 - direction.z * HEAD_LENGTH / 2); + } + + // create line + const geometry = new THREE.BufferGeometry().setFromPoints([new THREE.Vector3(x1, y1, z1), end]); + const material = new THREE.LineBasicMaterial({ color: color, linewidth: LINE_WIDTH, transparent: true, opacity: 0.8 }); + const line = new THREE.Line(geometry, material); + group.add(line); + + if (norm > HEAD_LENGTH) { + // Create arrow + const arrowGeometry = new THREE.ConeGeometry(HEAD_WIDTH, HEAD_LENGTH, 16); + const arrowMaterial = new THREE.MeshBasicMaterial({ color: color }); + const arrow = new THREE.Mesh(arrowGeometry, arrowMaterial); + arrow.position.set(x2 - direction.x * HEAD_LENGTH / 2, y2 - direction.y * HEAD_LENGTH / 2, z2 - direction.z * HEAD_LENGTH / 2); + arrow.lookAt(new THREE.Vector3(x1, y1, z1)); + arrow.rotateX(-Math.PI / 2); + group.add(arrow); + } + + // create label + if (label) group.add(createLabel(label, x2, y2, z2)); + scene.add(group); + return group; +} + +function changeVector(vector, x1, y1, z1, x2, y2, z2, color, label = '') { + vector.removeFromParent(); + return createVector(x1, y1, z1, x2, y2, z2, color, label); +} + +function createDrone(x, y, z) { + const group = new THREE.Group(); + + // Fuselage and wing triangle (main body) + const fuselageGeometry = new THREE.BufferGeometry(); + const fuselageVertices = new Float32Array([ + 1, 0, 0, + -1, 0.6, 0, + -1, -0.6, 0 + ]); + fuselageGeometry.setAttribute('position', new THREE.BufferAttribute(fuselageVertices, 3)); + const fuselageMaterial = new THREE.MeshBasicMaterial({ color: 0xb3b3b3, side: THREE.DoubleSide, transparent: true, opacity: 0.8 }); + const fuselage = new THREE.Mesh(fuselageGeometry, fuselageMaterial); + group.add(fuselage); + + // Tail triangle + const tailGeometry = new THREE.BufferGeometry(); + const tailVertices = new Float32Array([ + -0.2, 0, 0, + -1, 0, 0, + -1, 0, 0.5, + ]); + tailGeometry.setAttribute('position', new THREE.BufferAttribute(tailVertices, 3)); + const tailMaterial = new THREE.MeshBasicMaterial({ color: 0xd80100, side: THREE.DoubleSide, transparent: true, opacity: 0.9 }); + const tail = new THREE.Mesh(tailGeometry, tailMaterial); + group.add(tail); + + group.position.set(x, y, z); + group.scale.set(2, 2, 2); + scene.add(group); + return group; +} + +// Create axes +const AXES_LENGTH = 10; +createVector(0, 0, 0, AXES_LENGTH, 0, 0, 0xd80100, 'x'); +createVector(0, 0, 0, 0, AXES_LENGTH, 0, 0x0076ba, 'y'); +createVector(0, 0, 0, 0, 0, AXES_LENGTH, 0x57ed00, 'z'); + +// Rotation values +const rotationAxisSrc = new THREE.Vector3(2, 1, 3); +let rotationAngle = 0; +let rotationAxis = rotationAxisSrc.clone().normalize(); +let rotationVector = new THREE.Vector3(rotationAxis.x * rotationAngle, rotationAxis.y * rotationAngle, rotationAxis.z * rotationAngle); + +let rotationVectorObj = createVector(0, 0, 0, rotationVector.x, rotationVector.y, rotationVector.z, 0xff9900); +let axisObj = createLine(0, 0, 0, rotationAxis.x * AXES_LENGTH, rotationAxis.y * AXES_LENGTH, rotationAxis.z * AXES_LENGTH, 0xe8e8e8); + +const drone = createDrone(0, 0, 0); + +// UI +const angleInput = diagramEl.querySelector('input[name=angle]'); +const rotationVectorEl = diagramEl.querySelector('.rotation-vector'); +const angleEl = diagramEl.querySelector('.angle'); +const quaternionEl = diagramEl.querySelector('.quaternion'); +const eulerEl = diagramEl.querySelector('.euler'); +diagramEl.querySelector('.axis').innerHTML = `Ось вращения: (${rotationAxisSrc.x}, ${rotationAxisSrc.y}, ${rotationAxisSrc.z}) ∥ (${rotationAxis.x.toFixed(1)}, ${rotationAxis.y.toFixed(1)}, ${rotationAxis.z.toFixed(1)})`; + +function updateScene() { + rotationAngle = parseFloat(angleInput.value) * Math.PI / 180; + rotationVector.set(rotationAxis.x * rotationAngle, rotationAxis.y * rotationAngle, rotationAxis.z * rotationAngle); + rotationVectorObj = changeVector(rotationVectorObj, 0, 0, 0, rotationVector.x, rotationVector.y, rotationVector.z, 0xff9900); + + // rotate drone + drone.rotation.set(0, 0, 0); + drone.rotateOnAxis(rotationAxis, rotationAngle); + + // update labels + angleEl.innerHTML = `Угол вращения: ${parseFloat(angleInput.value).toFixed(0)}° = ${(rotationAngle).toFixed(2)} рад`; + rotationVectorEl.innerHTML = `Вектор вращения: (${rotationVector.x.toFixed(1)}, ${rotationVector.y.toFixed(1)}, ${rotationVector.z.toFixed(1)}) рад`; + + let quaternion = new THREE.Quaternion(); + quaternion.setFromAxisAngle(rotationAxis, rotationAngle); + + quaternionEl.innerHTML = `Кватернион: + + + ( + + cos + ( + + ${rotationAngle.toFixed(2)} + 2 + + ) + + , + + ${rotationAxis.x.toFixed(1)} + · + sin + ( + + ${rotationAngle.toFixed(2)} + 2 + + ) + + , + + ${rotationAxis.y.toFixed(1)} + · + sin + ( + + ${rotationAngle.toFixed(2)} + 2 + + ) + + , + + ${rotationAxis.z.toFixed(1)} + · + sin + ( + + ${rotationAngle.toFixed(2)} + 2 + + ) + + ) + + + = (${quaternion.w.toFixed(1)}, ${(quaternion.x).toFixed(1)}, ${(quaternion.y).toFixed(1)}, ${(quaternion.z).toFixed(1)})`; + + eulerEl.innerHTML = `Углы Эйлера: крен ${(drone.rotation.x * 180 / Math.PI).toFixed(0)}°, + тангаж ${(drone.rotation.y * 180 / Math.PI).toFixed(0)}°, рыскание ${(drone.rotation.z * 180 / Math.PI).toFixed(0)}°`; +} + +function updateCamera() { + const RANGE = 8; + const VERT_SHIFT = 2; + const HOR_SHIFT = -2; + const width = renderer.domElement.clientWidth; + const height = renderer.domElement.clientHeight; + const ratio = width / height; + if (ratio > 1) { + camera.left = -RANGE * ratio; + camera.right = RANGE * ratio; + camera.top = RANGE + VERT_SHIFT; + camera.bottom = -RANGE + VERT_SHIFT; + } else { + camera.left = -RANGE + HOR_SHIFT; + camera.right = RANGE + HOR_SHIFT; + camera.top = RANGE / ratio + VERT_SHIFT; + camera.bottom = -RANGE / ratio + VERT_SHIFT; + } + camera.updateProjectionMatrix(); + renderer.setSize(width, height); +} + +function update() { + // requestAnimationFrame(update); + updateCamera(); + updateScene(); + controls.update(); + renderer.render(scene, camera); +} +update(); + +window.addEventListener('resize', update); +angleInput.addEventListener('input', update); +angleInput.addEventListener('change', update); +diagramEl.addEventListener('mousemove', update); +diagramEl.addEventListener('touchmove', update); +diagramEl.addEventListener('scroll', update); +diagramEl.addEventListener('wheel', update); diff --git a/docs/img/axes-rotation.svg b/docs/img/axes-rotation.svg new file mode 100644 index 0000000..d53bb59 --- /dev/null +++ b/docs/img/axes-rotation.svg @@ -0,0 +1,94 @@ + + + + + + x + y + z + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/img/left-axes.svg b/docs/img/left-axes.svg new file mode 100644 index 0000000..1ecf4e7 --- /dev/null +++ b/docs/img/left-axes.svg @@ -0,0 +1,67 @@ + + + + + + x + z + + + + + + + + + + + + + + + + + + + y + + diff --git a/docs/img/right-axes.svg b/docs/img/right-axes.svg new file mode 100644 index 0000000..ef26c6d --- /dev/null +++ b/docs/img/right-axes.svg @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + x + y + z + + diff --git a/docs/rotation.css b/docs/rotation.css new file mode 100644 index 0000000..0a0c3db --- /dev/null +++ b/docs/rotation.css @@ -0,0 +1,29 @@ +.diagram svg { + display: block; + width: 100%; + height: 400px; +} +.diagram .label { + font-family: Arial, sans-serif; + font-size: 20px; + pointer-events: none; + color: black; + opacity: 0.8; + user-select: none; +} +.diagram label { + display: block; +} +@media (min-width: 800px) { + .diagram b { + width: 200px; + display: inline-block; + } +} +.diagram p.quaternion { + overflow-x: auto; +} +.diagram input { + text-align: center; + width: 100%; +}