static void
copy(int fd, uint32_t dst, uint32_t src)
{
uint32_t batch[1024], *b = batch;
struct drm_i915_gem_relocation_entry reloc[2], *r = reloc;
struct drm_i915_gem_exec_object2 obj[3];
struct drm_i915_gem_execbuffer2 exec;
uint32_t handle;
int ret;
/* invariant state */
*b++ = (_3DSTATE_AA_CMD |
AA_LINE_ECAAR_WIDTH_ENABLE |
AA_LINE_ECAAR_WIDTH_1_0 |
AA_LINE_REGION_WIDTH_ENABLE | AA_LINE_REGION_WIDTH_1_0);
*b++ = (_3DSTATE_INDEPENDENT_ALPHA_BLEND_CMD |
IAB_MODIFY_ENABLE |
IAB_MODIFY_FUNC | (BLENDFUNC_ADD << IAB_FUNC_SHIFT) |
IAB_MODIFY_SRC_FACTOR | (BLENDFACT_ONE <<
IAB_SRC_FACTOR_SHIFT) |
IAB_MODIFY_DST_FACTOR | (BLENDFACT_ZERO <<
IAB_DST_FACTOR_SHIFT));
*b++ = (_3DSTATE_DFLT_DIFFUSE_CMD);
*b++ = (0);
*b++ = (_3DSTATE_DFLT_SPEC_CMD);
*b++ = (0);
*b++ = (_3DSTATE_DFLT_Z_CMD);
*b++ = (0);
*b++ = (_3DSTATE_COORD_SET_BINDINGS |
CSB_TCB(0, 0) |
CSB_TCB(1, 1) |
CSB_TCB(2, 2) |
CSB_TCB(3, 3) |
CSB_TCB(4, 4) |
CSB_TCB(5, 5) | CSB_TCB(6, 6) | CSB_TCB(7, 7));
*b++ = (_3DSTATE_RASTER_RULES_CMD |
ENABLE_POINT_RASTER_RULE |
OGL_POINT_RASTER_RULE |
ENABLE_LINE_STRIP_PROVOKE_VRTX |
ENABLE_TRI_FAN_PROVOKE_VRTX |
LINE_STRIP_PROVOKE_VRTX(1) |
TRI_FAN_PROVOKE_VRTX(2) | ENABLE_TEXKILL_3D_4D | TEXKILL_4D);
*b++ = (_3DSTATE_MODES_4_CMD |
ENABLE_LOGIC_OP_FUNC | LOGIC_OP_FUNC(LOGICOP_COPY) |
ENABLE_STENCIL_WRITE_MASK | STENCIL_WRITE_MASK(0xff) |
ENABLE_STENCIL_TEST_MASK | STENCIL_TEST_MASK(0xff));
*b++ = (_3DSTATE_LOAD_STATE_IMMEDIATE_1 | I1_LOAD_S(3) | I1_LOAD_S(4) | I1_LOAD_S(5) | 2);
*b++ = (0x00000000); /* Disable texture coordinate wrap-shortest */
*b++ = ((1 << S4_POINT_WIDTH_SHIFT) |
S4_LINE_WIDTH_ONE |
S4_CULLMODE_NONE |
S4_VFMT_XY);
*b++ = (0x00000000); /* Stencil. */
*b++ = (_3DSTATE_SCISSOR_ENABLE_CMD | DISABLE_SCISSOR_RECT);
*b++ = (_3DSTATE_SCISSOR_RECT_0_CMD);
*b++ = (0);
*b++ = (0);
*b++ = (_3DSTATE_DEPTH_SUBRECT_DISABLE);
*b++ = (_3DSTATE_LOAD_INDIRECT | 0); /* disable indirect state */
*b++ = (0);
*b++ = (_3DSTATE_STIPPLE);
*b++ = (0x00000000);
*b++ = (_3DSTATE_BACKFACE_STENCIL_OPS | BFO_ENABLE_STENCIL_TWO_SIDE | 0);
/* samler state */
#define TEX_COUNT 1
*b++ = (_3DSTATE_MAP_STATE | (3 * TEX_COUNT));
*b++ = ((1 << TEX_COUNT) - 1);
*b = fill_reloc(r++, b-batch, src, I915_GEM_DOMAIN_SAMPLER, 0); b++;
*b++ = (MAPSURF_32BIT | MT_32BIT_ARGB8888 |
MS3_TILED_SURFACE |
(HEIGHT - 1) << MS3_HEIGHT_SHIFT |
(WIDTH - 1) << MS3_WIDTH_SHIFT);
*b++ = ((WIDTH-1) << MS4_PITCH_SHIFT);
*b++ = (_3DSTATE_SAMPLER_STATE | (3 * TEX_COUNT));
*b++ = ((1 << TEX_COUNT) - 1);
*b++ = (MIPFILTER_NONE << SS2_MIP_FILTER_SHIFT |
FILTER_NEAREST << SS2_MAG_FILTER_SHIFT |
FILTER_NEAREST << SS2_MIN_FILTER_SHIFT);
*b++ = (TEXCOORDMODE_WRAP << SS3_TCX_ADDR_MODE_SHIFT |
TEXCOORDMODE_WRAP << SS3_TCY_ADDR_MODE_SHIFT |
0 << SS3_TEXTUREMAP_INDEX_SHIFT);
*b++ = (0x00000000);
/* render target state */
*b++ = (_3DSTATE_BUF_INFO_CMD);
*b++ = (BUF_3D_ID_COLOR_BACK | BUF_3D_TILED_SURFACE | WIDTH*4);
*b = fill_reloc(r++, b-batch, dst,
I915_GEM_DOMAIN_RENDER, I915_GEM_DOMAIN_RENDER);
b++;
*b++ = (_3DSTATE_DST_BUF_VARS_CMD);
*b++ = (COLR_BUF_ARGB8888 |
DSTORG_HORT_BIAS(0x8) |
DSTORG_VERT_BIAS(0x8));
/* draw rect is unconditional */
*b++ = (_3DSTATE_DRAW_RECT_CMD);
*b++ = (0x00000000);
*b++ = (0x00000000); /* ymin, xmin */
*b++ = (DRAW_YMAX(HEIGHT - 1) |
DRAW_XMAX(WIDTH - 1));
/* yorig, xorig (relate to color buffer?) */
*b++ = (0x00000000);
/* texfmt */
*b++ = (_3DSTATE_LOAD_STATE_IMMEDIATE_1 | I1_LOAD_S(1) | I1_LOAD_S(2) | I1_LOAD_S(6) | 2);
*b++ = ((4 << S1_VERTEX_WIDTH_SHIFT) | (4 << S1_VERTEX_PITCH_SHIFT));
*b++ = (~S2_TEXCOORD_FMT(0, TEXCOORDFMT_NOT_PRESENT) |
S2_TEXCOORD_FMT(0, TEXCOORDFMT_2D));
*b++ = (S6_CBUF_BLEND_ENABLE | S6_COLOR_WRITE_ENABLE |
BLENDFUNC_ADD << S6_CBUF_BLEND_FUNC_SHIFT |
BLENDFACT_ONE << S6_CBUF_SRC_BLEND_FACT_SHIFT |
BLENDFACT_ZERO << S6_CBUF_DST_BLEND_FACT_SHIFT);
/* pixel shader */
*b++ = (_3DSTATE_PIXEL_SHADER_PROGRAM | (1 + 3*3 - 2));
/* decl FS_T0 */
*b++ = (D0_DCL |
REG_TYPE(FS_T0) << D0_TYPE_SHIFT |
REG_NR(FS_T0) << D0_NR_SHIFT |
((REG_TYPE(FS_T0) != REG_TYPE_S) ? D0_CHANNEL_ALL : 0));
*b++ = (0);
*b++ = (0);
/* decl FS_S0 */
*b++ = (D0_DCL |
(REG_TYPE(FS_S0) << D0_TYPE_SHIFT) |
(REG_NR(FS_S0) << D0_NR_SHIFT) |
((REG_TYPE(FS_S0) != REG_TYPE_S) ? D0_CHANNEL_ALL : 0));
*b++ = (0);
*b++ = (0);
/* texld(FS_OC, FS_S0, FS_T0 */
*b++ = (T0_TEXLD |
(REG_TYPE(FS_OC) << T0_DEST_TYPE_SHIFT) |
(REG_NR(FS_OC) << T0_DEST_NR_SHIFT) |
(REG_NR(FS_S0) << T0_SAMPLER_NR_SHIFT));
*b++ = ((REG_TYPE(FS_T0) << T1_ADDRESS_REG_TYPE_SHIFT) |
(REG_NR(FS_T0) << T1_ADDRESS_REG_NR_SHIFT));
*b++ = (0);
*b++ = (PRIM3D_RECTLIST | (3*4 - 1));
*b++ = pack_float(WIDTH);
*b++ = pack_float(HEIGHT);
*b++ = pack_float(WIDTH);
*b++ = pack_float(HEIGHT);
*b++ = pack_float(0);
*b++ = pack_float(HEIGHT);
*b++ = pack_float(0);
*b++ = pack_float(HEIGHT);
*b++ = pack_float(0);
*b++ = pack_float(0);
*b++ = pack_float(0);
*b++ = pack_float(0);
*b++ = MI_BATCH_BUFFER_END;
if ((b - batch) & 1)
*b++ = 0;
igt_assert(b - batch <= 1024);
handle = gem_create(fd, 4096);
gem_write(fd, handle, 0, batch, (b-batch)*sizeof(batch[0]));
igt_assert(r-reloc == 2);
obj[0].handle = dst;
obj[0].relocation_count = 0;
obj[0].relocs_ptr = 0;
obj[0].alignment = 0;
obj[0].offset = 0;
obj[0].flags = 0;
obj[0].rsvd1 = 0;
obj[0].rsvd2 = 0;
obj[1].handle = src;
obj[1].relocation_count = 0;
obj[1].relocs_ptr = 0;
obj[1].alignment = 0;
obj[1].offset = 0;
obj[1].flags = 0;
obj[1].rsvd1 = 0;
obj[1].rsvd2 = 0;
obj[2].handle = handle;
obj[2].relocation_count = 2;
obj[2].relocs_ptr = (uintptr_t)reloc;
obj[2].alignment = 0;
obj[2].offset = 0;
obj[2].flags = 0;
obj[2].rsvd1 = obj[2].rsvd2 = 0;
exec.buffers_ptr = (uintptr_t)obj;
exec.buffer_count = 3;
exec.batch_start_offset = 0;
exec.batch_len = (b-batch)*sizeof(batch[0]);
exec.DR1 = exec.DR4 = 0;
exec.num_cliprects = 0;
exec.cliprects_ptr = 0;
exec.flags = 0;
i915_execbuffer2_set_context_id(exec, 0);
exec.rsvd2 = 0;
ret = drmIoctl(fd, DRM_IOCTL_I915_GEM_EXECBUFFER2, &exec);
while (ret && errno == EBUSY) {
drmCommandNone(fd, DRM_I915_GEM_THROTTLE);
ret = drmIoctl(fd, DRM_IOCTL_I915_GEM_EXECBUFFER2, &exec);
}
igt_assert_eq(ret, 0);
gem_close(fd, handle);
}
inline void packet(char* buff, size_t* buff_n, bool pack)
{
pack_u8(&attribute_group, buff, buff_n, pack);
pack_u8(&attribute_type, buff, buff_n, pack);
pack_float(&value, buff, buff_n, pack);
}
RTC::ReturnCode_t RTCOpenNI::publish_depth()
{
if (!enable_depth_)
{
return RTC::RTC_OK;
}
if (!depth_gen_.IsNewDataAvailable())
{
return RTC::RTC_OK;
}
// Read and publish depth data
depth_gen_.WaitAndUpdateData();
XnMapOutputMode output_mode;
depth_gen_.GetMapOutputMode(output_mode);
xn::DepthMetaData depth_md;
depth_gen_.GetMetaData(depth_md);
std::cout << "Depth time stamp is " << depth_md.Timestamp() << '\n';
// Depth map
depth_map_.tm.sec = 0;
depth_map_.tm.nsec = 0;
unsigned int num_pixels = output_mode.nXRes * output_mode.nYRes;
for (unsigned int pixel = 0, dest = 0; pixel < num_pixels;
pixel++, dest += 4)
{
if (depth_md[pixel] == no_sample_val_ ||
depth_md[pixel] == shadow_val_ ||
depth_md[pixel] == 0)
{
depth_map_.pixels[dest] = std::numeric_limits<float>::quiet_NaN();
}
else
{
// Convert the distance from mm as an int to metres as a float
*(reinterpret_cast<float*>(&(depth_map_.pixels[dest]))) =
static_cast<float>(depth_md[pixel]) / 1000.0;
}
}
depth_map_port_.write();
// Point cloud
cloud_.tm.sec = 0;
cloud_.tm.nsec = 0;
cloud_.seq = depth_md.FrameID();
cloud_.height = output_mode.nXRes;
cloud_.width = output_mode.nYRes;
cloud_.data.length(depth_md.XRes() * depth_md.YRes() * cloud_.point_step);
double x, y, z;
// Oh, how I wish for a decent CORBA API with things like iterators...
unsigned int dest_ii = 0;
unsigned int source_ii = 0;
for (unsigned int pixel_y = 0; pixel_y < cloud_.height; ++pixel_y)
{
for (unsigned int pixel_x = 0; pixel_x < cloud_.width;
++pixel_x, dest_ii += cloud_.point_step, ++source_ii)
{
// x = (X - cx).C.Z
// y = (Y - cy).C.Z
// z = Z
// where:
// x, y, z = 3D point position relative to IR camera
// X, Y = Unprojected coordinates (pixel x, pixel y)
// cx, cy = Centre of the image, calculated in onActivated()
// C = Constant of project, calculated in onActivated()
// Z = Depth measurement
if (depth_md[source_ii] == no_sample_val_ ||
depth_md[source_ii] == shadow_val_ ||
depth_md[source_ii] == 0)
{
x = std::numeric_limits<float>::quiet_NaN();
y = std::numeric_limits<float>::quiet_NaN();
z = std::numeric_limits<float>::quiet_NaN();
}
else
{
// The mm -> m scaling for x and y is handled in proj_const_
x = (pixel_x - centre_x_) * proj_const_ * depth_md[source_ii];
y = (pixel_y - centre_y_) * proj_const_ * depth_md[source_ii];
z = depth_md[source_ii] / 1000.0;
}
// Ugly packing of 3 floats into a byte array because it's a CORBA
// destination, not a PCL point cloud structure.
pack_float(cloud_.data, x, dest_ii);
pack_float(cloud_.data, y, dest_ii + 4);
pack_float(cloud_.data, z, dest_ii + 8);
}
}
cloud_port_.write();
return RTC::RTC_OK;
}
/*
* float
*/
static void
enc_float(amqp_value_t *v, mnbytestream_t *bs)
{
pack_float(bs, v->value.f);
}
