(ฟรี)โปรแกรมภาษาไพทอนบนเครื่องคิดเลขคาสิโอ fx-9750GIII fx-9860GIII และ fx-cg50 โปรแกรมพื้นฐานงานสำรวจชุดที่ 3 (COGO Selected Serie 3)

ตอนนี้มีเครื่องคิดเลขของคาสิโอสามรุ่นที่สามารถโปรแกรมด้วยภาษาไพทอนหรือไมโครไพทอน (MicroPython) ได้คือ fx-9750GIII, fx-9860GIII และ fx-cg50 ทั้งสามรุ่นสามารถหาซื้อได้ไม่ยากนัก ผมซื้อมาทางออนไลน์สะดวกดี สนนราคาเรียงตามรุ่นตอนนี้อยู่ที่ สามพันบาท สี่พันห้าร้อยบาทและเจ็ดพันกว่าบาทตามลำดับ ผมแนะนำให้สำหรับคนที่เบี้ยน้อยหอยน้อยลงทุนกับ fx-9750GIII เพราะราคาไม่แพง คุ้มค่าเกินราคา ทั้งสามรุ่นสามารถโปรแกรมด้วยภาษาคาสิโอเบสิคและภาษาไพทอน แล้วแต่ความถนัด ความชอบ

เครื่องคิดเลขคาสิโอ fx-9750GIII, fx-9860GIII และ fx-cg50

ถ้าโปรแกรมด้วยไพทอนจะสามารถเขียนโปรแกรมที่ยากๆหรือคณิตศาสตร์ซับซ้อนขึ้นมาได้ นี่เป็นเหตุผลสำคัญ ถึงแม้หน่วยความจำของเครื่องคิดเลขจะน้อยก็เป็นโจทย์ให้ผู้พัฒนาโปรแกรมได้ออปติไมซ์โปรแกรมให้ทำงานได้ แต่สำหรับโปรแกรมพื้นฐานงานสำรวจส่วนใหญ่ก็พื้นๆ ไม่มีอะไรซับซ้อน

ชุดโปรแกรมพื้นฐานงานสำรวจ COGO (Coordinate Geometry) ชุดที่ 3

มาถึงโปรแกรมพื้นฐานงานสำรวจชุดที่ 3 (COGO Selected Serie 3) ซี่รี่ย์ว่าด้วยเรื่องการ offset ของจุดและเส้นตรง การประยุกต์งานเช่นบางสถานการณ์ไปวางจุดหน้างานแต่ทำไม่ได้เนื่องจากตำแหน่งที่วางจุดมีอุปสรรคจำเป็นต้องเลื่อนหรือออฟเซ็ทไป สามารถใช้เครื่องคิดเลขคิดได้ทันที

โปรแกรมพื้นฐานงานสำรวจชุดที่ 3 บนเครื่องคิดเลขคาสิโอ fx-9860GIII

ส่วนประกอบของโปรแกรม

สำหรับโปรแกรมพื้นฐานงานสำรวจในชุดนี้จะจัดโปรแกรมย่อยเล็กๆ ไว้ 4 โปรแกรม เนื่องจากหน้ากว้างของหน้าจอเครื่องคิดเลขมีแค่ 21 ตัวอักษร ดังนั้นบางอย่างผมใช้คำย่อเช่น OS ย่อมาจาก Offset

  1. OS Point to Line เมื่อรู้ค่าพิกัดของจุดและรู้เส้นตรงจากค่าพิกัด 2 จุด สามารถหาระยะ offset จากจุดไปตั้งฉากกับเส้นตรงได้
  2. OS Point from Line เมื่อกำหนดเส้นตรงจากค่าพิกัด 2 จุด (จุดที่ 1 และจุดที่ 2) และต้องการ offset จุดจากเส้นตรงโดยที่ทราบระยะทางจากจุดที่ 1 (Chainage) และระยะ offset สามารถ offset จุดไปทางซ้ายหรือทางขวาได้ จากนั้นสามารถคำนวณหาค่าพิกัดของจุด
  3. OS Point to Plane มีระนาบที่กำหนดจากจุด 3 จุดที่ทราบค่า (X, Y, Z) และต้องการหาระยะทางที่สั้นที่สุดระยะหว่างระนาบและจุดที่ทราบค่า (X, Y, Z)
  4. OS Point to 3D Line เมื่อกำหนดเส้นตรง 2 จุด ที่ทราบค่า (X, Y, Z) ต้องการหาระยะ offset ที่สั้นที่สุดระหว่างจุดที่ทราบค่า (X, Y, Z)

