 //				ssParticleCenterVsTime << realTime << ", " << p_rigid.y << ", " << p_rigid.z;
 //				ssParticleCenterVsDistance << p_rigidCumul.x << ", " << p_rigid.y << ", " << p_rigid.z;
 
+
+				//Calc angle between rigid Z and reference frame
+				real3 aD1 = AD1[j];
+				real3 aD2 = AD2[j];
+				real3 aD3 = AD3[j];
+				real3 axisZ = R3(aD1.z, aD2.z, aD3.z);
+				real_ thetaRigid_ZAxis_withX = AngleF3F3(axisZ, R3(1, 0, 0));
+				real_ thetaRigid_ZAxis_withY = AngleF3F3(axisZ, R3(0, 1, 0));
+				real_ thetaRigid_ZAxis_withZ = AngleF3F3(axisZ, R3(0, 0, 1));
+
 //				//***radial distance (tube)
 				real2 dist2 = R2(channelCenterYZ.x - p_rigid.y, channelCenterYZ.y - p_rigid.z);
 				printf("center %f %f and radius %f and py and pz %f %f\n", channelCenterYZ.x, channelCenterYZ.y, channelRadius, p_rigid.y, p_rigid.z);

using namespace std;


real_ AngleF3F3(real3 a, real3 b) {
	return acos(dot(a, b));
}
//*******************************************************************************************************************************


				int2 nodesPortioni2 = ANCF_ReferenceArrayNodesOnBeams[j];
				real3 p_tip = ANCF_NodesH[nodesPortioni2.y - 1];
				real3 v_tip = ANCF_NodesVelH[nodesPortioni2.y - 1];
				real3 s_tip = ANCF_SlopesH[nodesPortioni2.y - 1];

				real_ thetaX = AngleF3F3(s_tip, R3(1,0,0));
				real_ thetaY = AngleF3F3(s_tip, R3(0,1,0));
				real_ thetaZ = AngleF3F3(s_tip, R3(0,0,1));


				ssBeamTipVsTime << realTime << ", " <<
						p_tip.x << ", " << p_tip.y << ", " << p_tip.z << ", " <<
						v_tip.x << ", " << v_tip.y << ", " << v_tip.z << ", " <<
						thetaX << ", " << thetaY << ", " << thetaZ << ", " <<
						length(aveVel) << ", ";
			}
			ssBeamTipVsTime<<endl;
