1// Dihedral angle across a hinge and its gradient.
2//
3// Two triangles (i, j, k) and (l, k, j) share the edge j-k; i and l are the
4// opposite vertices. theta is the signed angle between the two triangle
5// normals about the edge (0 when the triangles are coplanar). The gradient
6// is that of Bridson, Marino and Fedkiw (2003), "Simulation of clothing
7// with folds and wrinkles".
9// Writes d theta / d(x_i, x_j, x_k, x_l) into g[off .. off+12) and returns theta.
10export function hingeAngle(x: Float64Array, i: number, j: number, k: number, l: number, g: Float64Array, off = 0): number {
11 const x1 = 3 * i; // opposite vertex of the first triangle
12 const x2 = 3 * l; // opposite vertex of the second triangle
13 const x3 = 3 * j; // edge start
14 const x4 = 3 * k; // edge end
15 const e0 = x[x4] - x[x3];
16 const e1 = x[x4 + 1] - x[x3 + 1];
17 const e2 = x[x4 + 2] - x[x3 + 2];
18 const eLen = Math.sqrt(e0 * e0 + e1 * e1 + e2 * e2);
19 // n1 = (x1 - x3) x (x1 - x4), n2 = (x2 - x4) x (x2 - x3)
20 const a0 = x[x1] - x[x3], a1 = x[x1 + 1] - x[x3 + 1], a2 = x[x1 + 2] - x[x3 + 2];
21 const b0 = x[x1] - x[x4], b1 = x[x1 + 1] - x[x4 + 1], b2 = x[x1 + 2] - x[x4 + 2];
22 const c0 = x[x2] - x[x4], c1 = x[x2 + 1] - x[x4 + 1], c2 = x[x2 + 2] - x[x4 + 2];
23 const d0 = x[x2] - x[x3], d1 = x[x2 + 1] - x[x3 + 1], d2 = x[x2 + 2] - x[x3 + 2];
24 const n10 = a1 * b2 - a2 * b1, n11 = a2 * b0 - a0 * b2, n12 = a0 * b1 - a1 * b0;
25 const n20 = c1 * d2 - c2 * d1, n21 = c2 * d0 - c0 * d2, n22 = c0 * d1 - c1 * d0;
26 const n1sq = n10 * n10 + n11 * n11 + n12 * n12;
27 const n2sq = n20 * n20 + n21 * n21 + n22 * n22;
28 // angle from atan2((n1 x n2) . e/|e|, n1 . n2)
29 const cr0 = n11 * n22 - n12 * n21, cr1 = n12 * n20 - n10 * n22, cr2 = n10 * n21 - n11 * n20;
30 const sinPart = (cr0 * e0 + cr1 * e1 + cr2 * e2) / eLen;
31 const cosPart = n10 * n20 + n11 * n21 + n12 * n22;
32 // sign chosen to match the gradient below
33 const theta = -Math.atan2(sinPart, cosPart);
34 // gradient
35 const s1 = eLen / n1sq;
36 const s2 = eLen / n2sq;
37 // u1 = |e| n1/|n1|^2, u2 = |e| n2/|n2|^2
38 const p1 = (a0 * e0 + a1 * e1 + a2 * e2) / eLen; // (x1 - x3).e/|e|
39 const q1 = (b0 * e0 + b1 * e1 + b2 * e2) / eLen; // (x1 - x4).e/|e|
40 const p2 = (d0 * e0 + d1 * e1 + d2 * e2) / eLen; // (x2 - x3).e/|e|
41 const q2 = (c0 * e0 + c1 * e1 + c2 * e2) / eLen; // (x2 - x4).e/|e|
42 const m1 = 1 / n1sq;
43 const m2 = 1 / n2sq;
44 const aj = q1 * m1, bj = q2 * m2; // x_j (edge start)
45 const ak = -p1 * m1, bk = -p2 * m2; // x_k (edge end)
46 g[off] = s1 * n10; // x_i
47 g[off + 1] = s1 * n11;
48 g[off + 2] = s1 * n12;
49 g[off + 3] = aj * n10 + bj * n20;
50 g[off + 4] = aj * n11 + bj * n21;
51 g[off + 5] = aj * n12 + bj * n22;
52 g[off + 6] = ak * n10 + bk * n20;
53 g[off + 7] = ak * n11 + bk * n21;
54 g[off + 8] = ak * n12 + bk * n22;
55 g[off + 9] = s2 * n20; // x_l
56 g[off + 10] = s2 * n21;
57 g[off + 11] = s2 * n22;
58 return theta;
59}
61// theta - theta0 wrapped to (-pi, pi]
62export function angleDiff(theta: number, theta0: number): number {
63 let d = theta - theta0;
64 if (d > Math.PI) d -= 2 * Math.PI;
65 else if (d <= -Math.PI) d += 2 * Math.PI;
66 return d;
67}