planck-v2 2.0.0

package/src/collision/shape/CollidePolygon.ts

@@ -0,0 +1,280 @@
+/*
+ * Planck.js
+ *
+ * Copyright (c) Erin Catto, Ali Shakiba
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+import { TransformValue } from "../../common/Transform";
+import * as geo from "../../common/Geo";
+import { SettingsInternal as Settings } from "../../Settings";
+import { Manifold, clipSegmentToLine, ClipVertex, ContactFeatureType, ManifoldType } from "../Manifold";
+import { Contact } from "../../dynamics/Contact";
+import { PolygonShape } from "./PolygonShape";
+import { Fixture } from "../../dynamics/Fixture";
+
+/** @internal */ const _ASSERT = typeof ASSERT === "undefined" ? false : ASSERT;
+
+/** @internal */ const incidentEdge = [new ClipVertex(), new ClipVertex()];
+/** @internal */ const clipPoints1 = [new ClipVertex(), new ClipVertex()];
+/** @internal */ const clipPoints2 = [new ClipVertex(), new ClipVertex()];
+/** @internal */ const clipSegmentToLineNormal = geo.vec2(0, 0);
+/** @internal */ const v1 = geo.vec2(0, 0);
+/** @internal */ const n = geo.vec2(0, 0);
+/** @internal */ const xf = geo.transform(0, 0, 0);
+/** @internal */ const v11 = geo.vec2(0, 0);
+/** @internal */ const v12 = geo.vec2(0, 0);
+/** @internal */ const localTangent = geo.vec2(0, 0);
+/** @internal */ const localNormal = geo.vec2(0, 0);
+/** @internal */ const planePoint = geo.vec2(0, 0);
+/** @internal */ const tangent = geo.vec2(0, 0);
+/** @internal */ const normal = geo.vec2(0, 0);
+/** @internal */ const normal1 = geo.vec2(0, 0);
+
+Contact.addType(PolygonShape.TYPE, PolygonShape.TYPE, PolygonContact);
+
+/** @internal */ function PolygonContact(
+  manifold: Manifold,
+  xfA: TransformValue,
+  fixtureA: Fixture,
+  indexA: number,
+  xfB: TransformValue,
+  fixtureB: Fixture,
+  indexB: number,
+): void {
+  if (_ASSERT) console.assert(fixtureA.getType() == PolygonShape.TYPE);
+  if (_ASSERT) console.assert(fixtureB.getType() == PolygonShape.TYPE);
+  CollidePolygons(manifold, fixtureA.getShape() as PolygonShape, xfA, fixtureB.getShape() as PolygonShape, xfB);
+}
+
+/** @internal */ interface MaxSeparation {
+  maxSeparation: number;
+  bestIndex: number;
+}
+
+/**
+ * Find the max separation between poly1 and poly2 using edge normals from
+ * poly1.
+ */
+/** @internal */ function findMaxSeparation(
+  poly1: PolygonShape,
+  xf1: TransformValue,
+  poly2: PolygonShape,
+  xf2: TransformValue,
+  output: MaxSeparation,
+): void {
+  const count1 = poly1.m_count;
+  const count2 = poly2.m_count;
+  const n1s = poly1.m_normals;
+  const v1s = poly1.m_vertices;
+  const v2s = poly2.m_vertices;
+
+  geo.detransformTransform(xf, xf2, xf1);
+
+  let bestIndex = 0;
+  let maxSeparation = -Infinity;
+  for (let i = 0; i < count1; ++i) {
+    // Get poly1 normal in frame2.
+    geo.rotVec2(n, xf.q, n1s[i]);
+    geo.transformVec2(v1, xf, v1s[i]);
+
+    // Find deepest point for normal i.
+    let si = Infinity;
+    for (let j = 0; j < count2; ++j) {
+      const sij = geo.dotVec2(n, v2s[j]) - geo.dotVec2(n, v1);
+      if (sij < si) {
+        si = sij;
+      }
+    }
+
+    if (si > maxSeparation) {
+      maxSeparation = si;
+      bestIndex = i;
+    }
+  }
+
+  // used to keep last FindMaxSeparation call values
+  output.maxSeparation = maxSeparation;
+  output.bestIndex = bestIndex;
+}
+
+/** @internal */ function findIncidentEdge(
+  clipVertex: ClipVertex[],
+  poly1: PolygonShape,
+  xf1: TransformValue,
+  edge1: number,
+  poly2: PolygonShape,
+  xf2: TransformValue,
+): void {
+  const normals1 = poly1.m_normals;
+
+  const count2 = poly2.m_count;
+  const vertices2 = poly2.m_vertices;
+  const normals2 = poly2.m_normals;
+
+  if (_ASSERT) console.assert(0 <= edge1 && edge1 < poly1.m_count);
+
+  // Get the normal of the reference edge in poly2's frame.
+  geo.rerotVec2(normal1, xf2.q, xf1.q, normals1[edge1]);
+
+  // Find the incident edge on poly2.
+  let index = 0;
+  let minDot = Infinity;
+  for (let i = 0; i < count2; ++i) {
+    const dot = geo.dotVec2(normal1, normals2[i]);
+    if (dot < minDot) {
+      minDot = dot;
+      index = i;
+    }
+  }
+
+  // Build the clip vertices for the incident edge.
+  const i1 = index;
+  const i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+  geo.transformVec2(clipVertex[0].v, xf2, vertices2[i1]);
+  clipVertex[0].id.setFeatures(edge1, ContactFeatureType.e_face, i1, ContactFeatureType.e_vertex);
+
+  geo.transformVec2(clipVertex[1].v, xf2, vertices2[i2]);
+  clipVertex[1].id.setFeatures(edge1, ContactFeatureType.e_face, i2, ContactFeatureType.e_vertex);
+}
+
+/** @internal */ const maxSeparation = {
+  maxSeparation: 0,
+  bestIndex: 0,
+};
+
+/**
+ *
+ * Find edge normal of max separation on A - return if separating axis is found
+ * Find edge normal of max separation on B - return if separation axis is found
+ * Choose reference edge as min(minA, minB)
+ * Find incident edge
+ * Clip
+ *
+ * The normal points from 1 to 2
+ */
+export const CollidePolygons = function (
+  manifold: Manifold,
+  polyA: PolygonShape,
+  xfA: TransformValue,
+  polyB: PolygonShape,
+  xfB: TransformValue,
+): void {
+  manifold.pointCount = 0;
+  const totalRadius = polyA.m_radius + polyB.m_radius;
+
+  findMaxSeparation(polyA, xfA, polyB, xfB, maxSeparation);
+  const edgeA = maxSeparation.bestIndex;
+  const separationA = maxSeparation.maxSeparation;
+  if (separationA > totalRadius) return;
+
+  findMaxSeparation(polyB, xfB, polyA, xfA, maxSeparation);
+  const edgeB = maxSeparation.bestIndex;
+  const separationB = maxSeparation.maxSeparation;
+  if (separationB > totalRadius) return;
+
+  let poly1: PolygonShape; // reference polygon
+  let poly2: PolygonShape; // incident polygon
+  let xf1: TransformValue;
+  let xf2: TransformValue;
+  let edge1: number; // reference edge
+  let flip: boolean;
+  const k_tol = 0.1 * Settings.linearSlop;
+
+  if (separationB > separationA + k_tol) {
+    poly1 = polyB;
+    poly2 = polyA;
+    xf1 = xfB;
+    xf2 = xfA;
+    edge1 = edgeB;
+    manifold.type = ManifoldType.e_faceB;
+    flip = true;
+  } else {
+    poly1 = polyA;
+    poly2 = polyB;
+    xf1 = xfA;
+    xf2 = xfB;
+    edge1 = edgeA;
+    manifold.type = ManifoldType.e_faceA;
+    flip = false;
+  }
+
+  incidentEdge[0].recycle();
+  incidentEdge[1].recycle();
+  findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+  const count1 = poly1.m_count;
+  const vertices1 = poly1.m_vertices;
+
+  const iv1 = edge1;
+  const iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
+
+  geo.copyVec2(v11, vertices1[iv1]);
+  geo.copyVec2(v12, vertices1[iv2]);
+
+  geo.subVec2(localTangent, v12, v11);
+  geo.normalizeVec2(localTangent);
+
+  geo.crossVec2Num(localNormal, localTangent, 1.0);
+  geo.combine2Vec2(planePoint, 0.5, v11, 0.5, v12);
+
+  geo.rotVec2(tangent, xf1.q, localTangent);
+  geo.crossVec2Num(normal, tangent, 1.0);
+
+  geo.transformVec2(v11, xf1, v11);
+  geo.transformVec2(v12, xf1, v12);
+
+  // Face offset.
+  const frontOffset = geo.dotVec2(normal, v11);
+
+  // Side offsets, extended by polytope skin thickness.
+  const sideOffset1 = -geo.dotVec2(tangent, v11) + totalRadius;
+  const sideOffset2 = geo.dotVec2(tangent, v12) + totalRadius;
+
+  // Clip incident edge against extruded edge1 side edges.
+  clipPoints1[0].recycle();
+  clipPoints1[1].recycle();
+  clipPoints2[0].recycle();
+  clipPoints2[1].recycle();
+
+  // Clip to box side 1
+  geo.setVec2(clipSegmentToLineNormal, -tangent.x, -tangent.y);
+  const np1 = clipSegmentToLine(clipPoints1, incidentEdge, clipSegmentToLineNormal, sideOffset1, iv1);
+
+  if (np1 < 2) {
+    return;
+  }
+
+  // Clip to negative box side 1
+  geo.setVec2(clipSegmentToLineNormal, tangent.x, tangent.y);
+  const np2 = clipSegmentToLine(clipPoints2, clipPoints1, clipSegmentToLineNormal, sideOffset2, iv2);
+
+  if (np2 < 2) {
+    return;
+  }
+
+  // Now clipPoints2 contains the clipped points.
+  geo.copyVec2(manifold.localNormal, localNormal);
+  geo.copyVec2(manifold.localPoint, planePoint);
+
+  let pointCount = 0;
+  for (let i = 0; i < clipPoints2.length /* maxManifoldPoints */; ++i) {
+    const separation = geo.dotVec2(normal, clipPoints2[i].v) - frontOffset;
+
+    if (separation <= totalRadius) {
+      const cp = manifold.points[pointCount];
+      geo.detransformVec2(cp.localPoint, xf2, clipPoints2[i].v);
+      cp.id.set(clipPoints2[i].id);
+      if (flip) {
+        // Swap features
+        cp.id.swapFeatures();
+      }
+      ++pointCount;
+    }
+  }
+
+  manifold.pointCount = pointCount;
+};

