1: |
procedure selfCollisionDetection |
2: |
for each line segment i = 1 → N
do
|
3: |
for each proximate line segment j (found in broad phase) do
|
4: |
compute w(t+Δt) from current positions |
5: |
compute [or retrieve] w(t) from previous positions |
6: |
compute |w(t)|, |w(t+Δt)|, m = sign(w(t).w(t+Δt)) |
7: |
if |w(t+Δt)| − m|w(t+Δt)| ≤ 0 then
|
8: |
return false; (no collision) |
9: |
else
|
10: |
if
si(t+Δt) < 0 or
si(t+Δt) > 1 or
si(t+Δt) < 0 or
si(t+Δt) > 1 then
|
11: |
(case when w(t+Δt) lies outside any of line segment) |
12: |
compute w̄(t+Δt)
|
13: |
else
|
14: |
set w̄(t+Δt) = w(t+Δt)
|
15: |
end if
|
16: |
if |w̄(t+Δt)| < D
then
|
17: |
return true; (collision) |
18: |
else
|
19: |
if
sign(w(t).w(t+Δt) < 0 and 0 ≤ si(t+Δt), sj(t+Δt) ≤ 1 then
|
20: |
return true; (collision) |
21: |
else
|
22: |
return false; (no collision) |
23: |
end if
|
24: |
end if
|
25: |
end if
|
26: |
end for
|
27: |
end for
|
28: |
end procedure |