void Creature::loadConnectionTransform(const BodyPart& body, const Constraint& constraint, btTransform & trans) {
if (body.getId() == constraint.getIdBodyA()) {
Constraint::locateConnectionOrigin(body, constraint.getConnectionA(), trans);
} else {
assert(body.getId() == constraint.getIdBodyB());
Constraint::locateConnectionOrigin(body, constraint.getConnectionB(), trans);
}
trans = body.getRigidBody()->getCenterOfMassTransform() * trans;
}
void Body::removePart(std::string part_uuid) {
//Get shared pointer of the Part to be removed
boost::shared_ptr<Part> part = getPartByUUID(part_uuid);
//TODO: Handle removal of root BP or root Organ
if (part == nullptr || part->getId() == "ROOT" || part->getId() == "UPPER_TORSO")
{
return;
}
debug_print("Removing Part %s...\n", part->getId().c_str());
//Create a list of all parts to be removed
std::vector<string>* rem_list = new std::vector<string>();
//Add the part given to the function...
rem_list->push_back(part_uuid);
//...and everything that lies downstream of it (Organs and Bodyparts)
makeDownstreamPartList(part_uuid, rem_list);
//Remove duplicates from the list
std::sort(rem_list->begin(), rem_list->end());
rem_list->erase(std::unique(rem_list->begin(), rem_list->end()), rem_list->end());
#ifdef _DEBUG
for (auto it = rem_list->begin(); it != rem_list->end(); it++)
{
debug_print("UUID %s is Part %s \n", getPartByUUID(*it)->getUUID().c_str(), getPartByUUID(*it)->getId().c_str());
}
#endif
//Make a temporary list, in which UUIDs of empty bodyparts are stored.
// Its contents are later added to the rem_list
std::vector<string>* bp_rem = new std::vector<string>();
//Iterate over all parts to be removed...
for (auto it_rl = rem_list->begin(); it_rl != rem_list->end(); it_rl++)
{
//...if the part is an Organ, remove it from its connectors connected_organs list
if (getPartByUUID(*it_rl)->getType() == TYPE_ORGAN)
{
Organ *o = static_cast<Organ*>(getPartByUUID(*it_rl).get());
Organ *con = static_cast<Organ*>(getPartByUUID(o->getConnectorUUID()).get());
con->removeConnectedOrgan(*it_rl);
}
//..for all parts: remove from super part child list.
// If the super part is found to be empty (removeChild() returns true), add it
// to the temporary bp_rem list.
BodyPart* super = static_cast<BodyPart*>(getPartByUUID(getPartByUUID(*it_rl)->getSuperPartUUID()).get());
BodyPart* old_super;
if (super->removeChild(*it_rl))
{
debug_print("BodyPart %s is empty, add to unregister\n", super->getId().c_str());
bp_rem->push_back(super->getUUID());
bool done = false;
//For every BodyPart that is to be deleted as empty, remove it from its own
// super BodyPart and check wheter it needs to be removed as well!
while (!done)
{
old_super = super;
super = static_cast<BodyPart*>(getPartByUUID(super->getSuperPartUUID()).get());
if (super->removeChild(old_super->getUUID()))
{
debug_print("BodyPart %s is empty, add to unregister\n", super->getId().c_str());
bp_rem->push_back(super->getUUID());
}
else {
done = true;
}
}
}
}
//Merge the temporary removal list into the rem_list
rem_list->insert(rem_list->end(), bp_rem->begin(), bp_rem->end());
//Remove duplicates again
std::sort(rem_list->begin(), rem_list->end());
rem_list->erase(std::unique(rem_list->begin(), rem_list->end()), rem_list->end());
//Unregister the Parts, causing the shared pointers to destroy their references and themselves.
unregisterParts(rem_list);
//Clear the part variable. This should cause the last use of the shared pointer
// to the part to be freed, therefore destroying the part.
part.reset();
//Refresh all lists and maps
refreshLists();
#ifdef _DEBUG
printBodyMap("body_mt.gv", root.get());
#endif
debug_print("done.\n");
}
string Body::enter(rapidxml::xml_node<> *node, std::map<string, string>* child_map, std::map<string, string>* organ_link_map) {
using namespace rapidxml;
int organ_count, bodyparts, it;
xml_node<> *temp;
xml_node<> **organ_node_list;
char *id = nullptr, *name = nullptr, *_name = nullptr;
float surface = 0.0f;
//Make a count of the body_part nodes in this node and parse all standard
// nodes for the bodypart of this node
temp = node->first_node();
bodyparts = 0;
while (temp != nullptr){
_name = temp->name();
if (!strcmp(_name, "body_part")) { bodyparts++; }
if (!strcmp(_name, "id")) { id = temp->value(); }
if (!strcmp(_name, "name")) { name = temp->value(); }
if (!strcmp(_name, "surface")) { surface = atof(temp->value()); }
temp = temp->next_sibling();
}
//if any of the mandatory vars for bodyparts are NULL, ERROR!
if (id == nullptr || name == nullptr){
throw new bdef_parse_error("Not all mandatory BodyPart variables defined!", node);
}
//make the bodypart, make the pointer to it shared and store it in the part_map
BodyPart* bp = new BodyPart((string)id, (string)name, surface, this);
boost::shared_ptr<BodyPart> p (bp);
part_map->insert(
std::pair<std::string, boost::shared_ptr<Part>>(
bp->getUUID(), boost::static_pointer_cast<Part>(p)));
//reset temporary variables for reuse with the organs
id = nullptr; name = nullptr;
//DEBUG: Print new BodyPart
debug_print("New BodyPart created: \n\tID: %s \n\tName: %s \n\tSurface: %f\n",
bp->getId().c_str(), bp->getName().c_str(), bp->getSurface());
//If there are no body_part nodes...
if (bodyparts == 0){
//...there must be organs instead!
// variables
Organ **organs;
xml_attribute<> *attr;
xml_node<> *tdef_node;
char *connector = nullptr;
bool symmetrical = false;
it = 0;
// temp vars for tissue definitions
char *tdef_id, *tdef_custom_id, *tdef_name;
float tdef_hit_prob;
tissue_def *tdefs = nullptr;
int tdef_count = 0, tdef_it = 0;
//Reset back to the first node in the given node
temp = node->first_node();
//scan for organ nodes in the given node
organ_count = 0;
while (temp != nullptr){
if (!strcmp(temp->name(),"organ")) { organ_count++; }
temp = temp->next_sibling();
}
//If there are no organs AND no bodyparts, ERROR!
if (organ_count == 0){
throw bdef_parse_error("No body part and no organ definition!", node);
}
//compile a list of nodes holding the organ definitions
//and parse the remaining nodes to make the bodypart
organ_node_list = new xml_node<>*[organ_count];
temp = node->first_node();
while (temp != nullptr){
_name = temp->name();
if (!strcmp(_name,"organ")) {
organ_node_list[it] = temp;
it++;
}
temp = temp->next_sibling();
}
//create temporary organ array
organs = new Organ*[organ_count];
//parse each organ definition in the list
for (int i=0; i < organ_count; i++){
//Enter into organ node
temp = organ_node_list[i]->first_node();
//Iterate through all nodes within the organ node
while (temp != nullptr){
_name = temp->name();
//Parse standard tags
if (!strcmp(_name,"id")) { id = temp->value(); }
if (!strcmp(_name,"name")) { name = temp->value(); }
if (!strcmp(_name,"surface")) { surface = atof(temp->value()); }
if (!strcmp(_name,"connector")) { connector = temp->value(); }
//Parse the organ tissue definitions, create (or rather link) them
// and add to the organs
if (!strcmp(_name,"organ_tissue")) {
attr = temp->first_attribute();
if (attr != nullptr && !strcmp(attr->name(), "symmetrical")){
symmetrical = true;
}
attr = nullptr;
//Get the count of tissue defs
tdef_count = 0;
tdef_node = temp->first_node();
while (tdef_node != nullptr)
{
//If any other node than tissue_def, ERROR!
if (strcmp(tdef_node->name(), "tissue_def")) {
throw bdef_parse_error("Invalid node for organ tissue (only tissue_def allowed)!", temp);
}
tdef_count++;
tdef_node = tdef_node->next_sibling();
}
//Create organ tissue array, double size if organ is symmetrical
tdefs = new tissue_def[tdef_count * (symmetrical+1)];
//Enter the organ_tissue node, create the tissue_def for
// each tissue_def child and store it in tdefs[] array
tdef_it = 0;
tdef_node = temp->first_node();
while (tdef_node != nullptr){
//The value of a tissue_def node is the id of the tissue (e.g. M_ARTERY)
tdef_id = tdef_node->value();
tdef_hit_prob = 0;
tdef_name = nullptr;
tdef_custom_id = nullptr;
//Get the other parameters
attr = tdef_node->first_attribute();
while (attr != nullptr){
if (!strcmp(attr->name(), "hit_prob")){ tdef_hit_prob = atof(attr->value()); }
if (!strcmp(attr->name(), "name")) { tdef_name = attr->value(); }
if (!strcmp(attr->name(), "custom_id")) {
tdef_custom_id = attr->value();
}
attr = attr->next_attribute();
}
//Store the parameters in the tissue definition
if (tdef_custom_id != nullptr) {
tdefs[tdef_it].custom_id = string(tdef_custom_id);
}
else { tdefs[tdef_it].custom_id = ""; }
if (tdef_name != nullptr) {
tdefs[tdef_it].name = string(tdef_name);
}
else { tdefs[tdef_it].name = ""; }
tdefs[tdef_it].hit_prob = tdef_hit_prob;
//Link the tissue definition to it's base tissue
// NOTE: the tissue_map variable is a POINTER to the
// actual tissue map which is defined in the loadBody (super)
// function, therefore it must be dereferenced before use.
//If the base tissue cannot be linked, ERROR!
std::string key = tdef_id;
std::map<std::string, boost::shared_ptr<Tissue>>::const_iterator pos
= tissue_map->find(key);
if (pos == tissue_map->end()){ throw bdef_parse_error("Tissue not found!", tdef_node); }
tdefs[tdef_it].tissue = pos->second;
tdef_it++;
tdef_node = tdef_node->next_sibling();
}
}
temp = temp->next_sibling();
}
//Check whether all necessary data for organ creation has been read
// if not, ERROR!
if (id == nullptr || name == nullptr || connector == nullptr || tdefs == nullptr){
throw bdef_parse_error("Not all necessary data for organ creation found.", organ_node_list[i]);
}
//if organ is symmetrical, create the symmetry by duplicating all entries but the last
// and appending them in reverse order
int d;
for (int j=tdef_count-2; j >= 0; j--)
{
d = (j - (tdef_count - 1))*-1;
tdefs[(tdef_count-1)+d] = tdefs[j];
}
//Create the organ
organs[i] = new Organ(string(id), string(name), surface, this, tdefs, tdef_count, connector, !strcmp(connector, "_ROOT"));
//DEBUG: Print Organ
#ifdef _DEBUG
debug_print("\tNew Organ created:\n\t\tID: %s \n\t\tName: %s \n\t\tSurface: %f \n\t\tRoot: %s \n\t\tTissues:",
organs[i]->getId().c_str(), organs[i]->getName().c_str(), organs[i]->getSurface(), connector);
for (int di = 0; di < (tdef_count * (symmetrical+1))-1; di++){
debug_print("\n\t\t\tBase Tissue Name: %s \n\t\t\t\tHit Prob.: %f",
tdefs[di].tissue->getName().c_str(), tdefs[di].hit_prob);
if (!tdefs[di].name.empty()){ debug_print("\n\t\t\t\tCustom Name: %s", tdefs[di].name.c_str()); }
if (!tdefs[di].custom_id.empty()){ debug_print("\n\t\t\t\tCustom ID: %s", tdefs[di].custom_id.c_str()); }
}
debug_print("\n\tEND Organ.\n");
#endif
}
//...and add all organs to the part map. Add the "child note"
// between the organs and the new bodypart to the child_map.
// Add the "connector note" between the organs and their connectors to
// the organ_link_map.
for (int i = 0; i < organ_count; i++){
part_map->insert(
std::pair<string, boost::shared_ptr<Part>>(
organs[i]->getUUID(),
boost::static_pointer_cast<Part>(boost::make_shared<Organ>(*(organs[i])))
));
child_map->insert(
std::pair<string, string>(
organs[i]->getUUID(),
bp->getUUID()
));
if (organs[i]->getConnectorId() != "_ROOT"){
organ_link_map->insert(
std::pair<string, string>(
organs[i]->getUUID(),
dynamic_cast<Organ*>(organs[i])->getConnectorId()
));
}
}
} else {
//...its another BodyPart
//Go through all nodes and call this function on every
// node containing a part definition, add the returned BodyPart/Organ
// to this ones' children (per child_map)
temp = node->first_node();
it = 0;
while (temp != nullptr){
_name = temp->name();
if (!strcmp(_name, "body_part")) {
string child_uuid = enter(temp, child_map, organ_link_map);
child_map->insert(
std::pair<string, string>(
child_uuid,
bp->getUUID()
));
//bp->addChild(enter(temp));
}
temp = temp->next_sibling();
}
}
//return this bodyparts uuid
return bp->getUUID();
}
