ASP.NET SignalR でデバイスの回転状態を同期する

前回の transform と deviceorientation における回転の表現 (HTML) では、
そのデバイスのブラウザー上で回転の状態を表示していましたが、
今回は他のデバイスのブラウザーにネットワーク経由で同期するようにしました。

WebSocket で同期するためのフレームワークとして ASP.NET SignalR を利用し、
Azure Web App に GitHub からの継続的デプロイを設定しています。

HTML 3D Device Orientation on Web Browsers

 

これを実装する方法を以下に示します。

Visual Studio で空の ASP.NET Web プロジェクトを作成し、NuGet で Microsoft.AspNet.SignalR をインストールします。
まずサーバー側の C# コードとして、次のクラスを実装します。

using System;
using Microsoft.AspNet.SignalR;
namespace DeviceSyncWeb
{
public class SensorHub : Hub
{
public void UpdateOrientation(double alpha, double beta, double gamma)
{
Clients.Others.NotifyOrientation(alpha, beta, gamma);
}
}
}
view raw SensorHub.cs hosted with ❤ by GitHub
using System;
using Owin;
namespace DeviceSyncWeb
{
public class Startup
{
public void Configuration(IAppBuilder app)
{
app.MapSignalR();
}
}
}
view raw Startup.cs hosted with ❤ by GitHub

Startup.Configuration メソッドの中で ASP.NET SignalR を有効にします。
そして、送受信をするためのハブとして SensorHub クラスを作成しています。
今回は、JavaScript の deviceorientation イベントで取得できる alpha, beta, gamma の値を引数で受け取って
クライアントに通知するだけです。なお、Clients.Others は送信者以外のクライアントを表します。

次に、クライアントとなる HTML を実装します。

<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width" />
<title>DeviceSync - Sensor</title>
<script type="text/javascript" src="/Scripts/jquery-1.6.4.min.js"></script>
<script type="text/javascript" src="/Scripts/jquery.signalR-2.4.0.min.js"></script>
<script type="text/javascript" src="/signalr/hubs"></script>
</head>
<body>
<script type="text/javascript">
var sensorHub = $.connection.sensorHub;
$.connection.hub.start();
if (window.DeviceOrientationEvent) {
window.addEventListener("deviceorientation", function (e) {
sensorHub.server.updateOrientation(e.alpha, e.beta, e.gamma);
});
}
</script>
</body>
</html>
view raw sensor.html hosted with ❤ by GitHub
<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width" />
<title>DeviceSync - Viewer</title>
<script type="text/javascript" src="/Scripts/jquery-1.6.4.min.js"></script>
<script type="text/javascript" src="/Scripts/jquery.signalR-2.4.0.min.js"></script>
<script type="text/javascript" src="/signalr/hubs"></script>
<!-- style の記述を省略 -->
</head>
<body>
<div id="log"></div>
<div class="container">
<div id="cube">
<div class="face front">1</div>
<div class="face back">6</div>
<div class="face right">2</div>
<div class="face left">5</div>
<div class="face top">3</div>
<div class="face bottom">4</div>
</div>
</div>
<img id="image" src="website_news.png" style="margin: 80px;" />
<script type="text/javascript">
var sensorHub = $.connection.sensorHub;
sensorHub.client.notifyOrientation = updateOrientation;
$.connection.hub.start();
var logEl = document.querySelector("#log");
var cubeEl = document.querySelector("#cube");
var imageEl = document.querySelector("#image");
function updateOrientation(alpha, beta, gamma) {
logEl.innerHTML = `alpha: ${alpha}<br />beta: ${beta}<br />gamma: ${gamma}`;
var rotation = `rotateZ(${-alpha}deg) rotateX(${-beta}deg) rotateY(${gamma}deg)`;
cubeEl.style.transform = rotation;
imageEl.style.transform = rotation;
}
</script>
</body>
</html>
view raw viewer.html hosted with ❤ by GitHub

