void track(){
//pcl::PointCloud<PointT>::Ptr cloudWithNormals = addNormalsToPointCloud(cloud_input);
tracker_->setInputCloud(cloud);
tracker_->compute();
pcl::tracking::ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
//transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
Eigen::Affine3f transs = tracker_->getTrans();
pcl::transformPointCloud<PointT>(*(tracker_->getReferenceCloud ()),*tracked_object,transformation);
particles = tracker_->getParticles ();
Eigen::Vector4f c;
pcl::compute3DCentroid<PointT>(*tracked_object,c);
tracked_object_centroid->clear();
PointT pt;
pt.x = c[0]; pt.y = c[1]; pt.z = c[2];
tracked_object_centroid->push_back(pt);
retrieved_object = findNearestObject();
}
Color _renderPixel(const Scene &scene, const RenderParams ¶ms, const Ray &ray, ObjectId prevObjectID, int depth, float rIndex) {
ObjectId objectID;
Vec3 pos, norm;
Material *m;
bool isInside = false;
if (!findNearestObject(scene, params, ray, prevObjectID, false, objectID, pos, norm, &m, isInside)) {
return scene.bgColor;
}
Color texture = { 1.0, 1.0, 1.0 };
if (m->texFunc && objectID.type == TRIANGLE) {
const Triangle &obj = scene.triangles[objectID.index];
Vec3 f1 = obj.vertex[0] - pos;
Vec3 f2 = obj.vertex[1] - pos;
Vec3 f3 = obj.vertex[2] - pos;
float a = glm::length(glm::cross(obj.vertex[0] - obj.vertex[1], obj.vertex[1] - obj.vertex[2]));
float a1 = glm::length(glm::cross(f2, f3)) / a;
float a2 = glm::length(glm::cross(f3, f1)) / a;
float a3 = glm::length(glm::cross(f1, f2)) / a;
texture = m->texFunc(obj.texCoord[0] * a1 + obj.texCoord[1] * a2 + obj.texCoord[2] * a3);
}
Color c = scene.bgColor * m->ambientFactor * texture;
Vec3 reflectionDir = glm::normalize(glm::reflect(ray.dir, norm));
for (const Light &light : scene.lights) {
Vec3 lightDir;
if (light.type == LT_POINT) {
lightDir = glm::normalize(light.position - pos);
}
else if (light.type == LT_DIRECTIONAL) {
lightDir = -light.position;
}
else if (light.type == LT_SPOT) {
lightDir = glm::normalize(light.position - pos);
float p = glm::dot(-lightDir, light.spotDir);
if (p < light.spotCutoff)
continue;
}
float s = glm::dot(norm, lightDir);
if (s > 0.0f && !isShaded(scene, params, { pos, lightDir }, objectID)) {
Color diffuse(s * light.intensity * texture);
c += diffuse * light.color * m->diffuseFactor;
}
float t = glm::dot(lightDir, reflectionDir);
if (t > 0.0f && !isShaded(scene, params, { pos, reflectionDir }, objectID)) {
Color specular = Color(powf(t, m->shininess) * light.intensity);
c += specular * light.color * m->specularFactor;
}
}
if (depth < params.depthLimit) {
c += _renderPixel(scene, params, { pos, reflectionDir }, objectID, depth + 1, rIndex) * m->reflectionFactor;
if (m->refract) {
float n = rIndex / m->refraction;
Vec3 N = norm;
if (isInside)
N *= -1;
float cosI = glm::dot(N, ray.dir);
float cosT2 = 1.0f - n * n * (1.0f - cosI * cosI);
Color r;
if (cosT2 > 0.0f) {
Vec3 refractionDir = n * ray.dir + (n * cosI - sqrtf(cosT2)) * N;
// For refraction we don't exclude current object
r = _renderPixel(scene, params, { pos + refractionDir * 1e-5f, refractionDir }, {}, depth + 1, m->refraction) * m->refractionFactor;
}
c = c * (1 - m->refractionFactor) + r;
}
}
return glm::clamp(c, 0.0f, 1.0f);
}