@perplexdotgg/bounce 1.0.0

package/src/shape/Cylinder.ts

@@ -0,0 +1,313 @@
+import {
+  createClass,
+  LazyReferenceType,
+  MonomorphType,
+  NumberType,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+} from "monomorph";
+import { squared } from "../math/scalar";
+import { Vec3 } from "../math/vec3";
+import { Face } from "../physics/manifold/Face";
+import { Mat4 } from "../math/mat4";
+import { Aabb } from "./Aabb";
+import { ConvexShapeInterface, ShapeType } from "./Shape";
+import type { World } from "../world";
+import { Isometry } from "../math/isometry";
+import { SupportMode, SupportShapeWithConvexRadius } from "./Convex";
+import { Mat3 } from "../math/mat3";
+import { Quat } from "../math/quat";
+
+// Approximation of top face with 8 vertices
+const topFace: [number, number, number][] = [
+  [0.0, 1.0, 1.0],
+  [Math.sin(Math.PI / 4), 1.0, Math.sin(Math.PI / 4)],
+  [1.0, 1.0, 0.0],
+  [Math.sin(Math.PI / 4), 1.0, -Math.sin(Math.PI / 4)],
+  [0.0, 1.0, -1.0],
+  [-Math.sin(Math.PI / 4), 1.0, -Math.sin(Math.PI / 4)],
+  [-1.0, 1.0, 0.0],
+  [-Math.sin(Math.PI / 4), 1.0, Math.sin(Math.PI / 4)],
+];
+
+// Approximation of bottom face with 8 vertices
+const bottomFace: [number, number, number][] = [
+  [0.0, -1.0, 1.0],
+  [-Math.sin(Math.PI / 4), -1.0, Math.sin(Math.PI / 4)],
+  [-1.0, -1.0, 0.0],
+  [-Math.sin(Math.PI / 4), -1.0, -Math.sin(Math.PI / 4)],
+  [0.0, -1.0, -1.0],
+  [Math.sin(Math.PI / 4), -1.0, -Math.sin(Math.PI / 4)],
+  [1.0, -1.0, 0.0],
+  [Math.sin(Math.PI / 4), -1.0, Math.sin(Math.PI / 4)],
+];
+
+const local_surface_position_xz = /*@__PURE__*/ Vec3.create();
+const abs_scale = /*@__PURE__*/ Vec3.create();
+const vertex = /*@__PURE__*/ Vec3.create();
+const multiplier = /*@__PURE__*/ Vec3.create();
+const transform = /*@__PURE__*/ Mat4.create();
+const isometry = /*@__PURE__*/ Isometry.create();
+
+const cylinderSupportShapeProps = {
+  halfHeight: NumberType(0),
+  radius: NumberType(0),
+  convexRadius: NumberType(0),
+} as const satisfies PropertyDefinitionMap;
+
+export class CylinderSupportShape extends createClass<CylinderSupportShape, typeof cylinderSupportShapeProps>(
+  cylinderSupportShapeProps
+) {
+  getConvexRadius(): number {
+    return this.convexRadius;
+  }
+
+  computeSupport(out: Vec3, direction: Vec3): void {
+    // Support mapping, taken from:
+    // A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen
+    // page 8
+    const x = direction.x;
+    const y = direction.y;
+    const z = direction.z;
+    const o = Math.sqrt(squared(x) + squared(z));
+
+    if (o > 0.0) {
+      out.x = (this.radius * x) / o;
+      out.y = Math.sign(y) * this.halfHeight;
+      out.z = (this.radius * z) / o;
+    } else {
+      out.x = 0;
+      out.y = Math.sign(y) * this.halfHeight;
+      out.z = 0;
+    }
+  }
+
+  computeSupportingFace(out: Face, subShapeID: number, direction: Vec3, scale: number, centerOfMassTransform: Mat4) {
+    throw new Error("Method not implemented.");
+  }
+}
+
+const cylinderProps = {
+  computedCenterOfMass: MonomorphType(Vec3, undefined, true),
+  computedVolume: NumberType(0.0, true),
+  computedAabb: MonomorphType(Aabb, undefined, true),
+  radius: NumberType(0),
+  halfHeight: NumberType(0),
+  convexRadius: NumberType(0),
+  translation: MonomorphType(Vec3, undefined, true),
+  supportShape: MonomorphType(CylinderSupportShape, undefined, true),
+  copyForDiff: LazyReferenceType((() => Cylinder) as () => never) as PropertyDefinitionReference<Cylinder | null, true>,
+} as const satisfies PropertyDefinitionMap;
+
+const afterConstructorCode = `
+  this.world = null;
+`;
+
+export class Cylinder
+  extends createClass<Cylinder, typeof cylinderProps>(cylinderProps, { afterConstructorCode })
+  implements ConvexShapeInterface
+{
+  type: ShapeType.cylinder = ShapeType.cylinder;
+  declare world: World | null;
+
+  intersectsAabb(aabb: Aabb): boolean {
+    throw new Error("Method not implemented.");
+  }
+
+  computeSupport(out: Vec3, direction: Vec3): void {
+    throw new Error("Method not implemented.");
+  }
+
+  getConvexRadius(): number {
+    return this.convexRadius;
+  }
+
+  computeSurfaceNormal(out: Vec3, inLocalSurfacePosition: Vec3, subShapeId?: number): void {
+    // Calculate distance to infinite cylinder surface
+
+    local_surface_position_xz.fromArray([inLocalSurfacePosition.x, 0, inLocalSurfacePosition.z]);
+    const local_surface_position_xz_len = local_surface_position_xz.length();
+    const distance_to_curved_surface = Math.abs(local_surface_position_xz_len - this.radius);
+
+    // Calculate distance to top or bottom plane
+    const distance_to_top_or_bottom = Math.abs(Math.abs(inLocalSurfacePosition.y) - this.halfHeight);
+
+    // Return normal according to closest surface
+    if (distance_to_curved_surface < distance_to_top_or_bottom) {
+      out.copy(local_surface_position_xz).scale(1 / local_surface_position_xz_len);
+    } else {
+      out.fromArray([0, inLocalSurfacePosition.y > 0 ? +1 : -1, 0]);
+    }
+  }
+
+  computeSupportShape(mode: SupportMode, scale: number): SupportShapeWithConvexRadius {
+    abs_scale.replicate(Math.abs(scale));
+    const scale_xz = abs_scale.x;
+    const scale_y = abs_scale.y;
+    const scaled_half_height = scale_y * this.halfHeight;
+    const scaled_radius = scale_xz * this.radius;
+    const scaled_convex_radius = this.convexRadius * scale;
+
+    const supportShape = this.supportShape;
+
+    switch (mode) {
+      case SupportMode.IncludeConvexRadius: {
+        supportShape.set({ halfHeight: scaled_half_height, radius: scaled_radius, convexRadius: 0.0 });
+        break;
+      }
+
+      case SupportMode.ExcludeConvexRadius: {
+        supportShape.set({
+          halfHeight: scaled_half_height - scaled_convex_radius,
+          radius: scaled_radius - scaled_convex_radius,
+          convexRadius: scaled_convex_radius,
+        });
+        break;
+      }
+
+      default: {
+        throw new Error("Invalid mode");
+      }
+    }
+
+    return supportShape;
+  }
+
+  computeSupportingFace(
+    out: Face,
+    subShapeID: number,
+    direction: Vec3,
+    scale: number,
+    centerOfMassTransform: Mat4
+  ): void {
+    out.clear();
+
+    // Get scaled cylinder
+
+    abs_scale.replicate(Math.abs(scale));
+    const scale_xz = abs_scale.x;
+    const scale_y = abs_scale.y;
+    const scaled_half_height = scale_y * this.halfHeight;
+    const scaled_radius = scale_xz * this.radius;
+
+    const x = direction.x;
+    const y = direction.y;
+    const z = direction.z;
+    const o = Math.sqrt(squared(x) + squared(z));
+
+    // If o / |y| > scaled_radius / scaled_half_height, we're hitting the side
+    if (o * scaled_half_height > scaled_radius * Math.abs(y)) {
+      // Hitting side
+      const f = -scaled_radius / o;
+      const vx = x * f;
+      const vz = z * f;
+
+      vertex.fromArray([vx, +scaled_half_height, vz]);
+      out.pushVertex(vertex);
+
+      vertex.fromArray([vx, -scaled_half_height, vz]);
+      out.pushVertex(vertex);
+
+      out.transform(centerOfMassTransform);
+      // out.log("hitting side, cylinder supporting face");
+    } else {
+      // // Hitting top or bottom
+
+      multiplier.fromArray([+scaled_radius, +scaled_half_height, +scaled_radius]);
+      // if (y < 0) {
+      //   multiplier.fromArray([+scaled_radius, +scaled_half_height, +scaled_radius]);
+      // } else {
+      //   multiplier.fromArray([-scaled_radius, -scaled_half_height, -scaled_radius]);
+      // }
+
+      const theFace = y < 0 ? topFace : bottomFace;
+
+      for (const topFaceVertex of theFace) {
+        vertex.fromArray(topFaceVertex);
+        out.pushVertex(vertex);
+      }
+
+      transform.copy(centerOfMassTransform).preScaled(multiplier);
+      out.transform(transform);
+      // out.log("hitting top or bottom, cylinder supporting face");
+    }
+
+    // console.log("cylinder supporting face with vertex count", out.vertexCount);
+  }
+
+  computeInertiaTensor(out: Mat3, mass: number): void {
+    const radiusSquared = squared(this.radius);
+    const height = this.halfHeight * 2;
+    const yyInertia = radiusSquared * mass * 0.5;
+    const xxInertia = yyInertia * 0.5 + (mass * height * height) / 12;
+    const zzInertia = xxInertia;
+    out.fromArray([xxInertia, 0, 0, 0, yyInertia, 0, 0, 0, zzInertia]);
+  }
+
+  computeInverseInertiaTensor(out: Mat3, mass: number): void {
+    this.computeInertiaTensor(out, mass);
+    out.invert();
+  }
+
+  computeWorldBounds(out: Aabb, translation: Vec3, rotation: Quat): void {
+    // TODO: apply rotation if shape's center of mass is not at the origin?
+    isometry.fromRotationAndTranslation(rotation, translation);
+    out.transformAabb(this.computedAabb, isometry);
+  }
+
+  get isConvex(): boolean {
+    return true;
+  }
+
+  hasChanged() {
+    return (
+      this.copyForDiff !== null &&
+      (this.radius !== this.copyForDiff.radius || this.halfHeight !== this.copyForDiff.halfHeight)
+    );
+  }
+
+  commitChanges() {
+    if (this.hasChanged()) {
+      updateShape(this);
+      // this.world?.updateBodyProperties();
+    }
+  }
+}
+
+const oldCreate = Cylinder.create;
+Cylinder.create = function () {
+  const shape = oldCreate.apply(this, arguments as any);
+  updateShape(shape);
+  return shape;
+};
+
+function updateVolume(shape: Cylinder) {
+  shape.computedVolume = Math.PI * squared(shape.radius) * shape.halfHeight * 2;
+}
+
+function updateLocalBounds(shape: Cylinder) {
+  shape.computedAabb.min.x = -shape.radius;
+  shape.computedAabb.min.y = -shape.halfHeight;
+  shape.computedAabb.min.z = -shape.radius;
+  shape.computedAabb.max.x = +shape.radius;
+  shape.computedAabb.max.y = +shape.halfHeight;
+  shape.computedAabb.max.z = +shape.radius;
+}
+
+function updateCopyForDiff(shape: Cylinder) {
+  if (shape.copyForDiff) {
+    shape.copyForDiff.copy(shape);
+    shape.copyForDiff.copyForDiff = null;
+  }
+}
+
+function updateCenterOfMass(shape: Cylinder) {
+  shape.computedCenterOfMass.zero();
+}
+
+function updateShape(shape: Cylinder) {
+  updateCopyForDiff(shape);
+  updateCenterOfMass(shape);
+  updateVolume(shape);
+  updateLocalBounds(shape);
+}