void ATemplateNodeHandler_RESOURCE::Node::process(ATemplateContext& ctx, AOutputBuffer& output)
{
AAutoPtr<AEventVisitor::ScopedEvent> scoped;
if (ctx.useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
{
scoped.reset(new AEventVisitor::ScopedEvent(ctx.useEventVisitor(), ASW("ATemplateNodeHandler_RESOURCE",28), m_BlockData, AEventVisitor::EL_DEBUG));
}
if (m_BlockData.isEmpty())
return;
// Get resource
AOSContext *pContext = ctx.useObjects().useAsPtr<AOSContext>(AOSContext::OBJECTNAME);
if (!pContext)
{
ARope rope("Must have AOSContext object named: ",35);
rope.append(AOSContext::OBJECTNAME);
ATHROW_EX(this, AException::NotFound, rope);
}
LIST_AString languages;
pContext->useRequestHeader().getAcceptLanguageList(languages);
const AXmlDocument *pDoc = pContext->useServices().useResourceManager().getResources(languages);
if (!pDoc)
{
if (pContext->useEventVisitor().isLogging(AEventVisitor::EL_WARN))
{
pContext->useEventVisitor().startEvent(ASW("Unable to find resource container for locales in Accept-Languages",65), AEventVisitor::EL_WARN);
}
return;
}
const AXmlElement *pElement = pDoc->getRoot().findElement(m_BlockData);
if (pElement)
{
//a_Found object
pElement->emitContent(output);
}
else
{
if (pContext->useEventVisitor().isLogging(AEventVisitor::EL_WARN))
{
ARope rope("Unable to find resource element: ",33);
rope.append(m_BlockData);
pContext->useEventVisitor().startEvent(rope, AEventVisitor::EL_WARN);
}
}
}
void AOSContextQueue_IsAvailable::add(AOSContext *pContext)
{
AASSERT(NULL, pContext);
//a_Clear these in case this is not the first trip here
//a_More than one trip can result if some bytes were read but not enough
pContext->useConnectionFlags().clearBit(AOSContext::CONFLAG_ISAVAILABLE_SELECT_SET);
pContext->useConnectionFlags().clearBit(AOSContext::CONFLAG_ISAVAILABLE_PENDING);
volatile long currentQueue = ::InterlockedIncrement(&m_currentQueue) % m_queueCount;
{
ALock lock(m_Queues.at(currentQueue)->sync);
//a_Start timeout timer
pContext->useTimeoutTimer().start();
if (pContext->useEventVisitor().isLoggingDebug())
{
ARope rope(getClass());
rope.append("::add[",6);
rope.append(AString::fromInt(currentQueue));
rope.append("]=",2);
rope.append(AString::fromPointer(pContext));
pContext->useEventVisitor().addEvent(rope, AEventVisitor::EL_DEBUG);
}
m_Queues.at(currentQueue)->queue.pushBack(pContext);
++m_Queues.at(currentQueue)->count;
}
}
void ATemplateNodeHandler_MODEL::Node::process(ATemplateContext& context, AOutputBuffer& output)
{
AAutoPtr<AEventVisitor::ScopedEvent> scoped;
if (context.useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
{
scoped.reset(new AEventVisitor::ScopedEvent(context.useEventVisitor(), ASW("ATemplateNodeHandler_MODEL",26), m_BlockData, AEventVisitor::EL_DEBUG));
}
if (m_BlockData.isEmpty())
return;
AXmlElement *pElement = context.useModel().useRoot().findElement(m_BlockData);
if (pElement)
{
//a_Found object
pElement->emitContent(output);
}
else
{
ARope rope("<!--Unable to find element for '",32);
rope.append(m_BlockData);
rope.append("'-->",4);
output.append(rope);
}
}
void CLaser::PostRender() const
{
BaseClass::PostRender();
if (!GameServer()->GetRenderer()->IsRenderingTransparent())
return;
if (!m_bShouldRender)
return;
if (!m_hOwner)
return;
CRenderingContext r(DigitanksGame()->GetDigitanksRenderer(), true);
r.SetBlend(BLEND_ADDITIVE);
Vector vecForward, vecRight, vecUp;
float flLength = LaserLength();
CDigitank* pOwner = dynamic_cast<CDigitank*>(GetOwner());
Vector vecMuzzle = m_hOwner->GetGlobalOrigin();
Vector vecTarget = vecMuzzle + AngleVector(GetGlobalAngles()) * flLength;
if (pOwner)
{
Vector vecDirection = (pOwner->GetLastAim() - pOwner->GetGlobalOrigin()).Normalized();
vecTarget = vecMuzzle + vecDirection * flLength;
AngleVectors(VectorAngles(vecDirection), &vecForward, &vecRight, &vecUp);
vecMuzzle = pOwner->GetGlobalOrigin() + vecDirection * 3 + Vector(0, 0, 3);
}
float flBeamWidth = 1.5;
Vector avecRayColors[] =
{
Vector(1, 0, 0),
Vector(0, 1, 0),
Vector(0, 0, 1),
};
float flRayRamp = RemapValClamped((float)(GameServer()->GetGameTime() - GetSpawnTime()), 0.5f, 1.5f, 0.0f, 1);
float flAlphaRamp = RemapValClamped((float)(GameServer()->GetGameTime() - GetSpawnTime()), 1, 2, 1.0f, 0);
size_t iBeams = 21;
for (size_t i = 0; i < iBeams; i++)
{
float flUp = RemapVal((float)i, 0, (float)iBeams, -flLength, flLength);
Vector vecRay = LerpValue<Vector>(Vector(1, 1, 1), avecRayColors[i%3], flRayRamp);
Color clrRay = vecRay;
clrRay.SetAlpha((int)(200*flAlphaRamp));
r.SetColor(clrRay);
CRopeRenderer rope(DigitanksGame()->GetDigitanksRenderer(), s_hBeam, vecMuzzle, flBeamWidth);
rope.SetTextureOffset(((float)i/20) - GameServer()->GetGameTime() - GetSpawnTime());
rope.Finish(vecTarget + vecUp*flUp);
}
}
AOSContext *AOSContextManager::allocate(AFile_Socket *pSocket)
{
AASSERT(this, pSocket);
AOSContext *p = _newContext(pSocket);
//a_Set logging level
switch(m_DefaultEventLogLevel)
{
case AEventVisitor::EL_DEBUG: p->useEventVisitor().setEventThresholdLevel(AEventVisitor::EL_DEBUG); break;
case AEventVisitor::EL_INFO: p->useEventVisitor().setEventThresholdLevel(AEventVisitor::EL_INFO); break;
case AEventVisitor::EL_WARN: p->useEventVisitor().setEventThresholdLevel(AEventVisitor::EL_WARN); break;
case AEventVisitor::EL_ERROR: p->useEventVisitor().setEventThresholdLevel(AEventVisitor::EL_ERROR); break;
case AEventVisitor::EL_NONE: p->useEventVisitor().setEventThresholdLevel(AEventVisitor::EL_NONE); break;
default:
p->useEventVisitor().setEventThresholdLevel(AEventVisitor::EL_EVENT);
}
ARope rope("AOSContextManager::allocate[",28);
rope.append(AString::fromPointer(p));
rope.append(", pFile=",8);
rope.append(AString::fromPointer(pSocket));
rope.append("] new create",12);
p->useEventVisitor().startEvent(rope);
{
ALock lock(m_InUseSync);
AASSERT(this, p && m_InUse.end() == m_InUse.find(p));
m_InUse.insert(p);
}
AASSERT(this, &p->useSocket());
return p;
}
void testPeek()
{
AFile_AString strfile;
ARope rope(ASWNL("0123456789"), 4);
size_t r;
r = rope.peek(strfile, 3);
std::cout << r << ":" << strfile << std::endl;
AString str;
r = rope.peek(str);
std::cout << r << ":" << str << std::endl;
str.clear();
r = rope.peek(str, 7, 2);
std::cout << r << ":" << str << std::endl;
str.clear();
r = rope.peek(str, 7, 0);
std::cout << r << ":" << str << std::endl;
str.clear();
r = rope.peek(str, 1, 50);
std::cout << r << ":" << str << std::endl;
str.clear();
r = rope.peek(str, 10, 1);
std::cout << r << ":" << str << std::endl;
}
void unroll_digging( int const numer_of_2x4s )
{
// refund components!
item rope( "rope_30" );
g->m.add_item_or_charges( g->u.pos(), rope );
// presuming 2x4 to conserve lumber.
g->m.spawn_item( g->u.pos(), "2x4", numer_of_2x4s );
}
void testRope()
{
ARope rope("TextBlock0000", 8);
rope.append("TextBlock0123456789");
AString str;
rope.emit(str);
std::cout << str << std::endl;
std::cout << rope << std::endl;
ARope ropeCopy(rope);
ropeCopy.emit(str);
std::cout << str << std::endl;
std::cout << ropeCopy << std::endl;
}
bool Character::move_effects(bool attacking)
{
if (has_effect("downed")) {
///\EFFECT_DEX increases chance to stand up when knocked down
///\EFFECT_STR increases chance to stand up when knocked down, slightly
if (rng(0, 40) > get_dex() + get_str() / 2) {
add_msg_if_player(_("You struggle to stand."));
} else {
add_msg_player_or_npc(m_good, _("You stand up."),
_("<npcname> stands up."));
remove_effect("downed");
}
return false;
}
if (has_effect("webbed")) {
///\EFFECT_STR increases chance to escape webs
if (x_in_y(get_str(), 6 * get_effect_int("webbed"))) {
add_msg_player_or_npc(m_good, _("You free yourself from the webs!"),
_("<npcname> frees themselves from the webs!"));
remove_effect("webbed");
} else {
add_msg_if_player(_("You try to free yourself from the webs, but can't get loose!"));
}
return false;
}
if (has_effect("lightsnare")) {
///\EFFECT_STR increases chance to escape light snare
///\EFFECT_DEX increases chance to escape light snare
if(x_in_y(get_str(), 12) || x_in_y(get_dex(), 8)) {
remove_effect("lightsnare");
add_msg_player_or_npc(m_good, _("You free yourself from the light snare!"),
_("<npcname> frees themselves from the light snare!"));
item string("string_36", calendar::turn);
item snare("snare_trigger", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), string);
g->m.add_item_or_charges(posx(), posy(), snare);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the light snare, but can't get loose!"));
}
return false;
}
if (has_effect("heavysnare")) {
///\EFFECT_STR increases chance to escape heavy snare, slightly
///\EFFECT_DEX increases chance to escape light snare
if(x_in_y(get_str(), 32) || x_in_y(get_dex(), 16)) {
remove_effect("heavysnare");
add_msg_player_or_npc(m_good, _("You free yourself from the heavy snare!"),
_("<npcname> frees themselves from the heavy snare!"));
item rope("rope_6", calendar::turn);
item snare("snare_trigger", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), rope);
g->m.add_item_or_charges(posx(), posy(), snare);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the heavy snare, but can't get loose!"));
}
return false;
}
if (has_effect("beartrap")) {
/* Real bear traps can't be removed without the proper tools or immense strength; eventually this should
allow normal players two options: removal of the limb or removal of the trap from the ground
(at which point the player could later remove it from the leg with the right tools).
As such we are currently making it a bit easier for players and NPC's to get out of bear traps.
*/
///\EFFECT_STR increases chance to escape bear trap
if(x_in_y(get_str(), 100)) {
remove_effect("beartrap");
add_msg_player_or_npc(m_good, _("You free yourself from the bear trap!"),
_("<npcname> frees themselves from the bear trap!"));
item beartrap("beartrap", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), beartrap);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the bear trap, but can't get loose!"));
}
return false;
}
if (has_effect("crushed")) {
///\EFFECT_STR increases chance to escape crushing rubble
///\EFFECT_DEX increases chance to escape crushing rubble, slightly
if(x_in_y(get_str() + get_dex() / 4, 100)) {
remove_effect("crushed");
add_msg_player_or_npc(m_good, _("You free yourself from the rubble!"),
_("<npcname> frees themselves from the rubble!"));
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the rubble, but can't get loose!"));
}
return false;
}
// Below this point are things that allow for movement if they succeed
// Currently we only have one thing that forces movement if you succeed, should we get more
// than this will need to be reworked to only have success effects if /all/ checks succeed
if (has_effect("in_pit")) {
///\EFFECT_STR increases chance to escape pit
///\EFFECT_DEX increases chance to escape pit, slightly
if (rng(0, 40) > get_str() + get_dex() / 2) {
add_msg_if_player(m_bad, _("You try to escape the pit, but slip back in."));
return false;
} else {
add_msg_player_or_npc(m_good, _("You escape the pit!"),
_("<npcname> escapes the pit!"));
remove_effect("in_pit");
}
}
if( has_effect( "grabbed" ) && !attacking ) {
int zed_number = 0;
for( auto &&dest : g->m.points_in_radius( pos(), 1, 0 ) ){
if( g->mon_at( dest ) != -1 &&
( g->zombie( g->mon_at( dest ) ).has_flag( MF_GRABS ) ||
g->zombie( g->mon_at( dest ) ).type->has_special_attack( "GRAB" ) ) ) {
zed_number ++;
}
}
if( zed_number == 0 ) {
add_msg_player_or_npc( m_good, _( "You find yourself no longer grabbed." ),
_( "<npcname> finds themselves no longer grabbed." ) );
remove_effect( "grabbed" );
///\EFFECT_DEX increases chance to escape grab, if >STR
///\EFFECT_STR increases chance to escape grab, if >DEX
} else if( rng( 0, std::max( get_dex(), get_str() ) ) < rng( get_effect_int( "grabbed" ), 8 ) ) {
// Randomly compare higher of dex or str to grab intensity.
add_msg_player_or_npc( m_bad, _( "You try break out of the grab, but fail!" ),
_( "<npcname> tries to break out of the grab, but fails!" ) );
return false;
} else {
add_msg_player_or_npc( m_good, _( "You break out of the grab!" ),
_( "<npcname> breaks out of the grab!" ) );
remove_effect( "grabbed" );
}
}
return Creature::move_effects( attacking );
}
u4 AOSContextQueue_IsAvailable::_threadprocWorker(AThread& thread)
{
AOSContextQueue_IsAvailable::QueueWithThread *pThis = dynamic_cast<AOSContextQueue_IsAvailable::QueueWithThread *>(thread.getThis());
AOSContextQueue_IsAvailable *pOwner = dynamic_cast<AOSContextQueue_IsAvailable *>(thread.getParameter());
AASSERT(&thread, pThis);
AASSERT(&thread, pOwner);
timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 0;
thread.setRunning(true);
fd_set sockSet;
while (thread.isRun())
{
try
{
if (pThis->queue.size() > 0)
{
FD_ZERO(&sockSet);
int count = 0;
// Convert to ABasePtrQueue
ABase *p = pThis->queue.useHead();
while (count < FD_SETSIZE && p)
{
AOSContext *pContext = (AOSContext *)p;
FD_SET((SOCKET)pContext->useSocket().getSocketInfo().m_handle, &sockSet);
++count;
p = p->useNext();
}
if (count > 0)
{
int availCount = ::select(count, &sockSet, NULL, NULL, &timeout);
if (availCount > 0)
{
int count2 = 0;
p = pThis->queue.useHead();
while (availCount > 0 && count2 <= count && p)
{
AOSContext *pContext = (AOSContext *)p;
p = p->useNext();
if (FD_ISSET(pContext->useSocket().getSocketInfo().m_handle, &sockSet))
{
pContext->useConnectionFlags().setBit(AOSContext::CONFLAG_ISAVAILABLE_SELECT_SET);
//a_Attempt to read some data to check if it actually has data, if not then connection is closed
size_t bytesRead = AConstant::npos;
try
{
bytesRead = pContext->useSocket().readBlockIntoLookahead();
if (!bytesRead || AConstant::npos == bytesRead)
{
//a_No data to read, socket is closed
pContext->useConnectionFlags().setBit(AOSContext::CONFLAG_IS_SOCKET_CLOSED);
pContext->useEventVisitor().startEvent(ASW("AOSContextQueue_IsAvailable: Detected closed remote socket",58), AEventVisitor::EL_WARN);
pContext->useSocket().close();
{
ALock lock(pThis->sync);
pThis->queue.remove(pContext);
}
pOwner->m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
}
else if (AConstant::unavail == bytesRead)
{
// Would block, keep waiting
if (pContext->useTimeoutTimer().getInterval() > pOwner->m_NoDataTimeout)
{
AString str;
str.append("AOSContextQueue_IsAvailable: No data (non-blocking) after timeout: ",67);
pContext->useTimeoutTimer().emit(str);
//a_We are done with this request, still no data
pContext->useEventVisitor().startEvent(
str,
(pContext->useConnectionFlags().isSet(AOSContext::CONFLAG_IS_HTTP11_PIPELINING) ? AEventVisitor::EL_EVENT : AEventVisitor::EL_ERROR)
);
{
ALock lock(pThis->sync);
pThis->queue.remove(pContext);
}
pOwner->m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
}
}
else
{
//a_Add to next queue
pContext->useConnectionFlags().setBit(AOSContext::CONFLAG_ISAVAILABLE_PENDING);
if (pContext->useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
pContext->useEventVisitor().startEvent(ASW("AOSContextQueue_IsAvailable: HTTP header has more data",54), AEventVisitor::EL_DEBUG);
{
ALock lock(pThis->sync);
pThis->queue.remove(pContext);
}
pOwner->m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_PRE_EXECUTE, &pContext);
}
}
catch(AException e)
{
bytesRead = AConstant::npos; //a_Following switch will handle this
ARope rope("AOSContextQueue_IsAvailable: Exception reading from socket",58);
rope.append(e);
pContext->useEventVisitor().addEvent(rope, AEventVisitor::EL_ERROR);
}
--availCount;
}
else
{
//a_Select called on this AOSContext and no data available
if (pContext->useTimeoutTimer().getInterval() > pOwner->m_NoDataTimeout)
{
AString str;
str.append("AOSContextQueue_IsAvailable: No data after timeout: ",52);
pContext->useTimeoutTimer().emit(str);
//a_We are done with this request, still no data
pContext->useEventVisitor().startEvent(
str,
(pContext->useConnectionFlags().isSet(AOSContext::CONFLAG_IS_HTTP11_PIPELINING) ? AEventVisitor::EL_EVENT : AEventVisitor::EL_ERROR)
);
{
ALock lock(pThis->sync);
pThis->queue.remove(pContext);
}
pOwner->m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
}
}
++count2;
}
AASSERT(pOwner, !availCount);
}
else
{
//a_Nothing has data, flag them all
p = pThis->queue.useHead();
while (p)
{
AOSContext *pContext = (AOSContext *)p;
p = p->useNext();
if (pContext->useTimeoutTimer().getInterval() > pOwner->m_NoDataTimeout)
{
AString str("AOSContextQueue_IsAvailable: No data after timeout: ",52);
pContext->useTimeoutTimer().emit(str);
//a_We are done with this request, still no data
ALock lock(pThis->sync);
pContext->useEventVisitor().startEvent(
str,
(pContext->useConnectionFlags().isSet(AOSContext::CONFLAG_IS_HTTP11_PIPELINING) ? AEventVisitor::EL_EVENT : AEventVisitor::EL_ERROR)
);
pThis->queue.remove(pContext);
pOwner->m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
}
}
}
}
//a_Sleep between selects, requests in this queue are already slow
AThread::sleep(pOwner->m_LoopDelay);
}
else
AThread::sleep(pOwner->m_EmptyQueueDelay); //a_Nothing in queue, deep sleep
}
catch(AException& e)
{
pOwner->m_Services.useLog().addException(e);
}
catch(std::exception& e)
{
pOwner->m_Services.useLog().addException(e);
}
catch(...)
{
pOwner->m_Services.useLog().add(ASWNL("Unknown exception caught in AOSContextQueue_IsAvailable::threadproc"), ALog::EVENT_EXCEPTION);
}
}
thread.setRunning(false);
return 0;
}
int main(int argc, char *argv[]) {
// command line options
GeneralConfig::scale = 10;
SceneConfig::enableIK = SceneConfig::enableHaptics = false;
SceneConfig::enableRobot = true;
Parser parser;
parser.addGroup(TrackingConfig());
parser.addGroup(RecordingConfig());
parser.addGroup(GeneralConfig());
parser.addGroup(BulletConfig());
parser.addGroup(SceneConfig());
parser.read(argc,argv);
// comm stuff
initComm();
FileSubscriber pcSub("kinect","pcd");
CloudMessage cloudMsg;
FileSubscriber ropeSub("rope_pts","pcd");
CloudMessage ropeMsg;
FileSubscriber labelSub("labels","png");
ImageMessage labelMsg;
FileSubscriber endSub("rope_ends","txt");
VecVecMessage<float> endMsg;
FileSubscriber jointSub("joint_states","txt");
Retimer<VectorMessage<double> > retimer(&jointSub);
Scene scene;
PR2Manager pr2m(scene);
MonitorForGrabbing lMonitor(pr2m.pr2->robot->GetManipulators()[5], scene.env->bullet->dynamicsWorld);
MonitorForGrabbing rMonitor(pr2m.pr2->robot->GetManipulators()[7], scene.env->bullet->dynamicsWorld);
vector<double> firstJoints = doubleVecFromFile(filePath("data000000000000.txt", "joint_states").string());
ValuesInds vi = getValuesInds(firstJoints);
pr2m.pr2->setDOFValues(vi.second, vi.first);
// get kinect transform
KinectTrans kinectTrans(pr2m.pr2->robot);
kinectTrans.calibrate(btTransform(btQuaternion(0.669785, -0.668418, 0.222562, -0.234671), btVector3(0.263565, -0.038203, 1.762524)));
CoordinateTransformer CT(kinectTrans.getKinectTrans());
// load table
/////////////// load table
vector<btVector3> tableCornersCam = toBulletVectors(floatMatFromFile(onceFile("table_corners.txt").string()));
vector<btVector3> tableCornersWorld = CT.toWorldFromCamN(tableCornersCam);
BulletObject::Ptr table = makeTable(tableCornersWorld, .1*GeneralConfig::scale);
table->setColor(1,1,1,.25);
// load rope
vector<btVector3> ropePtsCam = toBulletVectors(floatMatFromFile(onceFile("init_rope.txt").string()));
CapsuleRope::Ptr rope(new CapsuleRope(CT.toWorldFromCamN(ropePtsCam), .0075*METERS));
// plots
PointCloudPlot::Ptr kinectPts(new PointCloudPlot(2));
CorrPlots corrPlots;
// setup scene
if (TrackingConfig::showKinect) scene.env->add(kinectPts);
scene.env->add(rope);
scene.env->add(table);
if (TrackingConfig:: showLines) scene.env->add(corrPlots.m_lines);
lMonitor.setBodies(rope->children);
rMonitor.setBodies(rope->children);
// recording
ScreenRecorder* rec;
if (RecordingConfig::record != DONT_RECORD){
rec = new ScreenRecorder(scene.viewer);
}
// end tracker
vector<RigidBodyPtr> rope_ends;
rope_ends.push_back(rope->bodies[0]);
rope_ends.push_back(rope->bodies[rope->bodies.size()-1]);
MultiPointTrackerRigid endTracker(rope_ends,scene.env->bullet->dynamicsWorld);
TrackerPlotter trackerPlotter(endTracker);
//scene.env->add(trackerPlotter.m_fakeObjects[0]);
//scene.env->add(trackerPlotter.m_fakeObjects[1]);
scene.startViewer();
scene.setSyncTime(true);
scene.idle(true);
vector<double> oldvals, newvals;
int count=0;
while (pcSub.recv(cloudMsg)) {
ENSURE(ropeSub.recv(ropeMsg));
vector<btVector3> obsPts = CT.toWorldFromCamN(toBulletVectors(ropeMsg.m_data));
ENSURE(labelSub.recv(labelMsg));
cv::Mat labels = toSingleChannel(labelMsg.m_data);
ENSURE(endSub.recv(endMsg));
vector<btVector3> newEnds = CT.toWorldFromCamN(toBulletVectors(endMsg.m_data));
endTracker.update(newEnds);
trackerPlotter.update();
ColorCloudPtr cloudCam = cloudMsg.m_data;
ColorCloudPtr cloudWorld(new ColorCloud());
pcl::transformPointCloud(*cloudCam, *cloudWorld, CT.worldFromCamEigen);
kinectPts->setPoints1(cloudWorld);
cout << "loaded cloud " << count << endl;
count++;
VectorMessage<double>* jointMsgPtr = retimer.msgAt(cloudMsg.getTime());
vector<double> currentJoints = jointMsgPtr->m_data;
ValuesInds vi = getValuesInds(currentJoints);
newvals = vi.first;
if (oldvals.size()==0) {
cout << "first one" << endl;
oldvals = newvals;
}
CT.reset(kinectTrans.getKinectTrans());
cv::Mat ropeMask = toSingleChannel(labels) == 1;
for (int iter=0; iter<TrackingConfig::nIter; iter++) {
cout << "iteration " << iter << endl;
pr2m.pr2->setDOFValues(vi.second, interpolateBetween(oldvals, newvals, (iter+0.00001)/TrackingConfig::nIter));
vector<btVector3> estPts = rope->getNodes();
Eigen::MatrixXf ropePtsCam = toEigenMatrix(CT.toCamFromWorldN(estPts));
vector<float> pVis = calcVisibility(rope->bodies, scene.env->bullet->dynamicsWorld, CT.worldFromCamUnscaled.getOrigin()*METERS, TrackingConfig::sigA*METERS, TrackingConfig::nSamples);
colorByVisibility(rope, pVis);
SparseArray corr = toSparseArray(calcCorrProb(toEigenMatrix(estPts), toEigenMatrix(obsPts), toVectorXf(pVis), TrackingConfig::sigB*METERS, TrackingConfig::outlierParam),TrackingConfig::cutoff);
corrPlots.update(estPts, obsPts, corr);
vector<btVector3> impulses = calcImpulsesSimple(estPts, obsPts, corr, TrackingConfig::impulseSize);
applyImpulses(impulses, rope);
if (RecordingConfig::record == EVERY_ITERATION ||
RecordingConfig::record == FINAL_ITERATION && iter==TrackingConfig::nIter-1)
rec->snapshot();
lMonitor.update();
rMonitor.update();
scene.step(DT);
}
oldvals = newvals;
}
}
bool Character::move_effects(bool attacking)
{
if (has_effect("downed")) {
if (rng(0, 40) > get_dex() + int(get_str() / 2)) {
add_msg_if_player(_("You struggle to stand."));
} else {
add_msg_player_or_npc(m_good, _("You stand up."),
_("<npcname> stands up."));
remove_effect("downed");
}
return false;
}
if (has_effect("webbed")) {
if (x_in_y(get_str(), 6 * get_effect_int("webbed"))) {
add_msg_player_or_npc(m_good, _("You free yourself from the webs!"),
_("<npcname> frees themselves from the webs!"));
remove_effect("webbed");
} else {
add_msg_if_player(_("You try to free yourself from the webs, but can't get loose!"));
}
return false;
}
if (has_effect("lightsnare")) {
if(x_in_y(get_str(), 12) || x_in_y(get_dex(), 8)) {
remove_effect("lightsnare");
add_msg_player_or_npc(m_good, _("You free yourself from the light snare!"),
_("<npcname> frees themselves from the light snare!"));
item string("string_36", calendar::turn);
item snare("snare_trigger", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), string);
g->m.add_item_or_charges(posx(), posy(), snare);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the light snare, but can't get loose!"));
}
return false;
}
if (has_effect("heavysnare")) {
if(x_in_y(get_str(), 32) || x_in_y(get_dex(), 16)) {
remove_effect("heavysnare");
add_msg_player_or_npc(m_good, _("You free yourself from the heavy snare!"),
_("<npcname> frees themselves from the heavy snare!"));
item rope("rope_6", calendar::turn);
item snare("snare_trigger", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), rope);
g->m.add_item_or_charges(posx(), posy(), snare);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the heavy snare, but can't get loose!"));
}
return false;
}
if (has_effect("beartrap")) {
/* Real bear traps can't be removed without the proper tools or immense strength; eventually this should
allow normal players two options: removal of the limb or removal of the trap from the ground
(at which point the player could later remove it from the leg with the right tools).
As such we are currently making it a bit easier for players and NPC's to get out of bear traps.
*/
if(x_in_y(get_str(), 100)) {
remove_effect("beartrap");
add_msg_player_or_npc(m_good, _("You free yourself from the bear trap!"),
_("<npcname> frees themselves from the bear trap!"));
item beartrap("beartrap", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), beartrap);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the bear trap, but can't get loose!"));
}
return false;
}
if (has_effect("crushed")) {
// Strength helps in getting free, but dex also helps you worm your way out of the rubble
if(x_in_y(get_str() + get_dex() / 4, 100)) {
remove_effect("crushed");
add_msg_player_or_npc(m_good, _("You free yourself from the rubble!"),
_("<npcname> frees themselves from the rubble!"));
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the rubble, but can't get loose!"));
}
return false;
}
// Below this point are things that allow for movement if they succeed
// Currently we only have one thing that forces movement if you succeed, should we get more
// than this will need to be reworked to only have success effects if /all/ checks succeed
if (has_effect("in_pit")) {
if (rng(0, 40) > get_str() + int(get_dex() / 2)) {
add_msg_if_player(m_bad, _("You try to escape the pit, but slip back in."));
return false;
} else {
add_msg_player_or_npc(m_good, _("You escape the pit!"),
_("<npcname> escapes the pit!"));
remove_effect("in_pit");
}
}
if (has_effect("grabbed")){
int zed_number = 0;
for( auto &&dest : g->m.points_in_radius( pos(), 1, 0 ) ){
if (g->mon_at(dest) != -1){
zed_number ++;
}
}
if (attacking == true || zed_number == 0){
return true;
}
if (get_dex() > get_str() ? rng(0, get_dex()) : rng( 0, get_str()) < rng( get_effect_int("grabbed") , 8) ){
add_msg_player_or_npc(m_bad, _("You try break out of the grab, but fail!"),
_("<npcname> tries to break out of the grab, but fails!"));
return false;
} else {
add_msg_player_or_npc(m_good, _("You break out of the grab!"),
_("<npcname> breaks out of the grab!"));
remove_effect("grabbed");
}
}
return Creature::move_effects(attacking);
}
bool Character::move_effects()
{
if (has_effect("downed")) {
if (rng(0, 40) > get_dex() + int(get_str() / 2)) {
add_msg_if_player(_("You struggle to stand."));
} else {
add_msg_player_or_npc(m_good, _("You stand up."),
_("<npcname> stands up."));
remove_effect("downed");
}
return false;
}
if (has_effect("webbed")) {
if (x_in_y(get_str(), 6 * get_effect_int("webbed"))) {
add_msg_player_or_npc(m_good, _("You free yourself from the webs!"),
_("<npcname> frees themselves from the webs!"));
remove_effect("webbed");
} else {
add_msg_if_player(_("You try to free yourself from the webs, but can't get loose!"));
}
return false;
}
if (has_effect("lightsnare")) {
if(x_in_y(get_str(), 12) || x_in_y(get_dex(), 8)) {
remove_effect("lightsnare");
add_msg_player_or_npc(m_good, _("You free yourself from the light snare!"),
_("<npcname> frees themselves from the light snare!"));
item string("string_36", calendar::turn);
item snare("snare_trigger", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), string);
g->m.add_item_or_charges(posx(), posy(), snare);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the light snare, but can't get loose!"));
}
return false;
}
if (has_effect("heavysnare")) {
if(x_in_y(get_str(), 32) || x_in_y(get_dex(), 16)) {
remove_effect("heavysnare");
add_msg_player_or_npc(m_good, _("You free yourself from the heavy snare!"),
_("<npcname> frees themselves from the heavy snare!"));
item rope("rope_6", calendar::turn);
item snare("snare_trigger", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), rope);
g->m.add_item_or_charges(posx(), posy(), snare);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the heavy snare, but can't get loose!"));
}
return false;
}
if (has_effect("beartrap")) {
/* Real bear traps can't be removed without the proper tools or immense strength; eventually this should
allow normal players two options: removal of the limb or removal of the trap from the ground
(at which point the player could later remove it from the leg with the right tools).
As such we are currently making it a bit easier for players and NPC's to get out of bear traps.
*/
if(x_in_y(get_str(), 100)) {
remove_effect("beartrap");
add_msg_player_or_npc(m_good, _("You free yourself from the bear trap!"),
_("<npcname> frees themselves from the bear trap!"));
item beartrap("beartrap", calendar::turn);
g->m.add_item_or_charges(posx(), posy(), beartrap);
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the bear trap, but can't get loose!"));
}
return false;
}
if (has_effect("crushed")) {
// Strength helps in getting free, but dex also helps you worm your way out of the rubble
if(x_in_y(get_str() + get_dex() / 4, 100)) {
remove_effect("crushed");
add_msg_player_or_npc(m_good, _("You free yourself from the rubble!"),
_("<npcname> frees themselves from the rubble!"));
} else {
add_msg_if_player(m_bad, _("You try to free yourself from the rubble, but can't get loose!"));
}
return false;
}
if (has_effect("amigara")) {
int curdist = 999, newdist = 999;
for (int cx = 0; cx < SEEX * MAPSIZE; cx++) {
for (int cy = 0; cy < SEEY * MAPSIZE; cy++) {
if (g->m.ter(cx, cy) == t_fault) {
int dist = rl_dist(cx, cy, posx(), posy());
if (dist < curdist) {
curdist = dist;
}
dist = rl_dist(cx, cy, posx(), posy());
if (dist < newdist) {
newdist = dist;
}
}
}
}
if (newdist > curdist) {
add_msg_if_player(m_info, _("You cannot pull yourself away from the faultline..."));
return false;
}
}
// Below this point are things that allow for movement if they succeed
// Currently we only have one thing that forces movement if you succeed, should we get more
// than this will need to be reworked to only have success effects if /all/ checks succeed
if (has_effect("in_pit")) {
if (rng(0, 40) > get_str() + int(get_dex() / 2)) {
add_msg_if_player(m_bad, _("You try to escape the pit, but slip back in."));
return false;
} else {
add_msg_player_or_npc(m_good, _("You escape the pit!"),
_("<npcname> escapes the pit!"));
remove_effect("in_pit");
}
}
return Creature::move_effects();
}
void AOSOutputExecutor::execute(AOSContext& context)
{
AString command;
if (
context.useRequestParameterPairs().exists(OVERRIDE_OUTPUT)
&& m_Services.useConfiguration().isOutputOverrideAllowed()
)
{
//a_Override requested and allowed
context.useRequestParameterPairs().get(OVERRIDE_OUTPUT, command);
}
else
{
command = context.getOutputCommand();
if (context.useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
{
ARope rope("Default output generator overridden to: ",40);
rope.append(command);
context.useEventVisitor().startEvent(rope, AEventVisitor::EL_DEBUG);
}
}
if (command.equals("NOP"))
{
//a_If NOP was used force XML
m_Services.useConfiguration().setMimeTypeFromExt(ASW("xml",3), context);
if (context.useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
context.useEventVisitor().startEvent(ASWNL("NOP detected, defaulting to XML output"), AEventVisitor::EL_DEBUG);
}
if (command.isEmpty())
{
if (!m_Services.useConfiguration().getAosDefaultOutputGenerator().isEmpty())
{
command.assign(m_Services.useConfiguration().getAosDefaultOutputGenerator());
if (context.useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
{
ARope rope("No output generator specified, defaulting to: ",46);
rope.append(command);
context.useEventVisitor().startEvent(rope, AEventVisitor::EL_DEBUG);
}
}
else
{
if (context.useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
context.useEventVisitor().startEvent(ASW("No output generator, defaulting to XML",38), AEventVisitor::EL_DEBUG);
return;
}
}
try
{
//a_Find input command, if not found execute the default
OutputGeneratorContainer::iterator it = m_OutputGenerators.find(command);
if (it == m_OutputGenerators.end())
{
if (context.useEventVisitor().isLogging(AEventVisitor::EL_WARN))
{
ARope rope("Skipping unknown output generator: ",35);
rope.append(command);
context.useEventVisitor().startEvent(rope, AEventVisitor::EL_WARN);
}
}
else
{
ATimer timer(true);
//a_Generate output
if (context.useEventVisitor().isLogging(AEventVisitor::EL_INFO))
{
ARope rope("Generating output: ",19);
rope.append((*it).first);
context.useEventVisitor().startEvent(rope, AEventVisitor::EL_INFO);
}
if (context.useContextFlags().isClear(AOSContext::CTXFLAG_IS_AJAX))
{
context.useModel().overwriteElement(ASW("execute/output", 14)).addData(command);
//a_Publish timers
context.getRequestTimer().emitXml(context.useModel().overwriteElement(ASW("request_time",12)));
context.getContextTimer().emitXml(context.useModel().overwriteElement(ASW("context_time",12)));
}
//a_Generate output
AOSContext::ReturnCode ret = (*it).second->execute(context);
switch (ret)
{
case AOSContext::RETURN_OK:
break;
case AOSContext::RETURN_REDIRECT:
if (context.useEventVisitor().isLogging(AEventVisitor::EL_INFO))
{
context.useEventVisitor().startEvent(ASWNL("Output generator has done a redirect"), AEventVisitor::EL_DEBUG);
}
break;
default:
context.addError((*it).second->getClass(), ASWNL("Output generator returned neither OK nor REDIRECT"));
return;
}
//a_Event over
context.useEventVisitor().endEvent();
//a_Add execution time
(*it).second->addExecutionTimeSample(timer.getInterval());
}
}
catch(AException& ex)
{
AString strWhere("AOSOutputExecutor::execute(", 27);
strWhere.append(command);
strWhere.append(')');
context.addError(strWhere, ex.what());
AXmlElement& element = context.useModel().addElement("output_error");
element.addElement("where", strWhere);
element.addElement("exception", ex);
}
catch(...)
{
AString strWhere("AOSOutputExecutor::execute(", 27);
strWhere.append(command);
strWhere.append(')');
context.addError(strWhere, ASWNL("Unknown Exception"));
context.useModel().addElement("output_error").addData("Unknown Exception");
}
}
void AOSContextManager::deallocate(AOSContext *p)
{
if (p)
{
{
ALock lock(m_InUseSync);
CONTEXT_INUSE::iterator it = m_InUse.find(p);
AASSERT(this, p && m_InUse.end() != it);
m_InUse.erase(it);
}
}
else
return;
//a_Log
ARope rope("AOSContextManager::deallocate[",30);
rope.append(AString::fromPointer(p));
rope.append(']');
p->useEventVisitor().startEvent(rope);
//a_Write to log
if (p->useEventVisitor().getErrorCount() > 0)
{
m_Services.useLog().add(p->useEventVisitor(), ALog::EVENT_CRITICAL_ERROR);
}
if (p->useEventVisitor().getErrorCount() > 0)
{
//a_Add to error history
if (m_ErrorHistoryMaxSize > 0)
{
p->finalize();
m_ErrorHistory.pushBack(p);
}
else
_deleteContext(&p);
while (m_ErrorHistory.size() > m_ErrorHistoryMaxSize)
{
//a_Remove last
AOSContext *pC = (AOSContext *)m_ErrorHistory.popFront();
_deleteContext(&pC);
}
}
else
{
//a_Add to history
if (m_HistoryMaxSize > 0)
{
p->finalize();
m_History.pushBack(p);
}
else
{
_deleteContext(&p);
}
while (m_History.size() > m_HistoryMaxSize)
{
//a_Remove last
AOSContext *pC = (AOSContext *)m_History.popFront();
if (pC)
_deleteContext(&pC);
}
}
}
u4 AOSContextQueue_ErrorExecutor::_threadproc(AThread& thread)
{
AOSContextQueue_ErrorExecutor *pThis = dynamic_cast<AOSContextQueue_ErrorExecutor *>(thread.getThis());
AASSERT(&thread, pThis);
AOSContext *pContext = NULL;
thread.setRunning(true);
while(thread.isRun())
{
try
{
while (pContext = pThis->_nextContext())
{
#ifndef NDEBUG
if (!ADebugDumpable::isPointerValid(pContext))
{
AString error("AOSContext pointer is invalid: ");
error.append(AString::fromPointer(pContext));
AASSERT_EX(NULL, false, error);
continue;
}
#endif
pContext->useEventVisitor().startEvent(ASW("AOSContextQueue_ErrorExecutor: Processing error condition", 57));
//a_Should only be here if an error occured, if status not set >200, then assume 500
if (pContext->useResponseHeader().getStatusCode() == AHTTPResponseHeader::SC_200_Ok)
pContext->useResponseHeader().setStatusCode(AHTTPResponseHeader::SC_500_Internal_Server_Error);
//a_Check is socket was closed, if so do nothing else, we are done
if (pContext->useConnectionFlags().isSet(AOSContext::CONFLAG_IS_SOCKET_ERROR))
{
//a_Proceed
m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
continue;
}
if (
pContext->useContextFlags().isSet(AOSContext::CTXFLAG_IS_RESPONSE_HEADER_SENT)
|| pContext->useContextFlags().isSet(AOSContext::CTXFLAG_IS_OUTPUT_SENT)
)
{
//a_Log to file, output is done already
m_Services.useLog().add(pContext->useEventVisitor(), ALog::EVENT_FAILURE);
}
else
{
//a_Add XSLT stylesheet for the error XML
//if (m_Services.useConfiguration().exists(ASW("/config/server/error-stylesheet",31)))
//{
// AString errorStylesheet;
// m_Services.useConfiguration().emitString(ASW("/config/server/error-stylesheet",31), errorStylesheet);
// pContext->useModelXmlDocument().addInstruction(AXmlInstruction::XML_STYLESHEET)
// .addAttribute(ASW("type",4), ASW("text/xsl",8))
// .addAttribute(ASW("href",4), errorStylesheet);
//}
//a_Add request header to result XML
if (!pContext->useModel().exists(ASW("REQUEST",7)))
pContext->useRequestHeader().emitXml(pContext->useModel().overwriteElement(ASW("REQUEST",7)));
pContext->useResponseHeader().emitXml(pContext->useModel().overwriteElement(ASW("RESPONSE",8)));
//a_Check if dumpContext is specified to override and emit XML
int dumpContextLevel = pContext->getDumpContextLevel();
pContext->dumpContext(dumpContextLevel);
if (dumpContextLevel > 0)
{
//a_Write contents of the output XML instead of output buffer
m_Services.useConfiguration().setMimeTypeFromExt(ASW("xml",3), *pContext);
pContext->useResponseHeader().setStatusCode(AHTTPResponseHeader::SC_200_Ok);
pContext->writeOutputBuffer(true);
}
else
{
//a_Set the current content type as text/html
pContext->useResponseHeader().set(AHTTPHeader::HT_ENT_Content_Type, ASW("text/html; charset=utf-8",24));
int statusCode = pContext->useResponseHeader().getStatusCode();
AAutoPtr<ATemplate> pTemplate(NULL, false); //a_Call to cache manager will set a template
pContext->clearOutputBuffer();
if (m_Services.useCacheManager().getStatusTemplate(statusCode, pTemplate))
{
//a_Template for this status code is found, so process and emit into output buffer
pTemplate->process(pContext->useLuaTemplateContext(), pContext->useOutputBuffer());
if (pContext->useEventVisitor().isLogging(AEventVisitor::EL_DEBUG))
{
ARope rope("Using error template for status ");
rope.append(AString::fromInt(statusCode));
pContext->useEventVisitor().startEvent(rope, AEventVisitor::EL_DEBUG);
}
}
else
{
if (pContext->useEventVisitor().isLogging(AEventVisitor::EL_WARN))
{
ARope rope("Did not find error template for status ");
rope.append(AString::fromInt(statusCode));
pContext->useEventVisitor().startEvent(rope, AEventVisitor::EL_WARN);
}
}
if (pContext->isOutputBufferEmpty())
{
AString strError(1024, 256);
strError.assign("Error ",6);
strError.append(AString::fromInt(pContext->useResponseHeader().getStatusCode()));
strError.append(": ", 2);
strError.append(pContext->useResponseHeader().getStatusCodeReasonPhrase(pContext->useResponseHeader().getStatusCode()));
//a_Put some generic stuff since there is no error template
pContext->useOutputBuffer().append("<html><head><title>",19);
pContext->useOutputBuffer().append(strError);
pContext->useOutputBuffer().append("</title></head>",15);
pContext->useOutputBuffer().append("<body>",6);
pContext->useOutputBuffer().append(strError);
pContext->useOutputBuffer().append("</body></html>",14);
}
try
{
pContext->writeOutputBuffer();
}
catch(ASocketException& ex)
{
pContext->useEventVisitor().addEvent(ex, AEventVisitor::EL_ERROR);
pContext->useConnectionFlags().setBit(AOSContext::CONFLAG_IS_SOCKET_ERROR);
m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
continue;
}
}
}
//a_Close connection
pContext->useSocket().close();
//a_Proceed
m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
continue;
}
AThread::sleep(pThis->m_SleepDelay); //a_Empty queue, avoid thrashing
}
catch(AException& e)
{
pContext->useEventVisitor().addEvent(e, AEventVisitor::EL_ERROR);
m_Services.useLog().add(pContext->useEventVisitor(), ALog::EVENT_FAILURE);
m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
}
catch(std::exception& e)
{
pContext->useEventVisitor().addEvent(ASWNL(e.what()), AEventVisitor::EL_ERROR);
m_Services.useLog().add(pContext->useEventVisitor(), ALog::EVENT_FAILURE);
m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
}
catch(...)
{
m_Services.useLog().add(pContext->useEventVisitor(), ALog::EVENT_FAILURE);
pContext->useEventVisitor().addEvent(ASWNL("Unknown exception caught in AOSContextQueue_ErrorExecutor::_threadproc"), AEventVisitor::EL_ERROR);
m_Services.useContextManager().changeQueueState(AOSContextManager::STATE_TERMINATE, &pContext);
break;
}
}
thread.setRunning(false);
return 0;
}
