int ReferenceFrame2D::GetNode(Vector2D p_loc) const
{
double tol = 1e-8*GetMBS()->CharacteristicLength();
/*
for (int i = 1; i <=nodes.Length(); i++)
{
if (Dist(p_loc,nodes(i)->Pos2D()) <= tol) return i;
}
GetMBS()->UO() << "ERROR: node not found:" << p_loc << " !!!\n";
return 0;
*/
IVector items;
Vector3D p(p_loc.X(),p_loc.Y(),0.);
Box3D box(p, p);
box.Increase(tol);
searchtree.GetItemsInBox(box,items);
for (int i = 1; i <= items.Length(); i++)
{
Vector3D p2(nodes(items(i))->Pos().X(), nodes(items(i))->Pos().Y(), 0.);
if (Dist(p,p2) <= tol) return items(i);
}
GetMBS()->UO() << "ERROR: node not found:" << p_loc << " !!!\n";
return 0;
}
int ReferenceFrame2D::AddNode(Node* n)
{
n->SetAuxNode();
double tol = 1e-8*GetMBS()->CharacteristicLength();
//standard:
/*
for (int i = 1; i <=nodes.Length(); i++)
{
if (Dist(n->Pos(),nodes(i)->Pos()) <= tol) return i;
}
Node* nc = n->GetCopy();
return nodes.Add(nc);
*/
//optimized:
IVector items;
Box3D box(n->Pos(),n->Pos());
box.Increase(tol); //disabled in example for witteven adams comparison Milano conference 2007
searchtree.GetItemsInBox(box,items);
for (int i = 1; i <= items.Length(); i++)
{
//if (Dist(n->Pos(),nodes(items(i))->Pos()) <= tol) return items(i);
if (Dist(n->Pos(),nodes(items(i))->Pos()) <= tol && (n->GetBodyInd() == nodes(items(i))->GetBodyInd()) ) return items(i); //$ AD FENodes have domain
}
Node* nc = n->GetCopy();
int index = nodes.Add(nc);
nc->NodeNum() = index; //store node number in reference configuration (local == global) for const int& access of virtual function nodenum in referenceframe2D
searchtree.AddItem(Box3D(n->Pos(),n->Pos()), index);
return index;
}
