Post by DJLinux on Nov 14, 2009 9:00:48 GMT -5
Usefull 3D Math CollisionDetection, RayTracing, 3D Modeller etc.
Joshy
Joshy
' shortest distance point to ray (origin,direction)
function Point2Ray#(P#(),O#(),D#(),&Ret#())
dim v#(2) = D# - O#
dim w#(2) = P# - O#
dim c1# = w# * v#
dim c2# = v# * v#
dim b# = c1# / c2#
Ret# = O# + b# * v#
return Length(P#-Ret#)
end function
' shortest distance to segment (Start,End)
function Point2Segment#(P#(),S#(),E#(),&Ret#())
dim v#(2) = E#-S#
dim w#(2) = P#-S#
dim c1# = w#*v#
if (c1#<=0) then
Ret#=W#
return Length(w#)
end if
dim c2# = v#*v#
if (c2#<=c1#) then
Ret#=E#
return Length(P#-E#)
end if
dim b# = c1# / c2#
Ret# = S#+b#*v#
return Length(P#-Ret#)
end function
' shortest distance point to plain R# = point on plane
function Point2Plane#(P#(),V#(),N#(),&R#())
dim sn# = -(N# *(P# - V#))
dim sd# = N# * N#
dim sb# = sn# / sd#
R# = P# + sb# * N#
return Length(P#-R#)
end function
' intersection with Ray or RaySegment with Triangle
' Start,End,Triangle,TRUE,R#=PointOfCollision
' Origin,Direction,Triangle,FALSE,R#=PointOfCollision
function RaySegmentTriangle(S#(),E#(),T#()(),IsSegment,&R#() )
dim u#(2) = T#(1) - T#(0)
dim v#(2) = T#(2) - T#(0)
dim n#(2) = CrossProduct(u#,v#)
' is degenerate ?
if Length(n#)=0 then return 0:end if
dim dir#(2) = E# - S#
dim b# = n#*dir#
' parallel
if (abs(b#) < 0.00000001) then return False:end if
dim w0#(2) = S# - T#(0)
dim a# = -(n#*w0#)
' get intersect point of ray with triangle plane
dim c# = a# / b#
' ray goes away from triangle
if (c# < 0.0) then return False:end if
if (IsSegment) and (c#>1.0) then return False:end if
' intersect point of ray and plane
R# = S# + c# * dir#
' is R# inside triangle
dim uu# = u#*u#
dim uv# = u#*v#
dim vv# = v#*v#
dim w#(2) = R# - T#(0)
dim wu# = w#*u#
dim wv# = w#*v#
dim D# = uv# * uv# - uu# * vv#
dim f# = (uv# * wv# - vv# * wu#) / D#
if (f# < 0.0) or (f# > 1.0) then return False:end if
c# = (uv# * wu# - uu# * wv#) / D#
if (c# < 0.0) or ((c# + f#) > 1.0) then
return False
else
return True
end if
end function
' SphereCenter,SphereRadius,RayOrigin,RayDirection,Ret=PointOfCollision
function SphereRay(c#(),r#,O#(),D#(),&Ret#())
D#=Normalize(D#)
dim CO#(2)=C#-O#
dim v#=CO#*D#
dim disc#=r#*r# - (CO#*CO# - v#*v#)
if (disc#<0.0) then return false:end if
dim T#=sqr(disc#)
Ret#=O#+(V#-T#)*D#
return true
end function
' SphereCenter1,SphereRadius1, SphereCenter2,SphereRadius2,Ret=PointOfCollision
function SphereSphere(c1#(),r1#,c2#(),r2#,Ret#())
dim D#(2)= C2#-C1#
dim D2#= D#*D#
dim R2#=r1#+r2#:R2#=R2#*R2#
if D2# >= R2# then return False:end if
Ret#=Normalize(C1#-C2#)
Ret#=C1# + (Ret#*r1#)
return True
end function