double solve_1815(const Pt& p1, const Pt& p2, const Pt& p3, double w1, double w2, double w3)
{
Pt y0 = (p1 + p2 + p3) / 3.f;
Pt y = y0;
double result = computeDistances(y0, p1, p2, p3, w1, w2, w3);
double result2 = result;
do
{
y0 = y;
result = result2;
double t1 = (p1 - y).length();
double t2 = (p2 - y).length();
double t3 = (p3 - y).length();
Pt sum1 = p1 * w1 / t1
+ p2 * w2 / t2
+ p3 * w3 / t3;
double sum2 = w1 / t1
+ w2 / t2
+ w3 / t3;
y = sum1 / sum2;
result2 = computeDistances(y, p1, p2, p3, w1, w2, w3);
} while (result2 < result && result - result2 > 0.00000000001);
return result;
}
double computeHausdorffDistance(MeshType::Pointer mesh1, MeshType::Pointer mesh2, unsigned numberOfSamplingPoints) {
std::vector<double> distanceValues1 = computeDistances(mesh1, mesh2, numberOfSamplingPoints);
std::vector<double> distanceValues2 = computeDistances(mesh2, mesh1, numberOfSamplingPoints);
std::vector<double>::iterator maxElemIt1 = std::max_element(distanceValues1.begin(), distanceValues1.end());
std::vector<double>::iterator maxElemIt2 = std::max_element(distanceValues2.begin(), distanceValues2.end());
return std::max(*maxElemIt1, *maxElemIt2);
}
unsigned int arlCore::ICP::computeDistances( arlCore::PointList::sptr points )
{
unsigned int i;
std::vector<double> distances;
points->clear();
arlCore::Point::sptr point = arlCore::Point::New(3);;
if(m_point2PointMode)
{
computeDistances( distances );
#ifdef ANN
for( i=0 ; i<distances.size() ; ++i )
{
point->x(m_Pk[i][0]), point->y(m_Pk[i][1]), point->z(m_Pk[i][2]);
point->setError(distances[i]);
points->push_back(point);
}
#endif // ANN
}else
{ // Point to mesh
const arlCore::vnl_rigid_matrix InvM = m_solution.computeInverse();
assert(m_cloud && m_modelMesh);
for( i=0 ; i<m_cloud->size() ; ++i )
if(m_cloud->get(i))
if(!m_justVisible || m_cloud->get(i)->isVisible())
{
InvM.trf(m_cloud->get(i), point);
point->setError(m_modelMesh->computeDistance(point));
points->push_back(point);
}
}
return (unsigned int)points->size();
}
void Group::sortParticles()
{
computeDistances();
if (sortingEnabled)
sortParticles(0,pool.getNbActive() - 1);
}
static void computeGeometryAndDistances_proxy( const ModelHandler & model,
DataHandler & data,
const GeometryModelHandler & model_geom,
GeometryDataHandler & data_geom,
const Eigen::VectorXd & q
)
{
computeDistances(*model, *data, *model_geom, *data_geom, q);
}
void RadialLookUpTable::recompute(double scale, double maxRadius)
{
if(this->scale == scale && this->maxRadius == maxRadius)
return;
this->maxRadius = maxRadius;
this->scale = scale;
numElementsPerLine = maxRadius / scale * 2;
numElementsPerLineHalf = maxRadius / scale;
computeAngles();
computeDistances();
}
void vrpProblem::load(const char* filename) {
easer();
ifstream file(filename);
cout << ">> Loading [" << filename << "]" << endl;
double buffer;
if (file)
{
file >> _num_stops; //read the number of stops
file >> _capacity_veh; //read the capacity of vehicle
file >> _speed; //speed of vehicle
file >> _max_total_dist; //get maximum total dist of a vehicle
_list_stops = new vrpStop[_num_stops];
//read all property of stops
for (unsigned int i = 0; i < _num_stops; i++) {
file >> buffer;
_list_stops[i].ID = (unsigned int)buffer;
file >> buffer;
_list_stops[i].X = buffer;
file >> buffer;
_list_stops[i].Y = buffer;
file >> buffer;
_list_stops[i].EarlyTime = getMinutes(buffer);
file >> buffer;
_list_stops[i].LateTime = getMinutes(buffer);
file >> buffer;
_list_stops[i].ServiceTime = buffer/60;
file >> buffer;
_list_stops[i].Load = buffer;
file >> buffer;
_list_stops[i].Type = (unsigned int)buffer;
}
//get information about conflict stops
unsigned int count_group;
file >> count_group; //number of the group customer
for(unsigned int i = 0 ; i < _num_stops ; i++) {
if(_list_stops[i].Type != REGULAR_STOP)
_list_stops[i].Group = NON_GROUP;
else
_list_stops[i].Group = _list_stops[i].ID % count_group;
}
_conflict_group = new vrpGroupConflict(count_group);
unsigned int group_A, group_B;
while(file >> group_A >> group_B )
_conflict_group->pushConflictGroup(group_A, group_B);
file.close();
computeDistances();
}
else
{
closestPairEntry(R_TreeLeafEntry<2,TupleId>& e1, int _level1,
R_TreeInternalEntry<2>& e2, int _level2):
id1(0), rect1(e1.box), tid1(e1.info),level1(_level1),
id2(e2.pointer), rect2(e2.box), tid2(0), level2(_level2) {
computeDistances();
}
closestPairEntry( R_TreeInternalEntry<2>& e1, int _level1,
closestPairEntry e):
id1(e1.pointer), rect1(e1.box), tid1(0),level1(_level1),
id2(e.id2), rect2(e.rect2), tid2(e.tid2), level2(e.level2){
computeDistances();
}
closestPairEntry( R_TreeLeafEntry<2,TupleId>& e1, int _level1,
closestPairEntry e):
id1(0), rect1(e1.box), tid1(e1.info),level1(_level1),
id2(e.id2), rect2(e.rect2), tid2(e.tid2), level2(e.level2){
computeDistances();
}
closestPairEntry(closestPairEntry e,
R_TreeInternalEntry<2>& e2, int _level2):
id1(e.id1), rect1(e.rect1), tid1(e.tid1), level1(e.level1),
id2(e2.pointer), rect2(e2.box), tid2(0), level2(_level2){
computeDistances();
}
closestPairEntry(closestPairEntry e,
R_TreeLeafEntry<2,TupleId>& e2, int _level2):
id1(e.id1), rect1(e.rect1), tid1(e.tid1), level1(e.level1),
id2(0), rect2(e2.box), tid2(e2.info), level2(_level2){
computeDistances();
}
template <typename PointInT, typename PointOutT> void
pcl::BilateralUpsampling<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
output.resize (input_->size ());
float nan = std::numeric_limits<float>::quiet_NaN ();
Eigen::MatrixXf val_exp_depth_matrix;
Eigen::VectorXf val_exp_rgb_vector;
computeDistances (val_exp_depth_matrix, val_exp_rgb_vector);
for (int x = 0; x < static_cast<int> (input_->width); ++x)
for (int y = 0; y < static_cast<int> (input_->height); ++y)
{
int start_window_x = std::max (x - window_size_, 0),
start_window_y = std::max (y - window_size_, 0),
end_window_x = std::min (x + window_size_, static_cast<int> (input_->width)),
end_window_y = std::min (y + window_size_, static_cast<int> (input_->height));
float sum = 0.0f,
norm_sum = 0.0f;
for (int x_w = start_window_x; x_w < end_window_x; ++ x_w)
for (int y_w = start_window_y; y_w < end_window_y; ++ y_w)
{
float dx = float (x - x_w),
dy = float (y - y_w);
float val_exp_depth = val_exp_depth_matrix(dx+window_size_, dy+window_size_);
float d_color = static_cast<float> (
abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) +
abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) +
abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b));
float val_exp_rgb = val_exp_rgb_vector(Eigen::VectorXf::Index(d_color));
if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z))
{
sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z;
norm_sum += val_exp_depth * val_exp_rgb;
}
}
output.points[y*input_->width + x].r = input_->points[y*input_->width + x].r;
output.points[y*input_->width + x].g = input_->points[y*input_->width + x].g;
output.points[y*input_->width + x].b = input_->points[y*input_->width + x].b;
if (norm_sum != 0.0f)
{
float depth = sum / norm_sum;
Eigen::Vector3f pc (static_cast<float> (x) * depth, static_cast<float> (y) * depth, depth);
Eigen::Vector3f pw (unprojection_matrix_ * pc);
output.points[y*input_->width + x].x = pw[0];
output.points[y*input_->width + x].y = pw[1];
output.points[y*input_->width + x].z = pw[2];
}
else
{
output.points[y*input_->width + x].x = nan;
output.points[y*input_->width + x].y = nan;
output.points[y*input_->width + x].z = nan;
}
}
output.header = input_->header;
output.width = input_->width;
output.height = input_->height;
}
double computeAverageDistance(MeshType::Pointer mesh1, MeshType::Pointer mesh2, unsigned numberOfSamplingPoints) {
std::vector<double> distanceValues = computeDistances(mesh1, mesh2, numberOfSamplingPoints);
return std::accumulate(distanceValues.begin(), distanceValues.end(), 0.0) / numberOfSamplingPoints;
}
void computePairEnergy(int * int_array, REAL * real_array, REAL * termSum, int* interaction, short *resetTerm){
#pragma HLS INTERFACE s_axilite port=int_array bundle=control
#pragma HLS INTERFACE s_axilite port=real_array bundle=control
#pragma HLS INTERFACE s_axilite port=termSum bundle=control
#pragma HLS INTERFACE s_axilite port=resetTerm bundle=control
#pragma HLS INTERFACE s_axilite port=interaction bundle=control
#pragma HLS INTERFACE m_axi port=termSum offset=slave bundle=gmem2
#pragma HLS INTERFACE m_axi port=resetTerm offset=slave bundle=gmem3
#pragma HLS INTERFACE m_axi port=interaction offset=slave bundle=gmem0
#pragma HLS INTERFACE m_axi port=real_array offset=slave bundle=gmem4
#pragma HLS INTERFACE m_axi port=int_array offset=slave bundle=gmem1
#pragma HLS INTERFACE s_axilite port=return bundle=control
//read the input buffer...
int CLeafType = int_array[0];
int pNodeType = int_array[1];
int size1 = int_array[2];
int size2 = int_array[3];
int CLeaf_m_index = int_array[4];
int pNode_m_index = int_array[5];
int type1_m_aTypes[TYPE1_M_ATYPES_DIM];
int type2_m_aTypes[TYPE2_M_ATYPES_DIM];
int type1_m_groups[TYPE1_M_GROUPS_DIM];
int type2_m_groups[TYPE2_M_GROUPS_DIM];
int index = 6;
/*
memcpy(type1_m_aTypes, int_array + index, TYPE1_M_ATYPES_DIM * sizeof(int));
index += TYPE1_M_ATYPES_DIM;
memcpy(type2_m_aTypes, int_array + index, TYPE2_M_ATYPES_DIM * sizeof(int));
index += TYPE2_M_ATYPES_DIM;
*/
read_t1mat:for(int i = index, s_i = 0; i < index + TYPE1_M_ATYPES_DIM; i++, s_i++){
#pragma HLS PIPELINE II=1
type1_m_aTypes[s_i] = int_array[i];
}
index += TYPE1_M_ATYPES_DIM;
read_t2mat:for(int i = index, s_i = 0; i < index + TYPE2_M_ATYPES_DIM; i++, s_i++) {
#pragma HLS PIPELINE II=1
type2_m_aTypes[s_i] = int_array[i];
}
index += TYPE2_M_ATYPES_DIM;
//this as pointer? TODO
int type1_m_nGroups = int_array[index];
index++;
int type2_m_nGroups = int_array[index];
index++;
/*
memcpy(type1_m_groups, int_array + index, TYPE1_M_GROUPS_DIM * sizeof(int));
index += TYPE1_M_GROUPS_DIM;
memcpy(type2_m_groups, int_array + index, TYPE2_M_GROUPS_DIM * sizeof(int));
*/
read_t1mg:for(int i = index, s_i = 0; i < index + TYPE1_M_GROUPS_DIM; i++, s_i++){
#pragma HLS PIPELINE II=1
type1_m_groups[s_i] = int_array[i];
}
index += TYPE1_M_GROUPS_DIM;
read_t2mg:for(int i = index, s_i = 0; i < index + TYPE2_M_GROUPS_DIM; i++, s_i++) {
#pragma HLS PIPELINE II=1
type2_m_groups[s_i] = int_array[i];
}
REAL rot[3 * 3];
REAL trans[3];
REAL bv1_m_center[3];
REAL bv2_m_center[3];
REAL bv1_m_rad[1];
REAL bv2_m_rad[1];
REAL CLeaf_m_rotate[3 * 3];
REAL CLeaf_next_m_positions[12 * 3];
REAL CLeaf_m_translate[3];
REAL pNode_m_rotate[3 * 3];
REAL pNode_next_m_positions[12 * 3];
REAL pNode_m_translate[3];
REAL pNode_m_positions[12 * 3];
REAL CLeaf_m_positions[12 * 3];
REAL CLeaf_m_distances[MAX_ROTAMER_SIZE * MAX_ROTAMER_SIZE];
REAL type1_m_charges[5];
REAL type2_m_charges[12];
/*
raed_rot:for(int i = 0; i < 3 * 3; i++){
#pragma HLS PIPELINE
rot[i] = rot_p[i];
}*/
/*
index = 0;
memcpy(rot, real_array, 3 *3 * sizeof(REAL));
index += 9;
memcpy(trans, real_array + index, 3 * sizeof(REAL));
index +=3;
memcpy(bv1_m_center, real_array + index, 3 * sizeof(REAL));
index +=3;
memcpy(bv2_m_center, real_array + index, 3 * sizeof(REAL));
index += 3;
memcpy(bv1_m_rad, real_array + index, sizeof(REAL));
index ++;
memcpy(bv2_m_rad, real_array + index, sizeof(REAL));
index ++;
memcpy(CLeaf_m_rotate, real_array + index, 3 * 3 * sizeof(REAL));
index += 9;
memcpy(CLeaf_next_m_positions, real_array + index, 3 * 12 * sizeof(REAL));
index += 3*12;
memcpy(CLeaf_m_translate, real_array + index, 3 * sizeof(REAL));
index += 3;
memcpy(pNode_m_rotate, real_array + index, 12 * 3 * sizeof(REAL));
index += 3*12;
memcpy(pNode_m_translate, real_array + index, 3 * sizeof(REAL));
index += 3;
memcpy(pNode_m_positions, real_array + index, 12 * 3 * sizeof(REAL));
index += 12 * 3;
memcpy(CLeaf_m_positions, real_array + index, 12 * 3 * sizeof(REAL));
index += 12 * 3;
memcpy(CLeaf_m_distances, real_array + index, MAX_ROTAMER_SIZE * MAX_ROTAMER_SIZE * sizeof(REAL));
index += MAX_ROTAMER_SIZE * MAX_ROTAMER_SIZE;
memcpy(type1_m_charges, real_array + index, TYPE1_M_CHARGES_DIM * sizeof(REAL));
index += TYPE1_M_CHARGES_DIM;
memcpy(type2_m_charges, real_array + index, TYPE2_M_CHARGES_DIM * sizeof(REAL));
*/
index = 0;
read_rot:for(int i = index, s_i = 0; i < index + 9; i++, s_i++){
#pragma HLS PIPELINE
rot[s_i] = real_array[i];
}
index += 9;
read_trans:for(int i = index, s_i = 0; i < index + 3; i++, s_i++){
#pragma HLS PIPELINE
trans[s_i] = real_array[i];
}
index += 3;
read_bv1mc:for(int i = index, s_i = 0; i < index + 3; i++, s_i++){
#pragma HLS PIPELINE
bv1_m_center[s_i] = real_array[i];
}
index +=3;
read_bv2mc:for(int i = index, s_i = 0; i < index + 3; i++, s_i++){
#pragma HLS PIPELINE
bv2_m_center[s_i] = real_array[i];
}
index +=3;
bv1_m_rad[0] = real_array[index];
index ++;
bv2_m_rad[0] = real_array[index];
index ++;
read_cmr:for(int i = index, s_i = 0; i < index + 9; i++, s_i++){
#pragma HLS PIPELINE
CLeaf_m_rotate[s_i] = real_array[i];
}
index += 9;
read_cnmp:for(int i = index, s_i = 0; i < index + 12 * 3; i++, s_i++){
#pragma HLS PIPELINE
CLeaf_next_m_positions[s_i] = real_array[i];
}
index += 12 * 3;
read_cmt:for(int i = index, s_i = 0; i < index + 3; i++, s_i++){
#pragma HLS PIPELINE
CLeaf_m_translate [s_i] = real_array[i];
}
index += 3;
read_pmr:for(int i = index, s_i = 0; i < index + 9; i++, s_i++){
#pragma HLS PIPELINE
pNode_m_rotate[s_i] = real_array[i];
}
index += 9;
read_pnmp:for(int i = index, s_i = 0; i < index + 12 * 3; i++, s_i++){
#pragma HLS PIPELINE
pNode_next_m_positions[s_i] = real_array[i];
}
index += 12 * 3;
read_pmt:for(int i = index, s_i = 0; i < index + 3; i++, s_i++){
#pragma HLS PIPELINE
pNode_m_translate[s_i] = real_array[i];
}
index += 3;
read_pmnp:for(int i = index, s_i = 0; i < index + 12 * 3; i++, s_i++){
#pragma HLS PIPELINE
pNode_m_positions[s_i] = real_array[i];
}
index += 12 * 3;
read_cmp:for(int i = index, s_i = 0; i < index + 12 * 3; i++, s_i++){
#pragma HLS PIPELINE
CLeaf_m_positions[s_i] = real_array[i];
}
index += 12 * 3;
read_cmd:for(int i = index, s_i = 0; i < index + MAX_ROTAMER_SIZE * MAX_ROTAMER_SIZE; i++, s_i++){
#pragma HLS PIPELINE
CLeaf_m_distances[s_i] = real_array[i];
}
index += MAX_ROTAMER_SIZE * MAX_ROTAMER_SIZE;
read_t1mc:for(int i = index, s_i = 0; i < index + TYPE1_M_CHARGES_DIM; i++, s_i++){
#pragma HLS PIPELINE
type1_m_charges[s_i] = real_array[i];
}
index += TYPE1_M_CHARGES_DIM;
read_t2mc:for(int i = index, s_i = 0; i < index + TYPE2_M_CHARGES_DIM; i++, s_i++){
#pragma HLS PIPELINE
type2_m_charges[s_i] = real_array[i];
}
if(CLeafType == GLY || pNodeType == GLY){
interaction[0] = 0;
}
interaction[0] = 1; //means true
// If the BVs are too far away, no need to do anything
if ( computeDistance(bv1_m_center, bv2_m_center, *(bv1_m_rad),*(bv2_m_rad), rot, trans) > CUTOFF_DISTANCE)
{
resetTerm[0] = 0; //o = true
return;
}
computeDistances(size1, size2, CLeafType, pNodeType, CLeaf_next_m_positions, CLeaf_m_translate, CLeaf_m_rotate, pNode_next_m_positions, pNode_m_translate, pNode_m_rotate, pNode_m_positions, CLeaf_m_positions, CLeaf_m_distances, rot, trans);
REAL sum = 0.0;
int diff = pNode_m_index - CLeaf_m_index;
// Compute all vdW terms.
sum += computeVdW(size1, size2, CLeafType, pNodeType, type1_m_aTypes, type2_m_aTypes,
CLeaf_m_distances, diff);
// Compute all elctrostatic terms
sum += computeElectrostatics(size1, size2, type1_m_nGroups, type2_m_nGroups, type1_m_groups, type2_m_groups, type1_m_charges, type2_m_charges, CLeafType, pNodeType, CLeaf_m_distances, diff);
// Compute all Solvation terms.
sum += computeSolvation(size1, size2, type1_m_aTypes, type2_m_aTypes, CLeafType, pNodeType,
CLeaf_m_distances, diff);
*termSum = sum;
//*termSum = type1_m_aTypes[2] * type2_m_aTypes[1] * type1_m_groups[0] * type2_m_groups[0] * CLeaf_m_distances[0] * rot [3] * trans[1];
//*termSum = 0;
//TODO how to do the save of the pointer in case of undoing the last move?
return;
}
static void computeDistances_proxy(GeometryDataHandler & data_geom)
{
computeDistances(*data_geom);
}
