public static Trajectory combineTrajectory ( Trajectory a , Trajectory b ) { if ( a . getDimension ( ) != b . getDimension ( ) ) { throw new IllegalArgumentException ( "Combination not possible: The trajectorys does not have the same dimension" ) ; } if ( a . size ( ) != b . size ( ) ) { throw new IllegalArgumentException ( "Combination not possible: The trajectorys does not " + "have the same number of steps a=" + a . size ( ) + " b=" + b . size ( ) ) ; } Trajectory c = new Trajectory ( a . getDimension ( ) ) ; for ( int i = 0 ; i < a . size ( ) ; i ++ ) { Point3d pos = new Point3d ( a . get ( i ) . x + b . get ( i ) . x , a . get ( i ) . y + b . get ( i ) . y , a . get ( i ) . z + b . get ( i ) . z ) ; c . add ( pos ) ; } return c ; }
Combines two trajectories by adding the corresponding positions . The trajectories have to have the same length .