PositionBased(Constraint) による、剛体の衝突シミュレーションです。
[ 実行結果]
2Dシミュレーション
メッシュの描画には、 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
物理エンジンの作り方 その2
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 に相当。)
シミュレーションの方法
1 時間積分
(WorldConstraint.js class World, step メソッド)
1) 重力による速度の更新
2) 壁との衝突による速度と角速度の更新
(projectWalls メソッド)
衝突時の拘束を解いて、速度と角速度を更新する。
3) 剛体同士の衝突による速度と角速度の更新
(projectCollision メソッド)
壁との衝突と同様に、衝突時の拘束を解いて、速度と角速度を更新する。
4) 位置と角度の更新
新しく得られた速度と角速度を数値積分して、位置と角度を更新する。
2 壁との衝突
(WorldConstraint.js class World, projectWalls メソッド)
剛体と壁との衝突の様子
剛体と壁との衝突点は、前回の ForceBased の場合と同様にして、知ることが
できる。
衝突前後の速度と角速度を、それぞれ 、 、 、 とすると、
衝突点の速度は、並進の速度と回転による速度の合成速度で、次のようになる。
衝突前:
衝突後:
:剛体の重心から衝突点へのベクトル(重心-衝突点ベクトル)
貫通なし拘束は、速度を用いて表される。
( は衝突面の法線方向の単位ベクトル )
一般化速度 ベクトル(T:転置)を用いると、拘束の式は次のようになる。
はヤコビアンで、
拘束の式に、反発の効果を bias として取り入れる。
反発係数(restitution)を とすると
となるので、拘束の式を次のようにする。
,
次に拘束力を取り入れる。
拘束力 は、運動量の変化で表される。
は質量マトリックスで、(:単位マトリックス、
:慣性モーメントマトリックス)である。 は力の作用時間である。
拘束力の方向は となるので、大きさを (未定乗数)として、
を の中に含めると、次のようになる。
新たな はインパルスに相当する。
拘束の式は、次のようになる。
この式から を求める。
具体的には、以下の式を繰返し法を用いて解いていく。
,
このとき、繰返し毎にインパルス()を加算し、それがゼロとなるようにクランプ(clamp)する。
これで、 (拘束力が作用しない) 、 (剛体が壁から離れた状態)となり、拘束が解けたことになる。
実際のシミュレーションでは、安定性のため、次のような項を取り入れる。
1つ目は、貫通に関する安定項である。
:貫通の長さ、:貫通に対するslop(遊び)で適当に設定、
:係数で適当に設定
2つ目は、反発項に関する修正である。
:反発に対するslopで適当に設定
bias項 を にする。
摩擦がある場合には、摩擦による拘束を追加する。
この場合の拘束は、次のように表される。
( は衝突面の接線方向の単位ベクトル )
一般化速度 を用いると、拘束の式は次のようになる。
,
次に拘束力を取り入れる。摩擦力は、
:衝突面での垂直抗力
:摩擦係数
である。インパルスで表すと、次のようになる。
は上記の法線方向のインパルスである。加算したインパルスを用いる。
拘束の式は、インパルスを取り入れると次のようになる。
この式とインパルスの式から を求める。
具体的には、以下の式を、上述した法線方向の拘束の式の後に追加して、
繰返し法を用いて解いていく。
このとき、繰返し毎にインパルス()を加算し、それが と の間の値に
なるようにクランプ(clamp)する。これで拘束が解けたことになる。
繰返しの最後に得られた速度 が、拘束によって更新された速度となる。
3 剛体同士の衝突
(WorldConstraint.js class World, projectCollision メソッド)
剛体同士の衝突の様子
衝突判定には、ForceBased と同様な方法、sap法(sap() 関数)と gjk-epa 法(gjk-epa.js)を用いている。
gjk-epa から、貫通の深さ、衝突の方向(衝突面の法線方向)、衝突点の座標等の情報が得られる。
衝突前後の剛体A、Bの速度と角速度を、それぞれ 、 、 、 、 、 、 、 とすると、
剛体A、Bの衝突点の速度は、次のようになる。
剛体A
衝突前:
衝突後:
剛体B
衝突前:
衝突後:
、:剛体A、Bの重心から衝突点へのベクトル
貫通なし拘束は、剛体の相対速度を用いて表される。
( は衝突面の法線方向の単位ベクトル )
一般化速度 ベクトルを用いると、拘束の式は次のようになる。
拘束力をインパルス( )として取り入れると、拘束の式は次のようになる。
:質量 、、慣性モーメント 、の質量マトリックス
:bias
摩擦がある場合の拘束の式は、次のようになる。
( は衝突面の接線方向の単位ベクトル )
( :摩擦係数)
拘束は、壁との衝突の場合と同様に、繰返し法で解いていく。
プログラム
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; });