/*

* Copyright (c) 2003 Nokia Corporation and/or its subsidiary(-ies).

* All rights reserved.

* This component and the accompanying materials are made available

* under the terms of the License "Eclipse Public License v1.0"

* which accompanies this distribution, and is available

* at the URL "http://www.eclipse.org/legal/epl-v10.html".

*

* Initial Contributors:

* Nokia Corporation - initial contribution.

*

* Contributors:

*

* Description: Internal math function implementations

*

*/

/*!

 * \internal

 * \file

 * \brief Internal math function implementations

 *

 */

#ifndef M3G_CORE_INCLUDE

#   error included by m3g_core.c; do not compile separately.

#endif

#include "m3g_defs.h"

#include "m3g_memory.h"

#if defined(M3G_SOFT_FLOAT)

#include <math.h>

#endif

/*----------------------------------------------------------------------

 * Private types and definitions

 *--------------------------------------------------------------------*/

/* Enumerated common matrix classifications */

#define MC_IDENTITY             0x40100401 // 01000000 00010000 00000100 00000001

#define MC_FRUSTUM              0x30BF0C03 // 00110000 10111111 00001100 00000011

#define MC_PERSPECTIVE          0x30B00C03 // 00110000 10110000 00001100 00000011

#define MC_ORTHO                0x7F300C03 // 01111111 00110000 00001100 00000011

#define MC_PARALLEL             0x70300C03 // 01110000 00110000 00001100 00000011

#define MC_SCALING_ROTATION     0x403F3F3F // 01000000 00111111 00111111 00111111

#define MC_SCALING              0x40300C03 // 01000000 00110000 00001100 00000011

#define MC_TRANSLATION          0x7F100401 // 01111111 00010000 00000100 00000001

#define MC_X_ROTATION           0x403C3C01 // 01000000 00111100 00111100 00000001

#define MC_Y_ROTATION           0x40330433 // 01000000 00110011 00000100 00110011

#define MC_Z_ROTATION           0x40100F0F // 01000000 00010000 00001111 00001111

#define MC_W_UNITY              0x7F3F3F3F // 01111111 00111111 00111111 00111111

#define MC_GENERIC              0xFFFFFFFF

/* Partial masks for individual matrix components */

#define MC_TRANSLATION_PART     0x3F000000 // 00111111 00000000 00000000 00000000

#define MC_SCALE_PART           0x00300C03 // 00000000 00110000 00001100 00000011

#define MC_SCALE_ROTATION_PART  0x003F3F3F // 00000000 00111111 00111111 00111111

/* Matrix element classification masks */

#define ELEM_ZERO       0x00

#define ELEM_ONE        0x01

#define ELEM_MINUS_ONE  0x02

#define ELEM_ANY        0x03

/*!

 * \internal

 * \brief calculates the offset of a 4x4 matrix element in a linear

 * array

 *

 * \notes The current convention is column-major, as in OpenGL ES

 */

#define MELEM(row, col) ((row) + ((col) << 2))

/*!

 * \internal

 * \brief Macro for accessing 4x4 float matrix elements

 *

 * \param mtx pointer to the first element of the matrix

 * \param row matrix row

 * \param col matrix column

 */

#define M44F(mtx, row, col)     ((mtx)->elem[MELEM((row), (col))])

/*--------------------------------------------------------------------*/

/*----------------------------------------------------------------------

 * Private functions

 *--------------------------------------------------------------------*/

/*!

 * \internal

 * \brief ARM VFPv2 implementation of 4x4 matrix multiplication.

 *

 * \param dst	multiplication result

 * \param left	left-hand matrix

 * \param right right-hand matrix

 */

#if defined(M3G_HW_FLOAT_VFPV2)

__asm void _m3gGenericMatrixProduct(Matrix *dst,

                                    const Matrix *left, const Matrix *right)

