spieleprogrammierer.de - Forum und Wiki zur Spieleprogrammierung und Spieleentwicklung

C-/C++-Quelltext

int Mat3::EigenSolve( Vec3 v[3], float values[3], float epsilon ) const
{
    float m00 = m[0][0];
    float m11 = m[1][1];
    float m22 = m[2][2];
    float m02 = m[0][2];
    float m20 = m[2][0];
    float m00_11 = m00 * m11;
    float m20_02 = m20 * m02;
    float m10_01 = m[1][0] * m[0][1];
    float m12_21 = m[1][2] * m[2][1];
    float m10_21 = m[1][0] * m[2][1];
    float m01_12 = m[0][1] * m[1][2];

    float a = m00 + m11 + m22;
    float b = m10_01 + m20_02 + m12_21 - m00_11 - m00 * m22 - m11 * m22;
    float c = m00_11 * m22 - m12_21 * m00 - m10_01 * m22 + m10_21 * m02 + m01_12 * m20 - m20_02 * m11;

    int result = Polynomial::RootsCubic( -1, a, b, c, values );
    int numEigenVectors = 0;
    for ( int i = 0; i < result; ++i )
    {
        Mat3 tmp = *this;
        tmp[0][0] -= values[i];
        tmp[1][1] -= values[i];
        tmp[2][2] -= values[i];

        int r = 0;
        for ( int c = 0; c < 3; ++c )
        {
            int pivot = r;
            float max = FAbs( tmp[r][c] );
            for ( int k = r+1; k < 3; ++k )
            {
                float entry = FAbs( tmp[k][c] );
                if ( entry > max ) {
                    max = entry;
                    pivot = k;
                }
            }

            if ( FAbs( max ) < epsilon )
                continue;

            if ( pivot != r ) {
                Swap( tmp[r], tmp[pivot] );
            }

            tmp[r] /= tmp[r][c];

            for ( int k = 0; k < 3; ++k )
            {
                if ( k == r )
                    continue;

                tmp[k] -= tmp[k][c] * tmp[r];
            }

            ++r;
        }

        tmp.TransposeSelf();

        for ( int j = 0; j < 3; ++j )
        {
            if ( FAbs( tmp[j][j] ) < epsilon ) 
            {
                Vec3 vec = tmp[j] * -1;
                vec[j] = 1;
                v[numEigenVectors++] = Normalize( vec );
            }
        }
    }

    return result;
}

C-/C++-Quelltext

void OBB::FromPoints( const Vec3* points, int num )
{
    if ( !points || num < 1 ) {
        return;
    }

    Vec3 mean = points[0];
    for ( int i = 1; i < num; ++i ) {
        mean += points[i];
    }

    float invnum = 1.0f / num;
    mean *= invnum;

    // Build covariant matrix

    Mat3 covariants;
    covariants.Zero();
    for ( int i = 0; i < num; ++i ) 
    {
        Vec3 dir = points[i] - mean;
        covariants[0][0] += dir.x * dir.x;
        covariants[0][1] += dir.x * dir.y;
        covariants[0][2] += dir.x * dir.z;
        covariants[1][1] += dir.y * dir.y;
        covariants[1][2] += dir.y * dir.z;
        covariants[2][2] += dir.z * dir.z;
    }

    covariants[1][0] = covariants[0][1];
    covariants[2][0] = covariants[0][2];
    covariants[2][1] = covariants[1][2];
    covariants *= invnum;

    covariants.EigenSolveSymmetric( &mAxes[0], mExtends.ToFloatPtr() );

    Vec3 min( 99999.0f, 99999.0f, 99999.0f );
    Vec3 max = -min;

    for ( int i = 0; i < num; ++i )
    {
        float p0 = points[i] * mAxes[0];
        float p1 = points[i] * mAxes[1];
        float p2 = points[i] * mAxes[2];

        if ( p0 > max.x ) max.x = p0;
        if ( p0 < min.x ) min.x = p0;
        if ( p1 > max.y ) max.y = p1;
        if ( p1 < min.y ) min.y = p1;
        if ( p2 > max.z ) max.z = p2;
        if ( p2 < min.z ) min.z = p2;
    }

    mCenter = ( min + max ) * 0.5f;
    mExtends = max - mCenter;
    mCenter *= mAxes;
}

