Kyoto/Animation: add CAnimMathUtils, CMetaAnimBlend, CAnimTreeBlend

include/Kyoto/Animation/CAnimMathUtils.hpp

@@ -0,0 +1,12 @@
+#ifndef _CANIMMATHUTILS
+#define _CANIMMATHUTILS
+
+#include "Kyoto/Math/CQuaternion.hpp"
+
+class CAnimMathUtils {
+public:
+  static CQuaternion Slerp(const CQuaternion& a, const CQuaternion& b, float t);
+  static CQuaternion SlerpLocal(const CQuaternion& from, const CQuaternion& to, float t);
+};
+
+#endif // _CANIMMATHUTILS

include/Kyoto/Animation/CAnimTreeBlend.hpp

@@ -0,0 +1,20 @@
+#ifndef _CANIMTREEBLEND
+#define _CANIMTREEBLEND
+
+#include "Kyoto/Animation/CAnimTreeTweenBase.hpp"
+
+class CAnimTreeBlend : public CAnimTreeTweenBase {
+  float x24_blendWeight;
+public:
+  CAnimTreeBlend(bool b, const rstl::ncrc_ptr< CAnimTreeNode >& a,
+                 const rstl::ncrc_ptr< CAnimTreeNode >& b2, int flags,
+                 const rstl::string& name);
+  void SetBlendingWeight(float w) override { x24_blendWeight = w; }
+  float VGetBlendingWeight() const override { return x24_blendWeight; }
+  float VGetRightChildWeight() const override { return x24_blendWeight; }
+  CCharAnimTime VGetTimeRemaining() const override;
+  CSteadyStateAnimInfo VGetSteadyStateAnimInfo() const override;
+  rstl::auto_ptr< IAnimReader > VClone() const override;
+};
+
+#endif // _CANIMTREEBLEND

include/Kyoto/Animation/IMetaAnim.hpp

@@ -1,70 +1,55 @@
 #ifndef _IMETAANIM
 #define _IMETAANIM
-
 #include "Kyoto/Animation/CCharAnimTime.hpp"
-
 #include <rstl/optional_object.hpp>
 #include <rstl/rc_ptr.hpp>
 #include <rstl/set.hpp>
 #include <rstl/string.hpp>
-
 enum EMetaAnimType {
   kMAT_Play,
   kMAT_Blend,
   kMAT_PhaseBlend,
   kMAT_Random,
   kMAT_Sequence,
 };
-
 class CAnimTreeNode;
 class CPrimitive;
 class CCharAnimTime;
 class IAnimReader;
 class CAnimSysContext;
-
 class CPreAdvanceIndicator {
   bool mIsTime;
   CCharAnimTime mTime;
   const char* mString;
   uint x10_[11];
   ushort x3c_;
-
 public:
   explicit CPreAdvanceIndicator(const CCharAnimTime& time) : mIsTime(true), mTime(time), mString(0) {}
   explicit CPreAdvanceIndicator(const char* string) : mIsTime(false), mString(string) {}
   bool IsTime() const;
   const CCharAnimTime& GetTime() const;
-
   bool IsString() const;
   const char* const& GetString() const;
 };
-
 class CMetaAnimTreeBuildOrders {
 public:
   static CMetaAnimTreeBuildOrders NoSpecialOrders();
   static CMetaAnimTreeBuildOrders PreAdvanceForAll(const CPreAdvanceIndicator& ind);
-
-private:
   rstl::optional_object< CPreAdvanceIndicator > mRecursiveAdvance;
   rstl::optional_object< CPreAdvanceIndicator > mSingleAdvance;
 };
-
 class IMetaAnim {
 public:
   virtual ~IMetaAnim() {}
-
   virtual rstl::rc_ptr< CAnimTreeNode >
   GetAnimationTree(const CAnimSysContext& animSys, const CMetaAnimTreeBuildOrders& orders) const;
   virtual void GetUniquePrimitives(rstl::set< CPrimitive >& primsOut) const = 0;
   virtual EMetaAnimType GetType() const = 0;
   virtual rstl::rc_ptr< CAnimTreeNode >
   VGetAnimationTree(const CAnimSysContext& animSys,
                     const CMetaAnimTreeBuildOrders& orders) const = 0;
-
   void PutTo(COutputStream&) const;
-
   static void AdvanceAnim(IAnimReader& anim, const CCharAnimTime& dt);
   static CCharAnimTime GetTime(const CPreAdvanceIndicator& ind, const IAnimReader& anim);
 };
-
-#endif // _IMETAANIM
+#endif // _IMETAANIM

src/Kyoto/Animation/CAnimMathUtils.cpp

