void
PFEMElement2DBubble::getdF(Matrix& df) const {
df.resize(6,6);
df.Zero();
for(int a=0; a<3; a++) {
for(int b=0; b<6; b++) {
df(2*a,b) = bx*dJ(b);
df(2*a+1,b) = by*dJ(b);
}
}
df *= rho*thickness/6.0;
// velocity
if(mu > 0) {
Vector v(6);
for(int a=0; a<3; a++) {
const Vector& vel = nodes[2*a]->getTrialVel();
v(2*a) = vel(0);
v(2*a+1) = vel(1);
}
Matrix dk(6,6);
getdK(v,dk);
df -= dk;
}
}
/****************************************************
函数名： sepecial_interlocking
功能描述： 特殊联锁
返回值： void
作者： hejh
日期： 2015/07/27
****************************************************/
void sepecial_interlocking()
{
dk();
dk_bgfylz65(NO_INDEX);
dk_bgcpkz53();
}
/** Implementation of complex integral E_1
*/
std::complex<double> E1(std::complex<double> z) {
std::complex<double> exp_e1;
double rz = real(z);
double az = abs(z);
if (fabs(az) < 1.0E-8) {
// If z = 0, then the result is infinity... diverge!
std::complex<double> r(1.0E300, 0.0);
exp_e1 = r;
} else if (az <= 10.0 || (rz < 0.0 && az < 20.0)) {
// Some interesting region, equal to integrate to infinity, converged
// cout << "[DB] Type 1\n";
std::complex<double> r(1.0, 0.0);
exp_e1 = r;
std::complex<double> cr = r;
for (size_t k = 1; k <= 150; ++k) {
double dk = double(k);
cr = -cr * dk * z / ((dk + 1.0) * (dk + 1.0));
exp_e1 += cr;
if (abs(cr) < abs(exp_e1) * 1.0E-15) {
// cr is converged to zero
break;
}
} // ENDFOR k
const double el = 0.5772156649015328;
exp_e1 = -el - log(z) + (z * exp_e1);
} else {
// Rest of the region
std::complex<double> ct0(0.0, 0.0);
for (int k = 120; k > 0; --k) {
std::complex<double> dk(double(k), 0.0);
ct0 = dk / (10.0 + dk / (z + ct0));
} // ENDFOR k
exp_e1 = 1.0 / (z + ct0);
exp_e1 = exp_e1 * exp(-z);
if (rz < 0.0 && fabs(imag(z)) < 1.0E-10) {
std::complex<double> u(0.0, 1.0);
exp_e1 = exp_e1 - (M_PI * u);
}
}
return exp_e1;
}
void
PFEMElement2DBubble::getdF(Vector& df) const
{
df.resize(6);
// external force
if(parameterID == 2) {
for(int a=0; a<3; a++) {
df(2*a) = bx/6.*J*thickness;
df(2*a+1) = by/6.*J*thickness;
}
} else if(parameterID == 3) {
for(int a=0; a<3; a++) {
df(2*a) = rho/6.*J*thickness;
df(2*a+1) = 0.0;
}
} else if(parameterID == 4) {
for(int a=0; a<3; a++) {
df(2*a) = 0.0;
df(2*a+1) = rho/6.*J*thickness;
}
}
// velocity
if(mu > 0 && parameterID == 1) {
Vector v(6);
for(int a=0; a<3; a++) {
const Vector& vel = nodes[2*a]->getTrialVel();
v(2*a) = vel(0);
v(2*a+1) = vel(1);
}
Matrix dk(6,6);
getdK(dk);
df.addMatrixVector(1.0, dk, v, -1.0);
}
}
TEST_F(fisheyeTest, jacobians)
{
int n = 10;
cv::Mat X(1, n, CV_64FC3);
cv::Mat om(3, 1, CV_64F), theT(3, 1, CV_64F);
cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F);
cv::Mat k(4, 1, CV_64F);
double alpha;
cv::RNG r;
r.fill(X, cv::RNG::NORMAL, 2, 1);
X = cv::abs(X) * 10;
r.fill(om, cv::RNG::NORMAL, 0, 1);
om = cv::abs(om);
r.fill(theT, cv::RNG::NORMAL, 0, 1);
theT = cv::abs(theT); theT.at<double>(2) = 4; theT *= 10;
r.fill(f, cv::RNG::NORMAL, 0, 1);
f = cv::abs(f) * 1000;
r.fill(c, cv::RNG::NORMAL, 0, 1);
c = cv::abs(c) * 1000;
r.fill(k, cv::RNG::NORMAL, 0, 1);
k*= 0.5;
alpha = 0.01*r.gaussian(1);
cv::Mat x1, x2, xpred;
cv::Matx33d theK(f.at<double>(0), alpha * f.at<double>(0), c.at<double>(0),
0, f.at<double>(1), c.at<double>(1),
0, 0, 1);
cv::Mat jacobians;
cv::fisheye::projectPoints(X, x1, om, theT, theK, k, alpha, jacobians);
//test on T:
cv::Mat dT(3, 1, CV_64FC1);
r.fill(dT, cv::RNG::NORMAL, 0, 1);
dT *= 1e-9*cv::norm(theT);
cv::Mat T2 = theT + dT;
cv::fisheye::projectPoints(X, x2, om, T2, theK, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1);
CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on om:
cv::Mat dom(3, 1, CV_64FC1);
r.fill(dom, cv::RNG::NORMAL, 0, 1);
dom *= 1e-9*cv::norm(om);
cv::Mat om2 = om + dom;
cv::fisheye::projectPoints(X, x2, om2, theT, theK, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1);
CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on f:
cv::Mat df(2, 1, CV_64FC1);
r.fill(df, cv::RNG::NORMAL, 0, 1);
df *= 1e-9*cv::norm(f);
cv::Matx33d K2 = theK + cv::Matx33d(df.at<double>(0), df.at<double>(0) * alpha, 0, 0, df.at<double>(1), 0, 0, 0, 0);
cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1);
CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on c:
cv::Mat dc(2, 1, CV_64FC1);
r.fill(dc, cv::RNG::NORMAL, 0, 1);
dc *= 1e-9*cv::norm(c);
K2 = theK + cv::Matx33d(0, 0, dc.at<double>(0), 0, 0, dc.at<double>(1), 0, 0, 0);
cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1);
CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on k:
cv::Mat dk(4, 1, CV_64FC1);
r.fill(dk, cv::RNG::NORMAL, 0, 1);
dk *= 1e-9*cv::norm(k);
cv::Mat k2 = k + dk;
cv::fisheye::projectPoints(X, x2, om, theT, theK, k2, alpha, cv::noArray());
xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1);
CV_Assert (cv::norm(x2 - xpred) < 1e-10);
//test on alpha:
cv::Mat dalpha(1, 1, CV_64FC1);
r.fill(dalpha, cv::RNG::NORMAL, 0, 1);
dalpha *= 1e-9*cv::norm(f);
double alpha2 = alpha + dalpha.at<double>(0);
K2 = theK + cv::Matx33d(0, f.at<double>(0) * dalpha.at<double>(0), 0, 0, 0, 0, 0, 0, 0);
cv::fisheye::projectPoints(X, x2, om, theT, theK, k, alpha2, cv::noArray());
xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1);
CV_Assert (cv::norm(x2 - xpred) < 1e-10);
}