void _add_concave(const Vector<Vector2> &p_points, const Matrix32 &p_xform = Matrix32()) {
Physics2DServer *ps = Physics2DServer::get_singleton();
VisualServer *vs = VisualServer::get_singleton();
RID concave = ps->shape_create(Physics2DServer::SHAPE_CONCAVE_POLYGON);
ps->shape_set_data(concave, p_points);
RID body = ps->body_create(Physics2DServer::BODY_MODE_STATIC);
ps->body_set_space(body, space);
ps->body_add_shape(body, concave);
ps->body_set_state(body, Physics2DServer::BODY_STATE_TRANSFORM, p_xform);
RID sprite = vs->canvas_item_create();
vs->canvas_item_set_parent(sprite, canvas);
vs->canvas_item_set_transform(sprite, p_xform);
for (int i = 0; i < p_points.size(); i += 2) {
vs->canvas_item_add_line(sprite, p_points[i], p_points[i + 1], Color(0, 0, 0), 2);
}
}
RID _add_body(Physics2DServer::ShapeType p_shape, const Matrix32& p_xform) {
VisualServer *vs = VisualServer::get_singleton();
Physics2DServer *ps = Physics2DServer::get_singleton();
RID body = ps->body_create();
ps->body_add_shape(body,body_shape_data[p_shape].shape);
ps->body_set_space(body,space);
ps->body_set_continuous_collision_detection_mode(body,Physics2DServer::CCD_MODE_CAST_SHAPE);
ps->body_set_state(body,Physics2DServer::BODY_STATE_TRANSFORM,p_xform);
// print_line("add body with xform: "+p_xform);
RID sprite = vs->canvas_item_create();
vs->canvas_item_set_parent(sprite,canvas);
vs->canvas_item_set_transform(sprite,p_xform);
Size2 imgsize( vs->texture_get_width(body_shape_data[p_shape].image),vs->texture_get_height(body_shape_data[p_shape].image) );
vs->canvas_item_add_texture_rect(sprite,Rect2(-imgsize/2.0,imgsize),body_shape_data[p_shape].image);
ps->body_set_force_integration_callback(body,this,"_body_moved",sprite);
// RID q = ps->query_create(this,"_body_moved",sprite);
// ps->query_body_state(q,body);
return body;
}