WeaponPoseMathProbe.cpp

#include "src/WeaponPoseMath.h"
#include <cstdio>
#include <cstdlib>
using namespace WeaponPoseMath;
void Require(bool condition,const char* message){if(!condition){std::printf("FAIL: %s\n",message);std::exit(1);}}
void Near(float a,float b,const char* label){Require(std::abs(a-b)<0.0001f,label);}
int main(){
    PackedCamera base{{0,1,0,0,0,1,1,0,0,10,20,30}};
    Transport::Pose anchor{{0,0,0,1},{1,2,3}},hand{{0,0,0,1},{1.2f,1.7f,2.5f}};
    auto aim=EyeCamera(base,anchor,hand,2);auto weapon=PistolFromAim(aim);
    Require(Rigid(weapon),"pistol basis must be right handed and rigid");
    for(unsigned j=0;j<3;++j){Near(weapon.v[4+j],-aim.v[6+j],"barrel follows controller forward");Near(weapon.v[8+j],aim.v[3+j],"pistol up follows controller up");}
    Near(weapon.v[12],9,"headset translation uses native camera basis exactly once");
    Near(weapon.v[13],20.4f,"hand position x");Near(weapon.v[14],29.4f,"hand position y");
    Matrix parent=Expand(base),geomLocal=Expand(PackedCamera{{1,0,0,0,1,0,0,0,1,0.04f,-0.03f,0.06f}});
    auto attacher=Compose(weapon,InvertRigid(geomLocal));auto local=Compose(InvertRigid(parent),attacher);
    auto reconstructed=Compose(Compose(parent,local),geomLocal);
    for(unsigned j=0;j<16;++j)Near(reconstructed.v[j],weapon.v[j],"attachment cancels parent and authored item offset");
    auto child=Expand(PackedCamera{{1,0,0,0,1,0,0,0,1,-.0017733f,.1478012f,.0508504f}});
    auto muzzle=Compose(weapon,child);
    Matrix firearm{};Require(FirearmFromAim(aim,child,firearm),"identity authored muzzle calibration");
    for(unsigned j=0;j<16;++j)Near(firearm.v[j],weapon.v[j],"accepted pistol transform preserved");
    // Arbitrary asset muzzle rotations/offsets: the actual muzzle, projectile
    // frame and target all use controller-forward, without changing the grip.
    for(unsigned i=0;i<64;++i){
        float angle=float(i)*.09f,pitch=float(i)*.07f,roll=float(i)*.05f;
        Matrix yawRotation=Expand(PackedCamera{{std::cos(angle),std::sin(angle),0,-std::sin(angle),std::cos(angle),0,0,0,1,0,0,0}});
        Matrix pitchRotation=Expand(PackedCamera{{1,0,0,0,std::cos(pitch),std::sin(pitch),0,-std::sin(pitch),std::cos(pitch),0,0,0}});
        Matrix rollRotation=Expand(PackedCamera{{std::cos(roll),0,-std::sin(roll),0,1,0,std::sin(roll),0,std::cos(roll),0,0,0}});
        Matrix authored=Compose(yawRotation,Compose(pitchRotation,rollRotation));authored.v[12]=.02f;authored.v[13]=.3f+.01f*i;authored.v[14]=.06f;
        Require(FirearmFromAim(aim,authored,firearm),"rotated authored muzzle frame");
        auto calibratedMuzzle=Compose(firearm,authored);
        for(unsigned j=0;j<3;++j){Near(calibratedMuzzle.v[4+j],-aim.v[6+j],"render/firing/laser barrel direction agrees");Near(calibratedMuzzle.v[8+j],aim.v[3+j],"muzzle up uses controller up");Near(firearm.v[12+j],aim.v[9+j],"weapon grip position retained");}
        auto calibratedLocal=Compose(InvertRigid(parent),Compose(firearm,InvertRigid(geomLocal)));
        auto actual=Compose(Compose(parent,calibratedLocal),geomLocal);
        for(unsigned j=0;j<16;++j)Near(actual.v[j],firearm.v[j],"arbitrary firearm attachment reconstruction");
    }
    auto badMuzzle=child;badMuzzle.v[0]=NAN;Require(!FirearmFromAim(aim,badMuzzle,firearm),"invalid muzzle calibration rejected");
    Near(muzzle.v[12],weapon.v[12]-.1478012f,"native muzzle is forward from grip in this basis");
    auto turned=hand;turned.orientation={0,std::sin(.6f),0,std::cos(.6f)};
    auto yawed=PistolFromAim(EyeCamera(base,anchor,turned,2));Require(Rigid(yawed),"controller yaw remains rigid");
    auto bad=weapon;bad.v[0]=NAN;Require(!Rigid(bad),"reject non finite transforms");
    bad=weapon;bad.v[8]=bad.v[4];bad.v[9]=bad.v[5];bad.v[10]=bad.v[6];Require(!Rigid(bad),"reject collapsed basis");
    auto invalid=hand;invalid.orientation={0,0,0,0};Require(!ValidPose(invalid),"reject lost controller pose");
    std::puts("Weapon pose math passed: controller basis, units, parent/item offsets, muzzle and invalid poses.");
}