diff --git a/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningActivity.java b/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningActivity.java index ae1e5e99..706fdd0e 100644 --- a/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningActivity.java +++ b/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningActivity.java @@ -135,9 +135,13 @@ protected void onCreate(Bundle savedInstanceState) { // Configure OpenGL renderer mRenderer = setupGLViewAndRenderer(); - mPoses = new TangoPoseData[3]; + } - // Set the number of loop closures to zero at start. + /** + * Initializes pose data we keep track of. To be done + */ + private void initializePoseData() { + mPoses = new TangoPoseData[3]; mStart2DevicePoseCount = 0; mAdf2DevicePoseCount = 0; mAdf2StartPoseCount = 0; @@ -173,6 +177,9 @@ protected void onPause() { protected void onResume() { super.onResume(); + // Reset pose data and start counting from resume. + initializePoseData(); + // Clear the relocalization state: we don't know where the device has been since our app was paused. mIsRelocalized = false; diff --git a/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningRajawaliRenderer.java b/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningRajawaliRenderer.java index 3497daaf..b58a938f 100644 --- a/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningRajawaliRenderer.java +++ b/AreaLearningJava/app/src/main/java/com/projecttango/experiments/javaarealearning/AreaLearningRajawaliRenderer.java @@ -22,7 +22,7 @@ import com.google.atap.tangoservice.TangoPoseData; import com.projecttango.rajawali.Pose; -import com.projecttango.rajawali.ScenePoseCalcuator; +import com.projecttango.rajawali.ScenePoseCalculator; import com.projecttango.rajawali.TouchViewHandler; import com.projecttango.rajawali.renderables.FrustumAxes; import com.projecttango.rajawali.renderables.Grid; @@ -115,7 +115,7 @@ protected void onRender(long ellapsedRealtime, double deltaTime) { * updated. */ public synchronized void updateDevicePose(TangoPoseData tangoPoseData, boolean isRelocalized) { - mDevicePose = ScenePoseCalcuator.toOpenGLPose(tangoPoseData); + mDevicePose = ScenePoseCalculator.toOpenGLPose(tangoPoseData); mIsRelocalized = isRelocalized; mPoseUpdated = true; } diff --git a/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityActivity.java b/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityActivity.java index 4fb55ad5..3429584f 100644 --- a/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityActivity.java +++ b/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityActivity.java @@ -35,17 +35,23 @@ import com.google.atap.tangoservice.TangoXyzIjData; import java.util.ArrayList; +import java.util.concurrent.atomic.AtomicBoolean; +import com.projecttango.rajawali.DeviceExtrinsics; +import com.projecttango.rajawali.ScenePoseCalculator; import com.projecttango.rajawali.ar.TangoRajawaliView; import com.projecttango.tangosupport.TangoPointCloudManager; import com.projecttango.tangosupport.TangoSupport; import com.projecttango.tangosupport.TangoSupport.IntersectionPointPlaneModelPair; +import org.rajawali3d.scene.ASceneFrameCallback; + /** * An example showing how to build a very simple augmented reality application * in Java. It uses Rajawali to do the rendering through the utility classes * TangoRajawaliRenderer and TangoRajawaliView from - * TangoUtils. It also uses the TangoSupportLibrary to do plane fitting using + * TangoUtils. + * It also uses the TangoSupportLibrary to do plane fitting using * the PointCloud data. Whenever the user clicks on the camera display, plane * detection will be done on the surface closest to the click location and a 3D * object will be placed in the scene anchored in that location. @@ -61,6 +67,9 @@ * it to the view with the view's setSurfaceRenderer method. The implementation * of the 3D world is done by subclassing the Renderer, just like any other * Rajawali application. + * The Rajawali Scene camera is updated via a Rajawali Scene Frame Callback + * whenever a new RGB frame is rendered to the background texture, thus generating + * the Augmented Reality effect. *

* Note that it is important to include the KEY_BOOLEAN_LOWLATENCYIMUINTEGRATION * configuration parameter in order to achieve best results synchronizing the @@ -70,10 +79,16 @@ public class AugmentedRealityActivity extends Activity implements View.OnTouchLi private static final String TAG = AugmentedRealityActivity.class.getSimpleName(); private TangoRajawaliView mGLView; private AugmentedRealityRenderer mRenderer; - private TangoCameraIntrinsics mCameraIntrinsics; + private TangoCameraIntrinsics mIntrinsics; + private DeviceExtrinsics mExtrinsics; private TangoPointCloudManager mPointCloudManager; private Tango mTango; - private boolean mIsConnected; + private AtomicBoolean mIsConnected = new AtomicBoolean(false); + private double mCameraPoseTimestamp = 0; + + public static final TangoCoordinateFramePair FRAME_PAIR = new TangoCoordinateFramePair( + TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, + TangoPoseData.COORDINATE_FRAME_DEVICE); @Override protected void onCreate(Bundle savedInstanceState) { @@ -83,115 +98,165 @@ protected void onCreate(Bundle savedInstanceState) { mGLView.setSurfaceRenderer(mRenderer); mGLView.setOnTouchListener(this); mTango = new Tango(this); + mPointCloudManager = new TangoPointCloudManager(); setContentView(mGLView); } - // Augmented reality view and renderer. - private void startAugmentedReality() { - if (!mIsConnected) { + @Override + protected void onPause() { + super.onPause(); + if (mIsConnected.compareAndSet(true, false)) { + mRenderer.getCurrentScene().clearFrameCallbacks(); + mGLView.disconnectCamera(); + mTango.disconnect(); + } + } + + @Override + protected void onResume() { + super.onResume(); + if (mIsConnected.compareAndSet(false, true)) { try { - mIsConnected = true; - // Connect to color camera. - mGLView.connectToTangoCamera(mTango, - TangoCameraIntrinsics.TANGO_CAMERA_COLOR); - - // Use default configuration for Tango Service, plus low latency - // IMU integration. - TangoConfig config = mTango.getConfig( - TangoConfig.CONFIG_TYPE_DEFAULT); - // NOTE: Low latency integration is necessary to achieve a - // precise alignment of virtual objects with the RBG image and - // produce a good AR effect. - config.putBoolean( - TangoConfig.KEY_BOOLEAN_LOWLATENCYIMUINTEGRATION, true); - config.putBoolean(TangoConfig.KEY_BOOLEAN_DEPTH, true); - mTango.connect(config); - - // No need to add any coordinate frame pairs since we are not - // using pose data. So just initialize. - ArrayList framePairs = - new ArrayList(); - mTango.connectListener(framePairs, new OnTangoUpdateListener() { - @Override - public void onPoseAvailable(TangoPoseData pose) { - // We are not using OnPoseAvailable for this app. - } + connectTango(); + connectRenderer(); + } catch (TangoOutOfDateException e) { + Toast.makeText(getApplicationContext(), + R.string.TangoOutOfDateException, + Toast.LENGTH_SHORT).show(); + } + } + } - @Override - public void onFrameAvailable(int cameraId) { - // Check if the frame available is for the camera we - // want and update its frame on the view. - if (cameraId == TangoCameraIntrinsics.TANGO_CAMERA_COLOR) { - mGLView.onFrameAvailable(); - } - } + /** + * Configures the Tango service and connect it to callbacks. + */ + private void connectTango() { + // Use default configuration for Tango Service, plus low latency + // IMU integration. + TangoConfig config = mTango.getConfig( + TangoConfig.CONFIG_TYPE_DEFAULT); + // NOTE: Low latency integration is necessary to achieve a + // precise alignment of virtual objects with the RBG image and + // produce a good AR effect. + config.putBoolean( + TangoConfig.KEY_BOOLEAN_LOWLATENCYIMUINTEGRATION, true); + config.putBoolean(TangoConfig.KEY_BOOLEAN_DEPTH, true); + mTango.connect(config); - @Override - public void onXyzIjAvailable(TangoXyzIjData xyzIj) { - // Save the cloud and point data for later use. - mPointCloudManager.updateXyzIj(xyzIj); - } + // No need to add any coordinate frame pairs since we are not + // using pose data. So just initialize. + ArrayList framePairs = + new ArrayList(); + mTango.connectListener(framePairs, new OnTangoUpdateListener() { + @Override + public void onPoseAvailable(TangoPoseData pose) { + // We are not using OnPoseAvailable for this app. + } + + @Override + public void onFrameAvailable(int cameraId) { + // Check if the frame available is for the camera we + // want and update its frame on the view. + if (cameraId == TangoCameraIntrinsics.TANGO_CAMERA_COLOR) { + mGLView.onFrameAvailable(); + } + } - @Override - public void onTangoEvent(TangoEvent event) { - // We are not using OnPoseAvailable for this app. + @Override + public void onXyzIjAvailable(TangoXyzIjData xyzIj) { + // Save the cloud and point data for later use. + mPointCloudManager.updateXyzIj(xyzIj); + } + + @Override + public void onTangoEvent(TangoEvent event) { + // We are not using OnPoseAvailable for this app. + } + }); + + // Get extrinsics from device for use in transforms. This needs + // to be done after connecting Tango and listeners. + mExtrinsics = setupExtrinsics(mTango); + mIntrinsics = mTango.getCameraIntrinsics(TangoCameraIntrinsics.TANGO_CAMERA_COLOR); + } + + /** + * Connects the view and renderer to the color camara and callbacks. + */ + private void connectRenderer() { + // Connect to color camera. + mGLView.connectToTangoCamera(mTango, TangoCameraIntrinsics.TANGO_CAMERA_COLOR); + + // Register a Rajawali Scene Frame Callback to update the scene camera pose whenever a new + // RGB frame is rendered. + // (@see https://github.com/Rajawali/Rajawali/wiki/Scene-Frame-Callbacks) + mRenderer.getCurrentScene().registerFrameCallback(new ASceneFrameCallback() { + @Override + public void onPreFrame(long sceneTime, double deltaTime) { + if (!mIsConnected.get()) { + return; + } + // NOTE: This is called from the OpenGL render thread, after all the renderer + // onRender callbacks had a chance to run and before scene objects are rendered + // into the scene. + + // Note that the TangoRajwaliRenderer will update the RGB frame to the background + // texture and update the RGB timestamp before this callback is executed. + + // If a new RGB frame has been rendered, update the camera pose to match. + // NOTE: This doesn't need to be synchronized since the renderer provided timestamp + // is also set in this same OpenGL thread. + double rgbTimestamp = mRenderer.getTimestamp(); + if (rgbTimestamp > mCameraPoseTimestamp) { + // Calculate the device pose at the camera frame update time. + TangoPoseData lastFramePose = mTango.getPoseAtTime(rgbTimestamp, FRAME_PAIR); + if (lastFramePose.statusCode == TangoPoseData.POSE_VALID) { + // Update the camera pose from the renderer + mRenderer.updateRenderCameraPose(lastFramePose, mExtrinsics); + mCameraPoseTimestamp = lastFramePose.timestamp; + } else { + Log.w(TAG, "Unable to get device pose at time: " + rgbTimestamp); } - }); + } + } - // Get extrinsics from device for use in transforms. This needs - // to be done after connecting Tango and listeners. - setupExtrinsics(); + @Override + public void onPreDraw(long sceneTime, double deltaTime) { - mCameraIntrinsics = mTango.getCameraIntrinsics( - TangoCameraIntrinsics.TANGO_CAMERA_COLOR); - mPointCloudManager = new TangoPointCloudManager(); - } catch (TangoOutOfDateException e) { - Toast.makeText(getApplicationContext(), - R.string.TangoOutOfDateException, - Toast.LENGTH_SHORT).show(); } - } + + @Override + public void onPostFrame(long sceneTime, double deltaTime) { + + } + + @Override + public boolean callPreFrame() { + return true; + } + }); } /** * Calculates and stores the fixed transformations between the device and * the various sensors to be used later for transformations between frames. */ - private void setupExtrinsics() { + private static DeviceExtrinsics setupExtrinsics(Tango tango) { // Create camera to IMU transform. TangoCoordinateFramePair framePair = new TangoCoordinateFramePair(); framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU; framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR; - TangoPoseData imuTrgbPose = mTango.getPoseAtTime(0.0, framePair); + TangoPoseData imuTrgbPose = tango.getPoseAtTime(0.0, framePair); // Create device to IMU transform. framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_DEVICE; - TangoPoseData imuTdevicePose = mTango.getPoseAtTime(0.0, framePair); + TangoPoseData imuTdevicePose = tango.getPoseAtTime(0.0, framePair); // Create depth camera to IMU transform. framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH; - TangoPoseData imuTdepthPose = mTango.getPoseAtTime(0.0, framePair); + TangoPoseData imuTdepthPose = tango.getPoseAtTime(0.0, framePair); - mRenderer.setupExtrinsics(imuTdevicePose, imuTrgbPose, imuTdepthPose); - } - - - @Override - protected void onPause() { - super.onPause(); - if (mIsConnected) { - mGLView.disconnectCamera(); - mTango.disconnect(); - mIsConnected = false; - } - } - - @Override - protected void onResume() { - super.onResume(); - if (!mIsConnected) { - startAugmentedReality(); - } + return new DeviceExtrinsics(imuTdevicePose, imuTrgbPose, imuTdepthPose); } @Override @@ -202,7 +267,15 @@ public boolean onTouch(View view, MotionEvent motionEvent) { float v = motionEvent.getY() / view.getHeight(); try { - doFitPlane(u, v); + // Fit a plane on the clicked point using the latest poiont cloud data + TangoPoseData planeFitPose = doFitPlane(u, v, mRenderer.getTimestamp()); + + if (planeFitPose != null) { + // Update the position of the rendered cube to the pose of the detected plane + // This update is made thread safe by the renderer + mRenderer.updateObjectPose(planeFitPose); + } + } catch (TangoException t) { Toast.makeText(getApplicationContext(), R.string.failed_measurement, @@ -220,39 +293,37 @@ public boolean onTouch(View view, MotionEvent motionEvent) { /** * Use the TangoSupport library with point cloud data to calculate the plane - * of the world feature pointed at the location the camera is looking at and - * update the renderer to show a 3D object in that location. + * of the world feature pointed at the location the camera is looking. + * It returns the pose of the fitted plane in a TangoPoseData structure. */ - private void doFitPlane(float u, float v) { - // NOTE: We request measurement at the latest available time. If we - // wanted to be even more precise, we should use the timestamp of the - // RGB image rendered at the time the user clicked the screen. - double measurementTimestamp = 0.0; + private TangoPoseData doFitPlane(float u, float v, double rgbTimestamp) { TangoXyzIjData xyzIj = mPointCloudManager.getLatestXyzIj(); + if (xyzIj == null) { + return null; + } + // We need to calculate the transform between the color camera at the // time the user clicked and the depth camera at the time the depth // cloud was acquired. TangoPoseData colorTdepthPose = TangoSupport.calculateRelativePose( - measurementTimestamp, - TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR, xyzIj.timestamp, - TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH); + rgbTimestamp, TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR, + xyzIj.timestamp, TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH); // Perform plane fitting with the latest available point cloud data. IntersectionPointPlaneModelPair intersectionPointPlaneModelPair = - TangoSupport.fitPlaneModelNearClick(xyzIj, mCameraIntrinsics, + TangoSupport.fitPlaneModelNearClick(xyzIj, mIntrinsics, colorTdepthPose, u, v); // Get the device pose at the time the plane data was acquired. - TangoCoordinateFramePair framePair = new TangoCoordinateFramePair( - TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, - TangoPoseData.COORDINATE_FRAME_DEVICE); TangoPoseData devicePose = - mTango.getPoseAtTime(xyzIj.timestamp, framePair); + mTango.getPoseAtTime(xyzIj.timestamp, FRAME_PAIR); // Update the AR object location. - mRenderer.updateObjectPose( + TangoPoseData planeFitPose = ScenePoseCalculator.planeFitToTangoWorldPose( intersectionPointPlaneModelPair.intersectionPoint, - intersectionPointPlaneModelPair.planeModel, devicePose); + intersectionPointPlaneModelPair.planeModel, devicePose, mExtrinsics); + + return planeFitPose; } } diff --git a/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityRenderer.java b/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityRenderer.java index 79d6677b..ab1a468b 100644 --- a/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityRenderer.java +++ b/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/AugmentedRealityRenderer.java @@ -16,6 +16,7 @@ package com.projecttango.experiments.augmentedrealitysample; import android.content.Context; + import android.view.MotionEvent; import com.google.atap.tangoservice.TangoPoseData; @@ -29,28 +30,32 @@ import org.rajawali3d.math.vector.Vector3; import org.rajawali3d.primitives.Cube; +import com.projecttango.rajawali.DeviceExtrinsics; import com.projecttango.rajawali.Pose; +import com.projecttango.rajawali.ScenePoseCalculator; import com.projecttango.rajawali.ar.TangoRajawaliRenderer; /** - * Very simple example augmented reality renderer which displays two objects in - * a fixed position in the world and the uses the Tango position tracking to - * keep them in place. - *

+ * Very simple example augmented reality renderer which displays a cube fixed in place. + * Whenever the user clicks on the screen, the cube is placed flush with the surface detected + * with the depth camera in the position clicked. + * * This follows the same development model than any regular Rajawali application * with the following peculiarities: * - It extends TangoRajawaliArRenderer. * - It calls super.initScene() in the initialization. - * - It doesn't do anything with the camera, since that is handled automatically - * by Tango. + * - When an updated pose for the object is obtained after a user click, the object pose is updated + * in the render loop + * - The associated AugmentedRealityActivity is taking care of updating the camera pose to match + * the displayed RGB camera texture and produce the AR effect through a Scene Frame Callback + * (@see AugmentedRealityActivity) */ public class AugmentedRealityRenderer extends TangoRajawaliRenderer { private static final float CUBE_SIDE_LENGTH = 0.5f; - private Pose mPlanePose; - private boolean mPlanePoseUpdated = false; - private Object3D mObject; + private Pose mObjectPose; + private boolean mObjectPoseUpdated = false; public AugmentedRealityRenderer(Context context) { super(context); @@ -93,38 +98,49 @@ protected void initScene() { @Override protected void onRender(long elapsedRealTime, double deltaTime) { - super.onRender(elapsedRealTime, deltaTime); - + // Update the AR object if necessary + // Synchronize against concurrent access with the setter below. synchronized (this) { - if (mPlanePoseUpdated) { - mPlanePoseUpdated = false; + if (mObjectPoseUpdated) { // Place the 3D object in the location of the detected plane. - mObject.setPosition(mPlanePose.getPosition()); - mObject.setOrientation(mPlanePose.getOrientation()); + mObject.setPosition(mObjectPose.getPosition()); + mObject.setOrientation(mObjectPose.getOrientation()); // Move it forward by half of the size of the cube to make it // flush with the plane surface. mObject.moveForward(CUBE_SIDE_LENGTH / 2.0f); + mObjectPoseUpdated = false; } } + + super.onRender(elapsedRealTime, deltaTime); + } + + /** + * Save the updated plane fit pose to update the AR object on the next render pass. + * This is synchronized against concurrent access in the render loop above. + */ + public synchronized void updateObjectPose(TangoPoseData planeFitPose) { + mObjectPose = ScenePoseCalculator.toOpenGLPose(planeFitPose); + mObjectPoseUpdated = true; } /** - * Update the 3D object based on the provided measurement point, normal (in - * depth frame) and device pose at the time the point and normal were - * acquired. + * Update the scene camera based on the provided pose in Tango start of service frame. + * The device pose should match the pose of the device at the time the last rendered RGB + * frame, which can be retrieved with this.getTimestamp(); + * + * NOTE: This must be called from the OpenGL render thread - it is not thread safe. */ - public synchronized void updateObjectPose(double[] point, double[] normal, - TangoPoseData devicePose) { - mPlanePose = mScenePoseCalcuator.planeFitToOpenGLPose(point, normal, - devicePose); - mPlanePoseUpdated = true; + public void updateRenderCameraPose(TangoPoseData devicePose, DeviceExtrinsics extrinsics) { + Pose cameraPose = ScenePoseCalculator.toOpenGlCameraPose(devicePose, extrinsics); + getCurrentCamera().setRotation(cameraPose.getOrientation()); + getCurrentCamera().setPosition(cameraPose.getPosition()); } @Override public void onOffsetsChanged(float xOffset, float yOffset, float xOffsetStep, float yOffsetStep, int xPixelOffset, int yPixelOffset) { - } @Override diff --git a/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/PointCloudManager.java b/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/PointCloudManager.java deleted file mode 100644 index 5f793d90..00000000 --- a/AugmentedRealitySample/app/src/main/java/com/projecttango/experiments/augmentedrealitysample/PointCloudManager.java +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package com.projecttango.experiments.augmentedrealitysample; - -import com.google.atap.tangoservice.TangoCameraIntrinsics; -import com.google.atap.tangoservice.TangoPoseData; -import com.google.atap.tangoservice.TangoXyzIjData; -import com.projecttango.tangosupport.TangoSupport; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; - -/** - * This helper class keeps a copy of the point cloud data received in callbacks for use with the - * plane fitting function. - * It is implemented to be thread safe so that the caller (the Activity) doesn't need to worry - * about locking between the Tango callback and UI threads. - */ -public class PointCloudManager { - private static final String TAG = "PointCloudManager"; - - private final TangoCameraIntrinsics mTangoCameraIntrinsics; - private final TangoXyzIjData mXyzIjData; - - public PointCloudManager(TangoCameraIntrinsics intrinsics) { - mXyzIjData = new TangoXyzIjData(); - mTangoCameraIntrinsics = intrinsics; - } - - /** - * Update the current cloud data with the provided xyzIjData from a Tango callback. - * - * @param from The point cloud data - */ - public synchronized void updateXyzIjData(TangoXyzIjData from) { - if (mXyzIjData.xyz == null || mXyzIjData.xyz.capacity() < from.xyzCount * 3) { - mXyzIjData.xyz = ByteBuffer.allocateDirect(from.xyzCount * 3 * 4) - .order(ByteOrder.nativeOrder()).asFloatBuffer(); - } else { - mXyzIjData.xyz.rewind(); - } - - mXyzIjData.xyzCount = from.xyzCount; - mXyzIjData.timestamp = from.timestamp; - - from.xyz.rewind(); - mXyzIjData.xyz.put(from.xyz); - mXyzIjData.xyz.rewind(); - from.xyz.rewind(); - } - - /** - * Calculate the plane that best fits the current point cloud at the provided u,v coordinates - * in the 2D projection of the point cloud data (i.e.: point cloud image). - * - * @param u u (horizontal) component of the click location - * @param v v (vertical) component of the click location - * @param timestampAtClickTime Timestamp at the time this operation is requested - * @return The point and plane model, in depth sensor frame at the time - * the point cloud data was acquired and the timestamp when the - * the point cloud data was acquired. - */ - public synchronized FitPlaneResult fitPlane(float u, float v, - double timestampAtClickTime) { - - // We need to calculate the transform between the color camera at the time the user clicked - // and the depth camera at the time the depth cloud was acquired. - TangoPoseData colorCameraTDepthCameraWithTime - = TangoSupport.calculateRelativePose(timestampAtClickTime, - TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR, - mXyzIjData.timestamp, - TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH); - - return new FitPlaneResult(TangoSupport.fitPlaneModelNearClick(mXyzIjData, mTangoCameraIntrinsics, - colorCameraTDepthCameraWithTime, u, v), mXyzIjData.timestamp); - } - - /** - * Internal class used to return the information about a plane fitting call. - */ - public class FitPlaneResult { - public final TangoSupport.IntersectionPointPlaneModelPair planeModelPair; - public final double cloudTimestamp; - - public FitPlaneResult(TangoSupport.IntersectionPointPlaneModelPair planeModelPair, - double cloudTimestamp) { - this.planeModelPair = planeModelPair; - this.cloudTimestamp = cloudTimestamp; - } - } -} diff --git a/MotionTrackingJava/app/src/main/java/com/projecttango/experiments/javamotiontracking/MotionTrackingRajawaliRenderer.java b/MotionTrackingJava/app/src/main/java/com/projecttango/experiments/javamotiontracking/MotionTrackingRajawaliRenderer.java index e4841e2b..985315b7 100644 --- a/MotionTrackingJava/app/src/main/java/com/projecttango/experiments/javamotiontracking/MotionTrackingRajawaliRenderer.java +++ b/MotionTrackingJava/app/src/main/java/com/projecttango/experiments/javamotiontracking/MotionTrackingRajawaliRenderer.java @@ -22,10 +22,10 @@ import com.google.atap.tangoservice.TangoPoseData; import com.projecttango.rajawali.Pose; +import com.projecttango.rajawali.ScenePoseCalculator; import com.projecttango.rajawali.TouchViewHandler; import com.projecttango.rajawali.renderables.FrustumAxes; import com.projecttango.rajawali.renderables.Grid; -import com.projecttango.rajawali.ScenePoseCalcuator; import com.projecttango.rajawali.renderables.Trajectory; import org.rajawali3d.math.Quaternion; @@ -37,7 +37,7 @@ */ public class MotionTrackingRajawaliRenderer extends RajawaliRenderer { - private final String TAG = ScenePoseCalcuator.class.getSimpleName(); + private final String TAG = MotionTrackingRajawaliRenderer.class.getSimpleName(); // Only add line segments to the trajectory if the deviced moved more than THRESHOLD meters private static final double THRESHOLD = 0.002f; @@ -104,7 +104,7 @@ protected void onRender(long ellapsedRealtime, double deltaTime) { * concurrent access from the OpenGL thread above. */ public synchronized void updateDevicePose(TangoPoseData tangoPoseData) { - mDevicePose = ScenePoseCalcuator.toOpenGLPose(tangoPoseData); + mDevicePose = ScenePoseCalculator.toOpenGLPose(tangoPoseData); mPoseUpdated = true; } diff --git a/PointCloudJava/app/build.gradle b/PointCloudJava/app/build.gradle index 1838cb22..f16b612b 100644 --- a/PointCloudJava/app/build.gradle +++ b/PointCloudJava/app/build.gradle @@ -40,6 +40,7 @@ repositories{ dependencies { compile (name: 'tango-ux-support-library', ext: 'aar') + compile (name: 'tango_support_java_lib', ext: 'aar') compile 'org.rajawali3d:rajawali:1.0.294-SNAPSHOT@aar' compile project(':TangoUtils') } diff --git a/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudActivity.java b/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudActivity.java index 16aa85ee..fb48ea4c 100644 --- a/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudActivity.java +++ b/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudActivity.java @@ -27,13 +27,11 @@ import com.google.atap.tangoservice.TangoCoordinateFramePair; import com.google.atap.tangoservice.TangoErrorException; import com.google.atap.tangoservice.TangoEvent; -import com.google.atap.tangoservice.TangoInvalidException; import com.google.atap.tangoservice.TangoOutOfDateException; import com.google.atap.tangoservice.TangoPoseData; import com.google.atap.tangoservice.TangoXyzIjData; import android.app.Activity; -import android.content.Intent; import android.content.pm.PackageInfo; import android.content.pm.PackageManager.NameNotFoundException; import android.os.Bundle; @@ -45,8 +43,10 @@ import android.widget.TextView; import android.widget.Toast; +import com.projecttango.tangosupport.TangoPointCloudManager; import com.projecttango.tangoutils.TangoPoseUtilities; +import org.rajawali3d.scene.ASceneFrameCallback; import org.rajawali3d.surface.RajawaliSurfaceView; import java.lang.Override; @@ -63,6 +63,16 @@ public class PointCloudActivity extends Activity implements OnClickListener { private static final String TAG = PointCloudActivity.class.getSimpleName(); private static final int SECS_TO_MILLISECS = 1000; + + // Configure the Tango coordinate frame pair + private static final ArrayList FRAME_PAIRS = + new ArrayList(); + { + FRAME_PAIRS.add(new TangoCoordinateFramePair( + TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, + TangoPoseData.COORDINATE_FRAME_DEVICE)); + } + private Tango mTango; private TangoConfig mConfig; @@ -90,7 +100,7 @@ public class PointCloudActivity extends Activity implements OnClickListener { private double mXyIjPreviousTimeStamp; private boolean mIsTangoServiceConnected; private TangoPoseData mPose; - private PointCloudManager mPointCloudManager; + private TangoPointCloudManager mPointCloudManager; private TangoUx mTangoUx; private static final DecimalFormat FORMAT_THREE_DECIMAL = new DecimalFormat("0.000"); @@ -110,8 +120,7 @@ protected void onCreate(Bundle savedInstanceState) { setupTextViewsAndButtons(mConfig); int maxDepthPoints = mConfig.getInt("max_point_cloud_elements"); - mPointCloudManager = new PointCloudManager(maxDepthPoints); - mRenderer = setupGLViewAndRenderer(mPointCloudManager); + mRenderer = setupGLViewAndRenderer(); mTangoUx = setupTangoUxAndLayout(); mIsTangoServiceConnected = false; } @@ -121,8 +130,10 @@ protected void onPause() { super.onPause(); mTangoUx.stop(); try { + mRenderer.getCurrentScene().clearFrameCallbacks(); mTango.disconnect(); mIsTangoServiceConnected = false; + mPose = null; } catch (TangoErrorException e) { Toast.makeText(getApplicationContext(), R.string.TangoError, Toast.LENGTH_SHORT).show(); } @@ -135,6 +146,7 @@ protected void onResume() { mTangoUx.start(params); try { setTangoListeners(); + setRendererListener(); } catch (TangoErrorException e) { Toast.makeText(this, R.string.TangoError, Toast.LENGTH_SHORT).show(); } catch (SecurityException e) { @@ -143,8 +155,9 @@ protected void onResume() { } try { mTango.connect(mConfig); - mIsTangoServiceConnected = true; + mPointCloudManager = new TangoPointCloudManager(); setupExtrinsics(); + mIsTangoServiceConnected = true; } catch (TangoOutOfDateException outDateEx) { if (mTangoUx != null) { mTangoUx.showTangoOutOfDate(); @@ -185,14 +198,8 @@ public boolean onTouchEvent(MotionEvent event) { } private void setTangoListeners() { - // Configure the Tango coordinate frame pair - final ArrayList framePairs = new ArrayList(); - framePairs.add(new TangoCoordinateFramePair( - TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, - TangoPoseData.COORDINATE_FRAME_DEVICE)); // Listen for new Tango data - mTango.connectListener(framePairs, new OnTangoUpdateListener() { - + mTango.connectListener(FRAME_PAIRS, new OnTangoUpdateListener() { @Override public void onPoseAvailable(final TangoPoseData pose) { // Passing in the pose data to UX library produce exceptions. @@ -200,7 +207,12 @@ public void onPoseAvailable(final TangoPoseData pose) { mTangoUx.updatePoseStatus(pose.statusCode); } - mPose = pose; + // Update our copy of the latest pose + // Synchronize against concurrent use in the render loop. + synchronized (this) { + mPose = pose; + } + // Calculate the delta time from previous pose. final double deltaTime = (pose.timestamp - mPosePreviousTimeStamp) * SECS_TO_MILLISECS; @@ -233,8 +245,6 @@ public void run() { } }); } - - mRenderer.updateDevicePose(pose); } @Override @@ -242,10 +252,7 @@ public void onXyzIjAvailable(final TangoXyzIjData xyzIj) { if (mTangoUx != null) { mTangoUx.updateXyzCount(xyzIj.xyzCount); } - mPointCloudManager.updateCallbackBufferAndSwap(xyzIj.xyz, xyzIj.xyzCount); - TangoPoseData pointCloudPose = mTango.getPoseAtTime(xyzIj.timestamp, - framePairs.get(0)); - mRenderer.updatePointCloudPose(pointCloudPose); + mPointCloudManager.updateXyzIj(xyzIj); final double currentTimeStamp = xyzIj.timestamp; final double pointCloudFrameDelta = (currentTimeStamp - mXyIjPreviousTimeStamp) @@ -290,6 +297,46 @@ public void onFrameAvailable(int cameraId) { }); } + public void setRendererListener() { + mRenderer.getCurrentScene().registerFrameCallback(new ASceneFrameCallback() { + @Override + public void onPreFrame(long sceneTime, double deltaTime) { + // NOTE: This will be executed on each cycle before rendering, called from the + // OpenGL rendering thread + + // Update point cloud data + TangoXyzIjData pointCloud = mPointCloudManager.getLatestXyzIj(); + if (pointCloud != null && mIsTangoServiceConnected) { + TangoPoseData pointCloudPose = + mTango.getPoseAtTime(pointCloud.timestamp, FRAME_PAIRS.get(0)); + mRenderer.updatePointCloud(pointCloud, pointCloudPose); + } + + // Update current device pose + synchronized (this) { + if (mPose != null) { + mRenderer.updateDevicePose(mPose); + } + } + } + + @Override + public boolean callPreFrame() { + return true; + } + + @Override + public void onPreDraw(long sceneTime, double deltaTime) { + + } + + @Override + public void onPostFrame(long sceneTime, double deltaTime) { + + } + }); + } + private void setupExtrinsics() { TangoCoordinateFramePair framePair = new TangoCoordinateFramePair(); framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU; @@ -300,7 +347,7 @@ private void setupExtrinsics() { TangoPoseData imuTDepthCameraPose = mTango.getPoseAtTime(0.0,framePair); framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_DEVICE; - TangoPoseData imuTDevicePose = mTango.getPoseAtTime(0.0,framePair); + TangoPoseData imuTDevicePose = mTango.getPoseAtTime(0.0, framePair); mRenderer.setupExtrinsics(imuTDevicePose, imuTColorCameraPose, imuTDepthCameraPose); } @@ -399,8 +446,8 @@ private void setupTextViewsAndButtons(TangoConfig config){ /** * Sets Rajawalisurface view and its renderer. This is ideally called only once in onCreate. */ - private PointCloudRajawaliRenderer setupGLViewAndRenderer(PointCloudManager pointCloudManager){ - PointCloudRajawaliRenderer renderer = new PointCloudRajawaliRenderer(this, pointCloudManager); + private PointCloudRajawaliRenderer setupGLViewAndRenderer(){ + PointCloudRajawaliRenderer renderer = new PointCloudRajawaliRenderer(this); RajawaliSurfaceView glView = (RajawaliSurfaceView) findViewById(R.id.gl_surface_view); glView.setEGLContextClientVersion(2); glView.setSurfaceRenderer(renderer); @@ -434,5 +481,4 @@ private float getAveragedDepth(FloatBuffer pointCloudBuffer){ averageZ = totalZ / pointCount; return averageZ; } - } diff --git a/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudManager.java b/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudManager.java deleted file mode 100644 index aad091ff..00000000 --- a/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudManager.java +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package com.projecttango.experiments.javapointcloud; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.nio.FloatBuffer; - -/** - * This is a thread safe class that synchronizes the data copy between onXyzAvailable callbacks - * and render loop. - * Synchronization of point cloud data primarily involves registering for - * callbacks in {@link PointCloudActivity} and passing on the necessary information to stored objects. - * It also takes care of passing a floatbuffer which is a reference to - * latest point cloud buffer that is to used for rendering. - * To reduce the number of point cloud data copies between callback and render - * threads we use three buffers which are synchronously exchanged between renderloop and callbacks - * so that the render loop always contains the latest point cloud data. - * 1. Callback buffer : The buffer to which pointcloud data received from Tango - * Service callback is copied out. - * 2. Shared buffer: This buffer is used to share the data between Service - * callback and Render loop - * 3. Render Buffer: This buffer is used in the renderloop to project point - * cloud data to a 2D image plane which is of the same size as RGB image. We - * also make sure that this buffer contains the latest point cloud data that is - * received from the call back. - */ -public class PointCloudManager { - private static final int BYTES_PER_FLOAT = 4; - private static final int POINT_TO_XYZ = 3; - private PointCloudData mCallbackPointCloudData; - private PointCloudData mSharedPointCloudData; - private PointCloudData mRenderPointCloudData; - private boolean mSwapSignal; - private Object mPointCloudLock; - - - PointCloudManager(int maxDepthPoints){ - mSwapSignal = false; - setupBuffers(maxDepthPoints); - mPointCloudLock = new Object(); - } - - /** - * Sets up three buffers namely, Callback, Shared and Render buffers allocated with maximum - * number of points a point cloud can have. - * @param maxDepthPoints - */ - private void setupBuffers(int maxDepthPoints){ - mCallbackPointCloudData = new PointCloudData(); - mSharedPointCloudData = new PointCloudData(); - mRenderPointCloudData = new PointCloudData(); - mCallbackPointCloudData.floatBuffer = ByteBuffer - .allocateDirect(maxDepthPoints * BYTES_PER_FLOAT * POINT_TO_XYZ) - .order(ByteOrder.nativeOrder()).asFloatBuffer(); - mSharedPointCloudData.floatBuffer = ByteBuffer - .allocateDirect(maxDepthPoints * BYTES_PER_FLOAT * POINT_TO_XYZ) - .order(ByteOrder.nativeOrder()).asFloatBuffer(); - mRenderPointCloudData.floatBuffer = ByteBuffer - .allocateDirect(maxDepthPoints * BYTES_PER_FLOAT * POINT_TO_XYZ) - .order(ByteOrder.nativeOrder()).asFloatBuffer(); - } - - /** - * Updates the callbackbuffer with latest pointcloud and swaps the - * @param callbackBuffer - * @param pointCount - */ - public void updateCallbackBufferAndSwap(FloatBuffer callbackBuffer, int pointCount){ - mSharedPointCloudData.floatBuffer.position(0); - mCallbackPointCloudData.floatBuffer.put(callbackBuffer); - synchronized (mPointCloudLock){ - FloatBuffer temp = mSharedPointCloudData.floatBuffer; - int tempCount = pointCount; - mSharedPointCloudData.floatBuffer = mCallbackPointCloudData .floatBuffer; - mSharedPointCloudData.pointCount = mCallbackPointCloudData.pointCount; - mCallbackPointCloudData.floatBuffer = temp; - mCallbackPointCloudData.pointCount = tempCount; - mSwapSignal = true; - } - } - - /** - * Returns a shallow copy of latest Point Cloud Render buffer.If there is a swap signal available - * SharedPointCloud buffer is swapped with Render buffer and it is returned. - * @return PointClouData which contains a reference to latest PointCloud Floatbuffer and count. - */ - public PointCloudData updateAndGetLatestPointCloudRenderBuffer(){ - synchronized (mPointCloudLock){ - if(mSwapSignal) { - FloatBuffer temp = mRenderPointCloudData.floatBuffer; - int tempCount = mRenderPointCloudData.pointCount; - mRenderPointCloudData.floatBuffer = mSharedPointCloudData.floatBuffer; - mRenderPointCloudData.pointCount = mSharedPointCloudData.pointCount; - mSharedPointCloudData.floatBuffer = temp; - mSharedPointCloudData.pointCount = tempCount; - mSwapSignal = false; - } - } - return mRenderPointCloudData; - } - - /** - * A class to hold Depth data in a {@link FloatBuffer} and number of points associated with it. - */ - public class PointCloudData{ - public FloatBuffer floatBuffer; - public int pointCount; - } -} diff --git a/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudRajawaliRenderer.java b/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudRajawaliRenderer.java index 924254e0..a9838209 100644 --- a/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudRajawaliRenderer.java +++ b/PointCloudJava/app/src/main/java/com/projecttango/experiments/javapointcloud/PointCloudRajawaliRenderer.java @@ -20,42 +20,37 @@ import android.view.MotionEvent; import com.google.atap.tangoservice.TangoPoseData; +import com.google.atap.tangoservice.TangoXyzIjData; +import com.projecttango.rajawali.DeviceExtrinsics; import com.projecttango.rajawali.Pose; -import com.projecttango.rajawali.ScenePoseCalcuator; +import com.projecttango.rajawali.ScenePoseCalculator; import com.projecttango.rajawali.TouchViewHandler; import com.projecttango.rajawali.renderables.FrustumAxes; import com.projecttango.rajawali.renderables.Grid; -import com.projecttango.rajawali.renderables.primitives.Points; +import com.projecttango.rajawali.renderables.PointCloud; -import org.rajawali3d.math.Matrix4; import org.rajawali3d.renderer.RajawaliRenderer; /** - * Renderer for Point Cloud data. This is also a thread safe class as in when Renderloop and - * OnposeAvailable callbacks are synchronized with each other. + * Renderer for Point Cloud data. */ public class PointCloudRajawaliRenderer extends RajawaliRenderer { private static final float CAMERA_NEAR = 0.01f; private static final float CAMERA_FAR = 200f; private static final int MAX_NUMBER_OF_POINTS = 60000; - private FrustumAxes mFrustumAxes; - private Grid mGrid; private TouchViewHandler mTouchViewHandler; - private ScenePoseCalcuator mScenePoseCalcuator; - private Pose mCameraPose; - private Pose mPointCloudPose; + private DeviceExtrinsics mDeviceExtrinsics; - // Latest available device pose - private Points mPoints; - private PointCloudManager mPointCloudManager; + // Objects rendered in the scene + private PointCloud mPointCloud; + private FrustumAxes mFrustumAxes; + private Grid mGrid; - public PointCloudRajawaliRenderer(Context context, PointCloudManager pointCloudManager) { + public PointCloudRajawaliRenderer(Context context) { super(context); mTouchViewHandler = new TouchViewHandler(mContext, getCurrentCamera()); - mScenePoseCalcuator = new ScenePoseCalcuator(); - mPointCloudManager = pointCloudManager; } @Override @@ -67,46 +62,41 @@ protected void initScene() { mFrustumAxes = new FrustumAxes(3); getCurrentScene().addChild(mFrustumAxes); - mPoints = new Points(MAX_NUMBER_OF_POINTS); - getCurrentScene().addChild(mPoints); + mPointCloud = new PointCloud(MAX_NUMBER_OF_POINTS); + getCurrentScene().addChild(mPointCloud); getCurrentScene().setBackgroundColor(Color.WHITE); getCurrentCamera().setNearPlane(CAMERA_NEAR); getCurrentCamera().setFarPlane(CAMERA_FAR); getCurrentCamera().setFieldOfView(37.5); } - @Override - protected void onRender(long ellapsedRealtime, double deltaTime) { - super.onRender(ellapsedRealtime, deltaTime); - PointCloudManager.PointCloudData renderPointCloudData - = mPointCloudManager.updateAndGetLatestPointCloudRenderBuffer(); - mPoints.updatePoints(renderPointCloudData.floatBuffer, renderPointCloudData.pointCount); - if(mCameraPose==null || mPointCloudPose == null){ - return; - } - // Update the scene objects with the latest device position and orientation information. - // Synchronize to avoid concurrent access from the Tango callback thread below. - synchronized (this) { - mFrustumAxes.setPosition(mCameraPose.getPosition()); - mFrustumAxes.setOrientation(mCameraPose.getOrientation()); - - mPoints.setPosition(mPointCloudPose.getPosition()); - mPoints.setOrientation(mPointCloudPose.getOrientation()); - mTouchViewHandler.updateCamera(mCameraPose.getPosition(), mCameraPose.getOrientation()); + /** + * Updates the rendered point cloud. For this, we need the point cloud data and the device pose + * at the time the cloud data was acquired. + * NOTE: This needs to be called from the OpenGL rendering thread. + */ + public void updatePointCloud(TangoXyzIjData xyzIjData, TangoPoseData devicePose) { + if (mDeviceExtrinsics != null) { + Pose pointCloudPose = + ScenePoseCalculator.toDepthCameraOpenGlPose(devicePose, mDeviceExtrinsics); + mPointCloud.updateCloud(xyzIjData.xyzCount, xyzIjData.xyz); + mPointCloud.setPosition(pointCloudPose.getPosition()); + mPointCloud.setOrientation(pointCloudPose.getOrientation()); } } /** * Updates our information about the current device pose. - * This is called from the Tango service thread through the callback API. Synchronize to avoid - * concurrent access from the OpenGL thread above. + * NOTE: This needs to be called from the OpenGL rendering thread. */ - public synchronized void updateDevicePose(TangoPoseData tangoPoseData) { - mCameraPose = mScenePoseCalcuator.toOpenGLCameraPose(tangoPoseData); - } - - public synchronized void updatePointCloudPose(TangoPoseData pointCloudPose) { - mPointCloudPose = mScenePoseCalcuator.toOpenGLPointCloudPose(pointCloudPose); + public void updateDevicePose(TangoPoseData tangoPoseData) { + if (mDeviceExtrinsics != null) { + Pose cameraPose = + ScenePoseCalculator.toOpenGlCameraPose(tangoPoseData, mDeviceExtrinsics); + mFrustumAxes.setPosition(cameraPose.getPosition()); + mFrustumAxes.setOrientation(cameraPose.getOrientation()); + mTouchViewHandler.updateCamera(cameraPose.getPosition(), cameraPose.getOrientation()); + } } @Override @@ -139,7 +129,7 @@ public void setThirdPersonView() { */ public void setupExtrinsics(TangoPoseData imuTDevicePose, TangoPoseData imuTColorCameraPose, TangoPoseData imuTDepthCameraPose) { - mScenePoseCalcuator.setupExtrinsics(imuTDevicePose, imuTColorCameraPose, imuTDepthCameraPose); + mDeviceExtrinsics = + new DeviceExtrinsics(imuTDevicePose, imuTColorCameraPose, imuTDepthCameraPose); } - } diff --git a/TangoReleaseLibs/aar/TangoUtils.aar b/TangoReleaseLibs/aar/TangoUtils.aar index 8c379cdb..2905e509 100644 Binary files a/TangoReleaseLibs/aar/TangoUtils.aar and b/TangoReleaseLibs/aar/TangoUtils.aar differ diff --git a/TangoReleaseLibs/aar/tango-ux-support-library.aar b/TangoReleaseLibs/aar/tango-ux-support-library.aar index 1248aa50..94782eff 100644 Binary files a/TangoReleaseLibs/aar/tango-ux-support-library.aar and b/TangoReleaseLibs/aar/tango-ux-support-library.aar differ diff --git a/TangoReleaseLibs/aar/tango_support_java_lib.aar b/TangoReleaseLibs/aar/tango_support_java_lib.aar index 317b06a1..6405561c 100644 Binary files a/TangoReleaseLibs/aar/tango_support_java_lib.aar and b/TangoReleaseLibs/aar/tango_support_java_lib.aar differ diff --git a/TangoReleaseLibs/jar/tango_java_lib.jar b/TangoReleaseLibs/jar/tango_java_lib.jar index 8f81273f..46082870 100644 Binary files a/TangoReleaseLibs/jar/tango_java_lib.jar and b/TangoReleaseLibs/jar/tango_java_lib.jar differ diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/DeviceExtrinsics.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/DeviceExtrinsics.java new file mode 100644 index 00000000..906c44df --- /dev/null +++ b/TangoUtils/app/src/main/java/com/projecttango/rajawali/DeviceExtrinsics.java @@ -0,0 +1,49 @@ +/* + * Copyright 2015 Google Inc. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package com.projecttango.rajawali; + +import com.google.atap.tangoservice.TangoPoseData; + +import org.rajawali3d.math.Matrix4; + +/** + * Class used to hold device extrinsics information in a way that is easy to use to perform + * transformations with the ScenePoseCalculator. + */ +public class DeviceExtrinsics { + // Transformation from the position of the depth camera to the device frame. + private Matrix4 mDeviceTDepthCamera; + + // Transformation from the position of the color Camera to the device frame. + private Matrix4 mDeviceTColorCamera; + + public DeviceExtrinsics(TangoPoseData imuTDevicePose, TangoPoseData imuTColorCameraPose, + TangoPoseData imuTDepthCameraPose) { + Matrix4 deviceTImu = ScenePoseCalculator.tangoPoseToMatrix(imuTDevicePose).inverse(); + Matrix4 imuTColorCamera = ScenePoseCalculator.tangoPoseToMatrix(imuTColorCameraPose); + Matrix4 imuTDepthCamera = ScenePoseCalculator.tangoPoseToMatrix(imuTDepthCameraPose); + mDeviceTDepthCamera = deviceTImu.clone().multiply(imuTDepthCamera); + mDeviceTColorCamera = deviceTImu.multiply(imuTColorCamera); + } + + public Matrix4 getDeviceTColorCamera() { + return mDeviceTColorCamera; + } + + public Matrix4 getDeviceTDepthCamera() { + return mDeviceTDepthCamera; + } +} diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/ScenePoseCalcuator.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/ScenePoseCalculator.java similarity index 66% rename from TangoUtils/app/src/main/java/com/projecttango/rajawali/ScenePoseCalcuator.java rename to TangoUtils/app/src/main/java/com/projecttango/rajawali/ScenePoseCalculator.java index eab495f7..0ffe1a58 100644 --- a/TangoUtils/app/src/main/java/com/projecttango/rajawali/ScenePoseCalcuator.java +++ b/TangoUtils/app/src/main/java/com/projecttango/rajawali/ScenePoseCalculator.java @@ -23,14 +23,11 @@ import org.rajawali3d.math.vector.Vector3; /** - * Convenient class for calculating transformations from the Tango world to the OpenGL world, using - * Rajawali specific classes and conventions. - * Note that in order to do transformations to the OpenGL camera coordinate frame, it is necessary - * to first call setupExtrinsics. The recommended time to do this is right after - * the Tango service is connected. + * Convenient class for calculating transformations from the Tango world to the OpenGL world, + * using Rajawali specific classes and conventions. */ -public class ScenePoseCalcuator { - private static final String TAG = ScenePoseCalcuator.class.getSimpleName(); +public final class ScenePoseCalculator { + private static final String TAG = ScenePoseCalculator.class.getSimpleName(); // Transformation from the Tango Area Description or Start of Service coordinate frames // to the OpenGL coordinate frame. @@ -59,11 +56,10 @@ public class ScenePoseCalcuator { // Up vector in the Tango start of Service and Area Description frame. public static final Vector3 TANGO_WORLD_UP = new Vector3(0, 0, 1); - // Transformation from the position of the depth camera to the device frame. - private Matrix4 mDeviceTDepthCamera; - - // Transformation from the position of the color Camera to the device frame. - private Matrix4 mDeviceTColorCamera; + /** + * Avoid instantiating the class since it will only be used statically. + */ + private ScenePoseCalculator() {} /** * Converts from TangoPoseData to a Matrix4 for transformations. @@ -88,8 +84,6 @@ public static TangoPoseData matrixToTangoPose(Matrix4 transform) { Vector3 p = transform.getTranslation(); Quaternion q = new Quaternion(); q.fromMatrix(transform); - // NOTE: Rajawali quaternions use a left-hand rotation around the axis convention. - q.conjugate(); TangoPoseData tangoPose = new TangoPoseData(); double[] t = tangoPose.translation = new double[3]; @@ -122,142 +116,111 @@ public static Pose matrixToPose(Matrix4 m) { } /** - * Given a pose in start of service or area description frame, calculate the corresponding - * position and orientation for a OpenGL Scene Camera in the Rajawali world. + * Given a pose in start of service or area description frame calculate the corresponding + * position and orientation for a 3D object in the Rajawali world. */ - public Pose toOpenGLCameraPose(TangoPoseData tangoPose) { - // We can't do this calculation until extrinsics are set-up. - if (mDeviceTColorCamera == null) { - throw new RuntimeException("You must call setupExtrinsics first."); - } - - Matrix4 startServiceTdevice = tangoPoseToMatrix(tangoPose); + public static Pose toOpenGLPose(TangoPoseData tangoPose) { + Matrix4 start_service_T_device = tangoPoseToMatrix(tangoPose); // Get device pose in OpenGL world frame. - Matrix4 openglTDevice = OPENGL_T_TANGO_WORLD.clone().multiply(startServiceTdevice); - - // Get OpenGL camera pose in OpenGL world frame. - Matrix4 openglWorldTOpenglCamera = openglTDevice.multiply(mDeviceTColorCamera). - multiply(COLOR_CAMERA_T_OPENGL_CAMERA); + Matrix4 opengl_world_T_device = OPENGL_T_TANGO_WORLD.clone().multiply(start_service_T_device); - return matrixToPose(openglWorldTOpenglCamera); + return matrixToPose(opengl_world_T_device); } /** - * Given a TangoPoseData object, calculate the corresponding position and orientation for a - * PointCloud in Depth camera coordinate system to the Rajawali world. + * Use Tango camera intrinsics to calculate the projection Matrix for the Rajawali scene. */ - public Pose toOpenGLPointCloudPose(TangoPoseData tangoPose) { - // We can't do this calculation until extrinsics are set-up. - if (mDeviceTDepthCamera == null) { - throw new RuntimeException("You must call setupExtrinsics first."); - } - - //conversion matrix to put point cloud data in Rajawali/OpenGL coordinate system. - Matrix4 invertYandZMatrix = new Matrix4(new double[]{1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, -1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, -1.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f }); + public static Matrix4 calculateProjectionMatrix(int width, int height, double fx, double fy, + double cx, double cy) { + // Uses frustumM to create a projection matrix taking into account calibrated camera + // intrinsic parameter. + // Reference: http://ksimek.github.io/2013/06/03/calibrated_cameras_in_opengl/ + double near = 0.1; + double far = 100; - Matrix4 startServiceTdevice = tangoPoseToMatrix(tangoPose); + double xScale = near / fx; + double yScale = near / fy; + double xOffset = (cx - (width / 2.0)) * xScale; + // Color camera's coordinates has y pointing downwards so we negate this term. + double yOffset = -(cy - (height / 2.0)) * yScale; + + double m[] = new double[16]; + Matrix.frustumM(m, 0, + xScale * -width / 2.0 - xOffset, + xScale * width / 2.0 - xOffset, + yScale * -height / 2.0 - yOffset, + yScale * height / 2.0 - yOffset, + near, far); + return new Matrix4(m); + } + + /** + * Given the device pose in start of service frame, calculate the corresponding + * position and orientation for a OpenGL Scene Camera in the Rajawali world. + */ + public static Pose toOpenGlCameraPose(TangoPoseData devicePose, DeviceExtrinsics extrinsics) { + Matrix4 startServiceTdevice = tangoPoseToMatrix(devicePose); // Get device pose in OpenGL world frame. Matrix4 openglTDevice = OPENGL_T_TANGO_WORLD.clone().multiply(startServiceTdevice); // Get OpenGL camera pose in OpenGL world frame. - Matrix4 openglWorldTOpenglCamera = openglTDevice.multiply(mDeviceTDepthCamera). - multiply(DEPTH_CAMERA_T_OPENGL_CAMERA).multiply(invertYandZMatrix); + Matrix4 openglWorldTOpenglCamera = + openglTDevice.multiply(extrinsics.getDeviceTColorCamera()). + multiply(COLOR_CAMERA_T_OPENGL_CAMERA); return matrixToPose(openglWorldTOpenglCamera); } /** - * Given a pose in start of service or area description frame calculate the corresponding - * position and orientation for a 3D object in the Rajawali world. + * Given the device pose in start of service frame, calculate the position and orientation of + * the depth sensor in OpenGL coordinate frame. */ - static public Pose toOpenGLPose(TangoPoseData tangoPose) { - Matrix4 start_service_T_device = tangoPoseToMatrix(tangoPose); + public static Pose toDepthCameraOpenGlPose(TangoPoseData devicePose, + DeviceExtrinsics extrinsics) { + Matrix4 startServiceTdevice = tangoPoseToMatrix(devicePose); // Get device pose in OpenGL world frame. - Matrix4 opengl_world_T_device = OPENGL_T_TANGO_WORLD.clone().multiply(start_service_T_device); + Matrix4 openglTDevice = OPENGL_T_TANGO_WORLD.clone().multiply(startServiceTdevice); - return matrixToPose(opengl_world_T_device); + // Get OpenGL camera pose in OpenGL world frame. + Matrix4 openglWorldTOpenglCamera = + openglTDevice.multiply(extrinsics.getDeviceTDepthCamera()); + + return matrixToPose(openglWorldTOpenglCamera); } /** * Given a point and a normal in depth camera frame and the device pose in start of service - * frame at the time the point and normal were measured, calculate a TangoPoseData object in - * Tango start of service frame. + * frame at the time the point and normal were acquired, calculate a Pose object which + * represents the position and orientation of the fitted plane with its Y vector pointing + * up in the gravity vector, represented in the Tango start of service frame. * * @param point Point in depth frame where the plane has been detected. * @param normal Normal of the detected plane. * @param tangoPose Device pose with respect to start of service at the time the plane was * fitted. */ - public Pose planeFitToOpenGLPose(double[] point, double[] normal, TangoPoseData tangoPose) { - if (mDeviceTDepthCamera == null) { - throw new RuntimeException("You must call setupExtrinsics first"); - } - + public static TangoPoseData planeFitToTangoWorldPose(double[] point, double[] normal, + TangoPoseData tangoPose, DeviceExtrinsics extrinsics) { Matrix4 startServiceTdevice = tangoPoseToMatrix(tangoPose); // Calculate the UP vector in the depth frame at the provided measurement pose. Vector3 depthUp = TANGO_WORLD_UP.clone(); - startServiceTdevice.clone().multiply(mDeviceTDepthCamera).inverse().rotateVector(depthUp); + startServiceTdevice.clone().multiply(extrinsics.getDeviceTDepthCamera()) + .inverse().rotateVector(depthUp); // Calculate the transform in depth frame corresponding to the plane fitting information. Matrix4 depthTplane = matrixFromPointNormalUp(point, normal, depthUp); // Convert to OpenGL frame. - Matrix4 openglWorldTplane = OPENGL_T_TANGO_WORLD.clone().multiply(startServiceTdevice) - .multiply(mDeviceTDepthCamera).multiply(depthTplane); + Matrix4 tangoWorldTplane = startServiceTdevice.multiply(extrinsics.getDeviceTDepthCamera()). + multiply(depthTplane); - return matrixToPose(openglWorldTplane); + return matrixToTangoPose(tangoWorldTplane); } - /** - * Configure the scene pose calculator with the transformation between the selected - * camera and the device. - * Note that this requires going through the IMU since the Tango service can't calculate - * the transform between the camera and the device directly. - */ - public void setupExtrinsics(TangoPoseData imuTDevicePose, TangoPoseData imuTColorCameraPose, - TangoPoseData imuTDepthCameraPose) { - Matrix4 deviceTImu = ScenePoseCalcuator.tangoPoseToMatrix(imuTDevicePose).inverse(); - Matrix4 imuTColorCamera = ScenePoseCalcuator.tangoPoseToMatrix(imuTColorCameraPose); - Matrix4 imuTDepthCamera = ScenePoseCalcuator.tangoPoseToMatrix(imuTDepthCameraPose); - mDeviceTDepthCamera = deviceTImu.clone().multiply(imuTDepthCamera); - mDeviceTColorCamera = deviceTImu.multiply(imuTColorCamera); - } - - /** - * Use Tango camera intrinsics to calculate the projection Matrix for the Rajawali scene. - */ - public static Matrix4 calculateProjectionMatrix(int width, int height, double fx, double fy, - double cx, double cy) { - // Uses frustumM to create a projection matrix taking into account calibrated camera - // intrinsic parameter. - // Reference: http://ksimek.github.io/2013/06/03/calibrated_cameras_in_opengl/ - double near = 0.1; - double far = 100; - - double xScale = near / fx; - double yScale = near / fy; - double xOffset = (cx - (width / 2.0)) * xScale; - // Color camera's coordinates has y pointing downwards so we negate this term. - double yOffset = -(cy - (height / 2.0)) * yScale; - - double m[] = new double[16]; - Matrix.frustumM(m, 0, - xScale * -width / 2.0 - xOffset, - xScale * width / 2.0 - xOffset, - yScale * -height / 2.0 - yOffset, - yScale * height / 2.0 - yOffset, - near, far); - return new Matrix4(m); - } - - /** * Calculates a transformation matrix based on a point, a normal and the up gravity vector. * The coordinate frame of the target transformation will be Z forward, X left, Y up. diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliRenderer.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliRenderer.java index 189a21d7..ae297648 100644 --- a/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliRenderer.java +++ b/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliRenderer.java @@ -24,8 +24,9 @@ import com.google.atap.tangoservice.TangoErrorException; import com.google.atap.tangoservice.TangoInvalidException; import com.google.atap.tangoservice.TangoPoseData; +import com.projecttango.rajawali.DeviceExtrinsics; import com.projecttango.rajawali.Pose; -import com.projecttango.rajawali.ScenePoseCalcuator; +import com.projecttango.rajawali.ScenePoseCalculator; import org.rajawali3d.materials.Material; import org.rajawali3d.materials.textures.ATexture; @@ -55,30 +56,27 @@ * handled by this mRenderer. */ public abstract class TangoRajawaliRenderer extends RajawaliRenderer { - private static final String TAG = "TangoRajawaliRenderer"; + private static final String TAG = TangoRajawaliRenderer.class.getSimpleName(); private static TangoCoordinateFramePair TANGO_WORLD_T_DEVICE = new TangoCoordinateFramePair( TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, TangoPoseData.COORDINATE_FRAME_DEVICE ); - // Tango support objects. + // Rajawali scene objects to render the color camera private StreamingTexture mTangoCameraTexture; + private ScreenQuad mBackgroundQuad; + private Tango mTango; private int mCameraId; private boolean mUpdatePending = false; private int mConnectedTextureId = -1; private double mLastRGBFrameTimestamp = -1; - private double mLastSceneCameraFrameTimestamp = -1; - // The ScreenQuad used to render the Tango camera in the background of the scene. - private ScreenQuad mBackgroundQuad; private boolean mIsCameraConfigured = false; - - protected ScenePoseCalcuator mScenePoseCalcuator; + private Matrix4 mProjectionMatrix; public TangoRajawaliRenderer(Context context) { super(context); - mScenePoseCalcuator = new ScenePoseCalcuator(); } /** @@ -105,35 +103,18 @@ protected void initScene() { @Override protected void onRender(long elapsedRealTime, double deltaTime) { - super.onRender(elapsedRealTime, deltaTime); - synchronized (this) { + // mTango != null is used to indicate that a Tango device is connected to this + // renderer, via a corresponding TangoRajawaliView if (mTango != null) { try { if (mUpdatePending) { mLastRGBFrameTimestamp = updateTexture(); mUpdatePending = false; } - if (mLastRGBFrameTimestamp != mLastSceneCameraFrameTimestamp) { - // We delay the camera set-up until now because if we do it earlier (i.e., when the - // camera is connected to the renderer) the Tango service may still not have the - // necessary intrinsic and extrinsic transformation information available. - if (!mIsCameraConfigured) { - configureCamera(); - mIsCameraConfigured = true; - } - - // Calculate the device pose at the camera frame update time. - TangoPoseData lastFramePose = - mTango.getPoseAtTime(mLastRGBFrameTimestamp, TANGO_WORLD_T_DEVICE); - - if (lastFramePose.statusCode != TangoPoseData.POSE_VALID) { - Log.w(TAG, "Unable to get device pose at camera frame update time = " + mLastRGBFrameTimestamp); - } else { - Pose sceneCameraPose = mScenePoseCalcuator.toOpenGLCameraPose(lastFramePose); - updateCameraPose(sceneCameraPose); - mLastSceneCameraFrameTimestamp = mLastRGBFrameTimestamp; - } + if (!mIsCameraConfigured) { + getCurrentCamera().setProjectionMatrix(mProjectionMatrix); + mIsCameraConfigured = true; } } catch (TangoInvalidException ex) { Log.e(TAG, "Error while updating texture!", ex); @@ -142,42 +123,8 @@ protected void onRender(long elapsedRealTime, double deltaTime) { } } } - } - - /** - * Override onRenderSurfaceSizeChanged() so that it will be called after onSurfaceCreated, - * nested view get reset or resized, including activity get paused and resumed, in this function - * sets mIsCameraConfigured to false since Rajawali will reset the scene camera if SurfaceSizeChanged - * get called. - */ - @Override - public void onRenderSurfaceSizeChanged(GL10 gl, int width, int height) { - super.onRenderSurfaceSizeChanged(gl, width, height); - mConnectedTextureId = -1; - mIsCameraConfigured = false; - } - - /** - * Triggered whenever there is new information of the Tango position, with the provided - * data to update the Rajawali camera to match. - * It doesn't need to be overwritten. By default it will automatically update the Rajawali - * Camera to match. - */ - protected void updateCameraPose(Pose sceneCameraPose) { - getCurrentCamera().setPosition(sceneCameraPose.getPosition()); - getCurrentCamera().setOrientation(sceneCameraPose.getOrientation()); - } - /** - * Sets up the coordinate frame to use as a device reference. It should be one of - * TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE or - * TangoPoseData.COORDINATE_FRAME_AREA_DESCRIPTION. - * - * COORDINATE_FRAME_START_OF_SERVICE will be used by default. - */ - public void setWorldFrameReference(int worldFrameReference) { - TANGO_WORLD_T_DEVICE = new TangoCoordinateFramePair( worldFrameReference, - TangoPoseData.COORDINATE_FRAME_DEVICE); + super.onRender(elapsedRealTime, deltaTime); } /** @@ -185,7 +132,7 @@ public void setWorldFrameReference(int worldFrameReference) { * * @return the timestamp of the RGB image rendered into the texture. */ - public double updateTexture() { + private double updateTexture() { // Try this again here because it is possible that when the user called // connectToTangoCamera the texture wasn't assigned yet and the connection couldn't // be done @@ -212,35 +159,17 @@ private int connectTangoTexture() { return textureId; } - private void configureCamera() { - // This should never happen, but it never hurts to double-check. - if (mTango == null) { - return; - } - - // Configure the Rajawali Scene camera projection to match the Tango camera intrinsic. - TangoCameraIntrinsics intrinsics = mTango.getCameraIntrinsics(mCameraId); - Matrix4 projectionMatrix = ScenePoseCalcuator.calculateProjectionMatrix( - intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy, intrinsics.cx, - intrinsics.cy); - getCurrentCamera().setProjectionMatrix(projectionMatrix); - } - - /** - * Set-up device to sensors transforms. - */ - public void setupExtrinsics(TangoPoseData imuTDevicePose, TangoPoseData imuTColorCameraPose, - TangoPoseData imuTDepthCameraPose) { - mScenePoseCalcuator.setupExtrinsics(imuTDevicePose, imuTColorCameraPose, imuTDepthCameraPose); - } - /** * Intended to be called from TangoRajawaliView. */ - void connectCamera(Tango tango, int cameraId) { - this.mTango = tango; - this.mCameraId = cameraId; - this.mConnectedTextureId = connectTangoTexture(); + synchronized void connectCamera(Tango tango, int cameraId) { + mTango = tango; + mCameraId = cameraId; + mConnectedTextureId = connectTangoTexture(); + TangoCameraIntrinsics intrinsics = tango.getCameraIntrinsics(mCameraId); + mProjectionMatrix = ScenePoseCalculator.calculateProjectionMatrix( + intrinsics.width, intrinsics.height, + intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy); } /** @@ -270,7 +199,14 @@ synchronized void onTangoFrameAvailable() { * @return The timestamp. This can be used to associate camera data with a * pose or other sensor data using other pieces of the Tango API. */ - public double getTimestamp() { + synchronized public double getTimestamp() { return mLastRGBFrameTimestamp; } + + @Override + public void onRenderSurfaceSizeChanged(GL10 gl, int width, int height) { + super.onRenderSurfaceSizeChanged(gl, width, height); + // The camera projection matrix gets reset whenever the render surface is changed + mIsCameraConfigured = false; + } } diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliView.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliView.java index 1f376c21..4b2e145a 100644 --- a/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliView.java +++ b/TangoUtils/app/src/main/java/com/projecttango/rajawali/ar/TangoRajawaliView.java @@ -71,17 +71,6 @@ public double getTimestamp() { return mRenderer.getTimestamp(); } - /** - * Updates the TangoRajawaliView with the latest camera data. This does not - * synchronize data in the OpenGL context. - * - * Note, you need to call connectToTangoCamera() with a valid camera ID before - * calling this method. - */ - public void updateTexture() { - mRenderer.updateTexture(); - } - /** * Updates the TangoRajawaliView with the latest camera data. This method * synchronizes the data in the OpenGL context. diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/Axes.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/Axes.java deleted file mode 100644 index 433fe502..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/Axes.java +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright 2015 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package com.projecttango.rajawali.renderables; - -import android.graphics.Color; - -import org.rajawali3d.Object3D; -import org.rajawali3d.materials.Material; -import org.rajawali3d.math.vector.Vector3; -import org.rajawali3d.primitives.Line3D; - -import java.util.Stack; - -/** - * Rajawali object representing XYZ axes in 3D space. X is Red, - * Y is Green, and Z is Blue. - */ -public class Axes extends Object3D { - private float mThickness = 3f; - - public Axes() { - addAxis(new Vector3(1, 0, 0), Color.RED); - addAxis(new Vector3(0, 1, 0), Color.GREEN); - addAxis(new Vector3(0, 0, 1), Color.BLUE); - } - - private void addAxis(Vector3 direction, int color) { - Stack points = new Stack(); - points.add(new Vector3()); - points.add(direction); - Line3D line = new Line3D(points, mThickness); - Material material = new Material(); - material.setColor(color); - line.setMaterial(material); - addChild(line); - } -} diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/Frustum.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/Frustum.java deleted file mode 100644 index 6475a688..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/Frustum.java +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright 2015 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package com.projecttango.rajawali.renderables; - -import android.graphics.Color; - -import org.rajawali3d.Object3D; -import org.rajawali3d.materials.Material; -import org.rajawali3d.math.vector.Vector3; -import org.rajawali3d.primitives.Line3D; - -import java.util.Collections; -import java.util.Stack; - -/** - * Frustum primitive created from Lines. - */ -public class Frustum extends Object3D { - private float mDepth; - private float mHeight; - private float mWidth; - private float mThickness = 3f; - - public Frustum(float width, float mHeight, float mDepth) { - this.mDepth = mDepth; - this.mWidth = width; - this.mHeight = mHeight; - - Material black = new Material(); - black.setColor(Color.BLACK); - - Vector3 o = new Vector3(0, 0, 0); - Vector3 a = new Vector3(-width/2f, mHeight /2f, -mDepth); - Vector3 b = new Vector3(width/2f, mHeight /2f, -mDepth); - Vector3 c = new Vector3(width/2f, -mHeight /2f, -mDepth); - Vector3 d = new Vector3(-width/2f, -mHeight /2f, -mDepth); - - Line3D line; - Stack points; - - points = new Stack(); - Collections.addAll(points, new Vector3[]{ o, b, c, o, a, d, o }); - line = new Line3D(points, mThickness); - line.setMaterial(black); - addChild(line); - - points = new Stack(); - Collections.addAll(points, new Vector3[]{ a, b }); - line = new Line3D(points, mThickness); - line.setMaterial(black); - addChild(line); - - points = new Stack(); - Collections.addAll(points, new Vector3[]{ c, d }); - line = new Line3D(points, mThickness); - line.setMaterial(black); - addChild(line); - - setMaterial(black); - } -} diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/PointCloud.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/PointCloud.java new file mode 100644 index 00000000..b0f6fa12 --- /dev/null +++ b/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/PointCloud.java @@ -0,0 +1,78 @@ +package com.projecttango.rajawali.renderables; + +import android.graphics.Color; + +import com.projecttango.rajawali.renderables.primitives.Points; + +import org.rajawali3d.materials.Material; + +import java.nio.FloatBuffer; + +/** + * Renders a point cloud using colors to indicate distance to the depth sensor. + * Coloring is based on the light spectrum: closest points are in red, farthest in violet. + */ +public class PointCloud extends Points { + // Maximum depth range used to calculate coloring (min = 0) + public static final float CLOUD_MAX_Z = 5; + + private float[] mColorArray; + private final int[] mPalette; + public static final int PALETTE_SIZE = 360; + public static final float HUE_BEGIN = 0; + public static final float HUE_END = 320; + + public PointCloud(int maxPoints) { + super(maxPoints, true); + mPalette = createPalette(); + mColorArray = new float[maxPoints * 4]; + Material m = new Material(); + m.useVertexColors(true); + setMaterial(m); + } + + /** + * Pre-calculate a palette to be used to translate between point distance and RGB color. + */ + private int[] createPalette() { + int[] palette = new int[PALETTE_SIZE]; + float[] hsv = new float[3]; + hsv[1] = hsv[2] = 1; + for (int i = 0; i < PALETTE_SIZE; i++) { + hsv[0] = (HUE_END - HUE_BEGIN) * i / PALETTE_SIZE + HUE_BEGIN; + palette[i] = Color.HSVToColor(hsv); + } + return palette; + } + + /** + * Calculate the right color for each point in the point cloud. + */ + private void calculateColors(int pointCount, FloatBuffer pointCloudBuffer) { + float[] points = new float[pointCount * 3]; + pointCloudBuffer.rewind(); + pointCloudBuffer.get(points); + pointCloudBuffer.rewind(); + + int color; + int colorIndex; + float z; + for (int i = 0; i < pointCount; i++) { + z = points[i * 3 + 2]; + colorIndex = (int) Math.min(z / CLOUD_MAX_Z * mPalette.length, mPalette.length - 1); + color = mPalette[colorIndex]; + mColorArray[i * 4] = Color.red(color) / 255f; + mColorArray[i * 4 + 1] = Color.green(color) / 255f; + mColorArray[i * 4 + 2] = Color.blue(color) / 255f; + mColorArray[i * 4 + 3] = Color.alpha(color) / 255f; + } + } + + /** + * Update the points and colors in the point cloud. + */ + public void updateCloud(int pointCount, FloatBuffer pointBuffer) { + calculateColors(pointCount, pointBuffer); + updatePoints(pointCount, pointBuffer, mColorArray); + } +} diff --git a/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/primitives/Points.java b/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/primitives/Points.java index 4579683b..78cfab83 100644 --- a/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/primitives/Points.java +++ b/TangoUtils/app/src/main/java/com/projecttango/rajawali/renderables/primitives/Points.java @@ -15,12 +15,10 @@ */ package com.projecttango.rajawali.renderables.primitives; -import android.graphics.Color; import android.opengl.GLES10; import android.opengl.GLES20; import org.rajawali3d.Object3D; -import org.rajawali3d.materials.Material; import java.nio.FloatBuffer; @@ -31,37 +29,48 @@ public class Points extends Object3D { private int mMaxNumberOfVertices; - public Points(int numberOfPoints) { + public Points(int numberOfPoints, boolean isCreateColors) { super(); mMaxNumberOfVertices = numberOfPoints; - init(true); - Material m = new Material(); - m.setColor(Color.GREEN); - setMaterial(m); + init(true, isCreateColors); } // Initialize the buffers for Points primitive. - // Since only vertex and index buffers are used, we only initialize them using setData call. - protected void init(boolean createVBOs) { + // Since only vertex, index and color buffers are used, + // we only initialize them using setData call. + protected void init(boolean createVBOs, boolean createColors) { float[] vertices = new float[mMaxNumberOfVertices * 3]; int[] indices = new int[mMaxNumberOfVertices]; - for(int i = 0; i < indices.length; ++i){ + for (int i = 0; i < indices.length; ++i) { indices[i] = i; } - setData(vertices, GLES20.GL_STATIC_DRAW, - null, GLES20.GL_STATIC_DRAW, - null, GLES20.GL_STATIC_DRAW, - null, GLES20.GL_STATIC_DRAW, - indices, GLES20.GL_STATIC_DRAW, - true); + float[] colors = null; + if (createColors) { + colors = new float[mMaxNumberOfVertices * 4]; + } + setData(vertices, null, null, colors, indices, true); } - // Update the geometry of the points once new Point Cloud Data is available. - public void updatePoints(FloatBuffer pointCloudBuffer, int pointCount) { - pointCloudBuffer.position(0); + // Update the geometry of the points based on the provided points float buffer. + public void updatePoints(int pointCount, FloatBuffer pointCloudBuffer) { + mGeometry.setNumIndices(pointCount); + mGeometry.setVertices(pointCloudBuffer); + mGeometry.changeBufferData(mGeometry.getVertexBufferInfo(), mGeometry.getVertices(), 0, pointCount * 3); + } + + // Update the geometry of the points based on the provided points float buffer and corresponding + // colors based on the provided float array. + public void updatePoints(int pointCount, FloatBuffer points, float[] colors) { + if (pointCount > mMaxNumberOfVertices) { + throw new RuntimeException( + String.format("pointClount = %d exceeds maximum number of points = %d", + pointCount, mMaxNumberOfVertices)); + } mGeometry.setNumIndices(pointCount); - mGeometry.getVertices().position(0); - mGeometry.changeBufferData(mGeometry.getVertexBufferInfo(), pointCloudBuffer, 0, pointCount * 3); + mGeometry.setVertices(points); + mGeometry.changeBufferData(mGeometry.getVertexBufferInfo(), mGeometry.getVertices(), 0, pointCount * 3); + mGeometry.setColors(colors); + mGeometry.changeBufferData(mGeometry.getColorBufferInfo(), mGeometry.getColors(), 0, pointCount * 4); } public void preRender() { diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/ModelMatCalculator.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/ModelMatCalculator.java deleted file mode 100644 index 2dce0e45..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/ModelMatCalculator.java +++ /dev/null @@ -1,238 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils; - -import android.opengl.Matrix; - -/** - * Utility class to manage the calculation of a Model Matrix from the - * translation and quaternion arrays obtained from an {@link TangoPose} object. - * Delegates some mathematical computations to the {@link MathUtils}. - */ -public class ModelMatCalculator { - - private static float[] mConversionMatrix = new float[] { 1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 1.0f }; - - private float[] mModelMatrix = new float[16]; - private float[] mPointCloudModelMatrix = new float[16]; - private float[] mDevice2IMUMatrix = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, - 1.0f }; - private float[] mColorCamera2IMUMatrix = new float[] { 1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 1.0f }; - private float[] mOpengl2ColorCameraMatrix = new float[] { 1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 1.0f }; - - public ModelMatCalculator() { - Matrix.setIdentityM(mModelMatrix, 0); - Matrix.setIdentityM(mPointCloudModelMatrix, 0); - } - - /** - * Updates the model matrix (rotation and translation). - * - * @param translation - * a three-element array of translation data. - * @param quaternion - * a four-element array of rotation data. - */ - public void updatePointCloudModelMatrix(float[] translation, - float[] quaternion) { - - float[] tempMultMatrix = new float[16]; - Matrix.setIdentityM(tempMultMatrix, 0); - Matrix.multiplyMM(tempMultMatrix, 0, mColorCamera2IMUMatrix, 0, - mOpengl2ColorCameraMatrix, 0); - float[] tempInvertMatrix = new float[16]; - Matrix.setIdentityM(tempInvertMatrix, 0); - Matrix.invertM(tempInvertMatrix, 0, mDevice2IMUMatrix, 0); - float[] tempMultMatrix2 = new float[16]; - Matrix.setIdentityM(tempMultMatrix2, 0); - Matrix.multiplyMM(tempMultMatrix2, 0, tempInvertMatrix, 0, - tempMultMatrix, 0); - - float[] quaternionMatrix = new float[16]; - Matrix.setIdentityM(quaternionMatrix, 0); - quaternionMatrix = quaternionMatrixOpenGL(quaternion); - float[] tempMultMatrix3 = new float[16]; - Matrix.setIdentityM(tempMultMatrix3, 0); - Matrix.setIdentityM(mPointCloudModelMatrix, 0); - Matrix.multiplyMM(tempMultMatrix3, 0, quaternionMatrix, 0, - tempMultMatrix2, 0); - Matrix.multiplyMM(mPointCloudModelMatrix, 0, mConversionMatrix, 0, - tempMultMatrix3, 0); - mPointCloudModelMatrix[12] += translation[0]; - mPointCloudModelMatrix[13] += translation[2]; - mPointCloudModelMatrix[14] += -1f * translation[1]; - } - - /** - * Updates the model matrix (rotation and translation). - * - * @param translation - * a three-element array of translation data. - * @param quaternion - * a four-element array of rotation data. - */ - public void updateModelMatrix(float[] translation, float[] quaternion) { - - float[] tempMultMatrix = new float[16]; - Matrix.setIdentityM(tempMultMatrix, 0); - Matrix.multiplyMM(tempMultMatrix, 0, mColorCamera2IMUMatrix, 0, - mOpengl2ColorCameraMatrix, 0); - float[] tempInvertMatrix = new float[16]; - Matrix.setIdentityM(tempInvertMatrix, 0); - Matrix.invertM(tempInvertMatrix, 0, mDevice2IMUMatrix, 0); - float[] tempMultMatrix2 = new float[16]; - Matrix.setIdentityM(tempMultMatrix2, 0); - Matrix.multiplyMM(tempMultMatrix2, 0, tempInvertMatrix, 0, - tempMultMatrix, 0); - - float[] quaternionMatrix = new float[16]; - Matrix.setIdentityM(quaternionMatrix, 0); - quaternionMatrix = quaternionMatrixOpenGL(quaternion); - float[] tempMultMatrix3 = new float[16]; - Matrix.setIdentityM(tempMultMatrix3, 0); - Matrix.setIdentityM(mModelMatrix, 0); - Matrix.multiplyMM(tempMultMatrix3, 0, quaternionMatrix, 0, - tempMultMatrix2, 0); - Matrix.multiplyMM(mModelMatrix, 0, mConversionMatrix, 0, - tempMultMatrix3, 0); - mModelMatrix[12] += translation[0]; - mModelMatrix[13] += translation[2]; - mModelMatrix[14] += -1f * translation[1]; - } - - public void SetDevice2IMUMatrix(float[] translation, float[] quaternion) { - mDevice2IMUMatrix = quaternionMatrixOpenGL(quaternion); - mDevice2IMUMatrix[12] = translation[0]; - mDevice2IMUMatrix[13] = translation[1]; - mDevice2IMUMatrix[14] = translation[2]; - } - - public void SetColorCamera2IMUMatrix(float[] translation, float[] quaternion) { - mOpengl2ColorCameraMatrix = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, - 1.0f }; - mColorCamera2IMUMatrix = quaternionMatrixOpenGL(quaternion); - mColorCamera2IMUMatrix[12] = translation[0]; - mColorCamera2IMUMatrix[13] = translation[1]; - mColorCamera2IMUMatrix[14] = translation[2]; - } - - public float[] getModelMatrix() { - return mModelMatrix; - } - - public float[] getModelMatrixCopy() { - float[] modelMatCopy = new float[16]; - System.arraycopy(mModelMatrix, 0, modelMatCopy, 0, 16); - return modelMatCopy; - } - - public float[] getPointCloudModelMatrixCopy() { - float[] modelMatCopy = new float[16]; - float[] tempMultMat = new float[16]; - Matrix.setIdentityM(tempMultMat, 0); - float[] invertYandZMatrix = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, - 1.0f }; - Matrix.multiplyMM(tempMultMat, 0, mPointCloudModelMatrix, 0, - invertYandZMatrix, 0); - System.arraycopy(tempMultMat, 0, modelMatCopy, 0, 16); - return modelMatCopy; - } - - public float[] getTranslation() { - return new float[] { mModelMatrix[12], mModelMatrix[13], - mModelMatrix[14] }; - } - - /** - * A function to convert a quaternion to quaternion Matrix. Please note that - * Opengl.Matrix is Column Major and so we construct the matrix in Column - * Major Format. - - - - | 0 4 8 12 | | 1 5 9 13 | | 2 6 10 14 | | 3 7 11 15 - * | - - - - - * - * @param quaternion - * Input quaternion with float[4] - * @return Quaternion Matrix of float[16] - */ - public static float[] quaternionMatrixOpenGL(float[] quaternion) { - float[] matrix = new float[16]; - normalizeVector(quaternion); - - float x = quaternion[0]; - float y = quaternion[1]; - float z = quaternion[2]; - float w = quaternion[3]; - - float x2 = x * x; - float y2 = y * y; - float z2 = z * z; - float xy = x * y; - float xz = x * z; - float yz = y * z; - float wx = w * x; - float wy = w * y; - float wz = w * z; - - matrix[0] = 1f - 2f * (y2 + z2); - matrix[4] = 2f * (xy - wz); - matrix[8] = 2f * (xz + wy); - matrix[12] = 0f; - - matrix[1] = 2f * (xy + wz); - matrix[5] = 1f - 2f * (x2 + z2); - matrix[9] = 2f * (yz - wx); - matrix[13] = 0f; - - matrix[2] = 2f * (xz - wy); - matrix[6] = 2f * (yz + wx); - matrix[10] = 1f - 2f * (x2 + y2); - matrix[14] = 0f; - - matrix[3] = 0f; - matrix[7] = 0f; - matrix[11] = 0f; - matrix[15] = 1f; - - return matrix; - } - - /** - * Creates a unit vector in the direction of an arbitrary vector. The - * original vector is modified in place. - * - * @param v - * the vector to normalize - */ - public static void normalizeVector(float[] v) { - float mag2 = v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]; - if (Math.abs(mag2) > 0.00001f && Math.abs(mag2 - 1.0f) > 0.00001f) { - float mag = (float) Math.sqrt(mag2); - v[0] = v[0] / mag; - v[1] = v[1] / mag; - v[2] = v[2] / mag; - v[3] = v[3] / mag; - } - } -} diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/Renderer.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/Renderer.java deleted file mode 100644 index e1d292e3..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/Renderer.java +++ /dev/null @@ -1,293 +0,0 @@ -package com.projecttango.tangoutils; - -import android.opengl.Matrix; -import android.util.Log; -import android.view.MotionEvent; - -public class Renderer { - - protected static final int FIRST_PERSON = 0; - protected static final int TOP_DOWN = 1; - protected static final int THIRD_PERSON = 2; - protected static final int THIRD_PERSON_FOV = 65; - protected static final int TOPDOWN_FOV = 65; - protected static final int MATRIX_4X4 = 16; - - protected static final float CAMERA_FOV = 37.8f; - protected static final float CAMERA_NEAR = 0.01f; - protected static final float CAMERA_FAR = 200f; - protected float mCameraAspect; - protected float[] mProjectionMatrix = new float[MATRIX_4X4]; - private ModelMatCalculator mModelMatCalculator; - private int viewId = 2; - protected float[] mViewMatrix = new float[MATRIX_4X4]; - protected float[] mCameraPosition; - protected float[] mLookAtPosition; - protected float[] mCameraUpVector; - private float[] mDevicePosition; - private float mCameraOrbitRadius; - private float mRotationX; - private float mRotationY; - private float mPreviousRotationX; - private float mPreviousRotationY; - private float mPreviousTouchX; - private float mPreviousTouchY; - private float mTouch1X, mTouch2X, mTouch1Y, mTouch2Y, mTouchStartDistance, - mTouchMoveDistance, mStartCameraRadius; - - public Renderer() { - mModelMatCalculator = new ModelMatCalculator(); - mRotationX = (float) Math.PI / 4; - mRotationY = 0; - mCameraOrbitRadius = 5.0f; - mCameraPosition = new float[3]; - mCameraPosition[0] = 5f; - mCameraPosition[1] = 5f; - mCameraPosition[2] = 5f; - mDevicePosition = new float[3]; - mDevicePosition[0] = 0; - mDevicePosition[1] = 0; - mDevicePosition[2] = 0; - } - - /** - * Update the view matrix of the Renderer to follow the position of the - * device in the current perspective. - */ - public void updateViewMatrix() { - mDevicePosition = mModelMatCalculator.getTranslation(); - - switch (viewId) { - case FIRST_PERSON: - float[] invertModelMat = new float[MATRIX_4X4]; - Matrix.setIdentityM(invertModelMat, 0); - - float[] temporaryMatrix = new float[MATRIX_4X4]; - Matrix.setIdentityM(temporaryMatrix, 0); - - Matrix.setIdentityM(mViewMatrix, 0); - Matrix.invertM(invertModelMat, 0, - mModelMatCalculator.getModelMatrix(), 0); - Matrix.multiplyMM(temporaryMatrix, 0, mViewMatrix, 0, - invertModelMat, 0); - System.arraycopy(temporaryMatrix, 0, mViewMatrix, 0, 16); - break; - case THIRD_PERSON: - - Matrix.setLookAtM(mViewMatrix, 0, mDevicePosition[0] - + mCameraPosition[0], mCameraPosition[1] - + mDevicePosition[1], mCameraPosition[2] - + mDevicePosition[2], mDevicePosition[0], - mDevicePosition[1], mDevicePosition[2], 0f, 1f, 0f); - break; - case TOP_DOWN: - // Matrix.setIdentityM(mViewMatrix, 0); - Matrix.setLookAtM(mViewMatrix, 0, mDevicePosition[0] - + mCameraPosition[0], mCameraPosition[1], - mCameraPosition[2] + mDevicePosition[2], mDevicePosition[0] - + mCameraPosition[0], mCameraPosition[1] - 5, - mCameraPosition[2] + mDevicePosition[2], 0f, 0f, -1f); - break; - default: - viewId = THIRD_PERSON; - return; - } - } - - public boolean onTouchEvent(MotionEvent event) { - if (viewId == THIRD_PERSON) { - int pointCount = event.getPointerCount(); - if (pointCount == 1) { - switch (event.getAction()) { - case MotionEvent.ACTION_DOWN: { - final float x = event.getX(); - final float y = event.getY(); - // Remember where we started - mPreviousTouchX = x; - mPreviousTouchY = y; - mPreviousRotationX = mRotationX; - mPreviousRotationY = mRotationY; - break; - } - case MotionEvent.ACTION_MOVE: { - final float x = event.getX(); - final float y = event.getY(); - // Calculate the distance moved - final float dx = mPreviousTouchX - x; - final float dy = mPreviousTouchY - y; - mRotationX = mPreviousRotationX - + (float) (Math.PI * dx / 1900); // ScreenWidth - mRotationY = mPreviousRotationY - + (float) (Math.PI * dy / 1200); // Screen height - if (mRotationY > (float) Math.PI) - mRotationY = (float) Math.PI; - if (mRotationY < 0) - mRotationY = 0.0f; - mCameraPosition[0] = (float) (mCameraOrbitRadius * Math - .sin(mRotationX)); - mCameraPosition[1] = (float) (mCameraOrbitRadius * Math - .cos(mRotationY)); - mCameraPosition[2] = (float) (mCameraOrbitRadius * Math - .cos(mRotationX)); - break; - } - } - } - if (pointCount == 2) { - switch (event.getActionMasked()) { - case MotionEvent.ACTION_DOWN: - case MotionEvent.ACTION_POINTER_DOWN: { - mTouch1X = event.getX(0); - mTouch1Y = event.getY(0); - mTouch2X = event.getX(1); - mTouch2Y = event.getY(1); - mTouchStartDistance = (float) Math.sqrt(Math.pow(mTouch1X - - mTouch2X, 2) - + Math.pow(mTouch1Y - mTouch2Y, 2)); - mStartCameraRadius = mCameraOrbitRadius; - break; - } - case MotionEvent.ACTION_MOVE: { - mTouch1X = event.getX(0); - mTouch1Y = event.getY(0); - mTouch2X = event.getX(1); - mTouch2Y = event.getY(1); - mTouchMoveDistance = (float) Math.sqrt(Math.pow(mTouch1X - - mTouch2X, 2) - + Math.pow(mTouch1Y - mTouch2Y, 2)); - float tmp = 0.05f * (mTouchMoveDistance - mTouchStartDistance); - mCameraOrbitRadius = mStartCameraRadius - tmp; - if (mCameraOrbitRadius < 1) - mCameraOrbitRadius = 1; - mCameraPosition[0] = (float) (mCameraOrbitRadius * Math - .sin(mRotationX)); - mCameraPosition[1] = (float) (mCameraOrbitRadius * Math - .cos(mRotationY)); - mCameraPosition[2] = (float) (mCameraOrbitRadius * Math - .cos(mRotationX)); - break; - } - case MotionEvent.ACTION_POINTER_UP: { - int index = event.getActionIndex() == 0 ? 1 : 0; - final float x = event.getX(index); - final float y = event.getY(index); - // Remember where we started - mPreviousTouchX = x; - mPreviousTouchY = y; - mPreviousRotationX = mRotationX; - mPreviousRotationY = mRotationY; - } - } - } - } else if (viewId == TOP_DOWN) { - int pointCount = event.getPointerCount(); - if (pointCount == 1) { - switch (event.getAction()) { - case MotionEvent.ACTION_DOWN: { - final float x = event.getX(); - final float y = event.getY(); - // Remember where we started - mPreviousTouchX = x; - mPreviousTouchY = y; - mPreviousRotationX = mCameraPosition[0]; - mPreviousRotationY = mCameraPosition[2]; - break; - } - case MotionEvent.ACTION_MOVE: { - final float x = event.getX(); - final float y = event.getY(); - // Calculate the distance moved - final float dx = mPreviousTouchX - x; - final float dy = mPreviousTouchY - y; - mCameraPosition[0] = mPreviousRotationX + dx / 190; - mCameraPosition[2] = mPreviousRotationY + dy / 120; - break; - } - } - } - if (pointCount == 2) { - switch (event.getActionMasked()) { - case MotionEvent.ACTION_DOWN: - case MotionEvent.ACTION_POINTER_DOWN: { - mTouch1X = event.getX(0); - mTouch1Y = event.getY(0); - mTouch2X = event.getX(1); - mTouch2Y = event.getY(1); - mTouchStartDistance = (float) Math.sqrt(Math.pow(mTouch1X - - mTouch2X, 2) - + Math.pow(mTouch1Y - mTouch2Y, 2)); - mStartCameraRadius = mCameraPosition[1]; - Log.i("Start Radius is :", "" + mStartCameraRadius); - break; - } - case MotionEvent.ACTION_MOVE: { - mTouch1X = event.getX(0); - mTouch1Y = event.getY(0); - mTouch2X = event.getX(1); - mTouch2Y = event.getY(1); - mTouchMoveDistance = (float) Math.sqrt(Math.pow(mTouch1X - - mTouch2X, 2) - + Math.pow(mTouch1Y - mTouch2Y, 2)); - float tmp = 0.05f * (mTouchMoveDistance - mTouchStartDistance); - mCameraPosition[1] = mStartCameraRadius - tmp; - break; - } - case MotionEvent.ACTION_POINTER_UP: { - int index = event.getActionIndex() == 0 ? 1 : 0; - final float x = event.getX(index); - final float y = event.getY(index); - // Remember where we started - mPreviousTouchX = x; - mPreviousTouchY = y; - mPreviousRotationX = mCameraPosition[0]; - mPreviousRotationY = mCameraPosition[2]; - } - } - } - } - return true; - } - - public void setFirstPersonView() { - viewId = FIRST_PERSON; - Matrix.perspectiveM(mProjectionMatrix, 0, CAMERA_FOV, mCameraAspect, - CAMERA_NEAR, CAMERA_FAR); - } - - public void setThirdPersonView() { - viewId = THIRD_PERSON; - mCameraPosition[0] = 5; - mCameraPosition[1] = 5; - mCameraPosition[2] = 5; - mRotationX = mRotationY = (float) (Math.PI / 4); - mCameraOrbitRadius = 5.0f; - Matrix.perspectiveM(mProjectionMatrix, 0, THIRD_PERSON_FOV, - mCameraAspect, CAMERA_NEAR, CAMERA_FAR); - } - - public void setTopDownView() { - viewId = TOP_DOWN; - mCameraPosition[0] = 0; - mCameraPosition[1] = 5; - mCameraPosition[2] = 0; - Matrix.perspectiveM(mProjectionMatrix, 0, TOPDOWN_FOV, mCameraAspect, - CAMERA_NEAR, CAMERA_FAR); - } - - public void resetModelMatCalculator() { - mModelMatCalculator = new ModelMatCalculator(); - } - - public ModelMatCalculator getModelMatCalculator() { - return mModelMatCalculator; - } - - public float[] getViewMatrix() { - return mViewMatrix; - } - - public float[] getProjectionMatrix() { - return mProjectionMatrix; - } - -} diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/CameraFrustum.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/CameraFrustum.java deleted file mode 100644 index 5cb6837f..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/CameraFrustum.java +++ /dev/null @@ -1,138 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils.renderables; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.nio.FloatBuffer; - -import android.opengl.GLES20; -import android.opengl.Matrix; - -/** - * {@link Renderable} OpenGL object showing the Camera Frustum in 3D. This shows - * the view of the Tango camera at the current translation and rotation. - */ -public class CameraFrustum extends Renderable { - - private static final int COORDS_PER_VERTEX = 3; - - private static final String sVertexShaderCode = "uniform mat4 uMVPMatrix;" - + "attribute vec4 vPosition;" + "attribute vec4 aColor;" - + "varying vec4 vColor;" + "void main() {" + " vColor=aColor;" - + "gl_Position = uMVPMatrix * vPosition;" + "}"; - - private static final String sFragmentShaderCode = "precision mediump float;" - + "varying vec4 vColor;" - + "void main() {" - + "gl_FragColor = vec4(0.8,0.5,0.8,1);" + "}"; - - private FloatBuffer mVertexBuffer, mColorBuffer; - - private float mVertices[] = { 0.0f, 0.0f, 0.0f, -0.4f, 0.3f, -0.5f, - - 0.0f, 0.0f, 0.0f, 0.4f, 0.3f, -0.5f, - - 0.0f, 0.0f, 0.0f, -0.4f, -0.3f, -0.5f, - - 0.0f, 0.0f, 0.0f, 0.4f, -0.3f, -0.5f, - - -0.4f, 0.3f, -0.5f, 0.4f, 0.3f, -0.5f, - - 0.4f, 0.3f, -0.5f, 0.4f, -0.3f, -0.5f, - - 0.4f, -0.3f, -0.5f, -0.4f, -0.3f, -0.5f, - - -0.4f, -0.3f, -0.5f, -0.4f, 0.3f, -0.5f }; - - private float mColors[] = { 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, - - 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, - - 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f }; - - private final int mProgram; - private int mPosHandle, mColorHandle; - private int mMVPMatrixHandle; - - public CameraFrustum() { - // Reset the model matrix to the identity - Matrix.setIdentityM(getModelMatrix(), 0); - - // Load the vertices into a vertex buffer - ByteBuffer byteBuf = ByteBuffer.allocateDirect(mVertices.length * 4); - byteBuf.order(ByteOrder.nativeOrder()); - mVertexBuffer = byteBuf.asFloatBuffer(); - mVertexBuffer.put(mVertices); - mVertexBuffer.position(0); - - // Load the colors into a color buffer - ByteBuffer cByteBuff = ByteBuffer.allocateDirect(mColors.length * 4); - cByteBuff.order(ByteOrder.nativeOrder()); - mColorBuffer = cByteBuff.asFloatBuffer(); - mColorBuffer.put(mColors); - mColorBuffer.position(0); - - // Load the vertex and fragment shaders, then link the program - int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER, - sVertexShaderCode); - int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER, - sFragmentShaderCode); - mProgram = GLES20.glCreateProgram(); - GLES20.glAttachShader(mProgram, vertexShader); - GLES20.glAttachShader(mProgram, fragShader); - GLES20.glLinkProgram(mProgram); - } - - @Override - public void draw(float[] viewMatrix, float[] projectionMatrix) { - GLES20.glUseProgram(mProgram); - // updateViewMatrix(viewMatrix); - - // Compose the model, view, and projection matrices into a single mvp - // matrix - updateMvpMatrix(viewMatrix, projectionMatrix); - - // Load vertex attribute data - mPosHandle = GLES20.glGetAttribLocation(mProgram, "vPosition"); - GLES20.glVertexAttribPointer(mPosHandle, COORDS_PER_VERTEX, - GLES20.GL_FLOAT, false, 0, mVertexBuffer); - GLES20.glEnableVertexAttribArray(mPosHandle); - - // Load color attribute data - mColorHandle = GLES20.glGetAttribLocation(mProgram, "aColor"); - GLES20.glVertexAttribPointer(mColorHandle, 4, GLES20.GL_FLOAT, false, - 0, mColorBuffer); - GLES20.glEnableVertexAttribArray(mColorHandle); - - // Draw the CameraFrustum - mMVPMatrixHandle = GLES20.glGetUniformLocation(mProgram, "uMVPMatrix"); - GLES20.glUniformMatrix4fv(mMVPMatrixHandle, 1, false, getMvpMatrix(), 0); - GLES20.glLineWidth(1); - GLES20.glDrawArrays(GLES20.GL_LINES, 0, 16); - } -} \ No newline at end of file diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/CameraFrustumAndAxis.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/CameraFrustumAndAxis.java deleted file mode 100644 index 20f5c81c..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/CameraFrustumAndAxis.java +++ /dev/null @@ -1,150 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils.renderables; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.nio.FloatBuffer; - -import android.opengl.GLES20; -import android.opengl.Matrix; - -/** - * {@link Renderable} OpenGL object representing XYZ axes in 3D space. X is Red, - * Y is Green, and Z is Blue. - */ -public class CameraFrustumAndAxis extends Renderable { - - private static final int COORDS_PER_VERTEX = 3; - - private static final String sVertexShaderCode = "uniform mat4 uMVPMatrix;" - + "attribute vec4 vPosition;" + "attribute vec4 aColor;" - + "varying vec4 vColor;" + "void main() {" + " vColor=aColor;" - + "gl_Position = uMVPMatrix * vPosition;" + "}"; - - private static final String sFragmentShaderCode = "precision mediump float;" - + "varying vec4 vColor;" - + "void main() {" - + "gl_FragColor = vColor;" + "}"; - private FloatBuffer mVertexBuffer, mColorBuffer; - - private float mVertices[] = { 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, - - 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, - - 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, -0.4f, 0.3f, -0.5f, - - 0.0f, 0.0f, 0.0f, 0.4f, 0.3f, -0.5f, - - 0.0f, 0.0f, 0.0f, -0.4f, -0.3f, -0.5f, - - 0.0f, 0.0f, 0.0f, 0.4f, -0.3f, -0.5f, - - -0.4f, 0.3f, -0.5f, 0.4f, 0.3f, -0.5f, - - 0.4f, 0.3f, -0.5f, 0.4f, -0.3f, -0.5f, - - 0.4f, -0.3f, -0.5f, -0.4f, -0.3f, -0.5f, - - -0.4f, -0.3f, -0.5f, -0.4f, 0.3f, -0.5f }; - - private float mColors[] = { 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, - - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, }; - - private final int mProgram; - private int mPosHandle, mColorHandle; - private int mMVPMatrixHandle; - - public CameraFrustumAndAxis() { - // Set model matrix to the identity - Matrix.setIdentityM(getModelMatrix(), 0); - - // Put vertices into a vertex buffer - ByteBuffer byteBuf = ByteBuffer.allocateDirect(mVertices.length * 4); - byteBuf.order(ByteOrder.nativeOrder()); - mVertexBuffer = byteBuf.asFloatBuffer(); - mVertexBuffer.put(mVertices); - mVertexBuffer.position(0); - - // Put colors into a color buffer - ByteBuffer cByteBuff = ByteBuffer.allocateDirect(mColors.length * 4); - cByteBuff.order(ByteOrder.nativeOrder()); - mColorBuffer = cByteBuff.asFloatBuffer(); - mColorBuffer.put(mColors); - mColorBuffer.position(0); - - // Load the vertex and fragment shaders, then link the program - int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER, - sVertexShaderCode); - int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER, - sFragmentShaderCode); - mProgram = GLES20.glCreateProgram(); - GLES20.glAttachShader(mProgram, vertexShader); - GLES20.glAttachShader(mProgram, fragShader); - GLES20.glLinkProgram(mProgram); - } - - @Override - public synchronized void draw(float[] viewMatrix, float[] projectionMatrix) { - GLES20.glUseProgram(mProgram); - - // Compose the model, view, and projection matrices into a single m-v-p - // matrix - updateMvpMatrix(viewMatrix, projectionMatrix); - - // Load vertex attribute data - mPosHandle = GLES20.glGetAttribLocation(mProgram, "vPosition"); - GLES20.glVertexAttribPointer(mPosHandle, COORDS_PER_VERTEX, - GLES20.GL_FLOAT, false, 0, mVertexBuffer); - GLES20.glEnableVertexAttribArray(mPosHandle); - - // Load color attribute data - mColorHandle = GLES20.glGetAttribLocation(mProgram, "aColor"); - GLES20.glVertexAttribPointer(mColorHandle, 4, GLES20.GL_FLOAT, false, - 0, mColorBuffer); - GLES20.glEnableVertexAttribArray(mColorHandle); - - // Draw the CameraFrustumAndAxis - mMVPMatrixHandle = GLES20.glGetUniformLocation(mProgram, "uMVPMatrix"); - GLES20.glUniformMatrix4fv(mMVPMatrixHandle, 1, false, getMvpMatrix(), 0); - GLES20.glLineWidth(3); - GLES20.glDrawArrays(GLES20.GL_LINES, 0, mVertices.length / 3); - - } - -} \ No newline at end of file diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Grid.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Grid.java deleted file mode 100644 index 58de3fda..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Grid.java +++ /dev/null @@ -1,106 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils.renderables; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.nio.FloatBuffer; - -import android.opengl.GLES20; -import android.opengl.Matrix; - -/** - * {@link Renderable} OpenGL object showing the 'floor' of the current scene. - * This is a static grid placed in the scene to provide perspective in the - * various views. - */ -public class Grid extends Renderable { - - private static final int COORDS_PER_VERTEX = 3; - private static final int GRID_RANGE_M = 100; - private static final int BYTES_PER_FLOAT = 4; - - private static final String sVertexShaderCode = "uniform mat4 uMVPMatrix;" - + "attribute vec4 vPosition;" + "void main() {" - + "gl_Position = uMVPMatrix * vPosition;" + "}"; - private static final String sFragmentShaderCode = "precision mediump float;" - + "uniform vec4 vColor;" - + "void main() {" - + " gl_FragColor = vec4(0.8,0.8,0.8,1.0);" + "}"; - - private FloatBuffer mVertexBuffer; - private final int mProgram; - private int mPosHandle; - private int mMVPMatrixHandle; - - public Grid() { - // Reset the model matrix to the identity - Matrix.setIdentityM(getModelMatrix(), 0); - - // Allocate a vertex buffer - ByteBuffer vertexByteBuffer = ByteBuffer - .allocateDirect((GRID_RANGE_M * 2 + 1) * 4 * 3 - * BYTES_PER_FLOAT); - vertexByteBuffer.order(ByteOrder.nativeOrder()); - mVertexBuffer = vertexByteBuffer.asFloatBuffer(); - - // Load the vertices for the z-axis grid lines into the vertex buffer - for (int x = -GRID_RANGE_M; x <= GRID_RANGE_M; x++) { - mVertexBuffer.put(new float[] { x, -1.3f, (float) -GRID_RANGE_M }); - mVertexBuffer.put(new float[] { x, -1.3f, (float) GRID_RANGE_M }); - } - - // Load the vertices for the x-axis grid lines into the vertex buffer - for (int z = -GRID_RANGE_M; z <= GRID_RANGE_M; z++) { - mVertexBuffer.put(new float[] { (float) -GRID_RANGE_M, -1.3f, z }); - mVertexBuffer.put(new float[] { (float) GRID_RANGE_M, -1.3f, z }); - } - - // Load the vertex and fragment shaders, then link the program - int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER, - sVertexShaderCode); - int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER, - sFragmentShaderCode); - mProgram = GLES20.glCreateProgram(); - GLES20.glAttachShader(mProgram, vertexShader); - GLES20.glAttachShader(mProgram, fragShader); - GLES20.glLinkProgram(mProgram); - } - - @Override - public void draw(float[] viewMatrix, float[] projectionMatrix) { - GLES20.glUseProgram(mProgram); - mVertexBuffer.position(0); - - // Compose the model, view, and projection matrices into a single m-v-p - // matrix - updateMvpMatrix(viewMatrix, projectionMatrix); - - // Load vertex attribute data - mPosHandle = GLES20.glGetAttribLocation(mProgram, "vPosition"); - GLES20.glVertexAttribPointer(mPosHandle, COORDS_PER_VERTEX, - GLES20.GL_FLOAT, false, 0, mVertexBuffer); - GLES20.glEnableVertexAttribArray(mPosHandle); - - // Draw the Grid - mMVPMatrixHandle = GLES20.glGetUniformLocation(mProgram, "uMVPMatrix"); - GLES20.glUniformMatrix4fv(mMVPMatrixHandle, 1, false, getMvpMatrix(), 0); - GLES20.glLineWidth(1); - GLES20.glDrawArrays(GLES20.GL_LINES, 0, (GRID_RANGE_M * 2 + 1) * 4); - } - -} \ No newline at end of file diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/PointCloud.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/PointCloud.java deleted file mode 100644 index 908feb54..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/PointCloud.java +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils.renderables; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.nio.FloatBuffer; -import java.util.concurrent.atomic.AtomicBoolean; - -import android.opengl.GLES20; -import android.opengl.Matrix; - -/** - * {@link Renderable} OpenGL showing a PointCloud obtained from Tango XyzIj - * data. The point count can vary over as the information is updated. - */ -public class PointCloud extends Renderable { - - private static final int COORDS_PER_VERTEX = 3; - - private static final String sVertexShaderCode = "uniform mat4 uMVPMatrix;" - + "attribute vec4 vPosition;" + "varying vec4 vColor;" - + "void main() {" + "gl_PointSize = 5.0;" - + " gl_Position = uMVPMatrix * vPosition;" - + " vColor = vPosition;" + "}"; - private static final String sFragmentShaderCode = "precision mediump float;" - + "varying vec4 vColor;" - + "void main() {" - + " gl_FragColor = vec4(vColor);" + "}"; - - private static final int BYTES_PER_FLOAT = 4; - private static final int POINT_TO_XYZ = 3; - - int mVertexVBO; // VertexBufferObject. - private AtomicBoolean mUpdateVBO = new AtomicBoolean(); - private volatile FloatBuffer mPointCloudBuffer; - - private final int mProgram; - private int mPosHandle; - private int mMVPMatrixHandle; - private int mPointCount; - private float mAverageZ; - - public PointCloud(int maxDepthPoints) { - mAverageZ = 0; - int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER, - sVertexShaderCode); - int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER, - sFragmentShaderCode); - mProgram = GLES20.glCreateProgram(); - GLES20.glAttachShader(mProgram, vertexShader); - GLES20.glAttachShader(mProgram, fragShader); - GLES20.glLinkProgram(mProgram); - Matrix.setIdentityM(getModelMatrix(), 0); - - final int buffers[] = new int[1]; - GLES20.glGenBuffers(1, buffers, 0); - mVertexVBO = buffers[0]; - } - - public synchronized void UpdatePoints(FloatBuffer pointCloudFloatBuffer) { - //save the reference in order to update this in the proper thread. - mPointCloudBuffer = pointCloudFloatBuffer; - - //signal the update - mUpdateVBO.set(true); - } - - @Override - public synchronized void draw(float[] viewMatrix, float[] projectionMatrix) { - GLES20.glBindBuffer(GLES20.GL_ARRAY_BUFFER, mVertexVBO); - - if (mUpdateVBO.getAndSet(false)) { - if (mPointCloudBuffer != null) { - mPointCloudBuffer.position(0); - // Pass the info to the VBO - GLES20.glBufferData(GLES20.GL_ARRAY_BUFFER, mPointCloudBuffer.capacity() - * BYTES_PER_FLOAT, mPointCloudBuffer, GLES20.GL_STATIC_DRAW); - mPointCount = mPointCloudBuffer.capacity() / 3; - float totalZ = 0; - for (int i = 0; i < mPointCloudBuffer.capacity() - 3; i = i + 3) { - totalZ = totalZ + mPointCloudBuffer.get(i + 2); - } - if (mPointCount != 0) - mAverageZ = totalZ / mPointCount; - // signal the update - mUpdateVBO.set(true); - } - mPointCloudBuffer = null; - } - - if (mPointCount > 0) { - - GLES20.glUseProgram(mProgram); - updateMvpMatrix(viewMatrix, projectionMatrix); - GLES20.glVertexAttribPointer(mPosHandle, COORDS_PER_VERTEX, GLES20.GL_FLOAT, false, 0, - 0); - GLES20.glEnableVertexAttribArray(mPosHandle); - GLES20.glUniformMatrix4fv(mMVPMatrixHandle, 1, false, getMvpMatrix(), 0); - GLES20.glDrawArrays(GLES20.GL_POINTS, 0, mPointCount); - } - GLES20.glBindBuffer(GLES20.GL_ARRAY_BUFFER, 0); - } - - public float getAverageZ() { - return mAverageZ; - } - - public int getPointCount() { - return mPointCount; - } -} diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/RenderUtils.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/RenderUtils.java deleted file mode 100644 index f4b57cc2..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/RenderUtils.java +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils.renderables; - -import android.opengl.GLES20; - -/** - * Static functions used by Renderer classes in Tango Java samples. - */ -public class RenderUtils { - - /** - * Creates a vertex or fragment shader. - * - * @param type - * one of GLES20.GL_VERTEX_SHADER or GLES20.GL_FRAGMENT_SHADER - * @param shaderCode - * GLSL code for the shader as a String - * @return a compiled shader. - */ - public static int loadShader(int type, String shaderCode) { - // Create a shader of the correct type - int shader = GLES20.glCreateShader(type); - - // Compile the shader from source code - GLES20.glShaderSource(shader, shaderCode); - GLES20.glCompileShader(shader); - - return shader; - } - -} diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Renderable.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Renderable.java deleted file mode 100644 index 399e69a8..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Renderable.java +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils.renderables; - -import android.opengl.Matrix; - -/** - * Base class for all self-drawing OpenGL objects used in Tango Java examples. - * Contains common logic for handling the MVP matrices. - */ -public abstract class Renderable { - - private float[] mModelMatrix = new float[16]; - private float[] mMvMatrix = new float[16]; - private float[] mMvpMatrix = new float[16]; - - /** - * Applies the view and projection matrices and draws the Renderable. - * - * @param viewMatrix - * the view matrix to map from world space to camera space. - * @param projectionMatrix - * the projection matrix to map from camera space to screen - * space. - */ - public abstract void draw(float[] viewMatrix, float[] projectionMatrix); - - public synchronized void updateMvpMatrix(float[] viewMatrix, - float[] projectionMatrix) { - // Compose the model, view, and projection matrices into a single mvp - // matrix - Matrix.setIdentityM(mMvMatrix, 0); - Matrix.setIdentityM(mMvpMatrix, 0); - Matrix.multiplyMM(mMvMatrix, 0, viewMatrix, 0, mModelMatrix, 0); - Matrix.multiplyMM(mMvpMatrix, 0, projectionMatrix, 0, mMvMatrix, 0); - } - - public float[] getModelMatrix() { - return mModelMatrix; - } - - public void setModelMatrix(float[] modelMatrix) { - mModelMatrix = modelMatrix; - } - - public float[] getMvMatrix() { - return mMvMatrix; - } - - public float[] getMvpMatrix() { - return mMvpMatrix; - } -} diff --git a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Trajectory.java b/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Trajectory.java deleted file mode 100644 index aa0dc01b..00000000 --- a/TangoUtils/app/src/main/java/com/projecttango/tangoutils/renderables/Trajectory.java +++ /dev/null @@ -1,180 +0,0 @@ -/* - * Copyright 2014 Google Inc. All Rights Reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package com.projecttango.tangoutils.renderables; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.nio.FloatBuffer; - -import android.opengl.GLES20; -import android.opengl.Matrix; -import android.util.Log; - -/** - * {@link Renderable} OpenGL object showing the Trajectory of the Project Tango - * device in 3D space. Points are added when the trajectory is updated by - * passing translation data obtained from Tango Pose Data. - */ -public class Trajectory extends Renderable { - - private static final int COORDS_PER_VERTEX = 3; - private static final float MIN_DISTANCE_CHECK = 0.025f; - - /** Note: due to resetPath() logic, keep this as a multiple of 9 **/ - private static final int MAX_VERTICES = 9000; - private static final int BYTES_PER_FLOAT = 4; - private static int mTrajectoryCount = 0; - - private static final String TAG = Trajectory.class.getSimpleName(); - private String mVertexShaderCode = "uniform mat4 uMVPMatrix;" - + "attribute vec4 vPosition;" + "uniform vec4 aColor;" - + "varying vec4 vColor;" + "void main() {" + "gl_PointSize = 5.0;" - + "vColor=aColor;" + "gl_Position = uMVPMatrix * vPosition;" + "}"; - private String mFragmentShaderCode = "precision mediump float;" - + "varying vec4 vColor;" + "void main() {" - + "gl_FragColor = vColor;" + "}"; - private FloatBuffer mVertexBuffer; - private float[] mColor = { 0.22f, 0.28f, 0.67f, 1.0f }; - private final int mProgram; - private int mPosHandle; - private int mMVPMatrixHandle; - private int mColorHandle; - private int mLineWidth; - - public Trajectory(int lineWidth) { - mLineWidth = lineWidth; - // Reset the model matrix to the identity - Matrix.setIdentityM(getModelMatrix(), 0); - - // Allocate a vertex buffer - ByteBuffer vertexByteBuffer = ByteBuffer.allocateDirect(MAX_VERTICES - * BYTES_PER_FLOAT); - vertexByteBuffer.order(ByteOrder.nativeOrder()); - mVertexBuffer = vertexByteBuffer.asFloatBuffer(); - - // Load the vertex and fragment shaders, then link the program - int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER, - mVertexShaderCode); - int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER, - mFragmentShaderCode); - mProgram = GLES20.glCreateProgram(); - GLES20.glAttachShader(mProgram, vertexShader); - GLES20.glAttachShader(mProgram, fragShader); - GLES20.glLinkProgram(mProgram); - } - - // float[] color should contain only 4 elements. - public Trajectory(int lineWidth, float[] color) { - mLineWidth = lineWidth; - mColor = color; - // Reset the model matrix to the identity - Matrix.setIdentityM(getModelMatrix(), 0); - - // Allocate a vertex buffer - ByteBuffer vertexByteBuffer = ByteBuffer.allocateDirect(MAX_VERTICES - * BYTES_PER_FLOAT); - vertexByteBuffer.order(ByteOrder.nativeOrder()); - mVertexBuffer = vertexByteBuffer.asFloatBuffer(); - - // Load the vertex and fragment shaders, then link the program - int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER, - mVertexShaderCode); - int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER, - mFragmentShaderCode); - mProgram = GLES20.glCreateProgram(); - GLES20.glAttachShader(mProgram, vertexShader); - GLES20.glAttachShader(mProgram, fragShader); - GLES20.glLinkProgram(mProgram); - } - - public void updateTrajectory(float[] translation) { - mVertexBuffer.position(mTrajectoryCount * 3); - if (((mTrajectoryCount + 1) * 3) >= MAX_VERTICES) { - Log.w(TAG, "Clearing float buffer"); - resetPath(); - } - float dx = 0, dy = 0, dz = 0; - try { - dx = mVertexBuffer.get(mVertexBuffer.position() - 3) - - translation[0]; - dy = mVertexBuffer.get(mVertexBuffer.position() - 2) - - translation[2]; - dz = mVertexBuffer.get(mVertexBuffer.position() - 1) - - (-translation[1]); - } catch (IndexOutOfBoundsException e) { - mVertexBuffer.put(new float[] { translation[0], translation[2], - -translation[1] }); - mTrajectoryCount++; - } - float distance = (float) Math.sqrt(dx * dx + dy * dy + dz * dz); - if (distance > MIN_DISTANCE_CHECK) { - mVertexBuffer.put(new float[] { translation[0], translation[2], - -translation[1] }); - mTrajectoryCount++; - } - } - - public void resetPath() { - int currentPosition = mVertexBuffer.position(); - int pointsToGet = (MAX_VERTICES / 3); - mVertexBuffer.position(currentPosition - pointsToGet); - - float[] tail = new float[pointsToGet]; - mVertexBuffer.get(tail, 0, pointsToGet); - - mVertexBuffer.clear(); - mVertexBuffer.put(tail); - - mTrajectoryCount = pointsToGet / 3; - } - - public void clearPath() { - ByteBuffer vertexByteBuffer = ByteBuffer.allocateDirect(MAX_VERTICES - * BYTES_PER_FLOAT); - vertexByteBuffer.order(ByteOrder.nativeOrder()); - mVertexBuffer = vertexByteBuffer.asFloatBuffer(); - } - - @Override - public void draw(float[] viewMatrix, float[] projectionMatrix) { - GLES20.glUseProgram(mProgram); - mVertexBuffer.position(0); - - // Compose the model, view, and projection matrices into a single m-v-p - // matrix - updateMvpMatrix(viewMatrix, projectionMatrix); - - // Load vertex attribute data - mPosHandle = GLES20.glGetAttribLocation(mProgram, "vPosition"); - GLES20.glVertexAttribPointer(mPosHandle, COORDS_PER_VERTEX, - GLES20.GL_FLOAT, false, 0, mVertexBuffer); - GLES20.glEnableVertexAttribArray(mPosHandle); - - mMVPMatrixHandle = GLES20.glGetUniformLocation(mProgram, "uMVPMatrix"); - GLES20.glUniformMatrix4fv(mMVPMatrixHandle, 1, false, getMvpMatrix(), 0); - - mColorHandle = GLES20.glGetUniformLocation(mProgram, "aColor"); - GLES20.glUniform4f(mColorHandle, mColor[0], mColor[1], mColor[2], - mColor[3]); - GLES20.glLineWidth(mLineWidth); - GLES20.glDrawArrays(GLES20.GL_LINE_STRIP, 0, mTrajectoryCount); - } - - public void setColor(float[] color) { - mColor = color; - } -} \ No newline at end of file