mirror of
https://github.com/simplysanatori/KartFrame.git
synced 2026-10-17 16:02:19 +02:00
128 lines
7.2 KiB
C++
128 lines
7.2 KiB
C++
// SPDX-License-Identifier: GPL-3.0-or-later
|
|
#include "vr/body_ik.h"
|
|
#include "vr/body_hand_defaults.h"
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <limits>
|
|
using namespace mkw::vr;
|
|
using namespace mkw::vr::body_ik;
|
|
static void Check(bool ok) { if (!ok) { std::fprintf(stderr,"body IK invariant failed\n");std::abort(); } }
|
|
static bool Near(float a,float b) { return std::abs(a-b)<0.0005f; }
|
|
int main() {
|
|
for(uint16_t id:{0,2,3,4,5,6,7,15,16,17,18,19}) Check(IsDrivingLegAnimation(id));
|
|
for(uint16_t id:{1,8,9,10,11,12,13,14,20,21,27,28,40,65535}) Check(!IsDrivingLegAnimation(id));
|
|
LegAnimationGate celebration;
|
|
Check(celebration.AllowNative(false,false,true,1000000000ull,false));
|
|
Check(!celebration.AllowNative(false,false,true,1010000000ull,true));
|
|
Check(!celebration.AllowNative(false,false,false,4000000000ull,true)); // boost already ended
|
|
Check(!celebration.AllowNative(false,false,false,4100000000ull,false)); // blend-out
|
|
Check(celebration.AllowNative(false,false,false,4300000000ull,false));
|
|
Check(celebration.AllowNative(false,false,true,4400000000ull,false));
|
|
LegAnimationGate gate;
|
|
Check(gate.AllowNative(false,false,false,1000000000ull));
|
|
Check(gate.AllowNative(false,false,true,1010000000ull)); // drift
|
|
Check(!gate.AllowNative(false,true,false,1020000000ull)); // MT release
|
|
Check(!gate.AllowNative(false,true,false,1100000000ull)); // reward animation
|
|
Check(!gate.AllowNative(false,false,false,1200000000ull)); // settling
|
|
Check(gate.AllowNative(false,false,false,1400000000ull));
|
|
Check(!gate.AllowNative(false,true,false,1500000000ull));
|
|
Check(gate.AllowNative(false,true,true,1510000000ull)); // next drift during boost
|
|
Check(!gate.AllowNative(true,true,true,1520000000ull));
|
|
Check(!gate.AllowNative(false,true,true,1600000000ull)); // trick hold still applies
|
|
Check(gate.AllowNative(false,true,true,1800000000ull));
|
|
// Turning/drifting leg motion survives, but an animated root's rotation
|
|
// and jump translation do not move the seated pelvis or chest.
|
|
auto seated_parent=kIdentityMtx34;seated_parent[7]=0.4f;
|
|
auto animated_parent=Align({}, {0,1,0},{1,0,0});
|
|
animated_parent[3]=4;animated_parent[7]=8;animated_parent[11]=-2;
|
|
auto leg_local=Align({}, {0,-1,0},{0,-1,-1});leg_local[7]=-0.3f;leg_local[11]=-0.1f;
|
|
const auto animated_leg=ComposeMtx(animated_parent,leg_local);
|
|
Mtx34 seated_leg{};
|
|
Check(AnimateSeatedLimb(seated_parent,animated_parent,animated_leg,seated_leg));
|
|
const auto expected_leg=ComposeMtx(seated_parent,leg_local);
|
|
for(unsigned i=0;i<12;++i) Check(Near(seated_leg[i],expected_leg[i]));
|
|
Check(!AnimateSeatedLimb(seated_parent,Mtx34{},animated_leg,seated_leg));
|
|
const auto defaults=DefaultBodyHandCalibrations();
|
|
Check(defaults.size()==24);
|
|
for(const auto& [key,profile]:defaults) {
|
|
Check(!key.empty() && profile.size()==24);
|
|
for(int hand=0;hand<2;++hand) {
|
|
Mtx34 pose{};
|
|
std::copy_n(profile.begin()+hand*12,12,pose.begin());
|
|
Check(detail::IsFiniteMtx34(pose));
|
|
Check(Length(Position(pose))<0.18f);
|
|
for(int column=0;column<3;++column)
|
|
Check(std::abs(Length({pose[column],pose[column+4],pose[column+8]})-1)<0.002f);
|
|
}
|
|
}
|
|
for (float scale: {0.2f,1.0f,2.5f}) {
|
|
// A close controller must not create the old 65cm folded sleeve.
|
|
const float reach_fit=FitArmReach(0.5f*scale,0.2f*scale,scale);
|
|
Check(Near(0.5f*scale*reach_fit,0.30f*scale));
|
|
const auto close=Solve({}, {0.25f*scale*reach_fit,0,0},
|
|
{0.5f*scale*reach_fit,0,0},{0,0,-0.2f*scale},{-0.7f,-1,0.25f});
|
|
Check(close.valid && Near(close.wrist.z,-0.2f*scale));
|
|
Check(Length(close.elbow)<0.16f*scale);
|
|
const auto extended=FitArmReach(0.5f*scale,0.8f*scale,scale);
|
|
Check(0.5f*scale*extended>0.8f*scale);
|
|
const Vec s{0,0,0}, e{scale,0,0}, w{2*scale,0,0};
|
|
for (Vec target: {Vec{scale,scale,0},Vec{100,0,0},Vec{0,0,0},Vec{-scale,0,0}}) {
|
|
const auto arm=Solve(s,e,w,target,{0,-1,0});
|
|
Check(arm.valid);
|
|
Check(Near(Length(Sub(arm.elbow,s)),scale));
|
|
Check(Near(Length(Sub(arm.wrist,arm.elbow)),scale));
|
|
Check(detail::IsFiniteFloat(&arm.elbow.x));
|
|
}
|
|
}
|
|
Check(!Solve({},{},{},{1,0,0},{0,-1,0}).valid);
|
|
Check(!Solve({},{1,0,0},{2,0,0},{std::numeric_limits<float>::quiet_NaN(),0,0},{0,-1,0}).valid);
|
|
const Vec pivot{4,5,6};
|
|
for(Vec direction: {Vec{1,0,0},Vec{-1,0,0},Vec{0,0,-1},Vec{0,1,0}}) {
|
|
auto grip=Align({}, {1,0,0}, direction);
|
|
grip[3]=0.3f;grip[7]=-0.2f;grip[11]=-0.4f;
|
|
const Vec expected{0.015f,-0.03f,0.055f};
|
|
const auto wrist=detail::TransformPoint(grip,expected.x,expected.y,expected.z);
|
|
auto wrist_pose=Align({}, {0,1,0}, direction);
|
|
wrist_pose[3]=wrist.x;wrist_pose[7]=wrist.y;wrist_pose[11]=wrist.z;
|
|
Mtx34 correction{};
|
|
Check(CalibrateGripPose(grip,wrist_pose,correction));
|
|
const auto restored=ComposeMtx(grip,correction);
|
|
for(unsigned i=0;i<12;++i) Check(Near(restored[i],wrist_pose[i]));
|
|
Vec captured{};
|
|
Check(CalibrateGripOffset(grip,wrist,captured));
|
|
Check(Near(captured.x,expected.x) && Near(captured.y,expected.y) && Near(captured.z,expected.z));
|
|
// A saved controller-local offset follows a new orientation and pose.
|
|
auto moved=Align({}, {1,0,0}, {-direction.x,-direction.y,-direction.z});
|
|
moved[3]=-0.5f;moved[7]=0.1f;moved[11]=-0.7f;
|
|
const auto applied=detail::TransformPoint(moved,captured.x,captured.y,captured.z);
|
|
Check(Near(Length(Sub(applied,Position(moved))),Length(expected)));
|
|
}
|
|
Vec rejected{};
|
|
Check(!CalibrateGripOffset(kIdentityMtx34,{1,0,0},rejected));
|
|
Check(!CalibrateGripOffset(Mtx34{}, {},rejected));
|
|
for(float scale: {0.2f,1.0f,2.5f}) {
|
|
for(Vec direction: {Vec{1,0,0},Vec{-1,0,0},Vec{0,0,-1},Vec{0,1,0}}) {
|
|
const auto rotation=Align({}, {1,0,0}, direction);
|
|
const auto wrist=WristForPalm(pivot,rotation,{1,0,0},0.055f*scale);
|
|
const auto palm=Add(wrist,Mul(direction,0.055f*scale));
|
|
Check(Near(palm.x,pivot.x) && Near(palm.y,pivot.y) && Near(palm.z,pivot.z));
|
|
}
|
|
}
|
|
const auto fit=MapSegment(pivot,{1,2,3},{1,0,0},{0,2,0});
|
|
const auto endpoint=detail::TransformPoint(fit,5,5,6);
|
|
Check(Near(endpoint.x,1) && Near(endpoint.y,4) && Near(endpoint.z,3));
|
|
Check(Near(Length(detail::TransformDirection(fit,{0,0,1})),1));
|
|
const auto thin=MapSegment(pivot,{1,2,3},{1,0,0},{0,2,0},0.65f);
|
|
const auto thin_endpoint=detail::TransformPoint(thin,5,5,6);
|
|
Check(Near(thin_endpoint.x,endpoint.x) && Near(thin_endpoint.y,endpoint.y) && Near(thin_endpoint.z,endpoint.z));
|
|
Check(Near(Length(detail::TransformDirection(thin,{0,0,1})),0.65f));
|
|
for (Vec to: {Vec{0,1,0},Vec{-1,0,0},Vec{1,0,0}}) {
|
|
const auto m=Align(pivot,{1,0,0},to);
|
|
const auto p=detail::TransformPoint(m,pivot.x,pivot.y,pivot.z);
|
|
const auto d=detail::TransformDirection(m,{1,0,0});
|
|
Check(Near(p.x,pivot.x) && Near(p.y,pivot.y) && Near(p.z,pivot.z));
|
|
Check(Near(d.x,to.x) && Near(d.y,to.y) && Near(d.z,to.z));
|
|
}
|
|
std::puts("body IK: reach, folded arms, lightning scale and joint rotation passed");
|
|
}
|