diff --git a/Examples/Tracker/TrackerClient.cxx b/Examples/Tracker/TrackerClient.cxx
index 87673057..f7b7a14f 100644
--- a/Examples/Tracker/TrackerClient.cxx
+++ b/Examples/Tracker/TrackerClient.cxx
@@ -73,7 +73,6 @@ int main(int argc, char* argv[])
     igtl::Matrix4x4 matrix;
     GetRandomTestMatrix(matrix);
     ts->GetTime();
-    transMsg->InitPack();
     transMsg->SetMatrix(matrix);
     transMsg->SetTimeStamp(ts);
     transMsg->Pack();
diff --git a/Examples/Tracker/TrackerClient2.cxx b/Examples/Tracker/TrackerClient2.cxx
index 82e4509e..5e81d9d4 100644
--- a/Examples/Tracker/TrackerClient2.cxx
+++ b/Examples/Tracker/TrackerClient2.cxx
@@ -71,7 +71,6 @@ int main(int argc, char* argv[])
     float quaternion[4];
 
     GetRandomTestVectors(position, quaternion);
-    positionMsg->InitPack();
     positionMsg->SetPosition(position);
     positionMsg->SetQuaternion(quaternion);
     positionMsg->Pack();
diff --git a/Examples/Tracker/TrackerClient3.cxx b/Examples/Tracker/TrackerClient3.cxx
index 38a7c198..8409618e 100644
--- a/Examples/Tracker/TrackerClient3.cxx
+++ b/Examples/Tracker/TrackerClient3.cxx
@@ -97,7 +97,6 @@ int main(int argc, char* argv[])
     {
     for (int i = 0; i < numdev; i ++)
       {
-      transMsg->InitPack();
       transMsg->SetDeviceName(devicename[i]);
       igtl::Matrix4x4 matrix;
       GetRandomTestMatrix(matrix, phi[i], theta[i]);
diff --git a/Examples/Tracker/TrackerServer.cxx b/Examples/Tracker/TrackerServer.cxx
index b43015a5..1fe3740d 100644
--- a/Examples/Tracker/TrackerServer.cxx
+++ b/Examples/Tracker/TrackerServer.cxx
@@ -68,7 +68,6 @@ int main(int argc, char* argv[])
         {
         igtl::Matrix4x4 matrix;
         GetRandomTestMatrix(matrix);
-        transMsg->InitPack();
         transMsg->SetDeviceName("Tracker");
         transMsg->SetMatrix(matrix);
         transMsg->Pack();
diff --git a/Source/igtlBindMessage.cxx b/Source/igtlBindMessage.cxx
index 40756a32..1476951a 100644
--- a/Source/igtlBindMessage.cxx
+++ b/Source/igtlBindMessage.cxx
@@ -42,6 +42,7 @@ void BindMessageBase::Init()
 
 int BindMessageBase::SetNumberOfChildMessages(unsigned int n)
 {
+  m_IsBodyPacked = false;
   this->m_ChildMessages.resize(n);
   return this->m_ChildMessages.size();
 }
@@ -57,6 +58,7 @@ int BindMessageBase::AppendChildMessage(igtl::MessageBase * child)
 {
   if (this->m_ChildMessages.size() < 0xFFFF)
     {
+    m_IsBodyPacked = false;
     ChildMessageInfo info;
 #if OpenIGTLink_HEADER_VERSION >= 2
     info.type = child->GetMessageType();
@@ -81,6 +83,7 @@ int BindMessageBase::SetChildMessage(unsigned int i, igtl::MessageBase * child)
 {
   if (i < this->m_ChildMessages.size())
     {
+    m_IsBodyPacked = false;
 #if OpenIGTLink_HEADER_VERSION >= 2
     this->m_ChildMessages[i].type = child->GetMessageType();
 #else
@@ -300,6 +303,7 @@ int GetBindMessage::AppendChildMessage(const char * type, const char * name)
   if (strlen(type) < IGTL_HEADER_TYPE_SIZE &&
       strlen(name) < IGTL_HEADER_NAME_SIZE)
     {
+    m_IsBodyPacked = false;
     BindMessageBase::ChildMessageInfo info;
     info.type = type;
     info.name = name;
@@ -408,6 +412,7 @@ StartBindMessage::~StartBindMessage()
 
 void StartBindMessage::SetResolution(igtlUint64 res)
 {
+  m_IsBodyPacked = false;
   this->m_Resolution = res; 
 }
 
@@ -534,4 +539,4 @@ int  RTSBindMessage::UnpackContent()
 
 
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlCapabilityMessage.cxx b/Source/igtlCapabilityMessage.cxx
index dee48da8..8696644e 100755
--- a/Source/igtlCapabilityMessage.cxx
+++ b/Source/igtlCapabilityMessage.cxx
@@ -52,6 +52,7 @@ CapabilityMessage::~CapabilityMessage()
 
 void CapabilityMessage::SetTypes(std::vector<std::string> types)
 {
+  m_IsBodyPacked = false;
   this->m_TypeNames.clear();
   this->m_TypeNames = types;
 }
@@ -61,6 +62,7 @@ int CapabilityMessage::SetType(int id, const char* type)
 {
   if (id < (int)this->m_TypeNames.size() && strlen(type) < IGTL_HEADER_TYPE_SIZE)
     {
+    m_IsBodyPacked = false;
     this->m_TypeNames[id] = type;
     return 1;
     }
@@ -152,4 +154,4 @@ int CapabilityMessage::UnpackContent()
 }
 
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlCommandMessage.cxx b/Source/igtlCommandMessage.cxx
index 0165a838..f8318dfb 100644
--- a/Source/igtlCommandMessage.cxx
+++ b/Source/igtlCommandMessage.cxx
@@ -64,6 +64,7 @@ CommandMessage::~CommandMessage()
   
 int CommandMessage::SetCommandId(igtlUint32 aId)
 {
+  m_IsBodyPacked = false;
   this->m_CommandId = aId;
   return 1;
 }
@@ -75,6 +76,7 @@ int CommandMessage::SetCommandName(const char* aCommandName)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   strcpy((char*)m_CommandName, aCommandName);
   return 1;
 }
@@ -92,6 +94,7 @@ int CommandMessage::SetCommandContent(const char* string)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   this->m_Command = string;
   return (int) this->m_Command.length();
 }
@@ -103,6 +106,7 @@ int CommandMessage::SetCommandContent(const std::string & string)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   this->m_Command = string;
   return (int) this->m_Command.length();
 }
@@ -114,6 +118,7 @@ int CommandMessage::SetContentEncoding(igtlUint16 enc)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   this->m_Encoding = enc;
   return 1;
 }
@@ -218,6 +223,7 @@ int RTSCommandMessage::SetCommandErrorString(const char* anErrorString)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   strcpy((char*)m_CommandName, anErrorString);
   return 1;
 }
@@ -229,6 +235,7 @@ int RTSCommandMessage::SetCommandErrorString(const std::string& anErrorString)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   strcpy((char*)m_CommandName, anErrorString.c_str());
   return 1;
 }
diff --git a/Source/igtlImageMessage.cxx b/Source/igtlImageMessage.cxx
index d8b1ebf2..b5b88799 100644
--- a/Source/igtlImageMessage.cxx
+++ b/Source/igtlImageMessage.cxx
@@ -67,6 +67,7 @@ ImageMessage::~ImageMessage()
 
 void ImageMessage::SetDimensions(int s[3])
 {
+  m_IsBodyPacked = false;
   dimensions[0] = s[0];
   dimensions[1] = s[1];
   dimensions[2] = s[2];
@@ -82,6 +83,7 @@ void ImageMessage::SetDimensions(int s[3])
 
 void ImageMessage::SetDimensions(int i, int j, int k)
 {
+  m_IsBodyPacked = false;
   dimensions[0] = i;
   dimensions[1] = j;
   dimensions[2] = k;
@@ -116,6 +118,7 @@ int ImageMessage::SetSubVolume(int dim[3], int off[3])
       off[1] + dim[1] <= dimensions[1] &&
       off[2] + dim[2] <= dimensions[2])
     {
+    m_IsBodyPacked = false;
     subDimensions[0] = dim[0];
     subDimensions[1] = dim[1];
     subDimensions[2] = dim[2];
@@ -137,6 +140,7 @@ int ImageMessage::SetSubVolume(int dimi, int dimj, int dimk, int offi, int offj,
       offj + dimj <= dimensions[1] &&
       offk + dimk <= dimensions[2])
     {
+    m_IsBodyPacked = false;
     subDimensions[0] = dimi;
     subDimensions[1] = dimj;
     subDimensions[2] = dimk;
@@ -175,6 +179,7 @@ void ImageMessage::GetSubVolume(int &dimi, int &dimj, int &dimk,
 
 void ImageMessage::SetSpacing(float s[3])
 {
+  m_IsBodyPacked = false;
   spacing[0] = s[0];
   spacing[1] = s[1];
   spacing[2] = s[2];
@@ -182,6 +187,7 @@ void ImageMessage::SetSpacing(float s[3])
 
 void ImageMessage::SetSpacing(float si, float sj, float sk)
 {
+  m_IsBodyPacked = false;
   spacing[0] = si;
   spacing[1] = sj;
   spacing[2] = sk;
@@ -203,6 +209,7 @@ void ImageMessage::GetSpacing(float &si, float &sj, float &sk)
   
 void ImageMessage::SetOrigin(float p[3])
 {
+  m_IsBodyPacked = false;
   matrix[0][3] = p[0];
   matrix[1][3] = p[1];
   matrix[2][3] = p[2];
@@ -210,6 +217,7 @@ void ImageMessage::SetOrigin(float p[3])
 
 void ImageMessage::SetOrigin(float px, float py, float pz)
 {
+  m_IsBodyPacked = false;
   matrix[0][3] = px;
   matrix[1][3] = py;
   matrix[2][3] = pz;
@@ -231,6 +239,7 @@ void ImageMessage::GetOrigin(float &px, float &py, float &pz)
 
 void ImageMessage::SetNormals(float o[3][3])
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = o[0][0];
   matrix[0][1] = o[0][1];
   matrix[0][2] = o[0][2];
@@ -244,6 +253,7 @@ void ImageMessage::SetNormals(float o[3][3])
 
 void ImageMessage::SetNormals(float t[3], float s[3], float n[3])
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = t[0];
   matrix[1][0] = t[1];
   matrix[2][0] = t[2];
@@ -283,6 +293,7 @@ void ImageMessage::GetNormals(float t[3], float s[3], float n[3])
 
 void ImageMessage::SetMatrix(Matrix4x4& mat)
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = mat[0][0];
   matrix[1][0] = mat[1][0];
   matrix[2][0] = mat[2][0];
diff --git a/Source/igtlImageMessage2.cxx b/Source/igtlImageMessage2.cxx
index d5292885..bf447224 100644
--- a/Source/igtlImageMessage2.cxx
+++ b/Source/igtlImageMessage2.cxx
@@ -101,6 +101,7 @@ ImageMessage2::~ImageMessage2()
 
 void ImageMessage2::SetDimensions(int s[3])
 {
+  m_IsBodyPacked = false;
   dimensions[0] = s[0];
   dimensions[1] = s[1];
   dimensions[2] = s[2];
@@ -116,6 +117,7 @@ void ImageMessage2::SetDimensions(int s[3])
 
 void ImageMessage2::SetDimensions(int i, int j, int k)
 {
+  m_IsBodyPacked = false;
   dimensions[0] = i;
   dimensions[1] = j;
   dimensions[2] = k;
@@ -150,6 +152,7 @@ int ImageMessage2::SetSubVolume(int dim[3], int off[3])
       off[1] + dim[1] <= dimensions[1] &&
       off[2] + dim[2] <= dimensions[2])
     {
+    m_IsBodyPacked = false;
     subDimensions[0] = dim[0];
     subDimensions[1] = dim[1];
     subDimensions[2] = dim[2];
@@ -171,6 +174,7 @@ int ImageMessage2::SetSubVolume(int dimi, int dimj, int dimk, int offi, int offj
       offj + dimj <= dimensions[1] &&
       offk + dimk <= dimensions[2])
     {
+    m_IsBodyPacked = false;
     subDimensions[0] = dimi;
     subDimensions[1] = dimj;
     subDimensions[2] = dimk;
@@ -209,6 +213,7 @@ void ImageMessage2::GetSubVolume(int &dimi, int &dimj, int &dimk,
 
 void ImageMessage2::SetSpacing(float s[3])
 {
+  m_IsBodyPacked = false;
   spacing[0] = s[0];
   spacing[1] = s[1];
   spacing[2] = s[2];
@@ -216,6 +221,7 @@ void ImageMessage2::SetSpacing(float s[3])
 
 void ImageMessage2::SetSpacing(float si, float sj, float sk)
 {
+  m_IsBodyPacked = false;
   spacing[0] = si;
   spacing[1] = sj;
   spacing[2] = sk;
@@ -237,6 +243,7 @@ void ImageMessage2::GetSpacing(float &si, float &sj, float &sk)
   
 void ImageMessage2::SetOrigin(float p[3])
 {
+  m_IsBodyPacked = false;
   matrix[0][3] = p[0];
   matrix[1][3] = p[1];
   matrix[2][3] = p[2];
@@ -244,6 +251,7 @@ void ImageMessage2::SetOrigin(float p[3])
 
 void ImageMessage2::SetOrigin(float px, float py, float pz)
 {
+  m_IsBodyPacked = false;
   matrix[0][3] = px;
   matrix[1][3] = py;
   matrix[2][3] = pz;
@@ -265,6 +273,7 @@ void ImageMessage2::GetOrigin(float &px, float &py, float &pz)
 
 void ImageMessage2::SetNormals(float o[3][3])
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = o[0][0];
   matrix[0][1] = o[0][1];
   matrix[0][2] = o[0][2];
@@ -278,6 +287,7 @@ void ImageMessage2::SetNormals(float o[3][3])
 
 void ImageMessage2::SetNormals(float t[3], float s[3], float n[3])
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = t[0];
   matrix[1][0] = t[1];
   matrix[2][0] = t[2];
@@ -317,6 +327,7 @@ void ImageMessage2::GetNormals(float t[3], float s[3], float n[3])
 
 void ImageMessage2::SetMatrix(Matrix4x4& mat)
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = mat[0][0];
   matrix[1][0] = mat[1][0];
   matrix[2][0] = mat[2][0];
diff --git a/Source/igtlImageMetaMessage.cxx b/Source/igtlImageMetaMessage.cxx
index 2fccf5eb..91a78542 100644
--- a/Source/igtlImageMetaMessage.cxx
+++ b/Source/igtlImageMetaMessage.cxx
@@ -203,6 +203,7 @@ ImageMetaMessage::~ImageMetaMessage()
 
 int ImageMetaMessage::AddImageMetaElement(ImageMetaElement::Pointer& elem)
 {
+  m_IsBodyPacked = false;
   this->m_ImageMetaList.push_back(elem);
   return this->m_ImageMetaList.size();
 }
@@ -330,4 +331,4 @@ int ImageMetaMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlLabelMetaMessage.cxx b/Source/igtlLabelMetaMessage.cxx
index 747e26d7..9ea30c10 100644
--- a/Source/igtlLabelMetaMessage.cxx
+++ b/Source/igtlLabelMetaMessage.cxx
@@ -181,6 +181,7 @@ LabelMetaMessage::~LabelMetaMessage()
 
 int LabelMetaMessage::AddLabelMetaElement(LabelMetaElement::Pointer& elem)
 {
+  m_IsBodyPacked = false;
   this->m_LabelMetaList.push_back(elem);
   return this->m_LabelMetaList.size();
 }
@@ -295,4 +296,4 @@ int LabelMetaMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlNDArrayMessage.cxx b/Source/igtlNDArrayMessage.cxx
index 9709c773..2dc57b07 100644
--- a/Source/igtlNDArrayMessage.cxx
+++ b/Source/igtlNDArrayMessage.cxx
@@ -154,6 +154,7 @@ NDArrayMessage::~NDArrayMessage()
 
 int NDArrayMessage::SetArray(int type, ArrayBase * a)
 {
+  m_IsBodyPacked = false;
   // Check if type is valid
   if (type < 2 || type > 13 ||
       type == 8 || type == 9 || type == 12)
@@ -287,4 +288,4 @@ int NDArrayMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlPointMessage.cxx b/Source/igtlPointMessage.cxx
index 1dbc9dc8..0fc7c319 100644
--- a/Source/igtlPointMessage.cxx
+++ b/Source/igtlPointMessage.cxx
@@ -176,6 +176,7 @@ PointMessage::~PointMessage()
 
 int PointMessage::AddPointElement(PointElement::Pointer& elem)
 {
+  m_IsBodyPacked = false;
   this->m_PointList.push_back(elem);
   return this->m_PointList.size();
 }
@@ -296,4 +297,4 @@ int PointMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlPolyDataMessage.cxx b/Source/igtlPolyDataMessage.cxx
index ddf48854..15f59f3e 100644
--- a/Source/igtlPolyDataMessage.cxx
+++ b/Source/igtlPolyDataMessage.cxx
@@ -953,6 +953,7 @@ void PolyDataMessage::ClearAttributes()
 
 void PolyDataMessage::AddAttribute(PolyDataAttribute * att)
 {
+  m_IsBodyPacked = false;
   this->m_Attributes.push_back(att);
 }
 
@@ -1009,6 +1010,7 @@ bool RTSPolyDataMessage::GetStatus() const
 
 void RTSPolyDataMessage::SetStatus(bool status)
 {
+  m_IsBodyPacked = false;
   m_Status = status ? 1 : 0;
 }
 
@@ -1049,4 +1051,4 @@ int RTSPolyDataMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlPositionMessage.cxx b/Source/igtlPositionMessage.cxx
index 28af8874..00b98299 100644
--- a/Source/igtlPositionMessage.cxx
+++ b/Source/igtlPositionMessage.cxx
@@ -57,6 +57,7 @@ void PositionMessage::SetPackType(int t)
 
 int PositionMessage::SetPackTypeByContentSize(int s)
 {
+  m_IsBodyPacked = false;
   if (s == IGTL_POSITION_MESSAGE_POSITON_ONLY_SIZE)
     {
     this->m_PackType = POSITION_ONLY;
@@ -81,6 +82,7 @@ int PositionMessage::SetPackTypeByContentSize(int s)
 
 void PositionMessage::SetPosition(const float* pos)
 {
+  m_IsBodyPacked = false;
   this->m_Position[0] = pos[0];
   this->m_Position[1] = pos[1];
   this->m_Position[2] = pos[2];
@@ -88,6 +90,7 @@ void PositionMessage::SetPosition(const float* pos)
 
 void PositionMessage::SetPosition(float x, float y, float z)
 {
+  m_IsBodyPacked = false;
   this->m_Position[0] = x;
   this->m_Position[1] = y;
   this->m_Position[2] = z;
@@ -96,6 +99,7 @@ void PositionMessage::SetPosition(float x, float y, float z)
 
 void PositionMessage::SetQuaternion(const float* quat)
 {
+  m_IsBodyPacked = false;
   this->m_Quaternion[0] = quat[0];
   this->m_Quaternion[1] = quat[1];
   this->m_Quaternion[2] = quat[2];
@@ -104,6 +108,7 @@ void PositionMessage::SetQuaternion(const float* quat)
 
 void PositionMessage::SetQuaternion(float ox, float oy, float oz, float w)
 {
+  m_IsBodyPacked = false;
   this->m_Quaternion[0] = ox;
   this->m_Quaternion[1] = oy;
   this->m_Quaternion[2] = oz;
@@ -280,4 +285,4 @@ int PositionMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlStatusMessage.cxx b/Source/igtlStatusMessage.cxx
index b165ca0d..d96760ea 100644
--- a/Source/igtlStatusMessage.cxx
+++ b/Source/igtlStatusMessage.cxx
@@ -42,7 +42,7 @@ StatusMessage::~StatusMessage()
 
 void StatusMessage::SetCode(int code)
 {
-
+  m_IsBodyPacked = false;
   if (code >= 0 && code < STATUS_NUM_TYPES)
     {
     this->m_Code = code;
@@ -60,6 +60,7 @@ int StatusMessage::GetCode()
 
 void StatusMessage::SetSubCode(igtlInt64 subcode)
 {
+  m_IsBodyPacked = false;
   this->m_SubCode = subcode;
 }
 
@@ -70,6 +71,7 @@ igtlInt64 StatusMessage::GetSubCode()
 
 void StatusMessage::SetErrorName(const char* name)
 {
+  m_IsBodyPacked = false;
   this->m_ErrorName[IGTL_STATUS_ERROR_NAME_LENGTH-1] = '\0';
   strncpy(this->m_ErrorName, name, IGTL_STATUS_ERROR_NAME_LENGTH);
 }
@@ -81,6 +83,7 @@ const char* StatusMessage::GetErrorName()
 
 void StatusMessage::SetStatusString(const char* str)
 {
+  m_IsBodyPacked = false;
   this->m_StatusMessageString = str;
 }
 
@@ -142,4 +145,4 @@ int StatusMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlStringMessage.cxx b/Source/igtlStringMessage.cxx
index e9817e94..d70107ef 100644
--- a/Source/igtlStringMessage.cxx
+++ b/Source/igtlStringMessage.cxx
@@ -42,6 +42,7 @@ int StringMessage::SetString(const char* string)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   this->m_String = string;
   return (int) this->m_String.length();
 }
@@ -53,6 +54,7 @@ int StringMessage::SetString(const std::string & string)
     {
     return 0;
     }
+  m_IsBodyPacked = false;
   this->m_String = string;
   return (int) this->m_String.length();
 }
@@ -62,6 +64,7 @@ int StringMessage::SetEncoding(igtlUint16 enc)
 {
   // TODO: the argument should be validated before it is substituted
   this->m_Encoding = enc;
+  m_IsBodyPacked = false;
   return 1;
 }
 
@@ -134,4 +137,4 @@ int StringMessage::UnpackContent()
   return 1;
 }
 
-} // namespace igtl
\ No newline at end of file
+} // namespace igtl
diff --git a/Source/igtlTrackingDataMessage.cxx b/Source/igtlTrackingDataMessage.cxx
index cfeb71c1..20a245b8 100644
--- a/Source/igtlTrackingDataMessage.cxx
+++ b/Source/igtlTrackingDataMessage.cxx
@@ -273,6 +273,7 @@ TrackingDataMessage::~TrackingDataMessage()
   
 int TrackingDataMessage::AddTrackingDataElement(TrackingDataElement::Pointer& elem)
 {
+  m_IsBodyPacked = false;
   this->m_TrackingDataList.push_back(elem);
   return this->m_TrackingDataList.size();
 }
diff --git a/Source/igtlTrajectoryMessage.cxx b/Source/igtlTrajectoryMessage.cxx
index ddca9966..aacdf06d 100644
--- a/Source/igtlTrajectoryMessage.cxx
+++ b/Source/igtlTrajectoryMessage.cxx
@@ -228,6 +228,7 @@ TrajectoryMessage::~TrajectoryMessage()
 
 int TrajectoryMessage::AddTrajectoryElement(TrajectoryElement::Pointer& elem)
 {
+  m_IsBodyPacked = false;
   this->m_TrajectoryList.push_back(elem);
   return this->m_TrajectoryList.size();
 }
diff --git a/Source/igtlTransformMessage.cxx b/Source/igtlTransformMessage.cxx
index 57a3ad7c..0d13cf29 100644
--- a/Source/igtlTransformMessage.cxx
+++ b/Source/igtlTransformMessage.cxx
@@ -57,6 +57,7 @@ TransformMessage::~TransformMessage()
   
 void TransformMessage::SetPosition(float p[3])
 {
+  m_IsBodyPacked = false;
   matrix[0][3] = p[0];
   matrix[1][3] = p[1];
   matrix[2][3] = p[2];
@@ -72,6 +73,7 @@ void TransformMessage::GetPosition(float p[3])
   
 void TransformMessage::SetPosition(float px, float py, float pz)
 {
+  m_IsBodyPacked = false;
   matrix[0][3] = px;
   matrix[1][3] = py;
   matrix[2][3] = pz;
@@ -86,6 +88,7 @@ void TransformMessage::GetPosition(float* px, float* py, float* pz)
   
 void TransformMessage::SetNormals(float o[3][3])
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = o[0][0];
   matrix[0][1] = o[0][1];
   matrix[0][2] = o[0][2];
@@ -112,6 +115,7 @@ void TransformMessage::GetNormals(float o[3][3])
   
 void TransformMessage::SetNormals(float t[3], float s[3], float n[3])
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = t[0];
   matrix[1][0] = t[1];
   matrix[2][0] = t[2];
@@ -138,6 +142,7 @@ void TransformMessage::GetNormals(float t[3], float s[3], float n[3])
   
 void TransformMessage::SetMatrix(Matrix4x4& mat)
 {
+  m_IsBodyPacked = false;
   matrix[0][0] = mat[0][0];
   matrix[1][0] = mat[1][0];
   matrix[2][0] = mat[2][0];
diff --git a/Testing/igtlCommandMessageTest.cxx b/Testing/igtlCommandMessageTest.cxx
index ed447f13..5669cec1 100644
--- a/Testing/igtlCommandMessageTest.cxx
+++ b/Testing/igtlCommandMessageTest.cxx
@@ -68,9 +68,37 @@ TEST(CommandMessageTest, UnPackFormatVersion1)
   }
   EXPECT_EQ(messageHeader->crc, crc);
   EXPECT_STREQ(receiveCommandMsg->GetCommandContent().c_str(), "Start the tracking machine");
+  EXPECT_EQ(receiveCommandMsg->GetContentEncoding(), 3);
+  
+  // Test start
+  // reuse the command message.
+  sendCommandMsg->SetContentEncoding(4);
+  sendCommandMsg->Pack();
+  headerMsg->InitBuffer();
+  memcpy(headerMsg->GetBufferPointer(), (const void*)sendCommandMsg->GetBufferPointer(), IGTL_HEADER_SIZE);
+  headerMsg->Unpack();
+  receiveCommandMsg->SetMessageHeader(headerMsg);
+  receiveCommandMsg->AllocatePack();
+  memcpy(receiveCommandMsg->GetBufferBodyPointer(), sendCommandMsg->GetBufferBodyPointer(), headerMsg->GetBodySizeToRead());
+  receiveCommandMsg->Unpack();
+  //header should still be the same, though crc should be different
+  messageHeader = (igtl_header *)receiveCommandMsg->GetBufferPointer();
+  EXPECT_STREQ(messageHeader->device_name, "");
+  EXPECT_STREQ(messageHeader->name, "COMMAND");
+  EXPECT_EQ(messageHeader->header_version, 1);
+  EXPECT_EQ(messageHeader->timestamp, 0);
+  EXPECT_EQ(messageHeader->body_size, sizeof(test_command_message)-IGTL_HEADER_SIZE);
+  if(igtl_is_little_endian())
+  {
+    crc = BYTE_SWAP_INT64(crc);
+  }
+  EXPECT_NE(messageHeader->crc, crc); // should not be equal as the content changed
+  EXPECT_EQ(receiveCommandMsg->GetContentEncoding(), 4);
+
 }
 
 
+
 #if OpenIGTLink_PROTOCOL_VERSION >= 3
 #include "igtlutil/igtl_test_data_commandFormat2.h"
 #include "igtlMessageFormat2TestMacro.h"
@@ -132,6 +160,33 @@ TEST(CommandMessageTest, UnpackFormatVersion2)
   EXPECT_EQ(messageHeader->crc, crc);
   EXPECT_STREQ(receiveCommandMsg->GetCommandContent().c_str(), "Start the tracking machine");
   igtlMetaDataComparisonMacro(receiveCommandMsg);
+  
+  // Test start
+  // reuse the command message.
+  sendCommandMsg->SetContentEncoding(4);
+  sendCommandMsg->Pack();
+  headerMsg->InitBuffer();
+  memcpy(headerMsg->GetBufferPointer(), (const void*)sendCommandMsg->GetBufferPointer(), IGTL_HEADER_SIZE);
+  headerMsg->Unpack();
+  receiveCommandMsg->SetMessageHeader(headerMsg);
+  receiveCommandMsg->AllocatePack();
+  memcpy(receiveCommandMsg->GetBufferBodyPointer(), sendCommandMsg->GetBufferBodyPointer(), headerMsg->GetBodySizeToRead());
+  receiveCommandMsg->Unpack();
+  //header should still be the same, though crc should be different
+  messageHeader = (igtl_header *)receiveCommandMsg->GetBufferPointer();
+  EXPECT_STREQ(messageHeader->device_name, "OpticalTracker");
+  EXPECT_STREQ(messageHeader->name, "COMMAND");
+  EXPECT_EQ(messageHeader->header_version, 2);
+  EXPECT_EQ(messageHeader->timestamp, 0);
+  EXPECT_EQ(messageHeader->body_size, sizeof(test_command_messageFormat2)-IGTL_HEADER_SIZE);
+  if(igtl_is_little_endian())
+  {
+    crc = BYTE_SWAP_INT64(crc);
+  }
+  EXPECT_NE(messageHeader->crc, crc); // should not be equal as the content changed
+  EXPECT_EQ(receiveCommandMsg->GetContentEncoding(), 4);
+  EXPECT_STREQ(receiveCommandMsg->GetCommandContent().c_str(), "Start the tracking machine");
+  igtlMetaDataComparisonMacro(receiveCommandMsg);
 }
 
 #endif
diff --git a/Testing/igtlImageMessageTest.cxx b/Testing/igtlImageMessageTest.cxx
index e7947c18..c0878afe 100644
--- a/Testing/igtlImageMessageTest.cxx
+++ b/Testing/igtlImageMessageTest.cxx
@@ -109,6 +109,50 @@ TEST(ImageMessageTest, Unpack)
   //The imageHeader is byte-wized converted, so we skip the comparison of the image header.
   int r = memcmp((const char*)imageReceiveMsg->GetPackBodyPointer()+IGTL_IMAGE_HEADER_SIZE, (const void*)(test_image_message+IGTL_HEADER_SIZE+IGTL_IMAGE_HEADER_SIZE), (size_t)(TEST_IMAGE_MESSAGE_SIZE));
   EXPECT_EQ(r, 0);
+    
+    
+  // Reuse message test
+  igtl::Matrix4x4 inMatrix2 = {{inT[1],inS[0],inN[2],inOrigin[0]},
+    {inT[0],inS[2],inN[0],inOrigin[1]},
+    {inT[2],inS[1],inN[1],inOrigin[2]},
+    {inT[3],inS[3],inN[3],inOrigin[3]}};
+  imageSendMsg->SetMatrix(inMatrix2);
+  imageSendMsg->Pack();
+  headerMsg->InitPack();
+  memcpy(headerMsg->GetPackPointer(), (const void*)imageSendMsg->GetPackPointer(), IGTL_HEADER_SIZE);
+  headerMsg->Unpack();
+  imageReceiveMsg->SetMessageHeader(headerMsg);
+  imageReceiveMsg->AllocatePack();
+  memcpy(imageReceiveMsg->GetPackBodyPointer(), imageSendMsg->GetPackBodyPointer(), IGTL_IMAGE_HEADER_SIZE+TEST_IMAGE_MESSAGE_SIZE);
+  imageReceiveMsg->Unpack();
+  
+  messageHeader = (igtl_header *)imageReceiveMsg->GetPackPointer();
+  EXPECT_STREQ(messageHeader->device_name, "DeviceName");
+  EXPECT_STREQ(messageHeader->name, "IMAGE");
+  EXPECT_EQ(messageHeader->header_version, 1);
+  EXPECT_EQ(messageHeader->timestamp, 1234567892);
+  EXPECT_EQ(messageHeader->body_size, IGTL_IMAGE_HEADER_SIZE+TEST_IMAGE_MESSAGE_SIZE);
+  
+  imageReceiveMsg->GetDimensions(returnSize);
+  EXPECT_THAT(returnSize,testing::ElementsAreArray(size));
+  imageReceiveMsg->GetSpacing(returnSpacing);
+  EXPECT_TRUE(ArrayFloatComparison(returnSpacing, spacing, 3, ABS_ERROR));
+  imageReceiveMsg->GetSubVolume(returnSvsize, returnSvoffset);
+  EXPECT_THAT(returnSvsize,testing::ElementsAreArray(svsize));
+  EXPECT_THAT(returnSvoffset,testing::ElementsAreArray(svoffset));
+  EXPECT_EQ(imageReceiveMsg->GetScalarType(), IGTL_IMAGE_STYPE_TYPE_UINT8);
+  EXPECT_EQ(imageReceiveMsg->GetEndian(), IGTL_IMAGE_ENDIAN_LITTLE);
+  EXPECT_EQ(imageReceiveMsg->GetCoordinateSystem(), IGTL_IMAGE_COORD_RAS);
+  
+  igtl::Matrix4x4 outMatrix2 = {{0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0}};
+  imageReceiveMsg->GetMatrix(outMatrix2);
+  EXPECT_TRUE(MatrixComparison(outMatrix2, inMatrix2, ABS_ERROR));
+  //The imageHeader is byte-wized converted, so we skip the comparison of the image header.
+  r = memcmp((const char*)imageReceiveMsg->GetPackBodyPointer()+IGTL_IMAGE_HEADER_SIZE, (const void*)(test_image_message+IGTL_HEADER_SIZE+IGTL_IMAGE_HEADER_SIZE), (size_t)(TEST_IMAGE_MESSAGE_SIZE));
+  EXPECT_EQ(r, 0);
 }
 
 #if OpenIGTLink_PROTOCOL_VERSION >= 3
@@ -159,6 +203,44 @@ TEST(ImageMessageTest, MetaDataGetsPackedAndRestoredFormatVersion2)
   for (igtlUint16 i = 0; i < ext_x*ext_y*ext_z; ++i, ++data) {
     EXPECT_EQ(42, *data); // even leads to access violations!
   }
+  
+  
+  // reuse message test
+  msg->SetMatrix(inMatrix);
+  msg->Pack();
+  header->InitPack();
+  memcpy(header->GetPackPointer(), msg->GetPackPointer(), header->GetPackSize());
+  header->Unpack();
+  
+  EXPECT_EQ(IGTL_HEADER_VERSION_2, header->GetHeaderVersion());
+  
+  target = igtl::ImageMessage::New();
+  target->SetMessageHeader(header);
+  target->AllocatePack();
+  
+  EXPECT_EQ(target->GetPackBodySize(), msg->GetPackBodySize());
+  
+  memcpy(target->GetPackBodyPointer(), msg->GetPackBodyPointer(), target->GetPackBodySize());
+  target->Unpack();
+  
+  // Message comparison
+  EXPECT_EQ(IGTL_HEADER_VERSION_2, target->GetHeaderVersion());
+  
+  target->GetDimensions(ext_x, ext_y, ext_z);
+  EXPECT_EQ(2, ext_x); // fails!
+  EXPECT_EQ(3, ext_y); // fails!
+  EXPECT_EQ(4, ext_z); // fails!
+  
+  data = static_cast<igtlUint16*>(target->GetScalarPointer());
+  for (igtlUint16 i = 0; i < ext_x*ext_y*ext_z; ++i, ++data) {
+    EXPECT_EQ(42, *data); // even leads to access violations!
+  }
+  igtl::Matrix4x4 outMatrix = {{0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0}};
+  msg->GetMatrix(outMatrix);
+  EXPECT_TRUE(MatrixComparison(outMatrix, inMatrix, ABS_ERROR));
 }
 #endif
 
diff --git a/Testing/igtlMessageFormat2TestMacro.h b/Testing/igtlMessageFormat2TestMacro.h
index 15256c4e..1bdb556e 100644
--- a/Testing/igtlMessageFormat2TestMacro.h
+++ b/Testing/igtlMessageFormat2TestMacro.h
@@ -28,6 +28,7 @@
   object->SetMessageID(1);
 
 #define igtlMetaDataComparisonMacro(object) \
+  {\
   std::vector<std::string> groundTruth(0); \
   groundTruth.push_back("First patient age");\
   groundTruth.push_back("Second patient age");\
@@ -45,6 +46,7 @@
     EXPECT_STREQ(it->first.c_str(), groundTruth[i].c_str());\
     EXPECT_EQ(it->second.first, IANA_TYPE_US_ASCII);\
     EXPECT_STREQ(it->second.second.c_str(), groundTruthAge[i].c_str());\
+  }\
   }
 
 
diff --git a/Testing/igtlTrackingDataMessageTest.cxx b/Testing/igtlTrackingDataMessageTest.cxx
index 75285e8c..27d78a10 100644
--- a/Testing/igtlTrackingDataMessageTest.cxx
+++ b/Testing/igtlTrackingDataMessageTest.cxx
@@ -25,6 +25,7 @@ igtl::TrackingDataMessage::Pointer trackingReceiveMsg = igtl::TrackingDataMessag
 igtl::TrackingDataElement::Pointer trackingElement0 = igtl::TrackingDataElement::New();
 igtl::TrackingDataElement::Pointer trackingElement1 = igtl::TrackingDataElement::New();
 igtl::TrackingDataElement::Pointer trackingElement2 = igtl::TrackingDataElement::New();
+igtl::TrackingDataElement::Pointer trackingElement3 = igtl::TrackingDataElement::New();
 
 float inT[4] = {-0.954892f, 0.196632f, -0.222525f, 0.0};
 float inS[4] = {-0.196632f, 0.142857f, 0.970014f, 0.0};
@@ -106,6 +107,41 @@ TEST(TrackingMessageTest, Unpack)
     elem->GetMatrix(outMatrix);
     EXPECT_TRUE(MatrixComparison(outMatrix, inMatrix, ABS_ERROR));
     }
+  
+  // reuse message test
+  trackingElement3->SetName("Tracker3");
+  trackingElement3->SetType(IGTL_TDATA_TYPE_6D);
+  trackingElement3->SetMatrix(inMatrix);
+  trackingSendMsg->AddTrackingDataElement(trackingElement3); // there are 4 tracking element now in the message
+  trackingSendMsg->Pack();
+  headerMsg->InitPack();
+  memcpy(headerMsg->GetPackPointer(), (const void*)trackingSendMsg->GetPackPointer(), IGTL_HEADER_SIZE);
+  headerMsg->Unpack();
+  trackingReceiveMsg->SetMessageHeader(headerMsg);
+  trackingReceiveMsg->AllocatePack();
+  memcpy(trackingReceiveMsg->GetPackBodyPointer(), trackingSendMsg->GetPackBodyPointer(), IGTL_TDATA_ELEMENT_SIZE*4);
+  trackingReceiveMsg->Unpack();
+  messageHeader = (igtl_header *)trackingReceiveMsg->GetPackPointer();
+  EXPECT_STREQ(messageHeader->device_name, "DeviceName");
+  EXPECT_STREQ(messageHeader->name, "TDATA");
+  EXPECT_EQ(messageHeader->header_version, 1);
+  EXPECT_EQ(messageHeader->timestamp, 1234567892);
+  EXPECT_EQ(messageHeader->body_size, IGTL_TDATA_ELEMENT_SIZE*4);
+
+  trackerDescription.push_back((char*)"Tracker3");
+  
+  for (int i = 0; i<4;++i)
+  {
+    igtl::TrackingDataElement::Pointer elem = igtl::TrackingDataElement::New();
+    trackingReceiveMsg->GetTrackingDataElement(i, elem);
+    EXPECT_EQ(strncmp((char*)(elem->GetName()), trackerDescription[i], 8),0);
+    igtl::Matrix4x4 outMatrix = {{0.0,0.0,0.0,0.0},
+      {0.0,0.0,0.0,0.0},
+      {0.0,0.0,0.0,0.0},
+      {0.0,0.0,0.0,0.0}};
+    elem->GetMatrix(outMatrix);
+    EXPECT_TRUE(MatrixComparison(outMatrix, inMatrix, ABS_ERROR));
+  }
 }
 
 
@@ -168,6 +204,42 @@ TEST(TrackingMessageTest, UnpackFormatVersion2)
     }
   
   igtlMetaDataComparisonMacro(trackingReceiveMsg);
+  
+  // reuse message test
+  trackingElement3->SetName("Tracker3");
+  trackingElement3->SetType(IGTL_TDATA_TYPE_6D);
+  trackingElement3->SetMatrix(inMatrix);
+  trackingSendMsg->AddTrackingDataElement(trackingElement3); // there are 4 tracking element now in the message
+  trackingSendMsg->Pack();
+  headerMsg->InitPack();
+  memcpy(headerMsg->GetPackPointer(), (const void*)trackingSendMsg->GetPackPointer(), IGTL_HEADER_SIZE);
+  headerMsg->Unpack();
+  trackingReceiveMsg->SetMessageHeader(headerMsg);
+  trackingReceiveMsg->AllocatePack();
+  memcpy(trackingReceiveMsg->GetPackBodyPointer(), trackingSendMsg->GetPackBodyPointer(), IGTL_TDATA_ELEMENT_SIZE*4 + EXTENDED_CONTENT_SIZE);
+  trackingReceiveMsg->Unpack();
+  messageHeader = (igtl_header *)trackingReceiveMsg->GetPackPointer();
+  EXPECT_STREQ(messageHeader->device_name, "DeviceName");
+  EXPECT_STREQ(messageHeader->name, "TDATA");
+  EXPECT_EQ(messageHeader->header_version, 2);
+  EXPECT_EQ(messageHeader->timestamp, 1234567892);
+  EXPECT_EQ(messageHeader->body_size, IGTL_TDATA_ELEMENT_SIZE*4 + EXTENDED_CONTENT_SIZE);
+  
+  trackerDescription.push_back((char*)"Tracker3");
+  
+  for (int i = 0; i<4;++i)
+  {
+    igtl::TrackingDataElement::Pointer elem = igtl::TrackingDataElement::New();
+    trackingReceiveMsg->GetTrackingDataElement(i, elem);
+    EXPECT_EQ(strncmp((char*)(elem->GetName()), trackerDescription[i], 8),0);
+    igtl::Matrix4x4 outMatrix = {{0.0,0.0,0.0,0.0},
+      {0.0,0.0,0.0,0.0},
+      {0.0,0.0,0.0,0.0},
+      {0.0,0.0,0.0,0.0}};
+    elem->GetMatrix(outMatrix);
+    EXPECT_TRUE(MatrixComparison(outMatrix, inMatrix, ABS_ERROR));
+  }
+  igtlMetaDataComparisonMacro(trackingReceiveMsg);
 }
 
 #endif
diff --git a/Testing/igtlTransformMessageTest.cxx b/Testing/igtlTransformMessageTest.cxx
index 953f3a1f..ac4107d3 100644
--- a/Testing/igtlTransformMessageTest.cxx
+++ b/Testing/igtlTransformMessageTest.cxx
@@ -72,7 +72,35 @@ TEST(TransformMessageTest, UnpackFormatVersion1)
                                {0.0,0.0,0.0,0.0}};
   transformReceiveMsg->GetMatrix(outMatrix);
   EXPECT_TRUE(MatrixComparison(outMatrix, inMatrix, ABS_ERROR));
+
+  //Reuse message test
+  igtl::Matrix4x4 inMatrix2 = {{inT[1],inS[0],inN[0],inOrigin[0]},
+    {inT[2],inS[1],inN[3],inOrigin[2]},
+    {inT[0],inS[3],inN[2],inOrigin[2]},
+    {inT[3],inS[3],inN[3],inOrigin[3]}};
+  
+  transformSendMsg->SetMatrix(inMatrix2);
+  transformSendMsg->Pack();
+  headerMsg->InitPack();
+  memcpy(headerMsg->GetPackPointer(), (const void*)transformSendMsg->GetPackPointer(), IGTL_HEADER_SIZE);
+  headerMsg->Unpack();
+  transformReceiveMsg->SetMessageHeader(headerMsg);
+  transformReceiveMsg->AllocatePack();
+  memcpy(transformReceiveMsg->GetPackBodyPointer(), transformSendMsg->GetPackBodyPointer(), transformReceiveMsg->GetPackBodySize());
+  transformReceiveMsg->Unpack();
+  messageHeader = (igtl_header *)transformReceiveMsg->GetPackPointer();
+  EXPECT_STREQ(messageHeader->device_name, "DeviceName");
+  EXPECT_STREQ(messageHeader->name, "TRANSFORM");
+  EXPECT_EQ(messageHeader->header_version, 1);
+  EXPECT_EQ(messageHeader->timestamp, 1234567892);
+  EXPECT_EQ(messageHeader->body_size, IGTL_TRANSFORM_SIZE);
   
+  igtl::Matrix4x4 outMatrix2 = {{0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0}};
+  transformReceiveMsg->GetMatrix(outMatrix2);
+  EXPECT_TRUE(MatrixComparison(outMatrix2, inMatrix2, ABS_ERROR));
 }
 
 #if OpenIGTLink_PROTOCOL_VERSION >= 3
@@ -123,6 +151,36 @@ TEST(TransformMessageTest, UnpackFormatVersion2)
   transformReceiveMsg->GetMatrix(outMatrix);
   EXPECT_TRUE(MatrixComparison(outMatrix, inMatrix, ABS_ERROR));
   igtlMetaDataComparisonMacro(transformReceiveMsg);
+  
+  //Reuse message test
+  igtl::Matrix4x4 inMatrix2 = {{inT[1],inS[0],inN[0],inOrigin[0]},
+    {inT[2],inS[1],inN[3],inOrigin[2]},
+    {inT[0],inS[3],inN[2],inOrigin[2]},
+    {inT[3],inS[3],inN[3],inOrigin[3]}};
+  
+  transformSendMsg->SetMatrix(inMatrix2);
+  transformSendMsg->Pack();
+  headerMsg->InitPack();
+  memcpy(headerMsg->GetPackPointer(), (const void*)transformSendMsg->GetPackPointer(), IGTL_HEADER_SIZE);
+  headerMsg->Unpack();
+  transformReceiveMsg->SetMessageHeader(headerMsg);
+  transformReceiveMsg->AllocatePack();
+  memcpy(transformReceiveMsg->GetPackBodyPointer(), transformSendMsg->GetPackBodyPointer(), transformReceiveMsg->GetPackBodySize());
+  transformReceiveMsg->Unpack();
+  messageHeader = (igtl_header *)transformReceiveMsg->GetPackPointer();
+  EXPECT_STREQ(messageHeader->device_name, "DeviceName");
+  EXPECT_STREQ(messageHeader->name, "TRANSFORM");
+  EXPECT_EQ(messageHeader->header_version, 2);
+  EXPECT_EQ(messageHeader->timestamp, 1234567892);
+  EXPECT_EQ(messageHeader->body_size, IGTL_TRANSFORM_SIZE + EXTENDED_CONTENT_SIZE);
+  
+  igtl::Matrix4x4 outMatrix2 = {{0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0},
+    {0.0,0.0,0.0,0.0}};
+  transformReceiveMsg->GetMatrix(outMatrix2);
+  EXPECT_TRUE(MatrixComparison(outMatrix2, inMatrix2, ABS_ERROR));
+  igtlMetaDataComparisonMacro(transformReceiveMsg);
 }
 
 #endif