void Try(int a)
{
if (d>2)
{
if (a==0)
{
if (kt(b[0]+b[1])==0 && kt(b[1]+b[2])==0)
{
for (int i=0;i<=2;i++) cout<<b[i]<<"+";
cout<<endl;
l[m++]=b[0]+b[1];
l[m++]=b[1]+b[2];
}
}
}
else
{
for (int i=2;i<=a;i++)
if (nt(i)==1)
{
b[d]=i;
d++;
Try(a-i);
d--;
}
}
}
double get_einstein_diffusion_coefficient(double r) {
MillipascalSecond e = eta(IMP::internal::DEFAULT_TEMPERATURE);
unit::SquareAngstromPerFemtosecond ret(
kt(IMP::internal::DEFAULT_TEMPERATURE) /
(6.0 * PI * e * unit::Angstrom(r)));
return ret.get_value();
}
void LayoutEngine::adjustGlyphPositions(const LEUnicode chars[], le_int32 offset, le_int32 count, le_bool reverse,
LEGlyphStorage &glyphStorage, LEErrorCode &success)
{
if (LE_FAILURE(success)) {
return;
}
if (chars == NULL || offset < 0 || count < 0) {
success = LE_ILLEGAL_ARGUMENT_ERROR;
return;
}
LEReferenceTo<GlyphDefinitionTableHeader> gdefTable((GlyphDefinitionTableHeader *) CanonShaping::glyphDefinitionTable,
CanonShaping::glyphDefinitionTableLen);
CanonMarkFilter filter(gdefTable, success);
adjustMarkGlyphs(&chars[offset], count, reverse, glyphStorage, &filter, success);
if (fTypoFlags & LE_Kerning_FEATURE_FLAG) { /* kerning enabled */
LETableReference kernTable(fFontInstance, LE_KERN_TABLE_TAG, success);
KernTable kt(kernTable, success);
kt.process(glyphStorage, success);
}
// default is no adjustments
return;
}
void LayoutEngine::adjustGlyphPositions(const LEUnicode chars[], le_int32 offset, le_int32 count, le_bool reverse,
LEGlyphStorage &glyphStorage, LEErrorCode &success)
{
if (LE_FAILURE(success)) {
return;
}
if (chars == NULL || offset < 0 || count < 0) {
success = LE_ILLEGAL_ARGUMENT_ERROR;
return;
}
GlyphDefinitionTableHeader *gdefTable = (GlyphDefinitionTableHeader *) CanonShaping::glyphDefinitionTable;
CanonMarkFilter filter(gdefTable);
adjustMarkGlyphs(&chars[offset], count, reverse, glyphStorage, &filter, success);
if (fTypoFlags & 0x1) { /* kerning enabled */
static const le_uint32 kernTableTag = LE_KERN_TABLE_TAG;
KernTable kt(fFontInstance, getFontTable(kernTableTag));
kt.process(glyphStorage);
}
// default is no adjustments
return;
}
double get_einstein_rotational_diffusion_coefficient(double r) {
MillipascalSecond e=eta(IMP::internal::DEFAULT_TEMPERATURE);
//double kt= get_kt(IMP::internal::DEFAULT_TEMPERATURE);
unit::PerFemtosecond ret=kt(IMP::internal::DEFAULT_TEMPERATURE)
/(8*PI*e*square(unit::Angstrom(r))*unit::Angstrom(r));
return ret.get_value();
}
void lfmxsp(smpar *sp, mxArray *mcell, int d)
{ double *alpha;
char str[16];
alpha = mxGetPr(mxGetField(mcell,0,"alpha"));
nn(sp) = alpha[0];
fixh(sp)= alpha[1];
pen(sp) = alpha[2];
mxGetString(mxGetField(mcell,0,"adaptive_criterion"),str,16);
acri(sp) = lfacri(str);
deg(sp) = mxGetPr(mxGetField(mcell,0,"degree"))[0];
deg0(sp) = -1;
mxGetString(mxGetField(mcell,0,"family"),str,16);
fam(sp) = lffamily(str);
mxGetString(mxGetField(mcell,0,"link"),str,16);
link(sp) = lflink(str);
setfamily(sp);
mxGetString(mxGetField(mcell,0,"kernel"),str,16);
ker(sp) = lfkernel(str);
mxGetString(mxGetField(mcell,0,"kernel_type"),str,16);
kt(sp) = lfketype(str);
npar(sp) = calcp(sp,d);
de_renorm = (int)(mxGetPr(mxGetField(mcell,0,"deren"))[0]);
mxGetString(mxGetField(mcell,0,"deit"),str,16);
de_itype = deitype(str);
de_mint = (int)(mxGetPr(mxGetField(mcell,0,"demint"))[0]);
lf_debug = (int)(mxGetPr(mxGetField(mcell,0,"debug"))[0]);
}
void XTable::fftwMeasure() const
{
// Make a new copy of data array since measurement will overwrite:
// Copy data into new array to avoid NaN's, etc., but not bothering
// with scaling, etc.
FFTW_Array<double> t_array = _array;
KTable kt( _N, 2*M_PI/(_N*_dx) );
fftw_plan plan = fftw_plan_dft_r2c_2d(
_N,_N, t_array.get_fftw(), kt._array.get_fftw(), FFTW_MEASURE);
#ifdef FFT_DEBUG
if (plan==NULL) throw FFTInvalid();
#endif
fftw_destroy_plan(plan);
}
// This is the same as LayoutEngline::adjustGlyphPositions() except that it doesn't call adjustMarkGlyphs
void ThaiLayoutEngine::adjustGlyphPositions(const LEUnicode chars[], le_int32 offset, le_int32 count, le_bool /*reverse*/,
LEGlyphStorage &glyphStorage, LEErrorCode &success)
{
if (LE_FAILURE(success)) {
return;
}
if (chars == NULL || offset < 0 || count < 0) {
success = LE_ILLEGAL_ARGUMENT_ERROR;
return;
}
if (fTypoFlags & LE_Kerning_FEATURE_FLAG) { /* kerning enabled */
LETableReference kernTable(fFontInstance, LE_KERN_TABLE_TAG, success);
KernTable kt(kernTable, success);
kt.process(glyphStorage, success);
}
// default is no adjustments
return;
}
void LayoutEngine::adjustGlyphPositions(const LEUnicode chars[], le_int32 offset, le_int32 count, le_bool /*reverse*/,
LEGlyphStorage &glyphStorage, LEErrorCode &success)
{
if (LE_FAILURE(success)) {
return;
}
if (chars == NULL || offset < 0 || count < 0) {
success = LE_ILLEGAL_ARGUMENT_ERROR;
return;
}
if (fTypoFlags & 0x1) { /* kerning enabled */
static const le_uint32 kernTableTag = LE_KERN_TABLE_TAG;
KernTable kt(fFontInstance, getFontTable(kernTableTag));
kt.process(glyphStorage);
}
// default is no adjustments
return;
}
int main()
{
std::string icub_urdf_filename = "icub_model.urdf";
// Create the iCub model
KDL::Tree icub_tree;
bool ok = iDynTree::treeFromUrdfFile(icub_urdf_filename,icub_tree);
if( !ok )
{
std::cerr <<"Could not import icub urdf" << std::endl;
return EXIT_FAILURE;
}
// Create the iCub undirected tree, that contains also
// a default serialization (i.e. a mapping between links/joints
// and integers (if you want to provide a user-defined serialization
// to the undirected tree, pass it as a second argument to the constructor
KDL::CoDyCo::UndirectedTree icub_undirected_tree(icub_tree);
std::cout << "icub_tree serialization 1 : " << icub_undirected_tree.getSerialization().toString();
// Load a sensors tree (for ft sensors) from the information extracted from urdf file
// and using the serialization provided in the undirected tree
iDynTree::SensorsList sensors_tree = iDynTree::sensorsListFromURDF(icub_undirected_tree,icub_urdf_filename);
//Create a regressor generator
KDL::CoDyCo::Regressors::DynamicRegressorGenerator regressor_generator(icub_undirected_tree,sensors_tree);
//Add a set of rows of the regressor of right arm
std::vector<std::string> l_arm_subtree;
l_arm_subtree.push_back("l_upper_arm");
regressor_generator.addSubtreeRegressorRows(l_arm_subtree);
//Create a random state for the robot
KDL::Twist base_velocity, base_acceleration;
base_velocity = base_acceleration = KDL::Twist::Zero();
KDL::JntArray q(regressor_generator.getNrOfDOFs());
SetToZero(q);
srand(time(0));
for(int i=0; i < q.rows(); i++ )
{
q(i) = random_double();
}
double gravity_norm = 9.8;
base_acceleration.vel[2] = -gravity_norm;
regressor_generator.setRobotState(q,q,q,base_velocity,base_acceleration);
// Estimate sensor measurements from the model
iDynTree::Wrench simulate_measurement = simulateFTSensorFromKinematicState(icub_undirected_tree,
q,q,q,base_velocity,base_acceleration,"l_arm_ft_sensor",sensors_tree);
//Create a regressor and check the returned sensor value
Eigen::MatrixXd regressor(regressor_generator.getNrOfOutputs(),regressor_generator.getNrOfParameters());
Eigen::VectorXd kt(regressor_generator.getNrOfOutputs());
regressor_generator.computeRegressor(regressor,kt);
//std::cout << "regressors : " << regressor << std::endl;
Eigen::VectorXd parameters(regressor_generator.getNrOfParameters());
parameters.setZero();
regressor_generator.getModelParameters(parameters);
assert(parameters.rows() == regressor_generator.getNrOfParameters());
Eigen::Matrix<double,6,1> sens = regressor*parameters;
////////////////////////////////////////////////////////////
///// Test also the new iDynTree regressor infrastructure
////////////////////////////////////////////////////////////
iDynTree::Regressors::DynamicsRegressorGenerator new_generator;
// load robot and sensor model
ok = ok && new_generator.loadRobotAndSensorsModelFromFile(icub_urdf_filename);
assert(ok);
// load regressor structure (this should be actually loaded from file)
std::string regressor_structure
= "<regressor> "
" <subtreeBaseDynamics> "
" <FTSensorLink>l_upper_arm</FTSensorLink> "
" </subtreeBaseDynamics> "
"</regressor>";
ok = ok && new_generator.loadRegressorStructureFromString(regressor_structure);
assert(ok);
iDynTree::VectorDynSize q_idyntree(q.rows());
ok = ok && iDynTree::ToiDynTree(q,q_idyntree);
assert(ok);
iDynTree::Twist gravity;
gravity(2) = gravity_norm;
ok = ok && new_generator.setRobotState(q_idyntree,q_idyntree,q_idyntree,gravity);
assert(ok);
iDynTree::MatrixDynSize regr_idyntree(new_generator.getNrOfOutputs(),new_generator.getNrOfParameters());
iDynTree::VectorDynSize kt_idyntree(new_generator.getNrOfOutputs());
iDynTree::VectorDynSize param_idyntree(new_generator.getNrOfParameters());
ok = ok && new_generator.getModelParameters(param_idyntree);
int sensorIndex = new_generator.getSensorsModel().getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE,"l_arm_ft_sensor");
ok = ok && new_generator.getSensorsMeasurements().setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,sensorIndex,simulate_measurement);
ok = ok && new_generator.computeRegressor(regr_idyntree,kt_idyntree);
Eigen::Matrix<double,6,1> sens_idyntree = Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(regr_idyntree.data(),regr_idyntree.rows(),regr_idyntree.cols())*
Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size());
Eigen::Matrix<double,6,1> kt_eigen = Eigen::Map< Eigen::Matrix<double,6,1> >(kt_idyntree.data(),kt_idyntree.size());
std::cout << "Parameters norm old " << parameters.norm() << std::endl;
std::cout << "Parameters norm new " << Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size()).norm() << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters: " << sens << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters (new): " << sens_idyntree << std::endl;
std::cout << "Sensor measurement from RNEA: " << KDL::CoDyCo::toEigen( iDynTree::ToKDL(simulate_measurement)) << std::endl;
std::cout << "Sensor measurement from known terms (new) " << kt_eigen << std::endl;
double tol = 1e-5;
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens_idyntree).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+kt_eigen).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
int main(int argc, char **argv)
{
if (argc < 2) {
std::cout << "usage: knn <pts> [queries] [nn] [epsilon]" << std::endl;
exit(1);
}
int pt_count, dim;
ANNpointArray pts = read_points(argv[1], pt_count, dim);
ANNkd_tree kt(pts, pt_count, dim);
if (argc < 3) return 1;
//read queries
int q_count;
int q_dim;
ANNpointArray queries = read_points(argv[2], q_count, q_dim);
if (dim != q_dim) {
std::cerr << "error: query dim: " << q_dim;
std::cerr << " does not match point dim: " << dim << std::endl;
exit(1);
}
//how many nearest neighbours to retrieve
int nn = 5;
if (argc >= 4) nn = atoi(argv[3]);
//read query epsilon
double epsilon = 0.0;
if (argc == 5) epsilon = atof(argv[4]);
//run queries
ANNidx *nn_idx = new ANNidx[nn];
ANNdist *dists = new ANNdist[nn];
for (int i = 0; i < q_count; ++i) {
kt.annkSearch(queries[i], nn, nn_idx, dists, epsilon);
std::cout << "query " << i << ": (";
for (int d = 0; d < dim; ++d) {
std::cout << queries[i][d];
if (d + 1 < dim) std::cout << ", ";
}
std::cout << ")\n";
for (int j = 0; j < nn; ++j) {
std::cout << "(";
for (int d = 0; d < dim; ++d) {
std::cout << pts[nn_idx[j]][d];
if (d + 1 < dim) std::cout << ", ";
}
std::cout << ") " << dists[j] << "\n";
}
}
std::cout << "done." << std::endl;
delete[] nn_idx;
delete[] dists;
annClose();
return 0;
}
// Same thing, but return a new KTable
boost::shared_ptr<KTable> XTable::transform() const
{
boost::shared_ptr<KTable> kt(new KTable( _N, 2*M_PI/(_N*_dx) ));
transform(*kt);
return kt;
}
// apply GPOS table, if any
void OpenTypeLayoutEngine::adjustGlyphPositions(const LEUnicode chars[], le_int32 offset, le_int32 count, le_bool reverse,
LEGlyphStorage &glyphStorage, LEErrorCode &success)
{
if (LE_FAILURE(success)) {
return;
}
if (chars == NULL || offset < 0 || count < 0) {
success = LE_ILLEGAL_ARGUMENT_ERROR;
return;
}
le_int32 glyphCount = glyphStorage.getGlyphCount();
if (glyphCount == 0) {
return;
}
if (!fGPOSTable.isEmpty()) {
GlyphPositionAdjustments *adjustments = new GlyphPositionAdjustments(glyphCount);
le_int32 i;
if (adjustments == NULL) {
success = LE_MEMORY_ALLOCATION_ERROR;
return;
}
#if 0
// Don't need to do this if we allocate
// the adjustments array w/ new...
for (i = 0; i < glyphCount; i += 1) {
adjustments->setXPlacement(i, 0);
adjustments->setYPlacement(i, 0);
adjustments->setXAdvance(i, 0);
adjustments->setYAdvance(i, 0);
adjustments->setBaseOffset(i, -1);
}
#endif
if (!fGPOSTable.isEmpty()) {
if (fScriptTagV2 != nullScriptTag &&
fGPOSTable->coversScriptAndLanguage(fGPOSTable, fScriptTagV2,fLangSysTag,success)) {
fGPOSTable->process(fGPOSTable, glyphStorage, adjustments, reverse, fScriptTagV2, fLangSysTag,
fGDEFTable, success, fFontInstance, fFeatureMap, fFeatureMapCount, fFeatureOrder);
} else {
fGPOSTable->process(fGPOSTable, glyphStorage, adjustments, reverse, fScriptTag, fLangSysTag,
fGDEFTable, success, fFontInstance, fFeatureMap, fFeatureMapCount, fFeatureOrder);
}
} else if (fTypoFlags & LE_Kerning_FEATURE_FLAG) { /* kerning enabled */
LETableReference kernTable(fFontInstance, LE_KERN_TABLE_TAG, success);
KernTable kt(kernTable, success);
kt.process(glyphStorage, success);
}
float xAdjust = 0, yAdjust = 0;
for (i = 0; i < glyphCount; i += 1) {
float xAdvance = adjustments->getXAdvance(i);
float yAdvance = adjustments->getYAdvance(i);
float xPlacement = 0;
float yPlacement = 0;
#if 0
// This is where separate kerning adjustments
// should get applied.
xAdjust += xKerning;
yAdjust += yKerning;
#endif
for (le_int32 base = i; base >= 0; base = adjustments->getBaseOffset(base)) {
xPlacement += adjustments->getXPlacement(base);
yPlacement += adjustments->getYPlacement(base);
}
xPlacement = fFontInstance->xUnitsToPoints(xPlacement);
yPlacement = fFontInstance->yUnitsToPoints(yPlacement);
glyphStorage.adjustPosition(i, xAdjust + xPlacement, -(yAdjust + yPlacement), success);
xAdjust += fFontInstance->xUnitsToPoints(xAdvance);
yAdjust += fFontInstance->yUnitsToPoints(yAdvance);
}
glyphStorage.adjustPosition(glyphCount, xAdjust, -yAdjust, success);
delete adjustments;
} else {
// if there was no GPOS table, maybe there's non-OpenType kerning we can use
LayoutEngine::adjustGlyphPositions(chars, offset, count, reverse, glyphStorage, success);
}
LEGlyphID zwnj = fFontInstance->mapCharToGlyph(0x200C);
if (zwnj != 0x0000) {
for (le_int32 g = 0; g < glyphCount; g += 1) {
LEGlyphID glyph = glyphStorage[g];
if (glyph == zwnj) {
glyphStorage[g] = LE_SET_GLYPH(glyph, 0xFFFF);
}
}
}
#if 0
// Don't know why this is here...
LE_DELETE_ARRAY(fFeatureTags);
fFeatureTags = NULL;
#endif
}
// apply GPOS table, if any
void OpenTypeLayoutEngine::adjustGlyphPositions(const LEUnicode chars[], le_int32 offset, le_int32 count, le_bool reverse,
LEGlyphStorage &glyphStorage, LEErrorCode &success)
{
if (LE_FAILURE(success)) {
return;
}
if (chars == NULL || offset < 0 || count < 0) {
success = LE_ILLEGAL_ARGUMENT_ERROR;
return;
}
le_int32 glyphCount = glyphStorage.getGlyphCount();
if (glyphCount == 0) {
return;
}
if (fGPOSTable != NULL) {
GlyphPositionAdjustments *adjustments = new GlyphPositionAdjustments(glyphCount);
le_int32 i;
if (adjustments == NULL) {
success = LE_MEMORY_ALLOCATION_ERROR;
return;
}
#if 0
// Don't need to do this if we allocate
// the adjustments array w/ new...
for (i = 0; i < glyphCount; i += 1) {
adjustments->setXPlacement(i, 0);
adjustments->setYPlacement(i, 0);
adjustments->setXAdvance(i, 0);
adjustments->setYAdvance(i, 0);
adjustments->setBaseOffset(i, -1);
}
#endif
if (fGPOSTable != NULL) {
if (fScriptTagV2 != nullScriptTag && fGPOSTable->coversScriptAndLanguage(fScriptTagV2,fLangSysTag)) {
fGPOSTable->process(glyphStorage, adjustments, reverse, fScriptTagV2, fLangSysTag, fGDEFTable, success, fFontInstance,
fFeatureMap, fFeatureMapCount, fFeatureOrder);
} else {
fGPOSTable->process(glyphStorage, adjustments, reverse, fScriptTag, fLangSysTag, fGDEFTable, success, fFontInstance,
fFeatureMap, fFeatureMapCount, fFeatureOrder);
}
} else if ( fTypoFlags & 0x1 ) {
static const le_uint32 kernTableTag = LE_KERN_TABLE_TAG;
KernTable kt(fFontInstance, getFontTable(kernTableTag));
kt.process(glyphStorage);
}
float xAdjust = 0, yAdjust = 0;
for (i = 0; i < glyphCount; i += 1) {
float xAdvance = adjustments->getXAdvance(i);
float yAdvance = adjustments->getYAdvance(i);
float xPlacement = 0;
float yPlacement = 0;
#if 0
// This is where separate kerning adjustments
// should get applied.
xAdjust += xKerning;
yAdjust += yKerning;
#endif
for (le_int32 base = i; base >= 0; base = adjustments->getBaseOffset(base)) {
xPlacement += adjustments->getXPlacement(base);
yPlacement += adjustments->getYPlacement(base);
}
xPlacement = fFontInstance->xUnitsToPoints(xPlacement);
yPlacement = fFontInstance->yUnitsToPoints(yPlacement);
glyphStorage.adjustPosition(i, xAdjust + xPlacement, -(yAdjust + yPlacement), success);
xAdjust += fFontInstance->xUnitsToPoints(xAdvance);
yAdjust += fFontInstance->yUnitsToPoints(yAdvance);
}
glyphStorage.adjustPosition(glyphCount, xAdjust, -yAdjust, success);
delete adjustments;
} else {
// if there was no GPOS table, maybe there's non-OpenType kerning we can use
// Google Patch: disable this. Causes problems with Tamil.
// Umesh says layout is poor both with and without the change, but
// worse with the change. See ocean/imageprocessing/layout_test_unittest.cc
// Public ICU ticket for this problem is #7742
// LayoutEngine::adjustGlyphPositions(chars, offset, count, reverse, glyphStorage, success);
}
LEGlyphID zwnj = fFontInstance->mapCharToGlyph(0x200C);
if (zwnj != 0x0000) {
for (le_int32 g = 0; g < glyphCount; g += 1) {
LEGlyphID glyph = glyphStorage[g];
if (glyph == zwnj) {
glyphStorage[g] = LE_SET_GLYPH(glyph, 0xFFFF);
}
}
}
#if 0
// Don't know why this is here...
LE_DELETE_ARRAY(fFeatureTags);
fFeatureTags = NULL;
#endif
}
