src/constraint.ts
This commit is contained in:
parent
7d253d385e
commit
15f315133b
|
|
@ -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<Delta>;
|
||||
}
|
||||
|
||||
export class Relax {
|
||||
rho = 0.25;
|
||||
epsilon = 0.0001;
|
||||
constraints: Array<Constraint> = [];
|
||||
|
||||
add<X extends Constraint>(constraint: X): X {
|
||||
this.constraints.push(constraint);
|
||||
return constraint;
|
||||
}
|
||||
|
||||
clear(): void {
|
||||
this.constraints = [];
|
||||
}
|
||||
|
||||
_iterate() {
|
||||
const allDeltas: Array<Delta> = [];
|
||||
|
||||
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<Delta> {
|
||||
return [new NumericDelta(this.v, this.targetValue - this.v.get())];
|
||||
}
|
||||
}
|
||||
|
||||
export class EqualityConstraint implements Constraint {
|
||||
constructor (public p1: Ref, public p2: Ref) {}
|
||||
computeDeltas(): Array<Delta> {
|
||||
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<Delta> {
|
||||
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<V extends Vectorish> 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<V extends Vectorish> implements Constraint {
|
||||
constructor (public p: V, public c: V) {}
|
||||
computeDeltas(): Array<Delta> {
|
||||
return [new VectorDelta(this.p, this.c.subtract(this.p))];
|
||||
}
|
||||
}
|
||||
|
||||
export class CoincidenceConstraint<V extends Vectorish> implements Constraint {
|
||||
constructor (public p1: V, public p2: V) {}
|
||||
computeDeltas(): Array<Delta> {
|
||||
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<V extends Vectorish> implements Constraint {
|
||||
constructor (public p1: V, public p2: V, public p3: V, public p4: V) {}
|
||||
computeDeltas(): Array<Delta> {
|
||||
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<V extends Vectorish> implements Constraint {
|
||||
constructor (public p1: V, public p2: V, public p3: V, public p4: V) {}
|
||||
computeDeltas(): Array<Delta> {
|
||||
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<V extends Vectorish> implements Constraint {
|
||||
constructor (public p1: V, public p2: V, public l: number) {}
|
||||
computeDeltas(): Array<Delta> {
|
||||
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<Delta> {
|
||||
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))];
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue