main.asy

Demonstrate how to define rigid-body motion using 3 translation values $(x, y, z)$ and 3 orientation values (roll, pitch, and yaw). The slide bar controls the interpolation of the motion: translation is interpolated linearly, while orientation follows an axis-angle motion interpretation implemented via Rodrigues' Rotation Formula (keeping the axis fixed).

View source (main.asy)
import graph3;


size(10cm);
defaultpen(fontsize(5));

currentprojection=perspective(
    camera=(21.6,-21.2,11.8),
    up=(-0.006,0.008,0.038),
    target=(3.5,3.4,2.9)
);

interaction constantsize = settings.autobillboard
    ? interaction(1,true)
    : interaction(0,true);


// ============================================================
// 1. Initial rigid-body position
// ============================================================

triple[] sp = {
    (2,3,3.1),
    (6,3,3.1),
    (6,7,3.1),
    (2,7,3.1)
};

triple T1c = (sp[0]+sp[1]+sp[2]+sp[3])/4;


// ============================================================
// 2. Static background: ground plane
// ============================================================

real groundMinX = -2;
real groundMaxX = 10;
real groundMinY = -1;
real groundMaxY = 11;
real groundZ = 0;

triple G0 = (groundMinX,groundMinY,groundZ);
triple G1 = (groundMaxX,groundMinY,groundZ);
triple G2 = (groundMaxX,groundMaxY,groundZ);
triple G3 = (groundMinX,groundMaxY,groundZ);

surface ground = surface(G0--G1--G2--G3--cycle);
draw(ground,lightgray+opacity(0.25));

real gridStep = 1.0;

// X-direction grid lines
for(real x=groundMinX; x<=groundMaxX+1e-6; x+=gridStep) {
    draw((x,groundMinY,groundZ+0.01)--(x,groundMaxY,groundZ+0.01),
         gray(0.75)+0.35);
}

// Y-direction grid lines
for(real y=groundMinY; y<=groundMaxY+1e-6; y+=gridStep) {
    draw((groundMinX,y,groundZ+0.01)--(groundMaxX,y,groundZ+0.01),
         gray(0.75)+0.35);
}


// ============================================================
// 3. Front-end algorithm and UI control logic
// ============================================================

