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

时间:2023-11-27 12:49:02

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

相关文章