forked from jakecoffman/cp
-
Notifications
You must be signed in to change notification settings - Fork 0
/
groovejoint.go
98 lines (76 loc) · 2.05 KB
/
groovejoint.go
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
package cp
type GrooveJoint struct {
*Constraint
GrooveN, GrooveA, GrooveB Vector
AnchorB Vector
grooveTn Vector
clamp float64
r1, r2 Vector
k Mat2x2
jAcc, bias Vector
}
func NewGrooveJoint(a, b *Body, grooveA, grooveB, anchorB Vector) *Constraint {
joint := &GrooveJoint{
GrooveA: grooveA,
GrooveB: grooveB,
GrooveN: grooveB.Sub(grooveA).Normalize().Perp(),
AnchorB: anchorB,
jAcc: Vector{},
}
joint.Constraint = NewConstraint(joint, a, b)
return joint.Constraint
}
func (joint *GrooveJoint) PreStep(dt float64) {
a := joint.a
b := joint.b
ta := a.transform.Point(joint.GrooveA)
tb := a.transform.Point(joint.GrooveB)
n := a.transform.Vect(joint.GrooveN)
d := ta.Dot(n)
joint.grooveTn = n
joint.r2 = b.transform.Vect(joint.AnchorB.Sub(b.cog))
td := b.p.Add(joint.r2).Cross(n)
if td <= ta.Cross(n) {
joint.clamp = 1
joint.r1 = ta.Sub(a.p)
} else if td >= tb.Cross(n) {
joint.clamp = -1
joint.r1 = tb.Sub(a.p)
} else {
joint.clamp = 0
joint.r1 = n.Perp().Mult(-td).Add(n.Mult(d)).Sub(a.p)
}
joint.k = k_tensor(a, b, joint.r1, joint.r2)
delta := b.p.Add(joint.r2).Sub(a.p.Add(joint.r1))
joint.bias = delta.Mult(-bias_coef(joint.errorBias, dt) / dt).Clamp(joint.maxBias)
}
func (joint *GrooveJoint) ApplyCachedImpulse(dt_coef float64) {
a := joint.a
b := joint.b
apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc.Mult(dt_coef))
}
func (joint *GrooveJoint) grooveConstrain(j Vector, dt float64) Vector {
n := joint.grooveTn
var jClamp Vector
if joint.clamp*j.Cross(n) > 0 {
jClamp = j
} else {
jClamp = j.Project(n)
}
return jClamp.Clamp(joint.maxForce * dt)
}
func (joint *GrooveJoint) ApplyImpulse(dt float64) {
a := joint.a
b := joint.b
r1 := joint.r1
r2 := joint.r2
vr := relative_velocity(a, b, r1, r2)
j := joint.k.Transform(joint.bias.Sub(vr))
jOld := joint.jAcc
joint.jAcc = joint.grooveConstrain(jOld.Add(j), dt)
j = joint.jAcc.Sub(jOld)
apply_impulses(a, b, joint.r1, joint.r2, j)
}
func (joint *GrooveJoint) GetImpulse() float64 {
return joint.jAcc.Length()
}