void StateManager::print_trace_aux(StatePtr p) // changes by Uli
{
state original;
char *s;
if (p.isStart()) {
// this is a startstate
// expand it into global variable `theworld`
// StateCopy(workingstate, s); // Uli: workingstate is set in
// StateName()
// output startstate
cout << "Startstate " << (s = StartState->StateName(p))
<< " fired.\n";
delete[]s; // Uli: avoid memory leak
theworld.print();
cout << "----------\n\n";
} else {
// print the prefix
print_trace_aux(p.previous());
// print the next state, which should be equivalent to state s
// and set theworld to that state.
// FALSE: no need to print full state
Rules->print_world_to_state(p, FALSE);
}
}
bool FootstepGraph::isGoal(StatePtr state)
{
FootstepState::Ptr goal = getGoal(state->getLeg());
if (publish_progress_) {
jsk_footstep_msgs::FootstepArray msg;
msg.header.frame_id = "odom"; // TODO fixed frame_id
msg.header.stamp = ros::Time::now();
msg.footsteps.push_back(*state->toROSMsg());
pub_progress_.publish(msg);
}
Eigen::Affine3f pose = state->getPose();
Eigen::Affine3f goal_pose = goal->getPose();
Eigen::Affine3f transformation = pose.inverse() * goal_pose;
if ((parameters_.goal_pos_thr > transformation.translation().norm()) &&
(parameters_.goal_rot_thr > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()))) {
// check collision
if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
if (right_goal_state_->crossCheck(state)) {
return true;
}
} else if (state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
if (left_goal_state_->crossCheck(state)) {
return true;
}
}
}
return false;
}
bool FootstepGraph::finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep,
std::vector<StatePtr> &finalizeSteps) {
// simple finalize (no check)
if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
finalizeSteps.push_back(right_goal_state_);
finalizeSteps.push_back(left_goal_state_);
} else if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
finalizeSteps.push_back(left_goal_state_);
finalizeSteps.push_back(right_goal_state_);
}
return true;
}
void StateManager::print_trace(StatePtr p)
{
// print the prefix
if (p.isStart()) {
print_trace_aux(p);
} else {
print_trace_aux(p.previous());
// print the next state, which should be equivalent to state s
// and set theworld to that state.
// TRUE: print full state please;
Rules->print_world_to_state(p, TRUE);
}
}
bool FootstepGraph::successors_original(StatePtr target_state, std::vector<FootstepGraph::StatePtr> &ret)
{
std::vector<Eigen::Affine3f> transformations;
int next_leg;
if (target_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
transformations = successors_from_left_to_right_;
next_leg = jsk_footstep_msgs::Footstep::RIGHT;
}
else if (target_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
transformations = successors_from_right_to_left_;
next_leg = jsk_footstep_msgs::Footstep::LEFT;
}
else {
// TODO: error
}
//std::vector<FootstepGraph::StatePtr> ret;
Eigen::Affine3f base_pose = target_state->getPose();
for (size_t i = 0; i < transformations.size(); i++) {
Eigen::Affine3f transform = transformations[i];
FootstepGraph::StatePtr next(new FootstepState(next_leg,
base_pose * transform,
target_state->getDimensions(),
resolution_));
if (use_pointcloud_model_ && !lazy_projection_) {
// Update footstep position by projection
unsigned int error_state;
FootstepGraph::StatePtr tmpnext = projectFootstep(next, error_state);
if (!tmpnext && localMovement() && error_state == projection_state::close_to_success) {
std::vector<StatePtr> locally_moved_nodes = localMoveFootstepState(next);
for (size_t j = 0; j < locally_moved_nodes.size(); j++) {
if (isSuccessable(locally_moved_nodes[j], target_state)) {
FootstepGraph::StatePtr tmp = projectFootstep(locally_moved_nodes[j], error_state);
if(!!tmp) {
ret.push_back(tmp);
}
}
}
}
next = tmpnext;
}
if (!!next) {
if (isSuccessable(next, target_state)) {
ret.push_back(next);
}
}
}
return true;
}
void StateManager::push(StatePtr state)
{
state->initialize();
// NOTE: should this check for duplicate states?
_stateStack.push(state);
}
bool FootstepGraph::isGoal(StatePtr state)
{
FootstepState::Ptr goal = getGoal(state->getLeg());
if (publish_progress_) {
jsk_footstep_msgs::FootstepArray msg;
msg.header.frame_id = "odom";
msg.header.stamp = ros::Time::now();
msg.footsteps.push_back(*state->toROSMsg());
pub_progress_.publish(msg);
}
Eigen::Affine3f pose = state->getPose();
Eigen::Affine3f goal_pose = goal->getPose();
Eigen::Affine3f transformation = pose.inverse() * goal_pose;
return (pos_goal_thr_ > transformation.translation().norm()) &&
(rot_goal_thr_ > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()));
}
void beginOcclusionQuery()
{
m_queries.resize( 0 );
m_queryNames.resize( 0 );
glClearColor( 0.0, 0.0, 0.0, 1.0 );
glClearDepth( 1.0 );
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
m_baseState->add( depthTestStateComponent(), true /* override */ );
}
void Mind::PushState( const StatePtr &state ) {
assert( state != NULL );
// Push the state to the front of the queue
_stateQueue.push_front( state );
state->SetOwner( _owner.GetEntity() );
// Trigger a stateswitch next round
_switchState = true;
//PrintStateQueue("push");
}
void GameController::setState(const StatePtr & statePtr) {
if(statePtr) {
state->onExit();
HIKARI_LOG(debug) << "<GameController> exited \"" << state->getName() << "\" state.";
prevState = state->getName();
currState = statePtr->getName();
state = statePtr;
state->onEnter();
HIKARI_LOG(debug) << "<GameController> entered \"" << state->getName() << "\" state.";
}
}
StatePtr SimpleTexturedMaterial::makeState(void)
{
StatePtr state = Inherited::makeState();
prepareLocalChunks();
state->addChunk(_textureChunk);
state->addChunk(_texGenChunk);
if(getImage() != NullFC &&
getImage()->hasAlphaChannel() &&
getEnvMode() != GL_DECAL)
{
if(getImage()->isAlphaBinary())
{
if(_blendChunk->getSrcFactor() == GL_SRC_ALPHA)
{
beginEditCP(_blendChunk);
_blendChunk->setSrcFactor(GL_ONE);
_blendChunk->setDestFactor(GL_ZERO);
_blendChunk->setAlphaFunc(GL_NOTEQUAL);
_blendChunk->setAlphaValue(0);
endEditCP(_blendChunk);
}
}
else
{
if(_blendChunk->getSrcFactor() != GL_SRC_ALPHA)
{
beginEditCP(_blendChunk);
_blendChunk->setSrcFactor(GL_SRC_ALPHA);
_blendChunk->setDestFactor(GL_ONE_MINUS_SRC_ALPHA);
_blendChunk->setAlphaFunc(GL_NONE);
_blendChunk->setAlphaValue(0);
endEditCP(_blendChunk);
}
}
}
return state;
}
// Activate callback
void TestState::activate(cycle_t cyc)
{
// Display callback text
DISPLAY(cout << "TestState: In activate()" << endl);
// Test trying to call callback-protected functions (this is a callback...)
StatePtr snew = NewStateInstance<KickBallState>(sc, 0);
EXPECT_EQ(SCR_BAD_CALLBACK, sc->init(snew));
EXPECT_EQ(SCR_BAD_CALLBACK, sc->forceState(snew));
EXPECT_EQ(SCR_BAD_CALLBACK, sc->step());
EXPECT_EQ(SCR_BAD_CALLBACK, sc->loop());
snew.reset();
// Refuse to activate this state by calling terminate() (...at least the first time this function is called)
if(!sc->g_tested_term)
{
DISPLAY(cout << "TestState: Calling terminate() from inside activate()" << endl);
sc->g_tested_term = true;
sc->terminate();
return; // Just here for good coding standard after a terminate()
}
}
IECore::RunTimeTypedPtr ToGLStateConverter::doConversion( IECore::ConstObjectPtr src, IECore::ConstCompoundObjectPtr operands ) const
{
const CompoundObject *co = runTimeCast<const CompoundObject>( src.get() );
if( !co )
{
throw Exception( "Expected a CompoundObject" );
}
const AttributeToStateMap &m = attributeToStateMap();
const StatePtr result = new State( false );
for( CompoundObject::ObjectMap::const_iterator it = co->members().begin(), eIt = co->members().end(); it != eIt; ++it )
{
AttributeToStateMap::const_iterator mIt = m.find( it->first );
if( mIt != m.end() )
{
StateComponentPtr s = mIt->second( it->second.get() );
result->add( s );
}
}
return result;
}
StatePtr PhongMaterial::makeState(void)
{
StatePtr state = State::create();
prepareLocalChunks();
Color3f v3;
Color4f v4;
float alpha = 1.f - getTransparency();
prepareLocalChunks();
beginEditCP(_materialChunk);
v3 = getAmbient();
v4.setValuesRGBA(v3[0], v3[1], v3[2], alpha);
_materialChunk->setAmbient(v4);
v3 = getDiffuse();
v4.setValuesRGBA(v3[0], v3[1], v3[2], alpha);
_materialChunk->setDiffuse(v4);
v3 = getSpecular();
v4.setValuesRGBA(v3[0], v3[1], v3[2], alpha);
_materialChunk->setSpecular(v4);
_materialChunk->setShininess(getShininess());
v3 = getEmission();
v4.setValuesRGBA(v3[0], v3[1], v3[2], alpha);
_materialChunk->setEmission(v4);
_materialChunk->setLit(getLit());
_materialChunk->setColorMaterial(getColorMaterial());
endEditCP (_materialChunk);
state->addChunk(_materialChunk);
if(isTransparent())
state->addChunk(_blendChunk);
if(_vpChunk != NullFC)
state->addChunk(_vpChunk);
createFragmentProgram();
if(_fpChunk != NullFC)
state->addChunk(_fpChunk);
for(MFStateChunkPtr::iterator i = _mfChunks.begin();
i != _mfChunks.end();
++i)
{
state->addChunk(*i);
}
return state;
}
void endOcclusionQuery()
{
if( m_queries.size() )
{
glEndQueryARB( GL_SAMPLES_PASSED_ARB );
}
for( size_t i = 0, e = m_queries.size(); i < e; i++ )
{
GLuint samplesPassed = 0;
glGetQueryObjectuivARB( m_queries[i], GL_QUERY_RESULT_ARB, &samplesPassed );
if( samplesPassed )
{
m_hits.push_back( HitRecord( 0, 0, m_queryNames[i] ) );
}
}
glDeleteQueriesARB( m_queries.size(), &(m_queries[0]) );
m_baseState->add( const_cast<DepthTestStateComponent *>( State::defaultState()->get<DepthTestStateComponent>() ), false /* no override */ );
}
void beginIDRender()
{
m_frameBuffer = new FrameBuffer();
m_frameBuffer->setColor( new UIntTexture( 128, 128 ) );
m_frameBuffer->setDepth( new DepthTexture( 128, 128 ) );
m_frameBuffer->validate();
m_frameBufferBinding = boost::shared_ptr<FrameBuffer::ScopedBinding>( new FrameBuffer::ScopedBinding( *m_frameBuffer ) );
glGetIntegerv( GL_VIEWPORT, m_prevViewport );
glViewport( 0, 0, 128, 128 );
glClearColor( 0.0, 0.0, 0.0, 1.0 );
glClearDepth( 1.0 );
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
const std::vector<StateComponentPtr> &stateComponents = idStateComponents();
for( std::vector<StateComponentPtr>::const_iterator it = stateComponents.begin(), eIt = stateComponents.end(); it != eIt; it++ )
{
m_baseState->add( *it, true /* override */ );
}
glGetIntegerv( GL_CURRENT_PROGRAM, &m_prevProgram );
pushIDShader( m_baseState->get<ShaderStateComponent>()->shaderSetup()->shader() );
}
StatePtr Game::popBackState() {
StatePtr popped = mvStates.back();
popped->dispose();
mvStates.pop_back();
return popped;
}
/********************
Auxiliary function for error trace printing
********************/
bool match(state* ns, StatePtr p)
{
int i;
static PermSet Perm;
static state temp;
StateCopy(&temp, ns);
if (args->symmetry_reduction.value)
{
if ( args->sym_alg.mode == argsym_alg::Exhaustive_Fast_Canonicalize) {
Perm.ResetToExplicit();
for (i=0; i<Perm.count; i++)
if (Perm.In(i))
{
if (ns != workingstate)
StateCopy(workingstate, ns);
mu_v.Permute(Perm,i);
if (args->multiset_reduction.value)
mu_v.MultisetSort();
if (p.compare(workingstate)) {
StateCopy(workingstate,&temp); return TRUE; }
}
StateCopy(workingstate,&temp);
return FALSE;
}
else {
Perm.ResetToSimple();
Perm.SimpleToOne();
if (ns != workingstate)
StateCopy(workingstate, ns);
mu_v.Permute(Perm,0);
if (args->multiset_reduction.value)
mu_v.MultisetSort();
if (p.compare(workingstate)) {
StateCopy(workingstate,&temp); return TRUE; }
while (Perm.NextPermutation())
{
if (ns != workingstate)
StateCopy(workingstate, ns);
mu_v.Permute(Perm,0);
if (args->multiset_reduction.value)
mu_v.MultisetSort();
if (p.compare(workingstate)) {
StateCopy(workingstate,&temp); return TRUE; }
}
StateCopy(workingstate,&temp);
return FALSE;
}
}
if (!args->symmetry_reduction.value
&& args->multiset_reduction.value)
{
if (ns != workingstate)
StateCopy(workingstate, ns);
mu_v.MultisetSort();
if (p.compare(workingstate)) {
StateCopy(workingstate,&temp); return TRUE; }
StateCopy(workingstate,&temp);
return FALSE;
}
return (p.compare(ns));
}
Eigen::Affine3f FootstepGraph::getRobotCoords(StatePtr current_state, StatePtr previous_state) const
{
Eigen::Affine3f mid = current_state->midcoords(*previous_state);
return mid * collision_bbox_offset_;
}
void Game::pushState(StatePtr state) {
if (state) {
state->initialize();
mvStates.push_front(state);
}
}
StatePtr Game::popState() {
StatePtr popped = mvStates.front();
popped->dispose();
mvStates.pop_front();
return popped;
}
void Game::pushBackState(StatePtr state) {
if (state) {
state->initialize();
mvStates.push_back(state);
}
}
