WireRouting/WireRouting_DLL/Intersection.cpp

#include "pch.h" // use stdafx.h in Visual Studio 2017 and earlier
#include "Intersection.h"

TriangleSegmentIntersectRes::TriangleSegmentIntersectRes(bool h, float tt) :hit(h), t(tt) {}//VS2008


TriangleSegmentIntersectRes
triangleSegmentIntersection(const LineSegment &segment, const Vec3f &a, const Vec3f &b, const Vec3f &c) {
    Vec3f e1 = b - a;
    Vec3f e2 = c - a;
    Vec3f s = segment.start - a;
    Vec3f s1 = segment.getDir().cross(e2);
    Vec3f s2 = s.cross(e1);
    float s1_dot_e1 = s1.dot(e1);
    float b1 = s1.dot(s) / s1_dot_e1;
    float b2 = s2.dot(segment.getDir()) / s1_dot_e1;
    if (b1 >= 0. && b2 >= 0. && b1 + b2 <= 1.) {
		return TriangleSegmentIntersectRes(true, s2.dot(e2) / s1_dot_e1);
    }
	return TriangleSegmentIntersectRes(false, 0.f);
}

BVHNode::BVHNode(size_t l, size_t r, size_t p, const AABB& aabb) : left(l), right(r), parent(p), boundingBox(aabb) {}

BVHNode::BVHNode() : left(0), right(0), parent(0), boundingBox() {}


BVH_intersection::BVH_intersection(const Mesh &mesh_) : mesh(mesh_) {
	faceCenters.reserve(mesh.indices.size());
	for (vector<Vec3u>::const_iterator it = mesh.indices.begin();it!=mesh.indices.end();++it) {
		const Vec3u& face = *it;
		Vec3f center = (mesh.vertices[face[0]] + mesh.vertices[face[1]] + mesh.vertices[face[2]]) / 3.0f;
		faceCenters.push_back(center);
	}
	vector<size_t> indicesList(mesh.indices.size());
	for(size_t i=0;i<indicesList.size();++i){
		indicesList[i]=i;
	}
	try{
		nodes.resize(2 * mesh.indices.size() - 1);
	}catch(const length_error& e){
		cout<<"vector.length:"<<nodes.size()<<" resize_pa:"<<(2 * mesh.indices.size() - 1)<<endl;
		cerr<<"Caught a length_error: "<<e.what()<<endl;
		}
	size_t nowIdx = 0;
	dfsBuild(indicesList, computeAABB(indicesList), nowIdx);
}

size_t BVH_intersection::dfsBuild(vector<size_t> &indicesList, AABB aabb, size_t &nowIdx) {
	const size_t nodeIdx = nowIdx;
	nowIdx++;
	if (indicesList.size() == 1) {
		nodes[nodeIdx] = BVHNode(0, indicesList[0], 0, aabb);//VS2008
		return nodeIdx;
	}
	int longAxis = -1;
	float longAxisLen = -1;
	for (int i = 0; i < 3; ++i) {
		const float axisLen = aabb.max[i] - aabb.min[i];
		if (axisLen > longAxisLen) {
			longAxisLen = axisLen;
			longAxis = i;
		}
	}
	const size_t k = indicesList.size() / 2;
	nth(indicesList, k - 1, longAxis);
	vector<size_t> leftIndices(indicesList.begin(), indicesList.begin() + k);
	vector<size_t> rightIndices(indicesList.begin() + k, indicesList.end());
	const AABB leftAABB = computeAABB(leftIndices);
	const AABB rightAABB = computeAABB(rightIndices);
	const size_t leftIdx = dfsBuild(leftIndices, leftAABB, nowIdx);
	const size_t rightIdx = dfsBuild(rightIndices, rightAABB, nowIdx);
	nodes[nodeIdx] = BVHNode(leftIdx, rightIdx, 0, aabb);//VS2008
	nodes[leftIdx].parent = nodeIdx;
	nodes[rightIdx].parent = nodeIdx;
	return nodeIdx;
}

bool BVH_intersection::intersectWithLineSegment(const LineSegment &lineSegment) const {
	return recursiveLineSegIntersection(lineSegment, 0);
}

AABB BVH_intersection::computeAABB(const vector<size_t> &indices) {
	AABB aabb;
	for (vector<size_t>::const_iterator it = indices.begin();it!=indices.end();++it) {
		const Vec3u &face = mesh.indices[*it];
		for (int i = 0; i < 3; ++i) {
			const Vec3f &vertex = mesh.vertices[face[i]];
			for (int j = 0; j < 3; ++j) {
				aabb.min[j] = min(aabb.min[j], vertex[j]);
				aabb.max[j] = max(aabb.max[j], vertex[j]);
			}
		}
	}
	return aabb;
}

bool BVH_intersection::recursiveLineSegIntersection(const LineSegment &lineSegment, size_t nodeIdx) const {
	const AABB &aabb = nodes[nodeIdx].boundingBox;
	const Vec3f &dir = lineSegment.getDir();
	bool hit = false;
	for (int i = 0; !hit && i < 3; ++i) {
		float t_min = (aabb.min[i] - lineSegment.start[i]) / dir[i];
		float t_max = (aabb.max[i] - lineSegment.start[i]) / dir[i];
		if (t_min > t_max) {
			swap(t_min, t_max);
		}
		if (t_min > lineSegment.getLength() || t_max < 0) return false;
		Vec3f hitPt = lineSegment.start + dir * t_min;
		int otherPlane1 = (i + 1) % 3, otherPlane2 = (i + 2) % 3;
		if (hitPt[otherPlane1] >= aabb.min[otherPlane1] && hitPt[otherPlane1] <= aabb.max[otherPlane1] &&
			hitPt[otherPlane2] >= aabb.min[otherPlane2] && hitPt[otherPlane2] <= aabb.max[otherPlane2]) {
				hit = true;
		}
	}
	if (hit) {
		if (nodes[nodeIdx].left == 0) {
			Vec3u face = mesh.indices[nodes[nodeIdx].right];
			TriangleSegmentIntersectRes res = triangleSegmentIntersection(lineSegment, mesh.vertices[face[0]], mesh.vertices[face[1]],
				mesh.vertices[face[2]]);
			if (!res.hit || res.t < 0 || res.t > lineSegment.getLength()) return false;
			return true;
		} else {
			return recursiveLineSegIntersection(lineSegment, nodes[nodeIdx].left) ||
				recursiveLineSegIntersection(lineSegment, nodes[nodeIdx].right);
		}
	}
	return false;
}

void BVH_intersection::nth(vector<size_t> &indicesList, size_t kth, int longAxis) {
	recursiveChoose(indicesList, 0, indicesList.size() - 1, kth, longAxis);
}

void BVH_intersection::recursiveChoose(vector<size_t> &indicesList, size_t begin, size_t end, size_t kth, int longAxis) {
	if (begin >= end) return;
	int i = begin, j = end;
	while (i < j) {
		while (i < j && faceCenters[indicesList[j]][longAxis] >= faceCenters[indicesList[begin]][longAxis]) j--;
		while (i < j && faceCenters[indicesList[i]][longAxis] <= faceCenters[indicesList[begin]][longAxis]) i++;
		swap(indicesList[i], indicesList[j]);
	}
	swap(indicesList[begin], indicesList[i]);
	if (i == kth) return;
	if (i < kth) recursiveChoose(indicesList, i + 1, end, kth, longAxis);
	else recursiveChoose(indicesList, begin, i - 1, kth, longAxis);
}