Simplified 2D Kinematicbody.. 3D will wait a bit.

This commit is contained in:
Juan Linietsky 2017-06-24 12:33:52 -03:00
parent cc2cd20870
commit bc509ac7bd
5 changed files with 208 additions and 322 deletions

View File

@ -160,9 +160,9 @@ void CollisionObject2D::shape_owner_set_transform(uint32_t p_owner, const Transf
sd.xform = p_transform;
for (int i = 0; i < sd.shapes.size(); i++) {
if (area) {
Physics2DServer::get_singleton()->area_set_shape_transform(rid, i, p_transform);
Physics2DServer::get_singleton()->area_set_shape_transform(rid, sd.shapes[i].index, p_transform);
} else {
Physics2DServer::get_singleton()->body_set_shape_transform(rid, i, p_transform);
Physics2DServer::get_singleton()->body_set_shape_transform(rid, sd.shapes[i].index, p_transform);
}
}
}

View File

@ -941,248 +941,105 @@ RigidBody2D::~RigidBody2D() {
//////////////////////////
Variant KinematicBody2D::_get_collider() const {
Dictionary KinematicBody2D::_move(const Vector2 &p_motion) {
ObjectID oid = get_collider();
if (oid == 0)
return Variant();
Object *obj = ObjectDB::get_instance(oid);
if (!obj)
return Variant();
Collision col;
if (move(p_motion, col)) {
Dictionary d;
d["position"] = col.collision;
d["normal"] = col.collision;
d["local_shape"] = col.local_shape;
d["travel"] = col.travel;
d["remainder"] = col.remainder;
d["collider_id"] = col.collider;
if (col.collider) {
d["collider"] = ObjectDB::get_instance(col.collider);
} else {
d["collider"] = Variant();
}
Reference *ref = obj->cast_to<Reference>();
if (ref) {
return Ref<Reference>(ref);
d["collider_shape_index"] = col.collider_shape;
d["collider_metadata"] = col.collider_metadata;
return d;
} else {
return Dictionary();
}
return obj;
}
void KinematicBody2D::revert_motion() {
Transform2D gt = get_global_transform();
gt.elements[2] -= travel;
travel = Vector2();
set_global_transform(gt);
}
Vector2 KinematicBody2D::get_travel() const {
return travel;
}
Vector2 KinematicBody2D::move(const Vector2 &p_motion) {
#if 1
bool KinematicBody2D::move(const Vector2 &p_motion, Collision &r_collision) {
Transform2D gt = get_global_transform();
Physics2DServer::MotionResult result;
colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, margin, &result);
bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, margin, &result);
collider_metadata = result.collider_metadata;
collider_shape = result.collider_shape;
collider_vel = result.collider_velocity;
collision = result.collision_point;
normal = result.collision_normal;
collider = result.collider_id;
if (colliding) {
r_collision.collider_metadata = result.collider_metadata;
r_collision.collider_shape = result.collider_shape;
r_collision.collider_vel = result.collider_velocity;
r_collision.collision = result.collision_point;
r_collision.normal = result.collision_normal;
r_collision.collider = result.collider_id;
r_collision.travel = result.motion;
r_collision.remainder = result.remainder;
r_collision.local_shape = result.collision_local_shape;
}
gt.elements[2] += result.motion;
set_global_transform(gt);
travel = result.motion;
return result.remainder;
#else
//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(), Vector2());
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
ERR_FAIL_COND_V(!dss, Vector2());
const int max_shapes = 32;
Vector2 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 (true)
mask |= Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (true)
mask |= Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (true)
mask |= Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (true)
mask |= Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
//print_line("margin: "+rtos(margin));
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), Vector2(), margin, sr, max_shapes, res_shapes, exclude, get_layer_mask(), mask))
collided = true;
}
if (!collided)
break;
Vector2 recover_motion;
for (int i = 0; i < res_shapes; i++) {
Vector2 a = sr[i * 2 + 0];
Vector2 b = sr[i * 2 + 1];
float d = a.distance_to(b);
/*
if (d<margin)
continue;
*/
recover_motion += (b - a) * 0.4;
}
if (recover_motion == Vector2()) {
collided = false;
break;
}
Transform2D gt = get_global_transform();
gt.elements[2] += recover_motion;
set_global_transform(gt);
recover_attempts--;
} while (recover_attempts);
//move second
float safe = 1.0;
float unsafe = 1.0;
int best_shape = -1;
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_set_as_trigger(i))
continue;
float lsafe, lunsafe;
bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0, lsafe, lunsafe, exclude, get_layer_mask(), mask);
//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
if (!valid) {
safe = 0;
unsafe = 0;
best_shape = i; //sadly it's the best
break;
}
if (lsafe == 1.0) {
continue;
}
if (lsafe < safe) {
safe = lsafe;
unsafe = lunsafe;
best_shape = i;
}
}
//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
if (safe >= 1) {
//not collided
colliding = false;
} else {
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = get_global_transform();
ugt.elements[2] += p_motion * unsafe;
Physics2DDirectSpaceState::ShapeRestInfo rest_info;
bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt * get_shape_transform(best_shape), Vector2(), margin, &rest_info, exclude, get_layer_mask(), mask);
if (!c2) {
//should not happen, but floating point precision is so weird..
colliding = false;
} else {
//print_line("Travel: "+rtos(travel));
colliding = true;
collision = rest_info.point;
normal = rest_info.normal;
collider = rest_info.collider_id;
collider_vel = rest_info.linear_velocity;
collider_shape = rest_info.shape;
collider_metadata = rest_info.metadata;
}
}
Vector2 motion = p_motion * safe;
Transform2D gt = get_global_transform();
gt.elements[2] += motion;
set_global_transform(gt);
return p_motion - motion;
#endif
return colliding;
}
Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, float p_slope_stop_min_velocity, int p_max_bounces, float p_floor_max_angle) {
Vector2 motion = (move_and_slide_floor_velocity + p_linear_velocity) * get_fixed_process_delta_time();
Vector2 motion = (floor_velocity + p_linear_velocity) * get_fixed_process_delta_time();
Vector2 lv = p_linear_velocity;
move_and_slide_on_floor = false;
move_and_slide_on_ceiling = false;
move_and_slide_on_wall = false;
move_and_slide_colliders.clear();
move_and_slide_floor_velocity = Vector2();
on_floor = false;
on_ceiling = false;
on_wall = false;
colliders.clear();
floor_velocity = Vector2();
while (p_max_bounces) {
motion = move(motion);
Collision collision;
if (is_colliding()) {
bool collided = move(motion, collision);
if (collided) {
motion = collision.remainder;
if (p_floor_direction == Vector2()) {
//all is a wall
move_and_slide_on_wall = true;
on_wall = true;
} else {
if (get_collision_normal().dot(p_floor_direction) >= Math::cos(p_floor_max_angle)) { //floor
if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle)) { //floor
move_and_slide_on_floor = true;
move_and_slide_floor_velocity = get_collider_velocity();
on_floor = true;
floor_velocity = collision.collider_vel;
if (get_travel().length() < 1 && ABS((lv.x - move_and_slide_floor_velocity.x)) < p_slope_stop_min_velocity) {
revert_motion();
if (collision.travel.length() < 1 && ABS((lv.x - floor_velocity.x)) < p_slope_stop_min_velocity) {
Transform2D gt = get_global_transform();
gt.elements[2] -= collision.travel;
set_global_transform(gt);
return Vector2();
}
} else if (get_collision_normal().dot(-p_floor_direction) >= Math::cos(p_floor_max_angle)) { //ceiling
move_and_slide_on_ceiling = true;
} else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle)) { //ceiling
on_ceiling = true;
} else {
move_and_slide_on_wall = true;
on_wall = true;
}
}
Vector2 n = get_collision_normal();
Vector2 n = collision.normal;
motion = motion.slide(n);
lv = lv.slide(n);
Variant collider = _get_collider();
if (collider.get_type() != Variant::NIL) {
move_and_slide_colliders.push_back(collider);
}
colliders.push_back(collision);
} else {
break;
@ -1196,26 +1053,22 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
return lv;
}
bool KinematicBody2D::is_move_and_slide_on_floor() const {
bool KinematicBody2D::is_on_floor() const {
return move_and_slide_on_floor;
return on_floor;
}
bool KinematicBody2D::is_move_and_slide_on_wall() const {
bool KinematicBody2D::is_on_wall() const {
return move_and_slide_on_wall;
return on_wall;
}
bool KinematicBody2D::is_move_and_slide_on_ceiling() const {
bool KinematicBody2D::is_on_ceiling() const {
return move_and_slide_on_ceiling;
}
Array KinematicBody2D::get_move_and_slide_colliders() const {
return move_and_slide_colliders;
return on_ceiling;
}
Vector2 KinematicBody2D::move_to(const Vector2 &p_position) {
Vector2 KinematicBody2D::get_floor_velocity() const {
return move(p_position - get_global_position());
return floor_velocity;
}
bool KinematicBody2D::test_move(const Transform2D &p_from, const Vector2 &p_motion) {
@ -1225,98 +1078,123 @@ bool KinematicBody2D::test_move(const Transform2D &p_from, const Vector2 &p_moti
return Physics2DServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, margin);
}
Vector2 KinematicBody2D::get_collision_pos() const {
ERR_FAIL_COND_V(!colliding, Vector2());
return collision;
}
Vector2 KinematicBody2D::get_collision_normal() const {
ERR_FAIL_COND_V(!colliding, Vector2());
return normal;
}
Vector2 KinematicBody2D::get_collider_velocity() const {
return collider_vel;
}
ObjectID KinematicBody2D::get_collider() const {
ERR_FAIL_COND_V(!colliding, 0);
return collider;
}
int KinematicBody2D::get_collider_shape() const {
ERR_FAIL_COND_V(!colliding, 0);
return collider_shape;
}
Variant KinematicBody2D::get_collider_metadata() const {
ERR_FAIL_COND_V(!colliding, 0);
return collider_metadata;
}
bool KinematicBody2D::is_colliding() const {
return colliding;
}
void KinematicBody2D::set_collision_margin(float p_margin) {
void KinematicBody2D::set_safe_margin(float p_margin) {
margin = p_margin;
}
float KinematicBody2D::get_collision_margin() const {
float KinematicBody2D::get_safe_margin() const {
return margin;
}
int KinematicBody2D::get_collision_count() const {
return colliders.size();
}
Vector2 KinematicBody2D::get_collision_position(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector2());
return colliders[p_collision].collision;
}
Vector2 KinematicBody2D::get_collision_normal(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector2());
return colliders[p_collision].normal;
}
Vector2 KinematicBody2D::get_collision_travel(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector2());
return colliders[p_collision].travel;
}
Vector2 KinematicBody2D::get_collision_remainder(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector2());
return colliders[p_collision].remainder;
}
Object *KinematicBody2D::get_collision_local_shape(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
uint32_t owner = shape_find_owner(colliders[p_collision].local_shape);
return shape_owner_get_owner(owner);
}
Object *KinematicBody2D::get_collision_collider(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
if (colliders[p_collision].collider) {
return ObjectDB::get_instance(colliders[p_collision].collider);
}
return NULL;
}
ObjectID KinematicBody2D::get_collision_collider_id(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), 0);
return colliders[p_collision].collider;
}
Object *KinematicBody2D::get_collision_collider_shape(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
Object *collider = get_collision_collider(p_collision);
if (collider) {
CollisionObject2D *obj2d = collider->cast_to<CollisionObject2D>();
if (obj2d) {
uint32_t owner = shape_find_owner(colliders[p_collision].collider_shape);
return obj2d->shape_owner_get_owner(owner);
}
}
return NULL;
}
int KinematicBody2D::get_collision_collider_shape_index(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), -1);
return colliders[p_collision].collider_shape;
}
Vector2 KinematicBody2D::get_collision_collider_velocity(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector2());
return colliders[p_collision].collider_vel;
}
Variant KinematicBody2D::get_collision_collider_metadata(int p_collision) const {
ERR_FAIL_INDEX_V(p_collision, colliders.size(), Variant());
return colliders[p_collision].collider_metadata;
}
void KinematicBody2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("move", "rel_vec"), &KinematicBody2D::move);
ClassDB::bind_method(D_METHOD("move_to", "position"), &KinematicBody2D::move_to);
ClassDB::bind_method(D_METHOD("move", "rel_vec"), &KinematicBody2D::_move);
ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody2D::move_and_slide, DEFVAL(Vector2(0, 0)), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec"), &KinematicBody2D::test_move);
ClassDB::bind_method(D_METHOD("get_travel"), &KinematicBody2D::get_travel);
ClassDB::bind_method(D_METHOD("revert_motion"), &KinematicBody2D::revert_motion);
ClassDB::bind_method(D_METHOD("is_colliding"), &KinematicBody2D::is_colliding);
ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody2D::is_on_floor);
ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody2D::is_on_ceiling);
ClassDB::bind_method(D_METHOD("is_on_wall"), &KinematicBody2D::is_on_wall);
ClassDB::bind_method(D_METHOD("get_floor_velocity"), &KinematicBody2D::get_floor_velocity);
ClassDB::bind_method(D_METHOD("get_collision_pos"), &KinematicBody2D::get_collision_pos);
ClassDB::bind_method(D_METHOD("get_collision_normal"), &KinematicBody2D::get_collision_normal);
ClassDB::bind_method(D_METHOD("get_collider_velocity"), &KinematicBody2D::get_collider_velocity);
ClassDB::bind_method(D_METHOD("get_collider:Variant"), &KinematicBody2D::_get_collider);
ClassDB::bind_method(D_METHOD("get_collider_shape"), &KinematicBody2D::get_collider_shape);
ClassDB::bind_method(D_METHOD("get_collider_metadata:Variant"), &KinematicBody2D::get_collider_metadata);
ClassDB::bind_method(D_METHOD("get_move_and_slide_colliders"), &KinematicBody2D::get_move_and_slide_colliders);
ClassDB::bind_method(D_METHOD("is_move_and_slide_on_floor"), &KinematicBody2D::is_move_and_slide_on_floor);
ClassDB::bind_method(D_METHOD("is_move_and_slide_on_ceiling"), &KinematicBody2D::is_move_and_slide_on_ceiling);
ClassDB::bind_method(D_METHOD("is_move_and_slide_on_wall"), &KinematicBody2D::is_move_and_slide_on_wall);
ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &KinematicBody2D::set_safe_margin);
ClassDB::bind_method(D_METHOD("get_safe_margin", "pixels"), &KinematicBody2D::get_safe_margin);
ClassDB::bind_method(D_METHOD("set_collision_margin", "pixels"), &KinematicBody2D::set_collision_margin);
ClassDB::bind_method(D_METHOD("get_collision_margin", "pixels"), &KinematicBody2D::get_collision_margin);
ClassDB::bind_method(D_METHOD("get_collision_count"), &KinematicBody2D::get_collision_count);
ClassDB::bind_method(D_METHOD("get_collision_position", "collision"), &KinematicBody2D::get_collision_position);
ClassDB::bind_method(D_METHOD("get_collision_normal", "collision"), &KinematicBody2D::get_collision_normal);
ClassDB::bind_method(D_METHOD("get_collision_travel", "collision"), &KinematicBody2D::get_collision_travel);
ClassDB::bind_method(D_METHOD("get_collision_remainder", "collision"), &KinematicBody2D::get_collision_remainder);
ClassDB::bind_method(D_METHOD("get_collision_local_shape", "collision"), &KinematicBody2D::get_collision_local_shape);
ClassDB::bind_method(D_METHOD("get_collision_collider", "collision"), &KinematicBody2D::get_collision_collider);
ClassDB::bind_method(D_METHOD("get_collision_collider_id", "collision"), &KinematicBody2D::get_collision_collider_id);
ClassDB::bind_method(D_METHOD("get_collision_collider_shape", "collision"), &KinematicBody2D::get_collision_collider_shape);
ClassDB::bind_method(D_METHOD("get_collision_collider_shape_index", "collision"), &KinematicBody2D::get_collision_collider_shape_index);
ClassDB::bind_method(D_METHOD("get_collision_collider_velocity", "collision"), &KinematicBody2D::get_collision_collider_velocity);
ClassDB::bind_method(D_METHOD("get_collision_collider_metadata", "collision"), &KinematicBody2D::get_collision_collider_metadata);
ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_collision_margin", "get_collision_margin");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
}
KinematicBody2D::KinematicBody2D()
: PhysicsBody2D(Physics2DServer::BODY_MODE_KINEMATIC) {
colliding = false;
collider = 0;
collider_shape = 0;
margin = 0.08;
move_and_slide_on_floor = false;
move_and_slide_on_ceiling = false;
move_and_slide_on_wall = false;
on_floor = false;
on_ceiling = false;
on_wall = false;
}
KinematicBody2D::~KinematicBody2D() {
}

