Color shadeWithMaterial(struct Scene* scene, struct HitRecord* record, Ray ray, int depth) {
Color result = {0,0,0};
Material material = *record->triangle->material;
struct HitRecord temp_record = createHitRecord();
Vector position = record->point;
Vector new_direction;
if(depth == scene->max_depth) {
return result;
}
if(material.is_light) {
return material.color;
}
Vector normal = record->triangle->normal;
float costheta = dotVector(normal, ray.direction);
if(costheta > 0) {
normal = negateVector(normal);
}
float path = (double)rand()/(double)RAND_MAX;
// diffuse BRDF
if(path < .33) {
new_direction = getDiffuseDirection(normal);
}
// BRDF (which is also diffuse)
else if(path < .66) {
new_direction = getDiffuseDirection(normal);
}
// point to light
else {
new_direction = getLightDirection(scene, position);
}
Ray new_ray = {position, new_direction};
resetHitRecord(&temp_record);
float cosphi = dotVector(normal, new_direction);
if(cosphi > 0) {
result = addColors(result, multiplyColorByNumber(hitScene(scene, &temp_record, new_ray, depth + 1), cosphi));
}
return multiplyColors(result, material.color);
}
bool SemanticLabelsVisualization::visualize(const grid_map::GridMap& mapMsg)
{
if (!isActive()) return true;
grid_map::GridMap map = mapMsg;
if (!map.exists(elevationLayer_)) {
ROS_WARN_STREAM("SemanticLabelsVisualization::visualize: No grid map layer with name '" << elevationLayer_ << "' found.");
return false;
}
if (!map.exists(labelLayer_)) {
ROS_WARN_STREAM("SemanticLabelsVisualization::visualize: No grid map layer with name '" << labelLayer_ << "' found.");
return false;
}
float path_color;
grid_map::colorVectorToValue(Eigen::Vector3i(255, 0, 0), path_color);
for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
{
// skip points without elevation (if set)
if (!map.isValid(*iterator, elevationLayer_) && onlyShowElevation_)
{
continue;
}
auto& colorVal = map.at("color", *iterator);
Eigen::Vector3i rgbVec;
if (!map.isValid(*iterator, elevationLayer_))
{
rgbVec = Eigen::Vector3i(255, 255, 255);
}
else
{
rgbVec = colorVectorFromFloat(colorVal);
}
if(map.isValid(*iterator, labelLayer_))
{
auto labelIndex = static_cast<int>(map.at(labelLayer_, *iterator));
if(labelIndex >= 0 && labelIndex < labelColors_.size())
{
multiplyColors(rgbVec, labelColors_.at(labelIndex));
grid_map::colorVectorToValue(rgbVec, colorVal);
}
}
if (!map.isValid(*iterator, elevationLayer_))
{
map.at(elevationLayer_, *iterator) = 0.0;
}
// add path in red above terrain
if (map.isValid(*iterator, "path"))
{
map.at("color", *iterator) = path_color;
}
}
sensor_msgs::PointCloud2 pointCloud;
grid_map::GridMapRosConverter::toPointCloud(map, elevationLayer_, pointCloud);
publisher_.publish(pointCloud);
return true;
}