センサーのデータを送信する sensor.html と、それを受信して表示する viewer.html に分かれています。
各 JS ファイルを <script> で読み込み、$.connection から各機能にアクセスします。

 

前回: transform と deviceorientation における回転の表現 (HTML)

作成したサンプル

バージョン情報

  • .NET Framework 4.7
  • ASP.NET SignalR 2.4.0

transform と deviceorientation における回転の表現 (HTML)

CSS の transform プロパティと JavaScript の deviceorientation イベントではともに 3D の回転状態 (姿勢、傾き) が登場しますが、
その扱い方に差があるため検証しました。
deviceorientation は、デバイスのジャイロ センサーが回転状態を通知することで発生するイベントです。

HTML の 3 次元座標系では、2 次元スクリーン座標系の x 軸および y 軸に加えて、スクリーンに垂直な z 軸が存在します。
デバイス (スマートフォンなど) を水平に持ち、北を向いた状態を基準に考えます。

このとき、CSS の transform プロパティと JavaScript の deviceorientation イベントにおける、
回転に関する性質の違いを下の表にまとめました。例えば z 軸を中心とする回転の角度は、現在は北を向いていたとしたら、
transform では東側 (時計回り) を向くと正、deviceorientation では西側を向くと正になります。

  transform deviceorientation
座標系 左手系 右手系
x 軸 右が正 右が正
y 軸 下 (手前) が正 上 (奥) が正
z 軸 (画面が水平のとき) 鉛直の上が正 鉛直の上が正
回転角度 回転軸の正方向に左ねじを進める場合が正 回転軸の正方向に右ねじを進める場合が正

 

検証のため、CSS の transform プロパティと JavaScript の deviceorientation イベントを利用して、
デバイスの回転状態を画面内の立方体オブジェクトに同期させるサンプルを作成しました。

DeviceOrientation

ジャイロ センサーを搭載した端末であれば、こちらのテストページで確認できます。
HTML のソースは以下の通りです。

<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width" />
<title>Device Orientation</title>
<style type="text/css">
.container {
width: 2px;
height: 2px;
margin: 100px auto;
perspective: 300px;
}
#cube {
transform-style: preserve-3d;
}
.face {
width: 100px;
height: 100px;
position: absolute;
left: -50px;
top: -50px;
background: rgba(255, 102, 0, 0.5);
border: 2px solid gray;
text-align: center;
font-size: 60px;
line-height: 100px;
}
.front {
transform: translateZ(50px);
}
.back {
transform: rotateY(180deg) translateZ(50px);
}
.right {
transform: rotateY(90deg) translateZ(50px);
}
.left {
transform: rotateY(-90deg) translateZ(50px);
}
.top {
transform: rotateX(90deg) translateZ(50px);
}
.bottom {
transform: rotateX(-90deg) translateZ(50px);
}
</style>
</head>
<body>
<div id="log"></div>
<div class="container">
<div id="cube">
<div class="face front">1</div>
<div class="face back">6</div>
<div class="face right">2</div>
<div class="face left">5</div>
<div class="face top">3</div>
<div class="face bottom">4</div>
</div>
</div>
<script type="text/javascript">
var logEl = document.querySelector("#log");
var cubeEl = document.querySelector("#cube");
if (window.DeviceOrientationEvent) {
window.addEventListener("deviceorientation", function (e) {
logEl.innerHTML = `alpha: ${e.alpha}<br />beta: ${e.beta}<br />gamma: ${e.gamma}`;
cubeEl.style.transform = `rotateZ(${-e.alpha}deg) rotateX(${-e.beta}deg) rotateY(${e.gamma}deg)`;
});
}
</script>
</body>
</html>
view raw cube-sync.html hosted with ❤ by GitHub

以下は、各技術についての説明です。

transform プロパティ

transform プロパティで rotateX などを利用して回転状態を指定する場合、次に示すように複数の回転を重ね合わせることができます。

