Tag: engineer

ทดสอบเขียนโปรแกรมไพทอน (Python)  บนเครื่องคิดเลข Casio fx-cg50 Prizm

ทดสอบเขียนโปรแกรมไพทอน (Python) บนเครื่องคิดเลข Casio fx-cg50 Prizm

ไพทอนบนเครื่องคิดเลข

ช่วงนี้ผมมีโอกาสทำงานใกล้ชิดกับภาคสนาม ทำให้มีโอกาสได้จับและใช้เครื่องคิดเลขมากกว่าปกติ ในเวลาที่ผ่านมาไม่ถึงเดือนผมได้ซื้อเครื่องคิดเลข Casio fx-CG50 Prizm เคสสีขาว ที่ซื้อมาเพราะทราบว่าถ้า update OS เป็นรุ่น 3.20 จะสามารถใช้ ไพทอน (Python) ได้ ก็ขอหมายเหตุสักนิดว่าเป็นไมโครไพทอน (Micropython) ที่ทางทีมงาน Micropython ได้พอร์ตออกมาให้มีขนาดเล็กเพื่อเอาไปรันในบอร์ด iOT ได้ หรือบอร์ดที่เป็นไมโครคอนโทรลเลอร์ทั้งหลาย เน้นขนาดเล็ก หน่วยความจำต่ำ กินไฟน้อย ต่ออินเทอร์เน็ตได้ในตัว ผมจะไม่มุ่งไปทางนี้หรอกครับ ในบทความนี้ แต่จะพูดถึงเครื่องคิดเลขคาสิโอ ที่นำเอาไมโครไพทอนมาลงเครื่องคิดเลขรุ่นนี้ เพราะว่าไมโครไพทอนกินหน่วยความจำต่ำ ก็เลยเหมาะสมที่จะเอามารันในเครื่องคิดเลขที่มีทรัพยากรต่ำอยู่แล้ว ให้เกิดประสิทธิภาพมากยิ่งขึ้นไป

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

สำหรับเครื่องคิดเลขที่ใช้ในแวดวงวิศวกรรม โปรแกรมที่เขียนด้วยไมโครไพทอนที่มีไลบรารี math หรืออาจจะเสริมด้วยไลบรารีพวกเมตริก (matrix) น่าจะพอนำมาเขียนใช้งานกันได้

 การพัฒนาโปรแกรมด้วยชุดพัฒนาโปรแกรมภาษาซี (Software Development Kit)

นี่เป็นความข้องใจของผมในฐานะแฟนเครื่องคิดเลขคาสิโอ รุ่น fx-9860G ทางคาสิโอจัดทำ SDK ให้สามารถเขียนโปรแกรมด้วยภาษาซี ที่ผมเขียนโปรแกรมมาแจกใช้ในแวดวงงานสำรวจแล้วหลายโปรแกรม แต่รุ่นนี้กลับไม่ทำมาให้  (ที่จริงไม่ทำมาให้ตั้งแต่ fx-CG10/fx-CG20)ไม่ทราบว่าเพราะเหตุใด เครื่องคิดเลขรุ่นนี้ไม่มี SD Card แต่ทดแทนด้วยการใส่ Flash memory มา 16 MB  ซึ่งก็พอจะใส่โปรแกรมใช้งานได้มากโขอยู่ หรือคิดว่ามีไมโครไพทอน มาให้แล้วน่าจะตอบโจทย์ได้หมด แต่ผมก็ไม่คิดอย่างนั้น ยังมีโปรแกรมเมอร์ภาษาซีอีกพอสมควร และในสภาพแวดล้อมของเครื่องคิดเลขจริงๆ โปรแกรมที่เขียนด้วยภาษาซีจะเร็วกว่าไพทอนอยู่แล้ว แต่ไพทอนได้เปรียบในด้านความง่าย

 เครื่องมือพัฒนาโปรแกรมของชุมชน

ยังมีชุมชนของนักพัฒนาที่สร้าง SDK  ขึ้นมาใช้งานเอง มีประมาณ 2-3 กลุ่มแต่สุดท้ายดูเหมือนไม่มีความเคลื่อนไหวกันมาหลายปีแล้ว เครื่องมือที่มีชื่อเสียงมากที่สุดคือ PrizmSDK และอีกอันคือ mini-SDK ผมเองใช้ไลบรารี MyLib แต่เผอิญผู้พัฒนาได้ทำไว้สำหรับเครื่อง fx-9860G เท่านั้น ไม่เป็นไรขอมุ่งลองไพทอนบนเครื่องคิดเลขรุ่น fx-CG50 นี้ก่อน ถ้าพัฒนาโปรแกรมด้วยภาษาซี ผมก็ยังมุ่งไปที่เครื่องคิดเลข fx-9860G เหมือนเดิม 

ผมลองเขียนโปรแกรมทดสอบเล็กๆลองดูด้วยเครื่องมือ PrizmSDK ก็ได้ดังรูปข้างล่าง (โปรแกรมไม่มีอะไรมีแต่เมนู) เทียบกับโปรแกรม System Manager ที่มากับเครื่อง

คุณสมบัติของเครื่องคิดเลข

โดยรวมรวมแล้วเครื่องคิดเลขนั้นเหมาะสำหรับนักศึกษามาก เพราะมีฟังก์ชั่นคณิตศาสตร์ การเงิน สถิติ มีกราฟมากมายให้ใช้ แต่สำหรับผมแล้วไม่มีอะไรต้องใช้เลย ยกเว้นเรื่องโปรแกรมบนเครื่องคิดเลขอย่างเดียว ถ้าไม่มีสิ่งนี้ก็ใช้เป็นที่ทับกระดาษได้เลย เครื่องรุ่นนี้ใช้โปรเซสเซอร์ตระกูล SH4 ขนาดหน้าจอ 384 x 216 จอ LCD จำนวนสี 65000 สี มีความสว่างพอสมควรและปรับได้ ความกว้างหน้าจอแบบทะแยง 3.17 นิ้ว หน่วยความจำของเครื่อง 60 KB มี Flash memory ที่สามารถเขียนอ่านได้ 16  MB ซึ่งจะเป็นที่เอาไว้เก็บโปรแกรมหรือข้อมูล ใช้ถ่าน AAA 4 ก้อน  เท่าที่ผมเปิดเครื่องใช้บ้างในแต่ละวันมาประมาณสองสัปดาห์ พบว่าแบตเตอรี่ลดลงมานิดหนึ่ง อนาคตอาจจะหาถ่านชาร์จมาใช้ ตอนนี้ใส่อัลคาไลน์ไปก่อน

