物理シミュレーション　剛体の衝突　4　PositionBased ( ShapeMatching )

PositionBased ( ShapeMatching ) による、剛体の衝突シミュレーションです。

[ 実行結果]

参考にした文献とサイト
Meshless Deformations Based on Shape Matching
　https://matthias-research.github.io/pages/publications/publications.html
藤澤誠著「CGのための物理シミュレーションの基礎」( マイナビ )

実装用に参考にしたサイト
Physical animation with shape matching (Claudio Esperança)
　https://observablehq.com/@esperanc/physical-animation-with-shape-matching
右上にある […] アイコンの Export からコードをダウンロードできる。
69eb2ec167e70941@1046.js がメインのプログラムである。

シェイプマッチング (Shape Matching) 法
　シェイプマッチング法は、剛体を頂点座標で表し、剛体が変形した際の頂点の移動を、剛体の元の形状を保つように移動させる方法である。
（図は、「CGのための物理シミュレーションの基礎」より作成）

剛体の初期状態の頂点座標を ${\boldsymbol{x}}_i^0$ 、変形後の頂点座標を ${\boldsymbol{x}}_i$ とすると、 ${\boldsymbol{x}}_i$ は、回転行列 $R$ 、変形の行列 $S$ 、平行移動ベクトル $T$ を使って、以下のように表される。
　　 ${\boldsymbol{x}}_i = RS({\boldsymbol{x}}_i^0 - {\boldsymbol{x}}_{cm}^0) + {\boldsymbol{T}}$
　( ${\boldsymbol{x}}_{cm}^0$ ：初期状態の剛体の重心座標 )
最終的に移動する位置 ${\boldsymbol{g}}_i$ (目標位置、goal position)は、剛体の元の形状を保つように移動させるため、回転行列と平行移動ベクトルを使って表す。
　　 ${\boldsymbol{g}}_i = R({\boldsymbol{x}}_i^0 - {\boldsymbol{x}}_{cm}^0) + {\boldsymbol{T}}$

　 $R$ と $T$ は、 ${\boldsymbol{x}}_i$ と ${\boldsymbol{x}}_i^0$ から推定するが、これらは ${\boldsymbol{g}}_i$ と ${\boldsymbol{x}}_i$ の差が最小となるように、つまり以下の式が最小となるように決める。
　　 $\begin{aligned} & E = \sum_{i = 1}^N m_i | R({\boldsymbol{x}}_i^0 - {\boldsymbol{x}}_{cm}^0) + {\boldsymbol{T}} - {\boldsymbol{x}}_i |^2 　( m_i：重み、N：剛体の頂点数) \end{aligned}$

${\boldsymbol{T}} = \sum_i m_i {\boldsymbol{x}}_i / \sum_i m_i = {\boldsymbol{x}}_{cm}$ ( ${\boldsymbol{x}}_{cm}$ ：変形後の剛体の重心座標 ) であるので、
　　 $\begin{aligned} E &= \sum_{i = 1}^N m_i | R({\boldsymbol{x}}_i^0 - {\boldsymbol{x}}_{cm}^0) + {\boldsymbol{x}_{cm}} - {\boldsymbol{x}}_i |^2 \\ &= \sum_{i = 1}^N m_i | A{\boldsymbol{q}}_i - {\boldsymbol{p}}_i |^2 \end{aligned}$
ここで、 ${\boldsymbol{q}}_i = {\boldsymbol{x}}_i^0 - {\boldsymbol{x}}_{cm}^0$ 、 ${\boldsymbol{p}}_i = {\boldsymbol{x}}_i - {\boldsymbol{x}}_{cm}$ 、
$A = RS$ (回転の行列に変形の行列を含めて拡張した行列)である。

$E$ を $A$ の成分 $a_{ij}$ ( $k, j = 1 \sim d$ 、 $d$ ：次元) で微分する。
　　 $\begin{aligned} & \frac{\partial E}{\partial a_{kj}} = \sum_i m_i \frac{\partial}{\partial a_{kj}} | A{\boldsymbol{q}}_i - {\boldsymbol{p}}_i |^2 = \sum_i m_i \frac{\partial}{\partial a_{kj}} | \boldsymbol{S}_i |^2 \ \ ({\boldsymbol{S}}_i = (A{\boldsymbol{q}}_i - {\boldsymbol{p}}_i) ) \\ & \frac{\partial}{\partial a_{kj}}| \boldsymbol{S} |^2 = \frac{\partial}{\partial a_{kj}} \sum_{r = 1}^d S_r^2 = 2 \sum_{r = 1}^d S_r \frac{\partial S_r}{\partial a_{kj}} \end{aligned}$

