Drawing point cloud + planes on Canvas

Java-only version of SkAR: drawing on android Canvas

Bug: skia:
Change-Id: I76285aa85c7523ad9ae213d32a159b671d8aa30f
Reviewed-on: https://skia-review.googlesource.com/141048
Reviewed-by: Mike Reed <reed@google.com>

3 files changed, 311 insertions, 157 deletions

diff --git a/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/DrawManager.java b/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/DrawManager.java
index bf85a767c2..00243193cd 100644
--- a/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/DrawManager.java
+++ b/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/DrawManager.java
@@ -1,24 +1,50 @@
 package com.google.ar.core.examples.java.helloskar;
 
+import android.content.Context;
+import android.graphics.Bitmap;
+import android.graphics.BitmapFactory;
+import android.graphics.BitmapShader;
 import android.graphics.Canvas;
+import android.graphics.Color;
 import android.graphics.ColorFilter;
 import android.graphics.Paint;
+import android.graphics.Path;
+import android.graphics.PointF;
+import android.graphics.PorterDuff;
+import android.graphics.PorterDuffColorFilter;
 import android.graphics.RectF;
-
+import android.graphics.Shader;
+import android.opengl.Matrix;
+import com.google.ar.core.Plane;
+import com.google.ar.core.PointCloud;
+import com.google.ar.core.Pose;
+import com.google.ar.core.TrackingState;
 import com.google.skar.SkARMatrix;
 import com.google.skar.SkARUtil;
-
+import java.io.IOException;
+import java.nio.FloatBuffer;
 import java.util.ArrayList;
