public double getDerivative(int order, double x) { setXPowers(xPowers, x); dPos = 0.0; int derivativeCoefficient = 0; for (int i = order; i < numberOfCoefficients.getIntegerValue(); i++) { derivativeCoefficient = getDerivativeCoefficient(order, i); dPos += derivativeCoefficient * a[i].getDoubleValue() * xPowers[i - order]; } return dPos; }
int order3Exponent1Func = septic.getDerivativeCoefficient(3, 1); int order3Exponent1Hand = 0; assertEquals(order3Exponent1Func, order3Exponent1Hand, EPSILON); int order6Exponent7Func = septic.getDerivativeCoefficient(6, 7); int order6Exponent7Hand = 5040; assertEquals(order6Exponent7Func, order6Exponent7Hand, EPSILON); int order0Exponent5Func = septic.getDerivativeCoefficient(0, 5); int order0Exponent5Hand = 1; assertEquals(order0Exponent5Func, order0Exponent5Hand, EPSILON); int order3Exponent4Func = septic.getDerivativeCoefficient(3, 4); int order3Exponent4Hand = 24; assertEquals(order3Exponent4Func, order3Exponent4Hand, EPSILON); int order5Exponent2Func = septic.getDerivativeCoefficient(5, 2); int order5Exponent2Hand = 0; assertEquals(order5Exponent2Func, order5Exponent2Hand, EPSILON); int order1Exponent5Func = septic.getDerivativeCoefficient(1, 5); int order1Exponent5Hand = 5; assertEquals(order1Exponent5Func, order1Exponent5Hand, EPSILON); int order11Exponent1Func = septic.getDerivativeCoefficient(11, 1); int order11Exponent1Hand = 0; assertEquals(order11Exponent1Func, order11Exponent1Hand, EPSILON); int order13Exponent8Func = septic.getDerivativeCoefficient(13, 8);
public void compareDerivativesPoint(YoPolynomial polynomial, double x) { double[] coefficients = polynomial.getCoefficients(); for(int i = 0; i < coefficients.length + 3; i++) { double generalizedDYPoly = polynomial.getDerivative(i, x); double generalizedDYHand = 0.0; if(i < coefficients.length) { for(int j = i; j < coefficients.length; j++) { double derivativeCoefficient = polynomial.getDerivativeCoefficient(i, j); generalizedDYHand += coefficients[j] * derivativeCoefficient * Math.pow(x, j-i); } } else { generalizedDYHand = 0.0; } assertEquals(generalizedDYPoly, generalizedDYHand, EPSILON); } }
/** * Returns the order-th derivative of the xPowers vector at value x (Note: does NOT return the YoPolynomials order-th derivative at x) * @param order * @param x * @return */ public DenseMatrix64F getXPowersDerivativeVector(int order, double x) { setXPowers(xPowers, x); xPowersDerivativeVector.zero(); int derivativeCoefficient = 0; for (int i = order; i < numberOfCoefficients.getIntegerValue(); i++) { derivativeCoefficient = getDerivativeCoefficient(order, i); xPowersDerivativeVector.set(i, derivativeCoefficient * xPowers[i - order]); } return xPowersDerivativeVector; }
public void compareXPowersDerivativesVector(YoPolynomial polynomial, double x) { double[] coefficients = polynomial.getCoefficients(); for(int i = 0; i < coefficients.length + 3; i++) { DenseMatrix64F generalizedDYPoly = polynomial.getXPowersDerivativeVector(i, x); DenseMatrix64F generalizedDYHand = new DenseMatrix64F(generalizedDYPoly.getNumRows(), generalizedDYPoly.getNumCols()); if(i < coefficients.length) { for(int j = i; j < coefficients.length; j++) { double derivativeCoefficient = polynomial.getDerivativeCoefficient(i, j); generalizedDYHand.set(j, 0, derivativeCoefficient * Math.pow(x, j - i)); } } for(int k = 0; k < coefficients.length; k++) { assertEquals(generalizedDYPoly.get(k, 0), generalizedDYHand.get(k, 0), EPSILON); } } }
public void getDerivative(YoTrajectory dervTraj, int order) { if (dervTraj.getMaximumNumberOfCoefficients() < this.getNumberOfCoefficients() - order) return; dervTraj.polynomial.reshape(getNumberOfCoefficients() - order); for (int i = order; i < this.getNumberOfCoefficients(); i++) { dervTraj.polynomial.setDirectlyFast(i - order, this.polynomial.getDerivativeCoefficient(order, i) * this.polynomial.getCoefficient(i)); } dervTraj.setInitialTime(this.getInitialTime()); dervTraj.setFinalTime(this.getFinalTime()); }