void SoftBody::set_collision_layer_bit(int p_bit, bool p_value) {
uint32_t layer = get_collision_layer();
if (p_value)
layer |= 1 << p_bit;
else
layer &= ~(1 << p_bit);
set_collision_layer(layer);
}
void PhysicsBody::set_collision_layer_bit(int p_bit, bool p_value) {
uint32_t mask = get_collision_layer();
if (p_value)
mask |= 1 << p_bit;
else
mask &= ~(1 << p_bit);
set_collision_layer(mask);
}
bool SoftBody::get_collision_layer_bit(int p_bit) const {
return get_collision_layer() & (1 << p_bit);
}
bool Area::get_collision_layer_bit(int p_bit) const {
return get_collision_layer() & (1 << p_bit);
}
Vector3 KinematicBody::move(const Vector3 &p_motion) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
//what it does is simpler than using physics
//this took about a week to get right..
//but is it right? who knows at this point..
colliding = false;
ERR_FAIL_COND_V(!is_inside_tree(), Vector3());
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
ERR_FAIL_COND_V(!dss, Vector3());
const int max_shapes = 32;
Vector3 sr[max_shapes * 2];
int res_shapes;
Set<RID> exclude;
exclude.insert(get_rid());
//recover first
int recover_attempts = 4;
bool collided = false;
uint32_t mask = 0;
if (collide_static)
mask |= PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask |= PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask |= PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask |= PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
//print_line("motion: "+p_motion+" margin: "+rtos(margin));
//print_line("margin: "+rtos(margin));
float m = margin;
//m=0.001;
do {
//motion recover
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_set_as_trigger(i))
continue;
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), m, sr, max_shapes, res_shapes, exclude, get_collision_layer(), mask)) {
collided = true;
}
}
if (!collided)
break;
//print_line("have to recover");
Vector3 recover_motion;
bool all_outside = true;
for (int j = 0; j < 8; j++) {
for (int i = 0; i < res_shapes; i++) {
Vector3 a = sr[i * 2 + 0];
Vector3 b = sr[i * 2 + 1];
//print_line(String()+a+" -> "+b);
#if 0
float d = a.distance_to(b);
/*
if (d<margin)
continue;
*/
recover_motion+=(b-a)*0.2;
#else
float dist = a.distance_to(b);
if (dist > CMP_EPSILON) {
Vector3 norm = (b - a).normalized();
if (dist > margin * 0.5)
all_outside = false;
float adv = norm.dot(recover_motion);
//print_line(itos(i)+" dist: "+rtos(dist)+" adv: "+rtos(adv));
recover_motion += norm * MAX(dist - adv, 0) * 0.4;
}
#endif
}
}
if (recover_motion == Vector3()) {
collided = false;
break;
}
//print_line("**** RECOVER: "+recover_motion);
Transform gt = get_global_transform();
gt.origin += recover_motion;
set_global_transform(gt);
recover_attempts--;
if (all_outside)
break;
} while (recover_attempts);
//move second
float safe = 1.0;
float unsafe = 1.0;
int best_shape = -1;
PhysicsDirectSpaceState::ShapeRestInfo rest;
//print_line("pos: "+get_global_transform().origin);
//print_line("motion: "+p_motion);
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_set_as_trigger(i))
continue;
float lsafe, lunsafe;
PhysicsDirectSpaceState::ShapeRestInfo lrest;
bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0, lsafe, lunsafe, exclude, get_collision_layer(), mask, &lrest);
//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
if (!valid) {
safe = 0;
unsafe = 0;
best_shape = i; //sadly it's the best
//print_line("initial stuck");
break;
}
if (lsafe == 1.0) {
//print_line("initial free");
continue;
}
if (lsafe < safe) {
//print_line("initial at "+rtos(lsafe));
safe = lsafe;
safe = MAX(0, lsafe - 0.01);
unsafe = lunsafe;
best_shape = i;
rest = lrest;
}
}
//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
if (safe >= 1) {
//not collided
colliding = false;
} else {
colliding = true;
if (true || (safe == 0 && unsafe == 0)) { //use it always because it's more precise than GJK
//no advance, use rest info from collision
Transform ugt = get_global_transform();
ugt.origin += p_motion * unsafe;
PhysicsDirectSpaceState::ShapeRestInfo rest_info;
bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt * get_shape_transform(best_shape), m, &rest, exclude, get_collision_layer(), mask);
if (!c2) {
//should not happen, but floating point precision is so weird..
colliding = false;
}
//print_line("Rest Travel: "+rest.normal);
}
if (colliding) {
collision = rest.point;
normal = rest.normal;
collider = rest.collider_id;
collider_vel = rest.linear_velocity;
collider_shape = rest.shape;
}
}
Vector3 motion = p_motion * safe;
/*
if (colliding)
motion+=normal*0.001;
*/
Transform gt = get_global_transform();
gt.origin += motion;
set_global_transform(gt);
return p_motion - motion;
}
uint32_t PhysicsBody::_get_layers() const {
return get_collision_layer();
}
bool KinematicBody::can_teleport_to(const Vector3 &p_position) {
ERR_FAIL_COND_V(!is_inside_tree(), false);
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
ERR_FAIL_COND_V(!dss, false);
uint32_t mask = 0;
if (collide_static)
mask |= PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask |= PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask |= PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask |= PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
Transform xform = get_global_transform();
xform.origin = p_position;
Set<RID> exclude;
exclude.insert(get_rid());
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_set_as_trigger(i))
continue;
bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i), 0, NULL, 1, exclude, get_collision_layer(), mask);
if (col)
return false;
}
return true;
}
