void RigidBodyBullet::reload_axis_lock() {
btBody->setLinearFactor(btVector3(!is_axis_locked(PhysicsServer::BODY_AXIS_LINEAR_X), !is_axis_locked(PhysicsServer::BODY_AXIS_LINEAR_Y), !is_axis_locked(PhysicsServer::BODY_AXIS_LINEAR_Z)));
if (PhysicsServer::BODY_MODE_CHARACTER == mode) {
/// When character angular is always locked
btBody->setAngularFactor(btVector3(0., 0., 0.));
} else {
btBody->setAngularFactor(btVector3(!is_axis_locked(PhysicsServer::BODY_AXIS_ANGULAR_X), !is_axis_locked(PhysicsServer::BODY_AXIS_ANGULAR_Y), !is_axis_locked(PhysicsServer::BODY_AXIS_ANGULAR_Z)));
}
}
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);
*/
}
