forked from schteppe/cannon.js
/
GSSolver.ts
146 lines (124 loc) 路 3.92 KB
/
GSSolver.ts
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
import { Solver } from '../solver/Solver'
import type { World } from '../world/World'
/**
* Constraint equation Gauss-Seidel solver.
* @todo The spook parameters should be specified for each constraint, not globally.
* @see https://www8.cs.umu.se/kurser/5DV058/VT09/lectures/spooknotes.pdf
*/
export class GSSolver extends Solver {
/**
* The number of solver iterations determines quality of the constraints in the world.
* The more iterations, the more correct simulation. More iterations need more computations though. If you have a large gravity force in your world, you will need more iterations.
*/
iterations: number
/**
* When tolerance is reached, the system is assumed to be converged.
*/
tolerance: number
/**
* @todo remove useless constructor
*/
constructor() {
super()
this.iterations = 10
this.tolerance = 1e-7
}
/**
* Solve
* @return number of iterations performed
*/
solve(dt: number, world: World): number {
let iter = 0
const maxIter = this.iterations
const tolSquared = this.tolerance * this.tolerance
const equations = this.equations
const Neq = equations.length
const bodies = world.bodies
const Nbodies = bodies.length
const h = dt
let q
let B
let invC
let deltalambda
let deltalambdaTot
let GWlambda
let lambdaj
// Update solve mass
if (Neq !== 0) {
for (let i = 0; i !== Nbodies; i++) {
bodies[i].updateSolveMassProperties()
}
}
// Things that do not change during iteration can be computed once
const invCs = GSSolver_solve_invCs
const Bs = GSSolver_solve_Bs
const lambda = GSSolver_solve_lambda
invCs.length = Neq
Bs.length = Neq
lambda.length = Neq
for (let i = 0; i !== Neq; i++) {
const c = equations[i] as any
lambda[i] = 0.0
Bs[i] = c.computeB(h)
invCs[i] = 1.0 / c.computeC()
}
if (Neq !== 0) {
// Reset vlambda
for (let i = 0; i !== Nbodies; i++) {
const b = bodies[i]
const vlambda = b.vlambda
const wlambda = b.wlambda
vlambda.set(0, 0, 0)
wlambda.set(0, 0, 0)
}
// Iterate over equations
for (iter = 0; iter !== maxIter; iter++) {
// Accumulate the total error for each iteration.
deltalambdaTot = 0.0
for (let j = 0; j !== Neq; j++) {
const c = equations[j]
// Compute iteration
B = Bs[j]
invC = invCs[j]
lambdaj = lambda[j]
GWlambda = c.computeGWlambda()
deltalambda = invC * (B - GWlambda - c.eps * lambdaj)
// Clamp if we are not within the min/max interval
if (lambdaj + deltalambda < c.minForce) {
deltalambda = c.minForce - lambdaj
} else if (lambdaj + deltalambda > c.maxForce) {
deltalambda = c.maxForce - lambdaj
}
lambda[j] += deltalambda
deltalambdaTot += deltalambda > 0.0 ? deltalambda : -deltalambda // abs(deltalambda)
c.addToWlambda(deltalambda)
}
// If the total error is small enough - stop iterate
if (deltalambdaTot * deltalambdaTot < tolSquared) {
break
}
}
// Add result to velocity
for (let i = 0; i !== Nbodies; i++) {
const b = bodies[i]
const v = b.velocity
const w = b.angularVelocity
b.vlambda.vmul(b.linearFactor, b.vlambda)
v.vadd(b.vlambda, v)
b.wlambda.vmul(b.angularFactor, b.wlambda)
w.vadd(b.wlambda, w)
}
// Set the `.multiplier` property of each equation
let l = equations.length
const invDt = 1 / h
while (l--) {
equations[l].multiplier = lambda[l] * invDt
}
}
return iter
}
}
// Just temporary number holders that we want to reuse each iteration.
const GSSolver_solve_lambda: number[] = []
const GSSolver_solve_invCs: number[] = []
const GSSolver_solve_Bs: number[] = []