Efficient Collision Detection for Animation and Robotics Part 12 pps

then RETURNNBR else RETURNfind-closestP, F.cobnd else RETURNNBR %% This procedure returns TRUE if Ea lies within the %% prismatic region swept out by Fb along its face normal %% directio

then RETURN(NBR) else RETURN(find-closest(P, F.cobnd)) else RETURN(NBR)

%% This procedure returns TRUE if Ea lies within the

%% prismatic region swept out by Fb along its face normal

%% direction, FALSE otherwise

PROCEDURE E-FPrism(E, F)

min = 0

max = length(Ea)

for (cell = Fb.cell till cell=NULL; cell = cell.next)

norm = vdot(Ea.vector,cell.cplane)

%% Ea points inward of the hyperplane

if (norm > 0)

%% compute the relative inclusion factor

then K = vdot(Ea.H, cell.cplane) / norm

if (K < max) { then max = K

if (min > max) RETURN(FALSE) }

%% Ea points outward from the hyperplane

else if (norm < 0)

%% compute the relative inclusion factor then K = vdot(Ea.T, cell.cplane) / norm

if (K > min) { min = K

if (max < min) RETURN(FALSE)}

%% norm = 0 if the edge Ea and Ei are parallel else if vdot(Ea.H, cell.cplane) < 0 RETURN(FALSE) RETURN(TRUE)

%%% Vertex-Vertex case:

PROCEDURE vertex-vertex (Va, Vb)

NBRb = point-cell-checkp (Va, Vb.cell)

if (NBRb = NULL)

then NBRa = point-cell-checkp (Vb, Va.cell))

if (NBRa = NULL)

then RETURN (Va, Vb, dist(Va,Vb)) else close-feat-checkp (NBRa, Vb) else close-feat-checkp (Va, NBRb)

%%% Vertex-Edge case:

PROCEDURE vertex-edge (Va, Eb)

NBRb = point-cell-checkp(Va, Eb.cell)

if (NBRb = NULL)

then NBRa = point-cell-checkp(nearest-pt(Va, Eb), Va.cell)

if (NBRa = NULL)

then RETURN (Va, Eb, dist(Va,Eb)) else close-feat-checkp (NBRa, Eb) else close-feat-checkp (Va, NBRb)

%%% Vertex-Face case:

PROCEDURE vertex-face (Va, Fb)

NBRb = point-face-checkp (Va,Fb)

if (NBRb = NULL)

then NBRa = point-cell-checkp (nearest-pt(Va, Fb), Va.cell)

if (NBRa = NULL)

then RETURN (Va, Fb, dist(Va,Fb)) else close-feat-checkp (NBRa, Fb) else close-feat-checkp (Va, NBRb)

%%% Edge-Edge case:

PROCEDURE edge-edge (Ea, Eb)

NBRb = point-cell-checkp (nearest-pt(Eb, Ea), Eb.cell)

if (NBRb = NULL)

then NBRa = point-cell-checkp (nearest-pt(Ea, Eb), Ea.cell))

if (NBRa = NULL)

then RETURN (Ea, Eb, dist(Ea,Eb)) else close-feat-checkp (NBRa, Eb) else close-feat-checkp (Ea, NBRb)

%%% Edge-Face case:

PROCEDURE edge-face (Ea Fb)

if (vdot(Ea.H, Fb.norm) = vdot(Ea.T, Fb.norm))

