Fixing bugs in new DefaultProbeController

- Had to deal with rotations properly. ProbeController now defines movements in xyz World space and then rotates them into the Probe's TransformedSpace when applying
- Moved Bregma relative offset into the CCFSpace class instead of the CoordinateTransform
- Added World2World function which in theory allows for mesh modification

Author: dbirman

Assets/Scenes/TrajectoryPlanner.unity

@@ -30,6 +30,9 @@ public class ProbeInsertion
     public float theta;
     public float spin;
 
+    /// <summary>
+    /// The **transformed** coordinate in the active CoordinateSpace
+    /// </summary>
     public Vector3 apmldv
     {
         get => new Vector3(ap, ml, dv);
@@ -79,17 +82,38 @@ public ProbeInsertion(Vector3 tipPosition, Vector3 angles,
 
     #endregion
 
-    public Vector3 GetPositionSpace()
+    /// <summary>
+    /// Get the corresponding **un-transformed** coordinate in the CoordinateSpace
+    /// </summary>
+    /// <returns></returns>
+    public Vector3 PositionSpace()
     {
         return _coordinateTransform.Transform2Space(apmldv);
     }
 
-    public Vector3 GetPositionWorld()
+    /// <summary>
+    /// Get the corresponding **transformed** coordinate in World
+    /// </summary>
+    /// <returns></returns>
+    public Vector3 PositionWorld()
     {
-        return _coordinateSpace.Space2World(GetPositionSpace());
+        return _coordinateSpace.Space2World(_coordinateTransform.Transform2SpaceRot(apmldv));
     }
 
-    // Convenience function for transforming world to this insertion's transformed space (i.e. world -> space -> transformed)
+    /// <summary>
+    /// Get the corresponding **un-transformed** coordinate in World
+    /// </summary>
+    /// <returns></returns>
+    public Vector3 GetPositionWorldUnTransformed()
+    {
+        return _coordinateSpace.Space2World(PositionSpace());
+    }
+
+    /// <summary>
+    /// Convert a world coordinate into the ProbeInsertion's transformed space
+    /// </summary>
+    /// <param name="coordWorld"></param>
+    /// <returns></returns>
     public Vector3 World2Transformed(Vector3 coordWorld)
     {
         return _coordinateTransform.Space2Transform(_coordinateSpace.World2Space(coordWorld));
@@ -100,6 +124,22 @@ public Vector3 World2TransformedRot(Vector3 coordWorld)
         return _coordinateTransform.Space2TransformRot(_coordinateSpace.World2SpaceRot(coordWorld));
     }
 
+    /// <summary>
+    /// Convert a world coordinate into the corresponding world coordinate after transformation
+    /// </summary>
+    /// <param name="coordWorld"></param>
+    /// <returns></returns>
+    public Vector3 World2World(Vector3 coordWorld)
+    {
+        //_coordinateSpace.Space2World(_coordinateTransform.Transform2SpaceRot(
+        Debug.Log(coordWorld);
+        Debug.Log(_coordinateSpace.World2Space(coordWorld));
+        Debug.Log(_coordinateTransform.Space2Transform(_coordinateSpace.World2Space(coordWorld)));
+        Debug.Log(_coordinateTransform.Transform2SpaceRot(_coordinateTransform.Space2Transform(_coordinateSpace.World2Space(coordWorld))));
+        Debug.Log(_coordinateSpace.Space2World(_coordinateTransform.Transform2SpaceRot(_coordinateTransform.Space2Transform(_coordinateSpace.World2Space(coordWorld)))));
+        return _coordinateSpace.Space2World(_coordinateTransform.Transform2SpaceRot(_coordinateTransform.Space2Transform(_coordinateSpace.World2Space(coordWorld))));
+    }
+
     public Vector3 Transformed2World(Vector3 coordTransformed)
     {
         return _coordinateSpace.Space2World(_coordinateTransform.Transform2Space(coordTransformed));