1 files changed, 53 insertions, 0 deletions
diff --git a/beam_tracing/python/beam2FaceDistance.py b/beam_tracing/python/beam2FaceDistance.py
new file mode 100644
index 0000000..8aeedec
--- /dev/null
+++ b/beam_tracing/python/beam2FaceDistance.py
@@ -0,0 +1,53 @@
+import numpy as np
+
+def beam2FaceDistance(beam, face):
+# return  distance to face if beam hits it or infinity otherwise
+# for beams and faces structure description see face_beam_interaction.m
+
+   # default return values for the case when beam misses the face
+   isFaceHit = False
+   hitPosition = [float('NaN'), float('NaN')]
+   hitDistance = float('Inf')
+
+   k = beam.k
+   # face direction or kf
+   kf = face.vertex2 - face.vertex1 # not a unit vector
+
+   # simple check for intersection of two vectors 
+   # if beam are parallel no intersection is possible
+   # we do this via cross product calculation
+   
+   angleTolerance = 1e-14;
+   if abs(np.cross(k, kf)) <= angleTolerance:
+      # beams misses face
+      return [hitDistance, hitPosition, isFaceHit]
+   
+   # let's find the intersection of lines passing through face and light beam
+   # we introduce parameters tb and tf 
+   # which define distance (in arb. units) along k vectors
+   # we are solving 
+   # xb_o+ k_x*tb = v1_x+ kf_x*tf
+   # yb_o+ k_y*tb = v1_y+ kf_y*tf
+   # A*t = B
+   A = np.array([[k[0], -kf[0]],\
+                [k[1], -kf[1]]])
+   B = np.array([[face.vertex1[0] - beam.origin[0]],\
+                [face.vertex1[1] - beam.origin[1]]])
+   A = np.linalg.inv(A)
+   t = np.dot(A, B)
+   tb = t[0] 
+   tf = t[1]
+
+   # beam intersects face only if 0<=tf<=1 and tb>=0
+   if 0 <= tf and tf <= 1 and tb >= angleTolerance:
+      # intersection,  beam hits the face
+      isFaceHit = True
+   else:
+      # beam misses the face
+      return [hitDistance, hitPosition, isFaceHit]
+
+   # calculating hit position
+   hitPosition = face.vertex1 + kf * tf
+   hitDistance = tb # distance along the light beam
+
+   return [hitDistance, hitPosition, isFaceHit]
+\ No newline at end of file