math.c

一个用于智能手机的多媒体库适合S60 WinCE的跨平台开发库

/*
 *			GPAC - Multimedia Framework C SDK
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005
 *					All rights reserved
 *
 *  This file is part of GPAC / common tools sub-project
 *
 *  GPAC is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *   
 *  GPAC 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 Lesser General Public License for more details.
 *   
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 *
 *	fixed-point trigo routines taken from freetype
 *		Copyright 1996-2001, 2002, 2004 by                                   
 *		David Turner, Robert Wilhelm, and Werner Lemberg.
 *	License: FTL or GPL
 *
 */

#include <gpac/math.h>

u32 gf_get_bit_size(u32 MaxVal)
{
	u32 k=0;
	while ((s32) MaxVal > ((1<<k)-1) ) k+=1;
	return k;
}

#ifdef GPAC_FIXED_POINT

#ifndef __GNUC__
#pragma message("Compiling with fixed-point arithmetic")
#endif

/* the following is 0.2715717684432231 * 2^30 */
#define GF_TRIG_COSCALE  0x11616E8EUL

/* this table was generated for degrees */
#define GF_TRIG_MAX_ITERS  23

static const Fixed gf_trig_arctan_table[24] =
{
	4157273L, 2949120L, 1740967L, 919879L, 466945L, 234379L, 117304L,
	58666L, 29335L, 14668L, 7334L, 3667L, 1833L, 917L, 458L, 229L, 115L,
	57L, 29L, 14L, 7L, 4L, 2L, 1L
};

/* the Cordic shrink factor, multiplied by 2^32 */
#define GF_TRIG_SCALE    1166391785  /* 0x4585BA38UL */

#define GF_ANGLE_PI		(180<<16)
#define GF_ANGLE_PI2	(90<<16)
#define GF_ANGLE_2PI	(360<<16)

#define GF_PAD_FLOOR( x, n )  ( (x) & ~((n)-1) )
#define GF_PAD_ROUND( x, n )  GF_PAD_FLOOR( (x) + ((n)/2), n )
#define GF_PAD_CEIL( x, n )   GF_PAD_FLOOR( (x) + ((n)-1), n )

static GFINLINE s32 gf_trig_prenorm(GF_Point2D *vec)
{
	Fixed  x, y, z;
	s32 shift;
	x = vec->x;
	y = vec->y;
	z     = ( ( x >= 0 ) ? x : - x ) | ( (y >= 0) ? y : -y );
	shift = 0;
	if ( z < ( 1L << 27 ) ) {
      do {
		  shift++;
		  z <<= 1;
      }
	  while ( z < ( 1L << 27 ) );

      vec->x = x << shift;
      vec->y = y << shift;
    }    
	else if ( z > ( 1L << 28 ) ) {
		do {
			shift++;
			z >>= 1;
		} while ( z > ( 1L << 28 ) );

		vec->x = x >> shift;
		vec->y = y >> shift;
		shift  = -shift;
    }
    return shift;
}

#define ANGLE_RAD_TO_DEG(_th) ((s32) ( (((s64)_th)*5729582)/100000))
#define ANGLE_DEG_TO_RAD(_th) ((s32) ( (((s64)_th)*100000)/5729582))

static GFINLINE void gf_trig_pseudo_polarize(GF_Point2D *vec)
{
	Fixed         theta;
	Fixed         yi, i;
	Fixed         x, y;
	const Fixed  *arctanptr;

	x = vec->x;
	y = vec->y;

	/* Get the vector into the right half plane */
	theta = 0;
	if ( x < 0 ) {
		x = -x;
		y = -y;
		theta = 2 * GF_ANGLE_PI2;
	}

	if ( y > 0 )
		theta = - theta;

	arctanptr = gf_trig_arctan_table;

	if ( y < 0 ) {
		/* Rotate positive */
		yi     = y + ( x << 1 );
		x      = x - ( y << 1 );
		y      = yi;
		theta -= *arctanptr++;  /* Subtract angle */
	} else {
		/* Rotate negative */
		yi     = y - ( x << 1 );
		x      = x + ( y << 1 );
		y      = yi;
		theta += *arctanptr++;  /* Add angle */
	}

	i = 0;
	do {
		if ( y < 0 ) {
			/* Rotate positive */
			yi     = y + ( x >> i );
			x      = x - ( y >> i );
			y      = yi;
			theta -= *arctanptr++;
		} else {
			/* Rotate negative */
			yi     = y - ( x >> i );
			x      = x + ( y >> i );
			y      = yi;
			theta += *arctanptr++;
		}
	}
	while ( ++i < GF_TRIG_MAX_ITERS );

	/* round theta */
	if ( theta >= 0 ) 
		theta = GF_PAD_ROUND( theta, 32 );
	else
		theta = - GF_PAD_ROUND( -theta, 32 );

	vec->x = x;
	vec->y = ANGLE_DEG_TO_RAD(theta);
}

