RID GrooveJoint2D::_configure_joint() {
Node *node_a = has_node(get_node_a()) ? get_node(get_node_a()) : (Node *)NULL;
Node *node_b = has_node(get_node_b()) ? get_node(get_node_b()) : (Node *)NULL;
if (!node_a || !node_b)
return RID();
PhysicsBody2D *body_a = node_a->cast_to<PhysicsBody2D>();
PhysicsBody2D *body_b = node_b->cast_to<PhysicsBody2D>();
if (!body_a || !body_b)
return RID();
if (get_exclude_nodes_from_collision())
Physics2DServer::get_singleton()->body_add_collision_exception(body_a->get_rid(), body_b->get_rid());
else
Physics2DServer::get_singleton()->body_remove_collision_exception(body_a->get_rid(), body_b->get_rid());
Transform2D gt = get_global_transform();
Vector2 groove_A1 = gt.get_origin();
Vector2 groove_A2 = gt.xform(Vector2(0, length));
Vector2 anchor_B = gt.xform(Vector2(0, initial_offset));
return Physics2DServer::get_singleton()->groove_joint_create(groove_A1, groove_A2, anchor_B, body_a->get_rid(), body_b->get_rid());
}
bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) {
const RayShape2DSW *ray = static_cast<const RayShape2DSW *>(p_shape_A);
if (p_shape_B->get_type() == Physics2DServer::SHAPE_RAY)
return false;
Vector2 from = p_transform_A.get_origin();
Vector2 to = from + p_transform_A[1] * ray->get_length();
Vector2 support_A = to;
Transform2D invb = p_transform_B.affine_inverse();
from = invb.xform(from);
to = invb.xform(to);
Vector2 p, n;
if (!p_shape_B->intersect_segment(from, to, p, n)) {
if (sep_axis)
*sep_axis = p_transform_A[1].normalized();
return false;
}
Vector2 support_B = p_transform_B.xform(p);
if (p_result_callback) {
if (p_swap_result)
p_result_callback(support_B, support_A, p_userdata);
else
p_result_callback(support_A, support_B, p_userdata);
}
return true;
}
void CollisionPolygon2DEditor::_canvas_draw() {
if (!node)
return;
Control *vpc = canvas_item_editor->get_viewport_control();
Vector<Vector2> poly;
if (wip_active)
poly = wip;
else
poly = node->get_polygon();
Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Ref<Texture> handle = get_icon("EditorHandle", "EditorIcons");
for (int i = 0; i < poly.size(); i++) {
Vector2 p, p2;
p = i == edited_point ? edited_point_pos : poly[i];
if ((wip_active && i == poly.size() - 1) || (((i + 1) % poly.size()) == edited_point))
p2 = edited_point_pos;
else
p2 = poly[(i + 1) % poly.size()];
Vector2 point = xform.xform(p);
Vector2 next_point = xform.xform(p2);
Color col = Color(1, 0.3, 0.1, 0.8);
vpc->draw_line(point, next_point, col, 2);
vpc->draw_texture(handle, point - handle->get_size() * 0.5);
}
}
void RayCast2D::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_ENTER_TREE: {
if (enabled && !get_tree()->is_editor_hint())
set_fixed_process(true);
else
set_fixed_process(false);
if (get_parent()->cast_to<PhysicsBody2D>()) {
if (exclude_parent_body)
exclude.insert(get_parent()->cast_to<PhysicsBody2D>()->get_rid());
else
exclude.erase(get_parent()->cast_to<PhysicsBody2D>()->get_rid());
}
} break;
case NOTIFICATION_EXIT_TREE: {
if (enabled)
set_fixed_process(false);
} break;
case NOTIFICATION_DRAW: {
if (!get_tree()->is_editor_hint() && !get_tree()->is_debugging_collisions_hint())
break;
Transform2D xf;
xf.rotate(cast_to.angle());
xf.translate(Vector2(cast_to.length(), 0));
//Vector2 tip = Vector2(0,s->get_length());
Color dcol = get_tree()->get_debug_collisions_color(); //0.9,0.2,0.2,0.4);
draw_line(Vector2(), cast_to, dcol, 3);
Vector<Vector2> pts;
float tsize = 4;
pts.push_back(xf.xform(Vector2(tsize, 0)));
pts.push_back(xf.xform(Vector2(0, 0.707 * tsize)));
pts.push_back(xf.xform(Vector2(0, -0.707 * tsize)));
Vector<Color> cols;
for (int i = 0; i < 3; i++)
cols.push_back(dcol);
draw_primitive(pts, cols, Vector<Vector2>()); //small arrow
} break;
case NOTIFICATION_FIXED_PROCESS: {
if (!enabled)
break;
_update_raycast_state();
} break;
}
}
RID GrooveJoint2D::_configure_joint(PhysicsBody2D *body_a, PhysicsBody2D *body_b) {
Transform2D gt = get_global_transform();
Vector2 groove_A1 = gt.get_origin();
Vector2 groove_A2 = gt.xform(Vector2(0, length));
Vector2 anchor_B = gt.xform(Vector2(0, initial_offset));
return Physics2DServer::get_singleton()->groove_joint_create(groove_A1, groove_A2, anchor_B, body_a->get_rid(), body_b->get_rid());
}
bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) {
Vector2 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length();
if (mlen < CMP_EPSILON)
return false;
Vector2 mnormal = motion / mlen;
real_t min, max;
p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
return false;
}
//cast a segment from support in motion normal, in the same direction of motion by motion length
//support is the worst case collision point, so real collision happened before
int a;
Vector2 s[2];
p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
Vector2 from = p_xform_A.xform(s[0]);
Vector2 to = from + motion;
Transform2D from_inv = p_xform_B.affine_inverse();
Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
Vector2 local_to = from_inv.xform(to);
Vector2 rpos, rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm))
return false;
//ray hit something
Vector2 hitpos = p_xform_B.xform(rpos);
Vector2 contact_A = to;
Vector2 contact_B = hitpos;
//create a contact
if (p_swap_result)
_contact_added_callback(contact_B, contact_A);
else
_contact_added_callback(contact_A, contact_B);
return true;
}
void RayCast2D::_update_raycast_state() {
Ref<World2D> w2d = get_world_2d();
ERR_FAIL_COND(w2d.is_null());
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(w2d->get_space());
ERR_FAIL_COND(!dss);
Transform2D gt = get_global_transform();
Vector2 to = cast_to;
if (to == Vector2())
to = Vector2(0, 0.01);
Physics2DDirectSpaceState::RayResult rr;
if (dss->intersect_ray(gt.get_origin(), gt.xform(to), rr, exclude, collision_layer, type_mask)) {
collided = true;
against = rr.collider_id;
collision_point = rr.position;
collision_normal = rr.normal;
against_shape = rr.shape;
} else {
collided = false;
}
}
RID DampedSpringJoint2D::_configure_joint() {
Node *node_a = has_node(get_node_a()) ? get_node(get_node_a()) : (Node *)NULL;
Node *node_b = has_node(get_node_b()) ? get_node(get_node_b()) : (Node *)NULL;
if (!node_a || !node_b)
return RID();
PhysicsBody2D *body_a = node_a->cast_to<PhysicsBody2D>();
PhysicsBody2D *body_b = node_b->cast_to<PhysicsBody2D>();
if (!body_a || !body_b)
return RID();
if (get_exclude_nodes_from_collision())
Physics2DServer::get_singleton()->body_add_collision_exception(body_a->get_rid(), body_b->get_rid());
else
Physics2DServer::get_singleton()->body_remove_collision_exception(body_a->get_rid(), body_b->get_rid());
Transform2D gt = get_global_transform();
Vector2 anchor_A = gt.get_origin();
Vector2 anchor_B = gt.xform(Vector2(0, length));
RID dsj = Physics2DServer::get_singleton()->damped_spring_joint_create(anchor_A, anchor_B, body_a->get_rid(), body_b->get_rid());
if (rest_length)
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_REST_LENGTH, rest_length);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_STIFFNESS, stiffness);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_DAMPING, damping);
return dsj;
}
void Node2D::_edit_set_rect(const Rect2 &p_edit_rect) {
ERR_FAIL_COND(!_edit_use_rect());
Rect2 r = _edit_get_rect();
Vector2 zero_offset;
if (r.size.x != 0)
zero_offset.x = -r.position.x / r.size.x;
if (r.size.y != 0)
zero_offset.y = -r.position.y / r.size.y;
Size2 new_scale(1, 1);
if (r.size.x != 0)
new_scale.x = p_edit_rect.size.x / r.size.x;
if (r.size.y != 0)
new_scale.y = p_edit_rect.size.y / r.size.y;
Point2 new_pos = p_edit_rect.position + p_edit_rect.size * zero_offset;
Transform2D postxf;
postxf.set_rotation_and_scale(angle, _scale);
new_pos = postxf.xform(new_pos);
pos += new_pos;
_scale *= new_scale;
_update_transform();
_change_notify("scale");
_change_notify("position");
}
bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
_RestCallbackData2D rcd;
rcd.best_len = 0;
rcd.best_object = NULL;
rcd.best_shape = 0;
for (int i = 0; i < amount; i++) {
if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask))
continue;
const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
if (p_exclude.has(col_obj->get_self()))
continue;
rcd.valid_dir = Vector2();
rcd.valid_depth = 0;
rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin);
if (!sc)
continue;
}
if (rcd.best_len == 0)
return false;
r_info->collider_id = rcd.best_object->get_instance_id();
r_info->shape = rcd.best_shape;
r_info->normal = rcd.best_normal;
r_info->point = rcd.best_contact;
r_info->rid = rcd.best_object->get_self();
r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
Vector2 rel_vec = r_info->point - body->get_transform().get_origin();
r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
} else {
r_info->linear_velocity = Vector2();
}
return true;
}
void Node2D::set_global_position(const Point2 &p_pos) {
Transform2D inv;
CanvasItem *pi = get_parent_item();
if (pi) {
inv = pi->get_global_transform().affine_inverse();
set_position(inv.xform(p_pos));
} else {
set_position(p_pos);
}
}
Ref<InputEvent> InputEventScreenTouch::xformed_by(const Transform2D &p_xform, const Vector2 &p_local_ofs) const {
Ref<InputEventScreenTouch> st;
st.instance();
st->set_device(get_device());
st->set_index(index);
st->set_position(p_xform.xform(pos + p_local_ofs));
st->set_pressed(pressed);
return st;
}
void NavigationPolygonEditor::_canvas_draw() {
if (!node)
return;
Control *vpc = canvas_item_editor->get_viewport_control();
if (node->get_navigation_polygon().is_null())
return;
Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Ref<Texture> handle = get_icon("EditorHandle", "EditorIcons");
for (int j = -1; j < node->get_navigation_polygon()->get_outline_count(); j++) {
Vector<Vector2> poly;
if (wip_active && j == edited_outline) {
poly = wip;
} else {
if (j == -1)
continue;
poly = Variant(node->get_navigation_polygon()->get_outline(j));
}
for (int i = 0; i < poly.size(); i++) {
Vector2 p, p2;
p = (j == edited_outline && i == edited_point) ? edited_point_pos : poly[i];
if (j == edited_outline && ((wip_active && i == poly.size() - 1) || (((i + 1) % poly.size()) == edited_point)))
p2 = edited_point_pos;
else
p2 = poly[(i + 1) % poly.size()];
Vector2 point = xform.xform(p);
Vector2 next_point = xform.xform(p2);
Color col = Color(1, 0.3, 0.1, 0.8);
vpc->draw_line(point, next_point, col, 2);
vpc->draw_texture(handle, point - handle->get_size() * 0.5);
}
}
}
bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
if (p_result_max <= 0)
return 0;
Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
bool collided = false;
r_result_count = 0;
Physics2DServerSW::CollCbkData cbk;
cbk.max = p_result_max;
cbk.amount = 0;
cbk.ptr = r_results;
CollisionSolver2DSW::CallbackResult cbkres = NULL;
Physics2DServerSW::CollCbkData *cbkptr = NULL;
if (p_result_max > 0) {
cbkptr = &cbk;
cbkres = Physics2DServerSW::_shape_col_cbk;
}
for (int i = 0; i < amount; i++) {
if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask))
continue;
const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
if (p_exclude.has(col_obj->get_self()))
continue;
cbk.valid_dir = Vector2();
cbk.valid_depth = 0;
if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
collided = p_result_max == 0 || cbk.amount > 0;
}
}
r_result_count = cbk.amount;
return collided;
}
Ref<InputEvent> InputEventPanGesture::xformed_by(const Transform2D &p_xform, const Vector2 &p_local_ofs) const {
Ref<InputEventPanGesture> ev;
ev.instance();
ev->set_device(get_device());
ev->set_modifiers_from_event(this);
ev->set_position(p_xform.xform(get_position() + p_local_ofs));
ev->set_delta(get_delta());
return ev;
}
void Path2DEditor::forward_draw_over_canvas(Control *p_canvas) {
if (!node)
return;
if (!node->is_visible_in_tree())
return;
if (!node->get_curve().is_valid())
return;
Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Ref<Texture> handle = get_icon("EditorHandle", "EditorIcons");
Size2 handle_size = handle->get_size();
Ref<Curve2D> curve = node->get_curve();
int len = curve->get_point_count();
Control *vpc = canvas_item_editor->get_viewport_control();
for (int i = 0; i < len; i++) {
Vector2 point = xform.xform(curve->get_point_position(i));
vpc->draw_texture_rect(handle, Rect2(point - handle_size * 0.5, handle_size), false, Color(1, 1, 1, 1));
if (i < len - 1) {
Vector2 pointout = xform.xform(curve->get_point_position(i) + curve->get_point_out(i));
vpc->draw_line(point, pointout, Color(0.5, 0.5, 1.0, 0.8), 1.0);
vpc->draw_texture_rect(handle, Rect2(pointout - handle_size * 0.5, handle_size), false, Color(1, 0.5, 1, 0.3));
}
if (i > 0) {
Vector2 pointin = xform.xform(curve->get_point_position(i) + curve->get_point_in(i));
vpc->draw_line(point, pointin, Color(0.5, 0.5, 1.0, 0.8), 1.0);
vpc->draw_texture_rect(handle, Rect2(pointin - handle_size * 0.5, handle_size), false, Color(1, 0.5, 1, 0.3));
}
}
}
Ref<InputEvent> InputEventMouseMotion::xformed_by(const Transform2D &p_xform, const Vector2 &p_local_ofs) const {
Vector2 g = p_xform.xform(get_global_position());
Vector2 l = p_xform.xform(get_position() + p_local_ofs);
Vector2 r = p_xform.basis_xform(get_relative());
Vector2 s = p_xform.basis_xform(get_speed());
Ref<InputEventMouseMotion> mm;
mm.instance();
mm->set_device(get_device());
mm->set_modifiers_from_event(this);
mm->set_position(l);
mm->set_global_position(g);
mm->set_button_mask(get_button_mask());
mm->set_relative(r);
mm->set_speed(s);
return mm;
}
RID DampedSpringJoint2D::_configure_joint(PhysicsBody2D *body_a, PhysicsBody2D *body_b) {
Transform2D gt = get_global_transform();
Vector2 anchor_A = gt.get_origin();
Vector2 anchor_B = gt.xform(Vector2(0, length));
RID dsj = Physics2DServer::get_singleton()->damped_spring_joint_create(anchor_A, anchor_B, body_a->get_rid(), body_b->get_rid());
if (rest_length)
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_REST_LENGTH, rest_length);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_STIFFNESS, stiffness);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_DAMPING, damping);
return dsj;
}
Ref<InputEvent> InputEventMouseButton::xformed_by(const Transform2D &p_xform, const Vector2 &p_local_ofs) const {
Vector2 g = p_xform.xform(get_global_position());
Vector2 l = p_xform.xform(get_position() + p_local_ofs);
Ref<InputEventMouseButton> mb;
mb.instance();
mb->set_device(get_device());
mb->set_modifiers_from_event(this);
mb->set_position(l);
mb->set_global_position(g);
mb->set_button_mask(get_button_mask());
mb->set_pressed(pressed);
mb->set_doubleclick(doubleclick);
mb->set_factor(factor);
mb->set_button_index(button_index);
return mb;
}
bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
const LineShape2DSW *line = static_cast<const LineShape2DSW *>(p_shape_A);
if (p_shape_B->get_type() == Physics2DServer::SHAPE_LINE)
return false;
Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized();
Vector2 p = p_transform_A.xform(line->get_normal() * line->get_d());
real_t d = n.dot(p);
Vector2 supports[2];
int support_count;
p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(), supports, support_count);
bool found = false;
for (int i = 0; i < support_count; i++) {
supports[i] = p_transform_B.xform(supports[i]);
real_t pd = n.dot(supports[i]);
if (pd >= d)
continue;
found = true;
Vector2 support_A = supports[i] - n * (pd - d);
if (p_result_callback) {
if (p_swap_result)
p_result_callback(supports[i], support_A, p_userdata);
else
p_result_callback(support_A, supports[i], p_userdata);
}
}
return found;
}
Ref<InputEvent> InputEventScreenDrag::xformed_by(const Transform2D &p_xform, const Vector2 &p_local_ofs) const {
Ref<InputEventScreenDrag> sd;
sd.instance();
sd->set_device(get_device());
sd->set_index(index);
sd->set_position(p_xform.xform(pos + p_local_ofs));
sd->set_relative(p_xform.basis_xform(relative));
sd->set_speed(p_xform.basis_xform(speed));
return sd;
}
int Physics2DDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask) {
if (p_result_max <= 0)
return 0;
Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_xform.xform(shape->get_aabb());
aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
int cc = 0;
for (int i = 0; i < amount; i++) {
if (cc >= p_result_max)
break;
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask))
continue;
if (p_exclude.has(space->intersection_query_results[i]->get_self()))
continue;
const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), NULL, NULL, NULL, p_margin))
continue;
r_results[cc].collider_id = col_obj->get_instance_id();
if (r_results[cc].collider_id != 0)
r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
r_results[cc].rid = col_obj->get_self();
r_results[cc].shape = shape_idx;
r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx);
cc++;
}
return cc;
}
void CollisionObject2DSW::_update_shapes() {
if (!space)
return;
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes[i];
if (s.bpid == 0) {
s.bpid = space->get_broadphase()->create(this, i);
space->get_broadphase()->set_static(s.bpid, _static);
}
//not quite correct, should compute the next matrix..
Rect2 shape_aabb = s.shape->get_aabb();
Transform2D xform = transform * s.xform;
shape_aabb = xform.xform(shape_aabb);
s.aabb_cache = shape_aabb;
s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
space->get_broadphase()->move(s.bpid, s.aabb_cache);
}
}
void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
if (!space)
return;
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes[i];
if (s.bpid == 0) {
s.bpid = space->get_broadphase()->create(this, i);
space->get_broadphase()->set_static(s.bpid, _static);
}
//not quite correct, should compute the next matrix..
Rect2 shape_aabb = s.shape->get_aabb();
Transform2D xform = transform * s.xform;
shape_aabb = xform.xform(shape_aabb);
shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
s.aabb_cache = shape_aabb;
space->get_broadphase()->move(s.bpid, shape_aabb);
}
}
void VisualServerCanvas::_render_canvas_item(Item *p_canvas_item, const Transform2D &p_transform, const Rect2 &p_clip_rect, const Color &p_modulate, int p_z, RasterizerCanvas::Item **z_list, RasterizerCanvas::Item **z_last_list, Item *p_canvas_clip, Item *p_material_owner) {
Item *ci = p_canvas_item;
if (!ci->visible)
return;
Rect2 rect = ci->get_rect();
Transform2D xform = p_transform * ci->xform;
Rect2 global_rect = xform.xform(rect);
global_rect.pos += p_clip_rect.pos;
if (ci->use_parent_material && p_material_owner)
ci->material_owner = p_material_owner;
else {
p_material_owner = ci;
ci->material_owner = NULL;
}
Color modulate(ci->modulate.r * p_modulate.r, ci->modulate.g * p_modulate.g, ci->modulate.b * p_modulate.b, ci->modulate.a * p_modulate.a);
if (modulate.a < 0.007)
return;
int child_item_count = ci->child_items.size();
Item **child_items = (Item **)alloca(child_item_count * sizeof(Item *));
copymem(child_items, ci->child_items.ptr(), child_item_count * sizeof(Item *));
if (ci->clip) {
if (p_canvas_clip != NULL) {
ci->final_clip_rect = p_canvas_clip->final_clip_rect.clip(global_rect);
} else {
ci->final_clip_rect = global_rect;
}
ci->final_clip_owner = ci;
} else {
ci->final_clip_owner = p_canvas_clip;
}
if (ci->sort_y) {
SortArray<Item *, ItemPtrSort> sorter;
sorter.sort(child_items, child_item_count);
}
if (ci->z_relative)
p_z = CLAMP(p_z + ci->z, VS::CANVAS_ITEM_Z_MIN, VS::CANVAS_ITEM_Z_MAX);
else
p_z = ci->z;
for (int i = 0; i < child_item_count; i++) {
if (!child_items[i]->behind)
continue;
_render_canvas_item(child_items[i], xform, p_clip_rect, modulate, p_z, z_list, z_last_list, (Item *)ci->final_clip_owner, p_material_owner);
}
if (ci->copy_back_buffer) {
ci->copy_back_buffer->screen_rect = xform.xform(ci->copy_back_buffer->rect).clip(p_clip_rect);
}
if ((!ci->commands.empty() && p_clip_rect.intersects(global_rect)) || ci->vp_render || ci->copy_back_buffer) {
//something to draw?
ci->final_transform = xform;
ci->final_modulate = Color(modulate.r * ci->self_modulate.r, modulate.g * ci->self_modulate.g, modulate.b * ci->self_modulate.b, modulate.a * ci->self_modulate.a);
ci->global_rect_cache = global_rect;
ci->global_rect_cache.pos -= p_clip_rect.pos;
ci->light_masked = false;
int zidx = p_z - VS::CANVAS_ITEM_Z_MIN;
if (z_last_list[zidx]) {
z_last_list[zidx]->next = ci;
z_last_list[zidx] = ci;
} else {
z_list[zidx] = ci;
z_last_list[zidx] = ci;
}
ci->next = NULL;
}
for (int i = 0; i < child_item_count; i++) {
if (child_items[i]->behind)
continue;
_render_canvas_item(child_items[i], xform, p_clip_rect, modulate, p_z, z_list, z_last_list, (Item *)ci->final_clip_owner, p_material_owner);
}
}
bool CollisionPolygon2DEditor::forward_gui_input(const Ref<InputEvent> &p_event) {
if (!node)
return false;
Ref<InputEventMouseButton> mb;
if (mb.is_valid()) {
Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Vector2 gpoint = mb->get_position();
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint = canvas_item_editor->snap_point(cpoint);
cpoint = node->get_global_transform().affine_inverse().xform(cpoint);
Vector<Vector2> poly = node->get_polygon();
//first check if a point is to be added (segment split)
real_t grab_treshold = EDITOR_DEF("editors/poly_editor/point_grab_radius", 8);
switch (mode) {
case MODE_CREATE: {
if (mb->get_button_index() == BUTTON_LEFT && mb->is_pressed()) {
if (!wip_active) {
wip.clear();
wip.push_back(cpoint);
wip_active = true;
edited_point_pos = cpoint;
canvas_item_editor->get_viewport_control()->update();
edited_point = 1;
return true;
} else {
if (wip.size() > 1 && xform.xform(wip[0]).distance_to(gpoint) < grab_treshold) {
//wip closed
_wip_close();
return true;
} else {
wip.push_back(cpoint);
edited_point = wip.size();
canvas_item_editor->get_viewport_control()->update();
return true;
//add wip point
}
}
} else if (mb->get_button_index() == BUTTON_RIGHT && mb->is_pressed() && wip_active) {
_wip_close();
}
} break;
case MODE_EDIT: {
if (mb->get_button_index() == BUTTON_LEFT) {
if (mb->is_pressed()) {
if (mb->get_control()) {
if (poly.size() < 3) {
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_undo_method(node, "set_polygon", poly);
poly.push_back(cpoint);
undo_redo->add_do_method(node, "set_polygon", poly);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->commit_action();
return true;
}
//search edges
int closest_idx = -1;
Vector2 closest_pos;
real_t closest_dist = 1e10;
for (int i = 0; i < poly.size(); i++) {
Vector2 points[2] = { xform.xform(poly[i]),
xform.xform(poly[(i + 1) % poly.size()]) };
Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint, points);
if (cp.distance_squared_to(points[0]) < CMP_EPSILON2 || cp.distance_squared_to(points[1]) < CMP_EPSILON2)
continue; //not valid to reuse point
real_t d = cp.distance_to(gpoint);
if (d < closest_dist && d < grab_treshold) {
closest_dist = d;
closest_pos = cp;
closest_idx = i;
}
}
if (closest_idx >= 0) {
pre_move_edit = poly;
poly.insert(closest_idx + 1, xform.affine_inverse().xform(closest_pos));
edited_point = closest_idx + 1;
edited_point_pos = xform.affine_inverse().xform(closest_pos);
node->set_polygon(poly);
canvas_item_editor->get_viewport_control()->update();
return true;
}
} else {
//look for points to move
int closest_idx = -1;
Vector2 closest_pos;
real_t closest_dist = 1e10;
for (int i = 0; i < poly.size(); i++) {
Vector2 cp = xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d < closest_dist && d < grab_treshold) {
closest_dist = d;
closest_pos = cp;
closest_idx = i;
}
}
if (closest_idx >= 0) {
pre_move_edit = poly;
edited_point = closest_idx;
edited_point_pos = xform.affine_inverse().xform(closest_pos);
canvas_item_editor->get_viewport_control()->update();
return true;
}
}
} else {
if (edited_point != -1) {
//apply
ERR_FAIL_INDEX_V(edited_point, poly.size(), false);
poly[edited_point] = edited_point_pos;
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_do_method(node, "set_polygon", poly);
undo_redo->add_undo_method(node, "set_polygon", pre_move_edit);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->commit_action();
edited_point = -1;
return true;
}
}
} else if (mb->get_button_index() == BUTTON_RIGHT && mb->is_pressed() && edited_point == -1) {
int closest_idx = -1;
Vector2 closest_pos;
real_t closest_dist = 1e10;
for (int i = 0; i < poly.size(); i++) {
Vector2 cp = xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d < closest_dist && d < grab_treshold) {
closest_dist = d;
closest_pos = cp;
closest_idx = i;
}
}
if (closest_idx >= 0) {
undo_redo->create_action(TTR("Edit Poly (Remove Point)"));
undo_redo->add_undo_method(node, "set_polygon", poly);
poly.remove(closest_idx);
undo_redo->add_do_method(node, "set_polygon", poly);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->commit_action();
return true;
}
}
} break;
}
}
Ref<InputEventMouseMotion> mm = p_event;
if (mm.is_valid()) {
if (edited_point != -1 && (wip_active || mm->get_button_mask() & BUTTON_MASK_LEFT)) {
Vector2 gpoint = mm->get_position();
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint = canvas_item_editor->snap_point(cpoint);
edited_point_pos = node->get_global_transform().affine_inverse().xform(cpoint);
canvas_item_editor->get_viewport_control()->update();
}
}
return false;
}
bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
ERR_FAIL_COND_V(space->locked, false);
Vector2 begin, end;
Vector2 normal;
begin = p_from;
end = p_to;
normal = (end - begin).normalized();
int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
bool collided = false;
Vector2 res_point, res_normal;
int res_shape;
const CollisionObject2DSW *res_obj;
real_t min_d = 1e10;
for (int i = 0; i < amount; i++) {
if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask))
continue;
if (p_exclude.has(space->intersection_query_results[i]->get_self()))
continue;
const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
Vector2 local_from = inv_xform.xform(begin);
Vector2 local_to = inv_xform.xform(end);
/*local_from = col_obj->get_inv_transform().xform(begin);
local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from);
local_to = col_obj->get_inv_transform().xform(end);
local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/
const Shape2DSW *shape = col_obj->get_shape(shape_idx);
Vector2 shape_point, shape_normal;
if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
shape_point = xform.xform(shape_point);
real_t ld = normal.dot(shape_point);
if (ld < min_d) {
min_d = ld;
res_point = shape_point;
res_normal = inv_xform.basis_xform_inv(shape_normal).normalized();
res_shape = shape_idx;
res_obj = col_obj;
collided = true;
}
}
}
if (!collided)
return false;
r_result.collider_id = res_obj->get_instance_id();
if (r_result.collider_id != 0)
r_result.collider = ObjectDB::get_instance(r_result.collider_id);
r_result.normal = res_normal;
r_result.metadata = res_obj->get_shape_metadata(res_shape);
r_result.position = res_point;
r_result.rid = res_obj->get_self();
r_result.shape = res_shape;
return true;
}
bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result) {
//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..
if (r_result) {
r_result->collider_id = 0;
r_result->collider_shape = 0;
}
Rect2 body_aabb;
for (int i = 0; i < p_body->get_shape_count(); i++) {
if (i == 0)
body_aabb = p_body->get_shape_aabb(i);
else
body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
}
// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_margin);
Transform2D body_transform = p_from;
{
//STEP 1, FREE BODY IF STUCK
const int max_results = 32;
int recover_attempts = 4;
Vector2 sr[max_results * 2];
do {
Physics2DServerSW::CollCbkData cbk;
cbk.max = max_results;
cbk.amount = 0;
cbk.ptr = sr;
Physics2DServerSW::CollCbkData *cbkptr = &cbk;
CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk;
bool collided = false;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j))
continue;
Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
cbk.valid_dir = body_shape_xform.get_axis(1).normalized();
cbk.valid_depth = p_margin; //only valid depth is the collision margin
} else {
cbk.valid_dir = Vector2();
cbk.valid_depth = 0;
}
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
collided = cbk.amount > 0;
}
}
}
if (!collided) {
break;
}
Vector2 recover_motion;
for (int i = 0; i < cbk.amount; i++) {
Vector2 a = sr[i * 2 + 0];
Vector2 b = sr[i * 2 + 1];
#if 0
Vector2 rel = b-a;
real_t d = rel.length();
if (d==0)
continue;
Vector2 n = rel/d;
real_t traveled = n.dot(recover_motion);
a+=n*traveled;
real_t d = a.distance_to(b);
if (d<margin)
continue;
#endif
recover_motion += (b - a) * 0.4;
}
if (recover_motion == Vector2()) {
collided = false;
break;
}
body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;
recover_attempts--;
} while (recover_attempts);
}
real_t safe = 1.0;
real_t unsafe = 1.0;
int best_shape = -1;
{
// STEP 2 ATTEMPT MOTION
Rect2 motion_aabb = body_aabb;
motion_aabb.position += p_motion;
motion_aabb = motion_aabb.merge(body_aabb);
int amount = _cull_aabb_for_body(p_body, motion_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j))
continue;
Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
bool stuck = false;
real_t best_safe = 1;
real_t best_unsafe = 1;
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
continue;
}
//test initial overlap
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
if (col_obj->is_shape_set_as_one_way_collision(j)) {
continue;
}
stuck = true;
break;
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector2 mnormal = p_motion.normalized();
for (int k = 0; k < 8; k++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, 0);
if (collided) {
hi = ofs;
} else {
low = ofs;
}
}
if (col_obj->is_shape_set_as_one_way_collision(j)) {
Vector2 cd[2];
Physics2DServerSW::CollCbkData cbk;
cbk.max = 1;
cbk.amount = 0;
cbk.ptr = cd;
cbk.valid_dir = body_shape_xform.get_axis(1).normalized();
;
cbk.valid_depth = 10e20;
Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0);
if (!collided || cbk.amount == 0) {
continue;
}
}
if (low < best_safe) {
best_safe = low;
best_unsafe = hi;
}
}
if (stuck) {
safe = 0;
unsafe = 0;
best_shape = j; //sadly it's the best
break;
}
if (best_safe == 1.0) {
continue;
}
if (best_safe < safe) {
safe = best_safe;
unsafe = best_unsafe;
best_shape = j;
}
}
}
bool collided = false;
if (safe >= 1) {
//not collided
collided = false;
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector2();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
} else {
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = body_transform;
ugt.elements[2] += p_motion * unsafe;
_RestCallbackData2D rcd;
rcd.best_len = 0;
rcd.best_object = NULL;
rcd.best_shape = 0;
Transform2D body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
Shape2DSW *body_shape = p_body->get_shape(best_shape);
body_aabb.position += p_motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
rcd.valid_dir = body_shape_xform.get_axis(1).normalized();
rcd.valid_depth = 10e20;
} else {
rcd.valid_dir = Vector2();
rcd.valid_depth = 0;
}
rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin);
if (!sc)
continue;
}
if (rcd.best_len != 0) {
if (r_result) {
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);
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin();
r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
r_result->motion = safe * p_motion;
r_result->remainder = p_motion - safe * p_motion;
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
collided = true;
} else {
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector2();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
collided = false;
}
}
return collided;
}
bool Physics2DDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape, false);
Rect2 aabb = p_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin);
/*
if (p_motion!=Vector2())
print_line(p_motion);
*/
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
real_t best_safe = 1;
real_t best_unsafe = 1;
for (int i = 0; i < amount; i++) {
if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask))
continue;
if (p_exclude.has(space->intersection_query_results[i]->get_self()))
continue; //ignore excluded
const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
/*if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
if (body->get_one_way_collision_direction()!=Vector2() && p_motion.dot(body->get_one_way_collision_direction())<=CMP_EPSILON) {
print_line("failed in motion dir");
continue;
}
}*/
Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) {
continue;
}
//test initial overlap
if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) {
return false;
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector2 mnormal = p_motion.normalized();
for (int i = 0; i < 8; i++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, p_margin);
if (collided) {
hi = ofs;
} else {
low = ofs;
}
}
if (low < best_safe) {
best_safe = low;
best_unsafe = hi;
}
}
p_closest_safe = best_safe;
p_closest_unsafe = best_unsafe;
return true;
}
bool NavigationPolygonEditor::forward_gui_input(const Ref<InputEvent> &p_event) {
if (!node)
return false;
if (node->get_navigation_polygon().is_null()) {
Ref<InputEventMouseButton> mb = p_event;
if (mb.is_valid() && mb->get_button_index() == 1 && mb->is_pressed()) {
create_nav->set_text("No NavigationPolygon resource on this node.\nCreate and assign one?");
create_nav->popup_centered_minsize();
}
return (mb.is_valid() && mb->get_button_index() == 1);
}
Ref<InputEventMouseButton> mb = p_event;
if (mb.is_valid()) {
Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Vector2 gpoint = mb->get_position();
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint = canvas_item_editor->snap_point(cpoint);
cpoint = node->get_global_transform().affine_inverse().xform(cpoint);
//first check if a point is to be added (segment split)
real_t grab_threshold = EDITOR_DEF("editors/poly_editor/point_grab_radius", 8);
switch (mode) {
case MODE_CREATE: {
if (mb->get_button_index() == BUTTON_LEFT && mb->is_pressed()) {
if (!wip_active) {
wip.clear();
wip.push_back(cpoint);
wip_active = true;
edited_point_pos = cpoint;
edited_outline = -1;
canvas_item_editor->get_viewport_control()->update();
edited_point = 1;
return true;
} else {
if (wip.size() > 1 && xform.xform(wip[0]).distance_to(gpoint) < grab_threshold) {
//wip closed
_wip_close();
return true;
} else {
wip.push_back(cpoint);
edited_point = wip.size();
canvas_item_editor->get_viewport_control()->update();
return true;
//add wip point
}
}
} else if (mb->get_button_index() == BUTTON_RIGHT && mb->is_pressed() && wip_active) {
_wip_close();
}
} break;
case MODE_EDIT: {
if (mb->get_button_index() == BUTTON_LEFT) {
if (mb->is_pressed()) {
if (mb->get_control()) {
//search edges
int closest_outline = -1;
int closest_idx = -1;
Vector2 closest_pos;
real_t closest_dist = 1e10;
for (int j = 0; j < node->get_navigation_polygon()->get_outline_count(); j++) {
PoolVector<Vector2> points = node->get_navigation_polygon()->get_outline(j);
int pc = points.size();
PoolVector<Vector2>::Read poly = points.read();
for (int i = 0; i < pc; i++) {
Vector2 points[2] = { xform.xform(poly[i]),
xform.xform(poly[(i + 1) % pc]) };
Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint, points);
if (cp.distance_squared_to(points[0]) < CMP_EPSILON2 || cp.distance_squared_to(points[1]) < CMP_EPSILON2)
continue; //not valid to reuse point
real_t d = cp.distance_to(gpoint);
if (d < closest_dist && d < grab_threshold) {
closest_dist = d;
closest_outline = j;
closest_pos = cp;
closest_idx = i;
}
}
}
if (closest_idx >= 0) {
pre_move_edit = node->get_navigation_polygon()->get_outline(closest_outline);
PoolVector<Point2> poly = pre_move_edit;
poly.insert(closest_idx + 1, xform.affine_inverse().xform(closest_pos));
edited_point = closest_idx + 1;
edited_outline = closest_outline;
edited_point_pos = xform.affine_inverse().xform(closest_pos);
node->get_navigation_polygon()->set_outline(closest_outline, poly);
canvas_item_editor->get_viewport_control()->update();
return true;
}
} else {
//look for points to move
int closest_outline = -1;
int closest_idx = -1;
Vector2 closest_pos;
real_t closest_dist = 1e10;
for (int j = 0; j < node->get_navigation_polygon()->get_outline_count(); j++) {
PoolVector<Vector2> points = node->get_navigation_polygon()->get_outline(j);
int pc = points.size();
PoolVector<Vector2>::Read poly = points.read();
for (int i = 0; i < pc; i++) {
Vector2 cp = xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d < closest_dist && d < grab_threshold) {
closest_dist = d;
closest_pos = cp;
closest_outline = j;
closest_idx = i;
}
}
}
if (closest_idx >= 0) {
pre_move_edit = node->get_navigation_polygon()->get_outline(closest_outline);
edited_point = closest_idx;
edited_outline = closest_outline;
edited_point_pos = xform.affine_inverse().xform(closest_pos);
canvas_item_editor->get_viewport_control()->update();
return true;
}
}
} else {
if (edited_point != -1) {
//apply
PoolVector<Vector2> poly = node->get_navigation_polygon()->get_outline(edited_outline);
ERR_FAIL_INDEX_V(edited_point, poly.size(), false);
poly.set(edited_point, edited_point_pos);
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_do_method(node->get_navigation_polygon().ptr(), "set_outline", edited_outline, poly);
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(), "set_outline", edited_outline, pre_move_edit);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(), "make_polygons_from_outlines");
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(), "make_polygons_from_outlines");
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->commit_action();
edited_point = -1;
return true;
}
}
} else if (mb->get_button_index() == BUTTON_RIGHT && mb->is_pressed() && edited_point == -1) {
int closest_outline = -1;
int closest_idx = -1;
Vector2 closest_pos;
real_t closest_dist = 1e10;
for (int j = 0; j < node->get_navigation_polygon()->get_outline_count(); j++) {
PoolVector<Vector2> points = node->get_navigation_polygon()->get_outline(j);
int pc = points.size();
PoolVector<Vector2>::Read poly = points.read();
for (int i = 0; i < pc; i++) {
Vector2 cp = xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d < closest_dist && d < grab_threshold) {
closest_dist = d;
closest_pos = cp;
closest_outline = j;
closest_idx = i;
}
}
}
if (closest_idx >= 0) {
PoolVector<Vector2> poly = node->get_navigation_polygon()->get_outline(closest_outline);
if (poly.size() > 3) {
undo_redo->create_action(TTR("Edit Poly (Remove Point)"));
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(), "set_outline", closest_outline, poly);
poly.remove(closest_idx);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(), "set_outline", closest_outline, poly);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(), "make_polygons_from_outlines");
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(), "make_polygons_from_outlines");
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->commit_action();
} else {
undo_redo->create_action(TTR("Remove Poly And Point"));
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(), "add_outline_at_index", poly, closest_outline);
poly.remove(closest_idx);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(), "remove_outline", closest_outline);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(), "make_polygons_from_outlines");
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(), "make_polygons_from_outlines");
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(), "update");
undo_redo->commit_action();
}
return true;
}
}
} break;
}
}
Ref<InputEventMouseMotion> mm = p_event;
if (mm.is_valid()) {
if (edited_point != -1 && (wip_active || mm->get_button_mask() & BUTTON_MASK_LEFT)) {
Vector2 gpoint = mm->get_position();
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint = canvas_item_editor->snap_point(cpoint);
edited_point_pos = node->get_global_transform().affine_inverse().xform(cpoint);
canvas_item_editor->get_viewport_control()->update();
}
}
return false;
}