@@ -0,0 +1,87 @@
+#include "Kyoto/Animation/CAnimMathUtils.hpp"
+#include "Kyoto/Math/CQuaternion.hpp"
+#include "Kyoto/Math/CMath.hpp"
+#include "rstl/vector.hpp"
+
+CQuaternion CAnimMathUtils::Slerp(const CQuaternion& a, const CQuaternion& b, float t) {
+  if (t <= 0.00001f) {
+    return a;
+  }
+  if (t >= 0.99999f) {
+    return b;
+  }
+  float dot = a.GetY() * b.GetY();
+  dot = a.GetX() * b.GetX() + dot;
+  dot = a.GetZ() * b.GetZ() + dot;
+  dot = a.GetW() * b.GetW() + dot;
+  if (dot >= 1.0f) {
+    return a;
+  }
+  if (dot > 0.95f) {
+    float dx = b.GetX() - a.GetX();
+    float dy = b.GetY() - a.GetY();
+    float dz = b.GetZ() - a.GetZ();
+    float lx = t * dx;
+    float lz = t * dz;
+    float ly = t * dy;
+    float rx = a.GetX() + lx;
+    float dw = b.GetW() - a.GetW();
+    float ry = a.GetY() + ly;
+    float rz = a.GetZ() + lz;
+    float lw = t * dw;
+    float rw = a.GetW() + lw;
+    float lenSq = rx * rx + ry * ry + rz * rz + rw * rw;
+    float inv = CMath::InvSqrtF(lenSq);
+    return CQuaternion(inv * rw, inv * rx, inv * ry, inv * rz);
+  }
+  float angle = CMath::FastArcCosR(dot);
+  float sinA = CMath::FastSinR(angle * (1.0f - t));
+  float sinB = CMath::FastSinR(angle * t);
+  #pragma fp_contract off
+  float r4 = sinA * a.GetX();
+  float r3 = sinB * b.GetX();
+  float r2 = sinA * a.GetY();
+  float r0 = sinB * b.GetY();
+  float r6 = sinA * a.GetZ();
+  float r5 = sinB * b.GetZ();
+  float r7 = sinA * a.GetW();
+  float r1 = sinB * b.GetW();
+  #pragma fp_contract on
+  float r29 = r2 + r0;
+  float r28 = r4 + r3;
+  float r31 = r6 + r5;
+  float r30 = r7 + r1;
+  float lenSq2 = r29 * r29 + r28 * r28 + r31 * r31 + r30 * r30;
+  float inv2 = CMath::InvSqrtF(lenSq2);
+  return CQuaternion(inv2 * r30, inv2 * r29, inv2 * r28, inv2 * r31);
+}
+
+CQuaternion CAnimMathUtils::SlerpLocal(const CQuaternion& from, const CQuaternion& to, float t) {
+  float dot = from.GetY() * to.GetY();
+  dot = from.GetX() * to.GetX() + dot;
+  dot = from.GetZ() * to.GetZ() + dot;
+  dot = from.GetW() * to.GetW() + dot;
+  if (dot >= 0.0f) {
+    return CAnimMathUtils::Slerp(from, to, t);
+  }
+  CQuaternion toEq = to.BuildEquivalent();
+  return CAnimMathUtils::Slerp(from, toEq, t);
+}
+
+template <>
+rstl::vector< CQuaternion, rstl::rmemory_allocator >::vector(CInputStream& in,
+    const rstl::rmemory_allocator& alloc)
+: x0_allocator(alloc), x4_count(0), x8_capacity(0), xc_items(nullptr) {
+  int count = in.ReadLong();
+  reserve(count);
+  for (int i = 0; i < count; ++i) {
+    CQuaternion q(in);
+    if (x4_count >= x8_capacity) {
+      reserve(x8_capacity == 0 ? 4 : x8_capacity * 2);
+    }
+    if (xc_items != nullptr) {
+      xc_items[x4_count] = q;
+    }
+    ++x4_count;
+  }
+}

src/Kyoto/Animation/CMetaAnimBlend.cpp

@@ -1,4 +1,5 @@
 #include "Kyoto/Animation/CMetaAnimBlend.hpp"
+#include "Kyoto/Animation/CAnimTreeBlend.hpp"
 #include "Kyoto/Animation/CMetaAnimFactory.hpp"
 #include "Kyoto/Streams/CInputStream.hpp"
 #include "Kyoto/Streams/COutputStream.hpp"
@@ -11,7 +12,26 @@ CMetaAnimBlend::CMetaAnimBlend(CInputStream& in)
 
 rstl::rc_ptr< CAnimTreeNode >
 CMetaAnimBlend::VGetAnimationTree(const CAnimSysContext& animSys,
-                                  const CMetaAnimTreeBuildOrders& orders) const {}
+                                  const CMetaAnimTreeBuildOrders& orders) const {
+  CMetaAnimTreeBuildOrders ordersA = CMetaAnimTreeBuildOrders::NoSpecialOrders();
+  CMetaAnimTreeBuildOrders ordersB = CMetaAnimTreeBuildOrders::NoSpecialOrders();
+
+  if (orders.mRecursiveAdvance) {
+    ordersA = CMetaAnimTreeBuildOrders::PreAdvanceForAll(*orders.mRecursiveAdvance);
+  }
+
+  if (orders.mSingleAdvance) {
+    ordersB = CMetaAnimTreeBuildOrders::PreAdvanceForAll(*orders.mSingleAdvance);
+  }
+
+  rstl::rc_ptr< CAnimTreeNode > treeA = x4_animA->GetAnimationTree(animSys, ordersA);
+  rstl::rc_ptr< CAnimTreeNode > treeB = x8_animB->GetAnimationTree(animSys, ordersB);
+
+  CAnimTreeBlend* blend = new CAnimTreeBlend(x10_, treeA, treeB, 0, rstl::string_l("??(??)")
+  );
+  blend->SetBlendingWeight(xc_blend);
+  return rstl::rc_ptr< CAnimTreeNode >(blend);
+}
 
 void CMetaAnimBlend::GetUniquePrimitives(rstl::set< CPrimitive >& primsOut) const {
   x4_animA->GetUniquePrimitives(primsOut);
@@ -23,4 +43,4 @@ void CMetaAnimBlend::WriteAnimData(COutputStream& out) const {
   x8_animB->PutTo(out);
   out.WriteReal32(xc_blend);
   out.WriteChar(bool(x10_));
-}
+}