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

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_capsule_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 CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);


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

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

	//capsule endpoints

	for(int i=0;i<2;i++) {

		Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(i==0?0.5:-0.5);

		for(int j=0;j<2;j++) {

			Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5);

			if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) )
				return;

		}
	}

	separator.generate_contacts();

}

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


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

	if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin()))
		return;


	separator.generate_contacts();

}

static void _collision_capsule_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 CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);


	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(capsule_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;

	//capsule axis

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


	//poly vs capsule
	for(int i=0;i<convex_B->get_point_count();i++) {

		Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));

		for(int j=0;j<2;j++) {

			Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5);

			if (TEST_POINT(capsule_endpoint_A,cpoint ))
				return;

		}

		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
			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;

}

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

		} break;
		case NOTIFICATION_EXIT_TREE: {

			if (enabled)
				set_fixed_process(false);

		} break;
#ifdef TOOLS_ENABLED
		case NOTIFICATION_DRAW: {

			if (!get_tree()->is_editor_hint())
				break;
			Matrix32 xf;
			xf.rotate(cast_to.atan2());
			xf.translate(Vector2(0,cast_to.length()));

			//Vector2 tip = Vector2(0,s->get_length());
			Color dcol(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(0,tsize)));
			pts.push_back(xf.xform(Vector2(0.707*tsize,0)));
			pts.push_back(xf.xform(Vector2(-0.707*tsize,0)));
			Vector<Color> cols;
			for(int i=0;i<3;i++)
				cols.push_back(dcol);

			draw_primitive(pts,cols,Vector<Vector2>()); //small arrow

		} break;
#endif

		case NOTIFICATION_FIXED_PROCESS: {

			if (!enabled)
				break;



			Ref<World2D> w2d = get_world_2d();
			ERR_BREAK( w2d.is_null() );

			Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(w2d->get_space());
			ERR_BREAK( !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)) {

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



		} break;
	}
}

void Body2DSW::update_inertias() {

	//update shapes and motions

	switch(mode) {

		case Physics2DServer::BODY_MODE_RIGID: {

			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
			float total_area=0;

			for (int i=0;i<get_shape_count();i++) {

				total_area+=get_shape_aabb(i).get_area();
			}

			real_t _inertia=0;

			for (int i=0;i<get_shape_count();i++) {

				const Shape2DSW* shape=get_shape(i);

				float area=get_shape_aabb(i).get_area();

				float mass = area * this->mass / total_area;

				Matrix32 mtx = get_shape_transform(i);
				Vector2 scale = mtx.get_scale();
				_inertia += shape->get_moment_of_inertia(mass,scale) + mass * mtx.get_origin().length_squared();
				//Rect2 ab = get_shape_aabb(i);
				//_inertia+=mass*ab.size.dot(ab.size)/12.0f;



			}

			if (_inertia!=0)
				_inv_inertia=1.0/_inertia;
			else
				_inv_inertia=0.0; //wathever

			if (mass)
				_inv_mass=1.0/mass;
			else
				_inv_mass=0;

		} break;
		case Physics2DServer::BODY_MODE_KINEMATIC:
		case Physics2DServer::BODY_MODE_STATIC: {

			_inv_inertia=0;
			_inv_mass=0;
		} break;
		case Physics2DServer::BODY_MODE_CHARACTER: {

			_inv_inertia=0;
			_inv_mass=1.0/mass;

		} break;
	}
	//_update_inertia_tensor();

	//_update_shapes();

}

static void _collision_rectangle_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 RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);


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

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

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

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


	//box endpoints to capsule circles

	Matrix32 boxinv = p_transform_a.affine_inverse();

	for(int i=0;i<2;i++) {

		{
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);

			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
				return;
		}


		if (castA) {
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
			capsule_endpoint-=p_motion_a;

			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
				return;
		}

		if (castB) {
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
			capsule_endpoint+=p_motion_b;

			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
				return;
		}

		if (castA && castB) {
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
			capsule_endpoint-=p_motion_a;
			capsule_endpoint+=p_motion_b;


			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
				return;
		}

	}


	separator.generate_contacts();
}

static void _collision_rectangle_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 RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);


	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB> separator(rectangle_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;

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

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

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


	//box endpoints to capsule circles

	Matrix32 boxinv = p_transform_a.affine_inverse();

	for(int i=0;i<2;i++) {

		{
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);

			const Vector2& half_extents = rectangle_A->get_half_extents();
			Vector2 local_v = boxinv.xform(capsule_endpoint);

			Vector2 he(
				(local_v.x<0) ? -half_extents.x : half_extents.x,
				(local_v.y<0) ? -half_extents.y : half_extents.y
			);

			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
				return;
		}


		if (castA) {
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
			capsule_endpoint-=p_motion_a;


			const Vector2& half_extents = rectangle_A->get_half_extents();
			Vector2 local_v = boxinv.xform(capsule_endpoint);

			Vector2 he(
				(local_v.x<0) ? -half_extents.x : half_extents.x,
				(local_v.y<0) ? -half_extents.y : half_extents.y
			);

			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
				return;
		}

		if (castB) {
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
			capsule_endpoint+=p_motion_b;


			const Vector2& half_extents = rectangle_A->get_half_extents();
			Vector2 local_v = boxinv.xform(capsule_endpoint);

			Vector2 he(
				(local_v.x<0) ? -half_extents.x : half_extents.x,
				(local_v.y<0) ? -half_extents.y : half_extents.y
			);

			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
				return;
		}

		if (castA && castB) {
			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
			capsule_endpoint-=p_motion_a;
			capsule_endpoint+=p_motion_b;


			const Vector2& half_extents = rectangle_A->get_half_extents();
			Vector2 local_v = boxinv.xform(capsule_endpoint);

			Vector2 he(
				(local_v.x<0) ? -half_extents.x : half_extents.x,
				(local_v.y<0) ? -half_extents.y : half_extents.y
			);

			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
				return;
		}

//.........这里部分代码省略.........

static void _collision_circle_rectangle(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 RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);


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

	if (!separator.test_previous_axis())
		return;

	if (!separator.test_cast())
		return;

	const Vector2 &sphere=p_transform_a.elements[2];
	const Vector2 *axis=&p_transform_b.elements[0];
	const Vector2& half_extents = rectangle_B->get_half_extents();

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

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

	{
		Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin());

		Vector2 he(
			(local_v.x<0) ? -half_extents.x : half_extents.x,
			(local_v.y<0) ? -half_extents.y : half_extents.y
		);


		if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
			return;
	}

	if (castA) {

		Vector2 sphereofs = sphere + p_motion_a;
		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);

		Vector2 he(
			(local_v.x<0) ? -half_extents.x : half_extents.x,
			(local_v.y<0) ? -half_extents.y : half_extents.y
		);


		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
			return;
	}

	if (castB) {

		Vector2 sphereofs = sphere - p_motion_b;
		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);

		Vector2 he(
			(local_v.x<0) ? -half_extents.x : half_extents.x,
			(local_v.y<0) ? -half_extents.y : half_extents.y
		);


		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
			return;
	}

	if (castA && castB) {

		Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);

		Vector2 he(
			(local_v.x<0) ? -half_extents.x : half_extents.x,
			(local_v.y<0) ? -half_extents.y : half_extents.y
		);


		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
			return;
	}

	separator.generate_contacts();
}