math::AABB<T, 3> compute_aabb( const Iterator& beginIter, const Iterator& endIter ) { assert(beginIter != endIter && "Point cloud must have at least one point."); math::AABB<T, 3> aabb( to_vec(*beginIter), to_vec(*beginIter) ); for (Iterator i = beginIter; i != endIter; ++i) { aabb.extend( to_vec(*i) ); } return aabb; }
BulletRigidBody::BulletRigidBody(RigidBody* pInterface_, BulletDynamicsWorld* dynamicsWorld_, btRigidBody* rigidBody_, const std::string& name_ ) : base_type(pInterface, dynamicsWorld_), rigidBody(rigidBody_) { assert(rigidBody); motionState.reset( new BulletMotionState(this) ); { btTransform transform; if (btMotionState* ms = rigidBody->getMotionState() ) { ms->getWorldTransform(transform); } else { transform = rigidBody->getWorldTransform(); } motionState->setWorldTransform(transform); rigidBody->setMotionState( motionState.get() ); } rigidBody->setUserPointer( pInterface ); // fill up desc pInterface_->desc.transform = getTransform(); if ( rigidBody->getCollisionFlags() & btCollisionObject::CF_KINEMATIC_OBJECT ) { pInterface_->desc.type = RigidBody::DT_KINEMATIC; } else if ( rigidBody->getCollisionFlags() & btCollisionObject::CF_STATIC_OBJECT ) { pInterface_->desc.type = RigidBody::DT_STATIC; } else { pInterface_->desc.type = RigidBody::DT_DYNAMIC; } pInterface_->desc.mass = real(1.0) / rigidBody->getInvMass(); pInterface_->desc.friction = rigidBody->getFriction(); pInterface_->desc.linearVelocity = to_vec( rigidBody->getLinearVelocity() ); pInterface_->desc.angularVelocity = to_vec( rigidBody->getAngularVelocity() ); pInterface_->desc.name = name_; CollisionShape* collisionShape = reinterpret_cast<CollisionShape*>( rigidBody->getCollisionShape()->getUserPointer() ); if (!collisionShape) { collisionShape = createCollisionShape( *rigidBody->getCollisionShape() ); } pInterface_->desc.collisionShape.reset(collisionShape); AUTO_LOGGER_MESSAGE(log::S_FLOOD, "Creating rigid body from btRigidBody:\n" << pInterface_->desc << LOG_FILE_AND_LINE); }
vec mix::process(bvec ce, mat x) { vec y; int N; bvec one = ("1"); #if (DEBUG_LEVEL==3) cout << "***** mix::process *****" << endl; cout << "ce=" << ce << endl; cout << "x=" << x << endl; sleep(1000); #endif N=ce.length(); if (x.rows()!=N) { throw sci_exception("mix::process - ce.size <> x.rows()", x.rows() ); } if (x.cols()!=2) { throw sci_exception("mix::process - x=[x1,x1] - x.cols()!=2", x.cols() ); } y.set_length(N); for (int i=0; i<N; i++) { if ( bool(ce[i])) { y0 = G.process(one, to_vec(x(i,0)*x(i,1)))(0); } y[i]=y0; } #if (DEBUG_LEVEL==3) cout << "y=" << y << endl; cout << "+++++ mix::process +++++" << endl; sleep(1000); #endif return (y); }
int main() { std::map<int, int> m1 = { {1, 10}, {2, 20}, {3, 30} }; std::map<char, std::string> m2 = { {'a', "apple"}, {'b', "ball"}, {'c', "car"} }; std::map<float, std::tuple<int, int>> m3 = { {1.1f, std::make_tuple(1, 1)}, {2.2f, std::make_tuple(2, 2)}, {3.3f, std::make_tuple(3, 3)} }; print_vec(to_vec(m1)); print_vec(to_vec(m2)); print_vec(to_vec(m3)); }
void BERC::estimate_delay(const bvec &in1, const bvec &in2, int mindelay, int maxdelay) { int num, start1, start2; int min_input_length = std::min(in1.length(), in2.length()); int bestdelay = mindelay; double correlation; double bestcorr = 0; for (int i = mindelay; i < maxdelay; i++) { num = min_input_length - std::abs(i) - ignorefirst - ignorelast; start1 = (i < 0) ? -i : 0; start2 = (i > 0) ? i : 0; correlation = fabs(sum(to_vec(elem_mult(in1.mid(start1, num), in2.mid(start2, num))))); if (correlation > bestcorr) { bestdelay = i; bestcorr = correlation; } } delay = bestdelay; }
math::Vector3r BulletRigidBody::getAngularVelocity() const { return to_vec( rigidBody->getAngularVelocity() ); }
math::Vector3r BulletRigidBody::getTotalTorque() const { // I wonder getTotalTorque function is not const return to_vec( const_cast<btRigidBody&>(*rigidBody).getTotalTorque() ); }
//' rcpp_make_compact_graph //' //' Removes nodes and edges from a graph that are not needed for routing //' //' @param graph graph to be processed //' @param quiet If TRUE, display progress //' @return \code{Rcpp::List} containing one \code{data.frame} with the compact //' graph, one \code{data.frame} with the original graph and one //' \code{data.frame} containing information about the relating edge ids of the //' original and compact graph. //' //' @noRd // [[Rcpp::export]] Rcpp::List rcpp_make_compact_graph (Rcpp::DataFrame graph, bool quiet) { vertex_map_t vertices; edge_map_t edge_map; std::unordered_map <osm_id_t, int> components; int largest_component; vert2edge_map_t vert2edge_map; if (!quiet) { Rcpp::Rcout << "Constructing graph ... "; Rcpp::Rcout.flush (); } graph_from_df (graph, vertices, edge_map, vert2edge_map); if (!quiet) { Rcpp::Rcout << std::endl << "Determining connected components ... "; Rcpp::Rcout.flush (); } get_largest_graph_component (vertices, components, largest_component); if (!quiet) { Rcpp::Rcout << std::endl << "Removing intermediate nodes ... "; Rcpp::Rcout.flush (); } vertex_map_t vertices2 = vertices; edge_map_t edge_map2 = edge_map; contract_graph (vertices2, edge_map2, vert2edge_map); if (!quiet) { Rcpp::Rcout << std::endl << "Mapping compact to original graph ... "; Rcpp::Rcout.flush (); } int nedges = edge_map2.size (); // These vectors are all for the contracted graph: Rcpp::StringVector from_vec (nedges), to_vec (nedges), highway_vec (nedges); Rcpp::NumericVector from_lat_vec (nedges), from_lon_vec (nedges), to_lat_vec (nedges), to_lon_vec (nedges), dist_vec (nedges), weight_vec (nedges), edgeid_vec (nedges); unsigned int map_size = 0; // size of edge map contracted -> original unsigned int en = 0; std::map <int, osm_edge_t> edge_ordered; for (auto e = edge_map2.begin (); e != edge_map2.end (); ++e) edge_ordered.emplace (e->first, e->second); for (auto e = edge_ordered.begin (); e != edge_ordered.end (); ++e) { osm_id_t from = e->second.get_from_vertex (); osm_id_t to = e->second.get_to_vertex (); osm_vertex_t from_vtx = vertices2.at (from); osm_vertex_t to_vtx = vertices2.at (to); from_vec (en) = from; to_vec (en) = to; highway_vec (en) = e->second.highway; dist_vec (en) = e->second.dist; weight_vec (en) = e->second.weight; from_lat_vec (en) = from_vtx.getLat (); from_lon_vec (en) = from_vtx.getLon (); to_lat_vec (en) = to_vtx.getLat (); to_lon_vec (en) = to_vtx.getLon (); edgeid_vec (en) = e->second.getID (); map_size += e->second.is_replacement_for ().size (); en++; } Rcpp::NumericVector edge_id_orig (map_size), edge_id_comp (map_size); int pos = 0; for (auto e = edge_ordered.begin (); e != edge_ordered.end (); ++e) { int eid = e->second.getID (); std::set <int> edges = e->second.is_replacement_for (); for (auto ei: edges) { edge_id_comp (pos) = eid; edge_id_orig (pos++) = ei; } } Rcpp::DataFrame compact = Rcpp::DataFrame::create ( Rcpp::Named ("from_id") = from_vec, Rcpp::Named ("to_id") = to_vec, Rcpp::Named ("edge_id") = edgeid_vec, Rcpp::Named ("d") = dist_vec, Rcpp::Named ("d_weighted") = weight_vec, Rcpp::Named ("from_lat") = from_lat_vec, Rcpp::Named ("from_lon") = from_lon_vec, Rcpp::Named ("to_lat") = to_lat_vec, Rcpp::Named ("to_lon") = to_lon_vec, Rcpp::Named ("highway") = highway_vec); Rcpp::DataFrame rel = Rcpp::DataFrame::create ( Rcpp::Named ("id_compact") = edge_id_comp, Rcpp::Named ("id_original") = edge_id_orig); if (!quiet) Rcpp::Rcout << std::endl; return Rcpp::List::create ( Rcpp::Named ("compact") = compact, Rcpp::Named ("original") = graph, Rcpp::Named ("map") = rel); }
int tf_csim_casdec_x() { CSIM::fir_x fir_x1, fir_x2, fir_x3; CSIM::counter cnt1, cnt2, cnt3; CSIM::wgn_x wgn_x1; CSIM::casdec_x casdec_x1; int N; cvec c0; cvec x_x; bvec ce, ce1, ce2, ce3; cvec ya_x1, ya_x2, ya_x3; cmat ya, yb, yc; //---------------------------------------- // test of csim //---------------------------------------- cout << "CSIM::casdec_x Test" << endl; try { // mav<4> c0="(0.25,0.0) (0.25,0.0) (0.25, 0.0) (0.25,0.0)"; cout << "c0=" << c0 << endl; // discrete cascade settings fir_x1.set_taps(c0); fir_x1.reset(); fir_x2.set_taps(c0); fir_x2.reset(); fir_x3.set_taps(c0); fir_x3.reset(); cnt1.set_N(2); cnt1.reset(); cnt2.set_N(2); cnt2.reset(); cnt3.set_N(2); cnt3.reset(); cout << "discrete cascade settings " << endl; // device cascade settings casdec_x1.set_size(3); casdec_x1.set_taps(c0); casdec_x1.reset(); cout << "device cascade settings " << endl; N=256; // clock and signal ce.set_length(N); ce.ones(); // clock cout << "ce=" << ce << endl; // discrete cascade proc while (1) { x_x = wgn_x1.generate(ce); // source - random mean = (0.0+j0.0) sigma =1.0 cout << "x_x=" << x_x << endl; ya_x1 = fir_x1.process(ce,x_x); ce1 = cnt1.generate(ce); ya_x2 = fir_x2.process(ce1,ya_x1); ce2 = cnt2.generate(ce1); ya_x3 = fir_x3.process(ce2,ya_x2); ce3 = cnt3.generate(ce2); ya.set_size(N,2); ya.set_col(0,ya_x3); ya.set_col(1,to_cvec(to_vec(ce3), zeros(ce3.length()))); // cout << "discrete cascade proc " << endl; // device cascade proc yb = casdec_x1.process(ce, x_x); // cout << "device cascade proc " << endl; yc.set_size(N,4); yc.set_col(0, ya.get_col(0)); yc.set_col(1, yb.get_col(0)); yc.set_col(2, ya.get_col(1)); yc.set_col(3, yb.get_col(1)); cout << "yc=" << endl << yc << endl; } } catch (sci_exception except) { cout<< "\n sci exception:" << endl <<except.get_msg() <<":" << except.get_info() << endl; } return 0; }