C++ Matrix32类代码示例

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();
        Matrix32 xform = transform * s.xform;
        shape_aabb=xform.xform(shape_aabb);
        shape_aabb=shape_aabb.merge(Rect2( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
        s.aabb_cache=shape_aabb;

        space->get_broadphase()->move(s.bpid,shape_aabb);
    }


}

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

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,float p_margin_A,float p_margin_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,withMargin> separator(circle_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;


	//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 TileMap::_update_quadrant_transform() {

	if (!is_inside_tree())
		return;

	Matrix32 global_transform = get_global_transform();

	Matrix32 nav_rel;
	if (navigation)
		nav_rel = get_relative_transform_to_parent(navigation);

	for (Map<PosKey,Quadrant>::Element *E=quadrant_map.front();E;E=E->next()) {

		Quadrant &q=E->get();
		Matrix32 xform;
		xform.set_origin( q.pos );
		xform = global_transform * xform;
		Physics2DServer::get_singleton()->body_set_state(q.body,Physics2DServer::BODY_STATE_TRANSFORM,xform);

		if (navigation) {
			for(Map<PosKey,Quadrant::NavPoly>::Element *E=q.navpoly_ids.front();E;E=E->next()) {

				navigation->navpoly_set_transform(E->get().id,nav_rel * E->get().xform);
			}
		}

		for(Map<PosKey,Quadrant::Occluder>::Element *E=q.occluder_instances.front();E;E=E->next()) {
			VS::get_singleton()->canvas_light_occluder_set_transform(E->get().id,global_transform * E->get().xform);
		}
	}
}

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

}

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

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

static void _collision_circle_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 CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);


	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(circle_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;

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

	//capsule endpoints
	if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
		return;
	if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
		return;

	separator.generate_contacts();


}

Map<TileMap::PosKey,TileMap::Quadrant>::Element *TileMap::_create_quadrant(const PosKey& p_qk) {

	Matrix32 xform;
	//xform.set_origin(Point2(p_qk.x,p_qk.y)*cell_size*quadrant_size);
	Quadrant q;
	q.pos = _map_to_world(p_qk.x*_get_quadrant_size(),p_qk.y*_get_quadrant_size());
	q.pos+=get_cell_draw_offset();
	if (tile_origin==TILE_ORIGIN_CENTER)
		q.pos+=cell_size/2;
	else if (tile_origin==TILE_ORIGIN_BOTTOM_LEFT)
		q.pos.y+=cell_size.y;
	

	xform.set_origin( q.pos );
//	q.canvas_item = VisualServer::get_singleton()->canvas_item_create();
	q.body=Physics2DServer::get_singleton()->body_create(use_kinematic?Physics2DServer::BODY_MODE_KINEMATIC:Physics2DServer::BODY_MODE_STATIC);
	Physics2DServer::get_singleton()->body_attach_object_instance_ID(q.body,get_instance_ID());
	Physics2DServer::get_singleton()->body_set_layer_mask(q.body,collision_layer);
	Physics2DServer::get_singleton()->body_set_collision_mask(q.body,collision_mask);
	Physics2DServer::get_singleton()->body_set_param(q.body,Physics2DServer::BODY_PARAM_FRICTION,friction);
	Physics2DServer::get_singleton()->body_set_param(q.body,Physics2DServer::BODY_PARAM_BOUNCE,bounce);

	if (is_inside_tree()) {
		xform = get_global_transform() * xform;
		RID space = get_world_2d()->get_space();
		Physics2DServer::get_singleton()->body_set_space(q.body,space);
	}

	Physics2DServer::get_singleton()->body_set_state(q.body,Physics2DServer::BODY_STATE_TRANSFORM,xform);

	rect_cache_dirty=true;
	quadrant_order_dirty=true;
	return quadrant_map.insert(p_qk,q);
}

InputEvent InputEvent::xform_by(const Matrix32& p_xform) const {


	InputEvent ev=*this;

	switch(ev.type) {

		case InputEvent::MOUSE_BUTTON: {

			Vector2 g = p_xform.xform(Vector2(ev.mouse_button.global_x,ev.mouse_button.global_y));
			Vector2 l = p_xform.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: {

			Vector2 g = p_xform.xform(Vector2(ev.mouse_motion.global_x,ev.mouse_motion.global_y));
			Vector2 l = p_xform.xform(Vector2(ev.mouse_motion.x,ev.mouse_motion.y));
			Vector2 r = p_xform.basis_xform(Vector2(ev.mouse_motion.relative_x,ev.mouse_motion.relative_y));
			Vector2 s = p_xform.basis_xform(Vector2(ev.mouse_motion.speed_x,ev.mouse_motion.speed_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;
			ev.mouse_motion.speed_x=s.x;
			ev.mouse_motion.speed_y=s.y;

		} break;
		case InputEvent::SCREEN_TOUCH: {


			Vector2 t = p_xform.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: {


			Vector2 t = p_xform.xform(Vector2(ev.screen_drag.x,ev.screen_drag.y));
			Vector2 r = p_xform.basis_xform(Vector2(ev.screen_drag.relative_x,ev.screen_drag.relative_y));
			Vector2 s = p_xform.basis_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;
	}

	return ev;
}

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

}

void Node2D::set_global_pos(const Point2& p_pos) {

	Matrix32 inv;
	CanvasItem *pi = get_parent_item();
	if (pi) {
		inv = pi->get_global_transform().affine_inverse();
		set_pos(inv.xform(p_pos));
	} else {
		set_pos(p_pos);
	}
}

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

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

	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> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_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();
}

void Viewport::set_canvas_transform(const Matrix32& p_transform) {

	canvas_transform=p_transform;
	VisualServer::get_singleton()->viewport_set_canvas_transform(viewport,find_world_2d()->get_canvas(),canvas_transform);

	Matrix32 xform = (global_canvas_transform * canvas_transform).affine_inverse();
	Size2 ss = get_visible_rect().size;
	SpatialSound2DServer::get_singleton()->listener_set_transform(listener_2d,Matrix32(0,xform.xform(ss*0.5)));
	Vector2 ss2 = ss*xform.get_scale();
	float panrange = MAX(ss2.x,ss2.y);

	SpatialSound2DServer::get_singleton()->listener_set_param(listener_2d,SpatialSound2DServer::LISTENER_PARAM_PAN_RANGE,panrange);


}

void TileMap::_update_quadrant_transform() {

	if (!is_inside_scene())
		return;

	Matrix32 global_transform = get_global_transform();

	for (Map<PosKey,Quadrant>::Element *E=quadrant_map.front();E;E=E->next()) {

		Quadrant &q=E->get();
		Matrix32 xform;
		xform.set_origin( q.pos );
		xform = global_transform * xform;
		Physics2DServer::get_singleton()->body_set_state(q.static_body,Physics2DServer::BODY_STATE_TRANSFORM,xform);
	}
}