ObiRopeをXR Interaction ToolKitで掴む、を実装する

# 概要
ObiRopeをVRで登ってみたかったので、本記事では登るシステムの基礎となる、ObiRopeをXR Interaction Toolkitで掴む実装の仕方について説明します。

ObiRooeを掴む様子は以下のXの投稿で確認できます。

https://x.com/UnagiHuman/status/1739320357159649473?s=20
# 検証環境
Unity 2022.3.11f1
XRInteractionToolKit ver2.5.2
ObiRope 6.5.4
Meta Quest3

# ObiRope
このアセットはロープをUnityで使う上で鉄板のアセットです。
[AssetStore](https://assetstore.unity.com/packages/tools/physics/obi-rope-55579)
どういうアセットかは、Qiitaに良い記事があるので参照してください。
https://qiita.com/Sase/items/0debe55ad0d80374a36a
https://qiita.com/OKsaiyowa/items/2f5e9dff5992dd18a65c

# XRInteractionToolKitセットアップ
XRInteractionToolKitに関する説明は以下のサイトが詳しいです。
https://tech.framesynthesis.co.jp/unity/xr/

PackageManagerでXRInteractionToolKit(2.5.2)をインストールした後にSamplesからStartarAssetsをDownLoad & importしておきます。このStartarAssetsにあるXR Interaction Setup prefabを使います。なお、本記事のソースコードはver2.5.2を利用しているので、他のバージョンだと互換性が無いかもしれないです。

# XRInteractionToolKitにてObiRopeを掴む
順を追って説明していきます。

## ObiRopeを掴む為のオブジェクトの実装
ObiRopeはObiPhysicsという独自の物理演算で衝突判定を計算している為、Unityの物理演算で衝突判定をとることが出来ないので、そのままではXRInteractionToolkitでGrabする事はできません。

XRInteractionToolkitでObiRopeを掴むために、XRInteractionToolKitとObiropeのハブとなるオブジェクトを用意します。このオブジェクトは以下の動画の青い球のように、コントローラーの動きにしたがってロープを移動し、コントローラーで掴んだ後は後述のObiParticleAttachimentでロープを追従させます。

![obirope_projection](https://github.com/unagiHuman/ObiRopeXRInteractionToolKitSample/assets/17809210/affe5985-c787-4982-ab2d-9e359249f86c)

この追従するオブジェクトは下記のようにUnityのCollider,RigidBody,XRGrabInteractable,XRGrabInteractable(XRInteractionToolkitで掴むオブジェクトにアタッチするもの)とObiPhysicsのObiColliderとObiRigidbody(後述するObiParticleAttachimentで使う)と、構成になっており、XRInteractionToolkitとObiRopeの橋渡し役になっています。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/116884/2e74e962-72c4-1521-9ccb-7b996e34a014.png)

尚、橋渡しオブジェクトのLayerはお互いに干渉しないようにセッティングしてあります。また、Grabは出来るように、XRInteractionToolkit側のレイヤー(コチラで追加したPlayerControllerレイヤー)とだけ干渉出来るようにしてあります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/116884/9d8be880-4b1a-9a02-405f-cb61feb330b4.png)


## コントローラーに一番近いObiRope上の座標を取得する
ObiRopeとコントローラーが接触しているときに、コントローラーに一番近いObiRope上の座標を計算します。この座標に橋渡し役のオブジェクトを配置するためです。

ObiPhysicsの衝突検知をするには、以下のようにObiSolverにObiContactEventDispatcherをアタッチします。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/116884/dae1d5fd-2dd6-9668-70fa-f580b2378508.png)

次にコントローラー側からRopeへの衝突検知をする為のRopeCollisionDetectorをXR Interaction Setup以下にあるDirect Interactorにアタッチします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/116884/14797c91-ef1e-84af-d3ac-963f0e367470.png)

RopeCollisionDetectorは以下のクラスになります。

