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

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

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

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

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

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

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

สำหรับโปรแกรมพื้นฐานงานสำรวจในชุดนี้จะจัดโปรแกรมย่อยเล็กๆ ไว้ 4 โปรแกรม

  1. Center Circle(1Pt) เมื่อทราบค่าพิกัด 1 จุดบนเส้นรอบวง ในภาคสนามจะหาค่าพิกัดจุดศูนย์กลางของวงกลม as-built ของเสา โดยการตั้งกล้องแล้วเล็งเป้าหลังมาแตะที่ขอบวงกลมด้านซ้ายวัดมุมและกวาดมาแตะขอบวงกลมด้านขวาวัดมุม ป้อนตัวเลขโปรแกรมจะบอกมุมให้เล็งมาที่กลางเสาทำการวัดระยะทาง โปรแกรมจะคำนวณค่าพิกัดจุดศูนย์กลางของวงกลมและรัศมีของเสาให้
  2. Center Circle(2Pt) เมื่อทราบค่าพิกัด 2 จุด บนเส้นรอบวง และทราบรัศมี สามารถคำนวณหาค่าพิกัดจุดศูนย์กลางได้ แต่จะมีวงกลมสองวงที่ผ่านจุดนี้ ดังนั้นคำตอบจะมีสองค่า
  3. Center Circle(3Pt) เมื่อทราบค่าพิกัด 3 จุด บนเส้นรอบวง ในภาคสนามที่ประยุกต์ใช้กันจะเป็นลักษณะงานก่อสร้างเจาะเสาเข็มที่ผมกล่าวไว้ข้างต้น ช่างสำรวจวัดค่าพิกัดบนเส้นรอบวงของเคสสิ้ง กะระยะให้ห่างเท่าๆกันทั้งสามจุด ทำมุมสามเหลี่ยมประมาณ 60 องศาเพื่อลดความคลาดเคลื่อน
  4. Center Circle(>3pt) เมื่อทราบค่าพิกัดมากกว่า 3 จุด ในกรณีนี้การคำนวณจะใช้วิธี least squares มาช่วยในการคำนวณ ตัวอย่างการประยุกต์ใช้งานเช่นการเก็บแท๊งค์วงกลมขนาดใหญ่ในโรงกลั่นแก๊สหรือน้ำมัน เราจะเก็บจุดมารอบๆ อาจจะ 10 จุดหรือถึง 100 จุดเพื่อมาคำนวณหารูปวงกลมที่ฟิต (Circle fit) ดีที่สุด ในสมัยนี้การวัดระยะทางแบบ Reflector-less ทำได้ง่าย

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

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

Center Circle(1Pt)

มาดูตัวอย่างจากรูปเพื่อให้เข้าใจได้ง่าย จุดตั้งกล้องมีค่า N=2643023.600, E=231848.291 เป้าหลัง N=2643030.274, E=231835.941วัดมุมไปที่จุดสัมผัสเสาเข็ม(สีเหลือง) 93° 7′ 26″ แล้ววัดมุมไปที่จุดสัมผัสเสาเข็มด้านขวาอ่านมุมได้ 115° 4′ 21″ เปิดมุมไปตรงกลางเสา (มุมคำนวณโดยโปรแกรม) 104° 5′ 54″ วัดระยะทางได้ 4.253 เมตร หาค่าพิกัดจุดศูนย์กลางเสาเข็มและคำนวณหารัศมี

ตัวอย่าง Center Circle (1 point)

กดคีย์ “1” และกดคีย์ “EXE” ป้อนค่าตามรูปด้านล่าง การป้อนมุมให้คีย์เครื่องหมายลบ (-) คั่น ไม่สามารถใช้รูปแบบป้อนมุมแบบเครื่องคิดเลขได้เพราะคาสิโอไม่เปิดฟังก์ชั่นให้ใช้งาน ในภาคสนามหลังจากป้อนมุมสัมผัสด้านซ้ายและด้านขวาโปรแกรมจะคำนวณมุมให้ 104° 5′ 53.5″ แล้วทำการวัดระยะทาง

จะได้ค่าพิกัดจุดศูนย์กลางของเสาเข็ม N=2643027.474, E=231851.839 รัศมีเสาเข็ม 1.0 เมตร

Center Circle(2Pt)

ในกรณีที่ทราบค่าพิกัดของจุดบนเส้นรอบวงกลม 2 จุด และทราบค่ารัศมีมาลอง จุดแรก N=2643014.220, E=231857.003 จุดที่ 2 N=2643013.181, E=231856.325 รัศมีวงกลม 1 เมตร

ตัวอย่าง Center Circle (2 points)

จากเมนูกดคีย์ “2” และกดคีย์ “EXE” ป้อนค่า จะได้ผลลัพธ์ออกมาเป็นวงกลมสองวงคือวงกลมสีม่วงมีค่าพิกัดที่ศูนย์กลางวงกลมคือ N=2643013.272, E=231857.321 วงกลมที่ 2 คือวงกลมสีน้ำเงิน N=2643014.129, E=231856.007

Center Circle(3Pt)

