Ticket #2026: skinning_update.patch

source/graphics/Model.cpp

@@ -175 +175 @@
         ValidatePosition();
 
         // extend bounds by vertex positions at the frame
+        CVector3D tempRes;
         for (size_t i=0;i<numverts;i++)
         {
-            result += CModelDef::SkinPoint(verts[i], GetAnimatedBoneMatrices());
+            CModelDef::SkinPoint(verts[i], GetAnimatedBoneMatrices(), tempRes);
+            result += tempRes;
         }
         // advance to next frame
         m_AnimTime += anim->GetFrameTime();
@@ -192 +194 @@
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////
 const CBoundingBoxAligned CModel::GetWorldBoundsRec()
 {
-    CBoundingBoxAligned bounds = GetWorldBounds();
-    for (size_t i = 0; i < m_Props.size(); ++i)
+    CBoundingBoxAligned bounds (GetWorldBounds());
+    const size_t propSize = m_Props.size();
+    for (size_t i = 0; i < propSize; i++)
         bounds += m_Props[i].m_Model->GetWorldBoundsRec();
     return bounds;
 }
@@ -203 +206 @@
     CBoundingBoxAligned objBounds = GetObjectBounds();      // updates the (children-not-included) object-space bounds if necessary
 
     // now extend these bounds to include the props' selection bounds (if any)
-    for (size_t i = 0; i < m_Props.size(); ++i)
+    const size_t propSize = m_Props.size();
+    for (size_t i = 0; i < propSize; ++i)
     {
         const Prop& prop = m_Props[i];
         if (prop.m_Hidden)
@@ -332 +336 @@
         ENSURE(m_PositionValid);
         return;
     }
-
-    if (m_Anim && m_BoneMatrices)
-    {
-//      PROFILE( "generating bone matrices" );
     
-        ENSURE(m_pModelDef->GetNumBones() == m_Anim->m_AnimDef->GetNumKeys());
-    
-        m_Anim->m_AnimDef->BuildBoneMatrices(m_AnimTime, m_BoneMatrices, !(m_Flags & MODELFLAG_NOLOOPANIMATION));
-    }
-    else if (m_BoneMatrices)
+    const size_t numBones = m_pModelDef->GetNumBones();
+    const bool hasBones = m_BoneMatrices && numBones != 0;      
+        // For CPU skinning, we precompute as much as possible so that the only
+        // per-vertex work is a single matrix*vec multiplication.
+        // For GPU skinning, we try to minimise CPU work by doing most computation
+        // in the vertex shader instead.
+        // Using g_Renderer.m_Options to detect CPU vs GPU is a bit hacky,
+        // and this doesn't allow the setting to change at runtime, but there isn't
+        // an obvious cleaner way to determine what data needs to be computed,
+        // and GPU skinning is a rarely-used experimental feature anyway.
+    const bool worldSpaceBoneMatrices = !g_Renderer.m_Options.m_GPUSkinning;
+    const bool computeBlendMatrices = !g_Renderer.m_Options.m_GPUSkinning;
+    const CMatrix3D transform (GetTransform());
+    CMatrix3D * const bonesMatrices = m_BoneMatrices;
+    if (hasBones)
     {
-        // Bones but no animation - probably a buggy actor forgot to set up the animation,
-        // so just render it in its bind pose
 
-        for (size_t i = 0; i < m_pModelDef->GetNumBones(); i++)
+        if (m_Anim)
         {
-            m_BoneMatrices[i].SetIdentity();
-            m_BoneMatrices[i].Rotate(m_pModelDef->GetBones()[i].m_Rotation);
-            m_BoneMatrices[i].Translate(m_pModelDef->GetBones()[i].m_Translation);
+    //      PROFILE( "generating bone matrices" );
+    
+            ENSURE(numBones == m_Anim->m_AnimDef->GetNumKeys());
+    
+            m_Anim->m_AnimDef->BuildBoneMatrices(m_AnimTime, bonesMatrices, !(m_Flags & MODELFLAG_NOLOOPANIMATION));
         }
-    }
+        else
+        {
+            // Bones but no animation - probably a buggy actor forgot to set up the animation,
+            // so just render it in its bind pose
+            const CBoneState * const boneStates = m_pModelDef->GetBones();
+            for (size_t i = 0; i < numBones; i++)
+            {
+                bonesMatrices[i].SetIdentity();
+                bonesMatrices[i].Rotate(boneStates[i].m_Rotation);
+                bonesMatrices[i].Translate(boneStates[i].m_Translation);
+            }
+        }
 
