bool BVHTree::intersectHelp(shared_ptr<Object> avoid, const Vector3f& pos, const Vector3f& ray, BVHNode *node, HitRecord *hitRecord)
{
if (node == NULL) { return false; }
if ((node->bb)->intersection(pos, ray)) {
// Check if leaf
if (node->left == NULL && node->right == NULL) {
shared_ptr<Object> object = node->bb->getObject();
if (object != avoid) {
Matrix4f inv = object->getInvXForm();
Vector4f modelPos = inv * Vector4f(pos(0), pos(1), pos(2), 1);
Vector4f modelRay = inv * Vector4f(ray(0), ray(1), ray(2), 0);
float t = object->intersection(modelPos.head(3), modelRay.head(3));
if (t >= 0) {
*hitRecord = HitRecord(t, object);
return HIT;
}
}
return !HIT;
} else {
// Either a node is a leaf, or it has two children
HitRecord lHR, rHR;
bool lHit, rHit;
lHit = intersectHelp(avoid, pos, ray, node->left, &lHR);
rHit = intersectHelp(avoid, pos, ray, node->right, &rHR);
if (!lHit && !rHit) {
return !HIT;
} else if (!lHit) {
*hitRecord = rHR;
} else if (!rHit) {
*hitRecord = lHR;
} else {
*hitRecord = lHR.getT() < rHR.getT() ? lHR : rHR;
}
return HIT;
}
}
return !HIT;
}
HitRecord Scene::intersections(shared_ptr<Object> avoid, const Vector3f& p0, const Vector3f& d)
{
HitRecord hrTree;
bool hitTree;
// Check tree
hitTree = bvhTree.intersection(avoid, p0, d, &hrTree);
// Check planes
float best = numeric_limits<float>::infinity();
float t;
shared_ptr<Object> object;
for (unsigned int i = 0; i < planes.size(); i++) {
Vector4f pXForm = Vector4f(p0(0), p0(1), p0(2), 1);
Vector4f dXForm = Vector4f(d(0), d(1), d(2), 0);
Matrix4f inv = planes[i]->getInvXForm();
Vector4f modelpos = inv * pXForm;
Vector4f modelray = inv * dXForm;
float t = planes[i]->intersection(modelpos.head(3), modelray.head(3));
if ( t >= 0 && t < best && planes[i] != avoid ) {
best = t;
object = planes[i];
}
}
if (!hitTree && best < 0) {
return HitRecord(-1.0, NULL);
} else {
if (!hitTree) { return HitRecord(best, object); }
if (best < 0) { return hrTree; }
return hrTree.getT() < best ? hrTree : HitRecord(best, object);
}
}
VoxRaytracer::RayData VoxRaytracer::TraceBetweenPoints(const Vector4f &in,
const Vector4f &out)
const
{
RayData ray;
Vector4f pt1 = m_Image->GetLocalPoint(in);
Vector4f pt2 = m_Image->GetLocalPoint(out);
Vector4f s = pt2 - pt1;
Vector3f scale; // FIXXX
scale << (m_Image->GetWorldMatrix() * Vector4f(1,0,0,0)).norm(),
(m_Image->GetWorldMatrix() * Vector4f(0,1,0,0)).norm(),
(m_Image->GetWorldMatrix() * Vector4f(0,0,1,0)).norm();
float l = s.head(3).norm();
Vector3f L(l/s(0), l/s(1), l/s(2));
Vector3f offset;
for(int i=0;i<3;++i)
offset(i) = (s(i)>=0) - (pt1(i)-floor(pt1(i))) ;
offset = offset.cwiseProduct(L).cwiseAbs();
L = L.cwiseAbs();
int id; float d;
Vector3i vid = m_Image->Find(in);
while(l>0)
{
d = offset.minCoeff(&id);
if(m_Image->IsInsideGrid(vid))
ray.AddElement(m_Image->Map(vid), d * scale(id) );
// nan check //
// if(unlikely(!isFinite(d * scale(id)))) {
// std:: cout << "NAN in raytracer\n";
// exit(1);
// }
vid(id) += (int)fast_sign(s(id));
l -= d;
offset.array() -= d;
offset(id) = fmin(L(id),l);
}
return ray;
}
bool VoxRaytracer::GetEntryPoint(const HLine3f line, Vector4f &pt) const
{
Vector4f s = m_Image->GetLocalPoint(line.direction());
pt = m_Image->GetLocalPoint(line.origin());
// Considers Structured grid dimensions //
Vector4f dims = m_Image->GetDims().homogeneous().cast<float>();
pt = pt.cwiseQuotient(dims);
s = s.cwiseQuotient(dims);
float l = s.head(3).norm();
Vector3f L(l/s(0), l/s(1), l/s(2));
Vector3f offset;
for(int i=0;i<3;++i)
offset(i) = (s(i)>0) - (pt(i)-floor(pt(i))) ;
offset = offset.cwiseProduct(L).cwiseAbs();
int id; float d;
for(int loop=0; loop<8; loop++)
{
int check_border = 0;
for ( int i=0; i<3 ;++i) {
check_border += pt(i) > 1;
check_border += pt(i) < 0;
}
if(check_border == 0) {
for(int i=0;i<3;++i)
pt(i) *= (float)dims(i);
pt = m_Image->GetWorldPoint(pt);
return true;
}
d = offset.minCoeff(&id);
for(int i=0; i<3; ++i)
pt(i) += d / L(i);
pt(id) = rintf(pt(id));
offset.array() -= d;
offset(id) = fabs(L(id));
}
for(int i=0;i<3;++i)
pt(i) *= (float)dims(i);
pt = m_Image->GetWorldPoint(pt);
return false;
}
Vector3f Triangle::getNormal(const Vector3f& hitPoint)
{
Vector4f n = invXForm.transpose() * Vector4f(normal(X), normal(Y), normal(Z), 0);
return n.head(3);
}
