Slerpメソッドで、近似判定して完了処理を行う方法

Question

前提・実現したいこと

以下のようなSlerpメソッドの回転処理を、コルーチンにしたいと思っているのですが、
コードの性質上、ずっとSlerpメソッドが呼び出され続けてしまうので、
ある程度、第1引数のQuaternionが第2引数のQuaternionに近づいたら、
近似判定をして完了処理をし、コルーチンが終了するようにしたいと思っているのですが、
その判定方法が分からない状態です。
ご教示お願いします。

  [SerializeField]
  GameObject target;

  float rotateSpeed = 2f;

  void Update () {
      Vector3 targetDir = target.transform.position - this.transform.position;
      this.transform.rotation = Quaternion.Slerp (
          this.transform.rotation,
          Quaternion.LookRotation(targetDir),
          Time.deltaTime * rotateSpeed
      );
  }

試したこと

近似判定して完了処理する以外の部分の実装はできました。
以下のコードのコメントのif文の近似判定方法がわからない状態です。

    [SerializeField]    
    GameObject target;

    void Start (){
        StartCoroutine(EaseOutLookAt(this.transform, target.transform, 2f));
    }

    IEnumerator EaseOutLookAt(Transform myTrans, Transform target, float rotateSpeed){
        while(true){
            Vector3 targetDir = target.position - myTrans.position;
            myTrans.rotation = Quaternion.Slerp(
                myTrans.rotation,
                Quaternion.LookRotation(targetDir),
                Time.deltaTime * rotateSpeed
            );
            // if(true){
            //     myTrans.rotation = Quaternion.LookRotation(targetDir);
            //     break;
            // }
            yield return null;
        }
        Debug.Log("finish");
    }

2つのQuaternionの近似判定をすればよいかと思うのですが、どういったコードで実装できますか？
ちなみに、距離の近似判定の場合は以下のように実装すればよいことはわかっています。

            float distance = (endPoint.position - move.transform.position).sqrMagnitude;
            if(distance < 0.0001){
                move.position = endPoint.position;
                break;
            }

Answer 1

姿勢の近さで判定するとなると、Dotを使う手もあるかと思います。

    IEnumerator EaseOutLookAt(Transform myTrans, Transform target, float rotateSpeed){
        while(true){
            Vector3 targetDir = target.position - myTrans.position;
            Quaternion targetRotation = Quaternion.LookRotation(targetDir);
            Quaternion newRotation = Quaternion.Slerp(
                myTrans.rotation,
                targetRotation,
                Time.deltaTime * rotateSpeed
            );
            myTrans.rotation = newRotation;

            // 目標姿勢と現在の姿勢の内積を求め...
            // （挙動確認目的でコンソールに内積の値を表示しています）
            float dot = Quaternion.Dot(targetRotation, newRotation);
            Debug.Log(dot);

            // その絶対値がおおむね1になったらループ終了とする
            // （コンソールに出力される値をご確認いただくと明確かと思いますが、内積がプラスの場合は1へ、
            // マイナスの場合は-1へ寄っていく動きになるかと思いますので、絶対値を取って判定しています）
            if(Mathf.Abs(dot) > 0.9999f){
                myTrans.rotation = targetRotation;
                break;
            }

            yield return null;
        }
        Debug.Log("finish");
    }

Comments

[退会済みユーザー]

ご回答ありがとうございます。
ご提示いただいたコードを試してみて、ログを確認し、正常に動作していることを確認いたしました。
ありがとうございます。
2点、質問させていただけたらと思います。

質問1。
ベクトルの内積に関しては、以下のようになることを認識しているのですが、
・A，Bが同じ向き・同じ大きさであれば1。
・A，Bが垂直の角度ならば0。
・A，Bが反対方向で同じ大きさであれば-1。
クォータニオンの場合、上記のような内積のイメージがわからず、
特に「内積が-1でも同じ回転とみなす」ということに関してイメージがわからず、
例えば、具体的なコードで-1になる(-1に近くなるような)ケースを教えていただくことは可能ですか？
内積が1になる場合は、下記のようなコードを試して確認いたしました。
Quaternion rotationA = Quaternion.Euler(10, 20, 30);
Quaternion rotationB = Quaternion.Euler(10, 19, 30);
float dot = Quaternion.Dot(rotationA, rotationB);
Debug.Log(dot); // 0.999962

