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

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

[ 実行結果]

２Dシミュレーション
メッシュの描画には、 BabylonJS を使用しています。

参考にした文献とサイト
Position Based Dynamics
　　https://matthias-research.github.io/pages/publications/publications.html

Modeling and Solving Constraints
　　https://ubm-twvideo01.s3.amazonaws.com/o1/vault/gdc09/slides/04-GDC09_Catto_Erin_Solver.pdf

物理エンジンの作り方
　　http://cedec.cesa.or.jp/2008/archives/file/pg07.pdf
物理エンジンの作り方　その２
　　http://cedec.cesa.or.jp/2009/ssn_archive/pdf/sep3rd/PG75.pdf

実装用に参考にしたサイト
Inequality Constraints
http://myselph.de/gamePhysics/inequalityConstraints.html

2D Rigid Body Physics Engine
https://github.com/Sopiro/Physics
(実際に参照したのは、古いバージョン:(2021/11/06 ダウンロード)
の game.js。新バージョンの contact.js に相当。)

シミュレーションの方法
１　時間積分
（WorldConstraint.js class World, step メソッド）
　1) 重力による速度の更新
　 2) 壁との衝突による速度と角速度の更新
　　　(projectWalls メソッド)
　　　衝突時の拘束を解いて、速度と角速度を更新する。
　 3) 剛体同士の衝突による速度と角速度の更新
　　　(projectCollision メソッド)
　　　壁との衝突と同様に、衝突時の拘束を解いて、速度と角速度を更新する。
　 4) 位置と角度の更新
　　　新しく得られた速度と角速度を数値積分して、位置と角度を更新する。

２　壁との衝突
（WorldConstraint.js class World, projectWalls メソッド）
　　剛体と壁との衝突の様子
　
　剛体と壁との衝突点は、前回の ForceBased の場合と同様にして、知ることが
できる。
　衝突前後の速度と角速度を、それぞれ ${\bf{v}}_1$ 、 ${\boldsymbol{\omega}}_1$ 、 ${\bf{v}}_2$ 、 ${\boldsymbol{\omega}}_2$ とすると、
衝突点の速度は、並進の速度と回転による速度の合成速度で、次のようになる。
　　衝突前： ${\bf{v}}_{p1} = {\bf{v}}_1 + {\boldsymbol{\omega}}_1 \times {\bf{R}}_g$
　　衝突後： ${\bf{v}}_{p2} = {\bf{v}}_2 + \boldsymbol{\omega}_2 \times {\bf{R}}_g$
　　 ${\bf{R}}_g$ ：剛体の重心から衝突点へのベクトル（重心-衝突点ベクトル）

貫通なし拘束は、速度を用いて表される。
　　 $\dot{C} = W = \hat{\bf{n}} \cdot {\bf{v}}_{p2} \ge 0$ ( $\hat{\bf{n}}$ は衝突面の法線方向の単位ベクトル )
一般化速度 $\bf{u} = (\bf{v}, \boldsymbol{\omega})^{\rm{T}}$ ベクトル（T：転置）を用いると、拘束の式は次のようになる。
　　 $W = \hat{\bf{n}} \cdot {\bf{v}}_{p2} = \hat{\bf{n}} \cdot ( {\bf{v}}_{2} + {\boldsymbol{\omega}}_2 \times {\bf{R}}_g ) = \hat{\bf{n}} \cdot {\bf{v}}_{2} + ({\bf{R}}_g \times \hat{\bf{n}}) \cdot {\boldsymbol{\omega}}_2 = {\bf{J}} {\bf{u}}_2$
$\bf{J}$ はヤコビアンで、
　　 ${\bf{J}} = ( \hat{\bf{n}}, {\bf{R}}_g \times \hat{\bf{n}} )$

