From 1192c4575ea7afdf38e3fa0df4587a08620af11e Mon Sep 17 00:00:00 2001 From: root Date: Tue, 21 Jul 2020 10:00:46 +0200 Subject: SZS v2.18a --- project/dclib/dclib-vector.c | 2582 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 2582 insertions(+) create mode 100644 project/dclib/dclib-vector.c (limited to 'project/dclib/dclib-vector.c') diff --git a/project/dclib/dclib-vector.c b/project/dclib/dclib-vector.c new file mode 100644 index 0000000..070d719 --- /dev/null +++ b/project/dclib/dclib-vector.c @@ -0,0 +1,2582 @@ + +/*************************************************************************** + * * + * _____ ____ * + * | __ \ / __ \ _ _ _____ * + * | | \ \ / / \_\ | | | | _ \ * + * | | \ \| | | | | | |_| | * + * | | | || | | | | | ___/ * + * | | / /| | __ | | | | _ \ * + * | |__/ / \ \__/ / | |___| | |_| | * + * |_____/ \____/ |_____|_|_____/ * + * * + * Wiimms source code library * + * * + *************************************************************************** + * * + * Copyright (c) 2012-2020 by Dirk Clemens * + * * + *************************************************************************** + * * + * 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. * + * * + * See file gpl-2.0.txt or http://www.gnu.org/licenses/gpl-2.0.txt * + * * + ***************************************************************************/ + +#define _GNU_SOURCE 1 + +#include + +#include "dclib-basics.h" +#include "dclib-numeric.h" +#include "dclib-debug.h" + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// float3 list support /////////////// +/////////////////////////////////////////////////////////////////////////////// + +void InitializeF3L +( + float3List_t *f3l, // valid data + uint bin_search // >0: enable binary search + // and allocate 'bin_search' elements +) +{ + DASSERT(f3l); + memset(f3l,0,sizeof(*f3l)); + if (bin_search) + { + f3l->size = bin_search; + f3l->list = MALLOC( f3l->size * sizeof(*f3l->list) ); + f3l->sort = MALLOC( f3l->size * sizeof(*f3l->sort) ); + } +} + +/////////////////////////////////////////////////////////////////////////////// + +void ResetF3L ( float3List_t * f3l ) +{ + DASSERT(f3l); + FREE(f3l->list); + FREE(f3l->sort); + memset(f3l,0,sizeof(*f3l)); +} + +/////////////////////////////////////////////////////////////////////////////// + +float3 * AppendF3L ( float3List_t * f3l ) +{ + DASSERT( f3l ); + DASSERT( f3l->used <= f3l->size ); + DASSERT( !f3l->list == !f3l->size ); + + if ( f3l->used == f3l->size ) + { + f3l->size = 3*f3l->size/2 + 100; + f3l->list = REALLOC( f3l->list, f3l->size * sizeof(*f3l->list) ); + if (f3l->sort) + f3l->sort = REALLOC( f3l->sort, f3l->size * sizeof(*f3l->sort) ); + } + + DASSERT( f3l->list ); + DASSERT( f3l->used < f3l->size ); + + if (f3l->sort) + f3l->sort[f3l->used] = 0; + + float3 * item = f3l->list + f3l->used++; + item->x = item->y = item->z = 0.0; + return item; +} + +/////////////////////////////////////////////////////////////////////////////// + +uint FindInsertFloatF3L ( float3List_t * f3l, float3 *val, bool fast ) +{ + DASSERT( f3l ); + DASSERT( f3l->used <= f3l->size ); + DASSERT( !f3l->list == !f3l->size ); + DASSERT( val ); + + int beg = 0; + if (!fast) + { + if (f3l->sort) + { + int end = f3l->used - 1; + while ( beg <= end ) + { + uint idx = (beg+end)/2; + DASSERT( idx >= 0 && idx < f3l->used ); + DASSERT_MSG( f3l->sort[idx] < f3l->used, + "idx=%d, sort-idx=%d, used=%u, size=%u\n", + idx, f3l->sort[idx], f3l->used, f3l->size ); + int stat = memcmp( val,f3l->list + f3l->sort[idx], sizeof(*val) ); + if ( stat < 0 ) + end = idx - 1 ; + else if ( stat > 0 ) + beg = idx + 1; + else + return f3l->sort[idx]; + } + } + else + { + float3 *ptr = f3l->list; + float3 *end = ptr + f3l->used; + for ( ; ptr < end; ptr++ ) + if (!memcmp(val,ptr,sizeof(*val))) + return ptr - f3l->list; + } + } + else if (f3l->sort) + { + // 'sort' not needed + BINGO; // don't remove this line! + FREE(f3l->sort); + f3l->sort = 0; + } + + float3 *res = AppendF3L(f3l); + DASSERT(res); + memcpy(res,val,sizeof(*res)); + + const uint idx = f3l->used - 1; + DASSERT( idx < f3l->used ); + if (f3l->sort) + { + DASSERT( beg >= 0 && beg <= idx ); + uint *dest = f3l->sort + beg; + memmove( dest+1, dest, (idx-beg)*sizeof(*f3l->sort) ); + *dest = idx; + + #if HAVE_PRINT0 + if ( f3l->used < 10 ) + { + uint i; + for ( i = 0; i < f3l->used; i++ ) + { + float3 *f = f3l->list + f3l->sort[i]; + PRINT("INS %2u: %2u -> %08x %08x %08x %11.5f %11.5f %11.5f\n", + i, f3l->sort[i], + be32(&f->x), be32(&f->y), be32(&f->z), + f->x, f->y, f->z ); + } + PRINT("---\n"); + } + #endif + } + + return idx; +} + +/////////////////////////////////////////////////////////////////////////////// + +float3 * GrowF3L ( float3List_t * f3l, uint n ) +{ + DASSERT( f3l ); + DASSERT( f3l->used <= f3l->size ); + DASSERT( !f3l->list == !f3l->size ); + + const uint new_used = f3l->used + n; + if ( new_used > f3l->size || !f3l->size ) + { + f3l->size = new_used ? new_used : 10; + f3l->list = REALLOC( f3l->list, f3l->size * sizeof(*f3l->list)) ; + } + + DASSERT( f3l->list ); + DASSERT( f3l->used + n <= f3l->size ); + + float3 * result = f3l->list + f3l->used; + f3l->used = new_used; + return result; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// double3 list support /////////////// +/////////////////////////////////////////////////////////////////////////////// + +void InitializeD3L ( double3List_t * d3l ) +{ + DASSERT(d3l); + memset(d3l,0,sizeof(*d3l)); +} + +/////////////////////////////////////////////////////////////////////////////// + +void ResetD3L ( double3List_t * d3l ) +{ + DASSERT(d3l); + FREE(d3l->list); + memset(d3l,0,sizeof(*d3l)); +} + +/////////////////////////////////////////////////////////////////////////////// + +double3 * AppendD3L ( double3List_t * d3l ) +{ + DASSERT( d3l ); + DASSERT( d3l->used <= d3l->size ); + DASSERT( !d3l->list == !d3l->size ); + + if ( d3l->used == d3l->size ) + { + d3l->size = 3*d3l->size/2 + 100; + d3l->list = REALLOC( d3l->list, d3l->size * sizeof(*d3l->list)) ; + } + + DASSERT( d3l->list ); + DASSERT( d3l->used < d3l->size ); + + double3 * item = d3l->list + d3l->used++; + item->x = item->y = item->z = 0.0; + return item; +} + +/////////////////////////////////////////////////////////////////////////////// + +uint FindInsertFloatD3L ( double3List_t * d3l, double3 *val, bool fast ) +{ + DASSERT( d3l ); + DASSERT( d3l->used <= d3l->size ); + DASSERT( !d3l->list == !d3l->size ); + DASSERT( val ); + + // first: reduce precision + double3 temp; + temp.x = double2float(val->x); + temp.y = double2float(val->y); + temp.z = double2float(val->z); + + if (!fast) + { + double3 *ptr = d3l->list; + double3 *end = ptr + d3l->used; + for ( ; ptr < end; ptr++ ) + if (!memcmp(&temp,ptr,sizeof(temp))) + return ptr - d3l->list; + } + + double3 *res = AppendD3L(d3l); + DASSERT(res); + memcpy(res,&temp,sizeof(*res)); + return d3l->used - 1; +} + +/////////////////////////////////////////////////////////////////////////////// + +double3 * GrowD3L ( double3List_t * d3l, uint n ) +{ + DASSERT( d3l ); + DASSERT( d3l->used <= d3l->size ); + DASSERT( !d3l->list == !d3l->size ); + + const uint new_used = d3l->used + n; + if ( new_used > d3l->size || !d3l->size ) + { + d3l->size = new_used ? new_used : 10; + d3l->list = REALLOC( d3l->list, d3l->size * sizeof(*d3l->list)) ; + } + + DASSERT( d3l->list ); + DASSERT( d3l->used + n <= d3l->size ); + + double3 * result = d3l->list + d3l->used; + d3l->used = new_used; + return result; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// check numbers /////////////// +/////////////////////////////////////////////////////////////////////////////// + +bool IsNormalF3 ( float *f3 ) +{ + DASSERT(f3); + const int fpclass = fpclassify(*f3++); + if ( fpclass == FP_NORMAL || fpclass == FP_ZERO ) + { + const int fpclass = fpclassify(*f3++); + if ( fpclass == FP_NORMAL || fpclass == FP_ZERO ) + { + const int fpclass = fpclassify(*f3); + return fpclass == FP_NORMAL || fpclass == FP_ZERO; + } + } + return false; +} + +/////////////////////////////////////////////////////////////////////////////// + +bool IsNormalF3be ( float *f3 ) +{ + DASSERT(f3); + const int fpclass = fpclassify(bef4(f3)); + if ( fpclass == FP_NORMAL || fpclass == FP_ZERO ) + { + const int fpclass = fpclassify(bef4(++f3)); + if ( fpclass == FP_NORMAL || fpclass == FP_ZERO ) + { + const int fpclass = fpclassify(bef4(++f3)); + return fpclass == FP_NORMAL || fpclass == FP_ZERO; + } + } + return false; +} + +/////////////////////////////////////////////////////////////////////////////// + +bool IsNormalD3 ( double *d3 ) +{ + DASSERT(d3); + const int fpclass = fpclassify(*d3++); + if ( fpclass == FP_NORMAL || fpclass == FP_ZERO ) + { + const int fpclass = fpclassify(*d3++); + if ( fpclass == FP_NORMAL || fpclass == FP_ZERO ) + { + const int fpclass = fpclassify(*d3); + return fpclass == FP_NORMAL || fpclass == FP_ZERO; + } + } + return false; +} + +/////////////////////////////////////////////////////////////////////////////// + +bool IsEqualD3 ( const double3 *a, const double3 *b, + double null_epsilon, double diff_epsilon ) +{ + DASSERT(a); + DASSERT(b); + DASSERT( null_epsilon > 0.0 ); + DASSERT( diff_epsilon > 0.0 ); + + double diff; + + diff = fabs( a->x - b->x ); + if ( diff > null_epsilon ) + { + const double abs_a = fabs(a->x); + const double abs_b = fabs(b->x); + if ( diff > ( abs_a > abs_b ? abs_a : abs_b ) * diff_epsilon ) + return false; + } + + diff = fabs( a->y - b->y ); + if ( diff > null_epsilon ) + { + const double abs_a = fabs(a->y); + const double abs_b = fabs(b->y); + if ( diff > ( abs_a > abs_b ? abs_a : abs_b ) * diff_epsilon ) + return false; + } + + diff = fabs( a->z - b->z ); + if ( diff > null_epsilon ) + { + const double abs_a = fabs(a->z); + const double abs_b = fabs(b->z); + if ( diff > ( abs_a > abs_b ? abs_a : abs_b ) * diff_epsilon ) + return false; + } + + return true; +} + +/////////////////////////////////////////////////////////////////////////////// + +bool IsSameF ( float a, float b, int bit_diff ) +{ + float delta = ldexpf( fabsf(a-b), 23-bit_diff ); + return IsNormalF(delta) && delta <= fabsf(a) && delta <= fabsf(b); +} + +bool IsSameD ( double a, double b, int bit_diff ) +{ + double delta = ldexp( fabs(a-b), 52-bit_diff ); + return IsNormalD(delta) && delta <= fabs(a) && delta <= fabs(b); +} + +bool IsSameF3 ( const float *a, const float *b, int bit_diff ) +{ + return IsSameF(a[0],b[0],bit_diff) + && IsSameF(a[1],b[1],bit_diff) + && IsSameF(a[2],b[2],bit_diff); +} + +bool IsSameD3 ( const double *a, const double *b, int bit_diff ) +{ + return IsSameD(a[0],b[0],bit_diff) + && IsSameD(a[1],b[1],bit_diff) + && IsSameD(a[2],b[2],bit_diff); +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// 2D vector functions /////////////// +/////////////////////////////////////////////////////////////////////////////// + +uint PointsInConvexPolygonF +( + // return, how many points are inside the polygon + + float *pts, // points: pointer to a array with '2*n_pts' elements + uint n_pts, // Number of (x,y) pairs in 'pts' + + float *pol, // polygon: pointer to a array with '2*n_pol' elements + uint n_pol, // number of (x,y) pairs in 'pol' + + bool all, // false: count the numbers of points inside + // true: return '1' for all points inside and otherwise '0' + + int *r_dir // If not NULL then return: + // -1: polygon is defined counterclockwise + // 0: unknown + // +1: polygon is defined clockwise + // If at least one point is inside, the result is known. +) +{ + if ( !n_pts || n_pol < 3 ) + return 0; + DASSERT(pts); + DASSERT(pol); + + int pol_dir = 0; + uint count = 0; + for(;;) + { + float *a = pol + 2*(n_pol-1); + float *b = pol; + int dir = pol_dir; + uint i = n_pol; + for(;;) + { + const double dstat = SideOfLine(a[0],a[1],b[0],b[1],pts[0],pts[1]); + const int new_dir = dstat > 0.0 ? -1 : dstat < 0; + noPRINT("STAT: %2d %2d %2d %4g\n", pol_dir, dir, new_dir, dstat ); + if (!dir) + dir = new_dir; + else if ( new_dir && dir != new_dir ) + { + if (!all) + break; + if (r_dir) + *r_dir = pol_dir; + return 0; + } + + if ( --i > 0 ) + { + a = b; + b += 2; + continue; + } + + if (!pol_dir) + pol_dir = dir; + count++; + break; + } + + if ( --n_pts > 0 ) + { + pts += 2; + continue; + } + + if (r_dir) + *r_dir = pol_dir; + noPRINT("DIR: %2d\n",pol_dir); + return all ? 1 : count; + } +} + +/////////////////////////////////////////////////////////////////////////////// + +uint PointsInConvexPolygonD +( + // return, how many points are inside the polygon + + double *pts, // points: pointer to a array with '2*n_pts' elements + uint n_pts, // Number of (x,y) pairs in 'pts' + + double *pol, // polygon: pointer to a array with '2*n_pol' elements + uint n_pol, // number of (x,y) pairs in 'pol' + + bool all, // false: count the numbers of points inside + // true: return '1' for all points inside and otherwise '0' + + int *r_dir // If not NULL then return: + // -1: polygon is defined counterclockwise + // 0: unknown + // +1: polygon is defined clockwise + // If at least one point is inside, the result is known. +) +{ + if ( !n_pts || n_pol < 3 ) + return 0; + DASSERT(pts); + DASSERT(pol); + + int pol_dir = 0; + uint count = 0; + for(;;) + { + double *a = pol + 2*(n_pol-1); + double *b = pol; + int dir = pol_dir; + uint i = n_pol; + for(;;) + { + const double dstat = SideOfLine(a[0],a[1],b[0],b[1],pts[0],pts[1]); + const int new_dir = dstat > 0.0 ? -1 : dstat < 0; + noPRINT("STAT: %2d %2d %2d %4g\n", pol_dir, dir, new_dir, dstat ); + if (!dir) + dir = new_dir; + else if ( new_dir && dir != new_dir ) + { + if (!all) + break; + if (r_dir) + *r_dir = pol_dir; + return 0; + } + + if ( --i > 0 ) + { + a = b; + b += 2; + continue; + } + + if (!pol_dir) + pol_dir = dir; + count++; + break; + } + + if ( --n_pts > 0 ) + { + pts += 2; + continue; + } + + if (r_dir) + *r_dir = pol_dir; + noPRINT("DIR: %2d\n",pol_dir); + return all ? 1 : count; + } +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// float34 /////////////// +/////////////////////////////////////////////////////////////////////////////// + +void ClearFloat34 ( float34 *f34 ) +{ + DASSERT(f34); + float *d = f34->v; + uint n; + for ( n = 0; n < 12; n++ ) + *d++ = 0.0; +} + +/////////////////////////////////////////////////////////////////////////////// + +void SetupFloat34 ( float34 *f34 ) +{ + DASSERT(f34); + float *d = f34->v; + uint n; + for ( n = 0; n < 12; n++ ) + *d++ = 0.0; + f34->m[0][0] = f34->m[1][1] = f34->m[2][2] = 1.0; +} + +/////////////////////////////////////////////////////////////////////////////// + +void SetupMatrixF34 +( + float34 *mat, // data to setup + float3 *scale, // NULL or scaling vector + float3 *rotate, // NULL or rotation vector in *radiant* + float3 *translate // NULL or translation vector +) +{ + // This function use 'scale', 'rotate' and 'translate' to calculate the + // transformation matrix. The calculations have been verified against + // original MDL files of Nintendo. The rotation is done in right-handed + // rotation with Euler angles (in degree) and x-y-z convention: + // => http://en.wikipedia.org/wiki/Matrix_rotation#General_rotations + + DASSERT(mat); + float *t = mat->v; + + + //--- setup scale + + float3 myscale; + if (scale) + { + myscale.x = fabs(scale->x) < NULL_EPSILON ? 1.0 : scale->x; + myscale.y = fabs(scale->y) < NULL_EPSILON ? 1.0 : scale->y; + myscale.z = fabs(scale->z) < NULL_EPSILON ? 1.0 : scale->z; + } + else + myscale.x = myscale.y = myscale.z = 1.0; + + + //--- calc translation matrix by scale and rotation + + if (rotate) + { + // be sure, that null values are exact + const double sin_x = fabs(rotate->x) < NULL_EPSILON ? 0.0 : sin( rotate->x ); + const double cos_x = fabs(rotate->x) < NULL_EPSILON ? 1.0 : cos( rotate->x ); + const double sin_y = fabs(rotate->y) < NULL_EPSILON ? 0.0 : sin( rotate->y ); + const double cos_y = fabs(rotate->y) < NULL_EPSILON ? 1.0 : cos( rotate->y ); + const double sin_z = fabs(rotate->z) < NULL_EPSILON ? 0.0 : sin( rotate->z ); + const double cos_z = fabs(rotate->z) < NULL_EPSILON ? 1.0 : cos( rotate->z ); + + t[ 0] = myscale.x * ( cos_y * cos_z ); + t[ 1] = myscale.y * ( - cos_x * sin_z + sin_x * sin_y * cos_z ); + t[ 2] = myscale.z * ( sin_x * sin_z + cos_x * sin_y * cos_z ); + + t[4+0] = myscale.x * ( cos_y * sin_z ); + t[4+1] = myscale.y * ( cos_x * cos_z + sin_x * sin_y * sin_z ); + t[4+2] = myscale.z * ( - sin_x * cos_z + cos_x * sin_y * sin_z ); + + t[8+0] = myscale.x * ( - sin_y ); + t[8+1] = myscale.y * ( sin_x * cos_y ); + t[8+2] = myscale.z * ( cos_x * cos_y ); + } + else + { + t[ 0] = myscale.x; + t[4+1] = myscale.y; + t[8+2] = myscale.z; + + t[ 1] = 0.0; + t[ 2] = 0.0; + t[4+0] = 0.0; + t[4+2] = 0.0; + t[8+0] = 0.0; + t[8+1] = 0.0; + } + + + //--- assign translation + + if (translate) + { + t[ 3] = translate->x; + t[4+3] = translate->y; + t[8+3] = translate->z; + } + else + { + t[ 3] = 0.0; + t[4+3] = 0.0; + t[8+3] = 0.0; + } +} + +/////////////////////////////////////////////////////////////////////////////// + +void CalcInverseMatrixF34 +( + float34 *i, // store inverse matrix here + const float34 *t // transformation matrix +) +{ + DASSERT(i); + DASSERT(t); + + //--- calc inverse matrix + + const double det = t->v[ 0] * t->v[4+1] * t->v[8+2] + + t->v[ 1] * t->v[4+2] * t->v[8+0] + + t->v[ 2] * t->v[4+0] * t->v[8+1] + - t->v[ 2] * t->v[4+1] * t->v[8+0] + - t->v[ 1] * t->v[4+0] * t->v[8+2] + - t->v[ 0] * t->v[4+2] * t->v[8+1]; + + if ( fabs(det) < NULL_EPSILON ) + { + // this should never happen => initialize inv_matrix with 0.0 + ClearFloat34(i); + } + else + { + const double det1 = 1.0 / det; // multplication is faster than division + + i->v[ 0] = ( t->v[4+1] * t->v[8+2] - t->v[4+2] * t->v[8+1] ) * det1; + i->v[ 1] = ( t->v[ 2] * t->v[8+1] - t->v[ 1] * t->v[8+2] ) * det1; + i->v[ 2] = ( t->v[ 1] * t->v[4+2] - t->v[ 2] * t->v[4+1] ) * det1; + + i->v[4+0] = ( t->v[4+2] * t->v[8+0] - t->v[4+0] * t->v[8+2] ) * det1; + i->v[4+1] = ( t->v[ 0] * t->v[8+2] - t->v[ 2] * t->v[8+0] ) * det1; + i->v[4+2] = ( t->v[ 2] * t->v[4+0] - t->v[ 0] * t->v[4+2] ) * det1; + + i->v[8+0] = ( t->v[4+0] * t->v[8+1] - t->v[4+1] * t->v[8+0] ) * det1; + i->v[8+1] = ( t->v[ 1] * t->v[8+0] - t->v[ 0] * t->v[8+1] ) * det1; + i->v[8+2] = ( t->v[ 0] * t->v[4+1] - t->v[ 1] * t->v[4+0] ) * det1; + } + + //--- calc inverse translation + + i->v[ 3] = - t->v[ 3] * i->v[ 0] + - t->v[4+3] * i->v[ 1] + - t->v[8+3] * i->v[ 2]; + + i->v[4+3] = - t->v[ 3] * i->v[4+0] + - t->v[4+3] * i->v[4+1] + - t->v[8+3] * i->v[4+2]; + + i->v[8+3] = - t->v[ 3] * i->v[8+0] + - t->v[4+3] * i->v[8+1] + - t->v[8+3] * i->v[8+2]; +} + +/////////////////////////////////////////////////////////////////////////////// + +void MultiplyF34 +( + float34 *dest, // valid, store result here, may be 'src1' or 'src2' + const float34 *src1, // first valid source, if NULL use dest + const float34 *src2 // second valid source, if NULL use dest +) +{ + DASSERT(dest); + + if ( dest == src1 || dest == src2 || !src1 || !src2 ) + { + float34 temp; + MultiplyF34( &temp, src1 ? src1 : dest, src2 ? src2 : dest ); + *dest = temp; + return; + } + DASSERT( src1 && src1 != dest ); + DASSERT( src2 && src2 != dest ); + + float *res = dest->v; + const float *a = src1->v; + const float *b = src2->v; + + res[0+0] = a[0+0]*b[0+0] + a[4+0]*b[0+1] + a[8+0]*b[0+2]; + res[0+1] = a[0+1]*b[0+0] + a[4+1]*b[0+1] + a[8+1]*b[0+2]; + res[0+2] = a[0+2]*b[0+0] + a[4+2]*b[0+1] + a[8+2]*b[0+2]; + res[0+3] = a[0+3]*b[0+0] + a[4+3]*b[0+1] + a[8+3]*b[0+2] + b[0+3]; + + res[4+0] = a[0+0]*b[4+0] + a[4+0]*b[4+1] + a[8+0]*b[4+2]; + res[4+1] = a[0+1]*b[4+0] + a[4+1]*b[4+1] + a[8+1]*b[4+2]; + res[4+2] = a[0+2]*b[4+0] + a[4+2]*b[4+1] + a[8+2]*b[4+2]; + res[4+3] = a[0+3]*b[4+0] + a[4+3]*b[4+1] + a[8+3]*b[4+2] + b[4+3]; + + res[8+0] = a[0+0]*b[8+0] + a[4+0]*b[8+1] + a[8+0]*b[8+2]; + res[8+1] = a[0+1]*b[8+0] + a[4+1]*b[8+1] + a[8+1]*b[8+2]; + res[8+2] = a[0+2]*b[8+0] + a[4+2]*b[8+1] + a[8+2]*b[8+2]; + res[8+3] = a[0+3]*b[8+0] + a[4+3]*b[8+1] + a[8+3]*b[8+2] + b[8+3]; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// double34 /////////////// +/////////////////////////////////////////////////////////////////////////////// + +void ClearDouble34 ( double34 *d34 ) +{ + DASSERT(d34); + double *d = d34->v; + uint n; + for ( n = 0; n < 12; n++ ) + *d++ = 0.0; +} + +/////////////////////////////////////////////////////////////////////////////// + +void SetupDouble34 ( double34 *d34 ) +{ + DASSERT(d34); + double *d = d34->v; + uint n; + for ( n = 0; n < 12; n++ ) + *d++ = 0.0; + d34->m[0][0] = d34->m[1][1] = d34->m[2][2] = 1.0; +} + +/////////////////////////////////////////////////////////////////////////////// + +void SetupMatrixD34 +( + double34 *mat, // data to setup + double3 *scale, // NULL or scaling vector + double3 *rotate, // NULL or rotation vector in *radiant* + double3 *translate // NULL or translation vector +) +{ + // This function use 'scale', 'rotate' and 'translate' to calculate the + // transformation matrix. The calculations have been verified against + // original MDL files of Nintendo. The rotation is done in right-handed + // rotation with Euler angles (in degree) and x-y-z convention: + // => http://en.wikipedia.org/wiki/Matrix_rotation#General_rotations + + DASSERT(mat); + double *t = mat->v; + + + //--- setup scale + + double3 myscale; + if (scale) + { + myscale.x = fabs(scale->x) < NULL_EPSILON ? 1.0 : scale->x; + myscale.y = fabs(scale->y) < NULL_EPSILON ? 1.0 : scale->y; + myscale.z = fabs(scale->z) < NULL_EPSILON ? 1.0 : scale->z; + } + else + myscale.x = myscale.y = myscale.z = 1.0; + + + //--- calc translation matrix by scale and rotation + + if (rotate) + { + // be sure, that null values are exact + const double sin_x = fabs(rotate->x) < NULL_EPSILON ? 0.0 : sin( rotate->x ); + const double cos_x = fabs(rotate->x) < NULL_EPSILON ? 1.0 : cos( rotate->x ); + const double sin_y = fabs(rotate->y) < NULL_EPSILON ? 0.0 : sin( rotate->y ); + const double cos_y = fabs(rotate->y) < NULL_EPSILON ? 1.0 : cos( rotate->y ); + const double sin_z = fabs(rotate->z) < NULL_EPSILON ? 0.0 : sin( rotate->z ); + const double cos_z = fabs(rotate->z) < NULL_EPSILON ? 1.0 : cos( rotate->z ); + + t[ 0] = myscale.x * ( cos_y * cos_z ); + t[ 1] = myscale.y * ( - cos_x * sin_z + sin_x * sin_y * cos_z ); + t[ 2] = myscale.z * ( sin_x * sin_z + cos_x * sin_y * cos_z ); + + t[4+0] = myscale.x * ( cos_y * sin_z ); + t[4+1] = myscale.y * ( cos_x * cos_z + sin_x * sin_y * sin_z ); + t[4+2] = myscale.z * ( - sin_x * cos_z + cos_x * sin_y * sin_z ); + + t[8+0] = myscale.x * ( - sin_y ); + t[8+1] = myscale.y * ( sin_x * cos_y ); + t[8+2] = myscale.z * ( cos_x * cos_y ); + } + else + { + t[ 0] = myscale.x; + t[4+1] = myscale.y; + t[8+2] = myscale.z; + + t[ 1] = 0.0; + t[ 2] = 0.0; + t[4+0] = 0.0; + t[4+2] = 0.0; + t[8+0] = 0.0; + t[8+1] = 0.0; + } + + + //--- assign translation + + if (translate) + { + t[ 3] = translate->x; + t[4+3] = translate->y; + t[8+3] = translate->z; + } + else + { + t[ 3] = 0.0; + t[4+3] = 0.0; + t[8+3] = 0.0; + } +} + +/////////////////////////////////////////////////////////////////////////////// + +void CalcInverseMatrixD34 +( + double34 *i, // store inverse matrix here + const double34 *t // transformation matrix +) +{ + DASSERT(i); + DASSERT(t); + + //--- calc inverse matrix + + const double det = t->v[ 0] * t->v[4+1] * t->v[8+2] + + t->v[ 1] * t->v[4+2] * t->v[8+0] + + t->v[ 2] * t->v[4+0] * t->v[8+1] + - t->v[ 2] * t->v[4+1] * t->v[8+0] + - t->v[ 1] * t->v[4+0] * t->v[8+2] + - t->v[ 0] * t->v[4+2] * t->v[8+1]; + + if ( fabs(det) < NULL_EPSILON ) + { + // this should never happen => initialize inv_matrix with 0.0 + ClearDouble34(i); + } + else + { + const double det1 = 1.0 / det; // multplication is faster than division + + i->v[ 0] = ( t->v[4+1] * t->v[8+2] - t->v[4+2] * t->v[8+1] ) * det1; + i->v[ 1] = ( t->v[ 2] * t->v[8+1] - t->v[ 1] * t->v[8+2] ) * det1; + i->v[ 2] = ( t->v[ 1] * t->v[4+2] - t->v[ 2] * t->v[4+1] ) * det1; + + i->v[4+0] = ( t->v[4+2] * t->v[8+0] - t->v[4+0] * t->v[8+2] ) * det1; + i->v[4+1] = ( t->v[ 0] * t->v[8+2] - t->v[ 2] * t->v[8+0] ) * det1; + i->v[4+2] = ( t->v[ 2] * t->v[4+0] - t->v[ 0] * t->v[4+2] ) * det1; + + i->v[8+0] = ( t->v[4+0] * t->v[8+1] - t->v[4+1] * t->v[8+0] ) * det1; + i->v[8+1] = ( t->v[ 1] * t->v[8+0] - t->v[ 0] * t->v[8+1] ) * det1; + i->v[8+2] = ( t->v[ 0] * t->v[4+1] - t->v[ 1] * t->v[4+0] ) * det1; + } + + //--- calc inverse translation + + i->v[ 3] = - t->v[ 3] * i->v[ 0] + - t->v[4+3] * i->v[ 1] + - t->v[8+3] * i->v[ 2]; + + i->v[4+3] = - t->v[ 3] * i->v[4+0] + - t->v[4+3] * i->v[4+1] + - t->v[8+3] * i->v[4+2]; + + i->v[8+3] = - t->v[ 3] * i->v[8+0] + - t->v[4+3] * i->v[8+1] + - t->v[8+3] * i->v[8+2]; +} + +/////////////////////////////////////////////////////////////////////////////// + +void MultiplyD34 +( + double34 *dest, // valid, store result here, may be 'src1' or 'src2' + const double34 *src1, // first valid source, if NULL use dest + const double34 *src2 // second valid source, if NULL use dest +) +{ + DASSERT(dest); + + if ( dest == src1 || dest == src2 || !src1 || !src2 ) + { + double34 temp; + MultiplyD34( &temp, src1 ? src1 : dest, src2 ? src2 : dest ); + *dest = temp; + return; + } + DASSERT( src1 && src1 != dest ); + DASSERT( src2 && src2 != dest ); + + double *res = dest->v; + const double *a = src1->v; + const double *b = src2->v; + + res[0+0] = a[0+0]*b[0+0] + a[4+0]*b[0+1] + a[8+0]*b[0+2]; + res[0+1] = a[0+1]*b[0+0] + a[4+1]*b[0+1] + a[8+1]*b[0+2]; + res[0+2] = a[0+2]*b[0+0] + a[4+2]*b[0+1] + a[8+2]*b[0+2]; + res[0+3] = a[0+3]*b[0+0] + a[4+3]*b[0+1] + a[8+3]*b[0+2] + b[0+3]; + + res[4+0] = a[0+0]*b[4+0] + a[4+0]*b[4+1] + a[8+0]*b[4+2]; + res[4+1] = a[0+1]*b[4+0] + a[4+1]*b[4+1] + a[8+1]*b[4+2]; + res[4+2] = a[0+2]*b[4+0] + a[4+2]*b[4+1] + a[8+2]*b[4+2]; + res[4+3] = a[0+3]*b[4+0] + a[4+3]*b[4+1] + a[8+3]*b[4+2] + b[4+3]; + + res[8+0] = a[0+0]*b[8+0] + a[4+0]*b[8+1] + a[8+0]*b[8+2]; + res[8+1] = a[0+1]*b[8+0] + a[4+1]*b[8+1] + a[8+1]*b[8+2]; + res[8+2] = a[0+2]*b[8+0] + a[4+2]*b[8+1] + a[8+2]*b[8+2]; + res[8+3] = a[0+3]*b[8+0] + a[4+3]*b[8+1] + a[8+3]*b[8+2] + b[8+3]; +} + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +void CopyF34toD34 +( + double34 *dest, // destination matrix + const float34 *src // source matrix +) +{ + DASSERT(dest); + DASSERT(src); + + double *d = dest->v; + const float *s = src->v; + + uint n; + for ( n = 0; n < 12; n++ ) + *d++ = *s++; +} + +/////////////////////////////////////////////////////////////////////////////// + +void CopyD34toF34 +( + float34 *dest, // destination matrix + const double34 *src // source matrix +) +{ + DASSERT(dest); + DASSERT(src); + + float *d = dest->v; + const double *s = src->v; + + uint n; + for ( n = 0; n < 12; n++ ) + *d++ = *s++; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// MatrixD_t: setup /////////////// +/////////////////////////////////////////////////////////////////////////////// + +// statistics +u64 N_MatrixD_forward = 0; // total number of forward transformations +u64 N_MatrixD_inverse = 0; // total number of inverse transformations + +/////////////////////////////////////////////////////////////////////////////// + +void InitializeMatrixD ( MatrixD_t * mat ) +{ + DASSERT(mat); + const uint seqnum = mat->valid ? mat->sequence_number : 0; + memset(mat,0,sizeof(*mat)); + mat->sequence_number = seqnum; + + mat->scale.x = mat->scale.y = mat->scale.z = 1.0; + mat->scale_origin.x = mat->scale_origin.y = mat->scale_origin.z = 0.0; + + mat->shift = mat->scale_origin; + mat->rotate_deg = mat->scale_origin; + mat->rotate_rad = mat->scale_origin; + mat->translate = mat->scale_origin; + + double *d = (double*)mat->rotate_origin; + uint n; + for ( n = 0; n < 9; n++ ) + *d++ = 0.0; + + mat->valid = true; +} + +/////////////////////////////////////////////////////////////////////////////// + +void CopyMatrixD +( + MatrixD_t *dest, // valid destination + const + MatrixD_t *src // NULL or source +) +{ + DASSERT(dest); + + if ( src && src->valid ) + { + const uint seqnum = dest->sequence_number; + *dest = *src; + dest->sequence_number = seqnum+1; + } + else + InitializeMatrixD(dest); +} + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetScaleMatrixD +( + MatrixD_t * mat, // valid data structure + double3 * scale, // NULL: clear scale, not NULL: set scale + double3 * origin // not NULL: Origin for scaling +) +{ + DASSERT(mat); + if (!mat->valid) + InitializeMatrixD(mat); + mat->norm_valid = mat->tmatrix_valid = mat->imatrix_valid = false; + + if (scale) + mat->scale = *scale; + else + { + mat->scale.x = mat->scale.y = mat->scale.z = 1.0; + origin = 0; + } + + if (origin) + mat->scale_origin = *origin; + else + mat->scale_origin.x = mat->scale_origin.y = mat->scale_origin.z = 0.0; + + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetShiftMatrixD +( + MatrixD_t * mat, // valid data structure + double3 * shift // NULL: clear Shift, not NULL: set Shift +) +{ + DASSERT(mat); + if (!mat->valid) + InitializeMatrixD(mat); + mat->norm_valid = mat->tmatrix_valid = mat->imatrix_valid = false; + + if (shift) + mat->shift = *shift; + else + mat->shift.x = mat->shift.y = mat->shift.z = 0.0; + + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetScaleShiftMatrixD +( + MatrixD_t * mat, // valid data structure + uint xyz, // 0..2: x/y/z coordinate to set + double old1, // old value of first point + double new1, // new value of first point + double old2, // old value of second point + double new2 // new value of second point +) +{ + DASSERT(mat); + if (!mat->valid) + InitializeMatrixD(mat); + + if ( fabs(old1-old2) >= NULL_EPSILON ) // ignore othwerwise + { + mat->norm_valid = mat->tmatrix_valid = mat->imatrix_valid = false; + + const double scale = ( new2 - new1 ) / ( old2 - old1 ); + mat->scale.v[xyz] = scale; + mat->scale_origin.v[xyz] = 0.0; + + // new = old * scale + shift + mat->shift.v[xyz] = new1 - old1 * scale; + } + + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetRotateMatrixD +( + MatrixD_t * mat, // valid data structure + uint xyz, // 0..2: x/y/z coordinate to set + double degree, // angle in degree + double radiant, // angle in radiant + double3 * origin // not NULL: Origin for the rotation +) +{ + DASSERT(mat); + DASSERT(xyz<3); + + if (!mat->valid) + InitializeMatrixD(mat); + mat->norm_valid = mat->tmatrix_valid = mat->imatrix_valid = false; + + mat->rotate_deg.v[xyz] = degree; + mat->rotate_rad.v[xyz] = radiant; + + if (origin) + mat->rotate_origin[xyz] = *origin; + else + { + origin = mat->rotate_origin + xyz; + origin->x = origin->y = origin->z = 0.0; + } + + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetTranslateMatrixD +( + MatrixD_t * mat, // valid data structure + double3 * translate // NULL: clear translate, not NULL: set translate +) +{ + DASSERT(mat); + if (!mat->valid) + InitializeMatrixD(mat); + mat->norm_valid = mat->tmatrix_valid = mat->imatrix_valid = false; + + if (translate) + mat->translate = *translate; + else + mat->translate.x = mat->translate.y = mat->translate.z = 0.0; + + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * CheckStatusMatrixD +( + MatrixD_t * mat // valid data structure +) +{ + DASSERT(mat); + + if (mat->tmatrix_valid) + { + double *t = mat->trans_matrix.v; + + //--- check rotation (only disabled for all axis) + + if ( mat->rotate_enabled & 1 + && fabs( t[4+2] ) < NULL_EPSILON + && fabs( t[8+1] ) < NULL_EPSILON + ) + { + mat->rotate_enabled &= ~1; + t[4+2] = 0.0; + t[8+1] = 0.0; + } + + if ( mat->rotate_enabled & 2 + && fabs( t[ 2] ) < NULL_EPSILON + && fabs( t[8+0] ) < NULL_EPSILON + ) + { + mat->rotate_enabled &= ~2; + t[ 2] = 0.0; + t[8+0] = 0.0; + } + + if ( mat->rotate_enabled & 4 + && fabs( t[ 1] ) < NULL_EPSILON + && fabs( t[4+0] ) < NULL_EPSILON + ) + { + mat->rotate_enabled &= ~4; + t[ 1] = 0.0; + t[4+0] = 0.0; + } + + + //--- check scaling (don't touch scale if roating is enabled) + + if ( mat->scale_enabled & 1 + && !( mat->rotate_enabled & ~1 ) + && fabs( t[ 0] - 1.0 ) < NULL_EPSILON ) + { + t[ 0] = 1.0; + mat->scale_enabled &= ~1; + } + + if ( mat->scale_enabled & 2 + && !( mat->rotate_enabled & ~2 ) + && fabs( t[4+1] - 1.0 ) < NULL_EPSILON ) + { + t[4+1] = 1.0; + mat->scale_enabled &= ~2; + } + + if ( mat->scale_enabled & 4 + && !( mat->rotate_enabled & ~4 ) + && fabs( t[8+2] - 1.0 ) < NULL_EPSILON ) + { + t[8+2] = 1.0; + mat->scale_enabled &= ~4; + } + + + //--- check transformation + + if ( mat->translate_enabled & 1 && fabs( t[ 3] ) < NULL_EPSILON ) + { + t[ 3] = 0.0; + mat->translate_enabled &= ~1; + } + + if ( mat->translate_enabled & 2 && fabs( t[4+3] ) < NULL_EPSILON ) + { + t[4+3] = 0.0; + mat->translate_enabled &= ~2; + } + + if ( mat->translate_enabled & 4 && fabs( t[8+3] ) < NULL_EPSILON ) + { + t[8+3] = 0.0; + mat->translate_enabled &= ~4; + } + + mat->transform_enabled = mat->scale_enabled + | mat->rotate_enabled + | mat->translate_enabled + | mat->use_matrix << 3; + } + + return mat; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// MatrixD_t: specials /////////////// +/////////////////////////////////////////////////////////////////////////////// + +void TermAssignMatrixD +( + // called after dirext matrix manipulations + MatrixD_t * mat // valid data structure +) +{ + DASSERT(mat); + + mat->valid = true; + mat->norm_valid = false; + mat->tmatrix_valid = true; + mat->imatrix_valid = false; + mat->use_matrix = 2; + mat->scale_enabled = 7; + mat->rotate_enabled = 7; + mat->translate_enabled = 7; + mat->sequence_number++; + + double34 *m = &mat->trans_matrix; + mat->norm_scale.x = m->x[0]; + mat->norm_scale.y = m->y[1]; + mat->norm_scale.z = m->z[2]; + mat->norm_translate.x = m->x[3]; + mat->norm_translate.y = m->y[3]; + mat->norm_translate.z = m->z[3]; + + CheckStatusMatrixD(mat); + + if (!mat->rotate_enabled) + { + mat->use_matrix = 0; + mat->norm_valid = true; + mat->norm_rotate_deg.x = 0; + mat->norm_rotate_deg.y = 0; + mat->norm_rotate_deg.z = 0; + mat->norm_rotate_rad = mat->norm_rotate_deg; + + mat->scale = mat->norm_scale; + mat->translate = mat->norm_translate; + } +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetMatrixD +( + MatrixD_t * mat, // valid data structure + const + double34 * src // NULL or matrix to set +) +{ + DASSERT(mat); + InitializeMatrixD(mat); + if (src) + { + mat->trans_matrix = *src; + TermAssignMatrixD(mat); + } + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetAScaleMatrixD +( + // calculate the matrix as rotation around a specified axis + + MatrixD_t * mat, // valid data structure + double scale, // scale factor + double3 * dir // NULL or scaling direction as vector +) +{ + DASSERT(mat); + InitializeMatrixD(mat); + + if (dir) + { + const double hrot = atan2(dir->x,dir->z); + const double hlen = sqrt( dir->x*dir->x + dir->z*dir->z ); + const double vrot = -atan2(dir->y,hlen); + + PRINT("HROT: %6.4f %7.2f / %11.3f\n",hrot,hrot*180.0/M_PI,hlen); + PRINT("VROT: %6.4f %7.2f\n",vrot,vrot*180.0/M_PI); + + MatrixD_t mrot; + InitializeMatrixD(&mrot); + mrot.rotate_rad.x = vrot; + mrot.rotate_rad.y = hrot; + CalcMatrixD(&mrot,true); + DASSERT(mrot.imatrix_valid); + SetMatrixD(mat,&mrot.inv_matrix); + + double3 scale3; + scale3.x = scale3.y = 0; + scale3.z = scale; + SetScaleMatrixD(&mrot,&scale3,0); + MultiplyMatrixD(mat,0,&mrot,0); + TermAssignMatrixD(mat); + } + + //--- terminate + + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * SetARotateMatrixD +( + // calculate the matrix as rotation around a specified axis + + MatrixD_t * mat, // valid data structure + double degree, // angle in degree + double radiant, // angle in radiant, add to degree with factor + double3 * pt1, // point #1 to define the axis; if NULL use (0,0,0) + double3 * pt2 // point #2 to define the axis; if NULL use (0,0,0) +) +{ + DASSERT(mat); + InitializeMatrixD(mat); + + double3 dir, base; + if (pt1) + base = *pt1; + else + base.x = base.y = base.z = 0.0; + + if (pt2) + Sub3(dir,*pt2,base); + else + { + dir.x = -base.x; + dir.y = -base.y; + dir.z = -base.z; + } + + #if 1 + radiant += degree * (M_PI/180.0); + const double len = Length3(dir); + if ( len >= NULL_EPSILON ) + #else + radiant = fmod( degree * (M_PI/180.0) + radiant + M_PI, 2*M_PI) - M_PI; + const double len = Length3(dir); + if ( len >= NULL_EPSILON && fabs(radiant) >= NULL_EPSILON ) + #endif + { + dir.x /= len; + dir.y /= len; + dir.z /= len; + + const double sin0 = sin(radiant); + const double cos0 = cos(radiant); + const double cos1 = 1.0 - cos0; + + double *t = mat->trans_matrix.v; + + t[ 0] = dir.x * dir.x * cos1 + cos0; + t[ 1] = dir.x * dir.y * cos1 - dir.z * sin0; + t[ 2] = dir.x * dir.z * cos1 + dir.y * sin0; + + t[4+0] = dir.y * dir.x * cos1 + dir.z * sin0; + t[4+1] = dir.y * dir.y * cos1 + cos0; + t[4+2] = dir.y * dir.z * cos1 - dir.x * sin0; + + t[8+0] = dir.z * dir.x * cos1 - dir.y * sin0; + t[8+1] = dir.z * dir.y * cos1 + dir.x * sin0; + t[8+2] = dir.z * dir.z * cos1 + cos0; + + double3 delta = TransformD3D34(&mat->trans_matrix,&base); + t[ 3] = base.x - delta.x; + t[4+3] = base.y - delta.y; + t[8+3] = base.z - delta.z; + + TermAssignMatrixD(mat); + } + + //--- terminate + + return mat; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// MatrixD_t: calc /////////////// +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * CalcNormMatrixD +( + MatrixD_t * mat // valid data structure +) +{ + DASSERT(mat); + if (!mat->valid) + InitializeMatrixD(mat); + + if ( mat->norm_valid || mat->tmatrix_valid ) + return mat; + + mat->sequence_number++; + mat->norm_valid = true; + mat->scale_enabled = mat->rotate_enabled = mat->translate_enabled = 0; + + + //--- scale & shift + + double3 trans; + + uint xyz; + for ( xyz = 0; xyz < 3; xyz++ ) + { + double scale = mat->scale.v[xyz]; + if ( fabs(scale) < NULL_EPSILON || fabs(scale-1.0) < NULL_EPSILON ) + mat->scale.v[xyz] = scale = 1.0; + else + mat->scale_enabled |= 1 << xyz; + + mat->norm_scale.v[xyz] = scale; + + trans.v[xyz] = mat->shift.v[xyz] + + ( 1.0 - scale ) * mat->scale_origin.v[xyz]; + } + + + //--- rotate + + mat->norm_pos2d.x = mat->norm_pos2d.y = mat->norm_pos2d.z = 0; + + for ( xyz = 0; xyz < 3; xyz++ ) + { + double deg = fmod( mat->rotate_deg.v[xyz] + + mat->rotate_rad.v[xyz] * (180.0/M_PI) + + 180.0, 360.0 ) - 180.0; + if ( fabs(deg) < MIN_DEGREE ) + { + mat->norm_rotate_deg.v[xyz] = 0.0; + mat->norm_rotate_rad.v[xyz] = 0.0; + } + else + { + mat->rotate_enabled |= 1 << xyz; + mat->norm_rotate_deg.v[xyz] = deg; + double rad = deg * (M_PI/180.0); + mat->norm_rotate_rad.v[xyz] = rad; + + // translate = rot_origin + rotated(translate-rot_origin) + + Sub3(trans,trans,mat->rotate_origin[xyz]); + + const uint a = (xyz+2) % 3; + const uint b = (xyz+1) % 3; + rad += atan2(trans.v[a],trans.v[b]); + const double len = sqrt(trans.v[a]*trans.v[a]+trans.v[b]*trans.v[b]); + + trans.v[a] = sin(rad) * len; + trans.v[b] = cos(rad) * len; + + Add3(trans,trans,mat->rotate_origin[xyz]); + + mat->norm_pos2d.v[a] += mat->rotate_origin[xyz].v[a]; + mat->norm_pos2d.v[b] += mat->rotate_origin[xyz].v[b]; + } + } + + if ( (mat->rotate_enabled|1) == 7 ) mat->norm_pos2d.x /= 2; + if ( (mat->rotate_enabled|2) == 7 ) mat->norm_pos2d.y /= 2; + if ( (mat->rotate_enabled|4) == 7 ) mat->norm_pos2d.z /= 2; + + + //--- translate + + mat->translate_enabled = false; + + for ( xyz = 0; xyz < 3; xyz++ ) + { + const double temp = trans.v[xyz] + mat->translate.v[xyz]; + if ( fabs(temp) < NULL_EPSILON ) + mat->norm_translate.v[xyz] = 0.0; + else + { + mat->norm_translate.v[xyz] = temp; + mat->translate_enabled |= 1 << xyz; + } + } + + + //--- status + + if ( mat->use_matrix < 2 ) + { + mat->use_matrix = mat->rotate_enabled ? 1 : 0; + mat->tmatrix_valid = mat->imatrix_valid = false; + } + + mat->transform_enabled = mat->scale_enabled + | mat->rotate_enabled + | mat->translate_enabled + | mat->use_matrix << 3; + + TRACE("%%%%%%%%%% CalcNormMatrixD(%p) => %u|%u|%u = %02x %%%%%%%%%%\n", + mat, mat->scale_enabled, mat->rotate_enabled, + mat->translate_enabled, mat->transform_enabled ); + + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * CalcTransMatrixD +( + MatrixD_t * mat, // valid data structure + bool always // false: calc transformation matrix only + // if needed for transformation + // true: calc matrix always + // if 'tmatrix_valid', the matrix is never calculated +) +{ + DASSERT(mat); + CalcNormMatrixD(mat); + + if ( mat->tmatrix_valid || !always && !mat->use_matrix ) + return mat; + + TRACE("%%%%%%%%%% CalcTransMatrixD(%p,%d) => %u|%u|%u = %02x %%%%%%%%%%\n", + mat, always, mat->scale_enabled, mat->rotate_enabled, + mat->translate_enabled, mat->transform_enabled ); + DASSERT(mat->use_matrix<2); + + SetupMatrixD34( &mat->trans_matrix, + &mat->norm_scale, + &mat->norm_rotate_rad, + &mat->norm_translate ); + + mat->tmatrix_valid = true; + mat->imatrix_valid = false; + CheckStatusMatrixD(mat); + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * CalcMatrixD +( + MatrixD_t * mat, // valid data structure + bool always // false: calc both matrices only + // if needed for transformation + // true: calc matrices always +) +{ + DASSERT(mat); + + if ( mat->imatrix_valid || !always && !mat->use_matrix ) + return mat; + + CalcTransMatrixD(mat,true); + + TRACE("%%%%%%%%%% CalcMatrixD (%p,%d) => %u|%u|%u = %02x %%%%%%%%%%\n", + mat, always, mat->scale_enabled, mat->rotate_enabled, + mat->translate_enabled, mat->transform_enabled ); + + CalcInverseMatrixD34(&mat->inv_matrix,&mat->trans_matrix); + mat->imatrix_valid = true; + return mat; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * MultiplyMatrixD +( + MatrixD_t *dest, // valid, store result here, may be 'src1' or 'src2' + MatrixD_t *src1, // first valid source, if NULL use dest + MatrixD_t *src2, // second valid source, if NULL use dest + uint inverse_mode // bit field: + // 1: use inverse matrix of src1 + // 2: use inverse matrix of src2 +) +{ + DASSERT(dest); + + if ( dest == src1 || dest == src2 || !src1 || !src2 ) + { + MatrixD_t temp; + MultiplyMatrixD( &temp, src1 ? src1 : dest, src2 ? src2 : dest, inverse_mode ); + *dest = temp; + return dest; + } + DASSERT( src1 && src1 != dest ); + DASSERT( src2 && src2 != dest ); + + CalcMatrixD(src1,true); + CalcMatrixD(src2,true); + InitializeMatrixD(dest); + MultiplyD34( &dest->trans_matrix, + inverse_mode & 1 ? &src1->inv_matrix : &src1->trans_matrix, + inverse_mode & 2 ? &src2->inv_matrix : &src2->trans_matrix ); + + + //--- setup normals, simply assumption + + dest->norm_scale.x = dest->trans_matrix.m[0][0]; + dest->norm_scale.y = dest->trans_matrix.m[1][1]; + dest->norm_scale.z = dest->trans_matrix.m[2][2]; + + dest->norm_rotate_deg.x = src1->norm_rotate_deg.x + src2->norm_rotate_deg.x; + dest->norm_rotate_deg.y = src1->norm_rotate_deg.y + src2->norm_rotate_deg.y; + dest->norm_rotate_deg.z = src1->norm_rotate_deg.z + src2->norm_rotate_deg.z; + Mult3f(dest->norm_rotate_rad,dest->norm_rotate_deg,M_PI/180.0); + + dest->norm_translate.x = dest->trans_matrix.m[0][3]; + dest->norm_translate.y = dest->trans_matrix.m[1][3]; + dest->norm_translate.z = dest->trans_matrix.m[2][3]; + + + //--- setup base values + + dest->scale = dest->norm_scale; + dest->rotate_deg = dest->norm_rotate_deg; + dest->translate = dest->norm_translate; + + + //--- terminate + + TermAssignMatrixD(dest); + return dest; +} + +/////////////////////////////////////////////////////////////////////////////// + +MatrixD_t * CalcVectorsMatrixD +( + MatrixD_t * mat, // valid data structure + bool always // false: calc vectors only if matrix is valid + // true: calc matrix if invalid & vectors always +) +{ + DASSERT(mat); + // search: Decomposing a rotation matrix + + if ( !always && !mat->tmatrix_valid ) + return mat; + + CalcMatrixD(mat,false); + + // [[2do]] ??? calculate scale + //mat->norm_scale.x = 1.0; + //mat->norm_scale.y = 1.0; + //mat->norm_scale.z = 1.0; + + double (*m)[4] = mat->trans_matrix.m; + + // [[2do]] ??? works only for scale == 1 ==> + mat->norm_rotate_rad.x = atan2( m[2][1], m[2][2] ); + mat->norm_rotate_rad.y = atan2( -m[2][0], sqrt( m[2][1]*m[2][1] + m[2][2]*m[2][2] ) ); + mat->norm_rotate_rad.z = atan2( m[1][0], m[0][0] ); + + mat->norm_rotate_deg.x = rad2deg(mat->norm_rotate_rad.x); + mat->norm_rotate_deg.y = rad2deg(mat->norm_rotate_rad.y); + mat->norm_rotate_deg.z = rad2deg(mat->norm_rotate_rad.z); + + mat->norm_translate.x = m[0][3]; + mat->norm_translate.y = m[1][3]; + mat->norm_translate.z = m[2][3]; + + return mat; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// MatrixD_t: double transform /////////////// +/////////////////////////////////////////////////////////////////////////////// + +double3 TransformD3MatrixD +( + MatrixD_t * mat, // valid data structure + const + double3 * val // value to transform +) +{ + DASSERT(mat); + DASSERT(val); + + N_MatrixD_forward++; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->tmatrix_valid) + CalcTransMatrixD(mat,true); + return TransformD3D34(&mat->trans_matrix,val); + } + + double3 res; + res.x = val->x * mat->norm_scale.x + mat->norm_translate.x; + res.y = val->y * mat->norm_scale.y + mat->norm_translate.y; + res.z = val->z * mat->norm_scale.z + mat->norm_translate.z; + return res; +} + +/////////////////////////////////////////////////////////////////////////////// + +double3 InvTransformD3MatrixD +( + MatrixD_t * mat, // valid data structure + const + double3 * val // value to transform +) +{ + DASSERT(mat); + DASSERT(val); + + N_MatrixD_inverse++; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->imatrix_valid) + CalcMatrixD(mat,true); + return TransformD3D34(&mat->inv_matrix,val); + } + + double3 res; + res.x = ( val->x - mat->norm_translate.x ) / mat->norm_scale.x; + res.y = ( val->y - mat->norm_translate.y ) / mat->norm_scale.y; + res.z = ( val->z - mat->norm_translate.z ) / mat->norm_scale.z; + return res; +} + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +void TransformD3NMatrixD +( + MatrixD_t * mat, // valid data structure + double3 * val, // value array to transform + int n, // number of 3D vectors in 'val' + uint off // if n>1: offset from one to next 'val' vector +) +{ + DASSERT(mat); + DASSERT(val); + DASSERT( n <= 1 || off >= sizeof(*val) ); + + N_MatrixD_forward += n; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->tmatrix_valid) + CalcTransMatrixD(mat,true); + + while ( n-- > 0 ) + { + *val = TransformD3D34(&mat->trans_matrix,val); + val = (double3*)( (u8*)val + off ); + } + } + else if (mat->transform_enabled) + { + while ( n-- > 0 ) + { + val->x = val->x * mat->norm_scale.x + mat->norm_translate.x; + val->y = val->y * mat->norm_scale.y + mat->norm_translate.y; + val->z = val->z * mat->norm_scale.z + mat->norm_translate.z; + val = (double3*)( (u8*)val + off ); + } + } +} + +/////////////////////////////////////////////////////////////////////////////// + +void InvTransformD3NMatrixD +( + MatrixD_t * mat, // valid data structure + double3 * val, // value array to transform + int n, // number of 3D vectors in 'val' + uint off // if n>1: offset from one to next 'val' vector +) +{ + DASSERT(mat); + DASSERT(val); + DASSERT( n <= 1 || off >= sizeof(*val) ); + + N_MatrixD_inverse += n; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->imatrix_valid) + CalcMatrixD(mat,true); + + while ( n-- > 0 ) + { + *val = TransformD3D34(&mat->inv_matrix,val); + val = (double3*)( (u8*)val + off ); + } + } + else if (mat->transform_enabled) + { + double3 f; // multiplication is faster than division + f.x = 1.0 / mat->norm_scale.x; + f.y = 1.0 / mat->norm_scale.y; + f.z = 1.0 / mat->norm_scale.z; + + while ( n-- > 0 ) + { + val->x = ( val->x - mat->norm_translate.x ) * f.x; + val->y = ( val->y - mat->norm_translate.y ) * f.y; + val->z = ( val->z - mat->norm_translate.z ) * f.z; + val = (double3*)( (u8*)val + off ); + } + } +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// MatrixD_t: float transform /////////////// +/////////////////////////////////////////////////////////////////////////////// + +float3 TransformF3MatrixD +( + MatrixD_t * mat, // valid data structure + const + float3 * val // value to transform +) +{ + DASSERT(mat); + DASSERT(val); + + N_MatrixD_forward++; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->tmatrix_valid) + CalcTransMatrixD(mat,true); + return TransformF3D34(&mat->trans_matrix,val); + } + + float3 res; + res.x = val->x * mat->norm_scale.x + mat->norm_translate.x; + res.y = val->y * mat->norm_scale.y + mat->norm_translate.y; + res.z = val->z * mat->norm_scale.z + mat->norm_translate.z; + return res; +} + +/////////////////////////////////////////////////////////////////////////////// + +float3 InvTransformF3MatrixD +( + MatrixD_t * mat, // valid data structure + const + float3 * val // value to transform +) +{ + DASSERT(mat); + DASSERT(val); + + N_MatrixD_inverse++; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->imatrix_valid) + CalcMatrixD(mat,true); + return TransformF3D34(&mat->inv_matrix,val); + } + + float3 res; + res.x = ( val->x - mat->norm_translate.x ) / mat->norm_scale.x; + res.y = ( val->y - mat->norm_translate.y ) / mat->norm_scale.y; + res.z = ( val->z - mat->norm_translate.z ) / mat->norm_scale.z; + return res; +} + +/////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +void TransformF3NMatrixD +( + MatrixD_t * mat, // valid data structure + float3 * val, // value array to transform + int n, // number of 3D vectors in 'val' + uint off // if n>1: offset from one to next 'val' vector +) +{ + DASSERT(mat); + DASSERT(val); + DASSERT_MSG( n <= 1 || off >= sizeof(*val), "n=%u,off=%u\n",n,off ); + + N_MatrixD_forward += n; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->tmatrix_valid) + CalcTransMatrixD(mat,true); + + while ( n-- > 0 ) + { + *val = TransformF3D34(&mat->trans_matrix,val); + val = (float3*)( (u8*)val + off ); + } + } + else if (mat->transform_enabled) + { + while ( n-- > 0 ) + { + val->x = val->x * mat->norm_scale.x + mat->norm_translate.x; + val->y = val->y * mat->norm_scale.y + mat->norm_translate.y; + val->z = val->z * mat->norm_scale.z + mat->norm_translate.z; + val = (float3*)( (u8*)val + off ); + } + } +} + +/////////////////////////////////////////////////////////////////////////////// + +void InvTransformF3NMatrixD +( + MatrixD_t * mat, // valid data structure + float3 * val, // value array to transform + int n, // number of 3D vectors in 'val' + uint off // if n>1: offset from one to next 'val' vector +) +{ + DASSERT(mat); + DASSERT(val); + DASSERT( n <= 1 || off >= sizeof(*val) ); + + N_MatrixD_inverse += n; + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if (mat->use_matrix) + { + if (!mat->imatrix_valid) + CalcMatrixD(mat,true); + + while ( n-- > 0 ) + { + *val = TransformF3D34(&mat->inv_matrix,val); + val = (float3*)( (u8*)val + off ); + } + } + else if (mat->transform_enabled) + { + double3 f; // multiplication is faster than division + f.x = 1.0 / mat->norm_scale.x; + f.y = 1.0 / mat->norm_scale.y; + f.z = 1.0 / mat->norm_scale.z; + + while ( n-- > 0 ) + { + val->x = ( val->x - mat->norm_translate.x ) * f.x; + val->y = ( val->y - mat->norm_translate.y ) * f.y; + val->z = ( val->z - mat->norm_translate.z ) * f.z; + val = (float3*)( (u8*)val + off ); + } + } +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// MatrixD_t: direct transform /////////////// +/////////////////////////////////////////////////////////////////////////////// + +double3 BaseTransformD3MatrixD +( + // transform using the base parameters + + MatrixD_t * mat, // valid data structure + const + double3 * val // value to transform +) +{ + DASSERT(mat); + DASSERT(val); + + if (!mat->valid) + InitializeMatrixD(mat); + + if ( mat->use_matrix > 1 ) + return TransformD3MatrixD(mat,val); + + double3 rot = mat->rotate_rad; + Mult3f(rot,rot,180.0/M_PI); + Add3(rot,rot,mat->rotate_deg); + + return TransformHelperD3MatrixD + ( &mat->scale, + &mat->scale_origin, + &mat->shift, + &rot, + mat->rotate_origin+0, + mat->rotate_origin+1, + mat->rotate_origin+2, + &mat->translate, + val + ); +} + +/////////////////////////////////////////////////////////////////////////////// + +double3 NormTransformD3MatrixD +( + // transform using the norm parameters + + MatrixD_t * mat, // valid data structure + const + double3 * val // value to transform +) +{ + DASSERT(mat); + DASSERT(val); + + if ( !mat->norm_valid && !mat->tmatrix_valid ) + CalcNormMatrixD(mat); + + if ( mat->use_matrix > 1 ) + return TransformD3MatrixD(mat,val); + + return TransformHelperD3MatrixD + ( &mat->norm_scale, + 0, + 0, + &mat->norm_rotate_deg, + 0, + 0, + 0, + &mat->norm_translate, + val + ); +} + +/////////////////////////////////////////////////////////////////////////////// + +double3 TransformHelperD3MatrixD +( + // transform using vectors + + double3 *scale, // NULL or scale vector + double3 *scale_origin, // NULL or origin for scaling + double3 *shift, // NULL or shift vector + double3 *rotate_deg, // NULL or rotation in degree + double3 *xrot_origin, // NULL or origin for x-rotation + double3 *yrot_origin, // NULL or origin for y-rotation + double3 *zrot_origin, // NULL or origin for z-rotation + double3 *translate, // NULL or translation vector + + const + double3 * val // value to transform +) +{ + DASSERT(val); + + double3 res; //, null3 = { 0.0, 0.0, 0.0 }; + + + //--- scale + + if (!scale) + res = *val; + else if (scale_origin) + { + res.x = ( val->x - scale_origin->x ) * scale->x + scale_origin->x; + res.y = ( val->y - scale_origin->y ) * scale->y + scale_origin->y; + res.z = ( val->z - scale_origin->z ) * scale->z + scale_origin->z; + } + else + { + res.x = val->x * scale->x; + res.y = val->y * scale->y; + res.z = val->z * scale->z; + } + + + //--- shift + + if (shift) + { + res.x += shift->x; + res.y += shift->y; + res.z += shift->z; + } + + + //--- rotate + + if (rotate_deg) + { + uint xyz; + for ( xyz = 0; xyz < 3; xyz++ ) + { + const double deg = fmod(rotate_deg->v[xyz]+180.0,360.0) - 180.0; + if ( fabs(deg) >= MIN_DEGREE ) + { + uint a = xyz + 2; if ( a >= 3 ) a -= 3; + uint b = xyz + 1; if ( b >= 3 ) b -= 3; + double rad = deg * (M_PI/180.0); + + if (xrot_origin) + { + const double da = res.v[a] - xrot_origin->v[a]; + const double db = res.v[b] - xrot_origin->v[b]; + rad += atan2(da,db); + const double len = sqrt(da*da+db*db); + + res.v[a] = sin(rad) * len + xrot_origin->v[a]; + res.v[b] = cos(rad) * len + xrot_origin->v[b]; + } + else + { + rad += atan2(res.v[a],res.v[b]); + const double len = sqrt(res.v[a]*res.v[a]+res.v[b]*res.v[b]); + + res.v[a] = sin(rad) * len; + res.v[b] = cos(rad) * len; + } + } + xrot_origin = yrot_origin; + yrot_origin = zrot_origin; + } + } + + + //--- translate + + if (translate) + { + res.x += translate->x; + res.y += translate->y; + res.z += translate->z; + } + + return res; +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// MatrixD_t: printing /////////////// +/////////////////////////////////////////////////////////////////////////////// + +static ccp print_matrix_stat + ( MatrixD_t * mat, char * buf, uint bufsize, bool for_inverse ) +{ + DASSERT(buf); + DASSERT(bufsize>10); + + char *dest = buf, *bufend = buf + bufsize; + if (mat->scale_enabled) + dest = StringCopyE(dest,bufend,"+scale"); + if (mat->rotate_enabled) + dest = StringCopyE(dest,bufend,"+rotate"); + if (mat->translate_enabled) + dest = StringCopyE(dest,bufend,"+translate"); + + if ( mat->use_matrix > 1 && !for_inverse ) + dest = StringCopyE(dest,bufend,",source"); + else if (mat->use_matrix) + dest = StringCopyE(dest,bufend,",required"); + + if ( dest == buf ) + StringCopyE(dest,bufend,"+no transformation"); + + return buf+1; +} + +/////////////////////////////////////////////////////////////////////////////// + +void PrintMatrixD +( + FILE * f, // file to print to + uint indent, // indention + ccp eol, // not NULL: print as 'end of line' + + MatrixD_t * mat, // valid data structure + uint create, // bitfield for setup: + // 1: create normals if invalid + // 2: create transform matrix if invalid + // 4: create inverse matrix if invalid + uint print // bitfield print selection: + // 1: print base vectors + // 2: print normalized values, if valid + // 4: print transformation matrix, if valid + // 8: print inverse matrix, if valid + // 0x10: print also header, if only 1 section is print + // 0x20: print all, don't suppress NULL vectors + // 0x40: print an additional EOL behind each section +) +{ + DASSERT(f); + DASSERT(mat); + + //--- setup + + indent = NormalizeIndent(indent); + if (!eol) + eol = "\n"; + ccp sep = print & 0x40 ? eol : ""; + + if ( create & 4 ) + CalcMatrixD(mat,true); + else if ( create & 2 ) + CalcTransMatrixD(mat,true); + else if ( create & 1 ) + CalcNormMatrixD(mat); + else if (!mat->valid) + InitializeMatrixD(mat); + + if (!mat->norm_valid) + print &= ~2; + if (!mat->tmatrix_valid) + print &= ~4; + else if (mat->use_matrix>1) + print &= ~1; + if (!mat->imatrix_valid) + print &= ~8; + + const uint print_all = print & 0x20; + uint print_head = print & 0x10; + print &= 0x0f; + if ( print != 1 && print != 2 && print != 4 && print != 8 ) + print_head = 1; + + + //--- print base vectors + + if ( print & 1 ) + { + if (print_head) + fprintf(f,"%*sBase Vectors:%s", indent,"", eol ); + + if ( print_all || !IsOne3(mat->scale) ) + fprintf(f,"%*s Scale: %11.3f %11.3f %11.3f%s", + indent,"", + mat->scale.x, mat->scale.y, mat->scale.z, + eol ); + + if ( print_all || !IsNull3(mat->scale_origin) ) + fprintf(f,"%*s Origin: %11.3f %11.3f %11.3f%s", + indent,"", + mat->scale_origin.x, mat->scale_origin.y, mat->scale_origin.z, + eol ); + + if ( print_all || !IsNull3(mat->shift) ) + fprintf(f,"%*s Shift: %11.3f %11.3f %11.3f%s", + indent,"", + mat->shift.x, mat->shift.y, mat->shift.z, + eol ); + + bool print_rot = false; + if ( print_all || !IsNull3(mat->rotate_deg) ) + { + print_rot = true; + fprintf(f,"%*s Rotate/deg: %11.3f %11.3f %11.3f%s", + indent,"", + mat->rotate_deg.x, mat->rotate_deg.y, mat->rotate_deg.z, + eol ); + } + + if ( print_all || !IsNull3(mat->rotate_rad) ) + { + print_rot = true; + fprintf(f,"%*s Rotate/rad: %11.3f %11.3f %11.3f%s", + indent,"", + mat->rotate_rad.x, mat->rotate_rad.y, mat->rotate_rad.z, + eol ); + } + + if (print_rot) + { + double3 *d = mat->rotate_origin; + uint xyz; + for ( xyz = 0; xyz < 3; xyz++, d++ ) + if ( print_all || !IsNull3(*d) ) + fprintf(f,"%*s %c-origin: %11.3f %11.3f %11.3f%s", + indent,"", 'x'+xyz, + d->x, d->y, d->z, + eol ); + } + + if ( print_all || !IsNull3(mat->translate) ) + fprintf(f,"%*s Translate: %11.3f %11.3f %11.3f%s", + indent,"", + mat->translate.x, mat->translate.y, mat->translate.z, + eol ); + + fputs(sep,f); + } + + + //--- print normalized vectors + + if ( print & 2 ) + { + if (print_head) + fprintf(f,"%*sNormalized Vectors (seq=%u):%s", + indent,"", mat->sequence_number, eol ); + + fprintf(f, + "%*s Scale: %11.3f %11.3f %11.3f [%c%c%c]%s" + "%*s Rotate/deg: %11.3f %11.3f %11.3f [%c%c%c]%s" + #if TEST0 + "%*s Rotate/rad: %11.3f %11.3f %11.3f%s" + #endif + "%*s Translate: %11.3f %11.3f %11.3f [%c%c%c]%s", + + indent,"", + mat->norm_scale.x, mat->norm_scale.y, mat->norm_scale.z, + mat->scale_enabled & 1 ? 'x' : '-', + mat->scale_enabled & 2 ? 'y' : '-', + mat->scale_enabled & 4 ? 'z' : '-', + eol, + + indent,"", + mat->norm_rotate_deg.x, mat->norm_rotate_deg.y, mat->norm_rotate_deg.z, + mat->rotate_enabled & 1 ? 'x' : '-', + mat->rotate_enabled & 2 ? 'y' : '-', + mat->rotate_enabled & 4 ? 'z' : '-', + eol, + #if TEST0 + indent,"", + mat->norm_rotate_rad.x, mat->norm_rotate_rad.y, mat->norm_rotate_rad.z, + eol, + #endif + indent,"", + mat->norm_translate.x, mat->norm_translate.y, mat->norm_translate.z, + mat->translate_enabled & 1 ? 'x' : '-', + mat->translate_enabled & 2 ? 'y' : '-', + mat->translate_enabled & 4 ? 'z' : '-', + eol ); + + if ( print_all || !IsNull3(mat->norm_pos2d) ) + fprintf(f,"%*s 2D tf-base: %11.3f %11.3f %11.3f (for 2D transform)%s", + indent,"", + mat->norm_pos2d.x, mat->norm_pos2d.y, mat->norm_pos2d.z, + eol ); + + fputs(sep,f); + } + + + //--- print transformation matrix + + char buf[50]; + if ( print & 4 ) + { + if (print_head) + fprintf(f,"%*sTransformation Matrix (%s):%s", + indent,"", print_matrix_stat(mat,buf,sizeof(buf),false), eol ); + + fprintf(f, + "%*s x' = %11.3f * x + %11.3f * y + %11.3f * z + %11.3f%s" + "%*s y' = %11.3f * x + %11.3f * y + %11.3f * z + %11.3f%s" + "%*s z' = %11.3f * x + %11.3f * y + %11.3f * z + %11.3f%s" + "%s", + + indent,"", + mat->trans_matrix.x[0], + mat->trans_matrix.x[1], + mat->trans_matrix.x[2], + mat->trans_matrix.x[3], + eol, + + indent,"", + mat->trans_matrix.y[0], + mat->trans_matrix.y[1], + mat->trans_matrix.y[2], + mat->trans_matrix.y[3], + eol, + + indent,"", + mat->trans_matrix.z[0], + mat->trans_matrix.z[1], + mat->trans_matrix.z[2], + mat->trans_matrix.z[3], + eol, sep ); + } + + + //--- print inverse matrix + + if ( print & 8 ) + { + if (print_head) + fprintf(f,"%*sInverse Matrix (%s):%s", + indent,"", print_matrix_stat(mat,buf,sizeof(buf),true), eol ); + + fprintf(f, + "%*s x' = %11.3f * x + %11.3f * y + %11.3f * z + %11.3f%s" + "%*s y' = %11.3f * x + %11.3f * y + %11.3f * z + %11.3f%s" + "%*s z' = %11.3f * x + %11.3f * y + %11.3f * z + %11.3f%s" + "%s", + + indent,"", + mat->inv_matrix.x[0], + mat->inv_matrix.x[1], + mat->inv_matrix.x[2], + mat->inv_matrix.x[3], + eol, + + indent,"", + mat->inv_matrix.y[0], + mat->inv_matrix.y[1], + mat->inv_matrix.y[2], + mat->inv_matrix.y[3], + eol, + + indent,"", + mat->inv_matrix.z[0], + mat->inv_matrix.z[1], + mat->inv_matrix.z[2], + mat->inv_matrix.z[3], + eol, sep ); + } +} + +// +/////////////////////////////////////////////////////////////////////////////// +/////////////// END /////////////// +/////////////////////////////////////////////////////////////////////////////// + -- cgit v1.3.1