ประเดิมโปรแกรมด้วยไพทอน

จะลองโปรแกรมทั้งทีผมพยายามให้โปรแกรมมีขนาดซับซ้อนมานิดหนึ่ง  และเรียกใช้โมดูลด้วย คิดไปคิดมาก็เลยจะลองโปรแกรมแปลงค่าพิกัดระหว่างค่าพิกัดภูมิศาสตร์กับค่าพิกัดยูทีเอ็ม เนื่องจากไม่ค่อยมีเวลาเขียน เลยลองหาไลบรารีที่ท่านอื่นได้ทำไว้ ผมเคยเกริ่นไปแล้วข้างต้นว่าไลบรารีรุ่นใหญ่เช่น pyproj ไม่สามารถเอามาใช้ได้ ลองค้นดูพบว่ามีไลบรารีไพทอนเล็กๆ ชื่อ utm มีสัญญาอนุญาตเป็น MIT-License ผมเอาโค้ดมาดัดแปลงนิดหน่อยให้เหมาะสมกับเครื่องคิดเลข แล้วเขียนไปอยู่ในไฟล์ utm.py เพื่อให้สะดวกเวลาเรียกใช้

การเขียนโปรแกรมไพทอน ถ้าไปเขียนบนเครื่องคิดเลข จะชักช้าเสียเวลาครับ เนื่องจากไมโครไพทอน พอร์ตไลบรารีเช่น math แล้ว ดังนั้นถ้าโปรแกรมของเราไม่ได้ใช้อะไรพิศดารมาก ก็สามารถมาเขียนโค้ดบน PyCharm หรือ Idle ได้ ผมเลือก PyCharm เมื่อทดสอบโปรแกรมเสร็จสามารถ โอนโปรแกรมเข้าไปไว้ในเครื่องด้วยการต่อเครื่องคิดเลขด้วยสาย USB เข้ากับคอมพิวเตอร์ที่รันวินโดส์ตามผังด้านล่าง

เมื่อรันโปรแกรมได้ตามความต้องการแล้วจากนั้นก็เอาเครื่องคิดเลขมาต่อกับคอมพิวเตอร์ วินโดส์จะมองเห็นเป็นไดรว์ สามารถใช้ File Explorer ก๊อปปี้โปรแกรมจากคอมพิวเตอร์ไปยังเครื่องคิดเลขได้ ตัวโค้ดโปรแกรมไลบรารีดูได้ด้านล่างครับ


import math
K0 = 0.9996

E = 0.00669438
E2 = E * E
E3 = E2 * E
E_P2 = E / (1.0 - E)

SQRT_E = math.sqrt(1 - E)
_E = (1 - SQRT_E) / (1 + SQRT_E)
_E2 = _E * _E
_E3 = _E2 * _E
_E4 = _E3 * _E
_E5 = _E4 * _E

M1 = (1 - E / 4 - 3 * E2 / 64 - 5 * E3 / 256)
M2 = (3 * E / 8 + 3 * E2 / 32 + 45 * E3 / 1024)
M3 = (15 * E2 / 256 + 45 * E3 / 1024)
M4 = (35 * E3 / 3072)

P2 = (3. / 2 * _E - 27. / 32 * _E3 + 269. / 512 * _E5)
P3 = (21. / 16 * _E2 - 55. / 32 * _E4)
P4 = (151. / 96 * _E3 - 417. / 128 * _E5)
P5 = (1097. / 512 * _E4)

R = 6378137

ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX"

class OutOfRangeError(ValueError):
    pass

def to_latlon(easting, northing, zone_number, hemi):

    northern = (hemi == 'N')

    x = easting - 500000
    y = northing

    if not northern:
        y -= 10000000

    m = y / K0
    mu = m / (R * M1)

    p_rad = (mu +
             P2 * math.sin(2 * mu) +
             P3 * math.sin(4 * mu) +
             P4 * math.sin(6 * mu) +
             P5 * math.sin(8 * mu))

    p_sin = math.sin(p_rad)
    p_sin2 = p_sin * p_sin

    p_cos = math.cos(p_rad)

    p_tan = p_sin / p_cos
    p_tan2 = p_tan * p_tan
    p_tan4 = p_tan2 * p_tan2

    ep_sin = 1 - E * p_sin2
    ep_sin_sqrt = math.sqrt(1 - E * p_sin2)

    n = R / ep_sin_sqrt
    r = (1 - E) / ep_sin

    c = _E * p_cos**2
    c2 = c * c

    d = x / (n * K0)
    d2 = d * d
    d3 = d2 * d
    d4 = d3 * d
    d5 = d4 * d
    d6 = d5 * d

    latitude = (p_rad - (p_tan / r) *
                (d2 / 2 -
                 d4 / 24 * (5 + 3 * p_tan2 + 10 * c - 4 * c2 - 9 * E_P2)) +
                 d6 / 720 * (61 + 90 * p_tan2 + 298 * c + 45 * p_tan4 - 252 * E_P2 - 3 * c2))

    longitude = (d -
                 d3 / 6 * (1 + 2 * p_tan2 + c) +
                 d5 / 120 * (5 - 2 * c + 28 * p_tan2 - 3 * c2 + 8 * E_P2 + 24 * p_tan4)) / p_cos

    return (180/math.pi*(latitude),
            180/math.pi*(longitude) + zone_number_to_central_longitude(zone_number))


