fix: fix numerical stability for area weighted centroid

piqnt · Dec 20, 2023 · 80803cb · 80803cb
1 parent 820f610
commit 80803cb
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 23 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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