　　 $\begin{aligned} \frac{\partial S_r}{\partial a_{kj}} &= \frac{\partial}{\partial a_{kj}} (A{\boldsymbol{q}} - {\boldsymbol{p}})_r = \frac{\partial}{\partial a_{kj}} \left( (A{\boldsymbol{q}})_r - p_r \right) = \frac{\partial}{\partial a_{kj}} \left( \sum_{l = 1}^d a_{rl} q_l - p_r \right)\\ &= \frac{\partial}{\partial a_{kj}} \left( \sum_{l = 1}^d a_{rl} q_l - p_r \right) = \sum_{l = 1}^d \delta_{k,r} \ \delta_{j,l} \ q_l = q_j \delta_{k,r} \end{aligned}$

　　 $\begin{aligned} \frac{\partial E}{\partial a_{kj}} & = \sum_i m_i \frac{\partial}{\partial a_{kj}}| \boldsymbol{S}_i |^2 = \sum_i 2 m_i \sum_{r = 1}^d (\boldsymbol{S}_i)_r \ (\boldsymbol{q}_i)_j \ \delta_{k,r} \\ & = \sum_i 2 m_i \ (\boldsymbol{S}_i)_k \ (\boldsymbol{q}_i)_j = \sum_i 2 m_i \left( \boldsymbol{S}_i \boldsymbol{q}_i^{\rm{T}} \right)_{kj} \\ & = \sum_i 2 m_i \left( (A{\boldsymbol{q}}_i - {\boldsymbol{p}}_i) \ \boldsymbol{q}_i^{\rm{T}} \right)_{kj} \end{aligned}$

$\partial E / \partial a_{kj} = 0$ より
　　 $\begin{aligned} & \sum_{i = 1}^N m_i \ (A{\boldsymbol{q}}_i - {\boldsymbol{p}}_i) \ \boldsymbol{q}_i^{\rm{T}} = 0 \\ & A = \left( \sum_{i = 1}^N m_i {\boldsymbol{p}}_i \boldsymbol{q}_i^{\rm{T}} \right) \left( \sum_{i = 1}^N m_i {\boldsymbol{q}}_i \boldsymbol{q}_i^{\rm{T}} \right)^{-1} = A_{pq} A_{qq}^{-1} \\ & A_{qq}^{-1}：対称行列で、スケーリング（拡大縮小）だけを含む部分 \\ & A_{pq}：変形 S と回転 R を含む部分（変形勾配テンソル） \end{aligned}$

