クォータニオンとか

# クォータニオン

**クォータニオン（四元数・しげんすう）** は、3D空間での回転（方向）を表現するための数学的なツールです。

## 四元数の表記
通常、4つの成分で構成されます。

```math
(w, x, y, z)
```

次の式で表されます。

```math
Q = w + xi + yj + zk
```

## 正規化（normalize）
クォータニオンを**正規化**することで、**単位四元数**となり、回転（方向）を表現するという目的から、この**単位四元数**のみを扱います。

```kotlin:Quaternion.kt
data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
    fun normalize(): Quaternion {
        val norm = kotlin.math.sqrt(w * w + x * x + y * y + z * z)
        return Quaternion(w / norm, x / norm, y / norm, z / norm)
    }
}
```

# 主な特性

## 乗算（外積）
クォータニオンの外積は**回転の合成**を意味し、次のように計算します。

```math
Q = Q_1 Q_2
```

```kotlin:Quaternion.kt
data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
    fun normalize(): Quaternion {
        val norm = kotlin.math.sqrt(w * w + x * x + y * y + z * z)
        return Quaternion(w / norm, x / norm, y / norm, z / norm)
    }
    
    operator fun times(q: Quaternion): Quaternion {
        return Quaternion(
            w * q.w - x * q.x - y * q.y - z * q.z,
            w * q.x + x * q.w + y * q.z - z * q.y,
            w * q.y - x * q.z + y * q.w + z * q.x,
            w * q.z + x * q.y - y * q.x + z * q.w
        ).normalize()
    }
}
```


## 内積（ドット積）
内積でクォータニオンの類似度を測り、回転角度を計算するのに利用します。

```math
Q_1 \cdot Q_2 = w_1 w_2 + x_1 x_2 + y_1 y_2 + z_1 z_2
```

```kotlin:Quaternion.kt
data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
    fun normalize(): Quaternion {
        val norm = kotlin.math.sqrt(w * w + x * x + y * y + z * z)
        return Quaternion(w / norm, x / norm, y / norm, z / norm)
    }

    fun dotProduct(q: Quaternion): Float {
        return w * q.w + x * q.x + y * q.y + z * q.z
    }

```

**回転角度の計算**
二つのクォータニオン間の回転角度を計算するための方法

```kotlin
data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
    fun normalize(): Quaternion {
        val norm = kotlin.math.sqrt(w * w + x * x + y * y + z * z)
        return Quaternion(w / norm, x / norm, y / norm, z / norm)
    }

    fun dotProduct(q: Quaternion): Float {
        return w * q.w + x * q.x + y * q.y + z * q.z
    }

    fun angleTo(q: Quaternion): Float {
        val dotProduct = this.dotProduct(q)
        return (2 * kotlin.math.acos(dotProduct)).toFloat()
    }
}
```

## 逆クォータニオン(共役)

あるクォータニオン `Quaternion(w, x, y, z)` を単位クォータニオン `Quaternion(1.0f, 0.0f, 0.0f, 0.0f)` に回転するには、逆クォータニオン(共役)を乗算します。
単位クォータニオンにおいて、クォータニオン `Quaternion(w, x, y, z)`の逆クォータニオンは、`Quaternion(w, -x, -y, -z)`です。

```kotlin:Quaternion.kt
data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
    fun normalize(): Quaternion {
        val norm = kotlin.math.sqrt(w * w + x * x + y * y + z * z)
        return Quaternion(w / norm, x / norm, y / norm, z / norm)
    }

    fun conjugate(): Quaternion {
        return Quaternion(w, -x, -y, -z)
    }

    operator fun times(q: Quaternion): Quaternion {
        return Quaternion(
            w * q.w - x * q.x - y * q.y - z * q.z,
            w * q.x + x * q.w + y * q.z - z * q.y,
            w * q.y - x * q.z + y * q.w + z * q.x,
            w * q.z + x * q.y - y * q.x + z * q.w
        ).normalize()
    }
}

```



## オイラー角との関係
クォータニオンとオイラー角（ヨー、ピッチ、ロール）は相互に変換可能です。

**オイラー角をクォータニオンに変換**

