/**
* Print a SIP Message to stdout
*
* @param msg SIP Message
*/
void sip_msg_dump(const struct sip_msg *msg)
{
struct le *le;
uint32_t i;
if (!msg)
return;
for (i=0; i<HDR_HASH_SIZE; i++) {
le = list_head(hash_list(msg->hdrht, i));
while (le) {
const struct sip_hdr *hdr = le->data;
le = le->next;
(void)re_printf("%02u '%r'='%r'\n", i, &hdr->name,
&hdr->val);
}
}
le = list_head(&msg->hdrl);
while (le) {
const struct sip_hdr *hdr = le->data;
le = le->next;
(void)re_printf("%02u '%r'='%r'\n", hdr->id, &hdr->name,
&hdr->val);
}
}
/**
* Apply a function handler to certain unknown SIP Headers
*
* @param msg SIP Message
* @param fwd True to traverse forwards, false to traverse backwards
* @param name SIP Header name
* @param h Function handler
* @param arg Handler argument
*
* @return SIP Header if handler returns true, otherwise NULL
*/
const struct sip_hdr *sip_msg_xhdr_apply(const struct sip_msg *msg,
bool fwd, const char *name,
sip_hdr_h *h, void *arg)
{
struct list *lst;
struct le *le;
struct pl pl;
if (!msg || !name)
return NULL;
pl_set_str(&pl, name);
lst = hash_list(msg->hdrht, hdr_hash(&pl));
le = fwd ? list_head(lst) : list_tail(lst);
while (le) {
const struct sip_hdr *hdr = le->data;
le = fwd ? le->next : le->prev;
if (pl_casecmp(&hdr->name, &pl))
continue;
if (!h || h(hdr, msg, arg))
return hdr;
}
return NULL;
}
/**
* Apply a function handler to certain SIP Headers
*
* @param msg SIP Message
* @param fwd True to traverse forwards, false to traverse backwards
* @param id SIP Header ID
* @param h Function handler
* @param arg Handler argument
*
* @return SIP Header if handler returns true, otherwise NULL
*/
const struct sip_hdr *sip_msg_hdr_apply(const struct sip_msg *msg,
bool fwd, enum sip_hdrid id,
sip_hdr_h *h, void *arg)
{
struct list *lst;
struct le *le;
if (!msg)
return NULL;
lst = hash_list(msg->hdrht, id);
le = fwd ? list_head(lst) : list_tail(lst);
while (le) {
const struct sip_hdr *hdr = le->data;
le = fwd ? le->next : le->prev;
if (hdr->id != id)
continue;
if (!h || h(hdr, msg, arg))
return hdr;
}
return NULL;
}
// This may execute thousands of queries (a block's worth).
void block_chain_impl::fetch_block_transaction_hashes(const hash_digest& hash,
transaction_hashes_fetch_handler handler)
{
const auto do_fetch = [this, hash, handler](size_t slock)
{
const auto result = database_.blocks.get(hash);
return result ?
finish_fetch(slock, handler, error::success, to_hashes(result)) :
finish_fetch(slock, handler, error::not_found, hash_list());
};
fetch_parallel(do_fetch);
}
void Socket::_bind()
{
static bool run = true;
if (run)
{
m_bind_hash = hash_list (m_bind_endpts);
if (!m_bind.count (m_bind_hash))
{
m_bind[m_bind_hash] = std::make_shared<zmq::socket_t> (zmq::socket_t (Context::get(), m_type));
for (auto opt : m_sockopts)
m_bind[m_bind_hash] -> setsockopt (opt.first, opt.second.val.get(), opt.second.vsize);
for (const std::string &e : m_bind_endpts)
m_bind[m_bind_hash]->bind (e.c_str());
}
m_sock = m_bind[m_bind_hash];
}
return;
}
static struct sip_udpconn *udpconn_find(struct sip *sip, struct udp_sock *us,
const struct sa *paddr)
{
struct le *le;
le = list_head(hash_list(sip->ht_udpconn, sa_hash(paddr, SA_ALL)));
for (; le; le = le->next) {
struct sip_udpconn *uc = le->data;
if (!sa_cmp(&uc->paddr, paddr, SA_ALL))
continue;
if (uc->us != us)
continue;
return uc;
}
return NULL;
}
static struct sip_conn *conn_find(struct sip *sip, const struct sa *paddr,
bool secure)
{
struct le *le;
le = list_head(hash_list(sip->ht_conn, sa_hash(paddr, SA_ALL)));
for (; le; le = le->next) {
struct sip_conn *conn = le->data;
if (!secure != (conn->sc == NULL))
continue;
if (!sa_cmp(&conn->paddr, paddr, SA_ALL))
continue;
return conn;
}
return NULL;
}
void initialize_simplex_list(simplex_list *sl, int size) {
initialize_list(sl,sizeof(simplex_index *),equal_simplex);
hash_list(sl, size, hash_simplex);
}
void initialize_face_list(face_list *AFL, int size) {
initialize_list(AFL,sizeof(face *),equal_face);
hash_list(AFL, size, hash_face);
}
ppfmap::Pose ppfmap::CudaPPFMatch<PointT, NormalT>::getPose(
const int reference_index,
const std::vector<int>& indices,
const PointCloudPtr cloud,
const NormalsPtr normals,
const float affine_s[12]) {
Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tsg_map(affine_s);
float affine_m[12];
const std::size_t n = indices.size();
const auto& ref_point = cloud->at(reference_index);
const auto& ref_normal = normals->at(reference_index);
thrust::host_vector<uint32_t> hash_list(n);
thrust::host_vector<float> alpha_s_list(n);
// Compute the PPF feature for all the pairs in the neighborhood
for (int i = 0; i < n; i++) {
const int index = indices[i];
const auto& point = cloud->at(index);
const auto& normal = normals->at(index);
// Compute the PPF between reference_point and the i-th neighbor
hash_list[i] = computePPFFeatureHash(ref_point, ref_normal,
point, normal,
distance_step,
angle_step);
// Compute the alpha_s angle
const Eigen::Vector3f transformed(Tsg_map * point.getVector4fMap());
alpha_s_list[i] = atan2f(-transformed(2), transformed(1));
}
int index;
float alpha;
int votes;
map->searchBestMatch(hash_list, alpha_s_list, index, alpha, votes);
const auto& model_point = model_->at(index);
const auto& model_normal = normals_->at(index);
getAlignmentToX(model_point, model_normal, &affine_m);
Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tmg_map(affine_m);
Eigen::Affine3f Tsg, Tmg;
Tsg.matrix().block<3, 4>(0, 0) = Tsg_map.matrix();
Tmg.matrix().block<3, 4>(0, 0) = Tmg_map.matrix();
// Set final pose
Pose final_pose;
final_pose.c = pcl::Correspondence(reference_index, index, 0.0f);
final_pose.t = Tsg.inverse() * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX()) * Tmg;
final_pose.votes = votes;
return final_pose;
}
