C++ Matrix32::xform方法代码示例

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

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

static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {


	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);


	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);

	if (!separator.test_previous_axis())
		return;

	if (!separator.test_cast())
		return;

	//segment normal
	if (!separator.test_axis(
				(p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent()
				))
		return;

	//endpoint a vs circle
	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.get_origin()))
		return;
	//endpoint b vs circle
	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.get_origin()))
		return;


	separator.generate_contacts();
}

static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {

	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);

	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);

	if (!separator.test_previous_axis())
		return;

	if (!separator.test_cast())
		return;

	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
		return;

	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
		return;

	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
		return;
	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
		return;
	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
		return;
	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
		return;

	separator.generate_contacts();
}

bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Shape2DSW *p_shape_B, const Matrix32 &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;

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

static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {

	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);

	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);

	if (!separator.test_previous_axis())
		return;

	if (!separator.test_cast())
		return;

	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
		return;

	for(int i=0;i<convex_B->get_point_count();i++) {

		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
			return;

		if (withMargin) {

			if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(convex_B->get_point(i) )))
				return;
			if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(convex_B->get_point(i) )))
				return;

		}

	}

	separator.generate_contacts();

}

static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {

	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);

	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);


	if (!separator.test_previous_axis())
		return;

	if (!separator.test_cast())
		return;

	//box faces
	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
		return;

	if (!separator.test_axis(p_transform_a.elements[1].normalized()))
		return;

	//convex faces
	Matrix32 boxinv;
	if (withMargin) {
		boxinv=p_transform_a.affine_inverse();
	}
	for(int i=0;i<convex_B->get_point_count();i++) {

		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
			return;

		if (withMargin) {
			//all points vs all points need to be tested if margin exist
			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i)))))
				return;
			if (castA) {

				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))-p_motion_a)))
					return;
			}
			if (castB) {

				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b)))
					return;
			}
			if (castA && castB) {

				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b-p_motion_a)))
					return;
			}

		}
	}

	separator.generate_contacts();

}

bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& 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;

	Matrix32 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 Viewport::_make_input_local(InputEvent& ev) {

	switch(ev.type) {

		case InputEvent::MOUSE_BUTTON: {

			Matrix32 ai = get_final_transform().affine_inverse() * _get_input_pre_xform();
			Vector2 g = ai.xform(Vector2(ev.mouse_button.global_x,ev.mouse_button.global_y));
			Vector2 l = ai.xform(Vector2(ev.mouse_button.x,ev.mouse_button.y));
			ev.mouse_button.x=l.x;
			ev.mouse_button.y=l.y;
			ev.mouse_button.global_x=g.x;
			ev.mouse_button.global_y=g.y;

		} break;
		case InputEvent::MOUSE_MOTION: {

			Matrix32 ai = get_final_transform().affine_inverse() * _get_input_pre_xform();
			Vector2 g = ai.xform(Vector2(ev.mouse_motion.global_x,ev.mouse_motion.global_y));
			Vector2 l = ai.xform(Vector2(ev.mouse_motion.x,ev.mouse_motion.y));
			Vector2 r = ai.xform(Vector2(ev.mouse_motion.relative_x,ev.mouse_motion.relative_y));
			ev.mouse_motion.x=l.x;
			ev.mouse_motion.y=l.y;
			ev.mouse_motion.global_x=g.x;
			ev.mouse_motion.global_y=g.y;
			ev.mouse_motion.relative_x=r.x;
			ev.mouse_motion.relative_y=r.y;

		} break;
		case InputEvent::SCREEN_TOUCH: {

			Matrix32 ai = get_final_transform().affine_inverse() * _get_input_pre_xform();
			Vector2 t = ai.xform(Vector2(ev.screen_touch.x,ev.screen_touch.y));
			ev.screen_touch.x=t.x;
			ev.screen_touch.y=t.y;

		} break;
		case InputEvent::SCREEN_DRAG: {

			Matrix32 ai = get_final_transform().affine_inverse() * _get_input_pre_xform();
			Vector2 t = ai.xform(Vector2(ev.screen_drag.x,ev.screen_drag.y));
			Vector2 r = ai.xform(Vector2(ev.screen_drag.relative_x,ev.screen_drag.relative_y));
			Vector2 s = ai.xform(Vector2(ev.screen_drag.speed_x,ev.screen_drag.speed_y));
			ev.screen_drag.x=t.x;
			ev.screen_drag.y=t.y;
			ev.screen_drag.relative_x=r.x;
			ev.screen_drag.relative_y=r.y;
			ev.screen_drag.speed_x=s.x;
			ev.screen_drag.speed_y=s.y;
		} break;
	}

}

