forked from vova616/chipmunk
/
pivotJoint.go
120 lines (95 loc) · 2.97 KB
/
pivotJoint.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
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
package chipmunk
import (
"github.com/TSavo/chipmunk/transform"
"github.com/TSavo/chipmunk/vect"
"math"
)
type PivotJoint struct {
BasicConstraint
Anchor1, Anchor2 vect.Vect
r1, r2 vect.Vect
k1, k2 vect.Vect
jAcc vect.Vect
jMaxLen vect.Float
bias vect.Vect
}
func NewPivotJointAnchor(a, b *Body, anchor1, anchor2 vect.Vect) *PivotJoint {
return &PivotJoint{BasicConstraint: NewConstraint(a, b), Anchor1: anchor1, Anchor2: anchor2}
}
func NewPivotJoint(a, b *Body) *PivotJoint {
return NewPivotJointAnchor(a, b, vect.Vector_Zero, vect.Vector_Zero)
}
func (this *PivotJoint) PreStep(dt vect.Float) {
a, b := this.BodyA, this.BodyB
this.r1 = transform.RotateVect(this.Anchor1, transform.Rotation{a.rot.X, a.rot.Y})
this.r2 = transform.RotateVect(this.Anchor2, transform.Rotation{b.rot.X, b.rot.Y})
// Calculate mass tensor
k_tensor(a, b, this.r1, this.r2, &this.k1, &this.k2)
// compute max impulse
this.jMaxLen = this.MaxForce * dt
// calculate bias velocity
delta := vect.Sub(vect.Add(b.p, this.r2), vect.Add(a.p, this.r1))
this.bias = vect.Clamp(vect.Mult(delta, -bias_coef(this.ErrorBias, dt)/dt), this.MaxBias)
}
func bias_coef(errorBias, dt vect.Float) vect.Float {
return vect.Float(1.0 - math.Pow(float64(errorBias), float64(dt)))
}
func (this *PivotJoint) ApplyCachedImpulse(dt_coef vect.Float) {
a, b := this.BodyA, this.BodyB
apply_impulses(a, b, this.r1, this.r2, vect.Mult(this.jAcc, dt_coef))
}
func (this *PivotJoint) ApplyImpulse() {
a, b := this.BodyA, this.BodyB
r1, r2 := this.r1, this.r2
// compute relative velocity
vr := relative_velocity2(a, b, r1, r2)
// compute normal impulse
j := mult_k(vect.Sub(this.bias, vr), this.k1, this.k2)
jOld := this.jAcc
this.jAcc = vect.Clamp(vect.Add(this.jAcc, j), this.jMaxLen)
j = vect.Sub(this.jAcc, jOld)
// apply impulse
apply_impulses(a, b, this.r1, this.r2, j)
}
func (this *PivotJoint) Impulse() vect.Float {
return vect.Length(this.jAcc)
}
func mult_k(vr, k1, k2 vect.Vect) vect.Vect {
return vect.Vect{vect.Dot(vr, k1), vect.Dot(vr, k2)}
}
func k_tensor(a, b *Body, r1, r2 vect.Vect, k1, k2 *vect.Vect) {
// calculate mass matrix
// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
m_sum := a.m_inv + b.m_inv
// start with I*m_sum
k11 := vect.Float(m_sum)
k12 := vect.Float(0)
k21 := vect.Float(0)
k22 := vect.Float(m_sum)
// add the influence from r1
a_i_inv := a.i_inv
r1xsq := r1.X * r1.X * a_i_inv
r1ysq := r1.Y * r1.Y * a_i_inv
r1nxy := -r1.X * r1.Y * a_i_inv
k11 += r1ysq
k12 += r1nxy
k21 += r1nxy
k22 += r1xsq
// add the influnce from r2
b_i_inv := b.i_inv
r2xsq := r2.X * r2.X * b_i_inv
r2ysq := r2.Y * r2.Y * b_i_inv
r2nxy := -r2.X * r2.Y * b_i_inv
k11 += r2ysq
k12 += r2nxy
k21 += r2nxy
k22 += r2xsq
// invert
determinant := (k11 * k22) - (k12 * k21)
if determinant == 0 {
panic("Unsolvable constraint.")
}
det_inv := 1.0 / determinant
*k1 = vect.Vect{k22 * det_inv, -k12 * det_inv}
*k2 = vect.Vect{-k21 * det_inv, k11 * det_inv}
}