วิธีการใช้งานโปรแกรม

กดคีย์ “MENU” ที่เครื่องคิดเลข กดคีย์ลูกศรเลื่อนไปที่ไอคอน “Python” กดเข้าไป ตรงนี้ผมสร้างโฟลเดอร์ชื่อ “PROGRAM” ผมเอาซอร์สโค้ดโปรแกรมไพทอนมาไว้ที่นี่ เมื่อกด “EXE” เข้าไปจะเห็นไฟล์ซอร์สโค้ดไพทอนเรียงกันประมาณนี้ ตอนนี้จะรันไฟล์ COGOSSE3.py ซึ่งจะต้องมีไฟล์ไลบรารี pbrutils.py ด้วย

OS Point to Line

ตัวอย่างกำหนดค่าพิกัดเส้นตรงจุดที่ 1 N=4880.374, E=5644.602 จุดที่ 2 N=4961.919, E=5699.887 กำหนดจุด N=4939.725, E=5669.923 หาระยะทาง (offset) จากจุดนี้ไปตั้งฉากกับเส้นตรงและคำนวณหาระยะทางจากเส้นตรงจุดที่ 1 (chainage)

แผนผังแสดงตัวอย่าง OS Point to Line (2D)

ที่เมนูกดคีย์ “1” และกดคีย์ “EXE” โปรแกรมจะถามค่าพิกัดจุดที่ 1 และจุดที่ 2 จากนั้นจะถามค่าพิกัดจุดที่ต้องการ offset ไปหาเส้นตรง

จะได้ระยะทาง (offset) จากจุดไปตั้งฉากกับเส้นตรงเท่ากับ 12.347 และระยะทาง (chainage) จากเส้นตรงจุดที่ 1 เท่ากับ 63.334

แผนผังแสดงผลลัพธ์ OS Point to Line (2D)

OS Point from Line 

เมื่อกำหนดเส้นตรงที่รู้ค่าพิกัด 2 จุด จากนั้นกำหนดระยะทาง (chainage) จากเส้นตรงจุดที่ 1 และกำหนดระยะ offset ไปทางซ้าย (ป้อนเครื่องหมายเป็นลบ) หรือไปทางขวา ต้องการหาค่าพิกัดของจุดที่ offset ตัวนี้ มาดูตัวอย่าง

แผนผังแสดงตัวอย่างกลุ่มเสาเข็มที่รู้ค่าพิกัด 2 จุด

กำหนดค่าพิกัดของกลุ่มเส้นตรงโดยที่จุดที่ 1 N=5181.378, E=5911.340 และจุดที่ 2 N=5195.600, E=5897.279 ต้องการทราบค่าพิกัดของเสาเข็มสีเขียวและสีเหลือง

ที่เมนูกดคีย์ “2” และกดคีย์ “EXE” โปรแกรมจะถามค่าพิกัดจุดที่ 1 และจุดที่ 2 หาค่าพิกัดเสาเข็มสีเขียวได้จากระยะทาง (chainage) จากจุดที่ 1 ไป 5 เมตร และระยะ offset ไปทางซ้ายเท่ากับ -7.5 เมตร

จะได้ค่าพิกัดเสาเข็มสีเขียว N=5179.661, E=5902.491จะคำนวณค่าพิกัดเสาเข็มสีเหลือง โปรแกรมจะถามว่าต้องการคำนวณต่อหรือไม่ “Continue (Y/N)?” ที่เครื่องคิดเลขให้กดคีย์ “ALPHA” แล้วกดคีย์ “-” เพื่อให้ออกเป็นตัว “y”

