AimOriginMathProbe.cpp

#include "AimOriginMath.h"
#include <cstdio>
#include <limits>
using namespace AimOriginMath;
bool Near(V a,V b){return std::abs(a.x-b.x)+std::abs(a.y-b.y)+std::abs(a.z-b.z)<.00001f;}
int main(){
    PackedCamera body{{1,0,0,0,1,0,0,0,1,10,20,30}};
    V stand{.03f,-.15f,1.67f},crouched{-.1f,-.3f,.78f},result{};
    Anchor a{};a.Seed(stand,crouched);
    V animated=WorldPoint(body,{-.2f,-.4f,1.5f});
    if(!a.Update(body,animated,0,true,1000,result)||!Near(result,WorldPoint(body,stand)))return 1;
    // Body motion and native crouch still move the corrected anchor while
    // the head/torso aiming animation cannot alter it.
    body.v[9]+=4;body.v[11]+=2;
    if(!a.Update(body,WorldPoint(body,{-.3f,-.4f,.7f}),1,true,1010,result)||!Near(result,WorldPoint(body,crouched)))return 2;
    V middle{(stand.x+crouched.x)*.5f,(stand.y+crouched.y)*.5f,(stand.z+crouched.z)*.5f};
    if(!a.Update(body,WorldPoint(body,{-.4f,-.5f,1}),.5f,true,1020,result)||!Near(result,WorldPoint(body,middle)))return 3;
    body.v[0]=0;body.v[1]=1;body.v[3]=-1;body.v[4]=0;
    if(!a.Update(body,WorldPoint(body,{-.2f,-.4f,1.5f}),0,true,1030,result)||!Near(result,WorldPoint(body,stand)))return 4;
    // Keep the neutral sample through release relaxation; resume the native
    // eye origin and calibrate only once its animation has settled.
    animated=WorldPoint(body,{0,-.2f,1.6f});
    if(!a.Update(body,animated,0,false,1200,result)||!Near(result,WorldPoint(body,stand)))return 5;
    V learned{0,-.14f,1.68f};animated=WorldPoint(body,learned);
    if(a.Update(body,animated,0,false,1700,result)||!Near(result,animated)||!Near(a.neutral[0],learned))return 6;
    if(!a.Update(body,WorldPoint(body,stand),0,true,1710,result)||!Near(result,animated))return 7;
    auto before=a.neutral[0];if(a.Update(body,animated,std::numeric_limits<float>::quiet_NaN(),true,1800,result)||!Near(a.neutral[0],before))return 8;
    Anchor unknown{};if(unknown.Update(body,animated,0,true,1900,result))return 9;
    Anchor locomotion{};locomotion.Seed(stand,crouched);
    for(unsigned i=0;i<20;++i){
        body.v[9]+=.03f;
        V walking=WorldPoint(body,{-.1f,-.3f,1.5f+.003f*i});
        if(!locomotion.Update(body,walking,0,false,2000+i*20,result,true)||!Near(result,WorldPoint(body,stand)))return 10;
    }
    if(!Near(locomotion.neutral[0],stand))return 11;
    if(!locomotion.Update(body,animated,.5f,true,2500,result,true)||!Near(result,WorldPoint(body,middle)))return 12;
    // Retail focused capture: the character turns while camera heading and
    // root height stay still. The old body-relative offset orbits by ~30cm.
    PackedCamera camera{{.315434f,-.948948f,0,.273823f,.091020f,.957464f,-.908583f,-.302017f,.288555f,0,0,0}};
    body={{1,0,0,0,1,0,0,0,1,-79,-190,49.94f}};
    if(!locomotion.Update(body,WorldPoint(body,stand),0,false,2600,result,true,&camera))return 13;
    V fixed=result;
    body.v[0]=-1;body.v[4]=-1;
    if(!locomotion.Update(body,WorldPoint(body,stand),0,true,2620,result,true,&camera)||!Near(result,fixed))return 14;
    body.v[9]+=2;body.v[11]+=1;
    if(!locomotion.Update(body,WorldPoint(body,crouched),1,true,2640,result,true,&camera))return 15;
    PackedCamera reference{};if(!ViewHeadingBody(body,camera,reference)||!Near(result,WorldPoint(reference,crouched)))return 16;
    // Pitch changes cannot rotate the horizontal anchor or change its height.
    camera.v[2]=.02f;
    if(!ViewHeadingBody(body,camera,reference)||std::abs(reference.v[2])+std::abs(reference.v[5])>0)return 17;
    // Actual rig eye-right vector is (-.062305,-.005938,.000615). Its -X
    // right/-Y forward convention must put the negative-Y neutral anchor
    // ahead of the body, including the larger crouch offset.
    PackedCamera forwardCamera{{1,0,0,0,0,1,0,-1,0,0,0,0}};
    body={{-1,0,0,0,-1,0,0,0,1,10,20,30}};
    Anchor retail{};retail.Seed({-.004980f,-.148527f,1.672373f},{-.127896f,-.304718f,.774248f});
    if(!retail.Update(body,WorldPoint(body,stand),0,false,2700,result,true,&forwardCamera)||
        !Near(result,{10.004980f,20.148527f,31.672373f}))return 18;
    if(!retail.Update(body,WorldPoint(body,crouched),1,true,2720,result,true,&forwardCamera)||
        !Near(result,{10.127896f,20.304718f,30.774248f}))return 19;
    if(!ViewHeadingBody(body,forwardCamera,reference)||
        !Near(WorldVector(reference,{-1,0,0}),{1,0,0})||
        !Near(WorldVector(reference,{0,-1,0}),{0,1,0}))return 20;
    puts("PASS: calibrated eye stays ahead of body in standing/crouch; rig axes, body-turn isolation, translation and aim-height stabilization preserved.");return 0;
}