```csharp:RopeCollisionDetector.cs
using Obi;
using UnityEngine;
using UnityEngine.XR.Interaction.Toolkit;

/// <summary>
/// XRDirectInteractorがあるGameObjectにアタッチするObiRopeの衝突検知用クラス。
/// XRDirectInteractorに一番近いObiRope上の座標を衝突時のパラメタとして渡す
/// </summary>
[RequireComponent(typeof(XRDirectInteractor))]
[RequireComponent(typeof(ObiCollider))]
public class RopeCollisionDetector : MonoBehaviour
{
    public bool IsLeft { get; private set;}

    private ObiContactEventDispatcher contactEventDispatcher;

    private XRDirectInteractor interactor;

    ObiCollider selfCollider;

    private void Awake()
    {
        this.selfCollider = GetComponent<ObiCollider>();
        this.contactEventDispatcher = FindObjectOfType<ObiContactEventDispatcher>();
        var interactionGroup = GetComponentInParent<XRInteractionGroup>();

        //XRInteractionToolKit(ver2.5.2)のStarterAssetsのXR Interaction Setupには、右手、左手を区別できるパラメタが設定されている
        if (interactionGroup.groupName == XRInteractionGroup.GroupNames.k_Left)
        {
            this.IsLeft = true;
        }
        this.interactor = GetComponent<XRDirectInteractor>();
    }

    private void OnEnable()
    {
        this.contactEventDispatcher.onContactEnter.AddListener(SolverContact_OnCollisionEnter);
        this.contactEventDispatcher.onContactStay.AddListener(SolverContact_OnCollisionStay);
        this.contactEventDispatcher.onContactExit.AddListener(SolverContact_OnExit);
    }

    private void OnDisable()
    {
        this.contactEventDispatcher.onContactEnter.RemoveListener(SolverContact_OnCollisionEnter);
        this.contactEventDispatcher.onContactStay.RemoveListener(SolverContact_OnCollisionStay);
        this.contactEventDispatcher.onContactExit.RemoveListener(SolverContact_OnExit);
    }

    public void SolverContact_OnCollisionEnter(ObiSolver sender, Oni.Contact contact)
    {
        AnalyzeContact(sender, contact, (ObiRope obiRope, Vector3 projectPos, Vector3 ropeDirection) =>
        {
            if (this.IsLeft)
            {
                obiRope.GetComponent<Rope>().LeftRopeGrabInteractable.OnObiCollisionEnter(this.interactor, projectPos, ropeDirection);
            }
            else
            {
                obiRope.GetComponent<Rope>().RightRopeGrabInteractable.OnObiCollisionEnter(this.interactor, projectPos, ropeDirection);
            }
        });
    }

    public void SolverContact_OnCollisionStay(ObiSolver sender, Oni.Contact contact)
    {
        AnalyzeContact(sender, contact, (ObiRope obiRope, Vector3 projectPos, Vector3 ropeDirection) =>
        {
            if (this.IsLeft)
            {
                obiRope.GetComponent<Rope>().LeftRopeGrabInteractable.OnObiCollisionStay(this.interactor, projectPos, ropeDirection);
            }
            else
            {
                obiRope.GetComponent<Rope>().RightRopeGrabInteractable.OnObiCollisionStay(this.interactor, projectPos, ropeDirection);
            }
        });
    }

    public void SolverContact_OnExit(ObiSolver sender, Oni.Contact contact)
    {
        AnalyzeContact(sender, contact, (ObiRope obiRope, Vector3 projectPos, Vector3 ropeDirection) =>
        {
            if (this.IsLeft)
            {
                obiRope.GetComponent<Rope>().LeftRopeGrabInteractable.OnObiCollisionExit(this.interactor, projectPos, ropeDirection);
            }
            else
            {
                obiRope.GetComponent<Rope>().RightRopeGrabInteractable.OnObiCollisionExit(this.interactor, projectPos, ropeDirection);
            }
        });
    }

    private void AnalyzeContact(ObiSolver sender, Oni.Contact contact, System.Action<ObiRope, Vector3, Vector3> OnCollisionAction)
    {
        int simplexIndex = sender.simplices[contact.bodyA];
        var particleInActor = sender.particleToActor[simplexIndex];

        var world = ObiColliderWorld.GetInstance();
        var contactCollider = world.colliderHandles[contact.bodyB].owner;

        if ((particleInActor.actor is ObiRope) && contactCollider == selfCollider)
        {
            var obiRope = particleInActor.actor as ObiRope;
            if (obiRope.TryGetNearestParticleIndex(this.transform.position, out var outParticleIndex))
            {
                if (obiRope.TryGetRopeProjectionPosition(this.transform.position, outParticleIndex, sender, out var projectionPosition, out var outRopeDirection))
                {
                    OnCollisionAction?.Invoke(obiRope, projectionPosition, outRopeDirection);
                }
            }
        }
    }
}


```