def from_latlon(latitude, longitude, force_zone_number=None):
    if not -80.0 <= latitude <= 84.0:
        raise OutOfRangeError('latitude out of range (must be between 80 deg S and 84 deg N)')
    if not -180.0 <= longitude <= 180.0: raise OutOfRangeError('longitude out of range (must be between 180 deg W and 180 deg E)') lat_rad = math.pi/180*(latitude) lat_sin = math.sin(lat_rad) lat_cos = math.cos(lat_rad) lat_tan = lat_sin / lat_cos lat_tan2 = lat_tan * lat_tan lat_tan4 = lat_tan2 * lat_tan2 if force_zone_number is None: zone_number = latlon_to_zone_number(latitude, longitude) else: zone_number = force_zone_number #zone_letter = latitude_to_zone_letter(latitude) if (latitude >= 0):
      hemi = 'N'
    else:
      hemi = 'S'

    lon_rad = math.pi/180*(longitude)
    central_lon = zone_number_to_central_longitude(zone_number)
    central_lon_rad = math.pi/180*(central_lon)

    n = R / math.sqrt(1 - E * lat_sin**2)
    c = E_P2 * lat_cos**2

    a = lat_cos * (lon_rad - central_lon_rad)
    a2 = a * a
    a3 = a2 * a
    a4 = a3 * a
    a5 = a4 * a
    a6 = a5 * a

    m = R * (M1 * lat_rad -
             M2 * math.sin(2 * lat_rad) +
             M3 * math.sin(4 * lat_rad) -
             M4 * math.sin(6 * lat_rad))

    easting = K0 * n * (a +
                        a3 / 6 * (1 - lat_tan2 + c) +
                        a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2)) + 500000

    northing = K0 * (m + n * lat_tan * (a2 / 2 +
                                        a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c**2) +
                                        a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - 330 * E_P2)))

    if latitude < 0:
        northing += 10000000

    return easting, northing, zone_number, hemi


def latitude_to_zone_letter(latitude):
    if -80 <= latitude <= 84: return ZONE_LETTERS[int(latitude + 80) >> 3]
    else:
        return None


def latlon_to_zone_number(latitude, longitude):
    if 56 <= latitude < 64 and 3 <= longitude < 12:
        return 32

    if 72 <= latitude <= 84 and longitude >= 0:
        if longitude <= 9:
            return 31
        elif longitude <= 21:
            return 33
        elif longitude <= 33:
            return 35
        elif longitude <= 42:
            return 37

    return int((longitude + 180) / 6) + 1


def zone_number_to_central_longitude(zone_number):
    return (zone_number - 1) * 6 - 180 + 3


โปรแกรมแปลงพิกัดภูมิศาสตร์ในภาคไพทอน

ผมเขียนไพทอนเป็นโมดูลอีกโมดูลเพื่อเรียกใช้ไลบรารี ตั้งชื่อว่า UTM2GEO.py โดยที่เขียนเมนูติดต่อการใช้งานง่ายๆ

from utm import *
   
def print_menu():
  print(5*'-',"MENU",5*'-')
  print('1: UTM to GEO')
  print('2: GEO to UTM')
  print('0: Exit')

def geo2utm(lon,lat):
  east,north,zn,hem=from_latlon(lat,lon)
  print("North={0:11.3f}".format(north))
  print("East={0:10.3f}".format(east))  
  print("UTM Zone No={0:0d}{1}".format(zn,hem))
    
def utm2geo(e,n,zoneno,hemi):
  lat,lon=to_latlon(e,n,zoneno,hemi)
  print("Latitude={0:11.7f}".format(lat))
  print("Longitude={0:10.7f}".format(lon))   
    
loop=True
while loop:   
  print_menu()
  choice=int(input('Selection[0-2]'))
  if (choice==0):
    loop=False
  elif (choice==1):
    loop=True
    y=float(input("Northing="))
    x=float(input("Easting="))
    zn=int(input("Zone No="))
    hem=input("Hemi (N/S)=")
    utm2geo(x,y,zn,hem)
  elif (choice==2):
    loop=True
    y=float(input("Latitude="))
    x=float(input("Longitude="))
    geo2utm(x,y)   

วิธีก๊อปปี้โปรแกรม

จากนั้นผมก๊อปปี้สองไฟล์คือ utm.py  และ UTM2GEO.py ลงบนไดรว์เครื่องคิดเลขดังนี้

ผมเก็บไว้ที่ไดเรคทอรี \SAVE-F\PROGRAM เวลาจะถอดสาย USB เพื่อเลิกการเชื่อมต่อต้อง Safely removal โดยการคลิกเมาส์ขวา “Eject” ที่ File Explorer จากนั้นมาที่เครื่องคิดเลขจากให้กดคีย์ “EXE” และ “EXIT” ตามลำดับ ถ้าไม่ทำไฟล์อาจจะไม่ได้ซิงค์กันอาจจะหายหรือไม่สมบูรณ์ได้ ที่เครื่องคิดเลขกดคีย์ “MENU” เลือก “Python

แปลงพิกัดจากค่าพิกัดภูมิศาสตร์เป็นค่าพิกัดยูทีเอ็ม

จากรูปด้านบนกดคีย์ F1-Run จะเห็นหน้าจอขึ้นเมนูติดต่อมาง่ายๆ

เราจะเลือกแปลงพิกัดจากค่าพิกัดภูมิศาสตร์ไปเป็นค่าพิกัดยูทีเอ็มเลือกกดคีย์ “2” ที่เครื่องคิดเลขแล้วกดคีย์ “EXE” ป้อนค่าพิกัด Latitude = 39.95259668 Longitude  = -75.15132081 (ป้อนเป็นหน่วยดีกรี ในตอนนี้ยังไม่รับค่าแบบอื่น) จะได้ผลการคำนวณออกมา เนื่องจากในตอนนี้ไม่มีคำสั่งเบรคการแสดงผลเมื่อเขียนด้วยไพทอน (เอาละจะมาบ่นทีหลัง ว่าใส่ไพทอนมาแล้วทางคาสิโอไม่ให้เครื่องมืออะไรมาเลย) การจะดูผลลัพธ์ ผู้ใช้ต้องกดคีย์ “0” เพื่อออกจากโปรแกรมและใช้ลูกศรกดขึ้นไปทางด้านบนเพื่อไปดูผลลัพธ์

จะได้ค่า Northing = 4422506.896 Easting = 487074.371 อยู่ในโซน 18N

แปลงพิกัดจากค่าพิกัดยูทีเอ็มเป็นค่าพิกัดภูมิศาสตร์

ทำการรันโปรแกรมใหม่อีกครั้ง ที่เมนูเลือกกดเลข “1” ป้อนค่าพิกัด Northing = 2642783.110 Easting =232030.949 UTM Zone No = 46 Hemi = N

