00001 #include "SIGEL_Robot/SIG_GlueJoint.h" 00002 #include "SIGEL_Robot/IFunctions.h" 00003 00004 namespace SIGEL_Robot { 00005 SIG_GlueJoint::SIG_GlueJoint (SIG_Robot *par, QString n, int nr = -1) 00006 : SIG_Joint (par, n, nr) 00007 { } 00008 00009 SIG_GlueJoint::SIG_GlueJoint (SIG_Robot *par, QTextStream & tx) 00010 : SIG_Joint (par, tx) 00011 { 00012 ptA1 = SIG_Robot::streamToVector (tx); 00013 ptA2 = SIG_Robot::streamToVector (tx); 00014 ptA3 = SIG_Robot::streamToVector (tx); 00015 ptB1 = SIG_Robot::streamToVector (tx); 00016 ptB2 = SIG_Robot::streamToVector (tx); 00017 ptB3 = SIG_Robot::streamToVector (tx); 00018 } 00019 00020 SIG_GlueJoint::~SIG_GlueJoint (void) 00021 { } 00022 00023 SIG_Joint::JointType SIG_GlueJoint::getJointType (void) const 00024 { 00025 return tGlueJoint; 00026 } 00027 00028 void SIG_GlueJoint::setPlaneA (DL_vector p1, DL_vector p2, DL_vector p3) 00029 { 00030 ptA1 = p1; 00031 ptA2 = p2; 00032 ptA3 = p3; 00033 } 00034 00035 void SIG_GlueJoint::setPlaneB (DL_vector p1, DL_vector p2, DL_vector p3) 00036 { 00037 ptB1 = p1; 00038 ptB2 = p2; 00039 ptB3 = p3; 00040 } 00041 00042 void SIG_GlueJoint::getPlaneA (DL_vector &p1, DL_vector &p2, DL_vector &p3) 00043 { 00044 p1 = ptA1; 00045 p2 = ptA2; 00046 p3 = ptA3; 00047 } 00048 00049 void SIG_GlueJoint::getPlaneB (DL_vector &p1, DL_vector &p2, DL_vector &p3) 00050 { 00051 p1 = ptB1; 00052 p2 = ptB2; 00053 p3 = ptB3; 00054 } 00055 00056 void SIG_GlueJoint::transformPoints 00057 (SIG_Link *side, DL_vector mov, DL_matrix rot) 00058 { 00059 if (side == leftLink) { 00060 tfap (mov, rot, &ptA1); 00061 tfap (mov, rot, &ptA2); 00062 tfap (mov, rot, &ptA3); 00063 } else if (side == rightLink) { 00064 tfap (mov, rot, &ptB1); 00065 tfap (mov, rot, &ptB2); 00066 tfap (mov, rot, &ptB3); 00067 } 00068 } 00069 00070 void SIG_GlueJoint::getGeomRelation (DL_vector &t, DL_matrix &o, SIG_Link *origin) 00071 { 00072 if (leftLink == origin) { 00073 calculateAnyJoint 00074 (ptA1, ptA2, ptA3, 00075 ptB1, ptB2, ptB3, 00076 0.0, 0.0, 00077 o, t, 00078 name); 00079 } else { 00080 calculateAnyJoint 00081 (ptA1, ptA2, ptA3, 00082 ptB1, ptB2, ptB3, 00083 0.0, 0.0, 00084 o, t, 00085 name); 00086 } 00087 } 00088 00089 void SIG_GlueJoint::writeToFileTransfer (QTextStream & tx) 00090 { 00091 tx << "GlueJoint "; 00092 SIG_Joint::writeToFileTransfer (tx); 00093 SIG_Robot::vectorToStream (tx, ptA1); 00094 SIG_Robot::vectorToStream (tx, ptA2); 00095 SIG_Robot::vectorToStream (tx, ptA3); 00096 SIG_Robot::vectorToStream (tx, ptB1); 00097 SIG_Robot::vectorToStream (tx, ptB2); 00098 SIG_Robot::vectorToStream (tx, ptB3); 00099 tx << '\n'; 00100 } 00101 }