C-/C++-Quelltext

void Mat3::EigenSolveSymmetric( Vec3 v[3], float values[3], float epsilon ) const
{
    static const int maxIter = 64;

    float m00 = m[0][0];
    float m01 = m[0][1];
    float m02 = m[0][2];
    float m11 = m[1][1];
    float m12 = m[1][2];
    float m22 = m[2][2];

    float theta, theta2, theta2Plus1, c, s, m_ik;

    v[0] = Vec3(1, 0, 0),
        v[1] = Vec3(0, 1, 0),
        v[2] = Vec3(0, 0, 1);

    for ( int i = 0; i < maxIter; ++i )
    {
        if ( FAbs( m01 ) < epsilon && FAbs( m02 ) < epsilon && FAbs( m12 ) < epsilon ) {
            break;
        }

        // (i = 0, j = 1, k = 2)

        if ( m01 != 0 )
        {
            theta = ( m11 - m00 ) * 0.5f / m01; // tan(2t)

            theta2 = theta * theta;
            theta2Plus1 = theta2 + 1.0f;
            if ( theta2Plus1 != theta2 ) 
            {
                theta = ( theta < 0.0f ) 
                    ? -( Sqrt( theta2Plus1 ) - FAbs( theta ) )
                    : +( Sqrt( theta2Plus1 ) - FAbs( theta ) );
            }
            else {
                theta = .5f / theta;
            }

            c = InvSqrt( 1.0f + theta * theta );
            s = theta * c;

            m_ik = c * m02 - s * m12;
            m12 = s * m02 + c * m12;
            m02 = m_ik;
            m00 -= theta * m01;
            m11 += theta * m01;
            m01 = 0;

            for ( int j = 0; j < 3; ++j )
            {
                float tmp = c * v[0][j] - s * v[1][j];
                v[1][j]   = s * v[0][j] + c * v[1][j];
                v[0][j]   = tmp;
            }
        }

        // (i = 0, j = 2, k = 1)

        if ( m02 != 0 )
        {
            theta = ( m22 - m00 ) * 0.5f / m02;
            theta2 = theta * theta;
            theta2Plus1 = theta2 + 1.0f;
            if ( theta2Plus1 != theta2 ) 
            {
                theta = ( theta < 0.0f ) 
                    ? -( Sqrt( theta2Plus1 ) - FAbs( theta ) )
                    : +( Sqrt( theta2Plus1 ) - FAbs( theta ) );
            }
            else {
                theta = .5f / theta;
            }

            c = InvSqrt( 1.0f + theta * theta );
            s = theta * c;

            m_ik = c * m01 - s * m12;
            m12 = s * m01 + c * m12;
            m01 = m_ik;
            m00 -= theta * m02;
            m22 += theta * m02;
            m02 = 0;

            for ( int j = 0; j < 3; ++j )
            {
                float tmp = c * v[0][j] - s * v[2][j];
                v[2][j]   = s * v[0][j] + c * v[2][j];
                v[0][j]   = tmp;
            }
        }

        // (i = 1, j = 2, k = 0)

        if ( m12 != 0 )
        {
            theta = ( m22 - m11 ) * 0.5f / m12;
            theta2 = theta * theta;
            theta2Plus1 = theta2 + 1.0f;
            if ( theta2Plus1 != theta2 ) 
            {
                theta = ( theta < 0.0f ) 
                    ? -( Sqrt( theta2Plus1 ) - FAbs( theta ) )
                    : +( Sqrt( theta2Plus1 ) - FAbs( theta ) );
            }
            else {
                theta = .5f / theta;
            }

            c = InvSqrt( 1.0f + theta * theta );
            s = theta * c;

            m_ik = c * m01 - s * m02;
            m02 = s * m01 + c * m02;
            m01 = m_ik;
            m11 -= theta * m12;
            m22 += theta * m12;
            m12 = 0;

            for ( int j = 0; j < 3; ++j )
            {
                float tmp = c * v[1][j] - s * v[2][j];
                v[2][j]   = s * v[1][j] + c * v[2][j];
                v[1][j]   = tmp;
            }
        }
    }

    values[0] = m00;
    values[1] = m11;
    values[2] = m22;
}