GF_EXPORT
Fixed gf_atan2(Fixed dy, Fixed dx)
{
	GF_Point2D v;
	if ( dx == 0 && dy == 0 ) return 0;
	v.x = dx;
	v.y = dy;
	gf_trig_prenorm( &v );
	gf_trig_pseudo_polarize( &v );
	return v.y;
}

/*define one of these to enable IntelGPP support and link to gpp_WMMX40_d.lib (debug) or gpp_WMMX40_r.lib (release)
this is not configured by default for lack of real perf increase.*/
#if defined(GPAC_USE_IGPP_HP) || defined(GPAC_USE_IGPP)

#include <gpp.h>

GF_EXPORT
Fixed gf_invfix(Fixed a)
{
	Fixed res;
	if (!a) return (s32)0x7FFFFFFFL;
	gppInv_16_32s(a, &res);
	return res;
}
GF_EXPORT
Fixed gf_divfix(Fixed a, Fixed b)
{
	Fixed res;
	if (!a) return 0;
    else if (!b) return (a<0) ? -(s32)0x7FFFFFFFL : (s32)0x7FFFFFFFL;
	else if (a==FIX_ONE) gppInv_16_32s(b, &res);
	else gppDiv_16_32s(a, b, &res);
	return res;
}

GF_EXPORT
Fixed gf_mulfix(Fixed a, Fixed b)
{
	Fixed  res;
	if (!a || !b) return 0;
	else if (b == FIX_ONE) return a;
	else gppMul_16_32s(a, b, &res);
	return res;
}

GF_EXPORT
Fixed gf_sqrt(Fixed x)
{
	Fixed res;
#ifdef GPAC_USE_IGPP_HP
	gppSqrtHP_16_32s(x, &res);
#else
	gppSqrtLP_16_32s(x, &res);
#endif
	return res;
}

GF_EXPORT
Fixed gf_cos(Fixed angle)
{
	Fixed res;
#ifdef GPAC_USE_IGPP_HP
	gppCosHP_16_32s(angle, &res);
#else
	gppCosLP_16_32s(angle, &res);
#endif
	return res;
}

GF_EXPORT
Fixed gf_sin(Fixed angle)
{
	Fixed res;
#ifdef GPAC_USE_IGPP_HP
	gppSinHP_16_32s (angle, &res);
#else
	gppSinLP_16_32s (angle, &res);
#endif
	return res;
}


GF_EXPORT
Fixed gf_tan(Fixed angle)
{
	Fixed cosa, sina;
#ifdef GPAC_USE_IGPP_HP
	gppSinCosHP_16_32s(angle, &sina, &cosa);
#else
	gppSinCosLP_16_32s(angle, &sina, &cosa);
#endif
	if (!cosa) return (sina<0) ? -GF_PI2 : GF_PI2;
	return gf_divfix(sina, cosa);
}

GF_EXPORT
GF_Point2D gf_v2d_from_polar(Fixed length, Fixed angle)
{
	GF_Point2D vec;
	Fixed cosa, sina;
#ifdef GPAC_USE_IGPP_HP
	gppSinCosHP_16_32s(angle, &sina, &cosa);
#else
	gppSinCosLP_16_32s(angle, &sina, &cosa);
#endif
    vec.x = gf_mulfix(length, cosa);
    vec.y = gf_mulfix(length, sina);
	return vec;
}

