forked from g3n/engine
/
contact.go
124 lines (89 loc) · 2.87 KB
/
contact.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
121
122
123
124
// 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"
)
// Contact is a contact/non-penetration constraint equation.
type Contact struct {
Equation
restitution float32 // "bounciness": u1 = -e*u0
rA *math32.Vector3 // World-oriented vector that goes from the center of bA to the contact point.
rB *math32.Vector3 // World-oriented vector that goes from the center of bB to the contact point.
nA *math32.Vector3 // Contact normal, pointing out of body A.
}
// NewContact creates and returns a pointer to a new Contact equation object.
func NewContact(bodyA, bodyB IBody, minForce, maxForce float32) *Contact {
ce := new(Contact)
// minForce default should be 0.
ce.restitution = 0.5
ce.rA = &math32.Vector3{0,0,0}
ce.rB = &math32.Vector3{0,0,0}
ce.nA = &math32.Vector3{0,0,0}
ce.Equation.initialize(bodyA, bodyB, minForce, maxForce)
return ce
}
func (ce *Contact) SetRestitution(r float32) {
ce.restitution = r
}
func (ce *Contact) Restitution() float32 {
return ce.restitution
}
func (ce *Contact) SetNormal(newNormal *math32.Vector3) {
ce.nA = newNormal
}
func (ce *Contact) Normal() math32.Vector3 {
return *ce.nA
}
func (ce *Contact) SetRA(newRa *math32.Vector3) {
ce.rA = newRa
}
func (ce *Contact) RA() math32.Vector3 {
return *ce.rA
}
func (ce *Contact) SetRB(newRb *math32.Vector3) {
ce.rB = newRb
}
func (ce *Contact) RB() math32.Vector3 {
return *ce.rB
}
// ComputeB
func (ce *Contact) ComputeB(h float32) float32 {
vA := ce.bA.Velocity()
wA := ce.bA.AngularVelocity()
vB := ce.bB.Velocity()
wB := ce.bB.AngularVelocity()
// Calculate cross products
rnA := math32.NewVec3().CrossVectors(ce.rA, ce.nA)
rnB := math32.NewVec3().CrossVectors(ce.rB, ce.nA)
// g = xj+rB -(xi+rA)
// G = [ -nA -rnA nA rnB ]
ce.jeA.SetSpatial(ce.nA.Clone().Negate())
ce.jeA.SetRotational(rnA.Clone().Negate())
ce.jeB.SetSpatial(ce.nA.Clone())
ce.jeB.SetRotational(rnB.Clone())
// Calculate the penetration vector
posA := ce.bA.Position()
posB := ce.bB.Position()
penetrationVec := ce.rB.Clone().Add(&posB).Sub(ce.rA).Sub(&posA)
g := ce.nA.Dot(penetrationVec)
// Compute iteration
ePlusOne := ce.restitution + 1
GW := ePlusOne * vB.Dot(ce.nA) - ePlusOne * vA.Dot(ce.nA) + wB.Dot(rnB) - wA.Dot(rnA)
GiMf := ce.ComputeGiMf()
return -g*ce.a - GW*ce.b - h*GiMf
}
//// GetImpactVelocityAlongNormal returns the current relative velocity at the contact point.
//func (ce *Contact) GetImpactVelocityAlongNormal() float32 {
//
// xi := ce.bA.Position().Add(ce.rA)
// xj := ce.bB.Position().Add(ce.rB)
//
// vi := ce.bA.GetVelocityAtWorldPoint(xi)
// vj := ce.bB.GetVelocityAtWorldPoint(xj)
//
// relVel := math32.NewVec3().SubVectors(vi, vj)
//
// return ce.nA.Dot(relVel)
//}