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

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

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

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

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

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

　　　　　out代表结果向量，

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

 1     public static void rotateVector(Quaternion q, float[] v, int offsetIn, float[] out, int offsetOut) {
 2         float x = v[offsetIn + 0];
 3         float y = v[offsetIn + 1];
 4         float z = v[offsetIn + 2];
 5         float qx = q.x();
 6         float qy = q.y();
 7         float qz = q.z();
 8         float qw = q.w();
 9         float ix = qw * x + qy * z - qz * y;
10         float iy = qw * y + qz * x - qx * z;
11         float iz = qw * z + qx * y - qy * x;
12         float iw = -qx * x - qy * y - qz * z;
13         out[offsetOut + 0] = ix * qw + iw * -qx + iy * -qz - iz * -qy;
14         out[offsetOut + 1] = iy * qw + iw * -qy + iz * -qx - ix * -qz;
15         out[offsetOut + 2] = iz * qw + iw * -qz + ix * -qy - iy * -qx;
16     }

2. 变换一个点：

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

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

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

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

1 　　public void transformPoint(float[] pointIn, int inOffset, float[] pointOut, int outOffset) {
2         rotateVector(pointIn, inOffset, pointOut, outOffset);//先旋转点:等同于R * pointIn
3
4         for(int i = 0; i < 3; ++i) {
5             pointOut[i + outOffset] += this.translation[i];//平移点：等同于 T * pointIn
6         }
7 　　}

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