[GJK] Solving for lambda values
Posted: Tue Sep 13, 2005 2:17 am
Hey guys, I decided to try putting the matrix from page 127 of Gino's book into matlab and getting the symbolic solution. I have added this to my gjk implementation. The results are so-so currently, and I'm kind blocked on what to do next. The problem is that in solve_lambdas3 & solve_lambdas4 the denominator can turn out to be zero sometimes. Has anyone else gone down this road? Any suggestions in avoiding the zero denominator would be greatly appreciated!
Code: Select all
void solve_lambdas2 (int ibuf[4])
{
scalar a = _subdot[ibuf[1]][ibuf[0]];
scalar b = _subdot[ibuf[1]][ibuf[1]];
_lambdas[ibuf[0]] = b/(-a+b);
_lambdas[ibuf[1]] = -1.0/(-a+b)*a;
}
void solve_lambdas3 (int ibuf[4])
{
scalar a = _subdot[ibuf[1]][ibuf[0]];
scalar b = _subdot[ibuf[1]][ibuf[1]];
scalar c = _subdot[ibuf[1]][ibuf[2]];
scalar d = _subdot[ibuf[2]][ibuf[0]];
scalar e = _subdot[ibuf[2]][ibuf[1]];
scalar f = _subdot[ibuf[2]][ibuf[2]];
scalar denom = (d*b+a*f-f*b-d*c+e*c-e*a);
pal_assert (denom != 0.0);
scalar idenom = 1.0 / denom;
_lambdas[ibuf[0]] = -(f*b-e*c) * idenom;
_lambdas[ibuf[1]] = -(d*c-a*f) * idenom;
_lambdas[ibuf[2]] = (d*b-e*a) * idenom;
}
void solve_lambdas4 (int ibuf[4])
{
#define a _subdot[ibuf[1]][ibuf[0]]
#define b _subdot[ibuf[1]][ibuf[1]]
#define c _subdot[ibuf[1]][ibuf[2]]
#define d _subdot[ibuf[1]][ibuf[3]]
#define e _subdot[ibuf[2]][ibuf[0]]
#define f _subdot[ibuf[2]][ibuf[1]]
#define g _subdot[ibuf[2]][ibuf[2]]
#define h _subdot[ibuf[2]][ibuf[3]]
#define i _subdot[ibuf[3]][ibuf[0]]
#define j _subdot[ibuf[3]][ibuf[1]]
#define k _subdot[ibuf[3]][ibuf[2]]
#define l _subdot[ibuf[3]][ibuf[3]]
scalar denom = ((b*g-f*c-a*g+a*f+e*c-e*b)*l-e*d*k-b*h*k+e*b*k+f*d*k+j*c*h-j*d*g+i*d*g+a*h*k-i*b*g-a*f*k-a*j*h+a*j*g+e*j*d-e*j*c-i*c*h+i*f*c+i*b*h-i*f*d);
pal_assert (denom != 0.0);
scalar idenom = 1.0 / denom;
_lambdas[ibuf[0]] = ((b*g-f*c)*l+j*c*h-b*h*k+f*d*k-j*d*g) * idenom;
_lambdas[ibuf[1]] = -((-e*c+a*g)*l+i*c*h-a*h*k+e*d*k-i*d*g) * idenom;
_lambdas[ibuf[2]] = -((e*b-a*f)*l-i*b*h+a*j*h-e*j*d+i*f*d) * idenom;
_lambdas[ibuf[3]] = -(a*f*k-a*j*g-e*b*k+e*j*c+i*b*g-i*f*c) * idenom;
#undef a
#undef b
#undef c
#undef d
#undef e
#undef f
#undef g
#undef h
#undef i
#undef j
#undef k
#undef l
}
void solve_lambdas ()
{
int size = 0;
int ibuf[4];
size = get_indexes (ibuf);
switch (size)
{
case 1:
_lambdas[ibuf[0]] = 1.0;
break;
case 2:
solve_lambdas2 (ibuf);
break;
case 3:
solve_lambdas3 (ibuf);
break;
case 4:
solve_lambdas4 (ibuf);
break;
default:
pal_assert (false);
}
}