1/**************************************************************************/
2/* vehicle_body_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "vehicle_body_3d.h"
32
33#define ROLLING_INFLUENCE_FIX
34
35class btVehicleJacobianEntry {
36public:
37 Vector3 m_linearJointAxis;
38 Vector3 m_aJ;
39 Vector3 m_bJ;
40 Vector3 m_0MinvJt;
41 Vector3 m_1MinvJt;
42 //Optimization: can be stored in the w/last component of one of the vectors
43 real_t m_Adiag = 0.0;
44
45 real_t getDiagonal() const { return m_Adiag; }
46
47 btVehicleJacobianEntry() {}
48 //constraint between two different rigidbodies
49 btVehicleJacobianEntry(
50 const Basis &world2A,
51 const Basis &world2B,
52 const Vector3 &rel_pos1,
53 const Vector3 &rel_pos2,
54 const Vector3 &jointAxis,
55 const Vector3 &inertiaInvA,
56 const real_t massInvA,
57 const Vector3 &inertiaInvB,
58 const real_t massInvB) :
59 m_linearJointAxis(jointAxis) {
60 m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis));
61 m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis));
62 m_0MinvJt = inertiaInvA * m_aJ;
63 m_1MinvJt = inertiaInvB * m_bJ;
64 m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
65
66 //btAssert(m_Adiag > real_t(0.0));
67 }
68
69 real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) {
70 Vector3 linrel = linvelA - linvelB;
71 Vector3 angvela = angvelA * m_aJ;
72 Vector3 angvelb = angvelB * m_bJ;
73 linrel *= m_linearJointAxis;
74 angvela += angvelb;
75 angvela += linrel;
76 real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2];
77 return rel_vel2 + CMP_EPSILON;
78 }
79};
80
81void VehicleWheel3D::_notification(int p_what) {
82 switch (p_what) {
83 case NOTIFICATION_ENTER_TREE: {
84 VehicleBody3D *cb = Object::cast_to<VehicleBody3D>(get_parent());
85 if (!cb) {
86 return;
87 }
88 body = cb;
89 local_xform = get_transform();
90 cb->wheels.push_back(this);
91
92 m_chassisConnectionPointCS = get_transform().origin;
93 m_wheelDirectionCS = -get_transform().basis.get_column(Vector3::AXIS_Y).normalized();
94 m_wheelAxleCS = get_transform().basis.get_column(Vector3::AXIS_X).normalized();
95 } break;
96
97 case NOTIFICATION_EXIT_TREE: {
98 VehicleBody3D *cb = Object::cast_to<VehicleBody3D>(get_parent());
99 if (!cb) {
100 return;
101 }
102 cb->wheels.erase(this);
103 body = nullptr;
104 } break;
105 }
106}
107
108PackedStringArray VehicleWheel3D::get_configuration_warnings() const {
109 PackedStringArray warnings = Node::get_configuration_warnings();
110
111 if (!Object::cast_to<VehicleBody3D>(get_parent())) {
112 warnings.push_back(RTR("VehicleWheel3D serves to provide a wheel system to a VehicleBody3D. Please use it as a child of a VehicleBody3D."));
113 }
114
115 return warnings;
116}
117
118void VehicleWheel3D::_update(PhysicsDirectBodyState3D *s) {
119 if (m_raycastInfo.m_isInContact) {
120 real_t project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS);
121 Vector3 chassis_velocity_at_contactPoint;
122 Vector3 relpos = m_raycastInfo.m_contactPointWS - s->get_transform().origin;
123
124 chassis_velocity_at_contactPoint = s->get_linear_velocity() +
125 (s->get_angular_velocity()).cross(relpos); // * mPos);
126
127 real_t projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
128 if (project >= real_t(-0.1)) {
129 m_suspensionRelativeVelocity = real_t(0.0);
130 m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1);
131 } else {
132 real_t inv = real_t(-1.) / project;
133 m_suspensionRelativeVelocity = projVel * inv;
134 m_clippedInvContactDotSuspension = inv;
135 }
136 } else { // Not in contact : position wheel in a nice (rest length) position
137 m_raycastInfo.m_suspensionLength = m_suspensionRestLength;
138 m_suspensionRelativeVelocity = real_t(0.0);
139 m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
140 m_clippedInvContactDotSuspension = real_t(1.0);
141 }
142}
143
144void VehicleWheel3D::set_radius(real_t p_radius) {
145 m_wheelRadius = p_radius;
146 update_gizmos();
147}
148
149real_t VehicleWheel3D::get_radius() const {
150 return m_wheelRadius;
151}
152
153void VehicleWheel3D::set_suspension_rest_length(real_t p_length) {
154 m_suspensionRestLength = p_length;
155 update_gizmos();
156}
157
158real_t VehicleWheel3D::get_suspension_rest_length() const {
159 return m_suspensionRestLength;
160}
161
162void VehicleWheel3D::set_suspension_travel(real_t p_length) {
163 m_maxSuspensionTravel = p_length;
164}
165
166real_t VehicleWheel3D::get_suspension_travel() const {
167 return m_maxSuspensionTravel;
168}
169
170void VehicleWheel3D::set_suspension_stiffness(real_t p_value) {
171 m_suspensionStiffness = p_value;
172}
173
174real_t VehicleWheel3D::get_suspension_stiffness() const {
175 return m_suspensionStiffness;
176}
177
178void VehicleWheel3D::set_suspension_max_force(real_t p_value) {
179 m_maxSuspensionForce = p_value;
180}
181
182real_t VehicleWheel3D::get_suspension_max_force() const {
183 return m_maxSuspensionForce;
184}
185
186void VehicleWheel3D::set_damping_compression(real_t p_value) {
187 m_wheelsDampingCompression = p_value;
188}
189
190real_t VehicleWheel3D::get_damping_compression() const {
191 return m_wheelsDampingCompression;
192}
193
194void VehicleWheel3D::set_damping_relaxation(real_t p_value) {
195 m_wheelsDampingRelaxation = p_value;
196}
197
198real_t VehicleWheel3D::get_damping_relaxation() const {
199 return m_wheelsDampingRelaxation;
200}
201
202void VehicleWheel3D::set_friction_slip(real_t p_value) {
203 m_frictionSlip = p_value;
204}
205
206real_t VehicleWheel3D::get_friction_slip() const {
207 return m_frictionSlip;
208}
209
210void VehicleWheel3D::set_roll_influence(real_t p_value) {
211 m_rollInfluence = p_value;
212}
213
214real_t VehicleWheel3D::get_roll_influence() const {
215 return m_rollInfluence;
216}
217
218bool VehicleWheel3D::is_in_contact() const {
219 return m_raycastInfo.m_isInContact;
220}
221
222Node3D *VehicleWheel3D::get_contact_body() const {
223 return m_raycastInfo.m_groundObject;
224}
225
226void VehicleWheel3D::_bind_methods() {
227 ClassDB::bind_method(D_METHOD("set_radius", "length"), &VehicleWheel3D::set_radius);
228 ClassDB::bind_method(D_METHOD("get_radius"), &VehicleWheel3D::get_radius);
229
230 ClassDB::bind_method(D_METHOD("set_suspension_rest_length", "length"), &VehicleWheel3D::set_suspension_rest_length);
231 ClassDB::bind_method(D_METHOD("get_suspension_rest_length"), &VehicleWheel3D::get_suspension_rest_length);
232
233 ClassDB::bind_method(D_METHOD("set_suspension_travel", "length"), &VehicleWheel3D::set_suspension_travel);
234 ClassDB::bind_method(D_METHOD("get_suspension_travel"), &VehicleWheel3D::get_suspension_travel);
235
236 ClassDB::bind_method(D_METHOD("set_suspension_stiffness", "length"), &VehicleWheel3D::set_suspension_stiffness);
237 ClassDB::bind_method(D_METHOD("get_suspension_stiffness"), &VehicleWheel3D::get_suspension_stiffness);
238
239 ClassDB::bind_method(D_METHOD("set_suspension_max_force", "length"), &VehicleWheel3D::set_suspension_max_force);
240 ClassDB::bind_method(D_METHOD("get_suspension_max_force"), &VehicleWheel3D::get_suspension_max_force);
241
242 ClassDB::bind_method(D_METHOD("set_damping_compression", "length"), &VehicleWheel3D::set_damping_compression);
243 ClassDB::bind_method(D_METHOD("get_damping_compression"), &VehicleWheel3D::get_damping_compression);
244
245 ClassDB::bind_method(D_METHOD("set_damping_relaxation", "length"), &VehicleWheel3D::set_damping_relaxation);
246 ClassDB::bind_method(D_METHOD("get_damping_relaxation"), &VehicleWheel3D::get_damping_relaxation);
247
248 ClassDB::bind_method(D_METHOD("set_use_as_traction", "enable"), &VehicleWheel3D::set_use_as_traction);
249 ClassDB::bind_method(D_METHOD("is_used_as_traction"), &VehicleWheel3D::is_used_as_traction);
250
251 ClassDB::bind_method(D_METHOD("set_use_as_steering", "enable"), &VehicleWheel3D::set_use_as_steering);
252 ClassDB::bind_method(D_METHOD("is_used_as_steering"), &VehicleWheel3D::is_used_as_steering);
253
254 ClassDB::bind_method(D_METHOD("set_friction_slip", "length"), &VehicleWheel3D::set_friction_slip);
255 ClassDB::bind_method(D_METHOD("get_friction_slip"), &VehicleWheel3D::get_friction_slip);
256
257 ClassDB::bind_method(D_METHOD("is_in_contact"), &VehicleWheel3D::is_in_contact);
258 ClassDB::bind_method(D_METHOD("get_contact_body"), &VehicleWheel3D::get_contact_body);
259
260 ClassDB::bind_method(D_METHOD("set_roll_influence", "roll_influence"), &VehicleWheel3D::set_roll_influence);
261 ClassDB::bind_method(D_METHOD("get_roll_influence"), &VehicleWheel3D::get_roll_influence);
262
263 ClassDB::bind_method(D_METHOD("get_skidinfo"), &VehicleWheel3D::get_skidinfo);
264
265 ClassDB::bind_method(D_METHOD("get_rpm"), &VehicleWheel3D::get_rpm);
266
267 ClassDB::bind_method(D_METHOD("set_engine_force", "engine_force"), &VehicleWheel3D::set_engine_force);
268 ClassDB::bind_method(D_METHOD("get_engine_force"), &VehicleWheel3D::get_engine_force);
269
270 ClassDB::bind_method(D_METHOD("set_brake", "brake"), &VehicleWheel3D::set_brake);
271 ClassDB::bind_method(D_METHOD("get_brake"), &VehicleWheel3D::get_brake);
272
273 ClassDB::bind_method(D_METHOD("set_steering", "steering"), &VehicleWheel3D::set_steering);
274 ClassDB::bind_method(D_METHOD("get_steering"), &VehicleWheel3D::get_steering);
275
276 ADD_GROUP("Per-Wheel Motion", "");
277 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "engine_force", PROPERTY_HINT_RANGE, U"-1024,1024,0.01,or_less,or_greater,suffix:kg\u22C5m/s\u00B2 (N)"), "set_engine_force", "get_engine_force");
278 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "brake", PROPERTY_HINT_RANGE, U"-128,128,0.01,or_less,or_greater,suffix:kg\u22C5m/s\u00B2 (N)"), "set_brake", "get_brake");
279 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "steering", PROPERTY_HINT_RANGE, "-180,180,0.01,radians"), "set_steering", "get_steering");
280 ADD_GROUP("VehicleBody3D Motion", "");
281 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_as_traction"), "set_use_as_traction", "is_used_as_traction");
282 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_as_steering"), "set_use_as_steering", "is_used_as_steering");
283 ADD_GROUP("Wheel", "wheel_");
284 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_roll_influence"), "set_roll_influence", "get_roll_influence");
285 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_radius", PROPERTY_HINT_NONE, "suffix:m"), "set_radius", "get_radius");
286 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_rest_length", PROPERTY_HINT_NONE, "suffix:m"), "set_suspension_rest_length", "get_suspension_rest_length");
287 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_friction_slip"), "set_friction_slip", "get_friction_slip");
288 ADD_GROUP("Suspension", "suspension_");
289 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "suspension_travel", PROPERTY_HINT_NONE, "suffix:m"), "set_suspension_travel", "get_suspension_travel");
290 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "suspension_stiffness"), "set_suspension_stiffness", "get_suspension_stiffness");
291 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "suspension_max_force", PROPERTY_HINT_NONE, U"suffix:kg\u22C5m/s\u00B2 (N)"), "set_suspension_max_force", "get_suspension_max_force");
292 ADD_GROUP("Damping", "damping_");
293 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "damping_compression"), "set_damping_compression", "get_damping_compression");
294 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "damping_relaxation"), "set_damping_relaxation", "get_damping_relaxation");
295}
296
297void VehicleWheel3D::set_engine_force(real_t p_engine_force) {
298 m_engineForce = p_engine_force;
299}
300
301real_t VehicleWheel3D::get_engine_force() const {
302 return m_engineForce;
303}
304
305void VehicleWheel3D::set_brake(real_t p_brake) {
306 m_brake = p_brake;
307}
308
309real_t VehicleWheel3D::get_brake() const {
310 return m_brake;
311}
312
313void VehicleWheel3D::set_steering(real_t p_steering) {
314 m_steering = p_steering;
315}
316
317real_t VehicleWheel3D::get_steering() const {
318 return m_steering;
319}
320
321void VehicleWheel3D::set_use_as_traction(bool p_enable) {
322 engine_traction = p_enable;
323}
324
325bool VehicleWheel3D::is_used_as_traction() const {
326 return engine_traction;
327}
328
329void VehicleWheel3D::set_use_as_steering(bool p_enabled) {
330 steers = p_enabled;
331}
332
333bool VehicleWheel3D::is_used_as_steering() const {
334 return steers;
335}
336
337real_t VehicleWheel3D::get_skidinfo() const {
338 return m_skidInfo;
339}
340
341real_t VehicleWheel3D::get_rpm() const {
342 return m_rpm;
343}
344
345VehicleWheel3D::VehicleWheel3D() {
346}
347
348void VehicleBody3D::_update_wheel_transform(VehicleWheel3D &wheel, PhysicsDirectBodyState3D *s) {
349 wheel.m_raycastInfo.m_isInContact = false;
350
351 Transform3D chassisTrans = s->get_transform();
352 /*
353 if (interpolatedTransform && (getRigidBody()->getMotionState())) {
354 getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
355 }
356 */
357
358 wheel.m_raycastInfo.m_hardPointWS = chassisTrans.xform(wheel.m_chassisConnectionPointCS);
359 //wheel.m_raycastInfo.m_hardPointWS+=s->get_linear_velocity()*s->get_step();
360 wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.get_basis().xform(wheel.m_wheelDirectionCS).normalized();
361 wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.get_basis().xform(wheel.m_wheelAxleCS).normalized();
362}
363
364void VehicleBody3D::_update_wheel(int p_idx, PhysicsDirectBodyState3D *s) {
365 VehicleWheel3D &wheel = *wheels[p_idx];
366 _update_wheel_transform(wheel, s);
367
368 Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
369 const Vector3 &right = wheel.m_raycastInfo.m_wheelAxleWS;
370 Vector3 fwd = up.cross(right);
371 fwd = fwd.normalized();
372
373 Basis steeringMat(up, wheel.m_steering);
374
375 Basis rotatingMat(right, wheel.m_rotation);
376
377 Basis basis2(
378 right[0], up[0], fwd[0],
379 right[1], up[1], fwd[1],
380 right[2], up[2], fwd[2]);
381
382 wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2);
383 //wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat));
384 wheel.m_worldTransform.set_origin(
385 wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
386}
387
388real_t VehicleBody3D::_ray_cast(int p_idx, PhysicsDirectBodyState3D *s) {
389 VehicleWheel3D &wheel = *wheels[p_idx];
390
391 _update_wheel_transform(wheel, s);
392
393 real_t depth = -1;
394
395 real_t raylen = wheel.m_suspensionRestLength + wheel.m_wheelRadius;
396
397 Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
398 Vector3 source = wheel.m_raycastInfo.m_hardPointWS;
399 wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
400 const Vector3 &target = wheel.m_raycastInfo.m_contactPointWS;
401 source -= wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS;
402
403 real_t param = real_t(0.);
404
405 PhysicsDirectSpaceState3D::RayResult rr;
406
407 PhysicsDirectSpaceState3D *ss = s->get_space_state();
408
409 PhysicsDirectSpaceState3D::RayParameters ray_params;
410 ray_params.from = source;
411 ray_params.to = target;
412 ray_params.exclude = exclude;
413 ray_params.collision_mask = get_collision_mask();
414
415 wheel.m_raycastInfo.m_groundObject = nullptr;
416 bool col = ss->intersect_ray(ray_params, rr);
417
418 if (col) {
419 param = source.distance_to(rr.position) / source.distance_to(target);
420 depth = raylen * param;
421 wheel.m_raycastInfo.m_contactNormalWS = rr.normal;
422
423 wheel.m_raycastInfo.m_isInContact = true;
424 if (rr.collider) {
425 wheel.m_raycastInfo.m_groundObject = Object::cast_to<PhysicsBody3D>(rr.collider);
426 }
427
428 real_t hitDistance = param * raylen;
429 wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelRadius;
430 //clamp on max suspension travel
431
432 real_t minSuspensionLength = wheel.m_suspensionRestLength - wheel.m_maxSuspensionTravel;
433 real_t maxSuspensionLength = wheel.m_suspensionRestLength + wheel.m_maxSuspensionTravel;
434 if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) {
435 wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
436 }
437 if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) {
438 wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
439 }
440
441 wheel.m_raycastInfo.m_contactPointWS = rr.position;
442
443 real_t denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS);
444
445 Vector3 chassis_velocity_at_contactPoint;
446 //Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
447
448 //chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
449
450 chassis_velocity_at_contactPoint = s->get_linear_velocity() +
451 (s->get_angular_velocity()).cross(wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin); // * mPos);
452
453 real_t projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
454
455 if (denominator >= real_t(-0.1)) {
456 wheel.m_suspensionRelativeVelocity = real_t(0.0);
457 wheel.m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1);
458 } else {
459 real_t inv = real_t(-1.) / denominator;
460 wheel.m_suspensionRelativeVelocity = projVel * inv;
461 wheel.m_clippedInvContactDotSuspension = inv;
462 }
463
464 } else {
465 wheel.m_raycastInfo.m_isInContact = false;
466 //put wheel info as in rest position
467 wheel.m_raycastInfo.m_suspensionLength = wheel.m_suspensionRestLength;
468 wheel.m_suspensionRelativeVelocity = real_t(0.0);
469 wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
470 wheel.m_clippedInvContactDotSuspension = real_t(1.0);
471 }
472
473 return depth;
474}
475
476void VehicleBody3D::_update_suspension(PhysicsDirectBodyState3D *s) {
477 real_t chassisMass = get_mass();
478
479 for (int w_it = 0; w_it < wheels.size(); w_it++) {
480 VehicleWheel3D &wheel_info = *wheels[w_it];
481
482 if (wheel_info.m_raycastInfo.m_isInContact) {
483 real_t force;
484 //Spring
485 {
486 real_t susp_length = wheel_info.m_suspensionRestLength;
487 real_t current_length = wheel_info.m_raycastInfo.m_suspensionLength;
488
489 real_t length_diff = (susp_length - current_length);
490
491 force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension;
492 }
493
494 // Damper
495 {
496 real_t projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
497 {
498 real_t susp_damping;
499 if (projected_rel_vel < real_t(0.0)) {
500 susp_damping = wheel_info.m_wheelsDampingCompression;
501 } else {
502 susp_damping = wheel_info.m_wheelsDampingRelaxation;
503 }
504 force -= susp_damping * projected_rel_vel;
505 }
506 }
507
508 // RESULT
509 wheel_info.m_wheelsSuspensionForce = force * chassisMass;
510 if (wheel_info.m_wheelsSuspensionForce < real_t(0.)) {
511 wheel_info.m_wheelsSuspensionForce = real_t(0.);
512 }
513 } else {
514 wheel_info.m_wheelsSuspensionForce = real_t(0.0);
515 }
516 }
517}
518
519//bilateral constraint between two dynamic objects
520void VehicleBody3D::_resolve_single_bilateral(PhysicsDirectBodyState3D *s, const Vector3 &pos1,
521 PhysicsBody3D *body2, const Vector3 &pos2, const Vector3 &normal, real_t &impulse, const real_t p_rollInfluence) {
522 real_t normalLenSqr = normal.length_squared();
523 //ERR_FAIL_COND( normalLenSqr < real_t(1.1));
524
525 if (normalLenSqr > real_t(1.1)) {
526 impulse = real_t(0.);
527 return;
528 }
529
530 Vector3 rel_pos1 = pos1 - s->get_transform().origin;
531 Vector3 rel_pos2;
532 if (body2) {
533 rel_pos2 = pos2 - body2->get_global_transform().origin;
534 }
535 //this jacobian entry could be re-used for all iterations
536
537 Vector3 vel1 = s->get_linear_velocity() + (s->get_angular_velocity()).cross(rel_pos1); // * mPos);
538 Vector3 vel2;
539
540 if (body2) {
541 vel2 = body2->get_linear_velocity() + body2->get_angular_velocity().cross(rel_pos2);
542 }
543
544 Vector3 vel = vel1 - vel2;
545
546 Basis b2trans;
547 real_t b2invmass = 0;
548 Vector3 b2lv;
549 Vector3 b2av;
550 Vector3 b2invinertia; //todo
551
552 if (body2) {
553 b2trans = body2->get_global_transform().basis.transposed();
554 b2invmass = body2->get_inverse_mass();
555 b2lv = body2->get_linear_velocity();
556 b2av = body2->get_angular_velocity();
557 }
558
559 btVehicleJacobianEntry jac(s->get_transform().basis.transposed(),
560 b2trans,
561 rel_pos1,
562 rel_pos2,
563 normal,
564 s->get_inverse_inertia_tensor().get_main_diagonal(),
565 1.0 / get_mass(),
566 b2invinertia,
567 b2invmass);
568
569 // FIXME: rel_vel assignment here is overwritten by the following assignment.
570 // What seems to be intended in the next assignment is: rel_vel = normal.dot(rel_vel);
571 // Investigate why.
572 real_t rel_vel = jac.getRelativeVelocity(
573 s->get_linear_velocity(),
574 s->get_transform().basis.transposed().xform(s->get_angular_velocity()),
575 b2lv,
576 b2trans.xform(b2av));
577
578 rel_vel = normal.dot(vel);
579
580 // !BAS! We had this set to 0.4, in bullet its 0.2
581 real_t contactDamping = real_t(0.2);
582
583 if (p_rollInfluence > 0.0) {
584 // !BAS! But seeing we apply this frame by frame, makes more sense to me to make this time based
585 // keeping in mind our anti roll factor if it is set
586 contactDamping = MIN(contactDamping, s->get_step() / p_rollInfluence);
587 }
588
589#define ONLY_USE_LINEAR_MASS
590#ifdef ONLY_USE_LINEAR_MASS
591 real_t massTerm = real_t(1.) / ((1.0 / get_mass()) + b2invmass);
592 impulse = -contactDamping * rel_vel * massTerm;
593#else
594 real_t velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
595 impulse = velocityImpulse;
596#endif
597}
598
599VehicleBody3D::btVehicleWheelContactPoint::btVehicleWheelContactPoint(PhysicsDirectBodyState3D *s, PhysicsBody3D *body1, const Vector3 &frictionPosWorld, const Vector3 &frictionDirectionWorld, real_t maxImpulse) :
600 m_s(s),
601 m_body1(body1),
602 m_frictionPositionWorld(frictionPosWorld),
603 m_frictionDirectionWorld(frictionDirectionWorld),
604 m_maxImpulse(maxImpulse) {
605 real_t denom0 = 0;
606 real_t denom1 = 0;
607
608 {
609 Vector3 r0 = frictionPosWorld - s->get_transform().origin;
610 Vector3 c0 = (r0).cross(frictionDirectionWorld);
611 Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0);
612 denom0 = s->get_inverse_mass() + frictionDirectionWorld.dot(vec);
613 }
614
615 /* TODO: Why is this code unused?
616 if (body1) {
617 Vector3 r0 = frictionPosWorld - body1->get_global_transform().origin;
618 Vector3 c0 = (r0).cross(frictionDirectionWorld);
619 Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0);
620 //denom1= body1->get_inverse_mass() + frictionDirectionWorld.dot(vec);
621
622 }
623 */
624
625 real_t relaxation = 1.f;
626 m_jacDiagABInv = relaxation / (denom0 + denom1);
627}
628
629real_t VehicleBody3D::_calc_rolling_friction(btVehicleWheelContactPoint &contactPoint) {
630 real_t j1 = 0.f;
631
632 const Vector3 &contactPosWorld = contactPoint.m_frictionPositionWorld;
633
634 Vector3 rel_pos1 = contactPosWorld - contactPoint.m_s->get_transform().origin;
635 Vector3 rel_pos2;
636 if (contactPoint.m_body1) {
637 rel_pos2 = contactPosWorld - contactPoint.m_body1->get_global_transform().origin;
638 }
639
640 real_t maxImpulse = contactPoint.m_maxImpulse;
641
642 Vector3 vel1 = contactPoint.m_s->get_linear_velocity() + (contactPoint.m_s->get_angular_velocity()).cross(rel_pos1); // * mPos);
643
644 Vector3 vel2;
645 if (contactPoint.m_body1) {
646 vel2 = contactPoint.m_body1->get_linear_velocity() + contactPoint.m_body1->get_angular_velocity().cross(rel_pos2);
647 }
648
649 Vector3 vel = vel1 - vel2;
650
651 real_t vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
652
653 // calculate j that moves us to zero relative velocity
654 j1 = -vrel * contactPoint.m_jacDiagABInv;
655
656 return CLAMP(j1, -maxImpulse, maxImpulse);
657}
658
659static const real_t sideFrictionStiffness2 = real_t(1.0);
660void VehicleBody3D::_update_friction(PhysicsDirectBodyState3D *s) {
661 //calculate the impulse, so that the wheels don't move sidewards
662 int numWheel = wheels.size();
663 if (!numWheel) {
664 return;
665 }
666
667 m_forwardWS.resize(numWheel);
668 m_axle.resize(numWheel);
669 m_forwardImpulse.resize(numWheel);
670 m_sideImpulse.resize(numWheel);
671
672 //collapse all those loops into one!
673 for (int i = 0; i < wheels.size(); i++) {
674 m_sideImpulse.write[i] = real_t(0.);
675 m_forwardImpulse.write[i] = real_t(0.);
676 }
677
678 {
679 for (int i = 0; i < wheels.size(); i++) {
680 VehicleWheel3D &wheelInfo = *wheels[i];
681
682 if (wheelInfo.m_raycastInfo.m_isInContact) {
683 //const btTransform& wheelTrans = getWheelTransformWS( i );
684
685 Basis wheelBasis0 = wheelInfo.m_worldTransform.basis; //get_global_transform().basis;
686
687 m_axle.write[i] = wheelBasis0.get_column(Vector3::AXIS_X);
688 //m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS;
689
690 const Vector3 &surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
691 real_t proj = m_axle[i].dot(surfNormalWS);
692 m_axle.write[i] -= surfNormalWS * proj;
693 m_axle.write[i] = m_axle[i].normalized();
694
695 m_forwardWS.write[i] = surfNormalWS.cross(m_axle[i]);
696 m_forwardWS.write[i].normalize();
697
698 _resolve_single_bilateral(s, wheelInfo.m_raycastInfo.m_contactPointWS,
699 wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
700 m_axle[i], m_sideImpulse.write[i], wheelInfo.m_rollInfluence);
701
702 m_sideImpulse.write[i] *= sideFrictionStiffness2;
703 }
704 }
705 }
706
707 real_t sideFactor = real_t(1.);
708 real_t fwdFactor = 0.5;
709
710 bool sliding = false;
711 {
712 for (int wheel = 0; wheel < wheels.size(); wheel++) {
713 VehicleWheel3D &wheelInfo = *wheels[wheel];
714
715 //class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
716
717 real_t rollingFriction = 0.f;
718
719 if (wheelInfo.m_raycastInfo.m_isInContact) {
720 if (wheelInfo.m_engineForce != 0.f) {
721 rollingFriction = -wheelInfo.m_engineForce * s->get_step();
722 } else {
723 real_t defaultRollingFrictionImpulse = 0.f;
724 real_t maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
725 btVehicleWheelContactPoint contactPt(s, wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse);
726 rollingFriction = _calc_rolling_friction(contactPt);
727 }
728 }
729
730 //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
731
732 m_forwardImpulse.write[wheel] = real_t(0.);
733 wheelInfo.m_skidInfo = real_t(1.);
734
735 if (wheelInfo.m_raycastInfo.m_isInContact) {
736 wheelInfo.m_skidInfo = real_t(1.);
737
738 real_t maximp = wheelInfo.m_wheelsSuspensionForce * s->get_step() * wheelInfo.m_frictionSlip;
739 real_t maximpSide = maximp;
740
741 real_t maximpSquared = maximp * maximpSide;
742
743 m_forwardImpulse.write[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep;
744
745 real_t x = (m_forwardImpulse[wheel]) * fwdFactor;
746 real_t y = (m_sideImpulse[wheel]) * sideFactor;
747
748 real_t impulseSquared = (x * x + y * y);
749
750 if (impulseSquared > maximpSquared) {
751 sliding = true;
752
753 real_t factor = maximp / Math::sqrt(impulseSquared);
754
755 wheelInfo.m_skidInfo *= factor;
756 }
757 }
758 }
759 }
760
761 if (sliding) {
762 for (int wheel = 0; wheel < wheels.size(); wheel++) {
763 if (m_sideImpulse[wheel] != real_t(0.)) {
764 if (wheels[wheel]->m_skidInfo < real_t(1.)) {
765 m_forwardImpulse.write[wheel] *= wheels[wheel]->m_skidInfo;
766 m_sideImpulse.write[wheel] *= wheels[wheel]->m_skidInfo;
767 }
768 }
769 }
770 }
771
772 // apply the impulses
773 {
774 for (int wheel = 0; wheel < wheels.size(); wheel++) {
775 VehicleWheel3D &wheelInfo = *wheels[wheel];
776
777 Vector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
778 s->get_transform().origin;
779
780 if (m_forwardImpulse[wheel] != real_t(0.)) {
781 s->apply_impulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
782 }
783 if (m_sideImpulse[wheel] != real_t(0.)) {
784 PhysicsBody3D *groundObject = wheelInfo.m_raycastInfo.m_groundObject;
785
786 Vector3 rel_pos2;
787 if (groundObject) {
788 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->get_global_transform().origin;
789 }
790
791 Vector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
792
793#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
794 Vector3 vChassisWorldUp = s->get_transform().basis.transposed()[1]; //getRigidBody()->getCenterOfMassTransform3D().getBasis().getColumn(m_indexUpAxis);
795 rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence));
796#else
797 rel_pos[1] *= wheelInfo.m_rollInfluence; //?
798#endif
799 s->apply_impulse(sideImp, rel_pos);
800
801 //apply friction impulse on the ground
802 //todo
803 //groundObject->applyImpulse(-sideImp,rel_pos2);
804 }
805 }
806 }
807}
808
809void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
810 RigidBody3D::_body_state_changed(p_state);
811
812 real_t step = p_state->get_step();
813
814 for (int i = 0; i < wheels.size(); i++) {
815 _update_wheel(i, p_state);
816 }
817
818 for (int i = 0; i < wheels.size(); i++) {
819 _ray_cast(i, p_state);
820 wheels[i]->set_transform(p_state->get_transform().inverse() * wheels[i]->m_worldTransform);
821 }
822
823 _update_suspension(p_state);
824
825 for (int i = 0; i < wheels.size(); i++) {
826 //apply suspension force
827 VehicleWheel3D &wheel = *wheels[i];
828
829 real_t suspensionForce = wheel.m_wheelsSuspensionForce;
830
831 if (suspensionForce > wheel.m_maxSuspensionForce) {
832 suspensionForce = wheel.m_maxSuspensionForce;
833 }
834 Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
835 Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - p_state->get_transform().origin;
836
837 p_state->apply_impulse(impulse, relative_position);
838 }
839
840 _update_friction(p_state);
841
842 for (int i = 0; i < wheels.size(); i++) {
843 VehicleWheel3D &wheel = *wheels[i];
844 Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - p_state->get_transform().origin;
845 Vector3 vel = p_state->get_linear_velocity() + (p_state->get_angular_velocity()).cross(relpos); // * mPos);
846
847 if (wheel.m_raycastInfo.m_isInContact) {
848 const Transform3D &chassisWorldTransform = p_state->get_transform();
849
850 Vector3 fwd(
851 chassisWorldTransform.basis[0][Vector3::AXIS_Z],
852 chassisWorldTransform.basis[1][Vector3::AXIS_Z],
853 chassisWorldTransform.basis[2][Vector3::AXIS_Z]);
854
855 real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
856 fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
857
858 real_t proj2 = fwd.dot(vel);
859
860 wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius);
861 }
862
863 wheel.m_rotation += wheel.m_deltaRotation;
864 wheel.m_rpm = ((wheel.m_deltaRotation / step) * 60) / Math_TAU;
865
866 wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact
867 }
868}
869
870void VehicleBody3D::set_engine_force(real_t p_engine_force) {
871 engine_force = p_engine_force;
872 for (int i = 0; i < wheels.size(); i++) {
873 VehicleWheel3D &wheelInfo = *wheels[i];
874 if (wheelInfo.engine_traction) {
875 wheelInfo.m_engineForce = p_engine_force;
876 }
877 }
878}
879
880real_t VehicleBody3D::get_engine_force() const {
881 return engine_force;
882}
883
884void VehicleBody3D::set_brake(real_t p_brake) {
885 brake = p_brake;
886 for (int i = 0; i < wheels.size(); i++) {
887 VehicleWheel3D &wheelInfo = *wheels[i];
888 wheelInfo.m_brake = p_brake;
889 }
890}
891
892real_t VehicleBody3D::get_brake() const {
893 return brake;
894}
895
896void VehicleBody3D::set_steering(real_t p_steering) {
897 m_steeringValue = p_steering;
898 for (int i = 0; i < wheels.size(); i++) {
899 VehicleWheel3D &wheelInfo = *wheels[i];
900 if (wheelInfo.steers) {
901 wheelInfo.m_steering = p_steering;
902 }
903 }
904}
905
906real_t VehicleBody3D::get_steering() const {
907 return m_steeringValue;
908}
909
910void VehicleBody3D::_bind_methods() {
911 ClassDB::bind_method(D_METHOD("set_engine_force", "engine_force"), &VehicleBody3D::set_engine_force);
912 ClassDB::bind_method(D_METHOD("get_engine_force"), &VehicleBody3D::get_engine_force);
913
914 ClassDB::bind_method(D_METHOD("set_brake", "brake"), &VehicleBody3D::set_brake);
915 ClassDB::bind_method(D_METHOD("get_brake"), &VehicleBody3D::get_brake);
916
917 ClassDB::bind_method(D_METHOD("set_steering", "steering"), &VehicleBody3D::set_steering);
918 ClassDB::bind_method(D_METHOD("get_steering"), &VehicleBody3D::get_steering);
919
920 ADD_GROUP("Motion", "");
921 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "engine_force", PROPERTY_HINT_RANGE, U"-1024,1024,0.01,or_less,or_greater,suffix:kg\u22C5m/s\u00B2 (N)"), "set_engine_force", "get_engine_force");
922 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "brake", PROPERTY_HINT_RANGE, U"-128,128,0.01,or_less,or_greater,suffix:kg\u22C5m/s\u00B2 (N)"), "set_brake", "get_brake");
923 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "steering", PROPERTY_HINT_RANGE, "-180,180,0.01,radians"), "set_steering", "get_steering");
924}
925
926VehicleBody3D::VehicleBody3D() {
927 exclude.insert(get_rid());
928 set_mass(40);
929}
930