$R$ は、 $A_{pq} = RS$ の右極分解から得ることができる。( 極分解については下参照）
　　 $R = A_{pq} S^{-1} , \ \ \ S = \left( A_{pq}^{\rm{T}} A_{pq} \right)^{1/2}$
これから、剛体の移動後の目標位置 ${\boldsymbol{g}}_i$ が求められる。

極分解
　任意の正方行列は、対称行列と直交行列の積に分解される。これが極分解である。
　 $A$ ：正方行列、 $Q$ ：直交行列、 $L$ 、 $M$ ：対称行列として、
　　右極分解 $A = QL$
　　左極分解 $A = MQ$

$L$ 、 $M$ は対称行列なので、
　　 $L^{\rm{T}} = L , \ \ M^{\rm{T}} = M$
$Q$ は直交行列なので、
　　 $Q^{\rm{T}} Q = Q Q^{\rm{T}} = I$ 　( $I$ ：単位行列）
である。これより、右極分解の場合、 $L$ と $Q$ が次のように求まる。
　　 $A^{\rm{T}} A = (L^{\rm{T}}Q^{\rm{T}})QL = L^{\rm{T}}Q^{\rm{T}}QL = L^{\rm{T}}L = L^2$
　　 $L = (A^{\rm{T}}A)^{1/2}$
　　 $Q = AL^{-1}$

　ここで、行列の平方根が必要となるが、これは以下のように計算することができる。
１）ケーリー・ハミルトン(Cayley-Hamilton)の定理を使う
　　The Square Roots of 2 x 2 Matrices
　　 (https://www.maa.org/sites/default/files/pdf/cms_upload/Square_Roots-Sullivan13884.pdf) 参照
　 $X$ と $A$ は２次正方行列であるとする。 $X = A^{1/2} \ (X^2 = A)$ であるとき、 $X$ と $A$ にケーリー・ハミルトンの定理を用いると、 $X$ は次にように求まる。
　　 $X = \frac{A + \sqrt{\rm{det}A} \ I}{\sqrt{\rm{tr}A + 2\sqrt{\rm{det}A}}}$

２）対角化の方法を使う
　行列 $A = \begin{pmatrix} a & b \\ c & d \end{pmatrix}$ の固有値は $\lambda_1 , \lambda_2 = \frac{1}{2} \left((a + d) \mp \sqrt{(a+d)^2 - 4(ad - bc)} \right)$ 、固有ベクトルを $\boldsymbol{x}_1 = \begin{pmatrix} b \\ \lambda_1 - a \end{pmatrix}$ 、 $\boldsymbol{x}_2 = \begin{pmatrix} b \\ \lambda_2 - a \end{pmatrix}$ とすると、対角化行列 $P$ とその逆行列 $P^{-1}$ は、次のようになる。
　　 $P = \begin{pmatrix} b & b \\ \lambda_1 - a & \lambda_2 - a \end{pmatrix}$ 、
　　 $P^{-1} = \frac{1}{|P|} \begin{pmatrix} \lambda_2 - a & -b \\ -(\lambda_1 - a) & b \end{pmatrix}$ 、　 $|P| = b(\lambda_2 - \lambda_2)$
対角行列 $\Lambda = P^{-1} A P$ は、
　　 $\Lambda = \begin{pmatrix} \lambda_1 & 0 \\ 0 & \lambda_2 \end{pmatrix}$
である。対角行列の平方根はすぐに求まる。
　　 $\Lambda^{1/2} = \begin{pmatrix} \lambda_1^{1/2} & 0 \\ 0 & \lambda_2^{1/2} \end{pmatrix}$
　これより、行列 $A$ の平方根の行列 $X$ は、
　　 $X = P \Lambda^{1/2} P^{-1} = \frac{1}{\sqrt{\lambda_1} + \sqrt{\lambda_2}} \begin{pmatrix} a + \sqrt{\lambda_1 \lambda_2} & b \\ c & d + \sqrt{\lambda_1 \lambda_2} \end{pmatrix}$
$\lambda_1 \lambda_2 = {\rm{det}}A = ad - bc$ 、 $\lambda_1 + \lambda_2 = {\rm{tr}}A = a + d$ に注意して式を変形すると、１）の $X$ と同じ式が得られる。
mat2.js ライブラリーでの行列の平方根 (mat2sqrt) の計算は、この式を用いている。

シミュレーションの方法
１　時間積分
（WorldShapeMatching.js class World, step メソッド）
　1) 重力による速度の更新と位置、回転角の更新
　 2) 壁との衝突後に移動させる剛体の頂点座標(goal position, 目標位置)の計算
　　　(class World, projectWalls メソッド
　　　　 + BodyShapeMatching.js class Body, shapeMatch メソッド )
　 3) 剛体同士の衝突後に移動させる剛体の頂点座標(goal position)の計算
　　　(class World, projectCollision メソッド
　　　　 + class Body, shapeMatch メソッド )
　 4) 剛体の回転角と位置の更新
　　　(class Body, updateFromShape メソッド )
　　　2), 3) で得られた頂点座標から、剛体全体の回転角と位置を更新する。
　　　通常のシェイプマッチングでは、剛性(stiffness)の効果を各頂点に施すが、
　　　ここでは剛体全体の回転角と位置に施している。
　　　(shapeMatchAttenuation の部分)
　 5) 剛体の速度と角速度の更新

２　壁との衝突
　(class World, projectWalls メソッド + class Body, shapeMatch メソッド )
　剛体と壁との衝突
　　vertex で衝突する場合
　

　　edge で衝突する場合

　剛体が壁と衝突した状態（上図変形の左側の図）を初期状態とし、その時の頂点座標をとする。変形後の状態（上図変形の右側の図）の頂点座標を図のように、めり込んだ頂点を壁の表面まで押し返したようにとる。
　プログラムでは、 ${\boldsymbol{x}}_i^0$ 、 ${\boldsymbol{x}}_i$ を、それぞれ body.worldShape 、body.collisionShape としている。 ${\boldsymbol{x}}_i$ を計算しているのが、world.projectWalls(body) メソッドである。
これらの値から、実際に移動させる剛体の回転行列 $R$ と平行移動行列 $T$ (平行移動ベクトルを行列にしている)を計算し、頂点座標 ${\boldsymbol{g}}_i$ を求める。この部分が body.shapeMatch() メソッドである。
body.shapeMatch() メソッド
　　M = shapeMatch(this.worldShape, this.collisionShape)
　　　-> ${\boldsymbol{x}}_{cm}^0$ と ${\boldsymbol{x}}_{cm}$ の平行移動も含めた行列として $R$ と $T$ を計算し、
　　　　 $M = RT$ としている。
　　this.collisionShape = MT.mulVectors(this.worldShape, 1)
　　　-> ${\boldsymbol{g}}_i = M{\boldsymbol{x}}_i^0$ ( MT = M ) を計算し、 ${\boldsymbol{g}}_i$ を新たに body.collisionShape
　　　　としている。 ${\boldsymbol{g}}_i = R({\boldsymbol{x}}_i^0 - {\boldsymbol{x}}_{cm}^0) + {\boldsymbol{x}}_{cm}$ の計算をしている。