拘束の式に、反発の効果を bias として取り入れる。
反発係数(restitution)を $e$ とすると
　　 $\hat{\bf{n}} \cdot {\bf{v}}_{p2} \ge - e \hat{\bf{n}} \cdot {\bf{v}}_{p1}$
となるので、拘束の式を次のようにする。
　　 $W = {\bf{J}} {\bf{u}}_2 + b \ge 0$ ,　 $b = e \hat{\bf{n}} \cdot {\bf{v}}_{p1}$

次に拘束力を取り入れる。
拘束力 ${\bf{F}}_c$ は、運動量の変化で表される。
　　 ${\bf{M}} ( {\bf{u}}_{2} - {\bf{u}}_{1} ) = {\bf{F}}_c \ dt$
${\bf{M}}$ は質量マトリックスで、 ${\bf{M}} = (m{\bf{E}, {\bf{I}}})^{\rm{T}}$ （ $\bf{E}$ ：単位マトリックス、
$\bf{I}$ ：慣性モーメントマトリックス）である。 $dt$ は力の作用時間である。
拘束力の方向は ${\bf{J}}^{\rm{T}}$ となるので、大きさを $\lambda$ （未定乗数）として、
　　 ${\bf{J}}{\bf{u}}_{2} = {\bf{J}}{\bf{u}}_{1} + {\bf{J}}{\bf{M}}^{-1} \ {\bf{F}}_c \ dt = {\bf{J}}{\bf{u}}_{1} + {\bf{J}}{\bf{M}}^{-1} {\bf{J}}^{\rm{T}} \lambda dt 　( {\bf{F}}_c = {\bf{J}}^{\rm{T}} \lambda )$
$dt$ を $\lambda$ の中に含めると、次のようになる。
　　 ${\bf{J}}{\bf{u}}_{2} = {\bf{J}}{\bf{u}}_{1} + {\bf{J}}{\bf{M}}^{-1} {\bf{J}}^{\rm{T}} \lambda$
新たな $\lambda$ はインパルスに相当する。

拘束の式は、次のようになる。
　　 $W = {\bf{J}}{\bf{M}}^{-1} {\bf{J}}^{\rm{T}} \lambda + {\bf{J}}{\bf{u}}_{1} + b \ge 0$
この式から $\lambda$ を求める。
具体的には、以下の式を繰返し法を用いて解いていく。
　　 $\lambda = - m_C ({\bf{J}}{\bf{u}}_{1} + b)$ ,　 $m_C = \frac{1}{{\bf{J}}{\bf{M}}^{-1}{\bf{J}}^{\rm{T}}}$
　　 ${\bf{u}}_{2} = {\bf{u}}_{1} + {\bf{M}}^{-1} {\bf{J}}^{\rm{T}} \lambda$
　　 ${\bf{u}}_{2} \rightarrow {\bf{u}}_{1}$
このとき、繰返し毎にインパルス( $\lambda$ )を加算し、それがゼロとなるようにクランプ(clamp)する。
これで、 $\lambda \rightarrow 0$ (拘束力が作用しない) 、 $W_n = \hat{\bf{n}} \cdot {\bf{v}}_{p2} \ge 0$ (剛体が壁から離れた状態)となり、拘束が解けたことになる。

実際のシミュレーションでは、安定性のため、次のような項を取り入れる。
１つ目は、貫通に関する安定項である。
　　 $b_{1} = \beta (d - d_{slop}) \ / dt \ \ \ (d \ge d_{slop}) \ ,$
　　　　　 $0 \ \ (d < d_{slop} )$
　　 $d$ ：貫通の長さ、 $d_{slop}$ ：貫通に対するslop(遊び)で適当に設定、
　　 $\beta$ ：係数で適当に設定
２つ目は、反発項に関する修正である。
　　 $b_2 = e ({v}_n - v_{slop}) \ \ (v_n \ge v_{slop})\ ,$
　　　　　 $0 \ \ (v_n < v_{slop})$
　　 $v_n = \hat{\bf{n}} \cdot {\bf{v}}_{p1}$
　　 $v_{slop}$ ：反発に対するslopで適当に設定
bias項 $b$ を $b = b_1 + b_2$ にする。

