Irrlicht Engine

Code: Select all

core::vector3df CGame::getClosestPointToEllipsoid(const core::vector3df& Point, const core::vector3df& Scale, int &iterations)
{
    // create an initial guess by finding the intersection of the ray from the
    // centroid to point with the ellipsoid surface

    float a = Scale.X;
    float b = Scale.Y;
    float c = Scale.Z;
    core::vector3df p = Point;

    float k = sqrt( 1 / ( (p.X*p.X)/(a*a) + (p.Y*p.Y)/(b*b) + (p.Z*p.Z)/(c*c) ) );

    core::vector3df q = k * p;

    // this is the scale factor for our movement along the ellipse, this can
    // start off quite large because it will be reduced as needed
    float s = 1.f;

    // this is the main loop, you'll want to put some kind of tolerance based
    // terminal condition, i.e. when the cost function (distance) is only
    // bouncing back and forth between a tolerance or something more tailored to
    // your application, I will stop when the distance is +/- 1%
    core::vector3df plotQ[100];
    float plotJ[100];
    bool quit = false;
    int i = 1;

    do
    {
        // calculate the z for our given x and y
        float qz_plus = c * (float)sqrt( 1.f - ((q.X*q.X)/(a*a)) - ((q.Y*q.Y)/(b*b)) );
        float qz_minus = -qz_plus;

        // we want the one that get's us closest to the target so
        if ( fabs(p.Z - qz_plus) < fabs(p.Z - qz_minus) )
            q.Z = qz_plus;
        else
            q.Z = qz_minus;

        // calculate the current value of the cost function
        float J = (float)sqrt( ((p.X - q.X)*(p.X - q.X)) + ((p.Y - q.Y)*(p.Y - q.Y)) + ((p.Z - q.Z)*(p.Z - q.Z)) );

        // store the current values for the plots
        plotQ[i] = q;
        plotJ[i] = J;

        // check to see if we overshot the goal or jumped off the ellipsoid
        if ( i > 1 )
        {
            // if we jumped off the ellipsoid or overshot the minimal cost
            //if( imag(qz) ~= 0 || J > Jplot(i-1) ) ****************************
            if ( J > plotJ[i-1] )
            {
                // then go back to the previous position and use a finer
                // step size
                q = plotQ[i-1];
                J = plotJ[i-1];
                s /= 10.f;
                --i;

                // if we did just jump over the actual minimum, let's check to
                // see how confident we are at this point, if we're with in
                // 1% there's no need to continue
                if ( i > 3 && fabs( (plotJ[i-1] - plotJ[i-3]) / plotJ[i-3] ) < 0.001f ) quit = true;
            }
        }

        if (!quit)
        {
            // calculate the gradient of the cost with respect to our control
            // variables; since we divide by qz in the second term we first need
            // to check whether or not qz is too close to zero; if it is, we know
            // that the second term should be zero
            float dJdx, dJdy;

            //if( q.Z < 1e-10 )
            if (core::equals(q.Z, 0.f))
            {
                dJdx = -2.f*(p.X - q.X);
                dJdy = -2.f*(p.Y - q.Y);
            }
            else
            {
                dJdx = -2.f*(p.X - q.X) + 2*(q.Z < 0.f ? -1.f : 1.f)*(p.Z - q.Z)*( c/(a*a) * (q.X/q.Z) );
                dJdy = -2.f*(p.Y - q.Y) + 2*(q.Z < 0.f ? -1.f : 1.f)*(p.Z - q.Z)*( c/(b*b) * (q.Y/q.Z) );
            }

            // calculate the update vector that we will move along
            float dx = -J/dJdx, dy = -J/dJdy;

            // calculate the magnitude of that update vector
            float magnitude = (float)sqrt( dx*dx + dy*dy );

            // normalize our update vector so that we don't shoot off at an
            // uncontrolable rate
            dx = s * (1.f/magnitude) *dx;
            dy = s * (1.f/magnitude) *dy;

            // update the current position
            q.X = q.X + dx;
            q.Y = q.Y + dy;
        }

        // increment the index
        if (++i >= 100) quit = true;
    }
    while (!quit);

    iterations = i;
    return q;
}