３　剛体同士の衝突
(class World, projectCollision メソッド + class Body, shapeMatch メソッド )
　剛体同士の衝突
　　vertex と edge で衝突

　　edge と edge で衝突

　衝突判定には、前回と同様に、sap法と gjk-epa法を用いている。
剛体同士が衝突した状態を初期状態とし、その時の頂点座標が ${\boldsymbol{x}}_i^0$ である。変形後の状態の頂点座標 ${\boldsymbol{x}}_i$ は、gjk-epa から得られる貫通の深さの半分を、衝突している頂点を衝突の法線方向に押し戻したようにとる。
　壁との衝突と同様に、この ${\boldsymbol{x}}_i^0$ と ${\boldsymbol{x}}_i$ から、回転行列 $R$ と平行移動行列 $T$ を計算し、移動する頂点座標 ${\boldsymbol{g}}_i$ を求める。

プログラム
collision-shapeMatching.html

<!DOCTYPE html>
<meta charset="utf-8">
<title>BJS Physical Animation ( ShapeMatching )</title>
<script src="https://preview.babylonjs.com/babylon.js"></script>
<script src="https://cdnjs.cloudflare.com/ajax/libs/require.js/2.3.5/require.min.js"></script>

<body>
<canvas id="canvas" width="600" height="430" style="border: 1px solid gray;"></canvas>

