diff options
Diffstat (limited to 'src/libparsec/utl_math2.cpp')
| -rw-r--r-- | src/libparsec/utl_math2.cpp | 1231 |
1 files changed, 1231 insertions, 0 deletions
diff --git a/src/libparsec/utl_math2.cpp b/src/libparsec/utl_math2.cpp new file mode 100644 index 0000000..c783fe0 --- /dev/null +++ b/src/libparsec/utl_math2.cpp @@ -0,0 +1,1231 @@ +/* + * PARSEC - Math Code II (ANSI-C) + * + * $Author: uberlinuxguy $ - $Date: 2004/09/15 12:25:43 $ + * + * Orginally written by: + * Copyright (c) Markus Hadwiger <msh@parsec.org> 1996-1999 + * Copyright (c) Clemens Beer <cbx@parsec.org> 2001 + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +// C library +#include <stddef.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +// compilation flags/debug support +#include "config.h" +#include "debug.h" + +// general definitions +#include "general.h" +#include "objstruc.h" + +// global externals +#include "globals.h" + +// mathematics header +#include "utl_math.h" + +// local module header + + +// to prevent warnings when converting doubles to floats +#ifdef SYSTEM_COMPILER_MSVC + #pragma warning ( disable : 4244 ) +#endif // SYSTEM_COMPILER_MSVC + +// global matrix that can be used as destination by assembly math code -------- +// +ALLOC_DESTXMATRX( DestXmatrx ); + + +// calculate vector length ---------------------------------------------------- +// +geomv_t VctLenX( Vector3 *vec ) +{ + ASSERT( vec != NULL ); + + float x = GEOMV_TO_FLOAT( vec->X ); + float y = GEOMV_TO_FLOAT( vec->Y ); + float z = GEOMV_TO_FLOAT( vec->Z ); + + float norm = sqrtf( x*x + y*y + z*z ); + + return FLOAT_TO_GEOMV( norm ); +} + + +// calculate vector length (approximated version) ----------------------------- +// +geomv_t VctLen( Vector3 *vec ) +{ + ASSERT( vec != NULL ); + + //NOTE: + // this version approximates a vector's length + // by using the following formula: + // len = vmax + (11/32)vmed + (1/4)vmin + // [this gives a result that is accurate within 8%] + + geomv_t vmax = vec->X; + geomv_t vmed = vec->Y; + geomv_t vmin = vec->Z; + + ABS_GEOMV( vmax ); + ABS_GEOMV( vmed ); + ABS_GEOMV( vmin ); + + if ( DW32( vmax ) < DW32( vmed ) ) + SWAP_GEOMV( vmax, vmed ); + if ( DW32( vmax ) < DW32( vmin ) ) + SWAP_GEOMV( vmax, vmin ); + if ( DW32( vmed ) < DW32( vmin ) ) + SWAP_GEOMV( vmed, vmin ); + + geomv_t norm = vmax + ( vmed * ( 11.0f / 8.0f ) + vmin ) / 4.0f; + + return norm; +} + + +// normalize 3-D vector ------------------------------------------------------- +// +void NormVctX( Vector3 *vec ) +{ + //NOTE: + // this function doesn't handle null + // vectors transparently. there is only + // an assertion. otherwise NaN's will be + // the result. + + ASSERT( vec != NULL ); + + float x = GEOMV_TO_FLOAT( vec->X ); + float y = GEOMV_TO_FLOAT( vec->Y ); + float z = GEOMV_TO_FLOAT( vec->Z ); + + float norm = sqrtf( x*x + y*y + z*z ); + + ASSERT( norm > 1e-7 ); +// if ( norm <= 1e-7 ) { +// return; +// } + + vec->X = FLOAT_TO_GEOMV( x / norm ); + vec->Y = FLOAT_TO_GEOMV( y / norm ); + vec->Z = FLOAT_TO_GEOMV( z / norm ); +} + + +// normalize 3-D vector (approximated version) -------------------------------- +// +void NormVct( Vector3 *vec ) +{ + //NOTE: + // this function doesn't handle null + // vectors transparently. there is only + // an assertion. otherwise NaN's will be + // the result. + + ASSERT( vec != NULL ); + + geomv_t vmax = vec->X; + geomv_t vmed = vec->Y; + geomv_t vmin = vec->Z; + + ABS_GEOMV( vmax ); + ABS_GEOMV( vmed ); + ABS_GEOMV( vmin ); + + if ( DW32( vmax ) < DW32( vmed ) ) + SWAP_GEOMV( vmax, vmed ); + if ( DW32( vmax ) < DW32( vmin ) ) + SWAP_GEOMV( vmax, vmin ); + if ( DW32( vmed ) < DW32( vmin ) ) + SWAP_GEOMV( vmed, vmin ); + + geomv_t norm = vmax + ( vmed * ( 11.0f / 8.0f ) + vmin ) / 4.0f; + + ASSERT( norm > GEOMV_VANISHING ); +// if ( norm <= GEOMV_VANISHING ) { +// return; +// } + + vec->X = GEOMV_DIV( vec->X, norm ); + vec->Y = GEOMV_DIV( vec->Y, norm ); + vec->Z = GEOMV_DIV( vec->Z, norm ); +} + + +// normalize vectors of upper left 3x3 submatrix ------------------------------ +// +void NormMtx( Xmatrx matrix ) +{ + // normalize matrix column vectors + for ( int i = 0; i < 3; i++ ) { + + float x = GEOMV_TO_FLOAT( matrix[ 0 ][ i ] ); + float y = GEOMV_TO_FLOAT( matrix[ 1 ][ i ] ); + float z = GEOMV_TO_FLOAT( matrix[ 2 ][ i ] ); + + float norm = sqrtf( x*x + y*y + z*z ); + + matrix[ 0 ][ i ] = FLOAT_TO_GEOMV( x / norm ); + matrix[ 1 ][ i ] = FLOAT_TO_GEOMV( y / norm ); + matrix[ 2 ][ i ] = FLOAT_TO_GEOMV( z / norm ); + } +} + + +// re-orthonormalize upper left 3x3 submatrix --------------------------------- +// +void ReOrthoNormMtx( Xmatrx matrix ) +{ + ReOrthoMtx( matrix ); + NormMtx( matrix ); +} + + +// create identity matrix ----------------------------------------------------- +// +void MakeIdMatrx( Xmatrx matrx ) +{ + matrx[ 0 ][ 0 ] = GEOMV_1; + matrx[ 0 ][ 1 ] = GEOMV_0; + matrx[ 0 ][ 2 ] = GEOMV_0; + matrx[ 0 ][ 3 ] = GEOMV_0; + matrx[ 1 ][ 0 ] = GEOMV_0; + matrx[ 1 ][ 1 ] = GEOMV_1; + matrx[ 1 ][ 2 ] = GEOMV_0; + matrx[ 1 ][ 3 ] = GEOMV_0; + matrx[ 2 ][ 0 ] = GEOMV_0; + matrx[ 2 ][ 1 ] = GEOMV_0; + matrx[ 2 ][ 2 ] = GEOMV_1; + matrx[ 2 ][ 3 ] = GEOMV_0; +} + + +// create matrix with translation vector (0,0,0) ------------------------------ +// +void MakeNonTranslationMatrx( const Xmatrx matrx, Xmatrx dmatrx ) +{ + dmatrx[ 0 ][ 0 ] = matrx[ 0 ][ 0 ]; + dmatrx[ 0 ][ 1 ] = matrx[ 0 ][ 1 ]; + dmatrx[ 0 ][ 2 ] = matrx[ 0 ][ 2 ]; + dmatrx[ 0 ][ 3 ] = GEOMV_0; + dmatrx[ 1 ][ 0 ] = matrx[ 1 ][ 0 ]; + dmatrx[ 1 ][ 1 ] = matrx[ 1 ][ 1 ]; + dmatrx[ 1 ][ 2 ] = matrx[ 1 ][ 2 ]; + dmatrx[ 1 ][ 3 ] = GEOMV_0; + dmatrx[ 2 ][ 0 ] = matrx[ 2 ][ 0 ]; + dmatrx[ 2 ][ 1 ] = matrx[ 2 ][ 1 ]; + dmatrx[ 2 ][ 2 ] = matrx[ 2 ][ 2 ]; + dmatrx[ 2 ][ 3 ] = GEOMV_0; +} + + +// calculate the inverse of an orthogonal matrix ------------------------------ +// +void CalcOrthoInverse( const Xmatrx matrx, Xmatrx dmatrx ) +{ + //NOTE: + // assumes matrx is an T*R matrix, where + // T is translation only and R is orthogonal. + + Vector3 tvec; + Vector3 dvec; + + // transpose 3x3 submatrix + dmatrx[ 0 ][ 0 ] = matrx[ 0 ][ 0 ]; + dmatrx[ 0 ][ 1 ] = matrx[ 1 ][ 0 ]; + dmatrx[ 0 ][ 2 ] = matrx[ 2 ][ 0 ]; +// dmatrx[ 0 ][ 3 ] = GEOMV_0; // implicit + dmatrx[ 1 ][ 0 ] = matrx[ 0 ][ 1 ]; + dmatrx[ 1 ][ 1 ] = matrx[ 1 ][ 1 ]; + dmatrx[ 1 ][ 2 ] = matrx[ 2 ][ 1 ]; +// dmatrx[ 1 ][ 3 ] = GEOMV_0; // implicit + dmatrx[ 2 ][ 0 ] = matrx[ 0 ][ 2 ]; + dmatrx[ 2 ][ 1 ] = matrx[ 1 ][ 2 ]; + dmatrx[ 2 ][ 2 ] = matrx[ 2 ][ 2 ]; +// dmatrx[ 2 ][ 3 ] = GEOMV_0; // implicit + + // invert translation vector + tvec.X = -matrx[ 0 ][ 3 ]; + tvec.Y = -matrx[ 1 ][ 3 ]; + tvec.Z = -matrx[ 2 ][ 3 ]; + + // apply transformation to translation vector + MtxVctMULt( dmatrx, &tvec, &dvec ); + + // store result (new translation vector) + dmatrx[ 0 ][ 3 ] = dvec.X; + dmatrx[ 1 ][ 3 ] = dvec.Y; + dmatrx[ 2 ][ 3 ] = dvec.Z; +} + + +// calculate camera origin in object space as defined by CurrentXmatrx -------- +// +void CalcObjSpaceCamera( const GenObject *objectp, Vector3 *cameravec ) +{ + ASSERT( objectp != NULL ); + ASSERT( cameravec != NULL ); + + //NOTE: + // assumes CurrentXmatrx is an T*R matrix, where + // T is translation only and R is orthogonal. + + //NOTE: + // the calculation of the inverse CurrentXmatrx is sped up by + // calculating R^-1 and T^-1 separately, multiplying them afterwards + + Xmatrx inv; + inv[ 0 ][ 0 ] = objectp->CurrentXmatrx[ 0 ][ 0 ]; + inv[ 0 ][ 1 ] = objectp->CurrentXmatrx[ 1 ][ 0 ]; + inv[ 0 ][ 2 ] = objectp->CurrentXmatrx[ 2 ][ 0 ]; +// inv[ 0 ][ 3 ] = GEOMV_0; // implicit + inv[ 1 ][ 0 ] = objectp->CurrentXmatrx[ 0 ][ 1 ]; + inv[ 1 ][ 1 ] = objectp->CurrentXmatrx[ 1 ][ 1 ]; + inv[ 1 ][ 2 ] = objectp->CurrentXmatrx[ 2 ][ 1 ]; +// inv[ 1 ][ 3 ] = GEOMV_0; // implicit + inv[ 2 ][ 0 ] = objectp->CurrentXmatrx[ 0 ][ 2 ]; + inv[ 2 ][ 1 ] = objectp->CurrentXmatrx[ 1 ][ 2 ]; + inv[ 2 ][ 2 ] = objectp->CurrentXmatrx[ 2 ][ 2 ]; +// inv[ 2 ][ 3 ] = GEOMV_0; // implicit + + Vertex3 camobjloc; + camobjloc.X = -objectp->CurrentXmatrx[ 0 ][ 3 ]; + camobjloc.Y = -objectp->CurrentXmatrx[ 1 ][ 3 ]; + camobjloc.Z = -objectp->CurrentXmatrx[ 2 ][ 3 ]; + + MtxVctMULt( inv, &camobjloc, cameravec ); +} + + +// transform volume with transformation matrix -------------------------------- +// +void TransformVolume( const Xmatrx matrx, Plane3 *vol_in, Plane3 *vol_out, dword cullmask ) +{ + ASSERT( vol_in != NULL ); + ASSERT( vol_out != NULL ); + + //NOTE: + // assumes matrx is an T*R matrix, where + // T is translation only and R is orthogonal. + + Vector3 tvec; + tvec.X = matrx[ 0 ][ 3 ]; + tvec.Y = matrx[ 1 ][ 3 ]; + tvec.Z = matrx[ 2 ][ 3 ]; + + // transform all non-masked planes + for ( ; cullmask != 0x00; cullmask >>= 1, vol_in++, vol_out++ ) { + + if ( cullmask & 0x01 ) { + + // transform normal + MtxVctMULt( matrx, PLANE_NORMAL( vol_in ), PLANE_NORMAL( vol_out ) ); + + // transform distance + PLANE_OFFSET( vol_out ) = PLANE_OFFSET( vol_in ) + + DOT_PRODUCT( &tvec, PLANE_NORMAL( vol_out ) ); + } + } +} + + +// transform volume with inverse of transformation matrix --------------------- +// +void BackTransformVolume( const Xmatrx matrx, Plane3 *vol_in, Plane3 *vol_out, dword cullmask ) +{ + ASSERT( vol_in != NULL ); + ASSERT( vol_out != NULL ); + + //NOTE: + // assumes matrx is an T*R matrix, where + // T is translation only and R is orthogonal. + + // transpose R (yields R^-1) + Xmatrx rinv; + rinv[ 0 ][ 0 ] = matrx[ 0 ][ 0 ]; + rinv[ 0 ][ 1 ] = matrx[ 1 ][ 0 ]; + rinv[ 0 ][ 2 ] = matrx[ 2 ][ 0 ]; +// rinv[ 0 ][ 3 ] = GEOMV_0; // implicit + rinv[ 1 ][ 0 ] = matrx[ 0 ][ 1 ]; + rinv[ 1 ][ 1 ] = matrx[ 1 ][ 1 ]; + rinv[ 1 ][ 2 ] = matrx[ 2 ][ 1 ]; +// rinv[ 1 ][ 3 ] = GEOMV_0; // implicit + rinv[ 2 ][ 0 ] = matrx[ 0 ][ 2 ]; + rinv[ 2 ][ 1 ] = matrx[ 1 ][ 2 ]; + rinv[ 2 ][ 2 ] = matrx[ 2 ][ 2 ]; +// rinv[ 2 ][ 3 ] = GEOMV_0; // implicit + + // invert T + Vector3 tvec; + tvec.X = -matrx[ 0 ][ 3 ]; + tvec.Y = -matrx[ 1 ][ 3 ]; + tvec.Z = -matrx[ 2 ][ 3 ]; + + // transform all non-masked planes + for ( ; cullmask != 0x00; cullmask >>= 1, vol_in++, vol_out++ ) { + + if ( cullmask & 0x01 ) { + + // transform distance + PLANE_OFFSET( vol_out ) = PLANE_OFFSET( vol_in ) + + DOT_PRODUCT( &tvec, PLANE_NORMAL( vol_in ) ); + // transform normal + MtxVctMULt( rinv, PLANE_NORMAL( vol_in ), PLANE_NORMAL( vol_out ) ); + } + } +} + + +// check whether general quaternion is unit quaternion ------------------------ +// +int QuaternionIsUnit( const Quaternion *quat ) +{ + ASSERT( quat != NULL ); + + float qlen2 = GEOMV_TO_FLOAT( quat->X ) * GEOMV_TO_FLOAT( quat->X ) + + GEOMV_TO_FLOAT( quat->Y ) * GEOMV_TO_FLOAT( quat->Y ) + + GEOMV_TO_FLOAT( quat->Z ) * GEOMV_TO_FLOAT( quat->Z ) + + GEOMV_TO_FLOAT( quat->W ) * GEOMV_TO_FLOAT( quat->W ); + + return ( ( qlen2 > ( 1.0f - 1e-5f ) ) && ( qlen2 < ( 1.0f + 1e-5f ) ) ); +} + +int QuaternionIsUnit_f( const Quaternion_f *quat ) +{ + ASSERT( quat != NULL ); + + float qlen2 = quat->X * quat->X + quat->Y * quat->Y + + quat->Z * quat->Z + quat->W * quat->W; + + return ( ( qlen2 > ( 1.0f - 1e-5f ) ) && ( qlen2 < ( 1.0f + 1e-5f ) ) ); +} + + +// convert general quaternion into unit quaternion ---------------------------- +// +void QuaternionMakeUnit( Quaternion *quat ) +{ + ASSERT( quat != NULL ); + + float W = GEOMV_TO_FLOAT( quat->W ); + float X = GEOMV_TO_FLOAT( quat->X ); + float Y = GEOMV_TO_FLOAT( quat->Y ); + float Z = GEOMV_TO_FLOAT( quat->Z ); + + float invnorm = 1.0f / ( W*W + X*X + Y*Y + Z*Z ); + invnorm = sqrtf( invnorm ); + + quat->W = FLOAT_TO_GEOMV( W * invnorm ); + quat->X = FLOAT_TO_GEOMV( X * invnorm ); + quat->Y = FLOAT_TO_GEOMV( Y * invnorm ); + quat->Z = FLOAT_TO_GEOMV( Z * invnorm ); +} + +void QuaternionMakeUnit_f( Quaternion_f *quat ) +{ + ASSERT( quat != NULL ); + + float W = quat->W; + float X = quat->X; + float Y = quat->Y; + float Z = quat->Z; + + float invnorm = 1.0f / ( W*W + X*X + Y*Y + Z*Z ); + invnorm = sqrtf( invnorm ); + + quat->W = W * invnorm; + quat->X = X * invnorm; + quat->Y = Y * invnorm; + quat->Z = Z * invnorm; +} + + +// invert a unit quaternion --------------------------------------------------- +// +void QuaternionInvertUnit( Quaternion *quat ) +{ + ASSERT( quat != NULL ); + + quat->X = -quat->X; + quat->Y = -quat->Y; + quat->Z = -quat->Z; +} + +void QuaternionInvertUnit_f( Quaternion_f *quat ) +{ + ASSERT( quat != NULL ); + + quat->X = -quat->X; + quat->Y = -quat->Y; + quat->Z = -quat->Z; +} + + +// invert a general quaternion ------------------------------------------------ +// +void QuaternionInvertGeneral( Quaternion *quat ) +{ + ASSERT( quat != NULL ); + + float W = GEOMV_TO_FLOAT( quat->W ); + float X = GEOMV_TO_FLOAT( quat->X ); + float Y = GEOMV_TO_FLOAT( quat->Y ); + float Z = GEOMV_TO_FLOAT( quat->Z ); + + float invmag = 1.0f / ( W*W + X*X + Y*Y + Z*Z ); + + quat->W = FLOAT_TO_GEOMV( -W * invmag ); + quat->X = FLOAT_TO_GEOMV( -X * invmag ); + quat->Y = FLOAT_TO_GEOMV( -Y * invmag ); + quat->Z = FLOAT_TO_GEOMV( -Z * invmag ); +} + +void QuaternionInvertGeneral_f( Quaternion_f *quat ) +{ + ASSERT( quat != NULL ); + + float W = quat->W; + float X = quat->X; + float Y = quat->Y; + float Z = quat->Z; + + float invmag = 1.0f / ( W*W + X*X + Y*Y + Z*Z ); + + quat->W = -W * invmag; + quat->X = -X * invmag; + quat->Y = -Y * invmag; + quat->Z = -Z * invmag; +} + + +// diagonal forward cycling --------------------------------------------------- +// +static int nxt_wrp[ 3 ] = { 1, 2, 0 }; + + +// create a unit quaternion from a rotation matrix ---------------------------- +// +void QuaternionFromMatrx( Quaternion *quat, const Xmatrx matrix ) +{ + ASSERT( quat != NULL ); + ASSERT( matrix != NULL ); + + float fdiag[ 3 ]; + fdiag[ 0 ] = GEOMV_TO_FLOAT( matrix[ 0 ][ 0 ] ); + fdiag[ 1 ] = GEOMV_TO_FLOAT( matrix[ 1 ][ 1 ] ); + fdiag[ 2 ] = GEOMV_TO_FLOAT( matrix[ 2 ][ 2 ] ); + + // temp result + float qt[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // (W,X,Y,Z) + + float trace = fdiag[ 0 ] + fdiag[ 1 ] + fdiag[ 2 ]; + if ( trace > 0.0 ) { + + float scale = sqrtf( trace + 1.0f ); // 2W=sqrt(4W^2) + qt[ 0 ] = scale * 0.5f; // W + scale = 0.5f / scale; // 1/(4W) + + qt[ 1 ] = ( GEOMV_TO_FLOAT( matrix[ 2 ][ 1 ] ) - GEOMV_TO_FLOAT( matrix[ 1 ][ 2 ] ) ) * scale; + qt[ 2 ] = ( GEOMV_TO_FLOAT( matrix[ 0 ][ 2 ] ) - GEOMV_TO_FLOAT( matrix[ 2 ][ 0 ] ) ) * scale; + qt[ 3 ] = ( GEOMV_TO_FLOAT( matrix[ 1 ][ 0 ] ) - GEOMV_TO_FLOAT( matrix[ 0 ][ 1 ] ) ) * scale; + + } else { + + int mxi = 0; + if ( fdiag[ 1 ] > fdiag[ 0 ] ) + mxi = 1; + if ( fdiag[ 2 ] > fdiag[ mxi ] ) + mxi = 2; + + int nx1 = nxt_wrp[ mxi ]; + int nx2 = nxt_wrp[ nx1 ]; + + float scale = sqrtf( fdiag[ mxi ] - ( fdiag[ nx1 ] + fdiag[ nx2 ] ) + 1.0f ); + qt[ mxi+1 ] = scale * 0.5f; + scale = 0.5f / scale; + + qt[ 0 ] = ( GEOMV_TO_FLOAT( matrix[ nx2 ][ nx1 ] ) - GEOMV_TO_FLOAT( matrix[ nx1 ][ nx2 ] ) ) * scale; + qt[ nx1+1 ] = ( GEOMV_TO_FLOAT( matrix[ nx1 ][ mxi ] ) + GEOMV_TO_FLOAT( matrix[ mxi ][ nx1 ] ) ) * scale; + qt[ nx2+1 ] = ( GEOMV_TO_FLOAT( matrix[ nx2 ][ mxi ] ) + GEOMV_TO_FLOAT( matrix[ mxi ][ nx2 ] ) ) * scale; + } + + quat->W = FLOAT_TO_GEOMV( qt[ 0 ] ); + quat->X = FLOAT_TO_GEOMV( qt[ 1 ] ); + quat->Y = FLOAT_TO_GEOMV( qt[ 2 ] ); + quat->Z = FLOAT_TO_GEOMV( qt[ 3 ] ); +} + +void QuaternionFromMatrx_f( Quaternion_f *quat, const Xmatrx matrix ) +{ + ASSERT( quat != NULL ); + ASSERT( matrix != NULL ); + + float fdiag[ 3 ]; + fdiag[ 0 ] = GEOMV_TO_FLOAT( matrix[ 0 ][ 0 ] ); + fdiag[ 1 ] = GEOMV_TO_FLOAT( matrix[ 1 ][ 1 ] ); + fdiag[ 2 ] = GEOMV_TO_FLOAT( matrix[ 2 ][ 2 ] ); + + // direct result + float *qt = (float *) quat; // (W,X,Y,Z) + + float trace = fdiag[ 0 ] + fdiag[ 1 ] + fdiag[ 2 ]; + if ( trace > 0.0 ) { + + float scale = sqrtf( trace + 1.0f ); // 2W=sqrt(4W^2) + qt[ 0 ] = scale * 0.5f; // W + scale = 0.5f / scale; // 1/(4W) + + qt[ 1 ] = ( GEOMV_TO_FLOAT( matrix[ 2 ][ 1 ] ) - GEOMV_TO_FLOAT( matrix[ 1 ][ 2 ] ) ) * scale; + qt[ 2 ] = ( GEOMV_TO_FLOAT( matrix[ 0 ][ 2 ] ) - GEOMV_TO_FLOAT( matrix[ 2 ][ 0 ] ) ) * scale; + qt[ 3 ] = ( GEOMV_TO_FLOAT( matrix[ 1 ][ 0 ] ) - GEOMV_TO_FLOAT( matrix[ 0 ][ 1 ] ) ) * scale; + + } else { + + int mxi = 0; + if ( fdiag[ 1 ] > fdiag[ 0 ] ) + mxi = 1; + if ( fdiag[ 2 ] > fdiag[ mxi ] ) + mxi = 2; + + int nx1 = nxt_wrp[ mxi ]; + int nx2 = nxt_wrp[ nx1 ]; + + float scale = sqrtf( fdiag[ mxi ] - ( fdiag[ nx1 ] + fdiag[ nx2 ] ) + 1.0f ); + qt[ mxi+1 ] = scale * 0.5f; + scale = 0.5f / scale; + + qt[ 0 ] = ( GEOMV_TO_FLOAT( matrix[ nx2 ][ nx1 ] ) - GEOMV_TO_FLOAT( matrix[ nx1 ][ nx2 ] ) ) * scale; + qt[ nx1+1 ] = ( GEOMV_TO_FLOAT( matrix[ nx1 ][ mxi ] ) + GEOMV_TO_FLOAT( matrix[ mxi ][ nx1 ] ) ) * scale; + qt[ nx2+1 ] = ( GEOMV_TO_FLOAT( matrix[ nx2 ][ mxi ] ) + GEOMV_TO_FLOAT( matrix[ mxi ][ nx2 ] ) ) * scale; + } +} + + +// create a rotation matrix from a unit quaternion ---------------------------- +// +void MatrxFromQuaternion( Xmatrx matrix, const Quaternion *quat ) +{ + ASSERT( matrix != NULL ); + ASSERT( quat != NULL ); + + //NOTE: + // the translation in the matrix will be + // left untouched. + + //NOTE: + // unit quaternion check is only assertion. + + ASSERT( QuaternionIsUnit( quat ) ); + + // calculate intermediate terms + geomv_t X2 = quat->X * 2; + geomv_t Y2 = quat->Y * 2; + geomv_t Z2 = quat->Z * 2; + + geomv_t XX = GEOMV_MUL( quat->X, X2 ); + geomv_t YY = GEOMV_MUL( quat->Y, Y2 ); + geomv_t ZZ = GEOMV_MUL( quat->Z, Z2 ); + geomv_t XY = GEOMV_MUL( quat->X, Y2 ); + geomv_t XZ = GEOMV_MUL( quat->X, Z2 ); + geomv_t YZ = GEOMV_MUL( quat->Y, Z2 ); + geomv_t WX = GEOMV_MUL( quat->W, X2 ); + geomv_t WY = GEOMV_MUL( quat->W, Y2 ); + geomv_t WZ = GEOMV_MUL( quat->W, Z2 ); + + // convert quaternion into rotation matrix + matrix[ 0 ][ 0 ] = GEOMV_1 - ( YY + ZZ ); + matrix[ 0 ][ 1 ] = ( XY - WZ ); + matrix[ 0 ][ 2 ] = ( XZ + WY ); +// matrix[ 0 ][ 3 ] = GEOMV_0; + matrix[ 1 ][ 0 ] = ( XY + WZ ); + matrix[ 1 ][ 1 ] = GEOMV_1 - ( XX + ZZ ); + matrix[ 1 ][ 2 ] = ( YZ - WX ); +// matrix[ 1 ][ 3 ] = GEOMV_0; + matrix[ 2 ][ 0 ] = ( XZ - WY ); + matrix[ 2 ][ 1 ] = ( YZ + WX ); + matrix[ 2 ][ 2 ] = GEOMV_1 - ( XX + YY ); +// matrix[ 2 ][ 3 ] = GEOMV_0; +} + +void MatrxFromQuaternion_f( Xmatrx matrix, const Quaternion_f *quat ) +{ + ASSERT( matrix != NULL ); + ASSERT( quat != NULL ); + + //NOTE: + // the translation in the matrix will be + // left untouched. + + //NOTE: + // unit quaternion check is only assertion. + + ASSERT( QuaternionIsUnit_f( quat ) ); + + // calculate intermediate terms + float X2 = quat->X * 2; + float Y2 = quat->Y * 2; + float Z2 = quat->Z * 2; + + float XX = quat->X * X2; + float YY = quat->Y * Y2; + float ZZ = quat->Z * Z2; + float XY = quat->X * Y2; + float XZ = quat->X * Z2; + float YZ = quat->Y * Z2; + float WX = quat->W * X2; + float WY = quat->W * Y2; + float WZ = quat->W * Z2; + + // convert quaternion into rotation matrix + matrix[ 0 ][ 0 ] = FLOAT_TO_GEOMV( 1 - ( YY + ZZ ) ); + matrix[ 0 ][ 1 ] = FLOAT_TO_GEOMV( ( XY - WZ ) ); + matrix[ 0 ][ 2 ] = FLOAT_TO_GEOMV( ( XZ + WY ) ); +// matrix[ 0 ][ 3 ] = GEOMV_0; + matrix[ 1 ][ 0 ] = FLOAT_TO_GEOMV( ( XY + WZ ) ); + matrix[ 1 ][ 1 ] = FLOAT_TO_GEOMV( 1 - ( XX + ZZ ) ); + matrix[ 1 ][ 2 ] = FLOAT_TO_GEOMV( ( YZ - WX ) ); +// matrix[ 1 ][ 3 ] = GEOMV_0; + matrix[ 2 ][ 0 ] = FLOAT_TO_GEOMV( ( XZ - WY ) ); + matrix[ 2 ][ 1 ] = FLOAT_TO_GEOMV( ( YZ + WX ) ); + matrix[ 2 ][ 2 ] = FLOAT_TO_GEOMV( 1 - ( XX + YY ) ); +// matrix[ 2 ][ 3 ] = GEOMV_0; +} + + +// create a rotation matrix from an angular displacement (angle, axis) -------- +// +void MatrxFromAngularDisplacement( Xmatrx matrix, bams_t angle, Vertex3 *axis ) +{ + ASSERT( matrix != NULL ); + ASSERT( axis != NULL ); + + //NOTE: + // NULL axes are not handled transparently. + + // normalize axis of rotation + NormVctX( axis ); + + // calculate unit quaternion corresponding to rotation + float phi2 = -BAMS_TO_RAD( angle ) / 2; + float sinphi2 = sinf( phi2 ); + + Quaternion_f quat; + quat.W = cosf( phi2 ); + quat.X = sinphi2 * GEOMV_TO_FLOAT( axis->X ); + quat.Y = sinphi2 * GEOMV_TO_FLOAT( axis->Y ); + quat.Z = sinphi2 * GEOMV_TO_FLOAT( axis->Z ); + + // calculate intermediate terms + float X2 = quat.X * quat.X; + float Y2 = quat.Y * quat.Y; + float Z2 = quat.Z * quat.Z; + float XY = quat.X * quat.Y; + float XZ = quat.X * quat.Z; + float YZ = quat.Y * quat.Z; + float WX = quat.W * quat.X; + float WY = quat.W * quat.Y; + float WZ = quat.W * quat.Z; + + // convert quaternion into rotation matrix + matrix[ 0 ][ 0 ] = FLOAT_TO_GEOMV( 1 - ( Y2 + Z2 ) * 2 ); + matrix[ 0 ][ 1 ] = FLOAT_TO_GEOMV( ( XY - WZ ) * 2 ); + matrix[ 0 ][ 2 ] = FLOAT_TO_GEOMV( ( XZ + WY ) * 2 ); + matrix[ 0 ][ 3 ] = GEOMV_0; + matrix[ 1 ][ 0 ] = FLOAT_TO_GEOMV( ( XY + WZ ) * 2 ); + matrix[ 1 ][ 1 ] = FLOAT_TO_GEOMV( 1 - ( X2 + Z2 ) * 2 ); + matrix[ 1 ][ 2 ] = FLOAT_TO_GEOMV( ( YZ - WX ) * 2 ); + matrix[ 1 ][ 3 ] = GEOMV_0; + matrix[ 2 ][ 0 ] = FLOAT_TO_GEOMV( ( XZ - WY ) * 2 ); + matrix[ 2 ][ 1 ] = FLOAT_TO_GEOMV( ( YZ + WX ) * 2 ); + matrix[ 2 ][ 2 ] = FLOAT_TO_GEOMV( 1 - ( X2 + Y2 ) * 2 ); + matrix[ 2 ][ 3 ] = GEOMV_0; +} + + +// multiply two general quaternions ------------------------------------------- +// +void QuaternionMUL( Quaternion *qd, const Quaternion *qb, const Quaternion *qa ) +{ + ASSERT( qd != NULL ); + ASSERT( qb != NULL ); + ASSERT( qa != NULL ); + + //NOTE: + // QD = QB*QA; + + float BW = GEOMV_TO_FLOAT( qb->W ), AW = GEOMV_TO_FLOAT( qa->W ); + float BX = GEOMV_TO_FLOAT( qb->X ), AX = GEOMV_TO_FLOAT( qa->X ); + float BY = GEOMV_TO_FLOAT( qb->Y ), AY = GEOMV_TO_FLOAT( qa->Y ); + float BZ = GEOMV_TO_FLOAT( qb->Z ), AZ = GEOMV_TO_FLOAT( qa->Z ); + + qd->W = FLOAT_TO_GEOMV( BW*AW - BX*AX - BY*AY - BZ*AZ ); + qd->X = FLOAT_TO_GEOMV( BY*AZ - AY*BZ + BW*AX + AW*BX ); + qd->Y = FLOAT_TO_GEOMV( BZ*AX - AZ*BX + BW*AY + AW*BY ); + qd->Z = FLOAT_TO_GEOMV( BX*AY - AX*BY + BW*AZ + AW*BZ ); +} + +void QuaternionMUL_f( Quaternion_f *qd, const Quaternion_f *qb, const Quaternion_f *qa ) +{ + ASSERT( qd != NULL ); + ASSERT( qb != NULL ); + ASSERT( qa != NULL ); + + //NOTE: + // QD = QB*QA; + + float BW = qb->W, AW = qa->W; + float BX = qb->X, AX = qa->X; + float BY = qb->Y, AY = qa->Y; + float BZ = qb->Z, AZ = qa->Z; + + qd->W = BW*AW - BX*AX - BY*AY - BZ*AZ; + qd->X = BY*AZ - AY*BZ + BW*AX + AW*BX; + qd->Y = BZ*AX - AZ*BX + BW*AY + AW*BY; + qd->Z = BX*AY - AX*BY + BW*AZ + AW*BZ; +} + + +// flags and bounds for quaternion slerp -------------------------------------- +// +#define SLERP_ALWAYS_SHORTEST + +#define EPS_COSOM 1e-5 + + +// interpolate spherically between two unit quaternions ----------------------- +// +void QuaternionSlerp( Quaternion *qd, const Quaternion *qa, const Quaternion *qb, float alpha ) +{ + ASSERT( qd != NULL ); + ASSERT( qa != NULL ); + ASSERT( qb != NULL ); + + //NOTE: + // QD = SLERP(QA,QB,ALPHA); + // ( = QA->[ALPHA*ANGLE]->QB ) ) + + float scalea; + float scaleb; + + float AW = GEOMV_TO_FLOAT( qa->W ), BW = GEOMV_TO_FLOAT( qb->W ); + float AX = GEOMV_TO_FLOAT( qa->X ), BX = GEOMV_TO_FLOAT( qb->X ); + float AY = GEOMV_TO_FLOAT( qa->Y ), BY = GEOMV_TO_FLOAT( qb->Y ); + float AZ = GEOMV_TO_FLOAT( qa->Z ), BZ = GEOMV_TO_FLOAT( qb->Z ); + + float cosom = AX * BX + AY * BY + AZ * BZ + AW * BW; + +#ifdef SLERP_ALWAYS_SHORTEST + + // resolve ambiguity + if ( cosom < 0.0f ) { + cosom = -cosom; + BW = -BW; + BX = -BX; + BY = -BY; + BZ = -BZ; + } + + if ( ( 1.0f - cosom ) > EPS_COSOM ) { + + // full slerp + float omega = acosf( cosom ); + float sinom = 1.0f / sinf( omega ); + + alpha *= omega; + scalea = sinf( omega - alpha ) * sinom; + scaleb = sinf( alpha ) * sinom; + + } else { + + // use lerp if cos(omega) about 1.0 + scalea = 1.0f - alpha; + scaleb = alpha; + } + + qd->W = FLOAT_TO_GEOMV( scalea * AW + scaleb * BW ); + qd->X = FLOAT_TO_GEOMV( scalea * AX + scaleb * BX ); + qd->Y = FLOAT_TO_GEOMV( scalea * AY + scaleb * BY ); + qd->Z = FLOAT_TO_GEOMV( scalea * AZ + scaleb * BZ ); + +#else // SLERP_ALWAYS_SHORTEST + + if ( ( 1.0f + cosom ) > EPS_COSOM ) { + + if ( ( 1.0f - cosom ) > EPS_COSOM ) { + + // full slerp + float omega = acos( cosom ); + float sinom = 1.0f / sin( omega ); + + alpha *= omega; + scalea = sin( omega - alpha ) * sinom; + scaleb = sin( alpha ) * sinom; + + } else { + + // use lerp if cos(omega) about 1.0 + scalea = 1.0f - alpha; + scaleb = alpha; + } + + qd->W = FLOAT_TO_GEOMV( scalea * AW + scaleb * BW ); + qd->X = FLOAT_TO_GEOMV( scalea * AX + scaleb * BX ); + qd->Y = FLOAT_TO_GEOMV( scalea * AY + scaleb * BY ); + qd->Z = FLOAT_TO_GEOMV( scalea * AZ + scaleb * BZ ); + + } else { + + // handle diametrically opposite quaternions + qd->W = qa->Z; + qd->X = -qa->Y; + qd->Y = qa->X; + qd->Z = -qa->W; + + alpha *= HPREC_HALF_PI; + scalea = sin( HPREC_HALF_PI - alpha ); + scaleb = sin( alpha ); + + qd->X = FLOAT_TO_GEOMV( scalea * AX + scaleb * GEOMV_TO_FLOAT( qd->X ) ); + qd->Y = FLOAT_TO_GEOMV( scalea * AY + scaleb * GEOMV_TO_FLOAT( qd->Y ) ); + qd->Z = FLOAT_TO_GEOMV( scalea * AZ + scaleb * GEOMV_TO_FLOAT( qd->Z ) ); + } + +#endif // SLERP_ALWAYS_SHORTEST + +} + +void QuaternionSlerp_f( Quaternion_f *qd, const Quaternion_f *qa, const Quaternion_f *qb, float alpha ) +{ + ASSERT( qd != NULL ); + ASSERT( qa != NULL ); + ASSERT( qb != NULL ); + + //NOTE: + // QD = SLERP(QA,QB,ALPHA); + // ( = QA->[ALPHA*ANGLE]->QB ) ) + + float scalea; + float scaleb; + + float BW = qb->W; + float BX = qb->X; + float BY = qb->Y; + float BZ = qb->Z; + + float cosom = qa->X * BX + qa->Y * BY + qa->Z * BZ + qa->W * BW; + +#ifdef SLERP_ALWAYS_SHORTEST + + // resolve ambiguity + if ( cosom < 0.0f ) { + cosom = -cosom; + BW = -BW; + BX = -BX; + BY = -BY; + BZ = -BZ; + } + + if ( ( 1.0f - cosom ) > EPS_COSOM ) { + + // full slerp + float omega = acosf( cosom ); + float sinom = 1.0f / sinf( omega ); + + alpha *= omega; + scalea = sinf( omega - alpha ) * sinom; + scaleb = sinf( alpha ) * sinom; + + } else { + + // use lerp if cos(omega) about 1.0 + scalea = 1.0f - alpha; + scaleb = alpha; + } + + qd->W = scalea * qa->W + scaleb * BW; + qd->X = scalea * qa->X + scaleb * BX; + qd->Y = scalea * qa->Y + scaleb * BY; + qd->Z = scalea * qa->Z + scaleb * BZ; + +#else // SLERP_ALWAYS_SHORTEST + + if ( ( 1.0f + cosom ) > EPS_COSOM ) { + + if ( ( 1.0f - cosom ) > EPS_COSOM ) { + + // full slerp + float omega = acos( cosom ); + float sinom = 1.0f / sin( omega ); + + alpha *= omega; + scalea = sin( omega - alpha ) * sinom; + scaleb = sin( alpha ) * sinom; + + } else { + + // use lerp if cos(omega) about 1.0 + scalea = 1.0f - alpha; + scaleb = alpha; + } + + qd->W = scalea * qa->W + scaleb * BW; + qd->X = scalea * qa->X + scaleb * BX; + qd->Y = scalea * qa->Y + scaleb * BY; + qd->Z = scalea * qa->Z + scaleb * BZ; + + } else { + + // handle diametrically opposite quaternions + qd->W = qa->Z; + qd->X = -qa->Y; + qd->Y = qa->X; + qd->Z = -qa->W; + + alpha *= HPREC_HALF_PI; + scalea = sin( HPREC_HALF_PI - alpha ); + scaleb = sin( alpha ); + + qd->X = scalea * qa->X + scaleb * qd->X; + qd->Y = scalea * qa->Y + scaleb * qd->Y; + qd->Z = scalea * qa->Z + scaleb * qd->Z; + } + +#endif // SLERP_ALWAYS_SHORTEST + +} + +// do a quaternion slerp from frame to frame ---------------------------------- +// +void QuaternionSlerpFrames( Xmatrx slerp_frame, Xmatrx start_frame, Xmatrx end_frame, float t ) +{ + // get source orientation quaternion + Quaternion srcquat; + QuaternionFromMatrx( &srcquat, start_frame ); + QuaternionMakeUnit( &srcquat ); + + // get destination orientation quaternion + Quaternion dstquat; + QuaternionFromMatrx( &dstquat, end_frame ); + QuaternionMakeUnit( &dstquat ); + + // do slerp from src to dst (filter output this frame) + Quaternion slerpquat; + QuaternionSlerp( &slerpquat, &srcquat, &dstquat, t ); + QuaternionMakeUnit( &slerpquat ); + + // fill R part of view camera matrix (filtered orientation this frame) + MatrxFromQuaternion( slerp_frame, &slerpquat ); +} + +// calc rotation matrix as result of slerp between two quaternions ------------ +// +void CalcSlerpedMatrix( Xmatrx matrix, const playerlerp_s *playerlerp ) +{ + ASSERT( matrix != NULL ); + ASSERT( playerlerp != NULL ); + + // do slerp from src to dst + Quaternion slerpquat; + QuaternionSlerp( &slerpquat, &playerlerp->srcquat, &playerlerp->dstquat, playerlerp->curalpha ); + QuaternionMakeUnit( &slerpquat ); + + // fill R part of matrix + MatrxFromQuaternion( matrix, &slerpquat ); +} + + +// interpolate using the Hermite Interpolation -------------------------------- +// +void Hermite_Interpolate( Vector3* dest, float t, const Vector3* start, const Vector3* end, + const Vector3* start_tan, const Vector3* end_tan ) +{ + // do a Hermite interpolation for the position + float s = t; + float s2 = s * s; + float s3 = s2 * s; + + // calculate the basis functions + float h1 = 2 * s3 - 3 * s2 + 1; + float h2 = -2 * s3 + 3 * s2; + float h3 = s3 - 2 * s2 + s; + float h4 = s3 - s2; + + Vector3 h1P1, h2P2, h3T1, h4T2; + + VECMULS( &h1P1, start, (geomv_t)h1 ); + VECMULS( &h2P2, end, (geomv_t)h2 ); + VECMULS( &h3T1, start_tan, (geomv_t)h3 ); + VECMULS( &h4T2, end_tan, (geomv_t)h4 ); + + Vector3 hermite_pos; + VECADD( &hermite_pos, &h1P1, &h2P2 ); + VECADD( &hermite_pos, &h3T1, &hermite_pos ); + VECADD( &hermite_pos, &h4T2, &hermite_pos ); + + memcpy( dest, &hermite_pos, sizeof( Vector3 ) ); +} + + +// init a the Hermite ArcLen interpolation struct ----------------------------- +// +Hermite_ArcLen* Hermite_ArcLen_InitData( int num_steps, const Vector3* start, const Vector3* end, + const Vector3* start_tan, const Vector3* end_tan ) +{ + Hermite_ArcLen* hermite_data = (Hermite_ArcLen*)ALLOCMEM( sizeof( Hermite_ArcLen ) ); + + // init the arrays + size_t allocsize1 = (size_t)num_steps * sizeof( float ); + size_t allocsize2 = (size_t)num_steps * sizeof( Vector3 ); + hermite_data->table_u = (float*)ALLOCMEM( allocsize1 ); + hermite_data->table_s = (float*)ALLOCMEM( allocsize1 ); + hermite_data->table_Vecs = (Vector3*)ALLOCMEM( allocsize2 ); + + memset( hermite_data->table_u, 0, allocsize1 ); + memset( hermite_data->table_s, 0, allocsize1 ); + memset( hermite_data->table_Vecs, 0, allocsize2 ); + + //NOTE: u is original parameter space, s is arclen parameter space + + // store the # of steps for the interpolation + hermite_data->num_steps = num_steps; + + // store u and corresponding value + int step = 0; + for(step = 0; step < num_steps; step++ ) { + + // normalize u + float u = (float)step / (float)( num_steps - 1 ); + + hermite_data->table_u[ step ] = u; + + Hermite_Interpolate( &hermite_data->table_Vecs[ step ], u, start, end, start_tan, end_tan ); + } + + // calculate total arclength + hermite_data->total_arc_len = 0.0f; + + // store arclen for interpolation step s == 0 + hermite_data->table_s[ 0 ] = 0.0f; + + Vector3 diff; + for( step = 1; step < num_steps; step++ ) { + + // get the (approximated) arclen of vector ( step - 1 ) -> ( step ) + VECSUB( &diff, &hermite_data->table_Vecs[ step - 1 ], &hermite_data->table_Vecs[ step ] ); + float arc_len = GEOMV_TO_FLOAT( VctLenX( &diff ) ); + hermite_data->total_arc_len += arc_len; + + // store accumulated arclen in table_s + hermite_data->table_s[ step ] = hermite_data->total_arc_len; + } + + // normalize arc lens + for( step = 0; step < num_steps; step++ ) { + hermite_data->table_s[ step ] = hermite_data->table_s[ step ] / hermite_data->total_arc_len; + } + + return hermite_data; +} + +// kill data needed for the Hermite ArcLen interpolation ---------------------- +// +void Hermite_ArcLen_KillData( Hermite_ArcLen* hermite_data ) +{ + ASSERT( hermite_data != NULL ); + ASSERT( hermite_data->table_u != NULL ); + ASSERT( hermite_data->table_s != NULL ); + ASSERT( hermite_data->table_Vecs != NULL ); + + FREEMEM( hermite_data->table_u ); + FREEMEM( hermite_data->table_s ); + FREEMEM( hermite_data->table_Vecs ); +} + +// do arclen interpolation of the supplied hermite arclen data ---------------- +// +void Hermite_ArcLen_Interpolate( const Hermite_ArcLen* hermite_data, float s, Vector3* interp ) +{ + ASSERT( hermite_data != NULL ); + ASSERT( ( s >= 0 ) && ( s <= 1.0f ) ); + ASSERT( interp != NULL ); + + //NOTE: given a parameter s in arclen [0,1] we find an u in original parameter space + + ASSERT( hermite_data->num_steps >= 2 ); + + int left = 0; + int right = ( hermite_data->num_steps - 1 ); + + float* table_s = hermite_data->table_s; + + int start = 0, end = 0; + + for( ; right >= left; ) { + int x = ( left + right ) / 2; + + if( table_s[ x ] == s ) { + memcpy( interp, &hermite_data->table_Vecs[ x ], sizeof( Vector3 ) ); + return; + } else if ( s < table_s[ x ] ) { + if ( s > table_s[ x - 1 ] ) { + start = x - 1; + end = x; + break; + } else { + right = x - 1; + } + } else { + if ( s < table_s[ x + 1 ] ) { + start = x; + end = x + 1; + break; + } else { + left = x + 1; + } + } + } + + Vector3* start_vec = &hermite_data->table_Vecs[ start ]; + Vector3* end_vec = &hermite_data->table_Vecs[ end ]; + + float alpha = ( s - table_s[ start ] ) / ( table_s[ end ] - table_s[ start ] ); + + interp->X = start_vec->X + GEOMV_TO_FLOAT( end_vec->X - start_vec->X ) * FLOAT_TO_GEOMV( alpha ); + interp->Y = start_vec->Y + GEOMV_TO_FLOAT( end_vec->Y - start_vec->Y ) * FLOAT_TO_GEOMV( alpha ); + interp->Z = start_vec->Z + GEOMV_TO_FLOAT( end_vec->Z - start_vec->Z ) * FLOAT_TO_GEOMV( alpha ); +} + + +// dump the contents of a matrix ---------------------------------------------- +// +void DumpMatrix( Xmatrx mat ) +{ + MSGOUT( "%10.4f %10.4f %10.4f %10.4f | %10.4f %10.4f %10.4f %10.4f | %10.4f %10.4f %10.4f %10.4f", + mat[ 0 ][ 0 ], mat[ 0 ][ 1 ], mat[ 0 ][ 2 ], mat[ 0 ][ 3 ], + mat[ 1 ][ 0 ], mat[ 1 ][ 1 ], mat[ 1 ][ 2 ], mat[ 1 ][ 3 ], + mat[ 2 ][ 0 ], mat[ 2 ][ 1 ], mat[ 2 ][ 2 ], mat[ 2 ][ 3 ] ); +} + + |