Add OMI_physics_joint extension

README.md

@@ -6,14 +6,15 @@ This repository is used by the [Open Metaverse Interoperability Group](https://o
 
 Extensions implemented in this repository:
 
-| Extension name      | Import | Export | Godot version | Link                                                                                                                   |
-| ------------------- | ------ | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------- |
-| **OMI_seat**        | Yes    | Yes    | 4.0+          | [OMI_seat extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_seat)               |
-| **OMI_spawn_point** | Yes    | No     | 4.0+          | [OMI_spawn_point extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_spawn_point) |
+| Extension name        | Import | Export | Godot version | Link                                                                                                                       |
+| --------------------- | ------ | ------ | ------------- | -------------------------------------------------------------------------------------------------------------------------- |
+| **OMI_seat**          | Yes    | Yes    | 4.0+          | [OMI_seat extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_seat)                   |
+| **OMI_spawn_point**   | Yes    | No     | 4.0+          | [OMI_spawn_point extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_spawn_point)     |
+| **OMI_physics_joint** | Yes    | Yes    | 4.1+          | [OMI_physics_joint extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_physics_joint) |
 
 Extensions implemented upstream in Godot Engine:
 
 | Extension name       | Import | Export | Godot version | Link                                                                                                                     |
 | -------------------- | ------ | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------ |
-| **OMI_collider**     | Yes    | Yes    | 4.1+          | [OMI_collider extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_collider)         |
-| **OMI_physics_body** | Yes    | Yes    | 4.1+          | [OMI_physics_body extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_physics_body) |
+| **OMI_collider**     | Yes    | Yes    | 4.1+ or 3.6+  | [OMI_collider extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_collider)         |
+| **OMI_physics_body** | Yes    | Yes    | 4.1+ or 3.6+  | [OMI_physics_body extension spec](https://github.com/omigroup/gltf-extensions/tree/main/extensions/2.0/OMI_physics_body) |

addons/omi_extensions/omi_extensions_plugin.gd

@@ -9,3 +9,5 @@ func _enter_tree() -> void:
 	GLTFDocument.register_gltf_document_extension(ext)
 	ext = GLTFDocumentExtensionOMISpawnPoint.new()
 	GLTFDocument.register_gltf_document_extension(ext)
+	ext = GLTFDocumentExtensionOMIPhysicsJoint.new()
+	GLTFDocument.register_gltf_document_extension(ext)

addons/omi_extensions/physics_joint/gltf_physics_joint_constraint.gd

@@ -0,0 +1,66 @@
+@tool
+class_name GLTFPhysicsJointConstraint
+extends Resource
+
+
+var linear_axes: Array = []
+var angular_axes: Array = []
+var lower_limit: float = 0.0
+var upper_limit: float = 0.0
+var stiffness: float = INF
+var damping: float = 1.0
+
+
+func to_dictionary() -> Dictionary:
+	var ret: Dictionary = {}
+	if not linear_axes.is_empty():
+		ret["linearAxes"] = linear_axes
+	if not angular_axes.is_empty():
+		ret["angularAxes"] = angular_axes
+	if lower_limit != 0.0:
+		ret["lowerLimit"] = lower_limit
+	if upper_limit != 0.0:
+		ret["upperLimit"] = upper_limit
+	if stiffness != INF:
+		ret["stiffness"] = stiffness
+	if damping != 1.0:
+		ret["damping"] = damping
+	return ret
+
+
+static func from_dictionary(joint_dict: Dictionary) -> GLTFPhysicsJointConstraint:
+	var ret = GLTFPhysicsJointConstraint.new()
+	if joint_dict.has("linearAxes"):
+		var dict_axes: Array = joint_dict["linearAxes"]
+		for dict_axis in dict_axes:
+			ret.linear_axes.append(int(dict_axis))
+	if joint_dict.has("angularAxes"):
+		var dict_axes: Array = joint_dict["angularAxes"]
+		for dict_axis in dict_axes:
+			ret.angular_axes.append(int(dict_axis))
+	if joint_dict.has("lowerLimit"):
+		ret.lower_limit = joint_dict["lowerLimit"]
+	if joint_dict.has("upperLimit"):
+		ret.upper_limit = joint_dict["upperLimit"]
+	if joint_dict.has("stiffness"):
+		ret.stiffness = joint_dict["stiffness"]
+	if joint_dict.has("damping"):
+		ret.damping = joint_dict["damping"]
+	return ret
+
+
+func is_fixed_at_zero() -> bool:
+	return is_zero_approx(lower_limit) and is_zero_approx(upper_limit)
+
+
+func is_equal_to(other: GLTFPhysicsJointConstraint) -> bool:
+	return limits_equal_to(other) \
+			and linear_axes.hash() == other.linear_axes.hash() \
+			and angular_axes.hash() == other.angular_axes.hash()
+
+
+func limits_equal_to(other: GLTFPhysicsJointConstraint) -> bool:
+	return is_equal_approx(lower_limit, other.lower_limit) \
+			and is_equal_approx(upper_limit, other.upper_limit) \
+			and is_equal_approx(stiffness, other.stiffness) \
+			and is_equal_approx(damping, other.damping)

addons/omi_extensions/physics_joint/omi_physics_joint_doc_ext.gd

@@ -0,0 +1,172 @@
+@tool
+class_name GLTFDocumentExtensionOMIPhysicsJoint
+extends GLTFDocumentExtension
+
+
+# Import process.
+func _import_preflight(state: GLTFState, extensions: PackedStringArray) -> Error:
+	if not extensions.has("OMI_physics_joint"):
+		return ERR_SKIP
+	var state_json = state.get_json()
+	if not state_json.has("extensions"):
+		return ERR_FILE_CORRUPT
+	var state_extensions: Dictionary = state_json["extensions"]
+	if not state_extensions.has("OMI_physics_joint"):
+		return ERR_FILE_CORRUPT
+	var omi_physics_joint_doc_ext: Dictionary = state_extensions["OMI_physics_joint"]
+	if not omi_physics_joint_doc_ext.has("constraints"):
+		return ERR_FILE_CORRUPT
+	var state_constraint_dicts: Array = omi_physics_joint_doc_ext["constraints"]
+	var state_constraints: Array = []
+	for constraint_dict in state_constraint_dicts:
+		state_constraints.append(GLTFPhysicsJointConstraint.from_dictionary(constraint_dict))
+	state.set_additional_data("GLTFPhysicsJointConstraints", state_constraints)
+	return OK
+
+
+func _get_supported_extensions() -> PackedStringArray:
+	return PackedStringArray(["OMI_physics_joint"])
+
+
+func _parse_node_extensions(state: GLTFState, gltf_node: GLTFNode, extensions: Dictionary) -> Error:
+	if not extensions.has("OMI_physics_joint"):
+		return OK
+	var joint_dict = extensions.get("OMI_physics_joint")
+	if not joint_dict is Dictionary:
+		printerr("Error: OMI_physics_joint extension should be a Dictionary.")
+		return ERR_FILE_CORRUPT
+	var constraints = joint_dict.get("constraints")
+	if not constraints is Array or constraints.is_empty():
+		printerr("Error: OMI_physics_joint extension should have at least one constraint.")
+		return ERR_FILE_CORRUPT
+	var state_constraints: Array = state.get_additional_data("GLTFPhysicsJointConstraints")
+	var joint := GLTFPhysicsJoint.new()
+	for constraint in constraints:
+		var joint_constraint: GLTFPhysicsJointConstraint
+		if constraint is float: # Remember, JSON only stores "number".
+			joint_constraint = state_constraints[int(constraint)]
+		else:
+			joint_constraint = GLTFPhysicsJointConstraint.from_dictionary(constraint)
+		joint.apply_constraint(joint_constraint)
+	gltf_node.set_additional_data("GLTFPhysicsJoint", joint)
+	return OK
+
+
+func _generate_scene_node(state: GLTFState, gltf_node: GLTFNode, scene_parent: Node) -> Node3D:
+	var joint: GLTFPhysicsJoint = gltf_node.get_additional_data("GLTFPhysicsJoint")
+	if joint == null:
+		return null
+	return joint.to_node()
+
+
+func _import_node(state: GLTFState, _gltf_node: GLTFNode, json: Dictionary, node: Node) -> Error:
+	if not json.has("extensions"):
+		return OK
+	var extensions = json.get("extensions")
+	if not extensions.has("OMI_physics_joint"):
+		return OK
+	var joint_dict = extensions.get("OMI_physics_joint")
+	if not joint_dict is Dictionary:
+		printerr("Error: OMI_physics_joint extension should be a Dictionary.")
+		return ERR_FILE_CORRUPT
+	if not joint_dict.has("nodeA") or not joint_dict.has("nodeB"):
+		printerr("Error: OMI_physics_joint extension should have nodeA and nodeB.")
+		return ERR_FILE_CORRUPT
+	var joint_node: Joint3D = node as Joint3D
+	var node_a_index: int = int(joint_dict["nodeA"])
+	if node_a_index != -1:
+		var node_a: Node = state.get_scene_node(node_a_index)
+		if not node_a is PhysicsBody3D:
+			printerr("Error: OMI_physics_joint nodeA should be a physics body (non-trigger).")
+			return ERR_FILE_CORRUPT
+		joint_node.node_a = joint_node.get_path_to(node_a)
+	var node_b_index: int = int(joint_dict["nodeB"])
+	if node_b_index != -1:
+		var node_b: Node = state.get_scene_node(node_b_index)
+		if not node_b is PhysicsBody3D:
+			printerr("Error: OMI_physics_joint nodeB should be a physics body (non-trigger).")
+			return ERR_FILE_CORRUPT
+		joint_node.node_b = joint_node.get_path_to(node_b)
+	return OK
+
+
+# Export process.
+func _convert_scene_node(state: GLTFState, gltf_node: GLTFNode, scene_node: Node) -> void:
+	if not scene_node is Joint3D:
+		return
+	var joint := GLTFPhysicsJoint.from_node(scene_node)
+	gltf_node.set_additional_data("GLTFPhysicsJoint", joint)
+
+
+func _get_or_create_state_constraints_in_state(state: GLTFState) -> Array:
+	var state_json = state.get_json()
+	var state_extensions: Dictionary
+	if state_json.has("extensions"):
+		state_extensions = state_json["extensions"]
+	else:
+		state_extensions = {}
+		state_json["extensions"] = state_extensions
+	var omi_physics_joint_doc_ext: Dictionary
+	if state_extensions.has("OMI_physics_joint"):
+		omi_physics_joint_doc_ext = state_extensions["OMI_physics_joint"]
+	else:
+		omi_physics_joint_doc_ext = {}
+		state_extensions["OMI_physics_joint"] = omi_physics_joint_doc_ext
+		state.add_used_extension("OMI_physics_joint", false)
+	var state_constraints: Array
+	if omi_physics_joint_doc_ext.has("constraints"):
+		state_constraints = omi_physics_joint_doc_ext["constraints"]
+	else:
+		state_constraints = []
+		omi_physics_joint_doc_ext["constraints"] = state_constraints
+	return state_constraints
+
+
+func _get_or_insert_constraint_in_state(state: GLTFState, constraint: GLTFPhysicsJointConstraint) -> int:
+	var state_constraints: Array = _get_or_create_state_constraints_in_state(state)
+	var size: int = state_constraints.size()
+	var constraint_dict: Dictionary = constraint.to_dictionary()
+	for i in range(size):
+		var other: Dictionary = state_constraints[i]
+		if other == constraint_dict:
+			# De-duplication: If we already have an identical constraint,
+			# return the index of the existing constraint.
+			return i
+	# If we don't have an identical constraint, add it to the array.
+	state_constraints.push_back(constraint_dict)
+	return size
+
+
+func _node_index_from_scene_node(state: GLTFState, scene_node: Node) -> int:
+	var index: int = 0
+	var node: Node = state.get_scene_node(index)
+	while node != null:
+		if node == scene_node:
+			return index
+		index = index + 1
+		node = state.get_scene_node(index)
+	return -1
+
+
+func _export_node(state: GLTFState, gltf_node: GLTFNode, json: Dictionary, _node: Node) -> Error:
+	var gltf_physics_joint: GLTFPhysicsJoint = gltf_node.get_additional_data("GLTFPhysicsJoint")
+	if gltf_physics_joint == null:
+		return OK
+	var node_extensions = json["extensions"]
+	if not node_extensions is Dictionary:
+		node_extensions = {}
+		json["extensions"] = node_extensions
+	var omi_physics_joint_node_ext: Dictionary = {}
+	# Populate the constraints.
+	var constraints: Array = gltf_physics_joint.get_constraints()
+	var constraint_indices: Array[int] = []
+	for constraint in constraints:
+		var index: int = _get_or_insert_constraint_in_state(state, constraint)
+		if not index in constraint_indices:
+			constraint_indices.append(index)
+	omi_physics_joint_node_ext["constraints"] = constraint_indices
+	# Populate the node references.
+	omi_physics_joint_node_ext["nodeA"] = _node_index_from_scene_node(state, gltf_physics_joint.node_a)
+	omi_physics_joint_node_ext["nodeB"] = _node_index_from_scene_node(state, gltf_physics_joint.node_b)
+	node_extensions["OMI_physics_joint"] = omi_physics_joint_node_ext
+	return OK