-    // For CPU skinning, we precompute as much as possible so that the only
-    // per-vertex work is a single matrix*vec multiplication.
-    // For GPU skinning, we try to minimise CPU work by doing most computation
-    // in the vertex shader instead.
-    // Using g_Renderer.m_Options to detect CPU vs GPU is a bit hacky,
-    // and this doesn't allow the setting to change at runtime, but there isn't
-    // an obvious cleaner way to determine what data needs to be computed,
-    // and GPU skinning is a rarely-used experimental feature anyway.
-    bool worldSpaceBoneMatrices = !g_Renderer.m_Options.m_GPUSkinning;
-    bool computeBlendMatrices = !g_Renderer.m_Options.m_GPUSkinning;
 
-    if (m_BoneMatrices && worldSpaceBoneMatrices)
-    {
-        // add world-space transformation to m_BoneMatrices
-        const CMatrix3D transform = GetTransform();
-        for (size_t i = 0; i < m_pModelDef->GetNumBones(); i++)
-            m_BoneMatrices[i].Concatenate(transform);
+
+        if (worldSpaceBoneMatrices)
+        {
+            // add world-space transformation to m_BoneMatrices
+            for (size_t i = 0; i < numBones; i++)
+                bonesMatrices[i].Concatenate(transform);
+        }
     }
-
     // our own position is now valid; now we can safely update our props' positions without fearing 
     // that doing so will cause a revalidation of this model (see recursion above).
     m_PositionValid = true;
@@ -386 +397 @@
 
         if (prop.m_Point->m_BoneIndex != 0xff)
         {
-            CMatrix3D boneMatrix = m_BoneMatrices[prop.m_Point->m_BoneIndex];
+            CMatrix3D boneMatrix = (bonesMatrices[prop.m_Point->m_BoneIndex]);
             if (!worldSpaceBoneMatrices)
-                boneMatrix.Concatenate(GetTransform());
+                boneMatrix.Concatenate(transform);
             proptransform.Concatenate(boneMatrix);
         }
         else
@@ -420 +431 @@
         prop.m_Model->ValidatePosition();
     }
 
-    if (m_BoneMatrices)
+    if (hasBones)
     {
-        for (size_t i = 0; i < m_pModelDef->GetNumBones(); i++)
+        const CMatrix3D * const inverseBoneMatrices = m_pModelDef->GetInverseBindBoneMatrices();
+        for (size_t i = 0; i < numBones; i++)
         {
-            m_BoneMatrices[i] = m_BoneMatrices[i] * m_pModelDef->GetInverseBindBoneMatrices()[i];
+            bonesMatrices[i].mul(inverseBoneMatrices[i]);
         }
 
         // Note: there is a special case of joint influence, in which the vertex
@@ -433 +445 @@
         //  of bones. But since we're skinning in world space, we use the model's
         //  world space transform and store that matrix in this special index.
         //  (see http://trac.wildfiregames.com/ticket/1012)
-        m_BoneMatrices[m_pModelDef->GetNumBones()] = m_Transform;
+        bonesMatrices[numBones] = m_Transform;
 
         if (computeBlendMatrices)
-            m_pModelDef->BlendBoneMatrices(m_BoneMatrices);
+            m_pModelDef->BlendBoneMatrices(bonesMatrices);
     }
 }
 

source/graphics/ModelDef.cpp