then if (E-FPrism(Ea, Fb.cell) # NULL)

then if (vdot(Ea.H, Fb.norm) > 0)

then cp_right = triple(E.fright.norm, Ea.vector, Fb.norm) cp_left = triple(E.vector, F.fleft.norm, Fb.norm)

if (cp_right >= 0) then if (cp_left >= 0) then RETURN (Ea, Fb, dist(Ea,Fb)) else close-feat-checkp (Ea.fleft, Fb) else close-feat-checkp (Ea.fright, Fb) else close-feat-checkp (Ea, find-closest(Ea, Fb.cobnd)) else close-feat-checkp (Ea, closestToE(Ea, Fb))

else if (sign(vdot(Ea.H, Fb.norm)) # sign(vdot(Ea.T, Fb.norm)))

then close-feat-checkp (Ea, closestToE(Ea, Fb)) else if (dist(Ea.H, Fb) < dist(Ea.T, Fb))

%% dist returns unsigned magnitude

then sub-edge-face(Ea.H, Ea, Fb) else sub-edge-face(Ea.T, Ea, Fb)

%%% Sub-Edge-Face case:

PROCEDURE sub-edge-face (Ve, E, F)

NBRb = point-cell-checkp (Ve, F.cell)

if (NBRb = NULL)

then if (vdot(Ve, F.norm) > 0)

then NBRa = point-cell-checkp (nearest-pt(Ve, F), Ve.cell)

if (NBRa = NULL) then RETURN (Ve, F, dist(Ve,F)) else close-feat-checkp (NBRa, F) else close-feat-checkp (Ve, find-closest(Ve, F.cobnd)) else close-feat-checkp (E, closestToE(E, F))

%%% Face-Face-case:

PROCEDURE face-face (Fa Fb)

if (abs(vdot(Fa.norm, Fb.norm)) = 1) %% check if Fa and Fb parallel

then if (overlap(Fa, Fb))

then if (vdot(Va, Fb.norm) > 0)

then if (vdot(Vb, Fa.norm) > 0)

then RETURN (Fa, Fb, dist(Fa,Fb))

else close-feat-checkp(find-closest(Fb, Fa.cobnd),Fb) else close-feat-checkp (Fa, find-closest(Fa, Fb.cobnd)) else close-feat-checkp ((closest-edges(Fa, Fb))

else NBRa = closestToFplane(Fb, Fa)

if (type(NBRa) = VERTEX) %% check if NBRa is a vertex

then NBRb = point-face-checkp (NBRa, Fb)

if (NBRb = NULL) then close-feat-checkp (NBRa, Fb) else sub-face-face(Fa, Fb)

else if (E-FPrism(NBRa, Fb.cell))

then if (vdot(NBRa.H, Fb.norm) > 0)

then close-feat-checkp (NBRa, Fb) else close-feat-checkp

(NBRa, find-closest(NBRa, Fb.cobnd)) else sub-face-face(Fa, Fb)

%%% Sub-Face-Face:

PROCEDURE sub-face-face(Fa, Fb)

NBRb = closestToFplane(Fa, Fb)

if (type(NBRb) = VERTEX) %% Is NBRb a vertex?

then NBRa = point-face-checkp (NBRb, Fa)

if (NBRa = NULL)

then close-feat-checkp (Fa, NBRb) else close-feat-checkp (closest-edges(Fa,Fb)) else if (E-FPrism(NBRb, Fa.cell))

then if (vdot(NBRb.H, Fa.norm) > 0)

then close-feat-checkp (Fa, NBRb) else close-feat-checkp

((find-closest(NBRb, Fa.cobnd), NBRb) else close-feat-checkp (closest-edges(Fa,Fb))

%%% The main routine

PROCEDURE close-feat-checkp (feat1, feat2)

case (type(feat1), type(feat2))

(VERTEX, VERTEX) RETURN vertex-vertex(feat1, feat2)

(VERTEX, EDGE) RETURN vertex-edge(feat1, feat2)

(VERTEX, FACE) RETURN vertex-face(feat1, feat2)

(EDGE, VERTEX) RETURN reverse(vertex-edge(feat2, feat1)

(EDGE, EDGE) RETURN edge-edge(feat1, feat2)

(EDGE, FACE) RETURN edge-face(feat1 feat2)

(FACE, VERTEX) RETURN reverse(vertex-face(feat2, feat1))

(FACE, EDGE) RETURN reverse(edge-face(feat2, feat1))

(FACE, FACE) RETURN face-face(feat1, feat2)

%% To check if two objects collide by their minimum separation

if (dist(feat1,feat2) ~ 0)

then PRINT ("Collision")