static void collideCB(void *data, dGeomID o1, dGeomID o2)
{
ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1);
ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2);
// const int N = 32;
const int N = 10; // TODO: Magic number should be removed
dContact contacts[N];
int n = dCollide(o1, o2, N, &(contacts[0].geom), sizeof(contacts[0]));
if (n > 0) {
ODEWorld *world = ODEWorld::get();
for (int i=0; i<n; i++) {
dContact *c = &contacts[i];
c->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//
// Reflection of material parameters of the collided object
// Fliction force should be regarded as average of contiguous material (???)
// TODO: Calclation of fliction force sould be considered
//
if (odeobj1 && odeobj2) {
c->surface.mu = ( odeobj1->getMu1() + odeobj2->getMu1() ) / 2.0;
c->surface.mu2 = ( odeobj1->getMu2() + odeobj2->getMu2() ) / 2.0;
c->surface.slip1 = ( odeobj1->getSlip1() + odeobj2->getSlip1() ) / 2.0;
c->surface.slip2 = ( odeobj1->getSlip2() + odeobj2->getSlip2() ) / 2.0;
c->surface.soft_erp = ( odeobj1->getSoftErp() + odeobj2->getSoftErp() ) / 2.0;
c->surface.soft_cfm = ( odeobj1->getSoftCfm() + odeobj2->getSoftCfm() ) / 2.0;
c->surface.bounce = ( odeobj1->getBounce() + odeobj2->getBounce() ) / 2.0;
}
else {
c->surface.mu = SPARTS_MU1;
c->surface.mu2 = SPARTS_MU2;
c->surface.slip1 = SPARTS_SLIP1;
c->surface.slip2 = SPARTS_SLIP2;
c->surface.soft_erp = SPARTS_ERP;
c->surface.soft_cfm = SPARTS_CFM;
c->surface.bounce = SPARTS_BOUNCE;
}
dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c);
dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2));
}
}
}
static void nearCB(void *data, dGeomID o1, dGeomID o2)
{
ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1);
ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2);
/*
SSimRobotEntity *ent1 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o1);
SSimRobotEntity *ent2 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o2);
if(ent1 != NULL && ent2 != NULL && ent1->name() == ent2->name()){
//LOG_MSG(("name (%s, %s)",ent1->name().c_str(), ent2->name().c_str()));
return;
}
*/
SParts *p1 = NULL;
SParts *p2 = NULL;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 == b2) {
return;
}
ODEWorld *world = ODEWorld::get();
dGeomID ground = world->getGround();
if (b1 && b2) {
if (dAreConnected(b1, b2)) {
return;
}
}
if (b1) {
void *d = dBodyGetData(b1);
if (d) {
p1 = (SParts*)d;
if (p1->isBlind()) { return; }
}
}
if (b2) {
void *d = dBodyGetData(b2);
if (d) {
p2 = (SParts*)d;
if (p2->isBlind()) { return; }
}
}
if (p1 && p2 && p1->sameParent(*p2)) { return; }
if (dGeomIsSpace(o1) && dGeomIsSpace(o2)) {
dSpaceCollide2(o1, o2, data, &collideCB);
return;
}
#define F_SCHOLAR(V) sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2])
static dJointFeedback fb;
#define MAX_COLLISION 32
const int N = MAX_COLLISION;
dContact contacts[N];
int n = dCollide(o1, o2, N, &(contacts[0].geom), sizeof(contacts[0]));
if (n > 0) {
ODEWorld *world = ODEWorld::get();
for (int i=0; i<n; i++) {
dContact *c = &contacts[i];
dContactGeom &g = c->geom;
if (p1 && p2) {
#if 0
LOG_SYS(("Collision #%d/%d %s(geomID=%d) <-> %s(geomID=%d)", i, n,
p1->getParent()->name(), o1,
p2->getParent()->name(), o2));
LOG_SYS(("\tpos = (%f, %f, %f)", g.pos[0], g.pos[1], g.pos[2]));
LOG_SYS(("\tnormal = (%f, %f, %f)", g.normal[0], g.normal[1], g.normal[2]));
LOG_SYS(("\tfdir = (%f, %f, %f)", c->fdir1[0], c->fdir1[1], c->fdir1[2]));
LOG_SYS(("\tdepth = %f", g.depth));
#endif
const char *name1 = p1->getParent()->name();
const char *name2 = p2->getParent()->name();
std::string key = chash_key(name1, name2);
if (key.length() <= 0) { continue; }
if (chash_find(key)) { continue; }
s_chash[key] = true;
s_collisions.push_back(ODEWorld::Collision(name1, name2, p1->name(), p2->name()));
// Set the collision flag to be ON
p1->setOnCollision(true);
p2->setOnCollision(true);
}
//c->surface.mode = dContactBounce;
c->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//c->surface.mode = dContactSlip1 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//
// Reflection of material parameters of the collided object
// Fliction force should be regarded as average of contiguous material (???)
// TODO: Calclation of fliction force sould be considered
//
#if 0
if (odeobj1 && odeobj2) {
c->surface.mu = ( odeobj1->getMu1() + odeobj2->getMu1() ) / 2.0;
c->surface.mu2 = ( odeobj1->getMu2() + odeobj2->getMu2() ) / 2.0;
c->surface.slip1 = ( odeobj1->getSlip1() + odeobj2->getSlip1() ) / 2.0;
c->surface.slip2 = ( odeobj1->getSlip2() + odeobj2->getSlip2() ) / 2.0;
c->surface.soft_erp = ( odeobj1->getSoftErp() + odeobj2->getSoftErp() ) / 2.0;
c->surface.soft_cfm = ( odeobj1->getSoftCfm() + odeobj2->getSoftCfm() ) / 2.0;
c->surface.bounce = ( odeobj1->getBounce() + odeobj2->getBounce() ) / 2.0;
}
else {
c->surface.bounce_vel = world->getCollisionParam("bounce_vel");
c->surface.bounce = world->getCollisionParam("bounce");
c->surface.mu = world->getCollisionParam("mu");
c->surface.mu2 = world->getCollisionParam("mu2");
c->surface.slip1 = world->getCollisionParam("slip1");
c->surface.slip2 = world->getCollisionParam("slip2");
c->surface.soft_erp = world->getCollisionParam("soft_erp");
c->surface.soft_cfm = world->getCollisionParam("soft_cfm");
}
#else
c->surface.mu = SPARTS_MU1;
c->surface.mu2 = SPARTS_MU2;
c->surface.slip1 = SPARTS_SLIP1;
c->surface.slip2 = SPARTS_SLIP2;
c->surface.soft_erp = SPARTS_ERP; // parameter for modify the error of Joint position (0..1); if it is 1, modification will be perfect
c->surface.soft_cfm = SPARTS_CFM; // If this value=0, joint velocity is strange
c->surface.bounce = SPARTS_BOUNCE; // is a little smaller than ball
c->surface.bounce_vel = 0.0;
#endif
dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c);
//dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2)); //by MSI
dJointAttach(cj, dGeomGetBody(c->geom.g1), dGeomGetBody(c->geom.g2)); // by Demura.net
#if 0
if (p1 && p2) {
dJointSetFeedback(cj, &fb);
dJointFeedback *pfb = dJointGetFeedback(cj);
if (F_SCHOLAR(pfb->f1) > 1.0) {
LOG_SYS(("\tF1 = (%f, %f, %f)", pfb->f1[0], pfb->f1[1], pfb->f1[2]));
LOG_SYS(("\tF2 = (%f, %f, %f)\n", pfb->f2[0], pfb->f2[1], pfb->f2[2]));
}
}
#endif
}
}
}
void WorldXMLReader::startElement(const XMLCh * const tagName_,
xercesc::AttributeList &attrs)
{
char *tagName = XMLString::transcode(tagName_);
// std::cout << tagName << std::endl;
if (strcmp(tagName, "world") == 0 || strcmp(tagName, "World") == 0) {
if (!m_world) {
if (char *n = GET_VALUE(attrs, "name")) {
m_world = new SSimWorld(n);
RELEASE(n);
}
else {
NOATTR_ERR("world", "name", attrs);
}
}
if (char *fname = GET_VALUE(attrs, "inherit")) {
SAXParser *parser = new SAXParser();
parser->setDocumentHandler(this);
char buf[4096];
const char *fpath = m_fdb.getPath(fname, buf);
if (fpath != NULL) {
S last = setFilename(fpath);
parser->parse(fpath);
delete parser;
setFilename(last);
} else {
NOFILE_ERR(fname);
}
RELEASE(fname);
}
} else if (strcmp(tagName, "gravity") == 0 || strcmp(tagName, "Gravity") == 0) {
if (m_world) {
dReal x=0.0, y=0.0, z=0.0;
char *p;
p = GET_VALUE(attrs, "x");
if (p) {
x = atof(p);
RELEASE(p);
} else {
NOATTR_ERR("gravity", "x", attrs);
}
p = GET_VALUE(attrs, "y");
if (p) {
y = atof(p);
RELEASE(p);
} else {
NOATTR_ERR("gravity", "y", attrs);
}
p = GET_VALUE(attrs, "z");
if (p) {
z = atof(p);
RELEASE(p);
} else {
NOATTR_ERR("gravity", "z", attrs);
}
#if 0
std::cout << "x = " << x << std::endl;
std::cout << "y = " << y << std::endl;
std::cout << "z = " << z << std::endl;
#endif
m_world->set(ODEWorld::Gravity(x, y, z), 0.0);
}
}
else if (strcmp(tagName, "worldparam") == 0 || strcmp(tagName, "worldParam") == 0) {
char *erp = GET_VALUE(attrs, "erp");
if (erp) {
m_world->setERP(atof(erp));
RELEASE(erp);
}
char *cfm = GET_VALUE(attrs, "cfm");
if (cfm) {
m_world->setCFM(atof(cfm));
RELEASE(cfm);
}
if (char *autostep = GET_VALUE(attrs, "autostep")) {
if (strcmp(autostep, "false") == 0) {
m_world->setAutoStep(false);
RELEASE(autostep);
}
}
if (char *quickstep = GET_VALUE(attrs, "quickstep")) {
if (strcmp(quickstep, "true") == 0) {
m_world->setQuickStep(true);
RELEASE(quickstep);
}
}
if (char *stepsize = GET_VALUE(attrs, "stepsize")) {
m_world->setStepSize(atof(stepsize));
RELEASE(stepsize);
}
}
else if (strcmp(tagName, "collisionparam") == 0 || strcmp(tagName, "collisionParam") == 0) {
char *mu = GET_VALUE(attrs, "mu");
if (mu) {
m_world->setCollisionParam("mu",atof(mu));
RELEASE(mu);
}
char *mu2 = GET_VALUE(attrs, "mu2");
if (mu2) {
m_world->setCollisionParam("mu2",atof(mu2));
RELEASE(mu2);
}
char *slip1 = GET_VALUE(attrs, "slip1");
if (slip1) {
m_world->setCollisionParam("slip1", atof(slip1));
RELEASE(slip1);
}
char *slip2 = GET_VALUE(attrs, "slip2");
if (slip2) {
m_world->setCollisionParam("slip2", atof(slip2));
RELEASE(slip2);
}
char *soft_erp = GET_VALUE(attrs, "soft_erp");
if (soft_erp) {
m_world->setCollisionParam("soft_erp", atof(soft_erp));
RELEASE(soft_erp);
}
char *soft_cfm = GET_VALUE(attrs, "soft_cfm");
if (soft_cfm) {
m_world->setCollisionParam("soft_cfm", atof(soft_cfm));
RELEASE(soft_cfm);
}
char *bounce = GET_VALUE(attrs, "bounce");
if (bounce) {
m_world->setCollisionParam("bounce", atof(bounce));
RELEASE(bounce);
}
char *bounce_vel = GET_VALUE(attrs, "bounce_vel");
if (bounce_vel) {
m_world->setCollisionParam("bounce_vel", atof(bounce_vel));
RELEASE(bounce);
}
}
else if (strcmp(tagName, "instanciate") == 0) {
ODEWorld *w = m_world->odeWorld();
SSimObj *obj;
// To check whether the type is robot or not
char *type = GET_VALUE(attrs, "type");
if (!type) obj = new SSimObj(w->space());
else if (strcmp(type,"Robot") == 0) {
SRobotObj *robj = new SRobotObj(w->space());
obj = (SSimObj*)robj;
RELEASE(type);
}
else{
obj = new SSimObj(w->space());
RELEASE(type);
}
if (char *fname = GET_VALUE(attrs, "class")) {
assert(m_world);
assert(w);
// Read contents of the entity from XML file
// EntityXMLReader read(m_fdb, *obj, *w, m_x3ddb);
EntityXMLReader read(m_fdb, *obj, *w, m_x3ddb, m_ssdb);
read.setReadTaskContainer(this);
read(fname);
}
else {
NOATTR_ERR("instanciate", "class", attrs);
}
m_currobj = obj;
}
else if (strcmp(tagName, "set-attr-value") == 0) {
if (m_currobj) {
char *n = GET_VALUE(attrs, "name");
char *v = GET_VALUE(attrs, "value");
if (!n) {
NOATTR_ERR("set-attr-value", "name", attrs);
}
if (!v) {
NOATTR_ERR("set-attr-value", "value", attrs);
}
if (n && v) {
m_currobj->setAttrValue(n, v);
}
}
}
// Creation of new entity
else if (strcmp(tagName, "entity") == 0 || strcmp(tagName, "Entity") == 0) {
ODEWorld *w = m_world->odeWorld();
// Set a new version flag later than v2.1
m_world->setV21(true);
// Create ODE world and space
dWorldID world = w->world();
dSpaceID space = w->space();
char *entityName = GET_VALUE(attrs, "name");
if (!entityName) {
LOG_ERR(("Entity has no name"));
assert(entityName);
}
SSimEntity *ent;
char *robot = GET_VALUE(attrs, "robot");
if (robot && strcmp(robot, "true") == 0) {
ent = new SSimRobotEntity(world, space, entityName);
ent->setIsRobot(true);
}
else{
ent = new SSimEntity(world, space, entityName);
}
static int eid = 0;
ent->setID(eid);
eid++;
// Check whether it is agent or normal entity
char *ag = GET_VALUE(attrs, "agent");
if (ag && strcmp(ag, "true") == 0) {
ent->setIsAgent(true);
}
m_current = ent;
}
else if (strcmp(tagName, "x3d") == 0 || strcmp(tagName, "X3D") == 0) {
if (m_current != NULL) {
char *scale = GET_VALUE(attrs, "scale");
Vector3d sc(1.0, 1.0, 1.0);
if (scale) {
char *scalex = strtok(scale, " ");
char *scaley = strtok(NULL, " ");
char *scalez = strtok(NULL, "");
if (scalex == NULL || scaley == NULL || scalez == NULL) {
LOG_ERR(("scale setting failed (%s)",m_current->name().c_str()));
}
else{
sc.set(atof(scalex), atof(scaley), atof(scalez));
}
}
m_current->setScale(sc);
}
// Find the target XML file from current directory or SIGVERSE_DATADIR
std::string tmp_fname = GET_VALUE(attrs, "filename");
std::string path = getenv("SIGVERSE_DATADIR");
std::string fname = "./" + tmp_fname;
FILE *fp;
if ((fp = fopen(fname.c_str(), "r")) == NULL) {
fname = path + "/shape/" + tmp_fname;
if ((fp = fopen(fname.c_str(), "r")) == NULL) {
LOG_ERR(("cannot find shape file. [%s]", fname.c_str()));
assert(fp != NULL);
}
}
m_current->setShapeFile(tmp_fname);
bool b = false;
// Preparation of JNI
char *cfg = getenv("SIGVERSE_X3DPARSER_CONFIG");
if (cfg == NULL || strlen(cfg) == 0) {
b = CJNIUtil::init("X3DParser.cfg");
}
else{
b = CJNIUtil::init(cfg);
}
if (!b) {
fprintf(stderr, "cannot read x3d config file");
exit(1);
}
CX3DParser parser;
parser.parse((char*)fname.c_str());
//sread.read(fname.c_str());
ODEWorld *w = m_world->odeWorld();
//SSimObjBuilder builder(*m_currobj, *w);
//m_current = dynamic_cast<SSimRobotEntity*>(m_current);
//SSimRobotEntity *tmp;
//m_current = dynamic_cast<SSimRobotEntity*>(m_current);
//m_current = tmp;
ShapeFileReader sread(m_current);
LOG_SYS(("Creating object \"%s\"",m_current->name().c_str()));
LOG_SYS(("Reading shape file [%s]", fname.c_str()));
if (m_current->isRobot()) {
if (!sread.createRobotObj(&parser)) {
LOG_ERR(("Failed to read robot shape file [%s]", fname.c_str()));
}
}
else{
if (!sread.createObj(&parser)) {
LOG_ERR(("Failed to read shape file [%s]", fname.c_str()));
}
}
} // else if (strcmp(tagName, "x3d") == 0 || strcmp(tagName, "X3D") == 0) {
else if (strcmp(tagName, "attribute") == 0 || strcmp(tagName, "Attribute") == 0) {
if (m_current) {
char *position = GET_VALUE(attrs, "position"); // entity position
char *direction = GET_VALUE(attrs, "direction"); // entity direction
char *mass = GET_VALUE(attrs, "mass"); // entity mass
char *collision = GET_VALUE(attrs, "collision"); // collision detection flag
char *quaternion= GET_VALUE(attrs, "quaternion"); // quaternion
if (position) {
char *x = strtok(position, " ");
char *y = strtok(NULL, " ");
char *z = strtok(NULL, "");
Vector3d pos(atof(x), atof(y), atof(z));
if (m_current->isRobot()) {
SSimRobotEntity *tmp_ent = (SSimRobotEntity*)m_current;
tmp_ent->setInitPosition(pos);
}
else{
m_current->setInitPosition(pos);
}
}
//[ToDo]
if (direction) {
}
//[ToDo]
if (mass) {
m_current->setMass(atof(mass));
}
if (collision) {
if (strcmp(collision, "true") == 0) {
if (m_current->isRobot()) {
SSimRobotEntity *tmp_ent = (SSimRobotEntity*)m_current;
tmp_ent->setCollision(true);
}
else{
m_current->setCollision(true);
}
}
}
//[Todo]
if (quaternion) {
}
}
}
// Added by okamoto on 2012-08-11
// Reading and setting of camera parameter
else if (strcmp(tagName, "camera") == 0 || strcmp(tagName, "Camera") == 0) {
if (m_currobj) {
char *cid = GET_VALUE(attrs, "id"); // id number
char *link = GET_VALUE(attrs, "link"); // link name
char *fov = GET_VALUE(attrs, "fov"); // field of view
char *as = GET_VALUE(attrs, "aspectRatio"); // aspect ratio
int iid = -1;
std::string id = cid;
double dfov, das;
// Whether value is specified by users
bool isid = false;
bool islink = false;
bool isfov = false;
bool isas = false;
if (link) islink = true;
if (fov) isfov = true;
if (as) isas = true;
if (!cid) {
LOG_ERR(("Cannot find camera ID."));
}
else {
isid = true;
iid = atoi(cid);
}
// Add camera ID
m_currobj->addCameraID(iid);
// Setting of camera parameters
Value *vfov = new DoubleValue();
Value *vas = new DoubleValue();
Value *vlink = new StringValue();
// Setting of each attributions
std::string sfov = "FOV" + id;
std::string sas = "aspectRatio" + id;
std::string slink = "elnk" + id;
vfov ->setString(sfov. c_str());
vas ->setString(sas. c_str());
vlink->setString(slink.c_str());
// Add attribution info to entity
m_currobj->push(new Attribute(sfov, vfov, "camera"));
m_currobj->push(new Attribute(sas, vas, "camera"));
if (iid > 2)
m_currobj->push(new Attribute(slink, vlink, "camera"));
if (isfov)
m_currobj->setAttrValue(sfov.c_str(), fov);
else
m_currobj->setAttrValue(sfov.c_str(), "45.0"); // default value
if (isas)
m_currobj->setAttrValue(sas.c_str(), as);
else
m_currobj->setAttrValue(sas.c_str(), "1.5"); // default value
if (islink)
m_currobj->setAttrValue(slink.c_str(), link);
else
m_currobj->setAttrValue(slink.c_str(), "body");// default value
char *position = GET_VALUE(attrs, "position");
std::string epx = "epx" + id;
std::string epy = "epy" + id;
std::string epz = "epz" + id;
if (position) {
std::string x = strtok(position, " ");
std::string y = strtok(NULL, " ");
std::string z = strtok(NULL, "");
Vector3d pos(atof(x.c_str()), atof(y.c_str()), atof(z.c_str()));
if (iid > 2) {
Value *v_x = new DoubleValue();
Value *v_y = new DoubleValue();
Value *v_z = new DoubleValue();
v_x->setString(epx.c_str());
v_y->setString(epy.c_str());
v_z->setString(epz.c_str());
// [Comment] Is it OK to execute new here?
m_currobj->push(new Attribute(epx, v_x, "camera"));
m_currobj->push(new Attribute(epy, v_y, "camera"));
m_currobj->push(new Attribute(epz, v_z, "camera"));
}
m_currobj->setAttrValue(epx.c_str(), x.c_str());
m_currobj->setAttrValue(epy.c_str(), y.c_str());
m_currobj->setAttrValue(epz.c_str(), z.c_str());
RELEASE(position);
}
else {
// Default values
m_currobj->setAttrValue(epx.c_str(), "0.0");
m_currobj->setAttrValue(epy.c_str(), "0.0");
m_currobj->setAttrValue(epz.c_str(), "0.0");
}
// camera direction
char *direction = GET_VALUE(attrs, "direction");
char *quaternion = GET_VALUE(attrs, "quaternion"); // aspect ratio
std::string evx = "evx" + id;
std::string evy = "evy" + id;
std::string evz = "evz" + id;
std::string quw = "quw" + id;
std::string qux = "qux" + id;
std::string quy = "quy" + id;
std::string quz = "quz" + id;
if (direction && quaternion) {
LOG_MSG(("cannot set camera quaternion and direction simultaneously"));
}
if (direction) {
std::string vx = strtok(direction, " ");
std::string vy = strtok(NULL, " ");
std::string vz = strtok(NULL, "");
Vector3d dir(atof(vx.c_str()), atof(vy.c_str()), atof(vz.c_str()));
if (iid > 2) {
Value *v_x = new DoubleValue();
Value *v_y = new DoubleValue();
Value *v_z = new DoubleValue();
v_x->setString(evx.c_str());
v_y->setString(evy.c_str());
v_z->setString(evz.c_str());
// [Comment] Is it OK to execute new here?
m_currobj->push(new Attribute(evx, v_x, "camera"));
m_currobj->push(new Attribute(evy, v_y, "camera"));
m_currobj->push(new Attribute(evz, v_z, "camera"));
}
m_currobj->setAttrValue(evx.c_str(), vx.c_str());
m_currobj->setAttrValue(evy.c_str(), vy.c_str());
m_currobj->setAttrValue(evz.c_str(), vz.c_str());
RELEASE(direction);
}
else {
Vector3d dir(0.0, 0.0, 1.0);
m_currobj->setAttrValue(evx.c_str(), "0.0");
m_currobj->setAttrValue(evy.c_str(), "0.0");
m_currobj->setAttrValue(evz.c_str(), "1.0");
}
Value *q_w = new DoubleValue();
Value *q_x = new DoubleValue();
Value *q_y = new DoubleValue();
Value *q_z = new DoubleValue();
q_w->setString(quw.c_str());
q_x->setString(qux.c_str());
q_y->setString(quy.c_str());
q_z->setString(quz.c_str());
m_currobj->push(new Attribute(quw, q_w, "camera"));
m_currobj->push(new Attribute(qux, q_x, "camera"));
m_currobj->push(new Attribute(quy, q_y, "camera"));
m_currobj->push(new Attribute(quz, q_z, "camera"));
if (quaternion) {
std::string qw = strtok(quaternion, " ");
std::string qx = strtok(NULL, " ");
std::string qy = strtok(NULL, " ");
std::string qz = strtok(NULL, "");
m_currobj->setAttrValue(quw.c_str(), qw.c_str());
m_currobj->setAttrValue(qux.c_str(), qx.c_str());
m_currobj->setAttrValue(quy.c_str(), qy.c_str());
m_currobj->setAttrValue(quz.c_str(), qz.c_str());
RELEASE(quaternion);
}
else {
m_currobj->setAttrValue(quw.c_str(), "1.0");
m_currobj->setAttrValue(qux.c_str(), "0.0");
m_currobj->setAttrValue(quy.c_str(), "0.0");
m_currobj->setAttrValue(quz.c_str(), "0.0");
}
//RELEASE(id.c_str());
if (isid) RELEASE(cid);
if (islink) RELEASE(link);
if (isfov) RELEASE(fov);
if (isas) RELEASE(as);
} // if (m_currobj)
}
RELEASE(tagName);
}
