/*--
Cat lbxgl;Entity;Query
Form
int LBXGL_Entity_QueryRadius(LBXGL_Entity *ents, \
LBXGL_Entity **array, int max, \
double *origin, double radius);
Description
Query all entities in list withn the sphere defined by origin and \
radius.
array is used to hold the query results, and max is the maximal \
number of entities that can be placed in array (at least max+1 \
entries should exist within array to handle the trailing NULL).
This returns the number of entities placed in array.
--*/
int LBXGL_Entity_QueryRadius(LBXGL_Entity *ents,
LBXGL_Entity **array, int max,
double *origin, double radius)
{
LBXGL_Entity *cur;
int i, j;
double *corg, *cmins, *cmaxs;
double crad;
cur=ents;
i=0;
while(cur && (i<max))
{
corg=LBXGL_Entity_GetProperty(cur, "origin");
if(!corg)
{
cur=cur->next;
continue;
}
crad=LBXGL_Entity_GetRadius(cur);
if(vecdist(corg, origin)<=(radius+crad))
{
array[i++]=cur;
array[i]=NULL;
}
cur=cur->next;
}
return(i);
}
/*--
Cat lbxgl;Entity;Query
Form
LBXGL_Entity *LBXGL_Entity_QueryRadiusNext(LBXGL_Entity *ents, \
double *origin, double radius);
Description
Finds the first entity in the list of ents within the radius, \
and returns that entity.
Returns NULL if none match or the list is empty.
--*/
LBXGL_Entity *LBXGL_Entity_QueryRadiusNext(LBXGL_Entity *ents,
double *origin, double radius)
{
LBXGL_Entity *cur;
int i, j;
double *corg, *cmins, *cmaxs;
double crad;
cur=ents;
while(cur)
{
corg=LBXGL_Entity_GetProperty(cur, "origin");
if(!corg)
{
cur=cur->next;
continue;
}
crad=LBXGL_Entity_GetRadius(cur);
if(vecdist(corg, origin)<=(radius+crad))
return(cur);
cur=cur->next;
}
return(NULL);
}
int main() {
mt19937 rng;
uniform_int_distribution<int> posdist(-10000, 10000);
uniform_int_distribution<int> vecdist(-100, 100);
uniform_int_distribution<int> coefdist(-100, 100);
for(int i = 0; i < 10000; ++i) {
V s(posdist(rng), posdist(rng));
V v(vecdist(rng), vecdist(rng));
int a = coefdist(rng);
int b = coefdist(rng);
if(projectionParam(s + a * v + b * V(0, 1) * v, s, s + v) != a) fail();
}
return 0;
}
int draw_curve(const Vec coords[], size_t count)
{
int segc = 0;
int ret;
Vec* curve;
if(coords == NULL) {
return -1; // nurupo~
}
for(int i = 0, j = 1; j < count; i++, j++) {
segc += ceil(vecdist(coords[i], coords[j]) * 16);
}
curve = malloc(sizeof (Vec) * segc);
if(curve == NULL) {
return -1; // malloc error
}
for(int i = 0; i < segc; i++) {
curve[i] = decasteljau(coords, count, (double)i / segc);
}
ret = draw_polyline(curve, segc);
free(curve);
return ret;
}
/*
* Quantize pt[0..n_pt-1][0..veclen-1] into cb[0..vqsize-1][0..veclen-1] (where
* vqsize < n_pt, presumably). Do this with the following iterative procedure:
* 1. Choose an initial VQ codebook by selecting vqsize random points from pt.
* 2. Map each point in pt to the "nearest" codebook entry (currently based on
* Euclidean distance.
* 3. Re-estimate each VQ entry by taking the centroid of all pt entries mapped
* to it in step 2.
* 4. Repeat steps 2 and 3 until the "total error stabilizes".
* In the end, replace each point in pt with the nearest VQ value.
* Return value: final total error.
*/
static float64 vq (float32 **pt, float32 **cb, int32 n_pt, int32 vqsize, int32 veclen)
{
int32 p, c, i, iter, bestc, *pt2cb, *n_newcb;
float64 d, bestdist, err, prev_err;
float32 **newcb;
E_INFO("Clustering %d points into %d\n", n_pt, vqsize);
/* Allocate some temporary variables */
pt2cb = (int32 *) ckd_calloc (n_pt, sizeof(int32));
newcb = (float32 **)ckd_calloc_2d (vqsize, veclen, sizeof(float32));
n_newcb = (int32 *) ckd_calloc (vqsize, sizeof(int32));
/* Choose an initial codebook */
vq_init (pt, cb, n_pt, vqsize, veclen);
for (iter = 0;; iter++) {
timing_start (tmg);
/* Map each point to closest codebook entry (using Euclidean distance metric) */
err = 0.0;
for (p = 0; p < n_pt; p++) {
bestdist = 1e+200;
for (c = 0; c < vqsize; c++) {
d = vecdist (pt[p], cb[c], veclen);
if (d < bestdist) {
bestdist = d;
bestc = c;
}
}
pt2cb[p] = bestc;
err += bestdist;
}
/* Update codebook entries with centroid of mapped points */
for (c = 0; c < vqsize; c++) {
for (i = 0; i < veclen; i++)
newcb[c][i] = 0.0;
n_newcb[c] = 0;
}
for (p = 0; p < n_pt; p++) {
c = pt2cb[p];
for (i = 0; i < veclen; i++)
newcb[c][i] += pt[p][i];
n_newcb[c]++;
}
for (c = 0; c < vqsize; c++) {
if (n_newcb[c] == 0)
E_ERROR("Nothing mapped to codebook entry %d; entry not updated\n", c);
else {
float64 t;
t = 1.0 / n_newcb[c];
for (i = 0; i < veclen; i++)
cb[c][i] = newcb[c][i] * t;
}
}
timing_stop (tmg);
E_INFO("%4d: Error = %e, %.2f sec CPU, %.2f sec elapsed\n",
iter, err, tmg->t_cpu, tmg->t_elapsed);
timing_reset (tmg);
/* Check if VQ codebook converged */
if (iter > 10) {
if ((err == 0.0) || ((prev_err - err)/prev_err < 0.002))
break;
}
prev_err = err;
}
/* Replace points with nearest VQ entries created */
for (p = 0; p < n_pt; p++) {
c = pt2cb[p];
for (i = 0; i < veclen; i++)
pt[p][i] = cb[c][i];
}
ckd_free (pt2cb);
ckd_free_2d ((void **) newcb);
ckd_free (n_newcb);
return err;
}
