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");
}

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;
}

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();
}