+import java.util.Collection;
+
+/**
+ * Sample class that handles drawing different types of geometry using the matrices provided
+ * by ARCore. The matrices are handled by SkARMatrix in order to be passed to the drawing
+ * Canvas.
+ */
 
 public class DrawManager {
     private float[] projectionMatrix = new float[16];
     private float[] viewMatrix = new float[16];
-    private float[] viewportMatrix = new float[16];
+    private float viewportWidth;
+    private float viewportHeight;
     private ColorFilter lightFilter;
+    private BitmapShader planeShader;
+    private Bitmap planeTexture;
     public ArrayList<float[]> modelMatrices = new ArrayList<>();
 
-    public void updateViewportMatrix(float width, float height) {
-        viewportMatrix = SkARMatrix.createViewportMatrix(width, height);
+    public void updateViewport(float width, float height) {
+        viewportWidth = width;
+        viewportHeight = height;
     }
 
     public void updateProjectionMatrix(float[] projectionMatrix) {
@@ -33,6 +59,7 @@ public class DrawManager {
         lightFilter = SkARUtil.createLightCorrectionColorFilter(colorCorr);
     }
 
+    // Sample function for drawing a circle
     public void drawCircle(Canvas canvas) {
         if (modelMatrices.isEmpty()) {
             return;
@@ -43,11 +70,12 @@ public class DrawManager {
 
         canvas.save();
         canvas.setMatrix(SkARMatrix.createPerspectiveMatrix(modelMatrices.get(0),
-                viewMatrix, projectionMatrix, viewportMatrix));
+                viewMatrix, projectionMatrix, viewportWidth, viewportHeight));
         canvas.drawCircle(0, 0, 0.1f, p);
         canvas.restore();
     }
 
+    // Sample function for drawing a rect
     public void drawRect(Canvas canvas) {
         if (modelMatrices.isEmpty()) {
             return;
@@ -57,12 +85,13 @@ public class DrawManager {
         p.setARGB(180, 0, 0, 255);
         canvas.save();
         canvas.setMatrix(SkARMatrix.createPerspectiveMatrix(modelMatrices.get(0),
-                viewMatrix, projectionMatrix, viewportMatrix));
+                viewMatrix, projectionMatrix, viewportWidth, viewportHeight));
         RectF rect = new RectF(0, 0, 0.2f, 0.2f);
         canvas.drawRect(rect, p);
         canvas.restore();
     }
 
+    // Sample function for drawing text on a canvas
     public void drawText(Canvas canvas, String text) {
         if (modelMatrices.isEmpty()) {
             return;
@@ -73,22 +102,128 @@ public class DrawManager {
         p.setARGB(255, 0, 255, 0);
         p.setTextSize(textSize);
 
-
         float[] scaleMatrix = getTextScaleMatrix(textSize);
-        float[] rotateMatrix = createXYtoXZRotationMatrix();
-        float[] actualModel = new float[16];
-        android.opengl.Matrix.setIdentityM(actualModel, 0);
-
-        android.opengl.Matrix.multiplyMM(actualModel, 0, scaleMatrix, 0, rotateMatrix, 0);
-        android.opengl.Matrix.multiplyMM(actualModel, 0, modelMatrices.get(0), 0, actualModel, 0);
+        float[] rotateMatrix = SkARMatrix.createXYtoXZRotationMatrix();
+        float[][] matrices = { scaleMatrix, rotateMatrix, modelMatrices.get(0), viewMatrix,
+                                projectionMatrix,
+                                SkARMatrix.createViewportMatrix(viewportWidth, viewportHeight)};
 
         canvas.save();
-        canvas.setMatrix(SkARMatrix.createPerspectiveMatrix(actualModel,
-                viewMatrix, projectionMatrix, viewportMatrix, false));
+        canvas.setMatrix(SkARMatrix.createMatrixFrom4x4(SkARMatrix.multiplyMatrices4x4(matrices)));
         canvas.drawText(text, 0, 0, p);
         canvas.restore();
     }
 
+    // Sample function for drawing the AR point cloud
+    public void drawPointCloud(Canvas canvas, PointCloud cloud) {
+        FloatBuffer points = cloud.getPoints();
+        int numberOfPoints = points.remaining() / 4;
+
+        float[][] matrices = {viewMatrix, projectionMatrix, SkARMatrix.createViewportMatrix(viewportWidth, viewportHeight)};
+        float[] vpv = SkARMatrix.multiplyMatrices4x4(matrices);
+
+        float[] pointsToDraw = new float[numberOfPoints * 2];
+        for (int i = 0; i < numberOfPoints; i++) {
+            float[] point = {points.get(i * 4), points.get(i * 4 + 1), points.get(i * 4 + 2), 1};
+            PointF p = SkARMatrix.multiplyMatrixVector(vpv, point, true);
+            pointsToDraw[i * 2] = p.x;
+            pointsToDraw[i * 2 + 1] = p.y;
+        }
+
+        Paint p = new Paint();
+        p.setARGB(220, 20, 232, 255);
+        p.setStrokeCap(Paint.Cap.SQUARE);
+        p.setStrokeWidth(6.0f);
+
+        canvas.save();
+        float[] id = new float[16];
+        Matrix.setIdentityM(id, 0);
+        android.graphics.Matrix identity = SkARMatrix.createMatrixFrom4x4(id);
+        canvas.setMatrix(identity);
+        canvas.drawPoints(pointsToDraw, p);
+        canvas.restore();
+    }
+
+
+    // Sample function for drawing AR planes
+    public void drawPlanes(Canvas canvas, Pose cameraPose, Collection<Plane> allPlanes) {
+        if (allPlanes.size() <= 0) {
+            return;
+        }
+
+        for (Plane plane : allPlanes) {
+            Plane subsumePlane = plane.getSubsumedBy();
+            if (plane.getTrackingState() != TrackingState.TRACKING || subsumePlane != null) {
+                continue;
+            }
+
+            float distance = calculateDistanceToPlane(plane.getCenterPose(), cameraPose);
+            if (distance < 0) { // Plane is back-facing.
+                continue;
+            }
+
+
+            // Get plane model matrix
+            float[] model = new float[16];
+            plane.getCenterPose().toMatrix(model, 0);
+
+            // Initial rotation
+            float[] initRot = SkARMatrix.createXYtoXZRotationMatrix();
+
+            float[] initScale = new float[16];
+            Matrix.setIdentityM(initScale, 0);
+            Matrix.scaleM(initScale, 0, 1f, 1f, 1f);
+            android.graphics.Matrix scale = SkARMatrix.createMatrixFrom4x4(SkARMatrix.multiplyMatrices4x4(new float[][] {initScale}));
+
+            // Matrix = mvpv
+            float[][] matrices = {initRot, model, viewMatrix, projectionMatrix, SkARMatrix.createViewportMatrix(viewportWidth, viewportHeight)};
+            android.graphics.Matrix mvpv = SkARMatrix.createMatrixFrom4x4(SkARMatrix.multiplyMatrices4x4(matrices));
+
+            canvas.save();
+
+            canvas.setMatrix(mvpv);
+
+            drawPlaneAsPath(canvas, plane);
+            canvas.restore();
+        }
+    }
+
+    // Helper function that draws an AR plane using a path
+    private void drawPlaneAsPath(Canvas canvas, Plane plane) {
+        int vertsSize = plane.getPolygon().limit() / 2;
+        FloatBuffer polygon = plane.getPolygon();
+        polygon.rewind();
+
+        Path path = new Path();
+        path.moveTo(polygon.get(0), polygon.get(1));
+        for (int i = 1; i < vertsSize; i++) {
+            path.lineTo(polygon.get(i * 2), polygon.get(i * 2 + 1));
+        }
+        path.close();
+
+        Paint p = new Paint();
+        p.setShader(planeShader);
+        p.setColorFilter(new PorterDuffColorFilter(Color.argb(0.4f, 1, 0, 0), PorterDuff.Mode.SRC_ATOP));
+        p.setAlpha(120);
+
+        //canvas.drawPath(path, p); TODO: enable this when path is drawn on GPU
+
+        RectF r = new RectF();
+        path.computeBounds(r, true);
+        canvas.drawRect(r, p);
+    }
+
+    public void initializePlaneShader(Context context, String gridDistanceTextureName) throws IOException {
+        // Read the texture.
+        planeTexture =
+                BitmapFactory.decodeStream(context.getAssets().open(gridDistanceTextureName));
+        // Set up the shader
+        planeShader = new BitmapShader(planeTexture, Shader.TileMode.REPEAT, Shader.TileMode.REPEAT);
+        android.graphics.Matrix m = new android.graphics.Matrix();
+        m.setScale(0.0005f, 0.0005f);
+        planeShader.setLocalMatrix(m);
+    }
+
     private float[] getTextScaleMatrix(float size) {
         float scaleFactor = 1 / (size * 10);
         float[] initScale = new float[16];
@@ -97,10 +232,59 @@ public class DrawManager {
         return initScale;
     }
 
-    private float[] createXYtoXZRotationMatrix() {
-        float[] skiaRotation = new float[16];
-        android.opengl.Matrix.setIdentityM(skiaRotation, 0);
-        android.opengl.Matrix.rotateM(skiaRotation, 0, 90, 1, 0, 0);
-        return skiaRotation;
+    public static float calculateDistanceToPlane(Pose planePose, Pose cameraPose) {
+        float[] normal = new float[3];
+        float cameraX = cameraPose.tx();
+        float cameraY = cameraPose.ty();
+        float cameraZ = cameraPose.tz();
+        // Get transformed Y axis of plane's coordinate system.
+        planePose.getTransformedAxis(1, 1.0f, normal, 0);
+        // Compute dot product of plane's normal with vector from camera to plane center.
+        return (cameraX - planePose.tx()) * normal[0]
+                + (cameraY - planePose.ty()) * normal[1]
+                + (cameraZ - planePose.tz()) * normal[2];
+    }
+
+    // Drawing plane with drawVertices
+    // TODO: Wait until latest Android release for this to work..
+    private void drawPlane(Canvas canvas, android.graphics.Matrix mvpv, Plane plane) {
+        int vertsSize = plane.getPolygon().limit() / 2;
+        FloatBuffer polygon = plane.getPolygon();
+        float[] polyVerts = new float[vertsSize * 2];
+        int[] polyColors = new int[vertsSize];
+
+        for (int i = 0; i < vertsSize; i++) {
+            polyVerts[i * 2] = polygon.get(i * 2);
+            polyVerts[i * 2 + 1] = polygon.get(i * 2 + 1);
+
+            polyColors[i] = Color.RED;
+        }
+
+        // Construct indices through a list
+        ArrayList<Short> indices = new ArrayList<>();
+        for (int i = 1; i < vertsSize - 1; ++i) {
+            indices.add((short) 0);
+            indices.add((short) i);
+            indices.add((short) (i + 1));
+        }
+
+        // Copy indices into an array
+        short[] indicesArray = new short[indices.size()];
+        for (int i = 0; i < indices.size(); i++) {
+            indicesArray[i] = indices.get(i);
+        }
+
+        Paint p = new Paint();
+        p.setShader(planeShader);
+        p.setColor(Color.RED);
+        p.setAlpha(100);
+
+        canvas.save();
+        canvas.setMatrix(mvpv);
+
+        canvas.drawVertices(Canvas.VertexMode.TRIANGLE_FAN, vertsSize, polyVerts, 0,
+                null, 0, polyColors, 0, indicesArray, 0,
+                indicesArray.length, p);
+        canvas.restore();
     }
 }
diff --git a/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/HelloSkARActivity.java b/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/HelloSkARActivity.java
index a8ccb53478..b2d3cfd534 100644
--- a/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/HelloSkARActivity.java
+++ b/platform_tools/android/apps/skar_java/src/main/java/com/google/ar/core/examples/java/helloskar/HelloSkARActivity.java
@@ -46,8 +46,6 @@ import com.google.ar.core.examples.java.common.helpers.FullScreenHelper;
 import com.google.ar.core.examples.java.common.helpers.SnackbarHelper;
 import com.google.ar.core.examples.java.common.helpers.TapHelper;
 import com.google.ar.core.examples.java.common.rendering.BackgroundRenderer;
-import com.google.ar.core.examples.java.common.rendering.PlaneRenderer;
-import com.google.ar.core.examples.java.common.rendering.PointCloudRenderer;
 import com.google.ar.core.exceptions.CameraNotAvailableException;
 import com.google.ar.core.exceptions.UnavailableApkTooOldException;
 import com.google.ar.core.exceptions.UnavailableArcoreNotInstalledException;
@@ -66,6 +64,7 @@ import javax.microedition.khronos.opengles.GL10;
  * ARCore API. The application will display any detected planes and will allow the user to tap on a
  * plane to place 2D objects
  */
+
 public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceView.Renderer {
     private static final String TAG = HelloSkARActivity.class.getSimpleName();
 
@@ -83,12 +82,10 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
     private DisplayRotationHelper displayRotationHelper;
     private TapHelper tapHelper;
 
-    //Renderers
+    // OpenGL background renderer
     private final BackgroundRenderer backgroundRenderer = new BackgroundRenderer();
-    private final PlaneRenderer planeRenderer = new PlaneRenderer();
-    private final PointCloudRenderer pointCloudRenderer = new PointCloudRenderer();
 
-    //2D Renderer
+    // 2D Renderer
     private DrawManager drawManager = new DrawManager();
 
     // Temporary matrix allocated here to reduce number of allocations for each frame.
@@ -222,9 +219,8 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
         // Prepare the rendering objects. This involves reading shaders, so may throw an IOException.
         try {
             // Create the texture and pass it to ARCore session to be filled during update().
-            backgroundRenderer.createOnGlThread(/*context=*/ this);
-            planeRenderer.createOnGlThread(/*context=*/ this, "models/trigrid.png");
-            pointCloudRenderer.createOnGlThread(/*context=*/ this);
+            backgroundRenderer.createOnGlThread( this);
+            drawManager.initializePlaneShader(this, "models/trigrid.png");
         } catch (IOException e) {
             Log.e(TAG, "Failed to read an asset file", e);
         }
@@ -236,7 +232,7 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
         GLES20.glViewport(0, 0, width, height);
 
         // Send viewport information to 2D AR drawing manager
-        drawManager.updateViewportMatrix(width, height);
+        drawManager.updateViewport(width, height);
     }
 
     @Override
@@ -264,7 +260,7 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
                     // Creates an anchor if a plane or an oriented point was hit.
                     if ((trackable instanceof Plane
                             && ((Plane) trackable).isPoseInPolygon(hit.getHitPose())
-                            && (PlaneRenderer.calculateDistanceToPlane(hit.getHitPose(), camera.getPose())
+                            && (DrawManager.calculateDistanceToPlane(hit.getHitPose(), camera.getPose())
                             > 0))
                             || (trackable instanceof Point
                             && ((Point) trackable).getOrientationMode()
@@ -279,7 +275,8 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
                 }
             }
 
-            // Draw background.
+            // Draw background with OpenGL.
+            // TODO: possibly find a way to extract texture and draw on Canvas
             backgroundRenderer.draw(frame);
 
             // If not tracking, don't draw objects
@@ -301,30 +298,39 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
             frame.getLightEstimate().getColorCorrection(colorCorrectionRgba, 0);
             drawManager.updateLightColorFilter(colorCorrectionRgba);
 
-            PointCloud pointCloud = frame.acquirePointCloud();
-            pointCloudRenderer.update(pointCloud);
-            pointCloudRenderer.draw(viewmtx, projmtx);
-            pointCloud.release();
-
-            // Check if we detected at least one plane. If so, hide the loading message.
-            if (messageSnackbarHelper.isShowing()) {
-                for (Plane plane : session.getAllTrackables(Plane.class)) {
-                    if (plane.getType() == com.google.ar.core.Plane.Type.HORIZONTAL_UPWARD_FACING
-                            && plane.getTrackingState() == TrackingState.TRACKING) {
-                        messageSnackbarHelper.hide(this);
-                        break;
+            // Drawing on Canvas (SurfaceView)
+            if (arSurfaceView.isRunning()) {
+                // Lock canvas
+                SurfaceHolder holder = arSurfaceView.getHolder();
+                Canvas canvas = holder.lockHardwareCanvas();
+                canvas.drawColor(Color.TRANSPARENT, PorterDuff.Mode.CLEAR);
+
+                // Draw point cloud
+                PointCloud pointCloud = frame.acquirePointCloud();
+                drawPointCloud(canvas, pointCloud);
+                pointCloud.release();
+
+                // Draw planes
+                // Check if we detected at least one plane. If so, hide the loading message.
+                if (messageSnackbarHelper.isShowing()) {
+                    for (Plane plane : session.getAllTrackables(Plane.class)) {
+                        if (plane.getType() == com.google.ar.core.Plane.Type.HORIZONTAL_UPWARD_FACING
+                                && plane.getTrackingState() == TrackingState.TRACKING) {
+                            messageSnackbarHelper.hide(this);
+                            break;
+                        }
                     }
                 }
-            }
-            // Visualize planes.
-            planeRenderer.drawPlanes(
-                    session.getAllTrackables(Plane.class), camera.getDisplayOrientedPose(), projmtx);
 
-            // Draw models using Canvas
-            if (arSurfaceView.isRunning()) {
-                drawModels();
-            }
+                // Draw planes
+                drawPlanes(canvas, camera);
 
+                // Draw models
+                drawModels(canvas);
+
+                // Unlock canvas
+                holder.unlockCanvasAndPost(canvas);
+            }
 
         } catch (Throwable t) {
             // Avoid crashing the application due to unhandled exceptions.
@@ -332,10 +338,16 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
         }
     }
 
-    private void drawModels() {
-        SurfaceHolder holder = arSurfaceView.getHolder();
-        Canvas canvas = holder.lockHardwareCanvas();
-        canvas.drawColor(Color.TRANSPARENT, PorterDuff.Mode.CLEAR);
+    // Helper drawing functions that invoke drawManager
+    private void drawPlanes(Canvas canvas, Camera camera) {
+        drawManager.drawPlanes(canvas, camera.getPose(), session.getAllTrackables(Plane.class));
+    }
+
+    private void drawPointCloud(Canvas canvas, PointCloud cloud) {
+        drawManager.drawPointCloud(canvas, cloud);
+    }
+
+    private void drawModels(Canvas canvas) {
         for (Anchor anchor : anchors) {
             if (anchor.getTrackingState() != TrackingState.TRACKING) {
                 continue;
@@ -349,7 +361,5 @@ public class HelloSkARActivity extends AppCompatActivity implements GLSurfaceVie
             drawManager.drawCircle(canvas);
             drawManager.drawText(canvas, "HelloSkAR");
         }
-        holder.unlockCanvasAndPost(canvas);
-
     }
 }
diff --git a/platform_tools/android/apps/skar_java/src/main/java/com/google/skar/SkARMatrix.java b/platform_tools/android/apps/skar_java/src/main/java/com/google/skar/SkARMatrix.java
index fc9333c6df..13c249ce36 100644
--- a/platform_tools/android/apps/skar_java/src/main/java/com/google/skar/SkARMatrix.java
+++ b/platform_tools/android/apps/skar_java/src/main/java/com/google/skar/SkARMatrix.java
@@ -1,6 +1,7 @@
 package com.google.skar;
 
 import android.graphics.Matrix;
+import android.graphics.PointF;
 
 /**
  * Provides static methods for matrix manipulation. Input matrices are assumed to be 4x4
@@ -10,70 +11,8 @@ import android.graphics.Matrix;
  */
 
 public class SkARMatrix {
-    /**
-     * Returns an android.graphics.Matrix that can be used on a Canvas to draw a 2D object in
-     * perspective. Objects will be rotated towards the XZ plane. Undefined behavior when any of
-     * the matrices are not of size 16, or are null.
-     *
-     * @param model      4x4 model matrix of the object to be drawn (global/world)
-     * @param view       4x4 camera view matrix (brings objects to camera origin and orientation)
-     * @param projection 4x4 projection matrix
-     * @param viewport   4x4 viewport matrix
-     * @return 3x3 matrix that puts a 2D objects in perspective on a Canvas
-     */
-
-    public static Matrix createPerspectiveMatrix(float[] model, float[] view, float[] projection,
-                                                 float[] viewport) {
-        float[] skiaRotation = createXYtoXZRotationMatrix();
-        float[][] matrices = {skiaRotation, model, view, projection, viewport};
-        return createMatrixFrom4x4Array(matrices);
-    }
-
-    /**
-     * Returns an android.graphics.Matrix that can be used on a Canvas to draw a 2D object in
-     * perspective. Undefined behavior when any of the matrices are not of size 16, or are null.
-     *
-     * @param model       4x4 model matrix of the object to be drawn (global/world)
-     * @param view        4x4 camera view matrix (brings objects to camera origin and orientation)
-     * @param projection  4x4 projection matrix
-     * @param viewport    4x4 viewport matrix
-     * @param rotatePlane specifies if object should be from the XY plane to the XZ plane
-     * @return 3x3 matrix that puts a 2D objects in perspective on a Canvas
-     */
-
-    public static Matrix createPerspectiveMatrix(float[] model, float[] view, float[] projection,
-                                                 float[] viewport, boolean rotatePlane) {
-        if (rotatePlane) {
-            return createPerspectiveMatrix(model, view, projection, viewport);
-        } else {
-            float[][] matrices = {model, view, projection, viewport};
-            return createMatrixFrom4x4Array(matrices);
-        }
-    }
-
-    /**
-     * Returns an android.graphics.Matrix that can be used on a Canvas to draw a 2D object in
-     * perspective. Undefined behavior when any of the matrices are not of size 16, or are null.
-     *
-     * @param model          4x4 model matrix of the object to be drawn (global/world)
-     * @param view           4x4 camera view matrix (brings objects to camera origin and orientation)
-     * @param projection     4x4 projection matrix
-     * @param viewPortWidth  width of viewport of GLSurfaceView
-     * @param viewPortHeight height of viewport of GLSurfaceView
-     * @param rotatePlane    specifies if object should be from the XY plane to the XZ plane
-     * @return 3x3 matrix that puts a 2D objects in perspective on a Canvas
-     */
 
-    public static Matrix createPerspectiveMatrix(float[] model, float[] view, float[] projection,
-                                                 float viewPortWidth, float viewPortHeight, boolean rotatePlane) {
-        if (rotatePlane) {
-            return createPerspectiveMatrix(model, view, projection, viewPortWidth, viewPortHeight);
-        } else {
-            float[] viewPort = createViewportMatrix(viewPortWidth, viewPortHeight);
-            float[][] matrices = {model, view, projection, viewPort};
-            return createMatrixFrom4x4Array(matrices);
-        }
-    }
+    /******************* PUBLIC FUNCTIONS ***********************/
 
     /**
      * Returns an android.graphics.Matrix that can be used on a Canvas to draw a 2D object in
@@ -119,31 +58,19 @@ public class SkARMatrix {
      * to rotate them from the XY plane to the XZ plane.
      */
 
-    private static float[] createXYtoXZRotationMatrix() {
+    public static float[] createXYtoXZRotationMatrix() {
         float[] rotation = new float[16];
         android.opengl.Matrix.setIdentityM(rotation, 0);
         android.opengl.Matrix.rotateM(rotation, 0, 90, 1, 0, 0);
         return rotation;
     }
 
-    /**
-     * Returns an android.graphics.Matrix resulting from a 9-float matrix array in row-major order.
-     * Undefined behavior when the array is not of size 9 or is null.
-     *
-     * @param m3 9-float matrix array in row-major order
-     */
-
-    public static Matrix createMatrixFrom3x3(float[] m3) {
-        Matrix m = new Matrix();
-        m.setValues(m3);
-        return m;
-    }
 
     /**
      * Returns an android.graphics.Matrix resulting from a 16-float matrix array in column-major order
      * Undefined behavior when the array is not of size 16 or is null.
      *
-     * @param m4
+     * @param m4 16-float matrix in column-major order
      */
 
     public static Matrix createMatrixFrom4x4(float[] m4) {
@@ -152,6 +79,46 @@ public class SkARMatrix {
     }
 
     /**
+     * Returns an android.graphics.PointF resulting from the multiplication of a Vector of 4 floats
+     * with a 4x4 float Matrix. The return PointF is the (x, y) values of m4 * v4
+     * @param m4                16-float matrix in column-major order
+     * @param v4                4-float vector
+     * @param perspectiveDivide if true, divide return value by the w-coordinate
+     * @return                  PointF resulting from taking the (x, y) values of m4 * v4
+     */
+    public static PointF multiplyMatrixVector(float[] m4, float[] v4, boolean perspectiveDivide) {
+        float[] result = new float[4];
+        android.opengl.Matrix.multiplyMV(result, 0, m4, 0, v4, 0);
+        if (perspectiveDivide) {
+            return new PointF(result[0] / result[3], result[1] / result[3]);
+        }
+
+        return new PointF(result[0], result[1]);
+    }
+
+    /**
+     * Returns a 16-float matrix in column-major order resulting from the multiplication of matrices.
+     * e.g: m4Array = {m1, m2, m3} --> returns m = m3 * m2 * m1
+     * Undefined behavior when the array is empty, null, or contains arrays not of size 9 (or null)
+     *
+     * @param m4Array array of 16-float matrices in column-major order
+     */
+
+    public static float[] multiplyMatrices4x4(float[][] m4Array) {
+        float[] result = new float[16];
+        android.opengl.Matrix.setIdentityM(result, 0);
+        float[] rhs = result;
+        for (int i = 0; i < m4Array.length; i++) {
+            float[] lhs = m4Array[i];
+            android.opengl.Matrix.multiplyMM(result, 0, lhs, 0, rhs, 0);
+            rhs = result;
+        }
+        return result;
+    }
+
+    /******************* PRIVATE FUNCTIONS ***********************/
+
+    /**
      * Returns an android.graphics.Matrix resulting from the concatenation of 16-float matrices
      * in column-major order from left to right.
      * e.g: m4Array = {m1, m2, m3} --> returns m = m3 * m2 * m1
@@ -160,12 +127,25 @@ public class SkARMatrix {
      * @param m4Array array of 16-float matrices in column-major order
      */
 
-    public static Matrix createMatrixFrom4x4Array(float[][] m4Array) {
+    private static Matrix createMatrixFrom4x4Array(float[][] m4Array) {
         float[] result = multiplyMatrices4x4(m4Array);
         return createMatrixFrom4x4(result);
     }
 
     /**
+     * Returns an android.graphics.Matrix resulting from a 9-float matrix array in row-major order.
+     * Undefined behavior when the array is not of size 9 or is null.
+     *
+     * @param m3 9-float matrix array in row-major order
+     */
+
+    private static Matrix createMatrixFrom3x3(float[] m3) {
+        Matrix m = new Matrix();
+        m.setValues(m3);
+        return m;
+    }
+
+    /**
      * Returns a 9-float matrix in row-major order given a 16-float matrix in column-major order.
      * This will drop the Z column and row.
      * Undefined behavior when the array is not of size 9 or is null.
@@ -188,24 +168,4 @@ public class SkARMatrix {
         }
         return m3;
     }
-
-    /**
-     * Returns a 16-float matrix in column-major order resulting from the multiplication of matrices.
-     * e.g: m4Array = {m1, m2, m3} --> returns m = m3 * m2 * m1
-     * Undefined behavior when the array is empty, null, or contains arrays not of size 9 (or null)
-     *
-     * @param m4Array array of 16-float matrices in column-major order
-     */
-
-    private static float[] multiplyMatrices4x4(float[][] m4Array) {
-        float[] result = new float[16];
-        android.opengl.Matrix.setIdentityM(result, 0);
-        float[] rhs = result;
-        for (int i = 0; i < m4Array.length; i++) {
-            float[] lhs = m4Array[i];
-            android.opengl.Matrix.multiplyMM(result, 0, lhs, 0, rhs, 0);
-            rhs = result;
-        }
-        return result;
-    }
 }