OpenScad - How to find Robotic Arm Nodes Coordinates after Rotations - 5 axis

Question

I found the coordinates of 2 nodes out of 3. I need to find the last coordinate, that of the aqua colored sphere. Here is my code. Can someone help me? Thanks

Enable Animation to view it in moviment - FPS:30 STEPS:300

Modules

module spalla(){    
    translate([0,-50,0])
    rotate([0,90,0]){
        rotate([-90,0,0]){
            cylinder(50,50,40,true,$fn=6);
            difference(){
                union(){
                    translate([0,-20,50])cube([50,10,50],true);
                    translate([0,20,50])cube([50,10,50],true);
                }
                   translate([0,0,50])rotate([90,0,0])cylinder(100,10,10,true);
            }
        }
        translate([0,50,0])rotate([0,0,A1ROT])braccio();
    }
}
module braccio(){
    translate([A1LEN/2,0,0])cube([A1LEN,30,30],true);    
    translate([A1LEN,0,0])rotate([360-A21ROT,0,0])rotate([0,0,A22ROT])avambraccio();
}
module avambraccio(){
   translate([A2LEN/2,0,0])color("red")cube([A2LEN,30,30],true); 
   translate([A2LEN,0,0])color("aqua")sphere(30);   
}

Function to rotate point

Note: depending of your Openscad coordinate system it may be necessary to permute the parameters of the function.

