Quaternion “lookAt” function

别来无恙 提交于 2019-11-30 09:45:31
nasskov

I think this function will do what you need:

/// <summary>
/// Evaluates a rotation needed to be applied to an object positioned at sourcePoint to face destPoint
/// </summary>
/// <param name="sourcePoint">Coordinates of source point</param>
/// <param name="destPoint">Coordinates of destionation point</param>
/// <returns></returns>
public static Quaternion LookAt(Vector3 sourcePoint, Vector3 destPoint)
{
    Vector3 forwardVector = Vector3.Normalize(destPoint - sourcePoint);

    float dot = Vector3.Dot(Vector3.forward, forwardVector);

    if (Math.Abs(dot - (-1.0f)) < 0.000001f)
    {
        return new Quaternion(Vector3.up.x, Vector3.up.y, Vector3.up.z, 3.1415926535897932f);
    }
    if (Math.Abs(dot - (1.0f)) < 0.000001f)
    {
        return Quaternion.identity;
    }

    float rotAngle = (float)Math.Acos(dot);
    Vector3 rotAxis = Vector3.Cross(Vector3.forward, forwardVector);
    rotAxis = Vector3.Normalize(rotAxis);
    return CreateFromAxisAngle(rotAxis, rotAngle);
}

// just in case you need that function also
public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
{
    float halfAngle = angle * .5f;
    float s = (float)System.Math.Sin(halfAngle);
    Quaternion q;
    q.x = axis.x * s;
    q.y = axis.y * s;
    q.z = axis.z * s;
    q.w = (float)System.Math.Cos(halfAngle);
    return q;
}

This code comes from here: https://gamedev.stackexchange.com/questions/15070/orienting-a-model-to-face-a-target I just slightly modified it to fit my case, which was implementation of the transform.LookAt without using Unity3D.

You don't need to use acos and axis angle (which will in turn do 2 more trig functions) to get the quaternion from 2 vectors:

public static Quaternion LookAt(Vector3 sourcePoint, Vector3 destPoint)
{
    Vector3 forwardVector = Vector3.Normalize(destPoint - sourcePoint);

    Vector3 rotAxis = Vector3.Cross(Vector3.forward, forwardVector);
    float dot = Vector3.Dot(Vector3.forward, forwardVector);

    Quaternion q;
    q.x = rotAxis.x;
    q.y = rotAxis.y;
    q.z = rotAxis.z;
    q.w = dot+1;

    return q.normalize();
}

The reason for the dot+1 and subsequent normalize is because if you don't, you'll get the quaternion for the double rotation. Those 2 steps will effectively do slerp(identity, q, 0.5) which will be the proper quaternion.

Both of current answers have various problems for edge cases. The accepted answer is not correct for other reasons as well including the fact that it sets w=pi for one of the cases and also it doesn't do proper norms. After looking around quite a bit and testing several cases, I also found out that you need front and up vector to do this computation. So without further ado below is the code I'm using:

Quaternion lookAt(const Vector3f& sourcePoint, const Vector3f& destPoint, const Vector3f& front, const Vector3f& up)
{
    Vector3f toVector = (destPoint - sourcePoint).normalized();

    //compute rotation axis
    Vector3f rotAxis = front.cross(toVector).normalized();
    if (rotAxis.squaredNorm() == 0)
        rotAxis = up;

    //find the angle around rotation axis
    float dot = VectorMath::front().dot(toVector);
    float ang = std::acosf(dot);

    //convert axis angle to quaternion
    return Eigen::AngleAxisf(rotAxis, ang);
}

Bove uses popular Eigen library. If you don't want to use that then you might need following replacement for Eigen::AngleAxisf:

//Angle-Axis to Quaternion
Quaternionr angleAxisf(const Vector3r& axis, float angle) {
    auto s = std::sinf(angle / 2);
    auto u = axis.normalized();
    return Quaternionr(std::cosf(angle / 2), u.x() * s, u.y() * s, u.z() * s);
}

Note that special cases for dot product 0 or 1 or -1 gets automatically handled because normalized() returns 0 for the zero vector in Eigen library.

On the side note, for all your conversions worries, this is a great document to go to.

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!