nu_math.c

nucleus 文件系统,内核和彩色图形系统,在小系统上非常好用

	if (compare)
	{	/* interpolate between table points */
		compare /= (sinTbl[i] - sinTbl[i-1]);
	}
	return((i*10) - compare);
}


int dFix_add(dblFix *first, dblFix *second, dblFix *result)
{
	int lCarry = 0;
	unsigned long dTempL;

	/* get partial sum of lower */
	dTempL = first->fLower + second->fLower;

	/* test for overflow */
	if (first->fLower & 0x80000000)
	{	/* first_msb & (second_msb | !result_msb) is a carry */
		if ((second->fLower & 0x80000000) || (!(dTempL & 0x80000000)))
		lCarry = 1;
	}
	else
	{	/* (second_msb & !result_msb) is a carry */
		if ((second->fLower & 0x80000000) && (!(dTempL & 0x80000000)))
		lCarry = 1;
	}

	/* now get sum of upper and carry */
	result->fUpper = first->fUpper + second->fUpper + lCarry;
	result->fLower = dTempL;

	return (0);
}


int dFix_sub(dblFix *first, dblFix *second, dblFix *result)
{
	int lBorrow = 0;
	unsigned long dTempL;

	/* get partial sum of lower */
	dTempL = first->fLower - second->fLower;

	/* test for overflow */
	if (first->fLower & 0x80000000)
	{	/* first_msb & second_msb & result_msb) is a borrow */
		if ((second->fLower & 0x80000000) && (dTempL & 0x80000000))
		lBorrow = 1;
	}
	else
	{	/* (result_msb | second_msb) is a borrow */
		if ((dTempL & 0x80000000) || (second->fLower & 0x80000000))
		lBorrow = 1;
	}

	/* now subtract upper and borrow */
	result->fUpper = first->fUpper - second->fUpper - lBorrow;
	result->fLower = dTempL;

	return (0);
}


int dFix_mul(dblFix *first, dblFix *second, dblFix *result)
{
	int dFix_add(dblFix *first, dblFix *second, dblFix *result);
	int dFix_sub(dblFix *first, dblFix *second, dblFix *result);
	unsigned long fA, fB, fC, fD;
	unsigned long sA, sB, sC, sD;
	unsigned long rA, rB, rC, rD;
	int sign = 1;
	dblFix dFtemp, dblSum, dblZero = {0,0};

	/* test for trivial case */
	if (((first->fUpper == 0) && (first->fLower == 0)) ||
		((second->fUpper == 0) && (second->fLower == 0)))
	{	/* result is 0 */
		result->fUpper = 0;
		result->fLower = 0;
		return (0);
	}

	/* split the input into 4 14-bit short positive values */
	if (first->fUpper < 0)
	{	/* negative data - invert and set sign */
		sign = -1;
		dFix_sub(&dblZero, first, &dFtemp);
		fD = (dFtemp.fLower & 0x00003fff); 
		fC = ((dFtemp.fLower >> 14) & 0x00003fff);
		fB = (((dFtemp.fLower >> 28) | (dFtemp.fUpper << 4)) & 0x00003fff);
		fA = ((dFtemp.fUpper >> 10) & 0x00003fff);
	}
	else
	{	/* split it */
		fD = (first->fLower & 0x00003fff); 
		fC = ((first->fLower >> 14) & 0x00003fff);
		fB = (((first->fLower >> 28) | (first->fUpper << 4)) & 0x00003fff);
		fA = ((first->fUpper >> 10) & 0x00003fff);
	}

	if (second->fUpper < 0)
	{	/* negative data - invert and set sign */
		sign *= -1;
		dFix_sub(&dblZero, second, &dFtemp);
		sD = (dFtemp.fLower & 0x00003fff); 
		sC = ((dFtemp.fLower >> 14) & 0x00003fff);
		sB = (((dFtemp.fLower >> 28) | (dFtemp.fUpper << 4)) & 0x00003fff);
		sA = ((dFtemp.fUpper >> 10) & 0x00003fff);
	}
	else
	{	/* split it */
		sD = (second->fLower & 0x00003fff); 
		sC = ((second->fLower >> 14) & 0x00003fff);
		sB = (((second->fLower >> 28) | (second->fUpper << 4)) & 0x00003fff);
		sA = ((second->fUpper >> 10) & 0x00003fff);
	}

	/* calculate partial products */
	/* first check for trivial overflow */
	rA = fA * sA;
	if (rA)
	{
		return (-1);	/* overflow */
	}
	rA = fA * sB;
	if (rA)
	{
		return (-1);	/* overflow */
	}
	rA = fB * sA;
	if (rA)
	{
		return (-1);	/* overflow */
	}

	/* now go to bottom for rounding */
	rD = (((fD * sD) + 0x00002000) >> 14);
	rD += ((fC  * sD) + (fD * sC));
	rC = ((fB * sD) + (fC * sC) + (fD * sB));
	rB = ((fA * sD) + (fB * sC) + (fC * sB) + (fD * sA));
	rA = ((fA * sC) + (fB * sB) + (fC * sA));

	/* now combine back to result */
	if (sign < 0)
	{	/* negative result */
		dFtemp.fLower = (rD << 2);
		dFtemp.fUpper = 0;
		dblSum.fLower = (rC << 16);
		dblSum.fUpper = (rC >> 16);
		dFix_add(&dblSum, &dFtemp, &dblSum);
		dblSum.fLower = (((dblSum.fLower + 0x00000008) >> 4)
			+ (dblSum.fUpper << 28));
		dblSum.fUpper = (dblSum.fUpper >> 4);
		dFtemp.fLower = (rB << 26);
		dFtemp.fUpper = (rB >> 6);
		dFix_add(&dblSum, &dFtemp, &dblSum);
		dFtemp.fLower = 0;
		dFtemp.fUpper = (rA << 8);
		dFix_add(&dblSum, &dFtemp, &dblSum);
		dFix_sub(&dblZero, &dblSum, result);
	}
	else
	{
		dFtemp.fLower = (rD << 2);
		dFtemp.fUpper = 0;
		dblSum.fLower = (rC << 16);
		dblSum.fUpper = (rC >> 16);
		dFix_add(&dblSum, &dFtemp, &dblSum);
		dblSum.fLower = (((dblSum.fLower + 0x00000008) >> 4)
			+ (dblSum.fUpper << 28));
		dblSum.fUpper = (dblSum.fUpper >> 4);
		dFtemp.fLower = (rB << 26);
		dFtemp.fUpper = (rB >> 6);
		dFix_add(&dblSum, &dFtemp, &dblSum);
		dFtemp.fLower = 0;
		dFtemp.fUpper = (rA << 8);
		dFix_add(&dblSum, &dFtemp, result);
	}

	return (0);
	}


