Source/SR_Math.c

/******************************************************************************
 **                                                                          **
 **     Module:     SR_Math.c                                                **
 **                                                                          **
 **                                                                          **
 **     Purpose:    Various mathematical utility functions                   **
 **                                                                          **
 **                                                                          **
 **                                                                          **
 **     Copyright (C) 1996 Apple Computer, Inc.  All rights reserved.        **
 **                                                                          **
 **                                                                          **
 *****************************************************************************/
#include <assert.h>
#include <math.h>
 
#include "QD3D.h"
#include "QD3DMath.h"
#include "QD3DGeometry.h"
 
#include "SR.h"
#include "SR_Math.h"
 
 
/*===========================================================================*\
 *
 *  Routine:    SRTriangle_GetNormal()
 *
 *  Comments:   Compute the face normal for a triangle. If the triangle
 *              has a normal in its attribute set, use that; otherwise,
 *              compute the normal from the vertices.
 *
\*===========================================================================*/
 
TQ3Status SRTriangle_GetNormal(
    TQ3TriangleData     *triangleData,
    TQ3Vector3D         *normal)
{
    assert(triangleData != NULL);
        
    /*
     *  Check for face normal for triangle. Use it if it's there, but assume
     *  it's normalized, as per the rule.
     */
    if ((triangleData->triangleAttributeSet != NULL) && 
        (Q3AttributeSet_Get( 
            triangleData->triangleAttributeSet, 
            kQ3AttributeTypeNormal, 
            normal) == kQ3Success)) {
    } else {
        /* 
         *  Otherwise, compute plane normal, and normalize.
         */
        Q3Point3D_CrossProductTri(
            &triangleData->vertices[0].point,
            &triangleData->vertices[1].point,
            &triangleData->vertices[2].point,
            normal);
        Q3Vector3D_Normalize(normal, normal);
    }
    
    return (kQ3Success);    
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRMatrix_LUDecomposeSingular3x3()
 *
 *  Comments:   Decompose a singular 3x3 matrix "A" into PA = LU.
 *
 *              P is the permutation matrix for the row exchanges required
 *                  by pivoting.
 *              L is a lower triangular matrix with ones on the diagonal.
 *              U is an upper echelon matrix.  For nonsingular matrices,
 *                  it is upper triangular.
 *
 *              Upon return, this routine returns LU in place of the original
 *              matrix.  U occupies the upper triangular elements.  L occupies
 *              the elements below the diagonal.  P is represented in "short-
 *              hand" form by the three-element vector indexPtr.  If the
 *              number of row exchanges is even, then *rowInterchanges is 1.0,
 *              otherwise, -1.0.  The rank of the matrix is returned in *rank.
 *
 *              To fully understand this routine, read Chapters 1-3 of 
 *              Gilbert Strang's book, Linear Algebra and Its Applications.
 *
\*===========================================================================*/
 
/* 
 *  Macros for supporting SRMatrix_LUDecomposeSingular3x3 
 */
#define FLOAT_TOLERANCE     1.0e-5
#define EQUALS_ZERO(a,e)    (FABS(a) < (e))
#define SWAP(f,g)           \
    {                       \
        float t;            \
                            \
        t = (f);            \
        (f) = (g);          \
        (g) = t;            \
    }
 
void SRMatrix_LUDecomposeSingular3x3(
    float   matrix[3][3],
    long    indexPtr[3],
    float   *rowInterchanges,
    long    *rank)
{
    long    i, j;
    float   f;
    float   pivot;
    long    iPivot;
    float   max[3];
    
    /* Initialize parameters */
    *rowInterchanges = 1.0;
    for (i = 0; i < 3; i++) {
        indexPtr[i] = i;
    }
    *rank = 3;
    
    /* Determine magnitude of largest element in each column */
    for (j = 0; j < 3; j++) {
        max[j] = 0.0;
        for (i = 0; i < 3; i++) {
            if ((f = FABS(matrix[i][j])) > max[j]) {
                max[j] = f;
            }
        }
    }
    
    /*
     * Initial knowledge about LU decomposition:
     *
     *          |1 0 0|         |? ? ?|
     *      L = |? 1 0|     U = |? ? ?|
     *          |? ? 1|         |? ? ?|
     *
     *  ? = yet to be determined
     *  * = determined final value
     *  X = nonzero pivot
     */
     
    /* Find the row with the largest pivot in the first column */
    for (i = 0, pivot = 0.0, iPivot = 0; i < 3; i++) {
        f = matrix[i][0];
        if (FABS(f) > pivot) {
            pivot = f;
            iPivot = i;
        }
    }
    
    /* Record permutation of rows */
    if (iPivot != 0) {
        /* Swap row with largest pivot with first row */
        for (j = 0; j < 3; j++) {
            SWAP(matrix[0][j], matrix[iPivot][j]);
        }
        SWAP(indexPtr[0],indexPtr[iPivot]);
        *rowInterchanges = -(*rowInterchanges);
    }
    
    if ((max[0] == 0.0) || 
         EQUALS_ZERO(matrix[0][0] / max[0], FLOAT_TOLERANCE)) {
        /*
         * First column of U is zero:
         *
         *          |1 0 0|         |0 ? ?|
         *      L = |? 1 0|     U = |0 ? ?|
         *          |? ? 1|         |0 ? ?|
         */
        (*rank)--;
        matrix[0][0] = 0.0;
        
        /* Find the row with the largest pivot in the second column */
        for (i = 0, pivot = 0.0, iPivot = 0; i < 3; i++) {
            f = matrix[i][1];
            if (FABS(f) > pivot) {
                pivot = f;
                iPivot = i;
            }
        }
 
        /* Record permutation of rows */
        if (iPivot != 0) {
            /* Swap row with largest pivot with first row */
            for (j = 0; j < 3; j++) {
                SWAP(matrix[0][j], matrix[iPivot][j]);
            }
            SWAP(indexPtr[0],indexPtr[iPivot]);
            *rowInterchanges = -(*rowInterchanges);
        }
 
        if ((max[1] == 0.0) || 
             EQUALS_ZERO(matrix[0][1] / max[1], FLOAT_TOLERANCE)) {
            /*
             * Second column of U is zero:
             *
             *          |1 0 0|         |0 0 ?|
             *      L = |? 1 0|     U = |0 0 ?|
             *          |? ? 1|         |0 0 ?|
             */
            (*rank)--;
            matrix[0][1] = 0.0;
            matrix[1][1] = 0.0;
            
            /* Find the row with the largest pivot in the third column */
            for (i = 0, pivot = 0.0, iPivot = 0; i < 3; i++) {
                f = matrix[i][2];
                if (FABS(f) > pivot) {
                    pivot = f;
                    iPivot = i;
                }
            }
    
            /* Record permutation of rows */
            if (iPivot != 0) {
                /* Swap row with largest pivot with first row */
                for (j = 0; j < 3; j++) {
                    SWAP(matrix[0][j], matrix[iPivot][j]);
                }
                SWAP(indexPtr[0],indexPtr[iPivot]);
                *rowInterchanges = -(*rowInterchanges);
            }
 
            if ((max[2] == 0.0) || 
                 EQUALS_ZERO(matrix[0][2] / max[2], FLOAT_TOLERANCE)) {
                /*
                 * Third column of U is zero:
                 *
                 *          |1 0 0|         |0 0 0|
                 *      L = |0 1 0|     U = |0 0 0|
                 *          |0 0 1|         |0 0 0|
                 */
                (*rank)--;
                matrix[0][2] = 0.0;
                matrix[1][0] = 0.0;
                matrix[1][2] = 0.0;
                matrix[2][0] = 0.0;
                matrix[2][1] = 0.0;
                matrix[2][2] = 0.0;
            } else {
                /*
                 * Third column of U has a nonzero pivot:
                 * Need to calculate multiplier of L to
                 * eliminate third column of of U:
                 *
                 *          |1 0 0|         |0 0 X|
                 *      L = |* 1 0|     U = |0 0 0|
                 *          |* 0 1|         |0 0 0|
                 */
                matrix[1][0] = matrix[1][2] / matrix[0][2];
                matrix[1][2] = 0.0;
                matrix[2][0] = matrix[2][2] / matrix[0][2];
                matrix[2][1] = 0.0;
                matrix[2][2] = 0.0;
            }
        } else {
            /*
             * Second column of U has a nonzero pivot:
             * Need to calculate multiplier of L to
             * eliminate second column of of U:
             *
             *          |1 0 0|         |0 X *|
             *      L = |* 1 0|     U = |0 0 ?|
             *          |* ? 1|         |0 0 ?|
             */
            f = matrix[1][1] / matrix[0][1];
            matrix[1][0] = f;
            matrix[1][1] = 0.0;
            matrix[1][2] = matrix[1][2] - f * matrix[0][2];
             
            f = matrix[2][1] / matrix[0][1];
            matrix[2][0] = f;
            matrix[2][1] = 0.0;
            matrix[2][2] = matrix[2][2] - f * matrix[0][2];
 
            /* Find the row with the largest pivot in the third column */
            if (FABS(matrix[1][2]) < FABS(matrix[2][2])) {
                iPivot = 2;
                /* Swap third row (possessing largest pivot) with second row */
                for (j = 0; j < 3; j++) {
                    SWAP(matrix[1][j], matrix[iPivot][j]);
                }
                SWAP(indexPtr[1], indexPtr[iPivot]);
                *rowInterchanges = -(*rowInterchanges);
            } else {
                iPivot = 1;
            }
            
            if ((max[2] == 0.0) || 
                 EQUALS_ZERO(matrix[1][2] / max[2], FLOAT_TOLERANCE)) {
                /*
                 * Third column of U has no nonzero pivot.
                 *
                 *          |1 0 0|         |0 X *|
                 *      L = |* 1 0|     U = |0 0 0|
                 *          |* 0 1|         |0 0 0|
                 */
                (*rank)--;
                matrix[1][2] = 0.0;
                matrix[2][1] = 0.0;
                matrix[2][2] = 0.0;
            } else {
                /*
                 * Third column of U has a nonzero pivot.
                 * Need to calculate multiplier of L to
                 * eliminate third column of of U:
                 *
                 *          |1 0 0|         |0 X *|
                 *      L = |* 1 0|     U = |0 0 X|
                 *          |* * 1|         |0 0 0|
                 */
                matrix[2][1] = matrix[2][2] / matrix[1][2];;
                matrix[2][2] = 0.0;
            }
        }
    } else {
        /*
         * First column of U has a nonzero pivot.
         * Need to calculate multipliers of L to
         * eliminate first column of of U:
         *
         *          |1 0 0|         |X * *|
         *      L = |* 1 0|     U = |0 ? ?|
         *          |* ? 1|         |0 ? ?|
         */
        f = matrix[1][0] / matrix[0][0];
        matrix[1][0] = f;
        matrix[1][1] = matrix[1][1] - f * matrix[0][1];
        matrix[1][2] = matrix[1][2] - f * matrix[0][2];
         
        f = matrix[2][0] / matrix[0][0];
        matrix[2][0] = f;
        matrix[2][1] = matrix[2][1] - f * matrix[0][1];
        matrix[2][2] = matrix[2][2] - f * matrix[0][2];
 
        /* Find the row with the largest pivot in the second column */
        if (FABS(matrix[1][1]) < FABS(matrix[2][1])) {
            iPivot = 2;
            /* Swap third row (possessing largest pivot) with second row */
            for (j = 0; j < 3; j++) {
                SWAP(matrix[1][j], matrix[iPivot][j]);
            }
            SWAP(indexPtr[1], indexPtr[iPivot]);
            *rowInterchanges = -(*rowInterchanges);
        } else {
            iPivot = 1;
        }
        
        if ((max[1] == 0.0) || 
             EQUALS_ZERO(matrix[1][1] / max[1], FLOAT_TOLERANCE)) {
            /*
             * Second column of U has no nonzero pivot.
             *
             *          |1 0 0|         |X * *|
             *      L = |* 1 0|     U = |0 0 ?|
             *          |* ? 1|         |0 0 ?|
             */
            (*rank)--;
            matrix[1][1] = 0.0;
    
            /* Find the row with the largest pivot in the third column */
            if (FABS(matrix[1][2]) < FABS(matrix[2][2])) {
                iPivot = 2;
                /* Swap third row (possessing largest pivot) with second row */
                for (j = 0; j < 3; j++) {
                    SWAP(matrix[1][j], matrix[iPivot][j]);
                }
                SWAP(indexPtr[1], indexPtr[iPivot]);
                *rowInterchanges = -(*rowInterchanges);
            } else {
                iPivot = 1;
            }
            
            if ((max[2] == 0.0) || 
                 EQUALS_ZERO(matrix[1][2] / max[2], FLOAT_TOLERANCE)) {
                /*
                 * Third column of U has no nonzero pivot.
                 *
                 *          |1 0 0|         |X * *|
                 *      L = |* 1 0|     U = |0 0 0|
                 *          |* 0 1|         |0 0 0|
                 */
                (*rank)--;
                matrix[1][2] = 0.0;
                matrix[2][1] = 0.0;
                matrix[2][2] = 0.0;
            } else {
                /*
                 * Third column of U has a nonzero pivot.
                 * Need to calculate multiplier of L to
                 * eliminate third column of of U:
                 *
                 *          |1 0 0|         |X * *|
                 *      L = |* 1 0|     U = |0 0 X|
                 *          |* * 1|         |0 0 0|
                 */
                matrix[2][1] = matrix[2][2] / matrix[1][2];;
                matrix[2][2] = 0.0;
            }
        } else {
            /*
             * Second column of U has a nonzero pivot.
             * Need to calculate multipliers of L to
             * eliminate second column of of U:
             *
             *          |1 0 0|         |X * *|
             *      L = |* 1 0|     U = |0 X *|
             *          |* * 1|         |0 0 ?|
             */
            f = matrix[2][1] / matrix[1][1];
            matrix[2][1] = f;
            matrix[2][2] = matrix[2][2] - f * matrix[1][2];
            
            if ((max[2] == 0.0) || 
                 EQUALS_ZERO(matrix[2][2] / max[2], FLOAT_TOLERANCE)) {
                /*
                 * Third column of U has no nonzero pivot.
                 *
                 *          |1 0 0|         |X * *|
                 *      L = |* 1 0|     U = |0 X *|
                 *          |* * 1|         |0 0 0|
                 */
                (*rank)--;
                matrix[2][2] = 0.0;
            } else {
                /*
                 * Third column of U has a nonzero pivot.
                 * The matrix has full rank.
                 *
                 *          |1 0 0|         |X * *|
                 *      L = |* 1 0|     U = |0 X *|
                 *          |* * 1|         |0 0 X|
                 */
            }
        }
    }
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRMatrix_ComputeFlatLand()
 *
 *  Comments:   Computes the eye vector in local coordinates and the plane
 *              equation in world coordinates when the upper-left 3x3 submatrix
 *              of the local-to-world matrix has a rank of 2.  The eye vector
 *              in local coordinates can be used for culling, but should not
 *              be used for lighting because the local-to-world matrix does
 *              not preserve lengths.
 *
 *  Input:      matrix:             the local-to-world matrix
 *              luDecomp:           the LU decomposition of the upper-left 
 *                                  3x3 submatrix of the local-to-world matrix
 *              parallelProjection: true for parallel projections, false for
 *                                  perspective projections
 *              eyeVectorWC:        the eye vector in world coordinates
 *                                  (required only for parallel projections)
 *              eyePointWC:         the eye point in world coordinates
 *                                  (required only for perspective projections)
 *
 *  Output:     eyeVectorLC:        the eye vector in local coordinates
 *              flatWorld:          the plane equation in world coordinates
 *                                  to which all geometry is transformed with
 *                                  the normal in the same hemisphere as the 
 *                                  eye
 *
 *  Returns:    kQ3Success:         computation completed successfully
 *              kQ3Failure:         computation could not be completed 
 *                                  successfully
 *
 *              To fully understand this routine, read Chapters 1-3 of 
 *              Gilbert Strang's book, Linear Algebra and Its Applications.
 *
\*===========================================================================*/
 
TQ3Status SRMatrix_ComputeFlatLand(
    TQ3Matrix4x4        *matrix,
    TQ3Matrix3x3        *luDecomp,
    TQ3Boolean          parallelProjection,
    TSRVector4D         *eyeVectorWC,
    TQ3RationalPoint4D  *eyePointWC,
    TSRVector4D         *eyeVectorLC,
    TQ3PlaneEquation    *flatWorld)
{
    long            pivot0 = -1;
    long            pivot1 = -1;
    long            j;
    TQ3Vector3D     v0;
    TQ3Vector3D     v1;
    TQ3Vector3D     v2;
 
    /* Search for the columns of the first two nonzero pivots */
    for (j = 0; j < 3; j++) {
        if (luDecomp->value[0][j] != 0.0) {
            pivot0 = j;
            break;
        }
    }
 
    if (pivot0 != -1) {
        for (j = pivot0 + 1; j < 3; j++) {
            if (luDecomp->value[1][j] != 0.0) {
                pivot1 = j;
                break;
            }
        }
    }
 
    if (pivot0 == -1 || pivot1 == -1) {
        /* Two nonzero pivots not found */
        return kQ3Failure;
    }
 
    /*
     *  Points are represented as row vectors, and they are transformed as
     *  follows:    p M = q
     *  where M is a homogeneous matrix, and p and q are row vectors.
     *  If M is the local-to-world matrix and A is the upper-left 3x3 submatrix
     *  of M, then q is in the row space of A (in the absence of translation
     *  and assuming that M is affine).  Given a factorization of A as
     *  PA = LU, a basis for the row space of A is the basis of U.  Since
     *  the rank of A is 2, the first two rows are a basis for U.  The space
     *  spanned by these two rows is the plane in WC onto which all 3D geometry 
     *  in LC is transformed.  The cross product of the two rows is the normal
     *  of the plane.  Also note: the nullspace of A is orthogonal to the row 
     *  space: this one dimensional space is spanned by the normal of the plane
     *  in WC.
     */
    Q3Vector3D_Set(
        &v0, 
        luDecomp->value[0][0], 
        luDecomp->value[0][1], 
        luDecomp->value[0][2]);
    Q3Vector3D_Set(&v1, 0.0, luDecomp->value[1][1], luDecomp->value[1][2]);
    Q3Vector3D_Cross(&v0, &v1, &v2);
    Q3Vector3D_Normalize(&v2, &flatWorld->normal);
    
    /*
     *  A point on the plane in world coordinates (WC) is given by transforming
     *  any point in local coordinates (LC) to WC.  If (0, 0, 0, 1) is the
     *  LC point, the WC point is simply the last row of matrix M.  Substitute
     *  this into the plane equation to obtain the constant D = - Ax - By - Cz.
     */
    flatWorld->constant = - matrix->value[3][0] * flatWorld->normal.x
                          - matrix->value[3][1] * flatWorld->normal.y
                          - matrix->value[3][2] * flatWorld->normal.z;
    
    /*
     *  Each point in the column space of A has a one-to-one mapping to a point
     *  in the row space of A.  In WC, the eye is on one side of the plane or
     *  the other.  For the purposes of culling, it makes sense to put the eye
     *  in LC on the same side of the plane.  So the eye vector in LC should be
     *  orthogonal to the column space of A.  Otherwise, it would be in the 
     *  column space, which doesn't make sense since the eye is almost never 
     *  in the row space in WC.  A basis for the column space consists of the 
     *  columns in A corresponding to those in U with nonzero pivots.
     */
    Q3Vector3D_Set(
        &v0, 
        matrix->value[0][pivot0],
        matrix->value[1][pivot0], 
        matrix->value[2][pivot0]);
    Q3Vector3D_Set(
        &v1, 
        matrix->value[0][pivot1], 
        matrix->value[1][pivot1], 
        matrix->value[2][pivot1]);
    Q3Vector3D_Cross(&v0, &v1, &v2);
    Q3Vector3D_Normalize(&v2, &v2);
    SRVector3D_To4D(&v2, eyeVectorLC);
    
    /*
     *  We are almost done.  However, the sense of the eye vector in LC and
     *  the normal of the plane equation in WC still need to be established.
     *  The latter needs to point in the same hemisphere as the eye.
     */
    if (parallelProjection) {
    
        /* Parallel projection */
        
        SRVector4D_To3D(eyeVectorWC, &v2);
        if (Q3Vector3D_Dot(&flatWorld->normal, &v2) < 0.0) {
            /*
             *  WC eye vector and plane normal point in opposite directions.
             *  Need to reverse the sense of the plane equation.
             */
            Q3Vector3D_Negate(&flatWorld->normal, &flatWorld->normal);
            flatWorld->constant = -flatWorld->constant;
        }
    } else {
        TQ3Point3D  p0;
        TQ3Point3D  p1;
        
        /*  Perspective projection */
        
        /*
         *  A WC eye vector can be obtained by subtracting the eye point
         *  by a point on the plane.  The LC point (0, 0, 0, 1) transformed
         *  to WC is the last row of M.
         */
        Q3RationalPoint4D_To3D(eyePointWC, &p0);
        Q3Point3D_Set(
            &p1, 
            matrix->value[3][0], 
            matrix->value[3][1], 
            matrix->value[3][2]);
        Q3Point3D_Subtract(&p0, &p1, &v2);
        if (Q3Vector3D_Dot(&flatWorld->normal, &v2) < 0.0) {
            /*
             *  WC eye vector and plane normal point in opposite directions.
             *  Need to reverse the sense of the plane equation.
             */
            Q3Vector3D_Negate(&flatWorld->normal, &flatWorld->normal);
            flatWorld->constant = -flatWorld->constant;
        }
    }
    
    /*
     * The hard part to comprehend is the method for establishing the sense 
     * of the eye vector in LC.  We had computed the eye vector by taking
     * the cross product of v0 and v1, the two basis vectors for the column 
     * space.  This gave a right-handed normal vector for the two basis 
     * vectors.  If v0 and v1 are transformed by A, the two new vectors
     * (v0 A) and (v1 A) lie in the row space, which is a plane parallel
     * to the plane to which all geometry is transformed in WC.  The
     * cross product of (v0 A) and (v1 A) either has the same or opposite
     * sense as the normal to the plane, which points in the same hemisphere
     * as the eye in WC.  If it is opposite, then the eye vector in LC has
     * the opposite sense to the existing orientation.
     */
    Q3Vector3D_Transform(&v0, matrix, &v0);
    Q3Vector3D_Transform(&v1, matrix, &v1);
    Q3Vector3D_Cross(&v0, &v1, &v2);
    if (Q3Vector3D_Dot(&flatWorld->normal, &v2) < 0.0) {
        /*
         * The LC eye vector transformed into WC and plane normal point in 
         * opposite directions.  Need to reverse the sense of the LC eye
         * vector.
         */
        SRVector4D_Negate(eyeVectorLC, eyeVectorLC);
    }
    
    return kQ3Success;
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRVector4D_Set()
 *
 *  Comments:   Set a 4D vector
 *
\*===========================================================================*/
 
TSRVector4D *SRVector4D_Set(
    TSRVector4D     *vector4D,
    float           x, 
    float           y,
    float           z, 
    float           w)
{
    assert(vector4D != NULL);
    
    vector4D->x = x;
    vector4D->y = y;
    vector4D->z = z;
    vector4D->w = w;
    
    return (vector4D);
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRVector3D_To4D()
 *
 *  Comments:   Promote a 3D vector to 4D
 *
\*===========================================================================*/
 
TSRVector4D *SRVector3D_To4D(
    const TQ3Vector3D   *vector3D,
    TSRVector4D         *result)
{
    assert(vector3D != NULL);
    assert(result != NULL);
    
    result->x = vector3D->x;
    result->y = vector3D->y;
    result->z = vector3D->z;
    result->w = 1.0;
    
    return (result);
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRVector4D_To3D()
 *
 *  Comments:   Homogenize a 4D vector down to 3D
 *
\*===========================================================================*/
 
TQ3Vector3D *SRVector4D_To3D(
    const TSRVector4D   *vector4D,
    TQ3Vector3D         *result)
{
    float oneOverW;
    
    assert(vector4D != NULL);
    assert(result != NULL);
    
    oneOverW = 1.0 / vector4D->w;
    
    result->x = vector4D->x * oneOverW;
    result->y = vector4D->y * oneOverW;
    result->z = vector4D->z * oneOverW;
 
    return (result);
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRVector4D_Negate()
 *
 *  Comments:   Negate a 4D vector
 *
\*===========================================================================*/
 
TSRVector4D *SRVector4D_Negate(
    const TSRVector4D   *vector4D,
    TSRVector4D         *result)
{
    assert(vector4D != NULL);
    assert(result != NULL);
 
    result->x = -vector4D->x;
    result->y = -vector4D->y;
    result->z = -vector4D->z;
    result->w = -vector4D->w;
    
    return (result);
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRVector4D_Normalize()
 *
 *  Comments:   Normalize a 4D vector
 *
\*===========================================================================*/
 
TSRVector4D *SRVector4D_Normalize(
    const TSRVector4D   *vector4D,
    TSRVector4D         *result)
{
    float   length, oneOverLength;
    
    assert(vector4D != NULL);
    assert(result != NULL);
 
    length = sqrt(vector4D->x * vector4D->x + 
                    vector4D->y * vector4D->y +
                        vector4D->z * vector4D->z +
                            vector4D->w * vector4D->w);
    
    /*
     *  Check for zero-length vector
     */
    if (length < kQ3RealZero) {
        *result = *vector4D;
        return (NULL);
    } else {
        oneOverLength = 1.0 / length;
    
        result->x = vector4D->x * oneOverLength;
        result->y = vector4D->y * oneOverLength;
        result->z = vector4D->z * oneOverLength;
        result->w = vector4D->w * oneOverLength;
    }
    
    return (result);
}
 
 
/*===========================================================================*\
 *
 *  Routine:    SRVector4D_Transform()
 *
 *  Comments:   Transform a 4D vector by a matrix.
 *
\*===========================================================================*/
    
TSRVector4D *SRVector4D_Transform(
    const TSRVector4D   *vector4D,
    const TQ3Matrix4x4  *matrix4x4,
    TSRVector4D         *result)
{
    TSRVector4D         s;
    const TSRVector4D   *sPtr;
    
    assert(vector4D != NULL);
    assert(matrix4x4 != NULL);
    assert(result != NULL);
 
    /*
     *  Check if caller is bashing the vector
     */
    if (vector4D == result) {
        s = *vector4D;
        sPtr = &s;
    } else {
        sPtr = vector4D;
    }
        
    result->x = sPtr->x * matrix4x4->value[0][0] + 
                sPtr->y * matrix4x4->value[1][0] +
                sPtr->z * matrix4x4->value[2][0] +
                sPtr->w * matrix4x4->value[3][0];
                
    result->y = sPtr->x * matrix4x4->value[0][1] +
                sPtr->y * matrix4x4->value[1][1] +
                sPtr->z * matrix4x4->value[2][1] +
                sPtr->w * matrix4x4->value[3][1];
                
    result->z = sPtr->x * matrix4x4->value[0][2] + 
                sPtr->y * matrix4x4->value[1][2] +
                sPtr->z * matrix4x4->value[2][2] +
                sPtr->w * matrix4x4->value[3][2];
                
    result->w = sPtr->x * matrix4x4->value[0][3] + 
                sPtr->y * matrix4x4->value[1][3] +
                sPtr->z * matrix4x4->value[2][3] +
                sPtr->w * matrix4x4->value[3][3];
                
    return (result);
}


Copyright © 2003 Apple Computer, Inc. All Rights Reserved. Terms of Use | Privacy Policy | Updated: 2003-01-14