/**
* Return the double values for the variable with the specified
* name or null.
*
* @param name Name of variable.
* @return Values of variable.
*/
std::vector<double> vals_r(const std::string& name) const {
if (contains_r_only(name)) {
return (vars_r_.find(name)->second).first;
} else if (contains_i(name)) {
std::vector<int> vec_int = (vars_i_.find(name)->second).first;
std::vector<double> vec_r(vec_int.size());
for (size_t ii = 0; ii < vec_int.size(); ii++) {
vec_r[ii] = vec_int[ii];
}
return vec_r;
}
return empty_vec_r_;
}
void ElementWiseTransientCoupledResidualLocalAssembler::assembly( const NumLib::TimeStep ×tep,
const MeshLib::IElement &e,
const DiscreteLib::DofEquationIdTable &localDofManager,
const LocalVectorType &local_u_n1,
const LocalVectorType &local_u_n,
LocalVectorType &local_r
)
{
// parameters need to be passed
const size_t n_var = _n_var;
std::vector<size_t> &vec_order = _vec_order;
size_t mesh_id = 0;
//
std::vector<std::vector<size_t> > vec_local_pos(n_var);
for (size_t i=0; i<n_var; i++) {
std::vector<size_t> list_nodeid;
e.getNodeIDList(vec_order[i], list_nodeid);
localDofManager.mapEqsID(i, mesh_id, list_nodeid, vec_local_pos[i]);
}
std::vector<LocalVectorType> vec_u0(n_var), vec_u1(n_var);
for (size_t i=0; i<n_var; i++) {
DiscreteLib::getLocalVector(vec_local_pos[i], local_u_n, vec_u0[i]);
DiscreteLib::getLocalVector(vec_local_pos[i], local_u_n1, vec_u1[i]);
}
std::vector<LocalVectorType> vec_r(n_var);
this->assembleComponents(timestep, e, vec_order, vec_u0, vec_u1, vec_r);
for (size_t i=0; i<n_var; i++) {
for (size_t j=0; j<vec_local_pos[i].size(); j++) {
local_r[vec_local_pos[i][j]] += vec_r[i][j];
}
}
// if (e.getID()==0) {
// std::cout << "local_u_n0=" << local_u_n << std::endl;
// std::cout << "local_u_n1=" << local_u_n1 << std::endl;
// std::cout << "local_r=" << local_r << std::endl;
// }
}
//Tree * neighborJoin(const vector< vector<double> >& d,vector<string> names,vector<UnrootedNode *> nodes,unsigned int r) {
Tree * neighborJoin(const mat & d,vector<string> names,vector<UnrootedNode *> nodes,unsigned int r) {
// cerr<<"neighborJoin() "<<vectorToString(names)<<"\t"<<r<< endl;
if (r <= 2) { //end of recursion
//compute net divergence for all OTUs
// vector<double> vec_r=vector<double>(r,0.0);
// for (unsigned int i = 0; i < r; ++i) {
// double sum=0.0;
// for (unsigned int j = 0; j < r; ++j) {
// sum+=d[i][j];
// }
// vec_r[i]=sum;
// }
double distI= d(0,1);
// cout<<"distI "<<distI<<endl;
// exit(1);
//double distJ= d[0][1];
UnrootedNode * left =nodes[0];
UnrootedNode * right=nodes[1];
// cerr<<"unrooted done 1"<<endl;
// left->printRecur();
// cerr<<"popll "<<left->isLeaf()<<endl;
// cerr<<"popl "<<left->getPopulation()<<endl;
// cerr<<endl;
// cerr<<endl<<"----------------"<<endl;
// right->printRecur();
// cerr<<endl;
// cerr<<"poprl "<<right->isLeaf()<<endl;
// cerr<<"popr "<<right->getPopulation()<<endl;
// cerr<<endl<<"unrooted done 2"<<endl;
UnrootedNode * rootNode=0;
// cout<<"distI "<<distI<<endl;
if(left->isLeaf() && right->isLeaf() ){//both leaves
cerr<<"Cannot build tree using only two clades"<<endl;
exit(1);
}else{
if(!left->isLeaf() && right->isLeaf() ){
// cerr<<"case1"<<endl;
left->addNeighbor(right,distI);
left->findNode("root",rootNode);
}else{
if(left->isLeaf() && !right->isLeaf() ){
// cerr<<"case2"<<endl;
right->addNeighbor(left,distI);
right->findNode("root",rootNode);
}else{
//doesn't matter who is added to whom
left->addNeighbor(right,distI);
left->findNode("root",rootNode);
}
}
}
// cerr<<"unrooted done 3"<<endl;
// left->printRecur();
// cerr<<endl<<"----------------"<<endl;
if(rootNode==0){
cerr<<"Cannot find root on neither side"<<endl;
exit(1);
}
if(rootNode->getNeighbors().size() != 1){
cerr<<"Root does not have 2 neighbors"<<endl;
exit(1);
}
// cerr<<"unrooted done 1"<<endl;
// left->printRecur();
// cerr<<endl<<"----------------"<<endl;
// right->printRecur();
// cerr<<endl<<"unrooted done 2"<<endl;
// cerr<<rootNode->getDistance().at(0)<<endl;
//We root at half the distance between both left and right subtrees
Tree * toreturn=new Tree(rootNode,
rootNode->getNeighbors().at(0),
rootNode->getDistance().at(0)/2.0);
// cout<<"dist "<<toreturn->getRoot()->getLeftChild()->getDistance()<<endl;
// cout<<"dist "<<toreturn->getRoot()->getRightChild()->getDistance()<<endl;
// exit(1);
return toreturn;
} else {
//compute net divergence for all OTUs
// cout<<"pre vec_R"<<endl;
//vector<double> vec_r=vector<double>(r,0.0);
vec vec_r (r);
for (unsigned int i = 0; i < r; ++i) {
double sum=0.0;
for (unsigned int j = 0; j < r; ++j) {
sum+=d(i,j);
}
vec_r(i)=sum;
}
// cout<<"post vec_R"<<endl;
double minM=DBL_MAX;
unsigned int minMI=0;
unsigned int minMJ=0;
//vector< vector<double> > m(r, vector<double>(r,0.0));
mat m (r,r);
for (unsigned int i = 0; i < r; ++i) {
for (unsigned int j = 0; j < r; ++j) {
if(i==j){
m(i,j) = 0.0;
}else{
m(i,j) = d(i,j) - ( (vec_r(i) + vec_r(j)) / double(r-2) );
if(m(i,j) < minM){
minM=m(i,j);
minMI=i;
minMJ=j;
}
}
}
}
// cout<<"post minM"<<endl;
// cout<<"r "<<r<<endl;
// cout<<"d "<<d.size()<<endl;
// //construct new distance matrix with size -1, remove 2 OTU, add a new one
mat newD=d;
newD.shed_col(max(minMI,minMJ));
newD.shed_row(max(minMI,minMJ));
newD.shed_col(min(minMI,minMJ));
newD.shed_row(min(minMI,minMJ));
// vector< vector<double> > newD (r-1, vector<double>(r-1,0.0));//map_distance_matrix(d, minQA, minQB);
// for (unsigned int i = 0; i < r; i++) {
// for (unsigned int j = 0; j < r; j++) {
// if(i==minMI || i==minMJ)
// continue;
// if(j==minMI || j==minMJ)
// continue;
// unsigned i_2=i;
// unsigned j_2=j;
// if(i>minMI)
// i_2--;
// if(i>minMJ)
// i_2--;
// if(j>minMI)
// j_2--;
// if(j>minMJ)
// j_2--;
// #ifdef DEBUG
// cout<<"a) "<<i<<"\t"<<j<<endl;
// cout<<"a) "<<i_2<<"\t"<<j_2<<endl;
// cout<<"a) "<<minMI<<"\t"<<minMJ<<endl;
// cout<<d[i][j]<<endl;
// cout<<"b) "<<i<<"\t"<<j<<endl;
// cout<<newD[i_2][j_2] <<endl;
// #endif
// newD[i_2][j_2] = d[i][j] ;
// }
// }
//cout<<"-----pre new OTU-------"<<endl;
// #ifdef DEBUG
// for(unsigned int i=0;i<(r-1);i++){
// cout<<vectorToString(newD[i],"\t")<<endl;
// }
// #endif
//new distance for new OTU
newD.resize(newD.n_rows+1,newD.n_cols+1);
unsigned int k2=0;
for(unsigned int k=0;k<r;k++){
if(k==minMI || k==minMJ)
continue;
double newdist=((d(minMI,k)+d(k,minMJ) - d(minMI,minMJ))/2.0);
//cout<<names[k]<<"\t"<<names[minMI]<<"\t"<<names[minMJ]<<"\t"<<newdist<<endl;
newD(r-2,k2) = newdist;
newD(k2,r-2) = newdist;
k2++;
}
newD(r-2,r-2)=0.0;
double distI=( (d(minMI,minMJ))/2.0 + (vec_r(minMI)-vec_r(minMJ))/( 2.0*double(r-2) ) );
double distJ=( (d(minMI,minMJ))/2.0 + (vec_r(minMJ)-vec_r(minMI))/( 2.0*double(r-2) ) );
UnrootedNode * newnode=new UnrootedNode();
newnode->addNeighbor( nodes[minMI] ,distI);
newnode->addNeighbor( nodes[minMJ] ,distJ);
// cerr<<"new node "<<endl;
// newnode->printRecur();
// cerr<<endl<<"new node "<<endl;
//erasing from names
string newname="";
if(minMI<minMJ){
newname="("+names[minMI]+","+names[minMJ]+")";
names.erase(names.begin()+minMJ);
names.erase(names.begin()+minMI);
nodes.erase(nodes.begin()+minMJ);
nodes.erase(nodes.begin()+minMI);
}else{
newname="("+names[minMI]+","+names[minMJ]+")";
names.erase(names.begin()+minMI);
names.erase(names.begin()+minMJ);
nodes.erase(nodes.begin()+minMI);
nodes.erase(nodes.begin()+minMJ);
}
names.push_back(newname);
nodes.push_back(newnode);
return neighborJoin(newD,names,nodes, r-1);
}
return 0;
}
void Robot::moveRotate(bool isLeft, float radius, float arc)
{
if (isLeft)
{
//worldmap operation
cv::Point2f center(x,y);
cv::Point2f vec_r(radius*cos(ori),-radius*sin(ori));
center = center - vec_r;
cv::Point2f new_vec_r;
new_vec_r.x = vec_r.x*cos(arc)+vec_r.y*sin(arc);
new_vec_r.y = -vec_r.x*sin(arc)+vec_r.y*cos(arc);
cv::Point2f shift = new_vec_r - vec_r;
x += shift.x;
y += shift.y;
ori -= arc;
ori = ori<0?ori+2*M_PI:(ori>2*M_PI?ori-2*M_PI:ori);
//motor operation
int min1 = 0, min2 = 0;
float mind = 10000;
float radio = (radius + DIST_BETWEEN_WHEELS / 2) / (radius - DIST_BETWEEN_WHEELS / 2);
float nowr;
for (int i = 30; i >= 2; i--)//枚举搜索一定速度范围内最接近内外轮半径比的左右轮轮速比
for (int j = 1; j < i; j++) {
nowr = i *1.0 / j;
if (myabs(nowr - radio) < mind) {
mind = myabs(nowr - radio);
min1 = i;
min2 = j;
}
}
float t = (((arc - ARC_DELAY) * (radius + DIST_BETWEEN_WHEELS / 2) / min1)
+ ((arc - ARC_DELAY) * (radius - DIST_BETWEEN_WHEELS / 2) / min2)) / 2;
goWithSpeed(min2, min1, t);
}
else //turn right
{
//vice vesa
cv::Point2f center(x,y);
cv::Point2f vec_r(-radius*cos(ori),radius*sin(ori));
center = center - vec_r;
cv::Point2f new_vec_r;
new_vec_r.x = vec_r.x*cos(-arc)+vec_r.y*sin(-arc);
new_vec_r.y = -vec_r.x*sin(-arc)+vec_r.y*cos(-arc);
cv::Point2f shift = new_vec_r - vec_r;
x += shift.x;
y += shift.y;
ori += arc;
ori = ori<0?ori+2*M_PI:(ori>2*M_PI?ori-2*M_PI:ori);
//vice vesa
int min1 = 0, min2 = 0;
float mind = 10000;
float radio = (radius + DIST_BETWEEN_WHEELS / 2) / (radius - DIST_BETWEEN_WHEELS / 2);
float nowr;
for (int i = 30; i >= 2; i--)
for (int j = 1; j < i; j++) {
nowr = i *1.0 / j;
if (myabs(nowr - radio) < mind) {
mind = myabs(nowr - radio);
min1 = i;
min2 = j;
}
}
float t = (((arc - ARC_DELAY) * (radius + DIST_BETWEEN_WHEELS / 2) / min1)
+ ((arc - ARC_DELAY) * (radius - DIST_BETWEEN_WHEELS / 2) / min2)) / 2;
goWithSpeed(min1, min2, t);
}
}
