int QilexDoc::doc_new_kinematic_hand(ct_new_kinematic_chain *data)
{
int error = 0;
int tipus = 0;
void * buffer ; //char *buffer;
char *buftemp = (char*)malloc(1024);
SoOutput out;
size_t sizeModel = 0;
SoSeparator *kinechain = new SoSeparator;
SoSeparator *kinetest = new SoSeparator;
Rchain_hand *kineengine = new Rchain_hand();
SoTransform *pos_rot = new SoTransform;
SbVec3f joinax;
joinax.setValue(SbVec3f(data->x,data->y,data->z));
pos_rot->translation.setValue(joinax);
pos_rot->rotation.setValue(SbVec3f(data->axeX, data->axeY, data->axeZ), (float) rad((double) data->angle));
kinechain = readFile(data->QsModelFile.latin1(), tipus);
if (kinechain == NULL) // no object read
{ return 1; }
else // ok, there's no object with the same name
{
error = kineengine->init_dat(data->QsDatFile.latin1()); //
if (error == 0)
{
kinechain->ref();
kinetest = (SoSeparator*)SoNode::getByName(data->QsName.latin1());
if (kinetest==NULL)
{
//we need to put it in a buffer to write the xml file
// if is Ok
SoOutput out;
out.setBuffer(buftemp, 1024, reallocCB);
SoWriteAction wa1(&out);
wa1.apply(kinechain);
out.getBuffer(buffer, sizeModel);
kinechain->insertChild(pos_rot, 0);
}
error = doc_insert_kinematic_hand(kineengine, kinechain);
}
}
if (error==0)
{
writeXML_kineelement((char *)buffer, sizeModel, tipus, data, kineengine);
}
return error;
}
void ECMult(GroupElemJac &out, const GroupElemJac &a, const Number &an, const Number &gn) {
Number an1, an2;
Number gn1, gn2;
SplitExp(an, an1, an2);
// printf("an=%s\n", an.ToString().c_str());
// printf("an1=%s\n", an1.ToString().c_str());
// printf("an2=%s\n", an2.ToString().c_str());
// printf("an1.len=%i\n", an1.GetBits());
// printf("an2.len=%i\n", an2.GetBits());
gn.SplitInto(128, gn1, gn2);
WNAF<128> wa1(an1, WINDOW_A);
WNAF<128> wa2(an2, WINDOW_A);
WNAF<128> wg1(gn1, WINDOW_G);
WNAF<128> wg2(gn2, WINDOW_G);
GroupElemJac a2; a2.SetMulLambda(a);
WNAFPrecomp<GroupElemJac,WINDOW_A> wpa1(a);
WNAFPrecomp<GroupElemJac,WINDOW_A> wpa2(a2);
const ECMultConsts &c = GetECMultConsts();
int size_a1 = wa1.GetSize();
int size_a2 = wa2.GetSize();
int size_g1 = wg1.GetSize();
int size_g2 = wg2.GetSize();
int size = std::max(std::max(size_a1, size_a2), std::max(size_g1, size_g2));
out = GroupElemJac();
GroupElemJac tmpj;
GroupElem tmpa;
for (int i=size-1; i>=0; i--) {
out.SetDouble(out);
int nw;
if (i < size_a1 && (nw = wa1.Get(i))) {
wpa1.Get(tmpj, nw);
out.SetAdd(out, tmpj);
}
if (i < size_a2 && (nw = wa2.Get(i))) {
wpa2.Get(tmpj, nw);
out.SetAdd(out, tmpj);
}
if (i < size_g1 && (nw = wg1.Get(i))) {
c.wpg.Get(tmpa, nw);
out.SetAdd(out, tmpa);
}
if (i < size_g2 && (nw = wg2.Get(i))) {
c.wpg128.Get(tmpa, nw);
out.SetAdd(out, tmpa);
}
}
}
EndCriteria::Type LevenbergMarquardt::minimize(Problem& P,
const EndCriteria& endCriteria) {
EndCriteria::Type ecType = EndCriteria::None;
P.reset();
Array x_ = P.currentValue();
currentProblem_ = &P;
initCostValues_ = P.costFunction().values(x_);
int m = initCostValues_.size();
int n = x_.size();
boost::scoped_array<double> xx(new double[n]);
std::copy(x_.begin(), x_.end(), xx.get());
boost::scoped_array<double> fvec(new double[m]);
boost::scoped_array<double> diag(new double[n]);
int mode = 1;
double factor = 1;
int nprint = 0;
int info = 0;
int nfev =0;
boost::scoped_array<double> fjac(new double[m*n]);
int ldfjac = m;
boost::scoped_array<int> ipvt(new int[n]);
boost::scoped_array<double> qtf(new double[n]);
boost::scoped_array<double> wa1(new double[n]);
boost::scoped_array<double> wa2(new double[n]);
boost::scoped_array<double> wa3(new double[n]);
boost::scoped_array<double> wa4(new double[m]);
// requirements; check here to get more detailed error messages.
QL_REQUIRE(n > 0, "no variables given");
QL_REQUIRE(m >= n,
"less functions (" << m <<
") than available variables (" << n << ")");
QL_REQUIRE(endCriteria.functionEpsilon() >= 0.0,
"negative f tolerance");
QL_REQUIRE(xtol_ >= 0.0, "negative x tolerance");
QL_REQUIRE(gtol_ >= 0.0, "negative g tolerance");
QL_REQUIRE(endCriteria.maxIterations() > 0,
"null number of evaluations");
// call lmdif to minimize the sum of the squares of m functions
// in n variables by the Levenberg-Marquardt algorithm.
MINPACK::LmdifCostFunction lmdifCostFunction =
boost::bind(&LevenbergMarquardt::fcn, this, _1, _2, _3, _4, _5);
MINPACK::lmdif(m, n, xx.get(), fvec.get(),
static_cast<double>(endCriteria.functionEpsilon()),
static_cast<double>(xtol_),
static_cast<double>(gtol_),
static_cast<int>(endCriteria.maxIterations()),
static_cast<double>(epsfcn_),
diag.get(), mode, factor,
nprint, &info, &nfev, fjac.get(),
ldfjac, ipvt.get(), qtf.get(),
wa1.get(), wa2.get(), wa3.get(), wa4.get(),
lmdifCostFunction);
info_ = info;
// check requirements & endCriteria evaluation
QL_REQUIRE(info != 0, "MINPACK: improper input parameters");
//QL_REQUIRE(info != 6, "MINPACK: ftol is too small. no further "
// "reduction in the sum of squares "
// "is possible.");
if (info != 6) ecType = QuantLib::EndCriteria::StationaryFunctionValue;
//QL_REQUIRE(info != 5, "MINPACK: number of calls to fcn has "
// "reached or exceeded maxfev.");
endCriteria.checkMaxIterations(nfev, ecType);
QL_REQUIRE(info != 7, "MINPACK: xtol is too small. no further "
"improvement in the approximate "
"solution x is possible.");
QL_REQUIRE(info != 8, "MINPACK: gtol is too small. fvec is "
"orthogonal to the columns of the "
"jacobian to machine precision.");
// set problem
std::copy(xx.get(), xx.get()+n, x_.begin());
P.setCurrentValue(x_);
P.setFunctionValue(P.costFunction().value(x_));
return ecType;
}
int QilexDoc::doc_new_grasping_object(ct_new_grasping_object *data)
{
int error = 0;
int tipus = 0;
void *buffer; //char *buffer;
char *buftemp = (char*)malloc(1024);
size_t sizeModel = 0;;
SoSeparator *object = new SoSeparator;
SoSeparator *objecttest = new SoSeparator;
SoTransform *pos_rot = new SoTransform;
SbVec3f joinax;
SbVec3f joingrasp0;
SbVec3f joingrasp1;
SbVec3f joingrasp2;
SbVec3f joingrasp3;
joinax.setValue(SbVec3f(data->x,data->y,data->z));
pos_rot->translation.setValue(joinax);
pos_rot->recenter(joinax);
pos_rot->rotation.setValue(SbVec3f(data->axeX, data->axeY, data->axeZ), (float) rad((double) data->angle));
object = readFile(data->QsModelFile.latin1(), tipus);
if (object == NULL) // no object read
{
error = 1 ;
}
else // ok, there's no object with the same name
{
error = read_grasp_points(data);
SoMaterial *bronze = new SoMaterial;
bronze->ambientColor.setValue(0.33,0.22,0.27);
bronze->diffuseColor.setValue(0.78,0.57,0.11);
bronze->specularColor.setValue(0.99,0.94,0.81);
bronze->shininess=0.28;
SoSphere *grasp_sphere = new SoSphere;
grasp_sphere->radius=7.0;
SoFont *font = new SoFont;
font->size.setValue(28);
font->name.setValue("Times-Roman");
SoSeparator *grasp_sep0 = new SoSeparator;
SoTransform *grasp_transf0 = new SoTransform;
SoSeparator *text0 = new SoSeparator;
SoText2 *label_text0 = new SoText2;
SoSeparator *grasp_sep1 = new SoSeparator;
SoTransform *grasp_transf1 = new SoTransform;
SoSeparator *text1 = new SoSeparator;
SoText2 *label_text1 = new SoText2;
SoSeparator *grasp_sep2 = new SoSeparator;
SoTransform *grasp_transf2 = new SoTransform;
SoSeparator *text2 = new SoSeparator;
SoText2 *label_text2 = new SoText2;
SoSeparator *grasp_sep3 = new SoSeparator;
SoTransform *grasp_transf3 = new SoTransform;
SoSeparator *text3 = new SoSeparator;
SoText2 *label_text3 = new SoText2;
//for (int i=0;i<data->num_point;i++)
//{
joingrasp0.setValue(SbVec3f(data->grasp_points[0].px,data->grasp_points[0].py,data->grasp_points[0].pz));
grasp_transf0->translation.setValue(joingrasp0);
grasp_transf0->recenter(joingrasp0);
label_text0->string=" 1";
text0->addChild(font);
text0->addChild(label_text0);
grasp_sep0->addChild(bronze);
grasp_sep0->addChild(grasp_transf0);
grasp_sep0->addChild(grasp_sphere);
grasp_sep0->addChild(text0);
//grasp_sep0->addChild(line0);
joingrasp1.setValue(SbVec3f(data->grasp_points[1].px,data->grasp_points[1].py,data->grasp_points[1].pz));
grasp_transf1->translation.setValue(joingrasp1);
grasp_transf1->recenter(joingrasp1);
label_text1->string=" 2";
text1->addChild(font);
text1->addChild(label_text1);
grasp_sep1->addChild(bronze);
grasp_sep1->addChild(grasp_transf1);
grasp_sep1->addChild(grasp_sphere);
grasp_sep1->addChild(text1);
joingrasp2.setValue(SbVec3f(data->grasp_points[2].px,data->grasp_points[2].py,data->grasp_points[2].pz));
grasp_transf2->translation.setValue(joingrasp2);
grasp_transf2->recenter(joingrasp2);
label_text2->string=" 3";
text2->addChild(font);
text2->addChild(label_text2);
grasp_sep2->addChild(bronze);
grasp_sep2->addChild(grasp_transf2);
grasp_sep2->addChild(grasp_sphere);
grasp_sep2->addChild(text2);
joingrasp3.setValue(SbVec3f(data->grasp_points[3].px,data->grasp_points[3].py,data->grasp_points[3].pz));
grasp_transf3->translation.setValue(joingrasp3);
grasp_transf3->recenter(joingrasp3);
label_text3->string=" 4";
text3->addChild(font);
text3->addChild(label_text3);
grasp_sep3->addChild(bronze);
grasp_sep3->addChild(grasp_transf3);
grasp_sep3->addChild(grasp_sphere);
grasp_sep3->addChild(text3);
//object->addChild(grasp_sep);
//}
if (error == 0)
{
object->ref();
objecttest = (SoSeparator*)SoNode::getByName(data->QsName.latin1());
if (objecttest==NULL)
{
SoOutput out;
out.setBuffer(buftemp, 1024, reallocCB);
SoWriteAction wa1(&out);
wa1.apply(object);
out.getBuffer(buffer, sizeModel);
object->setName(data->QsName.latin1());
//grasp_object->addChild(model_object);
object->addChild(grasp_sep0);
object->addChild(grasp_sep1);
object->addChild(grasp_sep2);
object->addChild(grasp_sep3);
object->insertChild(pos_rot, 0);
view->addObjectCell(object);
error = 0;
//writeXML_geomelement((char *)buffer, sizeModel, tipus, data);
//S'ha de canviar!!!!!
}
else
{
object->unref();
error = 3;
}
}
}
return error;
}
int QilexDoc::doc_new_geometric_object(ct_new_geometric_object *data)
{
int error = 0;
int tipus = 0;
void *buffer; //char *buffer;
char *buftemp = (char*)malloc(1024);
size_t sizeModel = 0;;
SoSeparator *object = new SoSeparator;
SoSeparator *objecttest = new SoSeparator;
SoTransform *pos_rot = new SoTransform;
SbVec3f joinax;
joinax.setValue(SbVec3f(data->x,data->y,data->z));
pos_rot->translation.setValue(joinax);
pos_rot->recenter(joinax);
pos_rot->rotation.setValue(SbVec3f(data->axeX, data->axeY, data->axeZ), (float) rad((double) data->angle));
object = readFile(data->QsModelFile.latin1(), tipus);
if (object == NULL) // no object read
{
error = 1 ;
}
else // ok, there's no object with the same name
{
object->ref();
objecttest = (SoSeparator*)SoNode::getByName(data->QsName.latin1());
if (objecttest==NULL)
{
SoOutput out;
out.setBuffer(buftemp, 1024, reallocCB);
SoWriteAction wa1(&out);
wa1.apply(object);
out.getBuffer(buffer, sizeModel);
object->setName(data->QsName.latin1());
object->insertChild(pos_rot, 0);
view->addObjectCell(object);
error = 0;
writeXML_geomelement((char *)buffer, sizeModel, tipus, data);
}
else
{
object->unref();
error = 3;
}
}
return error;
}