ดูค่าผลลัพธ์ได้ (กดคีย์ “0” ออกจากเมนูก่อนแล้วเลื่อนขึ้นไปดู)

สรุปการใช้งาน

ตอนแรกผมคาดหวังจากที่ทางคาสิโอเอาไมโครไพทอนมาลงเครื่องคิดเลขรุ่นนี้ ยังไงการเขียนโปรแกรมใช้งานยังไงๆผลลัพธ์ที่ออกมาก็ต้องระดับเทพ เพราะไพทอนมันทรงพลังด้วยตัวของมัน แต่เมื่อลองแล้วผิดหวังมาก จนบัดนี้คาสิโอ้ยังไม่ได้ออกคู่มือแสดงฟังก์ชั่นที่ไพทอนสามารถเรียกมาใช้ได้ มีฟังก์ชั่น input กับ print สองฟังก์ชั่นนี้เท่านั้น เพียงแค่ผมค้นหาฟังก์ชั่น clear screen หน้าจอยังทำไม่ได้ ฟังก์ชั่นที่ต้องการสนับสนุนได้แก่การเขียนเมนูที่เรียกใช้ด้วยคีย์ F1 ถึง F6 การปริ๊นท์แสดงสีต่างๆ การเรียกใช้ฟังก์ชั่นกราฟต่างๆหรือพล็อทกราฟ หรือใช้งานเมตริก เป็นต้น

เอาละตอนนี้ไพทอนที่ปรากฎบน OS รุ่น 3.20 เพิ่งออกมาเตือนตุลาคม 2018 (ขณะที่เขียนบทความนี้เดือนพฤศจิกายน 2018) คงต้องให้เวลาสักพักว่าจะเป็นอย่างไร บอกตามตรงว่าคงต้องเอาเครื่องคิดเลขรุ่นนี้มาทับกระดาษอีกสักพักใหญ่ๆ

ติดปีกเครื่องคิดเลขเทพ Casio fx 9860G II SD ด้วยโปรแกรมภาษาซีบน AddIn ตอนที่ 5 โปรแกรมคำนวณ Resection ด้วยอัลกอริทึ่มสมัยใหม่

ติดปีกเครื่องคิดเลขเทพ Casio fx 9860G II SD ด้วยโปรแกรมภาษาซีบน AddIn ตอนที่ 5 โปรแกรมคำนวณ Resection ด้วยอัลกอริทึ่มสมัยใหม่

การเล็งสกัดย้อน (Resection) และความเป็นมา

ในที่สุดก็มาถึงตอนที่ 5 ตอนที่ผมใช้เวลามากที่สุดในการ implement อัลกอริทึ่มที่ใช้คำนวณปัญหา Resection จาก 3 จุดที่กำหนด (Three Points Resection Problem) เป็นที่ทราบกันดีว่าการคำนวณ Resection นั้นนักคณิตศาสตร์ได้คิดค้นกันมาหลายร้อยปีแล้ว มีอัลกอริทึ่มรวมๆกันไม่น้อยกว่า 500 อัลกอริทึ่ม แต่บางอัลกอริทึ่มนั้นอายุเก่าแก่มากใช้การคำนวณหาด้วยการวาดลงบนกระดาษ ถ้าจะคัดออกมาจริงๆที่ใช้กันในปัจจุบันมีประมาณ 18 อัลกอริทึ่มหลักๆ และสามารถนำมา implement เป็นโปรแกรมในคอมพิวเตอร์ได้ ก่อนจะไปต่อกันลึกๆมาดูกันว่า Resection คืออะไร

การเล็งสกัดย้อน(Resection) คือการวัดพิกัดจุดตั้งกล้องจากสถานีที่ทราบค่าพิกัด 3 สถานี ตามตัวอย่างได้แก A, B และ C และวัดมุมราบคือมุม α และ β ตามลำดับ

ผมคนรุ่นเก่ายังทันเครื่องมือวัดมุม Sextant ผมทัน Sextant นี้ในช่วงทำงานใหม่ๆ โดยที่ลงเรือไปในทะเลกับพี่ๆช่างสำรวจของกรมเจ้าท่า ตอนนั้นเพิ่งเรียนจบมาใหม่ ยุคนั้น GPS/GNSS ยังไม่เป็นที่รู้จัก การวัดตำแหน่งของเรือสำรวจใช้เครื่องมือ Sextant ที่อาศัยหลักการของ Resection มาประยุกต์ใช้ บนเรือสำรวจจะมีเจ้าหน้าที่ 2 คน คนแรกจะส่องสถานี A และ B เพื่อวัดมุม α และคนที่สองจะส่องสถานี B และ C เพื่อวัดมุม β สองคนนี้ตามหลักการแล้วต้องขี่คอกันแต่จริงๆคงไม่มีใครทำเพียงแต่นั่งใกล้ๆกัน การใช้ Sextant วัดตำแหน่งเรือต้องอาศัยความชำนาญอย่างสูง เพราะเรือไม่อยู่นิ่งกับที่เพราะคลื่มลม จะปะทะให้เคลื่อนไหวตลอดเวลา

เมื่อการวัดมุมเสร็จสิ้นลงทั้งสองคนจะจดค่ามุม ∝ และ ∅ พร้อมๆกัน การใช้ Sextant ควบคู่ไปกับกับใช้เครื่องมือวัดความลึกของท้องน้ำจำพวก Echo sounder งาน post processing ในออฟฟิศได้แต่การนำค่ามุม α และ β มาคำนวณหาค่าพิกัดแตละจุด จากนั้นก็จัดทำแผนที่แสดงความลึกของแม่น้ำหรือทะเลในบริเวณที่ทำการสำรวจ ถึงแม้กระนั้นเครื่องมือ Sextant จะให้ค่าความละเอียดด้านมุมไม่ดีนัก แต่ค่าพิกัดที่ได้สมัยนั้นก็เพียงพอสำหรับงานในทะเลหรือแม่น้ำ

หัวข้อต่อๆไปจะกล่าวถึงที่ไปที่มาของสูตรที่ผมใช้สำหรับเครื่องคิดเลข fx-9860G ถ้าผู้อ่านไม่สนใจก็ข้ามไปที่การใช้โปรแกรมเครื่องคิดเลขด้านท้ายๆเลยครับ

