1 #ifndef FILE_AUTODIFFDIFF
2 #define FILE_AUTODIFFDIFF
22 template <
int D,
typename SCAL =
double>
40 for (
int i = 0; i < D; i++)
41 dval[i] = ad2.dval[i];
42 for (
int i = 0; i < D*D; i++)
43 ddval[i] = ad2.ddval[i];
50 for (
int i = 0; i < D; i++)
52 for (
int i = 0; i < D*D; i++)
60 for (
int i = 0; i < D; i++)
61 dval[i] = ad2.DValue(i);
62 for (
int i = 0; i < D*D; i++)
70 for (
int i = 0; i < D; i++)
72 for (
int i = 0; i < D*D; i++)
81 for (
int i = 0; i < D*D; i++)
85 INLINE
AutoDiffDiff (SCAL aval,
const SCAL * grad,
const SCAL * hesse)
96 for (
int i = 0; i < D; i++)
98 for (
int i = 0; i < D*D; i++)
103 INLINE
void StoreGradient (SCAL * p)
const
105 for (
int i = 0; i < D; i++)
109 INLINE
void LoadGradient (
const SCAL * p)
111 for (
int i = 0; i < D; i++)
115 INLINE
void StoreHessian (SCAL * p)
const
117 for (
int i = 0; i < D*D; i++)
121 INLINE
void LoadHessian (
const SCAL * p)
123 for (
int i = 0; i < D*D; i++)
128 SCAL
Value()
const throw() {
return val; }
131 SCAL
DValue (
int i)
const throw() {
return dval[i]; }
136 for (
int j = 0; j < D; j++)
137 r.DValue(j) = ddval[i*D+j];
142 SCAL
DDValue (
int i)
const throw() {
return ddval[i]; }
145 SCAL
DDValue (
int i,
int j)
const throw() {
return ddval[i*D+j]; }
148 SCAL &
Value() throw() {
return val; }
151 SCAL &
DValue (
int i)
throw() {
return dval[i]; }
154 SCAL &
DDValue (
int i)
throw() {
return ddval[i]; }
157 SCAL &
DDValue (
int i,
int j)
throw() {
return ddval[i*D+j]; }
166 for (
int i = 0; i < D; i++)
167 dval[i] += y.dval[i];
168 for (
int i = 0; i < D*D; i++)
169 ddval[i] += y.ddval[i];
177 for (
int i = 0; i < D; i++)
178 dval[i] -= y.dval[i];
179 for (
int i = 0; i < D*D; i++)
180 ddval[i] -= y.ddval[i];
187 for (
int i = 0; i < D*D; i++)
188 ddval[i] = val * y.ddval[i] + y.val * ddval[i];
190 for (
int i = 0; i < D; i++)
191 for (
int j = 0; j < D; j++)
192 ddval[i*D+j] += dval[i] * y.dval[j] + dval[j] * y.dval[i];
194 for (
int i = 0; i < D; i++)
197 dval[i] += val * y.dval[i];
206 for (
int i = 0; i < D*D; i++ )
208 for (
int i = 0; i < D; i++)
218 for (
int i = 0; i < D*D; i++ )
220 for (
int i = 0; i < D; i++)
255 template<
int D,
typename SCAL>
258 ost << x.
Value() <<
", D = ";
259 for (
int i = 0; i < D; i++)
260 ost << x.
DValue(i) <<
" ";
262 for (
int i = 0; i < D*D; i++)
268 template<
int D,
typename SCAL>
269 inline AutoDiffDiff<D, SCAL> operator+ (
const AutoDiffDiff<D, SCAL> & x,
const AutoDiffDiff<D, SCAL> & y)
throw()
271 AutoDiffDiff<D, SCAL> res;
272 res.Value () = x.Value()+y.Value();
273 for (
int i = 0; i < D; i++)
274 res.DValue(i) = x.DValue(i) + y.DValue(i);
275 for (
int i = 0; i < D*D; i++)
276 res.DDValue(i) = x.DDValue(i) + y.DDValue(i);
282 template<
int D,
typename SCAL>
283 inline AutoDiffDiff<D, SCAL> operator- (
const AutoDiffDiff<D, SCAL> & x,
const AutoDiffDiff<D, SCAL> & y)
throw()
285 AutoDiffDiff<D, SCAL> res;
286 res.Value() = x.Value()-y.Value();
287 for (
int i = 0; i < D; i++)
288 res.DValue(i) = x.DValue(i) - y.DValue(i);
289 for (
int i = 0; i < D*D; i++)
290 res.DDValue(i) = x.DDValue(i) - y.DDValue(i);
296 template<
int D,
typename SCAL,
typename SCAL2,
297 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
298 inline AutoDiffDiff<D, SCAL> operator+ (SCAL2 x,
const AutoDiffDiff<D, SCAL> & y)
throw()
300 AutoDiffDiff<D, SCAL> res;
301 res.Value() = x+y.Value();
302 for (
int i = 0; i < D; i++)
303 res.DValue(i) = y.DValue(i);
304 for (
int i = 0; i < D*D; i++)
305 res.DDValue(i) = y.DDValue(i);
310 template<
int D,
typename SCAL,
typename SCAL2,
311 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
312 inline AutoDiffDiff<D, SCAL> operator+ (
const AutoDiffDiff<D, SCAL> & y, SCAL2 x)
throw()
314 AutoDiffDiff<D, SCAL> res;
315 res.Value() = x+y.Value();
316 for (
int i = 0; i < D; i++)
317 res.DValue(i) = y.DValue(i);
318 for (
int i = 0; i < D*D; i++)
319 res.DDValue(i) = y.DDValue(i);
325 template<
int D,
typename SCAL>
326 inline AutoDiffDiff<D, SCAL> operator- (
const AutoDiffDiff<D, SCAL> & x)
throw()
328 AutoDiffDiff<D, SCAL> res;
329 res.Value() = -x.Value();
330 for (
int i = 0; i < D; i++)
331 res.DValue(i) = -x.DValue(i);
332 for (
int i = 0; i < D*D; i++)
333 res.DDValue(i) = -x.DDValue(i);
338 template<
int D,
typename SCAL,
typename SCAL2,
339 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
340 inline AutoDiffDiff<D, SCAL> operator- (
const AutoDiffDiff<D, SCAL> & x, SCAL2 y)
throw()
342 AutoDiffDiff<D, SCAL> res;
343 res.Value() = x.Value()-y;
344 for (
int i = 0; i < D; i++)
345 res.DValue(i) = x.DValue(i);
346 for (
int i = 0; i < D*D; i++)
347 res.DDValue(i) = x.DDValue(i);
352 template<
int D,
typename SCAL,
typename SCAL2,
353 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
354 inline AutoDiffDiff<D, SCAL> operator- (SCAL2 x,
const AutoDiffDiff<D, SCAL> & y)
throw()
356 AutoDiffDiff<D, SCAL> res;
357 res.Value() = x-y.Value();
358 for (
int i = 0; i < D; i++)
359 res.DValue(i) = -y.DValue(i);
360 for (
int i = 0; i < D*D; i++)
361 res.DDValue(i) = -y.DDValue(i);
367 template<
int D,
typename SCAL,
typename SCAL2,
368 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
369 inline AutoDiffDiff<D, SCAL> operator* (SCAL2 x,
const AutoDiffDiff<D, SCAL> & y)
throw()
371 AutoDiffDiff<D, SCAL> res;
372 res.Value() = x*y.Value();
373 for (
int i = 0; i < D; i++)
374 res.DValue(i) = x*y.DValue(i);
375 for (
int i = 0; i < D*D; i++)
376 res.DDValue(i) = x*y.DDValue(i);
381 template<
int D,
typename SCAL,
typename SCAL2,
382 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
383 inline AutoDiffDiff<D, SCAL> operator* (
const AutoDiffDiff<D, SCAL> & y, SCAL2 x)
throw()
385 AutoDiffDiff<D, SCAL> res;
386 res.Value() = x*y.Value();
387 for (
int i = 0; i < D; i++)
388 res.DValue(i) = x*y.DValue(i);
389 for (
int i = 0; i < D*D; i++)
390 res.DDValue(i) = x*y.DDValue(i);
395 template<
int D,
typename SCAL>
396 inline AutoDiffDiff<D, SCAL> operator* (
const AutoDiffDiff<D, SCAL> & x,
const AutoDiffDiff<D, SCAL> & y)
throw()
398 AutoDiffDiff<D, SCAL> res;
403 for (
int i = 0; i < D; i++)
404 res.DValue(i) = hx*y.DValue(i) + hy*x.DValue(i);
406 for (
int i = 0; i < D; i++)
407 for (
int j = 0; j < D; j++)
408 res.DDValue(i,j) = hx * y.DDValue(i,j) + hy * x.DDValue(i,j)
409 + x.DValue(i) * y.DValue(j) + x.DValue(j) * y.DValue(i);
416 template<
int D,
typename SCAL>
417 inline AutoDiffDiff<D, SCAL> Inv (
const AutoDiffDiff<D, SCAL> & x)
419 AutoDiffDiff<D, SCAL> res(1.0 / x.Value());
420 for (
int i = 0; i < D; i++)
421 res.DValue(i) = -x.DValue(i) / (x.Value() * x.Value());
423 SCAL fac1 = 2/(x.Value()*x.Value()*x.Value());
424 SCAL fac2 = 1/
sqr(x.Value());
425 for (
int i = 0; i < D; i++)
426 for (
int j = 0; j < D; j++)
427 res.DDValue(i,j) = fac1*x.DValue(i)*x.DValue(j) - fac2*x.DDValue(i,j);
432 template<
int D,
typename SCAL>
433 inline AutoDiffDiff<D, SCAL> operator/ (
const AutoDiffDiff<D, SCAL> & x,
const AutoDiffDiff<D, SCAL> & y)
438 template<
int D,
typename SCAL,
typename SCAL2,
439 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
440 inline AutoDiffDiff<D, SCAL> operator/ (
const AutoDiffDiff<D, SCAL> & x, SCAL2 y)
445 template<
int D,
typename SCAL,
typename SCAL2,
446 typename std::enable_if<std::is_convertible<SCAL2,SCAL>::value,
int>::type = 0>
447 inline AutoDiffDiff<D, SCAL> operator/ (SCAL2 x,
const AutoDiffDiff<D, SCAL> & y)
453 template<
int D,
typename SCAL>
454 inline AutoDiffDiff<D, SCAL> sqrt (
const AutoDiffDiff<D, SCAL> & x)
456 AutoDiffDiff<D, SCAL> res;
457 res.Value() = sqrt(x.Value());
458 for (
int j = 0; j < D; j++)
459 res.DValue(j) = IfZero(x.DValue(j),SCAL{0.},0.5 / res.Value() * x.DValue(j));
462 for (
int i = 0; i < D; i++)
463 for (
int j = 0; j < D; j++)
464 res.DDValue(i,j) = IfZero(x.DDValue(i,j)+x.DValue(i) * x.DValue(j),SCAL{0.},0.5/res.Value() * x.DDValue(i,j) - 0.25 / (x.Value()*res.Value()) * x.DValue(i) * x.DValue(j));
471 template <
int D,
typename SCAL>
472 INLINE AutoDiffDiff<D, SCAL> exp (AutoDiffDiff<D, SCAL> x)
474 AutoDiffDiff<D, SCAL> res;
475 res.Value() = exp(x.Value());
476 for (
int k = 0; k < D; k++)
477 res.DValue(k) = x.DValue(k) * res.Value();
478 for (
int k = 0; k < D; k++)
479 for (
int l = 0; l < D; l++)
480 res.DDValue(k,l) = (x.DValue(k) * x.DValue(l)+x.DDValue(k,l)) * res.Value();
485 template <
int D,
typename SCAL>
486 INLINE AutoDiffDiff<D,SCAL> pow (AutoDiffDiff<D,SCAL> x, AutoDiffDiff<D,SCAL> y )
488 return exp(log(x)*y);
491 template <
int D,
typename SCAL>
492 INLINE AutoDiffDiff<D, SCAL> log (AutoDiffDiff<D, SCAL> x)
494 AutoDiffDiff<D, SCAL> res;
495 res.Value() = log(x.Value());
496 SCAL xinv = 1.0/x.Value();
497 for (
int k = 0; k < D; k++)
498 res.DValue(k) = x.DValue(k) * xinv;
499 for (
int k = 0; k < D; k++)
500 for (
int l = 0; l < D; l++)
501 res.DDValue(k,l) = -xinv*xinv*x.DValue(k) * x.DValue(l) + xinv * x.DDValue(k,l);
507 template <
int D,
typename SCAL>
508 INLINE AutoDiffDiff<D, SCAL> sin (AutoDiffDiff<D, SCAL> x)
510 AutoDiffDiff<D, SCAL> res;
511 SCAL s = sin(x.Value());
512 SCAL c = cos(x.Value());
515 for (
int k = 0; k < D; k++)
516 res.DValue(k) = x.DValue(k) * c;
517 for (
int k = 0; k < D; k++)
518 for (
int l = 0; l < D; l++)
519 res.DDValue(k,l) = -s * x.DValue(k) * x.DValue(l) + c * x.DDValue(k,l);
524 template <
int D,
typename SCAL>
525 INLINE AutoDiffDiff<D, SCAL> cos (AutoDiffDiff<D, SCAL> x)
527 AutoDiffDiff<D, SCAL> res;
528 SCAL s = sin(x.Value());
529 SCAL c = cos(x.Value());
532 for (
int k = 0; k < D; k++)
533 res.DValue(k) = -s * x.DValue(k);
534 for (
int k = 0; k < D; k++)
535 for (
int l = 0; l < D; l++)
536 res.DDValue(k,l) = -c * x.DValue(k) * x.DValue(l) - s * x.DDValue(k,l);
540 template <
int D,
typename SCAL>
541 INLINE AutoDiffDiff<D, SCAL> tan (AutoDiffDiff<D, SCAL> x)
542 {
return sin(x) / cos(x); }
545 template <
int D,
typename SCAL>
546 INLINE AutoDiffDiff<D, SCAL> atan (AutoDiffDiff<D, SCAL> x)
548 AutoDiffDiff<D, SCAL> res;
549 SCAL a = atan(x.Value());
551 for (
int k = 0; k < D; k++)
552 res.DValue(k) = x.DValue(k)/(1+x.Value()*x.Value()) ;
553 for (
int k = 0; k < D; k++)
554 for (
int l = 0; l < D; l++)
555 res.DDValue(k,l) = -2*x.Value()/((1+x.Value()*x.Value())*(1+x.Value()*x.Value())) * x.DValue(k) * x.DValue(l) + x.DDValue(k,l)/(1+x.Value()*x.Value());
559 template <
int D,
typename SCAL>
560 INLINE AutoDiffDiff<D, SCAL> atan2 (AutoDiffDiff<D, SCAL> x,AutoDiffDiff<D, SCAL> y)
562 AutoDiffDiff<D, SCAL> res;
563 SCAL a = atan2(x.Value(), y.Value());
565 for (
int k = 0; k < D; k++)
566 res.DValue(k) = (x.Value()*y.DValue(k)-y.Value()*x.DValue(k))/(y.Value()*y.Value()+x.Value()*x.Value());
568 for (
int k = 0; k < D; k++)
569 for (
int l = 0; l < D; l++)
570 res.DDValue(k,l) = (x.DValue(k)*y.DValue(l)+x.Value()*y.DDValue(l,k) - y.DValue(k)*x.DValue(l) - y.Value()*x.DDValue(l,k))/(y.Value()*y.Value()+x.Value()*x.Value()) - 2 * (x.Value()*y.DValue(k)-y.Value()*x.DValue(k)) * (x.Value()*x.DValue(k) + y.Value()*y.DValue(k))/( (y.Value()*y.Value()+x.Value()*x.Value()) * (y.Value()*y.Value()+x.Value()*x.Value()) );
577 template <
int D,
typename SCAL>
578 INLINE AutoDiffDiff<D,SCAL> acos (AutoDiffDiff<D,SCAL> x)
580 AutoDiffDiff<D,SCAL> res;
581 SCAL a = acos(x.Value());
583 auto omaa = 1-x.Value()*x.Value();
586 SCAL dda = -x.Value() / (s*omaa);
587 for (
int k = 0; k < D; k++)
588 res.DValue(k) = x.DValue(k)*da;
589 for (
int k = 0; k < D; k++)
590 for (
int l = 0; l < D; l++)
591 res.DDValue(k,l) = dda * x.DValue(k) * x.DValue(l) + da * x.DDValue(k,l);
598 template <
int D,
typename SCAL>
599 INLINE AutoDiffDiff<D,SCAL> asin (AutoDiffDiff<D,SCAL> x)
601 AutoDiffDiff<D,SCAL> res;
602 SCAL a = asin(x.Value());
604 auto omaa = 1-x.Value()*x.Value();
607 SCAL dda = x.Value() / (s*omaa);
608 for (
int k = 0; k < D; k++)
609 res.DValue(k) = x.DValue(k)*da;
610 for (
int k = 0; k < D; k++)
611 for (
int l = 0; l < D; l++)
612 res.DDValue(k,l) = dda * x.DValue(k) * x.DValue(l) + da * x.DDValue(k,l);
618 template <
int D,
typename SCAL>
619 INLINE AutoDiffDiff<D, SCAL> sinh (AutoDiffDiff<D, SCAL> x)
621 AutoDiffDiff<D, SCAL> res;
622 SCAL sh = sinh(x.Value());
623 SCAL ch = cosh(x.Value());
626 for (
int k = 0; k < D; k++)
627 res.DValue(k) = x.DValue(k) * ch;
628 for (
int k = 0; k < D; k++)
629 for (
int l = 0; l < D; l++)
630 res.DDValue(k,l) = sh * x.DValue(k) * x.DValue(l) + ch * x.DDValue(k,l);
635 template <
int D,
typename SCAL>
636 INLINE AutoDiffDiff<D, SCAL> cosh (AutoDiffDiff<D, SCAL> x)
638 AutoDiffDiff<D, SCAL> res;
639 SCAL sh = sinh(x.Value());
640 SCAL ch = cosh(x.Value());
643 for (
int k = 0; k < D; k++)
644 res.DValue(k) = sh * x.DValue(k);
645 for (
int k = 0; k < D; k++)
646 for (
int l = 0; l < D; l++)
647 res.DDValue(k,l) = ch * x.DValue(k) * x.DValue(l) + sh * x.DDValue(k,l);
652 template<
int D,
typename SCAL>
653 INLINE AutoDiffDiff<D,SCAL> floor (
const AutoDiffDiff<D,SCAL> & x)
655 return floor(x.Value());
659 template<
int D,
typename SCAL>
660 INLINE AutoDiffDiff<D,SCAL> ceil (
const AutoDiffDiff<D,SCAL> & x)
662 return ceil(x.Value());
666 template <
int D,
typename SCAL,
typename TB,
typename TC>
667 auto IfPos (AutoDiffDiff<D,SCAL> a, TB b, TC c) -> decltype(IfPos (a.Value(), b, c))
669 return IfPos (a.Value(), b, c);
672 template <
int D,
typename SCAL>
673 INLINE AutoDiffDiff<D,SCAL> IfPos (SCAL a, AutoDiffDiff<D,SCAL> b, AutoDiffDiff<D,SCAL> c)
675 AutoDiffDiff<D,SCAL> res;
676 res.Value() = IfPos (a, b.Value(), c.Value());
677 for (
int j = 0; j < D; j++)
679 res.DValue(j) = IfPos (a, b.DValue(j), c.DValue(j));
680 res.DDValue(j) = IfPos (a, b.DDValue(j), c.DDValue(j));
685 template <
int D,
typename SCAL,
typename TC>
686 INLINE AutoDiffDiff<D,SCAL> IfPos (SCAL a, AutoDiffDiff<D,SCAL> b, TC c)
688 return IfPos (a, b, AutoDiffDiff<D,SCAL> (c));