math.c

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

{
    s32 s, d;
	if (!b || !a) return 0;
    s = 1;
    if ( a < 0 ) { a = -a; s = -1; }
    if ( b < 0 ) { b = -b; s = -s; }
    if ( c < 0 ) { c = -c; s = -s; }

    d = (s32)( c > 0 ? ( (s64)a * b + ( c >> 1 ) ) / c : 0x7FFFFFFFL);
    return (Fixed) (( s > 0 ) ? d : -d);
}


GF_EXPORT
Fixed gf_ceil(Fixed a)
{
	return (a >= 0) ? (a + 0xFFFFL) & ~0xFFFFL : -((-a + 0xFFFFL) & ~0xFFFFL);
}

GF_EXPORT
Fixed gf_floor(Fixed a)
{
    return (a >= 0) ? (a & ~0xFFFFL) : -((-a) & ~0xFFFFL);
}


/*FIXME*/
GF_EXPORT
Fixed gf_acos(Fixed angle)
{
	return FLT2FIX( (Float) acos(FIX2FLT(angle)));
}

/*FIXME*/
GF_EXPORT
Fixed gf_asin(Fixed angle)
{
	return FLT2FIX( (Float) asin(FIX2FLT(angle)));
}


#else


GF_EXPORT
GF_Point2D gf_v2d_from_polar(Fixed length, Fixed angle)
{
	GF_Point2D vec;
    vec.x = length*(Float) cos(angle);
    vec.y = length*(Float) sin(angle);
	return vec;
}

GF_EXPORT
Fixed gf_v2d_len(GF_Point2D *vec)
{
	if (!vec->x) return ABS(vec->y);
	if (!vec->y) return ABS(vec->x);
	return (Fixed) sqrt(vec->x*vec->x + vec->y*vec->y);
}

/*

Fixed gf_cos(_a) { return (Float) cos(_a); }
Fixed gf_sin(_a) { return (Float) sin(_a); }
Fixed gf_tan(_a) { return (Float) tan(_a); }
Fixed gf_atan2(_y, _x) { return (Float) atan2(_y, _x); }
Fixed gf_sqrt(_x) { return (Float) sqrt(_x); }
Fixed gf_ceil(_a) { return (Float) ceil(_a); }
Fixed gf_floor(_a) { return (Float) floor(_a); }
Fixed gf_acos(_a) { return (Float) acos(_a); }
Fixed gf_asin(_a) { return (Float) asin(_a); }

*/

#endif


GF_EXPORT
Fixed gf_angle_diff(Fixed angle1, Fixed angle2)
{
    Fixed delta = angle2 - angle1;
#ifdef GPAC_FIXED_POINT
    delta %= GF_2PI;
	if (delta < 0) delta += GF_2PI;
    if (delta > GF_PI) delta -= GF_2PI;
#else
	while (delta < 0) delta += GF_2PI;
    while (delta > GF_PI) delta -= GF_2PI;
#endif
    return delta;
}



/*
	2D MATRIX TOOLS
 */

GF_EXPORT
void gf_mx2d_add_matrix(GF_Matrix2D *_this, GF_Matrix2D *from)
{
	GF_Matrix2D bck;
	if (!_this || !from) return;

	if (gf_mx2d_is_identity(*from)) return;
	else if (gf_mx2d_is_identity(*_this)) {
		gf_mx2d_copy(*_this, *from);
		return;
	}
	gf_mx2d_copy(bck, *_this);
	_this->m[0] = gf_mulfix(from->m[0], bck.m[0]) + gf_mulfix(from->m[1], bck.m[3]);
	_this->m[1] = gf_mulfix(from->m[0], bck.m[1]) + gf_mulfix(from->m[1], bck.m[4]);
	_this->m[2] = gf_mulfix(from->m[0], bck.m[2]) + gf_mulfix(from->m[1], bck.m[5]) + from->m[2];
	_this->m[3] = gf_mulfix(from->m[3], bck.m[0]) + gf_mulfix(from->m[4], bck.m[3]);
	_this->m[4] = gf_mulfix(from->m[3], bck.m[1]) + gf_mulfix(from->m[4], bck.m[4]);
	_this->m[5] = gf_mulfix(from->m[3], bck.m[2]) + gf_mulfix(from->m[4], bck.m[5]) + from->m[5];
}


