-
Notifications
You must be signed in to change notification settings - Fork 1
/
RobotStatus.cxx
98 lines (81 loc) · 1.95 KB
/
RobotStatus.cxx
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
/*=========================================================================
Program: BRP Prostate Robot: Testing Simulator (Robot)
Language: C++
Copyright (c) Brigham and Women's Hospital. All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. See the above copyright notices for more information.
Please see
http://wiki.na-mic.org/Wiki/index.php/ProstateBRP_OpenIGTLink_Communication_June_2013
for the detail of the testing protocol.
=========================================================================*/
#include "RobotStatus.h"
#include "igtlMath.h"
RobotStatus::RobotStatus()
{
igtl::IdentityMatrix(this->CalibrationMatrix);
igtl::IdentityMatrix(this->TargetMatrix);
this->FlagCalibration = 0;
this->FlagTarget = 0;
}
RobotStatus::~RobotStatus()
{
}
void RobotStatus::SetCalibrationMatrix(igtl::Matrix4x4& matrix)
{
this->FlagCalibration = 1;
for (int i = 0; i < 4; i ++)
{
for (int j = 0; j < 4; j ++)
{
this->CalibrationMatrix[i][j] = matrix[i][j];
}
}
}
int RobotStatus::GetCalibrationMatrix(igtl::Matrix4x4& matrix)
{
if (this->FlagCalibration)
{
for (int i = 0; i < 4; i ++)
{
for (int j = 0; j < 4; j ++)
{
matrix[i][j] = this->CalibrationMatrix[i][j];
}
}
return 1;
}
else
{
return 0;
}
}
void RobotStatus::SetTargetMatrix(igtl::Matrix4x4& matrix)
{
this->FlagTarget = 1;
for (int i = 0; i < 4; i ++)
{
for (int j = 0; j < 4; j ++)
{
this->TargetMatrix[i][j] = matrix[i][j];
}
}
}
int RobotStatus::GetTargetMatrix(igtl::Matrix4x4& matrix)
{
if (this->FlagTarget)
{
for (int i = 0; i < 4; i ++)
{
for (int j = 0; j < 4; j ++)
{
matrix[i][j] = this->TargetMatrix[i][j];
}
}
return 1;
}
else
{
return 0;
}
}