Algorithm implementation of pose class transform points in Arcore

The algorithm realization of pose class transform point in Arcore is divided into two steps, namely translation and rotation.

1. Rotation vector: Calculates the rotated vector by four-dollar number

List of parameters: Q for four Yuan,

V is a float array of length 4 that represents the vector to be rotated,

Offsetin represents the starting index of the first coordinate value,

Out represents the result vector,

The offsetout represents the starting index of the three coordinate values of the result vector in the out array.

1      Public Static voidRotatevector (quaternion Q,float[] V,intOffsetin,float[] out,intoffsetout) {2         floatx = v[offsetin + 0];3         floaty = v[offsetin + 1];4         floatz = v[offsetin + 2];5         floatQX =q.x ();6         floatQY =q.y ();7         floatQZ =q.z ();8         floatQW =Q.W ();9         floatIX = QW * x + qy * Z-QZ *y;Ten         floatiy = qw * y + QZ * x-qx *Z; One         floatiz = QW * z + qx * y-qy *x; A         floatIW =-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; theOut[offsetout + 2] = iz * QW + IW *-QZ + IX *-qy-iy *-QX; -}

2. Transform a point:

Parameter list: Pointin represents an array containing the points to be transformed.

Inoffset represents the starting index of the point to be transformed in the array,

Pointout represents an array of point coordinates written to the transformation,

The outoffset represents the starting index of the changed point coordinate in the pointout array.

1  　　  Public voidTransformpoint (float[] Pointin,intInoffset,float[] Pointout,intOutoffset) {2 Rotatevector (Pointin, Inoffset, Pointout, outoffset);//rotate point first: equivalent to R * Pointin3 4          for(inti = 0; I < 3; ++i) {5Pointout[i + Outoffset] + = This. translation[i];//translation point: equivalent to T * pointin6         }7}

This method is equivalent to: pointout = m * Pointin, where m = T * R

Algorithm implementation of pose class transform points in Arcore