CSS:
transform: rotateX(45deg) rotateY(30deg) rotateZ(60deg);

JavaScript:
element.style.transform = "rotateX(45deg) rotateY(30deg) rotateZ(60deg)";

ただし、座標系ごと回転させながら左から順に適用します (オイラー角)。
これは、以前に 3D における回転の表現と相互変換で書いた通り、元の座標系のまま右から順に適用する、と考えても同じです。

以下に rotateX(45deg)rotateY(45deg) を組み合わせた例を載せておきます。

初期状態

rotateX(45deg) (左)        rotateX(45deg) rotateY(45deg) (右)

rotateY(45deg) (左)        rotateY(45deg) rotateX(45deg) (右)

deviceorientation イベント

window.addEventListenerdeviceorientation に対するイベントリスナーを登録します。
デバイスの回転状態が変化すると、イベントリスナーが呼び出されます。

引数の alpha, beta, gamma はそれぞれ z 軸、x 軸、y 軸を中心とした回転の角度を表し、
座標系ごと回転させながらこの順に重ね合わせたものが回転状態を表します。
それぞれの値の範囲は次の通りです。

  • z 軸: 0 ≦ alpha < 360
    • 北を向いたとき、alpha = 0
  • x 軸: -180 ≦ beta < 180
  • y 軸: -90 ≦ gamma < 90

 

回転状態の同期

以上から、デバイスの回転状態を画面内のオブジェクトに同期させるには次のようにします。

cubeEl.style.transform = `rotateZ(${-e.alpha}deg) rotateX(${-e.beta}deg) rotateY(${e.gamma}deg)`;

正負の符号に注意します。
結果として、z 軸および x 軸における回転角度の正負は異なり、y 軸では同じになります。

 

次回は、回転状態をネットワーク経由で同期させます。

次回: ASP.NET SignalR でデバイスの回転状態を同期する

作成したサンプル
参照
transform
deviceorientation

Roslyn の構文解析を使ってデバッガーを自作する

// C# Advent Calendar 2018 の 23 日目の記事です。

デバッガーのようなものを自作してみました。

動機

  • 普段は Visual Studio を使っているが、デバッグ時に手動でステップ実行するのが面倒
    • ループなどでステップ数が多い場合
    • 分岐の様子や変数の状態を軽くチェックしたい場合

解決案

  • ステップの時間間隔だけを指定して、デバッガーを自動で実行させる
    • 変数の一覧が表示される
    • 時間間隔をリアルタイムで調節できる
  • .NET Compiler Platform (Roslyn) の構文解析の機能を使い、各ステップの間にデバッグ用のコードを差し込めば実現できそう

結果

というわけで、WPF でプロトタイプ「Tick-tack Debugger」を作ってみた結果、このようになりました。
例として、ニュートン法で平方根を求めています。 (クリックで拡大)

 

解説

以下は概略の技術解説です。
WPF アプリを作成する前に、まず .NET Framework 上のコンソール アプリで実験してみます。
C# の構文解析を使うには、NuGet で Microsoft.CodeAnalysis.CSharp をインストールします。

デバッグ対象となるソースコードにデバッグ コードを挿入し、それを動的にコンパイルして実行する、という方針です。
コンソール アプリのソースコードを以下に示します (全体のソリューションは SyntaxTreeSample にあります)。

