From 6ce8b9e22fc13be34b442c7b6af48b42cd44275a Mon Sep 17 00:00:00 2001
From: chai <215380520@qq.com>
Date: Wed, 13 Mar 2024 11:00:58 +0800
Subject: +init

---
 ConfigurableJointExtensions.cs | 41 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 41 insertions(+)
 create mode 100644 ConfigurableJointExtensions.cs

(limited to 'ConfigurableJointExtensions.cs')

diff --git a/ConfigurableJointExtensions.cs b/ConfigurableJointExtensions.cs
new file mode 100644
index 0000000..1fcbf91
--- /dev/null
+++ b/ConfigurableJointExtensions.cs
@@ -0,0 +1,41 @@
+using UnityEngine;
+
+public static class ConfigurableJointExtensions
+{
+	public static void SetTargetRotationLocal(this ConfigurableJoint joint, Quaternion targetLocalRotation, Quaternion startLocalRotation)
+	{
+		if (joint.configuredInWorldSpace)
+		{
+			Debug.LogError("SetTargetRotationLocal should not be used with joints that are configured in world space. For world space joints, use SetTargetRotation.", joint);
+		}
+		joint.SetTargetRotationInternal(targetLocalRotation, startLocalRotation, Space.Self);
+	}
+
+	public static void SetTargetRotation(this ConfigurableJoint joint, Quaternion targetWorldRotation, Quaternion startWorldRotation)
+	{
+		if (!joint.configuredInWorldSpace)
+		{
+			Debug.LogError("SetTargetRotation must be used with joints that are configured in world space. For local space joints, use SetTargetRotationLocal.", joint);
+		}
+		joint.SetTargetRotationInternal(targetWorldRotation, startWorldRotation, Space.World);
+	}
+
+	private static void SetTargetRotationInternal(this ConfigurableJoint joint, Quaternion targetRotation, Quaternion startRotation, Space space)
+	{
+		Vector3 axis = joint.axis;
+		Vector3 normalized = Vector3.Cross(joint.axis, joint.secondaryAxis).normalized;
+		Vector3 normalized2 = Vector3.Cross(normalized, axis).normalized;
+		Quaternion quaternion = Quaternion.LookRotation(normalized, normalized2);
+		Quaternion targetRotation2 = Quaternion.Inverse(quaternion);
+		if (space == Space.World)
+		{
+			targetRotation2 *= startRotation * Quaternion.Inverse(targetRotation);
+		}
+		else
+		{
+			targetRotation2 *= Quaternion.Inverse(targetRotation) * startRotation;
+		}
+		targetRotation2 *= quaternion;
+		joint.targetRotation = targetRotation2;
+	}
+}
-- 
cgit v1.1-26-g67d0