Add Ray Tracing

Add braces

Change-Id: I5355ccd8f745dfbd4fe3923a81aa3c9f8fda07b3

1 files changed, 46 insertions, 16 deletions

diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
index e6d412d28..dbba327b7 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
@@ -6,25 +6,57 @@ class MotionField {
     motion_field = new ArrayList<PVector>();
   }
 
-  void update(ArrayList<PVector> last_positions,
-              ArrayList<PVector> current_positions, int[] render_list) {
-    // build motion field
+  void update(Camera last_cam, Camera current_cam, PointCloud point_cloud,
+              BVH bvh) {
+    // clear motion field
     motion_field = new ArrayList<PVector>();
     int r_num = height / block_size, c_num = width / block_size;
     for (int i = 0; i < r_num * c_num; i++)
       motion_field.add(new PVector(0, 0, 0));
-    // accumate motion vector of pixel in each block
-    for (int i = 0; i < height; i++)
-      for (int j = 0; j < width; j++) {
-        if (render_list[i * width + j] == -1) continue;
-        PVector cur_pos = current_positions.get(render_list[i * width + j]);
-        PVector last_pos = last_positions.get(render_list[i * width + j]);
-        int idx = i / block_size * c_num + j / block_size;
-        PVector mv = PVector.sub(last_pos, cur_pos);
-        PVector acc_mv = motion_field.get(idx);
-        motion_field.set(
-            idx, new PVector(acc_mv.x + mv.x, acc_mv.y + mv.y, acc_mv.z + 1));
+    // estimate motion vector of each point in point cloud
+    for (int i = 0; i < point_cloud.size(); i++) {
+      PVector p = point_cloud.getPosition(i);
+      PVector p0 = current_cam.project(p);
+      PVector p1 = last_cam.project(p);
+      int row = int((p0.y + height / 2.0f) / block_size);
+      int col = int((p0.x + width / 2.0f) / block_size);
+      if (row >= 0 && row < r_num && col >= 0 && col < c_num) {
+        PVector accu = motion_field.get(row * c_num + col);
+        accu.x += p1.x - p0.x;
+        accu.y += p1.y - p0.y;
+        accu.z += 1;
       }
+    }
+    // if some blocks do not have point, then use ray tracing to see if they are
+    // in triangles
+    for (int i = 0; i < r_num; i++)
+      for (int j = 0; j < c_num; j++) {
+        PVector accu = motion_field.get(i * c_num + j);
+        if (accu.z > 0) {
+          continue;
+        }
+        // use the center of the block to generate view ray
+        float cx = j * block_size + block_size / 2.0f - width / 2.0f;
+        float cy = i * block_size + block_size / 2.0f - height / 2.0f;
+        float cz = 0.5f * height / tan(current_cam.fov / 2.0f);
+        PVector dir = new PVector(cx, cy, cz);
+        float[] camMat = current_cam.getCameraMat();
+        dir = MatxVec3(transpose3x3(camMat), dir);
+        dir.normalize();
+        Ray r = new Ray(current_cam.pos, dir);
+        // ray tracing
+        float[] param = new float[4];
+        param[0] = Float.POSITIVE_INFINITY;
+        if (bvh.intersect(r, param)) {
+          PVector p = new PVector(param[1], param[2], param[3]);
+          PVector p0 = current_cam.project(p);
+          PVector p1 = last_cam.project(p);
+          accu.x += p1.x - p0.x;
+          accu.y += p1.y - p0.y;
+          accu.z += 1;
+        }
+      }
+    // estimate the motion vector of each block
     for (int i = 0; i < r_num * c_num; i++) {
       PVector mv = motion_field.get(i);
       if (mv.z > 0) {
@@ -34,8 +66,6 @@ class MotionField {
   }
 
   void render() {
-    // ortho(-width,0,-height,0);
-    // camera(0,0,0,0,0,1,0,1,0);
     int r_num = height / block_size, c_num = width / block_size;
     for (int i = 0; i < r_num; i++)
       for (int j = 0; j < c_num; j++) {