หลักการคำนวณ Resection

อัลกอริทึ่มที่ผมกล่าวไปนั้นตั้งแต่ยุคอดีตกาลนั้นมากกว่า 500 อัลกอริทึ่ม แต่ส่วนใหญ่แล้วอาศัยหลักการคล้ายๆกันคือใช้หลักวงกลมสามวงตัดกันที่จุด P วงแรกจะลากผ่านจุด A-P-B วงที่สองลากผ่านจุด B-C-P วงที่สามลากผ่าน C-P-A ดังรูปด้านล่าง

ภาวะเอกฐาน (Singularity) ที่อัลกอริทึ่มล้มเหลว

ผมขอยืมคำแปล Singularity ที่แปลว่าภาวะเอกฐานจากเรื่องหลุมดำในทฤษฎีฟิสิกส์ควอนตัมหน่อย เพราะมันได้ใจความคือภาวะที่ทฤษฎีทางคณิตศาสตร์ล้มเหลว คือเหมือนกับพลัดตกลงไปในหลุมดำประมาณนั้น

การคำนวณ Resection ที่ใช้วงกลมสามวงมาตัดกันดังรูปด้านบน แต่จะเกิดอะไรขึ้น ถ้าจุดทั้ง 4 จุดนี้อยู่บนวงกลมวงเดียวกัน ก็หมายความว่าวงกลมสามวงนั้นจะซ้อนทับกันทั้งสามวง จนไม่สามารถหาจุดตัดกันได้ ดังนั้น Resection ไม่มีสูตรหรืออัลกอริทึ่มไหนในบรรณพิภพนี้ที่สามารถคำนวณได้บนภาวะเอกฐาน

ภาวะเอกฐานเสมือน (Pseudo Singularity)

ภาวะเอกฐานเสมือนเป็นสภาวะที่จุด P มาอยู่บนเส้นตรงระหว่าง A-B หรือ B-C หรือ A-C ด้านล่างจะเป็นกรณีจุด P อยู่บนเส้นตรงระหว่างจุด B และ C จะทำให้มุม β มีค่ากับ π เรเดียน (หรือเท่ากับ 180 องศา) หรือถ้าขยับจุด P ให้เลยออกจากจุด B แตยังอยู่ในแนวเส้นตรง ในกรณีนี้จะได้ มุม β = 0

ภาวะเอกฐานเสมือนนี้สูตรหลายๆสูตรไม่สามารถหาค่าได้เช่นสูตร Tienstra Method

อัลกอริทึ่มสมัยใหม่ (Modern Algorithm)

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

    1. A New Three Object Triangulation Algorithm for Mobile Robot Positioning โดย Vincent Pierlot and Marc Van Droogenbroeck ทั้งสองท่านจบวิศวกรไฟฟ้า งานวิจัยนี้มีโค้ดภาษา C ด้วย แต่เนื่องจากลิขสิทธิ์ที่ระบุให้ใช้ในวงการศึกษาหรือใช้งานส่วนตัวเท่านั้น ผมจึงไม่สามารถนำโค้ดมาใช้งานได้เพราะยังกำกวม ความจริงงานทั้ง 2 ท่านได้รวบรวมอัลกอริทึ่มรวมทั้งของตัวเองด้วยทั้งหมด 18 อัลกอริทึ่มและ implement มาเป็นโค้ด พร้อมทั้งวัด benchmark ว่าใค้ดใครเร็วที่สุด ก็ตามคาดหมายโค้ดที่ทั้งสองท่านคิดค้นมานั้นเข้าวิน แต่สำหรับผมแล้วความต่างมันหนึ่งในพันส่วนของวินาทีอาจจะจำเป็นสำหรับงานให้ตำแหน่งหุ่นยนต์ที่ต้องมีการคำนวณตำแหน่งแบบ real time แต่สำหรับงานสำรวจในภาคสนามความจำเป็นกลับต่างออกไป
    2. New Method That Solves the Three-Point Resection Problem Using Straight Lines Intersection โดย Josep M. Font-Llagunes and Joaquim A. Batlle ผมชอบความคิดของสองท่านนี้ดูจากโพรไฟล์แล้วจบวิศวกรเครื่องกล แต่เนื่องจากเอกสารเข้าใจยากไปนิด ผมกลับใช้เวลาแกะอัลกอริทึมโดยใช้เวลาพอสมควรกว่าจะออกมาเป็นโค้ดได้ โปรแกรมสามารถคำนวณในสภาวะเอกฐานเสมือนได้

หลักการคำนวณโดยย่อ

ผมไม่มีเวลาที่จะศึกษาสูตรในเบื้องลึกให้กระจ่างมากนั้นแต่เน้น implement มาเป็นโค้ดภาษา C ดังนั้นความเข้าใจจึงอยู่ในระดับผิวเผิน ต่อไปผมจะบอกเล่าสิ่งที่ผมเข้าใจแบบจำกัดจำเขี่ย เราจะมาเริ่มต้น สมมติว่าตอนนี้ถ้าทราบค่าพิกัด P แล้วเราสามารถหาค่าอะซิมัทจากสถานี A, B และ C ไปยังจุด P ได้ง่ายๆ ตามรูปด้านล่าง

ค่าอะซิมัทของสถานีที่ทราบค่าพิกัด

1.คำนวณหาค่าอะซิมัทโดยประมาณ (Θ)

แต่ในชีวิตจริงค่าพิกัด P เป็นสิ่งที่เรายังไม่ทราบดังนั้นสูตรคำนวณนี้จะมีการหาค่าโดยประมาณก่อน Θ = θ – โดย  คือค่าเบี่ยงเบนไปจากค่าจริงจากที่เราประมาณ ถ้าทุกๆเส้นเบี่ยงเบนไป  เราสามารถลากเส้นไปตัดกันเป็นรู)สามเหลี่ยมเล็กๆ แต่ถ้า  ที่ประมาณการณ์ไว้มีขนาดเบี่ยงเบนไปมาก ก็จะได้ขนาดสามเหลี่ยมนี้ใหญ่ขึ้น สามเหลี่ยมนี้ทางผู้คิดค้นเรียกว่า error triangle จุดตัดแทนที่ด้วย PAB, PBC และ PAC