下記クラスはObiRopeの座標計算に利用する拡張クラスです。
```csharp:ObiRopeExtension.cs
using Obi;
using UnityEngine;

public static class ObiRopeExtension
{
    /// <summary>
    /// targetWorldPositionに一番近いObiRope中のパーティクルのindexを取得します。
    /// </summary>
    /// <param name="rope"></param>
    /// <param name="targetWorldPosition"></param>
    /// <param name="outParticleIndex"></param>
    /// <returns></returns>
    public static bool TryGetNearestParticleIndex(this ObiRope rope, Vector3 targetWorldPosition, out int outParticleIndex)
    {
        var distance = 10000f;
        var targetIndex = -1;
        foreach (var particleIndex in rope.solver.simplices)
        {
            var particlePos = GetParticleWorldPosition(rope, particleIndex);
            var currentDistance = Vector3.Distance(particlePos, targetWorldPosition);
            if (currentDistance < distance)
            {
                distance = currentDistance;
                targetIndex = particleIndex;
            }
        }

        if (targetIndex == -1)
        {
            outParticleIndex = -1;
            return false;
        }

        outParticleIndex = targetIndex;
        return true;
    }

    public static Vector3 GetParticleWorldPosition(this ObiRope rope, int particleIndex)
    {
        var solver = rope.solver;
        Matrix4x4 solver2World = solver.transform.localToWorldMatrix;
        return solver2World.MultiplyPoint3x4(solver.positions[particleIndex]);
    }

    /// <summary>
    /// projectionWorldTarget座標に一番近いRope上の座標を取得します。
    /// </summary>
    /// <param name="rope"></param>
    /// <param name="projectionWorldTarget"></param>
    /// <param name="mostCloseParticleIndex"></param>
    /// <param name="solver"></param>
    /// <param name="outPos"></param>
    /// <param name="outDirection"></param>
    /// <returns></returns>
    public static bool TryGetRopeProjectionPosition(this ObiRope rope, Vector3 projectionWorldTarget, int mostCloseParticleIndex, ObiSolver solver, out Vector3 outPos, out Vector3 outDirection)
    {
        Matrix4x4 solver2World = solver.transform.localToWorldMatrix;

        if (rope.TryFindElement(mostCloseParticleIndex, out var outElement))
        {
            var currentIndex = outElement.particle2;
            var nextIndex = outElement.particle1;

            var currentParticlePos = solver2World.MultiplyPoint3x4(solver.positions[currentIndex]);
            var nextParticlePos = solver2World.MultiplyPoint3x4(solver.positions[nextIndex]);
            outPos = ObiUtils.ProjectPointLine(projectionWorldTarget, currentParticlePos, nextParticlePos, out var mu, false);
            outDirection = (nextParticlePos - currentParticlePos).normalized;
        }
        else
        {
            outPos = solver2World.MultiplyPoint3x4(solver.positions[mostCloseParticleIndex]);
            outDirection = Vector3.up;
        }
        return true;
    }

   
    private static bool TryFindElement(this ObiRope rope, int index, out ObiStructuralElement element)
    {
        foreach (var one in rope.elements)
        {
            if (one.particle1 == index)
            {
                element = one;
                return true;
            }
        }
        element = null;
        return false;
    }
}

```

## 橋渡しオブジェクトを掴んでObiRopeを動かす