using System;
using System.CodeDom.Compiler;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using DebuggerLib;
namespace DebuggerConsole
{
class Program
{
const string SourcePath = @"..\..\..\NumericConsole\Program.cs";
const string GeneratedPath = @"Program.g.cs";
static void Main(string[] args)
{
// Generates the code for debugging.
var sourceCode = File.ReadAllText(SourcePath);
var generatedCode = SyntaxHelper.InsertBreakpoints(sourceCode);
File.WriteAllText(GeneratedPath, generatedCode, Encoding.UTF8);
// Compiles and loads the assembly.
var provider = CodeDomProvider.CreateProvider("CSharp");
var compilerOption = new CompilerParameters(new[] { "System.Core.dll", "DebuggerLib.dll" }) { GenerateExecutable = true };
var compilerResult = provider.CompileAssemblyFromFile(compilerOption, GeneratedPath);
if (compilerResult.Errors.HasErrors) return;
// Registers the action for breakpoints.
DebugHelper.InfoNotified += (spanStart, spanLength, variables) =>
{
Console.WriteLine(string.Join(", ", variables.Select(v => $"{v.Name}: {v.Value}")));
Console.WriteLine(sourceCode.Substring(spanStart, spanLength));
Thread.Sleep(1000);
};
// Calls the Main method.
var entryPoint = compilerResult.CompiledAssembly.EntryPoint;
entryPoint.Invoke(null, new object[] { new string[0] });
}
}
}
view raw Program.cs hosted with ❤ by GitHub
using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.CodeAnalysis;
using Microsoft.CodeAnalysis.CSharp;
using Microsoft.CodeAnalysis.CSharp.Syntax;
using Microsoft.CodeAnalysis.Text;
using DebugStatement = System.ValueTuple<Microsoft.CodeAnalysis.CSharp.Syntax.StatementSyntax, string[]>;
namespace DebuggerConsole
{
public static class SyntaxHelper
{
public static string InsertBreakpoints(string sourceCode)
{
var root = ParseText(sourceCode);
var method = root.DescendantNodes()
.OfType<MethodDeclarationSyntax>()
.FirstOrDefault(m => m.Identifier.ValueText == "Main")
?? throw new FormatException("The Main method is not found.");
var statements = DetectStatements(method);
var result = sourceCode;
foreach (var (statement, variables) in statements.Reverse())
{
var (span, debugIndex) = GetSpan(statement);
result = result.Insert(debugIndex, $"DebugHelper.NotifyInfo({span.Start}, {span.Length}{ToParamsArrayText(variables)});\r\n");
}
return result.Insert(root.Usings.FullSpan.End, "using DebuggerLib;\r\n");
}
public static CompilationUnitSyntax ParseText(string text)
{
var tree = CSharpSyntaxTree.ParseText(text);
var diagnostics = tree.GetDiagnostics().ToArray();
if (diagnostics.Length > 0) throw new FormatException(diagnostics[0].ToString());
return tree.GetCompilationUnitRoot();
}
public static DebugStatement[] DetectStatements(MethodDeclarationSyntax method)
{
var statements = new List<DebugStatement>();
DetectStatements(method.Body, statements, new List<(string, SyntaxNode)>());
return statements.ToArray();
}
static void DetectStatements(SyntaxNode node, List<DebugStatement> statements, List<(string name, SyntaxNode scope)> variables)
{
// Adds variables.
if (node is VariableDeclarationSyntax varSyntax)
{
var varNames = varSyntax.Variables.Select(v => v.Identifier.ValueText).ToArray();
var scope = ((node.Parent is LocalDeclarationStatementSyntax) ? node.Parent : node)
.Ancestors()
.First(n => n is StatementSyntax);
variables.AddRange(varNames.Select(v => (v, scope)));
}
// Maps variables to the statement.
if ((node is StatementSyntax statement) &&
!(node is BlockSyntax) &&
!(node is BreakStatementSyntax))
statements.Add((statement, variables.Select(v => v.name).ToArray()));
// Recursively.
foreach (var child in node.ChildNodes())
DetectStatements(child, statements, variables);
// Maps variables to the last line of the block.
if (node is BlockSyntax block)
statements.Add((block, variables.Select(v => v.name).ToArray()));
// Clears variables out of the scope.
if (node is StatementSyntax)
for (var i = variables.Count - 1; i >= 0; i--)
if (variables[i].scope == node)
variables.RemoveAt(i);
else
break;
}
static (TextSpan, int) GetSpan(StatementSyntax statement)
{
switch (statement)
{
case ForStatementSyntax f:
var span = new TextSpan(f.ForKeyword.Span.Start, f.CloseParenToken.Span.End - f.ForKeyword.Span.Start);
return (span, statement.FullSpan.Start);
case BlockSyntax b:
return (b.CloseBraceToken.Span, b.CloseBraceToken.FullSpan.Start);
default:
return (statement.Span, statement.FullSpan.Start);
}
}
static string ToParamsArrayText(string[] variables) =>
string.Concat(variables.Select(v => $", new Var(\"{v}\", {v})"));
}
}
view raw SyntaxHelper.cs hosted with ❤ by GitHub