GF_EXPORT
void gf_mx2d_pre_multiply(GF_Matrix2D *_this, GF_Matrix2D *with)
{
	GF_Matrix2D bck;
	if (!_this || !with) return;

	if (gf_mx2d_is_identity(*with)) return;
	else if (gf_mx2d_is_identity(*_this)) {
		gf_mx2d_copy(*_this, *with);
		return;
	}
	gf_mx2d_copy(bck, *_this);
	_this->m[0] = gf_mulfix(bck.m[0], with->m[0]) + gf_mulfix(bck.m[1], with->m[3]);
	_this->m[1] = gf_mulfix(bck.m[0], with->m[1]) + gf_mulfix(bck.m[1], with->m[4]);
	_this->m[2] = gf_mulfix(bck.m[0], with->m[2]) + gf_mulfix(bck.m[1], with->m[5]) + bck.m[2];
	_this->m[3] = gf_mulfix(bck.m[3], with->m[0]) + gf_mulfix(bck.m[4], with->m[3]);
	_this->m[4] = gf_mulfix(bck.m[3], with->m[1]) + gf_mulfix(bck.m[4], with->m[4]);
	_this->m[5] = gf_mulfix(bck.m[3], with->m[2]) + gf_mulfix(bck.m[4], with->m[5]) + bck.m[5];
}


GF_EXPORT
void gf_mx2d_add_translation(GF_Matrix2D *_this, Fixed cx, Fixed cy)
{
	GF_Matrix2D tmp;
	if (!_this || (!cx && !cy) ) return;
	gf_mx2d_init(tmp);
	tmp.m[2] = cx;
	tmp.m[5] = cy;
	gf_mx2d_add_matrix(_this, &tmp);
}


GF_EXPORT
void gf_mx2d_add_rotation(GF_Matrix2D *_this, Fixed cx, Fixed cy, Fixed angle)
{
	GF_Matrix2D tmp;
	if (!_this) return;
	gf_mx2d_init(tmp);

	gf_mx2d_add_translation(_this, -cx, -cy);
	
	tmp.m[0] = gf_cos(angle);
	tmp.m[4] = tmp.m[0];
	tmp.m[3] = gf_sin(angle);
	tmp.m[1] = -1 * tmp.m[3];
	gf_mx2d_add_matrix(_this, &tmp);
	gf_mx2d_add_translation(_this, cx, cy);
}

GF_EXPORT
void gf_mx2d_add_scale(GF_Matrix2D *_this, Fixed scale_x, Fixed scale_y)
{
	GF_Matrix2D tmp;
	if (!_this || ((scale_x==FIX_ONE) && (scale_y==FIX_ONE)) ) return;
	gf_mx2d_init(tmp);
	tmp.m[0] = scale_x;
	tmp.m[4] = scale_y;
	gf_mx2d_add_matrix(_this, &tmp);
}

GF_EXPORT
void gf_mx2d_add_scale_at(GF_Matrix2D *_this, Fixed scale_x, Fixed scale_y, Fixed cx, Fixed cy, Fixed angle)
{
	GF_Matrix2D tmp;
	if (!_this) return;
	gf_mx2d_init(tmp);
	if (angle) {
		gf_mx2d_add_rotation(_this, cx, cy, -angle);
	}
	tmp.m[0] = scale_x;
	tmp.m[4] = scale_y;
	gf_mx2d_add_matrix(_this, &tmp);
	if (angle) gf_mx2d_add_rotation(_this, cx, cy, angle);
}

GF_EXPORT
void gf_mx2d_add_skew(GF_Matrix2D *_this, Fixed skew_x, Fixed skew_y)
{
	GF_Matrix2D tmp;
	if (!_this || (!skew_x && !skew_y) ) return;
	gf_mx2d_init(tmp);
	tmp.m[1] = skew_x;
	tmp.m[3] = skew_y;
	gf_mx2d_add_matrix(_this, &tmp);
}

GF_EXPORT
void gf_mx2d_add_skew_x(GF_Matrix2D *_this, Fixed angle)
{
	GF_Matrix2D tmp;
	if (!_this) return;
	gf_mx2d_init(tmp);
	tmp.m[1] = gf_tan(angle);
	tmp.m[3] = 0;
	gf_mx2d_add_matrix(_this, &tmp);
}

GF_EXPORT
void gf_mx2d_add_skew_y(GF_Matrix2D *_this, Fixed angle)
{
	GF_Matrix2D tmp;
	if (!_this) return;
	gf_mx2d_init(tmp);
	tmp.m[1] = 0;
	tmp.m[3] = gf_tan(angle);
	gf_mx2d_add_matrix(_this, &tmp);
}

static Fixed gf_mx2d_get_determinent(GF_Matrix2D *_this)
{
	if (_this)
		return gf_mulfix(_this->m[0], _this->m[4]) - gf_mulfix(_this->m[1], _this->m[3]);
	return 0;
}

