Motion: work in progress 2

diff --git a/src/motion/spline.cc b/src/motion/spline.cc
index ac117f16f6330f0be95fbce7c9e92edf7a1e45ff..bd92ed8d5886a2c15973772c6286586ee9eafa11 100644 (file)
--- a/src/motion/spline.cc
+++ b/src/motion/spline.cc
@@ -354,7 +354,7 @@ namespace Segment {
     TrajectorySegment* Spline::splitAtByTime(double time, Point *newEnd) {
         if (time <= t1 || time >= t2)
             return NULL;
-       printf("split %lf times t1 %lf tc %lf t2 %lf\n", time, t1, tc, t2);
+       printf("split %lf len %lf times t1 %lf tc %lf t2 %lf\n", time, length, t1, tc, t2);
         //getPointAt(getDistance(time), newEnd);
        double dst = getDistance(time);
 
@@ -368,6 +368,8 @@ namespace Segment {
        } else {
                ns->v1 = v2 = vc + acc2 * time;
        }
+       printf("splitA %lf len %lf speeds v1 %lf v2 %lf\n", time, length,  v1, v2);
+       printf("splitB %lf len %lf speeds v1 %lf v2 %lf\n", time, ns->length, ns->v1, ns->v2);
 
         return ns;
     }

diff --git a/src/motion/trajectory.cc b/src/motion/trajectory.cc
index d7e223ce07695c441ea2b72ce2f9a30dc62483e6..88ee7242cac4c453377348a0a43735a05e82bcaa 100644 (file)
--- a/src/motion/trajectory.cc
+++ b/src/motion/trajectory.cc
@@ -801,7 +801,8 @@ bool Trajectory::appendTrajectory(Trajectory &traj, double time)
         }
 
         // add new segments to the old trajectory
-        merge(traj);
+        //merge(traj);
+       splice(end(), traj);
 
         // add way points (why?)
         // final point