From 80803cbca451458df201eee38308429c515491c7 Mon Sep 17 00:00:00 2001 From: Oliver Zell Date: Wed, 20 Dec 2023 21:00:57 +0100 Subject: [PATCH] fix: fix numerical stability for area weighted centroid --- src/collision/Distance.ts | 4 ++-- src/collision/shape/PolygonShape.ts | 9 ++++----- src/common/Matrix.ts | 6 ++++++ src/dynamics/Contact.ts | 24 ++++++++---------------- 4 files changed, 20 insertions(+), 23 deletions(-) diff --git a/src/collision/Distance.ts b/src/collision/Distance.ts index 91d8bdc0..5c70f6e4 100644 --- a/src/collision/Distance.ts +++ b/src/collision/Distance.ts @@ -506,8 +506,8 @@ class Simplex { break; case 3: - pB.x = pA.x = v1.a * v1.wA.x + v2.a * v2.wA.x + v3.a * v3.wA.x; - pB.y = pA.y = v1.a * v1.wA.y + v2.a * v2.wA.y + v3.a * v3.wA.y; + matrix.combine3Vec2(pA, v1.a, v1.wA, v2.a, v2.wA, v3.a, v3.wA); + matrix.copyVec2(pB, pA); break; default: diff --git a/src/collision/shape/PolygonShape.ts b/src/collision/shape/PolygonShape.ts index fefc5e1c..c7b5827b 100644 --- a/src/collision/shape/PolygonShape.ts +++ b/src/collision/shape/PolygonShape.ts @@ -477,8 +477,8 @@ export class PolygonShape extends Shape { area += triangleArea; // Area weighted centroid - matrix.combineVec2(center, 1, center, triangleArea * k_inv3, e1); - matrix.combineVec2(center, 1, center, triangleArea * k_inv3, e2); + matrix.combineVec2(temp, triangleArea * k_inv3, e1, triangleArea * k_inv3, e2); + matrix.addVec2(center, temp); const ex1 = e1.x; const ey1 = e1.y; @@ -576,9 +576,8 @@ export class PolygonShape extends Shape { area += triangleArea; // Area weighted centroid - c.addMul(triangleArea * inv3, p1); - c.addMul(triangleArea * inv3, p2); - c.addMul(triangleArea * inv3, p3); + matrix.combine3Vec2(temp, 1, p1, 1, p2, 1, p3); + matrix.addMulVec2(c, triangleArea * inv3, temp); } // Centroid diff --git a/src/common/Matrix.ts b/src/common/Matrix.ts index 798758da..5436e0be 100644 --- a/src/common/Matrix.ts +++ b/src/common/Matrix.ts @@ -122,6 +122,12 @@ export function combineVec2(out: Vec2Value, am: number, a: Vec2Value, bm: number return out; } +export function combine3Vec2(out: Vec2Value, am: number, a: Vec2Value, bm: number, b: Vec2Value, cm: number, c: Vec2Value): Vec2Value { + out.x = am * a.x + bm * b.x + cm * c.x; + out.y = am * a.y + bm * b.y + cm * c.y; + return out; +} + export function normalizeVec2Length(out: Vec2Value): number { const length = math_sqrt(out.x * out.x + out.y * out.y); if (length !== 0) { diff --git a/src/dynamics/Contact.ts b/src/dynamics/Contact.ts index ef3db694..3547d69c 100644 --- a/src/dynamics/Contact.ts +++ b/src/dynamics/Contact.ts @@ -1146,13 +1146,11 @@ export class Contact { matrix.setMulVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); - matrix.subMulVec2(vA, mA, P1); - matrix.subMulVec2(vA, mA, P2); + matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2)); // vB.addCombine(mB, P1, mB, P2); - matrix.addMulVec2(vB, mB, P1); - matrix.addMulVec2(vB, mB, P2); + matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB); wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2)); // Accumulate @@ -1203,13 +1201,11 @@ export class Contact { matrix.setMulVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); - matrix.subMulVec2(vA, mA, P1); - matrix.subMulVec2(vA, mA, P2); + matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2)); // vB.addCombine(mB, P1, mB, P2); - matrix.addMulVec2(vB, mB, P1); - matrix.addMulVec2(vB, mB, P2); + matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB); wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2)); // Accumulate @@ -1252,13 +1248,11 @@ export class Contact { matrix.setMulVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); - matrix.subMulVec2(vA, mA, P1); - matrix.subMulVec2(vA, mA, P2); + matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2)); // vB.addCombine(mB, P1, mB, P2); - matrix.addMulVec2(vB, mB, P1); - matrix.addMulVec2(vB, mB, P2); + matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB); wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2)); // Accumulate @@ -1301,13 +1295,11 @@ export class Contact { matrix.setMulVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); - matrix.subMulVec2(vA, mA, P1); - matrix.subMulVec2(vA, mA, P2); + matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); wA -= iA * (matrix.crossVec2Vec2(vcp1.rA, P1) + matrix.crossVec2Vec2(vcp2.rA, P2)); // vB.addCombine(mB, P1, mB, P2); - matrix.addMulVec2(vB, mB, P1); - matrix.addMulVec2(vB, mB, P2); + matrix.combine3Vec2(vB, mB, P1, mB, P2, 1, vB); wB += iB * (matrix.crossVec2Vec2(vcp1.rB, P1) + matrix.crossVec2Vec2(vcp2.rB, P2)); // Accumulate