void normalize( Vector3 &n ){
float d;
d = vector_distance( n );
n.x = n.x/d;
n.y = n.y/d;
n.z = n.z/d;
}
static void make_propagated_sdiff_(void *context, u32 n,
const void *a, const void *b)
{
const navigation_measurement_t *m_a = (const navigation_measurement_t *)a;
const navigation_measurement_t *m_b = (const navigation_measurement_t *)b;
make_propagated_sdiff_ctxt *ctxt = (make_propagated_sdiff_ctxt *)context;
/* Construct sds[n].
* NOTE: The resulting sds have sat_pos and sat_vel taken from b. */
single_diff_(ctxt->sds, n, a, b);
double old_dist = vector_distance(3, m_a->sat_pos, ctxt->remote_pos_ecef);
double new_dist = vector_distance(3, m_b->sat_pos, ctxt->remote_pos_ecef);
double dr = new_dist - old_dist;
ctxt->sds[n].pseudorange += dr;
ctxt->sds[n].carrier_phase -= dr / GPS_L1_LAMBDA_NO_VAC;
}
/*
* Populates clusters.
*/
static void populate(void)
{
int i, j; /* Loop indexes. */
float tmp; /* Auxiliary variable. */
float distance; /* Smallest distance. */
start = timer_get();
memset(&too_far[rank*NUM_THREADS], 0, NUM_THREADS*sizeof(int));
/* Iterate over data points. */
#pragma omp parallel for schedule(static) default(shared) private(i, j, tmp, distance)
for (i = 0; i < lnpoints; i++)
{
distance = vector_distance(CENTROID(map[i]), POINT(i));
/* Look for closest cluster. */
for (j = 0; j < ncentroids; j++)
{
/* Point is in this cluster. */
if (j == map[i])
continue;
tmp = vector_distance(CENTROID(j), POINT(i));
/* Found. */
if (tmp < distance)
{
map[i] = j;
distance = tmp;
}
}
/* Cluster is too far away. */
if (distance > mindistance)
too_far[rank*NUM_THREADS + omp_get_thread_num()] = 1;
}
end = timer_get();
total += timer_diff(start, end);
}
float vector_distanceRayToPoint(vector3D_t* origin, vector3D_t* direction, vector3D_t* point)
{
//vector3D_t c = Point - a; // Vector from a to Point
vector3D_t c;
c.x = point->x - origin->x;
c.y = point->y - origin->y;
c.z = point->z - origin->z;
//vector3D_t v = (b - a).Normalize(); // Unit Vector from a to b
//float d = (b - a).Length(); // Length of the line segment
float t = vector_dot(direction, &c);//v.DotProduct©; // Intersection point Distance from a
vector3D_t v;
v.x = origin->x + t * direction->x;
v.y = origin->y + t * direction->y;
v.z = origin->z + t * direction->z;
return vector_distance(point, &v);
}
static void measure_b(u8 state_dim, const double *state_mean,
u8 num_sdiffs, sdiff_t *sdiffs_with_ref_first,
const double receiver_ecef[3], double *b)
{
double dd_measurements[2*(num_sdiffs-1)];
make_measurements(num_sdiffs - 1, sdiffs_with_ref_first, dd_measurements);
double b_old[3] = {0, 0, 0};
double ref_ecef[3];
ref_ecef[0] = receiver_ecef[0];
ref_ecef[1] = receiver_ecef[1];
ref_ecef[2] = receiver_ecef[2];
least_squares_solve_b_external_ambs(state_dim, state_mean,
sdiffs_with_ref_first, dd_measurements, ref_ecef, b, false, DEFAULT_RAIM_THRESHOLD);
while (vector_distance(3, b_old, b) > 1e-4) {
memcpy(b_old, b, sizeof(double)*3);
ref_ecef[0] = receiver_ecef[0] + 0.5 * b_old[0];
ref_ecef[1] = receiver_ecef[1] + 0.5 * b_old[1];
ref_ecef[2] = receiver_ecef[2] + 0.5 * b_old[2];
least_squares_solve_b_external_ambs(state_dim, state_mean,
sdiffs_with_ref_first, dd_measurements, ref_ecef, b, false, DEFAULT_RAIM_THRESHOLD);
}
}
int
CVDB::query(int tableid, int imageid, std::pair<int, int> range, std::ostream& outs)
{
profiler.start(); // EVENT_TOTAL
// load table
SDBConnectionPtr sdbconn = sdbconnect();
S3ConnectionPtr s3conn = s3connect();
ImageTableMetadata *tablemeta = new ImageTableMetadata(tableid);
load_image_table_meta(sdbconn, tablemeta);
// load query image
int vector_size = sizeof(float)*tablemeta->eigenspace->dimension/1000;
ImageMetadata query_meta(imageid);
query_meta.imagetable = tablemeta;
profiler.start();
load_image_meta(sdbconn, &query_meta);
profiler.stop(EVENT_SDB_GET);
char buf[32];
sprintf(buf, "%d", vector_size);
std::string val;
val.assign(EVENT_S3_GET);
val += Profiler::DELIM;
val += buf;
profiler.start();
load_image_eigen(s3conn, &query_meta);
profiler.stop(val);
double min_dist = 0;
double min_id = 0;
// to conserve memory, process one image at a time
for (int i=range.first; i<=range.second; ++i) {
// load image metadata
ImageMetadata meta(i);
meta.imagetable = tablemeta;
profiler.start();
load_image_meta(sdbconn, &meta);
profiler.stop(EVENT_SDB_GET);
// load vector
profiler.start();
load_image_eigen(s3conn, &meta);
profiler.stop(val);
double dist = vector_distance(tablemeta->eigenspace->dimension,
query_meta.features, meta.features);
if (i==range.first || dist < min_dist) {
min_dist = dist;
min_id = i;
}
}
// output
sprintf(buf, "%.6lf", min_dist);
outs << "{";
outs << imageid;
outs << " : ";
outs << "[ [";
outs << min_id;
outs << ", ";
outs << buf;
outs << "] ]";
outs << "}";
outs << std::endl;
// clean up
delete tablemeta;
profiler.stop(EVENT_TOTAL);
return EXIT_SUCCESS;
}
void reduce_group_vertices(int g)
{
int *rtab;
int g_size;
Bool changed=False;
g_size=group_size(g,group, vertex_used);
if(g_size==0)
{
printf("current group contains 0 vertices !!!\n");
return;
}
rtab= (int*) malloc(2*g_size*sizeof(int));
{
int i,j;
j=0;
for(i=0; i<VERTEX_MAX && j<g_size; i++)
if(vertex_used[i] && group[i]==g)
{
*(rtab+2*j)=i;
*(rtab+2*j+1)=j;
j++;
}
for(i=1; i<g_size; i++)
for(j=0; j<i && *(rtab+2*i+1)== i; j++)
if(vector_distance(vertex[*(rtab+2*i)],vertex[*(rtab+2*j)])<=
reduction_epsilon)
{
changed=True;
while(*(rtab+2*j+1)!=j) j=*(rtab+2*j+1);
*(rtab+2*i+1)=j;
}
}
if(!changed)
{
printf("No vertex reductions can be done.\n");
free(rtab);
return;
}
group_statistics(g);
backup();
printf("REDUCING ...\n");
{
int i, old_top, e[3], t[4];
graph_marked_init();
for(i=0; i<g_size; i++)
graph_marked[*(rtab+2*i)]= *(rtab+2*(*(rtab+2*i+1)));
/* if graph_marked[i]!=-1 thten group[i]== g and i should be replaced */
/* by graph_marked[i] */
/* REDUCE EDGES */
old_top=edge_top;
i=0;
while(i<edge_top)
if(graph_marked[edge[i][0]]!= -1||
graph_marked[edge[i][1]]!= -1
)
{
edgecpy(e, edge[i]);
edge_delete(i);
e[0]=graph_marked[e[0]];
e[1]=graph_marked[e[1]];
edge_sort(e);
edgecpy(edge[edge_top],e);
}
else i++;
qsort(edge+edge_top, old_top-edge_top, sizeof(int[3]), edge_compare);
i=edge_top;
while(i<old_top)
{
if(i==0 || !edge_eq(edge[i-1],edge[i]))
{
edgecpy(edge[edge_top],edge[i]);
vertex_used[edge[edge_top][0]]++;
vertex_used[edge[edge_top][1]]++;
edge_top++;
}
i++;
}
/* REDUCE TRIANGLES */
old_top=triangle_top;
i=0;
while(i<triangle_top)
if(graph_marked[triangle[i][0]]!= -1||
graph_marked[triangle[i][1]]!= -1||
graph_marked[triangle[i][2]]!= -1
)
{
trianglecpy(t, triangle[i]);
triangle_delete(i);
t[0]=graph_marked[t[0]];
t[1]=graph_marked[t[1]];
t[2]=graph_marked[t[2]];
triangle_sort(t);
trianglecpy(triangle[triangle_top],t);
}
else i++;
qsort(triangle+triangle_top, old_top-triangle_top, sizeof(int[4]),
triangle_compare);
i=triangle_top;
while(i<old_top)
{
if(i==0 || !triangle_eq(triangle[i-1],triangle[i]))
{
trianglecpy(triangle[triangle_top],triangle[i]);
vertex_used[triangle[triangle_top][0]]++;
vertex_used[triangle[triangle_top][1]]++;
vertex_used[triangle[triangle_top][2]]++;
triangle_top++;
}
i++;
}
/* DOKONCZ ... dla triangles */
}
free(rtab);
group_statistics(g);
}
int main(void) {
ALmatrix *m1 = _alMatrixAlloc(3, 3);
ALmatrix *m2 = _alMatrixAlloc(3, 3);
ALmatrix *m3 = _alMatrixAlloc(3, 3);
ALfloat axis[3] = { 0.0, 0.0, 1.0 };
ALfloat point[3] = { 15.0, 0.0, 0.0 };
ALfloat point1[3] = { 15.0, 0.0, 0.0 };
ALfloat point2[3] = { 15.0, 0.0, 0.0 };
ALfloat origin[3] = { 0.0, 0.0, 0.0 };
ALfloat vab;
ALfloat xaxis[3] = { 1.0, 0.0, 0.0 };
ALfloat yaxis[3] = { 0.0, 1.0, 0.0 };
ALfloat zaxis[3] = { 0.0, 0.0, 1.0 };
ALfloat mxaxis[3] = { -1.0, 0.0, 0.0 };
ALfloat myaxis[3] = { 0.0, -1.0, 0.0 };
ALfloat mzaxis[3] = { 0.0, 0.0, -1.0 };
int i;
int j;
ALfloat vec1[3], vec2[3], d[3];
for(i = 0; i < 3; i++) {
for(j = 0; j < 3; j++) {
m3->data[i][j] = 0.0;
if(i == j) {
m1->data[i][j] = 3.0;
m2->data[i][j] = 1.0;
} else {
m1->data[i][j] = 2.0;
m2->data[i][j] = 0.0;
}
}
}
#if 0
fprintf(stderr, "m1:\n[%f][%f][%f]\n[%f][%f][%f]\n[%f][%f][%f]\n\n",
m1->data[0][0], m1->data[0][1], m1->data[0][2],
m1->data[1][0], m1->data[1][1], m1->data[1][2],
m1->data[2][0], m1->data[2][1], m1->data[2][2]);
fprintf(stderr, "m2:\n[%f][%f][%f]\n[%f][%f][%f]\n[%f][%f][%f]\n\n",
m2->data[0][0], m2->data[0][1], m2->data[0][2],
m2->data[1][0], m2->data[1][1], m2->data[1][2],
m2->data[2][0], m2->data[2][1], m2->data[2][2]);
_alMatrixMul(m3, m2, m1);
fprintf(stderr, "[%f][%f][%f]\n[%f][%f][%f]\n[%f][%f][%f]\n",
m3->data[0][0], m3->data[0][1], m3->data[0][2],
m3->data[1][0], m3->data[1][1], m3->data[1][2],
m3->data[2][0], m3->data[2][1], m3->data[2][2]);
rotate_point_about_axis(1.00, point, axis);
fprintf(stderr, "point [%f][%f][%f]\n",
point[0], point[1], point[2]);
vab = vector_angle_between(origin, origin, origin);
fprintf(stderr, "origin should be 0.0, is %f\n", vab);
vab = vector_angle_between(origin, xaxis, yaxis);
fprintf(stderr, "xaxis/yaxis: should be %f, is %f\n", M_PI_2, vab);
vab = vector_angle_between(origin, xaxis, zaxis);
fprintf(stderr, "xaxis/zaxis: should be %f, is %f\n", M_PI_2, vab);
vab = vector_angle_between(origin, origin, point);
fprintf(stderr, "origin/point: should be %f, is %f\n", 0.0, vab);
vab = vector_angle_between(origin, point1, point2);
fprintf(stderr, "point1/point2: should be %f, is %f\n", 0.0, vab);
for(i = 0; i < 32; i++) {
if(ISPOWEROFTWO(i) == AL_TRUE) {
fprintf(stderr, "ISPOWEROFTWO %d = AL_TRUE\n", i);
}
}
for(i = 0; i < 32; i++) {
fprintf(stderr,
"nextPowerOfTwo(%d) = %d\n",
i, nextPowerOfTwo(i));
}
/* vector distance */
vec1[0] = -20.0;
vec1[1] = 0.0;
vec1[2] = 0.0;
vec2[0] = -22.0;
vec2[1] = 0.0;
vec2[2] = 0.0;
vector_distance(vec1, vec2, d);
fprintf(stderr, "\n\t %f %f %f \n\t+ (%f %f %f)\n\t= (%f %f %f)\n",
vec1[0], vec1[1], vec1[2],
vec2[0], vec2[1], vec2[2],
d[0], d[1], d[2]);
/* vector magnitude */
vec1[0] = -31.0;
vec1[1] = 0.0;
vec1[2] = 0.0;
vec2[0] = 30.0;
vec2[1] = 0.0;
vec2[2] = 0.0;
fprintf(stderr, "\n\t %f %f %f \n\t~~ (%f %f %f)\n\t= %f\n",
vec1[0], vec1[1], vec1[2],
vec2[0], vec2[1], vec2[2],
vector_magnitude(vec1, vec2));
/* vector magnitude again */
vec1[0] = 5.0;
vec1[1] = 0.0;
vec1[2] = 0.0;
vec2[0] = 0.0;
vec2[1] = 5.0;
vec2[2] = 0.0;
fprintf(stderr, "\n\t %f %f %f \n\t~~ (%f %f %f)\n\t= %f\n",
vec1[0], vec1[1], vec1[2],
vec2[0], vec2[1], vec2[2],
vector_magnitude(vec1, vec2));
/* vector intersect angle */
vec1[0] = 0.0; point1[0] = 4.0;
vec1[1] = 0.0; point1[1] = 0.0;
vec1[2] = 0.0; point1[2] = 0.0;
vec2[0] = 0.0; point2[0] = 0.0;
vec2[1] = 0.0; point2[1] = 4.0;
vec2[2] = 0.0; point2[2] = 0.0;
fprintf(stderr, "\n\t ---- VECTOR ANGLE INTERSECT PERPENDICULAR -----\n"
"\t1: (%f %f %f) -> (%f %f %f)\n"
"\t2: (%f %f %f) -> (%f %f %f)\n"
"\t=== %f\n",
vec1[0], vec1[1], vec1[2],
point1[0], point1[1], point1[2],
vec2[0], vec2[1], vec2[2],
point2[0], point2[1], point2[2],
vector_intersect_angle(vec1, point1, vec2, point2));
/* vector intersect angle */
vec1[0] = 0.0; point1[0] = 4.0;
vec1[1] = 0.0; point1[1] = 0.0;
vec1[2] = 0.0; point1[2] = 0.0;
vec2[0] = 2.0; point2[0] = 6.0;
vec2[1] = 0.0; point2[1] = 0.0;
vec2[2] = 0.0; point2[2] = 0.0;
fprintf(stderr, "\n\t ---- VECTOR ANGLE INTERSECT PARALLEL -----\n"
"\t1: (%f %f %f) -> (%f %f %f)\n"
"\t2: (%f %f %f) -> (%f %f %f)\n"
"\t=== %f\n",
vec1[0], vec1[1], vec1[2],
point1[0], point1[1], point1[2],
vec2[0], vec2[1], vec2[2],
point2[0], point2[1], point2[2],
vector_intersect_angle(vec1, point1, vec2, point2));
/* vector intersect angle */
vec1[0] = -2.0; point1[0] = 4.0;
vec1[1] = 0.0; point1[1] = 0.0;
vec1[2] = 0.0; point1[2] = 0.0;
vec2[0] = 0.0; point2[0] = -4.0;
vec2[1] = 0.0; point2[1] = 0.0;
vec2[2] = 0.0; point2[2] = 10.0;
fprintf(stderr, "\n\t ---- VECTOR ANGLE INTERSECT OBTUSE -----\n"
"\t1: (%f %f %f) -> (%f %f %f)\n"
"\t2: (%f %f %f) -> (%f %f %f)\n"
"\t=== %f\n",
vec1[0], vec1[1], vec1[2],
point1[0], point1[1], point1[2],
vec2[0], vec2[1], vec2[2],
point2[0], point2[1], point2[2],
vector_intersect_angle(vec1, point1, vec2, point2));
/* vector intersect angle */
vec1[0] = 0.0; point1[0] = 0.0;
vec1[1] = 4.0; point1[1] = 0.0;
vec1[2] = 0.0; point1[2] = 0.0;
vec2[0] = 0.0; point2[0] = 0.0;
vec2[1] = 0.0; point2[1] = 0.0;
vec2[2] = 4.0; point2[2] = 0.0;
fprintf(stderr, "\n\t ---- VECTOR ANGLE INTERSECT INTERSECTING -----\n"
"\t1: (%f %f %f) -> (%f %f %f)\n"
"\t2: (%f %f %f) -> (%f %f %f)\n"
"\t=== %f\n",
vec1[0], vec1[1], vec1[2],
point1[0], point1[1], point1[2],
vec2[0], vec2[1], vec2[2],
point2[0], point2[1], point2[2],
vector_intersect_angle(vec1, point1, vec2, point2));
#endif
return 0;
}