@@ -31 +31 @@
 #endif
 
 CVector3D CModelDef::SkinPoint(const SModelVertex& vtx,
-                               const CMatrix3D newPoseMatrices[])
+                               const CMatrix3D newPoseMatrices[],
+                               CVector3D &result)
 {
-    CVector3D result (0, 0, 0);
+    const uint8_t * const blendBones = vtx.m_Blend.m_Bone;
+    const float * const blendWeight = vtx.m_Blend.m_Weight;
+    const CVector3D &coords = vtx.m_Coords;
+    
+    if (blendWeight[0] != 0.0 && blendBones[0] != 0xff)
+        result.mulStore(newPoseMatrices[blendBones[0]].Transform(coords), blendWeight[0]);
+    if (blendWeight[1] != 0.0 && blendBones[1] != 0xff)
+        result.mulAdd(newPoseMatrices[blendBones[1]].Transform(coords), blendWeight[1]);
+    if (blendWeight[2] != 0.0 && blendBones[2] != 0xff)
+        result.mulAdd(newPoseMatrices[blendBones[2]].Transform(coords), blendWeight[2]);
+    if (blendWeight[3] != 0.0 && blendBones[3] != 0xff)
+        result.mulAdd(newPoseMatrices[blendBones[3]].Transform(coords), blendWeight[3]);
 
-    for (int i = 0; i < SVertexBlend::SIZE && vtx.m_Blend.m_Bone[i] != 0xff; ++i)
+    /*
+    result *= 0;
+    for (int i = 0; i < SVertexBlend::SIZE && blendBones[i] != 0xff; i++)
     {
-        result += newPoseMatrices[vtx.m_Blend.m_Bone[i]].Transform(vtx.m_Coords) * vtx.m_Blend.m_Weight[i];
-    }
-
+        if (blendWeight[i] != 0.0)
+            result.mulAdd(newPoseMatrices[blendBones[i]].Transform(coords), blendWeight[i]);
+    }*/
     return result;
 }
 
 CVector3D CModelDef::SkinNormal(const SModelVertex& vtx,
-                                const CMatrix3D newPoseMatrices[])
+                                const CMatrix3D newPoseMatrices[],
+                                CVector3D& result)
 {
     // To be correct, the normal vectors apparently need to be multiplied by the
     // inverse of the transpose. Unfortunately inverses are slow.
@@ -69 +84 @@
     // (This isn't very good as a proof, but it's better than assuming M is
     // orthogonal when it's clearly not.)
 
-    CVector3D result (0, 0, 0);
+    const uint8_t * const blendBones = vtx.m_Blend.m_Bone;
+    const float * const blendWeight = vtx.m_Blend.m_Weight;
+    const CVector3D &normal = vtx.m_Norm;
+    result *= 0;
+    if (blendWeight[0] != 0.0 && blendBones[0] != 0xff)
+            result.mulStore(newPoseMatrices[blendBones[0]].Rotate(normal), blendWeight[0]);
+    if (blendWeight[1] != 0.0 && blendBones[1] != 0xff)
+            result.mulAdd(newPoseMatrices[blendBones[1]].Rotate(normal), blendWeight[1]);
+    if (blendWeight[2] != 0.0 && blendBones[2] != 0xff)
+            result.mulAdd(newPoseMatrices[blendBones[2]].Rotate(normal), blendWeight[2]);
+    if (blendWeight[3] != 0.0 && blendBones[3] != 0xff)
+            result.mulAdd(newPoseMatrices[blendBones[3]].Rotate(normal), blendWeight[3]);
+    /*
+    result *= 0;
+    for (int i = 0; i < SVertexBlend::SIZE && blendBones[i] != 0xff; i++)
+    {
+        if (blendWeight[i] != 0.0f)
+            result.mulAdd(newPoseMatrices[blendBones[i]].Rotate(normal), blendWeight[i]);
+    }*/
 
-    for (int i = 0; i < SVertexBlend::SIZE && vtx.m_Blend.m_Bone[i] != 0xff; ++i)
-    {
-        result += newPoseMatrices[vtx.m_Blend.m_Bone[i]].Rotate(vtx.m_Norm) * vtx.m_Blend.m_Weight[i];
-    }
-    
+
     // If there was more than one influence, the result is probably not going
     // to be of unit length (since it's a weighted sum of several independent
     // unit vectors), so we need to normalise it.
     // (It's fairly common to only have one influence, so it seems sensible to
     // optimise that case a bit.)
-    if (vtx.m_Blend.m_Bone[1] != 0xff) // if more than one influence
+    if (blendBones[1] != 0xff) // if more than one influence
         result.Normalize();
 
     return result;
@@ -88 +117 @@
 }
 
 void CModelDef::SkinPointsAndNormals(
-        size_t numVertices,
+        const size_t numVertices,
         const VertexArrayIterator<CVector3D>& Position,
         const VertexArrayIterator<CVector3D>& Normal,
-        const SModelVertex* vertices,
-        const size_t* blendIndices,
+        const SModelVertex* const vertices,
+        const size_t * const blendIndices,
         const CMatrix3D newPoseMatrices[])
 {
     // To avoid some performance overhead, get the raw vertex array pointers
-    char* PositionData = Position.GetData();
-    size_t PositionStride = Position.GetStride();
-    char* NormalData = Normal.GetData();
-    size_t NormalStride = Normal.GetStride();
+    char* const PositionData = Position.GetData();
+    const size_t PositionStride = Position.GetStride();
+    char* const NormalData = Normal.GetData();
+    const size_t NormalStride = Normal.GetStride();
 
-    for (size_t j = 0; j < numVertices; ++j)
+    for (size_t j = 0; j < numVertices; j++)
     {
         const SModelVertex& vtx = vertices[j];
 
-        CVector3D pos = newPoseMatrices[blendIndices[j]].Transform(vtx.m_Coords);
-        CVector3D norm = newPoseMatrices[blendIndices[j]].Rotate(vtx.m_Norm);
+        CVector3D pos (newPoseMatrices[blendIndices[j]].Transform(vtx.m_Coords));
+        CVector3D norm (newPoseMatrices[blendIndices[j]].Rotate(vtx.m_Norm));
 
         // If there was more than one influence, the result is probably not going
         // to be of unit length (since it's a weighted sum of several independent
@@ -123 +152 @@
 
 #if ARCH_X86_X64
 void CModelDef::SkinPointsAndNormals_SSE(
-        size_t numVertices,
+        const size_t numVertices,
         const VertexArrayIterator<CVector3D>& Position,
         const VertexArrayIterator<CVector3D>& Normal,
         const SModelVertex* vertices,
-        const size_t* blendIndices,
+        const size_t * const blendIndices,
         const CMatrix3D newPoseMatrices[])
 {
     // To avoid some performance overhead, get the raw vertex array pointers
     char* PositionData = Position.GetData();
-    size_t PositionStride = Position.GetStride();
+    const size_t PositionStride = Position.GetStride();
     char* NormalData = Normal.GetData();
-    size_t NormalStride = Normal.GetStride();
+    const size_t NormalStride = Normal.GetStride();
 
     // Must be aligned correctly for SSE
     ASSERT((intptr_t)newPoseMatrices % 16 == 0);
@@ -206 +235 @@
 void CModelDef::BlendBoneMatrices(
         CMatrix3D boneMatrices[])
 {
+
     for (size_t i = 0; i < m_NumBlends; ++i)
     {
         const SVertexBlend& blend = m_pBlends[i];
         CMatrix3D& boneMatrix = boneMatrices[m_NumBones + 1 + i];
+        const uint8_t * const blendBones = blend.m_Bone;
+        const float * const blendWeight = blend.m_Weight;
         
         // Note: there is a special case of joint influence, in which the vertex
         //  is influenced by the bind-shape matrix instead of a particular bone,
@@ -218 +250 @@
         //  set up this matrix in boneMatrices.
         //  (see http://trac.wildfiregames.com/ticket/1012)
 
-        boneMatrix.Blend(boneMatrices[blend.m_Bone[0]], blend.m_Weight[0]);
-        for (size_t j = 1; j < SVertexBlend::SIZE && blend.m_Bone[j] != 0xFF; ++j)
-            boneMatrix.AddBlend(boneMatrices[blend.m_Bone[j]], blend.m_Weight[j]);
+        boneMatrix.Blend(boneMatrices[blendBones[0]], blendWeight[0]);
+        /*
+        for (size_t j = 1; j < SVertexBlend::SIZE && blendBones[j] != 0xFF; ++j)
+        {
+            if (blendWeight[j] != 0.0)
+                boneMatrix.AddBlend(boneMatrices[blendBones[j]], blendWeight[j]);
+        }*/     
+        if (blendWeight[1] != 0.0 && blendBones[1] != 0xff)
+            boneMatrix.AddBlend(boneMatrices[blendBones[1]], blendWeight[1]);
+        if (blendWeight[2] != 0.0 && blendBones[2] != 0xff)
+            boneMatrix.AddBlend(boneMatrices[blendBones[2]], blendWeight[2]);
+        if (blendWeight[3] != 0.0 && blendBones[3] != 0xff)
+            boneMatrix.AddBlend(boneMatrices[blendBones[3]], blendWeight[3]);
     }
 }
 
@@ -400 +442 @@
 
             for (size_t i = 0; i < mdef->m_NumVertices; ++i)
             {
-                mdef->m_pVertices[i].m_Coords = SkinPoint(mdef->m_pVertices[i], &bindPose[0]);
-                mdef->m_pVertices[i].m_Norm = SkinNormal(mdef->m_pVertices[i], &bindPose[0]);
+                mdef->m_pVertices[i].m_Coords = SkinPoint(mdef->m_pVertices[i], &bindPose[0], mdef->m_pVertices[i].m_Coords);
+                SkinNormal(mdef->m_pVertices[i], &bindPose[0], mdef->m_pVertices[i].m_Norm);
             }
         }
     }

source/graphics/ModelDef.h

@@ -84 +84 @@
 // with multiple bones 
 struct SVertexBlend
 {
-    enum { SIZE = 4 };
+    enum { SIZE = 4 }; // WARNING ANY MODIFICATION TO SIZE NEED to add/change loop unroll in SKinPoint/SkinNormal/BlendBonesMatrices
     // index of the influencing bone, or 0xff if none
     u8 m_Bone[SIZE];
     // weight of the influence; all weights sum to 1
@@ -193 +193 @@
      * @return new world-space vertex coordinates
      */
     static CVector3D SkinPoint(const SModelVertex& vtx,
-        const CMatrix3D newPoseMatrices[]);
+        const CMatrix3D newPoseMatrices[],
+        CVector3D &result);
 
     /**
      * Transform the given vertex's normal from the bind pose into the new pose.
@@ -201 +202 @@
      * @return new world-space vertex normal
      */
     static CVector3D SkinNormal(const SModelVertex& vtx,
-        const CMatrix3D newPoseMatrices[]);
+        const CMatrix3D newPoseMatrices[],
+        CVector3D &result);
 
     /**
      * Transform vertices' positions and normals.
@@ -209 +211 @@
      * but slightly more efficient.)
      */
     static void SkinPointsAndNormals(
-        size_t numVertices,
+        const size_t numVertices,
         const VertexArrayIterator<CVector3D>& Position,
         const VertexArrayIterator<CVector3D>& Normal,
         const SModelVertex* vertices,
-        const size_t* blendIndices,
+        const size_t* const blendIndices,
         const CMatrix3D newPoseMatrices[]);
 
 #if ARCH_X86_X64