<script>
function init() {
    const canvas = document.getElementById("canvas");
    const engine = new BABYLON.Engine(canvas);
    var width  = canvas.width;
    var height = canvas.height;

    var scene = new BABYLON.Scene(engine);
    scene.clearColor = new BABYLON.Color3(1.0, 1.0, 1.0);

    var camera = new BABYLON.ArcRotateCamera("camera1",
        3 * Math.PI / 2, Math.PI / 2, 25, new BABYLON.Vector3(0, 8, 0), scene);
    camera.attachControl(canvas, true);

    var light1 = new BABYLON.HemisphericLight("light1", new BABYLON.Vector3(0, 1, 0), scene);
    light1.intensity = 0.8;
    var light2 = new BABYLON.HemisphericLight("light2", new BABYLON.Vector3(0, -1, 0), scene);
    light2.intensity = 0.5;

    require(['./WorldShapeMatching', './BodyShapeMatching', './math'], function(World, Body, math) {
        let walls = [];
        let wall_angle1 = -0.3;
        let wall_angle2 =  0.5;
        walls.push(new Wall(new math.Vector2( 0, 0),
            new math.Vector2(-Math.sin(wall_angle1), Math.cos(wall_angle1))));
        walls.push(new Wall(new math.Vector2( 0, 0),
            new math.Vector2(-Math.sin(wall_angle2), Math.cos(wall_angle2))));

        let bodies = [];

        // body 0
        bodies.push(
            new Body(
                createPolygonPoints(new math.Vector2(0, 0), 2, 4),
                new math.Vector2(7, 14), // world position(pos)
                Math.PI / 4, // angle
                1 // mass
            )
        );
        bodies[0].vel = new math.Vector2(0, 0);
        bodies[0].angVel = 0.0;

        // body 1
        bodies.push(
            new Body(
                createPolygonPoints(new math.Vector2(0, 0), 1, 4),
                new math.Vector2(-12, 14),
                0,
                1
            )
        );
        bodies[1].vel = new math.Vector2(0, 0);
        bodies[1].angVel = 0.0;

        // body 2
        bodies.push(
            new Body(
                createPolygonPoints(new math.Vector2(0, 0), 1.5, 4),
                new math.Vector2(-8, 14),
                0,
                1
            )
        );
        bodies[2].vel = new math.Vector2(2, 0);
        bodies[2].angVel = 0.0;
        //
        console.log("bodies", Array.from(bodies));

        var d, p1, p2;
        for (let i = 0; i < walls.length; i++) {
            d = new math.Vector2(400 * walls[i].normal.y, -400 * walls[i].normal.x);
            p1 = walls[i].position.addV(d);
            p2 = walls[i].position.subV(d);
            let wallpath = [
                new BABYLON.Vector3(p1.x, p1.y, 0),
                new BABYLON.Vector3(p2.x, p2.y, 0)
            ];
            let wallLines = BABYLON.Mesh.CreateLines("lines" + i, wallpath, scene);
            wallLines.color = BABYLON.Color3.Black();
        }

        var i, j;
        var objects, object;

        var colors = [
            BABYLON.Color3.Blue(),
            BABYLON.Color3.Green(),
            BABYLON.Color3.Black(),
        ];

        var objectMesh = [];
        objects = bodies;

        for (j = 0; j < objects.length; ++j) {
            object = objects[j];

            var worldVertices = object.worldShape;
            console.log("object index", j, worldVertices);

            const abodyMat = new BABYLON.StandardMaterial("abodyMat" + j, scene);
            abodyMat.diffuseColor = colors[j];

            var points = [];
            for (i = 0; i < worldVertices.length; ++i) {
                const apoint = BABYLON.MeshBuilder.CreateSphere("apoint" + i,
                    {diameter: 0.3, segments: 8}, scene);
                apoint.position.x = worldVertices[i].x;
                apoint.position.y = worldVertices[i].y;
                apoint.position.z = 0;
                apoint.material = abodyMat;
                points.push(apoint);
            }

            var path = [];
            for (i = 0; i < worldVertices.length; i++)
                path.push(
                    new BABYLON.Vector3(worldVertices[i].x, worldVertices[i].y, 0)
                );
            path.push(new BABYLON.Vector3(worldVertices[0].x, worldVertices[0].y, 0));
            const options = {points: path, updatable: true};
            var aLine = BABYLON.MeshBuilder.CreateLines("aLine" + j, options, scene);
            aLine.color = colors[j];

            objectMesh.push({points: points, line: aLine});
        }

        function update_mesh() {
            var i, j

            for (j = 0; j < objects.length; j++) {
                var object = objects[j];

                var worldVertices = object.worldShape;

                //// points
                var apoint = objectMesh[j].points;
                for (i = 0; i < worldVertices.length; ++i) {
                    apoint[i].position.x = worldVertices[i].x;
                    apoint[i].position.y = worldVertices[i].y;
                    apoint[i].position.z = 0.0;
                }
                //// line
                var aline = objectMesh[j].line;
                var path = [];
                for (i = 0; i < worldVertices.length; i++)
                    path.push(
                        new BABYLON.Vector3(worldVertices[i].x, worldVertices[i].y, 0)
                    );
                path.push(new BABYLON.Vector3(worldVertices[0].x, worldVertices[0].y, 0));

                var options = {points: path, instance: aline};
                aline = BABYLON.MeshBuilder.CreateLines("aline", options);
            }
        }
    
        let world = new World();
        world.walls = walls;
        world.bodies = bodies;

        scene.registerBeforeRender(function () {
            world.step();
            update_mesh();
        });

        function createPolygonPoints(center = new math.Vector2(0, 0), radius = 1, nsides=3) {
            let poly = [];

            let delta = Math.PI * 2 / nsides;
            for (let i = 0; i < nsides; i++) {
                poly.push(
                    new math.Vector2(center.x+radius*Math.cos(delta*i),
                                     center.y+radius*Math.sin(delta*i))
                )
            }

            // inertia
            let [a, b] = [(1 + Math.cos(delta)) / 2, (1 - Math.cos(delta)) / 2];
            let inertia = radius * radius / 6 * ( 3*(a**3 + b**3) + 9*a*b - 2*b );

            return {shape:poly, inertia:inertia};
        }

        function Wall(position, normal) {
            this.position = position;
            this.normal = normal;
            this.dist = function(position) {
                return this.normal.dot(position.subV(this.position));
            };
        }
    });

    engine.runRenderLoop(() => {
        scene.render();
    });
}

init();
</script>

WorldShapeMatching.js

