void CFunnel::GetCorner(CVector3 *P1, CVector3 *Cntr, float Radius, bool WichSide, CVector3 &TResult)
{
if(WichSide)
{
float x0 = P1->x;
float y0 = P1->z;
float x1 = Cntr->x;
float y1 = Cntr->z;
// get distance from P1 to C
float D = P1->GetDistance(*Cntr);
// get center of 2nd circle
float x2 = (x0+x1)/2;
float y2 = (y0+y1)/2;
// get ramp.
float m = (x2-x1)/(y1-y2);
float n = (x1*x1 - x2*x2 + y1*y1 - y2*y2 + ((D*D)/4) - Radius*Radius)/( 2*(y1-y2));
// get parameters
float A = m*m + 1;
float B = 2*(m*n - x1 - y1*m);
float C = n*n + x1*x1 - 2*y1*n - Radius*Radius;
// get FINAL eq. solutions
float delta = sqrt(B*B - 4*A*C);
float a = (-B - delta) / (2*A);
float b = m*a+n;
TResult = CVector3(a,0,b);
}
else
{
float x0 = P1->x;
float y0 = P1->z;
float x1 = Cntr->x;
float y1 = Cntr->z;
// get distance from P1 to C
float D = P1->GetDistance(*Cntr);
// get center of 2nd circle
float x2 = (x0+x1)/2;
float y2 = (y0+y1)/2;
// get ramp.
float m = (x2-x1)/(y1-y2);
float n = (x1*x1 - x2*x2 + y1*y1 - y2*y2 + ((D*D)/4) - Radius*Radius)/( 2*(y1-y2));
// get parameters
float A = m*m + 1;
float B = 2*(m*n - x1 - y1*m);
float C = n*n + x1*x1 - 2*y1*n - Radius*Radius;
// get FINAL eq. solutions
float delta = sqrt(B*B - 4*A*C);
float c = (-B + delta) / (2*A);
float d = m*c+n;
TResult = CVector3(c,0,d);
}
}