GF_EXPORT
void gf_mx2d_inverse(GF_Matrix2D *_this)
{
	Fixed det;
	GF_Matrix2D tmp;
	if(!_this) return;
	if (gf_mx2d_is_identity(*_this)) return;
	det = gf_mx2d_get_determinent(_this);
	if (!det) {
		gf_mx2d_init(*_this);
		return;
	}
	tmp.m[0] = gf_divfix(_this->m[4], det);
	tmp.m[1] = -1 *  gf_divfix(_this->m[1], det);
	tmp.m[2] = gf_mulfix(_this->m[1], _this->m[5]) - gf_mulfix(_this->m[4], _this->m[2]);
	tmp.m[2] = gf_divfix(tmp.m[2], det);
	tmp.m[3] = -1 * gf_divfix(_this->m[3], det);
	tmp.m[4] = gf_divfix(_this->m[0], det);
	tmp.m[5] = gf_mulfix(_this->m[3], _this->m[2]) - gf_mulfix(_this->m[0],_this->m[5]);
	tmp.m[5] = gf_divfix(tmp.m[5], det);
	gf_mx2d_copy(*_this, tmp);
}

Bool gf_mx2d_decompose(GF_Matrix2D *mx, GF_Point2D *scale, Fixed *rotate, GF_Point2D *translate)
{
	Fixed det, angle;
	Fixed tmp[6];
	if(!mx) return 0;

	memcpy(tmp, mx->m, sizeof(Fixed)*6);
	translate->x = tmp[2];
	translate->y = tmp[5];

	/*check ac+bd=0*/
	det = gf_mulfix(tmp[0], tmp[3]) + gf_mulfix(tmp[1], tmp[4]);
	if (ABS(det) > FIX_EPSILON) {
		scale->x = scale->y = 0;
		*rotate = 0;
		return 0;
	}
	angle = gf_atan2(tmp[3], tmp[4]);
	if (angle < FIX_EPSILON) {
		scale->x = tmp[0];
		scale->y = tmp[4];
	} else {
		det = gf_cos(angle);
		scale->x = gf_divfix(tmp[0], det);
		scale->y = gf_divfix(tmp[4], det);
	}
	*rotate = angle;
	return 1;
}

GF_EXPORT
void gf_mx2d_apply_coords(GF_Matrix2D *_this, Fixed *x, Fixed *y)
{
	Fixed _x, _y;
	if (!_this || !x || !y) return;
	_x = gf_mulfix(*x, _this->m[0]) + gf_mulfix(*y, _this->m[1]) + _this->m[2];
	_y = gf_mulfix(*x, _this->m[3]) + gf_mulfix(*y, _this->m[4]) + _this->m[5];
	*x = _x;
	*y = _y;
}

GF_EXPORT
void gf_mx2d_apply_point(GF_Matrix2D *_this, GF_Point2D *pt)
{
	gf_mx2d_apply_coords(_this, &pt->x, &pt->y);
}

GF_EXPORT
void gf_mx2d_apply_rect(GF_Matrix2D *_this, GF_Rect *rc)
{
	GF_Point2D c1, c2, c3, c4;
	c1.x = c2.x = rc->x;
	c3.x = c4.x = rc->x + rc->width;
	c1.y = c3.y = rc->y;
	c2.y = c4.y = rc->y - rc->height;
	gf_mx2d_apply_point(_this, &c1);
	gf_mx2d_apply_point(_this, &c2);
	gf_mx2d_apply_point(_this, &c3);
	gf_mx2d_apply_point(_this, &c4);
	rc->x = MIN(c1.x, MIN(c2.x, MIN(c3.x, c4.x)));
	rc->width = MAX(c1.x, MAX(c2.x, MAX(c3.x, c4.x))) - rc->x;
	rc->height = MIN(c1.y, MIN(c2.y, MIN(c3.y, c4.y)));
	rc->y = MAX(c1.y, MAX(c2.y, MAX(c3.y, c4.y)));
	rc->height = rc->y - rc->height;
	assert(rc->height>=0);
	assert(rc->width>=0);
}

/*
	RECTANGLE TOOLS
 */

/*transform rect to smallest covering integer pixels rect - this is needed to make sure clearing
of screen is correctly handled, otherwise we have troubles with bitmap hardware blitting (always integer)*/
GF_EXPORT
GF_IRect gf_rect_pixelize(GF_Rect *r)
{
	GF_IRect rc;
	rc.x = FIX2INT(gf_floor(r->x));
	rc.y = FIX2INT(gf_ceil(r->y));
	rc.width = FIX2INT(gf_ceil(r->x + r->width)) - rc.x;
	rc.height = rc.y - FIX2INT(gf_floor(r->y - r->height));
	return rc;
}


