/******************************************************************** ** Image Component Library (ICL) ** ** ** ** Copyright (C) 2006-2013 CITEC, University of Bielefeld ** ** Neuroinformatics Group ** ** Website: www.iclcv.org and ** ** http://opensource.cit-ec.de/projects/icl ** ** ** ** File : ICLGeom/src/ICLGeom/Primitive3DFilter.cpp ** ** Module : ICLGeom ** ** Authors: Lukas Twardon ** ** ** ** ** ** GNU LESSER GENERAL PUBLIC LICENSE ** ** This file may be used under the terms of the GNU Lesser General ** ** Public License version 3.0 as published by the ** ** ** ** Free Software Foundation and appearing in the file LICENSE.LGPL ** ** included in the packaging of this file. Please review the ** ** following information to ensure the license requirements will ** ** be met: http://www.gnu.org/licenses/lgpl-3.0.txt ** ** ** ** The development of this software was supported by the ** ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** ** Forschungsgemeinschaft (DFG) in the context of the German ** ** Excellence Initiative. ** ** ** ********************************************************************/ #include #include #include namespace icl{ namespace geom{ const char *Primitive3DFilter::KERNEL_CODE = ( "float4 conj(float4 q) { \n" " float4 res; \n" " res.x = -q.x; \n" " res.y = -q.y; \n" " res.z = -q.z; \n" " res.w = q.w; \n" " return res; \n" "} \n" " \n" "float4 multiply(float4 q1, float4 q2) { \n" " float4 res; \n" " res.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; \n" " res.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x; \n" " res.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w; \n" " res.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; \n" " return res; \n" "} \n" " \n" "float4 rotateVector(float4 v, float4 q) { \n" " float4 res; \n" " v.w = 0; \n" " res = multiply(v, conj(q)); \n" " res = multiply(q, res); \n" " return res; \n" "} \n" " \n" "__kernel void createGroupMap(const __global float *pcbuffer, \n" " __global unsigned char *groupmapbuffer, \n" " const float a, const float b, const float c, \n" " const float4 orientation, \n" " const float4 position, \n" " const int type, const int groupBit) { \n" " const int i = get_global_id(0); \n" " float4 relpoint; \n" " relpoint.x = pcbuffer[4*i] - position.x; \n" " relpoint.y = pcbuffer[4*i+1] - position.y; \n" " relpoint.z = pcbuffer[4*i+2] - position.z; \n" " relpoint = rotateVector(relpoint, conj(orientation)); \n" " if(a == 0 || b == 0 || c == 0) \n" " return; \n" " if(type == 0) \n" " groupmapbuffer[i] |= ( (relpoint.x <= a) && (relpoint.x >= -a) && (relpoint.y <= b) && (relpoint.y >= -b) &&(relpoint.z <= c) && (relpoint.z >= -c) ) << groupBit; \n" " else if(type == 1) \n" " groupmapbuffer[i] |= ( (relpoint.x*relpoint.x)/(a*a) + (relpoint.y*relpoint.y)/(b*b) + (relpoint.z*relpoint.z)/(c*c) <= 1 ) << groupBit; \n" " else if(type == 2) \n" " groupmapbuffer[i] |= ( ((relpoint.x*relpoint.x)/(a*a) + (relpoint.y*relpoint.y)/(b*b) <= 1) && (relpoint.z <= c) && (relpoint.z >= -c) ) << groupBit; \n" "} \n" " \n" "__kernel void createActionMap(const __global unsigned char *groupmapbuffer, \n" " __global unsigned char *actionmapbuffer, \n" " const __global unsigned char *formulabuffer, \n" " const unsigned int formulaSize) { \n" " const int i = get_global_id(0); \n" " bool intersectionValue = false; \n" " for(unsigned int g = 0; g < formulaSize/3; g++) { \n" " if(formulabuffer[3*g]) { \n" " actionmapbuffer[i] = actionmapbuffer[i] || intersectionValue; \n" " intersectionValue = true; \n" " } \n" " if(formulabuffer[3*g+1]) \n" " intersectionValue = intersectionValue && (groupmapbuffer[i] & (1 << formulabuffer[3*g+2])); \n" " else \n" " intersectionValue = intersectionValue && !(groupmapbuffer[i] & (1 << formulabuffer[3*g+2])); \n" " } \n" " actionmapbuffer[i] = actionmapbuffer[i] || intersectionValue; \n" "} \n" ); Primitive3DFilter::Primitive3DFilter(const FilterConfig &config) : config(config) { #ifdef ICL_HAVE_OPENCL program = utils::CLProgram("gpu", KERNEL_CODE); kernelCreateGroupMap = program.createKernel("createGroupMap"); kernelCreateActionMap = program.createKernel("createActionMap"); #endif } void Primitive3DFilter::apply(const std::vector &primitives, PointCloudObjectBase &pcObj, core::Img32f *depthImage) { pcObj.lock(); // dimension of the point cloud int dim = pcObj.getDim(); // the group map (8 bits per point with 1 = point is inside the primitive group) std::vector groupMap(dim, 0); // the xyz(h) data segment core::DataSegment xyzh; core::DataSegment xyz; if(pcObj.supports(PointCloudObjectBase::XYZH)) { xyzh = pcObj.selectXYZH(); } else if(PointCloudObjectBase::XYZ) { xyz = pcObj.selectXYZ(); } else { DEBUG_LOG("Filter error: Point cloud has no XYZ or XYZH data."); return; } #ifdef ICL_HAVE_OPENCL pcbuffer = program.createBuffer("r", dim*4*sizeof(float)); if(pcObj.supports(PointCloudObjectBase::XYZH)) { if(xyzh.isPacked()) { pcbuffer.write(xyzh.getDataPointer(), dim*4*sizeof(float)); } else { std::vector xyzhPacked; for(int i = 0; i < dim; ++i) { xyzhPacked.push_back(xyzh[i]); } pcbuffer.write(xyzhPacked.data(), dim*4*sizeof(float)); } } else if(pcObj.supports(PointCloudObjectBase::XYZ)) { if(xyz.isPacked()) { pcbuffer.write(xyz.getDataPointer(), dim*4*sizeof(float)); } else { std::vector xyzPacked; for(int i = 0; i < dim; ++i) { xyzPacked.push_back(math::Vec4(xyz[i][0], xyz[i][1], xyz[i][2], 1)); } pcbuffer.write(xyzPacked.data(), dim*4*sizeof(float)); } } groupmapbuffer = program.createBuffer("rw", dim); groupmapbuffer.write(groupMap.data(), dim); #endif // for all primitives for(std::vector::const_iterator it = primitives.begin(); it != primitives.end(); ++it) { // match regular expression with description to find primitive group int groupBit = -1; for(std::map::const_iterator mapit = config.mapRegexToBit.begin(); mapit != config.mapRegexToBit.end(); ++mapit) { std::string regex = mapit->first; if(utils::match(it->description, regex)) { if(groupBit == -1) { groupBit = config.mapRegexToBit[regex]; } else { DEBUG_LOG("Filter error: Primitive groups are not disjoint."); return; } } } // padding float padding = config.mapGroupBitToPadding[groupBit]; // scale float a = it->scale[0]/2.0 + padding; float b = it->scale[1]/2.0 + padding; float c = it->scale[2]/2.0 + padding; if(a <= 0 || b <= 0 || c <= 0) continue; #ifdef ICL_HAVE_OPENCL int opencltype = 0; if(it->type == CUBE) opencltype = 0; else if(it->type == SPHERE) opencltype = 1; else if(it->type == CYLINDER) opencltype = 2; kernelCreateGroupMap.setArgs(pcbuffer, groupmapbuffer, a, b, c, math::Vec4(it->orientation.v[0], it->orientation.v[1], it->orientation.v[2], it->orientation.w), it->position, opencltype, groupBit); kernelCreateGroupMap.apply(dim); groupmapbuffer.read(groupMap.data(), dim); #else // for all points for(int i = 0; i < pcObj.getDim(); ++i) { // translation math::Vec3 relpoint(xyzh[i][0] - it->position[0], xyzh[i][1] - it->position[1], xyzh[i][2] - it->position[2]); // rotation relpoint = it->orientation.conj().rotateVector(relpoint); // filtering if(it->type == CUBE) groupMap[i] |= ( (relpoint[0] <= a) && (relpoint[0] >= -a) && (relpoint[1] <= b) && (relpoint[1] >= -b) &&(relpoint[2] <= c) && (relpoint[2] >= -c) ) << groupBit; else if(it->type == SPHERE) groupMap[i] |= ( (relpoint[0]*relpoint[0])/(a*a) + (relpoint[1]*relpoint[1])/(b*b) + (relpoint[2]*relpoint[2])/(c*c) <= 1 ) << groupBit; else if(it->type == CYLINDER) groupMap[i] |= ( ((relpoint[0]*relpoint[0])/(a*a) + (relpoint[1]*relpoint[1])/(b*b) <= 1) && (relpoint[2] <= c) && (relpoint[2] >= -c) ) << groupBit; } #endif } // for all filter actions for(std::vector >::iterator actionit = config.filterActions.begin(); actionit != config.filterActions.end(); ++actionit) { // the action map (1 byte per point with 1 = point should be filtered) std::vector actionMap(dim, 0); #ifdef ICL_HAVE_OPENCL groupmapbuffer = program.createBuffer("rw", dim); actionmapbuffer = program.createBuffer("rw", dim); formulabuffer = program.createBuffer("r", (*actionit)->formula.size()); groupmapbuffer.write(groupMap.data(), dim); actionmapbuffer.write(actionMap.data(), dim); formulabuffer.write((*actionit)->formula.data(), (*actionit)->formula.size()); kernelCreateActionMap.setArgs(groupmapbuffer, actionmapbuffer, formulabuffer, (unsigned int)((*actionit)->formula.size())); kernelCreateActionMap.apply(dim); actionmapbuffer.read(actionMap.data(), dim); #else // for all points for(int i = 0; i < pcObj.getDim(); ++i) { bool intersectionValue = false; // for all atomic primitive groups in the filter formula for(unsigned int g = 0; g < (*actionit)->formula.size()/3; g++) { // oring if((*actionit)->formula[3*g]) { actionMap[i] = actionMap[i] || intersectionValue; intersectionValue = true; } // possibly negating and anding if((*actionit)->formula[3*g+1]) intersectionValue = intersectionValue && (groupMap[i] & (1 << (*actionit)->formula[3*g+2])); else intersectionValue = intersectionValue && !(groupMap[i] & (1 << (*actionit)->formula[3*g+2])); } // the last oring actionMap[i] = actionMap[i] || intersectionValue; } #endif // perform actual filter action (*actionit)->performAction(pcObj, actionMap, groupMap, depthImage); // update point cloud dim dim = pcObj.getDim(); } pcObj.unlock(); } Primitive3DFilter::FilterConfig::FilterConfig(const std::string &filename) throw(utils::ParseException) { // parse XML document pugi::xml_document doc; pugi::xml_parse_result result = doc.load_file(filename.c_str()); if(!result) throw utils::ParseException("Could not parse XML config."); pugi::xml_node rootNode = doc.child("pointcloudfilter"); if(rootNode.empty()) throw utils::ParseException("No pointcloudfilter root node."); // parse primitivegroups unsigned char currentBit = 0; for(pugi::xml_node primitivegroupNode = rootNode.child("primitivegroup"); primitivegroupNode; primitivegroupNode = primitivegroupNode.next_sibling("primitivegroup")) { if(currentBit >= 8) throw utils::ParseException("Too many (more than 8) primitive groups."); pugi::xml_attribute idAttr = primitivegroupNode.attribute("id"); if(idAttr.empty()) throw utils::ParseException("Primitivegroup has no id attribute."); std::string id = idAttr.value(); pugi::xml_attribute regexAttr = primitivegroupNode.attribute("regex"); if(regexAttr.empty()) throw utils::ParseException("Primitivegroup has no regex attribute."); std::string regex = regexAttr.value(); pugi::xml_attribute paddingAttr = primitivegroupNode.attribute("padding"); float padding = 0.0; if(!paddingAttr.empty()) { padding = paddingAttr.as_float(); } mapGroupIdToBit[id] = currentBit; mapRegexToBit[regex] = currentBit; mapGroupBitToPadding[currentBit] = padding; currentBit++; } // parse unions pugi::xpath_node_set actionNodes = doc.select_nodes("/pointcloudfilter/*[self::remove or self::setpos or self::color or self::label or self::intensity or self::filterdepthimg]"); for(pugi::xpath_node_set::const_iterator it = actionNodes.begin(); it != actionNodes.end(); ++it) { pugi::xml_node actionNode = it->node(); std::vector formula; // parse groups (one element intersections) for(pugi::xml_node groupNode = actionNode.child("group"); groupNode; groupNode = groupNode.next_sibling("group")) { unsigned char newIntersection = 1; unsigned char inner; unsigned char groupBit; pugi::xml_attribute partAttr = groupNode.attribute("part"); if(partAttr.empty()) throw utils::ParseException("Group has no part attribute."); std::string part = partAttr.value(); pugi::xml_attribute idAttr = groupNode.attribute("id"); if(idAttr.empty()) throw utils::ParseException("Group has no id attribute."); std::string id = idAttr.value(); if(part == "inner") inner = 1; else if(part == "outer") inner = 0; else throw utils::ParseException("Part attribute must be inner or outer."); try{ groupBit = mapGroupIdToBit.at(id); } catch(const std::out_of_range&) { throw utils::ParseException("No primitive group with id " + id + " has been defined."); } formula.push_back(newIntersection); formula.push_back(inner); formula.push_back(groupBit); } // parse intersections of groups for(pugi::xml_node intersectionNode = actionNode.child("intersection"); intersectionNode; intersectionNode = intersectionNode.next_sibling("intersection")) { unsigned char newIntersection = 1; for(pugi::xml_node groupNode = intersectionNode.child("group"); groupNode; groupNode = groupNode.next_sibling("group")) { unsigned char inner; unsigned char groupBit; pugi::xml_attribute partAttr = groupNode.attribute("part"); if(partAttr.empty()) throw utils::ParseException("Group has no part attribute."); std::string part = partAttr.value(); pugi::xml_attribute idAttr = groupNode.attribute("id"); if(idAttr.empty()) throw utils::ParseException("Group has no id attribute."); std::string id = idAttr.value(); if(part == "inner") inner = true; else if(part == "outer") inner = false; else throw utils::ParseException("Part attribute must be inner or outer."); try{ groupBit = mapGroupIdToBit.at(id); } catch(const std::out_of_range&) { throw utils::ParseException("No primitive group with id " + id + " has been defined."); } formula.push_back(newIntersection); formula.push_back(inner); formula.push_back(groupBit); newIntersection = 0; } } // parse remove/setpos/color/label/intensity parameters if(formula.size() > 0) { std::string actionName = actionNode.name(); if(actionName == "remove") { utils::SmartPtr removeActionPtr = new RemoveAction(formula); filterActions.push_back(removeActionPtr); } else if(actionName == "setpos") { pugi::xml_attribute xAttr = actionNode.attribute("x"); if(xAttr.empty()) throw utils::ParseException("Setpos has no x attribute."); pugi::xml_attribute yAttr = actionNode.attribute("y"); if(yAttr.empty()) throw utils::ParseException("Setpos has no y attribute."); pugi::xml_attribute zAttr = actionNode.attribute("z"); if(zAttr.empty()) throw utils::ParseException("Setpos has no z attribute."); utils::SmartPtr setposActionPtr = new SetposAction(formula, xAttr.as_float(), yAttr.as_float(), zAttr.as_float()); filterActions.push_back(setposActionPtr); } else if(actionName == "color") { pugi::xml_attribute rAttr = actionNode.attribute("r"); if(rAttr.empty()) throw utils::ParseException("Color has no r attribute."); pugi::xml_attribute gAttr = actionNode.attribute("g"); if(gAttr.empty()) throw utils::ParseException("Color has no g attribute."); pugi::xml_attribute bAttr = actionNode.attribute("b"); if(bAttr.empty()) throw utils::ParseException("Color has no b attribute."); pugi::xml_attribute aAttr = actionNode.attribute("a"); if(aAttr.empty()) throw utils::ParseException("Color has no a attribute."); utils::SmartPtr colorActionPtr = new ColorAction(formula, rAttr.as_float(), gAttr.as_float(), bAttr.as_float(), aAttr.as_float()); filterActions.push_back(colorActionPtr); } else if(actionName == "label") { pugi::xml_attribute valueAttr = actionNode.attribute("value"); if(valueAttr.empty()) throw utils::ParseException("Label has no value attribute."); utils::SmartPtr labelActionPtr = new LabelAction(formula, valueAttr.as_int()); filterActions.push_back(labelActionPtr); } else if(actionName == "intensity") { pugi::xml_attribute valueAttr = actionNode.attribute("value"); if(valueAttr.empty()) throw utils::ParseException("Intensity has no value attribute."); utils::SmartPtr intensityActionPtr = new IntensityAction(formula, valueAttr.as_float()); filterActions.push_back(intensityActionPtr); } else if(actionName == "filterdepthimg") { pugi::xml_attribute valueAttr = actionNode.attribute("value"); if(valueAttr.empty()) throw utils::ParseException("Filterdepthimg has no value attribute."); utils::SmartPtr filterDepthImgActionPtr = new FilterDepthImgAction(formula, valueAttr.as_float()); filterActions.push_back(filterDepthImgActionPtr); } } } } void Primitive3DFilter::RemoveAction::performAction(PointCloudObjectBase &pcObj, std::vector &actionMap, std::vector &groupMap, core::Img32f *depthImage) { int new_i = 0; std::vector oldActionMap(actionMap); bool removedColor = false; bool removedPos = false; bool changedMaps = false; #define ICL_IMPLEMENT_FUNCTION(T,N,TYPE,CATEGORY) \ if(pcObj.supports(PointCloudObjectBase::TYPE)) { \ if(!(std::strcmp(#CATEGORY,"COLOR") == 0 && removedColor) && !(std::strcmp(#CATEGORY,"POS") == 0 && removedPos)) { \ core::DataSegment segment = pcObj.select##TYPE(); \ new_i = 0; \ for(int i = 0; i < pcObj.getDim(); ++i) { \ segment[new_i] = segment[i]; \ if(!changedMaps) { \ actionMap[new_i] = actionMap[i]; \ groupMap[new_i] = groupMap[i]; \ } \ if(!oldActionMap[i]) { \ new_i++; \ } \ } \ } \ } \ if(std::strcmp(#CATEGORY,"COLOR") == 0) removedColor = true; \ if(std::strcmp(#CATEGORY,"POS") == 0) removedPos = true; \ changedMaps = true; ICL_IMPLEMENT_FUNCTION(float,4,RGBA32f,COLOR) ICL_IMPLEMENT_FUNCTION(icl8u,4,BGRA,COLOR) ICL_IMPLEMENT_FUNCTION(icl8u,3,BGR,COLOR) ICL_IMPLEMENT_FUNCTION(icl32s,1,BGRA32s,COLOR) ICL_IMPLEMENT_FUNCTION(float,4,XYZH,POS) ICL_IMPLEMENT_FUNCTION(float,3,XYZ,POS) ICL_IMPLEMENT_FUNCTION(float,4,Normal,OTHER) ICL_IMPLEMENT_FUNCTION(float,1,Intensity,OTHER) ICL_IMPLEMENT_FUNCTION(icl32s,1,Label,OTHER) #undef ICL_IMPLEMENT_FUNCTION actionMap.resize(new_i); groupMap.resize(new_i); pcObj.setDim(new_i); } void Primitive3DFilter::SetposAction::performAction(PointCloudObjectBase &pcObj, std::vector &actionMap, std::vector &groupMap, core::Img32f *depthImage) { if(pcObj.supports(PointCloudObjectBase::XYZ)) { math::FixedColVector actionPos(x, y, z); core::DataSegment posSegment = pcObj.selectXYZ(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { posSegment[i] = actionPos; } } } else if(pcObj.supports(PointCloudObjectBase::XYZH) || pcObj.canAddFeature(PointCloudObjectBase::XYZH)) { if(!pcObj.supports(PointCloudObjectBase::XYZH)) pcObj.addFeature(PointCloudObjectBase::XYZH); math::FixedColVector actionPos(x, y, z, 1); core::DataSegment posSegment = pcObj.selectXYZH(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { posSegment[i] = actionPos; } } } else { DEBUG_LOG("Filter error: Position feature is not supported and can not be added."); } } void Primitive3DFilter::ColorAction::performAction(PointCloudObjectBase &pcObj, std::vector &actionMap, std::vector &groupMap, core::Img32f *depthImage) { if(pcObj.supports(PointCloudObjectBase::BGRA)) { math::FixedColVector actionColor(b, g, r, a); core::DataSegment colorSegment = pcObj.selectBGRA(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { colorSegment[i] = actionColor; } } } else if(pcObj.supports(PointCloudObjectBase::BGR)) { math::FixedColVector actionColor(b, g, r); core::DataSegment colorSegment = pcObj.selectBGR(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { colorSegment[i] = actionColor; } } } else if(pcObj.supports(PointCloudObjectBase::BGRA32s)) { icl32s actionColor = ((unsigned char)b) || (((unsigned char)g) << 8) || (((unsigned char)r) << 16) || (((unsigned char)a) << 24); core::DataSegment colorSegment = pcObj.selectBGRA32s(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { colorSegment[i] = actionColor; } } } else if(pcObj.supports(PointCloudObjectBase::RGBA32f) || pcObj.canAddFeature(PointCloudObjectBase::RGBA32f)) { if(!pcObj.supports(PointCloudObjectBase::RGBA32f)) pcObj.addFeature(PointCloudObjectBase::RGBA32f); math::FixedColVector actionColor(r, g, b, a); core::DataSegment colorSegment = pcObj.selectRGBA32f(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { colorSegment[i] = actionColor; } } } else { DEBUG_LOG("Filter error: Color feature is not supported and can not be added."); } } void Primitive3DFilter::LabelAction::performAction(PointCloudObjectBase &pcObj, std::vector &actionMap, std::vector &groupMap, core::Img32f *depthImage) { if(pcObj.supports(PointCloudObjectBase::Label) || pcObj.canAddFeature(PointCloudObjectBase::Label)) { if(!pcObj.supports(PointCloudObjectBase::Label)) pcObj.addFeature(PointCloudObjectBase::Label); icl32s actionLabel = value; core::DataSegment labelSegment = pcObj.selectLabel(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { labelSegment[i] = actionLabel; } } } else { DEBUG_LOG("Filter error: Label feature is not supported and can not be added."); } } void Primitive3DFilter::IntensityAction::performAction(PointCloudObjectBase &pcObj, std::vector &actionMap, std::vector &groupMap, core::Img32f *depthImage) { if(pcObj.supports(PointCloudObjectBase::Intensity) || pcObj.canAddFeature(PointCloudObjectBase::Intensity)) { if(!pcObj.supports(PointCloudObjectBase::Intensity)) pcObj.addFeature(PointCloudObjectBase::Intensity); float actionIntensity = value; core::DataSegment intensitySegment = pcObj.selectIntensity(); for(int i = 0; i < pcObj.getDim(); ++i) { if(actionMap[i]) { intensitySegment[i] = actionIntensity; } } } else { DEBUG_LOG("Filter error: Intensity feature is not supported and can not be added."); } } void Primitive3DFilter::FilterDepthImgAction::performAction(PointCloudObjectBase &pcObj, std::vector &actionMap, std::vector &groupMap, core::Img32f *depthImage) { if(depthImage == 0) { DEBUG_LOG("Filter error: There is no depth image to be filtered."); return; } if(pcObj.getDim() != depthImage->getDim()) { DEBUG_LOG("Filter error: Dimensions of point cloud and depth image must be the same."); return; } core::Channel32f cDepth = (*depthImage)[0]; for(int i = 0; i < depthImage->getDim(); ++i) { if(actionMap[i]) { cDepth[i] = value; } } } void Primitive3DFilter::Primitive3D::toSceneObject(icl::geom::SceneObject *object, uint32_t slices, icl::geom::GeomColor const &color) { math::Mat4 mat = orientation.getTransformationMatrix(); mat(3,0) = position[0]; mat(3,1) = position[1]; mat(3,2) = position[2]; SceneObject *added_obj = 0; if (object) { switch (type) { case(CUBE): { added_obj = object->addCuboid(0,0,0, scale[0],scale[1],scale[2]); break; } case(SPHERE): { added_obj = object->addSpheroid(0,0,0, scale[0]/2.0,scale[1]/2.0,scale[2]/2.0, slices,slices); break; } case(CYLINDER): { added_obj = object->addCylinder(0,0,0, scale[0],scale[1],scale[2], slices); break; } default: { std::stringstream sstream; sstream << type; WARNING_LOG("No primitive type for id: "+sstream.str()); return;//break; } } } if(added_obj){ added_obj->setColor(Primitive::all,color); added_obj->setTransformation(mat); } } } // namespace geom }