w i d e t i l d e t e x t b f q = b e g i n b m a t r i x t e x t b f q 1 t e x t b f q 2 e n d b m a t r i x ,
| widetilde textbfq |= | textbfq1 |+ epsilon(q10q20+ textbfqT1 textbfq2)
| widetilde textbfq|=| textbfq1|+ epsilon fracq10q20+ textbfqT1 textbfq2| textbfq1|
widetilde textbfq∗= beginbmatrix textbfq∗1 textbfq∗2 endbmatrix
widetilde textbfq pm widetilde textbfp= beginbmatrix textbfq1 pm textbfp1 textbfq2 pm textbfp2 endbmatrix
a widetilde textbfq= widetilde textbfqa= beginbmatrixa textbfq1a textbfq2 endbmatrix
widetilde textbfq otimes widetilde textbfp= beginbmatrix textbfq1 otimes textbfp1 textbfq1 otimes textbfp2+ textbfq2 otimes textbfp1 endbmatrix
widetilde textbfq−1= frac widetilde textbfq∗ | widetilde textbfq |
\ textbf {q} _1 = \ begin {bmatrix} \ cos \ frac {\ psi} {2} \ cos \ frac {\ vartheta} {2} \ cos \ frac {\ gamma} {2} & - & \ sin \ frac {\ psi} {2} \ sin \ frac {\ vartheta} {2} \ sin \ frac {\ gamma} {2} \\ \ cos \ frac {\ psi} {2} \ cos \ frac { \ vartheta} {2} \ sin \ frac {\ gamma} {2} & + & \ sin \ frac {\ psi} {2} \ sin \ frac {\ vartheta} {2} \ cos \ frac {\ gamma} {2} \\ \ cos \ frac {\ psi} {2} \ sin \ frac {\ vartheta} {2} \ sin \ frac {\ gamma} {2} & + & \ sin \ frac {\ psi} { 2} \ cos \ frac {\ vartheta} {2} \ cos \ frac {\ gamma} {2} \\ \ cos \ frac {\ psi} {2} \ sin \ frac {\ vartheta} {2} \ cos \ frac {\ gamma} {2} & - & \ sin \ frac {\ psi} {2} \ cos \ frac {\ vartheta} {2} \ sin \ frac {\ gamma} {2} \ end {bmatrix}
textbfq2= frac12 textbfr otimes textbfq1
psi= arctan frac2(q0q2−q1q3)q20+q21−q22−q23
vartheta= arcsin(2(q1q2+q0q3))
gamma= arctan frac2(q0q1−q2q3)q20−q21+q22−q23
textbfr=2 textbfq2 otimes textbfq−11
textbfr0= widetilde textbfq otimes textbfr otimes widetilde textbfq−1
textbfr= widetilde textbfq−1 otimes textbfr0 otimes widetilde textbfq
Function | Description |
---|---|
DualQuaternion.dq | The body of the biquaternion in the form of an array of 8 numbers |
DualQuaternion(dq0, dq1, dq2, dq3, dq4, dq5, dq6, dq7) | A constructor that defines a biquaternion by setting all eight numbers. |
DualQuaternion.fromEulerVector(psi, theta, gamma, v) | Get biquaternion by specifying the orientation of the object by the Euler angles and the object position vector |
DualQuaternion.getEulerVector() | Get Euler angles and position vector from biquaternion |
DualQuaternion.getVector() | Get position vector from biquaternion |
DualQuaternion.getReal() | Get the real part of the biquaternion (determines the orientation of the object in space) |
DualQuaternion.getDual() | Get the dual part of the biquaternion (determines the position of the object in space) |
DualQuaternion.norm() | Get the rate of the biquaternion in the form of a dual number |
DualQuaternion.mod() | Get the biquaternion module as a dual number |
DualQuaternion.conjugate() | Get conjugated biquaternion |
DualQuaternion.inverse() | Get reverse biquaternion |
DualQuaternion.mul(DQ2) | Biquaternion multiplication |
DualQuaternion.toString() | Convert the biquaternion to a line, for example, for output to a debug console |
/** * * Author 2017, Akhramovich A. Sergey (akhramovichsa@gmail.com) * see https://github.com/infusion/Quaternion.js */ // 'use strict'; /** * Dual Quaternion constructor * * @constructor * @returns {DualQuaternion} */ function DualQuaternion(dq0, dq1, dq2, dq3, dq4, dq5, dq6, dq7) { if (dq0 === undefined) { this.dq = [1, 0, 0, 0, 0, 0, 0, 0]; } else { this.dq = [dq0, dq1, dq2, dq3, dq4, dq5, dq6, dq7]; } return this; }; // Получение кватерниона по углам Эйлера и вектору положения DualQuaternion['fromEulerVector'] = function(psi, theta, gamma, v) { var q_real = new Quaternion.fromEuler(psi, theta, gamma); var q_v = new Quaternion(0, v[0], v[1], v[2]); var q_dual = q_v.mul(q_real); return new DualQuaternion( q_real.q[0], q_real.q[1], q_real.q[2], q_real.q[3], q_dual.q[0]*0.5, q_dual.q[1]*0.5, q_dual.q[2]*0.5, q_dual.q[3]*0.5); }; DualQuaternion.prototype = { 'dq': [1, 0, 0, 0, 0, 0, 0, 0], /** * Получение углов Эйлера (psi, theta, gamma) и вектора положения из бикватерниона */ 'getEulerVector': function() { var euler_angles = this.getReal().getEuler(); var q_dual = this.getDual(); var q_dual_2 = new Quaternion(2.0*q_dual.q[0], 2.0*q_dual.q[1], 2.0*q_dual.q[2], 2.0*q_dual.q[3]); var q_vector = q_dual_2.mul(this.getReal().conjugate()); return [euler_angles[0], euler_angles[1], euler_angles[2], q_vector.q[1], q_vector.q[2], q_vector.q[3]]; }, /** * Получение только вектора положения из бикватерниона */ 'getVector': function() { var euler_vector = this.getEulerVector(); return [euler_vector[3], euler_vector[4], euler_vector[5]]; }, /** * Получить действительную часть бикватерниона * @returns {Quaternion} */ 'getReal': function() { return new Quaternion(this.dq[0], this.dq[1], this.dq[2], this.dq[3]); }, /** * Получить дуальную часть бикватерниона * @returns {Quaternion} */ 'getDual': function() { return new Quaternion(this.dq[4], this.dq[5], this.dq[6], this.dq[7]); }, /** * Норма бикватерниона * Внимание! Возвращает дуальное число! */ 'norm': function() { return [Math.pow(this.dq[0], 2) + Math.pow(this.dq[1], 2) + Math.pow(this.dq[2], 2) + Math.pow(this.dq[3], 2), this.dq[0]*this.dq[4] + this.dq[1]*this.dq[5] + this.dq[2]*this.dq[6] + this.dq[3]*this.dq[7]]; }, /** * Модуль бикватерниона * Внимание! Возвращает дуальное число! */ 'mod': function() { var q_real_mod = Math.sqrt(Math.pow(this.dq[0], 2) + Math.pow(this.dq[1], 2) + Math.pow(this.dq[2], 2) + Math.pow(this.dq[3], 2)); return [q_real_mod, (this.dq[0]*this.dq[4] + this.dq[1]*this.dq[5] + this.dq[2]*this.dq[6] + this.dq[3]*this.dq[7])/q_real_mod]; }, /** * Сопряженный бикватернион * DQ' := (dq0, -dq1, -dq2, -dq3, dq4, -dq5, -dq6, -dq7) */ 'conjugate': function() { return new DualQuaternion(this.dq[0], -this.dq[1], -this.dq[2], -this.dq[3], this.dq[4], -this.dq[5], -this.dq[6], -this.dq[7]); }, // Вычислить обратный бикватернион 'inverse': function() { var q_real_norm = new Quaternion(this.dq[0], this.dq[1], this.dq[2], this.dq[3]).norm(); var dq_norm_inv = [q_real_norm, - (this.dq[0]*this.dq[4] + this.dq[1]*this.dq[5] + this.dq[2]*this.dq[6] + this.dq[3]*this.dq[7])/q_real_norm]; var dq_conj = this.conjugate(); // Умножение бикватерниона на дуальное число return new DualQuaternion( dq_norm_inv[0] * dq_conj.dq[0], dq_norm_inv[0] * dq_conj.dq[1], dq_norm_inv[0] * dq_conj.dq[2], dq_norm_inv[0] * dq_conj.dq[3], dq_norm_inv[0] * dq_conj.dq[4] + dq_norm_inv[1] * dq_conj.dq[0], dq_norm_inv[0] * dq_conj.dq[5] + dq_norm_inv[1] * dq_conj.dq[1], dq_norm_inv[0] * dq_conj.dq[6] + dq_norm_inv[1] * dq_conj.dq[2], dq_norm_inv[0] * dq_conj.dq[7] + dq_norm_inv[1] * dq_conj.dq[3]); }, /** * Бикватернионное умножение * q1_real*q2_real, q1_real*q2_dual + q1_dual*q2_real */ 'mul': function(DQ2) { var q1_real = this.getReal(); var q1_dual = this.getDual(); var q2_real = DQ2.getReal(); var q2_dual = DQ2.getDual(); var q_res_real = q1_real.mul(q2_real); var q_res_dual_1 = q1_real.mul(q2_dual); var q_res_dual_2 = q1_dual.mul(q2_real); return new DualQuaternion( q_res_real.q[0], q_res_real.q[1], q_res_real.q[2], q_res_real.q[3], q_res_dual_1.q[0] + q_res_dual_2.q[0], q_res_dual_1.q[1] + q_res_dual_2.q[1], q_res_dual_1.q[2] + q_res_dual_2.q[2], q_res_dual_1.q[3] + q_res_dual_2.q[3]); }, /** * Преобразование вектора бикватернионом */ 'transformVector': function (v) { var dq_res = this.mul(new DualQuaternion(1, 0, 0, 0, 0, v[0], v[1], v[2])).mul(this.conjugate()); return [dq_res.dq[5], dq_res.dq[6], dq_res.dq[7]]; }, /** * Преобразовать в строку, для отладки */ 'toString': function() { return '[' + this.dq[0].toString() + ', ' + this.dq[1].toString() + ', ' + this.dq[2].toString() + ', ' + this.dq[3].toString() + ', ' + this.dq[4].toString() + ', ' + this.dq[5].toString() + ', ' + this.dq[6].toString() + ', ' + this.dq[7].toString() + ']'; } } /* // TEST: var dq1 = new DualQuaternion.fromEulerVector(0 * Math.PI/180.0, 0 * Math.PI/180, 0 * Math.PI/180, [10, 20, 30]); console.log(dq1.toString()); console.log('getEulerVector = ', dq1.getEulerVector()); console.log('norm = ', dq1.norm()); console.log('mod = ', dq1.mod()); console.log('conjugate = ', dq1.conjugate().dq); console.log('inverse = ', dq1.inverse().dq); var dq2 = new DualQuaternion.fromEulerVector(0 * Math.PI/180.0, 0 * Math.PI/180, 0 * Math.PI/180, [10, 0, 0]); console.log('mul = ', dq1.mul(dq2).dq); console.log('transformVector ??? = ', dq1.transformVector([0, 0, 0])); */
Ship
and Gun
. In the class constructor of the ship, its form in the form of biquaternion points, the initial orientation and position on the map in the form of biquaternion this.dq_pos
. function Ship(ctx, v) { this.ctx = ctx; this.dq_pos = new DualQuaternion.fromEulerVector(0*Math.PI/180, 0, 0, v); // Форма корабля this.dq_forward_left = new DualQuaternion.fromEulerVector(0, 0, 0, [ 15, 0, -10]); this.dq_forward_right = new DualQuaternion.fromEulerVector(0, 0, 0, [ 15, 0, 10]); this.dq_backward_left = new DualQuaternion.fromEulerVector(0, 0, 0, [-15, 0, -10]); this.dq_backward_right = new DualQuaternion.fromEulerVector(0, 0, 0, [-15, 0, 10]); this.dq_forward_forward = new DualQuaternion.fromEulerVector(0, 0, 0, [ 30, 0, 0]); // Приращения текущей позиции при управлении this.dq_dx_left = new DualQuaternion.fromEulerVector( 1*Math.PI/180, 0, 0, [0, 0, 0]); this.dq_dx_right = new DualQuaternion.fromEulerVector(-1*Math.PI/180, 0, 0, [0, 0, 0]); this.dq_dx_forward = new DualQuaternion.fromEulerVector(0, 0, 0, [ 1, 0, 0]); this.dq_dx_backward = new DualQuaternion.fromEulerVector(0, 0, 0, [-1, 0, 0]); return this; };
Ship.draw()
implemented. Note the application of the biquaternion multiplication operation of each point of the ship by the current position and orientation of the ship to the biquaternion. Ship.prototype = { 'ctx': 0, 'dq_pos': new DualQuaternion.fromEulerVector(0, 0, 0, 0, 0, 0), /** * Нарисовать кораблик */ 'draw': function() { // Переместить все точки кораблика с помощью бикватернионного умножения v_pos = this.dq_pos.getVector(); v_forward_left = this.dq_pos.mul(this.dq_forward_left).getVector(); v_forward_right = this.dq_pos.mul(this.dq_forward_right).getVector(); v_backward_left = this.dq_pos.mul(this.dq_backward_left).getVector(); v_backward_right = this.dq_pos.mul(this.dq_backward_right).getVector(); v_forward_forward = this.dq_pos.mul(this.dq_forward_forward).getVector(); // Непосредственно рисование ctx.beginPath(); ctx.moveTo(v_backward_left[0], v_backward_left[2]); ctx.lineTo(v_forward_left[0], v_forward_left[2]); ctx.lineTo(v_forward_left[0], v_forward_left[2]); ctx.lineTo(v_forward_forward[0], v_forward_forward[2]); ctx.lineTo(v_forward_right[0], v_forward_right[2]); ctx.lineTo(v_backward_right[0], v_backward_right[2]); ctx.lineTo(v_backward_left[0], v_backward_left[2]); ctx.stroke(); ctx.closePath(); } };
this.dq_pos
. Also set and binding to the ship on which it is installed. The instrument on the ship can only rotate, so the biquaternion increments when controlling the instrument will change only the real part of the biquaternion, which sets the angle of rotation. In this example, the tool is guided with the mouse cursor, so the tool will rotate instantly. function Gun(ctx, ship, v) { this.ctx = ctx; this.ship = ship; // Позиция орудия относительно корабля this.dq_pos = new DualQuaternion.fromEulerVector(0, 0, 0, v); // Форма орудия this.dq_forward = new DualQuaternion.fromEulerVector(0, 0, 0, [20, 0, 0]); this.dq_backward = new DualQuaternion.fromEulerVector(0, 0, 0, [ 0, 0, 0]); // Вращение орудия при управлении this.dq_dx_left = new DualQuaternion.fromEulerVector( 1*Math.PI/180, 0, 0, [0, 0, 0]); this.dq_dx_right = new DualQuaternion.fromEulerVector(-1*Math.PI/180, 0, 0, [0, 0, 0]); return this; };
Ship.draw()
also implemented. The tool is displayed as a line, which is defined by two points this.dq_backward
and this.dq_forward
. To determine the coordinates of the instrument points, the operation of biquaternion multiplication is used. Gun.prototype = { 'ctx': 0, 'ship': 0, 'dq_pos': new DualQuaternion.fromEulerVector(0, 0, 0, [0, 0, 0]), /** * Нарисовать орудие */ 'draw': function() { // Переместить орудие относительно корабля v_pos = this.ship.dq_pos.getVector(); v_forward = this.ship.dq_pos.mul(this.dq_backward).mul(this.dq_forward).getVector(); v_backward = this.ship.dq_pos.mul(this.dq_backward).getVector(); // Непосредственно рисование ctx.beginPath(); ctx.moveTo(v_backward[0], v_backward[2]); ctx.lineTo(v_forward[0], v_forward[2]); ctx.stroke(); ctx.closePath(); } };
leftPressed, upPressed, rightPressed, downPressed
, which are processed in the main program loop. leftPressed = false; rightPressed = false; upPressed = false; downPressed = false; dq_mouse_pos = new DualQuaternion.fromEulerVector(0, 0, 0, [0, 0, 0]); document.addEventListener("keydown", keyDownHandler, false); document.addEventListener("keyup", keyUpHandler, false); document.addEventListener("mousemove", mouseMoveHandler, false); // Обработка нажатия клавиш управления function keyDownHandler(e) { if (e.keyCode == 37 || e.keyCode == 65 || e.keyCode == 97) { leftPressed = true; } // влево A else if (e.keyCode == 38 || e.keyCode == 87 || e.keyCode == 119) { upPressed = true; } // вверх W else if (e.keyCode == 39 || e.keyCode == 68 || e.keyCode == 100) { rightPressed = true; } // вправо D else if (e.keyCode == 40 || e.keyCode == 83 || e.keyCode == 115) { downPressed = true; } // вниз S } // Обработка отжатия клавиш управления function keyUpHandler(e) { if (e.keyCode == 37 || e.keyCode == 65 || e.keyCode == 97) { leftPressed = false; } // влево A else if (e.keyCode == 38 || e.keyCode == 87 || e.keyCode == 119) { upPressed = false; } // вверх W else if (e.keyCode == 39 || e.keyCode == 68 || e.keyCode == 100) { rightPressed = false; } // вправо D else if (e.keyCode == 40 || e.keyCode == 83 || e.keyCode == 115) { downPressed = false; } // вниз S }
dq_mouse_pos
biquaternion. Then, the biquaternion position of the mouse relative to the ship is calculated using biquaternion multiplication. The biquaternion of the ship is taken away from the mouse dq_mouse_pos_about_ship = ship_1.dq_pos.inverse().mul(dq_mouse_pos);
gun_1.dq_backward
assigned the resulting value. function mouseMoveHandler(e) { var relativeX = e.clientX - canvas.offsetLeft; var relativeY = e.clientY - canvas.offsetTop; // Обрабатывать события только когда курсор мышки находится в игровой области if (relativeX > 0 && relativeX < canvas.width && relativeY > 0 && relativeY < canvas.height) { // Бикватернион положения мышки dq_mouse_pos = new DualQuaternion.fromEulerVector(0, 0, 0, [relativeX, 0, relativeY]); // Бикватернион положения мышки относительно корабля // Направление орудия. От координат мышки отнимается координаты корабля // Последовательность бикватернионного умножения важна // DQ_ship^(-1) * DQ_mouse dq_mouse_pos_about_ship = ship_1.dq_pos.inverse().mul(dq_mouse_pos); // Угол между векторами орудия и мышки q_gun_mouse = new Quaternion.fromBetweenVectors(gun_1.dq_forward.getVector(), dq_mouse_pos_about_ship.getVector()); dq_gun_mouse = new DualQuaternion(q_gun_mouse.q[0], q_gun_mouse.q[1], q_gun_mouse.q[2], q_gun_mouse.q[3], 0, 0, 0, 0); gun_1.dq_backward = dq_gun_mouse; // console.log(dq_gun_mouse.getEulerVector()); // console.log(relativeX + ' ' + relativeY + ' ' + gun_1.dq_forward.toString()); } }
ship_1
and gun_1
tools ship_1
gun_1
, debug information is output and the control of the ship is processed. var canvas = document.getElementById("myCanvas"); var ctx = canvas.getContext("2d"); ship_1 = new Ship(ctx, [100, 0, 100]); gun_1 = new Gun(ctx, ship_1, [0, 0, 0]); function draw() { ctx.clearRect(0, 0, canvas.width, canvas.height); ship_1.draw(); gun_1.draw(); // Debug info ship_euler_vector = ship_1.dq_pos.getEulerVector(); ship_euler_vector[0] = ship_euler_vector[0]*180/Math.PI; ship_euler_vector[1] = ship_euler_vector[1]*180/Math.PI; ship_euler_vector[2] = ship_euler_vector[2]*180/Math.PI; ship_euler_vector = ship_euler_vector.map(function(each_element){ return each_element.toFixed(2); }); ship_dq = ship_1.dq_pos.dq.map(function(each_element){ return each_element.toFixed(2); }); gun_dq = ship_1.dq_pos.mul(gun_1.dq_backward).dq.map(function(each_element){ return each_element.toFixed(2); }); ctx.font = "8pt Courier"; ctx.fillText("Ship: " + ship_dq + " | psi, theta, gamma, vector:" + ship_euler_vector, 10, 20); ctx.fillText("Gun: " + gun_dq, 10, 40); // Управление корабликом if (leftPressed) { ship_1.dq_pos = ship_1.dq_pos.mul(ship_1.dq_dx_left); } if (rightPressed) { ship_1.dq_pos = ship_1.dq_pos.mul(ship_1.dq_dx_right); } if (upPressed) { ship_1.dq_pos = ship_1.dq_pos.mul(ship_1.dq_dx_forward); } if (downPressed) { ship_1.dq_pos = ship_1.dq_pos.mul(ship_1.dq_dx_backward); } requestAnimationFrame(draw); } draw();
Source: https://habr.com/ru/post/436210/