17 #include "analysis/OrcaKinFit/BaseHardConstraint.h"
18 #include <framework/logging/Logger.h>
32 namespace OrcaKinFit {
39 double dgdpi[BaseDefs::MAXINTERVARS];
40 for (
unsigned int i = 0; i <
fitobjects.size(); ++i) {
44 foi->addTo1stDerivatives(M, idim, dgdpi,
getGlobalNum(), getVarBasis());
86 double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS];
93 auto* dPidAk =
new double[n * BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS];
94 bool* dPidAkval =
new bool[n];
96 for (
int i = 0; i < n; ++i) dPidAkval[i] =
false;
100 double d2GdPdAl[BaseDefs::MAXINTERVARS * BaseDefs::MAXPAR];
102 double d2GdAkdAl[BaseDefs::MAXPAR * BaseDefs::MAXPAR] = {0};
106 int* parglobal =
new int[BaseDefs::MAXPAR * n];
108 for (
int i = 0; i < n; ++i) {
111 for (
int klocal = 0; klocal < foi->getNPar(); ++klocal) {
112 parglobal [BaseDefs::MAXPAR * i + klocal] = foi->getGlobalParNum(klocal);
117 for (
int i = 0; i < n; ++i) {
120 for (
int j = 0; j < n; ++j) {
125 foi->getDerivatives(dPidAk + i * (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS), BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS);
129 foj->getDerivatives(dPidAk + j * (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS), BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS);
137 for (
int llocal = 0; llocal < foj->getNPar(); ++llocal) {
138 for (
int ii = 0; ii < BaseDefs::MAXINTERVARS; ++ii) {
139 int ind1 = BaseDefs::MAXINTERVARS * ii;
140 int ind2 = (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS) * j + BaseDefs::MAXINTERVARS * llocal;
141 double& r = d2GdPdAl[BaseDefs::MAXINTERVARS * llocal + ii];
142 r = d2GdPidPj[ ind1] * dPidAk[ ind2];
143 r += d2GdPidPj[++ind1] * dPidAk[++ind2];
144 r += d2GdPidPj[++ind1] * dPidAk[++ind2];
145 r += d2GdPidPj[++ind1] * dPidAk[++ind2];
154 for (
int klocal = 0; klocal < foi->getNPar(); ++klocal) {
155 for (
int llocal = 0; llocal < foj->getNPar(); ++llocal) {
156 int ind1 = BaseDefs::MAXINTERVARS * llocal;
157 int ind2 = (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS) * i + BaseDefs::MAXINTERVARS * klocal;
158 double& r = d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal];
159 r = d2GdPdAl[ ind1] * dPidAk[ ind2];
160 r += d2GdPdAl[++ind1] * dPidAk[++ind2];
161 r += d2GdPdAl[++ind1] * dPidAk[++ind2];
162 r += d2GdPdAl[++ind1] * dPidAk[++ind2];
166 for (
int klocal = 0; klocal < foi->getNPar(); ++klocal) {
167 int kglobal = parglobal [BaseDefs::MAXPAR * i + klocal];
168 for (
int llocal = 0; llocal < foj->getNPar(); ++llocal) {
169 int lglobal = parglobal [BaseDefs::MAXPAR * j + llocal];
170 M [idim * kglobal + lglobal] += lambda * d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal];
185 double dgdpi[BaseDefs::MAXINTERVARS];
186 for (
int i = 0; i < n; ++i) {
190 foi->addTo2ndDerivatives(M, idim, lambda, dgdpi, getVarBasis());
201 double dgdpi[BaseDefs::MAXINTERVARS];
202 for (
unsigned int i = 0; i <
fitobjects.size(); ++i) {
206 foi->addToGlobalChi2DerVector(y, idim, lambda, dgdpi, getVarBasis());
214 double dgdpi[BaseDefs::MAXINTERVARS];
216 for (
unsigned int i = 0; i <
fitobjects.size(); ++i) {
220 error2 += foi->getError2(dgdpi, getVarBasis());
223 return std::sqrt(std::abs(error2));
230 for (pw = w; pw < w + idim; * (pw++) = 0);
233 for (pw = w, pp = p; pw < w + idim; result += *(pp++) * *(pw++));
240 if (val == 0)
return mu * std::fabs(
dirDer(p, w, idim, 1));
241 else if (val > 0)
return mu *
dirDer(p, w, idim, 1);
242 else return -mu *
dirDer(p, w, idim, 1);
246 void BaseHardConstraint::test1stDerivatives()
248 B2INFO(
"BaseConstraint::test1stDerivatives for " <<
getName());
250 for (
double& i : y) i = 0;
252 double eps = 0.00001;
253 for (
unsigned int ifo = 0; ifo <
fitobjects.size(); ++ifo) {
256 for (
int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
257 int iglobal = fo->getGlobalParNum(ilocal);
258 double calc = y[iglobal];
260 B2INFO(
"fo: " << fo->getName() <<
" par " << ilocal <<
"/"
261 << iglobal <<
" (" << fo->getParamName(ilocal)
262 <<
") calc: " << calc <<
" - num: " << num <<
" = " << calc - num);
267 void BaseHardConstraint::test2ndDerivatives()
269 B2INFO(
"BaseConstraint::test2ndDerivatives for " <<
getName());
270 const int idim = 100;
271 auto* M =
new double[idim * idim];
272 for (
int i = 0; i < idim * idim; ++i) M[i] = 0;
275 B2INFO(
"eps=" << eps);
277 for (
unsigned int ifo1 = 0; ifo1 <
fitobjects.size(); ++ifo1) {
280 for (
unsigned int ifo2 = ifo1; ifo2 <
fitobjects.size(); ++ifo2) {
283 for (
int ilocal1 = 0; ilocal1 < fo1->getNPar(); ++ilocal1) {
284 int iglobal1 = fo1->getGlobalParNum(ilocal1);
285 for (
int ilocal2 = (ifo1 == ifo2 ? ilocal1 : 0); ilocal2 < fo2->getNPar(); ++ilocal2) {
286 int iglobal2 = fo2->getGlobalParNum(ilocal2);
287 double calc = M[idim * iglobal1 + iglobal2];
289 B2INFO(
"fo1: " << fo1->getName() <<
" par " << ilocal1 <<
"/"
290 << iglobal1 <<
" (" << fo1->getParamName(ilocal1)
291 <<
"), fo2: " << fo2->getName() <<
" par " << ilocal2 <<
"/"
292 << iglobal2 <<
" (" << fo2->getParamName(ilocal2)
293 <<
") calc: " << calc <<
" - num: " << num <<
" = " << calc - num);
306 double save = fo->getParam(ilocal);
307 fo->setParam(ilocal, save + eps);
309 fo->setParam(ilocal, save - eps);
311 double result = (
v1 -
v2) / (2 * eps);
312 fo->setParam(ilocal, save);
317 int ifo2,
int ilocal2,
double eps2)
321 if (ifo1 == ifo2 && ilocal1 == ilocal2) {
324 double save = fo->getParam(ilocal1);
326 fo->setParam(ilocal1, save + eps1);
328 fo->setParam(ilocal1, save - eps1);
330 result = (v1 + v2 - 2 * v0) / (eps1 * eps1);
331 fo->setParam(ilocal1, save);
337 double save1 = fo1->getParam(ilocal1);
338 double save2 = fo2->getParam(ilocal2);
339 fo1->setParam(ilocal1, save1 + eps1);
340 fo2->setParam(ilocal2, save2 + eps2);
342 fo2->setParam(ilocal2, save2 - eps2);
344 fo1->setParam(ilocal1, save1 - eps1);
346 fo2->setParam(ilocal2, save2 + eps2);
348 result = (v11 + v22 - v12 - v21) / (4 * eps1 * eps2);
349 fo1->setParam(ilocal1, save1);
350 fo2->setParam(ilocal2, save2);
355 void BaseHardConstraint::printFirstDerivatives()
const
358 B2INFO(
"BaseHardConstraint::printFirstDerivatives " <<
fitobjects.size());
360 double dgdpi[BaseDefs::MAXINTERVARS];
361 for (
unsigned int i = 0; i <
fitobjects.size(); ++i) {
365 B2INFO(
"first derivs for obj " << i <<
" : ");
366 for (
double j : dgdpi)
374 void BaseHardConstraint::printSecondDerivatives()
const
377 double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS];
381 for (
int i = 0; i < n; ++i) {
384 for (
int j = 0; j < n; ++j) {
389 B2INFO(
"second derivs for objs " << i <<
" " << j);
392 for (
int k1 = 0; k1 < BaseDefs::MAXINTERVARS; k1++) {
393 for (
int k2 = 0; k2 < BaseDefs::MAXINTERVARS; k2++) {
394 B2INFO(d2GdPidPj[k++] <<
" ");