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 Transform2D::UpdateHierarchy()
{
m_bDirty = false;
Matrix localMatrix;
// Rebuild your local Matrix
RebuildTransform(localMatrix);
// Factor in the parent transform
Transform2D* parent = m_node.GetParent();
if (parent)
{
m_worldMatrix = parent->GetWorldTransform();
m_worldMatrix.SelfMultiply(localMatrix);
}
else
{
m_worldMatrix = localMatrix;
}
// Rebuild the bounds
RebuildBounds(m_worldBounds, m_dimensions);
// Rebuild the preserved bounds, if there are any
if (HasPreservedAspect())
RebuildBounds(m_worldBoundsPreserved, m_dimensionsPreserved);
// Inform your children of these updates
Transform2D* childTransform = m_node.GetChild();
while (childTransform)
{
childTransform->UpdateHierarchy();
childTransform = childTransform->m_node.GetSibling();
}
}
Transform2D Transform3D::projectTo2D(const arma::vec3& yawAxis, const arma::vec3& forwardAxis) const {
Transform2D result;
// Translation
arma::vec3 orthoForwardAxis = arma::normalise(arma::cross(yawAxis, arma::cross(forwardAxis, yawAxis)));
arma::vec3 r = translation();
Rotation3D newSpaceToWorld;
newSpaceToWorld.x() = orthoForwardAxis;
newSpaceToWorld.y() = arma::cross(yawAxis, orthoForwardAxis);
newSpaceToWorld.z() = yawAxis;
Rotation3D worldToNewSpace = newSpaceToWorld.i();
arma::vec3 rNewSpace = worldToNewSpace * r;
result.xy() = rNewSpace.rows(0, 1);
// Rotation
Rotation3D rot = rotation();
arma::vec3 x = rot.x();
arma::vec3 xNew = worldToNewSpace * x;
float theta_x_from_f = std::atan2(xNew[1], xNew[0]); // sin/cos
result.angle() = theta_x_from_f;
// std::cerr << "in = \n" << *this << std::endl;
// std::cerr << "out = \n" << result << std::endl;
return result;
}
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;
}
}
const vec2 Transform2D::ParentSpaceToChildSpace(const vec2& parentSpacePosition) const
{
vec2 childSpacePosition = parentSpacePosition;
__todo() //does this work without rotation?
vec2 unscale = vec2(1.0f, 1.0f);
vec2 untranslate = vec2(0.0f, 0.0f);
Transform2D* parent = m_node.GetParent();
while (parent)
{
const vec2& parentScale = parent->GetScale();
unscale.x *= parentScale.x;
unscale.y *= parentScale.y;
const vec2& parentPos = parent->GetPosition();
untranslate.x += parentPos.x;
untranslate.y += parentPos.y;
parent = parent->m_node.GetParent();
}
// 'un-apply' the translation
childSpacePosition -= untranslate;
// 'un-apply' the scale
childSpacePosition.x /= unscale.x;
childSpacePosition.y /= unscale.y;
return childSpacePosition;
}
void ProcAbsPos::addTestPointsAtTo(Mat& im, ProjAcq& pAcq, const Transform2D<float>& tr_rob2pg) const {
Transform2D<float> tr_pg2rob = tr_rob2pg.getReverse();
Cam const* cam = pAcq.getAcq()->getCam();
// get corners and edges of cam fov projected on playground
Mat camCorners[4];
camCorners[0] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getTopLeft()));
camCorners[1] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getTopRight()));
camCorners[2] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getBottomRight()));
camCorners[3] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getBottomLeft()));
float xMin = MIN(MIN(camCorners[0].at<float>(0), camCorners[1].at<float>(0)), MIN(camCorners[2].at<float>(0), camCorners[3].at<float>(0)));
float yMin = MIN(MIN(camCorners[0].at<float>(1), camCorners[1].at<float>(1)), MIN(camCorners[2].at<float>(1), camCorners[3].at<float>(1)));
float xMax = MAX(MAX(camCorners[0].at<float>(0), camCorners[1].at<float>(0)), MAX(camCorners[2].at<float>(0), camCorners[3].at<float>(0)));
float yMax = MAX(MAX(camCorners[0].at<float>(1), camCorners[1].at<float>(1)), MAX(camCorners[2].at<float>(1), camCorners[3].at<float>(1)));
Mat camEdges[4];
for (int i = 0; i < 4; i++) {
camEdges[i] = camCorners[(i + 1) % 4] - camCorners[i];
}
for (TestPoint const& tp : staticTP) {
cv::Mat tp_pos = tp.getPos();
float tp_x = tp_pos.at<float>(0);
float tp_y = tp_pos.at<float>(1);
if (tp_x > xMax || tp_x < xMin || tp_y > yMax || tp_y < yMin) {
continue;
}
// check if testpoint seen by camera
int i;
for (i = 0; i < 4; i++) {
Mat vi = tp_pos - camCorners[i];
double cross = vi.at<float>(0) * camEdges[i].at<float>(1) -
vi.at<float>(1) * camEdges[i].at<float>(0);
if (cross < 0) {
break;
}
}
if (i < 4) {
continue;
}
// get position of testpoint in original camera image
cv::Mat tp_cmRob = tr_pg2rob.transformLinPos(tp_pos);
cv::Mat tp_pxCam = pAcq.plane2cam(tp_cmRob);
int x = int(round(tp_pxCam.at<float>(0)));
int y = int(round(tp_pxCam.at<float>(1)));
assert(x >= 0 && x < cam->getSize().width);
assert(y >= 0 && y < cam->getSize().height);
// draw testpoint position in camera frame
im.at<Vec3b>(y, x) = Vec3b(255, 255, 255);
}
}
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");
}
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());
}
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);
}
}
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());
}
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);
}
}
bool TileSet::_set(const StringName &p_name, const Variant &p_value) {
String n = p_name;
int slash = n.find("/");
if (slash == -1)
return false;
int id = String::to_int(n.c_str(), slash);
if (!tile_map.has(id))
create_tile(id);
String what = n.substr(slash + 1, n.length());
if (what == "name")
tile_set_name(id, p_value);
else if (what == "texture")
tile_set_texture(id, p_value);
else if (what == "normal_map")
tile_set_normal_map(id, p_value);
else if (what == "tex_offset")
tile_set_texture_offset(id, p_value);
else if (what == "material")
tile_set_material(id, p_value);
else if (what == "modulate")
tile_set_modulate(id, p_value);
else if (what == "region")
tile_set_region(id, p_value);
else if (what == "shape")
tile_set_shape(id, 0, p_value);
else if (what == "shape_offset") {
Transform2D xform = tile_get_shape_transform(id, 0);
xform.set_origin(p_value);
tile_set_shape_transform(id, 0, xform);
} else if (what == "shape_transform")
tile_set_shape_transform(id, 0, p_value);
else if (what == "shape_one_way")
tile_set_shape_one_way(id, 0, p_value);
else if (what == "shapes")
_tile_set_shapes(id, p_value);
else if (what == "occluder")
tile_set_light_occluder(id, p_value);
else if (what == "occluder_offset")
tile_set_occluder_offset(id, p_value);
else if (what == "navigation")
tile_set_navigation_polygon(id, p_value);
else if (what == "navigation_offset")
tile_set_navigation_polygon_offset(id, p_value);
else
return false;
return true;
}
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;
}
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;
}
bool test(const Circle& circle, const RotatedRectangle& rect) {
/*
Let E be the centre of the rectangle (i.e. the origin of its local coordinate frame).
Define the regions A-I as below, where the central region E has the dimensions of the rectangle.
A │ B │ C
──┼───┼──
D │ E │ F
──┼───┼──
G │ H │ I
Check distance to side in regions: B, D, F, and H.
Check distance to corner in regions: A, C, G, and I.
Region E is always an intersection.
Note: This diagram and the circle are symmetric, so we can use absolute values to simplify the comparisons.
i.e.
hw
B │ C
──┼── hh
E │ F
*/
Transform2D trans = rect.getTransform();
Transform2D pos = trans.worldToLocal({circle.centre(0), circle.centre(1), 0});
double hw = 0.5 * rect.getSize()(0);
double hh = 0.5 * rect.getSize()(1);
double r = circle.radius;
double x = std::abs(pos(0));
double y = std::abs(pos(1));
if (x < hw && y < hh) { // E
return true;
}
if (x < hw && y > hh) { // B
return y < hh + r;
}
if (x > hw && y < hh) { // F
return x < hw + r;
}
// if (x > hw && y > hh) { // C
arma::vec2 cornerDiff = { hw - x, hh - y };
return arma::norm(cornerDiff) < r;
// }
}
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;
}
void Transform2D::ValidateCleanliness()
{
Transform2D* lastDirty = nullptr;
Transform2D* curNode = this;
while (curNode)
{
if (curNode->GetIsDirty())
lastDirty = curNode;
curNode = curNode->m_node.GetParent();
}
if (lastDirty)
{
ValidateTransforms(lastDirty);
}
}
void Transform2D::UpdateAnchors()
{
Box oldBound(m_worldBounds);
RebuildBounds(m_worldBounds, m_dimensions);
// Update the preserved data
UpdatePreservedAspectSizes();
Transform2D* childTransform = m_node.GetChild();
while (childTransform)
{
childTransform->ApplyAnchors(oldBound);
childTransform->UpdateAnchors();
childTransform = childTransform->m_node.GetSibling();
}
}
/**
Subtracts the specified transform to this vector. This
vector translated, then rotated.
*/
void Vect2D::subtract(Transform2D& xform) {
// translate
*this -= xform.getLocation();
// rotate
subtractRotation(xform);
}
/**
Adds the specified transform to this vector. This vector
is first rotated, then translated.
*/
void Vect2D::add(Transform2D& xform) {
// rotate
addRotation(xform);
// translate
*this += xform.getLocation();
}
void Area2DSW::set_transform(const Transform2D &p_transform) {
if (!moved_list.in_list() && get_space())
get_space()->area_add_to_moved_list(&moved_list);
_set_transform(p_transform);
_set_inv_transform(p_transform.affine_inverse());
}
void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_transform) {
ERR_FAIL_INDEX(p_index, shapes.size());
shapes[p_index].xform = p_transform;
shapes[p_index].xform_inv = p_transform.affine_inverse();
_update_shapes();
_shapes_changed();
}
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;
}
/** Write quadratic edge shape in PostScript format.
*
* Given the three points composing a quadratic mesh edge,
* write the cubic Bezier curve of the same shape in
* PostScript format. The formulas for P1 and P2
* at the start of this function will result in the cubic
* terms of the Bezier curve dropping out, leaving the
* quadratic curve matching the edge shape function as
* described in Section 3.6 of Hughes. (If you're attempting
* to verify this, don't forget to adjust for the different
* parameter ranges: \f$ \xi = 2 t - 1 \f$).
*/
static void write_eps_quadratic_edge( ostream &s,
Transform2D& xform,
Vector3D start,
Vector3D mid,
Vector3D end )
{
Vector3D P1 = 1./3 * (4 * mid - end);
Vector3D P2 = 1./3 * (4 * mid - start);
int x, y;
xform.transform( start, x, y );
s << x << ' ' << y << " moveto" << endl;
xform.transform( P1, x, y );
s << x << ' ' << y << ' ';
xform.transform( P2, x, y );
s << x << ' ' << y << ' ';
xform.transform( end, x, y );
s << x << ' ' << y << " curveto" << endl;
}
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;
}
STATIC void Transform2D::ValidateTransforms(Transform2D* root)
{
Transform2D* curTransform = root;
while (curTransform != NULL)
{
Hierarchy<Transform2D>& curNode = curTransform->m_node;
// If this transform is dirty, then update the rest of it's hierarchy
if (curTransform->GetIsDirty())
{
// We've got dirt on this transform; we know it's number... Update the transforms on downwards
curTransform->UpdateHierarchy();
// Traverse upward to continue our traversal
while (nullptr != curTransform)
{
Hierarchy<Transform2D>& upwardNode = curTransform->m_node;
// Check if there's a sibling of this node
if (nullptr != upwardNode.GetSibling())
{
curTransform = upwardNode.GetSibling();
break;
}
else
{
// If this is the root node, then it's time to leave
if (root == curTransform)
{
curTransform = nullptr;
break;
}
curTransform = upwardNode.GetParent();
}
}
}
else
{
curTransform = curNode.Next();
}
}
}
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 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;
}
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;
}
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;
}
}
