/*************************************************************************/ /* vehicle_body_3d.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ /* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ /* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ /* "Software"), to deal in the Software without restriction, including */ /* without limitation the rights to use, copy, modify, merge, publish, */ /* distribute, sublicense, and/or sell copies of the Software, and to */ /* permit persons to whom the Software is furnished to do so, subject to */ /* the following conditions: */ /* */ /* The above copyright notice and this permission notice shall be */ /* included in all copies or substantial portions of the Software. */ /* */ /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "vehicle_body_3d.h" #define ROLLING_INFLUENCE_FIX class btVehicleJacobianEntry { public: Vector3 m_linearJointAxis; Vector3 m_aJ; Vector3 m_bJ; Vector3 m_0MinvJt; Vector3 m_1MinvJt; //Optimization: can be stored in the w/last component of one of the vectors real_t m_Adiag; real_t getDiagonal() const { return m_Adiag; } btVehicleJacobianEntry() {} //constraint between two different rigidbodies btVehicleJacobianEntry( const Basis &world2A, const Basis &world2B, const Vector3 &rel_pos1, const Vector3 &rel_pos2, const Vector3 &jointAxis, const Vector3 &inertiaInvA, const real_t massInvA, const Vector3 &inertiaInvB, const real_t massInvB) : m_linearJointAxis(jointAxis) { m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); //btAssert(m_Adiag > real_t(0.0)); } real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) { Vector3 linrel = linvelA - linvelB; Vector3 angvela = angvelA * m_aJ; Vector3 angvelb = angvelB * m_bJ; linrel *= m_linearJointAxis; angvela += angvelb; angvela += linrel; real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; return rel_vel2 + CMP_EPSILON; } }; void VehicleWheel3D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_ENTER_TREE: { VehicleBody3D *cb = Object::cast_to(get_parent()); if (!cb) { return; } body = cb; local_xform = get_transform(); cb->wheels.push_back(this); m_chassisConnectionPointCS = get_transform().origin; m_wheelDirectionCS = -get_transform().basis.get_axis(Vector3::AXIS_Y).normalized(); m_wheelAxleCS = get_transform().basis.get_axis(Vector3::AXIS_X).normalized(); } break; case NOTIFICATION_EXIT_TREE: { VehicleBody3D *cb = Object::cast_to(get_parent()); if (!cb) { return; } cb->wheels.erase(this); body = nullptr; } break; } } TypedArray VehicleWheel3D::get_configuration_warnings() const { TypedArray warnings = Node::get_configuration_warnings(); if (!Object::cast_to(get_parent())) { warnings.push_back(TTR("VehicleWheel3D serves to provide a wheel system to a VehicleBody3D. Please use it as a child of a VehicleBody3D.")); } return warnings; } void VehicleWheel3D::_update(PhysicsDirectBodyState3D *s) { if (m_raycastInfo.m_isInContact) { real_t project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS); Vector3 chassis_velocity_at_contactPoint; Vector3 relpos = m_raycastInfo.m_contactPointWS - s->get_transform().origin; chassis_velocity_at_contactPoint = s->get_linear_velocity() + (s->get_angular_velocity()).cross(relpos); // * mPos); real_t projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint); if (project >= real_t(-0.1)) { m_suspensionRelativeVelocity = real_t(0.0); m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1); } else { real_t inv = real_t(-1.) / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = m_suspensionRestLength; m_suspensionRelativeVelocity = real_t(0.0); m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_clippedInvContactDotSuspension = real_t(1.0); } } void VehicleWheel3D::set_radius(real_t p_radius) { m_wheelRadius = p_radius; update_gizmos(); } real_t VehicleWheel3D::get_radius() const { return m_wheelRadius; } void VehicleWheel3D::set_suspension_rest_length(real_t p_length) { m_suspensionRestLength = p_length; update_gizmos(); } real_t VehicleWheel3D::get_suspension_rest_length() const { return m_suspensionRestLength; } void VehicleWheel3D::set_suspension_travel(real_t p_length) { m_maxSuspensionTravelCm = p_length / 0.01; } real_t VehicleWheel3D::get_suspension_travel() const { return m_maxSuspensionTravelCm * 0.01; } void VehicleWheel3D::set_suspension_stiffness(real_t p_value) { m_suspensionStiffness = p_value; } real_t VehicleWheel3D::get_suspension_stiffness() const { return m_suspensionStiffness; } void VehicleWheel3D::set_suspension_max_force(real_t p_value) { m_maxSuspensionForce = p_value; } real_t VehicleWheel3D::get_suspension_max_force() const { return m_maxSuspensionForce; } void VehicleWheel3D::set_damping_compression(real_t p_value) { m_wheelsDampingCompression = p_value; } real_t VehicleWheel3D::get_damping_compression() const { return m_wheelsDampingCompression; } void VehicleWheel3D::set_damping_relaxation(real_t p_value) { m_wheelsDampingRelaxation = p_value; } real_t VehicleWheel3D::get_damping_relaxation() const { return m_wheelsDampingRelaxation; } void VehicleWheel3D::set_friction_slip(real_t p_value) { m_frictionSlip = p_value; } real_t VehicleWheel3D::get_friction_slip() const { return m_frictionSlip; } void VehicleWheel3D::set_roll_influence(real_t p_value) { m_rollInfluence = p_value; } real_t VehicleWheel3D::get_roll_influence() const { return m_rollInfluence; } bool VehicleWheel3D::is_in_contact() const { return m_raycastInfo.m_isInContact; } void VehicleWheel3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_radius", "length"), &VehicleWheel3D::set_radius); ClassDB::bind_method(D_METHOD("get_radius"), &VehicleWheel3D::get_radius); ClassDB::bind_method(D_METHOD("set_suspension_rest_length", "length"), &VehicleWheel3D::set_suspension_rest_length); ClassDB::bind_method(D_METHOD("get_suspension_rest_length"), &VehicleWheel3D::get_suspension_rest_length); ClassDB::bind_method(D_METHOD("set_suspension_travel", "length"), &VehicleWheel3D::set_suspension_travel); ClassDB::bind_method(D_METHOD("get_suspension_travel"), &VehicleWheel3D::get_suspension_travel); ClassDB::bind_method(D_METHOD("set_suspension_stiffness", "length"), &VehicleWheel3D::set_suspension_stiffness); ClassDB::bind_method(D_METHOD("get_suspension_stiffness"), &VehicleWheel3D::get_suspension_stiffness); ClassDB::bind_method(D_METHOD("set_suspension_max_force", "length"), &VehicleWheel3D::set_suspension_max_force); ClassDB::bind_method(D_METHOD("get_suspension_max_force"), &VehicleWheel3D::get_suspension_max_force); ClassDB::bind_method(D_METHOD("set_damping_compression", "length"), &VehicleWheel3D::set_damping_compression); ClassDB::bind_method(D_METHOD("get_damping_compression"), &VehicleWheel3D::get_damping_compression); ClassDB::bind_method(D_METHOD("set_damping_relaxation", "length"), &VehicleWheel3D::set_damping_relaxation); ClassDB::bind_method(D_METHOD("get_damping_relaxation"), &VehicleWheel3D::get_damping_relaxation); ClassDB::bind_method(D_METHOD("set_use_as_traction", "enable"), &VehicleWheel3D::set_use_as_traction); ClassDB::bind_method(D_METHOD("is_used_as_traction"), &VehicleWheel3D::is_used_as_traction); ClassDB::bind_method(D_METHOD("set_use_as_steering", "enable"), &VehicleWheel3D::set_use_as_steering); ClassDB::bind_method(D_METHOD("is_used_as_steering"), &VehicleWheel3D::is_used_as_steering); ClassDB::bind_method(D_METHOD("set_friction_slip", "length"), &VehicleWheel3D::set_friction_slip); ClassDB::bind_method(D_METHOD("get_friction_slip"), &VehicleWheel3D::get_friction_slip); ClassDB::bind_method(D_METHOD("is_in_contact"), &VehicleWheel3D::is_in_contact); ClassDB::bind_method(D_METHOD("set_roll_influence", "roll_influence"), &VehicleWheel3D::set_roll_influence); ClassDB::bind_method(D_METHOD("get_roll_influence"), &VehicleWheel3D::get_roll_influence); ClassDB::bind_method(D_METHOD("get_skidinfo"), &VehicleWheel3D::get_skidinfo); ClassDB::bind_method(D_METHOD("get_rpm"), &VehicleWheel3D::get_rpm); ClassDB::bind_method(D_METHOD("set_engine_force", "engine_force"), &VehicleWheel3D::set_engine_force); ClassDB::bind_method(D_METHOD("get_engine_force"), &VehicleWheel3D::get_engine_force); ClassDB::bind_method(D_METHOD("set_brake", "brake"), &VehicleWheel3D::set_brake); ClassDB::bind_method(D_METHOD("get_brake"), &VehicleWheel3D::get_brake); ClassDB::bind_method(D_METHOD("set_steering", "steering"), &VehicleWheel3D::set_steering); ClassDB::bind_method(D_METHOD("get_steering"), &VehicleWheel3D::get_steering); ADD_GROUP("Per-Wheel Motion", ""); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "engine_force", PROPERTY_HINT_RANGE, "-1024,1024.0,0.01,or_greater"), "set_engine_force", "get_engine_force"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "brake", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_brake", "get_brake"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "steering", PROPERTY_HINT_RANGE, "-180,180.0,0.01"), "set_steering", "get_steering"); ADD_GROUP("VehicleBody3D Motion", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_as_traction"), "set_use_as_traction", "is_used_as_traction"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_as_steering"), "set_use_as_steering", "is_used_as_steering"); ADD_GROUP("Wheel", "wheel_"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_roll_influence"), "set_roll_influence", "get_roll_influence"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_radius"), "set_radius", "get_radius"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_rest_length"), "set_suspension_rest_length", "get_suspension_rest_length"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wheel_friction_slip"), "set_friction_slip", "get_friction_slip"); ADD_GROUP("Suspension", "suspension_"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "suspension_travel"), "set_suspension_travel", "get_suspension_travel"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "suspension_stiffness"), "set_suspension_stiffness", "get_suspension_stiffness"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "suspension_max_force"), "set_suspension_max_force", "get_suspension_max_force"); ADD_GROUP("Damping", "damping_"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "damping_compression"), "set_damping_compression", "get_damping_compression"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "damping_relaxation"), "set_damping_relaxation", "get_damping_relaxation"); } void VehicleWheel3D::set_engine_force(real_t p_engine_force) { m_engineForce = p_engine_force; } real_t VehicleWheel3D::get_engine_force() const { return m_engineForce; } void VehicleWheel3D::set_brake(real_t p_brake) { m_brake = p_brake; } real_t VehicleWheel3D::get_brake() const { return m_brake; } void VehicleWheel3D::set_steering(real_t p_steering) { m_steering = p_steering; } real_t VehicleWheel3D::get_steering() const { return m_steering; } void VehicleWheel3D::set_use_as_traction(bool p_enable) { engine_traction = p_enable; } bool VehicleWheel3D::is_used_as_traction() const { return engine_traction; } void VehicleWheel3D::set_use_as_steering(bool p_enabled) { steers = p_enabled; } bool VehicleWheel3D::is_used_as_steering() const { return steers; } real_t VehicleWheel3D::get_skidinfo() const { return m_skidInfo; } real_t VehicleWheel3D::get_rpm() const { return m_rpm; } VehicleWheel3D::VehicleWheel3D() { } void VehicleBody3D::_update_wheel_transform(VehicleWheel3D &wheel, PhysicsDirectBodyState3D *s) { wheel.m_raycastInfo.m_isInContact = false; Transform3D chassisTrans = s->get_transform(); /* if (interpolatedTransform && (getRigidBody()->getMotionState())) { getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); } */ wheel.m_raycastInfo.m_hardPointWS = chassisTrans.xform(wheel.m_chassisConnectionPointCS); //wheel.m_raycastInfo.m_hardPointWS+=s->get_linear_velocity()*s->get_step(); wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.get_basis().xform(wheel.m_wheelDirectionCS).normalized(); wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.get_basis().xform(wheel.m_wheelAxleCS).normalized(); } void VehicleBody3D::_update_wheel(int p_idx, PhysicsDirectBodyState3D *s) { VehicleWheel3D &wheel = *wheels[p_idx]; _update_wheel_transform(wheel, s); Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; const Vector3 &right = wheel.m_raycastInfo.m_wheelAxleWS; Vector3 fwd = up.cross(right); fwd = fwd.normalized(); Basis steeringMat(up, wheel.m_steering); Basis rotatingMat(right, wheel.m_rotation); Basis basis2( right[0], up[0], fwd[0], right[1], up[1], fwd[1], right[2], up[2], fwd[2]); wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2); //wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat)); wheel.m_worldTransform.set_origin( wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength); } real_t VehicleBody3D::_ray_cast(int p_idx, PhysicsDirectBodyState3D *s) { VehicleWheel3D &wheel = *wheels[p_idx]; _update_wheel_transform(wheel, s); real_t depth = -1; real_t raylen = wheel.m_suspensionRestLength + wheel.m_wheelRadius; Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); Vector3 source = wheel.m_raycastInfo.m_hardPointWS; wheel.m_raycastInfo.m_contactPointWS = source + rayvector; const Vector3 &target = wheel.m_raycastInfo.m_contactPointWS; source -= wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS; real_t param = real_t(0.); PhysicsDirectSpaceState3D::RayResult rr; PhysicsDirectSpaceState3D *ss = s->get_space_state(); bool col = ss->intersect_ray(source, target, rr, exclude, get_collision_mask()); wheel.m_raycastInfo.m_groundObject = nullptr; if (col) { param = source.distance_to(rr.position) / source.distance_to(target); depth = raylen * param; wheel.m_raycastInfo.m_contactNormalWS = rr.normal; wheel.m_raycastInfo.m_isInContact = true; if (rr.collider) { wheel.m_raycastInfo.m_groundObject = Object::cast_to(rr.collider); } real_t hitDistance = param * raylen; wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelRadius; //clamp on max suspension travel real_t minSuspensionLength = wheel.m_suspensionRestLength - wheel.m_maxSuspensionTravelCm * real_t(0.01); real_t maxSuspensionLength = wheel.m_suspensionRestLength + wheel.m_maxSuspensionTravelCm * real_t(0.01); if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; } if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; } wheel.m_raycastInfo.m_contactPointWS = rr.position; real_t denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS); Vector3 chassis_velocity_at_contactPoint; //Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); //chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); chassis_velocity_at_contactPoint = s->get_linear_velocity() + (s->get_angular_velocity()).cross(wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin); // * mPos); real_t projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint); if (denominator >= real_t(-0.1)) { wheel.m_suspensionRelativeVelocity = real_t(0.0); wheel.m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1); } else { real_t inv = real_t(-1.) / denominator; wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_clippedInvContactDotSuspension = inv; } } else { wheel.m_raycastInfo.m_isInContact = false; //put wheel info as in rest position wheel.m_raycastInfo.m_suspensionLength = wheel.m_suspensionRestLength; wheel.m_suspensionRelativeVelocity = real_t(0.0); wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS; wheel.m_clippedInvContactDotSuspension = real_t(1.0); } return depth; } void VehicleBody3D::_update_suspension(PhysicsDirectBodyState3D *s) { real_t chassisMass = get_mass(); for (int w_it = 0; w_it < wheels.size(); w_it++) { VehicleWheel3D &wheel_info = *wheels[w_it]; if (wheel_info.m_raycastInfo.m_isInContact) { real_t force; //Spring { real_t susp_length = wheel_info.m_suspensionRestLength; real_t current_length = wheel_info.m_raycastInfo.m_suspensionLength; real_t length_diff = (susp_length - current_length); force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension; } // Damper { real_t projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; { real_t susp_damping; if (projected_rel_vel < real_t(0.0)) { susp_damping = wheel_info.m_wheelsDampingCompression; } else { susp_damping = wheel_info.m_wheelsDampingRelaxation; } force -= susp_damping * projected_rel_vel; } } // RESULT wheel_info.m_wheelsSuspensionForce = force * chassisMass; if (wheel_info.m_wheelsSuspensionForce < real_t(0.)) { wheel_info.m_wheelsSuspensionForce = real_t(0.); } } else { wheel_info.m_wheelsSuspensionForce = real_t(0.0); } } } //bilateral constraint between two dynamic objects void VehicleBody3D::_resolve_single_bilateral(PhysicsDirectBodyState3D *s, const Vector3 &pos1, PhysicsBody3D *body2, const Vector3 &pos2, const Vector3 &normal, real_t &impulse, const real_t p_rollInfluence) { real_t normalLenSqr = normal.length_squared(); //ERR_FAIL_COND( normalLenSqr < real_t(1.1)); if (normalLenSqr > real_t(1.1)) { impulse = real_t(0.); return; } Vector3 rel_pos1 = pos1 - s->get_transform().origin; Vector3 rel_pos2; if (body2) { rel_pos2 = pos2 - body2->get_global_transform().origin; } //this jacobian entry could be re-used for all iterations Vector3 vel1 = s->get_linear_velocity() + (s->get_angular_velocity()).cross(rel_pos1); // * mPos); Vector3 vel2; if (body2) { vel2 = body2->get_linear_velocity() + body2->get_angular_velocity().cross(rel_pos2); } Vector3 vel = vel1 - vel2; Basis b2trans; real_t b2invmass = 0; Vector3 b2lv; Vector3 b2av; Vector3 b2invinertia; //todo if (body2) { b2trans = body2->get_global_transform().basis.transposed(); b2invmass = body2->get_inverse_mass(); b2lv = body2->get_linear_velocity(); b2av = body2->get_angular_velocity(); } btVehicleJacobianEntry jac(s->get_transform().basis.transposed(), b2trans, rel_pos1, rel_pos2, normal, s->get_inverse_inertia_tensor().get_main_diagonal(), 1.0 / get_mass(), b2invinertia, b2invmass); // FIXME: rel_vel assignment here is overwritten by the following assignment. // What seems to be intended in the next assignment is: rel_vel = normal.dot(rel_vel); // Investigate why. real_t rel_vel = jac.getRelativeVelocity( s->get_linear_velocity(), s->get_transform().basis.transposed().xform(s->get_angular_velocity()), b2lv, b2trans.xform(b2av)); rel_vel = normal.dot(vel); // !BAS! We had this set to 0.4, in bullet its 0.2 real_t contactDamping = real_t(0.2); if (p_rollInfluence > 0.0) { // !BAS! But seeing we apply this frame by frame, makes more sense to me to make this time based // keeping in mind our anti roll factor if it is set contactDamping = MIN(contactDamping, s->get_step() / p_rollInfluence); } #define ONLY_USE_LINEAR_MASS #ifdef ONLY_USE_LINEAR_MASS real_t massTerm = real_t(1.) / ((1.0 / get_mass()) + b2invmass); impulse = -contactDamping * rel_vel * massTerm; #else real_t velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; #endif } VehicleBody3D::btVehicleWheelContactPoint::btVehicleWheelContactPoint(PhysicsDirectBodyState3D *s, PhysicsBody3D *body1, const Vector3 &frictionPosWorld, const Vector3 &frictionDirectionWorld, real_t maxImpulse) : m_s(s), m_body1(body1), m_frictionPositionWorld(frictionPosWorld), m_frictionDirectionWorld(frictionDirectionWorld), m_maxImpulse(maxImpulse) { real_t denom0 = 0; real_t denom1 = 0; { Vector3 r0 = frictionPosWorld - s->get_transform().origin; Vector3 c0 = (r0).cross(frictionDirectionWorld); Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0); denom0 = s->get_inverse_mass() + frictionDirectionWorld.dot(vec); } /* TODO: Why is this code unused? if (body1) { Vector3 r0 = frictionPosWorld - body1->get_global_transform().origin; Vector3 c0 = (r0).cross(frictionDirectionWorld); Vector3 vec = s->get_inverse_inertia_tensor().xform_inv(c0).cross(r0); //denom1= body1->get_inverse_mass() + frictionDirectionWorld.dot(vec); } */ real_t relaxation = 1.f; m_jacDiagABInv = relaxation / (denom0 + denom1); } real_t VehicleBody3D::_calc_rolling_friction(btVehicleWheelContactPoint &contactPoint) { real_t j1 = 0.f; const Vector3 &contactPosWorld = contactPoint.m_frictionPositionWorld; Vector3 rel_pos1 = contactPosWorld - contactPoint.m_s->get_transform().origin; Vector3 rel_pos2; if (contactPoint.m_body1) { rel_pos2 = contactPosWorld - contactPoint.m_body1->get_global_transform().origin; } real_t maxImpulse = contactPoint.m_maxImpulse; Vector3 vel1 = contactPoint.m_s->get_linear_velocity() + (contactPoint.m_s->get_angular_velocity()).cross(rel_pos1); // * mPos); Vector3 vel2; if (contactPoint.m_body1) { vel2 = contactPoint.m_body1->get_linear_velocity() + contactPoint.m_body1->get_angular_velocity().cross(rel_pos2); } Vector3 vel = vel1 - vel2; real_t vrel = contactPoint.m_frictionDirectionWorld.dot(vel); // calculate j that moves us to zero relative velocity j1 = -vrel * contactPoint.m_jacDiagABInv; return CLAMP(j1, -maxImpulse, maxImpulse); } static const real_t sideFrictionStiffness2 = real_t(1.0); void VehicleBody3D::_update_friction(PhysicsDirectBodyState3D *s) { //calculate the impulse, so that the wheels don't move sidewards int numWheel = wheels.size(); if (!numWheel) { return; } m_forwardWS.resize(numWheel); m_axle.resize(numWheel); m_forwardImpulse.resize(numWheel); m_sideImpulse.resize(numWheel); //collapse all those loops into one! for (int i = 0; i < wheels.size(); i++) { m_sideImpulse.write[i] = real_t(0.); m_forwardImpulse.write[i] = real_t(0.); } { for (int i = 0; i < wheels.size(); i++) { VehicleWheel3D &wheelInfo = *wheels[i]; if (wheelInfo.m_raycastInfo.m_isInContact) { //const btTransform& wheelTrans = getWheelTransformWS( i ); Basis wheelBasis0 = wheelInfo.m_worldTransform.basis; //get_global_transform().basis; m_axle.write[i] = wheelBasis0.get_axis(Vector3::AXIS_X); //m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS; const Vector3 &surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; real_t proj = m_axle[i].dot(surfNormalWS); m_axle.write[i] -= surfNormalWS * proj; m_axle.write[i] = m_axle[i].normalized(); m_forwardWS.write[i] = surfNormalWS.cross(m_axle[i]); m_forwardWS.write[i].normalize(); _resolve_single_bilateral(s, wheelInfo.m_raycastInfo.m_contactPointWS, wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_axle[i], m_sideImpulse.write[i], wheelInfo.m_rollInfluence); m_sideImpulse.write[i] *= sideFrictionStiffness2; } } } real_t sideFactor = real_t(1.); real_t fwdFactor = 0.5; bool sliding = false; { for (int wheel = 0; wheel < wheels.size(); wheel++) { VehicleWheel3D &wheelInfo = *wheels[wheel]; //class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; real_t rollingFriction = 0.f; if (wheelInfo.m_raycastInfo.m_isInContact) { if (wheelInfo.m_engineForce != 0.f) { rollingFriction = -wheelInfo.m_engineForce * s->get_step(); } else { real_t defaultRollingFrictionImpulse = 0.f; real_t maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; btVehicleWheelContactPoint contactPt(s, wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse); rollingFriction = _calc_rolling_friction(contactPt); } } //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) m_forwardImpulse.write[wheel] = real_t(0.); wheelInfo.m_skidInfo = real_t(1.); if (wheelInfo.m_raycastInfo.m_isInContact) { wheelInfo.m_skidInfo = real_t(1.); real_t maximp = wheelInfo.m_wheelsSuspensionForce * s->get_step() * wheelInfo.m_frictionSlip; real_t maximpSide = maximp; real_t maximpSquared = maximp * maximpSide; m_forwardImpulse.write[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep; real_t x = (m_forwardImpulse[wheel]) * fwdFactor; real_t y = (m_sideImpulse[wheel]) * sideFactor; real_t impulseSquared = (x * x + y * y); if (impulseSquared > maximpSquared) { sliding = true; real_t factor = maximp / Math::sqrt(impulseSquared); wheelInfo.m_skidInfo *= factor; } } } } if (sliding) { for (int wheel = 0; wheel < wheels.size(); wheel++) { if (m_sideImpulse[wheel] != real_t(0.)) { if (wheels[wheel]->m_skidInfo < real_t(1.)) { m_forwardImpulse.write[wheel] *= wheels[wheel]->m_skidInfo; m_sideImpulse.write[wheel] *= wheels[wheel]->m_skidInfo; } } } } // apply the impulses { for (int wheel = 0; wheel < wheels.size(); wheel++) { VehicleWheel3D &wheelInfo = *wheels[wheel]; Vector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - s->get_transform().origin; if (m_forwardImpulse[wheel] != real_t(0.)) { s->apply_impulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos); } if (m_sideImpulse[wheel] != real_t(0.)) { PhysicsBody3D *groundObject = wheelInfo.m_raycastInfo.m_groundObject; Vector3 rel_pos2; if (groundObject) { rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->get_global_transform().origin; } Vector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; #if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. Vector3 vChassisWorldUp = s->get_transform().basis.transposed()[1]; //getRigidBody()->getCenterOfMassTransform3D().getBasis().getColumn(m_indexUpAxis); rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence)); #else rel_pos[1] *= wheelInfo.m_rollInfluence; //? #endif s->apply_impulse(sideImp, rel_pos); //apply friction impulse on the ground //todo //groundObject->applyImpulse(-sideImp,rel_pos2); } } } } void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { RigidDynamicBody3D::_body_state_changed(p_state); real_t step = p_state->get_step(); for (int i = 0; i < wheels.size(); i++) { _update_wheel(i, p_state); } for (int i = 0; i < wheels.size(); i++) { _ray_cast(i, p_state); wheels[i]->set_transform(p_state->get_transform().inverse() * wheels[i]->m_worldTransform); } _update_suspension(p_state); for (int i = 0; i < wheels.size(); i++) { //apply suspension force VehicleWheel3D &wheel = *wheels[i]; real_t suspensionForce = wheel.m_wheelsSuspensionForce; if (suspensionForce > wheel.m_maxSuspensionForce) { suspensionForce = wheel.m_maxSuspensionForce; } Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - p_state->get_transform().origin; p_state->apply_impulse(impulse, relative_position); } _update_friction(p_state); for (int i = 0; i < wheels.size(); i++) { VehicleWheel3D &wheel = *wheels[i]; Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - p_state->get_transform().origin; Vector3 vel = p_state->get_linear_velocity() + (p_state->get_angular_velocity()).cross(relpos); // * mPos); if (wheel.m_raycastInfo.m_isInContact) { const Transform3D &chassisWorldTransform = p_state->get_transform(); Vector3 fwd( chassisWorldTransform.basis[0][Vector3::AXIS_Z], chassisWorldTransform.basis[1][Vector3::AXIS_Z], chassisWorldTransform.basis[2][Vector3::AXIS_Z]); real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; real_t proj2 = fwd.dot(vel); wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius); } wheel.m_rotation += wheel.m_deltaRotation; wheel.m_rpm = ((wheel.m_deltaRotation / step) * 60) / Math_TAU; wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact } } void VehicleBody3D::set_engine_force(real_t p_engine_force) { engine_force = p_engine_force; for (int i = 0; i < wheels.size(); i++) { VehicleWheel3D &wheelInfo = *wheels[i]; if (wheelInfo.engine_traction) { wheelInfo.m_engineForce = p_engine_force; } } } real_t VehicleBody3D::get_engine_force() const { return engine_force; } void VehicleBody3D::set_brake(real_t p_brake) { brake = p_brake; for (int i = 0; i < wheels.size(); i++) { VehicleWheel3D &wheelInfo = *wheels[i]; wheelInfo.m_brake = p_brake; } } real_t VehicleBody3D::get_brake() const { return brake; } void VehicleBody3D::set_steering(real_t p_steering) { m_steeringValue = p_steering; for (int i = 0; i < wheels.size(); i++) { VehicleWheel3D &wheelInfo = *wheels[i]; if (wheelInfo.steers) { wheelInfo.m_steering = p_steering; } } } real_t VehicleBody3D::get_steering() const { return m_steeringValue; } void VehicleBody3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_engine_force", "engine_force"), &VehicleBody3D::set_engine_force); ClassDB::bind_method(D_METHOD("get_engine_force"), &VehicleBody3D::get_engine_force); ClassDB::bind_method(D_METHOD("set_brake", "brake"), &VehicleBody3D::set_brake); ClassDB::bind_method(D_METHOD("get_brake"), &VehicleBody3D::get_brake); ClassDB::bind_method(D_METHOD("set_steering", "steering"), &VehicleBody3D::set_steering); ClassDB::bind_method(D_METHOD("get_steering"), &VehicleBody3D::get_steering); ADD_GROUP("Motion", ""); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "engine_force", PROPERTY_HINT_RANGE, "-1024,1024.0,0.01,or_greater"), "set_engine_force", "get_engine_force"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "brake", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_brake", "get_brake"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "steering", PROPERTY_HINT_RANGE, "-180,180.0,0.01"), "set_steering", "get_steering"); } VehicleBody3D::VehicleBody3D() { exclude.insert(get_rid()); set_mass(40); }