Show match.c syntax highlighted
/*
*
* Copyright (c) 2005 The Regents of the University of California. All
* rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Neither the name of the University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#include <stdio.h>
#include <math.h>
#include <libmisc/misc.h>
struct node {
float x[3];
float mx[3];
int new;
int curr;
int id;
};
#define NODE_COUNT 20
struct node nodes[NODE_COUNT];
int lookup(int id)
{
int i;
int h = -1;
for (i=0; i<NODE_COUNT; i++) {
if (nodes[i].id == id) return i;
if (h < 0 && nodes[i].id == 0) h = i;
}
if (h>=0) nodes[h].id = id;
return h;
}
void print()
{
int i;
for (i=0; i<NODE_COUNT; i++) {
if (nodes[i].id && nodes[i].curr) {
printf("%d\t%.3f\t%.3f\t%.3f 0 \n",
nodes[i].id, nodes[i].x[0], nodes[i].x[1], nodes[i].x[2]);
}
}
}
int main(int argc, char **argv)
{
char input[8192];
memset(nodes, 0, sizeof(nodes));
int last_job = -1;
int done = 0;
int i,j;
fprintf(stderr,
"USAGE: job file format is \n"
" <job number> <node id> <y(m)> <x(m)> <z(m)>\n"
"the first job must be job 0, and usually this should be your \n"
"tiedown points to get the right rotation and translation.\n");
while (1) {
int job;
int id;
float x[3];
if (fgets(input, sizeof(input), stdin) == NULL) {
done = 1;
goto process;
}
if (input[0] == '#') continue;
/* NOTE: survey tools report Y,X,Z */
int status = sscanf(input, "%d %d %f %f %f",
&job, &id, &x[1], &x[0], &x[2]);
if (status != 5 || id <= 0) {
fprintf(stderr, "status was %d, id was %d..\n", status, id);
continue;
}
fprintf(stderr, "adding node %d, %f,%f,%f..\n", id, x[1],x[0],x[2]);
if (last_job >= 0 && last_job != job) {
process:
#if 0
print();
printf("#processing job %d\n", last_job);
#endif
/* process.. */
if (last_job == 0) {
for (i=0; i<NODE_COUNT; i++) {
if (nodes[i].id) {
for (j=0; j<3; j++)
nodes[i].x[j] = nodes[i].mx[j];
nodes[i].new = 0;
nodes[i].curr = 1;
}
}
}
else {
/* compute centroid */
float c[3] = {0,0,0};
float cm[3] = {0,0,0};
float count = 0;
for (i=0; i<NODE_COUNT; i++) {
if (nodes[i].id && nodes[i].new && nodes[i].curr) {
for (j=0; j<3; j++) {
c[j] += nodes[i].x[j];
cm[j] += nodes[i].mx[j];
}
count++;
}
}
if (count > 0) {
for (j=0; j<3; j++) {
c[j] /= count;
cm[j] /= count;
}
/* determine avg angle */
float a[2] = {0,0};
for (i=0; i<NODE_COUNT; i++)
if (nodes[i].id && nodes[i].new)
for (j=0; j<3; j++)
nodes[i].mx[j] -= cm[j];
#if 0
printf("#new data... \n");
for (i=0; i<NODE_COUNT; i++) {
if (nodes[i].id && nodes[i].new) {
printf("%d\t%.3f\t%.3f\t%.3f\t0\t0\t0\t%.3f\t%.3f\t%.3f\n",
nodes[i].id,
nodes[i].x[0]*100, nodes[i].x[1]*100, nodes[i].x[2]*100,
nodes[i].mx[0]*100, nodes[i].mx[1]*100, nodes[i].mx[2]*100);
}
}
#endif
for (i=0; i<NODE_COUNT; i++) {
if (nodes[i].id && nodes[i].new && nodes[i].curr) {
float D1 = sqrt(sqrf(nodes[i].x[0] - c[0]) +
sqrf(nodes[i].x[1] - c[1]) +
sqrf(nodes[i].x[2] - c[2]));
float dt =
atan2(nodes[i].mx[1], nodes[i].mx[0]) -
atan2(nodes[i].x[1] - c[1], nodes[i].x[0] - c[0]);
a[0] += D1*cos(dt);
a[1] += D1*sin(dt);
}
}
float dt = -atan2(a[1], a[0]);
printf("#c=%f,%f,%f, rot=%f... \n",
c[0], c[1], c[2], dt*180.0/M_PI);
/* rotate */
for (i=0; i<NODE_COUNT; i++) {
if (nodes[i].id && nodes[i].new) {
nodes[i].x[0] += (cos(dt)*nodes[i].mx[0] - sin(dt)*nodes[i].mx[1]) + c[0];
nodes[i].x[1] += (sin(dt)*nodes[i].mx[0] + cos(dt)*nodes[i].mx[1]) + c[1];
nodes[i].x[2] += nodes[i].mx[2] + c[2];
if (nodes[i].curr)
for (j=0; j<3; j++)
nodes[i].x[j] /= 2.0;
nodes[i].curr = 1;
nodes[i].new = 0;
}
}
}
}
if (done) goto output;
}
last_job = job;
int index = lookup(id);
for (i=0; i<3; i++)
nodes[index].mx[i] = x[i];
nodes[index].new = 1;
}
output:
print();
return 0;
}
See more files for this project here