Initialer Upload neues Unity-Projekt

Assets/SteamVR/Scripts/SteamVR_IK.cs

@@ -0,0 +1,170 @@
+//======= Copyright (c) Valve Corporation, All rights reserved. ===============
+//
+// Purpose: Simple two bone ik solver.
+//
+//=============================================================================
+
+using UnityEngine;
+
+public class SteamVR_IK : MonoBehaviour
+{
+	public Transform target;
+	public Transform start, joint, end;
+	public Transform poleVector, upVector;
+
+	public float blendPct = 1.0f;
+
+	[HideInInspector]
+	public Transform startXform, jointXform, endXform;
+
+	void LateUpdate()
+	{
+		const float epsilon = 0.001f;
+		if (blendPct < epsilon)
+			return;
+
+		var preUp = upVector ? upVector.up : Vector3.Cross(end.position - start.position, joint.position - start.position).normalized;
+
+		var targetPosition = target.position;
+		var targetRotation = target.rotation;
+
+		Vector3 forward, up, result = joint.position;
+		Solve(start.position, targetPosition, poleVector.position,
+			(joint.position - start.position).magnitude,
+			(end.position - joint.position).magnitude,
+			ref result, out forward, out up);
+
+		if (up == Vector3.zero)
+			return;
+
+		var startPosition = start.position;
+		var jointPosition = joint.position;
+		var endPosition = end.position;
+
+		var startRotationLocal = start.localRotation;
+		var jointRotationLocal = joint.localRotation;
+		var endRotationLocal = end.localRotation;
+
+		var startParent = start.parent;
+		var jointParent = joint.parent;
+		var endParent = end.parent;
+
+		var startScale = start.localScale;
+		var jointScale = joint.localScale;
+		var endScale = end.localScale;
+
+		if (startXform == null)
+		{
+			startXform = new GameObject("startXform").transform;
+			startXform.parent = transform;
+		}
+
+		startXform.position = startPosition;
+		startXform.LookAt(joint, preUp);
+		start.parent = startXform;
+
+		if (jointXform == null)
+		{
+			jointXform = new GameObject("jointXform").transform;
+			jointXform.parent = startXform;
+		}
+
+		jointXform.position = jointPosition;
+		jointXform.LookAt(end, preUp);
+		joint.parent = jointXform;
+
+		if (endXform == null)
+		{
+			endXform = new GameObject("endXform").transform;
+			endXform.parent = jointXform;
+		}
+
+		endXform.position = endPosition;
+		end.parent = endXform;
+
+		startXform.LookAt(result, up);
+		jointXform.LookAt(targetPosition, up);
+		endXform.rotation = targetRotation;
+
+		start.parent = startParent;
+		joint.parent = jointParent;
+		end.parent = endParent;
+
+		end.rotation = targetRotation; // optionally blend?
+
+		// handle blending in/out
+		if (blendPct < 1.0f)
+		{
+			start.localRotation = Quaternion.Slerp(startRotationLocal, start.localRotation, blendPct);
+			joint.localRotation = Quaternion.Slerp(jointRotationLocal, joint.localRotation, blendPct);
+			end.localRotation = Quaternion.Slerp(endRotationLocal, end.localRotation, blendPct);
+		}
+
+		// restore scale so it doesn't blow out
+		start.localScale = startScale;
+		joint.localScale = jointScale;
+		end.localScale = endScale;
+	}
+
+	public static bool Solve(
+		Vector3 start, // shoulder / hip
+		Vector3 end, // desired hand / foot position
+		Vector3 poleVector, // point to aim elbow / knee toward
+		float jointDist, // distance from start to elbow / knee
+		float targetDist, // distance from joint to hand / ankle
+		ref Vector3 result, // original and output elbow / knee position
+		out Vector3 forward, out Vector3 up) // plane formed by root, joint and target
+	{
+		var totalDist = jointDist + targetDist;
+		var start2end = end - start;
+		var poleVectorDir = (poleVector - start).normalized;
+		var baseDist = start2end.magnitude;
+
+		result = start;
+
+		const float epsilon = 0.001f;
+		if (baseDist < epsilon)
+		{
+			// move jointDist toward jointTarget
+			result += poleVectorDir * jointDist;
+
+			forward = Vector3.Cross(poleVectorDir, Vector3.up);
+			up = Vector3.Cross(forward, poleVectorDir).normalized;
+		}
+		else
+		{
+			forward = start2end * (1.0f / baseDist);
+			up = Vector3.Cross(forward, poleVectorDir).normalized;
+
+			if (baseDist + epsilon < totalDist)
+			{
+				// calculate the area of the triangle to determine its height
+				var p = (totalDist + baseDist) * 0.5f; // half perimeter
+				if (p > jointDist + epsilon && p > targetDist + epsilon)
+				{
+					var A = Mathf.Sqrt(p * (p - jointDist) * (p - targetDist) * (p - baseDist));
+					var height = 2.0f * A / baseDist; // distance of joint from line between root and target
+
+					var dist = Mathf.Sqrt((jointDist * jointDist) - (height * height));
+					var right = Vector3.Cross(up, forward); // no need to normalized - already orthonormal
+
+					result += (forward * dist) + (right * height);
+					return true; // in range
+				}
+				else
+				{
+					// move jointDist toward jointTarget
+					result += poleVectorDir * jointDist;
+				}
+			}
+			else
+			{
+				// move elboDist toward target
+				result += forward * jointDist;
+			}
+		}
+
+		return false; // edge cases
+	}
+}
+