package/src/collision/shape/EdgeShape.ts

@@ -0,0 +1,316 @@
+/*
+ * Planck.js
+ *
+ * Copyright (c) Erin Catto, Ali Shakiba
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+import { SettingsInternal as Settings } from "../../Settings";
+import * as geo from "../../common/Geo";
+import { Shape } from "../Shape";
+import { TransformValue } from "../../common/Transform";
+import { Vec2Value } from "../../common/Vec2";
+import { AABB, AABBValue } from "../AABB";
+import { RayCastInput, RayCastOutput } from "../Raycast";
+import { MassData } from "../../dynamics/Body";
+import { DistanceProxy } from "../Distance";
+
+/** @internal */ const v1 = geo.vec2(0, 0);
+/** @internal */ const v2 = geo.vec2(0, 0);
+
+/**
+ * A line segment (edge) shape. These can be connected in chains or loops to
+ * other edge shapes. The connectivity information is used to ensure correct
+ * contact normals.
+ */
+export class EdgeShape extends Shape {
+  static TYPE = "edge" as const;
+  /** @hidden */ m_type: "edge";
+
+  /** @hidden */ m_radius: number;
+
+  // These are the edge vertices
+  /** @hidden */ m_vertex1: Vec2Value;
+  /** @hidden */ m_vertex2: Vec2Value;
+
+  // Optional adjacent vertices. These are used for smooth collision.
+  // Used by chain shape.
+  /** @hidden */ m_vertex0: Vec2Value;
+  /** @hidden */ m_vertex3: Vec2Value;
+  /** @hidden */ m_hasVertex0: boolean;
+  /** @hidden */ m_hasVertex3: boolean;
+
+  constructor(v1?: Vec2Value, v2?: Vec2Value) {
+    super();
+
+    this.m_type = EdgeShape.TYPE;
+    this.m_radius = Settings.polygonRadius;
+
+    this.m_vertex1 = geo.vec2(0, 0);
+    if (v1) {
+      geo.copyVec2(this.m_vertex1, v1);
+    }
+
+    this.m_vertex2 = geo.vec2(0, 0);
+    if (v2) {
+      geo.copyVec2(this.m_vertex2, v2);
+    }
+
+    this.m_vertex0 = geo.vec2(0, 0);
+    this.m_vertex3 = geo.vec2(0, 0);
+    this.m_hasVertex0 = false;
+    this.m_hasVertex3 = false;
+  }
+
+  /** @hidden */
+  _serialize(): object {
+    return {
+      type: this.m_type,
+
+      vertex1: this.m_vertex1,
+      vertex2: this.m_vertex2,
+
+      vertex0: this.m_vertex0,
+      vertex3: this.m_vertex3,
+      hasVertex0: this.m_hasVertex0,
+      hasVertex3: this.m_hasVertex3,
+    };
+  }
+
+  /** @hidden */
+  static _deserialize(data: any): EdgeShape {
+    const shape = new EdgeShape(data.vertex1, data.vertex2);
+    if (shape.m_hasVertex0) {
+      shape.setPrevVertex(data.vertex0);
+    }
+    if (shape.m_hasVertex3) {
+      shape.setNextVertex(data.vertex3);
+    }
+    return shape;
+  }
+
+  /** @hidden */
+  _reset(): void {
+    // noop
+  }
+
+  getRadius(): number {
+    return this.m_radius;
+  }
+
+  getType(): "edge" {
+    return this.m_type;
+  }
+
+  /** @internal @deprecated */
+  setNext(v?: Vec2Value): EdgeShape {
+    return this.setNextVertex(v);
+  }
+
+  /**
+   * Optional next vertex, used for smooth collision.
+   */
+  setNextVertex(v?: Vec2Value): EdgeShape {
+    if (v) {
+      geo.copyVec2(this.m_vertex3, v);
+      this.m_hasVertex3 = true;
+    } else {
+      geo.zeroVec2(this.m_vertex3);
+      this.m_hasVertex3 = false;
+    }
+    return this;
+  }
+
+  /**
+   * Optional next vertex, used for smooth collision.
+   */
+  getNextVertex(): Vec2Value {
+    return this.m_vertex3;
+  }
+
+  /** @internal @deprecated */
+  setPrev(v?: Vec2Value): EdgeShape {
+    return this.setPrevVertex(v);
+  }
+
+  /**
+   * Optional prev vertex, used for smooth collision.
+   */
+  setPrevVertex(v?: Vec2Value): EdgeShape {
+    if (v) {
+      geo.copyVec2(this.m_vertex0, v);
+      this.m_hasVertex0 = true;
+    } else {
+      geo.zeroVec2(this.m_vertex0);
+      this.m_hasVertex0 = false;
+    }
+    return this;
+  }
+
+  /**
+   * Optional prev vertex, used for smooth collision.
+   */
+  getPrevVertex(): Vec2Value {
+    return this.m_vertex0;
+  }
+
+  /**
+   * Set this as an isolated edge.
+   */
+  _set(v1: Vec2Value, v2: Vec2Value): EdgeShape {
+    geo.copyVec2(this.m_vertex1, v1);
+    geo.copyVec2(this.m_vertex2, v2);
+    this.m_hasVertex0 = false;
+    this.m_hasVertex3 = false;
+    return this;
+  }
+
+  /**
+   * @internal @deprecated Shapes should be treated as immutable.
+   *
+   * clone the concrete shape.
+   */
+  _clone(): EdgeShape {
+    const clone = new EdgeShape();
+    clone.m_type = this.m_type;
+    clone.m_radius = this.m_radius;
+    geo.copyVec2(clone.m_vertex1, this.m_vertex1);
+    geo.copyVec2(clone.m_vertex2, this.m_vertex2);
+    geo.copyVec2(clone.m_vertex0, this.m_vertex0);
+    geo.copyVec2(clone.m_vertex3, this.m_vertex3);
+    clone.m_hasVertex0 = this.m_hasVertex0;
+    clone.m_hasVertex3 = this.m_hasVertex3;
+    return clone;
+  }
+
+  /**
+   * Get the number of child primitives.
+   */
+  getChildCount(): 1 {
+    return 1;
+  }
+
+  /**
+   * Test a point for containment in this shape. This only works for convex
+   * shapes.
+   *
+   * @param xf The shape world transform.
+   * @param p A point in world coordinates.
+   */
+  testPoint(xf: TransformValue, p: Vec2Value): false {
+    return false;
+  }
+
+  /**
+   * Cast a ray against a child shape.
+   *
+   * @param output The ray-cast results.
+   * @param input The ray-cast input parameters.
+   * @param xf The transform to be applied to the shape.
+   * @param childIndex The child shape index
+   */
+  rayCast(output: RayCastOutput, input: RayCastInput, xf: TransformValue, childIndex: number): boolean {
+    // p = p1 + t * d
+    // v = v1 + s * e
+    // p1 + t * d = v1 + s * e
+    // s * e - t * d = p1 - v1
+
+    // NOT_USED(childIndex);
+
+    // Put the ray into the edge's frame of reference.
+    const p1 = geo.vec2(0, 0);
+    geo.detransformVec2(p1, xf, input.p1);
+    const p2 = geo.vec2(0, 0);
+    geo.detransformVec2(p2, xf, input.p2);
+    const d = geo.vec2(0, 0);
+    geo.subVec2(d, p2, p1);
+
+    const v1 = this.m_vertex1;
+    const v2 = this.m_vertex2;
+    const e = geo.vec2(0, 0);
+    geo.subVec2(e, v2, v1);
+    const normal = geo.vec2(e.y, -e.x);
+    geo.normalizeVec2(normal);
+
+    // q = p1 + t * d
+    // dot(normal, q - v1) = 0
+    // dot(normal, p1 - v1) + t * dot(normal, d) = 0
+    const numerator = geo.dotSubVec2(v1, p1, normal);
+    const denominator = geo.dotVec2(normal, d);
+
+    if (denominator == 0.0) {
+      return false;
+    }
+
+    const t = numerator / denominator;
+    if (t < 0.0 || input.maxFraction < t) {
+      return false;
+    }
+
+    const q = geo.vec2(0, 0);
+    geo.combine2Vec2(q, 1, p1, t, d);
+
+    // q = v1 + s * r
+    // s = dot(q - v1, r) / dot(r, r)
+    const r = geo.vec2(0, 0);
+    geo.subVec2(r, v2, v1);
+    const rr = geo.dotVec2(r, r);
+    if (rr == 0.0) {
+      return false;
+    }
+
+    const s = geo.dotSubVec2(q, v1, r) / rr;
+    if (s < 0.0 || 1.0 < s) {
+      return false;
+    }
+
+    output.fraction = t;
+    output.normal = geo.vec2(0, 0);
+    geo.rotVec2(output.normal, xf.q, normal);
+    if (numerator > 0.0) {
+      geo.negVec2(output.normal);
+    }
+    return true;
+  }
+
+  /**
+   * Given a transform, compute the associated axis aligned bounding box for a
+   * child shape.
+   *
+   * @param aabb Returns the axis aligned box.
+   * @param xf The world transform of the shape.
+   * @param childIndex The child shape
+   */
+  computeAABB(aabb: AABBValue, xf: TransformValue, childIndex: number): void {
+    geo.transformVec2(v1, xf, this.m_vertex1);
+    geo.transformVec2(v2, xf, this.m_vertex2);
+
+    AABB.combinePoints(aabb, v1, v2);
+    AABB.extend(aabb, this.m_radius);
+  }
+
+  /**
+   * Compute the mass properties of this shape using its dimensions and density.
+   * The inertia tensor is computed about the local origin.
+   *
+   * @param massData Returns the mass data for this shape.
+   * @param density The density in kilograms per meter squared.
+   */
+  computeMass(massData: MassData, density?: number): void {
+    massData.mass = 0.0;
+    geo.combine2Vec2(massData.center, 0.5, this.m_vertex1, 0.5, this.m_vertex2);
+    massData.I = 0.0;
+  }
+
+  computeDistanceProxy(proxy: DistanceProxy): void {
+    proxy.m_vertices[0] = this.m_vertex1;
+    proxy.m_vertices[1] = this.m_vertex2;
+    proxy.m_vertices.length = 2;
+    proxy.m_count = 2;
+    proxy.m_radius = this.m_radius;
+  }
+}
+
+export { EdgeShape as Edge };