GF_EXPORT
Fixed gf_v2d_len(GF_Point2D *vec)
{
	Fixed a, b, res;
	if (!vec->x) return ABS(vec->y);
	if (!vec->y) return ABS(vec->x);
	gppSquare_16_32s(vec->x, &a);
	gppSquare_16_32s(vec->y, &b);
#ifdef GPAC_USE_IGPP_HP
	gppSqrtHP_16_32s(a+b, &res);
#else
	gppSqrtLP_16_32s(a+b, &res);
#endif
	return res;
}

#else

GF_EXPORT
Fixed gf_invfix(Fixed a)
{
	if (!a) return (s32)0x7FFFFFFFL;
	return gf_divfix(FIX_ONE, a);
}

GF_EXPORT
Fixed gf_divfix(Fixed a, Fixed b)
{
	s32 s;
	u32 q;

    s = 1;
    if ( a < 0 ) { a = -a; s = -1; }
    if ( b < 0 ) { b = -b; s = -s; }

    if ( b == 0 )
		/* check for division by 0 */
		q = 0x7FFFFFFFL;
    else
		/* compute result directly */
		q = (u32)( ( ( (s64)a << 16 ) + ( b >> 1 ) ) / b );
    return ( s < 0 ? -(s32)q : (s32)q );
}

GF_EXPORT
Fixed gf_mulfix(Fixed a, Fixed b)
{
	Fixed  s;
	u32 ua, ub;
	if ( !a || (b == 0x10000L)) return a;
	if (!b) return 0;

	s  = a; a = ABS(a);
	s ^= b; b = ABS(b);

	ua = (u32)a;
	ub = (u32)b;

	if ((ua <= 2048) && (ub <= 1048576L)) {
		ua = ( ua * ub + 0x8000L ) >> 16;
	} else {
		u32 al = ua & 0xFFFFL;
		ua = ( ua >> 16 ) * ub +  al * ( ub >> 16 ) + ( ( al * ( ub & 0xFFFFL ) + 0x8000L ) >> 16 );
	}
	if (ua & 0x80000000) s=-s;
	return ( s < 0 ? -(Fixed)(s32)ua : (Fixed)ua );
}


GF_EXPORT
Fixed gf_sqrt(Fixed x)
{
	u32 root, rem_hi, rem_lo, test_div;
	s32 count;
	root = 0;
	if ( x > 0 ) {
		rem_hi = 0;
		rem_lo = x;
		count  = 24;
		do {
			rem_hi   = ( rem_hi << 2 ) | ( rem_lo >> 30 );
			rem_lo <<= 2;
			root   <<= 1;
			test_div = ( root << 1 ) + 1;

			if ( rem_hi >= test_div ) {
				rem_hi -= test_div;
				root   += 1;
			}
		} while ( --count );
	}

	return (Fixed) root;
}


/* multiply a given value by the CORDIC shrink factor */
static Fixed gf_trig_downscale(Fixed  val)
{
	Fixed  s;
	s64 v;
	s   = val;
	val = ( val >= 0 ) ? val : -val;
#ifdef _MSC_VER
	v = ( val * (s64)GF_TRIG_SCALE ) + 0x100000000;
#else
	v = ( val * (s64)GF_TRIG_SCALE ) + 0x100000000ULL;
#endif
	val = (Fixed)( v >> 32 );
	return ( s >= 0 ) ? val : -val;
}

static void gf_trig_pseudo_rotate(GF_Point2D*  vec, Fixed theta)
{
    s32 i;
    Fixed x, y, xtemp;
    const Fixed  *arctanptr;

	/*trig funcs are in degrees*/
	theta = ANGLE_RAD_TO_DEG(theta);

    x = vec->x;
    y = vec->y;

    /* Get angle between -90 and 90 degrees */
    while ( theta <= -GF_ANGLE_PI2 ) {
		x = -x;
		y = -y;
		theta += GF_ANGLE_PI;
    }

    while ( theta > GF_ANGLE_PI2 ) {
		x = -x;
		y = -y;
		theta -= GF_ANGLE_PI;
    }

    /* Initial pseudorotation, with left shift */
    arctanptr = gf_trig_arctan_table;
    if ( theta < 0 ) {
		xtemp  = x + ( y << 1 );
		y      = y - ( x << 1 );
		x      = xtemp;
		theta += *arctanptr++;
    } else {
		xtemp  = x - ( y << 1 );
		y      = y + ( x << 1 );
		x      = xtemp;
		theta -= *arctanptr++;
    }
    /* Subsequent pseudorotations, with right shifts */
    i = 0;
    do {
		if ( theta < 0 ) {
			xtemp  = x + ( y >> i );
			y      = y - ( x >> i );
			x      = xtemp;
			theta += *arctanptr++;
		} else {
			xtemp  = x - ( y >> i );
			y      = y + ( x >> i );
			x      = xtemp;
			theta -= *arctanptr++;
		}
    } while ( ++i < GF_TRIG_MAX_ITERS );

    vec->x = x;
    vec->y = y;
}

