From bd531aa6c2996a588573403579645ed72dbf5de3 Mon Sep 17 00:00:00 2001
From: Wojtek Figat <wojtek@figat.pl>
Date: Wed, 3 Nov 2021 12:24:41 +0100
Subject: [PATCH] Add `Joint.SetJointOrientation` utility

---
 Source/Engine/Physics/Joints/Joint.cpp | 22 ++++++++++++++++++++++
 Source/Engine/Physics/Joints/Joint.h   |  7 +++++++
 2 files changed, 29 insertions(+)

diff --git a/Source/Engine/Physics/Joints/Joint.cpp b/Source/Engine/Physics/Joints/Joint.cpp
index f9808efc6..79ef9d4d5 100644
--- a/Source/Engine/Physics/Joints/Joint.cpp
+++ b/Source/Engine/Physics/Joints/Joint.cpp
@@ -101,6 +101,28 @@ void Joint::SetJointLocation(const Vector3& location)
     }
 }
 
+FORCE_INLINE Quaternion WorldToLocal(const Quaternion& world, const Quaternion& orientation)
+{
+    Quaternion rot;
+    const Quaternion invRotation = world.Conjugated();
+    Quaternion::Multiply(invRotation, orientation, rot);
+    rot.Normalize();
+    return rot;
+}
+
+void Joint::SetJointOrientation(const Quaternion& orientation)
+{
+    if (GetParent())
+    {
+        SetLocalOrientation(WorldToLocal(GetParent()->GetOrientation(), orientation));
+    }
+    if (Target)
+    {
+        //SetTargetAnchor(Target->GetTransform().WorldToLocal(orientation));
+        SetTargetAnchorRotation(WorldToLocal(Target->GetOrientation(), orientation));
+    }
+}
+
 void Joint::GetCurrentForce(Vector3& linear, Vector3& angular) const
 {
     if (_joint && _joint->getConstraint())
diff --git a/Source/Engine/Physics/Joints/Joint.h b/Source/Engine/Physics/Joints/Joint.h
index 95d33c421..046570462 100644
--- a/Source/Engine/Physics/Joints/Joint.h
+++ b/Source/Engine/Physics/Joints/Joint.h
@@ -135,6 +135,13 @@ public:
     /// <param name="location">The joint location to set (world-space).</param>
     API_FUNCTION() void SetJointLocation(const Vector3& location);
 
+    /// <summary>
+    /// Sets the orientation of the joint by automatically computing local orientation and target anchor orientation to orient a joint at the given rotation (world-space).
+    /// </summary>
+    /// <remarks>Use this utility to automatically rotate joint at the given location after setting up joint parent and target.</remarks>
+    /// <param name="orientation">The joint orientation to set (world-space).</param>
+    API_FUNCTION() void SetJointOrientation(const Quaternion& orientation);
+
     /// <summary>
     /// Gets the current force applied by the solver to maintain all constraints.
     /// </summary>