Expose Basis `set_orthogonal_index` method as a GridMap function

4 files changed, 130 insertions, 34 deletions

diff --git a/modules/gridmap/doc_classes/GridMap.xml b/modules/gridmap/doc_classes/GridMap.xml
index 5552b5b009..209890a333 100644
--- a/modules/gridmap/doc_classes/GridMap.xml
+++ b/modules/gridmap/doc_classes/GridMap.xml
@@ -39,6 +39,13 @@
 				Returns an array of [ArrayMesh]es and [Transform3D] references of all bake meshes that exist within the current GridMap.
 			</description>
 		</method>
+		<method name="get_basis_with_orthogonal_index" qualifiers="const">
+			<return type="Basis" />
+			<param index="0" name="index" type="int" />
+			<description>
+				Returns one of 24 possible rotations that lie along the vectors (x,y,z) with each component being either -1, 0, or 1. For further details, refer to the Godot source code.
+			</description>
+		</method>
 		<method name="get_cell_item" qualifiers="const">
 			<return type="int" />
 			<param index="0" name="position" type="Vector3i" />
@@ -46,6 +53,13 @@
 				The [MeshLibrary] item index located at the given grid coordinates. If the cell is empty, [constant INVALID_CELL_ITEM] will be returned.
 			</description>
 		</method>
+		<method name="get_cell_item_basis" qualifiers="const">
+			<return type="Basis" />
+			<param index="0" name="position" type="Vector3i" />
+			<description>
+				Returns the basis that gives the specificed cell its orientation.
+			</description>
+		</method>
 		<method name="get_cell_item_orientation" qualifiers="const">
 			<return type="int" />
 			<param index="0" name="position" type="Vector3i" />
@@ -80,6 +94,13 @@
 				Returns whether or not the specified layer of the [member navigation_layers] bitmask is enabled, given a [code]layer_number[/code] between 1 and 32.
 			</description>
 		</method>
+		<method name="get_orthogonal_index_from_basis" qualifiers="const">
+			<return type="int" />
+			<param index="0" name="basis" type="Basis" />
+			<description>
+				This function considers a discretization of rotations into 24 points on unit sphere, lying along the vectors (x,y,z) with each component being either -1, 0, or 1, and returns the index (in the range from 0 to 23) of the point best representing the orientation of the object. For further details, refer to the Godot source code.
+			</description>
+		</method>
 		<method name="get_used_cells" qualifiers="const">
 			<return type="Array" />
 			<description>
@@ -121,7 +142,7 @@
 			<description>
 				Sets the mesh index for the cell referenced by its grid coordinates.
 				A negative item index such as [constant INVALID_CELL_ITEM] will clear the cell.
-				Optionally, the item's orientation can be passed. For valid orientation values, see [method Basis.get_orthogonal_index].
+				Optionally, the item's orientation can be passed. For valid orientation values, see [method get_orthogonal_index_from_basis].
 			</description>
 		</method>
 		<method name="set_collision_layer_value">