質問2。
今更ながらで申し訳ないのですが、Lerp等のイーズアウト処理では、理論上は、
永遠に補間値1の値に到達しないけど、Unityでは近似されて、補間値1の値に到達することを思い出しました(そうだったような気がしています。記憶が曖昧で申し訳ありません)。
これより、今回のコードにおいては、補間処理の終点到達の調整処理において、
以下のように「if (myTrans.rotation == targetRotation)」で判定しても問題ないでしょうか？
試した限りでは大丈夫そうでした。

IEnumerator EaseOutLookAt(Transform myTrans, Transform target, float rotateSpeed){
while(true){
Vector3 targetDir = target.position - myTrans.position;
Quaternion targetRotation = Quaternion.LookRotation(targetDir);
myTrans.rotation = Quaternion.Slerp(
myTrans.rotation,
targetRotation,
Time.deltaTime * rotateSpeed
);
if (myTrans.rotation == targetRotation){
break;
}
yield return null;
}
Debug.Log("finish");
}

[Bongo]

質問1について

ランダムに角度を生成して探してみたところ、下記のような例がありました。

Quaternion rotationA = Quaternion.Euler(70, 10, 100);
Quaternion rotationB = Quaternion.Euler(40, 100, 0);
float dot = Quaternion.Dot(rotationA, rotationB);
Debug.Log(dot); // -0.004682228

質問2について

今回の場合、==で判定するのはまずいかもしれませんね。
https://github.com/Unity-Technologies/UnityCsReference/blob/61f92bd79ae862c4465d35270f9d1d57befd1761/Runtime/Export/Math/Quaternion.cs#L113 をご覧いただきたいのですが、==は...

// *undocumented*
public const float kEpsilon = 0.000001F;

// Is the dot product of two quaternions within tolerance for them to be considered equal?
private static bool IsEqualUsingDot(float dot)
{
// Returns false in the presence of NaN values.
return dot > 1.0f - kEpsilon;
}

// Are two quaternions equal to each other?
public static bool operator==(Quaternion lhs, Quaternion rhs)
{
return IsEqualUsingDot(Dot(lhs, rhs));
}

といった実装になっているようです。内積がほぼ1の場合のみ一致していると判定されるようですので、内積が-1に寄っていくケースだと、いつまでたっても到達したと判定されなくなってしまうでしょう。
「【Unity】 Quaternion API解説 - エフアンダーバー」（https://www.f-sp.com/entry/2017/08/11/194125#%E6%BC%94%E7%AE%97%E5%AD%90-%E6%BC%94%E7%AE%97%E5%AD%90 ）にも==や!=についての注意が書かれていました。

[退会済みユーザー]

ご回答ありがとうございます。

質問1について。
具体的なコードをご提示いただきありがとうございます。勉強になります。

質問2について。
==で判定するとバグになってしまうこと、ご教示ありがとうございます。
ご提示いただいたURLも拝見しました。勉強になります。

ご教示いただき、大変ありがとうございました。

Answer 2

rotationを使わずにeulerAnglesを使ってみてはいかがでしょう。
transform.rotationはQuaternion型ですが、
transform.eularAnglesはVector3型で、インスペクターの角度のように度数法で扱うことができます。
Vector3型には同じくSlerp関数が用意されていますので、同じことができると思います。

近似判定については、
現在角度が目標角度に近づけば、角度の差が0に近くなることを利用して
現在角度と目標角度との差を取り、
角度をベクトルとして扱い、長さから判定すると、簡単に作れそうです。

//角度はVector3型
if ((現在角度 - 目標角度).magnitude < 0.1f)
   //完了処理・・・

もし「Quaternionで作りたい」というのでしたら、お力になれず申し訳ないです･･･。

Comments

[退会済みユーザー]

ご回答ありがとうございます。
Quaternion.Dotで実装できるというご回答をいただきましたので、今回はそちらで実装したいと思います。
eulerAnglesを使うという方法も勉強になりました。
ご回答ありがとうございました。