From 15f315133bdf52e7d85b0ccebde2b277e941d442 Mon Sep 17 00:00:00 2001 From: Tony Garnock-Jones Date: Tue, 21 Feb 2023 21:25:20 +0100 Subject: [PATCH] src/constraint.ts --- src/constraint.ts | 220 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 220 insertions(+) create mode 100644 src/constraint.ts diff --git a/src/constraint.ts b/src/constraint.ts new file mode 100644 index 0000000..074249f --- /dev/null +++ b/src/constraint.ts @@ -0,0 +1,220 @@ +// After (a simplified form of) https://github.com/tonyg/relax-overveld/tree/deno, +// which is MIT-licensed. + +import { Vector2, Vector3 } from '@babylonjs/core/Legacy/legacy'; + +export interface Delta { + isSignificant(epsilon: number): boolean; + apply(rho: number): void; +} + +export interface Constraint { + computeDeltas(): Array; +} + +export class Relax { + rho = 0.25; + epsilon = 0.0001; + constraints: Array = []; + + add(constraint: X): X { + this.constraints.push(constraint); + return constraint; + } + + clear(): void { + this.constraints = []; + } + + _iterate() { + const allDeltas: Array = []; + + this.constraints.forEach(t => { + const deltas = t.computeDeltas(); + if (deltas.some(d => d.isSignificant(this.epsilon))) { + allDeltas.push(... deltas); + } + }); + + // (This shouldn't be done in the loop above b/c that would affect the other delta computations.) + allDeltas.forEach(d => d.apply(this.rho)); + + return allDeltas.length > 0; + } + + iterate(milliseconds: number): number { + let count = 0; + const t0 = Date.now(); + while ((Date.now() - t0) < milliseconds) { + if (!this._iterate()) break; + count++; + } + return count; + } +} + +//--------------------------------------------------------------------------- + +export interface Ref { + get(): number; + set(v: number): void; +} + +export class Path implements Ref { + path: string[]; + + constructor (public obj: any, ... path: string[]) { + this.path = path; + } + + get(): number { + return this.path.reduce((o, p) => o[p], this.obj) as unknown as number; + } + + set(v: number) { + this.path.reduce((o, p, i) => i === this.path.length - 1 ? o[p] = v : o[p], this.obj); + } +} + +export class NumericDelta implements Delta { + constructor (public ref: Ref, public amount: number) {} + isSignificant(epsilon: number): boolean { return Math.abs(this.amount) > epsilon; } + apply(rho: number): void { this.ref.set(this.ref.get() + (this.amount * rho)); } +} + +export class ValueConstraint implements Constraint { + constructor (public v: Ref, public targetValue: number) {} + computeDeltas(): Array { + return [new NumericDelta(this.v, this.targetValue - this.v.get())]; + } +} + +export class EqualityConstraint implements Constraint { + constructor (public p1: Ref, public p2: Ref) {} + computeDeltas(): Array { + const diff = this.p1.get() - this.p2.get(); + return [new NumericDelta(this.p1, -diff / 2), + new NumericDelta(this.p2, +diff / 2)]; + } +} + +export class SumConstraint implements Constraint { + constructor (public addend1: Ref, public addend2: Ref, public sum: Ref) {} + computeDeltas(): Array { + const diff = this.sum.get() - (this.addend1.get() + this.addend2.get()); + return [new NumericDelta(this.addend1, +diff / 3), + new NumericDelta(this.addend2, +diff / 3), + new NumericDelta(this.sum, -diff / 3)]; + } +} + +export interface Vectorish { + length(): number; + lengthSquared(): number; + scale(s: number): this; + addInPlace(v: this): void; + add(v: this): this; + subtract(v: this): this; + normalize(): this; // in place! +} + +export class VectorDelta implements Delta { + constructor (public p: V, public delta: V) {} + + isSignificant(epsilon: number): boolean { + return this.delta.lengthSquared() > epsilon * epsilon; + } + + apply(rho: number): void { + this.p.addInPlace(this.delta.scale(rho)); + } +} + +export class CoordinateConstraint implements Constraint { + constructor (public p: V, public c: V) {} + computeDeltas(): Array { + return [new VectorDelta(this.p, this.c.subtract(this.p))]; + } +} + +export class CoincidenceConstraint implements Constraint { + constructor (public p1: V, public p2: V) {} + computeDeltas(): Array { + const splitDiff = this.p2.subtract(this.p1).scale(0.5); + return [new VectorDelta(this.p1, splitDiff), + new VectorDelta(this.p2, splitDiff.scale(-1))]; + } +} + +export class EquivalenceConstraint implements Constraint { + constructor (public p1: V, public p2: V, public p3: V, public p4: V) {} + computeDeltas(): Array { + const splitDiff = this.p2.add(this.p3) + .subtract(this.p1.add(this.p4)) + .scale(0.25); + return [new VectorDelta(this.p1, splitDiff), + new VectorDelta(this.p2, splitDiff.scale(-1)), + new VectorDelta(this.p3, splitDiff.scale(-1)), + new VectorDelta(this.p4, splitDiff)]; + } +} + +export class EqualDistanceConstraint implements Constraint { + constructor (public p1: V, public p2: V, public p3: V, public p4: V) {} + computeDeltas(): Array { + const l12 = this.p1.subtract(this.p2).length(); + const l34 = this.p3.subtract(this.p4).length(); + const delta = (l12 - l34) / 4; + const e12 = this.p2.subtract(this.p1).normalize().scale(delta); + const e34 = this.p4.subtract(this.p3).normalize().scale(delta); + return [new VectorDelta(this.p1, e12), + new VectorDelta(this.p2, e12.scale(-1)), + new VectorDelta(this.p3, e34.scale(-1)), + new VectorDelta(this.p4, e34)]; + } +} + +export class LengthConstraint implements Constraint { + constructor (public p1: V, public p2: V, public l: number) {} + computeDeltas(): Array { + const l12 = this.p1.subtract(this.p2).length(); + const delta = (l12 - this.l) / 2; + const e12 = this.p2.subtract(this.p1).normalize().scale(delta); + return [new VectorDelta(this.p1, e12), + new VectorDelta(this.p2, e12.scale(-1))]; + } +} + +export function rotatedAround(p: Vector2, dTheta: number, axis: Vector2): Vector2 { + const v = p.subtract(axis); + v.rotateToRef(dTheta, v); + return axis.add(v); +} + +export class OrientationConstraint2 implements Constraint { + constructor (public p1: Vector2, + public p2: Vector2, + public p3: Vector2, + public p4: Vector2, + public theta: number) {} + + computeDeltas(): Array { + const v12 = this.p2.subtract(this.p1); + const a12 = Math.atan2(v12.y, v12.x); + const m12 = this.p1.add(this.p2.subtract(this.p1).scale(0.5)); + + const v34 = this.p4.subtract(this.p3); + const a34 = Math.atan2(v34.y, v34.x); + const m34 = this.p3.add(this.p4.subtract(this.p3).scale(0.5)); + + const currTheta = a12 - a34; + const dTheta = this.theta - currTheta; + // TODO: figure out why setting dTheta to 1/2 times this value (as shown in the paper + // and seems to make sense) results in jumpy/unstable behavior. + + return [new VectorDelta(this.p1, rotatedAround(this.p1, dTheta, m12).subtract(this.p1)), + new VectorDelta(this.p2, rotatedAround(this.p2, dTheta, m12).subtract(this.p2)), + new VectorDelta(this.p3, rotatedAround(this.p3, -dTheta, m34).subtract(this.p3)), + new VectorDelta(this.p4, rotatedAround(this.p4, -dTheta, m34).subtract(this.p4))]; + } +}