void Creature::set(const btTransform & at) {
btTransform offset;
offset.setIdentity();
double avg_distance = 0.0;
double max_height = getMaxHeight();
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
avg_distance += (getBodyPart(i).getSizeX() + getBodyPart(i).getSizeY() +
getBodyPart(i).getSizeZ()) / 3.0;
}
avg_distance *= 2;
avg_distance /= getNumberOfBodyParts();
offset.setOrigin(btVector3(0.0, avg_distance, 0.0));
float circle_fraction = 2 * M_PI / getNumberOfBodyParts();
float next_fraction = 0.0;
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
offset.setIdentity();
if (getBodyPart(i).isRoot()) {
offset.setOrigin(btVector3(0, max_height, 0));
} else {
offset.setOrigin(btVector3(cos(next_fraction) * avg_distance, max_height, sin(next_fraction) * avg_distance));
}
getBodyPart(i).set(at * offset);
next_fraction += circle_fraction;
}
for (int i = 0; i < getNumberOfConstraints(); ++i) {
getConstraint(i).set(*this);
}
calculateMaxTorque();
}
Creature::Creature(int body_parts, int constraints,
int hidden_layers, int neurons_per_layer)
:
_name(""),
_number_of_body_parts(body_parts),
_number_of_constraints(constraints),
_body_parts(new BodyPart*[body_parts]),
_constraints(new Constraint*[constraints]),
_neural_network(NULL),
_initial_position(btVector3(0.0, 0.0, 0.0)),
_final_position(btVector3(0.0, 0.0, 0.0)),
_fitness(0.0) {
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
setBodyPart(i, new BodyPart());
getBodyPart(i).setId(i);
getBodyPart(i).setName("Body Part #" + TO_STRING(i));
}
for (int i = 0; i < getNumberOfConstraints(); ++i) {
setConstraint(i, new Constraint());
getConstraint(i).setId(i);
getConstraint(i).setName("Constraint #" + TO_STRING(i));
}
_neural_network = new NeuralNetwork(4 * getNumberOfBodyParts(),
hidden_layers, neurons_per_layer, 3 * (getNumberOfBodyParts() - 1));
getNeuralNetwork().getInputLayer().setActivationFunction(Neuron::AF_NONE);
for (int i = 1; i < getNeuralNetwork().getNumberOfLayers(); ++i) {
getNeuralNetwork().getLayer(i).setActivationFunction(Neuron::AF_TANH);
}
}
int MailMessage::parseBodyParts(StringBuffer &rfcBody) {
BodyPart part;
// The boundary is the one defined in the headers preceded by
// a newline and two hypens
StringBuffer bound("\n--");
bound += boundary;
LOG.debug("parseBodyParts START");
size_t nextBoundary = rfcBody.find(bound);
getBodyPart(rfcBody, bound, body, nextBoundary, false);
if (contentType.ifind("multipart/alternative") == StringBuffer::npos) {
// If it's not multipart/alternative, get the other parts
while( getBodyPart(rfcBody, bound, part, nextBoundary, true) ) {
// some problem in the attachment?
if( part.getContent() ) {
attachments.add(part);
}
else LOG.error("Empty content in attachment.");
part = BodyPart();
}
}
LOG.debug("parseBodyParts END");
return 0;
}
BodyPart & Creature::getRoot() const {
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
if (getBodyPart(i).isRoot()) {
return getBodyPart(i);
}
}
assert(0);
}
void Creature::toXml(QDomDocument& xml, QDomElement * parent) const {
QDomElement creature = xml.createElement("Creature");
// creature.setAttribute("id", getId());
creature.setAttribute("name", QString(getName().c_str()));
// creature.setAttribute("type", QString::number(getType()));
creature.setAttribute("body-parts", getNumberOfBodyParts());
creature.setAttribute("constraints", getNumberOfConstraints());
creature.setAttribute("hidden-layers", getNeuralNetwork().getNumberOfLayers() - 2);
creature.setAttribute("neurons-per-layer", getNeuralNetwork().getLayer(1).getNumberOfNeurons());
creature.setAttribute("initial-position", QString((
TO_STRING(getInitialPosition().x()) + " ; " +
TO_STRING(getInitialPosition().y()) + " ; " +
TO_STRING(getInitialPosition().z())).c_str()));
creature.setAttribute("final-position", QString((
TO_STRING(getFinalPosition().x()) + " ; " +
TO_STRING(getFinalPosition().y()) + " ; " +
TO_STRING(getFinalPosition().z())).c_str()));
creature.setAttribute("fitness", getFitness());
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
getBodyPart(i).toXml(xml, &creature);
}
for (int i = 0; i < getNumberOfConstraints(); ++i) {
getConstraint(i).toXml(xml, &creature);
}
getNeuralNetwork().toXml(xml, &creature);
if (parent) {
parent->appendChild(creature);
} else {
xml.appendChild(creature);
}
}
void Creature::reset() {
for (int i = 0; i < getNumberOfConstraints(); ++i) {
getConstraint(i).reset();
}
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
getBodyPart(i).reset();
}
}
int Creature::getIndex(const btRigidBody * body) const {
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
if (getBodyPart(i).getRigidBody() == body) {
return i;
}
}
return -1;
}
int Creature::getIndex(const BodyPart & body) const {
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
if (&getBodyPart(i) == &body) {
return i;
}
}
return -1;
}
void Creature::onStep() {
btTransform trans;
int index = 0;
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
getBodyPart(i).getRigidBody()->getMotionState()->getWorldTransform(trans);
getNeuralNetwork().getInputLayer().getNeuron(index++).setInput(0, trans.getRotation().w());
getNeuralNetwork().getInputLayer().getNeuron(index++).setInput(0, trans.getRotation().x());
getNeuralNetwork().getInputLayer().getNeuron(index++).setInput(0, trans.getRotation().y());
getNeuralNetwork().getInputLayer().getNeuron(index++).setInput(0, trans.getRotation().z());
}
getNeuralNetwork().process();
btVector3 torque;
index = 0;
double mx = 0.0, my = 0.0, mz = 0.0, sum_m = 0.0;
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
if (getBodyPart(i).isRoot()) {
continue;
}
getBodyPart(i).getRigidBody()->activate(true);
mx = getNeuralNetwork().getOutputLayer().getNeuron(index++).output();
my = getNeuralNetwork().getOutputLayer().getNeuron(index++).output();
mz = getNeuralNetwork().getOutputLayer().getNeuron(index++).output();
// sum_m = Math::abs(mx) + Math::abs(mz);
// torque.setValue(mx * getBodyPart(i).getMaxTorque() / sum_m,
// my * 0.1 * getBodyPart(i).getMaxTorque(),
// mz * getBodyPart(i).getMaxTorque() / sum_m);
torque.setValue(mx * getBodyPart(i).getMaxTorque(),
my * 0.1 * getBodyPart(i).getMaxTorque(),
mz * getBodyPart(i).getMaxTorque());
getBodyPart(i).getRigidBody()->applyTorque(torque);
}
}
Item *BodyParts::getItemOnPart(const char *partName) const
{
BodyPart *part = getBodyPart(partName);
if (part)
{
return part->getEquippedItem();
}
return nullptr;
}
BodyPart* ResourceManager::getDefaultBody(int type)
{
switch (type)
{
case PART_BODY:
return getBodyPart(mDefaultBody);
case PART_HAIR:
return getBodyPart(mDefaultHair);
case PART_CHEST:
return getBodyPart(mDefaultChest);
case PART_LEGS:
return getBodyPart(mDefaultLegs);
case PART_FEET:
return getBodyPart(mDefaultFeet);
}
return NULL;
}
base::ReturnCode BodyParts::getLinkedParts(const char *linkedToName, PartList &result) const
{
if (!linkedToName || linkedToName[0] == '\0')
{
return base::NULL_PARAMETER;
}
BodyPart *part = getBodyPart(linkedToName);
if (!part)
{
return base::BODY_PART_NOT_FOUND;
}
return getLinkedParts(part, result);
}
//
// turn the snake left or right
//
void Serpent::turn( double angle, bool useRadians )
{
double angleRad = angle;
if( ! useRadians )
{
angleRad = angle * System::Math::PI / 180 ;
}
BodyPart * temp = getBodyPart(0);
if( temp )
{
temp->angleInRads += angleRad * turnSpeed * (1 + speedInfluenceOnTurn * sqrt(speed));
}
}
Creature * Creature::clone() const {
Creature* result = new Creature(getNumberOfBodyParts(), getNumberOfConstraints(),
getNeuralNetwork());
for (int i = 0; i < result->getNumberOfBodyParts(); ++i) {
result->setBodyPart(i, getBodyPart(i).clone());
}
for (int i = 0; i < result->getNumberOfConstraints(); ++i) {
result->setConstraint(i, getConstraint(i).clone());
}
result->setFinalPosition(getFinalPosition());
result->setInitialPosition(getInitialPosition());
result->setFitness(getFitness());
result->setName(getName());
return result;
}
Creature::Creature(int body_parts, int constraints, const NeuralNetwork & neural_network)
:
_name(""),
_number_of_body_parts(body_parts),
_number_of_constraints(constraints),
_body_parts(new BodyPart*[body_parts]),
_constraints(new Constraint*[constraints]),
_neural_network(NULL),
_initial_position(btVector3(0.0, 0.0, 0.0)),
_final_position(btVector3(0.0, 0.0, 0.0)),
_fitness(0.0) {
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
setBodyPart(i, new BodyPart());
getBodyPart(i).setName("Body Part #" + TO_STRING(i));
}
for (int i = 0; i < getNumberOfConstraints(); ++i) {
setConstraint(i, new Constraint());
getConstraint(i).setName("Constraint #" + TO_STRING(i));
}
setNeuralNetwork(neural_network.clone());
}
void Creature::scale(const btScalar factor) {
for (int i = 0; i < getNumberOfBodyParts(); ++i) {
getBodyPart(i).scaleSizeAndMass(factor);
}
}
BodyPart* ResourceManager::getFemaleBody()
{
return getBodyPart(mDefaultFemale);
}
void MatchCreature::set(const btTransform& at) {
getBodyPart(HEAD).setId(HEAD);
getBodyPart(HEAD).setName("Head");
getBodyPart(HEAD).setParentId(Creature::B_NONE);
getBodyPart(HEAD).setShape(BodyPart::S_SPHERE);
getBodyPart(HEAD).setSize(1.0, 1.0, 1.0);
getBodyPart(HEAD).setMass(1.0);
getBodyPart(STICK).setId(STICK);
getBodyPart(STICK).setName("Stick");
getBodyPart(STICK).setParentId(HEAD);
getBodyPart(STICK).setShape(BodyPart::S_CAPSULE_X);
getBodyPart(STICK).setSize(0.2, 2.0, 0.2);
getBodyPart(STICK).setMass(1.5);
getConstraint(NECK).setId(NECK);
getConstraint(NECK).setType(Constraint::CT_CONE_TWIST);
getConstraint(NECK).setName("Neck");
getConstraint(NECK).setIdBodyA(getBodyPart(HEAD).getId());
getConstraint(NECK).setIdBodyB(getBodyPart(STICK).getId());
getConstraint(NECK).setConnectionA(Constraint::SPHERE_BOTTOM);
getConstraint(NECK).setConnectionB(Constraint::CYLINDER_R3_FRONT);
getConstraint(NECK).setConeSwingX(0);
getConstraint(NECK).setConeTwistY(M_PI);
getConstraint(NECK).setConeSwingZ(0);
Creature::set(at);
}
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);
}
}
}
}
