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

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

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

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

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

bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) {

	const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B);

	_ConcaveCollisionInfo2D cinfo;
	cinfo.transform_A = &p_transform_A;
	cinfo.shape_A = p_shape_A;
	cinfo.transform_B = &p_transform_B;
	cinfo.motion_A = p_motion_A;
	cinfo.result_callback = p_result_callback;
	cinfo.userdata = p_userdata;
	cinfo.swap_result = p_swap_result;
	cinfo.collided = false;
	cinfo.collisions = 0;
	cinfo.sep_axis = sep_axis;
	cinfo.margin_A = p_margin_A;
	cinfo.margin_B = p_margin_B;

	cinfo.aabb_tests = 0;

	Transform2D rel_transform = p_transform_A;
	rel_transform.elements[2] -= p_transform_B.get_origin();

	//quickly compute a local Rect2

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

		Vector2 axis(p_transform_B.elements[i]);
		real_t axis_scale = 1.0 / axis.length();
		axis *= axis_scale;

		real_t smin, smax;
		p_shape_A->project_rangev(axis, rel_transform, smin, smax);
		smin *= axis_scale;
		smax *= axis_scale;

		local_aabb.position[i] = smin;
		local_aabb.size[i] = smax - smin;
	}

	concave_B->cull(local_aabb, concave_callback, &cinfo);

	//print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests));
	return cinfo.collided;
}

//.........这里部分代码省略.........
				if (low < best_safe) {
					best_safe = low;
					best_unsafe = hi;
				}
			}

			if (stuck) {

				safe = 0;
				unsafe = 0;
				best_shape = j; //sadly it's the best
				break;
			}
			if (best_safe == 1.0) {
				continue;
			}
			if (best_safe < safe) {

				safe = best_safe;
				unsafe = best_unsafe;
				best_shape = j;
			}
		}
	}

	bool collided = false;
	if (safe >= 1) {
		//not collided
		collided = false;
		if (r_result) {

			r_result->motion = p_motion;
			r_result->remainder = Vector2();
			r_result->motion += (body_transform.get_origin() - p_from.get_origin());
		}

	} else {

		//it collided, let's get the rest info in unsafe advance
		Transform2D ugt = body_transform;
		ugt.elements[2] += p_motion * unsafe;

		_RestCallbackData2D rcd;
		rcd.best_len = 0;
		rcd.best_object = NULL;
		rcd.best_shape = 0;

		Transform2D body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
		Shape2DSW *body_shape = p_body->get_shape(best_shape);

		body_aabb.position += p_motion * unsafe;

		int amount = _cull_aabb_for_body(p_body, body_aabb);

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

			const CollisionObject2DSW *col_obj = intersection_query_results[i];
			int shape_idx = intersection_query_subindex_results[i];

			if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {

				rcd.valid_dir = body_shape_xform.get_axis(1).normalized();
				rcd.valid_depth = 10e20;
			} else {
				rcd.valid_dir = Vector2();
				rcd.valid_depth = 0;

void Body2DSW::update_inertias() {

	//update shapes and motions

	switch (mode) {

		case Physics2DServer::BODY_MODE_RIGID: {

			if (user_inertia) break;

			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
			real_t 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);

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

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

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