double vertex_cache::position_closest_to(pixel_position const &target_pos)
{
bool first = true;
pixel_position old_pos, new_pos;
double lin_pos = 0.0, min_pos = 0.0, min_dist_sq = std::numeric_limits<double>::max();
// find closest approach of each individual segment to the
// target position. would be good if there were some kind
// of prior, or fast test to avoid calculating on each
// segment, but i can't think of one.
for (segment const &seg : current_subpath_->vector)
{
if (first)
{
old_pos = seg.pos;
min_pos = lin_pos;
min_dist_sq = dist_sq(target_pos - old_pos);
first = false;
}
else
{
new_pos = seg.pos;
pixel_position d = new_pos - old_pos;
if ((d.x != 0.0) || (d.y != 0))
{
pixel_position c = target_pos - old_pos;
double t = (c.x * d.x + c.y * d.y) / dist_sq(d);
if ((t >= 0.0) && (t <= 1.0))
{
pixel_position pt = (d * t) + old_pos;
double pt_dist_sq = dist_sq(target_pos - pt);
if (pt_dist_sq < min_dist_sq)
{
min_dist_sq = pt_dist_sq;
min_pos = lin_pos + seg.length * t;
}
}
}
old_pos = new_pos;
lin_pos += seg.length;
double end_dist_sq = dist_sq(target_pos - old_pos);
if (end_dist_sq < min_dist_sq)
{
min_dist_sq = end_dist_sq;
min_pos = lin_pos;
}
}
}
return min_pos;
}
static inline
void update_jprev( Tracer *p, const Junction *J )
{
if(
( p->jprev == 0 ) ||
( dist_sq(p->vertex,J->vertex) < dist_sq(p->vertex, p->jprev->vertex) )
)
{
p->jprev = J;
}
}
static inline
void update_jnext( Tracer *p, const Junction *J )
{
if(
( p->jnext == 0 ) ||
( dist_sq(p->vertex,J->vertex) < dist_sq(p->vertex, p->jnext->vertex) )
)
{
p->jnext = J;
}
}
TYPED_TEST(TripletLossLayerTest, TestForward) {
typedef typename TypeParam::Dtype Dtype;
LayerParameter layer_param;
TripletLossLayer<Dtype> layer(layer_param);
layer.SetUp(this->blob_bottom_vec_, this->blob_top_vec_);
layer.Forward(this->blob_bottom_vec_, this->blob_top_vec_);
// manually compute to compare
const Dtype margin = layer_param.contrastive_loss_param().margin();
const int num = this->blob_bottom_data_i_->num();
const int channels = this->blob_bottom_data_i_->channels();
Dtype loss(0);
for (int i = 0; i < num; ++i) {
Dtype dist_sq(0);
for (int j = 0; j < channels; ++j) {
Dtype diff = this->blob_bottom_data_i_->cpu_data()[i*channels+j] -
this->blob_bottom_data_j_->cpu_data()[i*channels+j];
dist_sq += diff*diff;
}
if (this->blob_bottom_y_->cpu_data()[i]) { // similar pairs
loss += dist_sq;
} else {
loss += std::max(margin-dist_sq, Dtype(0));
}
}
loss /= static_cast<Dtype>(num) * Dtype(2);
EXPECT_NEAR(this->blob_top_loss_->cpu_data()[0], loss, 1e-6);
}
static inline void dists_sq(const data_t1* q, length_t m, const data_t2* x,
length_t n, data_t3* out, length_t stride=1)
{
return mapSubseqs([m, q](const data_t2* x) {
return dist_sq(q, x, m);
}, m, x, n, out, stride);
}
/**
* vu_get_tz_at_location:
*
* @vc: Position for which the time zone is desired
*
* Returns: TimeZone string of the nearest known location. String may be NULL.
*
* Use the k-d tree method (http://en.wikipedia.org/wiki/Kd-tree) to quickly retreive
* the nearest location to the given position.
*/
gchar* vu_get_tz_at_location ( const VikCoord* vc )
{
gchar *tz = NULL;
if ( !vc || !kd )
return tz;
struct LatLon ll;
vik_coord_to_latlon ( vc, &ll );
double pt[2] = { ll.lat, ll.lon };
gdouble nearest;
if ( !a_settings_get_double(VIK_SETTINGS_NEAREST_TZ_FACTOR, &nearest) )
nearest = 1.0;
struct kdres *presults = kd_nearest_range ( kd, pt, nearest );
while( !kd_res_end( presults ) ) {
double pos[2];
gchar *ans = (gchar*)kd_res_item ( presults, pos );
// compute the distance of the current result from the pt
double dist = sqrt( dist_sq( pt, pos, 2 ) );
if ( dist < nearest ) {
//printf( "NEARER node at (%.3f, %.3f, %.3f) is %.3f away is %s\n", pos[0], pos[1], pos[2], dist, ans );
nearest = dist;
tz = ans;
}
kd_res_next ( presults );
}
g_debug ( "TZ lookup found %d results - picked %s", kd_res_size(presults), tz );
kd_res_free ( presults );
return tz;
}
void checkGoals(int x, int y){
CvPoint robotPt = cvPoint(x, y);
if (sqrt(dist_sq(goal, robotPt)) <= GOAL_TOLERANCE){
score++;
pickNewGoal();
printf("GOAL!\n");
}
}
int main(int argc, char **argv) {
int i, num_pts = DEF_NUM_PTS;
void *ptree;
char *data, *pch;
struct kdres *presults;
double pos[3], dist;
double pt[3] = { 0, 0, 1 };
double radius = 10;
if(argc > 1 && isdigit(argv[1][0])) {
num_pts = atoi(argv[1]);
}
if(!(data = malloc(num_pts))) {
perror("malloc failed");
return 1;
}
srand( time(0) );
/* create a k-d tree for 3-dimensional points */
ptree = kd_create( 3 );
/* add some random nodes to the tree (assert nodes are successfully inserted) */
for( i=0; i<num_pts; i++ ) {
data[i] = 'a' + i;
assert( 0 == kd_insert3( ptree, rd(), rd(), rd(), &data[i] ) );
}
/* find points closest to the origin and within distance radius */
presults = kd_nearest_range( ptree, pt, radius );
/* print out all the points found in results */
printf( "found %d results:\n", kd_res_size(presults) );
while( !kd_res_end( presults ) ) {
/* get the data and position of the current result item */
pch = (char*)kd_res_item( presults, pos );
/* compute the distance of the current result from the pt */
dist = sqrt( dist_sq( pt, pos, 3 ) );
/* print out the retrieved data */
printf( "node at (%.3f, %.3f, %.3f) is %.3f away and has data=%c\n",
pos[0], pos[1], pos[2], dist, *pch );
/* go to the next entry */
kd_res_next( presults );
}
/* free our tree, results set, and other allocated memory */
free( data );
kd_res_free( presults );
kd_free( ptree );
return 0;
}
/*! The IntersectAction callback for Geometry. It computes if the ray used in
the IntersectAction \a action hits this object and if that is the case,
which triangle is hit.
\param[in] action IntersectAction performing the intersect test.
\return Action result code, \see OSG::Action.
\note This method is registered with the IntersectAction and automatically
called from there, you probably never have to call it manually.
*/
Action::ResultE Geometry::intersect(Action * action)
{
IntersectAction *ia = dynamic_cast<IntersectAction*>(action);
ia->getActNode()->updateVolume();
const BoxVolume &bv = ia->getActNode()->getVolume();
if(bv.isValid() && !bv.intersect(ia->getLine()))
{
return Action::Skip; //bv missed -> can not hit children
}
TriangleIterator it = this->beginTriangles();
TriangleIterator end = this->endTriangles ();
Real32 t;
Vec3f norm;
Line ia_line(ia->getLine());
for(; it != end; ++it)
{
if(ia_line.intersect(it.getPosition(0),
it.getPosition(1),
it.getPosition(2), t, &norm))
{
ia->setHit(t, ia->getActNode(), it.getIndex(), norm, -1);
}
}
// If we need to test lines, iterate over lines and test for
// lines that are within width distance from the line
if(ia->getTestLines())
{
Real32 range_sq = ia->getTestLineWidth();
range_sq = range_sq * range_sq;
LineIterator it = this->beginLines();
LineIterator end = this->endLines ();
Pnt3f pt1, pt2;
OSG::Vec3f norm;
// Find closest points and if they are within the range, then add a hit
for(; it != end; ++it)
{
Line cur_line(it.getPosition(0), it.getPosition(1));
ia_line.getClosestPoints(cur_line, pt1, pt2);
Real32 dist_sq( pt1.dist2(pt2) );
if (dist_sq <= range_sq)
{
t = ia_line.getPosition().dist(pt1);
ia->setHit(t, ia->getActNode(), -1, norm, it.getIndex());
}
}
}
return Action::Continue;
}
void pickNewGoal(){
CvPoint newGoal;
do {
int x = boundedRandom(-2048+600, 2047-600);
int y = boundedRandom(-2048+600, 2047-600);
newGoal = cvPoint(x,y);
} while (dist_sq(newGoal, goal) < 1024*1024); //require goals to be at least 18in. (1024 ticks) apart
goal = newGoal;
}
vec2f* ControlOverlay::pick_point(const vec2i& canvas_pos)
{
vec2f world_pos = m_camera->canvas2world(canvas_pos);
float threshold = sq(2 * m_point_radius / (m_camera->scale() * m_camera->size().y));
for (auto& point : m_points)
{
if (dist_sq(world_pos, *point) <= threshold)
return point;
}
return nullptr;
}
XnFloat Cylinder::getCenterDistSq(XnPoint3D p)
{
return dist_sq(p, getCenter());
}
float RectPrism::getCenterDistSq(XnPoint3D p)
{
return dist_sq(p, getCenter());
}
void generate_binary()
{
struct bench_args_t data;
char *ptr;
int status, i, j, k, reject, fd, written=0;
dvector_t p, q;
ivector_t b;
dvector_t points[nAtoms];
int idx, entry;
const double infinity = (domainEdge*domainEdge*3.)*1000;//(max length)^2 * 1000
// Create random positions in the box [0,domainEdge]^3
srandom(1);
i=0;
while( i<nAtoms ) {
// Generate a new point
p.x = domainEdge*(((double)random())/((double)RAND_MAX));
p.y = domainEdge*(((double)random())/((double)RAND_MAX));
p.z = domainEdge*(((double)random())/((double)RAND_MAX));
// Assure that it's not directly on top of another atom
reject = 0;
for( idx=0; idx<nAtoms; idx++ ) {
q = points[idx];
if( dist_sq(p,q)<van_der_Waals_thresh ) {
reject=1;
break;
}
}
if(!reject) {
points[i] = p;
//printf("%lf %lf %lf\n", points[i].x, points[i].y, points[i].z );
++i;
}
}
// Insert points into blocks
for(i=0; i<blockSide; i++) {
for(j=0; j<blockSide; j++) {
for(k=0; k<blockSide; k++) {
data.n_points[i][j][k] = 0;
}}}
for( idx=0; idx<nAtoms; idx++ ) {
b.x = (int) (points[idx].x / blockEdge);
b.y = (int) (points[idx].y / blockEdge);
b.z = (int) (points[idx].z / blockEdge);
entry = data.n_points[b.x][b.y][b.z];
data.position[b.x][b.y][b.z][entry].x = points[idx].x;
data.position[b.x][b.y][b.z][entry].y = points[idx].y;
data.position[b.x][b.y][b.z][entry].z = points[idx].z;
++data.n_points[b.x][b.y][b.z];
assert(data.n_points[b.x][b.y][b.z]<densityFactor && "block overflow");
}
//for( b.x=0; b.x<blockSide; b.x++ ) {
//for( b.y=0; b.y<blockSide; b.y++ ) {
//for( b.z=0; b.z<blockSide; b.z++ ) {
// printf("grid[%d][%d][%d]: %d points\n", b.x, b.y, b.z, data.n_points[b.x][b.y][b.z]);
//}}}
// Fill remaining data structure
memset(data.d_force, 0, nBlocks*densityFactor*sizeof(dvector_t));
// Open and write
fd = open("input.data", O_WRONLY|O_CREAT|O_TRUNC, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH);
assert( fd>0 && "Couldn't open input data file" );
ptr = (char *) &data;
while( written<sizeof(data) ) {
status = write( fd, ptr, sizeof(data)-written );
assert( status>=0 && "Couldn't write input data file" );
written += status;
}
}
float tolerance_metric(NodeType const & left, NodeType const & right) const {
return dist_sq(left.projected_position_, right.projected_position_);
}