ในกรณีทราบค่าพิกัดจุดบนเส้นรอบวง 3 จุด จะสามารถหาค่าพิกัดจุดศูนย์กลางและรัศมีของวงกลมได้ ตัวอย่าง ค่าพิกัดจุดที่ 1 N=2643007.197, E=231878.894 ค่าพิกัดจุดที่ 2 N=2643004.825,
E=231881.448 ค่าพิกัดจุดที่ 3 N=2643004.037, E=231877.94

ตัวอย่าง Circle Center (3 points)

จากเมนูกดคีย์ “3” และกดคีย์ “EXE” ป้อนค่า จะได้ผลลัพธ์ออกมาค่าพิกัดจุดศูนย์กลาง ตลอดจนรัศมีวงกลมและขนาดเส้นผ่านศูนย์กลาง ค่าพิกัดของจุดศูนย์กลางคือ N=2643005.292, E=231879.503 รัศมี = 2.000 เมตร เส้นผ่าศูนย์กลาง 4.001 เมตร

การประยุกต์ใช้งานในการหาตำแหน่งปักเคสสิ้งเสาเข็ม

Center Circle(>3pt)

ถ้าจุดบนเส้นรอบวงเกิน 3 จุด การคำนวณจะยุ่งยากขึ้นมาทันที ในกรณนี้เราจะต้องใช้วิธีคำนวณแบบ least squares ในบางครั้งการคำนวณแบบนี้จะเรียกว่า Circle Fit by least squares วิธีอื่นที่ผมเห็นมาคือวิธี Circle Fit by algebraic ผมขอเน้นแบบ least squares เพราะว่าคุ้นเคยและให้ค่าดีกว่า ผมไปดัดแปลงโค้ดภาษาซีเป็นภาษาไพทอนจากลิ๊งค์นี้ เครดิต: Mark J. Kilgard มาดูตัวอย่าง

จากเมนูกดคีย์ “4” และกดคีย์ “EXE” โปรแกรมจะถามจำนวนจุดตอบ จากตัวอย่างมีทั้งหมด 11 จุด จากนั้นโปรแกรมจะเริ่มถามค่าพิกัดจุดที่1 ก็ป้อน N1=4935.6091, E1=5592.9047 จนถึงจุดที่ 11 N11=4930.2298, E11=5582.0201 จากนั้นโปรแกรมก็คำนวณค่าพิกัดให้จากวิธี least squares วนรอบ 22 ครั้งถึงได้คำตอบ จะได้ค่าพิกัดศูนย์กลางที่ฟิตที่สุด N=4915.8127, E=5595.8432 รัศมี 19.9945 เมตร ถ้าขนาดจริงในแบบก่อสร้างรัศมี 20 เมตร ต่างกันประมาณ 5 มม. อาจเป็นไปได้ว่าก่อสร้างไม่ได้ตามแบบเป๊ะหรือไม่อาจจะมีการบิดรูปของโครงสร้าง (deformation) ก็ต้องศึกษากันต่อไป

สรุป

สำหรับโปรแกรมย่อยที่ 4 ผมไม่ได้ตรวจหา error สมมติว่าจุดที่กระโดดออกไม่เข้ากลุ่มอาจจะเกิดจากความผิดพลาดตอนสำรวจหรือคีย์ข้อมูลผิดดูตัวอย่างภาพด้านล่างจะเห็นจุดที่ 8 และจุดที่ 15 ผมจะเพิ่มเติมให้ในอนาคต ส่วนโปรแกรมย่อยจาก 1 ถึง 3 นั้นเป็นพื้นฐานมากๆ โปรดติดตามโปรแกรมพื้นฐานงานสำรวจชุดที่ 3 ต่อไป (COGO Serie 3) ว่าด้วยเรื่องการ offset จุด

เครดิตภาพ: stackoverflow.com

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

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

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

from pbrutils import split_angle,deg2dms,DMS2str
from pbrutils import calcAzimuth,calcDistance,calcDiff2Angle
from pbrutils import calcNextAzimuth,calcNextCoordinate2D
from pbrutils import calcNextCoordinate3D
from pbrutils import PI,DEG2RAD,RAD2DEG,mean
import math

MAXITERATIONS = 256
TOLERANCE = 1e-06

def print_menu():
  print('COGO Selected Serie 2')
  print('1:Center Circle(1Pt)')
  print('2:Center Circle(2pt)')
  print('3:Center Circle(3pt)')
  print('4:Center Circle(>3pt)')
  print('5:Exit')

