packages/core/src/features/physics/lib/2d/shapes.ts
This is the source snapshot used to build these API details. View this revision on GitHub.
1 import type {
2 PibblPhysicsAabb2D,
3 PibblPhysicsMassProperties2D,
4 PibblPhysicsPose2D,
5 PibblPhysicsShape2D,
6 PibblPhysicsTransform2D,
7 PibblPhysicsVector2,
8 } from '../../2d-geometry.js';
9 import {
10 copyFiniteVector2,
11 requireFiniteNumber,
12 requireNonnegativeNumber,
13 requirePositiveNumber,
14 } from '../shared/validation.js';
15 import {
16 CURVE_FLATTENING_MAX_DEPTH,
17 CURVE_FLATTENING_TOLERANCE,
18 IDENTITY_POSE_2D,
19 canonicalizeContour2D,
20 cleanZero,
21 composePose2D,
22 copyPose2D,
23 freezeBounds2D,
24 polygonMassData2D,
25 transformPointInto,
26 triangulateContour2D,
27 type InternalPose2D,
28 } from './math.js';
29 import { DEGREES_TO_RADIANS, GEOMETRY_EPSILON } from './numeric-policy.js';
30 import {
31 createShape2D,
32 requireShapeRecord2D,
33 type CapsuleLeaf2D,
34 type ConvexLeaf2D,
35 type PolygonLeaf2D,
36 } from './shape-records.js';
37
38 const FULL_TURN_RADIANS = Math.PI * 2;
39
40 /**
41 * Creates an immutable circle collision shape without initializing a physics world.
42 *
43 * @param input - Circle radius and optional center. See {@link PibblPhysicsVector2}.
44 * @returns An immutable circle collision shape. See {@link PibblPhysicsShape2D}.
45 *
46 * @see {@link PibblPhysicsVector2}
47 * @see {@link PibblPhysicsShape2D}
48 */
49 export function circleShape2D(
50 input: Readonly<{ radius: number; center?: PibblPhysicsVector2 }>,
51 ): PibblPhysicsShape2D {
52 const radius = requirePositiveNumber(input.radius, 'circle.radius');
53 const center = ownedVector(input.center ?? [0, 0], 'circle.center');
54 const area = Math.PI * radius * radius;
55 return createShapeFromLeaves(
56 [Object.freeze({ kind: 'circle', center, radius, canonicalLeafIndex: 0 })],
57 area,
58 center[0]!,
59 center[1]!,
60 area * radius * radius / 2,
61 );
62 }
63
64 /**
65 * Creates an immutable ellipse collision shape without initializing a physics world.
66 *
67 * @param input - Ellipse radii, optional center, and rotation in degrees. See
68 * {@link PibblPhysicsVector2} .
69 * @returns An immutable ellipse collision shape. See {@link PibblPhysicsShape2D}.
70 *
71 * @see {@link PibblPhysicsVector2}
72 * @see {@link PibblPhysicsShape2D}
73 */
74 export function ellipseShape2D(
75 input: Readonly<{
76 radiusX: number;
77 radiusY: number;
78 center?: PibblPhysicsVector2;
79 rotationDegrees?: number;
80 }>,
81 ): PibblPhysicsShape2D {
82 const radiusX = requirePositiveNumber(input.radiusX, 'ellipse.radiusX');
83 const radiusY = requirePositiveNumber(input.radiusY, 'ellipse.radiusY');
84 const center = ownedVector(input.center ?? [0, 0], 'ellipse.center');
85 const rotationRadians =
86 requireFiniteNumber(input.rotationDegrees ?? 0, 'ellipse.rotationDegrees') *
87 DEGREES_TO_RADIANS;
88 const area = Math.PI * radiusX * radiusY;
89 return createShapeFromLeaves(
90 [Object.freeze({ kind: 'ellipse', center, radiusX, radiusY, rotationRadians, canonicalLeafIndex: 0 })],
91 area,
92 center[0]!,
93 center[1]!,
94 area * (radiusX * radiusX + radiusY * radiusY) / 4,
95 );
96 }
97
98 /**
99 * Creates an immutable box collision shape without initializing a physics world.
100 *
101 * @param input - Box width, height, and optional center. See {@link PibblPhysicsVector2}.
102 * @returns An immutable box collision shape. See {@link PibblPhysicsShape2D}.
103 *
104 * @see {@link PibblPhysicsVector2}
105 * @see {@link PibblPhysicsShape2D}
106 */
107 export function boxShape2D(
108 input: Readonly<{
109 width: number;
110 height: number;
111 center?: PibblPhysicsVector2;
112 }>,
113 ): PibblPhysicsShape2D {
114 const width = requirePositiveNumber(input.width, 'box.width');
115 const height = requirePositiveNumber(input.height, 'box.height');
116 const center = ownedVector(input.center ?? [0, 0], 'box.center');
117 const area = width * height;
118 return createShapeFromLeaves(
119 [Object.freeze({ kind: 'box', center, halfWidth: width / 2, halfHeight: height / 2, canonicalLeafIndex: 0 })],
120 area,
121 center[0]!,
122 center[1]!,
123 area * (width * width + height * height) / 12,
124 );
125 }
126
127 /**
128 * Creates an immutable rounded box collision shape without initializing a physics world.
129 *
130 * @param input - Box dimensions, center, and uniform or per-corner radii. See
131 * {@link PibblPhysicsVector2} .
132 * @returns An immutable rounded-box collision shape. See {@link PibblPhysicsShape2D}.
133 *
134 * @see {@link PibblPhysicsVector2}
135 * @see {@link PibblPhysicsShape2D}
136 */
137 export function roundedBoxShape2D(
138 input: Readonly<{
139 width: number;
140 height: number;
141 radius:
142 | number
143 | readonly [
144 topLeft: number,
145 topRight: number,
146 bottomRight: number,
147 bottomLeft: number,
148 ];
149 center?: PibblPhysicsVector2;
150 }>,
151 ): PibblPhysicsShape2D {
152 const width = requirePositiveNumber(input.width, 'roundedBox.width');
153 const height = requirePositiveNumber(input.height, 'roundedBox.height');
154 const center = ownedVector(input.center ?? [0, 0], 'roundedBox.center');
155 const sourceRadii = typeof input.radius === 'number'
156 ? [input.radius, input.radius, input.radius, input.radius] as const
157 : input.radius;
158 const radii = new Float64Array(4);
159 const maximum = Math.min(width, height) / 2;
160 for (let index = 0; index < radii.length; index += 1) {
161 const radius = requireNonnegativeNumber(
162 sourceRadii[index]!,
163 `roundedBox.radius[${index}]`,
164 );
165 if (radius > maximum) {
166 throw new RangeError(
167 `roundedBox.radius[${index}] must not exceed ${String(maximum)}; received ${String(radius)}.`,
168 );
169 }
170 radii[index] = radius;
171 }
172 const contour = roundedBoxContour(width / 2, height / 2, radii, center);
173 const mass = polygonMassData2D(contour);
174 return createShapeFromLeaves(
175 [Object.freeze({
176 kind: 'rounded-box',
177 center,
178 halfWidth: width / 2,
179 halfHeight: height / 2,
180 radii,
181 canonicalLeafIndex: 0,
182 })],
183 mass.area,
184 mass.centroidX,
185 mass.centroidY,
186 mass.unitInertia,
187 );
188 }
189
190 /**
191 * Creates an immutable capsule collision shape without initializing a physics world.
192 *
193 * @param input - Capsule centerline endpoints and radius. See {@link PibblPhysicsVector2}.
194 * @returns An immutable capsule collision shape. See {@link PibblPhysicsShape2D}.
195 *
196 * @see {@link PibblPhysicsVector2}
197 * @see {@link PibblPhysicsShape2D}
198 */
199 export function capsuleShape2D(
200 input: Readonly<{
201 start: PibblPhysicsVector2;
202 end: PibblPhysicsVector2;
203 radius: number;
204 }>,
205 ): PibblPhysicsShape2D {
206 const start = ownedVector(input.start, 'capsule.start');
207 const end = ownedVector(input.end, 'capsule.end');
208 const radius = requirePositiveNumber(input.radius, 'capsule.radius');
209 const leaf: CapsuleLeaf2D = Object.freeze({
210 kind: 'capsule', start, end, radius, canonicalLeafIndex: 0,
211 });
212 const mass = capsuleMassData(start, end, radius);
213 return createShapeFromLeaves(
214 [leaf],
215 mass.area,
216 mass.centroidX,
217 mass.centroidY,
218 mass.unitInertia,
219 );
220 }
221
222 /**
223 * Creates an immutable arc collision shape without initializing a physics world.
224 *
225 * @param input - Arc center, radius, angular endpoints in radians, sweep direction, and thickness.
226 * See {@link PibblPhysicsVector2} .
227 * @returns An immutable thick-arc collision shape. See {@link PibblPhysicsShape2D}.
228 *
229 * @see {@link PibblPhysicsVector2}
230 * @see {@link PibblPhysicsShape2D}
231 */
232 export function arcShape2D(
233 input: Readonly<{
234 center?: PibblPhysicsVector2;
235 radius: number;
236 startAngleRadians: number;
237 endAngleRadians: number;
238 counterclockwise?: boolean;
239 thickness: number;
240 }>,
241 ): PibblPhysicsShape2D {
242 const center = ownedVector(input.center ?? [0, 0], 'arc.center');
243 const radius = requirePositiveNumber(input.radius, 'arc.radius');
244 const startAngle = requireFiniteNumber(input.startAngleRadians, 'arc.startAngleRadians');
245 const endAngle = requireFiniteNumber(input.endAngleRadians, 'arc.endAngleRadians');
246 const thickness = requirePositiveNumber(input.thickness, 'arc.thickness');
247 const points = flattenCircularSweep(
248 center[0]!, center[1]!, radius, startAngle, endAngle, input.counterclockwise === true,
249 );
250 return chainFromOwnedPoints(points, false, thickness / 2);
251 }
252
253 /**
254 * Creates an immutable wedge collision shape without initializing a physics world.
255 *
256 * @param input - Wedge center, radius, angular endpoints in radians, and sweep direction. See
257 * {@link PibblPhysicsVector2} .
258 * @returns An immutable wedge collision shape. See {@link PibblPhysicsShape2D}.
259 *
260 * @see {@link PibblPhysicsVector2}
261 * @see {@link PibblPhysicsShape2D}
262 */
263 export function wedgeShape2D(
264 input: Readonly<{
265 center?: PibblPhysicsVector2;
266 radius: number;
267 startAngleRadians: number;
268 endAngleRadians: number;
269 counterclockwise?: boolean;
270 }>,
271 ): PibblPhysicsShape2D {
272 const center = ownedVector(input.center ?? [0, 0], 'wedge.center');
273 const radius = requirePositiveNumber(input.radius, 'wedge.radius');
274 const startAngle = requireFiniteNumber(input.startAngleRadians, 'wedge.startAngleRadians');
275 const endAngle = requireFiniteNumber(input.endAngleRadians, 'wedge.endAngleRadians');
276 const points = flattenCircularSweep(
277 center[0]!, center[1]!, radius, startAngle, endAngle, input.counterclockwise === true,
278 );
279 const sweep = directedSweep(startAngle, endAngle, input.counterclockwise === true);
280 if (Math.abs(sweep) <= GEOMETRY_EPSILON) {
281 throw new RangeError('wedge angles must describe a nonzero sweep.');
282 }
283 const contourSource: PibblPhysicsVector2[] = Math.abs(sweep) >= FULL_TURN_RADIANS
284 ? points.slice(0, -1).map(asReadonlyVector)
285 : [asReadonlyVector(center), ...points.map(asReadonlyVector)];
286 const contour = canonicalizeContour2D(contourSource, {
287 strictlyConvex: false,
288 path: 'wedge.contour',
289 });
290 const triangles = triangulateContour2D(contour);
291 const leaves = triangles.map<PolygonLeaf2D>((vertices, canonicalLeafIndex) =>
292 Object.freeze({ kind: 'polygon', vertices, canonicalLeafIndex }),
293 );
294 const mass = aggregateLeafMass(leaves);
295 return createShapeFromLeaves(
296 leaves,
297 mass.area,
298 mass.centroidX,
299 mass.centroidY,
300 mass.unitInertia,
301 );
302 }
303
304 /**
305 * Creates an immutable segment collision shape without initializing a physics world.
306 *
307 * @param input - Segment endpoints. See {@link PibblPhysicsVector2}.
308 * @returns An immutable line-segment collision shape. See {@link PibblPhysicsShape2D}.
309 *
310 * @see {@link PibblPhysicsVector2}
311 * @see {@link PibblPhysicsShape2D}
312 */
313 export function segmentShape2D(
314 input: Readonly<{ start: PibblPhysicsVector2; end: PibblPhysicsVector2 }>,
315 ): PibblPhysicsShape2D {
316 const start = ownedVector(input.start, 'segment.start');
317 const end = ownedVector(input.end, 'segment.end');
318 return createShapeFromLeaves(
319 [Object.freeze({ kind: 'segment', start, end, canonicalLeafIndex: 0 })],
320 0,
321 (start[0]! + end[0]!) / 2,
322 (start[1]! + end[1]!) / 2,
323 0,
324 );
325 }
326
327 /**
328 * Creates an immutable chain collision shape without initializing a physics world.
329 *
330 * @param input - Ordered chain points, closure policy, and optional radius. See
331 * {@link PibblPhysicsVector2} .
332 * @returns An immutable chain collision shape. See {@link PibblPhysicsShape2D}.
333 *
334 * @see {@link PibblPhysicsVector2}
335 * @see {@link PibblPhysicsShape2D}
336 */
337 export function chainShape2D(
338 input: Readonly<{
339 points: readonly PibblPhysicsVector2[];
340 closed?: boolean;
341 radius?: number;
342 }>,
343 ): PibblPhysicsShape2D {
344 if (input.points.length < 2) {
345 throw new RangeError('chain.points must contain at least two points.');
346 }
347 const points = input.points.map((point, index) =>
348 ownedVector(point, `chain.points[${index}]`),
349 );
350 const radius = requireNonnegativeNumber(input.radius ?? 0, 'chain.radius');
351 return chainFromOwnedPoints(points, input.closed === true, radius);
352 }
353
354 /**
355 * Creates an immutable polygon collision shape without initializing a physics world.
356 *
357 * @param input - Ordered polygon vertices. See {@link PibblPhysicsVector2}.
358 * @returns An immutable polygon collision shape. See {@link PibblPhysicsShape2D}.
359 *
360 * @see {@link PibblPhysicsVector2}
361 * @see {@link PibblPhysicsShape2D}
362 */
363 export function polygonShape2D(
364 input: Readonly<{ points: readonly PibblPhysicsVector2[] }>,
365 ): PibblPhysicsShape2D {
366 const vertices = canonicalizeContour2D(input.points, {
367 strictlyConvex: true,
368 path: 'polygon.points',
369 });
370 const mass = polygonMassData2D(vertices);
371 return createShapeFromLeaves(
372 [Object.freeze({ kind: 'polygon', vertices, canonicalLeafIndex: 0 })],
373 mass.area,
374 mass.centroidX,
375 mass.centroidY,
376 mass.unitInertia,
377 );
378 }
379
380 /**
381 * Computes axis-aligned bounds for an immutable shape at an optional pose.
382 *
383 * @param shape - Shape whose bounds are needed. See {@link PibblPhysicsShape2D}.
384 * @param pose - Optional pose at which to evaluate the shape. See {@link PibblPhysicsPose2D}.
385 * @returns Axis-aligned bounds of the posed shape. See {@link PibblPhysicsAabb2D}.
386 *
387 * @see {@link PibblPhysicsShape2D}
388 * @see {@link PibblPhysicsPose2D}
389 * @see {@link PibblPhysicsAabb2D}
390 */
391 export function boundsOfShape2D(
392 shape: PibblPhysicsShape2D,
393 pose?: PibblPhysicsPose2D,
394 ): PibblPhysicsAabb2D {
395 const record = requireShapeRecord2D(shape);
396 const outerPose = copyPose2D(pose, 'pose');
397 return boundsForLeaves(record.leaves, composePose2D(outerPose, record.localPose));
398 }
399
400 /**
401 * Computes shape area, centroid, mass, and rotational inertia at the requested density.
402 *
403 * @param shape - Shape whose mass properties are needed. See {@link PibblPhysicsShape2D}.
404 * @param options - Optional material density.
405 * @returns Mass, center of mass, and inertia for the shape. See {@link PibblPhysicsMassProperties2D}
406 * .
407 *
408 * @see {@link PibblPhysicsShape2D}
409 * @see {@link PibblPhysicsMassProperties2D}
410 */
411 export function massProperties2D(
412 shape: PibblPhysicsShape2D,
413 options?: Readonly<{ density?: number }>,
414 ): PibblPhysicsMassProperties2D {
415 const record = requireShapeRecord2D(shape);
416 const density = requirePositiveNumber(options?.density ?? 1, 'options.density');
417 const transformedCentroid = new Float64Array(2);
418 transformPointInto(
419 transformedCentroid,
420 0,
421 record.centroidX,
422 record.centroidY,
423 record.localPose,
424 );
425 return Object.freeze({
426 area: cleanZero(record.area),
427 centroid: Object.freeze([
428 cleanZero(transformedCentroid[0]!),
429 cleanZero(transformedCentroid[1]!),
430 ] as const),
431 mass: cleanZero(record.area * density),
432 rotationalInertia: cleanZero(record.unitInertia * density),
433 });
434 }
435
436 /**
437 * Returns independent shape geometry transformed by a rigid 2D pose.
438 *
439 * @param shape - Source shape; it is not modified. See {@link PibblPhysicsShape2D}.
440 * @param transform - Transform to apply to the shape. See {@link PibblPhysicsTransform2D}.
441 * @returns A shape containing the transformed geometry. See {@link PibblPhysicsShape2D}.
442 *
443 * @see {@link PibblPhysicsShape2D}
444 * @see {@link PibblPhysicsTransform2D}
445 */
446 export function transformShape2D(
447 shape: PibblPhysicsShape2D,
448 transform: PibblPhysicsTransform2D,
449 ): PibblPhysicsShape2D {
450 const source = requireShapeRecord2D(shape);
451 const localPose = composePose2D(copyPose2D(transform, 'transform'), source.localPose);
452 const localBounds = mutableBoundsForLeaves(source.leaves, localPose);
453 return createShape2D({
454 leaves: source.leaves,
455 localPose,
456 localBounds,
457 area: source.area,
458 centroidX: source.centroidX,
459 centroidY: source.centroidY,
460 unitInertia: source.unitInertia,
461 });
462 }
463
464 function createShapeFromLeaves(
465 leaves: readonly ConvexLeaf2D[],
466 area: number,
467 centroidX: number,
468 centroidY: number,
469 unitInertia: number,
470 ): PibblPhysicsShape2D {
471 return createShape2D({
472 leaves,
473 localBounds: mutableBoundsForLeaves(leaves, IDENTITY_POSE_2D),
474 area,
475 centroidX,
476 centroidY,
477 unitInertia: Math.max(0, unitInertia),
478 });
479 }
480
481 function chainFromOwnedPoints(
482 points: readonly Float64Array[],
483 closed: boolean,
484 radius: number,
485 ): PibblPhysicsShape2D {
486 const segmentCount = points.length === 1 ? 0 : points.length - 1 + (closed ? 1 : 0);
487 const leaves: ConvexLeaf2D[] = [];
488 for (let index = 0; index < segmentCount; index += 1) {
489 const start = points[index]!;
490 const end = points[(index + 1) % points.length]!;
491 leaves.push(Object.freeze(radius === 0
492 ? { kind: 'segment', start, end, canonicalLeafIndex: index }
493 : { kind: 'capsule', start, end, radius, canonicalLeafIndex: index }));
494 }
495 if (leaves.length === 0) {
496 const point = points[0]!;
497 const area = Math.PI * radius * radius;
498 leaves.push(Object.freeze({
499 kind: 'circle', center: point, radius, canonicalLeafIndex: 0,
500 }));
501 return createShapeFromLeaves(
502 leaves,
503 area,
504 point[0]!,
505 point[1]!,
506 area * radius * radius / 2,
507 );
508 }
509 if (radius > 0) {
510 const mass = aggregateLeafMass(leaves);
511 return createShapeFromLeaves(
512 leaves,
513 mass.area,
514 mass.centroidX,
515 mass.centroidY,
516 mass.unitInertia,
517 );
518 }
519 const centroid = lineCentroid(points, closed);
520 return createShapeFromLeaves(leaves, 0, centroid[0], centroid[1], 0);
521 }
522
523 function boundsForLeaves(
524 leaves: readonly ConvexLeaf2D[],
525 pose: InternalPose2D,
526 ): PibblPhysicsAabb2D {
527 const bounds = mutableBoundsForLeaves(leaves, pose);
528 return freezeBounds2D(bounds[0]!, bounds[1]!, bounds[2]!, bounds[3]!);
529 }
530
531 function mutableBoundsForLeaves(
532 leaves: readonly ConvexLeaf2D[],
533 pose: InternalPose2D,
534 ): Float64Array {
535 const result = new Float64Array([
536 Number.POSITIVE_INFINITY,
537 Number.POSITIVE_INFINITY,
538 Number.NEGATIVE_INFINITY,
539 Number.NEGATIVE_INFINITY,
540 ]);
541 const point = new Float64Array(2);
542 const include = (x: number, y: number, radius = 0): void => {
543 transformPointInto(point, 0, x, y, pose);
544 result[0] = Math.min(result[0]!, point[0]! - radius);
545 result[1] = Math.min(result[1]!, point[1]! - radius);
546 result[2] = Math.max(result[2]!, point[0]! + radius);
547 result[3] = Math.max(result[3]!, point[1]! + radius);
548 };
549 for (const leaf of leaves) {
550 switch (leaf.kind) {
551 case 'circle':
552 include(leaf.center[0]!, leaf.center[1]!, leaf.radius);
553 break;
554 case 'ellipse': {
555 transformPointInto(point, 0, leaf.center[0]!, leaf.center[1]!, pose);
556 const angle = pose.rotationRadians + leaf.rotationRadians;
557 const cosine = Math.cos(angle);
558 const sine = Math.sin(angle);
559 const extentX = Math.hypot(leaf.radiusX * cosine, leaf.radiusY * sine);
560 const extentY = Math.hypot(leaf.radiusX * sine, leaf.radiusY * cosine);
561 result[0] = Math.min(result[0]!, point[0]! - extentX);
562 result[1] = Math.min(result[1]!, point[1]! - extentY);
563 result[2] = Math.max(result[2]!, point[0]! + extentX);
564 result[3] = Math.max(result[3]!, point[1]! + extentY);
565 break;
566 }
567 case 'box':
568 includeBoxCorners(leaf.center, leaf.halfWidth, leaf.halfHeight, pose, result, point);
569 break;
570 case 'rounded-box':
571 includeRoundedBoxBounds(leaf, pose, result, point);
572 break;
573 case 'capsule':
574 include(leaf.start[0]!, leaf.start[1]!, leaf.radius);
575 include(leaf.end[0]!, leaf.end[1]!, leaf.radius);
576 break;
577 case 'segment':
578 include(leaf.start[0]!, leaf.start[1]!);
579 include(leaf.end[0]!, leaf.end[1]!);
580 break;
581 case 'polygon':
582 for (let offset = 0; offset < leaf.vertices.length; offset += 2) {
583 include(leaf.vertices[offset]!, leaf.vertices[offset + 1]!);
584 }
585 break;
586 }
587 }
588 return result;
589 }
590
591 function includeBoxCorners(
592 center: Float64Array,
593 halfWidth: number,
594 halfHeight: number,
595 pose: InternalPose2D,
596 bounds: Float64Array,
597 point: Float64Array,
598 ): void {
599 for (const xSign of [-1, 1]) {
600 for (const ySign of [-1, 1]) {
601 transformPointInto(
602 point,
603 0,
604 center[0]! + xSign * halfWidth,
605 center[1]! + ySign * halfHeight,
606 pose,
607 );
608 bounds[0] = Math.min(bounds[0]!, point[0]!);
609 bounds[1] = Math.min(bounds[1]!, point[1]!);
610 bounds[2] = Math.max(bounds[2]!, point[0]!);
611 bounds[3] = Math.max(bounds[3]!, point[1]!);
612 }
613 }
614 }
615
616 function includeRoundedBoxBounds(
617 leaf: Extract<ConvexLeaf2D, { kind: 'rounded-box' }>,
618 pose: InternalPose2D,
619 bounds: Float64Array,
620 point: Float64Array,
621 ): void {
622 const xSigns = [-1, 1, 1, -1] as const;
623 const ySigns = [-1, -1, 1, 1] as const;
624 for (let index = 0; index < 4; index += 1) {
625 const radius = leaf.radii[index]!;
626 transformPointInto(
627 point,
628 0,
629 leaf.center[0]! + xSigns[index]! * (leaf.halfWidth - radius),
630 leaf.center[1]! + ySigns[index]! * (leaf.halfHeight - radius),
631 pose,
632 );
633 bounds[0] = Math.min(bounds[0]!, point[0]! - radius);
634 bounds[1] = Math.min(bounds[1]!, point[1]! - radius);
635 bounds[2] = Math.max(bounds[2]!, point[0]! + radius);
636 bounds[3] = Math.max(bounds[3]!, point[1]! + radius);
637 }
638 }
639
640 function aggregateLeafMass(leaves: readonly ConvexLeaf2D[]): Readonly<{
641 area: number;
642 centroidX: number;
643 centroidY: number;
644 unitInertia: number;
645 }> {
646 const entries = leaves.map(leafMassData);
647 const area = entries.reduce((sum, entry) => sum + entry.area, 0);
648 if (area === 0) {
649 return Object.freeze({ area: 0, centroidX: 0, centroidY: 0, unitInertia: 0 });
650 }
651 const centroidX = entries.reduce((sum, entry) => sum + entry.area * entry.centroidX, 0) / area;
652 const centroidY = entries.reduce((sum, entry) => sum + entry.area * entry.centroidY, 0) / area;
653 const unitInertia = entries.reduce((sum, entry) => {
654 const dx = entry.centroidX - centroidX;
655 const dy = entry.centroidY - centroidY;
656 return sum + entry.unitInertia + entry.area * (dx * dx + dy * dy);
657 }, 0);
658 return Object.freeze({ area, centroidX, centroidY, unitInertia });
659 }
660
661 function leafMassData(leaf: ConvexLeaf2D): Readonly<{
662 area: number;
663 centroidX: number;
664 centroidY: number;
665 unitInertia: number;
666 }> {
667 switch (leaf.kind) {
668 case 'circle': {
669 const area = Math.PI * leaf.radius * leaf.radius;
670 return { area, centroidX: leaf.center[0]!, centroidY: leaf.center[1]!, unitInertia: area * leaf.radius * leaf.radius / 2 };
671 }
672 case 'ellipse': {
673 const area = Math.PI * leaf.radiusX * leaf.radiusY;
674 return { area, centroidX: leaf.center[0]!, centroidY: leaf.center[1]!, unitInertia: area * (leaf.radiusX * leaf.radiusX + leaf.radiusY * leaf.radiusY) / 4 };
675 }
676 case 'box': {
677 const width = leaf.halfWidth * 2;
678 const height = leaf.halfHeight * 2;
679 const area = width * height;
680 return { area, centroidX: leaf.center[0]!, centroidY: leaf.center[1]!, unitInertia: area * (width * width + height * height) / 12 };
681 }
682 case 'capsule':
683 return capsuleMassData(leaf.start, leaf.end, leaf.radius);
684 case 'polygon':
685 return polygonMassData2D(leaf.vertices);
686 case 'segment':
687 return { area: 0, centroidX: (leaf.start[0]! + leaf.end[0]!) / 2, centroidY: (leaf.start[1]! + leaf.end[1]!) / 2, unitInertia: 0 };
688 case 'rounded-box': {
689 const contour = roundedBoxContour(leaf.halfWidth, leaf.halfHeight, leaf.radii, leaf.center);
690 return polygonMassData2D(contour);
691 }
692 }
693 }
694
695 function capsuleMassData(
696 start: Float64Array,
697 end: Float64Array,
698 radius: number,
699 ): Readonly<{ area: number; centroidX: number; centroidY: number; unitInertia: number }> {
700 const dx = end[0]! - start[0]!;
701 const dy = end[1]! - start[1]!;
702 const length = Math.hypot(dx, dy);
703 const rectangleArea = 2 * radius * length;
704 const circleArea = Math.PI * radius * radius;
705 const area = rectangleArea + circleArea;
706 const rectangleInertia = rectangleArea * (length * length + 4 * radius * radius) / 12;
707 const capsInertia =
708 Math.PI * radius ** 4 / 2 +
709 circleArea * length * length / 4 +
710 4 * length * radius ** 3 / 3;
711 return Object.freeze({
712 area,
713 centroidX: (start[0]! + end[0]!) / 2,
714 centroidY: (start[1]! + end[1]!) / 2,
715 unitInertia: rectangleInertia + capsInertia,
716 });
717 }
718
719 function lineCentroid(
720 points: readonly Float64Array[],
721 closed: boolean,
722 ): readonly [number, number] {
723 const segmentCount = points.length - 1 + (closed ? 1 : 0);
724 let totalLength = 0;
725 let weightedX = 0;
726 let weightedY = 0;
727 for (let index = 0; index < segmentCount; index += 1) {
728 const start = points[index]!;
729 const end = points[(index + 1) % points.length]!;
730 const length = Math.hypot(end[0]! - start[0]!, end[1]! - start[1]!);
731 totalLength += length;
732 weightedX += length * (start[0]! + end[0]!) / 2;
733 weightedY += length * (start[1]! + end[1]!) / 2;
734 }
735 if (totalLength > 0) {
736 return [weightedX / totalLength, weightedY / totalLength];
737 }
738 return [points.reduce((sum, point) => sum + point[0]!, 0) / points.length, points.reduce((sum, point) => sum + point[1]!, 0) / points.length];
739 }
740
741 function roundedBoxContour(
742 halfWidth: number,
743 halfHeight: number,
744 radii: Float64Array,
745 center: Float64Array,
746 ): Float64Array {
747 const points: Float64Array[] = [];
748 const cornerData = [
749 [halfWidth - radii[1]!, -halfHeight + radii[1]!, -Math.PI / 2, 0, radii[1]!],
750 [halfWidth - radii[2]!, halfHeight - radii[2]!, 0, Math.PI / 2, radii[2]!],
751 [-halfWidth + radii[3]!, halfHeight - radii[3]!, Math.PI / 2, Math.PI, radii[3]!],
752 [-halfWidth + radii[0]!, -halfHeight + radii[0]!, Math.PI, 3 * Math.PI / 2, radii[0]!],
753 ] as const;
754 for (const [x, y, start, end, radius] of cornerData) {
755 const cornerPoints = radius === 0
756 ? [new Float64Array([center[0]! + x, center[1]! + y])]
757 : flattenCircularSweep(center[0]! + x, center[1]! + y, radius, start, end, false);
758 points.push(...cornerPoints);
759 }
760 return canonicalizeContour2D(points.map(asReadonlyVector), {
761 strictlyConvex: false,
762 path: 'roundedBox.contour',
763 });
764 }
765
766 function flattenCircularSweep(
767 centerX: number,
768 centerY: number,
769 radius: number,
770 startAngle: number,
771 endAngle: number,
772 counterclockwise: boolean,
773 ): Float64Array[] {
774 const sweep = directedSweep(startAngle, endAngle, counterclockwise);
775 const points = [pointOnCircle(centerX, centerY, radius, startAngle)];
776 appendFlattenedArc(points, centerX, centerY, radius, startAngle, startAngle + sweep, 0);
777 if (Math.abs(sweep) >= FULL_TURN_RADIANS) {
778 points[points.length - 1] = new Float64Array(points[0]!);
779 }
780 return points;
781 }
782
783 function appendFlattenedArc(
784 output: Float64Array[],
785 centerX: number,
786 centerY: number,
787 radius: number,
788 startAngle: number,
789 endAngle: number,
790 depth: number,
791 ): void {
792 const middleAngle = (startAngle + endAngle) / 2;
793 const halfSweep = Math.abs(endAngle - startAngle) / 2;
794 const chordError = radius * (1 - Math.cos(halfSweep));
795 if (
796 chordError <= CURVE_FLATTENING_TOLERANCE ||
797 depth >= CURVE_FLATTENING_MAX_DEPTH
798 ) {
799 output.push(pointOnCircle(centerX, centerY, radius, endAngle));
800 return;
801 }
802 appendFlattenedArc(output, centerX, centerY, radius, startAngle, middleAngle, depth + 1);
803 appendFlattenedArc(output, centerX, centerY, radius, middleAngle, endAngle, depth + 1);
804 }
805
806 function directedSweep(
807 startAngle: number,
808 endAngle: number,
809 counterclockwise: boolean,
810 ): number {
811 const raw = endAngle - startAngle;
812 if (Math.abs(raw) >= FULL_TURN_RADIANS) {
813 return counterclockwise ? -FULL_TURN_RADIANS : FULL_TURN_RADIANS;
814 }
815 if (counterclockwise) {
816 const magnitude = ((-raw % FULL_TURN_RADIANS) + FULL_TURN_RADIANS) % FULL_TURN_RADIANS;
817 return -magnitude;
818 }
819 return ((raw % FULL_TURN_RADIANS) + FULL_TURN_RADIANS) % FULL_TURN_RADIANS;
820 }
821
822 function pointOnCircle(
823 centerX: number,
824 centerY: number,
825 radius: number,
826 angle: number,
827 ): Float64Array {
828 return new Float64Array([
829 centerX + radius * Math.cos(angle),
830 centerY + radius * Math.sin(angle),
831 ]);
832 }
833
834 function ownedVector(value: PibblPhysicsVector2, path: string): Float64Array {
835 const copied = copyFiniteVector2(value, path);
836 return new Float64Array(copied);
837 }
838
839 function asReadonlyVector(value: Float64Array): PibblPhysicsVector2 {
840 return [value[0]!, value[1]!] as const;
841 }
842 Documentation version
Section titled “Documentation version”Documentation built with @pibbl/core 0.0.2, revision 272a94a. ALPHA — NOT FOR PRODUCTION USE.