PureBoard

Code: Alles auswählen

#EPSILON = 0.000001

Structure stVec3D
  x.f
  y.f
  z.f
EndStructure


Macro mVec3DCopy(ResultVec, Vec)
  ResultVec\x = Vec\x
  ResultVec\y = Vec\y
  ResultVec\z = Vec\z
EndMacro

Macro mVec3DAdd(ResultVec, Vec1, Vec2)
  ResultVec\x = Vec1\x + Vec2\x
  ResultVec\y = Vec1\y + Vec2\y
  ResultVec\z = Vec1\z + Vec2\z
EndMacro

Macro mVec3DSub(ResultVec, Vec1, Vec2)
  ResultVec\x = Vec1\x - Vec2\x
  ResultVec\y = Vec1\y - Vec2\y
  ResultVec\z = Vec1\z - Vec2\z
EndMacro

Macro mVec3DCross(ResultVec, Vec1, Vec2) 
  ResultVec\x = Vec1\y * Vec2\z - Vec1\z * Vec2\y 
  ResultVec\y = Vec1\z * Vec2\x - Vec1\x * Vec2\z 
  ResultVec\z = Vec1\x * Vec2\y - Vec1\y * Vec2\x 
EndMacro

Macro mVec3DScalarMul(ResultVec, Vec, Scalar)
  ResultVec\x = Vec\x * Scalar
  ResultVec\y = Vec\y * Scalar
  ResultVec\z = Vec\z * Scalar
EndMacro

Macro mVec3DDot(Vec1, Vec2)
  (Vec1\x * Vec2\x + Vec1\y * Vec2\y + Vec1\z * Vec2\z)
EndMacro

Macro mVec3DLength(Vec)
  Sqr(Vec\x * Vec\x + Vec\y * Vec\y + Vec\z * Vec\z)
EndMacro

Macro mVec3DNormalize(ResultVec, Vec)
  DisableExplicit     ; ! remove if not needed !
  mVec3DNormalizeRL.f = 1.0 / mVec3DLength(Vec)
  mVec3DScalarMul(ResultVec, Vec, mVec3DNormalizeRL)
  EnableExplicit      ; ! remove if not needed !
EndMacro



Procedure.f RaySphereHit(*Origin.stVec3D, *Dir.stVec3D, *Pos.stVec3D, Radius.f)
  Protected Dif.stVec3D, b.f, c.f, d.f, t1.f, t2.f
  mVec3DSub(Dif, *Origin, *Pos)
  b = -mVec3DDot(*Dir, Dif)
  If b > 0.0
    c = mVec3DDot(Dif, Dif) - Radius * Radius
    d = b * b - c 
    If d > 0.0
      d = Sqr(d) 
      t1 = (b - d)
      t2 = (b + d)
      If Abs(t2) < Abs(t1)
        t1 = t2
      EndIf
      ProcedureReturn t1
    EndIf
  EndIf
  ProcedureReturn -1.0
EndProcedure  

Procedure ClosestPointOnLine(*ResultPoint.stVec3D, *Vert0.stVec3D, *Vert1.stVec3D, *Point.stVec3D)
  Protected c.stVec3D, v.stVec3D, d.f, rd.f, t.f
  mVec3DSub(c, *Point, *Vert0)
  mVec3DSub(v, *Vert1, *Vert0)
  d = mVec3DLength(v)             ; normalize v     
  rd = 1.0 / d                  ; ...
  mVec3DScalarMul(v, v, rd)       ; ...  
  t = mVec3DDot(v,c)
  If t < 0.0
    mVec3DCopy(*ResultPoint, *Vert0)
    ProcedureReturn
  EndIf
  If t > d
    mVec3DCopy(*ResultPoint, *Vert1)
    ProcedureReturn
  EndIf
  mVec3DScalarMul(v, v, t)
  mVec3DAdd(*ResultPoint, *Vert0, v)
EndProcedure