static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {

	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);


	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);

	if (!separator.test_previous_axis())
		return;

	if (!separator.test_cast())
		return;


	//poly faces and poly points vs circle
	for(int i=0;i<convex_B->get_point_count();i++) {

		if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) ))
			return;

		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
			return;
	}

	separator.generate_contacts();
}

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

	Matrix32 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,layer_mask,type_mask)) {

		collided=true;
		against=rr.collider_id;
		collision_point=rr.position;
		collision_normal=rr.normal;
		against_shape=rr.shape;
	} else {
		collided=false;
	}
}

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();
        Matrix32 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 Node2D::edit_set_rect(const Rect2& p_edit_rect) {

	Rect2 r = get_item_rect();

	Vector2 zero_offset;
	if (r.size.x!=0)
		zero_offset.x = -r.pos.x / r.size.x;
	if (r.size.y!=0)
		zero_offset.y = -r.pos.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.pos + p_edit_rect.size*zero_offset;//p_edit_rect.pos - r.pos;

	Matrix32 postxf;
	postxf.set_rotation_and_scale(angle,_scale);
	new_pos = postxf.xform(new_pos);

	pos+=new_pos;
	_scale*=new_scale;

	_update_transform();
	_change_notify("transform/scale");
	_change_notify("transform/pos");

}

void NavigationPolygonEditor::_canvas_draw() {

	if (!node)
		return;

	Control *vpc = canvas_item_editor->get_viewport_control();
	if (node->get_navigation_polygon().is_null())
			return;

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

		int len = poly.size();

		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::rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {


	Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->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.pos+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);

	_RestCallbackData 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_layer_mask,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.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();
	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 Polygon2DEditor::_uv_draw() {

	Ref<Texture> base_tex = node->get_texture();
	if (base_tex.is_null())
		return;

	Matrix32 mtx;
	mtx.elements[2]=-uv_draw_ofs;
	mtx.scale_basis(Vector2(uv_draw_zoom,uv_draw_zoom));

	VS::get_singleton()->canvas_item_set_clip(uv_edit_draw->get_canvas_item(),true);
	VS::get_singleton()->canvas_item_add_set_transform(uv_edit_draw->get_canvas_item(),mtx);
	uv_edit_draw->draw_texture(base_tex,Point2());
	VS::get_singleton()->canvas_item_add_set_transform(uv_edit_draw->get_canvas_item(),Matrix32());

	DVector<Vector2> uvs = node->get_uv();
	Ref<Texture> handle = get_icon("EditorHandle","EditorIcons");

	Rect2 rect(Point2(),mtx.basis_xform(base_tex->get_size()));
	rect.expand_to(mtx.basis_xform(uv_edit_draw->get_size()));

	for(int i=0;i<uvs.size();i++) {

		int next = (i+1)%uvs.size();
		uv_edit_draw->draw_line(mtx.xform(uvs[i]),mtx.xform(uvs[next]),Color(0.9,0.5,0.5),2);
		uv_edit_draw->draw_texture(handle,mtx.xform(uvs[i])-handle->get_size()*0.5);
		rect.expand_to(mtx.basis_xform(uvs[i]));
	}

	rect=rect.grow(200);
	updating_uv_scroll=true;
	uv_hscroll->set_min(rect.pos.x);
	uv_hscroll->set_max(rect.pos.x+rect.size.x);
	uv_hscroll->set_page(uv_edit_draw->get_size().x);
	uv_hscroll->set_val(uv_draw_ofs.x);
	uv_hscroll->set_step(0.001);

	uv_vscroll->set_min(rect.pos.y);
	uv_vscroll->set_max(rect.pos.y+rect.size.y);
	uv_vscroll->set_page(uv_edit_draw->get_size().y);
	uv_vscroll->set_val(uv_draw_ofs.y);
	uv_vscroll->set_step(0.001);
	updating_uv_scroll=false;

}