forked from g3n/engine
-
Notifications
You must be signed in to change notification settings - Fork 0
/
friction.go
82 lines (57 loc) · 1.84 KB
/
friction.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
// Copyright 2016 The G3N Authors. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.
package equation
import (
"github.com/g3n/engine/math32"
)
// Friction is a friction constraint equation.
type Friction struct {
Equation
rA *math32.Vector3 // World-oriented vector that goes from the center of bA to the contact point.
rB *math32.Vector3 // World-oriented vector that starts in body j position and goes to the contact point.
t *math32.Vector3 // Contact tangent
}
// NewFriction creates and returns a pointer to a new Friction equation object.
// slipForce should be +-F_friction = +-mu * F_normal = +-mu * m * g
func NewFriction(bodyA, bodyB IBody, slipForce float32) *Friction {
fe := new(Friction)
fe.rA = math32.NewVec3()
fe.rB = math32.NewVec3()
fe.t = math32.NewVec3()
fe.Equation.initialize(bodyA, bodyB, -slipForce, slipForce)
return fe
}
func (fe *Friction) SetTangent(newTangent *math32.Vector3) {
fe.t = newTangent
}
func (fe *Friction) Tangent() math32.Vector3 {
return *fe.t
}
func (fe *Friction) SetRA(newRa *math32.Vector3) {
fe.rA = newRa
}
func (fe *Friction) RA() math32.Vector3 {
return *fe.rA
}
func (fe *Friction) SetRB(newRb *math32.Vector3) {
fe.rB = newRb
}
func (fe *Friction) RB() math32.Vector3 {
return *fe.rB
}
// ComputeB
func (fe *Friction) ComputeB(h float32) float32 {
// Calculate cross products
rtA := math32.NewVec3().CrossVectors(fe.rA, fe.t)
rtB := math32.NewVec3().CrossVectors(fe.rB, fe.t)
// G = [-t -rtA t rtB]
// And remember, this is a pure velocity constraint, g is always zero!
fe.jeA.SetSpatial(fe.t.Clone().Negate())
fe.jeA.SetRotational(rtA.Clone().Negate())
fe.jeB.SetSpatial(fe.t.Clone())
fe.jeB.SetRotational(rtB.Clone())
var GW = fe.ComputeGW()
var GiMf = fe.ComputeGiMf()
return -GW*fe.b - h*GiMf
}