int dFix_div(dblFix *first, dblFix *second, dblFix *result)
{
	int dFix_add(dblFix *first, dblFix *second, dblFix *result);
	int dFix_sub(dblFix *first, dblFix *second, dblFix *result);
	dblFix dFtemp1, dFtemp2, dFtemp3 = {0,0};
	dblFix dblZero = {0,0}, dblOne = {0,0x00010000};
	short sign = 1;
	int i;
	unsigned short qFract = 0;

	result->fUpper = 0;
	result->fLower = 0;

	/* test for trivial case */
	if ((first->fUpper == 0) && (first->fLower == 0))
	{	/* result is 0 */
		return (0);
	}
	if ((second->fUpper == 0) && (second->fLower == 0))
	{	/* divide by 0 */
		return (-1);
	}

	/* set up temp data */
	if (first->fUpper < 0)
	{	/* negative data - invert and set sign */
		sign = -1;
		dFix_sub(&dblZero, first, &dFtemp1);
	}
	else
	{	/* split it */
		dFtemp1.fUpper = first->fUpper;
		dFtemp1.fLower = first->fLower;
	}
	if (second->fUpper < 0)
	{	/* negative data - invert and set sign */
		sign *= -1;
		dFix_sub(&dblZero, second, &dFtemp2);
	}
	else
	{	/* split it */
		dFtemp2.fUpper = second->fUpper;
		dFtemp2.fLower = second->fLower;
	}

	/* get integer part if any */
	for (i = 0; i < 48; i++)
	{
		/* check if divisor > dividend */
		dFix_sub(&dFtemp1, &dFtemp2, &dFtemp3);
		if(dFtemp3.fUpper & 0x80000000) break;

		/* no so shift divisor left */
		dFtemp2.fUpper = (dFtemp2.fUpper << 1);
		if (dFtemp2.fLower & 0x80000000) dFtemp2.fUpper++;
		dFtemp2.fLower = (dFtemp2.fLower << 1);
	}

	/* now subtract and shift divisor back right */
	while (i--)
	{
		/* shift divisor right */
		dFtemp2.fLower = (dFtemp2.fLower >> 1);
		if (dFtemp2.fUpper & 0x00000001) dFtemp2.fLower += 0x80000000;
		dFtemp2.fUpper = (dFtemp2.fUpper >> 1);

		/* shift result integer left */
		result->fUpper = (result->fUpper << 1);
		if (result->fLower & 0x80000000) result->fUpper++;
		result->fLower = (result->fLower << 1);

		/* check if divisor > dividend */
		dFix_sub(&dFtemp1, &dFtemp2, &dFtemp3);
		if(dFtemp3.fUpper & 0x80000000) continue;

		/* got a bit */
		dFix_add(result, &dblOne, result);

		/* go to next cycle */
		dFtemp1.fUpper = dFtemp3.fUpper;
		dFtemp1.fLower = dFtemp3.fLower;
	}

	/* now get fractional part by comparison method */
	for (i = 0; i < 16; i++)
	{
		qFract = (qFract << 1);
		dFtemp1.fUpper = (dFtemp1.fUpper << 1);
		if (dFtemp1.fLower & 0x80000000) dFtemp1.fUpper++;
		dFtemp1.fLower = (dFtemp1.fLower << 1);

		dFix_sub(&dFtemp1, &dFtemp2, &dFtemp3);
		if(dFtemp3.fUpper & 0x80000000) continue;

		/* got a bit */
		qFract += 1;
		if ((dFtemp3.fLower == 0) && (dFtemp3.fUpper == 0))
		{	/* done early */
			qFract = (qFract << (15 - i));
			break;
		}

		/* go to next cycle */
		dFtemp1.fUpper = dFtemp3.fUpper;
		dFtemp1.fLower = dFtemp3.fLower;
	}

	/* finally, combine integer and fraction */
	result->fLower = result->fLower | ((unsigned long) qFract);
	if (sign < 0)
	{	/* negative result */
		dFix_sub(&dblZero, result, result);
	}

	return(0);
}
#endif