/**
* Evalue PY et QY en value et remplit les polynomes en Y P et Q
*/
void eval_biv(mpz_t value, mpz_t **PY, mpz_t **QY, int *degres_PY, int *degres_QY, mpz_t *P, mpz_t *Q, int deg_P, int deg_Q, mpz_t mod ){
int i;
for (i=0; i<deg_P+1; i++){
horner(P[i], PY[i], degres_PY[i], value, mod);
}
for(i=0; i<deg_Q+1; i++ ){
horner(Q[i], QY[i], degres_QY[i], value, mod);
}
}
double Bernsteins::secant(Bezier bz) {
double s = 0, t = 1;
double e = 1e-14;
int side = 0;
double r, fr, fs = bz.at0(), ft = bz.at1();
for (size_t n = 0; n < 100; ++n)
{
r = (fs*t - ft*s) / (fs - ft);
if (fabs(t-s) < e * fabs(t+s)) {
debug(std::cout << "error small " << fabs(t-s)
<< ", accepting solution " << r
<< "after " << n << "iterations\n");
return r;
}
fr = horner(bz, r);
if (fr * ft > 0)
{
t = r; ft = fr;
if (side == -1) fs /= 2;
side = -1;
}
else if (fs * fr > 0)
{
s = r; fs = fr;
if (side == +1) ft /= 2;
side = +1;
}
else break;
}
return r;
}
int main(void)
{
int i, m;
float a, b, x, aval1, aval2, h;
float coef[8];
FILE *entrada;
entrada = fopen("inputs\\coeficients.dad", "r");
if ( entrada == NULL )
{
printf("Error en obrir el fitxer inputs\\coeficients.dad\n");
exit(1);
}
for ( i = 0 ; i < 8 ; i++ )
{
fscanf(entrada, "%f", &coef[i]);
}
fclose(entrada);
/*/ printf("Indiqueu a, b, m = \n"); */
scanf("%f %f %d", &a, &b, &m);
h = ( b - a ) / m;
printf("%8s %17s %17s %17s\n", "x", "poli", "horner", "diferencia");
for ( i = 0 ; i < m ; i++ )
{
x = a + ( i * h );
aval1 = poli(x, coef);
aval2 = horner(x, coef);
printf("%15.6e %15.6e %15.6e %15.6e\n", x, aval1, aval2, fabs(aval1 - aval2));
}
return 0;
}
int main(int argc, char **argv) {
if (argc < 3) {
usage(argv[0]);
exit(EXIT_FAILURE);
}
int x, a[argc-2];
try {
if (!is_integer(argv[1]))
throw "Value must be integer!";
else x = atoi(argv[1]);
for (int i=2; i<argc; i++)
if (!is_integer(argv[i]))
throw "Arguments must be integers!";
else a[i-2] = atoi(argv[i]);
} catch (const char *e) {
usage(argv[0]);
std::cerr << e << std::endl;
}
print(a, argc-2, x);
std::cout << " = ";
std::cout << horner(a, argc-2, x) << std::endl;
}
void roots ( int n, dcmplx p[n], double eps, int maxit, dcmplx z[n-1] )
{
dcmplx pp[n],dz,dp;
double mpz,mdz,maxpz,maxdz;
int i,k;
for(i=0; i<n-1; i++)
{
z[i] = random_dcmplx1();
pp[i] = div_dcmplx(p[i],p[n-1]);
}
pp[n-1] = create1(1.0);
for(k=0; k<maxit; k++)
{
maxpz = 0.0;
maxdz = 0.0;
for(i=0; i<n-1; i++)
{
dz = horner(n,pp,z[i]);
/* printf("residual at z[%d] : ", i);
writeln_dcmplx(dz); */
mpz = modulus(dz);
if (mpz > maxpz) maxpz = mpz;
dp = difference_product(n-1,i,z);
dz = div_dcmplx(dz,dp);
mdz = modulus(dz);
if (mdz > maxdz) maxdz = mdz;
/* printf(" update to z[%d] : ", i);
writeln_dcmplx(dz); */
z[i] = sub_dcmplx(z[i],dz);
}
if ((maxpz <= eps) || (maxdz <= eps)) break;
}
}
int Newton ( int n, dcmplx p[n], dcmplx dp[n-1], dcmplx *z,
double eps, int maxit )
{
int i;
dcmplx delta,y,dy;
for(i=0; i<maxit; i++)
{
y = horner(n,p,*z);
dy = horner(n-1,dp,*z);
delta = div_dcmplx(y,dy);
*z = sub_dcmplx(*z,delta);
if(dcabs(delta) <= eps) return i;
}
return -1;
}
KFR_SINTRIN T cubic(M mu, T x0, T x1, T x2, T x3)
{
const T a0 = x3 - x2 - x0 + x1;
const T a1 = x0 - x1 - a0;
const T a2 = x2 - x0;
const T a3 = x1;
return horner(mu, a0, a1, a2, a3);
}
KFR_SINTRIN T catmullrom(M mu, T x0, T x1, T x2, T x3)
{
const T a0 = T(0.5) * (x3 - x0) - T(1.5) * (x2 - x1);
const T a1 = x0 - T(2.5) * x1 + T(2) * x2 - T(0.5) * x3;
const T a2 = T(0.5) * (x2 - x0);
const T a3 = x1;
return horner(mu, a0, a1, a2, a3);
}
inline double latToYapprox(const FloatLatitude latitude)
{
if (latitude < FloatLatitude(-70.) || latitude > FloatLatitude(70.))
return latToY(latitude);
// Approximate the inverse Gudermannian function with the Padé approximant [11/11]: deg → deg
// Coefficients are computed for the argument range [-70°,70°] by Remez algorithm |err|_∞=3.387e-12
const auto x = static_cast<double>(latitude);
return
horner(x, 0.00000000000000000000000000e+00, 1.00000000000089108431373566e+00, 2.34439410386997223035693483e-06,
-3.21291701673364717170998957e-04, -6.62778508496089940141103135e-10, 3.68188055470304769936079078e-08,
6.31192702320492485752941578e-14, -1.77274453235716299127325443e-12, -2.24563810831776747318521450e-18,
3.13524754818073129982475171e-17, 2.09014225025314211415458228e-23, -9.82938075991732185095509716e-23) /
horner(x, 1.00000000000000000000000000e+00, 2.34439410398970701719081061e-06, -3.72061271627251952928813333e-04,
-7.81802389685429267252612620e-10, 5.18418724186576447072888605e-08, 9.37468561198098681003717477e-14,
-3.30833288607921773936702558e-12, -4.78446279888774903983338274e-18, 9.32999229169156878168234191e-17,
9.17695141954265959600965170e-23, -8.72130728982012387640166055e-22, -3.23083224835967391884404730e-28);
}
double ln(double x)//ln(1+x)=x-x^2/2+x^3/3-x^4/4……
{
int i;
if (x>1.5)
{
for (i = 0; x>1.25; i++)
x = sqrt(x);
return (1 << i)*horner(x - 1);
}
else if (x<0.7&&x>0)
{
for (i = 0; x<0.7; i++)
x = sqrt(x);
return (1 << i)*horner(x - 1);
}
else if (x>0)
return horner(x - 1);
}
void manual_test ( int n )
{
int i, j, m[n+1], k;
dcmplx x[n+1], f[n+1], ff[n+1], tmp;
double tol=1.0e-8;
printf("multiple points test:\n");
printf("x[i] are multiple points,please give the f value like this:\n");
printf("x[i], function value at the x[i],\n");
printf("x[i+1], the first order derivative function value at x[i],\n");
printf("x[i+2], the second order derivative function value at x[i].\n");
for(i=0; i<=n; i++ )
{
printf("input x[%d]:\n", i);
read_dcmplx(&x[i]);
printf("input the multiplicity of the point\n");
scanf("%d", &(m[i]));
printf("input f[%d]:\n", i);
read_dcmplx(&f[i]);
}
group_points(n+1, tol, x, f, m);
for(i=0; i<=n; i++)
ff[i]=f[i];
divided_difference ( n+1, x, f, m );
printf("the result polynomial is:\n");
for(j=0; j<=n; j++)
{
printf("f[%d]=", j);
writeln_dcmplx(f[j]);
}
for(i=0; i<=n; i++)
{
printf("the point x with multiplicity %d is: ", m[i]);
writeln_dcmplx(x[i]);
printf("the given function value at point x is:");
writeln_dcmplx(ff[i]);
printf("the result polynomial value at point x is:");
tmp=horner(n+1, f, x[i]);
writeln_dcmplx(tmp);
printf("\n");
if(m[i]>1) /*skip the multiple points*/
{
k=m[i];
while(k>1)
{
i++;
k--;
}
}
}
}
//---------------- Evaluate nth order of trajectory at t ------------------
int PolyTraj::eval_trajectory (const double& t, std::vector<double>& eval) const {
//printf("t = %f, te = %f\n", t, te);
if ( t < te ) {
//return eval_poly(derivative, t, eval);
return horner(coeff_, t, eval);
}
else {
//t -= te;
// extrapolate with constant acceleration
std::vector<double> third_order_coeff(6);
third_order_coeff[0] = end_state.x;
third_order_coeff[1] = end_state.x_der;
third_order_coeff[2] = end_state.x_dder/2.0;
third_order_coeff[3] = 0.0;
third_order_coeff[4] = 0.0;
third_order_coeff[5] = 0.0;
return horner(third_order_coeff, (t-te), eval);
}
}
int PolyTraj::calc_end_state() {
// Memorize end state for faster extrapolation
// printf("te = %f",te);
std::vector<double> eval(6);
horner(coeff_, te, eval);
end_state.x = eval[0];
end_state.x_der = eval[1];
end_state.x_dder = eval[2];
return 0;
}
void evaluate_matrix( int n, int m, POLY a[n][m], dcmplx b[n][m], dcmplx x)
{
int i, j;
for(i=0; i<n; i++)
for(j=0; j<m; j++)
{
if(a[i][j].d>=1)
b[i][j] = horner(a[i][j].d+1, a[i][j].p, x);
else
b[i][j] = a[i][j].p[0];
}
}
static vektor& horner(matrix const& A, vektor const& v,
polynom const& p,unsigned int pos, vektor& res)
{
if( pos == p.size()-2 ){ //pos ist auf der vorletzten Stelle im Vektor
res = p[pos+1]*(A*v) + p[pos]*v;
return res;
}
res = p[pos]*v + A*horner(A,v,p,pos+1,res);
return res;
}
void search_roots(int *nb_racines, mpz_t *racines, mpz_t *P, mpz_t *Q, int deg_P, int deg_Q, mpz_t mod){
mpz_t resP_horner, resQ_horner, i;
mpz_inits(resP_horner, resQ_horner, i, NULL);
*nb_racines=0;
while (mpz_cmp(mod, i)){
horner(resP_horner, P, deg_P, i, mod);
horner(resQ_horner, Q, deg_Q, i, mod);
if (!mpz_cmp_si(resP_horner, 0)){
if(!mpz_cmp_si(resQ_horner, 0)){
printf("%ld est racine modulo %ld\n", mpz_get_si(i), mpz_get_si(mod));
/* si i annule les 2 polynomes, on a une solution a l equation */
mpz_set(racines[*nb_racines], i);
*nb_racines++;
}
}
mpz_add_ui(i, i, 1);
}
}
vektor& mult(polynom const& p, matrix const& A, vektor const& v,
vektor& ret)
{
if( deg(p)<1 ) {
// Wenn das Nullpolynom vorliegt, wird der Nullvektor zurückgegeben.
if( p.size() == 0 ) {
ret = vektor( v.size(),0);
return ret;
}
ret = p[0]*v;
return ret;
}
ret = horner(A,v,p,0,ret);
return ret;
}
void random_test ( int n )
{
dcmplx p[n+1], x[n+1], f[n+1];
int i, m[i+1];
srand(time(NULL));
/* printf(" The original polinomial is: \n");*/
for(i=0; i<n; i++)
{
p[i] = random_dcmplx1();
/* writeln_dcmplx(p[i]); */
}
p[n]=create1(1.0);
/*writeln_dcmplx(p[n]); */
for(i=0; i<=n; i++)
{
x[i] = random_dcmplx1();
m[i] = 1;
f[i] = horner(n+1, p, x[i]);
}
divided_difference ( n+1, x, f, m );
/* printf(" The polynomial after interpolation is: \n");
for(i=0; i<=n; i++)
{
writeln_dcmplx(f[i]);
}
*/
for(i=0; i<=n; i++)
{
if( !equal(p[i], f[i]) )
{
printf("i=%d\n", i);
printf("The original polynomial item is:");
writeln_dcmplx(p[i]);
printf("The polynomial after interpolation is:");
writeln_dcmplx(f[i]);
printf(" BUG!!\n ");
exit(0);
}
}
printf(" OK!\n ");
}
/* nb_racines sera rempli du nombre de racines de P */
void racines(mpz_t *rac, mpz_t *P, int deg_P, int *nb_racines, mpz_t mod){
/* k indice de remplissage de rac */
long int i, k=0;
mpz_t res_horner, m, m_plus;
/* m_plus= m+1 */
mpz_inits(res_horner, m, NULL);
mpz_init_set_str(m_plus, "1", 10);
/* on reduit P modulo mod */
for (i=0; i<deg_P+1; i++){
mpz_mod(P[i], P[i], mod);
}
while( mpz_cmp(mod, m_plus) ){
/* printf("mod=%ld, m=%ld\n", mpz_get_si(mod),mpz_get_si(m)); */
/* on teste la valeur m */
horner(res_horner, P, deg_P, m, mod);
/* si on a une racine, on l ajoute a rac */
if (!mpz_cmp_si(res_horner, 0)){
mpz_set(rac[k], m);
/* printf("-----------K=%ld---------------------\n", k); */
k++;
}
/* printf("apres if : k=%ld\n", k); */
/* on ne peut pas avoir plus de racines que le degre du polynome +1 */
if (k >=deg_P+1){
printf("deg_P+1 atteint : k=%ld, deg_P+1=%d\n", k, deg_P+1);
*nb_racines=k;
mpz_clear(res_horner);
mpz_clear(m);
mpz_clear(m_plus);
return;
}
mpz_add_ui(m, m, 1);
mpz_add_ui(m_plus, m_plus, 1);
/* printf("fin while :mod=%ld, m=%ld\n", mpz_get_si(mod),mpz_get_si(m)); */
}
/* printf("sortie:mod=%ld, m=%ld\n", mpz_get_si(mod),mpz_get_si(m)); */
*nb_racines=k;
if(*nb_racines == 0){
printf("racine : pas de racines modulo %ld, verifier la borne\n", mpz_get_si(mod));
}
mpz_clear(res_horner);
mpz_clear(m);
mpz_clear(m_plus);
return;
}
constexpr CMT_INLINE common_type<T1, T2, T3, Ts...> horner(const T1& x, const T2& c0, const T3& c1,
const Ts&... values)
{
return fmadd(horner(x, c1, values...), x, c0);
}
constexpr CMT_INLINE common_type<T1, T2, T3, Ts...> horner_even(const T1& x, const T2& c0, const T3& c2,
const Ts&... values)
{
const T1 x2 = x * x;
return fmadd(horner(x2, c2, values...), x2, c0);
}
constexpr CMT_INLINE common_type<T1, T2, T3, Ts...> horner_odd(const T1& x, const T2& c1, const T3& c3,
const Ts&... values)
{
const T1 x2 = x * x;
return fmadd(horner(x2, c3, values...), x2, c1) * x;
}
}
free(table);
table = NULL;
}
data *create_data(char *key, double value){
data *out = malloc(sizeof(data));
out->key = key;
out->value = value;
return out;
}
void input(data **table, char *key, double value){
data *new = create_data(key,value);
int place = horner(key)%BINS;
push(table[place], new);
table_count[place]++;
}
void print(data **table){
int i;
for(i=0;i<BINS;i++){
if(table_count[i] > 0){
printf("table[%d]:\n",i);
int j;
data *temp = table[i];
for(j=1;j<table_count[i]+1;j++){
temp = temp->ptr;
printf("key: \"%s\" value: %f\n",temp->key,temp->value);
}
constexpr double horner(double x, T an, U ...a) { return horner(x, a...) * x + an; }
int PolyTraj::calc_min_max_acceleration_of_1D_traj() {
double a0, ae, ag;
double tg, t_min, t_max;
std::vector<double> eval(6);
// calculate limit values
//a0 = eval_poly(2, 0.0);
//ae = eval_poly(2, te);
horner(coeff_, 0.0, eval);
a0 = eval[2];
horner(coeff_, te, eval);
ae = eval[2];
if ( a0 < ae ) { // compare limit values
a_min = a0;
a_max = ae;
t_min = 0;
t_max = te;
}
else {
a_min = ae;
a_max = a0;
t_min = te;
t_max = 0;
}
// Check for extrema between limits
if ( coeff_[5] == 0 ) {
if ( coeff_[4] == 0 ) { // polynomial 3rd order -> no global extremum
;// -> do nothing, use limit values
}
else { // polynomial 4th order -> only one global extremum
tg = -0.25 * coeff_[3] / coeff_[4];
horner(coeff_, tg, eval);
ag = eval[2];
//ag = eval_poly(2, tg);
if ( ( tg > 0 ) && (tg < te ) // if global extr between limits
&& ( ag > a_max ) ) { // and bigger than limit value
a_max = ag; // override limit values
t_max = tg;
}
else if ( ( tg > 0 ) && (tg < te ) // if global extr between limits
&& ( ag < a_min ) ) { // and smaller than limit value
a_min = ag; // override limit values
t_min = tg;
}
}
}
else { // polynomial 5th order
double discr;
discr = coeff_[4]*coeff_[4] - 2.5 * coeff_[3] * coeff_[5];
if ( discr < 0 ) {
;// -> do nothing, use limits
}
else {
double tg1,tg2;
tg1 = ( -coeff_[4] + sqrt(discr) ) / ( 5 * coeff_[5] );
tg2 = ( -coeff_[4] - sqrt(discr) ) / ( 5 * coeff_[5] );
//double ag1 = eval_poly(2, tg1);
horner(coeff_, tg1, eval);
double ag1 = eval[2];
//double ag2 = eval_poly(2, tg2);
horner(coeff_, tg2, eval);
double ag2 = eval[2];
double ag_max, ag_min;
double tg_max, tg_min;
if ( ag1 > ag2 ) { // determin global max and min
ag_max = ag1;
tg_max = tg1;
ag_min = ag2;
tg_min = tg2;
}
else {
ag_max = ag2;
tg_max = tg2;
ag_min = ag1;
tg_min = tg1;
}
if ( ( tg_max > 0 ) && (tg_max < te ) // if global tmax between limits
&& ( ag_max > a_max ) ) { // and bigger than limit max
a_max = ag_max; // override limit values
t_max = tg_max;
}
if ( ( tg_min > 0 ) && (tg_min < te ) // if global tmin between limits
&& ( ag_min < a_min ) ) { // and smaller than limit min
a_min = ag_min; // override limit values
t_min = tg_min;
}
}
}
return 0;
}
