-
Notifications
You must be signed in to change notification settings - Fork 1
/
WheelCast.gd
178 lines (123 loc) · 4.82 KB
/
WheelCast.gd
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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
extends RayCast
var wheelName = "" setget setWheelName, getWheelName
var BUMP = 0.7 # default: 0.95, seems to be stiff and non-bouncy
var REBOUND = 0.065 # default: 0.065
var SPRING = 3
var SPRING_LENGTH = 1
var suspensionVec = Vector3(0, 0, 0)
var collPoint = Vector3(0, 0, 0)
var parentBody: RigidBody
var springApplicationRatio
var prev_pos
var z_vel
export(float, 0, 2, 0.05) var peak = 1
export(float, 1, 2.5, 0.05) var x_shape = 1.35
export(float, 1, 2.5, 0.05) var z_shape = 1.65
export(float, 0, 20, 0.1) var stiff = 10
export(float, -10, 0, 0.05) var curve = 0
var wheelForce1 = Vector3(0, 0, 0)
var wheelForce2 = Vector3(0, 0, 0)
var wheelForce3 = Vector3(0, 0, 0)
var testVec = Vector3(0, 0, 0)
var y_force = 0
var compress = 0
var prev_compress = 0
var suspensionHeight
var normal = Vector3(0, 0, 0)
var wheelPosition setget setWheelPosition, getWheelPosition
var upwardForce : Vector3 = Vector3(0, 0, 0) setget setUpwardForce, getUpwardForce
var indicator
# Called when the node enters the scene tree for the first time.
func _ready():
parentBody = get_parent()
indicator = get_node("Indicator")
prev_pos = global_transform.origin
var local_vel
var x_slip
var z_slip
func calculate(delta):
collPoint = get_collision_point()
indicator.global_transform.origin = collPoint
suspension(delta)
#
# (global_transform.origin - prev_pos) / delta = movement in global coordinates
# global_transform xform_inv transforms from global to local
local_vel = global_transform.basis.xform_inv((global_transform.origin - prev_pos) / delta)
# local_vel = transform.basis.xform(local_vel)
z_vel = -local_vel.z
var planar_vect = Vector2(local_vel.x, local_vel.z).normalized()
prev_pos = global_transform.origin
x_slip = asin(clamp(-planar_vect.x, -1, 1))
var spin = 2
var radius = 1
z_slip = 0
if z_vel != 0:
z_slip = (spin * radius - z_vel) / abs(z_vel)
var x_force = pacejka(x_slip, x_shape)
var z_force = pacejka(z_slip, z_shape)
if is_colliding():
var contact = get_collision_point() - parentBody.global_transform.origin
wheelForce1 = global_transform.basis.x * x_force
wheelForce2 = normal * y_force
wheelForce3 = global_transform.basis.z * z_force
# if wheelName == "fl":
# testVec.x = 1
# parentBody.add_force(testVec, contact)
parentBody.add_force(wheelForce1 * 80, contact)
## parentBody.add_force(wheelForce2, contact)
# parentBody.add_force(-wheelForce3, contact)
func pacejka(slip, t_shape):
# var v1 = atan(stiff * slip)
# var v2 = (stiff * slip - v1)
# var v3 = stiff * slip - curve * v2
# var v4 = t_shape * atan(v3)
# return y_force * peak * sin(v4)
return y_force * peak * sin(t_shape * atan(stiff * slip - curve * (stiff * slip - atan(stiff * slip))))
func suspension(delta : float) -> void:
prev_compress = compress
if is_colliding():
suspensionVec = global_transform.origin - collPoint
suspensionHeight = global_transform.origin.distance_to(collPoint)
compress = 1 - (suspensionHeight / SPRING_LENGTH) # 1 = fully compressed
y_force = SPRING * compress * SPRING_LENGTH
if compress >= prev_compress: # compressing
var v = BUMP * (compress - prev_compress) / delta
y_force += v
else: # decompressing
var v = REBOUND * (compress - prev_compress) / delta
y_force += v
# upwardForce.y = y_force
# upwardForce = transform.basis.y * y_force
# upwardForce.y = global_transform.basis.xform(transform.basis.y * y_force).y
# springApplicationRatio = upwardForce.normalized().dot(global_transform.basis.y)
# upwardForce = upwardForce * springApplicationRatio
# parentBody.apply_impulse(parentBody.global_transform.basis.xform(parentBody.to_local(collPoint)), upwardForce)
# parentBody.add_force(upwardForce, parentBody.global_transform.basis.xform(parentBody.to_local(collPoint)))
var contact = get_collision_point() - parentBody.global_transform.origin
var normal = get_collision_normal()
# Check helps eliminate force spikes when tumbling
upwardForce = normal * y_force
parentBody.apply_impulse(contact, normal * y_force)
else:
springApplicationRatio = 0
upwardForce = Vector3(0, 0, 0)
func setWheelPosition(pos: Vector3):
print("Set wheel position")
func getWheelPosition():
print("Get wheel position")
func setUpwardForce(vec: Vector3):
upwardForce = vec
func getUpwardForce():
return upwardForce
func setWheelName(text: String):
wheelName = text
# if wheelName == "fl":
DebugOverlay.draw.add_vector(self, "wheelForce1", collPoint, false, 2, 4, Color(1,1,1, 0.5))
DebugOverlay.draw.add_vector(self, "wheelForce3", collPoint, false, 2, 4, Color(1,0,0, 0.5))
func getWheelName():
return upwardForce
# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta):
var varToWrite = z_slip
DebugOverlay.wheelText[wheelName].set_text(wheelName + ": " + (" " if varToWrite >=0 else "") + str(stepify(varToWrite, 0.001)))
# pass