void spacing_simulator_step(spacing_simulator *ss) {
linked_list* vertices = ss->vg->vertices;
linked_list* edges = ss->vg->edges;
for(node* i = vertices->first; i != NULL;i = i->next){
visual_vertex* v1 = (visual_vertex*)i->element;
v1->sim_data.speed = subtract_vector(make_vector_2d(400, 300), v1->position);
v1->sim_data.speed = multiply_vector(normalize(v1->sim_data.speed), ss->origin_point_force);
//debug_vector2d(v1->position, "POSITION");
//debug_vector2d(v1->sim_data.speed, "SPEED");
//debug("VVVVVVVVVV\n");
}
for(node* i = vertices->first; i != NULL;i = i->next){
visual_vertex* v1 = (visual_vertex*)i->element;
for(node* j = i->next;j != NULL;j = j->next){
visual_vertex* v2 = (visual_vertex*)j->element;
vector_2d speed1 = subtract_vector(v1->position, v2->position);
vector_2d speed2 = subtract_vector(v2->position, v1->position);
float distance = calculate_distance(v1->position, v2->position);
float force_factor = 100-distance;
force_factor = (force_factor > 0 ? force_factor : 0)*2;
speed1 = normalize(speed1);
speed2 = normalize(speed2);
speed1 = multiply_vector(speed1, force_factor);
speed2 = multiply_vector(speed2, force_factor);
v1->sim_data.speed = add_vector(v1->sim_data.speed, speed1);
v2->sim_data.speed = add_vector(v2->sim_data.speed, speed2);
//debug_vector2d(speed1, "SPEED1");
//debug_vector2d(speed2, "SPEED2");
}
}
for(node* i = edges->first;i != NULL;i = i->next){
visual_edge* e = (visual_edge*)i->element;
vector_2d pos1 = e->v1->position;
vector_2d pos2 = e->v2->position;
float distance = calculate_distance(pos1, pos2);
vector_2d direction1 = normalize(subtract_vector(pos2, pos1));
vector_2d direction2 = normalize(subtract_vector(pos1, pos2));
vector_2d speed1 = multiply_vector(direction1, distance*distance/500);
vector_2d speed2 = multiply_vector(direction2, distance*distance/500);
e->v1->sim_data.speed = add_vector(e->v1->sim_data.speed, speed1);
e->v2->sim_data.speed = add_vector(e->v2->sim_data.speed, speed2);
}
for(node* i = vertices->first; i != NULL;i = i->next){
visual_vertex* v1 = (visual_vertex*)i->element;
v1->sim_data.speed = multiply_vector(v1->sim_data.speed, ss->friction_factor);
v1->position = add_vector(v1->position, v1->sim_data.speed);
}
}
void *accept_connection(void *data)
{
char command;
struct accept_connection_data *d = (struct accept_connection_data*)data;
if(socket_read(d->fd, &command, 1) == 1)
{
switch(command)
{
case 'r':
register_vehicle(d->fd, d->vehicles);
break;
case 'd':
delete_vehicle(d->fd, d->vehicles);
break;
case 'h':
get_history(d->fd, d->vehicles);
break;
case 't':
calculate_distance(d->fd, d->vehicles);
break;
case 's':
check_status(d->fd, d->vehicles);
break;
}
}
TEMP_FAILURE_RETRY(close(d->fd));
stop_detached_thread(&d->mutex, d);
return NULL;
}
int main()
{
location start_location = get_location();
start_location.distance = 0;
unsigned int number = get_number_of_location();
location min, max; max = start_location;
for (unsigned j = 0; j < number; j++)
{
location destination = get_location();
destination.distance = calculate_distance(start_location, destination);
if (j==0) min = destination;
if (destination.distance < min.distance) min = destination;
if (destination.distance > max.distance) max = destination;
}
std::cout << "Start Location: " << print_location(start_location);
std::cout << "Closest Location: " << print_location(min);
std::cout << "Farthest Location: " << print_location(max);
return 0;
}
/**
* Calculate distance matrix using SIMPLE_HASH function.
* Complexity: O(n^2d) for n files and document length d
*/
void simple_hash(std::vector<document> &dbuffer, std::vector<std::vector<double>> &distance, parameters p) {
int n = distance.size();
for (int i = 0; i < n; i++) {
std::unique_ptr<SimpleHash> context(new SimpleHash(p.table_size));
context->build_substring_map(dbuffer[i].data(), dbuffer[i].size(), p.context_length);
for (int j = 0; j < n; j++) {
//don't bother comparing the file against itself - is always 0
if (i == j) {
distance[i][j] = 0.0;
continue;
}
int matches = context->count_matches(dbuffer[i].data(), dbuffer[i].size(),
dbuffer[j].data(), dbuffer[j].size(),
p.context_length);
distance[i][j] = calculate_distance(matches, p.context_length, dbuffer[j].size(),
p.exponent);
}
}
average_matrix(distance);
}
int main(void){
FILE *fp;
char *fname = "input_3.csv";//ファイルも書き換える
int i;
char xchar, ychar;
fp = fopen(fname, "r");
if(fp == NULL){
printf("%sファイルが開けません\n",fname);
return -1;
}
fscanf(fp, "%c,%c", &xchar, &ychar);
for(i=0; i<N; i++){
fscanf(fp, "%lf,%lf", &x[i], &y[i]);
printf("%lf , %lf\n", x[i], y[i]);
}
calculate_distance();
solve();
fclose(fp);
return 0;
}
void start_loop(t_player *pl)
{
int x;
ft_bzero(pl->str, WIDTH * 4 * HEIGHT);
x = -1;
while (++x < WIDTH)
{
// printf("%d ", x);
pl->x_camera = 2 * x / (double)WIDTH - 1;
pl->ray_x_dir = pl->x_dir + pl->x_plane * pl->x_camera;
pl->ray_y_dir = pl->y_dir + pl->y_plane * pl->x_camera;
pl->x_map = (int)pl->x_pos;
pl->y_map = (int)pl->y_pos;
pl->delta_x_dist = fabs(1 / pl->ray_x_dir);
pl->delta_y_dist = fabs(1 / pl->ray_y_dir);
calculate_step_and_initial_side(pl);
loop_while(pl);
calculate_distance(pl);
color_and_draw(x, pl);
}
pl->a = mlx_put_image_to_window(pl->mlx, pl->win, pl->img, 0, 0);
pl->old_time = pl->new_time;
// printf("%lu\n", pl->old_time);
pl->new_time = clock();
// printf("%lu\n", pl->new_time);
pl->frame_time = (double)(pl->old_time - pl->new_time) / 500000.0;
pl->move_speed = pl->frame_time * 0.0000000000001;
printf("%f\n", pl->move_speed);
pl->rot_speed = pl->frame_time * 3.0;
// printf("%lu\n", clock());
}
// -------------------------------------------------------------------------- //
//
void Test_Mathutils::testCalculateDistance2()
{
const double point1[3] = {1.0, 2.0, 3.0};
const double point2[3] = {-3.0, -2.0, -1.0};
const double distance = calculate_distance(point1, point2);
CPPUNIT_ASSERT_DOUBLES_EQUAL(distance, std::sqrt(3.0*16.0), EPS);
}
/*
* Draws the distances between face and the camera. Uses calculate_distance.
*/
void draw_distances(IplImage *img, CvSeq* faces)
{
/* Point for distance list */
CvPoint pt1;
distance_range range;
int i;
char buf[NUM_FACES_BUF];
memset( buf, 0, sizeof(buf) );
/* Distance for the face number */
pt1.x = DISTANCE_LIST_MARGIN;
pt1.y = 2*DISTANCE_LIST_MARGIN;
cvPutText( img, "Face number and its distance:", pt1, &font, CV_RGB(0,0,255) );
/* For every face draw */
for( i = 0; i < ( faces ? faces->total : 0 ); i++ )
{
CvRect *r = (CvRect*)cvGetSeqElem( faces, i );
/* Face number */
sprintf(buf,"%d",i);
pt1.y += ROW_VERTICAL;
cvPutText( img, buf, pt1, &font, CV_RGB(255, 0, 0) );
range = calculate_distance(r->width, r->height);
sprintf(buf,"~(%d : %d)", range.min, range.max);
pt1.x += COL_HORIZONTAL;
cvPutText( img, buf, pt1, &font, CV_RGB(255, 0, 0 ) );
pt1.x -= COL_HORIZONTAL;
}
}
inline void
CurveWarp::sync()
{
std::vector<synfig::BLinePoint> bline(param_bline.get_list().begin(),param_bline.get_list().end());
Point start_point=param_start_point.get(Point());
Point end_point=param_end_point.get(Point());
curve_length_=calculate_distance(bline);
perp_ = (end_point - start_point).perp().norm();
}
void init() {
for (int i = 2; i < 7; i++) {
for (int j = 2; j < 7; j++) {
if (i == j) {
distances[i-2][j-2] = 1000;
} else {
distances[i-2][j-2] = calculate_distance(location_data[i], location_data[j]);
}
}
}
}
void Timer0IntHandler(void) {
// Clear the timer interrupt.
TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
//update_array();
distance += calculate_distance();
acceleration = calculate_accleration();
buffed_speed_1_old = buffed_speed;
//IntMasterDisable();
//IntMasterEnable();
}
void AStarManhattanMove::add_to_queue(){
DEBUG_PRINT("Preparing for adding new child..\n");
string key = current_state_obj.get_id();
if (visit_map.count(key)) {
DEBUG_PRINT("Child exists! Abort adding\n");
return;
}
State new_state = current_state_obj;
new_state.increase_step();
calculate_distance(new_state);
state_priority_queue.push(new_state);
DEBUG_PRINT("New child added.\n");
}
int get_distances(CvSeq* faces, distance_range *distances)
{
int i;
int end;
end = faces ? faces->total : 0 ;
end = end > MAX_FACES ? MAX_FACES : end ;
/* For every face draw */
for( i = 0; i < end; i++ )
{
CvRect *r = (CvRect*)cvGetSeqElem( faces, i );
distances[i] = calculate_distance( r->width, r->height );
}
return end;
}
void graph_calculate_distances(graph_t *graph)
{
assert(graph != NULL);
assert(graph->nodes != NULL);
assert(graph->node_count > 0);
assert(graph->distances != NULL);
int i, j;
for (i = 0; i < graph->node_count; ++i) {
for (j = 0; j < graph->node_count; ++j) {
if (i == j) {
graph->distances[i][j] = INFINITY;
}
else {
graph->distances[i][j] = calculate_distance(&graph->nodes[i], &graph->nodes[j]);
}
}
}
}
// build graph
void buildGraph(FILE*in,Graph G){
int j, k;
double xy[MaxVertices][2]; // used to store the coordinates X and Y
for (j = 1; j <= G->numV; j++)
for (k = 1; k <= G->numV; k++)
{
if (j == 1)
{
G->vertex[k] = newGVertex(0);
G->vertex[k].id = k;
G->vertex[k].cost = Infinity;
G->vertex[k].parent = 0;
G->vertex[k].colour = White; // to Black when vertex is added to MST
fscanf(in, "%lf %lf", &xy[k][0], &xy[k][1]); // scanning the X and Y values to be used
}
if (j != k)
// cal the length of a line to be used as the weight
addEdge(j, k, calculate_distance(xy[j][0], xy[j][1], xy[k][0], xy[k][1]), G);
}
}
void fill_projected_point_array()
{
int i=0;
Point origin;
for(i=0;i<DIM;i++)
origin.coordinates[i]=0;
i=0;
while(i<size)
{
convert_to_uvw(i);
p_array[i].dist_projection_to_origine=calculate_distance(origin,p_array[i]);
// printf("\n( %.4f %.4f %.4f ) => ",arr[i].coordinates[0],arr[i].coordinates[1],arr[i].coordinates[2]);
// printf("projected: [%d]= ( %.4f %.4f %.4f ) dist: %.4f\n",i,arr[i].coordinates_projected[0],arr[i].coordinates_projected[1],arr[i].coordinates_projected[2],arr[i].dist_projection_to_origine);
i++;
}
}
vector_2d normalize(vector_2d a) {
float scale = 1/calculate_distance(make_vector_2d(0, 0), a);
vector_2d result = make_vector_2d(scale * a.x, scale * a.y);
return result;
}
int
main(void)
{
switch(fork()) {
case -1:
die("fork\n");
break;
case 0:
dev_id = hci_devid(DEV_ID);
if (dev_id < 0) {
die("Invalid device\n");
} else {
initsignals();
close(STDIN_FILENO);
close(STDOUT_FILENO);
close(STDERR_FILENO);
Display *dpy = NULL;
int rssi, screen;
Bool help = True;
check_version(dev_id);
add_to_white_list(dev_id);
handle = connect_to_device(dev_id);
/* encryption(dev_id, handle); */
while (running) {
if (help) {
if (!(dpy = XOpenDisplay(0)))
die("ble: cannot open display");
/* Get the number of screens in display "dpy" and blank them all. */
nscreens = ScreenCount(dpy);
locks = malloc(sizeof(Lock *) * nscreens);
if (locks == NULL)
die("ble: malloc: %s", strerror(errno));
}
if (help) {
rssi = read_rssi(dev_id, handle);
if ((calculate_distance(rssi) >= 2.0) && (rssi <= -71 && rssi >= -75)) {
if (locks != NULL && help) {
for (screen = 0; screen < nscreens; screen++)
locks[screen] = lockscreen(dpy, screen);
XSync(dpy, False);
}
help = False;
}
}
sleep(1);
if (!help) {
rssi = read_rssi(dev_id, handle);
if ((calculate_distance(rssi) <= 2.0) && (rssi <= -30 && rssi >= -70)) {
for (screen = 0; screen < nscreens; screen++)
unlockscreen(dpy, locks[screen]);
if (locks != NULL || dpy != NULL) {
free(locks);
XCloseDisplay(dpy);
help = True;
}
}
}
}
disconnect_from_device(dev_id, handle);
}
default:
break;
}
return EXIT_SUCCESS;
}
Result recursiveClosestPair(Point A_[], int _size)
{
Result r_;
Result result;
Result Left;
Result Right;
Result closest;
int N;
int ns_sequences={0};
double xm; int i=0;int j=0; int k=0;int ns=0; double _dist;int u=0;int m=0; int h=0; int dim=0;
int size_=0;
Point *Left_coordinates={0};
Point *Right_coordinates={0};
Point *S_coordinates={0};
r_.min_distance=(double)INT_MAX;
result.min_distance=(double)INT_MAX;
Left.min_distance=(double)INT_MAX;
Right.min_distance=(double)INT_MAX;
closest.min_distance=(double)INT_MAX;
N= _size;
// printf("N: %d\n",N);
if(N<=3)
{
result= bruteForceClosestPair(A_,N,1);
//counting++;
//if(closest.min_distance==0)
//printf ("counting : %d\n", counting);
//print_array_with_size(A_[0],N);
return result;
}
else
{
Right_coordinates= (Point *) malloc(sizeof(Point)* N);
Left_coordinates= (Point *) malloc(sizeof(Point)* N);
if(N%2==0)
{
size_=N/2;
array_copy(Left_coordinates, A_,size_,0);
array_copy(Right_coordinates, A_,N,size_);
}
else
{
size_=(N/2)+1;
array_copy(Left_coordinates, A_,size_,0);
array_copy(Right_coordinates, A_,N,size_-1);
}
//size_ = ceilf(N/2);
/*
counting++;
if(counting==73731)
scanf("%d",&a);
printf("size_: %d N= %d counting: %d\n",size_,N,counting);
*/
/*
printf("xL:\n");
print_array(Left_coordinates[0]);
printf("xR:\n");
print_array(Right_coordinates[0]);
*/
xm=A_[size_-1].coordinates[0];
for(i=0;i<N;i++)
{
if(A_[i].coordinates[0]<=xm)
{
Left_coordinates[u]=A_[i];
u++;
}
else
{
Right_coordinates[m]=A_[i];
m++;
}
}
/*
printf("yL:\n");
print_array(Left_coordinates[1]);
printf("yR:\n");
print_array(Right_coordinates[1]);
printf("zL:\n");
print_array(Left_coordinates[2]);
printf("zR:\n");
print_array(Right_coordinates[2]);
*/
Left=recursiveClosestPair(Left_coordinates,size_);
Right=recursiveClosestPair(Right_coordinates,size_);
result= Right;
if(Left.min_distance<Right.min_distance)
{
result= Left;
}
//free(S_coordinates[i]);
free(Right_coordinates);
free(Left_coordinates);
S_coordinates= (Point *) malloc(sizeof(Point)* N);
//Right_coordinates= (Point *) malloc(sizeof(Point)* N);
//Left_coordinates= (Point *) malloc(sizeof(Point)* N);
for(i=0;i<N;i++)
{
if(fabsf(xm-A_[i].coordinates[0])<result.min_distance)
{
S_coordinates[j]=A_[i];
j++;
}
}
ns_sequences=j;
j=0;
closest=result;
for(i=0;i<ns_sequences-1;i++)
{
k=i+1;
while(k<ns_sequences && fabsf(S_coordinates[k].coordinates[0]-S_coordinates[i].coordinates[0])<closest.min_distance)
{
//++;
_dist=calculate_distance(S_coordinates[k],S_coordinates[i]);
//if(_dist==0)
// printf("counting : %d\n",counting);
if(_dist<closest.min_distance && _dist!=0 )
{
r_.min_distance=_dist;
r_.p =S_coordinates[k];
r_.q= S_coordinates[i];
closest=r_;
}
k++;
}
}
//counting++;
//if(closest.min_distance==0)
//printf ("counting : %d\n", counting);
//free(Left_coordinates);
//free(Right_coordinates);
//free(S_coordinates);
free(S_coordinates);
//free(Right_coordinates);
//free(Left_coordinates);
return closest;
}
}
Result smartForce(Point *A_[],int flag)
{
double d_min=(double)INT_MAX; double d; double coordinates_min[DIM]; int n=size ,r,i;
Point closest1, closest2; Result result; int g=0; double total_coordinates_min=0;
for(r=1; r<=n-1; r++ )
{
total_coordinates_min=0;
for(g=0;g<DIM;g++)
coordinates_min[g]=(double)INT_MAX;
for(i=0;i<n-r;i++)
{
for(g=0;g<DIM;g++)
{
d=calculate_distance(A_[g][i+r],A_[g][i]);
if(d!=0 )
{
if(flag==1)
{
if((A_[g][i+r].coordinates[g]-A_[g][i].coordinates[g])<coordinates_min[g])
coordinates_min[g]=(A_[g][i+r].coordinates[g]-A_[g][i].coordinates[g]);
}
if(flag==3 )
{
if((A_[g][i+r].coordinates_projected[g]-A_[g][i].coordinates_projected[g])<coordinates_min[g])
coordinates_min[g]=(A_[g][i+r].coordinates_projected[g]-A_[g][i].coordinates_projected[g]);
}
if(d<d_min)
{
d_min=d;
closest1= A_[g][i+r];
closest2= A_[g][i];
}
}
else
continue;
}
}
for(g=0;g<DIM;g++)
total_coordinates_min= total_coordinates_min + coordinates_min[g] * coordinates_min[g];
total_coordinates_min=sqrtf(total_coordinates_min);
if( d_min <= total_coordinates_min )
break;
}
result.p=closest1;
result.q=closest2;
result.min_distance=d_min;
return result;
}
Result bruteForceClosestPair(Point p_array_[],int size_,int type)
{
double r;
Point p={0};
Point q={0};
Result result;
int n=0;
double Dist;
int i=0;int j=0;
r =(double) INT_MAX;
//result.min_distance=(double)INT_MAX;
if(size_>1)
{
for( i = 0; i <=size_-1; i++)
{
for( j = i+1; j <=size_-1; j++)
{
Dist= calculate_distance(p_array_[i],p_array_[j]);
if(Dist < r && Dist!=0)
{
r = Dist;
p= p_array_[i];
q = p_array_[j];
}
}
}
result.p=p;
result.q=q;
result.min_distance= r;
}
else
{
result.p=p;
result.q=q;
r =(double) INT_MAX;
result.min_distance=(double)INT_MAX;
}
return result;
}
inline void
CurveGradient::sync()
{
std::vector<synfig::BLinePoint> bline(param_bline.get_list().begin(), param_bline.get_list().end());
curve_length_=calculate_distance(bline, bline_loop);
}
// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData)
{
if (acommand.packet_counter != 4) {
acommand.packet_counter++;
return;
}
acommand.packet_counter = 0;
NatNetClient* pClient = (NatNetClient*) pUserData;
printf("Received frame %d\n", data->iFrame);
if(fp)
_WriteFrame(fp,data);
int i=0;
// same system latency test
float fThisTick = (float)GetTickCount();
float fDiff = fThisTick - data->fLatency;
double dDuration = fDiff;
printf("Latency (same system) (msecs): %3.2lf\n", dDuration);
// timecode
// decode to values
int hour, minute, second, frame, subframe;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
char szTimecode[128] = "";
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
printf("Timecode : %s\n", szTimecode);
// Other Markers
printf("Other Markers [Count=%d]\n", data->nOtherMarkers);
for(i=0; i < data->nOtherMarkers; i++)
{
printf("Other Marker %d : %3.2f\t%3.2f\t%3.2f\n",
i,
data->OtherMarkers[i][0],
data->OtherMarkers[i][1],
data->OtherMarkers[i][2]);
}
// Rigid Bodies
printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies);
double theta = calculate_angle(data->RigidBodies[0], data->RigidBodies[1], data->RigidBodies[2]);
double dist_calc = calculate_distance(data->RigidBodies[0], data->RigidBodies[3]);
printf("\n\n\nAngle is %Lf and distance is %Lf\n\n\n", theta*180/3.14, dist_calc);
for(i=0; i < data->nRigidBodies; i++)
{
//location_data[i] = data->RigidBodies[i];
printf("Rigid Body [ID=%d Error=%3.2f]\n", data->RigidBodies[i].ID, data->RigidBodies[i].MeanError);
printf("\tx\ty\tz\tqx\tqy\tqz\tqw\n");
printf("\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",
data->RigidBodies[i].x,
data->RigidBodies[i].y,
data->RigidBodies[i].z,
data->RigidBodies[i].qx,
data->RigidBodies[i].qy,
data->RigidBodies[i].qz,
data->RigidBodies[i].qw);
printf("\tRigid body markers [Count=%d]\n", data->RigidBodies[i].nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(data->RigidBodies[i].MarkerIDs)
printf("MarkerID:%d", data->RigidBodies[i].MarkerIDs[iMarker]);
if(data->RigidBodies[i].MarkerSizes)
printf("\tMarkerSize:%3.2f", data->RigidBodies[i].MarkerSizes[iMarker]);
if(data->RigidBodies[i].Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
// skeletons
printf("Skeletons [Count=%d]\n", data->nSkeletons);
for(i=0; i < data->nSkeletons; i++)
{
sSkeletonData skData = data->Skeletons[i];
printf("Skeleton [ID=%d Bone count=%d]\n", skData.skeletonID, skData.nRigidBodies);
for(int j=0; j< skData.nRigidBodies; j++)
{
sRigidBodyData rbData = skData.RigidBodyData[j];
printf("Bone %d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",
rbData.ID, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw );
printf("\tRigid body markers [Count=%d]\n", rbData.nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(rbData.MarkerIDs)
printf("MarkerID:%d", rbData.MarkerIDs[iMarker]);
if(rbData.MarkerSizes)
printf("\tMarkerSize:%3.2f", rbData.MarkerSizes[iMarker]);
if(rbData.Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
}
// labeled markers
printf("Labeled Markers [Count=%d]\n", data->nLabeledMarkers);
for(i=0; i < data->nLabeledMarkers; i++)
{
sMarker marker = data->LabeledMarkers[i];
printf("Labeled Marker [ID=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n", marker.ID, marker.size, marker.x, marker.y, marker.z);
}
if (!init_called) {
//init();
init_called = true;
}
EnterCriticalSection(&dest_cont.lock);
int destination = dest_cont.dests[dest_cont.step_num];
int stn = dest_cont.step_num;
int nsc = num_same_cmds;
int num_dests = dest_cont.num_dests;
LeaveCriticalSection(&dest_cont.lock);
if (destination >0) {
double angle_to_dest = calculate_angle(data->RigidBodies[0], data->RigidBodies[1], data->RigidBodies[destination]);
double dist_to_dest = calculate_distance(data->RigidBodies[0], data->RigidBodies[destination]);
printf("==================================== %f", dist_to_dest);
printf("angle is %f", angle_to_dest);
char return_val[1];
if (dist_to_dest < .4) {
if (acommand.state != '0' || last_dest != destination) {
acommand.cmd = '0';
send_cmd(NULL);
last_dest = destination;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
//state = '0';
EnterCriticalSection(&dest_cont.lock);
dest_cont.step_num += 1;
//dest_cont.num_dests -= 1;
LeaveCriticalSection(&dest_cont.lock);
num_same_cmds = 0;
} else {
num_same_cmds += 1;
if (num_same_cmds > 20) {
acommand.cmd = '0';
send_cmd(NULL);
num_same_cmds = 0;
}
}
//dest_cont.step_num += 1;
return;
}
if ( angle_to_dest <165 ) {
if (acommand.state != 'R') {
acommand.cmd = 'R';
send_cmd(NULL);
num_same_cmds = 0;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
} else {
num_same_cmds += 1;
if (num_same_cmds > 20) {
acommand.cmd = 'R';
send_cmd(NULL);
num_same_cmds = 0;
}
}
state = 'R';
return;
} else if (angle_to_dest > 170) {
if (acommand.state == 'R' || acommand.state == 'L') {
acommand.cmd = '0';
send_cmd(NULL);
num_same_cmds = 0;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
//state = '0';
} else {
if (acommand.state != 'D') {
acommand.cmd = 'D';
send_cmd(NULL);
num_same_cmds = 0;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
} else {
num_same_cmds += 1;
if (num_same_cmds > 20) {
acommand.cmd = 'D';
send_cmd(NULL);
num_same_cmds = 0;
}
}
//state = 'D';
}
}
}
}
void moveAutoPlayers()
{
int key;
for (int i=0;i<server_data.alien_count;i++)
{
if(server_data.aliens[i].isauto)
{
int nearerpos;
Alien a=server_data.aliens[i];
int pos;
int previ=a.i,prevj=a.j;
if(server_data.commando_count ==0) break;
int min=0;;
for(int pos=0;pos<server_data.commando_count;pos++)
{
nearerpos=0;
if(min>calculate_distance(a.i,a.j,server_data.commandos[pos].i,server_data.commandos[pos].j))
{min=calculate_distance(a.i,a.j,server_data.commandos[pos].i,server_data.commandos[pos].j); nearerpos=pos;}
}
if(timercount%5==0)
{
if(a.i > server_data.commandos[nearerpos].i) {key =0;}
else if(a.i < server_data.commandos[nearerpos].i) {key =1;}
else if(a.j < server_data.commandos[nearerpos].j) {key=3;
}
else if(a.j > server_data.commandos[nearerpos].j) {key=2;
}
switch (key)
{
case 0:
if(a.dir!=DOWN)a.dir=DOWN;
else if(a.i>0)
a.i--;
break;
case 1:
if(a.dir!=UP)a.dir=UP;
else if(a.i<(ROWS-1))
a.i++;
break;
case 2:
if(a.dir!=LEFT)a.dir=LEFT;
else if(a.j>0)
a.j--;
break;
case 3:
if(a.dir!=RIGHT)a.dir=RIGHT;
else if(a.j<(COLUMNS-1))
a.j++;
break;
}
if(!(isObstacle(a.i,a.j)))
{
if(checkAppleCollide(a.i,a.j)!=-1)
{
Bullet b;
b.i = previ;
b.j = prevj;
b.dir=a.dir;
b.pid=a.pid;
addBullet(b);
}
else if(checkBulletCollide(a.i,a.j)!=-1)
{
a.energy -=20;
int bulletpos =checkBulletCollide(a.i,a.j);
if(getCommandoIndex(server_data.bullets[bulletpos].pid)!=-1)
{
int commpos =getCommandoIndex(server_data.bullets[bulletpos].pid);
server_data.commandos[commpos].score +=100;
}
if(getAlienIndex(server_data.bullets[bulletpos].pid)!=-1)
{
int alienpos =getAlienIndex(server_data.bullets[bulletpos].pid);
server_data.aliens[alienpos].score -=100;
}
if(a.energy <=0)
{server_data.map_matrix[a.i][a.j]=NONE;}
}
else if(CheckPlayerCollide(a.i,a.j)!=-1 && CheckPlayerCollide(a.i,a.j) !=i)
{
}
else
{
if(checkCommandoCollide(a.i,a.j)==-1)
{
//server_data.map_matrix[previ][prevj]=NONE;
server_data.map_matrix[a.i][a.j]=BOOKED;
a.energy -=1;
server_data.aliens[i]=a;}
else
{Bullet b;
b.i = previ;
b.j = prevj;
b.dir=a.dir;
b.pid=a.pid;
addBullet(b);
}
}
}
else
{ Bullet b;
b.i = previ;
b.j = prevj;
b.dir=a.dir;
b.pid=a.pid;
addBullet(b); }
if(a.energy <= 0)
{deleteAlien(i);}
if(((key == 2 || key ==3 ) && (!aliensenserhorizontal(nearerpos,i)) )|| (prevj = server_data.commandos[nearerpos].j) && (!aliensenservertical(nearerpos,i)) ) {
Bullet b;
b.i = a.i;
b.j = a.j;
b.dir=a.dir;
b.pid=a.pid;
addBullet(b); }
}
}
}
for (int i=0;i<server_data.commando_count;i++)
{
if(server_data.commandos[i].isauto)
{
int nearerpos;
Commando a=server_data.commandos[i];
int pos;
int previ=a.i,prevj=a.j;
if(server_data.alien_count ==0) break;
int min=0;
for(int pos=0;pos<server_data.alien_count;pos++)
{
nearerpos=0;
if(min>calculate_distance(a.i,a.j,server_data.aliens[pos].i,server_data.aliens[pos].j))
{min=calculate_distance(a.i,a.j,server_data.aliens[pos].i,server_data.aliens[pos].j); nearerpos=pos;}
}
if(timercount%5==0)
{
if(a.i > server_data.aliens[nearerpos].i) {key =0;}
else if(a.i < server_data.aliens[nearerpos].i) {key =1;}
else if(a.j < server_data.aliens[nearerpos].j) {key=3;
}
else if(a.j > server_data.aliens[nearerpos].j) {key=2;
}
switch (key)
{
case 0:
if(a.dir!=DOWN)a.dir=DOWN;
else if(a.i>0)
a.i--;
break;
case 1:
if(a.dir!=UP)a.dir=UP;
else if(a.i<(ROWS-1))
a.i++;
break;
case 2:
if(a.dir!=LEFT)a.dir=LEFT;
else if(a.j>0)
a.j--;
break;
case 3:
if(a.dir!=RIGHT)a.dir=RIGHT;
else if(a.j<(COLUMNS-1))
a.j++;
break;
}
if(!(isObstacle(a.i,a.j)))
{
if(checkAppleCollide(a.i,a.j)!=-1)
{
server_data.map_matrix[server_data.commandos[i].i][server_data.commandos[i].j]=NONE;
int pos_app=checkAppleCollide(a.i,a.j);
destroyGenerateApple(pos_app);
if(server_data.commandos[i].energy <= 100 && server_data.commandos[i].energy >= 80) {server_data.commandos[i].energy =100;}
else if(server_data.commandos[i].energy < 80){
server_data.commandos[i].energy+=20;
}
server_data.commandos[i].score+=100;
server_data.commandos[i].i=a.i;
server_data.commandos[i].j=a.j;
server_data.map_matrix[a.i][a.j]=BOOKED;
server_data.commandos[i]=a;
}
else if(checkBulletCollide(a.i,a.j)!=-1)
{
a.energy -=25;
int bulletpos =checkBulletCollide(a.i,a.j);
if(getCommandoIndex(server_data.bullets[bulletpos].pid)!=-1)
{
int commpos =getCommandoIndex(server_data.bullets[bulletpos].pid);
server_data.commandos[commpos].score -=100;
}
if(getAlienIndex(server_data.bullets[bulletpos].pid)!=-1)
{
int alienpos =getAlienIndex(server_data.bullets[bulletpos].pid);
server_data.aliens[alienpos].score +=100;
}
if(a.energy <=0)
{
server_data.map_matrix[a.i][a.j]=NONE;}
}
else if(checkCommandoCollide(a.i,a.j)!=-1 && checkCommandoCollide(a.i,a.j) !=i)
{
}
else
{
if(CheckPlayerCollide(a.i,a.j)==-1)
{
// server_data.map_matrix[previ][prevj]=NONE;
server_data.map_matrix[a.i][a.j]=BOOKED;
server_data.commandos[i]=a;
}
else
{Bullet b;
b.i = previ;
b.j = prevj;
b.dir=a.dir;
b.pid=a.pid;
addBullet(b);
}
}
}
else{Bullet b;
b.i = previ;
b.j = prevj;
b.dir=a.dir;
b.pid=a.pid;
addBullet(b);
}
if(a.energy <= 0)
{deleteCommando(i);}
// printf("asfasfasdfas***** %d \n ",aliensenserhorizontal(nearerpos,i)));
if(((key == 2 || key ==3 ) && (!commandosenserhorizontal(nearerpos,i)) )|| (a.j == server_data.aliens[nearerpos].j) && (!commandosenservertical(nearerpos,i)) ) {
Bullet b;
b.i = a.i;
b.j = a.j;
b.dir=a.dir;
b.pid=a.pid;
addBullet(b);}
}
}
}
}
int main( int ac, char** av )
{
try
{
comma::command_line_options options( ac, av );
verbose = options.exists( "--verbose,-v" );
if( options.exists( "--help,-h" ) ) { usage( verbose ); }
csv = comma::csv::options( options );
csv.full_xpath = true;
ascii = comma::csv::ascii< Eigen::Vector3d >( "x,y,z", csv.delimiter );
const std::vector< std::string >& operations = options.unnamed( "--verbose,-v,--trace,--no-antialiasing,--next,--unit", "-.*" );
if( operations.size() != 1 ) { std::cerr << "points-calc: expected one operation, got " << operations.size() << ": " << comma::join( operations, ' ' ) << std::endl; return 1; }
const std::string& operation = operations[0];
if (vector_calc::has_operation(operation))
{
vector_calc::process(operation, options, csv);
return 0;
}
if( operation == "plane-intersection" )
{
plane_intersection::process(options, csv);
return 0;
}
if( operation == "distance" )
{
if( csv.has_field( "first" ) || csv.has_field( "second" )
|| csv.has_field( "first/x" ) || csv.has_field( "second/x" )
|| csv.has_field( "first/y" ) || csv.has_field( "second/y" )
|| csv.has_field( "first/z" ) || csv.has_field( "second/z" ) )
{
calculate_distance_for_pairs();
return 0;
}
if ( options.exists( "--next" ) ) { calculate_distance_next(); }
else { calculate_distance( false ); }
return 0;
}
if( operation == "cumulative-distance" )
{
calculate_distance( true );
return 0;
}
if( operation == "nearest" )
{
if( options.exists( "--point,--to" ) )
{
Eigen::Vector3d point = comma::csv::ascii< Eigen::Vector3d >().get( options.value< std::string >( "--point,--to" ) );
comma::csv::input_stream< Eigen::Vector3d > istream( std::cin, csv );
std::string record;
double min_distance = std::numeric_limits< double >::max();
while( istream.ready() || ( std::cin.good() && !std::cin.eof() ) )
{
const Eigen::Vector3d* p = istream.read();
if( !p ) { break; }
double d = ( *p - point ).norm();
if( d >= min_distance ) { continue; }
min_distance = d;
record = csv.binary() ? std::string( istream.binary().last(), csv.format().size() ) : comma::join( istream.ascii().last(), csv.delimiter );
}
if( !record.empty() ) { std::cout << record; }
if( csv.binary() ) { std::cout.write( reinterpret_cast< const char* >( &min_distance ), sizeof( double ) ); }
else { std::cout << csv.delimiter << min_distance << std::endl; }
return 0;
}
else
{
return 0;
}
}
if( operation == "thin" )
{
if( !options.exists( "--resolution" ) ) { std::cerr << "points-calc: --resolution is not specified " << std::endl; return 1; }
double resolution = options.value( "--resolution" , 0.0 );
thin( resolution );
return 0;
}
if( operation == "discretise" || operation == "discretize" )
{
if( !options.exists( "--step" ) ) { std::cerr << "points-calc: --step is not specified " << std::endl; return 1; }
double step = options.value( "--step" , 0.0 );
if( step <= 0 ) { std::cerr << "points-calc: expected positive step, got " << step << std::endl; return 1; }
// the last discretised point can be very close to the end of the interval, in which case the last two points can be identical in the output since ascii.put uses 12 digits by default
// setting --tolerance=1e-12 will not allow the last discretised point to be too close to the end of the interval and therefore the output will have two distinct points at the end
double tolerance = options.value( "--tolerance" , 0.0 );
if( tolerance < 0 ) { std::cerr << "points-calc: expected non-negative tolerance, got " << tolerance << std::endl; return 1; }
discretise( step, tolerance );
return 0;
}
if( operation == "local-max" || operation == "local-min" ) // todo: if( operation == "local-calc" ? )
{
double sign = operation == "local-max" ? 1 : -1;
if( csv.fields.empty() ) { csv.fields = "x,y,z,scalar"; }
csv.full_xpath = false;
bool has_id = csv.has_field( "id" );
comma::csv::input_stream< local_operation::point > istream( std::cin, csv );
std::deque< local_operation::record > records;
double radius = options.value< double >( "--radius" );
bool trace = options.exists( "--trace" );
Eigen::Vector3d resolution( radius, radius, radius );
snark::math::closed_interval< double, 3 > extents;
comma::uint32 id = 0;
if( verbose ) { std::cerr << "points-calc: reading input points..." << std::endl; }
while( istream.ready() || ( std::cin.good() && !std::cin.eof() ) )
{
const local_operation::point* p = istream.read();
if( !p ) { break; }
std::string line;
if( csv.binary() ) // quick and dirty
{
line.resize( csv.format().size() );
::memcpy( &line[0], istream.binary().last(), csv.format().size() );
}
else
{
line = comma::join( istream.ascii().last(), csv.delimiter );
}
local_operation::point q = *p;
if( !has_id ) { q.id = id++; }
records.push_back( local_operation::record( q, line ) );
records.back().reference_record = &records.back();
extents.set_hull( p->coordinates );
}
if( verbose ) { std::cerr << "points-calc: loading " << records.size() << " points into grid..." << std::endl; }
typedef std::vector< local_operation::record* > voxel_t; // todo: is vector a good container? use deque
typedef snark::voxel_map< voxel_t, 3 > grid_t;
grid_t grid( extents.min(), resolution );
for( std::size_t i = 0; i < records.size(); ++i ) { ( grid.touch_at( records[i].point.coordinates ) )->second.push_back( &records[i] ); }
if( verbose ) { std::cerr << "points-calc: searching for local extrema..." << std::endl; }
for( grid_t::iterator it = grid.begin(); it != grid.end(); ++it )
{
grid_t::index_type i;
for( i[0] = it->first[0] - 1; i[0] < it->first[0] + 2; ++i[0] )
{
for( i[1] = it->first[1] - 1; i[1] < it->first[1] + 2; ++i[1] )
{
for( i[2] = it->first[2] - 1; i[2] < it->first[2] + 2; ++i[2] )
{
grid_t::iterator git = grid.find( i );
if( git == grid.end() ) { continue; }
for( voxel_t::iterator vit = it->second.begin(); vit != it->second.end(); ++vit )
{
for( std::size_t k = 0; k < git->second.size() && ( *vit )->is_extremum; ++k )
{
local_operation::evaluate_local_extremum( *vit, git->second[k], radius, sign );
}
}
}
}
}
}
#ifdef WIN32
_setmode( _fileno( stdout ), _O_BINARY );
#endif
if( verbose ) { std::cerr << "points-calc: filling extrema grid..." << std::endl; }
grid_t extrema( extents.min(), resolution );
for( std::size_t i = 0; i < records.size(); ++i )
{
if( records[i].is_extremum )
{
( extrema.touch_at( records[i].point.coordinates ) )->second.push_back( &records[i] );
}
else
{
records[i].extremum_id = local_operation::record::invalid_id; // quick and dirty for now
// if( records[i].extremum_id == local_operation::record::invalid_id ) { continue; }
// while( records[i].reference_record->point.id != records[i].reference_record->reference_record->point.id )
// {
// records[i].reference_record = records[i].reference_record->reference_record;
// }
// records[i].extremum_id = records[i].reference_record->point.id;
}
}
if( verbose ) { std::cerr << "points-calc: calculating distances to " << extrema.size() << " local extrema..." << std::endl; }
for( grid_t::iterator it = grid.begin(); it != grid.end(); ++it )
{
grid_t::index_type i;
for( i[0] = it->first[0] - 1; i[0] < it->first[0] + 2; ++i[0] )
{
for( i[1] = it->first[1] - 1; i[1] < it->first[1] + 2; ++i[1] )
{
for( i[2] = it->first[2] - 1; i[2] < it->first[2] + 2; ++i[2] )
{
grid_t::iterator git = extrema.find( i );
if( git == extrema.end() ) { continue; }
for( std::size_t n = 0; n < it->second.size(); ++n )
{
for( std::size_t k = 0; k < git->second.size(); ++k )
{
local_operation::update_nearest_extremum( it->second[n], git->second[k], radius );
}
}
}
}
}
}
if( trace )
{
if( verbose ) { std::cerr << "points-calc: tracing extrema..." << std::endl; }
for( std::size_t i = 0; i < records.size(); ++i )
{
if( records[i].extremum_id == local_operation::record::invalid_id ) { continue; }
while( records[i].reference_record->point.id != records[i].reference_record->reference_record->point.id )
{
records[i].reference_record = records[i].reference_record->reference_record;
}
}
}
if( verbose ) { std::cerr << "points-calc: outputting..." << std::endl; }
std::string endl = csv.binary() ? "" : "\n";
std::string delimiter = csv.binary() ? "" : std::string( 1, csv.delimiter );
comma::csv::options output_csv;
if( csv.binary() ) { output_csv.format( "ui,d" ); }
comma::csv::output_stream< local_operation::output > ostream( std::cout, output_csv );
for( std::size_t i = 0; i < records.size(); ++i )
{
std::cout.write( &records[i].line[0], records[i].line.size() );
std::cout.write( &delimiter[0], delimiter.size() );
ostream.write( records[i].output( false ) ); // quick and dirty
}
if( verbose ) { std::cerr << "points-calc: done!" << std::endl; }
return 0;
}
if( operation == "nearest-max" || operation == "nearest-min" || operation == "nearest-any" )
{
double sign = operation == "nearest-max" ? 1 : -1;
bool any = operation == "nearest-any";
if( csv.fields.empty() ) { csv.fields = "x,y,z,scalar"; }
csv.full_xpath = false;
bool has_id = csv.has_field( "id" );
comma::csv::input_stream< local_operation::point > istream( std::cin, csv );
std::deque< local_operation::record > records;
double radius = options.value< double >( "--radius" );
Eigen::Vector3d resolution( radius, radius, radius );
snark::math::closed_interval< double, 3 > extents;
comma::uint32 id = 0;
if( verbose ) { std::cerr << "points-calc: reading input points..." << std::endl; }
while( istream.ready() || ( std::cin.good() && !std::cin.eof() ) )
{
const local_operation::point* p = istream.read();
if( !p ) { break; }
std::string line;
if( csv.binary() ) // quick and dirty
{
line.resize( csv.format().size() );
::memcpy( &line[0], istream.binary().last(), csv.format().size() );
}
else
{
line = comma::join( istream.ascii().last(), csv.delimiter );
}
local_operation::point q = *p;
if( !has_id ) { q.id = id++; }
records.push_back( local_operation::record( q, line ) );
records.back().reference_record = &records.back();
extents.set_hull( p->coordinates );
}
if( verbose ) { std::cerr << "points-calc: loading " << records.size() << " points into grid..." << std::endl; }
typedef std::vector< local_operation::record* > voxel_t; // todo: is vector a good container? use deque
typedef snark::voxel_map< voxel_t, 3 > grid_t;
grid_t grid( extents.min(), resolution );
for( std::size_t i = 0; i < records.size(); ++i ) { ( grid.touch_at( records[i].point.coordinates ) )->second.push_back( &records[i] ); }
if( verbose ) { std::cerr << "points-calc: searching for " << operation << "..." << std::endl; }
for( grid_t::iterator it = grid.begin(); it != grid.end(); ++it )
{
grid_t::index_type i;
for( i[0] = it->first[0] - 1; i[0] < it->first[0] + 2; ++i[0] )
{
for( i[1] = it->first[1] - 1; i[1] < it->first[1] + 2; ++i[1] )
{
for( i[2] = it->first[2] - 1; i[2] < it->first[2] + 2; ++i[2] )
{
grid_t::iterator git = grid.find( i );
if( git == grid.end() ) { continue; }
for( std::size_t n = 0; n < it->second.size(); ++n )
{
for( std::size_t k = 0; k < git->second.size(); ++k )
{
local_operation::update_nearest( it->second[n], git->second[k], radius, sign, any );
}
}
}
}
}
}
#ifdef WIN32
_setmode( _fileno( stdout ), _O_BINARY );
#endif
if( verbose ) { std::cerr << "points-calc: outputting..." << std::endl; }
std::string endl = csv.binary() ? "" : "\n";
std::string delimiter = csv.binary() ? "" : std::string( 1, csv.delimiter );
comma::csv::options output_csv;
if( csv.binary() ) { output_csv.format( "ui,d" ); }
comma::csv::output_stream< local_operation::output > ostream( std::cout, output_csv );
for( std::size_t i = 0; i < records.size(); ++i )
{
std::cout.write( &records[i].line[0], records[i].line.size() );
std::cout.write( &delimiter[0], delimiter.size() );
ostream.write( records[i].output() );
}
if( verbose ) { std::cerr << "points-calc: done!" << std::endl; }
return 0;
}
if( operation == "find-outliers" )
{
unsigned int size = options.value< unsigned int >( "--min-number-of-points-per-voxel,--size" );
double r = options.value< double >( "--resolution" );
Eigen::Vector3d resolution( r, r, r );
bool no_antialiasing = options.exists( "--no-antialiasing" );
comma::csv::input_stream< Eigen::Vector3d > istream( std::cin, csv );
std::deque< remove_outliers::record > records;
snark::math::closed_interval< double, 3 > extents;
if( verbose ) { std::cerr << "points-calc: reading input points..." << std::endl; }
while( istream.ready() || ( std::cin.good() && !std::cin.eof() ) )
{
const Eigen::Vector3d* p = istream.read();
if( !p ) { break; }
std::string line;
if( csv.binary() ) // quick and dirty
{
line.resize( csv.format().size() );
::memcpy( &line[0], istream.binary().last(), csv.format().size() );
}
else
{
line = comma::join( istream.ascii().last(), csv.delimiter );
}
records.push_back( remove_outliers::record( *p, line ) );
extents.set_hull( *p );
}
if( verbose ) { std::cerr << "points-calc: loading " << records.size() << " points into grid..." << std::endl; }
typedef std::vector< remove_outliers::record* > voxel_t; // todo: is vector a good container? use deque
typedef snark::voxel_map< voxel_t, 3 > grid_t;
grid_t grid( extents.min(), resolution );
for( std::size_t i = 0; i < records.size(); ++i ) { ( grid.touch_at( records[i].point ) )->second.push_back( &records[i] ); }
if( verbose ) { std::cerr << "points-calc: removing outliers..." << std::endl; }
for( grid_t::iterator it = grid.begin(); it != grid.end(); ++it )
{
bool rejected = true;
if( no_antialiasing )
{
rejected = it->second.size() < size;
}
else
{
grid_t::index_type i;
for( i[0] = it->first[0] - 1; i[0] < it->first[0] + 2 && rejected; ++i[0] )
{
for( i[1] = it->first[1] - 1; i[1] < it->first[1] + 2 && rejected; ++i[1] )
{
for( i[2] = it->first[2] - 1; i[2] < it->first[2] + 2 && rejected; ++i[2] )
{
grid_t::iterator git = grid.find( i );
rejected = git == grid.end() || git->second.size() < size;
}
}
}
}
if( rejected ) { for( std::size_t i = 0; i < it->second.size(); ++i ) { it->second[i]->rejected = true; } }
}
#ifdef WIN32
_setmode( _fileno( stdout ), _O_BINARY );
#endif
if( verbose ) { std::cerr << "points-calc: outputting..." << std::endl; }
std::string endl = csv.binary() ? "" : "\n";
std::string delimiter = csv.binary() ? "" : std::string( 1, csv.delimiter );
for( std::size_t i = 0; i < records.size(); ++i )
{
char valid = records[i].rejected ? 0 : 1;
std::cout.write( &records[i].line[0], records[i].line.size() );
std::cout.write( &delimiter[0], delimiter.size() );
std::cout.write( &valid, 1 );
std::cout.write( &endl[0], endl.size() );
}
if( verbose ) { std::cerr << "points-calc: done!" << std::endl; }
return 0;
}
std::cerr << "points-calc: please specify an operation" << std::endl;
return 1;
}
catch( std::exception& ex )
{
std::cerr << "points-calc: " << ex.what() << std::endl;
}
catch( ... )
{
std::cerr << "points-calc: unknown exception" << std::endl;
}
return 1;
}