function rotate3d(pitch, roll, yaw,point) = let(

function rotate3d(roll, pitch, yaw,point) = let(

function rotate3d(pitch, roll, yaw,point) = let(

    cosa = cos(yaw),
    sina = sin(yaw),

    cosb = cos(pitch),
    sinb = sin(pitch),

    cosc = cos(roll),
    sinc = sin(roll),

    Axx = cosa*cosb,
    Axy = cosa*sinb*sinc - sina*cosc,
    Axz = cosa*sinb*cosc + sina*sinc,

    Ayx = sina*cosb,
    Ayy = sina*sinb*sinc + cosa*cosc,
    Ayz = sina*sinb*cosc - cosa*sinc,

    Azx = -sinb,
    Azy = cosb*sinc,
    Azz = cosb*cosc,
    
    px = point[0],
    py = point[1],
    pz = point[2],

    rx = Axx*px + Axy*py + Axz*pz,
    ry = Ayx*px + Ayy*py + Ayz*pz,
    rz = Azx*px + Azy*py + Azz*pz
    
)[rx,ry,rz];

Functions to get positions coordinates of the arm nodes

RSGetPos Get position of Right Shoulder

LSGetPos Get position of Left Shoulder

RA1GetPos Get position of Right Arm part 1 (green color)

LA1GetPos Get position of Left Arm part 1 (green color)

*** The features I miss ***

RA2GetPos Get position of Right Arm part 2 (red color) i need coords of aqua color sphere

LA2GetPos Get position of Left Arm part 2 (red color) i need coords of aqua color sphere

function RSGetPos() = [cos(SROTZ)*SWIDE/2,sin(SROTZ)*SWIDE/2,0];
function LSGetPos() = [cos(SROTZ+180)*SWIDE/2,sin(SROTZ+180)*SWIDE/2,0];

function RA1GetPos() = rotate3d( 0, SROT, SROTZ, rotate3d(-A1ROT,0,0,[0,0,-A1LEN])+[SWIDE/2,0,0]);
function LA1GetPos() = rotate3d( 0, SROT, SROTZ, rotate3d(A1ROT,0,0,[0,0,-A1LEN])+[-SWIDE/2,0,0]);

Calling functions to get coordinaets of nodes and draw the nodes

color("red",0.5)translate(LSGetPos()){
    sphere(50);
    translate([-100,0,0])rotate($vpr) text(str(LSGetPos()),50);
}

color("blue",0.5)translate(LA1GetPos()){
    sphere(50); 
    translate([-100,0,0])rotate($vpr) text(str(LA1GetPos()),50);
}


//color("red",0.5)translate(RSGetPos()){sphere(50);rotate($vpr)text(str(RSGetPos()),130);}
//color("blue",0.5)translate(RA1GetPos()){sphere(50);rotate($vpr)text(str(RA1GetPos()),130);}

Dimensions of the Arm parts

A1LEN=300; //The length of A1 green part of the arm. 
A2LEN=200; //The length of A2 red part of the arm. 
SWIDE=400; //Width of Shoulders 

5 input rotations - look at the Drawing i have posted as refer

SROTZ =  sin($t*360*2)*45;
SROT  =  sin($t*360*4)*45+45;
A1ROT =  sin($t*360*2)*45+45;
A21ROT = sin($t*360*2)*45+45;
A22ROT = sin($t*360*2)*45+45;

/*
SROTZ =0;
SROT  =0;
A1ROT =0;
A21ROT=0;
A22ROT=0;
*/

Calling the main module

rotate([0,0,SROTZ]){
    translate([SWIDE/2,0,0])rotate([0,0,-90])rotate([0,SROT,0])spalla();
    translate([-SWIDE/2,0,0])mirror([0,1,0])rotate([0,0,90])rotate([0,SROT,0])spalla();
}

Answer 1

This is my answers to my question, with some simplification.

function rotate3d(rot,point) = let(
    roll  = rot[0] ,
    pitch=  rot[1] , 
    yaw =  rot[2],
    
    cosa = cos(yaw),
    sina = sin(yaw),

    cosb = cos(pitch),
    sinb = sin(pitch),

    cosc = cos(roll),
    sinc = sin(roll),

    Axx = cosa*cosb,
    Axy = cosa*sinb*sinc - sina*cosc,
    Axz = cosa*sinb*cosc + sina*sinc,

    Ayx = sina*cosb,
    Ayy = sina*sinb*sinc + cosa*cosc,
    Ayz = sina*sinb*cosc - cosa*sinc,

    Azx = -sinb,
    Azy = cosb*sinc,
    Azz = cosb*cosc,
    
    px = point[0],
    py = point[1],
    pz = point[2],

    rx = Axx*px + Axy*py + Axz*pz,
    ry = Ayx*px + Ayy*py + Ayz*pz,
    rz = Azx*px + Azy*py + Azz*pz
    
)[rx,ry,rz];

module draw(p,r=10){
    translate(p) sphere(r);    
    color( "blue" ,1)translate(p) rotate($vpr)text(str(p),40);
}

len1=75;
len2=150;
len3=180;
/*
rot1=[0,-45,10];
rot2=[0,-90,0];
rot3=[0,145,0];
*/
rot1=[0,sin($t*360*2)*100,sin($t*360)*100];
rot2=[0,sin($t*360)*100,0];
rot3=[0,sin($t*360*2)*100,0];

size1=[len1,30,30];
size2=[len2,30,30];
size3=[len3,30,30];

module m1(){ translate([len1/2,0,0])color("red")cube(size1 ,true);translate([len1,0,0])rotate(rot2)m2();}
module m2(){ translate([len2/2,0,0])color("green")cube(size2,true);translate([len2,0,0])rotate(rot3)m3();}
module m3(){ translate([len3/2,0,0])color("pink")cube(size3,true);translate([len3,0,0]); }

init1=[len1,0,0];
init2=[len2,0,0];
init3=[len3,0,0];

rp1=rotate3d(rot1,init1);
rp2=rotate3d(rot1+rot2,init2)+rp1;
rp3=rotate3d(rot1+rot2+rot3,init3)+rp2;

tr=[100,0,0];
rx=[sin($t*360)*100,0,sin($t*360)*100];
xrp1=rotate3d(rx,rp1)+tr;
xrp2=rotate3d(rx,rp2)+tr;
xrp3=rotate3d(rx,rp3)+tr;

draw(xrp1,20);
draw(xrp2,20);
draw(xrp3,20);

translate(tr)rotate(rx)rotate(rot1)m1();