```kotlin
fun eulerToQuaternion(yaw: Float, pitch: Float, roll: Float): Quaternion {
    val cy = kotlin.math.cos(yaw / 2)
    val sy = kotlin.math.sin(yaw / 2)
    val cp = kotlin.math.cos(pitch / 2)
    val sp = kotlin.math.sin(pitch / 2)
    val cr = kotlin.math.cos(roll / 2)
    val sr = kotlin.math.sin(roll / 2)

    return Quaternion(
        w = cy * cp * cr + sy * sp * sr,
        x = cy * cp * sr - sy * sp * cr,
        y = sy * cp * sr + cy * sp * cr,
        z = sy * cp * cr - cy * sp * sr
    ).normalize()
}
```

**クォータニオンをオイラー角に変換**

```kotlin
fun quaternionToEuler(q: Quaternion): EulerAngles {
    val ysqr = q.y * q.y

    val t0 = 2.0f * (q.w * q.x + q.y * q.z)
    val t1 = 1.0f - 2.0f * (q.x * q.x + ysqr)
    val roll = kotlin.math.atan2(t0, t1)

    var t2 = 2.0f * (q.w * q.y - q.z * q.x)
    t2 = if (t2 > 1.0f) 1.0f else if (t2 < -1.0f) -1.0f else t2
    val pitch = kotlin.math.asin(t2)

    val t3 = 2.0f * (q.w * q.z + q.x * q.y)
    val t4 = 1.0f - 2.0f * (ysqr + q.z * q.z)
    val yaw = kotlin.math.atan2(t3, t4)

    return EulerAngles(yaw, pitch, roll)
}
```

## ジンバルロック

**ロール（Roll）とヨー（Yaw）**
ロールとヨーは、**X軸**と**Z軸**周りの回転です。それぞれ、次のように計算されます。

```math
\text{Roll} = \arctan \left( \frac{2(w x + y z)}{1 - 2(x^2 + y^2)} \right)
```

```math
\text{Yaw} = \arctan \left( \frac{2(w z + x y)}{1 - 2(y^2 + z^2)} \right)
```

**ピッチ（Pitch）**
ピッチは、**Y軸**周りの回転です。次のように計算されます。

```math
\text{Pitch} = \arcsin \left( 2(w y - z x) \right)
```

ピッチの計算にはアークサインが使われ、他の軸とは異なります。これは、Y軸周りの回転が±90度の範囲での回転を表し、**ジンバルロック**の影響を受けやすいためです。具体的には、**ピッチが90度または-90度に近づくと、回転の特性が変わり、ロールとヨーの計算が安定しなくなります。**

## 東西南北の表現

**東（90度のヨー回転）**
```kotlin
val eastQuaternion = Quaternion(
    w = kotlin.math.cos(Math.PI.toFloat() / 4),
    x = 0f,
    y = 0f,
    z = kotlin.math.sin(Math.PI.toFloat() / 4)
).normalize()
```

**西（-90度のヨー回転）**
```kotlin
val westQuaternion = Quaternion(
    w = kotlin.math.cos(-Math.PI.toFloat() / 4),
    x = 0f,
    y = 0f,
    z = kotlin.math.sin(-Math.PI.toFloat() / 4)
).normalize()
```

**南（180度のヨー回転）**
```kotlin
val southQuaternion = Quaternion(
    w = kotlin.math.cos(Math.PI.toFloat() / 2),
    x = 0f,
    y = 0f,
    z = kotlin.math.sin(Math.PI.toFloat() / 2)
).normalize()
```

**（北0度の回転、初期状態）**
```kotlin
val northQuaternion = Quaternion(
    w = 1f,
    x = 0f,
    y = 0f,
    z = 0f
).normalize()
```

# Madgwick Filter

**アルゴリズムと実装リファレンス**

https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

**ライブラリー（C/Python）**

https://github.com/xioTechnologies/Fusion

**android実装**

https://github.com/IdeoG/android-madgwick

## サンプルプログラム（Android）

https://github.com/jp-96/android-ahrs

# フィルターの実装（いろいろ）

https://github.com/Mayitzin/ahrs


# 加速度センサーのみを使ったクオータニオン

