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