หาพิกัดเสาเข็มสีเหลืองโดยการป้อน chainage = 3×5=15 และค่า offset = 7.5 จะได้ค่าพิกัดเสาเข็มสีเหลือง N=5197.318, E=5906.127 เมื่อโปรแกรมว่าคำนวณต่ออีกหรือไม่ “Continue (Y/N)?” ตอนนี้กดคีย์ “EXE” เพื่อไม่คำนวณต่อ

แผนผังแสดงผลลัพธ์ค่าพิกัดผลการคำนวณ

OS Point to Plane

บางกรณีต้องทำงานบนพื้นที่ที่เป็นระนาบ (plane) เอียง ตัวอย่างเช่นการติดตั้งโบลท์บนพื้นที่เป็นระนาบเอียงต้องติดตั้งเพื่อให้ตั้งฉากกับระนาบ เวลาหาความสูงของโบลท์หรือระยะทางตั้งฉากจากจุดไปยังระนาบเอียง กรณีที่ทราบค่าพิกัด (X, Y, Z) ของระนาบซึ่งจะต้องทราบค่า 3 จุด เมื่อรังวัดหัวโบลต์ด้วยกล้องประมวลผลรวม (total station) จะทราบค่าพิกัดและค่าระดับหัวโบลท์ เมื่อนำมาคำนวณสามารถทราบความสูงของโบลท์จากระนาบได้

ไดอะแกรมแสดงจุดกับระนาบ (plane) ในระบบพิกัดสามมิติ (3D)

ที่เมนูกดคีย์ “3” และกดคีย์ “EXE” โปรแกรมจะถามค่าพิกัด 3D ของระนาบ จุดที่ 1, จุดที่ 2 และจุดที่ 3 จากนั้นป้อนจุด 3D โปรแกรมจะคำนวณหาระยะทางจากจุดมาตั้งฉากกับระนาบ (ระยะทางที่สั้นที่สุด)

กำหนดจุดบนระนาบ (plane) จำนวน 3 จุดดังนี้

  1. จุดที่ 1 N=1053.481, E=958.494, Elev=5.000 m
  2. จุดที่ 2 N=1053.694, E=959.699, Elev=4.651 m
  3. จุดที่ 3 N=1054.179, E=958.941, Elev=4.651 m

กำหนดจุดเซอร์เวย์ที่ได้จากรังวัด N=1053.901, E=959.813, Elev=4.519 m คำนวณหาระยะตั้งฉากจากจุดนี้ไปยังระนาบ

คำตอบระยะทางจากจุดมาตั้งฉากกับระนาบ = 0.030 m = 30mm

OS Point to 3D Line

ตัวอย่างต้องการคำนวณหาระยะทาง offset จากจุด p3 N=669.143, E=424.276, Elev=28.554 ไปหาเส้นตรงที่กำหนดค่าพิกัดและระดับดังนี้

  1. จุด p1N=663.002, E=431.425, Elev=27.891
  2. จุด p2 N=667.187, E=421.120, Elev=30.877
ไดอะแกรมแสดงเส้นตรง p1-p2 ในระบบสามมิติกับระยะทางที่สั้นที่สุดไปหาจุด p3

ที่เมนูกดคีย์ “4” และกดคีย์ “EXE” โปรแกรมจะถามค่าพิกัด 3D ของเส้นตรง จุดที่ 1, จุดที่ 2 จากนั้นจะถามค่าพิกัดและระดับของจุด โปรแกรมจะคำนวณหาระยะทางจากจุดมาตั้งฉากกับเส้นตรง 3D (ระยะทางที่สั้นที่สุด)

คำตอบระยะ offset = 3.436

สรุป

