742 lines
21 KiB
C++
742 lines
21 KiB
C++
|
/*************************************************************************/
|
||
|
/* car_body.cpp */
|
||
|
/*************************************************************************/
|
||
|
/* This file is part of: */
|
||
|
/* GODOT ENGINE */
|
||
|
/* http://www.godotengine.org */
|
||
|
/*************************************************************************/
|
||
|
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||
|
/* */
|
||
|
/* 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 "car_body.h"
|
||
|
|
||
|
#define DEG2RADMUL (Math_PI/180.0)
|
||
|
#define RAD2DEGMUL (180.0/Math_PI)
|
||
|
|
||
|
void CarWheel::_notification(int p_what) {
|
||
|
|
||
|
|
||
|
if (p_what==NOTIFICATION_ENTER_SCENE) {
|
||
|
|
||
|
if (!get_parent())
|
||
|
return;
|
||
|
CarBody *cb = get_parent()->cast_to<CarBody>();
|
||
|
if (!cb)
|
||
|
return;
|
||
|
body=cb;
|
||
|
local_xform=get_transform();
|
||
|
cb->wheels.push_back(this);
|
||
|
}
|
||
|
if (p_what==NOTIFICATION_EXIT_SCENE) {
|
||
|
|
||
|
if (!get_parent())
|
||
|
return;
|
||
|
CarBody *cb = get_parent()->cast_to<CarBody>();
|
||
|
if (!cb)
|
||
|
return;
|
||
|
cb->wheels.erase(this);
|
||
|
body=NULL;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void CarWheel::set_side_friction(real_t p_friction) {
|
||
|
|
||
|
side_friction=p_friction;
|
||
|
}
|
||
|
void CarWheel::set_forward_friction(real_t p_friction) {
|
||
|
|
||
|
forward_friction=p_friction;
|
||
|
}
|
||
|
void CarWheel::set_travel(real_t p_travel) {
|
||
|
|
||
|
travel=p_travel;
|
||
|
update_gizmo();
|
||
|
|
||
|
}
|
||
|
void CarWheel::set_radius(real_t p_radius) {
|
||
|
|
||
|
radius=p_radius;
|
||
|
update_gizmo();
|
||
|
|
||
|
}
|
||
|
void CarWheel::set_resting_frac(real_t p_frac) {
|
||
|
|
||
|
resting_frac=p_frac;
|
||
|
}
|
||
|
void CarWheel::set_damping_frac(real_t p_frac) {
|
||
|
|
||
|
damping_frac=p_frac;
|
||
|
}
|
||
|
void CarWheel::set_num_rays(real_t p_rays) {
|
||
|
|
||
|
num_rays=p_rays;
|
||
|
}
|
||
|
|
||
|
real_t CarWheel::get_side_friction() const{
|
||
|
|
||
|
return side_friction;
|
||
|
}
|
||
|
real_t CarWheel::get_forward_friction() const{
|
||
|
|
||
|
return forward_friction;
|
||
|
}
|
||
|
real_t CarWheel::get_travel() const{
|
||
|
|
||
|
return travel;
|
||
|
}
|
||
|
real_t CarWheel::get_radius() const{
|
||
|
|
||
|
return radius;
|
||
|
}
|
||
|
real_t CarWheel::get_resting_frac() const{
|
||
|
|
||
|
return resting_frac;
|
||
|
}
|
||
|
real_t CarWheel::get_damping_frac() const{
|
||
|
|
||
|
return damping_frac;
|
||
|
}
|
||
|
|
||
|
int CarWheel::get_num_rays() const{
|
||
|
|
||
|
return num_rays;
|
||
|
}
|
||
|
|
||
|
|
||
|
void CarWheel::update(real_t dt) {
|
||
|
|
||
|
|
||
|
if (dt <= 0.0f)
|
||
|
return;
|
||
|
|
||
|
float origAngVel = angVel;
|
||
|
|
||
|
if (locked)
|
||
|
{
|
||
|
angVel = 0;
|
||
|
torque = 0;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
|
||
|
float wheelMass = 0.03f * body->mass;
|
||
|
float inertia = 0.5f * (radius * radius) * wheelMass;
|
||
|
|
||
|
angVel += torque * dt / inertia;
|
||
|
torque = 0;
|
||
|
|
||
|
// prevent friction from reversing dir - todo do this better
|
||
|
// by limiting the torque
|
||
|
if (((origAngVel > angVelForGrip) && (angVel < angVelForGrip)) ||
|
||
|
((origAngVel < angVelForGrip) && (angVel > angVelForGrip)))
|
||
|
angVel = angVelForGrip;
|
||
|
|
||
|
angVel += driveTorque * dt / inertia;
|
||
|
driveTorque = 0;
|
||
|
|
||
|
float maxAngVel = 200;
|
||
|
print_line("angvel: "+rtos(angVel));
|
||
|
angVel = CLAMP(angVel, -maxAngVel, maxAngVel);
|
||
|
|
||
|
axisAngle += Math::rad2deg(dt * angVel);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
bool CarWheel::add_forces(PhysicsDirectBodyState *s) {
|
||
|
|
||
|
|
||
|
Vector3 force;
|
||
|
|
||
|
PhysicsDirectSpaceState *space = s->get_space_state();
|
||
|
|
||
|
Transform world = s->get_transform() * local_xform;
|
||
|
|
||
|
// OpenGl has differnet row/column order for matrixes than XNA has ..
|
||
|
//Vector3 wheelFwd = world.get_basis().get_axis(Vector3::AXIS_Z);
|
||
|
//Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.GetOrientation().GetCol(0);
|
||
|
Vector3 wheelUp = world.get_basis().get_axis(Vector3::AXIS_Y);
|
||
|
Vector3 wheelFwd = Matrix3(wheelUp,Math::deg2rad(steerAngle)).xform( world.get_basis().get_axis(Vector3::AXIS_Z) );
|
||
|
Vector3 wheelLeft = -wheelUp.cross(wheelFwd).normalized();
|
||
|
Vector3 worldPos = world.origin;
|
||
|
Vector3 worldAxis = wheelUp;
|
||
|
|
||
|
// start of ray
|
||
|
float rayLen = 2.0f * radius + travel;
|
||
|
Vector3 wheelRayEnd = worldPos - radius * worldAxis;
|
||
|
Vector3 wheelRayBegin = wheelRayEnd + rayLen * worldAxis;
|
||
|
//wheelRayEnd = -rayLen * worldAxis;
|
||
|
|
||
|
//Assert(PhysicsSystem.CurrentPhysicsSystem);
|
||
|
|
||
|
|
||
|
///Assert(collSystem);
|
||
|
///
|
||
|
const int maxNumRays = 32;
|
||
|
|
||
|
int numRaysUse = MIN(num_rays, maxNumRays);
|
||
|
|
||
|
// adjust the start position of the ray - divide the wheel into numRays+2
|
||
|
// rays, but don't use the first/last.
|
||
|
float deltaFwd = (2.0f * radius) / (numRaysUse + 1);
|
||
|
float deltaFwdStart = deltaFwd;
|
||
|
|
||
|
float fracs[maxNumRays];
|
||
|
Vector3 segmentEnds[maxNumRays];
|
||
|
Vector3 groundPositions[maxNumRays];
|
||
|
Vector3 groundNormals[maxNumRays];
|
||
|
|
||
|
|
||
|
lastOnFloor = false;
|
||
|
int bestIRay = 0;
|
||
|
int iRay;
|
||
|
|
||
|
|
||
|
for (iRay = 0; iRay < numRaysUse; ++iRay)
|
||
|
{
|
||
|
fracs[iRay] = 1e20;
|
||
|
// work out the offset relative to the middle ray
|
||
|
float distFwd = (deltaFwdStart + iRay * deltaFwd) - radius;
|
||
|
float zOffset = radius * (1.0f - (float)Math::cos( Math::deg2rad( 90.0f * (distFwd / radius))));
|
||
|
|
||
|
segmentEnds[iRay] = wheelRayEnd + distFwd * wheelFwd + zOffset * wheelUp;
|
||
|
|
||
|
|
||
|
PhysicsDirectSpaceState::RayResult rr;
|
||
|
|
||
|
bool collided = space->intersect_ray(wheelRayBegin,segmentEnds[iRay],rr,body->exclude);
|
||
|
|
||
|
|
||
|
if (collided){
|
||
|
lastOnFloor = true;
|
||
|
groundPositions[iRay]=rr.position;
|
||
|
groundNormals[iRay]=rr.normal;
|
||
|
fracs[iRay] = ((wheelRayBegin-rr.position).length() / (wheelRayBegin-wheelRayEnd).length());
|
||
|
if (fracs[iRay] < fracs[bestIRay])
|
||
|
bestIRay = iRay;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
if (!lastOnFloor)
|
||
|
return false;
|
||
|
|
||
|
//Assert(bestIRay < numRays);
|
||
|
|
||
|
// use the best one
|
||
|
Vector3 groundPos = groundPositions[bestIRay];
|
||
|
float frac = fracs[bestIRay];
|
||
|
|
||
|
// const Vector3 groundNormal = (worldPos - segments[bestIRay].GetEnd()).NormaliseSafe();
|
||
|
// const Vector3 groundNormal = groundNormals[bestIRay];
|
||
|
|
||
|
|
||
|
Vector3 groundNormal = worldAxis;
|
||
|
|
||
|
if (numRaysUse > 1)
|
||
|
{
|
||
|
for (iRay = 0; iRay < numRaysUse; ++iRay)
|
||
|
{
|
||
|
if (fracs[iRay] <= 1.0f)
|
||
|
{
|
||
|
groundNormal += (1.0f - fracs[iRay]) * (worldPos - segmentEnds[iRay]);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
groundNormal.normalize();
|
||
|
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
groundNormal = groundNormals[bestIRay];
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
float spring = (body->mass/body->wheels.size()) * s->get_total_gravity().length() / (resting_frac * travel);
|
||
|
|
||
|
float displacement = rayLen * (1.0f - frac);
|
||
|
displacement = CLAMP(displacement, 0, travel);
|
||
|
|
||
|
|
||
|
|
||
|
float displacementForceMag = displacement * spring;
|
||
|
|
||
|
// reduce force when suspension is par to ground
|
||
|
displacementForceMag *= groundNormals[bestIRay].dot(worldAxis);
|
||
|
|
||
|
// apply damping
|
||
|
float damping = 2.0f * (float)Math::sqrt(spring * body->mass);
|
||
|
damping /= body->wheels.size(); // assume wheels act together
|
||
|
damping *= damping_frac; // a bit bouncy
|
||
|
|
||
|
float upSpeed = (displacement - lastDisplacement) / s->get_step();
|
||
|
|
||
|
float dampingForceMag = upSpeed * damping;
|
||
|
|
||
|
float totalForceMag = displacementForceMag + dampingForceMag;
|
||
|
|
||
|
if (totalForceMag < 0.0f) totalForceMag = 0.0f;
|
||
|
|
||
|
Vector3 extraForce = totalForceMag * worldAxis;
|
||
|
|
||
|
|
||
|
force += extraForce;
|
||
|
// side-slip friction and drive force. Work out wheel- and floor-relative coordinate frame
|
||
|
Vector3 groundUp = groundNormal;
|
||
|
Vector3 groundLeft = groundNormal.cross(wheelFwd).normalized();
|
||
|
|
||
|
Vector3 groundFwd = groundLeft.cross(groundUp);
|
||
|
|
||
|
Vector3 wheelPointVel = s->get_linear_velocity() +
|
||
|
(s->get_angular_velocity()).cross(s->get_transform().basis.xform(local_xform.origin));// * mPos);
|
||
|
|
||
|
Vector3 rimVel = -angVel * wheelLeft.cross(groundPos - worldPos);
|
||
|
wheelPointVel += rimVel;
|
||
|
|
||
|
// if sitting on another body then adjust for its velocity.
|
||
|
/*if (worldBody != null)
|
||
|
{
|
||
|
Vector3 worldVel = worldBody.Velocity +
|
||
|
Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position);
|
||
|
|
||
|
wheelPointVel -= worldVel;
|
||
|
}*/
|
||
|
|
||
|
// sideways forces
|
||
|
float noslipVel = 0.2f;
|
||
|
float slipVel = 0.4f;
|
||
|
float slipFactor = 0.7f;
|
||
|
|
||
|
float smallVel = 3;
|
||
|
float friction = side_friction;
|
||
|
|
||
|
float sideVel = wheelPointVel.dot(groundLeft);
|
||
|
|
||
|
if ((sideVel > slipVel) || (sideVel < -slipVel))
|
||
|
friction *= slipFactor;
|
||
|
else
|
||
|
if ((sideVel > noslipVel) || (sideVel < -noslipVel))
|
||
|
friction *= 1.0f - (1.0f - slipFactor) * (Math::absf(sideVel) - noslipVel) / (slipVel - noslipVel);
|
||
|
|
||
|
if (sideVel < 0.0f)
|
||
|
friction *= -1.0f;
|
||
|
|
||
|
if (Math::absf(sideVel) < smallVel)
|
||
|
friction *= Math::absf(sideVel) / smallVel;
|
||
|
|
||
|
float sideForce = -friction * totalForceMag;
|
||
|
|
||
|
extraForce = sideForce * groundLeft;
|
||
|
force += extraForce;
|
||
|
// fwd/back forces
|
||
|
friction = forward_friction;
|
||
|
float fwdVel = wheelPointVel.dot(groundFwd);
|
||
|
|
||
|
if ((fwdVel > slipVel) || (fwdVel < -slipVel))
|
||
|
friction *= slipFactor;
|
||
|
else
|
||
|
if ((fwdVel > noslipVel) || (fwdVel < -noslipVel))
|
||
|
friction *= 1.0f - (1.0f - slipFactor) * (Math::absf(fwdVel) - noslipVel) / (slipVel - noslipVel);
|
||
|
|
||
|
if (fwdVel < 0.0f)
|
||
|
friction *= -1.0f;
|
||
|
|
||
|
if (Math::absf(fwdVel) < smallVel)
|
||
|
friction *= Math::absf(fwdVel) / smallVel;
|
||
|
|
||
|
float fwdForce = -friction * totalForceMag;
|
||
|
|
||
|
extraForce = fwdForce * groundFwd;
|
||
|
force += extraForce;
|
||
|
|
||
|
|
||
|
//if (!force.IsSensible())
|
||
|
//{
|
||
|
// TRACE_FILE_IF(ONCE_1)
|
||
|
// TRACE("Bad force in car wheel\n");
|
||
|
// return true;
|
||
|
//}
|
||
|
|
||
|
// fwd force also spins the wheel
|
||
|
Vector3 wheelCentreVel = s->get_linear_velocity() +
|
||
|
(s->get_angular_velocity()).cross(s->get_transform().basis.xform(local_xform.origin));
|
||
|
|
||
|
angVelForGrip = wheelCentreVel.dot(groundFwd) / radius;
|
||
|
torque += -fwdForce * radius;
|
||
|
|
||
|
// add force to car
|
||
|
// carBody.AddWorldForce(force, groundPos);
|
||
|
|
||
|
s->add_force(force,(groundPos-s->get_transform().origin));
|
||
|
|
||
|
// add force to the world
|
||
|
/*
|
||
|
if (worldBody != null && !worldBody.Immovable)
|
||
|
{
|
||
|
// todo get the position in the right place...
|
||
|
// also limit the velocity that this force can produce by looking at the
|
||
|
// mass/inertia of the other object
|
||
|
float maxOtherBodyAcc = 500.0f;
|
||
|
float maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass;
|
||
|
|
||
|
if (force.LengthSquared() > (maxOtherBodyForce * maxOtherBodyForce))
|
||
|
force *= maxOtherBodyForce / force.Length();
|
||
|
|
||
|
worldBody.AddWorldForce(-force, groundPos);
|
||
|
}*/
|
||
|
|
||
|
Transform wheel_xf = local_xform;
|
||
|
wheel_xf.origin += wheelUp * displacement;
|
||
|
wheel_xf.basis = wheel_xf.basis * Matrix3(Vector3(0,1,0),Math::deg2rad(steerAngle));
|
||
|
//wheel_xf.basis = wheel_xf.basis * Matrix3(wheel_xf.basis[0],-Math::deg2rad(axisAngle));
|
||
|
|
||
|
set_transform(wheel_xf);
|
||
|
lastDisplacement=displacement;
|
||
|
return true;
|
||
|
|
||
|
}
|
||
|
|
||
|
void CarWheel::set_type_drive(bool p_enable) {
|
||
|
|
||
|
type_drive=p_enable;
|
||
|
}
|
||
|
|
||
|
bool CarWheel::is_type_drive() const {
|
||
|
|
||
|
return type_drive;
|
||
|
}
|
||
|
|
||
|
void CarWheel::set_type_steer(bool p_enable) {
|
||
|
|
||
|
type_steer=p_enable;
|
||
|
}
|
||
|
|
||
|
bool CarWheel::is_type_steer() const {
|
||
|
|
||
|
return type_steer;
|
||
|
}
|
||
|
|
||
|
|
||
|
void CarWheel::_bind_methods() {
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("set_side_friction","friction"),&CarWheel::set_side_friction);
|
||
|
ObjectTypeDB::bind_method(_MD("set_forward_friction","friction"),&CarWheel::set_forward_friction);
|
||
|
ObjectTypeDB::bind_method(_MD("set_travel","distance"),&CarWheel::set_travel);
|
||
|
ObjectTypeDB::bind_method(_MD("set_radius","radius"),&CarWheel::set_radius);
|
||
|
ObjectTypeDB::bind_method(_MD("set_resting_frac","frac"),&CarWheel::set_resting_frac);
|
||
|
ObjectTypeDB::bind_method(_MD("set_damping_frac","frac"),&CarWheel::set_damping_frac);
|
||
|
ObjectTypeDB::bind_method(_MD("set_num_rays","amount"),&CarWheel::set_num_rays);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("get_side_friction"),&CarWheel::get_side_friction);
|
||
|
ObjectTypeDB::bind_method(_MD("get_forward_friction"),&CarWheel::get_forward_friction);
|
||
|
ObjectTypeDB::bind_method(_MD("get_travel"),&CarWheel::get_travel);
|
||
|
ObjectTypeDB::bind_method(_MD("get_radius"),&CarWheel::get_radius);
|
||
|
ObjectTypeDB::bind_method(_MD("get_resting_frac"),&CarWheel::get_resting_frac);
|
||
|
ObjectTypeDB::bind_method(_MD("get_damping_frac"),&CarWheel::get_damping_frac);
|
||
|
ObjectTypeDB::bind_method(_MD("get_num_rays"),&CarWheel::get_num_rays);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("set_type_drive","enable"),&CarWheel::set_type_drive);
|
||
|
ObjectTypeDB::bind_method(_MD("is_type_drive"),&CarWheel::is_type_drive);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("set_type_steer","enable"),&CarWheel::set_type_steer);
|
||
|
ObjectTypeDB::bind_method(_MD("is_type_steer"),&CarWheel::is_type_steer);
|
||
|
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::BOOL,"type/drive"),_SCS("set_type_drive"),_SCS("is_type_drive"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::BOOL,"type/steer"),_SCS("set_type_steer"),_SCS("is_type_steer"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/side_friction",PROPERTY_HINT_RANGE,"0.01,64,0.01"),_SCS("set_side_friction"),_SCS("get_side_friction"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/forward_friction",PROPERTY_HINT_RANGE,"0.01,64,0.01"),_SCS("set_forward_friction"),_SCS("get_forward_friction"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/travel",PROPERTY_HINT_RANGE,"0.01,1024,0.01"),_SCS("set_travel"),_SCS("get_travel"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/radius",PROPERTY_HINT_RANGE,"0.01,1024,0.01"),_SCS("set_radius"),_SCS("get_radius"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/resting_frac",PROPERTY_HINT_RANGE,"0.01,64,0.01"),_SCS("set_resting_frac"),_SCS("get_resting_frac"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/damping_frac",PROPERTY_HINT_RANGE,"0.01,64,0.01"),_SCS("set_damping_frac"),_SCS("get_damping_frac"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/num_rays",PROPERTY_HINT_RANGE,"1,32,1"),_SCS("set_num_rays"),_SCS("get_num_rays"));
|
||
|
|
||
|
}
|
||
|
|
||
|
CarWheel::CarWheel() {
|
||
|
|
||
|
side_friction=4.7;
|
||
|
forward_friction=5.0;
|
||
|
travel=0.2;
|
||
|
radius=0.4;
|
||
|
resting_frac=0.45;
|
||
|
damping_frac=0.3;
|
||
|
num_rays=1;
|
||
|
|
||
|
angVel = 0.0f;
|
||
|
steerAngle = 0.0f;
|
||
|
torque = 0.0f;
|
||
|
driveTorque = 0.0f;
|
||
|
axisAngle = 0.0f;
|
||
|
upSpeed = 0.0f;
|
||
|
locked = false;
|
||
|
lastDisplacement = 0.0f;
|
||
|
lastOnFloor = false;
|
||
|
angVelForGrip = 0.0f;
|
||
|
angVelForGrip=0;
|
||
|
|
||
|
type_drive=false;
|
||
|
type_steer=false;
|
||
|
|
||
|
}
|
||
|
|
||
|
///
|
||
|
|
||
|
|
||
|
void CarBody::set_max_steer_angle(real_t p_angle) {
|
||
|
|
||
|
max_steer_angle=p_angle;
|
||
|
}
|
||
|
void CarBody::set_steer_rate(real_t p_rate) {
|
||
|
|
||
|
steer_rate=p_rate;
|
||
|
}
|
||
|
void CarBody::set_drive_torque(real_t p_torque) {
|
||
|
|
||
|
drive_torque=p_torque;
|
||
|
}
|
||
|
|
||
|
real_t CarBody::get_max_steer_angle() const{
|
||
|
|
||
|
return max_steer_angle;
|
||
|
}
|
||
|
real_t CarBody::get_steer_rate() const{
|
||
|
|
||
|
return steer_rate;
|
||
|
}
|
||
|
real_t CarBody::get_drive_torque() const{
|
||
|
|
||
|
return drive_torque;
|
||
|
}
|
||
|
|
||
|
|
||
|
void CarBody::set_target_steering(float p_steering) {
|
||
|
|
||
|
target_steering=p_steering;
|
||
|
}
|
||
|
|
||
|
void CarBody::set_target_accelerate(float p_accelerate) {
|
||
|
target_accelerate=p_accelerate;
|
||
|
}
|
||
|
|
||
|
void CarBody::set_hand_brake(float p_amont) {
|
||
|
|
||
|
hand_brake=p_amont;
|
||
|
}
|
||
|
|
||
|
real_t CarBody::get_target_steering() const {
|
||
|
|
||
|
return target_steering;
|
||
|
}
|
||
|
real_t CarBody::get_target_accelerate() const {
|
||
|
|
||
|
return target_accelerate;
|
||
|
}
|
||
|
real_t CarBody::get_hand_brake() const {
|
||
|
|
||
|
return hand_brake;
|
||
|
}
|
||
|
|
||
|
|
||
|
void CarBody::_direct_state_changed(Object *p_state) {
|
||
|
|
||
|
PhysicsDirectBodyState *state=p_state->cast_to<PhysicsDirectBodyState>();
|
||
|
|
||
|
float dt = state->get_step();
|
||
|
AABB aabb;
|
||
|
int drive_total=0;
|
||
|
for(int i=0;i<wheels.size();i++) {
|
||
|
CarWheel *w=wheels[i];
|
||
|
if (i==0) {
|
||
|
aabb.pos=w->local_xform.origin;
|
||
|
} else {
|
||
|
aabb.expand_to(w->local_xform.origin);
|
||
|
}
|
||
|
if (w->type_drive)
|
||
|
drive_total++;
|
||
|
|
||
|
}
|
||
|
// control inputs
|
||
|
float deltaAccelerate = dt * 4.0f;
|
||
|
|
||
|
float dAccelerate = target_accelerate - accelerate;
|
||
|
dAccelerate = CLAMP(dAccelerate, -deltaAccelerate, deltaAccelerate);
|
||
|
accelerate += dAccelerate;
|
||
|
|
||
|
float deltaSteering = dt * steer_rate;
|
||
|
float dSteering = target_steering - steering;
|
||
|
dSteering = CLAMP(dSteering, -deltaSteering, deltaSteering);
|
||
|
steering += dSteering;
|
||
|
|
||
|
// apply these inputs
|
||
|
float maxTorque = drive_torque;
|
||
|
|
||
|
float torque_div = drive_total/2;
|
||
|
if (torque_div>0)
|
||
|
maxTorque/=torque_div;
|
||
|
|
||
|
|
||
|
float alpha = ABS(max_steer_angle * steering);
|
||
|
float angleSgn = steering > 0.0f ? 1.0f : -1.0f;
|
||
|
|
||
|
int wheels_on_floor=0;
|
||
|
|
||
|
for(int i=0;i<wheels.size();i++) {
|
||
|
|
||
|
CarWheel *w=wheels[i];
|
||
|
if (w->type_drive)
|
||
|
w->driveTorque+=maxTorque * accelerate;
|
||
|
w->locked = !w->type_steer && (hand_brake > 0.5f);
|
||
|
|
||
|
if (w->type_steer) {
|
||
|
//steering
|
||
|
|
||
|
bool inner = (steering > 0 && w->local_xform.origin.x > 0) || (steering < 0 && w->local_xform.origin.x < 0);
|
||
|
|
||
|
if (inner || alpha==0.0) {
|
||
|
|
||
|
w->steerAngle = (angleSgn * alpha);
|
||
|
} else {
|
||
|
float dx = aabb.size.z;
|
||
|
float dy = aabb.size.x;
|
||
|
|
||
|
float beta = Math::atan2(dy, dx + (dy / (float)Math::tan(Math::deg2rad(alpha))));
|
||
|
beta = Math::rad2deg(beta);
|
||
|
w->steerAngle = (angleSgn * beta);
|
||
|
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if (w->add_forces(state))
|
||
|
wheels_on_floor++;
|
||
|
w->update(dt);
|
||
|
|
||
|
|
||
|
}
|
||
|
|
||
|
print_line("onfloor: "+itos(wheels_on_floor));
|
||
|
|
||
|
|
||
|
set_ignore_transform_notification(true);
|
||
|
set_global_transform(state->get_transform());
|
||
|
linear_velocity=state->get_linear_velocity();
|
||
|
angular_velocity=state->get_angular_velocity();
|
||
|
//active=!state->is_sleeping();
|
||
|
//if (get_script_instance())
|
||
|
// get_script_instance()->call("_integrate_forces",state);
|
||
|
set_ignore_transform_notification(false);
|
||
|
|
||
|
|
||
|
}
|
||
|
|
||
|
void CarBody::set_mass(real_t p_mass) {
|
||
|
|
||
|
mass=p_mass;
|
||
|
PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_MASS,mass);
|
||
|
}
|
||
|
|
||
|
real_t CarBody::get_mass() const{
|
||
|
|
||
|
return mass;
|
||
|
}
|
||
|
|
||
|
|
||
|
void CarBody::set_friction(real_t p_friction) {
|
||
|
|
||
|
friction=p_friction;
|
||
|
PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_FRICTION,friction);
|
||
|
}
|
||
|
|
||
|
real_t CarBody::get_friction() const{
|
||
|
|
||
|
return friction;
|
||
|
}
|
||
|
|
||
|
|
||
|
void CarBody::_bind_methods() {
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("set_max_steer_angle","value"),&CarBody::set_max_steer_angle);
|
||
|
ObjectTypeDB::bind_method(_MD("set_steer_rate","rate"),&CarBody::set_steer_rate);
|
||
|
ObjectTypeDB::bind_method(_MD("set_drive_torque","value"),&CarBody::set_drive_torque);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("get_max_steer_angle"),&CarBody::get_max_steer_angle);
|
||
|
ObjectTypeDB::bind_method(_MD("get_steer_rate"),&CarBody::get_steer_rate);
|
||
|
ObjectTypeDB::bind_method(_MD("get_drive_torque"),&CarBody::get_drive_torque);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("set_target_steering","amount"),&CarBody::set_target_steering);
|
||
|
ObjectTypeDB::bind_method(_MD("set_target_accelerate","amount"),&CarBody::set_target_accelerate);
|
||
|
ObjectTypeDB::bind_method(_MD("set_hand_brake","amount"),&CarBody::set_hand_brake);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("get_target_steering"),&CarBody::get_target_steering);
|
||
|
ObjectTypeDB::bind_method(_MD("get_target_accelerate"),&CarBody::get_target_accelerate);
|
||
|
ObjectTypeDB::bind_method(_MD("get_hand_brake"),&CarBody::get_hand_brake);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("set_mass","mass"),&CarBody::set_mass);
|
||
|
ObjectTypeDB::bind_method(_MD("get_mass"),&CarBody::get_mass);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("set_friction","friction"),&CarBody::set_friction);
|
||
|
ObjectTypeDB::bind_method(_MD("get_friction"),&CarBody::get_friction);
|
||
|
|
||
|
ObjectTypeDB::bind_method(_MD("_direct_state_changed"),&CarBody::_direct_state_changed);
|
||
|
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/mass",PROPERTY_HINT_RANGE,"0.01,65536,0.01"),_SCS("set_mass"),_SCS("get_mass"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/friction",PROPERTY_HINT_RANGE,"0.01,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
|
||
|
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/max_steer_angle",PROPERTY_HINT_RANGE,"1,90,1"),_SCS("set_max_steer_angle"),_SCS("get_max_steer_angle"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/drive_torque",PROPERTY_HINT_RANGE,"0.01,64,0.01"),_SCS("set_drive_torque"),_SCS("get_drive_torque"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"config/steer_rate",PROPERTY_HINT_RANGE,"0.01,64,0.01"),_SCS("set_steer_rate"),_SCS("get_steer_rate"));
|
||
|
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"drive/target_steering",PROPERTY_HINT_RANGE,"-1,1,0.01"),_SCS("set_target_steering"),_SCS("get_target_steering"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"drive/target_accelerate",PROPERTY_HINT_RANGE,"-1,1,0.01"),_SCS("set_target_accelerate"),_SCS("get_target_accelerate"));
|
||
|
ADD_PROPERTY( PropertyInfo(Variant::REAL,"drive/hand_brake",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_hand_brake"),_SCS("get_hand_brake"));
|
||
|
|
||
|
}
|
||
|
|
||
|
CarBody::CarBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) {
|
||
|
|
||
|
forward_drive=true;
|
||
|
backward_drive=true;
|
||
|
max_steer_angle=30;
|
||
|
steer_rate=1;
|
||
|
drive_torque=520;
|
||
|
|
||
|
target_steering=0;
|
||
|
target_accelerate=0;
|
||
|
hand_brake=0;
|
||
|
|
||
|
steering=0;
|
||
|
accelerate=0;
|
||
|
|
||
|
mass=1;
|
||
|
friction=1;
|
||
|
|
||
|
ccd=false;
|
||
|
// can_sleep=true;
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
exclude.insert(get_rid());
|
||
|
PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed");
|
||
|
|
||
|
|
||
|
}
|