/*adds @rc2 to @rc1 - the new @rc1 contains the old @rc1 and @rc2*/
GF_EXPORT
void gf_rect_union(GF_Rect *rc1, GF_Rect *rc2) 
{
	if (!rc1->width || !rc1->height) {*rc1=*rc2; return;}
	if (rc2->x < rc1->x) { rc1->width += rc1->x - rc2->x; rc1->x = rc2->x; }
	if (rc2->x + rc2->width > rc1->x+rc1->width) rc1->width = rc2->x + rc2->width - rc1->x;
	if (rc2->y > rc1->y) { rc1->height += rc2->y - rc1->y; rc1->y = rc2->y; }
	if (rc2->y - rc2->height < rc1->y - rc1->height) rc1->height = rc1->y - rc2->y + rc2->height;
}

GF_EXPORT
GF_Rect gf_rect_center(Fixed w, Fixed h)
{
	GF_Rect rc;
	rc.x=-w/2; rc.y=h/2; rc.width=w; rc.height=h;
	return rc;
}

GF_EXPORT
Bool gf_rect_overlaps(GF_Rect rc1, GF_Rect rc2)
{
	if (! rc2.height || !rc2.width || !rc1.height || !rc1.width) return 0;
	if (rc2.x+rc2.width<=rc1.x) return 0;
	if (rc2.x>=rc1.x+rc1.width) return 0;
	if (rc2.y-rc2.height>=rc1.y) return 0;
	if (rc2.y<=rc1.y-rc1.height) return 0;
	return 1;
}

GF_EXPORT
Bool gf_rect_equal(GF_Rect rc1, GF_Rect rc2) 
{ 
	if ( (rc1.x == rc2.x)  && (rc1.y == rc2.y) && (rc1.width == rc2.width) && (rc1.height == rc2.height) )
		return 1;
	return 0;
}

#ifdef GPAC_FIXED_POINT

/* check if dimension is larger than FIX_ONE*/
#define IS_HIGH_DIM(_v)	((_v > FIX_ONE) || (_v < (s32)0xFFFF0000))
/* check if any vector dimension is larger than FIX_ONE*/
#define VEC_HIGH_MAG(_v)	(IS_HIGH_DIM(_v.x) || IS_HIGH_DIM(_v.y) || IS_HIGH_DIM(_v.z) )

GF_EXPORT
Fixed gf_vec_len(GF_Vec v)
{
	/*commented out atm - weird results (not enough precision?)...*/
//#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
#if 0
	Fixed res;
	gppVec3DLength_16_32s((GPP_VEC3D *) &v, &res);
	return res;
#else
	/*high-magnitude vector, use low precision on frac part to avoid overflow*/
	if (VEC_HIGH_MAG(v)) {
		v.x>>=8;
		v.y>>=8;
		v.z>>=8;
		return gf_sqrt( gf_mulfix(v.x, v.x) + gf_mulfix(v.y, v.y) + gf_mulfix(v.z, v.z) ) << 8;
	}
	/*low-res vector*/
	else {
		return gf_sqrt( gf_mulfix(v.x, v.x) + gf_mulfix(v.y, v.y) + gf_mulfix(v.z, v.z) );
	}
#endif
}

GF_EXPORT
Fixed gf_vec_lensq(GF_Vec v)
{
	/*commented out atm - weird results (not enough precision?)...*/
//#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
#if 0
	Fixed res;
	gppVec3DLengthSq_16_32s((GPP_VEC3D *) &v, &res);
	return res;
#else
	/*high-magnitude vector, use low precision on frac part to avoid overflow*/
	if (VEC_HIGH_MAG(v)) {
		v.x>>=8;
		v.y>>=8;
		v.z>>=8;
		return ( gf_mulfix(v.x, v.x) + gf_mulfix(v.y, v.y) + gf_mulfix(v.z, v.z) ) << 16;
	} else {
		return gf_mulfix(v.x, v.x) + gf_mulfix(v.y, v.y) + gf_mulfix(v.z, v.z);
	}
#endif
}

GF_EXPORT
Fixed gf_vec_dot(GF_Vec v1, GF_Vec v2)
{
	/*commented out atm - weird results (not enough precision?)...*/
//#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
#if 0
	Fixed res;
	gppVec3DDot_16_32s((GPP_VEC3D *) &v1, (GPP_VEC3D *) &v2, &res);
	return res;
#else
	/*both high-magnitude vectors, use low precision on frac part to avoid overflow
	if only one is, the dot product should still be in proper range*/
	if (VEC_HIGH_MAG(v1) && VEC_HIGH_MAG(v2)) {
		v1.x>>=8; v1.y>>=8; v1.z>>=8;
		v2.x>>=8; v2.y>>=8; v2.z>>=8;
		return ( gf_mulfix(v1.x, v2.x) + gf_mulfix(v1.y, v2.y) + gf_mulfix(v1.z, v2.z) ) << 16;
	} else {
		return gf_mulfix(v1.x, v2.x) + gf_mulfix(v1.y, v2.y) + gf_mulfix(v1.z, v2.z);
	}
#endif
}