2.คำนวณหาค่าพิกัดของ Error Triangle

ค่าพิกัดของจุดตัด P นี้สามารถคำนวณได้จากสูตร

โดยที่ mA = cot(Θ), mB = cot(Θ – α) และ mC = cot(Θ – α -β) ไม่ลืมว่า Θ คือค่าอะซิมัทโดยประมาณ

3.คำนวณหาค่าพิกัดของ Centers Triangle

ถ้าจากจุด P ลากเส้นตรงไปหาสถานีที่ทราบค่าพิกัดแล้วแบ่งครึ่งลากเส้นตั้งจาก เราจะได้สามเหลี่ยมอีกชุดหนึ่งเรียกว่า centers triangle  และเป็นสามเหลี่ยมคล้ายสามเหลี่ยม error triangle ดังนั้นความสัมพันธ์ด้านมุมและระยะระหว่างสามเหลี่ยมสองรูปนี้สามารถคำนวณได้ ดังนั้นค่าพิกัดของ centers triangle สามารถคำนวณหาค่าพิกัดจุดตัด CAB, CBC และ CAC ได้จากสูตรดังต่อไปนี้

4.คำนวณมุมเบี่ยงเบน

ค่าเบี่ยงเบนเมื่อคำนวณมาได้แล้วสามารถนำไปบวกหรือลบกับค่าอะซิมัทประมาณการในครั้งแรกจะได้ค่าอะซิมัทที่ถูกต้อง

สามารถคำนวณสมการ (9) จากระยะทางแต่ละด้านของ error triangle และ centers triangle เช่นตัวอย่าง |δθ| = arcsin(ระยะทางระหว่างจุด PAB– PBC / ระยะทางจุด CAB– CBC )

หรือในสมการ (10) สามารถใช้พื้นที่ของสามเหลี่ยมสองรูปนี้ได้

5.คำนวณหาเครื่องหมายมุมเบี่ยงเบน

ก่อนหน้านี้ที่แสดงค่าที่คำนวณได้ในสมการ (9) และ (10) จะเห็นว่าติดเครื่องหมาย absolute ไว้คือยังไม่ได้คิดเครื่องหมาย ส่วนเครื่องหมายมุมเบี่ยงเบนหาได้ดังนี้

ทางผู้พัฒนาแสดงทิศทางของ error triangle เมื่อเทียบ center triangle ตามเครื่องหมายของ error triangle ดังนี้

อาจจะดูยากไปนิดเป็นการคูณไขว้กัน ดูตัวอย่างเพื่อความง่าย

sign = (xPAC-xPBC)*(yCAC-yCBC) – (xCAC-xCBC)*(yPAC-yPBC)

ค่าของ  sign จะออกมาเป็นบวกหรือเป็นลบ แล้วจะเอาเครื่องหมายนี้ไปใส่ให้สมการในข้อต่อไป

6.คำนวณหาอะซิมัทที่ถูกต้อง

สมการ θ=Θ +sign(dθ)

7.คำนวณหาพิกัดของจุดตัด Resection

ถ้าจุดตัดไม่ตกหลุมดำ ก็สามารถคำนวณหาจุดตัดได้จาก 1 ใน 3 สมการ ของสมการ (1), (2) หรือ (3) เช่นตัวอย่าง

mA = cot(θ)
mB = cot(θ – α)
xP = (mA x xA – mB x xB – yA + yB) / (mA – mB)
yP = mA x (xP – xA) + yA

การคำนวณเมื่อจุดตัดตกภาวะเอกฐานเสมือน

จะมี 3 กรณีคือ

1) ค่า α = 180 หรือ α = 0

2)ค่า β = 180 หรือ β = 0

3)ค่ามุม α+β = 180 หรือ α+β = 0

จากการคำนวณในข้อ 3 จะสังเกตในสูตร (5) จะมีตัวคูณด้วย cot(α) อยู่ ในกรณีนี้จุดตัด P อยู่บนเส้นตรงระหว่างจุด A และ B ดังนั้นมุม α = 180 องศาจะทำให้ cot(α) ไม่สามารถคำนวณได้เพราะค่าเป็นอนันต์ (infinity)  ในเคสนี้เราจะไม่คำนวณหาจุด CAB เพราะหาไม่ได้นั่นเอง แต่จุด CBC และ CAC ก็ยังหาได้ปกติ ดังนั้นในกระบวนการสุดท้ายค่าพิกัดของจุด P สามารถคำนวณได้จากการใช้สมการอีก 2 สมการคือสมการ (2) และ (3)

ไม่ใช้สมการ (1) เพราะมีค่า (mA – mB)  = 0 ทำให้ห่าค่า xP ไม่ได้

ข้อสังเกต สามารถลากวงกลมได้แค่ 2 วงเท่านั้นคือวงกลม A-P-C และ B-P-C ส่วนอีกวงลากไ่ม่ได้เพราะว่า A-P-B เป็นเส้นตรง

ดาวน์โหลด (Download) โปรแกรมสำหรับเครื่องคิดเลข fx-9860G

ไปที่หน้าดาวน์โหลดมองหาโปรแกรม Resection เมื่อดาวน์โหลดมาแล้วจะได้ไฟล์ “RESCTION.G1A” ใช้โปรแกรม FA-124 ทำการโอนโปรแกรมเข้าเครื่องคิดเลข (ดูโพสต์เก่าได้วิธีการนี้) จะเห็นไอคอนปรากฎที่หน้า AddIn ดังรูป

กรณีที่ 1 ตัวอย่างงานรังวัดในงานสำรวจทั่วไป (Survey Engineering Example)

กำหนดค่าพิกัดของสถานี A, B และ C ดังนี้

วัดค่ามุม ∝ และ ∅ จากกล้อง total station ได้ดังนี้ ∝= 40°35’22.11“ และค่ามุม ∅ = 9°18’31.84“ ที่ไอคอนโปรแกรมกดคีย์ “EXE” เข้าไปป้อนค่าพิกัดสถานีทั้งสามดังนี้

จากนั้นป้อนมุมภายใน

โปรแกรมจะคำนวณหาค่าพิกัดของจุดตัด โดยที่แจ้งสถานะมาก่อนว่าคำนวณได้ Resection Solved…

