forked from schteppe/cannon.js
/
RotationalEquation.ts
87 lines (75 loc) 路 1.95 KB
/
RotationalEquation.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
import { Equation } from '../equations/Equation'
import { Vec3 } from '../math/Vec3'
import type { Body } from '../objects/Body'
export type RotationalEquationOptions = ConstructorParameters<typeof RotationalEquation>[2]
/**
* Rotational constraint. Works to keep the local vectors orthogonal to each other in world space.
*/
export class RotationalEquation extends Equation {
/**
* World oriented rotational axis.
*/
axisA: Vec3
/**
* World oriented rotational axis.
*/
axisB: Vec3
/**
* maxAngle
*/
maxAngle: number
constructor(
bodyA: Body,
bodyB: Body,
options: {
/**
* World oriented rotational axis.
*/
axisA?: Vec3
/**
* World oriented rotational axis.
*/
axisB?: Vec3
/**
* maxAngle
*/
maxAngle?: number
/**
* @default 1e6
*/
maxForce?: number
} = {}
) {
const maxForce = typeof options.maxForce !== 'undefined' ? options.maxForce : 1e6
super(bodyA, bodyB, -maxForce, maxForce)
this.axisA = options.axisA ? options.axisA.clone() : new Vec3(1, 0, 0)
this.axisB = options.axisB ? options.axisB.clone() : new Vec3(0, 1, 0)
this.maxAngle = Math.PI / 2
}
computeB(h: number): number {
const a = this.a
const b = this.b
const ni = this.axisA
const nj = this.axisB
const nixnj = tmpVec1
const njxni = tmpVec2
const GA = this.jacobianElementA
const GB = this.jacobianElementB
// Caluclate cross products
ni.cross(nj, nixnj)
nj.cross(ni, njxni)
// g = ni * nj
// gdot = (nj x ni) * wi + (ni x nj) * wj
// G = [0 njxni 0 nixnj]
// W = [vi wi vj wj]
GA.rotational.copy(njxni)
GB.rotational.copy(nixnj)
const g = Math.cos(this.maxAngle) - ni.dot(nj)
const GW = this.computeGW()
const GiMf = this.computeGiMf()
const B = -g * a - GW * b - h * GiMf
return B
}
}
const tmpVec1 = new Vec3()
const tmpVec2 = new Vec3()