Esempio n. 1
0
void MeshEditor::edit(Ref<Mesh> p_mesh) {

	mesh = p_mesh;
	mesh_instance->set_mesh(mesh);

	if (mesh.is_null()) {

		hide();
	} else {
		rot_x = 0;
		rot_y = 0;
		_update_rotation();

		Rect3 aabb = mesh->get_aabb();
		print_line("aabb: " + aabb);
		Vector3 ofs = aabb.position + aabb.size * 0.5;
		float m = aabb.get_longest_axis_size();
		if (m != 0) {
			m = 1.0 / m;
			m *= 0.5;
			//print_line("scale: "+rtos(m));
			Transform xform;
			xform.basis.scale(Vector3(m, m, m));
			xform.origin = -xform.basis.xform(ofs); //-ofs*m;
			//xform.origin.z -= aabb.get_longest_axis_size() * 2;
			mesh_instance->set_transform(xform);
		}
	}
}
Esempio n. 2
0
void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) {

	heights = p_heights;
	width = p_width;
	depth = p_depth;
	cell_size = p_cell_size;

	PoolVector<real_t>::Read r = heights.read();

	Rect3 aabb;

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

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

			real_t h = r[i * width + j];

			Vector3 pos(j * cell_size, h, i * cell_size);
			if (i == 0 || j == 0)
				aabb.pos = pos;
			else
				aabb.expand_to(pos);
		}
	}

	configure(aabb);
}
Esempio n. 3
0
bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {


	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
	ERR_FAIL_COND_V(!shape,0);

	Rect3 aabb = p_shape_xform.xform(shape->get_aabb());
	aabb=aabb.grow(p_margin);

	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::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 CollisionObjectSW *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 = CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),_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()==CollisionObjectSW::TYPE_BODY) {

		const BodySW *body = static_cast<const BodySW*>(rcd.best_object);
		r_info->linear_velocity = body->get_linear_velocity() +
				(body->get_angular_velocity()).cross(body->get_transform().origin-rcd.best_contact);// * mPos);


	} else {
		r_info->linear_velocity=Vector3();
	}

	return true;
}
Esempio n. 4
0
int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc) {

	if (p_depth > max_depth) {
		max_depth = p_depth;
	}

	if (p_size == 1) {

		return p_bb[p_from] - p_bvh;
	} else if (p_size == 0) {

		return -1;
	}

	Rect3 aabb;
	aabb = p_bb[p_from]->aabb;
	for (int i = 1; i < p_size; i++) {

		aabb.merge_with(p_bb[p_from + i]->aabb);
	}

	int li = aabb.get_longest_axis_index();

	switch (li) {

		case Vector3::AXIS_X: {
			SortArray<BVH *, BVHCmpX> sort_x;
			sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
			//sort_x.sort(&p_bb[p_from],p_size);
		} break;
		case Vector3::AXIS_Y: {
			SortArray<BVH *, BVHCmpY> sort_y;
			sort_y.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
			//sort_y.sort(&p_bb[p_from],p_size);
		} break;
		case Vector3::AXIS_Z: {
			SortArray<BVH *, BVHCmpZ> sort_z;
			sort_z.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
			//sort_z.sort(&p_bb[p_from],p_size);

		} break;
	}

	int left = _create_bvh(p_bvh, p_bb, p_from, p_size / 2, p_depth + 1, max_depth, max_alloc);
	int right = _create_bvh(p_bvh, p_bb, p_from + p_size / 2, p_size - p_size / 2, p_depth + 1, max_depth, max_alloc);

	int index = max_alloc++;
	BVH *_new = &p_bvh[index];
	_new->aabb = aabb;
	_new->center = aabb.position + aabb.size * 0.5;
	_new->face_index = -1;
	_new->left = left;
	_new->right = right;

	return index;
}
Esempio n. 5
0
bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask){

	if (p_result_max<=0)
		return 0;

	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
	ERR_FAIL_COND_V(!shape,0);

	Rect3 aabb = p_shape_xform.xform(shape->get_aabb());
	aabb=aabb.grow(p_margin);

	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);

	bool collided=false;
	r_result_count=0;

	PhysicsServerSW::CollCbkData cbk;
	cbk.max=p_result_max;
	cbk.amount=0;
	cbk.ptr=r_results;
	CollisionSolverSW::CallbackResult cbkres=NULL;

	PhysicsServerSW::CollCbkData *cbkptr=NULL;
	if (p_result_max>0) {
		cbkptr=&cbk;
		cbkres=PhysicsServerSW::_shape_col_cbk;
	}


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

		//print_line("AGAINST: "+itos(col_obj->get_self().get_id())+":"+itos(shape_idx));
		//print_line("THE ABBB: "+(col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).xform(col_obj->get_shape(shape_idx)->get_aabb()));

		if (CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),cbkres,cbkptr,NULL,p_margin)) {
			collided=true;
		}

	}

	r_result_count=cbk.amount;

	return collided;

}
Esempio n. 6
0
_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) {

	_VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH);

	if (p_size == 1) {
		//leaf
		bvh->aabb = p_elements[0].aabb;
		bvh->left = NULL;
		bvh->right = NULL;
		bvh->face_index = p_elements->face_index;
		count++;
		return bvh;
	} else {

		bvh->face_index = -1;
	}

	Rect3 aabb;
	for (int i = 0; i < p_size; i++) {

		if (i == 0)
			aabb = p_elements[i].aabb;
		else
			aabb.merge_with(p_elements[i].aabb);
	}
	bvh->aabb = aabb;
	switch (aabb.get_longest_axis_index()) {

		case 0: {

			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x;
			sort_x.sort(p_elements, p_size);

		} break;
		case 1: {

			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y;
			sort_y.sort(p_elements, p_size);
		} break;
		case 2: {

			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z;
			sort_z.sort(p_elements, p_size);
		} break;
	}

	int split = p_size / 2;
	bvh->left = _volume_sw_build_bvh(p_elements, split, count);
	bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count);

	//printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
	count++;
	return bvh;
}
Esempio n. 7
0
void Mesh::normalizeBoundingBox()
{
    int i;
    vector<Vector3> positions;
    for(i = 0; i < (int)vertices.size(); ++i) {
        positions.push_back(vertices[i].pos);
    }
    Rect3 boundingBox = Rect3(positions.begin(), positions.end());
    double cscale = .9 / boundingBox.getSize().accumulate(ident<double>(), maximum<double>());
    Vector3 ctoAdd = Vector3(0.5, 0.5, 0.5) - boundingBox.getCenter() * cscale;
    for(i = 0; i < (int)vertices.size(); ++i) {
        vertices[i].pos = ctoAdd + vertices[i].pos * cscale;
    }
    toAdd = ctoAdd + cscale * toAdd;
    scale *= cscale;
}
Esempio n. 8
0
int BroadPhaseBasic::cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {

	int rc = 0;

	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {

		const Rect3 aabb = E->get().aabb;
		if (aabb.intersects(p_aabb)) {

			p_results[rc] = E->get().owner;
			p_result_indices[rc] = E->get().subindex;
			rc++;
			if (rc >= p_max_results)
				break;
		}
	}

	return rc;
}
Esempio n. 9
0
Error GridMap::create_area(int p_id, const Rect3 &p_bounds) {

	ERR_FAIL_COND_V(area_map.has(p_id), ERR_ALREADY_EXISTS);
	ERR_EXPLAIN("ID 0 is taken as global area, start from 1");
	ERR_FAIL_COND_V(p_id == 0, ERR_INVALID_PARAMETER);
	ERR_FAIL_COND_V(p_bounds.has_no_area(), ERR_INVALID_PARAMETER);

	// FIRST VALIDATE AREA
	IndexKey from, to;
	from.x = p_bounds.pos.x;
	from.y = p_bounds.pos.y;
	from.z = p_bounds.pos.z;
	to.x = p_bounds.pos.x + p_bounds.size.x;
	to.y = p_bounds.pos.y + p_bounds.size.y;
	to.z = p_bounds.pos.z + p_bounds.size.z;

	for (Map<int, Area *>::Element *E = area_map.front(); E; E = E->next()) {
		//this should somehow be faster...
		Area &a = *E->get();

		//does it interset with anything else?

		if (from.x >= a.to.x ||
				to.x <= a.from.x ||
				from.y >= a.to.y ||
				to.y <= a.from.y ||
				from.z >= a.to.z ||
				to.z <= a.from.z) {

			// all good
		} else {

			return ERR_INVALID_PARAMETER;
		}
	}

	Area *area = memnew(Area);
	area->from = from;
	area->to = to;
	area->portal_disable_distance = 0;
	area->exterior_portal = false;
	area->name = "Area " + itos(p_id);
	area_map[p_id] = area;
	_recreate_octant_data();
	return OK;
}
Esempio n. 10
0
bool GridMapEditor::forward_spatial_input_event(Camera* p_camera,const InputEvent& p_event) {
	if (!node) {
		return false;
	}

	if (edit_mode->get_selected()==0) { // regular click
		switch (p_event.type) {
			case InputEvent::MOUSE_BUTTON: {

				if (p_event.mouse_button.button_index==BUTTON_WHEEL_UP && (p_event.mouse_button.mod.command || p_event.mouse_button.mod.shift)) {
					if (p_event.mouse_button.pressed)
						floor->set_value( floor->get_value() +1);

					return true; //eaten
				} else if (p_event.mouse_button.button_index==BUTTON_WHEEL_DOWN && (p_event.mouse_button.mod.command || p_event.mouse_button.mod.shift)) {
					if (p_event.mouse_button.pressed)
						floor->set_value( floor->get_value() -1);
					return true;
				}

				if (p_event.mouse_button.pressed) {

					if (p_event.mouse_button.button_index==BUTTON_LEFT) {

						if (input_action==INPUT_DUPLICATE) {

							//paste
							_duplicate_paste();
							input_action=INPUT_NONE;
							_update_duplicate_indicator();
						} else if (p_event.mouse_button.mod.shift) {
							input_action=INPUT_SELECT;
						} else if (p_event.mouse_button.mod.command)
							input_action=INPUT_COPY;
						else {
							input_action=INPUT_PAINT;
							set_items.clear();
						}
					} else if (p_event.mouse_button.button_index==BUTTON_RIGHT)
						if (input_action==INPUT_DUPLICATE) {

							input_action=INPUT_NONE;
							_update_duplicate_indicator();
						} else {
							input_action=INPUT_ERASE;
							set_items.clear();
						}
					else
						return false;

					return do_input_action(p_camera,Point2(p_event.mouse_button.x,p_event.mouse_button.y),true);
				} else {


					if (
						(p_event.mouse_button.button_index==BUTTON_RIGHT && input_action==INPUT_ERASE) ||
						(p_event.mouse_button.button_index==BUTTON_LEFT && input_action==INPUT_PAINT) ) {

						if (set_items.size()) {
							undo_redo->create_action("GridMap Paint");
							for(List<SetItem>::Element *E=set_items.front();E;E=E->next()) {

								const SetItem &si=E->get();
								undo_redo->add_do_method(node,"set_cell_item",si.pos.x,si.pos.y,si.pos.z,si.new_value,si.new_orientation);
							}
							for(List<SetItem>::Element *E=set_items.back();E;E=E->prev()) {

								const SetItem &si=E->get();
								undo_redo->add_undo_method(node,"set_cell_item",si.pos.x,si.pos.y,si.pos.z,si.old_value,si.old_orientation);
							}


							undo_redo->commit_action();
						}
						set_items.clear();
						input_action=INPUT_NONE;
						return true;

					}



					if (p_event.mouse_button.button_index==BUTTON_LEFT && input_action!=INPUT_NONE) {

						set_items.clear();
						input_action=INPUT_NONE;
						return true;
					}
					if (p_event.mouse_button.button_index==BUTTON_RIGHT && (input_action==INPUT_ERASE || input_action==INPUT_DUPLICATE)) {
						input_action=INPUT_NONE;
						return true;
					}
				}
			} break;
			case InputEvent::MOUSE_MOTION: {

				return do_input_action(p_camera,Point2(p_event.mouse_motion.x,p_event.mouse_motion.y),false);
			} break;
		}

	} else if (edit_mode->get_selected()==1) {
		//area mode, select an area

		switch (p_event.type) {
			case InputEvent::MOUSE_BUTTON: {

				if (p_event.mouse_button.button_index==BUTTON_LEFT && p_event.mouse_button.pressed) {

					Point2 point = Point2(p_event.mouse_motion.x,p_event.mouse_motion.y);

					Camera *camera = p_camera;
					Vector3 from = camera->project_ray_origin(point);
					Vector3 normal = camera->project_ray_normal(point);
					Transform local_xform = node->get_global_transform().affine_inverse();
					from=local_xform.xform(from);
					normal=local_xform.basis.xform(normal).normalized();

					List<int> areas;
					node->get_area_list(&areas);

					float min_d=1e10;
					int min_area=-1;


					for(List<int>::Element *E=areas.front();E;E=E->next()) {

						int area = E->get();
						Rect3 aabb = node->area_get_bounds(area);
						aabb.pos*=node->get_cell_size();
						aabb.size*=node->get_cell_size();


						Vector3 rclip,rnormal;
						if (!aabb.intersects_segment(from,from+normal*10000,&rclip,&rnormal))
							continue;

						float d = normal.dot(rclip);
						if (d<min_d) {
							min_d=d;
							min_area=area;
						}
					}

					selected_area=min_area;
					update_areas();

				}
			} break;
		}

	}


	return false;
}
Esempio n. 11
0
void GridMap::_octant_update(const OctantKey &p_key) {
	ERR_FAIL_COND(!octant_map.has(p_key));
	Octant &g = *octant_map[p_key];
	if (!g.dirty)
		return;

	Ref<Mesh> mesh;

	_octant_clear_navmesh(p_key);
	PhysicsServer::get_singleton()->body_clear_shapes(g.static_body);

	if (g.collision_debug.is_valid()) {

		VS::get_singleton()->mesh_clear(g.collision_debug);
	}

	PoolVector<Vector3> col_debug;

	/*
	 * foreach item in this octant,
	 * set item's multimesh's instance count to number of cells which have this item
	 * and set said multimesh bounding box to one containing all cells which have this item
	 */
	for (Map<int, Octant::ItemInstances>::Element *E = g.items.front(); E; E = E->next()) {

		Octant::ItemInstances &ii = E->get();

		ii.multimesh->set_instance_count(ii.cells.size());

		Rect3 aabb;
		Rect3 mesh_aabb = ii.mesh.is_null() ? Rect3() : ii.mesh->get_aabb();

		Vector3 ofs(cell_size * 0.5 * int(center_x), cell_size * 0.5 * int(center_y), cell_size * 0.5 * int(center_z));

		//print_line("OCTANT, CELLS: "+itos(ii.cells.size()));
		int idx = 0;
		// foreach cell containing this item type
		for (Set<IndexKey>::Element *F = ii.cells.front(); F; F = F->next()) {
			IndexKey ik = F->get();
			Map<IndexKey, Cell>::Element *C = cell_map.find(ik);
			ERR_CONTINUE(!C);

			Vector3 cellpos = Vector3(ik.x, ik.y, ik.z);

			Transform xform;

			if (clip && ((clip_above && cellpos[clip_axis] > clip_floor) || (!clip_above && cellpos[clip_axis] < clip_floor))) {

				xform.basis.set_zero();

			} else {

				xform.basis.set_orthogonal_index(C->get().rot);
			}

			xform.set_origin(cellpos * cell_size + ofs);
			xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));

			ii.multimesh->set_instance_transform(idx, xform);
			//ii.multimesh->set_instance_transform(idx,Transform()	);
			//ii.multimesh->set_instance_color(idx,Color(1,1,1,1));
			//print_line("MMINST: "+xform);

			if (idx == 0) {

				aabb = xform.xform(mesh_aabb);
			} else {

				aabb.merge_with(xform.xform(mesh_aabb));
			}

			// add the item's shape at given xform to octant's static_body
			if (ii.shape.is_valid()) {
				// add the item's shape
				PhysicsServer::get_singleton()->body_add_shape(g.static_body, ii.shape->get_rid(), xform);
				if (g.collision_debug.is_valid()) {
					ii.shape->add_vertices_to_array(col_debug, xform);
				}

				//print_line("PHIS x: "+xform);
			}

			// add the item's navmesh at given xform to GridMap's Navigation ancestor
			if (navigation) {
				if (ii.navmesh.is_valid()) {
					int nm_id = navigation->navmesh_create(ii.navmesh, xform, this);
					Octant::NavMesh nm;
					nm.id = nm_id;
					nm.xform = xform;
					g.navmesh_ids[ik] = nm;
				}
			}

			idx++;
		}

		//ii.multimesh->set_aabb(aabb);
	}

	if (col_debug.size()) {

		Array arr;
		arr.resize(VS::ARRAY_MAX);
		arr[VS::ARRAY_VERTEX] = col_debug;

		VS::get_singleton()->mesh_add_surface_from_arrays(g.collision_debug, VS::PRIMITIVE_LINES, arr);
		SceneTree *st = SceneTree::get_singleton();
		if (st) {
			VS::get_singleton()->mesh_surface_set_material(g.collision_debug, 0, st->get_debug_collision_material()->get_rid());
		}
	}

	g.dirty = false;
}
Esempio n. 12
0
bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) {



	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
	ERR_FAIL_COND_V(!shape,false);

	Rect3 aabb = p_xform.xform(shape->get_aabb());
	aabb=aabb.merge(Rect3(aabb.pos+p_motion,aabb.size)); //motion
	aabb=aabb.grow(p_margin);

	/*
	if (p_motion!=Vector3())
		print_line(p_motion);
	*/

	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);

	float best_safe=1;
	float best_unsafe=1;

	Transform xform_inv = p_xform.affine_inverse();
	MotionShapeSW mshape;
	mshape.shape=shape;
	mshape.motion=xform_inv.basis.xform(p_motion);

	bool best_first=true;

	Vector3 closest_A,closest_B;

	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;

		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
			continue; //ignore excluded


		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
		int shape_idx=space->intersection_query_subindex_results[i];

		Vector3 point_A,point_B;
		Vector3 sep_axis=p_motion.normalized();

		Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
		//test initial overlap, does it collide if going all the way?
		if (CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
			//print_line("failed motion cast (no collision)");
			continue;
		}


		//test initial overlap
