diff options
Diffstat (limited to 'servers/physics/body_sw.cpp')
| -rw-r--r-- | servers/physics/body_sw.cpp | 814 |
1 files changed, 0 insertions, 814 deletions
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp deleted file mode 100644 index 8819941f04..0000000000 --- a/servers/physics/body_sw.cpp +++ /dev/null @@ -1,814 +0,0 @@ -/*************************************************************************/ -/* body_sw.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 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 "body_sw.h" -#include "area_sw.h" -#include "space_sw.h" - -void BodySW::_update_inertia() { - - if (get_space() && !inertia_update_list.in_list()) - get_space()->body_add_to_inertia_update_list(&inertia_update_list); -} - -void BodySW::_update_transform_dependant() { - - center_of_mass = get_transform().basis.xform(center_of_mass_local); - principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; - - // update inertia tensor - Basis tb = principal_inertia_axes; - Basis tbt = tb.transposed(); - Basis diag; - diag.scale(_inv_inertia); - _inv_inertia_tensor = tb * diag * tbt; -} - -void BodySW::update_inertias() { - - //update shapes and motions - - switch (mode) { - - case PhysicsServer::BODY_MODE_RIGID: { - - //update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) - real_t total_area = 0; - - for (int i = 0; i < get_shape_count(); i++) { - - total_area += get_shape_area(i); - } - - // We have to recompute the center of mass - center_of_mass_local.zero(); - - for (int i = 0; i < get_shape_count(); i++) { - real_t area = get_shape_area(i); - - real_t mass = area * this->mass / total_area; - - // NOTE: we assume that the shape origin is also its center of mass - center_of_mass_local += mass * get_shape_transform(i).origin; - } - - center_of_mass_local /= mass; - - // Recompute the inertia tensor - Basis inertia_tensor; - inertia_tensor.set_zero(); - - for (int i = 0; i < get_shape_count(); i++) { - - if (is_shape_disabled(i)) { - continue; - } - - const ShapeSW *shape = get_shape(i); - - real_t area = get_shape_area(i); - - real_t mass = area * this->mass / total_area; - - Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix(); - Transform shape_transform = get_shape_transform(i); - Basis shape_basis = shape_transform.basis.orthonormalized(); - - // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! - shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); - - Vector3 shape_origin = shape_transform.origin - center_of_mass_local; - inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; - } - - // Compute the principal axes of inertia - principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); - _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); - - if (mass) - _inv_mass = 1.0 / mass; - else - _inv_mass = 0; - - } break; - - case PhysicsServer::BODY_MODE_KINEMATIC: - case PhysicsServer::BODY_MODE_STATIC: { - - _inv_inertia_tensor.set_zero(); - _inv_mass = 0; - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_inertia_tensor.set_zero(); - _inv_mass = 1.0 / mass; - - } break; - } - - //_update_shapes(); - - _update_transform_dependant(); -} - -void BodySW::set_active(bool p_active) { - - if (active == p_active) - return; - - active = p_active; - if (!p_active) { - if (get_space()) - get_space()->body_remove_from_active_list(&active_list); - } else { - if (mode == PhysicsServer::BODY_MODE_STATIC) - return; //static bodies can't become active - if (get_space()) - get_space()->body_add_to_active_list(&active_list); - - //still_time=0; - } - /* - if (!space) - return; - - for(int i=0;i<get_shape_count();i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ -} - -void BodySW::set_param(PhysicsServer::BodyParameter p_param, real_t p_value) { - - switch (p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - - bounce = p_value; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - - friction = p_value; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - ERR_FAIL_COND(p_value <= 0); - mass = p_value; - _update_inertia(); - - } break; - case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: { - gravity_scale = p_value; - } break; - case PhysicsServer::BODY_PARAM_LINEAR_DAMP: { - - linear_damp = p_value; - } break; - case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: { - - angular_damp = p_value; - } break; - default: { - } - } -} - -real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const { - - switch (p_param) { - case PhysicsServer::BODY_PARAM_BOUNCE: { - - return bounce; - } break; - case PhysicsServer::BODY_PARAM_FRICTION: { - - return friction; - } break; - case PhysicsServer::BODY_PARAM_MASS: { - return mass; - } break; - case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: { - return gravity_scale; - } break; - case PhysicsServer::BODY_PARAM_LINEAR_DAMP: { - - return linear_damp; - } break; - case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: { - - return angular_damp; - } break; - - default: { - } - } - - return 0; -} - -void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { - - PhysicsServer::BodyMode prev = mode; - mode = p_mode; - - switch (p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case PhysicsServer::BODY_MODE_STATIC: - case PhysicsServer::BODY_MODE_KINEMATIC: { - - _set_inv_transform(get_transform().affine_inverse()); - _inv_mass = 0; - _set_static(p_mode == PhysicsServer::BODY_MODE_STATIC); - //set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC); - set_active(p_mode == PhysicsServer::BODY_MODE_KINEMATIC && contacts.size()); - linear_velocity = Vector3(); - angular_velocity = Vector3(); - if (mode == PhysicsServer::BODY_MODE_KINEMATIC && prev != mode) { - first_time_kinematic = true; - } - - } break; - case PhysicsServer::BODY_MODE_RIGID: { - - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _set_static(false); - set_active(true); - - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_mass = mass > 0 ? (1.0 / mass) : 0; - _set_static(false); - set_active(true); - angular_velocity = Vector3(); - } break; - } - - _update_inertia(); - /* - if (get_space()) - _update_queries(); - */ -} -PhysicsServer::BodyMode BodySW::get_mode() const { - - return mode; -} - -void BodySW::_shapes_changed() { - - _update_inertia(); -} - -void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) { - - switch (p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - - if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { - new_transform = p_variant; - //wakeup_neighbours(); - set_active(true); - if (first_time_kinematic) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - first_time_kinematic = false; - } - - } else if (mode == PhysicsServer::BODY_MODE_STATIC) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform t = p_variant; - t.orthonormalize(); - new_transform = get_transform(); //used as old to compute motion - if (new_transform == t) - break; - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - } - wakeup(); - - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - - /* - if (mode==PhysicsServer::BODY_MODE_STATIC) - break; - */ - linear_velocity = p_variant; - wakeup(); - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - /* - if (mode!=PhysicsServer::BODY_MODE_RIGID) - break; - */ - angular_velocity = p_variant; - wakeup(); - - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - //? - if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) - break; - bool do_sleep = p_variant; - if (do_sleep) { - linear_velocity = Vector3(); - //biased_linear_velocity=Vector3(); - angular_velocity = Vector3(); - //biased_angular_velocity=Vector3(); - set_active(false); - } else { - set_active(true); - } - } break; - case PhysicsServer::BODY_STATE_CAN_SLEEP: { - can_sleep = p_variant; - if (mode == PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep) - set_active(true); - - } break; - } -} -Variant BodySW::get_state(PhysicsServer::BodyState p_state) const { - - switch (p_state) { - case PhysicsServer::BODY_STATE_TRANSFORM: { - return get_transform(); - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - return linear_velocity; - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - return angular_velocity; - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - return !is_active(); - } break; - case PhysicsServer::BODY_STATE_CAN_SLEEP: { - return can_sleep; - } break; - } - - return Variant(); -} - -void BodySW::set_space(SpaceSW *p_space) { - - if (get_space()) { - - if (inertia_update_list.in_list()) - get_space()->body_remove_from_inertia_update_list(&inertia_update_list); - if (active_list.in_list()) - get_space()->body_remove_from_active_list(&active_list); - if (direct_state_query_list.in_list()) - get_space()->body_remove_from_state_query_list(&direct_state_query_list); - } - - _set_space(p_space); - - if (get_space()) { - - _update_inertia(); - if (active) - get_space()->body_add_to_active_list(&active_list); - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ - } - - first_integration = true; -} - -void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) { - - if (p_area->is_gravity_point()) { - if (p_area->get_gravity_distance_scale() > 0) { - Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2)); - } else { - gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); - } - } else { - gravity += p_area->get_gravity_vector() * p_area->get_gravity(); - } - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - -void BodySW::set_axis_lock(PhysicsServer::BodyAxis p_axis, bool lock) { - if (lock) { - locked_axis |= p_axis; - } else { - locked_axis &= ~p_axis; - } -} - -bool BodySW::is_axis_locked(PhysicsServer::BodyAxis p_axis) const { - return locked_axis & p_axis; -} - -void BodySW::integrate_forces(real_t p_step) { - - if (mode == PhysicsServer::BODY_MODE_STATIC) - return; - - AreaSW *def_area = get_space()->get_default_area(); - // AreaSW *damp_area = def_area; - - ERR_FAIL_COND(!def_area); - - int ac = areas.size(); - bool stopped = false; - gravity = Vector3(0, 0, 0); - area_linear_damp = 0; - area_angular_damp = 0; - if (ac) { - areas.sort(); - const AreaCMP *aa = &areas[0]; - // damp_area = aa[ac-1].area; - for (int i = ac - 1; i >= 0 && !stopped; i--) { - PhysicsServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0, 0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_dampenings(aa[i].area); - stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } - } - } - - if (!stopped) { - _compute_area_gravity_and_dampenings(def_area); - } - - gravity *= gravity_scale; - - // If less than 0, override dampenings with that of the Body - if (angular_damp >= 0) - area_angular_damp = angular_damp; - /* - else - area_angular_damp=damp_area->get_angular_damp(); - */ - - if (linear_damp >= 0) - area_linear_damp = linear_damp; - /* - else - area_linear_damp=damp_area->get_linear_damp(); - */ - - Vector3 motion; - bool do_motion = false; - - if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { - - //compute motion, angular and etc. velocities from prev transform - linear_velocity = (new_transform.origin - get_transform().origin) / p_step; - - //compute a FAKE angular velocity, not so easy - Basis rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized(); - Vector3 axis; - real_t angle; - - rot.get_axis_angle(axis, angle); - axis.normalize(); - angular_velocity = axis.normalized() * (angle / p_step); - - motion = new_transform.origin - get_transform().origin; - do_motion = true; - - } else { - if (!omit_force_integration && !first_integration) { - //overridden by direct state query - - Vector3 force = gravity * mass; - force += applied_force; - Vector3 torque = applied_torque; - - real_t damp = 1.0 - p_step * area_linear_damp; - - if (damp < 0) // reached zero in the given time - damp = 0; - - real_t angular_damp = 1.0 - p_step * area_angular_damp; - - if (angular_damp < 0) // reached zero in the given time - angular_damp = 0; - - linear_velocity *= damp; - angular_velocity *= angular_damp; - - linear_velocity += _inv_mass * force * p_step; - angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; - } - - if (continuous_cd) { - motion = linear_velocity * p_step; - do_motion = true; - } - } - - applied_force = Vector3(); - applied_torque = Vector3(); - first_integration = false; - - //motion=linear_velocity*p_step; - - biased_angular_velocity = Vector3(); - biased_linear_velocity = Vector3(); - - if (do_motion) { //shapes temporarily extend for raycast - _update_shapes_with_motion(motion); - } - - def_area = NULL; // clear the area, so it is set in the next frame - contact_count = 0; -} - -void BodySW::integrate_velocities(real_t p_step) { - - if (mode == PhysicsServer::BODY_MODE_STATIC) - return; - - if (fi_callback) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - - //apply axis lock linear - for (int i = 0; i < 3; i++) { - if (is_axis_locked((PhysicsServer::BodyAxis)(1 << i))) { - linear_velocity[i] = 0; - biased_linear_velocity[i] = 0; - new_transform.origin[i] = get_transform().origin[i]; - } - } - //apply axis lock angular - for (int i = 0; i < 3; i++) { - if (is_axis_locked((PhysicsServer::BodyAxis)(1 << (i + 3)))) { - angular_velocity[i] = 0; - biased_angular_velocity[i] = 0; - } - } - - if (mode == PhysicsServer::BODY_MODE_KINEMATIC) { - - _set_transform(new_transform, false); - _set_inv_transform(new_transform.affine_inverse()); - if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) - set_active(false); //stopped moving, deactivate - - return; - } - - Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; - - real_t ang_vel = total_angular_velocity.length(); - Transform transform = get_transform(); - - if (ang_vel != 0.0) { - Vector3 ang_vel_axis = total_angular_velocity / ang_vel; - Basis rot(ang_vel_axis, ang_vel * p_step); - Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); - transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); - transform.basis = rot * transform.basis; - transform.orthonormalize(); - } - - Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; - /*for(int i=0;i<3;i++) { - if (axis_lock&(1<<i)) { - transform.origin[i]=0.0; - } - }*/ - - transform.origin += total_linear_velocity * p_step; - - _set_transform(transform); - _set_inv_transform(get_transform().inverse()); - - _update_transform_dependant(); - - /* - if (fi_callback) { - get_space()->body_add_to_state_query_list(&direct_state_query_list); - */ -} - -/* -void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { - - Transform inv_xform = p_xform.affine_inverse(); - if (!get_space()) { - _set_transform(p_xform); - _set_inv_transform(inv_xform); - - return; - } - - //compute a FAKE linear velocity - this is easy - - linear_velocity=(p_xform.origin - get_transform().origin)/p_step; - - //compute a FAKE angular velocity, not so easy - Basis rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized(); - Vector3 axis; - real_t angle; - - rot.get_axis_angle(axis,angle); - axis.normalize(); - angular_velocity=axis.normalized() * (angle/p_step); - linear_velocity = (p_xform.origin - get_transform().origin)/p_step; - - if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query)) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - simulated_motion=true; - _set_transform(p_xform); - - -} -*/ - -void BodySW::wakeup_neighbours() { - - for (Map<ConstraintSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { - - const ConstraintSW *c = E->key(); - BodySW **n = c->get_body_ptr(); - int bc = c->get_body_count(); - - for (int i = 0; i < bc; i++) { - - if (i == E->get()) - continue; - BodySW *b = n[i]; - if (b->mode != PhysicsServer::BODY_MODE_RIGID) - continue; - - if (!b->is_active()) - b->set_active(true); - } - } -} - -void BodySW::call_queries() { - - if (fi_callback) { - - PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton; - dbs->body = this; - - Variant v = dbs; - - Object *obj = ObjectDB::get_instance(fi_callback->id); - if (!obj) { - - set_force_integration_callback(ObjectID(), StringName()); - } else { - const Variant *vp[2] = { &v, &fi_callback->udata }; - - Callable::CallError ce; - int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2; - obj->call(fi_callback->method, vp, argc, ce); - } - } -} - -bool BodySW::sleep_test(real_t p_step) { - - if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC) - return true; // - else if (mode == PhysicsServer::BODY_MODE_CHARACTER) - return !active; // characters don't sleep unless asked to sleep - else if (!can_sleep) - return false; - - if (Math::abs(angular_velocity.length()) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { - - still_time += p_step; - - return still_time > get_space()->get_body_time_to_sleep(); - } else { - - still_time = 0; //maybe this should be set to 0 on set_active? - return false; - } -} - -void BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { - - if (fi_callback) { - - memdelete(fi_callback); - fi_callback = NULL; - } - - if (p_id.is_valid()) { - - fi_callback = memnew(ForceIntegrationCallback); - fi_callback->id = p_id; - fi_callback->method = p_method; - fi_callback->udata = p_udata; - } -} - -void BodySW::set_kinematic_margin(real_t p_margin) { - kinematic_safe_margin = p_margin; -} - -BodySW::BodySW() : - CollisionObjectSW(TYPE_BODY), - locked_axis(0), - active_list(this), - inertia_update_list(this), - direct_state_query_list(this) { - - mode = PhysicsServer::BODY_MODE_RIGID; - active = true; - - mass = 1; - kinematic_safe_margin = 0.01; - //_inv_inertia=Transform(); - _inv_mass = 1; - bounce = 0; - friction = 1; - omit_force_integration = false; - //applied_torque=0; - island_step = 0; - island_next = NULL; - island_list_next = NULL; - first_time_kinematic = false; - first_integration = false; - _set_static(false); - - contact_count = 0; - gravity_scale = 1.0; - linear_damp = -1; - angular_damp = -1; - area_angular_damp = 0; - area_linear_damp = 0; - - still_time = 0; - continuous_cd = false; - can_sleep = true; - fi_callback = NULL; -} - -BodySW::~BodySW() { - - if (fi_callback) - memdelete(fi_callback); -} - -PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton = NULL; - -PhysicsDirectSpaceState *PhysicsDirectBodyStateSW::get_space_state() { - - return body->get_space()->get_direct_state(); -} |