根据旋转前后的向量求旋转矩阵

Posted 2020-11-07 dsczl

https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html 

目前能够百度到的根据旋转前后的两个向量求旋转矩阵，基本上都是根据上部链接的博客原理，但我根据其代码所写的程序，发现有bug ，一时半会无法解决，估计是特殊情况没有考虑。

 

在cloundCompare开源里有矩阵的对应接口，亲测可用，非常准确，把代码贴下面供开发者使用。

其中的dot函数是三维向量的点积函数（附内部实现inline Type dot(const Vector3Tpl& v) const { return x*v.x + y*v.y + z*v.z; }）

static ccGLMatrixTpl<T> FromToRotation(const Vector3Tpl<T>& from, const Vector3Tpl<T>& to)
    {
        T c = from.dot(to);
        T f = (c < 0 ? -c : c);
        ccGLMatrixTpl<T> result;

        if (1.0-f < ZERO_TOLERANCE) //"from" and "to"-vector almost parallel
        {
            // "to" vector most nearly orthogonal to "from"
            Vector3Tpl<T> x(0,0,0);
            if (fabs(from.x) < fabs(from.y))
            {
                if (fabs(from.x) < fabs(from.z))
                    x.x = static_cast<T>(1);
                else
                    x.z = static_cast<T>(1);
            }
            else
            {
                if (fabs(from.y) < fabs(from.z))
                    x.y = static_cast<T>(1);
                else
                    x.z = static_cast<T>(1);
            }

            Vector3Tpl<T> u = x-from;
            Vector3Tpl<T> v = x-to;

            T c1 = 2 / u.dot(u);
            T c2 = 2 / v.dot(v);
            T c3 = c1 * c2  * u.dot(v);

            T* mat = result.data();
            for (unsigned i=0; i<3; i++)
            {
                for (unsigned j=0; j<3; j++)
                {
                    mat[i*4+j] =  c3 * v.u[i] * u.u[j]
                                - c2 * v.u[i] * v.u[j]
                                - c1 * u.u[i] * u.u[j];
                }
                mat[i*4+i] += static_cast<T>(1);
            }
        }
        else  // the most common case, unless "from"="to", or "from"=-"to"
        {
            //see Efficiently Building a Matrix to Rotate One Vector to Another
            //T. Moller and J.F. Hugues (1999)
            Vector3Tpl<T> v = from.cross(to);
            T h = 1 / (1 + c);
            T hvx = h * v.x;
            T hvz = h * v.z;
            T hvxy = hvx * v.y;
            T hvxz = hvx * v.z;
            T hvyz = hvz * v.y;

            T* mat = result.data();
            mat[0]  = c + hvx * v.x;
            mat[1]  = hvxy + v.z;
            mat[2]  = hvxz - v.y;

            mat[4]  = hvxy - v.z;
            mat[5]  = c + h * v.y * v.y;
            mat[6]  = hvyz + v.x;

            mat[8]  = hvxz + v.y;
            mat[9]  = hvyz - v.x;
            mat[10] = c + hvz * v.z;
        }

        return result;
    }