define(['./math', './gjk_epa'], function(math, Gjk) {
function centerOfMass(pts) {
    let sum = pts.reduce((a,b) => new math.Vector2(a.x+b.x, a.y+b.y));
    return new math.Vector2(sum.x/pts.length, sum.y/pts.length);
}

function supportEdge(pts, v) {
    let maxdot = Number.NEGATIVE_INFINITY;
    let maxdot2 = Number.NEGATIVE_INFINITY;
    let best, best2;
    for (let p of pts) {
        let dot = p.dot(v);
        if (dot > maxdot) {
            [maxdot2, best2] = [maxdot, best];
            [maxdot, best] = [dot, p];
        } else if (dot > maxdot2) {
            [maxdot2, best2] = [dot, p];
        }
    }
    if (Math.abs(maxdot - maxdot2) < 0.01) return [best2, best];
    return [best];
}

function closestSegmentPoint(p, q, r) {
    let qr = r.subV(q);
    let s = qr.getLength();
    if (s < 0.00001) return q; // Degenerate line segment
    let v = qr.normalized();
    let u = p.subV(q);
    let d = u.dot(v);
    if (d < 0) return q;
    if (d > s) return r;
    return lerp(q, r, d/s);
}

function lerp(a, b, t) {
    var ax = a.x,
        ay = a.y;
    out_x = ax + t * (b.x - ax);
    out_y = ay + t * (b.y - ay);
    return new math.Vector2(out_x, out_y);
}

function sap(polygons, v) {
    let n = polygons.length;
    let pairs = [];
    let proj = [];
    polygons.forEach((poly, i) => {
        let min = Number.POSITIVE_INFINITY;
        let max = Number.NEGATIVE_INFINITY;
        for (let p of poly) {
            let dot = p.dot(v);
            min = Math.min(min, dot);
            max = Math.max(max, dot);
        }
        proj.push([min, i], [max, i]);
    });
    proj.sort((a, b) => a[0] - b[0]);

    let inside = new Set();
    for (let [_, i] of proj) {
        if (inside.has(i)) {
            inside.delete(i);
        } else {
            pairs[i] = [];
            for (let j of inside) {
                if (i < j) pairs[j].push(i);
                else pairs[i].push(j);
            }
            inside.add(i);
        }
    }
    return pairs;
}

const g = 9.8;
const dt = 0.02;

const wall_restitution = 0.9;
const wall_friction = 0.02;

class World {
    constructor() {
        this.nIterations = 4;

        this.bodies = [];
        this.walls = [];
    }

    step() {
        //// Apply external force
        this.bodies.forEach(body => {
            body.vel = body.vel.addV(new math.Vector2(0, -g * dt));

            body.pos = body.oldPos.addV(body.vel.mulS(dt));
            body.ang = body.oldAng + body.angVel * dt;
        });

        //// collisions
        for (let irelax = 0; irelax < this.nIterations; irelax++) {
            //// walls
            this.bodies.forEach(body =>
                this.projectWalls(body)
            );

            this.bodies.forEach(body => body.shapeMatch());

            //// bodies
            let pairs = sap(this.bodies.map(body => body.worldShape), new math.Vector2(1, 0));

            this.bodies.forEach((body, i) => {
                pairs[i].forEach(j => this.projectCollision(body, this.bodies[j]), irelax);
            });
                
            this.bodies.forEach(body => body.shapeMatch());
        } // for irelax

        this.bodies.forEach(body => {
            if (body.collisionShape) body.updateFromShape(body.collisionShape);
        });

        this.bodies.forEach(body => {
            body.vel = body.pos.subV(body.oldPos).divS(dt);
            body.oldPos = body.pos;

            body.angVel = (body.ang - body.oldAng) / dt;
            body.oldAng = body.ang;
        });
    }

    //// projectWalls
    projectWalls(body) {
        let projected = false;
        let poly = body.collisionShape || body.worldShape.slice();

        poly.forEach((p, i) => {
            for (let wall of this.walls) {
                let dist = wall.dist(p);
                if (dist <= 0) {
                    var velocity_normal  = wall.normal.mulS(wall.normal.dot(body.vel));
                    var velocity_tangent = body.vel.subV(velocity_normal);
                    velocity_normal  = velocity_normal.mulS(-wall_restitution);
                    velocity_tangent = velocity_tangent.mulS(-wall_friction);
                    p = p.addV(velocity_normal.addV(velocity_tangent).mulS(dt));
                    dist = wall.dist(p);
                    if (dist < 0)
                        p = p.addV(wall.normal.mulS(-dist));
                    projected = true;
                }
            }
                poly[i] = p;
        });
        if (projected) body.collisionShape = poly;
    }

    //// projectCollision
    projectCollision(bodyA, bodyB, gravityBias = 0) {
      let a = bodyA.collisionShape || bodyA.worldShape.slice();
      let b = bodyB.collisionShape || bodyB.worldShape.slice();
      let hit = Gjk.gjk(a, b);

      if (hit) {
          let { p, q, n } = Gjk.epa(a, b, ...hit);
  
          let aPoints = supportEdge(a, n);
          let bPoints = supportEdge(b, n.mulS(-1));
  
          let [massA, massB] = [bodyA.mass, bodyB.mass];
          if (gravityBias) {
            if (bodyA.pos.y > bodyB.pos.y) massA += massB * gravityBias;
            else massB += massA * gravityBias;
          }
  
          let aContact, bContact, penetration;
          let aContactDisplaced1, aContactDisplaced2, bContactDisplaced1, bContactDisplaced2;
          if (aPoints.length + bPoints.length == 4) {
              //// Edge-edge collision
              let center = centerOfMass([...aPoints, ...bPoints]);
              aContact = closestSegmentPoint(center, ...aPoints);
              bContact = closestSegmentPoint(center, ...bPoints);
              penetration = aContact.subV(bContact).getLength();

              aContactDisplaced1 =
                  aPoints[0].addV(
                  n.mulS((-penetration * massA) / (massA + massB))
              );
              aContactDisplaced2 =
                  aPoints[1].addV(
                  n.mulS((-penetration * massA) / (massA + massB))
              );

              bContactDisplaced1 = 
                  bPoints[0].addV(
                  n.mulS((penetration * massB) / (massA + massB))
              );
              bContactDisplaced2 = 
                  bPoints[1].addV(
                  n.mulS((penetration * massB) / (massA + massB))
              );

              a.splice(a.lastIndexOf(aPoints[0]), 1, aContactDisplaced1);
              a.splice(a.lastIndexOf(aPoints[1]), 1, aContactDisplaced2);

              b.splice(b.lastIndexOf(bPoints[0]), 1, bContactDisplaced1);
              b.splice(b.lastIndexOf(bPoints[1]), 1, bContactDisplaced2);
          } else {
              //// Vertex-edge collision
              if (aPoints.length + bPoints.length != 3) {
                  throw "Weird collision";
              }
              if (aPoints.length == 2) {
                  aContact = closestSegmentPoint(bPoints[0], ...aPoints);
                  penetration = aContact.subV(bPoints[0]).getLength();

                  aContactDisplaced1 =
                      aPoints[0].addV(
                      n.mulS((-penetration * massA) / (massA + massB))
                  );
                  aContactDisplaced2 =
                      aPoints[1].addV(
                      n.mulS((-penetration * massA) / (massA + massB))
                  );

                  a.splice(a.lastIndexOf(aPoints[0]), 1, aContactDisplaced1);
                  a.splice(a.lastIndexOf(aPoints[1]), 1, aContactDisplaced2);

                  bContactDisplaced1 = 
                      bPoints[0].addV(
                      n.mulS((penetration * massB) / (massA + massB))
                  );
                  b.splice(b.lastIndexOf(bPoints[0]), 1, bContactDisplaced1);
              } else {
                  //// bPoints.length == 2!
                  bContact = closestSegmentPoint(aPoints[0], ...bPoints);
                  penetration = aPoints[0].subV(bContact).getLength();

                  bContactDisplaced1 = 
                      bPoints[0].addV(
                          n.mulS((penetration * massB) / (massA + massB))
                  );
                  bContactDisplaced2 = 
                      bPoints[1].addV(
                          n.mulS((penetration * massB) / (massA + massB))
                  );

                  b.splice(b.lastIndexOf(bPoints[0]), 1, bContactDisplaced1);
                  b.splice(b.lastIndexOf(bPoints[1]), 1, bContactDisplaced2);

                  aContactDisplaced1 =
                      aPoints[0].addV(
                        n.mulS((-penetration * massA) / (massA + massB))
                  );
                  a.splice(a.lastIndexOf(aPoints[0]), 1, aContactDisplaced1);
              }
          }
          bodyA.collisionShape = a;
          bodyB.collisionShape = b;
      }
    }
}

return World;
});