def center_circle_1pt():
  nbs=float(input("N-BS=?" ))
  ebs=float(input("E-BS=?" ))
  nin=float(input("N-Instr=?" ))
  ein=float(input("E-Instr=?" ))
  ang1=input("Lt H Angle=?")
  hang1=split_angle(ang1)*DEG2RAD 
  ang2=input("Rt H Angle=?")
  hang2=split_angle(ang2)*DEG2RAD 
  hang=hang2-hang1 if hang2-hang1>=0 else 2*math.pi+hang2-hang1
  theta = hang/2.0
  print("Point to mid angle.")
  azi12=calcAzimuth(nbs,ebs,nin,ein)
  midhang=hang1+theta if hang1+theta<2*math.pi else hang1+theta-2*math.pi
  sn,d,m,s=deg2dms(midhang*RAD2DEG)
  st=DMS2str(d,m,s,2)
  print("Mid Angle=",st) 
  hdist=float(input("H distance=?"))
  radius=(math.sin(theta)*hdist)/(1-math.sin(theta))
  print("Radius={0:.3f} m".format(radius))
  nc,ec=calcNextCoordinate2D(nin,ein,azi12,midhang,hdist+radius)
  print("N Center=","{0:.3f} m".format(nc))
  print("E Center=","{0:.3f} m".format(ec))
  input("Press EXE key")  

def center_circle_2pt():
  print("Enter known 2 points.")
  n1=float(input("N1=?" ))
  e1=float(input("E1=?" ))
  n2=float(input("N2=?" ))
  e2=float(input("E2=?" ))
  r=float(input("Radius=?"))
  q=math.sqrt((e2-e1)**2+(n2-n1)**2)
  e3=(e1+e2)/2.0
  n3=(n1+n2)/2.0
  ec1=e3+math.sqrt(r**2-(q/2.0)**2)*(n1-n2)/q
  nc1=n3+math.sqrt(r**2-(q/2.0)**2)*(e2-e1)/q
  ec2=e3-math.sqrt(r**2-(q/2.0)**2)*(n1-n2)/q
  nc2=n3-math.sqrt(r**2-(q/2.0)**2)*(e2-e1)/q
  print("First circle:")
  print("NC=","{0:.3f} m".format(nc1))
  print("EC=","{0:.3f} m".format(ec1))
  print("Second circle:")
  print("NC=","{0:.3f} m".format(nc2))
  print("EC=","{0:.3f} m".format(ec2))
  input("Press EXE key")  
  
def center_circle_3pt():
  print("Enter known 3 points.")
  y1=float(input("N1=?" ))
  x1=float(input("E1=?" ))
  y2=float(input("N2=?" ))
  x2=float(input("E2=?" ))
  y3=float(input("N3=?" ))
  x3=float(input("E3=?" ))
  v_dA = x2 - x1
  v_dB = y2 - y1
  v_dC = x3 - x1
  v_dD = y3 - y1
  v_dE = v_dA * (x1 + x2) + v_dB * (y1 + y2)
  v_dF = v_dC * (x1 + x3) + v_dD * (y1 + y3)
  v_dG = 2.0  * (v_dA * (y3 - y2) - v_dB * (x3 - x2))
  # If v_dG is zero then the three points are collinear and no finite-radius
  # circle through them exists.
  if (v_dG == 0):
    print("3 pts are collinear.")
    print("Cannot solve!!!")
  else:
    xc = (v_dD * v_dE - v_dB * v_dF) / v_dG
    yc = (v_dA * v_dF - v_dC * v_dE) / v_dG
    radius = math.sqrt((x1-xc)**2 + (y1-yc)**2)
  print("Center of Circle:")
  print("N= {0:.3f} m".format(yc))
  print("E= {0:.3f} m".format(xc))
  print("Radius= {0:.3f} m".format(radius))
  print("Diameter= {:.3f} m".format(2*radius))
  input("Press EXE key") 

def circlefit_leastsquares():
  num=int(input("How many points?"))
  ns=[]
  es=[]
  for i in range(num):
    ns.append(float(input("N{:n}=?".format(i+1))))
    es.append(float(input("E{:n}=?".format(i+1))))
  x_m=mean(es)
  y_m=mean(ns)
    
  a=x_m
  b=y_m
  for i in range(MAXITERATIONS):
    a0=a
    b0=b
    # compute average L, dL/da, dL/db 
    LAvr=0.0
    LaAvr=0.0
    LbAvr=0.0
    for x,y in zip(es,ns):
      dx=x - a
      dy=y - b
      L=math.sqrt(dx**2 + dy**2)
      if (abs(L) > TOLERANCE):
        LAvr+=L
        LaAvr-=dx / L
        LbAvr-=dy / L
    LAvr/=num;
    LaAvr/=num;
    LbAvr/=num;

    a=x_m + LAvr * LaAvr
    b=y_m + LAvr * LbAvr
    r=LAvr

    if (abs(a - a0) <= TOLERANCE and abs(b - b0) <= TOLERANCE):
      break
  if (i < MAXITERATIONS):    
    print("Iterations= {:n}".format(i+1))
    print("Circle center:")
    print("N= {0:.4f} m".format(b))
    print("E= {0:.4f} m".format(a)) 
    print("Radius= {0:.4f} m".format(r))
    print("Diameter= {:.4f} m".format(2*r))
  else:
    print("Cannot find solution!!!")
  input("Press EXE 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
    center_circle_1pt()
    ex_choice=1
  elif (choice==2):
    loop=True
    center_circle_2pt()
    ex_choice=2
  elif (choice==3):
    center_circle_3pt()
    loop=True
    ex_choice=3
  elif (choice==4):
    loop=True
    circlefit_leastsquares()
    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 *