ObiRopeを動かすには[ObiParticleAttachment](https://obi.virtualmethodstudio.com/manual/6.3/attachments.html)を利用します。ObiParticleAttachmentはUnityのFixedJointのように機能します。下記はRopeの上端にオブジェクトを配置して、ObiParticleAttachmentでオブジェクトが動くとObiRopeも連動して動くようにしたものです。
![obirope_attachment](https://github.com/unagiHuman/ObiRopeXRInteractionToolKitSample/assets/17809210/1474ce69-4237-492a-8b13-8bbc49bfa3a9)
### ObiRopeを掴んで動かす用のControlPointを追加する
PathEditorにてRopeにControlPointを追加します。(ControlPointを追加すると内部でParticleGroupが生成されます)ObiParticleAttachmentはこのParticleGroupを基準にRopeを動かします。追加するPaticleGroupは自分の検証では2mのRopenに対して10個ぐらい挿入しました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/116884/0794b79d-17a4-4714-6b45-f23885791099.png)

### 動的にObiParticleAttachmentをOnOffする。
ObiParticleAttachmentのパラメータは以下のようになります。
* Target
  * ObiRopeにアタッチしたいTransform
* ParticleGroup
  * PathEditorで追加したPath(Nameの部分がParticleGroupの名前になる)
* Type
  * 動的にObiParticleAttachmentをアタッチしたりする場合はDynamicに設定。
* Compliance
  * デフォルト値の0でOK。
* Break threshold
  * デフォルト値のInfinityでOK。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/116884/010c0e24-95dc-6d64-7b5b-d23208a064e9.png)

ParticleGroupは上記で追加したControlPointの中から橋渡しオブジェクトに一番近いものを選択します。

```csharp
   private ObiParticleGroup FindNearObiParticleGroup(Transform target)
    {
        var distance = 100000f;
        ObiParticleGroup findParticleGroup = null;
        foreach(var group in this.obiRope.blueprint.groups)
        {
            foreach(var particleindex in group.particleIndices)
            {
                //ObiRopeExtension参照
                var particlePosition = this.obiRope.GetParticleWorldPosition(particleindex);
                var currentDistance = Vector3.Distance(target.position, particlePosition);
                if(currentDistance <= distance)
                {
                    distance = currentDistance;
                    findParticleGroup = group;
                }
            }
        }

        return findParticleGroup;
    }
```

また、一回GameObjectにアタッチされたObiParticleAttachmentはenableをON,OFFする事でTargetに付けたり外したり出来ます。

### 掴まれる側の実装

XRinteractionToolkitのXRGrabInteractableをオーバーライドして作成します。GrabしたときにObiRopeと橋渡しオブジェクトをObiParticleAttachmentでアタッチし、DropしたときにObiParticleAttachmentを無効にします。また、コントローラーとObiRopeとの衝突結果を座標で受け取って位置を同期する役割もあります。

```csharp:RopeGrabInteractable.cs
using UnityEngine;
using UnityEngine.XR.Interaction.Toolkit;

/// <summary>
/// ObiRopeとの橋渡しオブジェクトを掴む為のクラス
/// </summary>
public class RopeGrabInteractable : XRGrabInteractable
{
    enum FollowState
    {
        No,
        Follow
    }

    enum GrabState
    {
        No,
        Grab
    }

    [SerializeField] private FollowState followState;

    [SerializeField] private GrabState grabState = GrabState.No;

    [SerializeField] private Rope rope;

    private Rigidbody selfRigidbody;

    private XRDirectInteractor interactor = null;

    private Vector3 grabRopePosition;

    private Vector3 grabRopeDirection;

    protected override void Awake()
    {
        base.Awake();
        this.followState = FollowState.No;
        this.interactor = null;
        this.selfRigidbody = GetComponent<Rigidbody>();
        this.rope = this.GetComponentInParent<Rope>();
        if(this.rope!=null) this.transform.parent = this.rope?.transform.parent;

    }

    protected override void Grab()
    {
        this.grabState = GrabState.Grab;
        base.Grab();
        if (firstInteractorSelecting.hasSelection)
        {
            this.rope.AddOrEnableParticleAttachment(this, this.transform);
        }
    }

    protected override void Drop()
    {
        this.grabState = GrabState.No;
        base.Drop();
        this.rope.DisableParticleAttachment(this);
    }

    public void OnObiCollisionEnter(XRDirectInteractor xRDirectInteractor, Vector3 ropePoint, Vector3 ropeDirection)
    {
        if (this.interactor != null) return;
        this.followState = FollowState.Follow;
        this.interactor = xRDirectInteractor;
        SetFollowParameter(ropePoint, ropeDirection);
    }

    public void OnObiCollisionStay(XRDirectInteractor xRDirectInteractor, Vector3 ropePoint, Vector3 ropeDirection)
    {
        SetFollowParameter(ropePoint, ropeDirection);
    }

    public void OnObiCollisionExit(XRDirectInteractor xRDirectInteractor, Vector3 ropePoint, Vector3 ropeDirection)
    {
        this.interactor = null;
        this.followState = FollowState.No;
    }

    private void SetFollowParameter(Vector3 grabRopePosition, Vector3 grabRopeDirection)
    {
        this.grabRopePosition = grabRopePosition;
        this.grabRopeDirection = grabRopeDirection;
    }

    /// <summary>
    /// コントローラーと連動して動かす
    /// </summary>
    private void FixedUpdate()
    {
        if(this.followState == FollowState.Follow && this.grabState == GrabState.No)
        {
            this.selfRigidbody.MovePosition(grabRopePosition);
        }
    }

}

```

