예제 #1
0
        /**
         * @param p0  The starting pose of the spline
         * @param p1  The ending pose of the spline
         */
        public QuinticSpline(Pose2d p0, Pose2d p1)
        {
            double scale = 1.2 * p0.getTranslation().distance(p1.getTranslation());

            x0   = p0.getTranslation().x();
            x1   = p1.getTranslation().x();
            dx0  = p0.getRotation().cos() * scale;
            dx1  = p1.getRotation().cos() * scale;
            ddx0 = 0;
            ddx1 = 0;
            y0   = p0.getTranslation().y();
            y1   = p1.getTranslation().y();
            dy0  = p0.getRotation().sin() * scale;
            dy1  = p1.getRotation().sin() * scale;
            ddy0 = 0;
            ddy1 = 0;

            computeCoefficients();
        }