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;
}
