ติดปีกเครื่องคิดเลขเทพ 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. 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 เป็นสิ่งที่เรายังไม่ทราบดังนั้นสูตรคำนวณนี้จะมีการหาค่าโดยประมาณก่อน Θ = θ – dθ โดย dθ คือค่าเบี่ยงเบนไปจากค่าจริงจากที่เราประมาณ ถ้าทุกๆเส้นเบี่ยงเบนไป dθ เราสามารถลากเส้นไปตัดกันเป็นรู)สามเหลี่ยมเล็กๆ แต่ถ้า dθ ที่ประมาณการณ์ไว้มีขนาดเบี่ยงเบนไปมาก ก็จะได้ขนาดสามเหลี่ยมนี้ใหญ่ขึ้น สามเหลี่ยมนี้ทางผู้คิดค้นเรียกว่า error triangle จุดตัดแทนที่ด้วย P_AB, P_BC และ P_AC

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

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

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

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

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

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

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

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

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

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

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

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

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

sign = (xP_AC-xP_BC)*(yC_AC-yC_BC) – (xC_AC-xC_BC)*(yP_AC-yP_BC)

ค่าของ  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)  ในเคสนี้เราจะไม่คำนวณหาจุด C_AB เพราะหาไม่ได้นั่นเอง แต่จุด C_BC และ C_AC ก็ยังหาได้ปกติ ดังนั้นในกระบวนการสุดท้ายค่าพิกัดของจุด 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 สำหรับคนที่สนใจเรื่องโปรแกรมมิ่งก็ศึกษาโค้ดภาษาซีกันได้ครับ ไม่มีอะไรยุ่งยาก

[sourcecode language=”cpp”]
/* 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;
}
}
[/sourcecode]