View File

@ -264,54 +264,60 @@ class KinematicBody2D : public PhysicsBody2D {
GDCLASS(KinematicBody2D, PhysicsBody2D);
public:
struct Collision {
Vector2 collision;
Vector2 normal;
Vector2 collider_vel;
ObjectID collider;
int collider_shape;
Variant collider_metadata;
Vector2 remainder;
Vector2 travel;
int local_shape;
};
private:
float margin;
bool colliding;
Vector2 collision;
Vector2 normal;
Vector2 collider_vel;
ObjectID collider;
int collider_shape;
Variant collider_metadata;
Vector2 travel;
Vector2 move_and_slide_floor_velocity;
bool move_and_slide_on_floor;
bool move_and_slide_on_ceiling;
bool move_and_slide_on_wall;
Array move_and_slide_colliders;
Variant _get_collider() const;
Vector2 floor_velocity;
bool on_floor;
bool on_ceiling;
bool on_wall;
Vector<Collision> colliders;
_FORCE_INLINE_ bool _ignores_mode(Physics2DServer::BodyMode) const;
Dictionary _move(const Vector2 &p_motion);
protected:
static void _bind_methods();
public:
Vector2 move(const Vector2 &p_motion);
Vector2 move_to(const Vector2 &p_position);
bool move(const Vector2 &p_motion, Collision &r_collision);
bool test_move(const Transform2D &p_from, const Vector2 &p_motion);
bool is_colliding() const;
Vector2 get_travel() const;
void revert_motion();
Vector2 get_collision_pos() const;
Vector2 get_collision_normal() const;
Vector2 get_collider_velocity() const;
ObjectID get_collider() const;
int get_collider_shape() const;
Variant get_collider_metadata() const;
void set_collision_margin(float p_margin);
float get_collision_margin() const;
void set_safe_margin(float p_margin);
float get_safe_margin() const;
Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction = Vector2(0, 0), float p_slope_stop_min_velocity = 5, int p_max_bounces = 4, float p_floor_max_angle = Math::deg2rad((float)45));
bool is_move_and_slide_on_floor() const;
bool is_move_and_slide_on_wall() const;
bool is_move_and_slide_on_ceiling() const;
Array get_move_and_slide_colliders() const;
bool is_on_floor() const;
bool is_on_wall() const;
bool is_on_ceiling() const;
Vector2 get_floor_velocity() const;
int get_collision_count() const;
Vector2 get_collision_position(int p_collision) const;
Vector2 get_collision_normal(int p_collision) const;
Vector2 get_collision_travel(int p_collision) const;
Vector2 get_collision_remainder(int p_collision) const;
Object *get_collision_local_shape(int p_collision) const;
Object *get_collision_collider(int p_collision) const;
ObjectID get_collision_collider_id(int p_collision) const;
Object *get_collision_collider_shape(int p_collision) const;
int get_collision_collider_shape_index(int p_collision) const;
Vector2 get_collision_collider_velocity(int p_collision) const;
Variant get_collision_collider_metadata(int p_collision) const;
KinematicBody2D();
~KinematicBody2D();

View File

@ -781,6 +781,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
r_result->collider = rcd.best_object->get_self();
r_result->collider_id = rcd.best_object->get_instance_id();
r_result->collider_shape = rcd.best_shape;
r_result->collision_local_shape = best_shape;
r_result->collision_normal = rcd.best_normal;
r_result->collision_point = rcd.best_contact;
r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);

View File

@ -474,6 +474,7 @@ public:
Vector2 collision_point;
Vector2 collision_normal;
Vector2 collider_velocity;
int collision_local_shape;
ObjectID collider_id;
RID collider;
int collider_shape;