{

// r0 = *dst

// r1 = *left

// r2 = *right

		CODE32

// save the VFP registers and set the vector STRIDE = 1, LENGTH = 4

		FSTMFDD	sp!, {d8-d15}

		FMRX	r3, FPSCR		 

		BIC		r12, r3, #0x00370000

		ORR		r12, #0x00030000

		FMXR	FPSCR, r12

// left = [a0  a1  a2  a3	right = [b0  b1  b2  b3

//	 	   a4  a5  a6  a7		     b4  b5  b6  b7

//		   a8  a9 a10 a11		     b8  b9 b10 b11

//		  a12 a13 a14 a15]		    b12 b13 b14 b15]

//

// dst = [a0*b0+a4*b1+a8*b2+a12*b3  a1*b0+a5*b1+a9*b2+a13*b3  ..

//		  a0*b4+a4*b5+a8*b6+a12*b7  a1*b4+a5*b5+a9*b6+a13*b7  ..

//					.							.

//					.							.

		FLDMIAS	r1!, {s8-s23}		// load the left matrix [a0-a15] to registers s8-s23

		FLDMIAS	r2!, {s0-s7}		// load [b0-b7] of right matrix to registers s0-s7

		FMULS	s24, s8, s0			// [s24-s27]  = [a0*b0  a1*b0  a2*b0  a3*b0]

		FMULS	s28, s8, s4			// [s28-s31]  = [a0*b4  a1*b4  a2*b4  a3*b4]

		FMACS	s24, s12, s1		// [s24-s27] += [a4*b1  a5*b1  a6*b1  a7*b1]

		FMACS	s28, s12, s5		// [s28-s31] += [a4*b5  a5*b5  a6*b5  a7*b5]

		FMACS	s24, s16, s2		// [s24-s27] += [a8*b2  a9*b2  a10*b2 a11*b2]

		FMACS	s28, s16, s6		// [s28-s31] += [a8*b6  a9*b6  a10*b6 a11*b6]

		FMACS	s24, s20, s3		// [s24-s27] += [a12*b3 a13*b3 a14*b3 a15*b3]

		FMACS	s28, s20, s7		// [s28-s31] += [a12*b7 a13*b37a14*b7 a15*b7]

		FLDMIAS	r2!, {s0-s7}		// load [b8-b15]

		FSTMIAS	r0!, {s24-s31}		// write [dst0-dst7]

		FMULS	s24, s8, s0			

		FMULS	s28, s8, s4			

		FMACS	s24, s12, s1		

		FMACS	s28, s12, s5		

		FMACS	s24, s16, s2		

		FMACS	s28, s16, s6		

		FMACS	s24, s20, s3		

		FMACS	s28, s20, s7		

		FSTMIAS	r0!, {s24-s31}

// Restore the VFP registers and return.

		FMXR	FPSCR, r3

		FLDMFDD	sp!, {d8-d15}	

		BX		lr

}

#endif /* #if defined(M3G_HW_FLOAT_VFPV2) */

/*------------------ Elementary float ------------------*/

#if defined(M3G_SOFT_FLOAT)

#if defined (M3G_BUILD_ARM)

/*!

 * \internal

 * \brief Floating point multiplication implementation for ARM.

 */

__asm M3Gfloat m3gMul(const M3Gfloat a,

                      const M3Gfloat b)

{

    /**

     * Extract the exponents of the multiplicands and add them

     * together. Flush to zero if either exponent or their sum

     * is zero.

     */

    mov     r12, #0xff;

    ands    r2, r0, r12, lsl #23;   // exit if e1 == 0

    andnes  r3, r1, r12, lsl #23;   // exit if e2 == 0

    subne   r2, r2, #(127 << 23);

    addnes  r12, r2, r3;            // exit if e1+e2-127 <= 0

    movle   r0, #0;

    bxle    lr;

    /**

     * Determine the sign of the result. Note that the exponent

     * may have overflowed to the sign bit, and thus the result

     * may be an arbitrary negative value when it really should

     * be +Inf or -Inf.

     */

    teq     r0, r1;

    orrmi   r12, r12, #0x80000000;

    /**

     * Multiply the mantissas. First shift the mantissas up to

     * unsigned 1.31 fixed point, adding the leading "1" bit at

     * the same time, and finally do a 32x32 -> 64 bit unsigned

     * multiplication. The result is in unsigned 2.62 fixed point,

     * representing the interval [1.0, 4.0).

     */

    mov     r2, #0x80000000;

    orr     r0, r2, r0, lsl #8;

    orr     r1, r2, r1, lsl #8;

    umulls  r2, r3, r0, r1;

    /**

     * If the highest bit of the 64-bit result is set, then the

     * mantissa lies in [2.0, 4.0) and needs to be renormalized.

     * That is, the mantissa is shifted one bit to the right and

     * the exponent correspondingly increased by 1. Note that we

     * lose the leading "1" bit from the mantissa by adding it up

     * with the exponent.

     */

    subpl   r12, r12, #(1 << 23);    // no overflow: exponent -= 1

    addpl   r0, r12, r3, lsr #7;     // no overflow: exponent += 1

    addmi   r0, r12, r3, lsr #8;     // overflow: exponent += 1

    bx      lr;

}

/*!

 * \internal

 * \brief Floating point addition implementation for ARM.

 */

__asm M3Gfloat m3gAdd(const M3Gfloat a,

                      const M3Gfloat b)

