bool CalibrationData::saveSLCALIB(const QString& filename){
FILE * fp = fopen(qPrintable(filename), "w");
if (!fp)
return false;
fprintf(fp, "#V1.0 SLStudio calibration\n");
fprintf(fp, "#Calibration time: %s\n\n", calibrationDateTime.c_str());
fprintf(fp, "Kc\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kc(0,0), Kc(0,1), Kc(0,2), Kc(1,0), Kc(1,1), Kc(1,2), Kc(2,0), Kc(2,1), Kc(2,2));
fprintf(fp, "kc\n%f %f %f %f %f\n\n", kc(0), kc(1), kc(2), kc(3), kc(4));
fprintf(fp, "Kp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kp(0,0), Kp(0,1), Kp(0,2), Kp(1,0), Kp(1,1), Kp(1,2), Kp(2,0), Kp(2,1), Kp(2,2));
fprintf(fp, "kp\n%f %f %f %f %f\n\n", kp(0), kp(1), kp(2), kp(3), kp(4));
fprintf(fp, "Rp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Rp(0,0), Rp(0,1), Rp(0,2), Rp(1,0), Rp(1,1), Rp(1,2), Rp(2,0), Rp(2,1), Rp(2,2));
fprintf(fp, "Tp\n%f %f %f\n\n", Tp(0), Tp(1), Tp(2));
fprintf(fp, "cam_error: %f\n\n", cam_error);
fprintf(fp, "proj_error: %f\n\n", proj_error);
fprintf(fp, "stereo_error: %f\n\n", stereo_error);
fclose(fp);
return true;
}
bool singleRPAJastrowBuilder::put(xmlNodePtr cur, int addOrbital)
{
MyName="Jep";
string rpafunc="RPA";
OhmmsAttributeSet a;
a.add(MyName,"name");
a.add(rpafunc,"function");
a.put(cur);
ParameterSet params;
RealType Rs(-1.0);
RealType Kc(-1.0);
params.add(Rs,"rs","double");
params.add(Kc,"kc","double");
params.put(cur);
if(Rs<0) {
Rs=tlen;
}
if(Kc<0){
Kc = 1e-6 ;
};
if (rpafunc=="RPA"){
myHandler= new LRRPAHandlerTemp<EPRPABreakup<RealType>,LPQHIBasis>(targetPtcl,Kc);
app_log()<<" using e-p RPA"<<endl;
}
else if (rpafunc=="dRPA") {
myHandler= new LRRPAHandlerTemp<derivEPRPABreakup<RealType>,LPQHIBasis>(targetPtcl,Kc);
app_log()<<" using e-p derivRPA"<<endl;
}
myHandler->Breakup(targetPtcl,Rs);
// app_log() << " Maximum K shell " << myHandler->MaxKshell << endl;
// app_log() << " Number of k vectors " << myHandler->Fk.size() << endl;
//Add short range part
Rcut = myHandler->get_rc()-0.1;
GridType* myGrid = new GridType;
int npts=static_cast<int>(Rcut/0.01)+1;
myGrid->set(0,Rcut,npts);
//create the numerical functor
nfunc = new FuncType;
SRA = new ShortRangePartAdapter<RealType>(myHandler);
SRA->setRmax(Rcut);
nfunc->initialize(SRA, myGrid);
J1s = new JneType (*sourcePtcl,targetPtcl);
for(int ig=0; ig<ng; ig++) {
J1s->addFunc(ig,nfunc);
}
app_log()<<" Only Short range part of E-I RPA is implemented"<<endl;
if (addOrbital) targetPsi.addOrbital(J1s,MyName);
return true;
}
K itemAtIndex(K a, I i) { // Return i-th item from any type as K - TODO: oom wherever this is used
I at=a->t;
if( 0< at)R ci(a);
if(-4==at)R Ks(kS(a)[i]); //could refactor all this
if(-3==at)R Kc(kC(a)[i]);
if(-2==at)R Kf(kF(a)[i]);
if(-1==at)R Ki(kI(a)[i]);
R ci(kK(a)[i]);
}
static PyObject*
_get_Kc(PyObject* self, PyObject* ko)
{
K kobj;
int ok;
ok = ko && IS_K(ko);
if (!ok) goto fail;
kobj = ((_K*)ko)->kobj;
if (kobj && kobj->t == 3) {
char c[2] = "\0\0";
c[0] = Kc(kobj);
return Py_BuildValue("s", c);
}
fail:
PyErr_BadArgument();
return NULL;
}
bool CartesianImpedance::startHook() {
RESTRICT_ALLOC;
for (size_t i = 0; i < K; i++) {
Kc(i * 6 + 0) = 1;
Kc(i * 6 + 1) = 1;
Kc(i * 6 + 2) = 1500;
Kc(i * 6 + 3) = 150;
Kc(i * 6 + 4) = 150;
Kc(i * 6 + 5) = 150;
Dxi(i * 6 + 0) = 0.7;
Dxi(i * 6 + 1) = 0.7;
Dxi(i * 6 + 2) = 0.7;
Dxi(i * 6 + 3) = 0.7;
Dxi(i * 6 + 4) = 0.7;
Dxi(i * 6 + 5) = 0.7;
geometry_msgs::Pose pos;
if (port_tool_position_command_[i]->read(pos) == RTT::NewData) {
tools[i](0) = pos.position.x;
tools[i](1) = pos.position.y;
tools[i](2) = pos.position.z;
tools[i](3) = pos.orientation.w;
tools[i](4) = pos.orientation.x;
tools[i](5) = pos.orientation.y;
tools[i](6) = pos.orientation.z;
}
else {
return false;
}
}
for (size_t i = 0; i < N; i++) {
nullspace_torque_command_(i) = 0.0;
}
if (port_joint_position_.read(joint_position_) == RTT::NewData) {
robot_->fkin(&r_cmd[0], joint_position_, &tools[0]);
for (size_t i = 0; i < K; i++) {
geometry_msgs::Pose pos;
tf::poseEigenToMsg(r_cmd[i], pos);
port_cartesian_position_[i]->write(pos);
}
} else {
return false;
}
UNRESTRICT_ALLOC;
return true;
}
static PyObject*
_set_Kc(PyObject* self, PyObject* args)
{
PyObject* ko;
char* c;
if (PyArg_ParseTuple(args, "O!s", &_KType, &ko, &c)) {
K k = ((_K*)ko)->kobj;
if (k && k->t == 3) {
Kc(k) = c[0];
Py_INCREF(ko);
return ko;
} else {
PyErr_SetString(PyExc_TypeError, "wrong k type");
return NULL;
}
}
PyErr_BadArgument();
return NULL;
}
/*!
Initialization of the tracking. The ellipse is defined thanks to the
coordinates of n points.
\warning It is better to use at least five points to well estimate the K parameters.
\param I : Image in which the ellipse appears.
\param n : The number of points in the list.
\param iP : A pointer to a list of pointsbelonging to the ellipse edge.
*/
void
vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
vpImagePoint *iP)
{
vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
if (circle==false)
{
vpMatrix A(n,5) ;
vpColVector b_(n) ;
vpColVector x(5) ;
// Construction du systeme Ax=b
//! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
// A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
for (unsigned int k =0 ; k < n ; k++)
{
A[k][0] = vpMath::sqr(iP[k].get_j()) ;
A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
A[k][2] = 2* iP[k].get_i() ;
A[k][3] = 2* iP[k].get_j() ;
A[k][4] = 1 ;
b_[k] = - vpMath::sqr(iP[k].get_i()) ;
}
K = A.pseudoInverse(1e-26)*b_ ;
std::cout << K << std::endl;
}
else
{
vpMatrix A(n,3) ;
vpColVector b_(n) ;
vpColVector x(3) ;
vpColVector Kc(3) ;
for (unsigned int k =0 ; k < n ; k++)
{
A[k][0] = 2* iP[k].get_i() ;
A[k][1] = 2* iP[k].get_j() ;
A[k][2] = 1 ;
b_[k] = - vpMath::sqr(iP[k].get_i()) - vpMath::sqr(iP[k].get_j()) ;
}
Kc = A.pseudoInverse(1e-26)*b_ ;
K[0] = 1 ;
K[1] = 0 ;
K[2] = Kc[0] ;
K[3] = Kc[1] ;
K[4] = Kc[2] ;
std::cout << K << std::endl;
}
iP1 = iP[0];
iP2 = iP[n-1];
getParameters() ;
std::cout << "vpMeEllipse::initTracking() ellipse avant: " << iPc << " " << a << " " << b << " " << vpMath::deg(e) << " alpha: " << alpha1 << " " << alpha2 << std::endl;
computeAngle(iP1, iP2) ;
std::cout << "vpMeEllipse::initTracking() ellipse apres: " << iPc << " " << a << " " << b << " " << vpMath::deg(e) << " alpha: " << alpha1 << " " << alpha2 << std::endl;
expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
display(I, vpColor::green) ;
sample(I) ;
// 2. On appelle ce qui n'est pas specifique
{
vpMeTracker::initTracking(I) ;
}
try{
track(I) ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
vpMeTracker::display(I) ;
vpDisplay::flush(I) ;
}
void
vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
unsigned *i, unsigned *j)
{
vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
if (circle==false)
{
vpMatrix A(n,5) ;
vpColVector b_(n) ;
vpColVector x(5) ;
// Construction du systeme Ax=b
//! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
// A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
for (unsigned int k =0 ; k < n ; k++)
{
A[k][0] = vpMath::sqr(j[k]) ;
A[k][1] = 2* i[k] * j[k] ;
A[k][2] = 2* i[k] ;
A[k][3] = 2* j[k] ;
A[k][4] = 1 ;
b_[k] = - vpMath::sqr(i[k]) ;
}
K = A.pseudoInverse(1e-26)*b_ ;
std::cout << K << std::endl;
}
else
{
vpMatrix A(n,3) ;
vpColVector b_(n) ;
vpColVector x(3) ;
vpColVector Kc(3) ;
for (unsigned int k =0 ; k < n ; k++)
{
A[k][0] = 2* i[k] ;
A[k][1] = 2* j[k] ;
A[k][2] = 1 ;
b_[k] = - vpMath::sqr(i[k]) - vpMath::sqr(j[k]) ;
}
Kc = A.pseudoInverse(1e-26)*b_ ;
K[0] = 1 ;
K[1] = 0 ;
K[2] = Kc[0] ;
K[3] = Kc[1] ;
K[4] = Kc[2] ;
std::cout << K << std::endl;
}
iP1.set_i( i[0] );
iP1.set_j( j[0] );
iP2.set_i( i[n-1] );
iP2.set_j( j[n-1] );
getParameters() ;
computeAngle(iP1, iP2) ;
display(I, vpColor::green) ;
sample(I) ;
// 2. On appelle ce qui n'est pas specifique
{
vpMeTracker::initTracking(I) ;
}
try{
track(I) ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
vpMeTracker::display(I) ;
vpDisplay::flush(I) ;
}
void CartesianImpedance::updateHook() {
RESTRICT_ALLOC;
// ToolMass toolsM[K];
Eigen::Affine3d r[K];
// read inputs
port_joint_position_.read(joint_position_);
if (!joint_position_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "joint_position_ contains NaN or inf" << RTT::endlog();
return;
}
port_joint_velocity_.read(joint_velocity_);
if (!joint_velocity_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "joint_velocity_ contains NaN or inf" << RTT::endlog();
return;
}
port_nullspace_torque_command_.read(nullspace_torque_command_);
if (!nullspace_torque_command_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "nullspace_torque_command_ contains NaN or inf" << RTT::endlog();
return;
}
for (size_t i = 0; i < K; i++) {
geometry_msgs::Pose pos;
if (port_cartesian_position_command_[i]->read(pos) == RTT::NewData) {
tf::poseMsgToEigen(pos, r_cmd[i]);
}
if (port_tool_position_command_[i]->read(pos) == RTT::NewData) {
tools[i](0) = pos.position.x;
tools[i](1) = pos.position.y;
tools[i](2) = pos.position.z;
tools[i](3) = pos.orientation.w;
tools[i](4) = pos.orientation.x;
tools[i](5) = pos.orientation.y;
tools[i](6) = pos.orientation.z;
}
cartesian_trajectory_msgs::CartesianImpedance impedance;
if (port_cartesian_impedance_command_[i]->read(impedance)
== RTT::NewData) {
Kc(i * 6 + 0) = impedance.stiffness.force.x;
Kc(i * 6 + 1) = impedance.stiffness.force.y;
Kc(i * 6 + 2) = impedance.stiffness.force.z;
Kc(i * 6 + 3) = impedance.stiffness.torque.x;
Kc(i * 6 + 4) = impedance.stiffness.torque.y;
Kc(i * 6 + 5) = impedance.stiffness.torque.z;
Dxi(i * 6 + 0) = impedance.damping.force.x;
Dxi(i * 6 + 1) = impedance.damping.force.y;
Dxi(i * 6 + 2) = impedance.damping.force.z;
Dxi(i * 6 + 3) = impedance.damping.torque.x;
Dxi(i * 6 + 4) = impedance.damping.torque.y;
Dxi(i * 6 + 5) = impedance.damping.torque.z;
}
}
port_mass_matrix_inv_.read(M);
if (!M.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "M contains NaN or inf" << RTT::endlog();
return;
}
// calculate robot data
robot_->jacobian(J, joint_position_, &tools[0]);
if (!J.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "J contains NaN or inf" << RTT::endlog();
return;
}
robot_->fkin(r, joint_position_, &tools[0]);
JT = J.transpose();
lu_.compute(M);
Mi = lu_.inverse();
if (!Mi.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "Mi contains NaN or inf" << RTT::endlog();
return;
}
// calculate stiffness component
for (size_t i = 0; i < K; i++) {
Eigen::Affine3d tmp;
tmp = r[i].inverse() * r_cmd[i];
p(i * 6) = tmp.translation().x();
p(i * 6 + 1) = tmp.translation().y();
p(i * 6 + 2) = tmp.translation().z();
Eigen::Quaternion<double> quat = Eigen::Quaternion<double>(
tmp.rotation());
p(i * 6 + 3) = quat.x();
p(i * 6 + 4) = quat.y();
p(i * 6 + 5) = quat.z();
}
F.noalias() = (Kc.array() * p.array()).matrix();
joint_torque_command_.noalias() = JT * F;
if (!joint_torque_command_.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "joint_torque_command_ contains NaN or inf" << RTT::endlog();
return;
}
// calculate damping component
#if 1
tmpNK_.noalias() = J * Mi;
A.noalias() = tmpNK_ * JT;
luKK_.compute(A);
A = luKK_.inverse();
tmpKK_ = Kc.asDiagonal();
UNRESTRICT_ALLOC;
es_.compute(tmpKK_, A);
RESTRICT_ALLOC;
K0 = es_.eigenvalues();
luKK_.compute(es_.eigenvectors());
Q = luKK_.inverse();
tmpKK_ = Dxi.asDiagonal();
Dc.noalias() = Q.transpose() * tmpKK_;
tmpKK_ = K0.cwiseSqrt().asDiagonal();
tmpKK2_.noalias() = Dc * tmpKK_;
Dc.noalias() = tmpKK2_ * Q;
tmpK_.noalias() = J * joint_velocity_;
F.noalias() = Dc * tmpK_;
if (!F.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "F contains NaN or inf" << RTT::endlog();
return;
}
joint_torque_command_.noalias() -= JT * F;
#else
UNRESTRICT_ALLOC;
tmpNN_.noalias() = JT * Kc.asDiagonal() * J;
es_.compute(tmpNN_, M, Eigen::ComputeEigenvectors | Eigen::BAx_lx);
K0 = es_.eigenvalues();
Q = es_.eigenvectors();
RESTRICT_ALLOC;
tmpNN_ = K0.cwiseSqrt().asDiagonal();
UNRESTRICT_ALLOC;
Dc.noalias() = Q * 0.7 * tmpNN_ * Q.adjoint();
RESTRICT_ALLOC;
joint_torque_command_.noalias() -= Dc * joint_velocity_;
#endif
// calculate null-space component
tmpNK_.noalias() = J * Mi;
tmpKK_.noalias() = tmpNK_ * JT;
luKK_.compute(tmpKK_);
tmpKK_ = luKK_.inverse();
tmpKN_.noalias() = Mi * JT;
Ji.noalias() = tmpKN_ * tmpKK_;
P.noalias() = Eigen::MatrixXd::Identity(P.rows(), P.cols());
P.noalias() -= J.transpose() * A * J * Mi;
if (!P.allFinite()) {
RTT::Logger::In in("CartesianImpedance::updateHook");
error();
RTT::log(RTT::Error) << "P contains NaN or inf" << RTT::endlog();
return;
}
joint_torque_command_.noalias() += P * nullspace_torque_command_;
// write outputs
UNRESTRICT_ALLOC;
port_joint_torque_command_.write(joint_torque_command_);
for (size_t i = 0; i < K; i++) {
geometry_msgs::Pose pos;
tf::poseEigenToMsg(r[i], pos);
port_cartesian_position_[i]->write(pos);
}
}
static PyObject*
_ktoarray(PyObject* self, PyObject* k)
{
PyArrayObject *ret;
if (!PyK_KCheck(k)) {
return PyErr_Format(PyExc_TypeError, "not k object");
}
K kobj = ((PyK_K*)k)->kobj;
if (!kobj) {
return PyErr_Format(PyExc_AssertionError, "null kobj");
}
int t = kobj->t;
/* XXX k objects of type 0 should be converted
* to non-contiguous arrays rather than trigger
* an error.
*/
if (abs(t) >= LEN(types) && t != 5 ) {
return PyErr_Format(PyExc_TypeError,
"cannot create an array from a "
"k object of type %d", t);
}
int type = types[abs(t)]; /* PyArray type */
int nd = t <= 0 || t == 5; /* Number of dimensions (0 or 1) */
int* d = &kobj->n; /* Shape */
char* data;
switch (t) {
case 1:
data = (char*)&Ki(kobj);
break;
case 3:
data = &Kc(kobj);
break;
case 4:
data = Ks(kobj);
break;
default:
data = KC(kobj);
}
/* Special handling for symbols arrays: convert data to Python strings */
PyObject** buf = 0;
if (t == -4) {
int n = *d, i = 0;
buf = (PyObject**)malloc(n * sizeof(PyObject*));
for (i = 0; i < n; ++i) {
char* s = KS(kobj)[i];
if (!s) goto fail;
buf[i] = PyString_FromString(s);
if (!buf[i]) goto fail;
}
data = (char*)buf;
} else if (t == 0 || t == 5) {
int n = *d, i = 0;
buf = (PyObject**)malloc(n * sizeof(PyObject*));
for (i = 0; i < n; ++i) {
K ki = KK(kobj)[i];
if (!ki) goto fail;
ci(ki);
buf[i] = PyK_mk_K(ki);
if (!buf[i]) {
cd(ki);
goto fail;
}
}
data = (char*)buf;
}
if (!(ret = (PyArrayObject *)PyArray_FromDimsAndData(nd, d, type, data))) {
goto fail;
}
if (buf) {
ret->flags |= OWN_DATA;
} else {
Py_INCREF(k);
ret->base = k;
}
return (PyObject*)ret;
fail:
if (buf) free(buf);
return NULL;
}
/* XXX unfortunately API function gnk of which pyk.gk
is based is a vararg function and therefore cannot
be portably exported to Python. It would be better
if libk20 supplied a function gnk_(I, K*)
in addition to gnk(I,...) which would take an array
of K objects as the second argument */
static PyObject*
_gk(PyObject* self, PyObject* args)
{
int n = PyTuple_Size(args);
if (!n) {
return _mk_K(gtn(0,0));
}
int i, type = INT_MAX;
K* ks = (K*)malloc(n*sizeof(K));
K kobj;
for(i = 0; i < n; i++) {
K ki;
int t;
PyObject* argi = PyTuple_GET_ITEM(args, i);
if (!IS_K(argi)) {
goto fail;
}
ks[i] = ki = ((_K*)argi)->kobj;
t = ki->t;
if (INT_MAX == type) {
type = t;
} else if (t > 4 || t < 1 || t != type) {
type = 0;
}
}
kobj = gtn((type>0 && type<5)?-type:0, n);
if (!kobj) {
free(ks);
return PyErr_Format(PyExc_TypeError, "gtn(%d,%d) returned null", -type, n);
}
switch (type) {
case 1:
for (i = 0; i < n; i++) {
KI(kobj)[i] = Ki(ks[i]);
}
break;
case 2:
for (i = 0; i < n; i++) {
KF(kobj)[i] = Kf(ks[i]);
}
break;
case 3:
for (i = 0; i < n; i++) {
KC(kobj)[i] = Kc(ks[i]);
}
break;
case 4:
for (i = 0; i < n; i++) {
KS(kobj)[i] = Ks(ks[i]);
}
break;
default:
memcpy(KK(kobj), ks, n*sizeof(K));
for (i = 0; i < n; i++) {
ci(ks[i]);
}
break;
}
free(ks);
return _mk_K(kobj);
fail:
free(ks);
PyErr_BadArgument();
return NULL;
}
real SH::YCheby(int l, int m, real theta, real phi)
{
if(m==0) return Kc(l,0)*T(l,cos(theta));
else if(m>0) return Kc(l,m)*cos(m*phi)*Tlm(l,m,cos(theta));
else return Kc(l,-m)*sin(-m*phi)*Tlm(l,-m,cos(theta));
}
// Calculate local stiffness matrix. Use reduced integration with hourglass stabilization.
void ElementQuadrangleLin::set_K_isoparametric()
{
double shear = E / (2 + 2 * nu);
double lame = E * nu / ((1 + nu) * (1 - 2 * nu));
Eigen::Matrix3d C;
C << 2 * shear + lame, lame, 0.,
lame, 2 * shear + lame, 0.,
0., 0., shear;
Eigen::Vector4d x, y;
int i;
for (i = 0; i < 4; i++)
{
x(i) = domain->nodes[nodes[i] - 1].x;
y(i) = domain->nodes[nodes[i] - 1].y;
}
const double gp = sqrt(1. / 3.);
double gps[5][2] = { {-gp,-gp},{ gp,-gp },{ gp,gp },{ -gp,gp },{0.,0.} };
Eigen::Vector4d g_xi(-0.25, 0.25, 0.25, -0.25);
Eigen::Vector4d g_eta(-0.25, -0.25, 0.25, 0.25);
Eigen::Vector4d h(0.25, -0.25, 0.25, -0.25);
Eigen::Matrix2d J[5];
Eigen::Matrix2d J_inv[5];
for (i = 0; i < 5; i++)
{
double xi = gps[i][0];
double eta = gps[i][1];
J[i](0, 0) = x.dot(g_xi + eta*h);
J[i](0, 1) = x.dot(g_eta + xi*h);
J[i](1, 0) = y.dot(g_xi + eta*h);
J[i](1, 1) = y.dot(g_eta + xi*h);
J_inv[i] = J[i].inverse();
}
Eigen::MatrixXd xieta(4, 2);
xieta << g_xi, g_eta;
Eigen::MatrixXd b(4, 2);
b = xieta*J_inv[4];
Eigen::MatrixXd B_0T(8,3);
Eigen::Vector4d zers = Eigen::Vector4d::Zero();
B_0T << b.block<4,1>(0,0), zers, b.block<4, 1>(0, 1), zers, b.block<4, 1>(0, 1), b.block<4, 1>(0, 0);
Eigen::MatrixXd xy(4,2);
xy << x,y;
Eigen::Matrix4d m_gamma;
Eigen::Vector4d gamma;
m_gamma = Eigen::Matrix4d::Identity() - b*xy.transpose();
gamma = m_gamma * h;
Eigen::MatrixXd j_0_dev(3, 4);
Eigen::Matrix2d j0iT = J_inv[4].transpose();
j_0_dev << 2 * j0iT.block<1, 2>(0, 0), -1 * j0iT.block<1, 2>(1, 0),
-1 * j0iT.block<1, 2>(0, 0), 2 * j0iT.block<1, 2>(1, 0),
3 * j0iT.block<1, 2>(0, 0), 3 * j0iT.block<1, 2>(1, 0);
j_0_dev *= 1. / 3.;
Eigen::MatrixXd L_hg[4];
for (i = 0; i < 4; i++)
{
double xi = gps[i][0];
double eta = gps[i][1];
L_hg[i].resize(4,2);
L_hg[i] << eta, 0.0,
xi, 0.0,
0.0, eta,
0.0, xi;
}
Eigen::MatrixXd M_hg(8,2);
M_hg << gamma, zers, zers, gamma;
M_hg.transposeInPlace(); // 2x8
Eigen::MatrixXd B_red[4];
Eigen::MatrixXd K_red = Eigen::MatrixXd::Zero(8,8);
Eigen::MatrixXd B[4];
Eigen::MatrixXd Bc[4];
volume = 4 * J[4].determinant()*thickness;
for (i = 0; i < 4; i++)
{
B_red[i].resize(3, 8);
B[i].resize(3, 8);
Bc[i].resize(3, 8);
B_red[i] = j_0_dev * L_hg[i] * M_hg; // 3x4 * 4x2 * 2x8 = 3x8
K_red += B_red[i].transpose()*C*B_red[i]; // 8x3 * 3x3 * 3x8 = 8x8
Bc[i] = B_0T.transpose() + B_red[i]; // 3x8 + 3x8 = 3x8
B[i] << Bc[i].block<3, 1>(0, 0), Bc[i].block<3, 1>(0, 4), Bc[i].block<3, 1>(0, 1), Bc[i].block<3, 1>(0, 5), Bc[i].block<3, 1>(0, 2), Bc[i].block<3, 1>(0, 6), Bc[i].block<3, 1>(0, 3), Bc[i].block<3, 1>(0, 7); // 3x8
}
K_red *= volume / 4.0;
Eigen::MatrixXd K(8, 8);
Eigen::MatrixXd Kc(8, 8);
Eigen::MatrixXd Kc2(8, 8);
Kc = K_red + volume*B_0T*C*B_0T.transpose(); // 8x8 + 8x3 * 3x3 * 3x8
Kc2 << Kc.block<8, 1>(0, 0), Kc.block<8, 1>(0, 4), Kc.block<8, 1>(0, 1), Kc.block<8, 1>(0, 5), Kc.block<8, 1>(0, 2), Kc.block<8, 1>(0, 6), Kc.block<8, 1>(0, 3), Kc.block<8, 1>(0, 7);
K << Kc2.block<1, 8>(0, 0), Kc2.block<1, 8>(4, 0), Kc2.block<1, 8>(1, 0), Kc2.block<1, 8>(5, 0), Kc2.block<1, 8>(2, 0), Kc2.block<1, 8>(6, 0), Kc2.block<1, 8>(3, 0), Kc2.block<1, 8>(7, 0);
// Don't forget to swap rows and/or columns of B and K matrix and store it in the object.
K_loc = K;
B_matrices = B;
}
K gc(C x) {K z=newK(3,1); Kc(z)=x; R z;}
int
main(int argc, char** argv)
{
F pi = atan(1.0)*4;
K a = gi(2);
K b = gi(3);
K c = gi(4);
K* v;
cd(ksk("",0));
tst(Ki(a)==2);
tst(Ki(b) + 1 == Ki(c));
cd(a); cd(b); cd(c);
b = gf(1.0); c = gf(2);
tst(Kf(b) + 1 == Kf(c));
cd(b); cd(c);
a = gs(sp("foo"));
b = ksk("`foo", 0);
tst(Ks(a) == Ks(b));
cd(a); cd(b);
a = ksk("2 + 3", 0);
tst(Ki(a) == 5);
cd(a);
a = ksk("_ci 65", 0);
tst(Kc(a) == 'A');
// XXX this should return type 1 uniform vector
a=gnk(3,gi(11),gi(22),gi(33));
tst(a->t == 0);
v = (K*)a->k;
tst(Ki(v[0])+Ki(v[1])==Ki(v[2]));
cd(a);
{
b = gsk("pi",gf(pi));
kap(&KTREE, &b);
a = X(".pi");
tst(Kf(a) == pi);
cd(a);
}
{
K dir = gtn(5,0);
K t;
t = gsk("x",gi(1)); kap(&dir, &t);
t = gsk("y",gi(2)); kap(&dir, &t);
t = gsk("z",dir); kap(&KTREE, &t);
a = X(".z.x");
tst(Ki(a) == 1);
cd(a);
a = X(".z.y");
tst(Ki(a) == 2);
cd(a);
}
{
I i;
K d = gtn(5,0);
K c0 = gtn(0,0);
K c1 = gtn(-1,0);
K t0, t1, e;
t0 = gsk("a", c0); kap(&d,&t0);
t1 = gsk("b", c1); kap(&d,&t1);
e = gp("hello1"); kap(&c0,&e);
e = gp("hello2"); kap(&c0,&e);
KK(KK(d)[0])[1] = c0;
i = 1; kap(&KK(KK(d)[1])[1], &i);
i = 2; kap(&KK(KK(d)[1])[1], &i);
//i = 1; kap(&c1, &i);
//i = 2; kap(&c1, &i);
//KK(KK(d)[1])[1] = c1;
show(d);
}
//b = ksk("+/", a);
//tst(Ki(b) == 66);
//argc--;argv++;
//DO(i, argc, {a=ksk(argv[i], 0);
//ksk("`0:,/$!10;`0:,\"\n\"", 0);
fprintf(stderr, "Pass:%4d, fail:%4d\n", pass, fail);
if (argc > 1 && strcmp(argv[1], "-i") == 0) {
boilerplate();
attend();
}
}