diff --git a/modules/gridmap/editor/grid_map_editor_plugin.cpp b/modules/gridmap/editor/grid_map_editor_plugin.cpp
index 518e2cf97d..7471bae093 100644
--- a/modules/gridmap/editor/grid_map_editor_plugin.cpp
+++ b/modules/gridmap/editor/grid_map_editor_plugin.cpp
@@ -94,91 +94,91 @@ void GridMapEditor::_menu_option(int p_option) {
 		case MENU_OPTION_CURSOR_ROTATE_Y: {
 			Basis r;
 			if (input_action == INPUT_PASTE) {
-				r.set_orthogonal_index(paste_indicator.orientation);
+				r = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 				r.rotate(Vector3(0, 1, 0), -Math_PI / 2.0);
-				paste_indicator.orientation = r.get_orthogonal_index();
+				paste_indicator.orientation = node->get_orthogonal_index_from_basis(r);
 				_update_paste_indicator();
 				break;
 			}
 
-			r.set_orthogonal_index(cursor_rot);
+			r = node->get_basis_with_orthogonal_index(cursor_rot);
 			r.rotate(Vector3(0, 1, 0), -Math_PI / 2.0);
-			cursor_rot = r.get_orthogonal_index();
+			cursor_rot = node->get_orthogonal_index_from_basis(r);
 			_update_cursor_transform();
 		} break;
 		case MENU_OPTION_CURSOR_ROTATE_X: {
 			Basis r;
 			if (input_action == INPUT_PASTE) {
-				r.set_orthogonal_index(paste_indicator.orientation);
+				r = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 				r.rotate(Vector3(1, 0, 0), -Math_PI / 2.0);
-				paste_indicator.orientation = r.get_orthogonal_index();
+				paste_indicator.orientation = node->get_orthogonal_index_from_basis(r);
 				_update_paste_indicator();
 				break;
 			}
 
-			r.set_orthogonal_index(cursor_rot);
+			r = node->get_basis_with_orthogonal_index(cursor_rot);
 			r.rotate(Vector3(1, 0, 0), -Math_PI / 2.0);
-			cursor_rot = r.get_orthogonal_index();
+			cursor_rot = node->get_orthogonal_index_from_basis(r);
 			_update_cursor_transform();
 		} break;
 		case MENU_OPTION_CURSOR_ROTATE_Z: {
 			Basis r;
 			if (input_action == INPUT_PASTE) {
-				r.set_orthogonal_index(paste_indicator.orientation);
+				r = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 				r.rotate(Vector3(0, 0, 1), -Math_PI / 2.0);
-				paste_indicator.orientation = r.get_orthogonal_index();
+				paste_indicator.orientation = node->get_orthogonal_index_from_basis(r);
 				_update_paste_indicator();
 				break;
 			}
 
-			r.set_orthogonal_index(cursor_rot);
+			r = node->get_basis_with_orthogonal_index(cursor_rot);
 			r.rotate(Vector3(0, 0, 1), -Math_PI / 2.0);
-			cursor_rot = r.get_orthogonal_index();
+			cursor_rot = node->get_orthogonal_index_from_basis(r);
 			_update_cursor_transform();
 		} break;
 		case MENU_OPTION_CURSOR_BACK_ROTATE_Y: {
 			Basis r;
 			if (input_action == INPUT_PASTE) {
-				r.set_orthogonal_index(paste_indicator.orientation);
+				r = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 				r.rotate(Vector3(0, 1, 0), Math_PI / 2.0);
-				paste_indicator.orientation = r.get_orthogonal_index();
+				paste_indicator.orientation = node->get_orthogonal_index_from_basis(r);
 				_update_paste_indicator();
 				break;
 			}
 
-			r.set_orthogonal_index(cursor_rot);
+			r = node->get_basis_with_orthogonal_index(cursor_rot);
 			r.rotate(Vector3(0, 1, 0), Math_PI / 2.0);
-			cursor_rot = r.get_orthogonal_index();
+			cursor_rot = node->get_orthogonal_index_from_basis(r);
 			_update_cursor_transform();
 		} break;
 		case MENU_OPTION_CURSOR_BACK_ROTATE_X: {
 			Basis r;
 			if (input_action == INPUT_PASTE) {
-				r.set_orthogonal_index(paste_indicator.orientation);
+				r = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 				r.rotate(Vector3(1, 0, 0), Math_PI / 2.0);
-				paste_indicator.orientation = r.get_orthogonal_index();
+				paste_indicator.orientation = node->get_orthogonal_index_from_basis(r);
 				_update_paste_indicator();
 				break;
 			}
 
-			r.set_orthogonal_index(cursor_rot);
+			r = node->get_basis_with_orthogonal_index(cursor_rot);
 			r.rotate(Vector3(1, 0, 0), Math_PI / 2.0);
-			cursor_rot = r.get_orthogonal_index();
+			cursor_rot = node->get_orthogonal_index_from_basis(r);
 			_update_cursor_transform();
 		} break;
 		case MENU_OPTION_CURSOR_BACK_ROTATE_Z: {
 			Basis r;
 			if (input_action == INPUT_PASTE) {
-				r.set_orthogonal_index(paste_indicator.orientation);
+				r = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 				r.rotate(Vector3(0, 0, 1), Math_PI / 2.0);
-				paste_indicator.orientation = r.get_orthogonal_index();
+				paste_indicator.orientation = node->get_orthogonal_index_from_basis(r);
 				_update_paste_indicator();
 				break;
 			}
 
-			r.set_orthogonal_index(cursor_rot);
+			r = node->get_basis_with_orthogonal_index(cursor_rot);
 			r.rotate(Vector3(0, 0, 1), Math_PI / 2.0);
-			cursor_rot = r.get_orthogonal_index();
+			cursor_rot = node->get_orthogonal_index_from_basis(r);
 			_update_cursor_transform();
 		} break;
 		case MENU_OPTION_CURSOR_CLEAR_ROTATION: {
@@ -242,7 +242,7 @@ void GridMapEditor::_menu_option(int p_option) {
 void GridMapEditor::_update_cursor_transform() {
 	cursor_transform = Transform3D();
 	cursor_transform.origin = cursor_origin;
-	cursor_transform.basis.set_orthogonal_index(cursor_rot);
+	cursor_transform.basis = node->get_basis_with_orthogonal_index(cursor_rot);
 	cursor_transform.basis *= node->get_cell_scale();
 	cursor_transform = node->get_global_transform() * cursor_transform;
 
@@ -543,7 +543,7 @@ void GridMapEditor::_update_paste_indicator() {
 	xf.scale(scale);
 	xf.origin = (paste_indicator.begin + (paste_indicator.current - paste_indicator.click) + center) * node->get_cell_size();
 	Basis rot;
-	rot.set_orthogonal_index(paste_indicator.orientation);
+	rot = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 	xf.basis = rot * xf.basis;
 	xf.translate_local((-center * node->get_cell_size()) / scale);
 
@@ -556,7 +556,7 @@ void GridMapEditor::_update_paste_indicator() {
 		xf.translate_local(item.grid_offset * node->get_cell_size());
 
 		Basis item_rot;
-		item_rot.set_orthogonal_index(item.orientation);
+		item_rot = node->get_basis_with_orthogonal_index(item.orientation);
 		xf.basis = item_rot * xf.basis * node->get_cell_scale();
 
 		RenderingServer::get_singleton()->instance_set_transform(item.instance, node->get_global_transform() * xf);
@@ -568,7 +568,7 @@ void GridMapEditor::_do_paste() {
 	bool reselect = options->get_popup()->is_item_checked(idx);
 
 	Basis rot;
-	rot.set_orthogonal_index(paste_indicator.orientation);
+	rot = node->get_basis_with_orthogonal_index(paste_indicator.orientation);
 
 	Vector3 ofs = paste_indicator.current - paste_indicator.click;
 	undo_redo->create_action(TTR("GridMap Paste Selection"));
@@ -577,10 +577,10 @@ void GridMapEditor::_do_paste() {
 		Vector3 position = rot.xform(item.grid_offset) + paste_indicator.begin + ofs;
 
 		Basis orm;
-		orm.set_orthogonal_index(item.orientation);
+		orm = node->get_basis_with_orthogonal_index(item.orientation);
 		orm = rot * orm;
 
-		undo_redo->add_do_method(node, "set_cell_item", position, item.cell_item, orm.get_orthogonal_index());
+		undo_redo->add_do_method(node, "set_cell_item", position, item.cell_item, node->get_orthogonal_index_from_basis(orm));
 		undo_redo->add_undo_method(node, "set_cell_item", position, node->get_cell_item(position), node->get_cell_item_orientation(position));
 	}
 
diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp
index 7d80cbef7c..6384446bce 100644
--- a/modules/gridmap/grid_map.cpp
+++ b/modules/gridmap/grid_map.cpp
@@ -428,6 +428,75 @@ int GridMap::get_cell_item_orientation(const Vector3i &p_position) const {
 	return cell_map[key].rot;
 }
 
+static const Basis _ortho_bases[24] = {
+	Basis(1, 0, 0, 0, 1, 0, 0, 0, 1),
+	Basis(0, -1, 0, 1, 0, 0, 0, 0, 1),
+	Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1),
+	Basis(0, 1, 0, -1, 0, 0, 0, 0, 1),
+	Basis(1, 0, 0, 0, 0, -1, 0, 1, 0),
+	Basis(0, 0, 1, 1, 0, 0, 0, 1, 0),
+	Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0),
+	Basis(0, 0, -1, -1, 0, 0, 0, 1, 0),
+	Basis(1, 0, 0, 0, -1, 0, 0, 0, -1),
+	Basis(0, 1, 0, 1, 0, 0, 0, 0, -1),
+	Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1),
+	Basis(0, -1, 0, -1, 0, 0, 0, 0, -1),
+	Basis(1, 0, 0, 0, 0, 1, 0, -1, 0),
+	Basis(0, 0, -1, 1, 0, 0, 0, -1, 0),
+	Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0),
+	Basis(0, 0, 1, -1, 0, 0, 0, -1, 0),
+	Basis(0, 0, 1, 0, 1, 0, -1, 0, 0),
+	Basis(0, -1, 0, 0, 0, 1, -1, 0, 0),
+	Basis(0, 0, -1, 0, -1, 0, -1, 0, 0),
+	Basis(0, 1, 0, 0, 0, -1, -1, 0, 0),
+	Basis(0, 0, 1, 0, -1, 0, 1, 0, 0),
+	Basis(0, 1, 0, 0, 0, 1, 1, 0, 0),
+	Basis(0, 0, -1, 0, 1, 0, 1, 0, 0),
+	Basis(0, -1, 0, 0, 0, -1, 1, 0, 0)
+};
+
+Basis GridMap::get_cell_item_basis(const Vector3i &p_position) const {
+	int orientation = get_cell_item_orientation(p_position);
+
+	if (orientation == -1) {
+		return Basis();
+	}
+
+	return get_basis_with_orthogonal_index(orientation);
+}
+
+Basis GridMap::get_basis_with_orthogonal_index(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, 24, Basis());
+
+	return _ortho_bases[p_index];
+}
+
+int GridMap::get_orthogonal_index_from_basis(const Basis &p_basis) const {
+	Basis orth = p_basis;
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			real_t v = orth[i][j];
+			if (v > 0.5) {
+				v = 1.0;
+			} else if (v < -0.5) {
+				v = -1.0;
+			} else {
+				v = 0;
+			}
+
+			orth[i][j] = v;
+		}
+	}
+
+	for (int i = 0; i < 24; i++) {
+		if (_ortho_bases[i] == orth) {
+			return i;
+		}
+	}
+
+	return 0;
+}
+
 Vector3i GridMap::world_to_map(const Vector3 &p_world_position) const {
 	Vector3 map_position = (p_world_position / cell_size).floor();
 	return Vector3i(map_position);
@@ -529,7 +598,7 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 
 		Transform3D xform;
 
-		xform.basis.set_orthogonal_index(c.rot);
+		xform.basis = _ortho_bases[c.rot];
 		xform.set_origin(cellpos * cell_size + ofs);
 		xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
 		if (baked_meshes.size() == 0) {
@@ -921,6 +990,9 @@ void GridMap::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_cell_item", "position", "item", "orientation"), &GridMap::set_cell_item, DEFVAL(0));
 	ClassDB::bind_method(D_METHOD("get_cell_item", "position"), &GridMap::get_cell_item);
 	ClassDB::bind_method(D_METHOD("get_cell_item_orientation", "position"), &GridMap::get_cell_item_orientation);
+	ClassDB::bind_method(D_METHOD("get_cell_item_basis", "position"), &GridMap::get_cell_item_basis);
+	ClassDB::bind_method(D_METHOD("get_basis_with_orthogonal_index", "index"), &GridMap::get_basis_with_orthogonal_index);
+	ClassDB::bind_method(D_METHOD("get_orthogonal_index_from_basis", "basis"), &GridMap::get_orthogonal_index_from_basis);
 
 	ClassDB::bind_method(D_METHOD("world_to_map", "world_position"), &GridMap::world_to_map);
 	ClassDB::bind_method(D_METHOD("map_to_world", "map_position"), &GridMap::map_to_world);
@@ -1025,7 +1097,7 @@ Array GridMap::get_meshes() const {
 
 		Transform3D xform;
 
-		xform.basis.set_orthogonal_index(E.value.rot);
+		xform.basis = _ortho_bases[E.value.rot];
 
 		xform.set_origin(cellpos * cell_size + ofs);
 		xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
@@ -1079,7 +1151,7 @@ void GridMap::make_baked_meshes(bool p_gen_lightmap_uv, float p_lightmap_uv_texe
 
 		Transform3D xform;
 
-		xform.basis.set_orthogonal_index(E.value.rot);
+		xform.basis = _ortho_bases[E.value.rot];
 		xform.set_origin(cellpos * cell_size + ofs);
 		xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
 
diff --git a/modules/gridmap/grid_map.h b/modules/gridmap/grid_map.h
index 078a1d9de5..00cebd35e9 100644
--- a/modules/gridmap/grid_map.h
+++ b/modules/gridmap/grid_map.h
@@ -263,6 +263,9 @@ public:
 	void set_cell_item(const Vector3i &p_position, int p_item, int p_rot = 0);
 	int get_cell_item(const Vector3i &p_position) const;
 	int get_cell_item_orientation(const Vector3i &p_position) const;
+	Basis get_cell_item_basis(const Vector3i &p_position) const;
+	Basis get_basis_with_orthogonal_index(int p_index) const;
+	int get_orthogonal_index_from_basis(const Basis &p_basis) const;
 
 	Vector3i world_to_map(const Vector3 &p_world_position) const;
 	Vector3 map_to_world(const Vector3i &p_map_position) const;