/* these macros return 0 for positive numbers, and -1 for negative ones */
#define GF_SIGN_LONG( x )   ( (x) >> ( 32 - 1 ) )
#define GF_SIGN_INT( x )    ( (x) >> ( 32 - 1 ) )
#define GF_SIGN_INT32( x )  ( (x) >> 31 )
#define GF_SIGN_INT16( x )  ( (x) >> 15 )

static void gf_v2d_rotate(GF_Point2D *vec, Fixed angle)
{
    s32 shift;
    GF_Point2D v;
    
	v.x   = vec->x;
    v.y   = vec->y;
	
    if ( angle && ( v.x != 0 || v.y != 0 ) ) {
		shift = gf_trig_prenorm( &v );
		gf_trig_pseudo_rotate( &v, angle );
		v.x = gf_trig_downscale( v.x );
		v.y = gf_trig_downscale( v.y );

		if ( shift > 0 ) {
			s32 half = 1L << ( shift - 1 );

			vec->x = ( v.x + half + GF_SIGN_LONG( v.x ) ) >> shift;
			vec->y = ( v.y + half + GF_SIGN_LONG( v.y ) ) >> shift;
		} else {
			shift  = -shift;
			vec->x = v.x << shift;
			vec->y = v.y << shift;
		}
    }
}

GF_EXPORT
Fixed gf_v2d_len(GF_Point2D *vec)
{
    s32 shift;
	GF_Point2D v;
    v = *vec;

    /* handle trivial cases */
    if ( v.x == 0 ) {
		return ( v.y >= 0 ) ? v.y : -v.y;
    } else if ( v.y == 0 ) {
		return ( v.x >= 0 ) ? v.x : -v.x;
    }

    /* general case */
    shift = gf_trig_prenorm( &v );
    gf_trig_pseudo_polarize( &v );
    v.x = gf_trig_downscale( v.x );
    if ( shift > 0 )
		return ( v.x + ( 1 << ( shift - 1 ) ) ) >> shift;

    return v.x << -shift;
}


GF_EXPORT
void gf_v2d_polarize(GF_Point2D *vec, Fixed *length, Fixed *angle)
{
    s32 shift;
	GF_Point2D v;
    v = *vec;
    if ( v.x == 0 && v.y == 0 )
		return;

    shift = gf_trig_prenorm( &v );
    gf_trig_pseudo_polarize( &v );
    v.x = gf_trig_downscale( v.x );
    *length = ( shift >= 0 ) ? ( v.x >> shift ) : ( v.x << -shift );
    *angle  = v.y;
}

GF_EXPORT
GF_Point2D gf_v2d_from_polar(Fixed length, Fixed angle)
{
	GF_Point2D vec;
    vec.x = length;
    vec.y = 0;
    gf_v2d_rotate(&vec, angle);
	return vec;
}

GF_EXPORT
Fixed gf_cos(Fixed angle)
{
	GF_Point2D v;
	v.x = GF_TRIG_COSCALE >> 2;
	v.y = 0;
	gf_trig_pseudo_rotate( &v, angle );
	return v.x / ( 1 << 12 );
}
GF_EXPORT
Fixed gf_sin(Fixed angle)
{
	return gf_cos( GF_PI2 - angle );
}
GF_EXPORT
Fixed gf_tan(Fixed angle)
{
	GF_Point2D v;
    v.x = GF_TRIG_COSCALE >> 2;
    v.y = 0;
    gf_trig_pseudo_rotate( &v, angle );
    return gf_divfix( v.y, v.x );
}

#endif

/*common parts*/
GF_EXPORT
Fixed gf_muldiv(Fixed a, Fixed b, Fixed c)