Procedure.f SphereTriangleCollision(*Pos.stVec3D, *Dir.stVec3D, Radius.f, *Vert0.stVec3D, *Vert1.stVec3D, *Vert2.stVec3D) 
  Protected Edge1.stVec3D, Edge2.stVec3D, PVec.stVec3D, TVec.stVec3D, QVec.stVec3D, IVec.stVec3D, Intersect.stVec3D
  Protected CollPlane.stVec3D, PlaneNorm.stVec3D, CPVec.stVec3D
  Protected Det.f, InvDet.f, u.f, v.f, DistIntersect.f, lEdge1.f, lEdge2.f, RelRad.f, Dif.stVec3D, dist.f, ClDist.f
  Protected Closest.stVec3D, Closest0.stVec3D, Closest1.stVec3D, Closest2.stVec3D, RevDir.stVec3D
  
  
  mVec3DSub(Edge1, *Vert1, *Vert0)
  mVec3DSub(Edge2, *Vert2, *Vert0)
  
  ; move triangle plane in direction of the normal vector by 'Radius' length
  mVec3DCross(PlaneNorm, Edge1, Edge2)
  mVec3DNormalize(PlaneNorm, PlaneNorm)
  mVec3DScalarMul(CPVec, PlaneNorm, Radius)
  mVec3DAdd(CollPlane, *Vert0, CPVec)
  
  
  mVec3DCross(PVec, *Dir, Edge2) 
  Det = mVec3DDot(Edge1, PVec)
  
  If Det > #EPSILON 
    ; frontfacing
    
    mVec3DSub(TVec, *Pos, CollPlane) 
    u = mVec3DDot(TVec, PVec)
    
    mVec3DCross(QVec, TVec, Edge1)
    v = mVec3DDot(*Dir, QVec)
    
    InvDet = 1.0 / Det
    
    If u >= 0.0 And v >= 0.0 And u + v < Det
      ; direct hit
      ProcedureReturn mVec3DDot(Edge2, QVec) * InvDet
    EndIf
    
    lEdge2 = mVec3DLength(Edge2)
    RelRad = Radius / lEdge2
    u = InvDet * u
    If u < -RelRad Or u > 1.0 + RelRad
      ; sphere misses triangle
      ProcedureReturn -1.0
    EndIf 
    
    lEdge1 = mVec3DLength(Edge1)
    RelRad = Radius / lEdge1
    v = InvDet * v
    If v < -RelRad Or u + v > 1.0 + RelRad
      ; sphere misses triangle
      ProcedureReturn -1.0 
    EndIf
    
  ElseIf Det < #EPSILON 
    ; backfacing
    
    mVec3DSub(TVec, *Pos, CollPlane) 
    u = mVec3DDot(TVec, PVec)
    
    mVec3DCross(QVec, TVec, Edge1)
    v = mVec3DDot(*Dir, QVec)
    
    InvDet = 1.0 / Det
    
    If u <= 0.0 And v <= 0.0 And u + v >= Det 
      ; direct hit
      ProcedureReturn mVec3DDot(Edge2, QVec) * InvDet
    EndIf
    
    lEdge2 = mVec3DLength(Edge2)
    RelRad = Radius / lEdge2
    u = InvDet * u
    If u > RelRad Or u < -1.0 - RelRad
      ; sphere misses triangle
      ProcedureReturn -1.0
    EndIf 
    
    lEdge1 = mVec3DLength(Edge1)
    RelRad = Radius / lEdge1
    v = InvDet * v
    If v > RelRad Or u + v < -1.0 - RelRad
      ; sphere misses triangle
      ProcedureReturn -1.0 
    EndIf
    
  Else
    ; sphere moves parallel to triangle
    ProcedureReturn -1.0 
  EndIf 
  ; sphere collides with edge of triangle
  
  InvDet = 1.0 / Det
  DistIntersect = mVec3DDot(Edge2, QVec) * InvDet
  
  ; compute intersection point on triangle plane
  mVec3DScalarMul(IVec, *Dir, DistIntersect)
  mVec3DAdd(Intersect, *Pos, IVec)
  
  ; get closest point on triangle to that point
  ClosestPointOnLine(Closest0, *Vert0, *Vert1, Intersect)
  ClosestPointOnLine(Closest1, *Vert1, *Vert2, Intersect)
  ClosestPointOnLine(Closest2, *Vert2, *Vert0, Intersect)
  mVec3DSub(Dif, Closest0, Intersect)
  ClDist = mVec3DDot(Dif, Dif)
  mVec3DCopy(Closest, Closest0)
  mVec3DSub(Dif, Closest1, Intersect)
  dist = mVec3DDot(Dif, Dif)
  If dist < ClDist
    ClDist = dist
    mVec3DCopy(Closest, Closest1)
  EndIf
  mVec3DSub(Dif, Closest2, Intersect)
  dist = mVec3DDot(Dif, Dif)
  If dist < ClDist
    mVec3DCopy(Closest, Closest2)
  EndIf
  ; test ray sphere hit from that point in reverse movement direction
  RevDir\x = - *Dir\x
  RevDir\y = - *Dir\y
  RevDir\z = - *Dir\z
  ProcedureReturn RaySphereHit(Closest, RevDir, *Pos, Radius)
EndProcedure