• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1class Transform {
2  float[] inv_rot;  // inverse of rotation matrix
3  PVector inv_mov;  // inverse of movement vector
4  float focal;      // the focal distacne of real camera
5  int w, h;         // the width and height of the frame
6  float normalier;  // nomalization factor of depth
7  Transform(float tx, float ty, float tz, float qx, float qy, float qz,
8            float qw, float fov, int w, int h, float normalier) {
9    // currently, we did not use the info of real camera's position and
10    // quaternion maybe we will use it in the future when combine all frames
11    float[] rot = quaternion2Mat3x3(qx, qy, qz, qw);
12    inv_rot = transpose3x3(rot);
13    inv_mov = new PVector(-tx, -ty, -tz);
14    this.focal = 0.5f * h / tan(fov / 2.0);
15    this.w = w;
16    this.h = h;
17    this.normalier = normalier;
18  }
19
20  PVector transform(int i, int j, float d) {
21    // transfer from camera view to world view
22    float z = d / normalier;
23    float x = (i - w / 2.0f) * z / focal;
24    float y = (j - h / 2.0f) * z / focal;
25    return new PVector(x, y, z);
26  }
27}
28
29// get rotation matrix by using rotation axis and angle
30float[] getRotationMat3x3(float angle, float ax, float ay, float az) {
31  float[] mat = new float[9];
32  float c = cos(angle);
33  float s = sin(angle);
34  mat[0] = c + ax * ax * (1 - c);
35  mat[1] = ax * ay * (1 - c) - az * s;
36  mat[2] = ax * az * (1 - c) + ay * s;
37  mat[3] = ay * ax * (1 - c) + az * s;
38  mat[4] = c + ay * ay * (1 - c);
39  mat[5] = ay * az * (1 - c) - ax * s;
40  mat[6] = az * ax * (1 - c) - ay * s;
41  mat[7] = az * ay * (1 - c) + ax * s;
42  mat[8] = c + az * az * (1 - c);
43  return mat;
44}
45
46// get rotation matrix by using quaternion
47float[] quaternion2Mat3x3(float qx, float qy, float qz, float qw) {
48  float[] mat = new float[9];
49  mat[0] = 1 - 2 * qy * qy - 2 * qz * qz;
50  mat[1] = 2 * qx * qy - 2 * qz * qw;
51  mat[2] = 2 * qx * qz + 2 * qy * qw;
52  mat[3] = 2 * qx * qy + 2 * qz * qw;
53  mat[4] = 1 - 2 * qx * qx - 2 * qz * qz;
54  mat[5] = 2 * qy * qz - 2 * qx * qw;
55  mat[6] = 2 * qx * qz - 2 * qy * qw;
56  mat[7] = 2 * qy * qz + 2 * qx * qw;
57  mat[8] = 1 - 2 * qx * qx - 2 * qy * qy;
58  return mat;
59}
60
61// tranpose a 3x3 matrix
62float[] transpose3x3(float[] mat) {
63  float[] Tmat = new float[9];
64  for (int i = 0; i < 3; i++)
65    for (int j = 0; j < 3; j++) {
66      Tmat[i * 3 + j] = mat[j * 3 + i];
67    }
68  return Tmat;
69}
70
71// multiply a matrix with vector
72PVector MatxVec3(float[] mat, PVector v) {
73  float[] vec = v.array();
74  float[] res = new float[3];
75  for (int i = 0; i < 3; i++) {
76    res[i] = 0.0f;
77    for (int j = 0; j < 3; j++) {
78      res[i] += mat[i * 3 + j] * vec[j];
79    }
80  }
81  return new PVector(res[0], res[1], res[2]);
82}
83