　摩擦がある場合には、摩擦による拘束を追加する。
この場合の拘束は、次のように表される。
　　 $\dot{C} = W_t = \hat{\bf{t}} \cdot {\bf{v}}_{p2} = 0$ ( $\hat{\bf{t}}$ は衝突面の接線方向の単位ベクトル )
一般化速度 $\bf{u} = (\bf{v}, \boldsymbol{\omega})^{\rm{T}}$ を用いると、拘束の式は次のようになる。
　　 $W_t = {\bf{J}}_t {\bf{u}}_2$ ,　 ${\bf{J}}_t = ( \hat{\bf{t}}, {\bf{R}}_g \times \hat{\bf{t}} )$

次に拘束力を取り入れる。摩擦力は、
　　 $|F_t| \le \mu F_{cn}$
　　 $F_{cn}$ ：衝突面での垂直抗力 $\hat{\bf{n}} \cdot {\bf{F}}_{c}$
　　 $\mu$ ：摩擦係数
である。インパルスで表すと、次のようになる。
　　 $|\lambda_t| \le \mu \lambda_n$
$\lambda_n$ は上記の法線方向のインパルスである。加算したインパルスを用いる。

拘束の式は、インパルスを取り入れると次のようになる。
　　 $W_t = {\bf{J}}_t {\bf{u}}_{2} = {\bf{J}}_t {\bf{u}}_{1} + {\bf{J}}_t {\bf{M}}^{-1} {\bf{J}}_t^{\rm{T}} \lambda_t = {\bf{J}}_t {\bf{u}}_{1} + m_t^{-1} \lambda_t = 0$
　　 $m_t = \frac{1}{{\bf{J}}_t {\bf{M}}^{-1} {\bf{J}}_t^{\rm{T}}}$

この式とインパルスの式から $\lambda_t$ を求める。
具体的には、以下の式を、上述した法線方向の拘束の式の後に追加して、
繰返し法を用いて解いていく。
　　 $\lambda_t = - m_t {\bf{J}}{\bf{u}}_{1}$
　　 ${\bf{u}}_{2} = {\bf{u}}_{1} + {\bf{M}}^{-1} {\bf{J}}_t^{\rm{T}} \lambda_t$
　　 ${\bf{u}}_{2} \rightarrow {\bf{u}}_{1}$
このとき、繰返し毎にインパルス( $\lambda_t$ )を加算し、それが $-\mu \lambda_n$ と $+ \mu \lambda_n$ の間の値に
なるようにクランプ(clamp)する。これで拘束が解けたことになる。

繰返しの最後に得られた速度 ${\bf{u}}_{2}$ が、拘束によって更新された速度となる。

３　剛体同士の衝突
（WorldConstraint.js class World, projectCollision メソッド）
　剛体同士の衝突の様子

　衝突判定には、ForceBased と同様な方法、sap法（sap() 関数）と gjk-epa 法（gjk-epa.js）を用いている。
　gjk-epa から、貫通の深さ、衝突の方向（衝突面の法線方向）、衝突点の座標等の情報が得られる。