javascript("
window.rigidParams={
    tau:0.0,
    tx:2.0,ty:0.5,tz:1.0,
    rx:5.0,ry:10.0,rz:0.0
};

window.T1_CENTER=[4.0,5.0,3.1];


// ------------------------------------------------------------
// 1. Standard RPY composite rotation matrix
// ------------------------------------------------------------

function getTargetRotationMatrix() {
    let rx=window.rigidParams.rx*Math.PI/180;
    let ry=window.rigidParams.ry*Math.PI/180;
    let rz=window.rigidParams.rz*Math.PI/180;

    let cphi=Math.cos(rx);
    let sphi=Math.sin(rx);
    let cthe=Math.cos(ry);
    let sthe=Math.sin(ry);
    let cpsi=Math.cos(rz);
    let spsi=Math.sin(rz);

    return [
        cpsi*cthe,
        spsi*cthe,
        -sthe,

        cpsi*sthe*sphi-spsi*cphi,
        spsi*sthe*sphi+cpsi*cphi,
        cthe*sphi,

        cpsi*sthe*cphi+spsi*sphi,
        spsi*sthe*cphi-cpsi*sphi,
        cthe*cphi
    ];
}


// ------------------------------------------------------------
// 2. Generate a 3x3 rotation matrix using Rodrigues' formula
// ------------------------------------------------------------

function getAxisAngleMatrix(k,Phi) {
    let c=Math.cos(Phi);
    let s=Math.sin(Phi);
    let v=1-c;

    let kx=k[0];
    let ky=k[1];
    let kz=k[2];

    return [
        kx*kx*v+c,
        kx*ky*v+kz*s,
        kx*kz*v-ky*s,

        kx*ky*v-kz*s,
        ky*ky*v+c,
        ky*kz*v+kx*s,

        kx*kz*v+ky*s,
        ky*kz*v-kx*s,
        kz*kz*v+c
    ];
}


// ------------------------------------------------------------
// 3. Core rigid-body transformation
// ------------------------------------------------------------

window.J_Rigid=function(p,t) {

    if(!p || p.length<3)
        return [0,0,0];

    let R_end=getTargetRotationMatrix();

    let trace=R_end[0]+R_end[4]+R_end[8];

    let Phi_total=Math.acos(
        Math.min(Math.max((trace-1)/2,-1),1)
    );

    let k_axis=[0,0,1];

    if(Math.abs(Phi_total)>1e-6) {
        let sinPhi2=2*Math.sin(Phi_total);

        k_axis=[
            (R_end[5]-R_end[7])/sinPhi2,
            (R_end[6]-R_end[2])/sinPhi2,
            (R_end[1]-R_end[3])/sinPhi2
        ];
    }

    let tau=window.rigidParams.tau;
    let cur_Phi=tau*Phi_total;
    let R_mat=getAxisAngleMatrix(k_axis,cur_Phi);

    // Rotate around the rigid-body center
    let px=p[0]-window.T1_CENTER[0];
    let py=p[1]-window.T1_CENTER[1];
    let pz=p[2]-window.T1_CENTER[2];

    let rx=R_mat[0]*px+R_mat[3]*py+R_mat[6]*pz;
    let ry=R_mat[1]*px+R_mat[4]*py+R_mat[7]*pz;
    let rz=R_mat[2]*px+R_mat[5]*py+R_mat[8]*pz;

    return [
        rx+tau*window.rigidParams.tx+window.T1_CENTER[0],
        ry+tau*window.rigidParams.ty+window.T1_CENTER[1],
        rz+tau*window.rigidParams.tz+window.T1_CENTER[2]
    ];
};


// ============================================================
// 4. User interface
// ============================================================

window.addEventListener('load',function() {

    let hide=document.createElement('style');

    hide.textContent=
        '.asy-player-controls,#asy-slider {'+
        'display:none !important;}';

    document.head.appendChild(hide);

    // Toggle button in the top-left corner (English)
    let toggleBtn=document.createElement('button');
    toggleBtn.innerHTML='⚙️ Panel';
    toggleBtn.style.cssText=
        'position:fixed;'+
        'top:15px;'+
        'left:15px;'+
        'z-index:10000;'+
        'background:rgba(255,255,255,0.95);'+
        'border:1px solid #ccc;'+
        'padding:5px 8px;'+
        'border-radius:6px;'+
        'font-family:sans-serif;'+
        'font-size:12px;'+
        'cursor:pointer;'+
        'box-shadow:0 2px 6px rgba(0,0,0,0.2);';
    document.body.appendChild(toggleBtn);

    let panel=document.createElement('div');

    panel.style.cssText=
        'position:fixed;'+
        'top:50px;'+
        'left:15px;'+
        'z-index:9999;'+
        'background:rgba(255,255,255,0.92);'+
        'padding:12px;'+
        'border-radius:8px;'+
        'font-family:sans-serif;'+
        'box-shadow:0 4px 12px rgba(0,0,0,0.25);'+
        'width:280px;'+
        'font-size:12px;';

    // Responsive logic: Collapse/hide by default if the window is small
    let isSmallScreen = (window.innerWidth < 600 || window.innerHeight < 500);
    panel.style.display = isSmallScreen ? 'none' : 'block';

    // Toggle panel visibility on button click
    toggleBtn.onclick=function() {
        if(panel.style.display==='none') {
            panel.style.display='block';
        } else {
            panel.style.display='none';
        }
    };

    panel.innerHTML=
        '<b style=\"display:block;'+
        'margin-bottom:8px;'+
        'border-bottom:1px solid #ccc;'+
        'padding-bottom:4px;'+
        'font-size:13px;\">'+
        '6-DoF Rigid-Body Transformation</b>'+

        '<div style=\"display:flex;'+
        'justify-content:space-between;\">'+
        '<label>Progress (tau):</label>'+
        '<span id=\"val-tau\">0.00</span>'+
        '</div>'+

        '<input type=\"range\" id=\"tauSlider\" '+
        'style=\"width:100%;margin:2px 0 8px 0;\" '+
        'min=\"0\" max=\"1\" step=\"0.01\" value=\"0\">'+

        '<div style=\"font-weight:bold;'+
        'margin-top:4px;color:#333;\">'+
        'Translation:</div>'+

        '<div style=\"display:grid;'+
        'grid-template-columns:1fr 1fr 1fr;'+
        'gap:4px;margin-top:2px;\">'+

        '<div>X: <input type=\"number\" '+
        'id=\"inp-tx\" value=\"2.0\" step=\"0.1\" '+
        'style=\"width:45px;\"></div>'+

        '<div>Y: <input type=\"number\" '+
        'id=\"inp-ty\" value=\"0.5\" step=\"0.1\" '+
        'style=\"width:45px;\"></div>'+

        '<div>Z: <input type=\"number\" '+
        'id=\"inp-tz\" value=\"1.0\" step=\"0.1\" '+
        'style=\"width:45px;\"></div>'+

        '</div>'+

        '<div style=\"font-weight:bold;'+
        'margin-top:8px;color:#333;\">'+
        'RPY Orientation Angles (degrees):</div>'+

        '<div style=\"display:grid;'+
        'grid-template-columns:1fr 1fr 1fr;'+
        'gap:4px;margin-top:2px;\">'+

        '<div>Rx: <input type=\"number\" '+
        'id=\"inp-rx\" value=\"5.0\" step=\"1\" '+
        'style=\"width:45px;\"></div>'+

        '<div>Ry: <input type=\"number\" '+
        'id=\"inp-ry\" value=\"10.0\" step=\"1\" '+
        'style=\"width:45px;\"></div>'+

        '<div>Rz: <input type=\"number\" '+
        'id=\"inp-rz\" value=\"0.0\" step=\"1\" '+
        'style=\"width:45px;\"></div>'+

        '</div>'+

        '<button id=\"playBtn\" '+
        'style=\"width:100%;margin-top:10px;'+
        'padding:5px;cursor:pointer;'+
        'font-weight:bold;\">'+
        'Play / Pause</button>';

    document.body.appendChild(panel);

    let slider=document.getElementById('tauSlider');
    let valSpan=document.getElementById('val-tau');
    let playBtn=document.getElementById('playBtn');
    let isPlaying=false;

    let refreshScene=function() {
        if(window.updateScene)
            window.updateScene();
        else if(window.asy && window.asy.render)
            window.asy.render();
    };

    let bindInput=function(id,key) {
        let el=document.getElementById(id);

        el.oninput=el.onchange=function() {
            window.rigidParams[key]=parseFloat(this.value)||0;
            refreshScene();
        };
    };

    bindInput('inp-tx','tx');
    bindInput('inp-ty','ty');
    bindInput('inp-tz','tz');
    bindInput('inp-rx','rx');
    bindInput('inp-ry','ry');
    bindInput('inp-rz','rz');

    slider.oninput=function() {
        window.rigidParams.tau=parseFloat(this.value);
        valSpan.innerText=parseFloat(this.value).toFixed(2);
        refreshScene();
    };

    playBtn.onclick=function() {
        isPlaying=!isPlaying;
        if(isPlaying)
            animate();
    };

    function animate() {
        if(!isPlaying)
            return;

        let val=parseFloat(slider.value)+0.01;

        if(val>1.0)
            val=0.0;

        slider.value=val;
        window.rigidParams.tau=val;
        valSpan.innerText=val.toFixed(2);

        refreshScene();
        requestAnimationFrame(animate);
    }
});
");


// ============================================================
// 5. Define the moving rigid body: rectangular cuboid
// ============================================================

real boxX=4.0;
real boxY=2.5;
real boxZ=1.2;

triple C=T1c;

triple[] box={
    // Bottom vertices
    C+(-boxX/2,-boxY/2,-boxZ/2),
    C+( boxX/2,-boxY/2,-boxZ/2),
    C+( boxX/2, boxY/2,-boxZ/2),
    C+(-boxX/2, boxY/2,-boxZ/2),

    // Top vertices
    C+(-boxX/2,-boxY/2, boxZ/2),
    C+( boxX/2,-boxY/2, boxZ/2),
    C+( boxX/2, boxY/2, boxZ/2),
    C+(-boxX/2, boxY/2, boxZ/2)
};


// ============================================================
// 6. Dynamic rigid body
// ============================================================

beginTransform("function(x,t){ return J_Rigid(x,t); }",10);

    // Bottom edges
    draw(box[0]--box[1],magenta+1.5);
    draw(box[1]--box[2],magenta+1.5);
    draw(box[2]--box[3],magenta+1.5);
    draw(box[3]--box[0],magenta+1.5);

    // Top edges
    draw(box[4]--box[5],magenta+1.5);
    draw(box[5]--box[6],magenta+1.5);
    draw(box[6]--box[7],magenta+1.5);
    draw(box[7]--box[4],magenta+1.5);

    // Vertical edges
    draw(box[0]--box[4],magenta+1.5);
    draw(box[1]--box[5],magenta+1.5);
    draw(box[2]--box[6],magenta+1.5);
    draw(box[3]--box[7],magenta+1.5);

    real axLen=1.5;

    triple O=T1c;
    triple X=T1c+(axLen,0,0);
    triple Y=T1c+(0,axLen,0);
    triple Z=T1c+(0,0,axLen);

    // Coordinate axes
    draw(O--X,red+1.2);
    draw(O--Y,green+1.2);
    draw(O--Z,blue+1.2);

    // Coordinate labels
    label("$O$",O,SW);
    label("$X$",X,E);
    label("$Y$",Y,N);
    label("$Z$",Z,N);

endTransform();


// ============================================================
// 7. Target pose
// ============================================================

beginTransform(
    "function(x,t){ "+
    "let sav=window.rigidParams.tau;"+
    "window.rigidParams.tau=1.0;"+
    "let res=J_Rigid(x,t);"+
    "window.rigidParams.tau=sav;"+
    "return res; }",
    10
);

    // Bottom edges
    draw(box[0]--box[1],dashed+orange+1.2);
    draw(box[1]--box[2],dashed+orange+1.2);
    draw(box[2]--box[3],dashed+orange+1.2);
    draw(box[3]--box[0],dashed+orange+1.2);

    // Top edges
    draw(box[4]--box[5],dashed+orange+1.2);
    draw(box[5]--box[6],dashed+orange+1.2);
    draw(box[6]--box[7],dashed+orange+1.2);
    draw(box[7]--box[4],dashed+orange+1.2);

    // Vertical edges
    draw(box[0]--box[4],dashed+orange+1.2);
    draw(box[1]--box[5],dashed+orange+1.2);
    draw(box[2]--box[6],dashed+orange+1.2);
    draw(box[3]--box[7],dashed+orange+1.2);

endTransform();

Source Files

Edit