quaternion_t::quaternion_t( const quaternion_t& other)
{
PRX_ASSERT(this != NULL);
PRX_ASSERT(&other != NULL);
q = other.q;
}
osg::ref_ptr<osg::Node> osg_scene_t::make_geometry(const std::string& material_name, const geometry_t* geom, osg_material_t* material, bool transparent, bool use_color)
{
// The material settings from XML
// PRX_DEBUG_S("Looking for material: " << material_name.c_str() << " for the type : " << geom->get_type());
PRX_ASSERT(template_material_mapping.find(material_name) != template_material_mapping.end());
material = new osg_material_t(*(template_material_mapping[material_name]));
PRX_ASSERT(geom != NULL);
// TODO: Eventually line_thickness could represent other attributes as well (i.e. include transparency and line)
osg::ref_ptr<osg::Node> node = osg_geode_t::setup_geometry(geom, transparent, line_thickness);
// Use diffuse/specular/shininess
if( !transparent && !use_color )
material->setColorMode(osg::Material::OFF);
osg::StateSet* stateSet = node->getOrCreateStateSet();
if( transparent )
{
stateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
// Enable depth test so that an opaque polygon will occlude a transparent one behind it.
stateSet->setMode(GL_BLEND, osg::StateAttribute::ON);
stateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
// Conversely, disable writing to depth buffer so that
// a transparent polygon will allow polygons behind it to shine thru.
// OSG renders transparent polygons after opaque ones.
osg::Depth* depth = new osg::Depth;
depth->setWriteMask(false);
stateSet->setAttributeAndModes(depth, osg::StateAttribute::ON);
// Disable conflicting modes.
stateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
PRX_DEBUG_S("setting transparency");
material->setTransparency(osg::Material::FRONT_AND_BACK, 0.1f);
}
// Apply the material to the node.
if( !use_color )
stateSet->setAttribute(material, osg::StateAttribute::ON);
if( !use_color )
node->setStateSet(stateSet);
return node;
}
void vector_t::get( double* x, double* y, double* z) const
{
PRX_ASSERT(_vec.size() == 3);
*x = _vec[0];
*y = _vec[1];
*z = _vec[2];
}
void geometry_t::get_triangle(vector_t* v1, vector_t* v2, vector_t* v3) const
{
PRX_ASSERT(type == PRX_TRIANGLE);
trimesh.get_vertex_at(0,v1);
trimesh.get_vertex_at(1,v2);
trimesh.get_vertex_at(2,v3);
}
void vector_t::get(double& x, double& y, double& z) const
{
PRX_ASSERT(_vec.size() == 3);
x = _vec[0];
y = _vec[1];
z = _vec[2];
}
void vector_t::set( const double x, const double y, const double z)
{
PRX_ASSERT(_vec.size() == 3);
_vec[0] = x;
_vec[1] = y;
_vec[2] = z;
}
vector_t& vector_t::operator/= ( const vector_t& vec )
{
PRX_ASSERT( _vec.size() == vec._vec.size() );
for( unsigned int i=0; i<_vec.size(); i++ )
_vec[i] /= vec._vec[i];
return *this;
}
vector_t vector_t::operator+ ( const vector_t& vec ) const
{
PRX_ASSERT( _vec.size() == vec._vec.size() );
vector_t ret = *this;
ret._vec += vec._vec;
return ret;
}
vector_t vector_t::operator- ( const vector_t& vec ) const
{
PRX_ASSERT( _vec.size() == vec._vec.size() );
vector_t ret( _vec.size() );
ret._vec = _vec - vec._vec;
return ret;
}
bool osg_visualization_t::run()
{
PRX_ASSERT(initialized);
// bool init = false;
sys_clock_t timer;
// Listens for updates to rigid body configurations.
osg_scene = dynamic_cast<osg_scene_t*>(get_scene());
while( ros::ok() && !viewer.done() )
{
// ros::getGlobalCallbackQueue()->callAvailable();
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.001));
update_configurations(*listener, *osg_scene);
for (unsigned i = 0; i <window_screenshot_queue.size(); i++)
{
viewer.take_screenshot(window_screenshot_queue[i].first, window_screenshot_queue[i].second );
}
viewer.frame();
window_screenshot_queue.clear();
// if(!init || timer.measure() > 5)
// {
// comm->poll_topics();
// timer.reset();
// init = true;
// }
}
return true;
}
void quaternion_t::get( vector_t& v) const
{
PRX_ASSERT(v.get_dim() == 4);
v[0] = q[0];
v[1] = q[1];
v[2] = q[2];
v[3] = q[3];
}
void quaternion_t::set( const vector_t& v)
{
PRX_ASSERT(v.get_dim() == 4);
q[0] = v[0];
q[1] = v[1];
q[2] = v[2];
q[3] = v[3];
}
void geometry_t::get_quad(vector_t* v1, vector_t* v2, vector_t* v3, vector_t* v4) const
{
PRX_ASSERT(type == PRX_QUAD);
trimesh.get_vertex_at(0,v1);
trimesh.get_vertex_at(1,v2);
trimesh.get_vertex_at(2,v3);
trimesh.get_vertex_at(3,v4);
}
void geometry_t::get_box(double& lx, double& ly, double& lz) const
{
PRX_ASSERT(type == PRX_BOX);
lx = info[0];
ly = info[1];
lz = info[2];
}
quaternion_t::quaternion_t( const vector_t* v)
{
PRX_ASSERT(v->get_dim() == 4);
q[0] = v->at(0);
q[1] = v->at(1);
q[2] = v->at(2);
q[3] = v->at(3);
}
void vector_t::cross_product( const vector_t& alpha, const vector_t& beta )
{
PRX_ASSERT( _vec.size() == 3 && alpha._vec.size() == 3 && beta._vec.size() == 3 );
_vec[0] = alpha._vec[1] * beta._vec[2] - alpha._vec[2] * beta._vec[1];
_vec[1] = alpha._vec[2] * beta._vec[0] - alpha._vec[0] * beta._vec[2];
_vec[2] = alpha._vec[0] * beta._vec[1] - alpha._vec[1] * beta._vec[0];
}
vector_t vector_t::operator/ ( const vector_t & vec ) const
{
PRX_ASSERT( _vec.size() == vec._vec.size() );
vector_t ret( _vec.size() );
for( unsigned int i=0; i<_vec.size(); ++ i)
ret._vec[i] = _vec[i] / vec._vec[i];
return ret;
}
double vector_t::dot_product( const vector_t& other ) const
{
PRX_ASSERT( _vec.size() == other._vec.size() );
double dotp = 0.0;
for( unsigned int i=0; i<_vec.size(); i++ )
dotp += _vec[i] * other._vec[i];
return dotp;
}
bool vector_t::operator> ( const vector_t & vec ) const
{
PRX_ASSERT( _vec.size() == vec._vec.size() );
for( unsigned int i=0; i<_vec.size(); ++i )
if( _vec[i] <= vec._vec[i] )
return false;
return true;
}
void augment_config_list(config_list_t& list, unsigned index)
{
PRX_ASSERT(list.size()>=index);
if( list.size() == index )
{
list.push_back(config_list_element_t("", config_t()));
}
}
void proximity_node_t::replace_neighbor( unsigned prev, int new_index )
{
int index;
for( index=0; index<nr_neighbors; index++ )
{
if( neighbors[index] == prev )
break;
}
PRX_ASSERT( index < nr_neighbors );
neighbors[index] = new_index;
}
bool motion_planner_t::execute()
{
PRX_ASSERT(input_specification != NULL);
do
{
// PRX_INFO_S("EXECUTE FROM MOTION PLANNER");
step();
}
while( !input_specification->get_stopping_criterion()->satisfied() );
return succeeded();
}
void quaternion_t::compute_rotation(const vector_t& v1, const vector_t& v2)
{
PRX_ASSERT(v1.get_dim() == v2.get_dim());
vector_t v(3);
double w;
v.cross_product(v1,v2);
w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
set(v[0],v[1],v[2],w);
normalize();
}
void goal_state_t::set_goal_state(const space_point_t* goal_state)
{
space->copy_point(point, goal_state);
if( goal_points.size() != 0 )
{
PRX_ASSERT(goal_points.size() <= 1);
space->copy_point(goal_points[0], point);
}
else
{
goal_points.push_back(point);
}
}
void update( astar_node_t* element, astar_node_t* new_element )
{
PRX_ASSERT(finder.find(element) != finder.end());
unsigned index = finder[element];
finder[new_element] = index;
finder.erase(element);
heap[index] = new_element;
reheap_up(index);
reheap_down(index);
delete element;
}
void proximity_node_t::delete_neighbor( unsigned int nd )
{
int index;
for( index=0; index<nr_neighbors; index++ )
{
if( neighbors[index] == nd )
break;
}
PRX_ASSERT( index < nr_neighbors );
for( int i=index; i<nr_neighbors-1; i++ )
neighbors[i] = neighbors[i+1];
nr_neighbors--;
}
void pebble_edge_t::get_plan(sim::plan_t& computed_plan, unsigned source)
{
if( p_source == source )
computed_plan += plan;
else
{
PRX_ASSERT(p_target == source);
for( int i = (int)plan.size() - 1; i >= 0; --i )
{
computed_plan.copy_onto_back(plan[i].control, plan[i].duration);
}
}
}
void config_t::global_to_relative( const config_t& root_config )
{
//Have an identity temporary orientaiton
util::quaternion_t tmp_orient;
tmp_orient.zero();
//Then, set our position to be the relative position
this->set_position( this->get_position() - root_config.get_position() );
tmp_orient /= root_config.get_orientation();
this->set_position(vector_t(tmp_orient.qv_rotation(this->get_position())));
tmp_orient *= this->get_orientation();
tmp_orient.normalize();
PRX_ASSERT(tmp_orient.is_valid());
this->set_orientation(tmp_orient);
}
void simple_controller_t::verify() const
{
// Check our spaces to make sure they exist
// PRX_ASSERT(input_control_space != NULL);
PRX_ASSERT(output_control_space != NULL);
PRX_ASSERT(state_space != NULL);
// Simple controllers must have one subsystem
PRX_ASSERT(subsystems.size() == 1);
// Verify validity of our spaces
if( input_control_space != NULL )
input_control_space->verify();
output_control_space->verify();
state_space->verify();
if( controller_state_space )
controller_state_space->verify();
hash_t<std::string, system_ptr_t >::const_iterator subsystems_const_iter;
// Call our subsystems verify
subsystems.begin()->second->verify();
// Check to make sure our output_control_space == subsytems->get_control_space()
std::string space_name;
const space_t* cspace;
cspace = subsystems.begin()->second->get_control_space();
if( cspace != NULL )
space_name = cspace->get_space_name();
// Throw an exception if we do not match control spaces
if( std::strcmp(space_name.c_str(), output_control_space->get_space_name().c_str()) )
{
PRX_ERROR_S("Output control space does not match subsystem's input control space");
throw space_mismatch_exception();
}
}
double default_uniform_t::trajectory_cost(const trajectory_t& t)
{
PRX_ASSERT(t.size()>0);
double cost = 0;
trajectory_t::const_iterator i = t.begin();
trajectory_t::const_iterator j = t.begin();
j++;
for ( ; j != t.end(); ++i,++j)
{
cost+=dist(*i,*j)*state_cost(*j);
}
return cost;
}
