การเล็งสกัดย้อน (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 ไม่สามารถนำมาใช้งานได้ หุ่นยนต์ในที่นี้ไม่ได้หมายถึงหุ่นยนต์ที่ติดตั้งแบบอยู่กับที่ในโรงงานนะครับ แต่เป็นหุ่นยนต์ที่สามารถเคลื่อนไหวได้อิสระ ตัวอย่างง่ายๆได้แก่การแข่งขันหุ่นยนต์ของนักศึกษาในอินดอร์ อดึตกาลสูตรเหล้านี้มาจากนักคณิตศาสคร์ แต่สำหรับสูตรสมัยใหม่เนื่องจากความต้องการใช้งานในวงการหุ่นยนต์ ทำให้คนที่คิดค้นสูตรสมัยใหม่กลายเป็นวิศวกรไฟฟ้าหรือวิศวกรเครื่องกล เท่าที่ผมศึกษางานวิจัยในเบื้องต้นผมสนใจงานของ
-
- A New Three Object Triangulation Algorithm for Mobile Robot Positioning โดย Vincent Pierlot and Marc Van Droogenbroeck ทั้งสองท่านจบวิศวกรไฟฟ้า งานวิจัยนี้มีโค้ดภาษา C ด้วย แต่เนื่องจากลิขสิทธิ์ที่ระบุให้ใช้ในวงการศึกษาหรือใช้งานส่วนตัวเท่านั้น ผมจึงไม่สามารถนำโค้ดมาใช้งานได้เพราะยังกำกวม ความจริงงานทั้ง 2 ท่านได้รวบรวมอัลกอริทึ่มรวมทั้งของตัวเองด้วยทั้งหมด 18 อัลกอริทึ่มและ implement มาเป็นโค้ด พร้อมทั้งวัด benchmark ว่าใค้ดใครเร็วที่สุด ก็ตามคาดหมายโค้ดที่ทั้งสองท่านคิดค้นมานั้นเข้าวิน แต่สำหรับผมแล้วความต่างมันหนึ่งในพันส่วนของวินาทีอาจจะจำเป็นสำหรับงานให้ตำแหน่งหุ่นยนต์ที่ต้องมีการคำนวณตำแหน่งแบบ real time แต่สำหรับงานสำรวจในภาคสนามความจำเป็นกลับต่างออกไป
- 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 จุดตัดแทนที่ด้วย 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
ไปที่หน้าดาวน์โหลดมองหาโปรแกรมชื่อ 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;
}
}