@@ -221 +223 @@
      * SSE-optimised version of SkinPointsAndNormals.
      */
     static void SkinPointsAndNormals_SSE(
-        size_t numVertices,
+        const size_t numVertices,
         const VertexArrayIterator<CVector3D>& Position,
         const VertexArrayIterator<CVector3D>& Normal,
         const SModelVertex* vertices,
-        const size_t* blendIndices,
+        const size_t* const blendIndices,
         const CMatrix3D newPoseMatrices[]);
 #endif
 

source/maths/Matrix3D.cpp

@@ -58 +58 @@
 //The following clear the matrix and set the 
 //rotation of each of the 3 axes
 
-void CMatrix3D::SetXRotation (float angle)
+void CMatrix3D::SetXRotation (const float angle)
 {
     const float Cos = cosf (angle);
     const float Sin = sinf (angle);
@@ -69 +69 @@
     _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
 }
 
-void CMatrix3D::SetYRotation (float angle)
+void CMatrix3D::SetYRotation (const float angle)
 {
     const float Cos = cosf (angle);
     const float Sin = sinf (angle);
@@ -80 +80 @@
     _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
 }
 
-void CMatrix3D::SetZRotation (float angle)
+void CMatrix3D::SetZRotation (const float angle)
 {
     const float Cos = cosf (angle);
     const float Sin = sinf (angle);
@@ -94 +94 @@
 //The following apply a rotation to the matrix
 //about each of the axes;
 
-void CMatrix3D::RotateX (float angle)
+void CMatrix3D::RotateX (const float angle)
 {
     const float Cos = cosf (angle);
     const float Sin = sinf (angle);
@@ -114 +114 @@
     _34 = Sin * tmp_24 + Cos * _34;
 }
 
-void CMatrix3D::RotateY (float angle)
+void CMatrix3D::RotateY (const float angle)
 {
     const float Cos = cosf (angle);
     const float Sin = sinf (angle);
@@ -134 +134 @@
     _34 = -Sin * tmp_14 + Cos * _34;
 }
 
-void CMatrix3D::RotateZ (float angle)
+void CMatrix3D::RotateZ (const float angle)
 {
     const float Cos = cosf (angle);
     const float Sin = sinf (angle);
@@ -155 +155 @@
 }
 
 //Sets the translation of the matrix
-void CMatrix3D::SetTranslation (float x, float y, float z)
+void CMatrix3D::SetTranslation (const float x, const float y, const float z)
 {
     _11=1.0f; _12=0.0f; _13=0.0f; _14=x;
     _21=0.0f; _22=1.0f; _23=0.0f; _24=y;
@@ -169 +169 @@
 }
 
 //Applies a translation to the matrix
-void CMatrix3D::Translate(float x, float y, float z)
+void CMatrix3D::Translate(const float x, const float y, const float z)
 {
     _14 += x;
     _24 += y;
@@ -183 +183 @@
     _34 += vector.Z;
 }
 
-void CMatrix3D::PostTranslate(float x, float y, float z)
+void CMatrix3D::PostTranslate(const float x, const float y, const float z)
 {
     // Equivalent to "m.SetTranslation(x, y, z); *this = *this * m;"
     _14 += _11*x + _12*y + _13*z;
@@ -198 +198 @@
 }
 
 //Clears and sets the scaling of the matrix
-void CMatrix3D::SetScaling (float x_scale, float y_scale, float z_scale)
+void CMatrix3D::SetScaling (const float x_scale, const float y_scale, const float z_scale)
 {
     _11=x_scale; _12=0.0f;    _13=0.0f;    _14=0.0f;
     _21=0.0f;    _22=y_scale; _23=0.0f;    _24=0.0f;
@@ -207 +207 @@
 }
 
 //Scales the matrix
-void CMatrix3D::Scale (float x_scale, float y_scale, float z_scale)
+void CMatrix3D::Scale (const float x_scale, const float y_scale, const float z_scale)
 {
     _11 *= x_scale;
     _12 *= x_scale;

source/maths/Matrix3D.h

@@ -98 +98 @@
     }
 
     // matrix multiplication
-    CMatrix3D operator*(const CMatrix3D &matrix) const
+    CMatrix3D operator*(const CMatrix3D& matrix) const
     {
         return CMatrix3D(
             _11*matrix._11 + _12*matrix._21 + _13*matrix._31 + _14*matrix._41,
@@ -124 +124 @@
     }
 
     // matrix multiplication/assignment
-    CMatrix3D& operator*=(const CMatrix3D &matrix)
+    CMatrix3D& operator*=(const CMatrix3D& matrix)
     {
         Concatenate(matrix);
         return *this;
@@ -142 +142 @@
     }
 
     // matrix addition
-    CMatrix3D operator+(const CMatrix3D &m) const
+    CMatrix3D operator+(const CMatrix3D& m) const
     {
         return CMatrix3D(
             _11+m._11, _12+m._12, _13+m._13, _14+m._14,
@@ -153 +153 @@
     }
 
     // matrix addition/assignment
-    CMatrix3D& operator+=(const CMatrix3D &m)
+    CMatrix3D& operator+=(const CMatrix3D& m)
     {
         _11 += m._11; _21 += m._21; _31 += m._31; _41 += m._41;
         _12 += m._12; _22 += m._22; _32 += m._32; _42 += m._42;
@@ -163 +163 @@
     }
 
     // equality
-    bool operator==(const CMatrix3D &m) const
+    bool operator==(const CMatrix3D& m) const
     {
         return _11 == m._11 && _21 == m._21 && _31 == m._31 && _41 == m._41 &&
                  _12 == m._12 && _22 == m._22 && _32 == m._32 && _42 == m._42 &&
@@ -187 +187 @@
     // concatenate arbitrary matrix onto this matrix
     void Concatenate(const CMatrix3D& m)
     {
-        (*this) = m * (*this);
+        //(*this) = m * (*this);
+        premul(m);
     }
+    // mul and add wihtout stack alloc
+    void mulAdd(const CMatrix3D& matrixA, const CMatrix3D& matrixB) 
+    {
+        _11 +=  matrixA._11*matrixB._11 +matrixA._12*matrixB._21 +matrixA._13*matrixB._31 +matrixA._14*matrixB._41;
+        _12 +=  matrixA._11*matrixB._12 +matrixA._12*matrixB._22 +matrixA._13*matrixB._32 +matrixA._14*matrixB._42;
+        _13 +=  matrixA._11*matrixB._13 +matrixA._12*matrixB._23 +matrixA._13*matrixB._33 +matrixA._14*matrixB._43;
+        _14 +=  matrixA._11*matrixB._14 +matrixA._12*matrixB._24 +matrixA._13*matrixB._34 +matrixA._14*matrixB._44;
+        
+        _21 +=  matrixA._21*matrixB._11 +matrixA._22*matrixB._21 +matrixA._23*matrixB._31 +matrixA._24*matrixB._41;
+        _22 +=  matrixA._21*matrixB._12 +matrixA._22*matrixB._22 +matrixA._23*matrixB._32 +matrixA._24*matrixB._42;
+        _23 +=  matrixA._21*matrixB._13 +matrixA._22*matrixB._23 +matrixA._23*matrixB._33 +matrixA._24*matrixB._43;
+        _24 +=  matrixA._21*matrixB._14 +matrixA._22*matrixB._24 +matrixA._23*matrixB._34 +matrixA._24*matrixB._44;
+        
+        _31 +=  matrixA._31*matrixB._11 +matrixA._32*matrixB._21 +matrixA._33*matrixB._31 +matrixA._34*matrixB._41;
+        _32 +=  matrixA._31*matrixB._12 +matrixA._32*matrixB._22 +matrixA._33*matrixB._32 +matrixA._34*matrixB._42;
+        _33 +=  matrixA._31*matrixB._13 +matrixA._32*matrixB._23 +matrixA._33*matrixB._33 +matrixA._34*matrixB._43;
+        _34 +=  matrixA._31*matrixB._14 +matrixA._32*matrixB._24 +matrixA._33*matrixB._34 +matrixA._34*matrixB._44;
+        
+        _41 +=  matrixA._41*matrixB._11 +matrixA._42*matrixB._21 +matrixA._43*matrixB._31 +matrixA._44*matrixB._41;
+        _42 +=  matrixA._41*matrixB._12 +matrixA._42*matrixB._22 +matrixA._43*matrixB._32 +matrixA._44*matrixB._42;
+        _43 +=  matrixA._41*matrixB._13 +matrixA._42*matrixB._23 +matrixA._43*matrixB._33 +matrixA._44*matrixB._43;
+        _44 +=  matrixA._41*matrixB._14 +matrixA._42*matrixB._24 +matrixA._43*matrixB._34 +matrixA._44*matrixB._44;
+            
+    }
+    // mul and and store wihtout stack alloc
+    void mul(const CMatrix3D& matrixA, const CMatrix3D& matrixB) 
+    {
+        _11 =   matrixA._11*matrixB._11 +matrixA._12*matrixB._21 +matrixA._13*matrixB._31 +matrixA._14*matrixB._41;
+        _12 =   matrixA._11*matrixB._12 +matrixA._12*matrixB._22 +matrixA._13*matrixB._32 +matrixA._14*matrixB._42;
+        _13 =   matrixA._11*matrixB._13 +matrixA._12*matrixB._23 +matrixA._13*matrixB._33 +matrixA._14*matrixB._43;
+        _14 =   matrixA._11*matrixB._14 +matrixA._12*matrixB._24 +matrixA._13*matrixB._34 +matrixA._14*matrixB._44;
+        
+        _21 =   matrixA._21*matrixB._11 +matrixA._22*matrixB._21 +matrixA._23*matrixB._31 +matrixA._24*matrixB._41;
+        _22 =   matrixA._21*matrixB._12 +matrixA._22*matrixB._22 +matrixA._23*matrixB._32 +matrixA._24*matrixB._42;
+        _23 =   matrixA._21*matrixB._13 +matrixA._22*matrixB._23 +matrixA._23*matrixB._33 +matrixA._24*matrixB._43;
+        _24 =   matrixA._21*matrixB._14 +matrixA._22*matrixB._24 +matrixA._23*matrixB._34 +matrixA._24*matrixB._44;
+        
+        _31 =   matrixA._31*matrixB._11 +matrixA._32*matrixB._21 +matrixA._33*matrixB._31 +matrixA._34*matrixB._41;
+        _32 =   matrixA._31*matrixB._12 +matrixA._32*matrixB._22 +matrixA._33*matrixB._32 +matrixA._34*matrixB._42;
+        _33 =   matrixA._31*matrixB._13 +matrixA._32*matrixB._23 +matrixA._33*matrixB._33 +matrixA._34*matrixB._43;
+        _34 =   matrixA._31*matrixB._14 +matrixA._32*matrixB._24 +matrixA._33*matrixB._34 +matrixA._34*matrixB._44;
+        
+        _41 =   matrixA._41*matrixB._11 +matrixA._42*matrixB._21 +matrixA._43*matrixB._31 +matrixA._44*matrixB._41;
+        _42 =   matrixA._41*matrixB._12 +matrixA._42*matrixB._22 +matrixA._43*matrixB._32 +matrixA._44*matrixB._42;
+        _43 =   matrixA._41*matrixB._13 +matrixA._42*matrixB._23 +matrixA._43*matrixB._33 +matrixA._44*matrixB._43;
+        _44 =   matrixA._41*matrixB._14 +matrixA._42*matrixB._24 +matrixA._43*matrixB._34 +matrixA._44*matrixB._44;
+            
+    }
+    
+    // matrix pre multiplication wihtout object alloc.
+    void premul(const CMatrix3D& matrixA) 
+    {   
+        const float __11 = matrixA._11*_11 + matrixA._12*_21 + matrixA._13*_31 + matrixA._14*_41;
+        const float __12 = matrixA._11*_12 + matrixA._12*_22 + matrixA._13*_32 + matrixA._14*_42;
+        const float __13 = matrixA._11*_13 + matrixA._12*_23 + matrixA._13*_33 + matrixA._14*_43;
+        const float __14 = matrixA._11*_14 + matrixA._12*_24 + matrixA._13*_34 + matrixA._14*_44;
+              
+        const float __21 = matrixA._21*_11 + matrixA._22*_21 + matrixA._23*_31 + matrixA._24*_41;
+        const float __22 = matrixA._21*_12 + matrixA._22*_22 + matrixA._23*_32 + matrixA._24*_42;
+        const float __23 = matrixA._21*_13 + matrixA._22*_23 + matrixA._23*_33 + matrixA._24*_43;
+        const float __24 = matrixA._21*_14 + matrixA._22*_24 + matrixA._23*_34 + matrixA._24*_44;
+              
+        const float __31 = matrixA._31*_11 + matrixA._32*_21 + matrixA._33*_31 + matrixA._34*_41;
+        const float __32 = matrixA._31*_12 + matrixA._32*_22 + matrixA._33*_32 + matrixA._34*_42;
+        const float __33 = matrixA._31*_13 + matrixA._32*_23 + matrixA._33*_33 + matrixA._34*_43;
+        const float __34 = matrixA._31*_14 + matrixA._32*_24 + matrixA._33*_34 + matrixA._34*_44;
+              
+        const float __41 = matrixA._41*_11 + matrixA._42*_21 + matrixA._43*_31 + matrixA._44*_41;
+        const float __42 = matrixA._41*_12 + matrixA._42*_22 + matrixA._43*_32 + matrixA._44*_42;
+        const float __43 = matrixA._41*_13 + matrixA._42*_23 + matrixA._43*_33 + matrixA._44*_43;
+        const float __44 = matrixA._41*_14 + matrixA._42*_24 + matrixA._43*_34 + matrixA._44*_44;
 
+        _11 = __11;
+        _21 = __21;
+        _31 = __31;
+        _41 = __41;
+          
+        _12 = __12;
+        _22 = __22;
+        _32 = __32;
+        _42 = __42;
+          
+        _13 = __13;
+        _23 = __23;
+        _33 = __33;
+        _43 = __43;
+          
+        _14 = __14;
+        _24 = __24;
+        _34 = __34;
+        _44 = __44;
+
+    }
+    // matrix multiplication wihtout object alloc.
+    void mul(const CMatrix3D& matrixB) 
+    {   
+        float __1, __2, __3, __4;
+
+        __1 = _11*matrixB._11 + _12*matrixB._21 + _13*matrixB._31 + _14*matrixB._41;
+        __2 = _11*matrixB._12 + _12*matrixB._22 + _13*matrixB._32 + _14*matrixB._42;
+        __3 = _11*matrixB._13 + _12*matrixB._23 + _13*matrixB._33 + _14*matrixB._43;
+        __4 = _11*matrixB._14 + _12*matrixB._24 + _13*matrixB._34 + _14*matrixB._44;
+              
+        _11 = __1;  _12 = __2;  _13 = __3;  _14 = __4;
+
+        __1 = _21*matrixB._11 + _22*matrixB._21 + _23*matrixB._31 + _24*matrixB._41;
+        __2 = _21*matrixB._12 + _22*matrixB._22 + _23*matrixB._32 + _24*matrixB._42;
+        __3 = _21*matrixB._13 + _22*matrixB._23 + _23*matrixB._33 + _24*matrixB._43;
+        __4 = _21*matrixB._14 + _22*matrixB._24 + _23*matrixB._34 + _24*matrixB._44;
+              
+        _21 = __1;  _22 = __2;  _23 = __3;  _24 = __4;
+
+        __1 = _31*matrixB._11 + _32*matrixB._21 + _33*matrixB._31 + _34*matrixB._41;
+        __2 = _31*matrixB._12 + _32*matrixB._22 + _33*matrixB._32 + _34*matrixB._42;
+        __3 = _31*matrixB._13 + _32*matrixB._23 + _33*matrixB._33 + _34*matrixB._43;
+        __4 = _31*matrixB._14 + _32*matrixB._24 + _33*matrixB._34 + _34*matrixB._44;
+              
+        _31 = __1;  _32 = __2;  _33 = __3;  _34 = __4;
+
+        __1 = _41*matrixB._11 + _42*matrixB._21 + _43*matrixB._31 + _44*matrixB._41;
+        __2 = _41*matrixB._12 + _42*matrixB._22 + _43*matrixB._32 + _44*matrixB._42;
+        __3 = _41*matrixB._13 + _42*matrixB._23 + _43*matrixB._33 + _44*matrixB._43;
+        __4 = _41*matrixB._14 + _42*matrixB._24 + _43*matrixB._34 + _44*matrixB._44;
+        
+        _41 = __1;  _42 = __2;  _43 = __3;  _44 = __4;
+    }
     // blend matrix using only 4x3 subset
-    void Blend(const CMatrix3D& m, float f)
+    void Blend(const CMatrix3D& m, const float f)
     {
         _11 = m._11*f; _21 = m._21*f; _31 = m._31*f;
         _12 = m._12*f; _22 = m._22*f; _32 = m._32*f;
@@ -200 +326 @@
     }
 
     // blend matrix using only 4x3 and add onto existing blend
-    void AddBlend(const CMatrix3D& m, float f)
+    void AddBlend(const CMatrix3D& m, const float f)
     {
         _11 += m._11*f; _21 += m._21*f; _31 += m._31*f;
         _12 += m._12*f; _22 += m._22*f; _32 += m._32*f;

source/maths/Vector3D.h

@@ -81 +81 @@
         {
             return CVector3D(X * value, Y * value, Z * value);
         }
+        
+        void mulAdd(const CVector3D& vector, const float value)
+        {
+            X += vector.X * value;
+            Y += vector.Y * value;
+            Z += vector.Z * value;
+        }
+        void mulStore(const CVector3D& vector, const float value)
+        {
+            X = vector.X * value;
+            Y = vector.Y * value;
+            Z = vector.Z * value;
+        }
+        void mul(const float value)
+        {
+            X *= value;
+            Y *= value;
+            Z *= value;
+        }
 
         CVector3D& operator*=(float value)
         {