การทำงานสำรวจนั้นพื้นฐานก็คือเรื่องเรขาคณิตและตรีโกณมิติ เกี่ยวพันเรื่องระยะทางและมุม เมื่อบวกเรื่องค่าระดับก็จะกลายเป็นงานในระบบสามมิติ (3D) ไม่จำเป็นต้องไปจำสูตรให้ยุ่งยาก ให้รู้ที่มาที่ไปของสูตรก็พอ เครื่องคิดเลขก็เป็นเครื่องมืออย่างหนึ่ง ถ้าจำสูตรไม่ได้ก็ต้องใช้โปรแกรมมาช่วย ผมตั้งใจจะเขียนโปรแกรมพื้นฐานงานสำรวจอีกหลายชุดทยอยตามกันมา ก็หวังว่าจะเป็นประโยชน์ไม่มากก็น้อยและก็เหมือนเดิมคือแจกฟรี ไม่คิดสตางค์ ตั้งใจเพื่อช่วยเหลือวงการสำรวจบ้านเราเล็กๆน้อยๆที่พอทำได้ สำหรับซอร์สโค้ดโปรแกรมพื้นฐานงานสำรวจชุดที่ 3 ก็เอามาแปะไว้ด้านล่าง

วิธีการติดตั้งโปรแกรมบนเครื่องคิดเลข

ผมจะปล่อยซอร์สโค้ดที่อัพเดทไว้ที่เมนูดาวน์โหลด (Download) เมื่อดาวน์โหลดไฟล์ “COGOSSE3.py” และไฟล์ “pbrutils.py” แล้ว สมมุติโหลดมาไว้ที่เครื่องคอมพิวเตอร์ จากนั้นนำเครื่องคิดเลขคาสิโอ fx-9750GIII หรือ fx-9860GIII หรือ fx-cg50 มาเสียบเข้าสาย USB ต่อกับเครื่องคอมพิวเตอร์ ให้กดคีย์ “F1” เพื่อจำลองเป็นไดรว์ จากนั้น copy สองไฟล์นี้เข้าไปที่ไดรว์ของเครื่องคิดเลข รายละเอียดอ่านที่ลิ๊งค์ที่ผมจัดทำมาเป็นพิเศษ

ซอร์สโค้ดโปรแกรมพื้นฐานงานสำรวจชุดที่ 3 (COGOSSE3.py)

from pbrutils import split_angle,deg2dms,DMS2str
from pbrutils import calcAzimuth,calcDistance,calcDiff2Angle
from pbrutils import calcNextAzimuth,shortest_distance_point_to_plane
from pbrutils import calcIntersectionAzimuthAzimuth,equation_plane
from pbrutils import PI,DEG2RAD,RAD2DEG,mean,calcNextCoordinate2D
from pbrutils import distance_point_to_3Dline
import math


def print_menu():
  print('COGO Selected Serie 3')
  print('1:OS Point to Line')
  print('2:OS Point from Line')
  print('3:OS Point to Plane')
  print('4:OS Point to 3D Line')
  print('5:Exit')

def offset_point_to_line():
  print('O/S point to 2D line.')
  print("Input Line:")
  n1=float(input("N1=?" ))
  e1=float(input("E1=?" ))
  n2=float(input("N2=?" ))
  e2=float(input("E2=?" ))
  print("Input point:")
  n3=float(input("N3=?" ))
  e3=float(input("E3=?" ))
  azimuth12=calcAzimuth(n1,e1,n2,e2)
  azimuthper=calcNextAzimuth(0.5*PI,azimuth12)
  ni,ei=calcIntersectionAzimuthAzimuth([n1,e1],azimuth12,[n3,e3],azimuthper)
  os=calcDistance(ni,ei,n3,e3)
  ch=calcDistance(ni,ei,n1,e1)
  print("Offset dist= {0:.3f}".format(os))
  print("Chainage= {0:.3f}".format(ch))
  print("N=","{0:.3f}".format(ni))
  print("E=","{0:.3f}".format(ei))
  input("Press any key")  