　衝突前後の剛体A、Bの速度と角速度を、それぞれ ${\bf{v}}_{A1}$ 、 ${\boldsymbol{\omega}}_{A1}$ 、 ${\bf{v}}_{A2}$ 、 ${\boldsymbol{\omega}}_{A2}$ 、 ${\bf{v}}_{B1}$ 、 ${\boldsymbol{\omega}}_{B1}$ 、 ${\bf{v}}_{B2}$ 、 ${\boldsymbol{\omega}}_{B2}$ とすると、
剛体A、Bの衝突点の速度は、次のようになる。
　　剛体A
　　衝突前： ${\bf{v}}_{Ap1} = {\bf{v}}_{A1} + {\boldsymbol{\omega}}_{A1} \times {\bf{R}}_A$
　　衝突後： ${\bf{v}}_{Ap2} = {\bf{v}}_{A2} + \boldsymbol{\omega}_{A2} \times {\bf{R}}_A$
　　剛体B
　　衝突前： ${\bf{v}}_{Bp1} = {\bf{v}}_{B1} + {\boldsymbol{\omega}}_{B1} \times {\bf{R}}_B$
　　衝突後： ${\bf{v}}_{Bp2} = {\bf{v}}_{B2} + \boldsymbol{\omega}_{B2} \times {\bf{R}}_B$
　　 ${\bf{R}}_A$ 、 ${\bf{R}}_B$ ：剛体A、Bの重心から衝突点へのベクトル

　貫通なし拘束は、剛体の相対速度を用いて表される。
　　 $\dot{C} = W = \hat{\bf{n}} \cdot ({\bf{v}}_{Bp2} - {\bf{v}}_{Ap2}) \ge 0$ ( $\hat{\bf{n}}$ は衝突面の法線方向の単位ベクトル )
一般化速度 ${\bf{u}} = ({\bf{v}}_{A}, \boldsymbol{\omega}_A, {\bf{v}}_{B}, \boldsymbol{\omega}_B)^{\rm{T}}$ ベクトルを用いると、拘束の式は次のようになる。
　　 $W = \hat{\bf{n}} \cdot ({\bf{v}}_{Bp2} - {\bf{v}}_{Ap2}) = {\bf{J}} {\bf{u}}_2$
　　 ${\bf{J}} = ( -\hat{\bf{n}}, -{\bf{R}}_A \times \hat{\bf{n}}, \ \hat{\bf{n}}, {\bf{R}}_B \times \hat{\bf{n}})$
　　 ${\bf{u}}_2 = ({\bf{v}}_{A2}, \boldsymbol{\omega}_{A2}, {\bf{v}}_{B2}, \boldsymbol{\omega}_{B2})^{\rm{T}}$

　拘束力をインパルス( ${\bf{J}}^{\rm{T}} \lambda$ )として取り入れると、拘束の式は次のようになる。
　　 $W = {\bf{J}}{\bf{M}}^{-1} {\bf{J}}^{\rm{T}} \lambda + {\bf{J}}{\bf{u}}_{1} + b \ge 0$
　　 ${\bf{M}}$ ：質量 $m_A$ 、 $m_B$ 、慣性モーメント $I_A$ 、 $I_B$ の質量マトリックス
　　 ${\bf{u}}_1 = ({\bf{v}}_{A1}, \boldsymbol{\omega}_{A1}, {\bf{v}}_{B1}, \boldsymbol{\omega}_{B1})^{\rm{T}}$
　　 $b$ ：bias

　摩擦がある場合の拘束の式は、次のようになる。
　　 $W_t = {\bf{J}}_t {\bf{u}}_{2} = {\bf{J}}_t {\bf{u}}_{1} + {\bf{J}}_t {\bf{M}}^{-1} {\bf{J}}_t^{\rm{T}} \lambda_t = 0$
　　 ${\bf{J}}_t = ( -\hat{\bf{t}}, -{\bf{R}}_A \times \hat{\bf{t}}, \ \hat{\bf{t}}, {\bf{R}}_B \times \hat{\bf{t}})$ 　( $\hat{\bf{t}}$ は衝突面の接線方向の単位ベクトル )
　　 $|\lambda_t| \le \mu \lambda_n$ （ $\mu$ ：摩擦係数）

　拘束は、壁との衝突の場合と同様に、繰返し法で解いていく。

プログラム
collision-constraint.html