GF_EXPORT
void gf_vec_norm(GF_Vec *v)
{
	/*commented out atm - weird results (not enough precision?)...*/
//#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
#if 0
	gppVec3DNormalize_16_32s((GPP_VEC3D *) &v);
#else
	Fixed __res = gf_vec_len(*v);
	v->x = gf_divfix(v->x, __res); 
	v->y = gf_divfix(v->y, __res);
	v->z = gf_divfix(v->z, __res);
#endif
}

GF_EXPORT
GF_Vec gf_vec_scale(GF_Vec v, Fixed f)
{
	GF_Vec res;
	res.x = gf_mulfix(v.x, f); 
	res.y = gf_mulfix(v.y, f); 
	res.z = gf_mulfix(v.z, f);
	return res;
}

GF_EXPORT
GF_Vec gf_vec_cross(GF_Vec v1, GF_Vec v2)
{
	GF_Vec res;
	/*commented out atm - weird results (not enough precision?)...*/
//#if defined(GPAC_USE_IGPP_HP) || defined (GPAC_USE_IGPP)
#if 0
	gppVec3DCross_16_32s((GPP_VEC3D *) &v1, (GPP_VEC3D *) &v2, (GPP_VEC3D *) &res);
#else
	/*both high-magnitude vectors, use low precision on frac part to avoid overflow
	if only one is, the cross product should still be in proper range*/
	if (VEC_HIGH_MAG(v1) && VEC_HIGH_MAG(v2)) {
		v1.x>>=8; v1.y>>=8; v1.z>>=8;
		v2.x>>=8; v2.y>>=8; v2.z>>=8;
		res.x = gf_mulfix(v1.y, v2.z) - gf_mulfix(v2.y, v1.z);
		res.y = gf_mulfix(v2.x, v1.z) - gf_mulfix(v1.x, v2.z);
		res.z = gf_mulfix(v1.x, v2.y) - gf_mulfix(v2.x, v1.y);
		res.x<<=16;
		res.y<<=16;
		res.z<<=16;
	} else {
		res.x = gf_mulfix(v1.y, v2.z) - gf_mulfix(v2.y, v1.z);
		res.y = gf_mulfix(v2.x, v1.z) - gf_mulfix(v1.x, v2.z);
		res.z = gf_mulfix(v1.x, v2.y) - gf_mulfix(v2.x, v1.y);
	}
#endif
	return res;
}

#else

GF_EXPORT
Fixed gf_vec_len(GF_Vec v) { return gf_sqrt(v.x*v.x + v.y*v.y + v.z*v.z); }
GF_EXPORT
Fixed gf_vec_lensq(GF_Vec v) { return v.x*v.x + v.y*v.y + v.z*v.z; }
GF_EXPORT
Fixed gf_vec_dot(GF_Vec v1, GF_Vec v2) { return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; }
GF_EXPORT
void gf_vec_norm(GF_Vec *v)
{
	Fixed __res = gf_vec_len(*v);
	if (__res ) __res = 1.0f/__res ;
	v->x *= __res; 
	v->y *= __res;
	v->z *= __res;
}

GF_EXPORT
GF_Vec gf_vec_scale(GF_Vec v, Fixed f)
{
	GF_Vec res = v;
	res.x *= f; 
	res.y *= f; 
	res.z *= f;
	return res;
}

GF_EXPORT
GF_Vec gf_vec_cross(GF_Vec v1, GF_Vec v2)
{
	GF_Vec res;
	res.x = v1.y*v2.z - v2.y*v1.z;
	res.y = v2.x*v1.z - v1.x*v2.z;
	res.z = v1.x*v2.y - v2.x*v1.y;
	return res;
}

#endif


GF_EXPORT
void gf_mx2d_from_mx(GF_Matrix2D *mat2D, GF_Matrix *mat)
{
	gf_mx2d_init(*mat2D);
	mat2D->m[0] = mat->m[0];
	mat2D->m[1] = mat->m[4];
	mat2D->m[2] = mat->m[12];
	mat2D->m[3] = mat->m[1];
	mat2D->m[4] = mat->m[5];
	mat2D->m[5] = mat->m[13];