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; }