diff options
Diffstat (limited to 'scene/3d/vehicle_body.cpp')
| -rw-r--r-- | scene/3d/vehicle_body.cpp | 743 |
1 files changed, 309 insertions, 434 deletions
diff --git a/scene/3d/vehicle_body.cpp b/scene/3d/vehicle_body.cpp index 4b32c62d14..4a647fe682 100644 --- a/scene/3d/vehicle_body.cpp +++ b/scene/3d/vehicle_body.cpp @@ -30,78 +30,70 @@ #define ROLLING_INFLUENCE_FIX -class btVehicleJacobianEntry -{ +class btVehicleJacobianEntry { public: - - Vector3 m_linearJointAxis; - Vector3 m_aJ; - Vector3 m_bJ; - Vector3 m_0MinvJt; - Vector3 m_1MinvJt; + 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 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, + 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 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; - } - + : 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 VehicleWheel::_notification(int p_what) { - - if (p_what==NOTIFICATION_ENTER_TREE) { + if (p_what == NOTIFICATION_ENTER_TREE) { if (!get_parent()) return; VehicleBody *cb = get_parent()->cast_to<VehicleBody>(); if (!cb) return; - body=cb; - local_xform=get_transform(); + 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(); - } - if (p_what==NOTIFICATION_EXIT_TREE) { + if (p_what == NOTIFICATION_EXIT_TREE) { if (!get_parent()) return; @@ -109,34 +101,27 @@ void VehicleWheel::_notification(int p_what) { if (!cb) return; cb->wheels.erase(this); - body=NULL; + body = NULL; } - } - void VehicleWheel::_update(PhysicsDirectBodyState *s) { - - if (m_raycastInfo.m_isInContact) { - real_t project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); - Vector3 chassis_velocity_at_contactPoint; + 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); + (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)) - { + 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 - { + } else { real_t inv = real_t(-1.) / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; @@ -144,7 +129,7 @@ void VehicleWheel::_update(PhysicsDirectBodyState *s) { } - else // Not in contact : position wheel in a nice (rest length) position + else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = m_suspensionRestLength; m_suspensionRelativeVelocity = real_t(0.0); @@ -155,160 +140,150 @@ void VehicleWheel::_update(PhysicsDirectBodyState *s) { void VehicleWheel::set_radius(float p_radius) { - m_wheelRadius=p_radius; + m_wheelRadius = p_radius; update_gizmo(); } -float VehicleWheel::get_radius() const{ +float VehicleWheel::get_radius() const { return m_wheelRadius; } -void VehicleWheel::set_suspension_rest_length(float p_length){ +void VehicleWheel::set_suspension_rest_length(float p_length) { - m_suspensionRestLength=p_length; + m_suspensionRestLength = p_length; update_gizmo(); } -float VehicleWheel::get_suspension_rest_length() const{ +float VehicleWheel::get_suspension_rest_length() const { return m_suspensionRestLength; } -void VehicleWheel::set_suspension_travel(float p_length){ +void VehicleWheel::set_suspension_travel(float p_length) { - m_maxSuspensionTravelCm=p_length/0.01; + m_maxSuspensionTravelCm = p_length / 0.01; } -float VehicleWheel::get_suspension_travel() const{ +float VehicleWheel::get_suspension_travel() const { - return m_maxSuspensionTravelCm*0.01; + return m_maxSuspensionTravelCm * 0.01; } -void VehicleWheel::set_suspension_stiffness(float p_value){ +void VehicleWheel::set_suspension_stiffness(float p_value) { - m_suspensionStiffness=p_value; + m_suspensionStiffness = p_value; } -float VehicleWheel::get_suspension_stiffness() const{ +float VehicleWheel::get_suspension_stiffness() const { return m_suspensionStiffness; } -void VehicleWheel::set_suspension_max_force(float p_value){ +void VehicleWheel::set_suspension_max_force(float p_value) { - m_maxSuspensionForce=p_value; + m_maxSuspensionForce = p_value; } -float VehicleWheel::get_suspension_max_force() const{ +float VehicleWheel::get_suspension_max_force() const { return m_maxSuspensionForce; } -void VehicleWheel::set_damping_compression(float p_value){ +void VehicleWheel::set_damping_compression(float p_value) { - m_wheelsDampingCompression=p_value; + m_wheelsDampingCompression = p_value; } -float VehicleWheel::get_damping_compression() const{ +float VehicleWheel::get_damping_compression() const { return m_wheelsDampingCompression; } -void VehicleWheel::set_damping_relaxation(float p_value){ +void VehicleWheel::set_damping_relaxation(float p_value) { - m_wheelsDampingRelaxation=p_value; + m_wheelsDampingRelaxation = p_value; } -float VehicleWheel::get_damping_relaxation() const{ +float VehicleWheel::get_damping_relaxation() const { return m_wheelsDampingRelaxation; } void VehicleWheel::set_friction_slip(float p_value) { - m_frictionSlip=p_value; + m_frictionSlip = p_value; } -float VehicleWheel::get_friction_slip() const{ +float VehicleWheel::get_friction_slip() const { return m_frictionSlip; } - void VehicleWheel::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_radius", "length"), &VehicleWheel::set_radius); + ClassDB::bind_method(D_METHOD("get_radius"), &VehicleWheel::get_radius); - ClassDB::bind_method(D_METHOD("set_radius","length"),&VehicleWheel::set_radius); - ClassDB::bind_method(D_METHOD("get_radius"),&VehicleWheel::get_radius); - - ClassDB::bind_method(D_METHOD("set_suspension_rest_length","length"),&VehicleWheel::set_suspension_rest_length); - ClassDB::bind_method(D_METHOD("get_suspension_rest_length"),&VehicleWheel::get_suspension_rest_length); - - ClassDB::bind_method(D_METHOD("set_suspension_travel","length"),&VehicleWheel::set_suspension_travel); - ClassDB::bind_method(D_METHOD("get_suspension_travel"),&VehicleWheel::get_suspension_travel); - - ClassDB::bind_method(D_METHOD("set_suspension_stiffness","length"),&VehicleWheel::set_suspension_stiffness); - ClassDB::bind_method(D_METHOD("get_suspension_stiffness"),&VehicleWheel::get_suspension_stiffness); - - ClassDB::bind_method(D_METHOD("set_suspension_max_force","length"),&VehicleWheel::set_suspension_max_force); - ClassDB::bind_method(D_METHOD("get_suspension_max_force"),&VehicleWheel::get_suspension_max_force); + ClassDB::bind_method(D_METHOD("set_suspension_rest_length", "length"), &VehicleWheel::set_suspension_rest_length); + ClassDB::bind_method(D_METHOD("get_suspension_rest_length"), &VehicleWheel::get_suspension_rest_length); + ClassDB::bind_method(D_METHOD("set_suspension_travel", "length"), &VehicleWheel::set_suspension_travel); + ClassDB::bind_method(D_METHOD("get_suspension_travel"), &VehicleWheel::get_suspension_travel); - ClassDB::bind_method(D_METHOD("set_damping_compression","length"),&VehicleWheel::set_damping_compression); - ClassDB::bind_method(D_METHOD("get_damping_compression"),&VehicleWheel::get_damping_compression); + ClassDB::bind_method(D_METHOD("set_suspension_stiffness", "length"), &VehicleWheel::set_suspension_stiffness); + ClassDB::bind_method(D_METHOD("get_suspension_stiffness"), &VehicleWheel::get_suspension_stiffness); - ClassDB::bind_method(D_METHOD("set_damping_relaxation","length"),&VehicleWheel::set_damping_relaxation); - ClassDB::bind_method(D_METHOD("get_damping_relaxation"),&VehicleWheel::get_damping_relaxation); + ClassDB::bind_method(D_METHOD("set_suspension_max_force", "length"), &VehicleWheel::set_suspension_max_force); + ClassDB::bind_method(D_METHOD("get_suspension_max_force"), &VehicleWheel::get_suspension_max_force); - ClassDB::bind_method(D_METHOD("set_use_as_traction","enable"),&VehicleWheel::set_use_as_traction); - ClassDB::bind_method(D_METHOD("is_used_as_traction"),&VehicleWheel::is_used_as_traction); + ClassDB::bind_method(D_METHOD("set_damping_compression", "length"), &VehicleWheel::set_damping_compression); + ClassDB::bind_method(D_METHOD("get_damping_compression"), &VehicleWheel::get_damping_compression); - ClassDB::bind_method(D_METHOD("set_use_as_steering","enable"),&VehicleWheel::set_use_as_steering); - ClassDB::bind_method(D_METHOD("is_used_as_steering"),&VehicleWheel::is_used_as_steering); + ClassDB::bind_method(D_METHOD("set_damping_relaxation", "length"), &VehicleWheel::set_damping_relaxation); + ClassDB::bind_method(D_METHOD("get_damping_relaxation"), &VehicleWheel::get_damping_relaxation); - ClassDB::bind_method(D_METHOD("set_friction_slip","length"),&VehicleWheel::set_friction_slip); - ClassDB::bind_method(D_METHOD("get_friction_slip"),&VehicleWheel::get_friction_slip); + ClassDB::bind_method(D_METHOD("set_use_as_traction", "enable"), &VehicleWheel::set_use_as_traction); + ClassDB::bind_method(D_METHOD("is_used_as_traction"), &VehicleWheel::is_used_as_traction); + ClassDB::bind_method(D_METHOD("set_use_as_steering", "enable"), &VehicleWheel::set_use_as_steering); + ClassDB::bind_method(D_METHOD("is_used_as_steering"), &VehicleWheel::is_used_as_steering); + ClassDB::bind_method(D_METHOD("set_friction_slip", "length"), &VehicleWheel::set_friction_slip); + ClassDB::bind_method(D_METHOD("get_friction_slip"), &VehicleWheel::get_friction_slip); - 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::REAL,"wheel_radius"),"set_radius","get_radius"); - ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel_rest_length"),"set_suspension_rest_length","get_suspension_rest_length"); - ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel_friction_slip"),"set_friction_slip","get_friction_slip"); - ADD_GROUP("Suspension","suspension_"); - ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension_travel"),"set_suspension_travel","get_suspension_travel"); - ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension_stiffness"),"set_suspension_stiffness","get_suspension_stiffness"); - ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension_max_force"),"set_suspension_max_force","get_suspension_max_force"); - ADD_GROUP("Damping","damping_"); - ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping_compression"),"set_damping_compression","get_damping_compression"); - ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping_relaxation"),"set_damping_relaxation","get_damping_relaxation"); - + 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::REAL, "wheel_radius"), "set_radius", "get_radius"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "wheel_rest_length"), "set_suspension_rest_length", "get_suspension_rest_length"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "wheel_friction_slip"), "set_friction_slip", "get_friction_slip"); + ADD_GROUP("Suspension", "suspension_"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "suspension_travel"), "set_suspension_travel", "get_suspension_travel"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "suspension_stiffness"), "set_suspension_stiffness", "get_suspension_stiffness"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "suspension_max_force"), "set_suspension_max_force", "get_suspension_max_force"); + ADD_GROUP("Damping", "damping_"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "damping_compression"), "set_damping_compression", "get_damping_compression"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "damping_relaxation"), "set_damping_relaxation", "get_damping_relaxation"); } - void VehicleWheel::set_use_as_traction(bool p_enable) { - engine_traction=p_enable; + engine_traction = p_enable; } -bool VehicleWheel::is_used_as_traction() const{ +bool VehicleWheel::is_used_as_traction() const { return engine_traction; } +void VehicleWheel::set_use_as_steering(bool p_enabled) { -void VehicleWheel::set_use_as_steering(bool p_enabled){ - - steers=p_enabled; + steers = p_enabled; } -bool VehicleWheel::is_used_as_steering() const{ +bool VehicleWheel::is_used_as_steering() const { return steers; } - VehicleWheel::VehicleWheel() { - - steers=false; - engine_traction=false; + steers = false; + engine_traction = false; m_steering = real_t(0.); //m_engineForce = real_t(0.); @@ -318,7 +293,7 @@ VehicleWheel::VehicleWheel() { m_rollInfluence = real_t(0.1); m_suspensionRestLength = 0.15; - m_wheelRadius = 0.5;//0.28; + m_wheelRadius = 0.5; //0.28; m_suspensionStiffness = 5.88; m_wheelsDampingCompression = 0.83; m_wheelsDampingRelaxation = 0.88; @@ -327,15 +302,14 @@ VehicleWheel::VehicleWheel() { m_maxSuspensionTravelCm = 500; m_maxSuspensionForce = 6000; - m_suspensionRelativeVelocity=0; - m_clippedInvContactDotSuspension=1.0; - m_raycastInfo.m_isInContact=false; + m_suspensionRelativeVelocity = 0; + m_clippedInvContactDotSuspension = 1.0; + m_raycastInfo.m_isInContact = false; - body=NULL; + body = NULL; } - -void VehicleBody::_update_wheel_transform(VehicleWheel& wheel ,PhysicsDirectBodyState *s) { +void VehicleBody::_update_wheel_transform(VehicleWheel &wheel, PhysicsDirectBodyState *s) { wheel.m_raycastInfo.m_isInContact = false; @@ -346,31 +320,31 @@ void VehicleBody::_update_wheel_transform(VehicleWheel& wheel ,PhysicsDirectBody } */ - wheel.m_raycastInfo.m_hardPointWS = chassisTrans.xform( wheel.m_chassisConnectionPointCS ); + 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(); + 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 VehicleBody::_update_wheel(int p_idx,PhysicsDirectBodyState *s) { +void VehicleBody::_update_wheel(int p_idx, PhysicsDirectBodyState *s) { - VehicleWheel& wheel = *wheels[p_idx]; - _update_wheel_transform(wheel,s); + VehicleWheel &wheel = *wheels[p_idx]; + _update_wheel_transform(wheel, s); Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; - const Vector3& right = wheel.m_raycastInfo.m_wheelAxleWS; + const Vector3 &right = wheel.m_raycastInfo.m_wheelAxleWS; Vector3 fwd = up.cross(right); fwd = fwd.normalized(); //up = right.cross(fwd); //up.normalize(); //rotate around steering over de wheelAxleWS - real_t steering = wheel.steers?m_steeringValue:0.0; + real_t steering = wheel.steers ? m_steeringValue : 0.0; //print_line(itos(p_idx)+": "+rtos(steering)); - Basis steeringMat(up,steering); + Basis steeringMat(up, steering); - Basis rotatingMat(right,-wheel.m_rotation); + Basis rotatingMat(right, -wheel.m_rotation); /* if (p_idx==1) @@ -378,82 +352,69 @@ void VehicleBody::_update_wheel(int p_idx,PhysicsDirectBodyState *s) { */ Basis basis2( - right[0],up[0],fwd[0], - right[1],up[1],fwd[1], - right[2],up[2],fwd[2] - ); + 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 - ); - + wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength); } +real_t VehicleBody::_ray_cast(int p_idx, PhysicsDirectBodyState *s) { -real_t VehicleBody::_ray_cast(int p_idx,PhysicsDirectBodyState *s) { - - - VehicleWheel& wheel = *wheels[p_idx]; - - _update_wheel_transform(wheel,s); + VehicleWheel &wheel = *wheels[p_idx]; + _update_wheel_transform(wheel, s); real_t depth = -1; - real_t raylen = wheel.m_suspensionRestLength+wheel.m_wheelRadius; + 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; + const Vector3 &target = wheel.m_raycastInfo.m_contactPointWS; + source -= wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS; real_t param = real_t(0.); - PhysicsDirectSpaceState::RayResult rr; + PhysicsDirectSpaceState *ss = s->get_space_state(); - PhysicsDirectSpaceState *ss=s->get_space_state(); - - bool col = ss->intersect_ray(source,target,rr,exclude); - + bool col = ss->intersect_ray(source, target, rr, exclude); wheel.m_raycastInfo.m_groundObject = 0; - if (col) - { + if (col) { //print_line("WHEEL "+itos(p_idx)+" FROM "+source+" TO: "+target); //print_line("WHEEL "+itos(p_idx)+" COLLIDE? "+itos(col)); - param = source.distance_to(rr.position)/source.distance_to(target); + param = source.distance_to(rr.position) / source.distance_to(target); depth = raylen * param; - wheel.m_raycastInfo.m_contactNormalWS = rr.normal; + wheel.m_raycastInfo.m_contactNormalWS = rr.normal; wheel.m_raycastInfo.m_isInContact = true; if (rr.collider) - wheel.m_raycastInfo.m_groundObject=rr.collider->cast_to<PhysicsBody>(); - + wheel.m_raycastInfo.m_groundObject = rr.collider->cast_to<PhysicsBody>(); - real_t hitDistance = param*raylen; + 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) - { + 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) - { + 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 ); + 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(); @@ -461,72 +422,58 @@ real_t VehicleBody::_ray_cast(int p_idx,PhysicsDirectBodyState *s) { //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); - + (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 ); + real_t projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint); - if ( denominator >= real_t(-0.1)) - { + 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 - { + } else { real_t inv = real_t(-1.) / denominator; wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_clippedInvContactDotSuspension = inv; } - } else - { + } 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_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS; wheel.m_clippedInvContactDotSuspension = real_t(1.0); } return depth; } - -void VehicleBody::_update_suspension(PhysicsDirectBodyState *s) -{ +void VehicleBody::_update_suspension(PhysicsDirectBodyState *s) { real_t chassisMass = mass; - for (int w_it=0; w_it<wheels.size(); w_it++) - { - VehicleWheel& wheel_info = *wheels[w_it]; - + for (int w_it = 0; w_it < wheels.size(); w_it++) { + VehicleWheel &wheel_info = *wheels[w_it]; - if ( wheel_info.m_raycastInfo.m_isInContact ) - { + 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 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; + 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) ) - { + real_t susp_damping; + if (projected_rel_vel < real_t(0.0)) { susp_damping = wheel_info.m_wheelsDampingCompression; - } - else - { + } else { susp_damping = wheel_info.m_wheelsDampingRelaxation; } force -= susp_damping * projected_rel_vel; @@ -535,30 +482,23 @@ void VehicleBody::_update_suspension(PhysicsDirectBodyState *s) // RESULT wheel_info.m_wheelsSuspensionForce = force * chassisMass; - if (wheel_info.m_wheelsSuspensionForce < real_t(0.)) - { + if (wheel_info.m_wheelsSuspensionForce < real_t(0.)) { wheel_info.m_wheelsSuspensionForce = real_t(0.); } - } - else - { + } else { wheel_info.m_wheelsSuspensionForce = real_t(0.0); } } - } - //bilateral constraint between two dynamic objects -void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vector3& pos1, - PhysicsBody* body2, const Vector3& pos2, const Vector3& normal,real_t& impulse) -{ +void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vector3 &pos1, + PhysicsBody *body2, const Vector3 &pos2, const Vector3 &normal, real_t &impulse) { real_t normalLenSqr = normal.length_squared(); //ERR_FAIL_COND( normalLenSqr < real_t(1.1)); - if (normalLenSqr > real_t(1.1)) - { + if (normalLenSqr > real_t(1.1)) { impulse = real_t(0.); return; } @@ -569,16 +509,16 @@ void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vec 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 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); + vel2 = body2->get_linear_velocity() + body2->get_angular_velocity().cross(rel_pos2); Vector3 vel = vel1 - vel2; Basis b2trans; - float b2invmass=0; + float b2invmass = 0; Vector3 b2lv; Vector3 b2av; Vector3 b2invinertia; //todo @@ -590,24 +530,21 @@ void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vec 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/mass, - b2invinertia, - b2invmass); + b2trans, + rel_pos1, + rel_pos2, + normal, + s->get_inverse_inertia_tensor().get_main_diagonal(), + 1.0 / mass, + b2invinertia, + b2invmass); real_t rel_vel = jac.getRelativeVelocity( - s->get_linear_velocity(), - s->get_transform().basis.transposed().xform(s->get_angular_velocity()), - b2lv, - b2trans.xform(b2av)); - + s->get_linear_velocity(), + s->get_transform().basis.transposed().xform(s->get_angular_velocity()), + b2lv, + b2trans.xform(b2av)); rel_vel = normal.dot(vel); @@ -615,32 +552,28 @@ void VehicleBody::_resolve_single_bilateral(PhysicsDirectBodyState *s, const Vec real_t contactDamping = real_t(0.4); #define ONLY_USE_LINEAR_MASS #ifdef ONLY_USE_LINEAR_MASS - real_t massTerm = real_t(1.) / ((1.0/mass) + b2invmass); - impulse = - contactDamping * rel_vel * massTerm; + real_t massTerm = real_t(1.) / ((1.0 / mass) + b2invmass); + impulse = -contactDamping * rel_vel * massTerm; #else real_t velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; #endif - } - - -VehicleBody::btVehicleWheelContactPoint::btVehicleWheelContactPoint(PhysicsDirectBodyState *s,PhysicsBody* 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) -{ - float denom0=0; - float denom1=0; +VehicleBody::btVehicleWheelContactPoint::btVehicleWheelContactPoint(PhysicsDirectBodyState *s, PhysicsBody *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) { + float denom0 = 0; + float 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); + denom0 = s->get_inverse_mass() + frictionDirectionWorld.dot(vec); } /* TODO: Why is this code unused? @@ -654,30 +587,28 @@ VehicleBody::btVehicleWheelContactPoint::btVehicleWheelContactPoint(PhysicsDirec } */ - real_t relaxation = 1.f; - m_jacDiagABInv = relaxation/(denom0+denom1); + real_t relaxation = 1.f; + m_jacDiagABInv = relaxation / (denom0 + denom1); } +real_t VehicleBody::_calc_rolling_friction(btVehicleWheelContactPoint &contactPoint) { -real_t VehicleBody::_calc_rolling_friction(btVehicleWheelContactPoint& contactPoint) { - - real_t j1=0.f; + real_t j1 = 0.f; - const Vector3& contactPosWorld = contactPoint.m_frictionPositionWorld; + 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; + 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 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); - + vel2 = contactPoint.m_body1->get_linear_velocity() + contactPoint.m_body1->get_angular_velocity().cross(rel_pos2); } Vector3 vel = vel1 - vel2; @@ -687,10 +618,9 @@ real_t VehicleBody::_calc_rolling_friction(btVehicleWheelContactPoint& contactPo // calculate j that moves us to zero relative velocity j1 = -vrel * contactPoint.m_jacDiagABInv; - return CLAMP(j1,-maxImpulse,maxImpulse); + return CLAMP(j1, -maxImpulse, maxImpulse); } - static const real_t sideFrictionStiffness2 = real_t(1.0); void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { @@ -706,37 +636,31 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { int numWheelsOnGround = 0; - //collapse all those loops into one! - for (int i=0;i<wheels.size();i++) - { - VehicleWheel& wheelInfo = *wheels[i]; + for (int i = 0; i < wheels.size(); i++) { + VehicleWheel &wheelInfo = *wheels[i]; if (wheelInfo.m_raycastInfo.m_isInContact) numWheelsOnGround++; m_sideImpulse[i] = real_t(0.); m_forwardImpulse[i] = real_t(0.); - } { - for (int i=0;i<wheels.size();i++) - { + for (int i = 0; i < wheels.size(); i++) { - VehicleWheel& wheelInfo = *wheels[i]; + VehicleWheel &wheelInfo = *wheels[i]; - - if (wheelInfo.m_raycastInfo.m_isInContact) - { + if (wheelInfo.m_raycastInfo.m_isInContact) { //const btTransform& wheelTrans = getWheelTransformWS( i ); - Basis wheelBasis0 = wheelInfo.m_worldTransform.basis;//get_global_transform().basis; + Basis wheelBasis0 = wheelInfo.m_worldTransform.basis; //get_global_transform().basis; m_axle[i] = wheelBasis0.get_axis(Vector3::AXIS_X); //m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS; - const Vector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; + const Vector3 &surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; real_t proj = m_axle[i].dot(surfNormalWS); m_axle[i] -= surfNormalWS * proj; m_axle[i] = m_axle[i].normalized(); @@ -744,14 +668,11 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { m_forwardWS[i] = surfNormalWS.cross(m_axle[i]); m_forwardWS[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[i]); + wheelInfo.m_raycastInfo.m_groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, + m_axle[i], m_sideImpulse[i]); m_sideImpulse[i] *= sideFrictionStiffness2; - - } } } @@ -761,57 +682,46 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { bool sliding = false; { - for (int wheel =0;wheel <wheels.size();wheel++) - { - VehicleWheel& wheelInfo = *wheels[wheel]; - + for (int wheel = 0; wheel < wheels.size(); wheel++) { + VehicleWheel &wheelInfo = *wheels[wheel]; //class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; - real_t rollingFriction = 0.f; + real_t rollingFriction = 0.f; - if (wheelInfo.m_raycastInfo.m_isInContact) - { - if (engine_force != 0.f) - { - rollingFriction = -engine_force* s->get_step(); - } else - { + if (wheelInfo.m_raycastInfo.m_isInContact) { + if (engine_force != 0.f) { + rollingFriction = -engine_force * s->get_step(); + } else { real_t defaultRollingFrictionImpulse = 0.f; - float cbrake = MAX(wheelInfo.m_brake,brake); + float cbrake = MAX(wheelInfo.m_brake, brake); real_t maxImpulse = cbrake ? cbrake : defaultRollingFrictionImpulse; - btVehicleWheelContactPoint contactPt(s,wheelInfo.m_raycastInfo.m_groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse); + 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[wheel] = real_t(0.); - wheelInfo.m_skidInfo= real_t(1.); + wheelInfo.m_skidInfo = real_t(1.); - if (wheelInfo.m_raycastInfo.m_isInContact) - { - 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[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep; - m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; + real_t x = (m_forwardImpulse[wheel]) * fwdFactor; + real_t y = (m_sideImpulse[wheel]) * sideFactor; - real_t x = (m_forwardImpulse[wheel] ) * fwdFactor; - real_t y = (m_sideImpulse[wheel] ) * sideFactor; + real_t impulseSquared = (x * x + y * y); - real_t impulseSquared = (x*x + y*y); - - if (impulseSquared > maximpSquared) - { + if (impulseSquared > maximpSquared) { sliding = true; real_t factor = maximp / Math::sqrt(impulseSquared); @@ -819,22 +729,14 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { 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[wheel] *= wheels[wheel]->m_skidInfo; + 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[wheel] *= wheels[wheel]->m_skidInfo; m_sideImpulse[wheel] *= wheels[wheel]->m_skidInfo; } } @@ -843,36 +745,32 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { // apply the impulses { - for (int wheel = 0;wheel<wheels.size(); wheel++) - { - VehicleWheel& wheelInfo = *wheels[wheel]; + for (int wheel = 0; wheel < wheels.size(); wheel++) { + VehicleWheel &wheelInfo = *wheels[wheel]; Vector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - - s->get_transform().origin; + s->get_transform().origin; - if (m_forwardImpulse[wheel] != real_t(0.)) - { - s->apply_impulse(rel_pos,m_forwardWS[wheel]*(m_forwardImpulse[wheel])); + if (m_forwardImpulse[wheel] != real_t(0.)) { + s->apply_impulse(rel_pos, m_forwardWS[wheel] * (m_forwardImpulse[wheel])); } - if (m_sideImpulse[wheel] != real_t(0.)) - { - PhysicsBody* groundObject = wheelInfo.m_raycastInfo.m_groundObject; + if (m_sideImpulse[wheel] != real_t(0.)) { + PhysicsBody *groundObject = wheelInfo.m_raycastInfo.m_groundObject; Vector3 rel_pos2; if (groundObject) { - rel_pos2=wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->get_global_transform().origin; + 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()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis); - rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence)); + Vector3 vChassisWorldUp = s->get_transform().basis.transposed()[1]; //getRigidBody()->getCenterOfMassTransform().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(rel_pos,sideImp); + s->apply_impulse(rel_pos, sideImp); //apply friction impulse on the ground //todo @@ -880,74 +778,62 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) { } } } - - } - void VehicleBody::_direct_state_changed(Object *p_state) { - PhysicsDirectBodyState *s = p_state->cast_to<PhysicsDirectBodyState>(); set_ignore_transform_notification(true); set_global_transform(s->get_transform()); set_ignore_transform_notification(false); - float step = s->get_step(); - for(int i=0;i<wheels.size();i++) { + for (int i = 0; i < wheels.size(); i++) { - _update_wheel(i,s); + _update_wheel(i, s); } + for (int i = 0; i < wheels.size(); i++) { - for(int i=0;i<wheels.size();i++) { - - _ray_cast(i,s); + _ray_cast(i, s); wheels[i]->set_transform(s->get_transform().inverse() * wheels[i]->m_worldTransform); } _update_suspension(s); - for(int i=0;i<wheels.size();i++) { + for (int i = 0; i < wheels.size(); i++) { //apply suspension force - VehicleWheel& wheel = *wheels[i]; + VehicleWheel &wheel = *wheels[i]; real_t suspensionForce = wheel.m_wheelsSuspensionForce; - if (suspensionForce > wheel.m_maxSuspensionForce) - { + if (suspensionForce > wheel.m_maxSuspensionForce) { suspensionForce = wheel.m_maxSuspensionForce; } Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin; - s->apply_impulse(relpos,impulse); + s->apply_impulse(relpos, impulse); //getRigidBody()->applyImpulse(impulse, relpos); - } - _update_friction(s); - - for (int i=0;i<wheels.size();i++) - { - VehicleWheel& wheel = *wheels[i]; + for (int i = 0; i < wheels.size(); i++) { + VehicleWheel &wheel = *wheels[i]; Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - s->get_transform().origin; - Vector3 vel = s->get_linear_velocity() + (s->get_angular_velocity()).cross(relpos);// * mPos); + Vector3 vel = s->get_linear_velocity() + (s->get_angular_velocity()).cross(relpos); // * mPos); - if (wheel.m_raycastInfo.m_isInContact) - { - const Transform& chassisWorldTransform = s->get_transform(); + if (wheel.m_raycastInfo.m_isInContact) { + const Transform &chassisWorldTransform = s->get_transform(); - Vector3 fwd ( - chassisWorldTransform.basis[0][Vector3::AXIS_Z], - chassisWorldTransform.basis[1][Vector3::AXIS_Z], - chassisWorldTransform.basis[2][Vector3::AXIS_Z]); + 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; @@ -957,126 +843,115 @@ void VehicleBody::_direct_state_changed(Object *p_state) { wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius); wheel.m_rotation += wheel.m_deltaRotation; - } else - { + } else { wheel.m_rotation += wheel.m_deltaRotation; } - wheel.m_deltaRotation *= real_t(0.99);//damping of rotation when not in contact - + wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact } linear_velocity = s->get_linear_velocity(); } void VehicleBody::set_mass(real_t p_mass) { - mass=p_mass; - PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_MASS,mass); + mass = p_mass; + PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_MASS, mass); } -real_t VehicleBody::get_mass() const{ +real_t VehicleBody::get_mass() const { return mass; } - void VehicleBody::set_friction(real_t p_friction) { - friction=p_friction; - PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_FRICTION,friction); + friction = p_friction; + PhysicsServer::get_singleton()->body_set_param(get_rid(), PhysicsServer::BODY_PARAM_FRICTION, friction); } -real_t VehicleBody::get_friction() const{ +real_t VehicleBody::get_friction() const { return friction; } void VehicleBody::set_engine_force(float p_force) { - engine_force=p_force; + engine_force = p_force; } -float VehicleBody::get_engine_force() const{ +float VehicleBody::get_engine_force() const { return engine_force; } -void VehicleBody::set_brake(float p_brake){ +void VehicleBody::set_brake(float p_brake) { - brake=p_brake; + brake = p_brake; } -float VehicleBody::get_brake() const{ +float VehicleBody::get_brake() const { return brake; } -void VehicleBody::set_steering(float p_steering){ +void VehicleBody::set_steering(float p_steering) { - m_steeringValue=p_steering; + m_steeringValue = p_steering; } -float VehicleBody::get_steering() const{ +float VehicleBody::get_steering() const { return m_steeringValue; } -Vector3 VehicleBody::get_linear_velocity() const -{ +Vector3 VehicleBody::get_linear_velocity() const { return linear_velocity; } -void VehicleBody::_bind_methods(){ - - ClassDB::bind_method(D_METHOD("set_mass","mass"),&VehicleBody::set_mass); - ClassDB::bind_method(D_METHOD("get_mass"),&VehicleBody::get_mass); - - ClassDB::bind_method(D_METHOD("set_friction","friction"),&VehicleBody::set_friction); - ClassDB::bind_method(D_METHOD("get_friction"),&VehicleBody::get_friction); +void VehicleBody::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_engine_force","engine_force"),&VehicleBody::set_engine_force); - ClassDB::bind_method(D_METHOD("get_engine_force"),&VehicleBody::get_engine_force); + ClassDB::bind_method(D_METHOD("set_mass", "mass"), &VehicleBody::set_mass); + ClassDB::bind_method(D_METHOD("get_mass"), &VehicleBody::get_mass); - ClassDB::bind_method(D_METHOD("set_brake","brake"),&VehicleBody::set_brake); - ClassDB::bind_method(D_METHOD("get_brake"),&VehicleBody::get_brake); + ClassDB::bind_method(D_METHOD("set_friction", "friction"), &VehicleBody::set_friction); + ClassDB::bind_method(D_METHOD("get_friction"), &VehicleBody::get_friction); - ClassDB::bind_method(D_METHOD("set_steering","steering"),&VehicleBody::set_steering); - ClassDB::bind_method(D_METHOD("get_steering"),&VehicleBody::get_steering); + ClassDB::bind_method(D_METHOD("set_engine_force", "engine_force"), &VehicleBody::set_engine_force); + ClassDB::bind_method(D_METHOD("get_engine_force"), &VehicleBody::get_engine_force); - ClassDB::bind_method(D_METHOD("get_linear_velocity"),&VehicleBody::get_linear_velocity); + ClassDB::bind_method(D_METHOD("set_brake", "brake"), &VehicleBody::set_brake); + ClassDB::bind_method(D_METHOD("get_brake"), &VehicleBody::get_brake); - ClassDB::bind_method(D_METHOD("_direct_state_changed"),&VehicleBody::_direct_state_changed); + ClassDB::bind_method(D_METHOD("set_steering", "steering"), &VehicleBody::set_steering); + ClassDB::bind_method(D_METHOD("get_steering"), &VehicleBody::get_steering); - ADD_GROUP("Motion",""); - ADD_PROPERTY( PropertyInfo(Variant::REAL,"engine_force",PROPERTY_HINT_RANGE,"0.00,1024.0,0.01"),"set_engine_force","get_engine_force"); - ADD_PROPERTY( PropertyInfo(Variant::REAL,"brake",PROPERTY_HINT_RANGE,"0.0,1.0,0.01"),"set_brake","get_brake"); - ADD_PROPERTY( PropertyInfo(Variant::REAL,"steering",PROPERTY_HINT_RANGE,"-180,180.0,0.01"),"set_steering","get_steering"); - ADD_GROUP("Mass",""); - ADD_PROPERTY( PropertyInfo(Variant::REAL,"mass",PROPERTY_HINT_RANGE,"0.01,65536,0.01"),"set_mass","get_mass"); - ADD_PROPERTY( PropertyInfo(Variant::REAL,"friction",PROPERTY_HINT_RANGE,"0.01,1,0.01"),"set_friction","get_friction"); + ClassDB::bind_method(D_METHOD("get_linear_velocity"), &VehicleBody::get_linear_velocity); + ClassDB::bind_method(D_METHOD("_direct_state_changed"), &VehicleBody::_direct_state_changed); + ADD_GROUP("Motion", ""); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "engine_force", PROPERTY_HINT_RANGE, "0.00,1024.0,0.01"), "set_engine_force", "get_engine_force"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "brake", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_brake", "get_brake"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "steering", PROPERTY_HINT_RANGE, "-180,180.0,0.01"), "set_steering", "get_steering"); + ADD_GROUP("Mass", ""); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "mass", PROPERTY_HINT_RANGE, "0.01,65536,0.01"), "set_mass", "get_mass"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "friction", PROPERTY_HINT_RANGE, "0.01,1,0.01"), "set_friction", "get_friction"); } +VehicleBody::VehicleBody() + : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) { - -VehicleBody::VehicleBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) { - - - m_pitchControl=0; + m_pitchControl = 0; m_currentVehicleSpeedKmHour = real_t(0.); m_steeringValue = real_t(0.); - engine_force=0; - brake=0; + engine_force = 0; + brake = 0; + friction = 1; - - friction=1; - - ccd=false; + ccd = false; exclude.insert(get_rid()); - PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed"); + PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed"); set_mass(40); } - |