SyntaxHelper クラスでは、デバッグ対象の C# ソースコードを構文ツリー (SyntaxTree) に変換して走査し、
各ステートメントの前にデバッグ用のコード行を挿入していきます。

CSharpSyntaxTree.ParseText メソッドを使うことで、ソースコードを構文ツリーに変換できます。
また、メソッド・ステートメント・式など、すべてのノードを表す親クラスは SyntaxNode クラスであり、

  • Parent プロパティ: 親
  • Ancestors メソッド: 祖先
  • ChildNodes メソッド: 子
  • DescendantNodes メソッド: 子孫

が存在することを知っておけば、だいたいの探索ができるでしょう。

この他に、デバッグ用のコードから呼び出されるメソッドを定義するクラス ライブラリとして DebuggerLib を作成しています。
各ステートメントの位置、およびその直前で存在する変数とその値を通知するために、このライブラリを経由させます。

Program クラスでは、生成されたデバッグ用のソースコードをファイルに保存したら、
System.CodeDom.Compiler 名前空間の CodeDomProvider を使ってこれをコンパイルし、
そのエントリ ポイント (Main メソッド) を呼び出します。
また、デバッグ コードが実行されたときのイベントハンドラーを登録しておき、
Thread.Sleep メソッドを使って、指定した時間だけ停止させます。

これで、デバッグ対象の元のソースコードが次の Program.cs だとすると、
デバッグ用のソースコードとして下の Program.g.cs が生成されます。

using System;
namespace NumericConsole
{
class Program
{
static void Main(string[] args)
{
// Square root by the Newton's method.
var a = 5.0;
var x = a;
for (var i = 0; i < 100; i++)
{
var xi = (x + a / x) / 2;
if (x == xi) break;
x = xi;
}
Console.WriteLine(x);
}
}
}
view raw Program.cs hosted with ❤ by GitHub
using System;
using DebuggerLib;
namespace NumericConsole
{
class Program
{
static void Main(string[] args)
{
DebugHelper.NotifyInfo(188, 12);
// Square root by the Newton's method.
var a = 5.0;
DebugHelper.NotifyInfo(214, 10, new Var("a", a));
var x = a;
DebugHelper.NotifyInfo(240, 29, new Var("a", a), new Var("x", x));
for (var i = 0; i < 100; i++)
{
DebugHelper.NotifyInfo(302, 25, new Var("a", a), new Var("x", x), new Var("i", i));
var xi = (x + a / x) / 2;
DebugHelper.NotifyInfo(347, 19, new Var("a", a), new Var("x", x), new Var("i", i), new Var("xi", xi));
if (x == xi) break;
DebugHelper.NotifyInfo(384, 7, new Var("a", a), new Var("x", x), new Var("i", i), new Var("xi", xi));
x = xi;
DebugHelper.NotifyInfo(405, 1, new Var("a", a), new Var("x", x), new Var("i", i), new Var("xi", xi));
}
DebugHelper.NotifyInfo(422, 21, new Var("a", a), new Var("x", x));
Console.WriteLine(x);
DebugHelper.NotifyInfo(453, 1, new Var("a", a), new Var("x", x));
}
}
}
view raw Program.g.cs hosted with ❤ by GitHub

