Modelicaでピタゴラ装置を開発したい2(執筆中)

# はじめに

Modelicaを使ってピタゴラ装置のシミュレーションを行って、実際に実験をしたいという試みです。
まだ、シミュレーションのひな形を作っている段階です。

今回作成したボールやボックスの衝突サンプルとしての動画です。動画は2次元ですが、3次元方向の運動にも対応しています。

![ballinBox2_36ball.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/6e3068a5-13db-eed7-e2be-0c6cc8b972c1.gif)

モデル図は以下です。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/486652ac-bc0d-e68d-cdf9-d9a28cacb7ea.png)

以下にソースコードをアップしています。
https://github.com/ModelicaLibraryStudyMeeting/PythagoraDevice

[前回の記事](https://qiita.com/UedaShigenori/items/1d438d9576bfd45fc768)で以下の解説を行いました。

* 1軸並進運動とその接触のモデル化

* アニメーション機能

今回は以下の機能を実装しました。

* ボールの3軸並進運動のモデル化
* ボール同士の衝突
* ボールとボックスの衝突


# サンプル1　2個のボールの衝突
まずは2個のボールの衝突について紹介します。

![contactBall2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/521a2aee-f086-a80c-4690-4d6ba4c113aa.gif)

モデル図
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/9fe30437-a3fe-227e-9cfb-b3a2f7ac26b0.png)

# Ballモデル

ボールは回転を考慮しない3軸並進運動モデルです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/cc3c40f1-a163-2b3a-777c-5f81403ce842.png)

xyz軸方向の運動方程式が実装されています。
重力はデフォルトで画面下方向(-y方向)に加わっています。一応、パラメータ化されているので重力方向を変えることも出来ます。



## Flangeポート

ポートは以下のような変数を持っています。
特徴としてgeometry.shapeという形状を示す情報を持っております。
このgeometry.shapeを利用して衝突の判定、反力の計算を行います。

| 型         | 変数           | 説明                                        |
| ---------- | -------------- | ------------------------------------------- |
| Postion    | xyz[3]         | 物体のxyz軸の中心座標値                     |
| flow Force | f[3]           | 物体に加わるxyz方向の力                     |
| Geometry   | geometry       | 物体の幾何学的な情報                        |
| String     | geometry.shape | 物体の形状　ball、boxなど                   |
| Length     | geometry.L     | 物体の長さ。ボール形状ならLが直径となります |
| Length     | geometry.W     | 物体の幅                                    |
| Length     | geometry.H     | 物体の高さ                                  |

```
connector Flange "One-dimensional translational flange"
  SI.Position xyz[3] "Absolute position of center of object";
  flow SI.Force f[3] "Cut force directed into flange";
  Geometry geometry "Geometry infomation of object";
end Flange;

record Geometry
  String shape "shape of object. ex. ball, box";
  SI.Length L "length. if shape is ball, L is Diameter";
  SI.Length W "wide";
  SI.Length H "height";
end Geometry;
```

回転を考えたい場合、上記にxyz軸周りのトルクと回転角が必要となります。
実装は追々検討します。

## 衝突の計算 Contactモデル

コンタクトモデルはモデル同士の衝突の影響を計算するモデルです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/cc958bae-d515-2895-105e-877ac4cf7c53.png)
両端に接続されたモデル同士の形状と寸法(geometryデータ)から最短距離を計算して、閾値以下なら衝突と判断します。
衝突した場合、衝突時の食い込み量に見合う反力を物体に与えます。
ペナルティ法というらしいです。<sup>1</sup>
反力の方向は物体の相対速度ベクトルの反対方向です。

## ボール同士の衝突の判定

ボール同士の衝突判定はお互いの中心距離がお互いの半径の和以下になった場合に衝突となります。<sup>2</sup>

# サンプル2　ボールとボックスの衝突
ボールとボックスの衝突は以下のようになります。

![fixedWallAndBall.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/ca1fd7b0-01d9-f475-e1bc-1238bef5fb2c.gif)
モデル図
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/ac492183-30c1-952e-b830-6accc670febb.png)

## Boxモデル

Boxモデルはxyz軸に対して傾きがないaxis-aligned bounding boxes (AABB)を実装しました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244254/64f23d63-ab8d-46fd-9766-6929ab73738f.png)

アニメーションを傾ける事は出来ますが、Contactモデルの方がAABBにのみ対応しているため傾けた場合衝突は判定できません。

## ボールとボックスの衝突の計算 Contactモデル

ボールとボックスの衝突判定は以下となります。<sup>3</sup>
(参考サイトの引用コードです)

```
function intersect(sphere, box) {
  // get box closest point to sphere center by clamping
  var x = Math.max(box.minX, Math.min(sphere.x, box.maxX));
  var y = Math.max(box.minY, Math.min(sphere.y, box.maxY));
  var z = Math.max(box.minZ, Math.min(sphere.z, box.maxZ));

  // this is the same as isPointInsideSphere
  var distance = Math.sqrt((x - sphere.x) * (x - sphere.x) +
                           (y - sphere.y) * (y - sphere.y) +
                           (z - sphere.z) * (z - sphere.z));
  
  return distance < sphere.radius;
```

# 課題
以下が現状の課題です。モデルを一緒に作ったりモデルを使って遊んでみませんか？
やりたい人は[Modelicaライブラリ勉強会](https://modelica-library.connpass.com/)のSlackでお教えください。

* Ball、Boxモデルの回転の考慮、衝突の計算式実装(実装は私が実施するのも大丈夫です)
* BallとFixedBallが分かれているので統一化
* モデルが大きくなるとアニメーションの表示が異常な物となる
* モデルが大きくなるとコンパイルの時間が大きくなる
* BoxとBallの衝突をdasslで計算すると、BallがBoxを透過する
    * 解決しました。dasslの最大ステップを短くすることで透過しなくなりました。ご助言頂いた勉強会の方に感謝致します。
* Contactモデルはflange_aにBox、flange_bにBallを接続しないといけない
* PythagoraDeviceパッケージモデルを複数のmoファイルに分ける

# Reference

1. [ペナルティ法](https://www.cybernet.co.jp/ansys/glossary/penaltyhou.html)
2. [3D collision detection](https://developer.mozilla.org/en-US/docs/Games/Techniques/3D_collision_detection)
3. [○×（まるぺけ）つくろーどっとコム](http://marupeke296.com/COL_main.html)