Change of Basis Matrix example

I am trying to study a little about matrices but I am having a hard time figuring out what is wrong with my code. The purpose of this code is to setup some 3d transformation (position and orientation) objects and than get the position and orientation of a neighbour object from the point of view of the current object. In other words, It is just getting a transform coordinate from different origin, (including orientation).

Current code will run perfectly. Blinking point in space is how A looks at C, if A was fixed at 0,0,0. In the other hand when I set C to look at A, the code will fail at some angles of C... This has to be something about rotation ranges that I am not aware of how to solve...Maybe something not so hard but I don't have enough background to do it by myself.

[I am using peasy cam library just to visualize the results a little better but a any camera will do]

code is commented below

import peasy.*;
PeasyCam pcam;

Transform A, B, C = new Transform();
float c,d; //counters

void setup() {
  size(400, 400, P3D);
  rectMode(CENTER);
  pcam = new PeasyCam(this, 100);
  pcam.setDistance(200);
  pcam.rotateY(degrees(45));
  pcam.rotateX(degrees(-45));

  //initial Transformations tx, ty, tz, rx, ry, rz
  float RZ = 0;//PI; //will fail if PI
  A = new Transform(0, 0, 50, 0, 0, RZ);
  A.label = 'A';
  B = new Transform(0, 0, -50, 0, 0, 0);
  B.label = 'B';
  C = new Transform(0, 30, 0, 0, 0, 0);
  C.label = 'C';

  //testing outputs
  PMatrix3D Am = A.getTMatrix();
  Am.print();
  Transform At = mat2Transform(Am);
  At.print();
}

void draw() {
  background(100);
  c+=.2;
  d+=.01;
  pushMatrix();
  rotateX(HALF_PI);
  noFill();
  stroke(255);
  point(0,0,0);
  rect(0, 0, 100, 100);
  popMatrix();

  //testing some moviments
  A.ty = radians( sin(c)*10 );
  A.ry = radians( sin(d)*45 );
  C.rz = radians( c ); //fails to get reference

  A.draw();
  //B.draw();
  C.draw();
  A.showReference(C);// this is how A sees C if A was fixed at 0,0,0
  //C.showReference(A); //fails
}



Transform mat2Transform(PMatrix3D m) {
  float ax = m.m00;
  float ay = m.m10;
  float az = m.m20;
  float bz = m.m21;
  float cz = m.m22;
  float tx = m.m03;
  float ty = m.m13;
  float tz = m.m23;
  float rx = atan2(bz, cz);
  float ry = atan2(-az, sqrt( pow(bz, 2)+pow(cz, 2) ) );
  float rz = atan2(ay, ax) ;
  Transform t = new Transform(tx, ty, tz, rx, ry, rz);
  return t;
}



class Transform {

  float tx, ty, tz, rx, ry, rz;
  float axisRadius = 30;
  PMatrix3D tm; //transform matrix
  int cor;
  char label;

  Transform() {
    cor = color(random(0,255),random(0,255),random(0,255));
  }
  Transform(float tx, float ty, float tz, float rx, float ry, float rz) {
    this.tx = tx;
    this.ty = ty;
    this.tz = tz;
    this.rx = rx;
    this.ry = ry;
    this.rz = rz;
    cor = color(random(0,255),random(0,255),random(0,255));
  }

  PMatrix3D getTMatrix() {
    float Cx = cos(rx);
    float Cy = cos(ry);
    float Cz = cos(rz);
    float Sx = sin(rx);
    float Sy = sin(ry);
    float Sz = sin(rz);

    /*
    double[][] rxm = {  { 1,   0,   0 },
                        { 0,  Cx,  -Sx},
                        { 0,  Sx,   Cx}};

    double[][] rym = {  {  Cy , 0,  Sy},
                        {  0  , 1,  0 },
                        { -Sy , 0,  Cy}};

    double[][] rzm = {  { Cz, -Sz , 0},
                        { Sz,  Cz , 0},
                        { 0 ,  0  , 1}};
    */
    //http://planning.cs.uiuc.edu/node102.html
    tm = new PMatrix3D(  Cz*Cy  , Sz*Sy*Sx - Sz*Sx , Cz*Sy*Cz + Sz*Sx , tx ,
                          Sz*Cy  , Sz*Sy*Sx + Cz*Cx , Sz*Sy*Cx - Cz*Sx , ty ,
                          -Sy    ,       Cy*Sx      ,       Cy*Cx      , tz ,
                           0     ,          0       ,         0        ,  1);
    return tm;
  }


  void print() {
    println(tx, "  ", ty, "  ", tz, " | ", rx, "  ", ry, "  ", rz);
  }


  void showReference(Transform referenceTransform){

    PMatrix3D refMtx = referenceTransform.getTMatrix();
    refMtx.invert();//
    PMatrix3D thisMtx = this.getTMatrix();
    refMtx.apply(thisMtx);//multiplying reference T matrix with this T matrix
    refMtx.invert();//

    Transform output = new Transform();
    output = mat2Transform(refMtx);
    output.axisRadius = 10;
    output.draw();

  }


  void draw() {
    strokeWeight(6);
    stroke(cor);
    pushMatrix();
    translate(tx, ty, tz);
    point(0, 0, 0);
    rotateX( rx );
    rotateY( ry );
    rotateZ( rz );
    pushMatrix();
    strokeWeight(1);
    stroke(lerpColor(#00ff00,cor,.2)); //X
    line(0, 0, axisRadius, 0);
    stroke(lerpColor(#ff0000,cor,.2)); //Y
    rotateZ(HALF_PI);
    line(0, 0, axisRadius, 0);
    stroke(lerpColor(#0000ff,cor,.2)); //Z
    rotateY(HALF_PI);
    line(0, 0, axisRadius, 0);
    popMatrix();
    popMatrix();
    strokeWeight(1);

    //label
    pushMatrix();
    translate(tx, ty, tz);
    float[] r = pcam.getRotations();
    rotateX( r[0]);
    rotateY( r[1]);
    rotateZ( r[2]);
    textSize(9);
    fill(255);
    text(label, 0,-0);
    popMatrix();

  }
}