物理シミュレーション 剛体の衝突 １
物理シミュレーションの勉強のため、剛体の衝突についてまとめてみました。
剛体衝突の物理シミュレーションは、主に以下の３つの方法で行われています。
１ Force Based
２ Position Based(Constraint)
３ Position Based(ShapeMatching)
衝突のシミュレーションを行うには、衝突検出処理と衝突応答処理が必要に
なりますが、これらは後者に相当します。
１番目の Force Based は、従来からのニュートンの運動方程式を数値積分して
いく方法です。衝突の際の、反発力や摩擦力を考慮して計算します。
２番目の Position Based(Constraint) は 2006年に考案された方法です。（下の
参考文献 Position Based Dynamics) 剛体の衝突を拘束(Constraint)と見なし、
その拘束条件を満たすように拘束方程式を解いて、衝突後の剛体の位置(Position)
を知る方法です。衝突の場合は、速度についての拘束条件を用います。
３番目の Position Based(ShapeMatching) は 2005年に出てきた方法です。
（下の参考文献 Meshless Deformations Based on Shape Matching、２番目と
同じグループが考案) これは、剛体を頂点座標で表し、衝突した際の頂点の
移動点を、剛体のもとの形状を保つように頂点を移動させる方法です。
これら３つの方法を理解するために、数学ライブラリーと衝突検出処理には
同じアルゴリズムを使い、Javascript で実装してみました。
物理シミュレーションの理解に役立つ参考文献、サイトです。
１ 酒井幸市著「WebGLによる 物理シミュレーション」( 工学社 )
２ 藤澤 誠著「CGのための物理シミュレーションの基礎」( マイナビ )
３ 特定非営利活動法人natural science
http://www.natural-science.or.jp/
コンピュータ・シミュレーション講座、仮想物理実験室ページの下部にある
記事リストに様々な現象の実装が有ります。
４ 物理学の見つけ方
https://physics-htfi.github.io/
など。
衝突応答処理に関する参考文献、サイトです。
１ Detailed Rigid Body Simulation with Extended 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
４ A Survey on Position Based Dynamics, 2017
https://matthias-research.github.io/pages/publications/publications.html
５ Position Based Dynamics
https://matthias-research.github.io/pages/publications/publications.html
６ Meshless Deformations Based on Shape Matching
https://matthias-research.github.io/pages/publications/publications.html
など。
実装用に参照した文献、サイトです。
ForceBased
酒井幸市著「WebGLによる 物理シミュレーション」( 工学社 )
第４章 剛体の衝突 (参考コードは書籍のページからダウンロード可)
PositionBased(Constraint)
2D Rigid Body Physics Engine
https://github.com/Sopiro/Physics
(実際に参照したのは、古いバージョン: 2021/11/06 ダウンロード
このバージョンの game.js )
PositionBased(ShapeMatching)
Physical animation with shape matching
Claudio Esperança
https://observablehq.com/@esperanc/physical-animation-with-shape-matching
右上にある […] アイコンの Export からコードをダウンロードできる。
69eb2ec167e70941@1046.js がメインのプログラムである。
数学ライブラリーと衝突検出処理には、ShapeMatching で参照したプログラムを使用しています。衝突検出の方法として、主にSAT(Separating Axis Theorem) と GJK(Gilbert-Johnson-Keerthi)法がありますが、ここでは、GJK法を使用します。また、衝突時のめり込みの長さや方向を知るため、EPA(Expanding Polytope Algorithm)法を使用します。
数学ライブラリー（math.js)
define( function() {
function clamp(value, min, max) {
if (value < min)
return min;
else if (value > max)
return max;
else
return value;
}
class Vector2 {
constructor(x = 0, y = 0) {
this.x = x;
this.y = y;
}
clear() {
this.x = 0;
this.y = 0;
}
copy() {
return new Vector2(this.x, this.y);
}
toFixed() {
this.x = Math.round(this.x * 1e9) / 1e9;
this.y = Math.round(this.y * 1e9) / 1e9;
}
fixed() {
return new Vector2(Math.round(this.x * 1e9) / 1e9, Math.round(this.y * 1e9) / 1e9);
}
invert() {
this.x *= -1;
this.y *= -1;
}
inverted() {
return new Vector2(this.x * -1, this.y * -1);
}
normalize() {
const len = this.getLength();
if (len != 0) {
this.x /= len;
this.y /= len;
}
}
normalized() {
const len = this.getLength();
if (len != 0)
return this.divS(len);
else
return this;
}
getLength() {
return Math.sqrt(this.x * this.x + this.y * this.y);
}
dot(v) {
return this.x * v.x + this.y * v.y;
}
cross(v) {
return this.x * v.y - this.y * v.x;
}
addV(v) {
return new Vector2(this.x + v.x, this.y + v.y);
}
subV(v) {
return new Vector2(this.x - v.x, this.y - v.y);
}
divS(v) {
return new Vector2(this.x / v, this.y / v);
}
mulS(v) {
return new Vector2(this.x * v, this.y * v);
}
equals(v) {
return this.x == v.x && this.y == v.y;
}
unNaN() {
if (isNaN(this.x) || isNaN(this.y)) {
this.x = 0;
this.y = 0;
}
}
}
class Vector3 {
constructor(x = 0, y = 0, z = 0) {
this.x = x;
this.y = y;
this.z = z;
}
clear() {
this.x = 0;
this.y = 0;
this.z = 0;
}
copy() {
return new Vector3(this.x, this.y, this.z);
}
toFixed() {
this.x = Math.round(this.x * 1e9) / 1e9;
this.y = Math.round(this.y * 1e9) / 1e9;
this.z = Math.round(this.z * 1e9) / 1e9;
}
fixed() {
return new Vector3(Math.round(this.x * 1e9) / 1e9, Math.round(this.y * 1e9) / 1e9, Math.round(this.z * 1e9) / 1e9);
}
normalize() {
const len = this.getLength();
this.x /= len;
this.y /= len;
this.z /= len;
}
normalized() {
const len = this.getLength();
if (len != 0)
return this.divS(len);
else
return this;
}
invert() {
this.x *= -1;
this.y *= -1;
this.z *= -1;
}
inverted() {
return new Vector3(this.x * -1, this.y * -1, this.z * -1);
}
getLength() {
return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
}
dot(v) {
return this.x * v.x + this.y * v.y + this.z * v.z;
}
cross(v) {
return new Vector3(this.y * v.z - this.z * v.y, this.z * v.x - this.x * v.z, this.x * v.y - this.y * v.x);
}
add(v) {
return new Vector3(this.x + v.x, this.y + v.y, this.z + v.z);
}
sub(v) {
return new Vector3(this.x - v.x, this.y - v.y, this.z - v.z);
}
divS(v) {
return new Vector3(this.x / v, this.y / v, this.z / v);
}
divXYZ(x, y, z) {
return new Vector3(this.x / x, this.y / y, this.z / z);
}
mulS(v) {
return new Vector3(this.x * v, this.y * v, this.z * v);
}
mulXYZ(x, y, z) {
return new Vector3(this.x * x, this.y * y, this.z * z);
}
equals(v) {
return this.x == v.x && this.y == v.y && this.z == v.z;
}
unNaN() {
if (isNaN(this.x) || isNaN(this.y) || isNaN(this.z)) {
this.x = 0;
this.y = 0;
this.z = 0;
}
}
}
class Vector4 {
constructor(x = 0, y = 0, z = 0, w = 0) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
clear() {
this.x = 0;
this.y = 0;
this.z = 0;
this.w = 0;
}
copy() {
return new Vector4(this.x, this.y, this.z, this.w);
}
toFixed() {
this.x = Math.round(this.x * 1e9) / 1e9;
this.y = Math.round(this.y * 1e9) / 1e9;
this.z = Math.round(this.z * 1e9) / 1e9;
this.w = Math.round(this.w * 1e9) / 1e9;
}
fixed() {
return new Vector4(Math.round(this.x * 1e9) / 1e9, Math.round(this.y * 1e9) / 1e9, Math.round(this.z * 1e9) / 1e9, Math.round(this.w * 1e9) / 1e9);
}
normalize() {
const len = this.getLength();
this.x /= len;
this.y /= len;
this.z /= len;
this.w /= len;
}
normalized() {
const len = this.getLength();
if (len != 0)
return this.divS(len);
else
return this;
}
invert() {
this.x *= -1;
this.y *= -1;
this.z *= -1;
this.w *= -1;
}
inverted() {
return new Vector4(this.x * -1, this.y * -1, this.z * -1, this.w * -1);
}
getLength() {
return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
}
dot(v) {
return this.x * v.x + this.y * v.y + this.z * v.z + this.w * v.w;
}
add(v) {
return new Vector4(this.x + v.x, this.y + v.y, this.z + v.z, this.w + v.w);
}
sub(v) {
return new Vector4(this.x - v.x, this.y - v.y, this.z - v.z, this.w - v.w);
}
divS(v) {
return new Vector4(this.x / v, this.y / v, this.z / v, this.w / v);
}
divXYZW(x, y, z, w) {
return new Vector4(this.x / x, this.y / y, this.z / z, this.w / w);
}
mulS(v) {
return new Vector4(this.x * v, this.y * v, this.z * v, this.w * v);
}
mulXYZW(x, y, z, w) {
return new Vector4(this.x * x, this.y * y, this.z * z, this.w * w);
}
equals(v) {
return this.x == v.x && this.y == v.y && this.z == v.z && this.w == v.w;
}
unNaN() {
if (isNaN(this.x) || isNaN(this.y) || isNaN(this.z) || isNaN(this.w)) {
this.x = 0;
this.y = 0;
this.z = 0;
this.w = 0;
}
}
}
class Matrix3 {
constructor() {
this.m00 = 1;
this.m01 = 0;
this.m02 = 0;
this.m10 = 0;
this.m11 = 1;
this.m12 = 0;
this.m20 = 0;
this.m21 = 0;
this.m22 = 1;
}
loadIdentity() {
this.m00 = 1;
this.m01 = 0;
this.m02 = 0;
this.m10 = 0;
this.m11 = 1;
this.m12 = 0;
this.m20 = 0;
this.m21 = 0;
this.m22 = 1;
}
copy() {
let res = new Matrix3();
res.m00 = this.m00;
res.m01 = this.m01;
res.m02 = this.m02;
res.m10 = this.m10;
res.m11 = this.m11;
res.m12 = this.m12;
res.m20 = this.m20;
res.m21 = this.m21;
res.m22 = this.m22;
return res;
}
fromValues(a, b, c, d, e, f, g, h, i) {
let res = new Matrix3();
res.m00 = a;
res.m01 = b;
res.m02 = c;
res.m10 = d;
res.m11 = e;
res.m12 = f;
res.m20 = g;
res.m21 = h;
res.m22 = i;
return res;
}
mulMatrix(right) {
let res = new Matrix3();
res.m00 = this.m00 * right.m00 + this.m01 * right.m10 + this.m02 * right.m20;
res.m01 = this.m00 * right.m01 + this.m01 * right.m11 + this.m02 * right.m21;
res.m02 = this.m00 * right.m02 + this.m01 * right.m12 + this.m02 * right.m22;
res.m10 = this.m10 * right.m00 + this.m11 * right.m10 + this.m12 * right.m20;
res.m11 = this.m10 * right.m01 + this.m11 * right.m11 + this.m12 * right.m21;
res.m12 = this.m10 * right.m02 + this.m11 * right.m12 + this.m12 * right.m22;
res.m20 = this.m20 * right.m00 + this.m21 * right.m10 + this.m22 * right.m20;
res.m21 = this.m20 * right.m01 + this.m21 * right.m11 + this.m22 * right.m21;
res.m22 = this.m20 * right.m02 + this.m21 * right.m12 + this.m22 * right.m22;
return res;
}
mulVector(right, z) {
let res = new Vector2(0, 0);
if (z == undefined)
z = 1;
res.x = this.m00 * right.x + this.m01 * right.y + this.m02 * z;
res.y = this.m10 * right.x + this.m11 * right.y + this.m12 * z;
return res;
}
mulVectors(right, z) {
let res = [];
for (let i = 0; i < right.length; i++)
res.push(this.mulVector(right[i], 1));
return res;
}
scale(x, y) {
if (y == undefined)
y = x;
let scale = new Matrix3();
scale.m00 = x;
scale.m11 = y;
return this.mulMatrix(scale);
}
rotate(r) {
const sin = Math.sin(r);
const cos = Math.cos(r);
let res = new Matrix3();
res.m00 = cos;
res.m01 = -sin;
res.m10 = sin;
res.m11 = cos;
return this.mulMatrix(res);
}
translate(x, y) {
let res = new Matrix3();
res.m02 = x;
res.m12 = y;
return this.mulMatrix(res);
}
}
class Matrix4 {
constructor() {
this.m00 = 1;
this.m01 = 0;
this.m02 = 0;
this.m03 = 0;
this.m10 = 0;
this.m11 = 1;
this.m12 = 0;
this.m13 = 0;
this.m20 = 0;
this.m21 = 0;
this.m22 = 1;
this.m23 = 0;
this.m30 = 0;
this.m31 = 0;
this.m32 = 0;
this.m33 = 1;
}
loadIdentity() {
this.m00 = 1;
this.m01 = 0;
this.m02 = 0;
this.m03 = 0;
this.m10 = 0;
this.m11 = 1;
this.m12 = 0;
this.m13 = 0;
this.m20 = 0;
this.m21 = 0;
this.m22 = 1;
this.m23 = 0;
this.m30 = 0;
this.m31 = 0;
this.m32 = 0;
this.m33 = 1;
}
copy() {
let res = new Matrix4();
res.m00 = this.m00;
res.m01 = this.m01;
res.m02 = this.m02;
res.m03 = this.m03;
res.m10 = this.m10;
res.m11 = this.m11;
res.m12 = this.m12;
res.m13 = this.m13;
res.m20 = this.m20;
res.m21 = this.m21;
res.m22 = this.m22;
res.m23 = this.m23;
res.m20 = this.m30;
res.m31 = this.m31;
res.m32 = this.m32;
res.m33 = this.m33;
return res;
}
fromAxis(vx, vy, vz) {
let res = new Matrix4();
res.m00 = vx.x;
res.m01 = vy.x;
res.m02 = vz.x;
res.m10 = vx.y;
res.m11 = vy.y;
res.m12 = vz.y;
res.m20 = vx.z;
res.m21 = vy.z;
res.m22 = vz.z;
return res;
}
mulMatrix(right) {
let res = new Matrix4();
res.m00 = this.m00 * right.m00 + this.m01 * right.m10 + this.m02 * right.m20 + this.m03 * right.m30;
res.m01 = this.m00 * right.m01 + this.m01 * right.m11 + this.m02 * right.m21 + this.m03 * right.m31;
res.m02 = this.m00 * right.m02 + this.m01 * right.m12 + this.m02 * right.m22 + this.m03 * right.m32;
res.m03 = this.m00 * right.m03 + this.m01 * right.m13 + this.m02 * right.m23 + this.m03 * right.m33;
res.m10 = this.m10 * right.m00 + this.m11 * right.m10 + this.m12 * right.m20 + this.m13 * right.m30;
res.m11 = this.m10 * right.m01 + this.m11 * right.m11 + this.m12 * right.m21 + this.m13 * right.m31;
res.m12 = this.m10 * right.m02 + this.m11 * right.m12 + this.m12 * right.m22 + this.m13 * right.m32;
res.m13 = this.m10 * right.m03 + this.m11 * right.m13 + this.m12 * right.m23 + this.m13 * right.m33;
res.m20 = this.m20 * right.m00 + this.m21 * right.m10 + this.m22 * right.m20 + this.m23 * right.m30;
res.m21 = this.m20 * right.m01 + this.m21 * right.m11 + this.m22 * right.m21 + this.m23 * right.m31;
res.m22 = this.m20 * right.m02 + this.m21 * right.m12 + this.m22 * right.m22 + this.m23 * right.m32;
res.m23 = this.m20 * right.m03 + this.m21 * right.m13 + this.m22 * right.m23 + this.m23 * right.m33;
res.m30 = this.m30 * right.m00 + this.m31 * right.m10 + this.m32 * right.m20 + this.m33 * right.m30;
res.m31 = this.m30 * right.m01 + this.m31 * right.m11 + this.m32 * right.m21 + this.m33 * right.m31;
res.m32 = this.m30 * right.m02 + this.m31 * right.m12 + this.m32 * right.m22 + this.m33 * right.m32;
res.m33 = this.m30 * right.m03 + this.m31 * right.m13 + this.m32 * right.m23 + this.m33 * right.m33;
return res;
}
mulVector(right, w) {
let res = new Vector3(0, 0, 0);
if (w == undefined)
w = 1;
res.x = this.m00 * right.x + this.m01 * right.y + this.m02 * right.z + this.m03 * w;
res.y = this.m10 * right.x + this.m11 * right.y + this.m12 * right.z + this.m13 * w;
res.z = this.m20 * right.x + this.m21 * right.y + this.m22 * right.z + this.m23 * w;
return res;
}
mulVectors(right, z) {
let res = [];
for (let i = 0; i < right.length; i++)
res.push(this.mulVector(right[i], 1));
return res;
}
scale(x, y, z) {
if (y == undefined && z == undefined) {
y = x;
z = x;
}
let scale = new Matrix4();
scale.m00 = x;
scale.m11 = y;
scale.m22 = z;
return this.mulMatrix(scale);
}
rotate(x, y, z) {
const sinX = Math.sin(x);
const cosX = Math.cos(x);
const sinY = Math.sin(y);
const cosY = Math.cos(y);
const sinZ = Math.sin(z);
const cosZ = Math.cos(z);
let res = new Matrix4();
res.m00 = cosY * cosZ;
res.m01 = -cosY * sinZ;
res.m02 = sinY;
res.m03 = 0;
res.m10 = sinX * sinY * cosZ + cosX * sinZ;
res.m11 = -sinX * sinY * sinZ + cosX * cosZ;
res.m12 = -sinX * cosY;
res.m13 = 0;
res.m20 = -cosX * sinY * cosZ + sinX * sinZ;
res.m21 = cosX * sinY * sinZ + sinX * cosZ;
res.m22 = cosX * cosY;
res.m23 = 0;
res.m30 = 0;
res.m31 = 0;
res.m32 = 0;
res.m33 = 1;
return this.mulMatrix(res);
}
translate(x, y, z) {
let res = new Matrix4();
res.m03 = x;
res.m13 = y;
res.m23 = z;
return this.mulMatrix(res);
}
}
return {Vector2: Vector2, Vector3: Vector3, Vector4: Vector4,
Matrix3: Matrix3, Matrix4: Matrix4, clamp: clamp};
});衝突検出処理（gjk_epa.js)
define(['./math'], function(math) {
function support(pts, v) {
//// pts: Array, element Vector2
//// v: Vector2
//// return: Vector2
let maxdot = Number.NEGATIVE_INFINITY;
let best;
for (let p of pts) {
let dot = p.dot(v);
if (dot > maxdot) [maxdot, best] = [dot, p];
}
return best;
}
function support2(pts1, pts2, v) {
//// pts1, pts2: Array, element Vector2
//// v: Vector2
//// return: Vector2
try {
return support(pts1, v).subV(support(pts2, v.mulS(-1)));
} catch (err) {
throw "BUG:" + err;
}
}
function tripleProduct(a, b, c) {
//// a, b, c: Vector2
//// return: Vector2
let z = a.x * b.y - a.y * b.x;
return new math.Vector2(-c.y * z, c.x * z);
}
function closestEdge(polytope) {
//// polytope: [a, b, c] a,b,c Vector2
let npts = polytope.length;
let dmin = Number.POSITIVE_INFINITY;
let closest;
for (let i = 0; i < npts; i++) {
let [p, q] = [polytope[i], polytope[(i + 1) % npts]];
//// p, q: Vector2
let e = q.subV(p);
let epPerp = tripleProduct(e, p, e);
let n = epPerp.normalized();
let dist = n.dot(p);
if (dist < dmin) [dmin, closest] = [dist, { dist, i, p, q, n }];
}
return closest;
}
return {
gjk: function(pts1, pts2) {
//// pts1, pts2: Array, element Vector2
//// return: false or [a, b, c] a,b,c Vector2
// First point of pts1-pts2
let a = support2(pts1, pts2, new math.Vector2(1, 1));
// First direction
let v = a.mulS(-1);
// Second point
let b = support2(pts1, pts2, v);
if (b.dot(v) <= 0) return false; // Second point fails
// Second direction
let ab = b.subV(a);
v = tripleProduct(ab, a.mulS(-1), ab);
for (;;) {
// Third point
let c = support2(pts1, pts2, v);
if (c.dot(v) <= 0) return false; // Third point fails
let c0 = c.mulS(-1);
let cb = b.subV(c);
let ca = a.subV(c);
let cbPerp = tripleProduct(ca, cb, cb);
let caPerp = tripleProduct(cb, ca, ca);
if (caPerp.dot(c0) > 0) {
b = c;
v = caPerp;
} else if (cbPerp.dot(c0) > 0) {
a = c;
v = cbPerp;
} else return [a, b, c];
}
},
epa: function(pts1, pts2, a, b, c) {
//// pts1, pts2: Array, element Vector2
//// a, b, c: Vector2
//// [a, b, c]: simplex
let polytope = [a, b, c];
for (;;) {
let { dist, i, p, q, n } = closestEdge(polytope);
let r = support2(pts1, pts2, n);
if (Math.abs(n.dot(r) - dist) < 0.001) return { p, q, dist, n };
polytope.splice(i + 1, 0, r);
}
}
};
});jsファイルは、それぞれ require/define を使ってモジュール化している。
（次回につづく）
