/*

* 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: SkinnedMesh implementation

*

*/

/*!

 * \internal

 * \file

 * \brief SkinnedMesh implementation

 */

#ifndef M3G_CORE_INCLUDE

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

#endif

#include "m3g_skinnedmesh.h"

#include "m3g_memory.h"

#include "m3g_animationtrack.h"

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

 * Internal structures

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

struct BoneRecord

{

    Node *node;

    /*! \internal \brief "At-rest" transformation from skinned mesh to bone */

    Matrix toBone;

    /*! \internal \brief Cached animated transformation for positions */

    M3Gshort baseMatrix[9];

    M3Gshort posVec[3];

    M3Gshort baseExp, posExp, maxExp;

    /*! \internal \brief Cached animated transformation for normals */

    M3Gshort normalMatrix[9];

};

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

 * Internal functions

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

/*!

 * \internal

 * \brief Destroys this SkinnedMesh object.

 *

 * \param obj SkinnedMesh object

 */

static void m3gDestroySkinnedMesh(Object *obj)

{

    SkinnedMesh *mesh = (SkinnedMesh *) obj;

    M3G_VALIDATE_OBJECT(mesh);

    {

        int i;

        Interface *m3g = M3G_INTERFACE(mesh);

        m3gDeleteVertexBuffer(mesh->morphedVB);

        for (i = 0; i < mesh->bonesPerVertex; ++i) {

            m3gFree(m3g, mesh->boneIndices[i]);

            m3gFree(m3g, mesh->boneWeights[i]);

            m3gFree(m3g, mesh->normalizedWeights[i]);

        }

        m3gFree(m3g, mesh->weightShifts);

        for (i = 0; i < m3gArraySize(&mesh->bones); ++i) {

            m3gFree(m3g, m3gGetArrayElement(&mesh->bones, i));

        }

        m3gDestroyArray(&mesh->bones, m3g);

        if (mesh->skeleton != NULL) {

            m3gSetParent((Node*) mesh->skeleton, NULL);

            M3G_ASSIGN_REF(mesh->skeleton, NULL);

        }

    }

    m3gDestroyMesh(obj);

}

/*!

 * \internal

 * \brief Get a bone index for a given node

 *

 * This finds an existing record if the bone has been added

 * previously, or creates a new one if no record exists yet.

 *

 * \note Inline because only called from AddTransform.

 */

static M3G_INLINE M3Gint m3gBoneIndex(SkinnedMesh *mesh, Node *node)

{

    PointerArray *boneArray = &mesh->bones;

    const int numBones = m3gArraySize(boneArray);

    /* First look for an existing record in the array */

    {

        int i;

        for (i = 0; i < numBones; ++i) {

            Bone *b = m3gGetArrayElement(boneArray, i);

            if (b->node == node) {

                return i;

            }

        }

    }

    /* Not found; create a new one, append to the array, and set up

     * the "at-rest" transformation for the bone. Note, however, that

     * we can only store a maximum of 256 bones with byte indices! */

    {

        Interface *m3g = M3G_INTERFACE(mesh);

        if (numBones >= 256) {

            /* Out of available bone indices */

            m3gRaiseError(m3g, M3G_OUT_OF_MEMORY);

            return -1;

        }

        else {

            M3Gint idx;

            Bone *bone = (Bone*) m3gAllocZ(m3g, sizeof(Bone));

            if (!bone || !m3gGetTransformTo((Node*) mesh, node,

                                            &bone->toBone)) {

                m3gFree(m3g, bone);

                return -1; /* out of memory or singular transform */

            }

            bone->node = node;

            idx = m3gArrayAppend(boneArray, bone, m3g);

            if (idx < 0) {

                m3gFree(m3g, bone);

                return -1; /* out of memory */

            }

            return idx;

        }

    }

}

/*!

 * \internal

 * \brief Reallocate the per-vertex data if necessary.

 *

 * \note Inline because only called from AddTransform.

 */

static M3G_INLINE M3Gbool m3gEnsureVertexCount(SkinnedMesh *mesh, M3Gint count)

