From 6b9579e9eb49e9d3185cd3129f6efac57d1afd06 Mon Sep 17 00:00:00 2001
From: felipetrentin <felipetrentintecnico@hotmail.com>
Date: Sat, 7 May 2022 11:15:46 -0300
Subject: [PATCH] solved setPose for real now

---
 lib/src/main/cpp/t265wrapper.hpp                  |  2 +-
 .../java/com/spartronics4915/lib/T265Camera.java  | 15 +++++++++------
 2 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/lib/src/main/cpp/t265wrapper.hpp b/lib/src/main/cpp/t265wrapper.hpp
index aed552c..87d7c4f 100644
--- a/lib/src/main/cpp/t265wrapper.hpp
+++ b/lib/src/main/cpp/t265wrapper.hpp
@@ -15,7 +15,7 @@
 // <https://www.gnu.org/licenses/>.
 
 #include <jni.h>
-#include <librealsense2/rs.hpp>
+#include "librealsense2/rs.hpp"
 #include <mutex>
 /* Header for class com_spartronics4915_lib_T265Camera */
 
diff --git a/lib/src/main/java/com/spartronics4915/lib/T265Camera.java b/lib/src/main/java/com/spartronics4915/lib/T265Camera.java
index 858af6c..baaffa3 100644
--- a/lib/src/main/java/com/spartronics4915/lib/T265Camera.java
+++ b/lib/src/main/java/com/spartronics4915/lib/T265Camera.java
@@ -334,11 +334,15 @@ public void sendOdometry(double velocityXMetersPerSecond, double velocityYMeters
      */
     public synchronized void setPose(Pose2d newPose) {
         synchronized (mUpdateMutex) {
+            Pose2d campose = (mLastRecievedCameraUpdate == null
+                    ? new Pose2d()
+                    : mLastRecievedCameraUpdate);
+
+            //camera offset, relative to arena
             mOriginOffset =
-                    newPose.relativeTo(
-                            mLastRecievedCameraUpdate == null
-                                    ? new Pose2d()
-                                    : mLastRecievedCameraUpdate);
+                    new Pose2d(
+                            newPose.getTranslation().minus(campose.getTranslation().rotateBy(campose.getRotation().times(-1).plus(newPose.getRotation()))),
+                            newPose.getRotation().minus(campose.getRotation()));
         }
     }
 
@@ -416,8 +420,7 @@ private synchronized void consumePoseUpdate(
         }
 
         final Pose2d transformedPose =
-                mOriginOffset.transformBy(
-                        new Transform2d(currentPose.getTranslation(), currentPose.getRotation()));
+                mOriginOffset.transformBy(new Transform2d(currentPose.getTranslation(), currentPose.getRotation()));
 
         mPoseConsumer.accept(
                 new CameraUpdate(transformedPose, new ChassisSpeeds(dx, dy, dtheta), confidence));