void Epoll_Watcher::watcher_loop(void) {
int nfds = ::epoll_wait(this->epfd_, this->events_, this->max_events_, this->calculate_timeout());
if (nfds == -1) {
if (errno != EINTR)
LOG_SYS("epoll_wait");
return ;
}
GUARD(Mutex, mon, io_lock_);
if (end_flag_ == 1)
return ;
Event_Handler *evh = 0;
for (int i = 0; i < nfds; ++i) {
if ((evh = pending_io_map_[events_[i].data.fd]) != 0) {
if (events_[i].events & EPOLLIN) {
evh->handle_input();
if ((type_ & WITH_IO_HEARTBEAT) && (events_[i].data.fd != pipe_fd_[0])
&& (! (evh->get_io_flag() & EVENT_ONCE_IO_IN)) && (evh->get_heart_idx() != next_heart_idx())) {
io_heart_map_[evh->get_heart_idx()].erase(evh->get_fd());
io_heart_map_[next_heart_idx()].insert(std::make_pair(evh->get_fd(), evh));
evh->set_heart_idx(next_heart_idx());
}
}
}
if (events_[i].events & EPOLLOUT) {
evh->handle_output();
}
if ((evh->get_io_flag() & EVENT_ONCE_IO_IN) || (evh->get_io_flag() & EVENT_ONCE_IO_OUT)) { /// 清除一次性IO事件
pending_io_map_[events_[i].data.fd] = 0;
}
}
}
int Epoll_Watcher::notify(void) {
int ret = 0;
if ((ret = ::write(pipe_fd_[1], "a", 1)) < 0) {
LOG_SYS("write");
}
return ret;
}
int Epoll_Watcher::timer_open(void) {
if (pipe(pipe_fd_) < 0) {
LOG_SYS("pipe");
return -1;
}
set_nonblock(pipe_fd_[0]);
set_nonblock(pipe_fd_[1]);
this->set_fd(pipe_fd_[0]);
this->add(this, EVENT_INPUT);
return 0;
}
int Epoll_Watcher::io_open(void) {
if ((events_ = (struct epoll_event *)calloc(max_events_, sizeof(struct epoll_event))) == 0) {
LOG_SYS("realloc");
return -1;
}
if (type_ & WITH_IO_HEARTBEAT) {
Time_Value tv(heart_second_, 0);
this->add(this, EVENT_TIMEOUT, &tv);
}
return 0;
}
int main( int argc , char * argv[] )
{
if ( !CMDParameter::parse( argc , argv ) )
{
return 0;
}
// Server mode
if ( Variable::mode == 1 )
{
LOG_SYS( "system start in master mode" );
MRT::Maraton::instance( )->regist( make_uptr( NodeListener ,
"0.0.0.0" ,
100));
MRT::Maraton::instance( )->regist( make_uptr( ClientListener ,
"0.0.0.0" ,
101));
MRT::Maraton::instance( )->regist( make_uptr( MasterHTTPListener ,
"0.0.0.0" ,
80 ) );
MRT::Maraton::instance( )->run( );
LOG_SYS( "system shutdown" );
}
// Node mode
else if ( Variable::mode == 0 )
{
LOG_SYS( "system start in node mode" );
LOG_SYS( "load index file" );
BlockTable::instance( )->load_from_file( );
LOG_SYS( "connecting %:%" , Variable::server_ip.c_str( ) , Variable::port );
MRT::Maraton::instance( )->regist( make_uptr( ClientListener ,
"0.0.0.0" ,
101 ) );
MRT::Maraton::instance( )->regist( make_uptr( NodeHTTPListener ,
"0.0.0.0" ,
80 ) );
MRT::Maraton::instance( )->regist( make_uptr( MasterConnector ,
Variable::server_ip ,
100));
while ( true )
{
MRT::Maraton::instance( )->run( );
LOG_SYS( "disconnected to server , reconnecting" );
}
}
return 0;
}
int Epoll_Watcher::add_io(Event_Handler *evh, int op) {
GUARD(Mutex, mon, io_lock_);
if (end_flag_ == 1)
return -1;
if (evh->get_fd() == 0) {
LOG_USER_TRACE("fd == 0");
return -1;
}
struct epoll_event ev;
std::memset(&ev, 0, sizeof(ev));
if (op & EVENT_INPUT) {
evh->set_io_flag(EVENT_INPUT);
ev.events |= EPOLLIN;
if (op & EVENT_ONCE_IO_IN) {
evh->set_io_flag(EVENT_ONCE_IO_IN);
ev.events |= EPOLLONESHOT;
}
}
if (op & EVENT_OUTPUT) {
evh->set_io_flag(EVENT_OUTPUT);
ev.events |= EPOLLOUT;
if (op & EVENT_ONCE_IO_OUT)
evh->set_io_flag(EVENT_ONCE_IO_OUT);
ev.events |= EPOLLONESHOT;
}
if (ev.events)
ev.events |= EPOLLET;
ev.data.fd = evh->get_fd();
pending_io_map_[evh->get_fd()] = evh;
if (evh->get_fd() != pipe_fd_[0] && (type_ & WITH_IO_HEARTBEAT)) {
evh->set_heart_idx(next_heart_idx());
io_heart_map_[next_heart_idx()].insert(std::make_pair(evh->get_fd(), evh));
}
if (::epoll_ctl(this->epfd_, EPOLL_CTL_ADD, evh->get_fd(), &ev) == -1) {
LOG_SYS("epoll_ctl");
pending_io_map_[evh->get_fd()] = 0;
io_heart_map_[evh->get_heart_idx()].erase(evh->get_fd());
return -1;
}
return 0;
}
int Epoll_Watcher::open(void) {
pending_io_map_.resize(max_fd(), (Event_Handler *)0);
Heart_Map heart_map0(max_fd()), heart_map1(max_fd());
io_heart_map_[0].swap(heart_map0);
io_heart_map_[1].swap(heart_map1);
if ((this->epfd_ = ::epoll_create(1)) == -1) {
LOG_SYS("epoll_create");
return -1;
}
timer_open();
io_open();
return 0;
}
int Epoll_Watcher::handle_input(void) {
int ret = 0;
char tbuf[2048];
while (1) {
ret = ::read(pipe_fd_[0], tbuf, sizeof(tbuf));
if (ret == 0) {
LOG_SYS("read EOF");
break;
}
if (ret < 0) {
if (errno == EWOULDBLOCK)
break;
}
}
return 0;
}
int Epoll_Watcher::remove_io(Event_Handler *evh) {
GUARD(Mutex, mon, this->io_lock_);
/// 删除IO事件
int ret = 0;
if ((ret = ::epoll_ctl(epfd_, EPOLL_CTL_DEL, evh->get_fd(), NULL)) == -1) {
if (errno != EINTR)
LOG_SYS("epoll_ctl");
}
pending_io_map_[evh->get_fd()] = 0;
/// 删除IO心跳
if (type_ & WITH_IO_HEARTBEAT) {
for (size_t i = 0; i < (sizeof(io_heart_map_) / sizeof(io_heart_map_[0])); ++i) {
io_heart_map_[i].erase(evh->get_fd());
}
}
return 0;
}
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
}
}
}
bool CTReader::read()
{
ControllerInf *con = m_decoder.getController();
static bool start_sim = false;
static double timewidth = 0.0;
struct timeval start, eoa;
timeval now;
static double server_startTime = 0.0;
if (start_sim) {
//now = clock();
gettimeofday(&now, NULL);
double tmp_time = (double)(now.tv_sec - eoa.tv_sec) + (double)(now.tv_usec - eoa.tv_usec) * 0.001 * 0.001;
if (tmp_time >= timewidth) {
ActionEvent aevt;
double nowtime = (double)(now.tv_sec - start.tv_sec) + (double)(now.tv_usec - start.tv_usec) * 0.001 * 0.001;
nowtime += server_startTime;
aevt.setTime(nowtime);
timewidth = con->onAction(aevt);
gettimeofday(&eoa, NULL);
Controller *conn = (Controller*)con;
//conn->updateObjs();
}
}
SOCKET s = m_sock;
fd_set rfds;
struct timeval tv;
FD_ZERO(&rfds);
FD_SET(s, &rfds);
ControllerImpl* coni = (ControllerImpl*)con;
std::map<std::string, SOCKET> ssocks = coni->getSrvSocks();
std::map<std::string, SOCKET>::iterator it = ssocks.begin();
while (it != ssocks.end()) {
FD_SET((*it).second, &rfds);
it++;
}
tv.tv_sec = 0;
//tv.tv_usec = 100000;
tv.tv_usec = 1000;
int ret = select(FD_SETSIZE, &rfds, NULL, NULL, &tv);
if (ret == -1) {
perror("select");
return false;
}
else if (ret == 0) {
return true;
}
else if (ret > 0) {
if (FD_ISSET(s, &rfds)) {
int rbytes;
if (m_buf->datasize() == 0) {
rbytes = m_buf->read(s, 4);
#if 1
// sekikawa(FIX20100826)
if (rbytes < 0) {
if (errno == ECONNRESET) {
LOG_SYS(("connection closed by service provider [%s:%d]", __FILE__, __LINE__));
} else {
LOG_SYS(("socket error (errno=%d) [%s:%d]", errno, __FILE__, __LINE__));
}
throw ConnectionClosedException();
}
#endif
if (rbytes > 0) {
char *data = m_buf->data();
char *p = data;
unsigned short token = BINARY_GET_DATA_S_INCR(p, unsigned short);
if (token == COMM_DATA_PACKET_START_TOKEN) {
unsigned short size = BINARY_GET_DATA_S_INCR(p, unsigned short);
#if 1
// sekikawa(FIX20100826)
int rbytes2 = m_buf->read(s, size-4);
if (rbytes2 < 0) {
if (errno == ECONNRESET) {
LOG_SYS(("connection closed by service provider [%s:%d]", __FILE__, __LINE__));
} else {
LOG_SYS(("socket error (errno=%d) [%s:%d]", errno, __FILE__, __LINE__));
}
throw ConnectionClosedException();
}
rbytes += rbytes2;
/*
#else
// orig
rbytes += m_buf->read(s, size-4);
}
*/
#endif
}
else if (token == START_SIM) {
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);
}