{

    /* Reallocate only if vertex count increased */

    if (count > mesh->weightedVertexCount) {

        Interface *m3g = M3G_INTERFACE(mesh);

        int i;

        /* Reallocate the weight shift array */

        {

            M3Gubyte *pNew = (M3Gubyte*) m3gAllocZ(m3g, count);

            if (!pNew) {

                return M3G_FALSE;

            }

            m3gCopy(pNew, mesh->weightShifts, mesh->weightedVertexCount);

            m3gFree(m3g, mesh->weightShifts);

            mesh->weightShifts = pNew;

        }

        /* Reallocate each of the bone index and weight arrays */

        for (i = 0; i < mesh->bonesPerVertex; ++i) {

            M3Gubyte *pNew;

            /* Weights */

            pNew = (M3Gubyte*) m3gAllocZ(m3g, count);

            if (!pNew) {

                return M3G_FALSE; /* out of memory */

            }

            m3gCopy(pNew, mesh->boneWeights[i], mesh->weightedVertexCount);

            m3gFree(m3g, mesh->boneWeights[i]);

            mesh->boneWeights[i] = pNew;

            pNew = (M3Gubyte*) m3gAllocZ(m3g, count);

            if (!pNew) {

                return M3G_FALSE; /* out of memory */

            }

            m3gCopy(pNew, mesh->normalizedWeights[i],

                    mesh->weightedVertexCount);

            m3gFree(m3g, mesh->normalizedWeights[i]);

            mesh->normalizedWeights[i] = pNew;

            /* Indices */

            pNew = (M3Gubyte*) m3gAllocZ(m3g, count);

            if (!pNew) {

                return M3G_FALSE; /* out of memory */

            }

            m3gCopy(pNew, mesh->boneIndices[i], mesh->weightedVertexCount);

            m3gFree(m3g, mesh->boneIndices[i]);

            mesh->boneIndices[i] = pNew;

        }

        mesh->weightedVertexCount = count;

    }

    return M3G_TRUE;

}

/*!

 * \internal

 * \brief Reallocate the per-vertex data if necessary.

 *

 * \note Inline because only called from AddTransform.

 */

static M3G_INLINE M3Gbool m3gEnsureBonesPerVertex(SkinnedMesh *mesh,

                                                  M3Gint count)

{

    M3G_ASSERT(count <= M3G_MAX_VERTEX_TRANSFORMS);

    /* Allocate only if per-vertex bone count increased */

    if (count > mesh->bonesPerVertex) {

        Interface *m3g = M3G_INTERFACE(mesh);

        const M3Gint vertexCount = mesh->weightedVertexCount;

        M3Gubyte *pNew;

        int i;

        /* Allocate new arrays for bone indices and weights until

         * we're satisfied */

        for (i = mesh->bonesPerVertex; i < count; ++i) {

            pNew = (M3Gubyte*) m3gAllocZ(m3g, vertexCount);

            if (!pNew) {

                goto AllocFailed; /* out of memory */

            }

            mesh->boneIndices[i] = pNew;

            pNew = (M3Gubyte*) m3gAllocZ(m3g, vertexCount);

            if (!pNew) {

                goto AllocFailed; /* out of memory */

            }

            mesh->boneWeights[i] = pNew;

            pNew = (M3Gubyte*) m3gAllocZ(m3g, vertexCount);

            if (!pNew) {

                goto AllocFailed; /* out of memory */

            }

            mesh->normalizedWeights[i] = pNew;

        }

        mesh->bonesPerVertex = count;

        return M3G_TRUE;

        /* In case of failure, clean up to keep the bonesPerVertex

         * counter in sync with the actual number of arrays

         * allocated */

    AllocFailed:

        for (i = mesh->bonesPerVertex; i < count; ++i) {

            m3gFree(m3g, mesh->boneIndices[i]);

            m3gFree(m3g, mesh->boneWeights[i]);

            m3gFree(m3g, mesh->normalizedWeights[i]);

            mesh->boneIndices[i] = NULL;

            mesh->boneWeights[i] = NULL;

            mesh->normalizedWeights[i] = NULL;

        }

        return M3G_FALSE;

    }

    return M3G_TRUE;

}

/*!

 * \internal

 * \brief Add a new bone influence to a vertex

 *

 * If the target vertex is already affected by

 * M3G_MAX_VERTEX_TRANSFORMS bones, the one with the lowest weight is

 * discarded.

 */

static M3G_INLINE void m3gAddInfluence(SkinnedMesh *mesh,

                                       M3Gint vertexIndex,

                                       M3Gint boneIndex,

                                       M3Gint weight) 

