bool Octree::Node::split() { if (is_leaf && hasGeometry()) { carve::geom3d::Vector mid = 0.5 * (min + max); char *ptr = new char[sizeof(Node)*8]; children[0] = new (ptr + sizeof(Node) * 0) Node(this, min.x, min.y, min.z, mid.x, mid.y, mid.z); children[1] = new (ptr + sizeof(Node) * 1) Node(this, mid.x, min.y, min.z, max.x, mid.y, mid.z); children[2] = new (ptr + sizeof(Node) * 2) Node(this, min.x, mid.y, min.z, mid.x, max.y, mid.z); children[3] = new (ptr + sizeof(Node) * 3) Node(this, mid.x, mid.y, min.z, max.x, max.y, mid.z); children[4] = new (ptr + sizeof(Node) * 4) Node(this, min.x, min.y, mid.z, mid.x, mid.y, max.z); children[5] = new (ptr + sizeof(Node) * 5) Node(this, mid.x, min.y, mid.z, max.x, mid.y, max.z); children[6] = new (ptr + sizeof(Node) * 6) Node(this, min.x, mid.y, mid.z, mid.x, max.y, max.z); children[7] = new (ptr + sizeof(Node) * 7) Node(this, mid.x, mid.y, mid.z, max.x, max.y, max.z); for (int i = 0; i < 8; ++i) { putInside(faces, children[i], children[i]->faces); putInside(edges, children[i], children[i]->edges); putInside(vertices, children[i], children[i]->vertices); } faces.clear(); edges.clear(); vertices.clear(); is_leaf = false; } return is_leaf; }
void Element::getTerrainContactPoints(Eigen::Matrix3Xd& local_points) const { if (!hasGeometry()) { local_points = Eigen::Matrix3Xd(); return; } Eigen::Matrix3Xd points; geometry->getTerrainContactPoints(points); local_points = T_element_to_local * points; }
void Element::getTerrainContactPoints(Eigen::Matrix3Xd &local_points) { if ( !hasGeometry() ) { local_points = Eigen::Matrix3Xd(); return; } Eigen::Matrix3Xd points; geometry->getTerrainContactPoints(points); Eigen::Matrix4Xd transformed_points = T_element_to_local * points.colwise().homogeneous(); local_points = transformed_points.colwise().hnormalized(); }
void CVK::Node::render( glm::mat4 modelMatrix) { // accumulate model matrix modelMatrix = modelMatrix * *getModelMatrix(); // update shader CVK::State::getInstance()->getShader()->update(this); CVK::State::getInstance()->getShader()->updateModelMatrix(modelMatrix); // render geometry if ( hasGeometry()) m_geometry->render(); // render childs for(unsigned int i=0; i<m_childs.size(); i++) { m_childs[i]->render( modelMatrix); } }
RobotLink::RobotLink(Robot* robot, const urdf::LinkConstPtr& link, const std::string& parent_joint_name, bool visual, bool collision, bool use_model_path, MetaPointCloud& link_pointclouds) : robot_( robot ) , name_( link->name ) , parent_joint_name_( parent_joint_name ) , visual_node_( NULL ) , collision_node_( NULL ) , link_pointclouds_(link_pointclouds) , pose_calculated_(false) , use_model_path_(use_model_path) { pose_property_ = KDL::Frame(); visual_node_ = new node; collision_node_ = new node; // we prefer collisions over visuals if (collision) createCollision(link); else if (visual) createVisual(link); // create description and fill in child_joint_names_ vector std::stringstream desc; if (parent_joint_name_.empty()) { desc << "Root Link " << name_; } else { desc << "Link " << name_; desc << " with parent joint " << parent_joint_name_; } if (link->child_joints.empty()) { desc << " has no children."; } else { desc << " has " << link->child_joints.size(); if (link->child_joints.size() > 1) { desc << " child joints: "; } else { desc << " child joint: "; } std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_it = link->child_joints.begin(); std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_end = link->child_joints.end(); for ( ; child_it != child_end ; ++child_it ) { urdf::Joint *child_joint = child_it->get(); if (child_joint && !child_joint->name.empty()) { child_joint_names_.push_back(child_joint->name); desc << child_joint->name << ((child_it+1 == child_end) ? "." : ", "); } } } if (hasGeometry()) { if (visual_meshes_.empty()) { desc << " This link has collision geometry but no visible geometry."; } else if (collision_meshes_.empty()) { desc << " This link has visible geometry but no collision geometry."; } } else { desc << " This link has NO geometry."; } LOGGING_DEBUG_C(RobotLog, RobotLink, desc << endl); }