blob: af2204e8cf28e53bec523b766eb6b2cbd98a00be [file] [log] [blame]
 class Transform { float[] inv_rot; // inverse of rotation matrix PVector inv_mov; // inverse of movement vector float focal; // the focal distacne of real camera int w, h; // the width and height of the frame float normalier; // nomalization factor of depth Transform(float tx, float ty, float tz, float qx, float qy, float qz, float qw, float fov, int w, int h, float normalier) { // currently, we did not use the info of real camera's position and // quaternion maybe we will use it in the future when combine all frames float[] rot = quaternion2Mat3x3(qx, qy, qz, qw); inv_rot = transpose3x3(rot); inv_mov = new PVector(-tx, -ty, -tz); this.focal = 0.5f * h / tan(fov / 2.0); this.w = w; this.h = h; this.normalier = normalier; } PVector transform(int i, int j, float d) { // transfer from camera view to world view float z = d / normalier; float x = (i - w / 2.0f) * z / focal; float y = (j - h / 2.0f) * z / focal; return new PVector(x, y, z); } } // get rotation matrix by using rotation axis and angle float[] getRotationMat3x3(float angle, float ax, float ay, float az) { float[] mat = new float[9]; float c = cos(angle); float s = sin(angle); mat[0] = c + ax * ax * (1 - c); mat[1] = ax * ay * (1 - c) - az * s; mat[2] = ax * az * (1 - c) + ay * s; mat[3] = ay * ax * (1 - c) + az * s; mat[4] = c + ay * ay * (1 - c); mat[5] = ay * az * (1 - c) - ax * s; mat[6] = az * ax * (1 - c) - ay * s; mat[7] = az * ay * (1 - c) + ax * s; mat[8] = c + az * az * (1 - c); return mat; } // get rotation matrix by using quaternion float[] quaternion2Mat3x3(float qx, float qy, float qz, float qw) { float[] mat = new float[9]; mat[0] = 1 - 2 * qy * qy - 2 * qz * qz; mat[1] = 2 * qx * qy - 2 * qz * qw; mat[2] = 2 * qx * qz + 2 * qy * qw; mat[3] = 2 * qx * qy + 2 * qz * qw; mat[4] = 1 - 2 * qx * qx - 2 * qz * qz; mat[5] = 2 * qy * qz - 2 * qx * qw; mat[6] = 2 * qx * qz - 2 * qy * qw; mat[7] = 2 * qy * qz + 2 * qx * qw; mat[8] = 1 - 2 * qx * qx - 2 * qy * qy; return mat; } // tranpose a 3x3 matrix float[] transpose3x3(float[] mat) { float[] Tmat = new float[9]; for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) { Tmat[i * 3 + j] = mat[j * 3 + i]; } return Tmat; } // multiply a matrix with vector PVector MatxVec3(float[] mat, PVector v) { float[] vec = v.array(); float[] res = new float[3]; for (int i = 0; i < 3; i++) { res[i] = 0.0f; for (int j = 0; j < 3; j++) { res[i] += mat[i * 3 + j] * vec[j]; } } return new PVector(res[0], res[1], res[2]); }