กรณีที่ 2 ตัวอย่างงานที่จุดตัดตกอยู่ในภาวะเอกฐานเสมือน (Pseudo Singularity)

นี่เป็นกรณีพิเศษจริงๆ เพราะว่าหลายๆสูตรคำนวณด้วยวิธีนี้ไม่ได้เช่นสูตร Tienstra กำหนดค่าพิกัดสถานี  A (2639303.349mN, 231605.043mE) ค่าพิกัดสถานี B (2639271.845mN, 231419.755mE) และสถานี C (2639180.389mN, 231561.178mE) มุมที่รังวัดมา α = 180° มุม β = 105°3’14.94“

ข้อสังเกตุถ้ามุม α เท่ากับ 180 แสดงว่าจุดตัดตกอยู่บนเส้นตรงระหว่างสถานี A และ B แต่เขยิบเข้าไปใกล้ B มากกว่าเพราะว่ามุม β เป็นมุมป้าน มาดูการคำนวณจากเครื่องคิดเลข เมื่อเรียกโปรแกรมมาแล้วป้อนค่าพิกัดสถานีตามลำดับ A, B และ C แล้ว

จากนั้นป้อนมุม α และ β

ผลลัพธ์ที่ได้

กรณีที่ 3 ตัวอย่างจุดตัดตกหลุมดำในภาวะเอกฐาน (Singularity)

กรณีสุดท้าย โอกาสที่จะเจอแบบนี้คือสถานีทั้งสามสถานีอยู่บนวงกลมเดียวกันและจุดที่ตั้งกล้องที่ต้องการทราบค่าพิกัดและยังมาอยู่บนวงกลมเดียวกันทั้ง 4 จุด ในชีวิตจริงมีโอกาสน้อยมากเหมือนกับถูกล็อตเตอรีรางวัลที่ 1 ยังไงยังงั้น มาลองคำนวณดู

กำหนดค่าพิกัดสถานี A (2369180.389mN, 231561.178mE) สถานีพิกัดสถานี B (2639303.349mN, 231605.093mE) และสถานี C (2639478.455mN, 231509.233mE) วัดมุม α = 29°32’23.9“และ β = 18°48’43.9“

เมื่อเข้าไปในโปรแกรมป้อนค่าพิกัด A, B และ C ตามลำดับ

จากนั้นป้อนมุม α และ β ตามลำดับ

สุดท้ายโปรแกรมไม่สามารถคำนวณหาพิกัดจุดตัดได้และแสดงว่า Resection unsolved…

เครดิต (Credit)

ก็ยกเครดิตสำหรับอัลกอริทึ่มหรือสูตรคำนวณนี้ให้กับสองท่านคือ Josep M. Font-Llagunes and Joaquim A. Batlle.

ซอร์สโค้ดสูตรคำนวณ (Sourcecode)

ผมยกมาเฉพาะสูตรคำนวณตั้งชื่อฟังก์ชั่น straightLineIntersection สำหรับคนที่สนใจเรื่องโปรแกรมมิ่งก็ศึกษาโค้ดภาษาซีกันได้ครับ ไม่มีอะไรยุ่งยาก

/* Algorithm based on Josep M. Font-Llagunes and Joaquim A. Batlle.
  - Input angles are radians. 
  - Internal angles is clock-wise direction.
  - A, B and C must be located from right to left respectively.*/
