void MainWindow::showBodyDetails( const Creature& creature, const BodyPart& body ) {
getUiInspector().lst_body_details->addItem(QString(
body.getName().c_str()));
getUiInspector().lst_body_details->addItem(QString(
(" Mass: " + TO_STRING(body.getMass())).c_str()));
getUiInspector().lst_body_details->addItem(QString(
(" Torque: " + TO_STRING(body.getMaxTorque())).c_str()));
getUiInspector().lst_body_details->addItem(QString(
std::string(" Size").c_str()));
getUiInspector().lst_body_details->addItem(QString(
(" [x]: " + TO_STRING(body.getSizeX())).c_str()));
getUiInspector().lst_body_details->addItem(QString(
(" [y]: " + TO_STRING(body.getSizeY())).c_str()));
getUiInspector().lst_body_details->addItem(QString(
(" [z]: " + TO_STRING(body.getSizeZ())).c_str()));
//Angles
BodyPart* parent = body.getParentId() == Creature::B_NONE
? NULL : &creature.getBodyPart(body.getParentId());
if (parent) {
getUiInspector().lst_body_details->addItem(QString(
(" Parent: " + parent->getName()).c_str()));
// getUiInspector().lst_body_details->addItem(QString(std::string(" Angles:").c_str()));
// getUiInspector().lst_body_details->addItem(QString(
// (" XY:" + TO_STRING(
// MathUtil::getAngle(MathUtil::XY,
// parent->getRigidBody()->getCenterOfMassPosition(),
// body.getRigidBody()->getCenterOfMassPosition()))).c_str()));
// getUiInspector().lst_body_details->addItem(QString(
// (" YZ:" + TO_STRING(
// MathUtil::getAngle(MathUtil::YZ,
// parent->getRigidBody()->getCenterOfMassPosition(),
// body.getRigidBody()->getCenterOfMassPosition()))).c_str()));
// getUiInspector().lst_body_details->addItem(QString(
// (" ZX:" + TO_STRING(
// MathUtil::getAngle(MathUtil::ZX,
// parent->getRigidBody()->getCenterOfMassPosition(),
// body.getRigidBody()->getCenterOfMassPosition()))).c_str()));
}
}
void Creature::calculateMaxTorque() {
double gravity = Math::abs(BulletSimulator::getSingleton().getWorld().getGravity().y());
int terminal[getNumberOfBodyParts()];
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
terminal[i] = 0;
getBodyPart(i).setMaxTorque(0.0);
}
//Determinar nodos terminales (sum == 1)
for (int i = 0; i < getNumberOfConstraints(); ++i) {
assert(getConstraint(i).getIdBodyA() < getNumberOfBodyParts() &&
getConstraint(i).getIdBodyA() >= 0);
assert(getConstraint(i).getIdBodyB() < getNumberOfBodyParts() &&
getConstraint(i).getIdBodyB() >= 0);
// if (!getBodyPart(getConstraint(i).getIdBodyA()).isRoot()) {
++terminal[getConstraint(i).getIdBodyA()];
// }
// if (!getBodyPart(getConstraint(i).getIdBodyB()).isRoot()) {
++terminal[getConstraint(i).getIdBodyB()];
// }
}
for (int e = 0; e < getNumberOfBodyParts(); ++e) {
if (terminal[e] != 1) continue;
int count = 0;
BodyPart * body = &getBodyPart(e);
std::vector<BodyPart*> extremities;
while (body != &getRoot() && count++ < getNumberOfBodyParts()) {
extremities.push_back(body);
body = &getBodyPart(body->getParentId());
}
extremities.push_back(body);
double arm_length = 0.0;
btTransform trans;
btVector3 center_of_mass;
btVector3 connection;
// A 0 B 1 C 2 Root 3
// |-----|-----|-----|-----|
for (int i = extremities.size() - 2; i >= 0; --i) {
// DEBUG(extremity[i]->getName() + " ; " + extremity[i+1]->getName());
Constraint* constraint = getConstraint(*extremities[i], *extremities[i + 1]);
assert(constraint);
center_of_mass = extremities[i]->getRigidBody()->getCenterOfMassPosition();
loadConnectionTransform(*extremities[i], *constraint, trans);
connection = btVector3(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z());
arm_length = (center_of_mass - connection).length();
// BDEBUG("arm=" + TO_STRING(arm_length));
extremities[i]->addMaxTorque(arm_length * extremities[i]->getMass() * gravity);
for (int j = i - 1; j >= 0; --j) {
constraint = getConstraint(*extremities[j], *extremities[i]);
if (!constraint) {
// DEBUG("NOT FOUND: " + extremity[i]->getName() + " ; " + extremity[i + 1]->getName());
continue;
}
// DEBUG("FOUND (" + constraint->getName() + "): " + extremity[i]->getName() + " ; " + extremity[i + 1]->getName());
assert(constraint);
center_of_mass = extremities[i]->getRigidBody()->getCenterOfMassPosition();
loadConnectionTransform(*extremities[i], *constraint, trans);
connection = btVector3(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z());
arm_length += (center_of_mass - connection).length();
center_of_mass = extremities[j]->getRigidBody()->getCenterOfMassPosition();
loadConnectionTransform(*extremities[j], *constraint, trans);
connection = btVector3(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z());
arm_length += (center_of_mass - connection).length();
extremities[i]->addMaxTorque(arm_length * extremities[j]->getMass() * gravity);
}
}
}
}