<!-- collision-constraint.html <= BJS-gjk-constraint-0-1.html -->
<!DOCTYPE html>
<meta charset="utf-8">
<title>BJS Physical Animation ( constraint )</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(['./WorldConstraint', './math'], function(World, 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.getWorldShape();
            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.getWorldShape();

                //// 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) {
            //// position, normal: Vector2
            this.position = position;
            this.normal = normal;
            this.dist = function(position) {
                return this.normal.dot(position.subV(this.position));
            };
        }

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

            this.mass = mass;
            this.invMass = 1 / this.mass;
            this.inertia = this.mass * shape.inertia;
            this.invInertia = 1 / this.inertia;

            this.pos = pos;
            this.ang = 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));
  
            //// Returns the shape points in world space
            this.getWorldShape = function () {
                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;
            }
        }

        function centerOfMass(pts) {
            //// pts: Array, element Vector2
            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);
        }
    });

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

init();
</script>

WorldConstraint.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
    //// both endpoints at approximately the same distance
    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 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;
}

function lerp(a, b, t) {
    //// glmatrix vec2 lerp: linear interpolation between two vec2's
    //// a, b: Vector2, t: float (0 - 1)
    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);
}

const g = 9.8;
const dt = 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));
        });

        //// Solve Constraints
        //// Constraint Walls
        this.projectWalls();
        //// Constraint Collisions
        this.projectCollision();
        
        //// Update position and angle
        this.bodies.forEach(body => {
            body.pos = body.pos.addV(body.vel.mulS(dt));
            body.ang += body.angVel * dt;
        });
    }

    //// projectWalls (Resolve violated velocity constraint)
    projectWalls() {
        let normalImpulseSum = [];
        let tangentImpulseSum = [];

        let cnt2 = [];
        for (let i = 0; i < this.bodies.length; i++) cnt2[i] = [];

        let contact = [];
        let contactIdx = 0;
        this.bodies.forEach((body, j) => {
            this.walls.forEach((wall, k) => {
                let worldVertices = body.getWorldShape().slice();
                let contactPoint = new math.Vector2(0, 0);
                cnt2[j][k] = 0;
                let dist_min = Number.POSITIVE_INFINITY;

                for (let i = 0; i < worldVertices.length; i++) {
                    let dist = wall.dist(worldVertices[i]);
                    if (dist < dist_min) dist_min = dist;

                    if (dist <= 0) {
                        contactPoint = contactPoint.addV(worldVertices[i]);
                        cnt2[j][k]++;
                    }
                }

                if (cnt2[j][k] != 0) {
                    contactPoint = contactPoint.divS(cnt2[j][k]);
                    contact.push({contactPoint: contactPoint, dist_min: dist_min});
                    normalImpulseSum[contactIdx] = 0.0;
                    tangentImpulseSum[contactIdx] = 0.0;
                    contactIdx++;
                }
            });
        });

        for (let i = 0; i < this.nIterations; i++) {
            let contactIdx = 0;
            this.bodies.forEach((body, j) => {
                this.walls.forEach((wall, k) => {
                    if (cnt2[j][k] != 0) {
                        applyImpulseWall(contactIdx, contact, body, wall);
                        contactIdx++;
                    }
                });
            });
        }
        
        function applyImpulseWall(cIdx, contact, body, wall) {
            let contactNormal = wall.normal.inverted();
            let ra = contact[cIdx].contactPoint.subV(body.pos);

            //// Jacobian for non-penetration constraint
            let j_va = contactNormal.inverted();
            let j_wa = -ra.cross(contactNormal);
            
            let beta = 0.4;
            let restitution = 0.5;

            //// Velocity at contact point
            let vRot = new math.Vector2(
                -body.angVel * ra.y,
                body.angVel * ra.x);
            let vp = body.vel.addV(vRot);
            vp.invert();
            let vpn = vp.dot(contactNormal);
            
            let penetration_slop = 0.05;
            let restitution_slop = 5.0;
            let bias = -(beta / dt) * Math.max(-contact[cIdx].dist_min - penetration_slop, 0)
                + restitution * Math.max(vpn - restitution_slop, 0);
                
            let k = body.invMass
                + j_wa * body.invInertia * j_wa;
            let massNormal = 1.0 / k;

            //// Jacobian * velocity vector          
            let jv = j_va.dot(body.vel) + j_wa * body.angVel;
            let lambda = massNormal * -(jv + bias);

            let previousTotalLambda = normalImpulseSum[cIdx];
            normalImpulseSum[cIdx] = Math.max(0.0, normalImpulseSum[cIdx] + lambda);
            lambda = normalImpulseSum[cIdx] - previousTotalLambda;
            
            body.vel = body.vel.addV(j_va.mulS(body.invMass * lambda));
            body.angVel += body.invInertia * j_wa * lambda;

            //// Jacobian for friction constraint
            let contactTangent = new math.Vector2(-contactNormal.y, contactNormal.x);
            contactTangent.normalize();
    
            j_va = contactTangent.inverted();
            j_wa = -ra.cross(contactTangent);

            jv = j_va.dot(body.vel) + j_wa * body.angVel;
            k = body.invMass + j_wa * body.invInertia * j_wa;
            let massTangent = 1.0 / k;
            lambda = massTangent * -jv;

            let friction = 0.01;

            previousTotalLambda = tangentImpulseSum[cIdx];
            let maxFriction = friction * normalImpulseSum[cIdx];
            tangentImpulseSum[cIdx] = math.clamp(tangentImpulseSum[cIdx] + lambda, -maxFriction, maxFriction);
            lambda = tangentImpulseSum[cIdx] - previousTotalLambda;
            
            body.vel = body.vel.addV(j_va.mulS(body.invMass * lambda));
            body.angVel = body.angVel + body.invInertia * j_wa * lambda;
        } // function applyImpluseWall
    } // projectWalls

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

        let normalImpulseSum = [];
        let tangentImpulseSum = [];

        let nContacts = 0;
        for (let j = 0; j < this.bodies.length; j++) {
            nContacts += pairs[j].length;
        }

        for (let i = 0; i < nContacts; i++) {
            normalImpulseSum[i] = 0.0;
            tangentImpulseSum[i] = 0.0;
        }

        for (let i = 0; i < this.nIterations; i++) {
            let contactIdx = 0;
            this.bodies.forEach((body, j) => {
                pairs[j].forEach(k => {
                    applyImpulse(contactIdx, body, this.bodies[k]);
                    contactIdx++;
                });
            });
        } // for iteration

        function applyImpulse(cIdx, bodyA, bodyB) {
            let a = bodyA.getWorldShape().slice();
            let b = bodyB.getWorldShape().slice();

            let hit = Gjk.gjk(a, b);

            if (hit) {
                let { p, q, dist, 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];
  
                let aContact, bContact, aContactDisplaced, bContactDisplaced, penetration;
                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();

                    aContactDisplaced =
                        aContact.addV(
                            n.mulS((-penetration * massA) / (massA + massB))
                        );
                    bContactDisplaced = 
                        bContact.addV(
                            n.mulS((penetration * massB) / (massA + massB))
                    );
                } 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();
                        aContactDisplaced =
                            aContact.addV(
                                n.mulS((-penetration * massA) / (massA + massB))
                        );
                        bContactDisplaced = 
                            bPoints[0].addV(
                                n.mulS((penetration * massB) / (massA + massB))
                        );
                    } else {
                    //// bPoints.length == 2!
                        bContact = closestSegmentPoint(aPoints[0], ...bPoints);
                        penetration = aPoints[0].subV(bContact).getLength();
                        bContactDisplaced = 
                            bContact.addV(
                                n.mulS((penetration * massB) / (massA + massB))
                        );
                        aContactDisplaced =
                            aPoints[0].addV(
                                n.mulS((-penetration * massA) / (massA + massB))
                        );
                    }
                }

                let ra = aContactDisplaced.subV(bodyA.pos);
                let rb = bContactDisplaced.subV(bodyB.pos);

                //// Jacobian for non-penetration constraint
                let j_va = n.inverted();
                let j_wa = -ra.cross(n);
                let j_vb = n;
                let j_wb = rb.cross(n);

                let beta = 0.2;
                let restitution = 0.3;

                //// Relative velocity at contact point
                let vaRot = new math.Vector2(
                    -bodyA.angVel * ra.y, bodyA.angVel * ra.x);
                let vbRot = new math.Vector2(
                    -bodyB.angVel * rb.y, bodyB.angVel * rb.x);
                let vpa = bodyA.vel.addV(vaRot);
                let vpb = bodyB.vel.addV(vbRot);
                let vp = vpb.subV(vpa);
                let vpn = vp.dot(n);

                let penetration_slop = 0.1;
                let restitution_slop = 4.0;
                let bias = -(beta / dt) * Math.max(penetration - penetration_slop, 0) +
                    restitution * Math.max(vpn - restitution_slop, 0);

                let k = +bodyA.invMass
                    + j_wa * bodyA.invInertia * j_wa
                    + bodyB.invMass
                    + j_wb * bodyB.invInertia * j_wb;
                let massNormal = 1.0 / k;

                //// Jacobian * velocity vector
                let jv = +j_va.dot(bodyA.vel)
                    + j_wa * bodyA.angVel
                    + j_vb.dot(bodyB.vel)
                    + j_wb * bodyB.angVel;
                let lambda = massNormal * -(jv + bias);

                let previousTotalLambda = normalImpulseSum[cIdx];
                normalImpulseSum[cIdx] = Math.max(0.0, normalImpulseSum[cIdx] + lambda);
                lambda = normalImpulseSum[cIdx] - previousTotalLambda;

                bodyA.vel = bodyA.vel.addV(j_va.mulS(bodyA.invMass * lambda));
                bodyA.angVel += bodyA.invInertia * j_wa * lambda;
                bodyB.vel = bodyB.vel.addV(j_vb.mulS(bodyB.invMass * lambda));
                bodyB.angVel += bodyB.invInertia * j_wb * lambda;

                //// Jacobian for friction constraint
                let vTangent = new math.Vector2(-n.y, n.x);
                vTangent.normalize();
    
                j_va = vTangent.inverted();
                j_wa = -ra.cross(vTangent);
                j_vb = vTangent;
                j_wb = rb.cross(vTangent);
                jv = j_va.dot(bodyA.vel)
                    + j_wa * bodyA.angVel;
                    + j_vb.dot(bodyB.vel)
                    + j_wb * bodyB.angVel;

                k = bodyA.invMass
                    + j_wa * bodyA.invInertia * j_wa;
                    + bodyB.invMass
                    + j_wb * bodyB.invInertia * j_wb;
                let massTangent = 1.0 / k;
                lambda = massTangent * -jv;

                let friction = 0.2;

                previousTotalLambda = tangentImpulseSum[cIdx];
                let maxFriction = friction * normalImpulseSum[cIdx];
                tangentImpulseSum[cIdx] = math.clamp(tangentImpulseSum[cIdx] + lambda, -maxFriction, maxFriction);
                lambda = tangentImpulseSum[cIdx] - previousTotalLambda;

                bodyA.vel = bodyA.vel.addV(j_va.mulS(bodyA.invMass * lambda));
                bodyA.angVel = bodyA.angVel + bodyA.invInertia * j_wa * lambda;
                bodyB.vel = bodyB.vel.addV(j_vb.mulS(bodyB.invMass * lambda));
                bodyB.angVel = bodyB.angVel + bodyB.invInertia * j_wb * lambda;
            } // end-if(hit)
        } // end applyImplus function

    } // end projectCollision()
}

return World;
});

GNUstep プログラミング

おなが@京都先端科学大

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