bool straightLineIntersection(double *xP, double *yP,
				double alpha_AB, double alpha_BC,
				double xA, double yA, double xB, double yB, double xC, double yC)
{
  double mA, mB, mC; //slope of lines.
  double cot_12, cot_23, cot_31;
  double pAB, pAC, pBC; //Euclidean distance between station.
  double estB; //Estimated angle A-B-C.
  double xPAB, yPAB, xPBC, yPBC, xPAC, yPAC; //error triangle.
  double xCAB, yCAB, xCBC, yCBC, xCAC, yCAC; //center of triangle.
  double deltatheta;
  double theta; //first estimated and actual azimuth from P to A at the end.
  double AP, AC;
  double sign;
  double dPAC_PBC, dCAC_CBC;
  double dPAB_PBC, dCAB_CBC;
  double dPAB_PAC, dCAB_CAC;

  pAB = sqrt((xA-xB)*(xA-xB) + (yA-yB)*(yA-yB));
  pAC = sqrt((xA-xC)*(xA-xC) + (yA-yC)*(yA-yC));
  pBC = sqrt((xB-xC)*(xB-xC) + (yB-yC)*(yB-yC));

  estB = acos((pAB*pAB + pBC*pBC - pAC*pAC) / (2*pAB*pBC));
  //Check if found absolutely singularity then stop and return.
  if (((estB + alpha_AB + alpha_BC - PI) >= -0.0001) and 
      ((estB + alpha_AB + alpha_BC - PI) <= 0.0001))
    return false;

  /*first guess (theta), try to avoid for cot(angle) 
    when angle == PI or zero).*/ 
  theta = alpha_AB + alpha_BC/2.0;    
  mA = cot(theta);
  mB = cot(theta - alpha_AB);
  mC = cot(theta - alpha_AB - alpha_BC);
	
  //calc coordinates of error triangle.
  xPAB = (mA*xA - mB*xB - yA + yB) / (mA - mB);
  yPAB = mA*(xPAB - xA) + yA;  
  xPBC = (mB*xB - mC*xC - yB + yC) / (mB - mC);
  yPBC = mB*(xPBC - xB) + yB;
  xPAC = (mA*xA - mC*xC - yA + yC) / (mA - mC);
  yPAC = mA*(xPAC - xA) + yA;
	
  dPAC_PBC = sqrt((xPAC-xPBC)*(xPAC-xPBC) + (yPAC-yPBC)*(yPAC-yPBC));
  dPAB_PBC = sqrt((xPAB-xPBC)*(xPAB-xPBC) + (yPAB-yPBC)*(yPAB-yPBC));
  dPAB_PAC = sqrt((xPAB-xPAC)*(xPAB-xPAC) + (yPAB-yPAC)*(yPAB-yPAC));
  
  AP = ((xPAB - xPBC) * (yPBC - yPAC) - (xPBC - xPAC) * (yPAB - yPBC))/* / 2*/ ;
  AP = (AP < 0.0) ? -AP : AP;

  /* The next 3 Cases are psudosingularities.
    
    1st case: P is aligned with A & B.Therefore cannot calc PAB & CAB.*/
  if (alpha_AB == PI || alpha_AB == 0.0){ /* P is aligned on A & B.*/
    /* cot(alpha_AB) is infinity */
    cot_23 = cot(alpha_BC);
    cot_31 = cot(alpha_AB+alpha_BC);
   
    //calc coordinates of center triangle.
    xCBC = 0.5 * (xB + xC + (yB - yC) * cot_23);
    yCBC = 0.5 * (yB + yC + (xC - xB) * cot_23);
    xCAC = 0.5 * (xA + xC + (yA - yC) * cot_31);
    yCAC = 0.5 * (yA + yC + (xC - xA) * cot_31);

    //distance CAC to CBC (center triangle).
    dCAC_CBC = sqrt((xCAC-xCBC)*(xCAC-xCBC)+(yCAC-yCBC)*(yCAC-yCBC));

    deltatheta = asin(0.5*(dPAC_PBC/dCAC_CBC));
	deltatheta = (deltatheta < 0.0) ? -deltatheta : deltatheta; 
    sign = (xPAC-xPBC)*(yCAC-yCBC) - (xCAC-xCBC)*(yPAC-yPBC);
	if (sign < 0.0 ) deltatheta = -deltatheta ;   
    theta += deltatheta;

    mB = cot(theta - alpha_AB);
    mC = cot(theta - alpha_AB - alpha_BC);
  
    *xP = (mB * xB - mC * xC - yB + yC) / (mB - mC);
    *yP = mB * ((*xP) - xB) + yB; 
    return true;
  }else if ((alpha_BC == PI) || (alpha_BC == 0)){ 
    /* 2nd case: P is aligned on B & C.
                 cot(alpha_BC) is infinity */
    cot_12 = cot(alpha_AB);
    cot_31 = cot(alpha_AB+alpha_BC);
   
    //calc coordinates of center triangle.
    xCAB = 0.5 * (xA + xB + (yA - yB) * cot_12);
    yCAB = 0.5 * (yA + yB + (xB - xA) * cot_12);
    xCAC = 0.5 * (xA + xC + (yA - yC) * cot_31);
    yCAC = 0.5 * (yA + yC + (xC - xA) * cot_31);

    //distance CAB ot CAC (center triangle)
    dCAB_CAC = sqrt((xCAB-xCAC)*(xCAB-xCAC)+(yCAB-yCAC)*(yCAB-yCAC));

    deltatheta = asin(0.5*(dPAB_PAC/dCAB_CAC));
	deltatheta = (deltatheta < 0.0) ? -deltatheta : deltatheta; 
    sign = (xPAB-xPAC)*(yCAB-yCAC) - (xCAB-xCAC)*(yPAB-yPAC);
	if (sign < 0.0 ) deltatheta = -deltatheta ;   
    theta += deltatheta;

    mA = cot(theta);
    mB = cot(theta - alpha_AB);
  
    *xP = (mA * xA - mB * xB - yA + yB) / (mA - mB);
    *yP = mA * ((*xP) - xA) + yA; 
    return true;
  }else if (((alpha_AB + alpha_BC) == PI) || ((alpha_AB + alpha_BC) == 0)){
    /* 3rd case: P is aligned on A & C. 
       cot(alpha_AB+alpha_BC) is infinity.*/
    cot_12 = cot(alpha_AB);
    cot_23 = cot(alpha_BC);
   
    //calc coordinates of center triangle.
    xCAB = 0.5 * (xA + xB + (yA - yB) * cot_12);
    yCAB = 0.5 * (yA + yB + (xB - xA) * cot_12);
    xCBC = 0.5 * (xB + xC + (yB - yC) * cot_23);
    yCBC = 0.5 * (yB + yC + (xC - xB) * cot_23);

    //distance CAB ot CBC (center triangle)
    dCAB_CBC = sqrt((xCAB-xCBC)*(xCAB-xCBC)+(yCAB-yCBC)*(yCAB-yCBC));

    deltatheta = asin(0.5*(dPAB_PBC/dCAB_CBC));
	deltatheta = (deltatheta < 0.0) ? -deltatheta : deltatheta; 
	sign = (xPBC - xPAB) * (yCBC - yCAB) - (xCBC - xCAB) * (yPBC - yPAB);
	if (sign < 0.0 ) deltatheta = -deltatheta;   
    theta += deltatheta;

    mA = cot(theta);
    mB = cot(theta - alpha_AB);
  
	*xP = (mA * xA - mB * xB - yA + yB) / (mA - mB);
	*yP = mA * ((*xP) - xA) + yA;   
    return true;
  }else {
    /* Normal case can be calculated by other methods as well.*/
    cot_12 = cot(alpha_AB);
    cot_23 = cot(alpha_BC);
    cot_31 = cot(alpha_AB+alpha_BC);
   
    //calc coordinates of center triangle.
    xCAB = 0.5 * (xA + xB + (yA - yB) * cot_12);
    yCAB = 0.5 * (yA + yB + (xB - xA) * cot_12);
    xCBC = 0.5 * (xB + xC + (yB - yC) * cot_23);
    yCBC = 0.5 * (yB + yC + (xC - xB) * cot_23);
    xCAC = 0.5 * (xA + xC + (yA - yC) * cot_31);
    yCAC = 0.5 * (yA + yC + (xC - xA) * cot_31);

	AC = ((xCAB - xCBC) * (yCBC - yCAC) - (xCBC - xCAC) * (yCAB - yCBC))/* / 2*/ ;
	AC = (AC < 0.0) ? -AC : AC;

    deltatheta = asin(0.5*sqrt(AP/AC));
	deltatheta = (deltatheta < 0.0) ? -deltatheta : deltatheta; 
	sign = (xPBC - xPAB) * (yCBC - yCAB) - (xCBC - xCAB) * (yPBC - yPAB);
	if (sign < 0.0 ) deltatheta = -deltatheta ;   
    theta += deltatheta;

    mA = cot(theta);
    mB = cot(theta - alpha_AB);
  
	*xP = (mA * xA - mB * xB - yA + yB) / (mA - mB);
	*yP = mA * ((*xP) - xA) + yA;  
    return true;
  }
}