def offset_point_from_line():
  f=True
  print("O/S point from 2D line.")
  print("Input Line:")
  n1=float(input("N1=?" ))
  e1=float(input("E1=?" ))
  n2=float(input("N2=?" ))
  e2=float(input("E2=?" ))
  while f:
    print("Input chainage & dist")
    ch=float(input("Chainage=?"))
    print("Input offset distance.")
    print("- left and + right")
    os=float(input("Offset dist=?"))
    azimuth12=calcAzimuth(n1,e1,n2,e2)
    if os<0:
      azimuthper=calcNextAzimuth(0.5*math.pi,azimuth12)
    else:
      azimuthper=calcNextAzimuth(1.5*math.pi,azimuth12)
    ni=n1+math.cos(azimuth12)*ch
    ei=e1+math.sin(azimuth12)*ch
    nos=ni+math.cos(azimuthper)*abs(os)
    eos=ei+math.sin(azimuthper)*abs(os)
    print("Offset pt coordinates:")
    print("N= {:.3f}".format(nos))
    print("E= {:.3f}".format(eos))
    con = input("Continue (Y/N)?")
    if con=="Y" or con=="y":
      f=True
    else:
      f=False

def offset_point_to_plane():
  print("O/S pt to 3D plane.")
  print("Input 3 pts for plane:")
  n1=float(input("N1=?" ))
  e1=float(input("E1=?" ))
  z1=float(input("Z1=?"))
  n2=float(input("N2=?" ))
  e2=float(input("E2=?" ))
  z2=float(input("Z2=?"))
  n3=float(input("N3=?" ))
  e3=float(input("E3=?" ))
  z3=float(input("Z3=?"))
  print("Input pt to project.")
  n4=float(input("N=?" ))
  e4=float(input("E=?" ))  
  z4=float(input("Z=?"))
  a,b,c,d=equation_plane(n1,e1,z1,n2,e2,z2,n3,e3,z3)
  shdist=shortest_distance_point_to_plane(n4,e4,z4,a,b,c,d)
  print("Shortest Distance:")
  print("Distance= {:0.3f}".format(shdist))
  input("Press any key") 

def offset_distance_point_to_3Dline():
  print("O/S point to 3D line.")
  print("Input 3D line:")
  n2=float(input("N1=?" ))
  e2=float(input("E1=?" ))
  z2=float(input("Z1=?"))
  n3=float(input("N2=?" ))
  e3=float(input("E2=?" ))
  z3=float(input("Z2=?"))
  print("Input pt to project.")
  n1=float(input("N=?" ))
  e1=float(input("E=?" ))  
  z1=float(input("Z=?"))
  os=distance_point_to_3Dline(n1,e1,z1,n2,e2,z2,n3,e3,z3)
  print("Offset dist= {:.3f}".format(os))
  input("Press any key")  

loop=True
choice=-1
ex_choice=-1
while loop:      
  print_menu()
  choice=int(input('Selection[1-5]'))  
  if (choice==5):
    loop=False
  elif (choice==1):
    loop=True
    offset_point_to_line()
    ex_choice=1
  elif (choice==2):
    loop=True
    offset_point_from_line()
    ex_choice=2
  elif (choice==3):
    offset_point_to_plane()
    loop=True
    ex_choice=3
  elif (choice==4):
    loop=True
    offset_distance_point_to_3Dline()
    ex_choice=4

ไลบรารี pbrutils.py

import math
PI=math.pi
DEG2RAD=PI/180.0
RAD2DEG=180.0/PI
TOLERANCE=1e-10
def clear():
  for i in range(6):
    print()
	
def split_angle(st):
  ss=st.split('-')
  j=0
  d,m,s=0.0,0.0,0.0
  for c in ss:
    if j==0:
      d=float(c)
    elif j==1:
      m=float(c)
    elif j==2:
      s=float(c)
    j+=1    
  return d+m/60.0+s/3600.0

def deg2dms(dd):
  sign=1
  if (dd<0):
    sign=-1
  dd=abs(dd)
  minutes,seconds=divmod(dd*3600,60)
  degrees,minutes=divmod(minutes,60)
  return (sign,degrees,minutes,seconds)

def DMS2str(degree,minute,second,numdec):
  degree=abs(degree); minute=abs(minute); second=abs(second)
  s='{:.%df}' % numdec
  ss=s.format(second)
  smin=s.format(60.0)
  mm='{:.0f}'.format(minute)
  if (ss==smin):
    minute+=1
    ss=s.format(0.0)
    if (minute>=60):
      mm='0'
      degree+=1
    else:
      mm='{:.0f}'.format(minute)
  return '{:.0f}'.format(degree)+"-"+mm+"-"+ss

