void Robots::AddRobot(std::string robotfilename)
{
Robot* robot = new Robot();
//
robot->fi = new ModRobotInterface();
robot->fi->loadAirplane(robotfilename.c_str(), (FDMEnviroment*)0, (SimpleXMLTransfer*)0);
if (robot->fi->robot)
{
SimpleXMLTransfer* header = robot->fi->robot->GetHeader();
std::string filename = FileSysTools::getDataPath(header->getString("airplane.file"));
SimpleXMLTransfer* xml = new SimpleXMLTransfer(filename);
XMLModelFile::SetGraphics(xml, header->getInt("airplane.graphics"));
SimpleXMLTransfer* graphics = XMLModelFile::getGraphics(xml);
//
robot->vis_id = Video::new_visualization("objects/" + graphics->attribute("model"),
"textures",
CRRCMath::Vector3(), // todo
xml);
list.push_back(robot);
}
}
/**
* Create a CRRCControlSurfaceAnimation object
*
* Initialize the animation from an
* <animation type="ControlSurface"> tag
*/
CRRCControlSurfaceAnimation::CRRCControlSurfaceAnimation(SimpleXMLTransfer *xml)
: CRRCAnimation(new ssgTransform()), fallback_data(0.0f),
eventAdapter(this, &CRRCControlSurfaceAnimation::axisValueCallback, Event::Input),
aileron(0.0f), elevator(0.0f), rudder(0.0f), throttle(0.0f),
spoiler(0.0f), flap(0.0f), retract(0.0f), pitch(0.0f)
{
bool failed = false;
// evaluate <object> tag
SimpleXMLTransfer *map = xml->getChild("object", true);
symbolic_name = map->getString("name", "no_name_set");
max_angle = (float)(map->getDouble("max_angle", 0.0) * SG_RADIANS_TO_DEGREES);
abs_max_angle = (float)fabs((double)max_angle);
// find hinges and evaluate all <control> tags
int num_controls = 0;
int num_hinges = 0;
for (int i = 0; i < xml->getChildCount(); i++)
{
SimpleXMLTransfer *child = xml->getChildAt(i);
if (child->getName() == "hinge")
{
// found a <hinge> child
sgVec3 pos;
pos[SG_X] = (float)(-1 * child->getDouble("y", 0.0));
pos[SG_Y] = (float)(-1 * child->getDouble("x", 0.0));
pos[SG_Z] = (float)(-1 * child->getDouble("z", 0.0));
if (num_hinges == 0)
{
sgCopyVec3(hinge_1, pos);
}
else if (num_hinges == 1)
{
sgCopyVec3(hinge_2, pos);
}
num_hinges++;
}
else if (child->getName() == "control")
{
// found a <control> child
// The "*2" factor for each gain value scales the control input
// values from -0.5...+0.5 to -1.0...+1.0. This saves one
// float multiplication per mapping in the runtime update() routine.
std::string mapping = child->getString("mapping", "NOTHING");
float gain = (float)child->getDouble("gain", 1.0);
if (mapping == "ELEVATOR")
{
datasource.push_back(&elevator);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "AILERON")
{
datasource.push_back(&aileron);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "THROTTLE")
{
datasource.push_back(&throttle);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "RUDDER")
{
datasource.push_back(&rudder);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "FLAP")
{
datasource.push_back(&flap);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "SPOILER")
{
datasource.push_back(&spoiler);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "RETRACT")
{
datasource.push_back(&retract);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "PITCH")
{
datasource.push_back(&pitch);
source_gain.push_back(gain * 2);
num_controls++;
}
else
{
std::cerr << "ControlSurfaceAnimation: ignoring <control> tag without mapping." << std::endl;
}
}
}
if (num_controls < 1)
{
std::cerr << "ControlSurfaceAnimation: found animation without proper <control> tag. Animation disabled." << std::endl;
failed = true;
}
if (num_hinges < 2)
{
std::cerr << "ControlSurfaceAnimation: Must specify exactly two hinges!" << std::endl;
failed = true;
}
else
{
if (num_hinges > 2)
{
std::cerr << "ControlSurfaceAnimation: Must specify exactly two hinges!" << std::endl;
std::cerr << "ControlSurfaceAnimation: Ignoring excessive hinge tag(s)." << std::endl;
}
sgSubVec3(axis, hinge_2, hinge_1);
if (sgLengthVec3(axis) < 0.001)
{
std::cerr << "ControlSurfaceAnimation: Insufficient spacing between hinges!" << std::endl;
failed = true;
}
}
if (failed)
{
std::cerr << "ControlSurfaceAnimation: Animation setup failed." << std::endl;
// set to some non-critical defaults
datasource.resize(1);
datasource[0] = &fallback_data;
source_gain.resize(1);
source_gain[0] = 1.0;
sgSetVec3(hinge_1, 0.0f, 0.0f, 0.0f);
sgSetVec3(hinge_2, 1.0f, 0.0f, 0.0f);
sgSubVec3(axis, hinge_2, hinge_1);
}
sgMakeIdentMat4(move_to_origin);
move_to_origin[3][0] = -hinge_1[0];
move_to_origin[3][1] = -hinge_1[1];
move_to_origin[3][2] = -hinge_1[2];
sgMakeTransMat4(move_back, hinge_1);
realInit();
}
/**
* Create a HardPointRotation
*
* \param xml <animation> part of the model file that contains the
* description of the animation
*/
HardPointRotation::HardPointRotation(SimpleXMLTransfer *xml, TSimInputs const& in)
{
bool failed = false;
// evaluate <object> tag
SimpleXMLTransfer *map = xml->getChild("object", true);
symbolic_name = map->getString("name", "no_name_set");
max_angle_rad = (float)map->getDouble("max_angle", 0.0);
abs_max_angle_rad = (float)fabs((double)max_angle_rad);
// find hinges and evaluate all <control> tags
int num_controls = 0;
int num_hinges = 0;
for (int i = 0; i < xml->getChildCount(); i++)
{
SimpleXMLTransfer *child = xml->getChildAt(i);
if (child->getName() == "hinge")
{
// found a <hinge> child
CRRCMath::Vector3 pos;
pos.r[0] = (float)(child->getDouble("x", 0.0));
pos.r[1] = (float)(child->getDouble("y", 0.0));
pos.r[2] = (float)(child->getDouble("z", 0.0));
if (num_hinges < 2)
{
hinge[num_hinges] = pos;
}
num_hinges++;
}
else if (child->getName() == "control")
{
// found a <control> child
// The "*2" factor for each gain value scales the control input
// values from -0.5...+0.5 to -1.0...+1.0. This saves one
// float multiplication per mapping in the runtime update() routine.
// NB: unsigned control (throttle, spoiler, retract) are not scaled,
// since control input is already in range 0.0...+1.0
std::string mapping = child->getString("mapping", "NOTHING");
float gain = (float)child->getDouble("gain", 1.0);
std::cout << " mapped to " << mapping << " with gain " << gain;
std::cout << " and max_angle_rad " << max_angle_rad << std::endl;
if (mapping == "ELEVATOR")
{
datasource.push_back(&in.elevator);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "AILERON")
{
datasource.push_back(&in.aileron);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "THROTTLE")
{
datasource.push_back(&in.throttle);
source_gain.push_back(gain);
num_controls++;
}
else if (mapping == "RUDDER")
{
datasource.push_back(&in.rudder);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "FLAP")
{
datasource.push_back(&in.flap);
source_gain.push_back(gain * 2);
num_controls++;
}
else if (mapping == "SPOILER")
{
datasource.push_back(&in.spoiler);
source_gain.push_back(gain);
num_controls++;
}
else if (mapping == "RETRACT")
{
datasource.push_back(&in.retract);
source_gain.push_back(gain);
num_controls++;
}
else if (mapping == "PITCH")
{
datasource.push_back(&in.pitch);
source_gain.push_back(gain * 2);
num_controls++;
}
else
{
fprintf(stderr, "HardPointRotation: ignoring <control> tag without mapping.\n");
}
}
}
if (num_controls < 1)
{
fprintf(stderr, "HardPointRotation: found animation without proper <control> tag. Animation disabled.\n");
failed = true;
}
if (num_hinges < 2)
{
fprintf(stderr, "HardPointRotation: Must specify exactly two hinges!\n");
failed = true;
}
else
{
if (num_hinges > 2)
{
fprintf(stderr, "HardPointRotation: Must specify exactly two hinges!\n");
fprintf(stderr, "HardPointRotation: Ignoring excessive hinge tag(s).\n");
}
axis = hinge[1] - hinge[0];
if (axis.length() < 0.001)
{
fprintf(stderr, "HardPointRotation: Insufficient spacing between hinges!\n");
failed = true;
}
}
if (failed)
{
fprintf(stderr, "HardPointRotation: Animation setup failed.\n");
// set to some non-critical defaults
datasource.resize(1);
datasource[0] = &fallback_data;
source_gain.resize(1);
source_gain[0] = 1.0;
hinge[0] = CRRCMath::Vector3(0.0, 0.0, 0.0);
hinge[1] = CRRCMath::Vector3(1.0, 0.0, 0.0);
axis = hinge[1] - hinge[0];
}
else
{
std::cerr << "HardPointRotation: set up animated hardpoint ";
std::cerr << symbolic_name << std::endl;
}
move_orig.makeTranslation(hinge[0] * -1);
move_back.makeTranslation(hinge[0]);
//~ realInit();
}
/**
* Initialize a WheelSystem from an XML file.
*
* \param ModelFile pointer to file class
* \param def_span default span if no hardpoints are found to calculate it
*/
void WheelSystem::init(SimpleXMLTransfer *ModelFile, SCALAR def_span)
{
Wheel wheel(this);
SimpleXMLTransfer *e, *i;
unsigned int uSize;
double x, y, z;
double dist;
double to_ft;
double to_lbf_per_ft;
double to_lbf_s_per_ft;
double to_lbf;
CRRCMath::Vector3 pCG;
/**
* Tracks wingspan [m]
*/
double span = 0;
span_ft = 0.0;
//
pCG = CRRCMath::Vector3(0, 0, 0);
if (ModelFile->indexOfChild("CG") >= 0)
{
i = ModelFile->getChild("CG");
pCG.r[0] = i->attributeAsDouble("x", 0);
pCG.r[1] = i->attributeAsDouble("y", 0);
pCG.r[2] = i->attributeAsDouble("z", 0);
if (i->attributeAsInt("units") == 1)
pCG *= M_TO_FT;
}
// let's assume that there is nothing below/above the CG:
dZLow = 0;
dZHigh = 0;
// let's assume that there is nothing distant from the CG:
dMaxSize = 0;
wheels.clear();
i = ModelFile->getChild("wheels");
switch (i->getInt("units"))
{
case 0:
to_ft = 1;
to_lbf_per_ft = 1;
to_lbf_s_per_ft = 1;
to_lbf = 1;
break;
case 1:
to_ft = M_TO_FT;
to_lbf_per_ft = FT_TO_M / LBF_TO_N;
to_lbf_s_per_ft = FT_TO_M / LBF_TO_N;
to_lbf = N_TO_LBF;
break;
default:
{
throw std::runtime_error("Unknown units in wheels");
}
break;
}
uSize = i->getChildCount();
for (unsigned int n=0; n<uSize; n++)
{
// we assign the child number as a unique ID for
// debugging
wheel.nID = n;
e = i->getChildAt(n);
x = e->getDouble("pos.x") * to_ft - pCG.r[0];
y = e->getDouble("pos.y") * to_ft - pCG.r[1];
z = e->getDouble("pos.z") * to_ft - pCG.r[2];
wheel.v_P = CRRCMath::Vector3(x, y, z);
// let's see if this wheel is coupled to an animation
wheel.anim_name = e->getString("pos.animation", "");
if (wheel.anim_name != "")
{
std::cout << "WheelSystem::init: hardpoint is coupled to anim ";
std::cout << wheel.anim_name << std::endl;
}
wheel.hpt = NULL;
if (ModelFile->indexOfChild("animations") >= 0)
{
SimpleXMLTransfer *a = ModelFile->getChild("animations");
unsigned int numAnims = a->getChildCount();
for (unsigned int animIndex = 0; animIndex < numAnims; animIndex++)
{
SimpleXMLTransfer *an = a->getChildAt(animIndex);
if (an->getString("object.name", "") == wheel.anim_name)
{
printf("Found %s animation for wheel %s\n",
an->getString("type", "<unknown>").c_str(),
wheel.anim_name.c_str());
wheel.hpt = new HardPointRotation(an, wheel_inputs);
break;
}
}
}
wheel.spring_constant = e->getDouble("spring.constant") * to_lbf_per_ft;
wheel.spring_damping = e->getDouble("spring.damping") * to_lbf_s_per_ft;
wheel.max_force = e->getDouble("spring.max_force", 9999) * to_lbf;
wheel.percent_brake = e->getDouble("percent_brake");
wheel.caster_angle_rad = e->getDouble("caster_angle_rad");
if (e->indexOfChild("steering") >= 0)
{
std::string s = e->getString("steering.mapping", "NOTHING");
wheel.steering_max_angle = e->getDouble("steering.max_angle", 1.0);
wheel.steering_mapping = XMLModelFile::GetSteering(s);
}
else
{
wheel.steering_mapping = TSimInputs::smNOTHING;
wheel.steering_max_angle = 0;
}
wheels.push_back(wheel);
// track wingspan
if (span < y)
span = y;
// lowest point?
if (dZLow < z)
dZLow = z;
// highest point?
if (dZHigh > z)
dZHigh = z;
// far away (Z distance is assumed to be low)?
dist = x*x + y*y;
if (dist > dMaxSize)
dMaxSize = dist;
}
dMaxSize = sqrt(dMaxSize);
span_ft = 2 * span;
// just in case: if there were no hardpoints, use the reference span
if (span_ft == 0.0)
{
span_ft = def_span;
}
}
/** \brief Add animations to a model
*
* This method reads animation description tags from a model file
* and tries to add the corresponding animations to the 3D model.
*
* \todo Right now there's only one type of animation: movable control
* surfaces. Therefore this method receives a pointer to the control
* input class. If animations are added that need a different kind of
* input for their update() method, we need to decide how to pass all
* this stuff to initAnimations().
*
* \param model_file XML model description file
* \param model scenegraph of the 3D model
* \param fInputs pointer to the control input class
* \param anim_list list of all created CRRCAnimation objects
*/
void initAnimations(SimpleXMLTransfer *model_file, ssgEntity* model,
TSimInputs *fInput, std::vector<CRRCAnimation*>& anim_list)
{
SimpleXMLTransfer *animations = model_file->getChild("animations", true);
int num_anims = animations->getChildCount();
fprintf(stdout, "initAnimations: found %d children\n", num_anims);
for (int i = 0; i < num_anims; i++)
{
SimpleXMLTransfer *animation = animations->getChildAt(i);
ssgEntity *node;
if (animation->getName() != "animation")
{
fprintf(stderr, "initAnimations: invalid child <%s>\n", animation->getName().c_str());
}
else
{
std::string node_name = animation->getString("object.name", "default");
std::string type = animation->getString("type", "default");
node = SSGUtil::findNamedNode(model, node_name.c_str());
if (node != NULL)
{
CRRCAnimation *anim = NULL;
printf("initAnimations: found animation node %s, type %s\n",
node_name.c_str(), type.c_str());
if (type == "ControlSurface")
{
anim = new CRRCControlSurfaceAnimation(animation, fInput);
}
else
{
fprintf(stderr, "initAnimations: unknown animation type '%s'\n", type.c_str());
}
if (anim != NULL)
{
if (anim->getBranch() == NULL)
{
fprintf(stderr, "initAnimations: defunct animation class (animation branch is <NULL>)\n");
exit(0);
}
else
{
SSGUtil::spliceBranch(anim->getBranch(), node);
anim->init();
anim->setName("Animation");
anim->getBranch()->setUserData(anim);
anim->getBranch()->setTravCallback(SSG_CALLBACK_PRETRAV, animation_callback);
anim_list.push_back(anim);
}
}
}
else
{
fprintf(stderr, "initAnimations: node '%s' not found in 3D model\n", node_name.c_str());
}
}
}
}