作成したコンソール アプリを実行すると、次の図のようになります (時間間隔は 0.3 秒)。

 

以上をもとに、WPF アプリでデバッグ ツールを作成しました。
左側の C# ソースコードの部分は TextBox で、編集もできます。
デバッグ実行時は、各ステートメントを選択状態にすることでハイライトしています。
右側の変数一覧が表示される部分は DataGrid です。

(図は円周率を求める例)

今回は上記の方法でプロトタイプを作ってみましたが、
デバッグ コードの挿入やコンパイルに関しては、よりスマートな方法があるのではないかと思います。

注意点
  • 考えられうるすべてのステートメントには対応できていません。また、Main メソッドしか構文解析していません。
  • コンパイル時に生成されるアセンブリ (EXE) は、%TEMP% フォルダー (ユーザーの AppData\Local\Temp) に保存されていきます。
  • TextBox で、IsInactiveSelectionHighlightEnabled を True に設定しても利かないことがあります。
    また、選択状態のハイライトがずれることがあります。
    RichTextBox で Run などを使うのがよいかもしれません。

 

作成したサンプル
バージョン情報
参照

Leap Motion で手の回転状態を取得する

Leap Motion Controller の公式 SDK では、手の回転の状態をオイラー角で取得できるようになっています。
具体的には、Hand.Direction (Vector オブジェクト) の Yaw, Pitch, Roll プロパティが用意されています。
ただし、Hand クラスの説明を参照すると、 ロールについては Direction.Roll ではなく PalmNormal.Roll を使うように書かれています。

float pitch = hand.Direction.Pitch;
float yaw = hand.Direction.Yaw;
float roll = hand.PalmNormal.Roll;

しかし、これらの値を使って実装してみても、期待通りの動作にはなりません。

そこで、前回の 3D における回転の表現と相互変換の内容をもとに、手の回転の状態を取得する機能を自作しました。

Hand.Direction と Hand.PalmNormal はともに長さ 1 で直交しているため、
これらをそれぞれ (0, 0, -1) と (0, -1, 0) の回転後のベクトルと見なして、
前回作成した Rotation3DHelper クラスを利用してオイラー角を求めれば OK です。

using System;
using System.Windows.Media.Media3D;
namespace HandRotationLeap
{
public static class Leap3DHelper
{
public static Vector3D ToVector3D(this Leap.Vector v) => new Vector3D(v.x, v.y, v.z);
// Wrong values.
public static EulerAngles GetEulerAngles_org(this Leap.Hand h) => new EulerAngles
{
Yaw = h.Direction.Yaw,
Pitch = h.Direction.Pitch,
Roll = h.PalmNormal.Roll,
};
// Improved values.
public static EulerAngles GetEulerAngles(this Leap.Hand h) =>
Rotation3DHelper.ToEulerAngles(-h.Direction.ToVector3D(), -h.PalmNormal.ToVector3D());
}
}
view raw AppModel.cs hosted with ❤ by GitHub

全体のソースコードは HandRotationLeap (GitHub) にあります。
このサンプルでは、手とさいころの回転の状態を同期させています。

Hand Rotation by Leap Motion Controller

前回: 3D における回転の表現と相互変換

バージョン情報
.NET Framework 4.5
Leap Motion SDK 2.3.1

参照
Hand クラス

3D における回転の表現と相互変換