```kotlin
import kotlin.math.sqrt

data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
    fun normalize(): Quaternion {
        val norm = sqrt(w * w + x * x + y * y + z * z)
        return Quaternion(w / norm, x / norm, y / norm, z / norm)
    }
}

fun calculateQuaternionFromAccelerometer(ax: Float, ay: Float, az: Float): Quaternion {
    // 加速度データの正規化
    val norm = sqrt(ax * ax + ay * ay + az * az)
    val axN = ax / norm
    val ayN = ay / norm
    val azN = az / norm

    // クォータニオンの計算（ここでは簡略化した例を使用）
    val w = sqrt((azN + 1.0f) / 2.0f)
    val x = -ayN / (2.0f * w)
    val y = axN / (2.0f * w)
    val z = 0.0f

    return Quaternion(w, x, y, z).normalize()
}

```

```kotlin
import kotlin.math.sqrt

data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
    fun normalize(): Quaternion {
        val norm = sqrt(w * w + x * x + y * y + z * z)
        return Quaternion(w / norm, x / norm, y / norm, z / norm)
    }
}

fun calculateQuaternionFromAccelerometer(ax: Float, ay: Float, az: Float): Quaternion {
    // 加速度データの正規化
    val norm = sqrt(ax * ax + ay * ay + az * az)
    val axN = ax / norm
    val ayN = ay / norm
    val azN = az / norm

    // クォータニオンの計算
    val qw = sqrt((azN + 1.0f) / 2.0f)
    val qx = -ayN / sqrt(2 * (1.0f + azN))
    val qy = axN / sqrt(2 * (1.0f + azN))
    val qz = 0.0f

    return Quaternion(qw, qx, qy, qz).normalize()
}

```

# 加速度センサーと磁場センサーを使ったクォータニオン

https://ahrs.readthedocs.io/en/latest/filters/famc.html

**論文：Fast Accelerometer-Magnetometer Combination (FAMC)**

