Rect3 Rect3::intersection(const Rect3 &p_aabb) const {
Vector3 src_min = pos;
Vector3 src_max = pos + size;
Vector3 dst_min = p_aabb.pos;
Vector3 dst_max = p_aabb.pos + p_aabb.size;
Vector3 min, max;
if (src_min.x > dst_max.x || src_max.x < dst_min.x)
return Rect3();
else {
min.x = (src_min.x > dst_min.x) ? src_min.x : dst_min.x;
max.x = (src_max.x < dst_max.x) ? src_max.x : dst_max.x;
}
if (src_min.y > dst_max.y || src_max.y < dst_min.y)
return Rect3();
else {
min.y = (src_min.y > dst_min.y) ? src_min.y : dst_min.y;
max.y = (src_max.y < dst_max.y) ? src_max.y : dst_max.y;
}
if (src_min.z > dst_max.z || src_max.z < dst_min.z)
return Rect3();
else {
min.z = (src_min.z > dst_min.z) ? src_min.z : dst_min.z;
max.z = (src_max.z < dst_max.z) ? src_max.z : dst_max.z;
}
return Rect3(min, max - min);
}
void GIProbe::_find_meshes(Node *p_at_node,Baker *p_baker){
MeshInstance *mi = p_at_node->cast_to<MeshInstance>();
if (mi && mi->get_flag(GeometryInstance::FLAG_USE_BAKED_LIGHT)) {
Ref<Mesh> mesh = mi->get_mesh();
if (mesh.is_valid()) {
Rect3 aabb = mesh->get_aabb();
Transform xf = get_global_transform().affine_inverse() * mi->get_global_transform();
if (Rect3(-extents,extents*2).intersects(xf.xform(aabb))) {
Baker::PlotMesh pm;
pm.local_xform=xf;
pm.mesh=mesh;
for(int i=0;i<mesh->get_surface_count();i++) {
pm.instance_materials.push_back(mi->get_surface_material(i));
}
pm.override_material=mi->get_material_override();
p_baker->mesh_list.push_back(pm);
}
}
}
for(int i=0;i<p_at_node->get_child_count();i++) {
Node *child = p_at_node->get_child(i);
if (!child->get_owner())
continue; //maybe a helper
_find_meshes(child,p_baker);
}
}
void ConnectDialog::_add_bind() {
if (cdbinds->params.size() >= VARIANT_ARG_MAX)
return;
Variant::Type vt = (Variant::Type)type_list->get_item_ID(type_list->get_selected());
Variant value;
switch(vt) {
case Variant::BOOL: value = false ; break;
case Variant::INT: value = 0; break;
case Variant::REAL: value = 0.0; break;
case Variant::STRING: value = ""; break;
case Variant::VECTOR2: value = Vector2(); break;
case Variant::RECT2: value = Rect2(); break;
case Variant::VECTOR3: value = Vector3(); break;
case Variant::PLANE: value = Plane(); break;
case Variant::QUAT: value = Quat(); break;
case Variant::RECT3: value = Rect3(); break;
case Variant::BASIS: value = Basis(); break;
case Variant::TRANSFORM: value = Transform(); break;
case Variant::COLOR: value = Color(); break;
case Variant::IMAGE: value = Image(); break;
default: { ERR_FAIL(); } break;
}
ERR_FAIL_COND(value.get_type()==Variant::NIL);
cdbinds->params.push_back(value);
cdbinds->notify_changed();
}
Rect3 GridMap::area_get_bounds(int p_area) const {
ERR_FAIL_COND_V(!area_map.has(p_area), Rect3());
const Area *a = area_map[p_area];
Rect3 aabb;
aabb.pos = Vector3(a->from.x, a->from.y, a->from.z);
aabb.size = Vector3(a->to.x, a->to.y, a->to.z) - aabb.pos;
return aabb;
}
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;
}
void PlaneShapeSW::_setup(const Plane &p_plane) {
plane = p_plane;
configure(Rect3(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2)));
}
void GridMapEditor::_menu_option(int p_option) {
switch(p_option) {
case MENU_OPTION_CONFIGURE: {
} break;
case MENU_OPTION_LOCK_VIEW: {
int index=options->get_popup()->get_item_index(MENU_OPTION_LOCK_VIEW);
lock_view=!options->get_popup()->is_item_checked(index);
options->get_popup()->set_item_checked(index,lock_view);
} break;
case MENU_OPTION_CLIP_DISABLED:
case MENU_OPTION_CLIP_ABOVE:
case MENU_OPTION_CLIP_BELOW: {
clip_mode=ClipMode(p_option-MENU_OPTION_CLIP_DISABLED);
for(int i=0;i<3;i++) {
int index=options->get_popup()->get_item_index(MENU_OPTION_CLIP_DISABLED+i);
options->get_popup()->set_item_checked(index,i==clip_mode);
}
_update_clip();
} break;
case MENU_OPTION_X_AXIS:
case MENU_OPTION_Y_AXIS:
case MENU_OPTION_Z_AXIS: {
int new_axis = p_option-MENU_OPTION_X_AXIS;
for(int i=0;i<3;i++) {
int idx=options->get_popup()->get_item_index(MENU_OPTION_X_AXIS+i);
options->get_popup()->set_item_checked(idx,i==new_axis);
}
edit_axis=Vector3::Axis(new_axis);
update_grid();
_update_clip();
} break;
case MENU_OPTION_CURSOR_ROTATE_Y: {
Basis r;
if (input_action==INPUT_DUPLICATE) {
r.set_orthogonal_index(selection.duplicate_rot);
r.rotate(Vector3(0,1,0),-Math_PI/2.0);
selection.duplicate_rot=r.get_orthogonal_index();
_update_duplicate_indicator();
break;
}
r.set_orthogonal_index(cursor_rot);
r.rotate(Vector3(0,1,0),-Math_PI/2.0);
cursor_rot=r.get_orthogonal_index();
_update_cursor_transform();
} break;
case MENU_OPTION_CURSOR_ROTATE_X: {
Basis r;
if (input_action==INPUT_DUPLICATE) {
r.set_orthogonal_index(selection.duplicate_rot);
r.rotate(Vector3(1,0,0),-Math_PI/2.0);
selection.duplicate_rot=r.get_orthogonal_index();
_update_duplicate_indicator();
break;
}
r.set_orthogonal_index(cursor_rot);
r.rotate(Vector3(1,0,0),-Math_PI/2.0);
cursor_rot=r.get_orthogonal_index();
_update_cursor_transform();
} break;
case MENU_OPTION_CURSOR_ROTATE_Z: {
Basis r;
if (input_action==INPUT_DUPLICATE) {
r.set_orthogonal_index(selection.duplicate_rot);
r.rotate(Vector3(0,0,1),-Math_PI/2.0);
selection.duplicate_rot=r.get_orthogonal_index();
_update_duplicate_indicator();
break;
}
r.set_orthogonal_index(cursor_rot);
r.rotate(Vector3(0,0,1),-Math_PI/2.0);
cursor_rot=r.get_orthogonal_index();
_update_cursor_transform();
} break;
case MENU_OPTION_CURSOR_BACK_ROTATE_Y: {
Basis r;
r.set_orthogonal_index(cursor_rot);
r.rotate(Vector3(0,1,0),Math_PI/2.0);
cursor_rot=r.get_orthogonal_index();
_update_cursor_transform();
} break;
case MENU_OPTION_CURSOR_BACK_ROTATE_X: {
Basis r;
r.set_orthogonal_index(cursor_rot);
r.rotate(Vector3(1,0,0),Math_PI/2.0);
cursor_rot=r.get_orthogonal_index();
_update_cursor_transform();
} break;
case MENU_OPTION_CURSOR_BACK_ROTATE_Z: {
Basis r;
r.set_orthogonal_index(cursor_rot);
r.rotate(Vector3(0,0,1),Math_PI/2.0);
cursor_rot=r.get_orthogonal_index();
_update_cursor_transform();
} break;
case MENU_OPTION_CURSOR_CLEAR_ROTATION: {
if (input_action==INPUT_DUPLICATE) {
selection.duplicate_rot=0;
_update_duplicate_indicator();
break;
}
cursor_rot=0;
_update_cursor_transform();
} break;
case MENU_OPTION_DUPLICATE_SELECTS: {
int idx = options->get_popup()->get_item_index(MENU_OPTION_DUPLICATE_SELECTS);
options->get_popup()->set_item_checked( idx, !options->get_popup()->is_item_checked( idx ) );
} break;
case MENU_OPTION_SELECTION_MAKE_AREA:
case MENU_OPTION_SELECTION_MAKE_EXTERIOR_CONNECTOR: {
if (!selection.active)
break;
int area = node->get_unused_area_id();
Error err = node->create_area(area,Rect3(selection.begin,selection.end-selection.begin+Vector3(1,1,1)));
if (err!=OK) {
}
if (p_option==MENU_OPTION_SELECTION_MAKE_EXTERIOR_CONNECTOR) {
node->area_set_exterior_portal(area,true);
}
_update_areas_display();
update_areas();
} break;
case MENU_OPTION_REMOVE_AREA: {
if (selected_area<1)
return;
node->erase_area(selected_area);
_update_areas_display();
update_areas();
} break;
case MENU_OPTION_SELECTION_DUPLICATE:
if (!(selection.active && input_action==INPUT_NONE))
return;
if (last_mouseover==Vector3(-1,-1,-1)) //nono mouseovering anythin
break;
input_action=INPUT_DUPLICATE;
selection.click=last_mouseover;
selection.current=last_mouseover;
selection.duplicate_rot=0;
_update_duplicate_indicator();
break;
case MENU_OPTION_SELECTION_CLEAR: {
if (!selection.active)
return;
_delete_selection();
} break;
case MENU_OPTION_GRIDMAP_SETTINGS: {
settings_dialog->popup_centered(settings_vbc->get_combined_minimum_size() + Size2(50, 50));
} break;
}
}
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;
}
void GDAPI godot_rect3_new(godot_rect3 *r_dest, const godot_vector3 *p_pos, const godot_vector3 *p_size) {
const Vector3 *pos = (const Vector3 *)p_pos;
const Vector3 *size = (const Vector3 *)p_size;
Rect3 *dest = (Rect3 *)r_dest;
*dest = Rect3(*pos, *size);
}
void CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) {
height = p_height;
radius = p_radius;
configure(Rect3(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0)));
}
void SphereShapeSW::_setup(real_t p_radius) {
radius = p_radius;
configure(Rect3(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0)));
}
void Quad::_update() {
if (!is_inside_tree())
return;
Vector3 normal;
normal[axis] = 1.0;
const int axis_order_1[3] = { 1, 2, 0 };
const int axis_order_2[3] = { 2, 0, 1 };
const int a1 = axis_order_1[axis];
const int a2 = axis_order_2[axis];
PoolVector<Vector3> points;
points.resize(4);
PoolVector<Vector3>::Write pointsw = points.write();
Vector2 s2 = size * 0.5;
Vector2 o = offset;
if (!centered)
o += s2;
pointsw[0][a1] = -s2.x + offset.x;
pointsw[0][a2] = s2.y + offset.y;
pointsw[1][a1] = s2.x + offset.x;
pointsw[1][a2] = s2.y + offset.y;
pointsw[2][a1] = s2.x + offset.x;
pointsw[2][a2] = -s2.y + offset.y;
pointsw[3][a1] = -s2.x + offset.x;
pointsw[3][a2] = -s2.y + offset.y;
aabb = Rect3(pointsw[0], Vector3());
for (int i = 1; i < 4; i++)
aabb.expand_to(pointsw[i]);
pointsw = PoolVector<Vector3>::Write();
PoolVector<Vector3> normals;
normals.resize(4);
PoolVector<Vector3>::Write normalsw = normals.write();
for (int i = 0; i < 4; i++)
normalsw[i] = normal;
normalsw = PoolVector<Vector3>::Write();
PoolVector<Vector2> uvs;
uvs.resize(4);
PoolVector<Vector2>::Write uvsw = uvs.write();
uvsw[0] = Vector2(0, 0);
uvsw[1] = Vector2(1, 0);
uvsw[2] = Vector2(1, 1);
uvsw[3] = Vector2(0, 1);
uvsw = PoolVector<Vector2>::Write();
PoolVector<int> indices;
indices.resize(6);
PoolVector<int>::Write indicesw = indices.write();
indicesw[0] = 0;
indicesw[1] = 1;
indicesw[2] = 2;
indicesw[3] = 2;
indicesw[4] = 3;
indicesw[5] = 0;
indicesw = PoolVector<int>::Write();
Array arr;
arr.resize(VS::ARRAY_MAX);
arr[VS::ARRAY_VERTEX] = points;
arr[VS::ARRAY_NORMAL] = normals;
arr[VS::ARRAY_TEX_UV] = uvs;
arr[VS::ARRAY_INDEX] = indices;
if (configured) {
VS::get_singleton()->mesh_remove_surface(mesh, 0);
} else {
configured = true;
}
VS::get_singleton()->mesh_add_surface_from_arrays(mesh, VS::PRIMITIVE_TRIANGLES, arr);
pending_update = false;
}
Rect3 GIProbe::get_aabb() const {
return Rect3(-extents,extents*2);
}
void GIProbe::bake(Node *p_from_node, bool p_create_visual_debug){
Baker baker;
static const int subdiv_value[SUBDIV_MAX]={7,8,9,10};
baker.cell_subdiv=subdiv_value[subdiv];
baker.bake_cells.resize(1);
//find out the actual real bounds, power of 2, which gets the highest subdivision
baker.po2_bounds=Rect3(-extents,extents*2.0);
int longest_axis = baker.po2_bounds.get_longest_axis_index();
baker.axis_cell_size[longest_axis]=(1<<(baker.cell_subdiv-1));
baker.leaf_voxel_count=0;
for(int i=0;i<3;i++) {
if (i==longest_axis)
continue;
baker.axis_cell_size[i]=baker.axis_cell_size[longest_axis];
float axis_size = baker.po2_bounds.size[longest_axis];
//shrink until fit subdiv
while (axis_size/2.0 >= baker.po2_bounds.size[i]) {
axis_size/=2.0;
baker.axis_cell_size[i]>>=1;
}
baker.po2_bounds.size[i]=baker.po2_bounds.size[longest_axis];
}
Transform to_bounds;
to_bounds.basis.scale(Vector3(baker.po2_bounds.size[longest_axis],baker.po2_bounds.size[longest_axis],baker.po2_bounds.size[longest_axis]));
to_bounds.origin=baker.po2_bounds.pos;
Transform to_grid;
to_grid.basis.scale(Vector3(baker.axis_cell_size[longest_axis],baker.axis_cell_size[longest_axis],baker.axis_cell_size[longest_axis]));
baker.to_cell_space = to_grid * to_bounds.affine_inverse();
_find_meshes(p_from_node?p_from_node:get_parent(),&baker);
int pmc=0;
for(List<Baker::PlotMesh>::Element *E=baker.mesh_list.front();E;E=E->next()) {
print_line("plotting mesh "+itos(pmc++)+"/"+itos(baker.mesh_list.size()));
_plot_mesh(E->get().local_xform,E->get().mesh,&baker,E->get().instance_materials,E->get().override_material);
}
_fixup_plot(0,0,0,0,0,&baker);
//create the data for visual server
PoolVector<int> data;
data.resize( 16+(8+1+1+1+1)*baker.bake_cells.size() ); //4 for header, rest for rest.
{
PoolVector<int>::Write w = data.write();
uint32_t * w32 = (uint32_t*)w.ptr();
w32[0]=0;//version
w32[1]=baker.cell_subdiv; //subdiv
w32[2]=baker.axis_cell_size[0];
w32[3]=baker.axis_cell_size[1];
w32[4]=baker.axis_cell_size[2];
w32[5]=baker.bake_cells.size();
w32[6]=baker.leaf_voxel_count;
int ofs=16;
for(int i=0;i<baker.bake_cells.size();i++) {
for(int j=0;j<8;j++) {
w32[ofs++]=baker.bake_cells[i].childs[j];
}
{ //albedo
uint32_t rgba=uint32_t(CLAMP(baker.bake_cells[i].albedo[0]*255.0,0,255))<<16;
rgba|=uint32_t(CLAMP(baker.bake_cells[i].albedo[1]*255.0,0,255))<<8;
rgba|=uint32_t(CLAMP(baker.bake_cells[i].albedo[2]*255.0,0,255))<<0;
w32[ofs++]=rgba;
}
{ //emission
Vector3 e(baker.bake_cells[i].emission[0],baker.bake_cells[i].emission[1],baker.bake_cells[i].emission[2]);
float l = e.length();
if (l>0) {
e.normalize();
l=CLAMP(l/8.0,0,1.0);
}
uint32_t em=uint32_t(CLAMP(e[0]*255,0,255))<<24;
em|=uint32_t(CLAMP(e[1]*255,0,255))<<16;
em|=uint32_t(CLAMP(e[2]*255,0,255))<<8;
em|=uint32_t(CLAMP(l*255,0,255));
w32[ofs++]=em;
}
//w32[ofs++]=baker.bake_cells[i].used_sides;
{ //normal
Vector3 n(baker.bake_cells[i].normal[0],baker.bake_cells[i].normal[1],baker.bake_cells[i].normal[2]);
n=n*Vector3(0.5,0.5,0.5)+Vector3(0.5,0.5,0.5);
uint32_t norm=0;
norm|=uint32_t(CLAMP( n.x*255.0, 0, 255))<<16;
norm|=uint32_t(CLAMP( n.y*255.0, 0, 255))<<8;
norm|=uint32_t(CLAMP( n.z*255.0, 0, 255))<<0;
w32[ofs++]=norm;
}
{
uint16_t alpha = CLAMP(uint32_t(baker.bake_cells[i].alpha*65535.0),0,65535);
uint16_t level = baker.bake_cells[i].level;
w32[ofs++] = (uint32_t(level)<<16)|uint32_t(alpha);
}
}
}
Ref<GIProbeData> probe_data;
probe_data.instance();
probe_data->set_bounds(Rect3(-extents,extents*2.0));
probe_data->set_cell_size(baker.po2_bounds.size[longest_axis]/baker.axis_cell_size[longest_axis]);
probe_data->set_dynamic_data(data);
probe_data->set_dynamic_range(dynamic_range);
probe_data->set_energy(energy);
probe_data->set_interior(interior);
probe_data->set_compress(compress);
probe_data->set_to_cell_xform(baker.to_cell_space);
set_probe_data(probe_data);
if (p_create_visual_debug) {
//_create_debug_mesh(&baker);
}
}
void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) {
int src_face_count = p_faces.size();
if (src_face_count == 0) {
configure(Rect3());
return;
}
ERR_FAIL_COND(src_face_count % 3);
src_face_count /= 3;
PoolVector<Vector3>::Read r = p_faces.read();
const Vector3 *facesr = r.ptr();
#if 0
Map<Vector3,int> point_map;
List<Face> face_list;
for(int i=0;i<src_face_count;i++) {
Face3 faceaux;
for(int j=0;j<3;j++) {
faceaux.vertex[j]=facesr[i*3+j].snapped(_POINT_SNAP);
//faceaux.vertex[j]=facesr[i*3+j];//facesr[i*3+j].snapped(_POINT_SNAP);
}
ERR_CONTINUE( faceaux.is_degenerate() );
Face face;
for(int j=0;j<3;j++) {
Map<Vector3,int>::Element *E=point_map.find(faceaux.vertex[j]);
if (E) {
face.indices[j]=E->value();
} else {
face.indices[j]=point_map.size();
point_map.insert(faceaux.vertex[j],point_map.size());
}
}
face_list.push_back(face);
}
vertices.resize( point_map.size() );
PoolVector<Vector3>::Write vw = vertices.write();
Vector3 *verticesw=vw.ptr();
AABB _aabb;
for( Map<Vector3,int>::Element *E=point_map.front();E;E=E->next()) {
if (E==point_map.front()) {
_aabb.pos=E->key();
} else {
_aabb.expand_to(E->key());
}
verticesw[E->value()]=E->key();
}
point_map.clear(); // not needed anymore
faces.resize(face_list.size());
PoolVector<Face>::Write w = faces.write();
Face *facesw=w.ptr();
int fc=0;
for( List<Face>::Element *E=face_list.front();E;E=E->next()) {
facesw[fc++]=E->get();
}
face_list.clear();
PoolVector<_VolumeSW_BVH_Element> bvh_array;
bvh_array.resize( fc );
PoolVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
for(int i=0;i<fc;i++) {
AABB face_aabb;
face_aabb.pos=verticesw[facesw[i].indices[0]];
face_aabb.expand_to( verticesw[facesw[i].indices[1]] );
face_aabb.expand_to( verticesw[facesw[i].indices[2]] );
bvh_arrayw[i].face_index=i;
bvh_arrayw[i].aabb=face_aabb;
bvh_arrayw[i].center=face_aabb.pos+face_aabb.size*0.5;
}
w=PoolVector<Face>::Write();
vw=PoolVector<Vector3>::Write();
int count=0;
_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,fc,count);
ERR_FAIL_COND(count==0);
bvhw=PoolVector<_VolumeSW_BVH_Element>::Write();
bvh.resize( count+1 );
PoolVector<BVH>::Write bvhw2 = bvh.write();
BVH*bvh_arrayw2=bvhw2.ptr();
int idx=0;
_fill_bvh(bvh_tree,bvh_arrayw2,idx);
set_aabb(_aabb);
#else
PoolVector<_VolumeSW_BVH_Element> bvh_array;
bvh_array.resize(src_face_count);
PoolVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
_VolumeSW_BVH_Element *bvh_arrayw = bvhw.ptr();
faces.resize(src_face_count);
PoolVector<Face>::Write w = faces.write();
Face *facesw = w.ptr();
vertices.resize(src_face_count * 3);
PoolVector<Vector3>::Write vw = vertices.write();
Vector3 *verticesw = vw.ptr();
Rect3 _aabb;
for (int i = 0; i < src_face_count; i++) {
Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]);
bvh_arrayw[i].aabb = face.get_aabb();
bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5;
bvh_arrayw[i].face_index = i;
facesw[i].indices[0] = i * 3 + 0;
facesw[i].indices[1] = i * 3 + 1;
facesw[i].indices[2] = i * 3 + 2;
facesw[i].normal = face.get_plane().normal;
verticesw[i * 3 + 0] = face.vertex[0];
verticesw[i * 3 + 1] = face.vertex[1];
verticesw[i * 3 + 2] = face.vertex[2];
if (i == 0)
_aabb = bvh_arrayw[i].aabb;
else
_aabb.merge_with(bvh_arrayw[i].aabb);
}
w = PoolVector<Face>::Write();
vw = PoolVector<Vector3>::Write();
int count = 0;
_VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count);
bvh.resize(count + 1);
PoolVector<BVH>::Write bvhw2 = bvh.write();
BVH *bvh_arrayw2 = bvhw2.ptr();
int idx = 0;
_fill_bvh(bvh_tree, bvh_arrayw2, idx);
configure(_aabb); // this type of shape has no margin
#endif
}
void RayShapeSW::_setup(real_t p_length) {
length = p_length;
configure(Rect3(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
}
bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const Rect3 &p_concave_hint, Vector3 *r_sep_axis) {
if (p_shape_A->is_concave())
return false;
if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) {
Vector3 a, b;
bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
r_point_A = b;
r_point_B = a;
return !col;
} else if (p_shape_B->is_concave()) {
if (p_shape_A->is_concave())
return false;
const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
_ConcaveCollisionInfo cinfo;
cinfo.transform_A = &p_transform_A;
cinfo.shape_A = p_shape_A;
cinfo.transform_B = &p_transform_B;
cinfo.result_callback = NULL;
cinfo.userdata = NULL;
cinfo.swap_result = false;
cinfo.collided = false;
cinfo.collisions = 0;
cinfo.aabb_tests = 0;
cinfo.tested = false;
Transform rel_transform = p_transform_A;
rel_transform.origin -= p_transform_B.origin;
//quickly compute a local AABB
bool use_cc_hint = p_concave_hint != Rect3();
Rect3 cc_hint_aabb;
if (use_cc_hint) {
cc_hint_aabb = p_concave_hint;
cc_hint_aabb.position -= p_transform_B.origin;
}
Rect3 local_aabb;
for (int i = 0; i < 3; i++) {
Vector3 axis(p_transform_B.basis.get_axis(i));
real_t axis_scale = ((real_t)1.0) / axis.length();
axis *= axis_scale;
real_t smin, smax;
if (use_cc_hint) {
cc_hint_aabb.project_range_in_plane(Plane(axis, 0), smin, smax);
} else {
p_shape_A->project_range(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_distance_callback, &cinfo);
if (!cinfo.collided) {
//print_line(itos(cinfo.tested));
r_point_A = cinfo.close_A;
r_point_B = cinfo.close_B;
}
//print_line("DIST AABB TESTS: "+itos(cinfo.aabb_tests));
return !cinfo.collided;
} else {
return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis..
}
return false;
}
void BoxShapeSW::_setup(const Vector3 &p_half_extents) {
half_extents = p_half_extents.abs();
configure(Rect3(-half_extents, half_extents * 2));
}
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;
}
FaceShapeSW::FaceShapeSW() {
configure(Rect3());
}
void rgl_dev_open(int* successptr, double* rect)
{
*successptr = as_success( deviceManager && deviceManager->openDevice(Rect3(rect[0], rect[1], rect[2], rect[3])) );
CHECKGLERROR;
}