def calcDistance(n1,e1,n2,e2):
  return (math.sqrt(math.pow(n2-n1, 2.0) + math.pow(e2-e1, 2)))

def calcAzimuth(n1,e1,n2,e2):
  """ atan2 not same as excel function"""
  angle=math.atan2(e2-e1,n2-n1)
  if angle>=0:
    return angle
  else:
    return angle+2*PI
    
def calcDiff2Angle(azi2,azi1):
  temp=azi2-azi1
  if (temp<0):
    return temp + 2*PI
  else:
    return temp
	
def calcNextAzimuth(currentang,prevazi):
  temp=currentang+prevazi
  if (temp<PI):
    temp=temp+PI
  elif (temp>3*PI):
    temp=temp-3*PI
  else:
    temp=temp-PI
  return temp

def calcNextCoordinate2D(n,e,prevazi,horang,griddist):
  nextazi=calcNextAzimuth(horang,prevazi)
  nt=n+math.cos(nextazi)*griddist
  et=e+math.sin(nextazi)*griddist
  return [nt,et]

def calcNextCoordinate3D(n,e,z,prevazi,horang,verang,slopedist,hi,ht):
  hordist=slopedist*math.sin(verang)
  verdist=slopedist*math.cos(verang)
  nextazi=calcNextAzimuth(horang,prevazi)
  nt=n+math.cos(nextazi)*hordist
  et=e+math.sin(nextazi)*hordist
  zt=z+hi+verdist-ht
  return [nt,et,zt]

def mean(num):
  sum_num = 0
  for t in num:
    sum_num = sum_num + t           
  avg = sum_num / len(num)
  return avg

def calcIntersectionFrom4Points(pt1, pt2, pt3, pt4):
    '''Compute the intersection points which given 4 points.
       If intersection on line return point otherwise return None.
    '''
    azimuth12 = calcAzimuth(pt1, pt2)
    distance12 = calcDistance(pt1, pt2)
    azimuth34 = calcAzimuth(pt3, pt4)
    distance34 = calcDistance(pt3, pt4)
    #print('++++ pt1 pt2 pt3 pt4 azimuth12 azimuth34 = ', pt1, pt2, pt3, pt4, azimuth12, azimuth34)

    if (math.abs(azimuth12 - (0.5 * PI)) <= TOLERANCE) or (math.abs(azimuth12 - (1.5 * PI)) <= TOLERANCE):
        pti = [(pt1[1] - pt3[1]) * math.tan(azimuth34) + pt3[0], pt1[1]]
    elif (azimuth34 == (0.5 * PI)) or (azimuth34 == (1.5 * PI)):
        pti = [(pt3[1] - pt1[1]) * math.tan(azimuth12) + pt1[0], pt3[1]]
    elif (azimuth34 == azimuth12):
        return []
    else:
        y = (math.tan(azimuth34) * pt3[1] - math.tan(azimuth12) * pt1[1] + pt1[0] 
            - pt3[0]) / (math.tan(azimuth34) - math.tan(azimuth12))
        x = (y - pt1[1]) * math.tan(azimuth12) + pt1[0]
        pti = [x, y]
    
    dist1ni = calcDistance(pt1, pti)
    dist2ni = calcDistance(pt2, pti)
    dist3ni = calcDistance(pt3, pti)
    dist4ni = calcDistance(pt4, pti)
    if (math.abs((dist1ni + dist2ni) - distance12) <= TOLERANCE) and (math.abs((dist3ni + dist4ni) - distance34) <= TOLERANCE):
        return pti
    else:
        return []
    
def calcIntersectionAzimuthAzimuth(pt1,azimuth1,pt2,azimuth2):
  '''Azimuth is radian'''
  n1=pt1[0] #N
  e1=pt1[1] #E
  n2=pt2[0]
  e2=pt2[1]
  delty=n2-n1
  deltx=e2-e1
  ni=(math.tan(azimuth2)*n2-math.tan(azimuth1)*n1+e1-e2)/(math.tan(azimuth2)-
     math.tan(azimuth1))
  ei=(ni-n1)*math.tan(azimuth1)+e1
  return [ni,ei]
  
