GeometryPtr parseGeometry(TiXmlElement *g)
{
GeometryPtr geom;
if (!g) return geom;
TiXmlElement *shape = g->FirstChildElement();
if (!shape)
{
logError("Geometry tag contains no child element.");
return geom;
}
std::string type_name = shape->ValueStr();
if (type_name == "sphere")
{
Sphere *s = new Sphere();
resetPtr(geom,s);
if (parseSphere(*s, shape))
return geom;
}
else if (type_name == "box")
{
Box *b = new Box();
resetPtr(geom,b);
if (parseBox(*b, shape))
return geom;
}
else if (type_name == "cylinder")
{
Cylinder *c = new Cylinder();
resetPtr(geom,c);
if (parseCylinder(*c, shape))
return geom;
}
else if (type_name == "mesh")
{
Mesh *m = new Mesh();
resetPtr(geom,m);
if (parseMesh(*m, shape))
return geom;
}
else
{
logError("Unknown geometry type '%s'", type_name.c_str());
return geom;
}
return GeometryPtr();
}
bool exportGeometry(GeometryPtr &geom, TiXmlElement *xml)
{
TiXmlElement *geometry_xml = new TiXmlElement("geometry");
if( geom->type == Geometry::SPHERE )
{
exportSphere(*((Sphere*)toPlainGeometryPtr(geom)), geometry_xml);
}
else if ( geom->type == Geometry::BOX )
{
exportBox(*((Box*)toPlainGeometryPtr(geom)), geometry_xml);
}
else if (geom->type == Geometry::CYLINDER)
{
exportCylinder(*((Cylinder*)toPlainGeometryPtr(geom)), geometry_xml);
}
else if (geom->type == Geometry::MESH)
{
exportMesh(*((Mesh*)toPlainGeometryPtr(geom)), geometry_xml);
}
else
{
logError("geometry not specified, I'll make one up for you!");
Sphere *s = new Sphere();
s->radius = 0.03;
resetPtr(geom,s);
exportSphere(*s, geometry_xml);
}
xml->LinkEndChild(geometry_xml);
return true;
}
bool parseVisual(Visual &vis, TiXmlElement *config)
{
vis.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o) {
if (!parsePose(vis.origin, o))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
vis.geometry = parseGeometry(geom);
if (!vis.geometry)
return false;
const char *name_char = config->Attribute("name");
if (name_char)
vis.name = name_char;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat) {
// get material name
if (!mat->Attribute("name")) {
logError("Visual material must contain a name attribute");
return false;
}
vis.material_name = mat->Attribute("name");
// try to parse material element in place
resetPtr(vis.material,new Material());
if (!parseMaterial(*vis.material, mat, true))
{
logDebug("urdfdom: material has only name, actual material definition may be in the model");
}
}
return true;
}
void freeObjectList(cellStruct *pListHead) {
int var_2 = 0;
cellStruct *pCurrent = pListHead->next;
while (pCurrent) {
cellStruct *pNext = pCurrent->next;
if (pCurrent->freeze == 0) {
if (pCurrent->gfxPtr)
freeGfx(pCurrent->gfxPtr);
MemFree(pCurrent);
}
var_2 = 1;
pCurrent = pNext;
}
if (var_2) {
resetPtr(pListHead);
}
}
bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model)
{
robot_model.clear();
robot_model.name_ = robot_name;
//Add all links
KDL::SegmentMap::iterator seg;
KDL::SegmentMap segs;
KDL::SegmentMap::const_iterator root_seg;
root_seg = tree.getRootSegment();
segs = tree.getSegments();
for( seg = segs.begin(); seg != segs.end(); seg++ ) {
if (robot_model.getLink(seg->first))
{
std::cerr << "[ERR] link " << seg->first << " is not unique." << std::endl;
robot_model.clear();
return false;
}
else
{
urdf::LinkPtr link;
resetPtr(link, new urdf::Link);
//add name
link->name = seg->first;
//insert link
robot_model.links_.insert(make_pair(seg->first,link));
std::cerr << "[DEBUG] successfully added a new link " << link->name << std::endl;
}
//inserting joint
//The fake root segment has no joint to add
if( seg->first != root_seg->first ) {
KDL::Joint jnt;
jnt = GetTreeElementSegment(seg->second).getJoint();
if (robot_model.getJoint(jnt.getName()))
{
std::cerr << "[ERR] joint " << jnt.getName() << " is not unique." << std::endl;
robot_model.clear();
return false;
}
else
{
urdf::JointPtr joint;
urdf::LinkPtr link = robot_model.links_[seg->first];
//This variable will be set by toUrdf
KDL::Frame H_new_old_successor;
KDL::Frame H_new_old_predecessor = getH_new_old(GetTreeElementSegment(GetTreeElementParent(seg->second)->second));
urdf::resetPtr(joint, new urdf::Joint());
//convert joint
*joint = toUrdf(jnt, GetTreeElementSegment(seg->second).getFrameToTip(),H_new_old_predecessor,H_new_old_successor);
//insert parent
joint->parent_link_name = GetTreeElementParent(seg->second)->first;
//insert child
joint->child_link_name = seg->first;
//insert joint
robot_model.joints_.insert(make_pair(seg->first,joint));
std::cerr << "[DEBUG] successfully added a new joint" << jnt.getName() << std::endl;
//add inertial, taking in account an eventual change in the link frame
resetPtr(link->inertial, new urdf::Inertial());
*(link->inertial) = toUrdf(H_new_old_successor * GetTreeElementSegment(seg->second).getInertia());
}
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
//try
//{
robot_model.initTree(parent_link_tree);
//}
/*
catch(ParseError &e)
{
logError("Failed to build tree: %s", e.what());
robot_model.clear();
return false;
}*/
// find the root link
//try
//{
robot_model.initRoot(parent_link_tree);
//}
/*
catch(ParseError &e)
{
logError("Failed to find root link: %s", e.what());
robot_model.reset();
return false;
}
*/
return true;
}
void initVars() {
closeAllMenu();
resetFileEntryRange(0, NUM_FILE_ENTRIES);
resetPreload();
freeCTP();
freeBackgroundIncrustList(&backgroundIncrustHead);
freezeCell(&cellHead, -1, -1, -1, -1, -1, 0);
// TODO: unfreeze anims
freeObjectList(&cellHead);
removeAnimation(&actorHead, -1, -1, -1);
removeAllScripts(&relHead);
removeAllScripts(&procHead);
changeScriptParamInList(-1, -1, &procHead, -1, 0);
removeFinishedScripts(&procHead);
changeScriptParamInList(-1, -1, &relHead, -1, 0);
removeFinishedScripts(&relHead);
for (unsigned long int i = 0; i < 90; i++) {
if (strlen(overlayTable[i].overlayName) && overlayTable[i].alreadyLoaded) {
unloadOverlay(overlayTable[i].overlayName, i);
}
}
// TODO:
// stopSound();
// removeSound();
closeBase();
closeCnf();
initOverlayTable();
stateID = 0;
masterScreen = 0;
freeDisk();
soundList[0].frameNum = -1;
soundList[1].frameNum = -1;
soundList[2].frameNum = -1;
soundList[3].frameNum = -1;
for (unsigned long int i = 0; i < 8; i++) {
menuTable[i] = NULL;
}
for (unsigned long int i = 0; i < 2000; i++) {
globalVars[i] = 0;
}
for (unsigned long int i = 0; i < 8; i++) {
backgroundTable[i].name[0] = 0;
}
for (unsigned long int i = 0; i < NUM_FILE_ENTRIES; i++) {
filesDatabase[i].subData.ptr = NULL;
filesDatabase[i].subData.ptrMask = NULL;
}
initBigVar3();
resetPtr2(&procHead);
resetPtr2(&relHead);
resetPtr(&cellHead);
resetActorPtr(&actorHead);
resetBackgroundIncrustList(&backgroundIncrustHead);
vblLimit = 0;
remdo = 0;
songLoaded = 0;
songPlayed = 0;
songLoop = 1;
activeMouse = 0;
userEnabled = 1;
dialogueEnabled = 0;
dialogueOvl = 0;
dialogueObj = 0;
userDelay = 0;
sysKey = -1;
sysX = 0;
sysY = 0;
automoveInc = 0;
automoveMax = 0;
displayOn = true;
// here used to init clip
isMessage = 0;
fadeFlag = 0;
automaticMode = 0;
// video param (vga and mcga mode)
titleColor = 2;
itemColor = 1;
selectColor = 3;
subColor = 5;
//
narratorOvl = 0;
narratorIdx = 0;
aniX = 0;
aniY = 0;
animationStart = false;
selectDown = 0;
menuDown = 0;
buttonDown = 0;
var41 = 0;
playerMenuEnabled = 0;
PCFadeFlag = 0;
}
void CruiseEngine::initAllData() {
int i;
setupFuncArray();
initOverlayTable();
stateID = 0;
masterScreen = 0;
freeDisk();
soundList[0].frameNum = -1;
soundList[1].frameNum = -1;
soundList[2].frameNum = -1;
soundList[3].frameNum = -1;
menuTable[0] = NULL;
for (i = 0; i < 2000; i++) {
globalVars[i] = 0;
}
for (i = 0; i < 8; i++) {
backgroundTable[i].name[0] = 0;
}
for (i = 0; i < NUM_FILE_ENTRIES; i++) {
filesDatabase[i].subData.ptr = NULL;
filesDatabase[i].subData.ptrMask = NULL;
}
initBigVar3();
resetPtr2(&procHead);
resetPtr2(&relHead);
resetPtr(&cellHead);
resetActorPtr(&actorHead);
resetBackgroundIncrustList(&backgroundIncrustHead);
bootOverlayNumber = loadOverlay("AUTO00");
#ifdef DUMP_SCRIPT
loadOverlay("TITRE");
loadOverlay("TOM");
loadOverlay("XX2");
loadOverlay("SUPER");
loadOverlay("BEBE1");
loadOverlay("BIBLIO");
loadOverlay("BRACAGE");
loadOverlay("CONVERS");
loadOverlay("DAF");
loadOverlay("DAPHNEE");
loadOverlay("DESIRE");
loadOverlay("FAB");
loadOverlay("FABIANI");
loadOverlay("FIN");
loadOverlay("FIN01");
loadOverlay("FINBRAC");
loadOverlay("GEN");
loadOverlay("GENDEB");
loadOverlay("GIFLE");
loadOverlay("HECTOR");
loadOverlay("HECTOR2");
loadOverlay("I00");
loadOverlay("I01");
loadOverlay("I04");
loadOverlay("I06");
loadOverlay("I07");
loadOverlay("INVENT");
loadOverlay("JULIO");
loadOverlay("LOGO");
loadOverlay("MANOIR");
loadOverlay("MISSEL");
loadOverlay("POKER");
loadOverlay("PROJ");
loadOverlay("REB");
loadOverlay("REBECCA");
loadOverlay("ROS");
loadOverlay("ROSE");
loadOverlay("S01");
loadOverlay("S02");
loadOverlay("S03");
loadOverlay("S04");
loadOverlay("S06");
loadOverlay("S07");
loadOverlay("S08");
loadOverlay("S09");
loadOverlay("S10");
loadOverlay("S103");
loadOverlay("S11");
loadOverlay("S113");
loadOverlay("S12");
loadOverlay("S129");
loadOverlay("S131");
loadOverlay("S132");
loadOverlay("S133");
loadOverlay("int16");
loadOverlay("S17");
loadOverlay("S18");
loadOverlay("S19");
loadOverlay("S20");
loadOverlay("S21");
loadOverlay("S22");
loadOverlay("S23");
loadOverlay("S24");
loadOverlay("S25");
loadOverlay("S26");
loadOverlay("S27");
loadOverlay("S29");
loadOverlay("S30");
loadOverlay("S31");
loadOverlay("int32");
loadOverlay("S33");
loadOverlay("S33B");
loadOverlay("S34");
loadOverlay("S35");
loadOverlay("S36");
loadOverlay("S37");
loadOverlay("SHIP");
loadOverlay("SUPER");
loadOverlay("SUZAN");
loadOverlay("SUZAN2");
loadOverlay("TESTA1");
loadOverlay("TESTA2");
//exit(1);
#endif
if (bootOverlayNumber) {
positionInStack = 0;
attacheNewScriptToTail(&procHead, bootOverlayNumber, 0, 20, 0, 0, scriptType_PROC);
scriptFunc2(bootOverlayNumber, &procHead, 1, 0);
}
strcpy(lastOverlay, "AUTO00");
_gameSpeed = GAME_FRAME_DELAY_1;
_speedFlag = false;
return;
}
bool parseLink(Link &link, TiXmlElement* config)
{
link.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
logError("No name given for the link.");
return false;
}
link.name = std::string(name_char);
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
resetPtr(link.inertial,new Inertial());
if (!parseInertial(*link.inertial, i))
{
logError("Could not parse inertial element for Link [%s]", link.name.c_str());
return false;
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
VisualPtr vis;
resetPtr(vis,new Visual());
if (parseVisual(*vis, vis_xml))
{
link.visual_array.push_back(vis);
}
else
{
resetPtr(vis);
logError("Could not parse visual element for Link [%s]", link.name.c_str());
return false;
}
}
// Visual (optional)
// Assign the first visual to the .visual ptr, if it exists
if (!link.visual_array.empty())
link.visual = link.visual_array[0];
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
CollisionPtr col;
resetPtr(col,new Collision());
if (parseCollision(*col, col_xml))
{
link.collision_array.push_back(col);
}
else
{
resetPtr(col);
logError("Could not parse collision element for Link [%s]", link.name.c_str());
return false;
}
}
// Collision (optional)
// Assign the first collision to the .collision ptr, if it exists
if (!link.collision_array.empty())
link.collision = link.collision_array[0];
}
void Coor::attach(double* __x, int _len) {
x = __x;
len = _len;
resetPtr();
}
// ****************************************************************
// * COOR *
// ****************************************************************
Coor::Coor(double* __x, int _len) : x(__x), len(_len) {
resetPtr();
}
