void Elements::recount(int time)
{
vector<double> Temp(numofnods);
double kxx=MATERIAL_COND;
double kyy=MATERIAL_COND;
double h=MATERIAL_EXCH;
Matrix<double> K(numofnods);
for(int i=0;i<numofnods;i++)
K[i][i]=0;
Row<double> F(numofnods);
Row<double> Fi(3);
Matrix<double> Ki(3);
for(unsigned int i=0;i<Set.size();i++)
{
double A=Set[i].square();
Ki=kxx/4/A*Set[i].B()+kyy/4/A*Set[i].C();
Fi=Set[i].Q(time);
for(int s=0;s<3;s++)
{
F[Data[i].index(s)]+=Fi[s];
for(int p=0;p<3;p++)
K[Data[i].index(s)][Data[i].index(p)]+=Ki[s][p];
}
}
for(unsigned int i=0;i<Conditions.size();i++)
{
Ki=Conditions[i].L()*h;
Fi=Conditions[i].R();
if(Conditions[i].type()) Fi*=h;
for(int s=0;s<2;s++)
{
F[Conditions[i].index(s)]+=Fi[s];
for(int p=0;p<2;p++)
K[Conditions[i].index(s)][Conditions[i].index(p)]+=Ki[s][p];
}
}
int diag=(min(NUMOFCOLS,NUMOFROWS)+1)*2+1;
DiagonalSystem<double> Solution(K,F,diag);
Solution.Gauss();
Temp=Solution.solutuion().ToVector();
if(time==0)
{
ofstream file;
file.open("matrixoutput.txt");
file<<K<<"Free vector:\n"<<F;
file<<"Solution:\n";
file<<Solution.solutuion();
file.close();
/*DWORD thID;
DSystem TempSystem(K,F);
CreateThread(NULL, NULL, FileWriter, &TempSystem, NULL, &thID);*/
}
for(unsigned int i=0;i<Set.size();i++)
{
Set[i].settemp(Temp[Data[i].i()],Temp[Data[i].k()],Temp[Data[i].j()]);
Set[i].recounttemp();
}
}
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_Ki(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 == 1) {
int i = Ki(kobj);
return Py_BuildValue("i", i);
}
fail:
PyErr_BadArgument();
return NULL;
}
static PyObject*
_set_Ki(PyObject* self, PyObject* args)
{
PyObject* ko;
int i;
if (PyArg_ParseTuple(args, "O!i", &_KType, &ko, &i)) {
K k = ((_K*)ko)->kobj;
if (k && k->t == 1) {
Ki(k) = i;
Py_INCREF(ko);
return ko;
} else {
PyErr_SetString(PyExc_TypeError, "wrong k type");
return NULL;
}
}
PyErr_BadArgument();
return NULL;
}
Localizer::Localizer(exchangeData *_commData, const unsigned int _period) :
RateThread(_period), commData(_commData), period(_period)
{
iCubHeadCenter eyeC(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// remove constraints on the links
// we use the chains for logging purpose
eyeL->setAllConstraints(false);
eyeR->setAllConstraints(false);
// release links
eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
}
// get the absolute reference frame of the head
Vector q(eyeC.getDOF(),0.0);
eyeCAbsFrame=eyeC.getH(q);
// ... and its inverse
invEyeCAbsFrame=SE3inv(eyeCAbsFrame);
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
bool ret;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,true))
{
delete PrjL;
PrjL=Prj;
}
}
if (ret)
{
cxl=(*PrjL)(0,2);
cyl=(*PrjL)(1,2);
invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
}
else
PrjL=invPrjL=NULL;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,true))
{
delete PrjR;
PrjR=Prj;
}
}
if (ret)
{
cxr=(*PrjR)(0,2);
cyr=(*PrjR)(1,2);
invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
}
else
PrjR=invPrjR=NULL;
Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
Vector Wp(1,1.0), Wi(1,1.0), Wd(1,1.0);
Vector N(1,10.0), Tt(1,1.0);
Matrix satLim(1,2);
satLim(0,0)=0.05;
satLim(0,1)=10.0;
pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);
Vector z0(1,0.5);
pid->reset(z0);
dominantEye="left";
port_xd=NULL;
}
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;
}
Foam::tmp<Foam::surfaceScalarField> Foam::virtualMassModel::Kf() const
{
return
fvc::interpolate(pair_.dispersed())*fvc::interpolate(Ki());
}
Foam::tmp<Foam::volScalarField> Foam::virtualMassModel::K() const
{
return pair_.dispersed()*Ki();
}
I dj(I j) { I d; K a = gi(j), r = _jd(a); cd(a); d = Ki(r); cd(r); R d; }
I jd(I d) { I j; K a = gi(d), r = _jd(a); cd(a); j = Ki(r); cd(r); R j; }
K gi(I x) {K z=newK(1,1); Ki(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();
}
}
