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); }