物理シミュレーション 剛体の衝突 2 ( Force Based )
Force Based による、剛体の衝突シミュレーションです。
[ 実行結果 ]
２Dシミュレーション
メッシュの描画には、 BabylonJS を使用しています。
シミュレーションの方法
壁との衝突、剛体同士の衝突での力、トルクの計算は、以下を参照しています。
「WebGLによる 物理シミュレーション」( 酒井幸市、工学社 ）
第４章 剛体の衝突
１ 時間積分
剛体に作用する力
・重力 force_g
・壁との衝突時
壁から受ける力 force_wall
その力によるトルク（モーメント）torque_wall
・剛体同士の衝突時
他の剛体から受ける力 force_collision
その力によるトルク（モーメント）torque_collision
これらの力の総和(force)とトルクの総和(torque)から、加速度(acc)、
角加速度(angAcc)、速度(vel)、角速度(angVel)、そして位置(pos)、角度(ang)と
数値積分していきます。(html simulate() 関数）
２Dを考えているので、トルクはz成分のみとなる。
２ 壁との衝突
（BodyForce.js class Body, projectWalls メソッド）
衝突点に作用する力とそれによって生じるトルクを計算する。
まず、衝突点(図の赤丸)を調べる。四角や三角など多角形の剛体では、衝突の
仕方は２通り起こる。edge または vertex での衝突である。
これを判定するため、多角形の各頂点と壁との距離を計算し、距離がゼロ以下
となる頂点数を数える。１ならvertex、2 なら edge で衝突している。これで
衝突点を知ることができる。
衝突のような撃力が働く場合は、力積から力を求める。力積は、力とその力が
作用する時間との積である。また、力積は、力が働く前後の運動量の変化として
表すこともできる。
(１) 摩擦を考慮しない場合
この場合は、剛体は壁の法線方向の力を受ける。
力積を 、力を
、作用する時間を
、剛体の質量を
、衝突前後の速度を
、
として、
衝突点での力 によって生じるトルクは、
,
は剛体の重心から衝突点へのベクトル（重心-衝突点ベクトル）
(2Dを考えているので、トルクはz成分のみであるが、ベクトルで表している。)
角力積 は、角運動量の変化に等しいので、慣性モーメントを
、衝突前後の
角速度を 、
として、
2Dを考えているので、慣性モーメントはスカラー量である。
剛体が四角の場合、 (
四角の
方向と
方向の辺の
長さ)。（プログラムでは、多角形の慣性モーメントを用いている。）
剛体の衝突点の速度は、並進の速度と回転による速度の合成速度で、
衝突前：
衝突後：
力積と角力積の式、反発係数(restitution) の式
(
は衝突面の法線方向の単位ベクトル)
から、
力積 は法線方向の成分のみなので、以下のようになる。
これから、力とトルクが以下のように求まる。
(２) 摩擦を考慮した場合
この場合は、壁の接線方向の力(摩擦力)も働く。
衝突面の接線方向の単位ベクトルは、
動摩擦係数を 、法線方向の力を
((１)で求めた力)として、
摩擦力 は、
摩擦力によるトルク は、
摩擦力による力積は、
摩擦力による角力積は、
である。
衝突点の、衝突後の合成速度の接線成分 は、
合成速度 は、以下のとき、ゼロとなる。
この時、剛体は、滑らずに転がるようになる。
衝突している時間 内に
がゼロとなる動摩擦係数（臨界動摩擦係数）は、
・ の場合
となるので、衝突している時間
内に
がゼロとなる。
力積と角力積は、
とする。
・ の場合
なので、
とする。
これから、求める力とトルクは以下のようになる。
の場合
の場合
３ 剛体同士の衝突
（BodyForce.js class Body, projectCollision メソッド）
剛体同士の衝突でも、着目剛体が他の剛体から受ける力とトルクは、壁との衝突の場合と同様に計算できる。
衝突判定には、sweep-and-prune(sap) 法（html sap() 関数）と gjk-epa 法 (前回記事、gjk-epa.js)を用いている。
衝突は、edge 同士 または edge と vertex で起こる。sapで衝突ペアを検出し、gjk-epa から、edge で衝突しているのか、vertex で衝突しているのか、penetration の深さ、衝突の方向、衝突点の座標等の情報が得られる。
各衝突ペアについて、それぞれの剛体に作用する力積と角力積を計算し、それから力とトルクを求めます。
(１) 摩擦を考慮しない場合
剛体１の質量を 、衝突前後の速度を
、
、剛体２の質量を
、衝突前後の速度を
、
とする。また、剛体１に働く力を
、作用する時間を
とすると、
剛体１に作用する力積：
剛体２に作用する力積：
剛体１の慣性モーメントを 、衝突前後の角速度を
、
、また剛体２の慣性モーメントを
、衝突前後の角速度を
、
として、
剛体１に作用する角力積：
剛体２に作用する角力積：
、
、
、
は、それぞれ剛体１、２に作用するトルクと重心-衝突点ベクトルである。
衝突点での合成速度は、以下のようになる。
剛体１ 衝突前：
衝突後：
剛体２ 衝突前：
衝突後：
力積、角力積、反発係数(restitution) の式
(
は衝突面の法線方向の単位ベクトル)
から、剛体１に作用する力 とトルク
は、
剛体２に作用する力 とトルク
は、
(２) 摩擦を考慮した場合
壁との衝突の場合と同様に、臨界動摩擦係数を以下のように定義する。
は衝突面の接線方向の単位ベクトルである。
摩擦力による、剛体１、２の力とトルクは、以下のようになる。
の場合
剛体１
剛体２
の場合
剛体１
剛体２
プログラム
collision-force.html
<!DOCTYPE html>
<meta charset="utf-8">
<title>BJS Physical Animation ( force based )</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(['./BodyForce', './math'], function(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), 1, 4),
new math.Vector2(-8, 10), // world position(pos)
Math.PI / 4, // angle
1 // mass
)
);
bodies[0].vel = new math.Vector2(2, 0);
bodies[0].angVel = 0.0;
// body [1]
bodies.push(
new Body(
createPolygonPoints(new math.Vector2(0, 0), 1, 3),
new math.Vector2(8, 10),
//Math.PI / 4, // for nsides = 4
0, // for nsides = 3
1
)
);
bodies[1].vel = new math.Vector2(0, 0);
bodies[1].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];
console.log("object index", j, object.worldShape);
const abodyMat = new BABYLON.StandardMaterial("abodyMat" + j, scene);
abodyMat.diffuseColor = colors[j];
var points = [];
for (i = 0; i < object.worldShape.length; ++i) {
const apoint = BABYLON.MeshBuilder.CreateSphere("apoint" + i,
{diameter: 0.3, segments: 8}, scene);
apoint.position.x = object.worldShape[i].x;
apoint.position.y = object.worldShape[i].y;
apoint.position.z = 0;
apoint.material = abodyMat;
points.push(apoint);
}
var path = [];
for (i = 0; i < object.worldShape.length; i++)
path.push(
new BABYLON.Vector3(object.worldShape[i].x, object.worldShape[i].y, 0)
);
path.push(new BABYLON.Vector3(object.worldShape[0].x, object.worldShape[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);
}
}
scene.registerBeforeRender(function () {
simulate();
update_mesh();
});
function simulate() {
var g = 9.8;
var dt = 0.02;
//// Integrate
bodies.forEach(body => {
body.force = body.force0.addV(new math.Vector2(0, -body.mass * g));
body.torque = 0.0;
body.projectWalls(walls);
});
let pairs = sap(bodies.map(body => body.worldShape), new math.Vector2(1, 0));
bodies.forEach((body, i) => {
pairs[i].forEach(j => body.projectCollision(bodies[j]));
});
bodies.forEach(body => {
var acc = body.force.divS(body.mass);
body.vel = body.vel.addV(acc.mulS(dt));
body.pos = body.pos.addV(body.vel.mulS(dt));
var angAcc = body.invInertia * body.torque;
body.angVel += angAcc * dt;
body.ang += body.angVel * dt;
});
}
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 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 Wall(position, normal) {
//// position, normal: Vector2
this.position = position;
this.normal = normal;
this.dist = function(position) {
return this.normal.dot(position.subV(this.position));
};
}
});
engine.runRenderLoop(() => {
scene.render();
});
}
init();
</script>
BodyForce.js
define(['./math', './gjk_epa'], function(math, Gjk) {
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);
}
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) {
//// p, q, r: Vector2
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 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;
const restitution = 0.5;
const restitution2 = 0.4;
class Body {
constructor(shape, pos = new Vector2(0, 0), ang = 0, mass = 1) {
console.log("ceate Bodies", shape);
this.mass = 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;
this.force = new math.Vector2(0, 0);
this.force0 = new math.Vector2(0, 0);
this.torque = 0; // z成分のみ
this.vGravityToPoint = new math.Vector2(0, 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
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;
}
//// projectWalls
projectWalls(walls) {
//// walls: Array, element: wall info
//// 注 wall.normal の方向
let muK = 0.1;
var worldVertices = this.worldShape.slice();
for (let wall of walls) {
var cnt = 0;
var vPointCollision = new math.Vector2(0, 0);
var 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) {
vPointCollision = vPointCollision.addV(worldVertices[i]);
cnt++;
}
}
var flagCollisionF = false;
if (cnt != 0) {
vPointCollision = vPointCollision.divS(cnt);
this.vGravityToPoint = vPointCollision.subV(this.pos);
if (dist_min <= 0 && this.vel.y < -0.3)
{
var c = 0.4;
var vRot = new math.Vector2(
-this.angVel * this.vGravityToPoint.y,
this.angVel * this.vGravityToPoint.x);
var vp = this.vel.addV(vRot);
var vpxn = vp.cross(wall.normal);
var vTangent = new math.Vector2(wall.normal.y * vpxn, -wall.normal.x * vpxn);
vTangent.normalize();
var rxn = this.vGravityToPoint.cross(wall.normal)
var a2 = this.invInertia * rxn;
var a3 = new math.Vector2(-a2 * this.vGravityToPoint.y,
a2 * this.vGravityToPoint.x);
var rikiseki = - (restitution + 1.0) * wall.normal.dot(vp) / (1.0/this.mass + wall.normal.dot(a3));
this.force = this.force.addV(wall.normal.mulS(rikiseki / dt));
var torq2 = rxn * c * rikiseki / dt;
this.torque += torq2;
////摩擦を考慮
var rxt = this.invInertia * this.vGravityToPoint.cross(vTangent);
var rxt2 = this.vGravityToPoint.cross(vTangent);
var c2 = vTangent.dot(
new math.Vector2(-rxt * this.vGravityToPoint.y,
rxt * this.vGravityToPoint.x));
var B = -vTangent.dot(vp) / (1.0/this.mass + c2);
var muC = Math.abs(B / rikiseki);
if(muK >= muC){
this.force = this.force.addV(vTangent.mulS(B/dt));
this.torque += rxt2 * c * B/dt;
}
else
{
this.force = this.force.addV(vTangent.mulS(-muK * rikiseki / dt));
this.torque += -rxt2 * c*muK * rikiseki / dt;
}
flagCollisionF = true;
}
if (!flagCollisionF) {
////重力と抗力によるトルク
var torq = this.vGravityToPoint.cross(new math.Vector2(0.0, this.mass * g)) * 0.5;
this.torque += torq;
}
}
if (dist_min < 0.0) this.pos.y -= dist_min;
}
}
//// projectCollision
projectCollision(other, gravityBias = 0) {
let a = this.worldShape.slice();
let b = other.worldShape.slice();
let hit = Gjk.gjk(a, b);
//---- 重心-衝突点ベクトル vGravityToPoint を求める
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] = [this.mass, other.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))
);
}
}
this.vGravityToPoint = aContactDisplaced.subV(this.pos);
other.vGravityToPoint = bContactDisplaced.subV(other.pos);
//---- 力積の計算
let muK = 0.5;
var rikiseki;
var vn1, vn2;
var muc,B, m1, m2, mI1, mI2;
var vVelocityP1, vVelocityP2;
var vTangential; //接線方向
var vA1 = new math.Vector2(0.0, 0.0);
var vA2 = new math.Vector2(0.0, 0.0);
var vRg1 = new math.Vector2(0.0, 0.0);
var vRg2 = new math.Vector2(0.0, 0.0);
this.force = new math.Vector2(0.0, 0.0);
this.torque = 0.0;
other.force = new math.Vector2(0.0, 0.0);
other.torque = 0.0;
vRg1 = this.vGravityToPoint.copy();
vRg2 = other.vGravityToPoint.copy();
m1 = this.mass;
m2 = other.mass;
mI1 = this.invInertia;
mI2 = other.invInertia;
var vRot1 = new math.Vector2(
-this.angVel * vRg1.y, this.angVel * vRg1.x);
vVelocityP1 = this.vel.addV(vRot1);
var vNormal = n;
vn1 = vVelocityP1.dot(vNormal);
var vRot2 = new math.Vector2(
-other.angVel * vRg2.y, other.angVel * vRg2.x);
vVelocityP2 = other.vel.addV(vRot2);
vn2 = vVelocityP2.dot(vNormal);
////衝突応答
var vp12 = vVelocityP1.subV(vVelocityP2);
var vp12xn = vp12.cross(vNormal);
vTangential = new math.Vector2(vNormal.y * vp12xn, -vNormal.x * vp12xn);
vTangential.normalize();
////力積
var a11 = vRg1.cross(vNormal);
var a12 = mI1 * a11;
var a13 = new math.Vector2(-a12 * vRg1.y, a12 * vRg1.x);
var a21 = vRg2.cross(vNormal);
var a22 = mI2 * a21;
var a23 = new math.Vector2(-a22 * vRg2.y, a22 * vRg2.x);
rikiseki = - (restitution2 + 1.0) * (vn1 - vn2) / (1.0/m1 + 1.0/m2
+ vNormal.dot(a13) + vNormal.dot(a23));
////力の総和
this.force = this.force.addV(vNormal.mulS(rikiseki / dt));
other.force = other.force.subV(vNormal.mulS(rikiseki / dt));
////トルクの総和
this.torque += a11 * rikiseki / dt;
other.torque -= a21 * rikiseki / dt;
////摩擦を考慮
var rxt1 = vRg1.cross(vTangential);
var rxt2 = vRg2.cross(vTangential);
vA1 = new math.Vector2(-mI1 * rxt1 * vRg1.y, mI1 * rxt1 * vRg1.x);
vA2 = new math.Vector2(-mI2 * rxt2 * vRg2.y, mI2 * rxt2 * vRg2.x);
B = -vTangential.dot(vp12) / (1.0/m1+1.0/m2+ vTangential.dot(vA1.addV(vA2)));
muc = Math.abs(B / rikiseki);
if(muK >= muc)
{
this.force = this.force.addV(vTangential.mulS(B/dt));
this.torque += rxt1 * B/dt;
other.force = other.force.subV(vTangential.mulS(B/dt));
other.torque -= rxt2 * B/dt;
} else
{
this.force = this.force.addV(vTangential.mulS(muK * rikiseki / dt));
this.torque += rxt1 * muK * rikiseki / dt;
other.force = other.force.subV(vTangential.mulS(muK * rikiseki / dt));
other.torque -= rxt2 * muK * rikiseki / dt;
}
////衝突時にめり込んだ分引き離す
////0.01は分離を確実にするため
this.pos = this.pos.subV(vNormal.mulS(penetration/4.0+0.01));
other.pos = other.pos.addV(vNormal.mulS(penetration/4.0+0.01));
}
}
}
return Body;
});