```csharp:Rope.cs
using Obi;
using System.Collections.Generic;
using Unity.VisualScripting;
using UnityEngine;

/// <summary>
/// ObiRopeにアタッチする
/// </summary>
[RequireComponent(typeof(ObiRope))]
public class Rope : MonoBehaviour
{
    public RopeGrabInteractable LeftRopeGrabInteractable => this.leftRopeGrabInteractable;

    public RopeGrabInteractable RightRopeGrabInteractable => this.rightRopeGrabInteractable;

    [SerializeField] RopeGrabInteractable leftRopeGrabInteractable;

    [SerializeField] RopeGrabInteractable rightRopeGrabInteractable;

    ObiRope obiRope;

    Dictionary<RopeGrabInteractable, ObiParticleAttachment> attachimentDict = new Dictionary<RopeGrabInteractable, ObiParticleAttachment>();

    private void Awake()
    {
        this.obiRope = this.GetComponent<ObiRope>();
    }

    /// <summary>
    /// ObiParticleAttachmentをtargetに対して有効にする
    /// </summary>
    /// <param name="ropeGrabInteractable"></param>
    /// <param name="target"></param>
    public void AddOrEnableParticleAttachment(RopeGrabInteractable ropeGrabInteractable, Transform target)
    {
        if (!this.attachimentDict.ContainsKey(ropeGrabInteractable))
        {
            var particleAttachment = this.AddComponent<ObiParticleAttachment>();
            particleAttachment.target = target;
           
            particleAttachment.attachmentType = ObiParticleAttachment.AttachmentType.Dynamic;
            particleAttachment.particleGroup = FindNearObiParticleGroup(target);
            this.attachimentDict[ropeGrabInteractable] = particleAttachment;
        }
        else
        {
            this.attachimentDict[ropeGrabInteractable].particleGroup = FindNearObiParticleGroup(target);
            this.attachimentDict[ropeGrabInteractable].enabled = true;
        }
    }

    /// <summary>
    /// ObiParticleAttachmentをtarget無効にする
    /// </summary>
    /// <param name="ropeGrabInteractable"></param>
    public void DisableParticleAttachment(RopeGrabInteractable ropeGrabInteractable)
    {
        if (this.attachimentDict.ContainsKey(ropeGrabInteractable))
        {
            this.attachimentDict[ropeGrabInteractable].enabled = false;
        }
    }

    private ObiParticleGroup FindNearObiParticleGroup(Transform target)
    {
        var distance = 100000f;
        ObiParticleGroup findParticleGroup = null;
        foreach(var group in this.obiRope.blueprint.groups)
        {
            foreach(var particleindex in group.particleIndices)
            {
                var particlePosition = this.obiRope.GetParticleWorldPosition(particleindex);
                var currentDistance = Vector3.Distance(target.position, particlePosition);
                if(currentDistance <= distance)
                {
                    distance = currentDistance;
                    findParticleGroup = group;
                }
            }
        }

        return findParticleGroup;
    }
}
```