#if 0
		if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) {
			print_line("failed initial cast (collision at begining)");
			return false;
		}
#else
		sep_axis=p_motion.normalized();

		if (!CollisionSolverSW::solve_distance(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
			//print_line("failed motion cast (no collision)");
			return false;
		}
#endif


		//just do kinematic solving
		float low=0;
		float hi=1;
		Vector3 mnormal=p_motion.normalized();

		for(int i=0;i<8;i++) { //steps should be customizable..

			float ofs = (low+hi)*0.5;

			Vector3 sep=mnormal; //important optimization for this to work fast enough

			mshape.motion=xform_inv.basis.xform(p_motion*ofs);

			Vector3 lA,lB;

			bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,aabb,&sep);

			if (collided) {

				//print_line(itos(i)+": "+rtos(ofs));
				hi=ofs;
			} else {

				point_A=lA;
				point_B=lB;
				low=ofs;
			}
		}

		if (low<best_safe) {
			best_first=true; //force reset
			best_safe=low;
			best_unsafe=hi;
		}

		if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) {
			closest_A=point_A;
			closest_B=point_B;
			r_info->collider_id=col_obj->get_instance_id();
			r_info->rid=col_obj->get_self();
			r_info->shape=shape_idx;
			r_info->point=closest_B;
			r_info->normal=(closest_A-closest_B).normalized();
			best_first=false;
			if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) {
				const BodySW *body=static_cast<const BodySW*>(col_obj);
				r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
			}

		}


	}

	p_closest_safe=best_safe;
	p_closest_unsafe=best_unsafe;

	return true;
}
Esempio n. 13
0
Rect3 Rect3::grow(real_t p_by) const {

	Rect3 aabb = *this;
	aabb.grow_by(p_by);
	return aabb;
}
Esempio n. 14
0
Rect3 Rect3::expand(const Vector3 &p_vector) const {
	Rect3 aabb = *this;
	aabb.expand_to(p_vector);
	return aabb;
}
Esempio n. 15
0
Rect3 Rect3::merge(const Rect3 &p_with) const {

	Rect3 aabb = *this;
	aabb.merge_with(p_with);
	return aabb;
}