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