def equation_plane(y1, x1, z1, y2, x2, z2, y3, x3, z3):  
  '''Function to find equation of plane.'''
  a1 = x2 - x1 
  b1 = y2 - y1 
  c1 = z2 - z1 
  a2 = x3 - x1 
  b2 = y3 - y1 
  c2 = z3 - z1 
  a = b1 * c2 - b2 * c1 
  b = a2 * c1 - a1 * c2 
  c = a1 * b2 - b1 * a2 
  d = (- a * x1 - b * y1 - c * z1) 
  # plane equation is ax + by + cz + d = 0
  return [a,b,c,d]

def shortest_distance_point_to_plane(y1, x1, z1, a, b, c, d):  
  '''Function to find distance.
     3D point to plane.'''
  d = abs((a * x1 + b * y1 + c * z1 + d))  
  e = math.sqrt(a * a + b * b + c * c)
  return d/e 
    
def distance_point_to_3Dline(y1,x1,z1,y2,x2,z2,y3,x3,z3):
  '''3D point y1,x1,z1
     3D line from y2,x2,z2 to y3,x3,z3'''
  b = math.sqrt((x2-x3)**2+(y2-y3)**2+(z2-z3)**2)
  S = math.sqrt(((y2-y1)*(z3-z1)-(z2-z1)*(y3-y1))**2 +
                ((z2-z1)*(x3-x1)-(x2-x1)*(z3-z1))**2 +
                ((x2-x1)*(y3-y1)-(y2-y1)*(x3-x1))**2) / 2.0
  return 2*S/b

def calcExtend3DLineCoords(n1,e1,z1,n2,e2,z2,dist):
  lenAB = math.sqrt((n2-n1)**2+(e2-e1)**2+(z2-z1)**2)
  n3=n2+(n2-n1)/lenAB*dist
  e3=e2+(e2-e1)/lenAB*dist
  z3=z2+(z2-z1)/lenAB*dist
  return [n3,e3,z3]

def isect_line_plane_v3_4d(p0, p1, plane, epsilon=1e-6):
  u = sub_v3v3(p1, p0)
  dot = dot_v3v3(plane, u)
  if abs(dot) > epsilon:
    # Calculate a point on the plane
    # (divide can be omitted for unit hessian-normal form).
    p_co=mul_v3_fl(plane,-plane[3]/len_squared_v3(plane))
    w = sub_v3v3(p0, p_co)
    fac = -dot_v3v3(plane, w)/dot
    ang=math.acos(dot/(math.sqrt(len_squared_v3(plane))*math.sqrt(len_squared_v3(u))))
    if ang>math.pi/2.0:
      ang-=math.pi/2.0
    u = mul_v3_fl(u, fac)
    return add_v3v3(p0, u),ang
  else:
    return None,None

def area_by_shoelace(x, y):
  '''Assumes x,y points go around the polygon in one direction'''
  return abs(sum(i * j for i, j in zip(x, y[1:] + y[:1]))
            -sum(i * j for i, j in zip(x[1:] + x[:1], y))) / 2

# generic math functions
def add_v3v3(v0, v1):
  return (
  v0[0] + v1[0],
  v0[1] + v1[1],
  v0[2] + v1[2],
  )

def sub_v3v3(v0, v1):
  return (
    v0[0] - v1[0],
    v0[1] - v1[1],
    v0[2] - v1[2],
  )

def dot_v3v3(v0, v1):
  return (
    (v0[0] * v1[0]) +
    (v0[1] * v1[1]) +
    (v0[2] * v1[2])
    )

def len_squared_v3(v0):
  return dot_v3v3(v0, v0)

def mul_v3_fl(v0, f):
  return (
    v0[0] * f,
    v0[1] * f,
    v0[2] * f,
    )

Leave a Reply

Your email address will not be published. Required fields are marked *