以前に投稿した WPF で 3D オブジェクトを回転させるではオブジェクトの回転の状態を行列で表していましたが、
3 次元空間における回転を表現する方法は、次のように何通りか考えられます。
なお、原点を中心に回転させるものとし、回転角度は、回転軸を表すベクトルの方向に右ねじを押し込む場合を正とします。

  1. 行列
    • 3 次正方行列。とくに、回転を表すものは直交行列であり、P^{-1} = {}^t P が成り立つ
    • WPF では Matrix3D 構造体
  2. 四元数 (クォータニオン)
  3. オイラー角 (ロール、ピッチ、ヨー)
    • 任意の回転を 3 回の単純な回転の合成により表す。詳細の定義はロール・ピッチ・ヨー ― Kinectで学ぶ数学を参照
    • 座標系ごと回転させる
    • ロール、ピッチ、ヨーをそれぞれどの軸に対応させるか、どの順番で作用させるかで結果が変わってしまうため注意が必要
  4. 特定の 2 点の回転前後の座標
    • とくに、直交する 2 つのベクトルを用いるとよい

3D のプログラミングをしていると、API によってどの表現を利用するかが異なることがあります。
以下では、これらの表現を互いに変換する方法について考えます。

■ 行列 → 2 点の座標

任意のベクトルに対して行列を作用させれば回転後のベクトルが求められます。
WPF では演算子 * が用意されています。

■ 四元数 → 行列

四元数の成分から行列の各成分を計算できます (詳細は四元数による回転行列の表現を参照)。
WPF では Matrix3D.Rotate メソッドとして用意されています。

■ オイラー角 → 行列または四元数

ここでは、ヨーは y 軸、ピッチは x 軸、ロールは z 軸のまわりの回転を表し、この順に適用されるとします。
(元の座標系における) ヨー、ピッチ、ロールを表す回転行列をそれぞれ R_y, R_p, R_r とし、それぞれの回転角度を \theta_y, \theta_p, \theta_r とします。
すなわち、

R_y = \left( \begin{array}{ccc} \cos \theta_y & 0 & \sin \theta_y \\ 0 & 1 & 0 \\ -\sin \theta_y & 0 & \cos \theta_y \end{array} \right), R_p = \left( \begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta_p & -\sin \theta_p \\ 0 & \sin \theta_p & \cos \theta_p \end{array} \right), R_r = \left( \begin{array}{ccc} \cos \theta_r & -\sin \theta_r & 0 \\ \sin \theta_r & \cos \theta_r & 0 \\ 0 & 0 & 1 \end{array} \right)

このとき、座標系ごとヨー、ピッチ、ロールの順に適用した回転は、元の座標系で R_y R_p R_r で表されます (適用の順序が逆になる)。
証明は次のようにできますが、実際の回転をイメージするとわかりやすいと思います。

証明

ベクトル {\bf x} にヨーを作用させると、
R_y {\bf x}
次に、これにピッチを作用させるには、いったん座標系を戻して R_p を作用させるから、
R_y R_p R_y^{-1} \cdot R_y {\bf x} = R_y R_p {\bf x}
同様に、これにロールを作用させると、
(R_y R_p) R_r (R_y R_p)^{-1} \cdot R_y R_p {\bf x} = R_y R_p R_r {\bf x}
(証明終)

WPF での実際の演算では四元数を使うとよいでしょう。

■ 2 点の座標 → オイラー角

ここでは、2 点として {\bf e}_z = (0, 0, 1), {\bf e}_y = (0, 1, 0) を選びます。
これらの回転後のベクトルがそれぞれ {\bf u}, {\bf t} で与えられたとすると、オイラー角は以下の手続きにより求められます。

まず、{\bf u} の x 要素と z 要素の比はピッチおよびロールの影響を受けないから、
\tan \theta_y = \dfrac{{\bf u}_x}{{\bf u}_z}
により、\theta_y, R_y が決まる。

ピッチおよびロールの決め方から、
\tan \theta_p = \dfrac{- (R_p {\bf e}_z)_y}{(R_p {\bf e}_z)_z}, \tan \theta_r = \dfrac{- (R_r {\bf e}_y)_x}{(R_r {\bf e}_y)_y}

また、前項と同様に考えて、{\bf u} = R_y R_p {\bf e}_z, {\bf t} = R_y R_p R_r {\bf e}_y であるから、
R_p {\bf e}_z = R_y^{-1} {\bf u}, R_r {\bf e}_y = R_p^{-1} R_y^{-1} {\bf t}

