/* return distance of a spatial point to the ellipsoid */
double ELLIP_Spatial_Point_Distance (void *dummy, ELLIP *eli, double *point)
{
double q [3];
return gjk_ellip_point (eli->cur_center, eli->cur_sca, eli->cur_rot, point, q);
}
/* generate test */
static void gen ()
{
double d [3], move [3];
switch (mode)
{
case GJK_CONVEX_CONVEX:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
while (bsize < minim) bsize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
for (int n = 0; n < bsize; n ++)
{ SETRAND (bpoint [n], 0.75);
SUB (bpoint [n], move, bpoint [n]); }
free (a);
free (b);
a = hull ((double*)apoint, asize, &alength);
b = hull ((double*)bpoint, bsize, &blength);
double *va, *vb;
int nva, nvb;
va = TRI_Vertices (a, alength, &nva);
vb = TRI_Vertices (b, blength, &nvb);
gjk (va, nva, vb, nvb, p, q);
free (va);
free (vb);
}
break;
case GJK_CONVEX_SPHERE:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
free (a);
a = hull ((double*)apoint, asize, &alength);
SETRAND (center, 1.0);
SUB (center, move, center);
radius = 0.75 * DRAND ();
double *va;
int nva;
va = TRI_Vertices (a, alength, &nva);
gjk_convex_sphere (va, nva, center, radius, p, q);
free (va);
}
break;
case GJK_CONVEX_POINT:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
free (a);
a = hull ((double*)apoint, asize, &alength);
SETRAND (center, 1.0);
SUB (center, move, center);
radius = 0.0;
double *va;
int nva;
va = TRI_Vertices (a, alength, &nva);
COPY (center, p);
gjk_convex_point (va, nva, p, q);
free (va);
}
break;
case GJK_CONVEX_ELLIP:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
free (a);
a = hull ((double*)apoint, asize, &alength);
SETRAND (el1_center, 1.0);
SUB (el1_center, move, el1_center);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
double *va;
int nva;
va = TRI_Vertices (a, alength, &nva);
gjk_convex_ellip (va, nva, el1_center, el1_sca, el1_rot, p, q);
free (va);
}
break;
case GJK_SPHERE_ELLIP:
{
SETRAND (move, 1.0);
SETRAND (center, 1.0);
radius = 0.75 * DRAND ();
SETRAND (el1_center, 1.0);
SUB (el1_center, move, el1_center);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
gjk_sphere_ellip (center, radius, el1_center, el1_sca, el1_rot, p, q);
}
break;
case GJK_ELLIP_ELLIP:
{
SETRAND (move, 1.0);
SETRAND (el1_center, 1.0);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
SETRAND (el2_center, 1.0);
SUB (el2_center, move, el2_center);
el2_sca [0] = 0.75 * DRAND ();
el2_sca [1] = 0.75 * DRAND ();
el2_sca [2] = 0.75 * DRAND ();
EXPMAP (el2_sca, el2_rot);
gjk_ellip_ellip (el1_center, el1_sca, el1_rot, el2_center, el2_sca, el2_rot, p, q);
}
break;
case GJK_ELLIP_POINT:
{
SETRAND (move, 1.0);
SETRAND (el1_center, 1.0);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
SETRAND (center, 1.0);
SUB (center, move, center);
radius = 0.0;
COPY (center, p);
gjk_ellip_point (el1_center, el1_sca, el1_rot, p, q);
}
break;
}
SUB (p, q, d);
printf ("|p-q|=%g\n", LEN (d));
}
