//------------------------------------------------------------ // Nhân 2 ma trận 3x3 //------------------------------------------------------------ public static VisionMatrixObject Mul(VisionMatrixObject inMat01, VisionMatrixObject inMat02) { float[] arrOut = new float[9]; float m11 = inMat01.Value[0]; float m12 = inMat01.Value[1]; float m13 = inMat01.Value[2]; float m21 = inMat01.Value[3]; float m22 = inMat01.Value[4]; float m23 = inMat01.Value[5]; float m31 = inMat01.Value[6]; float m32 = inMat01.Value[7]; float m33 = inMat01.Value[8]; float n11 = inMat02.Value[0]; float n12 = inMat02.Value[1]; float n13 = inMat02.Value[2]; float n21 = inMat02.Value[3]; float n22 = inMat02.Value[4]; float n23 = inMat02.Value[5]; float n31 = inMat02.Value[6]; float n32 = inMat02.Value[7]; float n33 = inMat02.Value[8]; arrOut[0] = m11 * n11 + m12 * n21 + m13 * n31; arrOut[1] = m11 * n12 + m12 * n22 + m13 * n32; arrOut[2] = m11 * n13 + m12 * n23 + m13 * n33; arrOut[3] = m21 * n11 + m22 * n21 + m23 * n31; arrOut[4] = m21 * n12 + m22 * n22 + m23 * n32; arrOut[5] = m21 * n13 + m22 * n23 + m23 * n33; arrOut[6] = m31 * n11 + m32 * n21 + m33 * n31; arrOut[7] = m31 * n12 + m32 * n22 + m33 * n32; arrOut[8] = m31 * n13 + m32 * n23 + m33 * n33; return(new VisionMatrixObject(arrOut)); }
//------------------------------------------------------------ // Chuyển đổi ma trận 3x3 sang Point //------------------------------------------------------------ public static AlignPointObject ConvertMatrixToPoint(VisionMatrixObject inputMatrix) { float x, y, theta; x = inputMatrix.Value[2]; y = inputMatrix.Value[5]; //------------------------------------------------------------ // Cal Theta from sin + cos //------------------------------------------------------------ float tempTheta = (float)Math.Asin(inputMatrix.Value[3]); if (Math.Cos(tempTheta) == inputMatrix.Value[0]) { theta = tempTheta; } else { theta = (float)(Math.PI - tempTheta); } //------------------------------------------------------------ // Convert theta if over -PI/PI //------------------------------------------------------------ for (int i = 0; i < 4; i++) { if (theta < (float)(-Math.PI)) { theta += (float)Math.PI; } if (theta > (float)(Math.PI)) { theta -= (float)Math.PI; } } return(new AlignPointObject(x, y, theta)); }
//------------------------------------------------------------ // Nghịch đảo ma trận 3x3 //------------------------------------------------------------ public static VisionMatrixObject Invert(VisionMatrixObject inMat) { float[] arrOut = new float[9]; float m11 = inMat.Value[0]; float m12 = inMat.Value[1]; float m13 = inMat.Value[2]; float m21 = inMat.Value[3]; float m22 = inMat.Value[4]; float m23 = inMat.Value[5]; float m31 = inMat.Value[6]; float m32 = inMat.Value[7]; float m33 = inMat.Value[8]; arrOut[0] = (m22 * m33 - m23 * m32) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[1] = -(m12 * m33 - m13 * m32) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[2] = (m12 * m23 - m13 * m22) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[3] = -(m21 * m33 - m23 * m31) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[4] = (m11 * m33 - m13 * m31) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[5] = -(m11 * m23 - m13 * m21) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[6] = (m21 * m32 - m22 * m31) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[7] = -(m11 * m32 - m12 * m31) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); arrOut[8] = (m11 * m22 - m12 * m21) / (m11 * m22 * m33 - m11 * m23 * m32 - m21 * m12 * m33 + m31 * m12 * m23 + m21 * m13 * m32 - m31 * m13 * m22); return(new VisionMatrixObject(arrOut)); }