> Zhuohua Liu, Wei Liu, Xiangyang Gong, and Jin Wu. Simplified attitude determination algorithm using accelerometer and magnetometer with extremely low execution time. Journal of Sensors, 2018. URL: [https://onlinelibrary.wiley.com/doi/abs/10.1155/2018/8787236, doi:10.1155/2018/8787236.](https://onlinelibrary.wiley.com/doi/full/10.1155/2018/8787236)

https://onlinelibrary.wiley.com/doi/full/10.1155/2018/8787236

https://github.com/zarathustr/Analytic-AMC

**python ライブラリーの実装**

https://github.com/Mayitzin/ahrs/blob/master/ahrs/filters/famc.py

**MATLABからの移植**

```kotlin
import kotlin.math.sqrt

/**
 * Fast Accelerometer-Magnetometer Combination
 * https://onlinelibrary.wiley.com/doi/full/10.1155/2018/8787236
 */
object FastAmc {

    /**
     *
     * @param x
     * @see "https://stackoverflow.com/questions/73463413/how-to-implement-the-fast-inverse-square-root-with-kotlin"
     */
    private fun fastInverseSqrt(x: Float): Float {
        val i = x.toBits()
        val y = Float.fromBits(0x5f3759df - (i shr 1))
        return y * (1.5F - (x * 0.5F * y * y))
    }

    /**
     * https://onlinelibrary.wiley.com/doi/full/10.1155/2018/8787236
     */
    fun estimate(
        accelX: Float, accelY: Float, accelZ: Float, magneX: Float, magneY: Float, magneZ: Float
    ): FloatArray {
        /**
         *
         * https://github.com/zarathustr/Analytic-AMC/blob/master/FAMC.m
         *
         * % A Simplified Analytic Attitude Determination Algorithm Using Accelerometer and Magnetometer
         * % Fast Accelerometer-Magnetometer Combination (FAMC) algorithm by Zhuohua Liu and Jin Wu
         * %
         * % author: Zhuohua Liu, Jin Wu
         * % e-mail: liuzhuohua@bupt.edu.cn; jin_wu_uestc@hotmail.com;
         *
         * function q = FAMC(Ab, Mb)
         *
         *     ax = Ab(1);       ay = Ab(2);       az = Ab(3);
         *     mx = Mb(1);       my = Mb(2);       mz = Mb(3);
         *
         *     mD = dot(Ab, Mb);
         *     mN = sqrt(1 - mD^2);
         *
         *     B11 = (mN * mx) / 2; B13 = ax / 2 + (mD * mx) / 2;
         *     B21 = (mN * my) / 2; B23 = ay / 2 + (mD * my) / 2;
         *     B31 = (mN * mz) / 2; B33 = az / 2 + (mD * mz) / 2;
         *
         *     tau = (B13 + B31);
         *
         *     p1  = B33 - B11 + 1;
         *     A11 = -1 / p1;
         *     A12 = B21 / p1;
         *     A13 = tau / p1;
         *
         *     p2  = (-B21^2 / p1 + B11 + B33 + 1);
         *     A21 =  -B21 / (p1 * p2);
         *     A22 =  -1 / p2;
         *     A23 = ( B23 + B21 * tau / p1) / p2;
         *
         *     p3  = p1 - 2 + tau^2 / p1 + A23^2 * p2;
         *
         *     A31 = (tau / p1 + B21 * A23 / p1) / p3;
         *     A32 = A23 / p3;
         *     A33 = 1 / p3;
         *
         *     a = B23 * (A11 + A12 * (A21 + A23 * A31) + A13*A31) ...
         *         - (B13 - B31) * (A21 + A23 * A31) - A31 * B21;
         *     b = B23 * (A12 * (A22 + A23 * A32) + A13 * A32) ...
         *         - (B13 - B31) * (A22 + A23 * A32) - A32 * B21;
         *
         *     c = B23 * (A13 * A33 + A12 * A23 * A33) - A33 * B21 - A23 * A33 * (B13 - B31);
         *
         *     q = [-1; a; b; c];
         *     q = q ./ norm(q);
         * end
         */

        var fis: Float

        // Accelerometer
        var ax = accelX
        var ay = accelY
        var az = accelZ
        fis = fastInverseSqrt(ax * ax + ay * ay + az * az)
        ax *= fis
        ay *= fis
        az *= fis

        // Magnetometer
        var mx = magneX
        var my = magneY
        var mz = magneZ
        fis = fastInverseSqrt(mx * mx + my * my + mz * mz)
        mx *= fis
        my *= fis
        mz *= fis

        val mD = ax * mx + ay * my + az * mz
        val mN = sqrt(1 - mD * mD)

        val B11 = (mN * mx) / 2
        val B13 = ax / 2 + (mD * mx) / 2
        val B21 = (mN * my) / 2
        val B23 = ay / 2 + (mD * my) / 2
        val B31 = (mN * mz) / 2
        val B33 = az / 2 + (mD * mz) / 2

        val tau = (B13 + B31)

        val p1 = B33 - B11 + 1
        val A11 = -1 / p1
        val A12 = B21 / p1
        val A13 = tau / p1

        val p2 = (-B21 * B21 / p1 + B11 + B33 + 1)
        val A21 = -B21 / (p1 * p2)
        val A22 = -1 / p2
        val A23 = (B23 + B21 * tau / p1) / p2

        val p3 = p1 - 2 + tau * tau / p1 + A23 * A23 * p2
        val A31 = (tau / p1 + B21 * A23 / p1) / p3
        val A32 = A23 / p3
        val A33 = 1 / p3

//        var a = B23 * (A11 + A12 * (A21 + A23 * A31) + A13 * A31) -
//                (B13 - B31) * (A21 + A23 * A31) -
//                A31 * B21
        var a = (B13 - B31) * (A21 + A23 * A31) +
                A31 * B21 -
                B23 * (A11 + A12 * (A21 + A23 * A31) + A13 * A31)
//        var b = B23 * (A12 * (A22 + A23 * A32) + A13 * A32) -
//                (B13 - B31) * (A22 + A23 * A32) -
//                A32 * B21
        var b = (B13 - B31) * (A22 + A23 * A32) +
                A32 * B21 -
                B23 * (A12 * (A22 + A23 * A32) + A13 * A32)
//        var c = B23 * (A13 * A33 + A12 * A23 * A33) -
//                A33 * B21 -
//                A23 * A33 * (B13 - B31)
        var c = A33 * B21 +
                A23 * A33 * (B13 - B31) -
                B23 * (A13 * A33 + A12 * A23 * A33)
//        var w = -1.0f
        var w = 1.0f
        fis = fastInverseSqrt(w * w + a * a + b * b + c * c)
        w *= fis
        a *= fis
        b *= fis
        c *= fis

        return floatArrayOf(w, a, b, c)
    }
}

```

## CoordinateSystem (座標系)

micro:bitの座標系は、CoordinateSystemで管理されており、コメントで図示されています。

https://github.com/lancaster-university/microbit-dal/blob/master/inc/types/CoordinateSystem.h#L30

https://github.com/lancaster-university/microbit-dal/blob/master/source/types/CoordinateSystem.cpp

# micro:bit で、 FAMC

```ts:custom.ts
/**
* Use this file to define custom functions and blocks.
* Read more at https://makecode.microbit.org/blocks/custom
*/

enum MyEnum {
    //% block="one-1"
    One,
    //% block="two-2"
    Two
}

/**
 * Custom blocks
 * icon: a Unicode identifier for an icon from the Font Awesome icon set.
 *       http://fontawesome.io/icons
 */
//% block="Custom Blocks"
//% weight=100 color=#696969 icon="\uf1b2"
namespace custom {

    let estimated = 0;
    let qw = 1.0;
    let qx = 0.0;
    let qy = 0.0;
    let qz = 0.0;

    //% shim=custom::getQuaternionTimestamp
    export function getQuaternionTimestamp(): number {
        return estimated;
    }

    //% shim=custom::getQuaternionW
    export function getQuaternionW(): number {
        return qw;
    }

    //% shim=custom::getQuaternionX
    export function getQuaternionX(): number {
        return qx;
    }

    //% shim=custom::getQuaternionY
    export function getQuaternionY(): number {
        return qy;
    }

    //% shim=custom::getQuaternionZ
    export function getQuaternionZ(): number {
        return qz;
    }

    //% block
    export function quaternion(): number[] {
        return [
            getQuaternionW(),
            getQuaternionX(),
            getQuaternionY(),
            getQuaternionZ(),
        ];
    }

    let eulerTimestamp = 0;
    let yaw = 0.0;
    let pitch = 0.0;
    let roll = 0.0;
    
    function quaternionToEuler(): void {
        const timestamp = getQuaternionTimestamp();
        if (timestamp == eulerTimestamp) {
            return;
        }
        eulerTimestamp = timestamp;

        const w = getQuaternionW();
        const x = getQuaternionX();
        const y = getQuaternionX();
        const z = getQuaternionX();

        // Right-Handed Coordinate System
        const ysqr = y * y;
        const t0 = 2.0 * (w * x + y * z);
        const t1 = 1.0 - 2.0 * (x * x + ysqr);
        roll = Math.atan2(t0, t1);

        let t2 = 2.0 * (w * y - z * x);
        t2 = t2 > 1.0 ? 1.0 : (t2 < -1.0 ? -1.0 : t2);
        pitch = Math.asin(t2);

        const t3 = 2.0 * (w * z + x * y);
        const t4 = 1.0 - 2.0 * (ysqr + z * z);
        yaw = Math.atan2(t3, t4);
    }

    //% block
    export function getYaw(): number {
        quaternionToEuler();
        return yaw;
    }

    //% block
    export function getPitch(): number {
        quaternionToEuler();
        return pitch;
    }

    //% block
    export function getRoll(): number {
        quaternionToEuler();
        return roll;
    }

    let ax = 0.0;
    let ay = 0.0;
    let az = 1.0;

    //% block
    //% shim=custom::setAcceleration
    export function setAcceleration(x: number, y: number, z: number): void {
        ax = x;
        ay = y;
        az = z;
    }

    //% block
    //% shim=custom::setMagneticForce
    export function setMagneticForce(x: number, y: number, z: number): void {
        const a = x + y + z;
    }

    //% block
    //% shim=custom::estimate
    export function estimate(): void {
        qw = 1.0;
        qx = ax;
        qy = ay;
        qz = az;
        ++estimated;
    }

    //% block
    //% shim=custom::buildErr
    export function buildErr(p1: number, p2: number, p3: number, p4: number, p5: number, p6: number): void {
        // nop
    }
    /**
     * TODO: describe your function here
     * @param n describe parameter here, eg: 5
     * @param s describe parameter here, eg: "Hello"
     * @param e describe parameter here
     */
    //% block
    export function foo(n: number, s: string, e: MyEnum): void {
        // Add code here
    }

    /**
     * Using C++ in addition to the simulator implementation
     * Read more at https://makecode.com/simshim
     * @returns device runtime
     */
    //% block
    //% shim=custom::baz
    export function baz(): number {
        // implementation for simulator
        return DEVICE_RUNTIME.RUNTIME_SIMU + 5;
    }

}

```

```cpp:custom.cpp
#include "pxt.h"
#include "customlib.h"

namespace custom
{
    int estimated = 0;
    double qw = 1.0;
    double qx = 2.0;
    double qy = 3.0;
    double qz = 4.0;

    //%
    int getQuaternionTimestamp()
    {
        return estimated;
    }

    //%
    TNumber getQuaternionW()
    {
        return fromDouble(qw);
    }

    //%
    TNumber getQuaternionX()
    {
        return fromDouble(qx);
    }

    //%
    TNumber getQuaternionY()
    {
        return fromDouble(qy);
    }

    //%
    TNumber getQuaternionZ()
    {
        return fromDouble(qz);
    }

    double ax = 0.0;
    double ay = 0.0;
    double az = 1.0;

    //%
    void setAcceleration(TNumber x, TNumber y, TNumber z)
    {
        ax = toDouble(x);
        ay = toDouble(y);
        az = toDouble(z);
        double norm = sqrt(ax * ax + ay * ay + az * az);
        if (norm > 0)
        {
            ax /= norm;
            ay /= norm;
            az /= norm;
        } else {
            ax = 0.0;
            ay = 0.0;
            az = 1.0;
        }
    }

    double mx = 0.0;
    double my = 0.0;
    double mz = 1.0;

    //%
    void setMagneticForce(TNumber x, TNumber y, TNumber z)
    {
        mx = toDouble(x);
        my = toDouble(y);
        mz = toDouble(z);
        double norm = sqrt(mx * mx + my * my + mz * mz);
        if (norm > 0)
        {
            mx /= norm;
            my /= norm;
            mz /= norm;
        } else {
            // default
            mx = 0.0;
            my = 0.0;
            mz = 1.0;
        }
    }

    //%
    void estimate()
    {
        // ---------------------------------------------------------------------------------------------
        // A Simplified Analytic Attitude Determination Algorithm Using Accelerometer and Magnetometer
        // Fast Accelerometer-Magnetometer Combination (FAMC) algorithm by Zhuohua Liu and Jin Wu
        // ---------------------------------------------------------------------------------------------
        // https://github.com/zarathustr/Analytic-AMC/blob/master/FAMC.m
        
        // Dynamic magnetometer reference vector
        double m_D = ax * mx + ay * my + az * mz;
        double m_N = sqrt(1.0 - m_D * m_D);

        // Parameters
        double B11 = (m_N * mx) / 2.0;
        double B13 = ax / 2.0 + (m_D * mx) / 2.0;
        double B21 = (m_N * my) / 2.0;
        double B23 = ay / 2.0 + (m_D * my) / 2.0;
        double B31 = (m_N * mz) / 2.0;
        double B33 = az / 2.0 + (m_D * mz) / 2.0;

        double tau = B13 + B31;

        // First Row
        double p1 = B33 - B11 + 1.0;
        double A11 = -1.0 / p1;
        double A12 = B21 / p1;
        double A13 = tau / p1;

        // Second Row
        double p2 = -B21 * B21 / p1 + B11 + B33 + 1.0;
        double A21 = -B21 / (p1 * p2);
        double A22 = -1.0 / p2;
        double A23 = (B23 + B21 * tau / p1) / p2;

        // Third Row
        double p3 = p1 - 2.0 + tau * tau / p1 + A23 * A23 * p2;
        double A31 = (tau / p1 + B21 * A23 / p1) / p3;
        double A32 = A23 / p3;
        double A33 = 1.0 / p3;

        // Quaternion Elements
        double a = B23 * (A11 + A12 * (A21 + A23 * A31) + A13 * A31)
                 - (B13 - B31) * (A21 + A23 * A31) - A31 * B21;
        double b = B23 * (A12 * (A22 + A23 * A32) + A13 * A32)
                 - (B13 - B31) * (A22 + A23 * A32) - A32 * B21;
        double c = B23 * (A13 * A33 + A12 * A23 * A33)
                 - A33 * B21 - A23 * A33 * (B13 - B31);

        ++estimated;
        qw = 1.0;
        qx = -a;
        qy = -b;
        qz = -c;

        double norm = sqrt(qw * qw + qx * qx + qy * qy + qz * qz);
        qw /= norm;
        qx /= norm;
        qy /= norm;
        qz /= norm;
    }

    //%
    void buildErr(TNumber p1, TNumber p2, TNumber p3, TNumber p4, TNumber p5, TNumber p6)
    {
        // nop
    }

    /**
     * Block definition from C++, no implementation for simulator
     * https://makecode.com/simshim
     */
    //% block
    int bar()
    {
        return (int)customlib::getDeviceRuntime();
    }

    //%
    int baz()
    {
        return (int)customlib::getDeviceRuntime() + 5;
    }

}

```

```ts:test.ts
/**
 * tests go here; this will not be compiled when this package is used as an extension.
 */
serial.redirectToUSB()
serial.setBaudRate(BaudRate.BaudRate115200)
serial.writeLine("Hello, Quaternion.")
serial.writeNumbers(custom.quaternion())
serial.writeLine("")
basic.forever(function () {
    custom.setAcceleration(input.acceleration(Dimension.X), input.acceleration(Dimension.Y), input.acceleration(Dimension.Z))
    custom.setMagneticForce(input.magneticForce(Dimension.X), input.magneticForce(Dimension.Y), input.magneticForce(Dimension.Z))
    custom.estimate()
    serial.writeValue("yaw", custom.getYaw())
    serial.writeValue("pit", custom.getPitch())
    serial.writeValue("rol", custom.getRoll())
    basic.pause(200)
})

```

# scratch の3D

https://scratch.mit.edu/projects/92360676

https://scratch.mit.edu/projects/1044512819

<iframe src="https://scratch.mit.edu/projects/1044512819/embed" allowtransparency="true" width="485" height="402" frameborder="0" scrolling="no" allowfullscreen></iframe>

# reactで

https://codesandbox.io/p/sandbox/springy-quaternion-rotation-widvi?file=%2Fsrc%2FApp.js%3A14%2C27

**手順メモ**

https://github.com/jp-rad/vscode-react-parcel からプロジェクトを作成

モジュールのインストール

```bash

yarn add three @react-three/fiber react-spring react-use-gesture
yarn add --dev @types/three

```

```css: App.css
.App {
  font-family: sans-serif;
  text-align: center;
  width: 100vw;
  height: 100vh;
}

.Canvas {
  width: 100vw;
  height: 100vh;
}
```

```ts: ThreeFiberSample.tsx

// https://qiita.com/sho-19202325/items/b1d56c627856818f4bf0

import * as THREE from 'three'
import React, { useRef, useState } from 'react'
import { Canvas, useFrame, ThreeElements } from '@react-three/fiber'

function Box(props: ThreeElements['mesh']) {
  const ref = useRef<THREE.Mesh>(null!)
  const [hovered, hover] = useState(false)
  const [clicked, click] = useState(false)
  useFrame((state, delta) => (ref.current.rotation.x += delta))
  return (
    <mesh
      {...props}
      ref={ref}
      scale={hovered ? 1.5 : 1}
      onClick={(event) => click(!clicked)}
      onPointerOver={(event) => hover(true)}
      onPointerOut={(event) => hover(false)}>
      <boxGeometry args={[1, 1, 1]} />
      <meshNormalMaterial attach='material' />
    </mesh>
  )
}

export const ThreeFiberSample = () => {
  return (
    <Canvas>
      <ambientLight />
      <pointLight position={[10, 10, 10]} />
      <Box position={[0, 0, 0]} />
    </Canvas>
  )
}

// export const ThreeFiberSample = () => {
//   return (
//     <Canvas>
//       <ambientLight />
//       <pointLight position={[10, 10, 10]} />
//       <Box position={[-1.2, 0, 0]} />
//       <Box position={[1.2, 0, 0]} />
//     </Canvas>
//   )
// }


```