{

    /**

     * If the operands have opposite signs then this is not really

     * an addition but a subtraction. Subtraction is much slower,

     * so we have a separate code path for it, rather than trying

     * to save space by handling both in the same place.

     */

    teq     r0, r1;

    bmi     _m3gSub;

    /**

     * Sort the operands such that the larger operand is in R0 and

     * the smaller in R1. The sign bits do not affect the ordering,

     * since they are known to be equal.

     */

    subs    r2, r0, r1;

    submi   r0, r0, r2;

    addmi   r1, r1, r2;

    /**

     * Extract the exponent of the smaller operand into R2 and compute

     * the difference between the larger and smaller exponent into R3.

     * (Note that the sign bits cancel out in the subtraction.) The

     * exponent delta tells how many bits the mantissa of the smaller

     * operand must be shifted to the right in order to bring the

     * operands into equal scale.

     */

    mov     r2, r1, lsr #23;

    rsb     r3, r2, r0, lsr #23;

    /**

     * Check if the exponent delta is bigger than 23 bits, or if the

     * smaller exponent is zero. In either case, exit the routine and

     * return the larger operand (which is already in R0). Note that

     * this means that subnormals are treated as zero.

     */

    rsbs    r12, r3, #23;               // N set, V clear if R3 > 23

    tstpl   r2, #0xff;                  // execute only if R3 <= 23

    bxle    lr;                         // exit if Z set or N != V

    /**

     * Extract the mantissas and shift them up to unsigned 1.31 fixed

     * point, inserting the implied leading "1" bit at the same time.

     * Finally, align the decimal points and add up the mantissas.

     */

    mov     r12, #0x80000000;

    orr     r0, r12, r0, lsl #8;

    orr     r1, r12, r1, lsl #8;

    adds    r0, r0, r1, lsr r3;

    /**

     * Compute the final exponent by adding up the smaller exponent

     * (R2), the exponent delta (R3), and the possible overflow bit

     * (carry flag). Note that in case of overflow, the leading "1"

     * has ended up in the carry flag and thus needs not be explicitly

     * discarded. Finally, put the mantissa together with the sign and

     * exponent.

     */

    adc     r2, r2, r3;                 // r2 = smallExp + deltaExp + overflow

    movcc   r0, r0, lsl #1;             // no overflow: discard leading 1

    mov     r0, r0, lsr #9;

    orr     r0, r0, r2, lsl #23;

    bx      lr;

_m3gSub

    /**

     * Sort the operands such that the one with larger magnitude is

     * in R0 and has the correct sign (the sign of the final result),

     * while the smaller operand is in R1 with an inverted sign bit.

     */

    eor     r1, r1, #0x80000000;

    subs    r2, r0, r1;

    eormi   r2, r2, #0x80000000;

    submi   r0, r0, r2;

    addmi   r1, r1, r2;

    /**

     * Extract the exponent of the smaller operand into R2 and compute

     * the difference between the larger and smaller exponent into R3.

     * (Note that the sign bits cancel out in the subtraction.) The

     * exponent delta tells how many bits the mantissa of the smaller

     * operand must be shifted to the right in order to bring the

     * operands into equal scale.

     */

    mov     r2, r1, lsr #23;

    rsbs    r3, r2, r0, lsr #23;

    /**

     * Check if the exponent delta is bigger than 31 bits, or if the

     * smaller exponent is zero. In either case, exit the routine and

     * return the larger operand (which is already in R0). Note that

     * this means that subnormals are treated as zero.

     */

    rsbs    r12, r3, #31;               // N set, V clear if R3 > 31

    tstpl   r2, #0xff;                  // execute only if R3 <= 31

    bxle    lr;                         // exit if Z set or N != V

    /**

     * Extract the mantissas and shift them up to unsigned 1.31 fixed

     * point, inserting the implied leading "1" bit at the same time.

     * Then align the decimal points and subtract the smaller mantissa

     * from the larger one.

     */

    mov     r12, #0x80000000;

    orr     r0, r12, r0, lsl #8;

    orr     r1, r12, r1, lsl #8;

    subs    r0, r0, r1, lsr r3;

    /**

     * We split the range of possible results into three categories:

     * 

     *   1. [1.0, 2.0) ==> no renormalization needed (highest bit set)

     *   2. [0.5, 1.0) ==> only one left-shift needed

     *   3. (0.0, 0.5) ==> multiple left-shifts needed

     *   4. zero       ==> just return

     *   

     * Cases 1 and 2 are handled in the main code path. Cases 3 and 4

     * are less common by far, so we branch to a separate code fragment

     * for those.

     */

    movpls  r0, r0, lsl #1;             // Cases 2,3,4: shift left

    bpl     _m3gSubRenormalize;         // Cases 3 & 4: branch out

    /**

     * Now we have normalized the mantissa such that the highest bit

     * is set. Here we only need to adjust the exponent, if necessary,

     * and put the pieces together. Note that we lose the leading "1"

     * bit from the mantissa by adding it up with the exponent. We can

     * also do proper rounding (towards nearest) instead of truncation

     * (towards zero) at no extra cost!

     */

    sbc     r3, r3, #1;                 // deltaExp -= 1 (Case 1) or 2 (Case 2)

    add     r2, r2, r3;                 // resultExp = smallExp + deltaExp

    movs    r0, r0, lsr #8;             // shift mantissa, keep leading "1"

    adc     r0, r0, r2, lsl #23;        // resultExp += 1, mantissa += carry

    bx      lr;

    /**

     * Separate code path for cases 3 and 4 (see above). The mantissa

     * has already been shifted up by one, but the exponent has not

     * been correspondingly decreased. We also know that the highest

     * bit is still not set, and that the carry flag is clear.

     */

_m3gSubRenormalize

    bxeq    lr;

    subcc   r3, r3, #2;

    movccs  r0, r0, lsl #2;

    /**

     * If the carry flag is still not set, i.e. there were more than

     * two leading zeros in the mantissa initially, loop until we

     * find the highest set bit.

     */

_m3gSubRenormalizeLoop

    subcc   r3, r3, #1;

    movccs  r0, r0, lsl #1;

    bcc     _m3gSubRenormalizeLoop;

    /**

     * Now the leading "1" is in the carry flag, so we can just add up

     * the exponent and mantissa as usual, doing proper rounding at

     * the same time. However, cases where the exponent goes negative

     * (that is, underflows) must be detected and flushed to zero.

     */

    add     r3, r2, r3;

    movs    r0, r0, lsr #9;

    adc     r0, r0, r3, lsl #23;

    teq     r0, r2, lsl #23;

    movmi   r0, #0;

    bx      lr;

}

