diff options
Diffstat (limited to 'servers/physics/body_sw.cpp')
| -rw-r--r-- | servers/physics/body_sw.cpp | 107 |
1 files changed, 72 insertions, 35 deletions
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index 52edc0faa7..0fd754ba25 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -195,7 +195,7 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { _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); linear_velocity=Vector3(); angular_velocity=Vector3(); } break; @@ -203,14 +203,12 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) { _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; } @@ -235,13 +233,19 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian case PhysicsServer::BODY_STATE_TRANSFORM: { - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) { + if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { + new_transform=p_variant; + //wakeup_neighbours(); + set_active(true); + + } 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 _set_transform(t); _set_inv_transform(get_transform().inverse()); @@ -353,7 +357,7 @@ void BodySW::_compute_area_gravity(const AreaSW *p_area) { void BodySW::integrate_forces(real_t p_step) { - if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) + if (mode==PhysicsServer::BODY_MODE_STATIC) return; AreaSW *current_area = get_space()->get_default_area(); @@ -374,28 +378,56 @@ void BodySW::integrate_forces(real_t p_step) { _compute_area_gravity(current_area); density=current_area->get_density(); - if (!omit_force_integration) { - //overriden by direct state query + Vector3 motion; + bool do_motion=false; - Vector3 force=gravity*mass; - force+=applied_force; - Vector3 torque=applied_torque; + if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { - real_t damp = 1.0 - p_step * density; + //compute motion, angular and etc. velocities from prev transform + linear_velocity = (new_transform.origin - get_transform().origin)/p_step; - if (damp<0) // reached zero in the given time - damp=0; + //compute a FAKE angular velocity, not so easy + Matrix3 rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized(); + Vector3 axis; + float angle; + + rot.get_axis_and_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) { + //overriden by direct state query - real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio(); + Vector3 force=gravity*mass; + force+=applied_force; + Vector3 torque=applied_torque; - if (angular_damp<0) // reached zero in the given time - angular_damp=0; + real_t damp = 1.0 - p_step * density; - linear_velocity*=damp; - angular_velocity*=angular_damp; + 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; + } + + if (continuous_cd) { + motion=linear_velocity*p_step; + do_motion=true; + } - linear_velocity+=_inv_mass * force * p_step; - angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step; } applied_force=Vector3(); @@ -406,8 +438,11 @@ void BodySW::integrate_forces(real_t 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); + + if (do_motion) {//shapes temporarily extend for raycast + _update_shapes_with_motion(motion); + } + current_area=NULL; // clear the area, so it is set in the next frame contact_count=0; @@ -419,9 +454,16 @@ 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); + if (mode==PhysicsServer::BODY_MODE_KINEMATIC) { - if (fi_callback) - get_space()->body_add_to_state_query_list(&direct_state_query_list); + + _set_transform(new_transform,false); + _set_inv_transform(new_transform.affine_inverse()); ; + if (linear_velocity==Vector3() && angular_velocity==Vector3()) + set_active(false); //stopped moving, deactivate + return; } @@ -475,14 +517,13 @@ void BodySW::integrate_velocities(real_t p_step) { _update_inertia_tensor(); - if (fi_callback) { - - get_space()->body_add_to_state_query_list(&direct_state_query_list); - } + //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(); @@ -514,6 +555,7 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { } +*/ void BodySW::wakeup_neighbours() { @@ -562,12 +604,7 @@ void BodySW::call_queries() { } - if (simulated_motion) { - // linear_velocity=Vector3(); - // angular_velocity=0; - simulated_motion=false; - } } @@ -634,7 +671,7 @@ BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_upda _set_static(false); density=0; contact_count=0; - simulated_motion=false; + still_time=0; continuous_cd=false; can_sleep=false; |