function [VMap,NMap,tMap,CMap] = raycastingTSDF(camRtC2W, castingRange)
% VMap and NMap are in the world coordinate
% DDA based ray casting
global raycastingDirectionC;
global voxel;
global tsdf_value;
global tsdf_color;
camCenterW = getCameraCenter(camRtC2W);
raycastingDirectionW = transformRTdir(raycastingDirectionC,camRtC2W);
camCenterWgrid = (camCenterW - voxel.range(:,1)) / voxel.unit + 1;
%camCenterWcopy = repmat( (camCenterW - voxel.range(:,1) ) / voxel.unit + 1,1,640*480);
%startPoints = camCenterWcopy + raycastingDirectionW * (castingRange(1)/ voxel.unit);
%endPoints = camCenterWcopy + raycastingDirectionW * (castingRange(2)/ voxel.unit);
% http://stellar.mit.edu/S/course/6/fa10/6.837/courseMaterial/topics/topic1/lectureNotes/13_RayTracing-Acceleration/13_RayTracing-Acceleration.pdf
% we assume the camera must be inside. otherwise, it is an error.
% so we don't test the camera
raycastingDirectionWinv = raycastingDirectionW.^-1;
maxTx = max((voxel.size_grid(1)-2-camCenterWgrid(1))*raycastingDirectionWinv(1,:),(2-camCenterWgrid(1))*raycastingDirectionWinv(1,:));
maxTy = max((voxel.size_grid(2)-2-camCenterWgrid(2))*raycastingDirectionWinv(2,:),(2-camCenterWgrid(2))*raycastingDirectionWinv(2,:));
maxTz = max((voxel.size_grid(3)-2-camCenterWgrid(3))*raycastingDirectionWinv(3,:),(2-camCenterWgrid(3))*raycastingDirectionWinv(3,:));
maxT = min(maxTx, min(maxTy, maxTz));
castingRangeGrid = castingRange/voxel.unit;
maxT = min(maxT, castingRangeGrid(2));
% parallel setting
parallelBlock = matlabpool('size');
blockDim = (640*480)/parallelBlock;
if blockDim ~= round(blockDim)
error('thread does not align');
end
backMove = voxel.mu_grid;
tMap = NaN(1,640*480);
NMap = NaN(3,640*480);
CMap = NaN(3,640*480);
initDis = 3/ voxel.unit;
% http://www.cse.yorku.ca/~amana/research/grid.pdf
prevT = initDis;
for i=1:(640*480)
% Ray-Box Intersection to get the range
tMin = castingRangeGrid(1);
tMax = maxT(i);
if tMax>tMin % max range > min range
tStart = max(castingRangeGrid(1),min(tMax,prevT-backMove));
rayDir = raycastingDirectionW(:,i);
startPoint = camCenterWgrid + rayDir*tStart;
% The initialization phase begins by identifying the voxel in which the ray origin, ?u, is found.
% If the ray origin is outside the grid, we ?nd the point in which the ray enters the grid and take the adjacent voxel.
% The integer variables X and Y are initialized to the starting voxel coordinates.
X = round(startPoint(1));
Y = round(startPoint(2));
Z = round(startPoint(3));
valCurrentVoxel = tsdf_value(X,Y,Z);
tOptimal = NaN;
if valCurrentVoxel == 0
% lucky, you got the surface immediately!
%tMap(i) = tStart;
prevT = tStart;
tOptimal = tStart;
else
if valCurrentVoxel>0
localDir = rayDir;
tMax = tMax-tStart;
lookforSign = -1;
else
localDir = -rayDir;
tMax = tStart-tMin;
lookforSign = +1;
end
% In addition, the variables stepX and stepY are initialized to either 1 or -1 indicating
% whether X and Y are incremented or decremented as the ray crosses voxel boundaries
% (this is determined by the sign of the x and y components of ?v).
stepX = sign(localDir(1));
stepY = sign(localDir(2));
stepZ = sign(localDir(3));
% Next, we determine the value of t at which the ray crosses the ?rst vertical voxel boundary
% and store it in variable tMaxX. We perform a similar computation in y and store the result in tMaxY.
% The minimum of these two values will indicate how much we can travel along the ray and still remain in the current voxel.
if localDir(1)>0
tMaxX = (X+0.5 - startPoint(1))/localDir(1);
elseif localDir(1)<0
tMaxX = (X-0.5 - startPoint(1))/localDir(1);
else
tMaxX = Inf;
end
if localDir(2)>0
tMaxY = (Y+0.5 - startPoint(2))/localDir(2);
elseif localDir(2)<0
tMaxY = (Y-0.5 - startPoint(2))/localDir(2);
else
tMaxY = Inf;
end
if localDir(3)>0
tMaxZ = (Z+0.5 - startPoint(3))/localDir(3);
elseif localDir(3)<0
tMaxZ = (Z-0.5 - startPoint(3))/localDir(3);
else
tMaxZ = Inf;
end
if min(min(tMaxX,tMaxY),tMaxZ)<0
error('tStart<0');
end
% Finally, we compute tDeltaX and tDeltaY.
% tDeltaX indicates how far along the ray we must move (in units of t) for the horizontal component of such a movement to equal the width of a voxel.
% Similarly,we store in tDeltaY the amount of movement along the ray which has a vertical component equal to the height of a voxel.
tDeltaX = 1/abs(localDir(1));
tDeltaY = 1/abs(localDir(2));
tDeltaZ = 1/abs(localDir(3));
t = 0;
while t<tMax
if tMaxX < tMaxY
if tMaxX < tMaxZ
X= X + stepX;
tMaxX= tMaxX + tDeltaX;
else
Z= Z + stepZ;
tMaxZ= tMaxZ + tDeltaZ;
end
else
if tMaxY < tMaxZ
Y= Y + stepY;
tMaxY= tMaxY + tDeltaY;
else
Z= Z + stepZ;
tMaxZ= tMaxZ + tDeltaZ;
end
end
prevT = t;
t = min(min(tMaxX,tMaxY),tMaxZ);
if tsdf_value(X,Y,Z) * lookforSign > 0
if (tsdf_value(X,Y,Z)==-1 && valCurrentVoxel==+1) || (tsdf_value(X,Y,Z)==+1 && valCurrentVoxel==-1)
tOptimal = NaN;
else
% found the zero crossing points!
% simplest
% tOptimal = t;
% simple average
tOptimal = (prevT + t)/2;
%{
% buggy: Ftdt == Ft
if lookforSign>0
prevTswap = prevT;
prevT = t;
t= prevTswap;
end
XYZt = startPoint + localDir*t;
XYZtdt = startPoint + localDir*prevT;
Ft = interpolateTrilineary(XYZt(1),XYZt(2),XYZt(3));
Ftdt = interpolateTrilineary(XYZtdt(1),XYZtdt(2),XYZtdt(3));
tOptimal = t - abs(t-prevT) * Ft / (Ftdt - Ft);
if tOptimal>1000
error('tOptimal>1000');
end
%}
%fprintf('i=%d: t=%f tOptimal=%f Ft=%f Ftdt=%f\n',i,t,t - abs(t-prevT) * Ft / (Ftdt - Ft), Ft, Ftdt);
tOptimal = tStart - tOptimal * lookforSign;
prevT = tOptimal;
end
break;
end
valCurrentVoxel = tsdf_value(X,Y,Z);
end
end
if ~isnan(tOptimal)
tMap(i) = tOptimal;
% compute normal map
XYZgrid = camCenterWgrid + rayDir*tOptimal;
NMap(1,i) = interpolateTrilineary(XYZgrid(1)+1,XYZgrid(2),XYZgrid(3))-interpolateTrilineary(XYZgrid(1)-1,XYZgrid(2),XYZgrid(3));
NMap(2,i) = interpolateTrilineary(XYZgrid(1),XYZgrid(2)+1,XYZgrid(3))-interpolateTrilineary(XYZgrid(1),XYZgrid(2)-1,XYZgrid(3));
NMap(3,i) = interpolateTrilineary(XYZgrid(1),XYZgrid(2),XYZgrid(3)+1)-interpolateTrilineary(XYZgrid(1),XYZgrid(2),XYZgrid(3)-1);
if ~isempty(tsdf_color)
CMap(:,i) = interpolateTrilinearyColor(XYZgrid(1),XYZgrid(2),XYZgrid(3));
end
end
end
end
% normalize normal map
NMap = NMap ./ repmat(sqrt(sum(NMap.^2,1)),3,1);
% computer vertex map
VMap = repmat(camCenterW,1,640*480) + raycastingDirectionW .* (repmat(tMap*voxel.unit,3,1));
%imagesc(reshape(tMap,480,640)); axis equal; axis tight