From d7d65fa2f2b51d03f7bdfcbceedca99188ce979c Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Wed, 19 Feb 2014 11:57:14 -0300 Subject: -improved physics ccd -html5 exporter works again -disable repeat on image loader by default -can change shape offset en tileset, texture offset was broken --- servers/physics/body_sw.cpp | 1206 +++++++++++++++++++++---------------------- 1 file changed, 603 insertions(+), 603 deletions(-) (limited to 'servers/physics/body_sw.cpp') diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index b926a93773..f0f72b471c 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -26,606 +26,606 @@ /* 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 "space_sw.h" -#include "area_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_inertia_tensor() { - - Matrix3 tb = get_transform().basis; - tb.scale(_inv_inertia); - _inv_inertia_tensor = tb * get_transform().basis.transposed(); - -} - -void BodySW::update_inertias() { - - //update shapes and motions - - switch(mode) { - - case PhysicsServer::BODY_MODE_RIGID: { - - //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) - float total_area=0; - - for (int i=0;imass / total_area; - - _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin(); - - } - - if (_inertia!=Vector3()) - _inv_inertia=_inertia.inverse(); - else - _inv_inertia=Vector3(); - - if (mass) - _inv_mass=1.0/mass; - else - _inv_mass=0; - - } break; - - case PhysicsServer::BODY_MODE_STATIC_ACTIVE: - case PhysicsServer::BODY_MODE_STATIC: { - - _inv_inertia=Vector3(); - _inv_mass=0; - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_inertia=Vector3(); - _inv_mass=1.0/mass; - - } break; - } - _update_inertia_tensor(); - - //_update_shapes(); - -} - - - -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;i0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ -} - - - -void BodySW::set_param(PhysicsServer::BodyParameter p_param, float 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; - default:{} - } -} - -float 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; - default:{} - } - - return 0; -} - -void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { - - mode=p_mode; - - switch(p_mode) { - //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! - case PhysicsServer::BODY_MODE_STATIC: - case PhysicsServer::BODY_MODE_STATIC_ACTIVE: { - - _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_STATIC_ACTIVE); - linear_velocity=Vector3(); - angular_velocity=Vector3(); - } break; - case PhysicsServer::BODY_MODE_RIGID: { - - _inv_mass=mass>0?(1.0/mass):0; - _set_static(false); - simulated_motion=false; //jic - - } break; - case PhysicsServer::BODY_MODE_CHARACTER: { - - _inv_mass=mass>0?(1.0/mass):0; - _set_static(false); - simulated_motion=false; //jic - } 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_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) { - _set_transform(p_variant); - _set_inv_transform(get_transform().affine_inverse()); - wakeup_neighbours(); - } else { - Transform t = p_variant; - t.orthonormalize(); - _set_transform(t); - _set_inv_transform(get_transform().inverse()); - - } - - } break; - case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { - - //if (mode==PhysicsServer::BODY_MODE_STATIC) - // break; - linear_velocity=p_variant; - } break; - case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { - //if (mode!=PhysicsServer::BODY_MODE_RIGID) - // break; - angular_velocity=p_variant; - - } break; - case PhysicsServer::BODY_STATE_SLEEPING: { - //? - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) - 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 { - if (mode!=PhysicsServer::BODY_MODE_STATIC) - 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); - //} - - } - -} - -void BodySW::_compute_area_gravity(const AreaSW *p_area) { - - if (p_area->is_gravity_point()) { - - gravity = (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(); - } -} - -void BodySW::integrate_forces(real_t p_step) { - - - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) - return; - - AreaSW *current_area = get_space()->get_default_area(); - ERR_FAIL_COND(!current_area); - - int prio = current_area->get_priority(); - int ac = areas.size(); - if (ac) { - const AreaCMP *aa = &areas[0]; - for(int i=0;iget_priority() > prio) { - current_area=aa[i].area; - prio=current_area->get_priority(); - } - } - } - - _compute_area_gravity(current_area); - density=current_area->get_density(); - - if (!omit_force_integration) { - //overriden by direct state query - - Vector3 force=gravity*mass; - force+=applied_force; - Vector3 torque=applied_torque; - - real_t damp = 1.0 - p_step * density; - - if (damp<0) // reached zero in the given time - damp=0; - - real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); - - 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; - } - - applied_force=Vector3(); - applied_torque=Vector3(); - - //motion=linear_velocity*p_step; - - biased_angular_velocity=Vector3(); - biased_linear_velocity=Vector3(); - - if (continuous_cd) //shapes temporarily extend for raycast - _update_shapes_with_motion(linear_velocity*p_step); - - current_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 (mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) { - if (fi_callback) - get_space()->body_add_to_state_query_list(&direct_state_query_list); - return; - } - - Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity; - - - - float ang_vel = total_angular_velocity.length(); - Transform transform = get_transform(); - - - if (ang_vel!=0.0) { - Vector3 ang_vel_axis = total_angular_velocity / ang_vel; - Matrix3 rot( ang_vel_axis, -ang_vel*p_step ); - transform.basis = rot * transform.basis; - transform.orthonormalize(); - } - - Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity; - - - transform.origin+=total_linear_velocity * p_step; - - _set_transform(transform); - _set_inv_transform(get_transform().inverse()); - - _update_inertia_tensor(); - - 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 - Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized(); - Vector3 axis; - float angle; - - rot.get_axis_and_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::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;iget()) - 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(0,StringName()); - } else { - const Variant *vp[2]={&v,&fi_callback->udata}; - - Variant::CallError ce; - int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2; - obj->call(fi_callback->method,vp,argc,ce); - } - - - } - - if (simulated_motion) { - - // linear_velocity=Vector3(); - // angular_velocity=0; - simulated_motion=false; - } -} - - -bool BodySW::sleep_test(real_t p_step) { - - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) - 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_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { - - 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!=0) { - - fi_callback=memnew(ForceIntegrationCallback); - fi_callback->id=p_id; - fi_callback->method=p_method; - fi_callback->udata=p_udata; - } - -} - -BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { - - - mode=PhysicsServer::BODY_MODE_RIGID; - active=true; - - mass=1; -// _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; - _set_static(false); - density=0; - contact_count=0; - simulated_motion=false; - still_time=0; - continuous_cd=false; - can_sleep=false; - 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(); -} +#include "body_sw.h" +#include "space_sw.h" +#include "area_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_inertia_tensor() { + + Matrix3 tb = get_transform().basis; + tb.scale(_inv_inertia); + _inv_inertia_tensor = tb * get_transform().basis.transposed(); + +} + +void BodySW::update_inertias() { + + //update shapes and motions + + switch(mode) { + + case PhysicsServer::BODY_MODE_RIGID: { + + //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet) + float total_area=0; + + for (int i=0;imass / total_area; + + _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin(); + + } + + if (_inertia!=Vector3()) + _inv_inertia=_inertia.inverse(); + else + _inv_inertia=Vector3(); + + if (mass) + _inv_mass=1.0/mass; + else + _inv_mass=0; + + } break; + + case PhysicsServer::BODY_MODE_KINEMATIC: + case PhysicsServer::BODY_MODE_STATIC: { + + _inv_inertia=Vector3(); + _inv_mass=0; + } break; + case PhysicsServer::BODY_MODE_CHARACTER: { + + _inv_inertia=Vector3(); + _inv_mass=1.0/mass; + + } break; + } + _update_inertia_tensor(); + + //_update_shapes(); + +} + + + +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;i0) { + get_space()->get_broadphase()->set_active(s.bpid,active); + } + } +*/ +} + + + +void BodySW::set_param(PhysicsServer::BodyParameter p_param, float 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; + default:{} + } +} + +float 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; + default:{} + } + + return 0; +} + +void BodySW::set_mode(PhysicsServer::BodyMode p_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); + linear_velocity=Vector3(); + angular_velocity=Vector3(); + } break; + case PhysicsServer::BODY_MODE_RIGID: { + + _inv_mass=mass>0?(1.0/mass):0; + _set_static(false); + simulated_motion=false; //jic + + } break; + case PhysicsServer::BODY_MODE_CHARACTER: { + + _inv_mass=mass>0?(1.0/mass):0; + _set_static(false); + simulated_motion=false; //jic + } 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_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Transform t = p_variant; + t.orthonormalize(); + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + + } + + } break; + case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: { + + //if (mode==PhysicsServer::BODY_MODE_STATIC) + // break; + linear_velocity=p_variant; + } break; + case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: { + //if (mode!=PhysicsServer::BODY_MODE_RIGID) + // break; + angular_velocity=p_variant; + + } 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 { + if (mode!=PhysicsServer::BODY_MODE_STATIC) + 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); + //} + + } + +} + +void BodySW::_compute_area_gravity(const AreaSW *p_area) { + + if (p_area->is_gravity_point()) { + + gravity = (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(); + } +} + +void BodySW::integrate_forces(real_t p_step) { + + + if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + return; + + AreaSW *current_area = get_space()->get_default_area(); + ERR_FAIL_COND(!current_area); + + int prio = current_area->get_priority(); + int ac = areas.size(); + if (ac) { + const AreaCMP *aa = &areas[0]; + for(int i=0;iget_priority() > prio) { + current_area=aa[i].area; + prio=current_area->get_priority(); + } + } + } + + _compute_area_gravity(current_area); + density=current_area->get_density(); + + if (!omit_force_integration) { + //overriden by direct state query + + Vector3 force=gravity*mass; + force+=applied_force; + Vector3 torque=applied_torque; + + real_t damp = 1.0 - p_step * density; + + if (damp<0) // reached zero in the given time + damp=0; + + real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); + + 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; + } + + applied_force=Vector3(); + applied_torque=Vector3(); + + //motion=linear_velocity*p_step; + + biased_angular_velocity=Vector3(); + biased_linear_velocity=Vector3(); + + if (continuous_cd) //shapes temporarily extend for raycast + _update_shapes_with_motion(linear_velocity*p_step); + + current_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 (mode==PhysicsServer::BODY_MODE_KINEMATIC) { + if (fi_callback) + get_space()->body_add_to_state_query_list(&direct_state_query_list); + return; + } + + Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity; + + + + float ang_vel = total_angular_velocity.length(); + Transform transform = get_transform(); + + + if (ang_vel!=0.0) { + Vector3 ang_vel_axis = total_angular_velocity / ang_vel; + Matrix3 rot( ang_vel_axis, -ang_vel*p_step ); + transform.basis = rot * transform.basis; + transform.orthonormalize(); + } + + Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity; + + + transform.origin+=total_linear_velocity * p_step; + + _set_transform(transform); + _set_inv_transform(get_transform().inverse()); + + _update_inertia_tensor(); + + 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 + Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized(); + Vector3 axis; + float angle; + + rot.get_axis_and_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::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;iget()) + 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(0,StringName()); + } else { + const Variant *vp[2]={&v,&fi_callback->udata}; + + Variant::CallError ce; + int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2; + obj->call(fi_callback->method,vp,argc,ce); + } + + + } + + if (simulated_motion) { + + // linear_velocity=Vector3(); + // angular_velocity=0; + simulated_motion=false; + } +} + + +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_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) { + + 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!=0) { + + fi_callback=memnew(ForceIntegrationCallback); + fi_callback->id=p_id; + fi_callback->method=p_method; + fi_callback->udata=p_udata; + } + +} + +BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) { + + + mode=PhysicsServer::BODY_MODE_RIGID; + active=true; + + mass=1; +// _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; + _set_static(false); + density=0; + contact_count=0; + simulated_motion=false; + still_time=0; + continuous_cd=false; + can_sleep=false; + 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(); +} -- cgit v1.2.3