#else /* M3G_BUILD_ARM */

/*!

 * \internal

 * \brief Floating point addition implementation

 *

 */

static M3G_INLINE M3Gfloat m3gFloatAdd(const M3Guint aBits,

                                       const M3Guint bBits)

{

    M3Guint large, small, signMask;

    /* Early exits for underflow cases */

    large = (M3Gint)(aBits & ~SIGN_MASK);

    if (large <= 0x00800000) {

        return INT_AS_FLOAT(bBits);

    }

    small = (M3Gint)(bBits & ~SIGN_MASK);

    if (small <= 0x00800000) {

        return INT_AS_FLOAT(aBits);

    }

    /* Swap the numbers so that "large" really is larger; the unsigned

     * (or de-signed) bitmasks for floats are nicely monotonous, so we

     * can compare directly.  Also store the sign of the larger number

     * for future reference. */

    if (small > large) {

        M3Gint temp = small;

        small = large;

        large = temp;

        signMask = (bBits & SIGN_MASK);

    }

    else {

        signMask = (aBits & SIGN_MASK);

    }

    {

        M3Guint res, overflow;

        M3Guint resExp, expDelta;

        /* Store the larger exponent as our candidate result exponent,

         * and compute the difference between the exponents */

        resExp = (large >> 23);

        expDelta = resExp - (small >> 23);

        /* Take an early exit if the change would be insignificant;

         * this also guards against odd results from shifting by more

         * than 31 (undefined in C) */

        if (expDelta >= 24) {

            res = large | signMask;

            return INT_AS_FLOAT(res);

        }

        /* Convert the mantissas into shifted integers, and shift the

         * smaller number to the same scale with the larger one. */

        large = (large & MANTISSA_MASK) | LEADING_ONE;

        small = (small & MANTISSA_MASK) | LEADING_ONE;

        small >>= expDelta;

        M3G_ASSERT(large >= small);

        /* Check whether we're really adding or subtracting the

         * smaller number, and branch to slightly different routines

         * respectively */

        if (((aBits ^ bBits) & SIGN_MASK) == 0) {

            /* Matching signs; just add the numbers and check for

             * overflow, shifting to compensate as necessary. */

            res = large + small;

            overflow = (res >> 24);

            res >>= overflow;

            resExp += overflow;

        }

        else {

            /* Different signs, so let's subtract the smaller value;

             * also check that we're not subtracting a number from

             * itself (so we don't have to normalize a zero below) */

            if (small == large) {

                return 0.0f; /* x - x = 0 */

            }

            res = (large << 8) - (small << 8);

            /* Renormalize the number by shifting until we've got the

             * high bit in place */

            while ((res >> 24) == 0) {

                res <<= 8;

                resExp -= 8;

            }

            while ((res >> 31) == 0) {

                res <<= 1;

                --resExp;

            }

            res >>= 8;

        }