void Body2DSW::integrate_forces(real_t p_step) { if (mode==Physics2DServer::BODY_MODE_STATIC) return; Area2DSW *def_area = get_space()->get_default_area(); // Area2DSW *damp_area = def_area; ERR_FAIL_COND(!def_area); int ac = areas.size(); bool stopped = false; gravity = Vector2(0,0); area_angular_damp = 0; area_linear_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--) { Physics2DServer::AreaSpaceOverrideMode mode=aa[i].area->get_space_override_mode(); switch (mode) { case Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE: case Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { _compute_area_gravity_and_dampenings(aa[i].area); stopped = mode==Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; } break; case Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE: case Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { gravity = Vector2(0,0); area_angular_damp = 0; area_linear_damp = 0; _compute_area_gravity_and_dampenings(aa[i].area); stopped = mode==Physics2DServer::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 Body2D 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(); Vector2 motion; bool do_motion=false; if (mode==Physics2DServer::BODY_MODE_KINEMATIC) { //compute motion, angular and etc. velocities from prev transform linear_velocity = (new_transform.elements[2] - get_transform().elements[2])/p_step; real_t rot = new_transform.affine_inverse().basis_xform(get_transform().elements[1]).angle(); angular_velocity = rot / p_step; motion = new_transform.elements[2] - get_transform().elements[2]; do_motion=true; //for(int i=0;i<get_shape_count();i++) { // set_shape_kinematic_advance(i,Vector2()); // set_shape_kinematic_retreat(i,0); //} } else { if (!omit_force_integration) { //overriden by direct state query Vector2 force=gravity*mass; force+=applied_force; real_t 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 * torque * p_step; } if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED) { motion = new_transform.get_origin() - get_transform().get_origin(); //linear_velocity*p_step; do_motion=true; } } //motion=linear_velocity*p_step; biased_angular_velocity=0; biased_linear_velocity=Vector2(); if (do_motion) {//shapes temporarily extend for raycast _update_shapes_with_motion(motion); } // damp_area=NULL; // clear the area, so it is set in the next frame def_area=NULL; // clear the area, so it is set in the next frame contact_count=0; }
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 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 && !first_integration) { //overriden 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; }