ARCore中Pose类变换点的算法实现

ARCore中Pose类变换点的算法实现，主要分为两步，分别是平移和旋转。

 

 

1. 旋转向量：通过四元数计算旋转后的向量

参数列表：q表示四元数，

　　　　　v是长度为4的float数组，表示待旋转的向量，

　　　　   offsetIn表示第一个坐标值的起始索引，

　　　　　out代表结果向量，

　　　　　offsetOut表示结果向量的三个坐标值在out数组中的起始索引。

    public static void rotateVector(Quaternion q, float[] v, int offsetIn, float[] out, int offsetOut) {
        float x = v[offsetIn + 0];
        float y = v[offsetIn + 1];
        float z = v[offsetIn + 2];
        float qx = q.x();
        float qy = q.y();
        float qz = q.z();
        float qw = q.w();
        float ix = qw * x + qy * z - qz * y;
        float iy = qw * y + qz * x - qx * z;
        float iz = qw * z + qx * y - qy * x;
        float iw = -qx * x - qy * y - qz * z;
        out[offsetOut + 0] = ix * qw + iw * -qx + iy * -qz - iz * -qy;
        out[offsetOut + 1] = iy * qw + iw * -qy + iz * -qx - ix * -qz;
        out[offsetOut + 2] = iz * qw + iw * -qz + ix * -qy - iy * -qx;
    }

2. 变换一个点：

参数列表：pointIn表示包含待变换点的数组，

　　　　   inOffset表示待变换的点在数组中的起始索引，

　　　　   pointOut表示写入变换后的点坐标的数组，

　　　　   outOffset表示变化后的点坐标在pointOut数组中的起始索引。

　　public void transformPoint(float[] pointIn, int inOffset, float[] pointOut, int outOffset) {
        rotateVector(pointIn, inOffset, pointOut, outOffset);//先旋转点:等同于R * pointIn

        for(int i = 0; i < 3; ++i) {
            pointOut[i + outOffset] += this.translation[i];//平移点：等同于 T * pointIn
        }
　　}

此方法等同于 ： pointOut = M * pointIn ， 其中 M = T * R