したがって、\theta_p, \theta_r も順に決まる。
なお、この決め方の場合、回転角度の範囲は、
- \pi < \theta_y \le \pi, - \dfrac{\pi}{2} \le \theta_p \le \dfrac{\pi}{2}, - \pi < \theta_r \le \pi
となる。
(終)

arctan を求めるには、Math.Atan2 メソッドを使うとよいでしょう。

以上により、行列 → 2 点の座標 → オイラー角 → 四元数 → 行列と変換する方法が与えられたので、
回転の表現を相互に変換できるようになります。

では、これらを実装してみます。
ベクトル、行列、四元数を扱うための System.Numerics.Vectors という、WPF のライブラリよりも高機能なライブラリもあるのですが、
今回は WPF のライブラリのみを利用して実装したいと思います。

using System;
using System.Diagnostics;
using System.Windows.Media.Media3D;
namespace RotationTest
{
public static class Rotation3DHelper
{
public static readonly Vector3D UnitX = new Vector3D(1, 0, 0);
public static readonly Vector3D UnitY = new Vector3D(0, 1, 0);
public static readonly Vector3D UnitZ = new Vector3D(0, 0, 1);
public static double ToDegrees(double radians) => radians * 180 / Math.PI;
public static double ToRadians(double degrees) => degrees * Math.PI / 180;
public static Quaternion CreateQuaternionInRadians(Vector3D axis, double angleInRadians) => new Quaternion(axis, ToDegrees(angleInRadians));
// 四元数 → 行列
public static Matrix3D ToMatrix3D(this Quaternion q)
{
var m = new Matrix3D();
m.Rotate(q);
return m;
}
// オイラー角 → 四元数
public static Quaternion ToQuaternion(this EulerAngles e) =>
CreateQuaternionInRadians(UnitY, e.Yaw) *
CreateQuaternionInRadians(UnitX, e.Pitch) *
CreateQuaternionInRadians(UnitZ, e.Roll);
// 2 点の座標 → オイラー角
public static EulerAngles ToEulerAngles(Vector3D rotatedUnitZ, Vector3D rotatedUnitY)
{
var y_yaw = Math.Atan2(rotatedUnitZ.X, rotatedUnitZ.Z);
var m_yaw_inv = CreateQuaternionInRadians(UnitY, -y_yaw).ToMatrix3D();
rotatedUnitZ = rotatedUnitZ * m_yaw_inv;
rotatedUnitY = rotatedUnitY * m_yaw_inv;
var x_pitch = Math.Atan2(-rotatedUnitZ.Y, rotatedUnitZ.Z);
var m_pitch_inv = CreateQuaternionInRadians(UnitX, -x_pitch).ToMatrix3D();
rotatedUnitY = rotatedUnitY * m_pitch_inv;
// 本来は -rotatedUnitY.X だけでよいはずです。しかし、X=0 のときに π でなく -π となってしまうため、場合分けします。
var z_roll = Math.Atan2(rotatedUnitY.X == 0 ? 0 : -rotatedUnitY.X, rotatedUnitY.Y);
return new EulerAngles { Yaw = y_yaw, Pitch = x_pitch, Roll = z_roll };
}
}
[DebuggerDisplay(@"\{Yaw={Yaw}, Pitch={Pitch}, Roll={Roll}\}")]
public struct EulerAngles
{
public double Yaw { get; set; }
public double Pitch { get; set; }
public double Roll { get; set; }
}
}
view raw Rotation3DHelper.cs hosted with ❤ by GitHub

全体のソースコードは RotationTest (GitHub) にあります。
これらのメソッドを呼び出すテストが付いています。

前回: WPF で 3D オブジェクトを回転させる
次回: Leap Motion で手の回転状態を取得する

バージョン情報
.NET Framework 4.5

参照