void Body2DSW::integrate_velocities(real_t p_step) {
if (mode==Physics2DServer::BODY_MODE_STATIC)
return;
if (fi_callback)
get_space()->body_add_to_state_query_list(&direct_state_query_list);
if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
_set_transform(new_transform,false);
_set_inv_transform(new_transform.affine_inverse());
if (contacts.size()==0 && linear_velocity==Vector2() && angular_velocity==0)
set_active(false); //stopped moving, deactivate
return;
}
real_t total_angular_velocity = angular_velocity+biased_angular_velocity;
Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity;
real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step;
Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
_set_transform(Matrix32(angle,pos),continuous_cd_mode==Physics2DServer::CCD_MODE_DISABLED);
_set_inv_transform(get_transform().inverse());
if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED)
new_transform=get_transform();
//_update_inertia_tensor();
}
void Area2DSW::set_transform(const Matrix32& p_transform) {
if (!moved_list.in_list() && get_space())
get_space()->area_add_to_moved_list(&moved_list);
_set_transform(p_transform);
_set_inv_transform(p_transform.affine_inverse());
}
void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) {
switch(p_state) {
case Physics2DServer::BODY_STATE_TRANSFORM: {
if (mode==Physics2DServer::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==Physics2DServer::BODY_MODE_STATIC) {
_set_transform(p_variant);
_set_inv_transform(get_transform().affine_inverse());
wakeup_neighbours();
} else {
Matrix32 t = p_variant;
t.orthonormalize();
new_transform=get_transform(); //used as old to compute motion
if (t==new_transform)
break;
_set_transform(t);
_set_inv_transform(get_transform().inverse());
}
wakeup();
} break;
case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
//if (mode==Physics2DServer::BODY_MODE_STATIC)
// break;
linear_velocity=p_variant;
wakeup();
} break;
case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
//if (mode!=Physics2DServer::BODY_MODE_RIGID)
// break;
angular_velocity=p_variant;
wakeup();
} break;
case Physics2DServer::BODY_STATE_SLEEPING: {
//?
if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
break;
bool do_sleep=p_variant;
if (do_sleep) {
linear_velocity=Vector2();
//biased_linear_velocity=Vector3();
angular_velocity=0;
//biased_angular_velocity=Vector3();
set_active(false);
} else {
if (mode!=Physics2DServer::BODY_MODE_STATIC)
set_active(true);
}
} break;
case Physics2DServer::BODY_STATE_CAN_SLEEP: {
can_sleep=p_variant;
if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep)
set_active(true);
} break;
}
}
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) {
_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;
}
//apply axis lock
if (axis_lock!=PhysicsServer::BODY_AXIS_LOCK_DISABLED) {
int axis=axis_lock-1;
for(int i=0;i<3;i++) {
if (i==axis) {
linear_velocity[i]=0;
biased_linear_velocity[i]=0;
} else {
angular_velocity[i]=0;
biased_angular_velocity[i]=0;
}
}
}
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;
/*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_inertia_tensor();
//if (fi_callback) {
// get_space()->body_add_to_state_query_list(&direct_state_query_list);
//
}
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);
*/
}
