mirror of https://github.com/AxioDL/metaforce.git
92 lines
3.9 KiB
C++
92 lines
3.9 KiB
C++
#include "Runtime/Character/CIkChain.hpp"
|
|
|
|
#include "Runtime/Character/CAnimData.hpp"
|
|
|
|
namespace metaforce {
|
|
|
|
void CIkChain::Update(float dt) {
|
|
if (x44_24_activated)
|
|
x40_time = zeus::min(x40_time + dt, 1.f);
|
|
else
|
|
x40_time = zeus::max(0.f, x40_time - dt);
|
|
}
|
|
|
|
void CIkChain::Deactivate() { x44_24_activated = false; }
|
|
|
|
void CIkChain::Activate(const CAnimData& animData, const CSegId& segId, const zeus::CTransform& xf) {
|
|
x0_bone = segId;
|
|
const auto& info = animData.GetPoseBuilder().CharLayoutInfo();
|
|
x1_p1 = info->GetRootNode()->GetBoneMap()[x0_bone].x0_parentId;
|
|
if (x1_p1 != 2) {
|
|
x2_p2 = info->GetRootNode()->GetBoneMap()[x1_p1].x0_parentId;
|
|
x4_p2p1Dir = info->GetFromParentUnrotated(x1_p1);
|
|
x1c_p2p1Length = x4_p2p1Dir.magnitude();
|
|
x4_p2p1Dir = x4_p2p1Dir / x1c_p2p1Length;
|
|
x10_p1BoneDir = info->GetFromParentUnrotated(x0_bone);
|
|
x20_p1BoneLength = x10_p1BoneDir.magnitude();
|
|
x10_p1BoneDir = x10_p1BoneDir / x20_p1BoneLength;
|
|
x34_holdPos = xf.origin;
|
|
x24_holdRot = zeus::CQuaternion(xf.basis);
|
|
x44_24_activated = true;
|
|
}
|
|
}
|
|
|
|
void CIkChain::PreRender(CAnimData& animData, const zeus::CTransform& xf, const zeus::CVector3f& scale) {
|
|
if (x40_time > 0.f) {
|
|
zeus::CTransform p2Xf = animData.GetLocatorTransform(x2_p2, nullptr);
|
|
zeus::CVector3f localDelta = xf.transposeRotate(x34_holdPos - xf.origin);
|
|
localDelta /= scale;
|
|
localDelta = p2Xf.transposeRotate(localDelta - p2Xf.origin);
|
|
zeus::CQuaternion p2Rot = animData.PoseBuilder().GetTreeMap()[x2_p2].x4_rotation;
|
|
zeus::CQuaternion p1Rot = animData.PoseBuilder().GetTreeMap()[x1_p1].x4_rotation;
|
|
zeus::CQuaternion boneRot = animData.PoseBuilder().GetTreeMap()[x0_bone].x4_rotation;
|
|
zeus::CQuaternion newP2Rot = p2Rot;
|
|
zeus::CQuaternion newP1Rot = p1Rot;
|
|
Solve(newP2Rot, newP1Rot, localDelta);
|
|
zeus::CQuaternion newBoneRot =
|
|
(zeus::CQuaternion((xf * p2Xf).basis) * p2Rot.inverse() * newP2Rot * newP1Rot).inverse() * x24_holdRot;
|
|
if (x40_time < 1.f) {
|
|
newP2Rot = zeus::CQuaternion::slerpShort(p2Rot, newP2Rot, x40_time);
|
|
newP1Rot = zeus::CQuaternion::slerpShort(p1Rot, newP1Rot, x40_time);
|
|
newBoneRot = zeus::CQuaternion::slerpShort(boneRot, newBoneRot, x40_time);
|
|
}
|
|
animData.PoseBuilder().GetTreeMap()[x2_p2].x4_rotation = newP2Rot;
|
|
animData.PoseBuilder().GetTreeMap()[x1_p1].x4_rotation = newP1Rot;
|
|
animData.PoseBuilder().GetTreeMap()[x0_bone].x4_rotation = newBoneRot;
|
|
animData.MarkPoseDirty();
|
|
}
|
|
}
|
|
|
|
void CIkChain::Solve(zeus::CQuaternion& q1, zeus::CQuaternion& q2, const zeus::CVector3f& vec) {
|
|
const float mag = vec.magnitude();
|
|
const float magSq = mag * mag;
|
|
const float twoMag = (2.0f * mag);
|
|
float f29 =
|
|
std::acos(zeus::clamp(-1.f,
|
|
(((x20_p1BoneLength * magSq) + x20_p1BoneLength) - (x1c_p2p1Length * x1c_p2p1Length)) /
|
|
(twoMag * x20_p1BoneLength),
|
|
1.f));
|
|
float f30 = std::acos(zeus::clamp(
|
|
-1.f,
|
|
((x1c_p2p1Length * (magSq - (x20_p1BoneLength * x20_p1BoneLength))) + x1c_p2p1Length) / (twoMag * x1c_p2p1Length),
|
|
1.f));
|
|
|
|
zeus::CVector3f vecA = q2.transform(x10_p1BoneDir);
|
|
zeus::CVector3f crossVecA = x4_p2p1Dir.cross(vecA);
|
|
float crossAMag = crossVecA.magnitude();
|
|
crossVecA *= zeus::CVector3f(1.f / crossVecA.magnitude());
|
|
float angle = std::asin(zeus::min(crossAMag, 1.f));
|
|
if (x4_p2p1Dir.dot(vecA) < 0.f)
|
|
angle = M_PIF - angle;
|
|
q2 = zeus::CQuaternion::fromAxisAngle(crossVecA, (f30 + f29) - angle) * q2;
|
|
zeus::CVector3f v1 = q1.transform((x1c_p2p1Length * x4_p2p1Dir) + (x20_p1BoneLength * q2.transform(x10_p1BoneDir)));
|
|
zeus::CVector3f v2 = q1.transform(vec);
|
|
zeus::CVector3f crossVecB = v1.normalized().cross((1.f / mag) * v2);
|
|
angle = std::asin(zeus::min(crossVecB.magnitude(), 1.f));
|
|
if (v1.dot((1.f / mag) * v2) < 0.f)
|
|
angle = M_PIF - angle;
|
|
|
|
q1 = zeus::CQuaternion::fromAxisAngle(crossVecB * (1.f / crossVecB.magnitude()), angle) * q1;
|
|
}
|
|
} // namespace metaforce
|