From ba52e0c9ff60e672b28377f4c8042c9462a24edc Mon Sep 17 00:00:00 2001 From: Ali Shakiba Date: Mon, 25 Dec 2023 18:27:49 -0800 Subject: [PATCH] rename matrix functions --- src/collision/Distance.ts | 38 +++--- src/collision/Manifold.ts | 30 ++--- src/collision/TimeOfImpact.ts | 18 +-- src/collision/shape/CollideCirclePolygon.ts | 8 +- src/collision/shape/CollideEdgeCircle.ts | 8 +- src/collision/shape/CollideEdgePolygon.ts | 68 +++++------ src/collision/shape/CollidePolygon.ts | 8 +- src/collision/shape/EdgeShape.ts | 2 +- src/collision/shape/PolygonShape.ts | 26 ++-- src/common/Matrix.ts | 24 ++-- src/common/Sweep.ts | 6 +- src/dynamics/Body.ts | 12 +- src/dynamics/Contact.ts | 124 ++++++++++---------- src/dynamics/Fixture.ts | 2 +- src/dynamics/Solver.ts | 18 +-- 15 files changed, 196 insertions(+), 196 deletions(-) diff --git a/src/collision/Distance.ts b/src/collision/Distance.ts index 5c70f6e4..65c88714 100644 --- a/src/collision/Distance.ts +++ b/src/collision/Distance.ts @@ -171,13 +171,13 @@ export const Distance = function (output: DistanceOutput, cache: SimplexCache, i // Compute a tentative new simplex vertex using support points. const vertex = vertices[simplex.m_count]; // SimplexVertex - vertex.indexA = proxyA.getSupport(matrix.invRotVec2(temp, xfA.q, matrix.setMulVec2(temp, -1, d))); + vertex.indexA = proxyA.getSupport(matrix.derotVec2(temp, xfA.q, matrix.scaleVec2(temp, -1, d))); matrix.transformVec2(vertex.wA, xfA, proxyA.getVertex(vertex.indexA)); - vertex.indexB = proxyB.getSupport(matrix.invRotVec2(temp, xfB.q, d)); + vertex.indexB = proxyB.getSupport(matrix.derotVec2(temp, xfB.q, d)); matrix.transformVec2(vertex.wB, xfB, proxyB.getVertex(vertex.indexB)); - matrix.diffVec2(vertex.w, vertex.wB, vertex.wA); + matrix.subVec2(vertex.w, vertex.wB, vertex.wA); // Iteration count is equated to the number of support point calls. ++iter; @@ -221,14 +221,14 @@ export const Distance = function (output: DistanceOutput, cache: SimplexCache, i // Shapes are still no overlapped. // Move the witness points to the outer surface. output.distance -= rA + rB; - matrix.diffVec2(normal, output.pointB, output.pointA); + matrix.subVec2(normal, output.pointB, output.pointA); matrix.normalizeVec2(normal); - matrix.addMulVec2(output.pointA, rA, normal); - matrix.subMulVec2(output.pointB, rB, normal); + matrix.plusScaleVec2(output.pointA, rA, normal); + matrix.minusScaleVec2(output.pointB, rB, normal); } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. - const p = matrix.diffVec2(temp, output.pointA, output.pointB); + const p = matrix.subVec2(temp, output.pointA, output.pointB); matrix.copyVec2(output.pointA, p); matrix.copyVec2(output.pointB, p); output.distance = 0.0; @@ -397,7 +397,7 @@ class Simplex { const wBLocal = proxyB.getVertex(v.indexB); matrix.transformVec2(v.wA, transformA, wALocal); matrix.transformVec2(v.wB, transformB, wBLocal); - matrix.diffVec2(v.w,v.wB, v.wA); + matrix.subVec2(v.w,v.wB, v.wA); v.a = 0.0; } @@ -421,7 +421,7 @@ class Simplex { const wBLocal = proxyB.getVertex(0); matrix.transformVec2(v.wA, transformA, wALocal); matrix.transformVec2(v.wB, transformB, wBLocal); - matrix.diffVec2(v.w,v.wB, v.wA); + matrix.subVec2(v.w,v.wB, v.wA); v.a = 1.0; this.m_count = 1; } @@ -445,7 +445,7 @@ class Simplex { return matrix.setVec2(searchDirection_reuse, -v1.w.x, -v1.w.y); case 2: { - matrix.diffVec2(e12, v2.w, v1.w); + matrix.subVec2(e12, v2.w, v1.w); const sgn = -matrix.crossVec2Vec2(e12, v1.w); if (sgn > 0.0) { // Origin is left of e12. @@ -475,7 +475,7 @@ class Simplex { return matrix.copyVec2(closestPoint_reuse, v1.w); case 2: - return matrix.combineVec2(closestPoint_reuse, v1.a, v1.w, v2.a, v2.w); + return matrix.combine2Vec2(closestPoint_reuse, v1.a, v1.w, v2.a, v2.w); case 3: return matrix.zeroVec2(closestPoint_reuse); @@ -501,8 +501,8 @@ class Simplex { break; case 2: - matrix.combineVec2(pA, v1.a, v1.wA, v2.a, v2.wA); - matrix.combineVec2(pB, v1.a, v1.wB, v2.a, v2.wB); + matrix.combine2Vec2(pA, v1.a, v1.wA, v2.a, v2.wA); + matrix.combine2Vec2(pB, v1.a, v1.wB, v2.a, v2.wB); break; case 3: @@ -530,8 +530,8 @@ class Simplex { case 3: return matrix.crossVec2Vec2( - matrix.diffVec2(temp1, this.m_v2.w, this.m_v1.w), - matrix.diffVec2(temp2, this.m_v3.w, this.m_v1.w), + matrix.subVec2(temp1, this.m_v2.w, this.m_v1.w), + matrix.subVec2(temp2, this.m_v3.w, this.m_v1.w), ); default: @@ -584,7 +584,7 @@ class Simplex { solve2(): void { const w1 = this.m_v1.w; const w2 = this.m_v2.w; - matrix.diffVec2(e12, w2, w1); + matrix.subVec2(e12, w2, w1); // w1 region const d12_2 = -matrix.dotVec2(w1, e12); @@ -626,7 +626,7 @@ class Simplex { // [1 1 ][a1] = [1] // [w1.e12 w2.e12][a2] = [0] // a3 = 0 - matrix.diffVec2(e12, w2, w1); + matrix.subVec2(e12, w2, w1); const w1e12 = matrix.dotVec2(w1, e12); const w2e12 = matrix.dotVec2(w2, e12); const d12_1 = w2e12; @@ -636,7 +636,7 @@ class Simplex { // [1 1 ][a1] = [1] // [w1.e13 w3.e13][a3] = [0] // a2 = 0 - matrix.diffVec2(e13, w3, w1); + matrix.subVec2(e13, w3, w1); const w1e13 = matrix.dotVec2(w1, e13); const w3e13 = matrix.dotVec2(w3, e13); const d13_1 = w3e13; @@ -646,7 +646,7 @@ class Simplex { // [1 1 ][a2] = [1] // [w2.e23 w3.e23][a3] = [0] // a1 = 0 - matrix.diffVec2(e23, w3, w2); + matrix.subVec2(e23, w3, w2); const w2e23 = matrix.dotVec2(w2, e23); const w3e23 = matrix.dotVec2(w3, e23); const d23_1 = w3e23; diff --git a/src/collision/Manifold.ts b/src/collision/Manifold.ts index daa13d1b..2226c7e3 100644 --- a/src/collision/Manifold.ts +++ b/src/collision/Manifold.ts @@ -162,16 +162,16 @@ export class Manifold { const manifoldPoint = this.points[0]; matrix.transformVec2(pointA, xfA, this.localPoint); matrix.transformVec2(pointB, xfB, manifoldPoint.localPoint); - matrix.diffVec2(dist, pointB, pointA); + matrix.subVec2(dist, pointB, pointA); const lengthSqr = matrix.lengthSqrVec2(dist); if (lengthSqr > EPSILON * EPSILON) { const length = math_sqrt(lengthSqr); - matrix.setMulVec2(normal, 1 / length, dist); + matrix.scaleVec2(normal, 1 / length, dist); } - matrix.combineVec2(cA, 1, pointA, radiusA, normal); - matrix.combineVec2(cB, 1, pointB, -radiusB, normal); - matrix.combineVec2(points[0], 0.5, cA, 0.5, cB); - separations[0] = matrix.dotVec2(matrix.diffVec2(temp, cB, cA), normal); + matrix.combine2Vec2(cA, 1, pointA, radiusA, normal); + matrix.combine2Vec2(cB, 1, pointB, -radiusB, normal); + matrix.combine2Vec2(points[0], 0.5, cA, 0.5, cB); + separations[0] = matrix.dotVec2(matrix.subVec2(temp, cB, cA), normal); break; } @@ -182,10 +182,10 @@ export class Manifold { for (let i = 0; i < this.pointCount; ++i) { const manifoldPoint = this.points[i]; matrix.transformVec2(clipPoint, xfB, manifoldPoint.localPoint); - matrix.combineVec2(cA, 1, clipPoint, radiusA - matrix.dotVec2(matrix.diffVec2(temp, clipPoint, planePoint), normal), normal); - matrix.combineVec2(cB, 1, clipPoint, -radiusB, normal); - matrix.combineVec2(points[i], 0.5, cA, 0.5, cB); - separations[i] = matrix.dotVec2(matrix.diffVec2(temp, cB, cA), normal); + matrix.combine2Vec2(cA, 1, clipPoint, radiusA - matrix.dotVec2(matrix.subVec2(temp, clipPoint, planePoint), normal), normal); + matrix.combine2Vec2(cB, 1, clipPoint, -radiusB, normal); + matrix.combine2Vec2(points[i], 0.5, cA, 0.5, cB); + separations[i] = matrix.dotVec2(matrix.subVec2(temp, cB, cA), normal); } break; } @@ -197,10 +197,10 @@ export class Manifold { for (let i = 0; i < this.pointCount; ++i) { const manifoldPoint = this.points[i]; matrix.transformVec2(clipPoint, xfA, manifoldPoint.localPoint); - matrix.combineVec2(cB, 1, clipPoint, radiusB - matrix.dotVec2(matrix.diffVec2(temp, clipPoint, planePoint), normal), normal); - matrix.combineVec2(cA, 1, clipPoint, -radiusA, normal); - matrix.combineVec2(points[i], 0.5, cA, 0.5, cB); - separations[i] = matrix.dotVec2(matrix.diffVec2(temp, cA, cB), normal); + matrix.combine2Vec2(cB, 1, clipPoint, radiusB - matrix.dotVec2(matrix.subVec2(temp, clipPoint, planePoint), normal), normal); + matrix.combine2Vec2(cA, 1, clipPoint, -radiusA, normal); + matrix.combine2Vec2(points[i], 0.5, cA, 0.5, cB); + separations[i] = matrix.dotVec2(matrix.subVec2(temp, cA, cB), normal); } // Ensure normal points from A to B. matrix.negVec2(normal); @@ -423,7 +423,7 @@ export function clipSegmentToLine( if (distance0 * distance1 < 0.0) { // Find intersection point of edge and plane const interp = distance0 / (distance0 - distance1); - matrix.combineVec2(vOut[numOut].v, 1 - interp, vIn[0].v, interp, vIn[1].v); + matrix.combine2Vec2(vOut[numOut].v, 1 - interp, vIn[0].v, interp, vIn[1].v); // VertexA is hitting edgeB. vOut[numOut].id.setFeatures(vertexIndexA, ContactFeatureType.e_vertex, vIn[0].id.indexB, ContactFeatureType.e_face); diff --git a/src/collision/TimeOfImpact.ts b/src/collision/TimeOfImpact.ts index 910d18bf..46c073ca 100644 --- a/src/collision/TimeOfImpact.ts +++ b/src/collision/TimeOfImpact.ts @@ -377,7 +377,7 @@ class SeparationFunction { const localPointB = this.m_proxyB.getVertex(cache.indexB[0]); matrix.transformVec2(pointA, xfA, localPointA); matrix.transformVec2(pointB, xfB, localPointB); - matrix.diffVec2(this.m_axis, pointB, pointA); + matrix.subVec2(this.m_axis, pointB, pointA); const s = matrix.normalizeVec2Length(this.m_axis); return s; @@ -387,11 +387,11 @@ class SeparationFunction { const localPointB1 = proxyB.getVertex(cache.indexB[0]); const localPointB2 = proxyB.getVertex(cache.indexB[1]); - matrix.crossVec2Num(this.m_axis, matrix.diffVec2(temp, localPointB2, localPointB1), 1.0); + matrix.crossVec2Num(this.m_axis, matrix.subVec2(temp, localPointB2, localPointB1), 1.0); matrix.normalizeVec2(this.m_axis); matrix.rotVec2(normal, xfB.q, this.m_axis); - matrix.combineVec2(this.m_localPoint, 0.5, localPointB1, 0.5, localPointB2); + matrix.combine2Vec2(this.m_localPoint, 0.5, localPointB1, 0.5, localPointB2); matrix.transformVec2(pointB, xfB, this.m_localPoint); const localPointA = proxyA.getVertex(cache.indexA[0]); @@ -410,11 +410,11 @@ class SeparationFunction { const localPointA1 = this.m_proxyA.getVertex(cache.indexA[0]); const localPointA2 = this.m_proxyA.getVertex(cache.indexA[1]); - matrix.crossVec2Num(this.m_axis, matrix.diffVec2(temp, localPointA2, localPointA1), 1.0); + matrix.crossVec2Num(this.m_axis, matrix.subVec2(temp, localPointA2, localPointA1), 1.0); matrix.normalizeVec2(this.m_axis); matrix.rotVec2(normal, xfA.q, this.m_axis); - matrix.combineVec2(this.m_localPoint, 0.5, localPointA1, 0.5, localPointA2); + matrix.combine2Vec2(this.m_localPoint, 0.5, localPointA1, 0.5, localPointA2); matrix.transformVec2(pointA, xfA, this.m_localPoint); const localPointB = this.m_proxyB.getVertex(cache.indexB[0]); @@ -437,8 +437,8 @@ class SeparationFunction { switch (this.m_type) { case SeparationFunctionType.e_points: { if (find) { - matrix.invRotVec2(axisA, xfA.q, this.m_axis); - matrix.invRotVec2(axisB, xfB.q, matrix.setMulVec2(temp, -1, this.m_axis)); + matrix.derotVec2(axisA, xfA.q, this.m_axis); + matrix.derotVec2(axisB, xfB.q, matrix.scaleVec2(temp, -1, this.m_axis)); this.indexA = this.m_proxyA.getSupport(axisA); this.indexB = this.m_proxyB.getSupport(axisB); @@ -459,7 +459,7 @@ class SeparationFunction { matrix.transformVec2(pointA, xfA, this.m_localPoint); if (find) { - matrix.invRotVec2(axisB, xfB.q, matrix.setMulVec2(temp, -1, normal)); + matrix.derotVec2(axisB, xfB.q, matrix.scaleVec2(temp, -1, normal)); this.indexA = -1; this.indexB = this.m_proxyB.getSupport(axisB); @@ -477,7 +477,7 @@ class SeparationFunction { matrix.transformVec2(pointB, xfB, this.m_localPoint); if (find) { - matrix.invRotVec2(axisA, xfA.q, matrix.setMulVec2(temp, -1, normal)); + matrix.derotVec2(axisA, xfA.q, matrix.scaleVec2(temp, -1, normal)); this.indexB = -1; this.indexA = this.m_proxyA.getSupport(axisA); diff --git a/src/collision/shape/CollideCirclePolygon.ts b/src/collision/shape/CollideCirclePolygon.ts index 0b7a720e..ec909bdc 100644 --- a/src/collision/shape/CollideCirclePolygon.ts +++ b/src/collision/shape/CollideCirclePolygon.ts @@ -85,7 +85,7 @@ export const CollidePolygonCircle = function (manifold: Manifold, polygonA: Poly manifold.pointCount = 1; manifold.type = ManifoldType.e_faceA; matrix.copyVec2(manifold.localNormal, normals[normalIndex]); - matrix.combineVec2(manifold.localPoint, 0.5, v1, 0.5, v2); + matrix.combine2Vec2(manifold.localPoint, 0.5, v1, 0.5, v2); matrix.copyVec2(manifold.points[0].localPoint, circleB.m_p); // manifold.points[0].id.key = 0; @@ -105,7 +105,7 @@ export const CollidePolygonCircle = function (manifold: Manifold, polygonA: Poly manifold.pointCount = 1; manifold.type = ManifoldType.e_faceA; - matrix.diffVec2(manifold.localNormal, cLocal, v1); + matrix.subVec2(manifold.localNormal, cLocal, v1); matrix.normalizeVec2(manifold.localNormal); matrix.copyVec2(manifold.localPoint, v1); matrix.copyVec2(manifold.points[0].localPoint, circleB.m_p); @@ -119,7 +119,7 @@ export const CollidePolygonCircle = function (manifold: Manifold, polygonA: Poly manifold.pointCount = 1; manifold.type = ManifoldType.e_faceA; - matrix.diffVec2(manifold.localNormal, cLocal, v2); + matrix.subVec2(manifold.localNormal, cLocal, v2); matrix.normalizeVec2(manifold.localNormal); matrix.copyVec2(manifold.localPoint, v2); matrix.copyVec2(manifold.points[0].localPoint, circleB.m_p); @@ -127,7 +127,7 @@ export const CollidePolygonCircle = function (manifold: Manifold, polygonA: Poly // manifold.points[0].id.key = 0; manifold.points[0].id.setFeatures(0, ContactFeatureType.e_vertex, 0, ContactFeatureType.e_vertex); } else { - matrix.combineVec2(faceCenter, 0.5, v1, 0.5, v2); + matrix.combine2Vec2(faceCenter, 0.5, v1, 0.5, v2); const separation = matrix.dotVec2(cLocal, normals[vertIndex1]) - matrix.dotVec2(faceCenter, normals[vertIndex1]); if (separation > radius) { return; diff --git a/src/collision/shape/CollideEdgeCircle.ts b/src/collision/shape/CollideEdgeCircle.ts index ded8718d..b70f2fc3 100644 --- a/src/collision/shape/CollideEdgeCircle.ts +++ b/src/collision/shape/CollideEdgeCircle.ts @@ -80,7 +80,7 @@ export const CollideEdgeCircle = function (manifold: Manifold, edgeA: EdgeShape, const A = edgeA.m_vertex1; const B = edgeA.m_vertex2; - matrix.diffVec2(e, B, A); + matrix.subVec2(e, B, A); // Barycentric coordinates const u = matrix.dotVec2(e, B) - matrix.dotVec2(e, Q); @@ -100,7 +100,7 @@ export const CollideEdgeCircle = function (manifold: Manifold, edgeA: EdgeShape, if (edgeA.m_hasVertex0) { const A1 = edgeA.m_vertex0; const B1 = A; - matrix.diffVec2(e1, B1, A1); + matrix.subVec2(e1, B1, A1); const u1 = matrix.dotVec2(e1, B1) - matrix.dotVec2(e1, Q); // Is the circle in Region AB of the previous edge? @@ -132,7 +132,7 @@ export const CollideEdgeCircle = function (manifold: Manifold, edgeA: EdgeShape, if (edgeA.m_hasVertex3) { const B2 = edgeA.m_vertex3; const A2 = B; - matrix.diffVec2(e2, B2, A2); + matrix.subVec2(e2, B2, A2); const v2 = matrix.dotVec2(e2, Q) - matrix.dotVec2(e2, A2); // Is the circle in Region AB of the next edge? @@ -156,7 +156,7 @@ export const CollideEdgeCircle = function (manifold: Manifold, edgeA: EdgeShape, // Region AB const den = matrix.lengthSqrVec2(e); _ASSERT && console.assert(den > 0.0); - matrix.combineVec2(P, u / den, A, v / den, B); + matrix.combine2Vec2(P, u / den, A, v / den, B); const dd = matrix.distSqrVec2(Q, P); if (dd > radius * radius) { return; diff --git a/src/collision/shape/CollideEdgePolygon.ts b/src/collision/shape/CollideEdgePolygon.ts index 06dd83a9..0e281c68 100644 --- a/src/collision/shape/CollideEdgePolygon.ts +++ b/src/collision/shape/CollideEdgePolygon.ts @@ -159,7 +159,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape // let m_type1: VertexType; // let m_type2: VertexType; - matrix.invTransformTransform(xf, xfA, xfB); + matrix.detransformTransform(xf, xfA, xfB); matrix.transformVec2(centroidB, xf, polygonB.m_centroid); const v0 = edgeA.m_vertex0; @@ -170,7 +170,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape const hasVertex0 = edgeA.m_hasVertex0; const hasVertex3 = edgeA.m_hasVertex3; - matrix.diffVec2(edge1, v2, v1); + matrix.subVec2(edge1, v2, v1); matrix.normalizeVec2(edge1); matrix.setVec2(normal1, edge1.y, -edge1.x) const offset1 = matrix.dotVec2(normal1, centroidB) - matrix.dotVec2(normal1, v1); @@ -184,7 +184,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape // Is there a preceding edge? if (hasVertex0) { - matrix.diffVec2(edge0, v1, v0); + matrix.subVec2(edge0, v1, v0); matrix.normalizeVec2(edge0); matrix.setVec2(normal0, edge0.y, -edge0.x); convex1 = matrix.crossVec2Vec2(edge0, edge1) >= 0.0; @@ -193,7 +193,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape // Is there a following edge? if (hasVertex3) { - matrix.diffVec2(edge2, v3, v2); + matrix.subVec2(edge2, v3, v2); matrix.normalizeVec2(edge2); matrix.setVec2(normal2, edge2.y, -edge2.x); convex2 = Vec2.crossVec2Vec2(edge1, edge2) > 0.0; @@ -214,9 +214,9 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape matrix.copyVec2(lowerLimit, normal0); matrix.copyVec2(upperLimit, normal2); } else { - matrix.setMulVec2(normal, -1, normal1); - matrix.setMulVec2(lowerLimit, -1, normal1); - matrix.setMulVec2(upperLimit, -1, normal1); + matrix.scaleVec2(normal, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal1); + matrix.scaleVec2(upperLimit, -1, normal1); } } else if (convex1) { front = offset0 >= 0.0 || (offset1 >= 0.0 && offset2 >= 0.0); @@ -225,9 +225,9 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape matrix.copyVec2(lowerLimit, normal0); matrix.copyVec2(upperLimit, normal1); } else { - matrix.setMulVec2(normal, -1, normal1); - matrix.setMulVec2(lowerLimit, -1, normal2); - matrix.setMulVec2(upperLimit, -1, normal1); + matrix.scaleVec2(normal, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal2); + matrix.scaleVec2(upperLimit, -1, normal1); } } else if (convex2) { front = offset2 >= 0.0 || (offset0 >= 0.0 && offset1 >= 0.0); @@ -236,9 +236,9 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape matrix.copyVec2(lowerLimit, normal1); matrix.copyVec2(upperLimit, normal2); } else { - matrix.setMulVec2(normal, -1, normal1); - matrix.setMulVec2(lowerLimit, -1, normal1); - matrix.setMulVec2(upperLimit, -1, normal0); + matrix.scaleVec2(normal, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal1); + matrix.scaleVec2(upperLimit, -1, normal0); } } else { front = offset0 >= 0.0 && offset1 >= 0.0 && offset2 >= 0.0; @@ -247,9 +247,9 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape matrix.copyVec2(lowerLimit, normal1); matrix.copyVec2(upperLimit, normal1); } else { - matrix.setMulVec2(normal, -1, normal1); - matrix.setMulVec2(lowerLimit, -1, normal2); - matrix.setMulVec2(upperLimit, -1, normal0); + matrix.scaleVec2(normal, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal2); + matrix.scaleVec2(upperLimit, -1, normal0); } } } else if (hasVertex0) { @@ -258,22 +258,22 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape if (front) { matrix.copyVec2(normal, normal1); matrix.copyVec2(lowerLimit, normal0); - matrix.setMulVec2(upperLimit, -1, normal1); + matrix.scaleVec2(upperLimit, -1, normal1); } else { - matrix.setMulVec2(normal, -1, normal1); + matrix.scaleVec2(normal, -1, normal1); matrix.copyVec2(lowerLimit, normal1); - matrix.setMulVec2(upperLimit, -1, normal1); + matrix.scaleVec2(upperLimit, -1, normal1); } } else { front = offset0 >= 0.0 && offset1 >= 0.0; if (front) { matrix.copyVec2(normal, normal1); matrix.copyVec2(lowerLimit, normal1); - matrix.setMulVec2(upperLimit, -1, normal1); + matrix.scaleVec2(upperLimit, -1, normal1); } else { - matrix.setMulVec2(normal, -1, normal1); + matrix.scaleVec2(normal, -1, normal1); matrix.copyVec2(lowerLimit, normal1); - matrix.setMulVec2(upperLimit, -1, normal0); + matrix.scaleVec2(upperLimit, -1, normal0); } } } else if (hasVertex3) { @@ -281,22 +281,22 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape front = offset1 >= 0.0 || offset2 >= 0.0; if (front) { matrix.copyVec2(normal, normal1); - matrix.setMulVec2(lowerLimit, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal1); matrix.copyVec2(upperLimit, normal2); } else { - matrix.setMulVec2(normal, -1, normal1); - matrix.setMulVec2(lowerLimit, -1, normal1); + matrix.scaleVec2(normal, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal1); matrix.copyVec2(upperLimit, normal1); } } else { front = offset1 >= 0.0 && offset2 >= 0.0; if (front) { matrix.copyVec2(normal, normal1); - matrix.setMulVec2(lowerLimit, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal1); matrix.copyVec2(upperLimit, normal1); } else { - matrix.setMulVec2(normal, -1, normal1); - matrix.setMulVec2(lowerLimit, -1, normal2); + matrix.scaleVec2(normal, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal2); matrix.copyVec2(upperLimit, normal1); } } @@ -304,10 +304,10 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape front = offset1 >= 0.0; if (front) { matrix.copyVec2(normal, normal1); - matrix.setMulVec2(lowerLimit, -1, normal1); - matrix.setMulVec2(upperLimit, -1, normal1); + matrix.scaleVec2(lowerLimit, -1, normal1); + matrix.scaleVec2(upperLimit, -1, normal1); } else { - matrix.setMulVec2(normal, -1, normal1); + matrix.scaleVec2(normal, -1, normal1); matrix.copyVec2(lowerLimit, normal1); matrix.copyVec2(upperLimit, normal1); } @@ -356,7 +356,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape matrix.setVec2(perp, -normal.y, normal.x); for (let i = 0; i < polygonBA.count; ++i) { - matrix.setMulVec2(n, -1, polygonBA.normals[i]); + matrix.scaleVec2(n, -1, polygonBA.normals[i]); const s1 = matrix.dotVec2(n, polygonBA.vertices[i]) - matrix.dotVec2(n, v1); const s2 = matrix.dotVec2(n, polygonBA.vertices[i]) - matrix.dotVec2(n, v2); @@ -443,7 +443,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape rf.i2 = 0; matrix.copyVec2(rf.v1, v2); matrix.copyVec2(rf.v2, v1); - matrix.setMulVec2(rf.normal, -1, normal1); + matrix.scaleVec2(rf.normal, -1, normal1); } } else { manifold.type = ManifoldType.e_faceB; @@ -501,7 +501,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape const cp = manifold.points[pointCount]; // ManifoldPoint if (primaryAxis.type == EPAxisType.e_edgeA) { - matrix.invTransformVec2(cp.localPoint, xf, clipPoints2[i].v); + matrix.detransformVec2(cp.localPoint, xf, clipPoints2[i].v); cp.id.set(clipPoints2[i].id); } else { matrix.copyVec2(cp.localPoint, clipPoints2[i].v); diff --git a/src/collision/shape/CollidePolygon.ts b/src/collision/shape/CollidePolygon.ts index 38e93bda..fb0fa3e1 100644 --- a/src/collision/shape/CollidePolygon.ts +++ b/src/collision/shape/CollidePolygon.ts @@ -89,7 +89,7 @@ Contact.addType(PolygonShape.TYPE, PolygonShape.TYPE, PolygonContact); const v1s = poly1.m_vertices; const v2s = poly2.m_vertices; - matrix.invTransformTransform(xf, xf2, xf1); + matrix.detransformTransform(xf, xf2, xf1); let bestIndex = 0; let maxSeparation = -Infinity; @@ -234,11 +234,11 @@ export const CollidePolygons = function ( matrix.copyVec2(v11, vertices1[iv1]); matrix.copyVec2(v12, vertices1[iv2]); - matrix.diffVec2(localTangent, v12, v11); + matrix.subVec2(localTangent, v12, v11); matrix.normalizeVec2(localTangent); matrix.crossVec2Num(localNormal, localTangent, 1.0); - matrix.combineVec2(planePoint, 0.5, v11, 0.5, v12); + matrix.combine2Vec2(planePoint, 0.5, v11, 0.5, v12); matrix.rotVec2(tangent, xf1.q, localTangent); matrix.crossVec2Num(normal, tangent, 1.0); @@ -283,7 +283,7 @@ export const CollidePolygons = function ( if (separation <= totalRadius) { const cp = manifold.points[pointCount]; - matrix.invTransformVec2(cp.localPoint, xf2, clipPoints2[i].v); + matrix.detransformVec2(cp.localPoint, xf2, clipPoints2[i].v); cp.id.set(clipPoints2[i].id); if (flip) { // Swap features diff --git a/src/collision/shape/EdgeShape.ts b/src/collision/shape/EdgeShape.ts index 15b9a5fd..0d5a77c9 100644 --- a/src/collision/shape/EdgeShape.ts +++ b/src/collision/shape/EdgeShape.ts @@ -311,7 +311,7 @@ export class EdgeShape extends Shape { */ computeMass(massData: MassData, density?: number): void { massData.mass = 0.0; - matrix.combineVec2(massData.center, 0.5, this.m_vertex1, 0.5, this.m_vertex2); + matrix.combine2Vec2(massData.center, 0.5, this.m_vertex1, 0.5, this.m_vertex2); massData.I = 0.0; } diff --git a/src/collision/shape/PolygonShape.ts b/src/collision/shape/PolygonShape.ts index c7b5827b..9010400f 100644 --- a/src/collision/shape/PolygonShape.ts +++ b/src/collision/shape/PolygonShape.ts @@ -307,7 +307,7 @@ export class PolygonShape extends Shape { * @param p A point in world coordinates. */ testPoint(xf: TransformValue, p: Vec2): boolean { - const pLocal = matrix.invTransformVec2(temp, xf, p); + const pLocal = matrix.detransformVec2(temp, xf, p); for (let i = 0; i < this.m_count; ++i) { const dot = matrix.dotVec2(this.m_normals[i], pLocal) - matrix.dotVec2(this.m_normals[i], this.m_vertices[i]); @@ -456,19 +456,19 @@ export class PolygonShape extends Shape { // This code would put the reference point inside the polygon. for (let i = 0; i < this.m_count; ++i) { - matrix.addVec2(s, this.m_vertices[i]); + matrix.plusVec2(s, this.m_vertices[i]); } - matrix.setMulVec2(s, 1.0 / this.m_count, s); + matrix.scaleVec2(s, 1.0 / this.m_count, s); const k_inv3 = 1.0 / 3.0; for (let i = 0; i < this.m_count; ++i) { // Triangle vertices. - matrix.diffVec2(e1, this.m_vertices[i], s); + matrix.subVec2(e1, this.m_vertices[i], s); if ( i + 1 < this.m_count) { - matrix.diffVec2(e2, this.m_vertices[i + 1], s); + matrix.subVec2(e2, this.m_vertices[i + 1], s); } else { - matrix.diffVec2(e2, this.m_vertices[0], s); + matrix.subVec2(e2, this.m_vertices[0], s); } const D = matrix.crossVec2Vec2(e1, e2); @@ -477,8 +477,8 @@ export class PolygonShape extends Shape { area += triangleArea; // Area weighted centroid - matrix.combineVec2(temp, triangleArea * k_inv3, e1, triangleArea * k_inv3, e2); - matrix.addVec2(center, temp); + matrix.combine2Vec2(temp, triangleArea * k_inv3, e1, triangleArea * k_inv3, e2); + matrix.plusVec2(center, temp); const ex1 = e1.x; const ey1 = e1.y; @@ -496,8 +496,8 @@ export class PolygonShape extends Shape { // Center of mass _ASSERT && console.assert(area > EPSILON); - matrix.setMulVec2(center, 1.0 / area, center); - matrix.sumVec2(massData.center, center, s); + matrix.scaleVec2(center, 1.0 / area, center); + matrix.addVec2(massData.center, center, s); // Inertia tensor relative to the local origin (point s). massData.I = density * I; @@ -515,14 +515,14 @@ export class PolygonShape extends Shape { const i1 = i; const i2 = i < this.m_count - 1 ? i1 + 1 : 0; const p = this.m_vertices[i1]; - matrix.diffVec2(e, this.m_vertices[i2], p); + matrix.subVec2(e, this.m_vertices[i2], p); for (let j = 0; j < this.m_count; ++j) { if (j == i1 || j == i2) { continue; } - const c = matrix.crossVec2Vec2(e, matrix.diffVec2(temp, this.m_vertices[j], p)); + const c = matrix.crossVec2Vec2(e, matrix.subVec2(temp, this.m_vertices[j], p)); if (c < 0.0) { return false; } @@ -577,7 +577,7 @@ export class PolygonShape extends Shape { // Area weighted centroid matrix.combine3Vec2(temp, 1, p1, 1, p2, 1, p3); - matrix.addMulVec2(c, triangleArea * inv3, temp); + matrix.plusScaleVec2(c, triangleArea * inv3, temp); } // Centroid diff --git a/src/common/Matrix.ts b/src/common/Matrix.ts index 5436e0be..d1d1dfe3 100644 --- a/src/common/Matrix.ts +++ b/src/common/Matrix.ts @@ -68,55 +68,55 @@ export function negVec2(out: Vec2Value): Vec2Value { return out; } -export function addVec2(out: Vec2Value, w: Vec2Value): Vec2Value { +export function plusVec2(out: Vec2Value, w: Vec2Value): Vec2Value { out.x += w.x; out.y += w.y; return out; } -export function sumVec2(out: Vec2Value, v: Vec2Value, w: Vec2Value): Vec2Value { +export function addVec2(out: Vec2Value, v: Vec2Value, w: Vec2Value): Vec2Value { out.x = v.x + w.x; out.y = v.x + w.y; return out; } -export function subVec2(out: Vec2Value, w: Vec2Value): Vec2Value { +export function minusVec2(out: Vec2Value, w: Vec2Value): Vec2Value { out.x -= w.x; out.y -= w.y; return out; } -export function diffVec2(out: Vec2Value, v: Vec2Value, w: Vec2Value): Vec2Value { +export function subVec2(out: Vec2Value, v: Vec2Value, w: Vec2Value): Vec2Value { out.x = v.x - w.x; out.y = v.y - w.y; return out; } -export function scaleVec2(out: Vec2Value, m: number): Vec2Value { +export function mulVec2(out: Vec2Value, m: number): Vec2Value { out.x *= m; out.y *= m; return out; } -export function setMulVec2(out: Vec2Value, m: number, w: Vec2Value): Vec2Value { +export function scaleVec2(out: Vec2Value, m: number, w: Vec2Value): Vec2Value { out.x = m * w.x; out.y = m * w.y; return out; } -export function addMulVec2(out: Vec2Value, m: number, w: Vec2Value): Vec2Value { +export function plusScaleVec2(out: Vec2Value, m: number, w: Vec2Value): Vec2Value { out.x += m * w.x; out.y += m * w.y; return out; } -export function subMulVec2(out: Vec2Value, m: number, w: Vec2Value): Vec2Value { +export function minusScaleVec2(out: Vec2Value, m: number, w: Vec2Value): Vec2Value { out.x -= m * w.x; out.y -= m * w.y; return out; } -export function combineVec2(out: Vec2Value, am: number, a: Vec2Value, bm: number, b: Vec2Value): Vec2Value { +export function combine2Vec2(out: Vec2Value, am: number, a: Vec2Value, bm: number, b: Vec2Value): Vec2Value { out.x = am * a.x + bm * b.x; out.y = am * a.y + bm * b.y; return out; @@ -208,7 +208,7 @@ export function rotVec2(out: Vec2Value, q: RotValue, v: Vec2Value): Vec2Value { return out; } -export function invRotVec2(out: Vec2Value, q: RotValue, v: Vec2Value): Vec2Value { +export function derotVec2(out: Vec2Value, q: RotValue, v: Vec2Value): Vec2Value { const x = q.c * v.x + q.s * v.y; const y = -q.s * v.x + q.c * v.y; out.x = x; @@ -246,7 +246,7 @@ export function transformVec2(out: Vec2Value, xf: TransformValue, v: Vec2Value): return out; } -export function invTransformVec2(out: Vec2Value, xf: TransformValue, v: Vec2Value): Vec2Value { +export function detransformVec2(out: Vec2Value, xf: TransformValue, v: Vec2Value): Vec2Value { const px = v.x - xf.p.x; const py = v.y - xf.p.y; const x = (xf.q.c * px + xf.q.s * py); @@ -268,7 +268,7 @@ export function retransformVec2(out: Vec2Value, from: TransformValue, to: Transf return out; } -export function invTransformTransform(out: TransformValue, a: TransformValue, b: TransformValue): TransformValue { +export function detransformTransform(out: TransformValue, a: TransformValue, b: TransformValue): TransformValue { const c = a.q.c * b.q.c + a.q.s * b.q.s; const s = a.q.c * b.q.s - a.q.s * b.q.c; const x = a.q.c * (b.p.x - a.p.x) + a.q.s * (b.p.y - a.p.y); diff --git a/src/common/Sweep.ts b/src/common/Sweep.ts index d7c044cc..c7a7ded6 100644 --- a/src/common/Sweep.ts +++ b/src/common/Sweep.ts @@ -91,10 +91,10 @@ export class Sweep { */ getTransform(xf: TransformValue, beta: number = 0): void { matrix.setRotAngle(xf.q, (1.0 - beta) * this.a0 + beta * this.a); - matrix.combineVec2(xf.p, (1.0 - beta), this.c0, beta, this.c); + matrix.combine2Vec2(xf.p, (1.0 - beta), this.c0, beta, this.c); // shift to origin - matrix.subVec2(xf.p, matrix.rotVec2(temp, xf.q, this.localCenter)); + matrix.minusVec2(xf.p, matrix.rotVec2(temp, xf.q, this.localCenter)); } /** @@ -105,7 +105,7 @@ export class Sweep { advance(alpha: number): void { _ASSERT && console.assert(this.alpha0 < 1.0); const beta = (alpha - this.alpha0) / (1.0 - this.alpha0); - matrix.combineVec2(this.c0, beta, this.c, 1 - beta, this.c0); + matrix.combine2Vec2(this.c0, beta, this.c, 1 - beta, this.c0); this.a0 = beta * this.a + (1 - beta) * this.a0; this.alpha0 = alpha; } diff --git a/src/dynamics/Body.ts b/src/dynamics/Body.ts index 2fa3deed..90b1b4d8 100644 --- a/src/dynamics/Body.ts +++ b/src/dynamics/Body.ts @@ -816,14 +816,14 @@ export class Body { }; f.getMassData(massData); this.m_mass += massData.mass; - matrix.addMulVec2(localCenter, massData.mass, massData.center) + matrix.plusScaleVec2(localCenter, massData.mass, massData.center) this.m_I += massData.I; } // Compute center of mass. if (this.m_mass > 0.0) { this.m_invMass = 1.0 / this.m_mass; - matrix.setMulVec2(localCenter, this.m_invMass, localCenter) + matrix.scaleVec2(localCenter, this.m_invMass, localCenter) } else { // Force all dynamic bodies to have a positive mass. @@ -847,9 +847,9 @@ export class Body { this.m_sweep.setLocalCenter(localCenter, this.m_xf); // Update center of mass velocity. - matrix.diffVec2(shift, this.m_sweep.c, oldCenter); + matrix.subVec2(shift, this.m_sweep.c, oldCenter); matrix.crossNumVec2(temp, this.m_angularVelocity, shift); - matrix.addVec2(this.m_linearVelocity, temp); + matrix.plusVec2(this.m_linearVelocity, temp); } /** @@ -892,9 +892,9 @@ export class Body { this.m_sweep.setLocalCenter(massData.center, this.m_xf); // Update center of mass velocity. - matrix.diffVec2(shift, this.m_sweep.c, oldCenter); + matrix.subVec2(shift, this.m_sweep.c, oldCenter); matrix.crossNumVec2(temp, this.m_angularVelocity, shift); - matrix.addVec2(this.m_linearVelocity, temp); + matrix.plusVec2(this.m_linearVelocity, temp); } /** diff --git a/src/dynamics/Contact.ts b/src/dynamics/Contact.ts index 3547d69c..1ab7ca1a 100644 --- a/src/dynamics/Contact.ts +++ b/src/dynamics/Contact.ts @@ -698,10 +698,10 @@ export class Contact { case ManifoldType.e_circles: { matrix.transformVec2(pointA, xfA, this.p_localPoint); matrix.transformVec2(pointB, xfB, this.p_localPoints[0]); - matrix.diffVec2(normal, pointB, pointA); + matrix.subVec2(normal, pointB, pointA); matrix.normalizeVec2(normal); - matrix.combineVec2(point, 0.5, pointA, 0.5, pointB); + matrix.combine2Vec2(point, 0.5, pointA, 0.5, pointB); separation = matrix.dotVec2(pointB, normal) - matrix.dotVec2(pointA, normal) - this.p_radiusA - this.p_radiusB; break; } @@ -732,8 +732,8 @@ export class Contact { } } - matrix.diffVec2(rA, point, cA); - matrix.diffVec2(rB, point, cB); + matrix.subVec2(rA, point, cA); + matrix.subVec2(rB, point, cB); // Track max constraint error. minSeparation = math_min(minSeparation, separation); @@ -753,12 +753,12 @@ export class Contact { // Compute normal impulse const impulse = K > 0.0 ? -C / K : 0.0; - matrix.setMulVec2(P, impulse, normal); + matrix.scaleVec2(P, impulse, normal); - matrix.subMulVec2(cA, mA, P); + matrix.minusScaleVec2(cA, mA, P); aA -= iA * matrix.crossVec2Vec2(rA, P); - matrix.addMulVec2(cB, mB, P); + matrix.plusScaleVec2(cB, mB, P); aB += iB * matrix.crossVec2Vec2(rB, P); } @@ -820,8 +820,8 @@ export class Contact { const vcp = this.v_points[j]; // VelocityConstraintPoint const wmp = worldManifold.points[j]; - matrix.diffVec2(vcp.rA, wmp, cA); - matrix.diffVec2(vcp.rB, wmp, cB); + matrix.subVec2(vcp.rA, wmp, cA); + matrix.subVec2(vcp.rB, wmp, cB); const rnA = matrix.crossVec2Vec2(vcp.rA, this.v_normal); const rnB = matrix.crossVec2Vec2(vcp.rB, this.v_normal); @@ -932,12 +932,12 @@ export class Contact { for (let j = 0; j < this.v_pointCount; ++j) { const vcp = this.v_points[j]; // VelocityConstraintPoint - matrix.combineVec2(P, vcp.normalImpulse, normal, vcp.tangentImpulse, tangent); + matrix.combine2Vec2(P, vcp.normalImpulse, normal, vcp.tangentImpulse, tangent); wA -= iA * matrix.crossVec2Vec2(vcp.rA, P); - matrix.subMulVec2(vA, mA, P); + matrix.minusScaleVec2(vA, mA, P); wB += iB * matrix.crossVec2Vec2(vcp.rB, P); - matrix.addMulVec2(vB, mB, P); + matrix.plusScaleVec2(vB, mB, P); } matrix.copyVec2(velocityA.v, vA); @@ -991,10 +991,10 @@ export class Contact { // Relative velocity at contact matrix.zeroVec2(dv); - matrix.addVec2(dv, vB); - matrix.addVec2(dv, matrix.crossNumVec2(temp, wB, vcp.rB)); - matrix.subVec2(dv, vA); - matrix.subVec2(dv, matrix.crossNumVec2(temp, wA, vcp.rA)); + matrix.plusVec2(dv, vB); + matrix.plusVec2(dv, matrix.crossNumVec2(temp, wB, vcp.rB)); + matrix.minusVec2(dv, vA); + matrix.minusVec2(dv, matrix.crossNumVec2(temp, wA, vcp.rA)); // Compute tangent force const vt = matrix.dotVec2(dv, tangent) - this.v_tangentSpeed; @@ -1007,12 +1007,12 @@ export class Contact { vcp.tangentImpulse = newImpulse; // Apply contact impulse - matrix.setMulVec2(P, lambda, tangent); + matrix.scaleVec2(P, lambda, tangent); - matrix.subMulVec2(vA, mA, P); + matrix.minusScaleVec2(vA, mA, P); wA -= iA * matrix.crossVec2Vec2(vcp.rA, P); - matrix.addMulVec2(vB, mB, P); + matrix.plusScaleVec2(vB, mB, P); wB += iB * matrix.crossVec2Vec2(vcp.rB, P); } @@ -1023,10 +1023,10 @@ export class Contact { // Relative velocity at contact matrix.zeroVec2(dv); - matrix.addVec2(dv, vB); - matrix.addVec2(dv, matrix.crossNumVec2(temp, wB, vcp.rB)); - matrix.subVec2(dv, vA); - matrix.subVec2(dv, matrix.crossNumVec2(temp, wA, vcp.rA)); + matrix.plusVec2(dv, vB); + matrix.plusVec2(dv, matrix.crossNumVec2(temp, wB, vcp.rB)); + matrix.minusVec2(dv, vA); + matrix.minusVec2(dv, matrix.crossNumVec2(temp, wA, vcp.rA)); // Compute normal impulse const vn = matrix.dotVec2(dv, normal); @@ -1038,12 +1038,12 @@ export class Contact { vcp.normalImpulse = newImpulse; // Apply contact impulse - matrix.setMulVec2(P, lambda, normal); + matrix.scaleVec2(P, lambda, normal); - matrix.subMulVec2(vA, mA, P); + matrix.minusScaleVec2(vA, mA, P); wA -= iA * matrix.crossVec2Vec2(vcp.rA, P); - matrix.addMulVec2(vB, mB, P); + matrix.plusScaleVec2(vB, mB, P); wB += iB * matrix.crossVec2Vec2(vcp.rB, P); } } else { @@ -1096,17 +1096,17 @@ export class Contact { // Relative velocity at contact // let dv1 = Vec2.zero().add(vB).add(Vec2.crossNumVec2(wB, vcp1.rB)).sub(vA).sub(Vec2.crossNumVec2(wA, vcp1.rA)); matrix.zeroVec2(dv1); - matrix.addVec2(dv1, vB); - matrix.addVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB)); - matrix.subVec2(dv1, vA); - matrix.subVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA)); + matrix.plusVec2(dv1, vB); + matrix.plusVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB)); + matrix.minusVec2(dv1, vA); + matrix.minusVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA)); // let dv2 = Vec2.zero().add(vB).add(Vec2.crossNumVec2(wB, vcp2.rB)).sub(vA).sub(Vec2.crossNumVec2(wA, vcp2.rA)); matrix.zeroVec2(dv2); - matrix.addVec2(dv2, vB); - matrix.addVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB)); - matrix.subVec2(dv2, vA); - matrix.subVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA)); + matrix.plusVec2(dv2, vB); + matrix.plusVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB)); + matrix.minusVec2(dv2, vA); + matrix.minusVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA)); // Compute normal velocity let vn1 = matrix.dotVec2(dv1, normal); @@ -1139,11 +1139,11 @@ export class Contact { if (x.x >= 0.0 && x.y >= 0.0) { // Get the incremental impulse - matrix.diffVec2(d, x, a) + matrix.subVec2(d, x, a) // Apply incremental impulse - matrix.setMulVec2(P1, d.x, normal); - matrix.setMulVec2(P2, d.y, normal); + matrix.scaleVec2(P1, d.x, normal); + matrix.scaleVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); @@ -1160,16 +1160,16 @@ export class Contact { if (DEBUG_SOLVER) { // Postconditions matrix.zeroVec2(dv1); - matrix.addVec2(dv1, vB); - matrix.addVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB)); - matrix.subVec2(dv1, vA); - matrix.subVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA)); + matrix.plusVec2(dv1, vB); + matrix.plusVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB)); + matrix.minusVec2(dv1, vA); + matrix.minusVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA)); matrix.zeroVec2(dv2); - matrix.addVec2(dv2, vB); - matrix.addVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB)); - matrix.subVec2(dv2, vA); - matrix.subVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA)); + matrix.plusVec2(dv2, vB); + matrix.plusVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB)); + matrix.minusVec2(dv2, vA); + matrix.minusVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA)); // Compute normal velocity vn1 = matrix.dotVec2(dv1, normal); @@ -1194,11 +1194,11 @@ export class Contact { if (x.x >= 0.0 && vn2 >= 0.0) { // Get the incremental impulse - matrix.diffVec2(d, x, a); + matrix.subVec2(d, x, a); // Apply incremental impulse - matrix.setMulVec2(P1, d.x, normal); - matrix.setMulVec2(P2, d.y, normal); + matrix.scaleVec2(P1, d.x, normal); + matrix.scaleVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); @@ -1215,10 +1215,10 @@ export class Contact { if (DEBUG_SOLVER) { // Postconditions matrix.zeroVec2(dv1); - matrix.addVec2(dv1, vB); - matrix.addVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB)); - matrix.subVec2(dv1, vA); - matrix.subVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA)); + matrix.plusVec2(dv1, vB); + matrix.plusVec2(dv1, matrix.crossNumVec2(temp, wB, vcp1.rB)); + matrix.minusVec2(dv1, vA); + matrix.minusVec2(dv1, matrix.crossNumVec2(temp, wA, vcp1.rA)); // Compute normal velocity vn1 = matrix.dotVec2(dv1, normal); @@ -1241,11 +1241,11 @@ export class Contact { if (x.y >= 0.0 && vn1 >= 0.0) { // Resubstitute for the incremental impulse - matrix.diffVec2(d, x, a); + matrix.subVec2(d, x, a); // Apply incremental impulse - matrix.setMulVec2(P1, d.x, normal); - matrix.setMulVec2(P2, d.y, normal); + matrix.scaleVec2(P1, d.x, normal); + matrix.scaleVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); @@ -1262,10 +1262,10 @@ export class Contact { if (DEBUG_SOLVER) { // Postconditions matrix.zeroVec2(dv2); - matrix.addVec2(dv2, vB); - matrix.addVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB)); - matrix.subVec2(dv2, vA); - matrix.subVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA)); + matrix.plusVec2(dv2, vB); + matrix.plusVec2(dv2, matrix.crossNumVec2(temp, wB, vcp2.rB)); + matrix.minusVec2(dv2, vA); + matrix.minusVec2(dv2, matrix.crossNumVec2(temp, wA, vcp2.rA)); // Compute normal velocity vn2 = matrix.dotVec2(dv2, normal); @@ -1288,11 +1288,11 @@ export class Contact { if (vn1 >= 0.0 && vn2 >= 0.0) { // Resubstitute for the incremental impulse - matrix.diffVec2(d, x, a); + matrix.subVec2(d, x, a); // Apply incremental impulse - matrix.setMulVec2(P1, d.x, normal); - matrix.setMulVec2(P2, d.y, normal); + matrix.scaleVec2(P1, d.x, normal); + matrix.scaleVec2(P2, d.y, normal); // vA.subCombine(mA, P1, mA, P2); matrix.combine3Vec2(vA, -mA, P1, -mA, P2, 1, vA); diff --git a/src/dynamics/Fixture.ts b/src/dynamics/Fixture.ts index 2a6616eb..60e463b9 100644 --- a/src/dynamics/Fixture.ts +++ b/src/dynamics/Fixture.ts @@ -409,7 +409,7 @@ export class Fixture { proxy.aabb.combine(synchronize_aabb1, synchronize_aabb2); - matrix.diffVec2(displacement, xf2.p, xf1.p); + matrix.subVec2(displacement, xf2.p, xf1.p); broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement); } diff --git a/src/dynamics/Solver.ts b/src/dynamics/Solver.ts index ca18a055..f08890ca 100644 --- a/src/dynamics/Solver.ts +++ b/src/dynamics/Solver.ts @@ -319,8 +319,8 @@ export class Solver { if (body.isDynamic()) { // Integrate velocities. - matrix.addMulVec2(v, h * body.m_gravityScale, gravity); - matrix.addMulVec2(v, h * body.m_invMass, body.m_force); + matrix.plusScaleVec2(v, h * body.m_gravityScale, gravity); + matrix.plusScaleVec2(v, h * body.m_invMass, body.m_force); w += h * body.m_invI * body.m_torque; /** * 
@@ -333,7 +333,7 @@ export class Solver {
          * v2 = v1 * 1 / (1 + c * dt)
          *
*/ - matrix.setMulVec2(v, 1.0 / (1.0 + h * body.m_linearDamping), v) + matrix.scaleVec2(v, 1.0 / (1.0 + h * body.m_linearDamping), v) w *= 1.0 / (1.0 + h * body.m_angularDamping); } @@ -395,11 +395,11 @@ export class Solver { let w = body.c_velocity.w; // Check for large velocities - matrix.setMulVec2(translation, h, v); + matrix.scaleVec2(translation, h, v); const translationLengthSqr = matrix.lengthSqrVec2(translation); if (translationLengthSqr > Settings.maxTranslationSquared) { const ratio = Settings.maxTranslation / math_sqrt(translationLengthSqr); - matrix.scaleVec2(v, ratio); + matrix.mulVec2(v, ratio); } const rotation = h * w; @@ -409,7 +409,7 @@ export class Solver { } // Integrate - matrix.addMulVec2(c, h, v); + matrix.plusScaleVec2(c, h, v); a += h * w; matrix.copyVec2(body.c_position.c, c); @@ -860,11 +860,11 @@ export class Solver { let w = body.c_velocity.w; // Check for large velocities - matrix.setMulVec2(translation, h, v); + matrix.scaleVec2(translation, h, v); const translationLengthSqr = matrix.lengthSqrVec2(translation); if (translationLengthSqr > Settings.maxTranslationSquared) { const ratio = Settings.maxTranslation / math_sqrt(translationLengthSqr); - matrix.scaleVec2(v, ratio); + matrix.mulVec2(v, ratio); } const rotation = h * w; @@ -874,7 +874,7 @@ export class Solver { } // Integrate - matrix.addMulVec2(c, h, v); + matrix.plusScaleVec2(c, h, v); a += h * w; matrix.copyVec2(body.c_position.c, c);