{

    M3Gint bonesPerVertex = mesh->bonesPerVertex;

    M3Guint minWeight = weight;

    M3Gint minWeightIndex = -1;

    int i;

    /* Shift the weight into the same scale with the other weights for

     * this vertex. */

    weight >>= mesh->weightShifts[vertexIndex];

    /* Look for an existing weight for our bone, or find the index

     * with the lowest weight if not found, and store it in

     * minWeightIndex.  Note that we're not separately tagging indices

     * as used/unused; unused ones will merely have a weight of

     * zero. */

    for (i = 0; i < bonesPerVertex; ++i) {

        /* If we find an existing weight for our bone, just add to

         * that and break out. Otherwise, keep track of the minimum

         * weight encountered so far. */

        if (mesh->boneIndices[i][vertexIndex] == boneIndex) {

            weight += mesh->boneWeights[i][vertexIndex];

            minWeightIndex = i;

            break;

        }

        else {

            M3Guint tempWeight = mesh->boneWeights[i][vertexIndex];

            if (tempWeight < minWeight) {

                minWeight = tempWeight;

                minWeightIndex = i;

            }

        }

    }

    /* Check whether our total weight exceeds the allocated range,

     * shifting all existing weights down if necessary */

    while (weight >= (1 << 8)) { /* byte range */

        weight >>= 1;

        mesh->weightShifts[vertexIndex] += 1;

        for (i = 0; i < bonesPerVertex; ++i) {

            mesh->boneWeights[i][vertexIndex] >>= 1;

        }

        M3G_ASSERT(mesh->weightShifts[vertexIndex] <= 31);

    }

    /* Add the index and weight contribution of the new

     * transformation, provided that the minimum weight found was

     * indeed smaller than the one we're adding */

    if (minWeightIndex >= 0) {

        mesh->boneIndices[minWeightIndex][vertexIndex] = (M3Gubyte) boneIndex;

        mesh->boneWeights[minWeightIndex][vertexIndex] = (M3Gubyte) weight;

        /* Need an update of the normalizing scales, too, as well as

         * the actual transformed vertices */

        mesh->weightsDirty = M3G_TRUE;

        m3gInvalidateNode((Node*) mesh, NODE_TRANSFORMS_BIT|NODE_BBOX_BIT);

    }

}

/*!

 * \internal

 * \brief Computes the normalization scales for vertex weights

 */

static void m3gNormalizeWeights(SkinnedMesh *mesh)

{

    const M3Gint bonesPerVertex = mesh->bonesPerVertex;

    const M3Gint vertexCount = mesh->weightedVertexCount;

    M3Gint vi;

    for (vi = 0; vi < vertexCount; ++vi) {

        M3Gint k;

        /* Sum up the 8-bit (possibly downshifted) weights */

        M3Guint sum = 0;

        for (k = 0; k < bonesPerVertex; ++k) {

            sum += mesh->boneWeights[k][vi];

        }

        /* Compute an 8.24 reciprocal of the weights, then scale with

         * that to normalize, and shift to 1.7 fixed point */

        {

            M3Guint s = (sum > 0 ? (1U << 24) / sum : 0);

            sum = 0;

            for (k = 0; k < bonesPerVertex; ++k) {

                M3Guint normalized = (s * mesh->boneWeights[k][vi]) >> 17;

                M3G_ASSERT(m3gInRange((M3Gint)normalized, 0, 128));

                sum += normalized;

                mesh->normalizedWeights[k][vi] = (M3Gubyte) normalized;

            }

            /* NOTE there is a maximum of ½ rounding error per

             * component, plus the rounding error from the reciprocal

             * calculation, so the sum of weights will often not sum

             * to 128 exactly! We therefore only assert against

             * clearly out-of-range values here */

            M3G_ASSERT(sum == 0 || m3gInRange((M3Gint) sum, 96, 128));

        }

    }

    mesh->weightsDirty = M3G_FALSE;

}

/*!

 * \internal

 * \brief Computes an optimal exponent value for a fixed point

 * transformation

 *

 * This scales the translation exponent up to optimally utilize the

 * 32-bit intermediate precision if the matrix exponent is smaller.

 */

static M3Gint m3gOptimalExponent(M3Gint matrixExp, M3Gint transExp)

