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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
|
#include "XnLink16zParser.h"
#include "XnShiftToDepth.h"
#define MAX_SMALL_DIFF_NIBBLE 0x0C
#define SMALL_DIFF_OFFSET 6
#define BIG_DIFF_OFFSET 64
namespace xn
{
template<bool TS2D>
Link16zParser<TS2D>::Link16zParser(const XnShiftToDepthTables& shiftToDepthTables) :
m_pShiftToDepth(shiftToDepthTables.pShiftToDepthTable),
m_nShift(0),
m_nState(STATE_OPCODE),
m_nBigDiff(0),
m_nMaxShift((XnUInt16)(shiftToDepthTables.nShiftsCount - 1))
{
}
template<bool TS2D>
Link16zParser<TS2D>::~Link16zParser()
{
}
template<>
inline OniDepthPixel Link16zParser<true>::TranslatePixel(XnUInt32 nShift)
{
return m_pShiftToDepth[nShift];
}
template<>
inline OniDepthPixel Link16zParser<false>::TranslatePixel(XnUInt32 nShift)
{
return static_cast<OniDepthPixel>(nShift);
}
template<bool TS2D>
XnStatus Link16zParser<TS2D>::ParsePacketImpl(XnLinkFragmentation fragmentation,
const XnUInt8* pSrc,
const XnUInt8* pSrcEnd,
XnUInt8*& pDst,
const XnUInt8* pDstEnd)
{
OniDepthPixel*& pDstPixel = reinterpret_cast<OniDepthPixel*&>(pDst); //Reference to pointer - moving pDstPixel affects pDst also.
const OniDepthPixel* pDstPixelEnd = reinterpret_cast<const OniDepthPixel*>(pDstEnd);
XnUInt32 nRLERepeats = 0;
XnBool bReadHigh = TRUE; //TRUE when reading high part of byte, FALSE when reading low part
XnUInt32 nNibble = 0;
if ((fragmentation & XN_LINK_FRAG_BEGIN) != 0)
{
//We purposely give m_nShift a value that will mess up diff calculations if it's in the beginning by mistake
m_nShift = (m_nMaxShift + BIG_DIFF_OFFSET + 1);
m_nBigDiff = 0;
m_nState = STATE_OPCODE;
bReadHigh = TRUE;
}
////////////////////////////////////////////
while ((pSrc < pSrcEnd) && (pDstPixel < pDstPixelEnd))
{
//Read next nibble
if (bReadHigh)
{
//Read high nibble of this byte
nNibble = (*pSrc >> 4);
bReadHigh = FALSE;
}
else
{
//Read low nibble of this byte and skip to next byte
nNibble = (*pSrc & 0x0F);
pSrc++;
bReadHigh = TRUE;
}
switch (m_nState)
{
case STATE_OPCODE:
if (nNibble <= MAX_SMALL_DIFF_NIBBLE)
{
//Read value is small diff - calculate shift and write it.
m_nShift += nNibble - SMALL_DIFF_OFFSET;
if (m_nShift > m_nMaxShift)
{
// XN_ASSERT(FALSE);
m_nState = STATE_BAD_FRAME;
return XN_STATUS_LINK_BAD_PACKET_FORMAT;
}
*pDstPixel++ = TranslatePixel(m_nShift);
//Stay in STATE_OPCODE
}
else
{
m_nState = State(nNibble); //Next state is determined by opcode nibble
}
break;
case STATE_RLE:
if (m_nShift > m_nMaxShift)
{
// XN_ASSERT(FALSE);
m_nState = STATE_BAD_FRAME;
return XN_STATUS_LINK_BAD_PACKET_FORMAT;
}
//Write as many values as needed or just fill remaining space.
nRLERepeats = XN_MIN(nNibble + 1, XnUInt32(pDstPixelEnd - pDstPixel));
while (nRLERepeats > 0)
{
*pDstPixel++ = TranslatePixel(m_nShift);
nRLERepeats--;
}
m_nState = STATE_OPCODE;
break;
case STATE_FULL_ENC1:
if (nNibble & 0x08)
{
//Take 3 lower bits from nibble, make room for 4 bits more
m_nBigDiff = (nNibble & 0x07) << 4;
m_nState = STATE_FULL_ENC_BIG_DIFF;
}
else
{
//Take 3 lower bits from nibble, make room for 12 bits more
//m_nShift = (nNibble & 0x07) << 12;
//There's no point doing this since max shift is 12 bits
m_nState = State(m_nState + 1); //To STATE_FULL_ENC2
}
break;
case STATE_FULL_ENC2:
//Take 4 bits from nibble, make room for 8 more
//m_nShift |= (nNibble << 8);
//There's no point saving anything from the previous shift value since max shift is 12 bits
m_nShift = (nNibble << 8);
m_nState = State(m_nState + 1); //To STATE_FULL_ENC3
break;
case STATE_FULL_ENC3:
//Take 4 bits from nibble, make room for 4 more
m_nShift |= (nNibble << 4);
m_nState = State(m_nState + 1); //To STATE_FULL_ENC4
break;
case STATE_FULL_ENC4:
//Take 4 bits from nibble, write value to destination
m_nShift |= nNibble;
if (m_nShift > m_nMaxShift)
{
XN_ASSERT(FALSE);
return XN_STATUS_LINK_RESP_CORRUPT_PACKET;
}
*pDstPixel++ = TranslatePixel(m_nShift);
m_nState = STATE_OPCODE;
break;
case STATE_FULL_ENC_BIG_DIFF:
//Take 4 bits from nibble, write value with difference to destination
m_nBigDiff |= nNibble;
m_nShift += (m_nBigDiff - BIG_DIFF_OFFSET);
if (m_nShift > m_nMaxShift)
{
// XN_ASSERT(FALSE);
m_nState = STATE_BAD_FRAME;
return XN_STATUS_LINK_BAD_PACKET_FORMAT;
}
*pDstPixel++ = TranslatePixel(m_nShift);
m_nState = STATE_OPCODE;
break;
case STATE_BAD_FRAME:
//We stay in this state until the next frame beginning arrives
return XN_STATUS_LINK_BAD_PACKET_FORMAT;
default:
XN_ASSERT(FALSE);
return XN_STATUS_ERROR;
} //switch
} //while
////////////////////////////////////////////
/* if (pSrc < pSrcEnd)
{
//We had more bytes in input but not enough space in output to write them
XN_ASSERT(FALSE);
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
*/
return XN_STATUS_OK;
}
//---------------------------------------------------------------------------
// Explicit instantiations
//---------------------------------------------------------------------------
template class Link16zParser<true>;
template class Link16zParser<false>;
}
|