BodyShapeMatching.js

define(['./math', './mat2'], function(math, mat2) {
function centerOfMass(pts) {
    let sum = pts.reduce((a,b) => new math.Vector2(a.x+b.x, a.y+b.y));
    return new math.Vector2(sum.x/pts.length, sum.y/pts.length);
}

function lerp(a, b, t) {
    var ax = a.x,
        ay = a.y;
    out_x = ax + t * (b.x - ax);
    out_y = ay + t * (b.y - ay);
    return new math.Vector2(out_x, out_y);
}

function shapeMatch(srcPoints, dstPoints) {
    //// srcPoints
    let srcCenter = centerOfMass(srcPoints);
    let srcVectors = srcPoints.map(p => p.subV(srcCenter));
    
    //// dstPoints
    let dstCenter = centerOfMass(dstPoints);
    let dstVectors = dstPoints.map (p => p.subV(dstCenter));
    
    //// Compute rotation and compose with the two translations
    let [[a,b],[c,d]] = shapeMatchRotation(srcVectors,dstVectors);
    
    let rot = new math.Matrix3();
    rot = rot.fromValues(a, b, 0, c, d, 0, dstCenter.x, dstCenter.y, 1);

    let trans = new math.Matrix3();
    trans = trans.fromValues(1, 0, 0, 0, 1, 0, -srcCenter.x, -srcCenter.y, 1);
    let transformMatrix = trans.mulMatrix(rot);

    return transformMatrix;
  }

function shapeMatchRotation(srcVectors, dstVectors) {
    //// srcVectors, dstVectors: convert element Vector2 -> element Array
    let srcVectors2 = srcVectors.map(p => [p.x, p.y]);
    let dstVectors2 = dstVectors.map(p => [p.x, p.y]);

    //// function that computes p x q^T
    let pqT = (p,q) => [[p[0]*q[0],p[0]*q[1]],
                        [p[1]*q[0],p[1]*q[1]]];
    
    let Apq = srcVectors2.map((p,i) => pqT(p,dstVectors2[i])).reduce(mat2.mat2sum)

    let ApqTxApq = mat2.mat2mul(mat2.mat2transpose(Apq),Apq);
    let S = mat2.mat2sqrt(ApqTxApq)
    let Sinv = mat2.mat2inv(S);

    return mat2.mat2mul(Apq,Sinv)
}

const shapeMatchAttenuation = 0.98;

class Body {
    constructor(shape, pos = new Vector2(0, 0), ang = 0, mass = 1) {
        console.log("ceate Bodies", shape);

        this.mass = mass;
        this.pos = this.oldPos = pos;
        this.ang = this.oldAng = ang;

        this.vel = new math.Vector2(0, 0);
        this.angVel = 0;

        let center = centerOfMass(shape.shape);
        this.shape = shape.shape.map(p => new math.Vector2(p.x - center.x, p.y - center.y));
        this.collisionShape = null; // Non-null if collision was detected
    }
  
    //// Returns the shape points in world space
    get worldShape() {
        let [c, s] = [Math.cos(this.ang), Math.sin(this.ang)];
        let transf = new math.Matrix3();
        transf = transf.fromValues(c, -s, this.pos.x, s, c, this.pos.y, 0, 0, 0);
        this.curWorldShape = transf.mulVectors(this.shape, 1);

        return this.curWorldShape;
    }
  
    //// ShapeMatching
    shapeMatch() {
      if (this.collisionShape) {
        let M = shapeMatch(this.worldShape, this.collisionShape);

        let MT = new math.Matrix3();
        MT = MT.fromValues(
            M.m00, M.m10, M.m20,
            M.m01, M.m11, M.m21,
            M.m02, M.m12, M.m22
        );
        
        this.collisionShape = MT.mulVectors(this.worldShape, 1);
      }
    }
  
    //// Updates rotation and position
    updateFromShape(shape) {
        let center = centerOfMass(shape);
  
        //// Rotation component
        let rot = shapeMatchRotation(
            this.worldShape.map(p => p.subV(this.pos)),
            shape.map(p => p.subV(center))
        );

        if (!Number.isNaN(rot[0][0])) {
            //// Avoid degenerate projections
            let dang = Math.atan2(rot[0][1], rot[0][0]);
            this.ang += dang * shapeMatchAttenuation;
        }
  
        //// Translation component
        this.pos = lerp(this.pos, center, shapeMatchAttenuation);
  
        //// new worldShape
        this.curWorldShape = shape;
        this.collisionShape = null;
    }
}

return Body;
});

mat2.js

// 2x2 matrix utilities
define( function() {
return {
    mat2mul: function(a,b) {
        let prod = (i,j) => a[i][0]*b[0][j]+a[i][1]*b[1][j];
        return [[prod(0,0),prod(0,1)],
                [prod(1,0),prod(1,1)]]
    },
    mat2sum: function(m1,m2) {
        return [[m1[0][0]+m2[0][0],m1[0][1]+m2[0][1]],
                [m1[1][0]+m2[1][0],m1[1][1]+m2[1][1]]]
    },
    mat2transpose: function(m) {
        return [[m[0][0],m[1][0]],
                [m[0][1],m[1][1]]]
    },
    mat2det: function([[a,b],[c,d]]) {
        return a*d-b*c
    },
    mat2sqrt: function([[a,b],[c,d]]) {
        let s = Math.sqrt(a*d-b*c);
        let t = Math.sqrt(a+d+2*s);
        return [[(a+s)/t,b/t],[c/t,(d+s)/t]]
    },
    mat2inv: function([[a,b],[c,d]]) {
        let det = a*d-b*c;
        return [[d/det,-c/det],[-b/det,a/det]]
    }
}

});

gjk-epa.js、math.js は「剛体の衝突１」に掲載してある。