{

    M3Gint maxExp = matrixExp;

    M3Gint shift = transExp - matrixExp;

    if (shift > 0) {

        /* The matrix part will always occupy less than half of the

         * available range if shifted down by at least one bit, so we

         * can shift the translation up by a maximum of 15 bits.  If

         * the matrix is shifted by more than 31 bits, it will always

         * flush to zero, freeing the full 32-bit range for the

         * translation alone. */

        if (shift >= 32) {      /* matrix will flush to zero */

            shift = 16;

        }

        else if (shift >= 16) { /* matrix always < half of the range */

            shift = 15;

        }

        else {

            shift -= 1;     /* shift matrix by at least one bit */

        }

        maxExp = transExp - shift;

    }

    M3G_ASSERT(maxExp >= matrixExp && maxExp >= transExp - 16);

    return maxExp;

}

/*

 * \brief Fixed point vertex transformation

 *

 * \param mtx        pointer to a 3x3 16-bit matrix

 * \param mtxExp     exponent for the matrix elements (upshift from int)

 * \param trans      pointer to 3-element 16-bit translation vector

 * \param transExp   exponent for the translation vector

 * \param maxExp     precalculated "optimal" exponent

 * \param vx         vertex X coordinate (16-bit range)

 * \param vy         vertex Y coordinate (16-bit range)

 * \param vz         vertex Z coordinate (16-bit range)

 * \param out        output vertex, 25 bits of precision

 * \return exponent value for \c out

 */

static M3Gint m3gFixedPointTransform(const M3Gshort *mtx, M3Gint mtxExp,

                                     const M3Gshort *trans, M3Gint transExp,

                                     M3Gint maxExp,

                                     M3Gint vx, M3Gint vy, M3Gint vz,

                                     M3Gint *out)

{

    M3Gint shift;

    M3Gint ox = 0, oy = 0, oz = 0;

    /* First put in the translation part, upscaled to the optimal

     * range for this bone */

    if (trans) {

        shift = maxExp - (transExp - 16);

        M3G_ASSERT(shift >= 0);

        if (shift < 32) {

            ox += ((M3Gint) trans[0] << 16) >> shift;

            oy += ((M3Gint) trans[1] << 16) >> shift;

            oz += ((M3Gint) trans[2] << 16) >> shift;

        }

    }

    /* Add the input multiplied with the base 3x3 matrix and shifted

     * to the "maxExp" scale, provided that it has any effect on the

     * outcome */

    shift = maxExp - mtxExp;

    M3G_ASSERT(shift >= 0);

    if (shift < 32) {

#       if defined(M3G_DEBUG)

        M3Gint iMin = (-1 << 31) + (65535 * 32768 >> shift);

        M3Gint iMax = (M3Gint)((1u << 31)-1) - (65535 * 32768 >> shift);

        M3G_ASSERT(m3gInRange(ox, iMin, iMax));

        M3G_ASSERT(m3gInRange(oy, iMin, iMax));

        M3G_ASSERT(m3gInRange(oz, iMin, iMax));

#       endif /* M3G_DEBUG */

        ox += (mtx[0] * vx + mtx[3] * vy + mtx[6] * vz) >> shift;

        oy += (mtx[1] * vx + mtx[4] * vy + mtx[7] * vz) >> shift;

        oz += (mtx[2] * vx + mtx[5] * vy + mtx[8] * vz) >> shift;

    }

    /* Shift the output down to fit into 25 bits; we're dropping 7

     * bits of precision here, so adjust the exponent accordingly */

    out[0] = ox >> 7;

    out[1] = oy >> 7;

    out[2] = oz >> 7;

    return maxExp + 7;

}

/*!

 * \internal

 * \brief Applies scale and bias to a vertex

 *

 * This is required for vertices that have no bones attached.

 * 

 * \param mesh    the SkinnedMesh object

 * \param vx      vertex X coordinate (16-bit range)

 * \param vy      vertex Y coordinate (16-bit range)

 * \param vz      vertex Z coordinate (16-bit range)

 * \param upshift scaling value for the input coordinates and the

 *                translation component of the transformation

 * \param vertex  output vertex position

 * \return exponent value for \c vertex

 */

static M3Gint m3gScaleAndBiasVertex(const SkinnedMesh *mesh,

                                    M3Gint vx, M3Gint vy, M3Gint vz,

                                    M3Gint upshift,

                                    M3Gshort *vertex)