Test_coplane.cpp 空间中共面直线求交点

测试两直线共面，相交。求其交点。

利用参数t。

参数t=(p2*(y1-y2)+n2*(z2-z1))/(n2*p1-p2*n1);

 

// Test_coplane.cpp : 定义控制台应用程序的入口点。
#include "stdafx.h"
#include <stdio.h>
#include <iostream>
#include<math.h>
int _tmain(int argc, _TCHAR* argv[])
{
    double x_inImage1,y_inImage1,x_inImage2,y_inImage2,y,X,Y,alpha,gamma;//像面坐标(x,y)和图像尺寸（X，Y）以及成像视场角（alpha,gamma）
    double x1,y1,z1,x2,y2,z2;//双站坐标
    double alpha1,gamma1;//双站俯仰角和偏转角
    double alpha2,gamma2;

    //赋予初始值
    alpha1=90;//测试共面
    gamma1=45;
    alpha2=270;
    gamma2=45;

    X=640;
    Y=480;
    double FOVx=10;
    double FOVy=FOVx*Y/X;
    x1=0,y1=0,z1=0;
    x2=0,y2=200,z2=0;

    //开始计算
    double pi=16*(atan(1.0/5))-4*atan(1.0/239);//精确定义圆周率
    std::cout<<"pi为:"<<pi<<std::endl;
    alpha1=alpha1*pi/180;//角度弧度转换
    gamma1=gamma1*pi/180;
    alpha2=alpha2*pi/180;
    gamma2=gamma2*pi/180;

    //    std::cout<<"cos(alpha1)为:"<<cos(alpha1)<<std::endl;
    //    std::cout<<"cos(gamma1)为:"<<cos(gamma1)<<std::endl;
    double m1=(cos(alpha1))*(cos(gamma1));
    double n1=(sin(alpha1))*(cos(gamma1));
    double p1=sin(gamma1);
    double m2=(cos(alpha2))*(cos(gamma2));
    double n2=(sin(alpha2))*(cos(gamma2));
    double p2=sin(gamma2);

    std::cout<<"方向向量1为:"<<m1<<"，"<<n1<<"，"<<p1<<std::endl;
    std::cout<<"方向向量2为:"<<m2<<"，"<<n2<<"，"<<p2<<std::endl;
    double t;

    if(n2*p1-p2*n1!=0)
    {
        t=(p2*(y1-y2)+n2*(z2-z1))/(n2*p1-p2*n1);
    }
    else
    {
        t=(m2*(y1-y2)+n2*(x2-x1))/(n2*m1-m2*n1);
    }
    double Xtarget,Ytarget,Ztarget;
    Xtarget=x1+m1*t;
    Ytarget=y1+n1*t;
    Ztarget=z1+p1*t;
    std::cout<<"目标坐标为:"<<Xtarget<<"，"<<Ytarget<<"，"<<Ztarget<<std::endl<<std::endl;

    getchar();
    return 0;
}

结果正确：

    if(n2*p1-p2*n1!=0)
    {
        t=(p2*(y1-y2)+n2*(z2-z1))/(n2*p1-p2*n1);
    }
    else
    {
        t=(m2*(y1-y2)+n2*(x2-x1))/(n2*m1-m2*n1);
    }

---

出错：

 

    if((n2*m1-m2*n1)!=0)
        {
            t=(m2*(y1-y2)+n2*(x2-x1))/(n2*m1-m2*n1);
        }
        else
        {
            t=(p2*(y1-y2)+n2*(z2-z1))/(n2*p1-p2*n1);
        }    

出错原因：

(n2*m1-m2*n1)的值判断失误，由于各种计算误差的存在，不会刚好等于0。

---

其实只用一个公式即可：

t=(p2*(y1-y2)+n2*(z2-z1))/(n2*p1-p2*n1)

因为双站定位情况中，在YOZ平面，空间直线投影无论怎样都不会平行。

// Test_coplane.cpp : 定义控制台应用程序的入口点。
#include "stdafx.h"
#include <stdio.h>
#include <iostream>
#include<math.h>
int _tmain(int argc, _TCHAR* argv[])
{
    double x_inImage1,y_inImage1,x_inImage2,y_inImage2,y,X,Y,alpha,gamma;//像面坐标(x,y)和图像尺寸（X，Y）以及成像视场角（alpha,gamma）
    double x1,y1,z1,x2,y2,z2;//双站坐标
    double alpha1,gamma1;//双站俯仰角和偏转角
    double alpha2,gamma2;

    //赋予初始值
    alpha1=90;//测试共面
    gamma1=45;
    alpha2=270;
    gamma2=45;

    X=640;
    Y=480;
    double FOVx=10;
    double FOVy=FOVx*Y/X;
    x1=0,y1=0,z1=0;
    x2=0,y2=200,z2=0;

    //开始计算
    double pi=16*(atan(1.0/5))-4*atan(1.0/239);//精确定义圆周率
    std::cout<<"pi为:"<<pi<<std::endl;
    alpha1=alpha1*pi/180;//角度弧度转换
    gamma1=gamma1*pi/180;
    alpha2=alpha2*pi/180;
    gamma2=gamma2*pi/180;

    //    std::cout<<"cos(alpha1)为:"<<cos(alpha1)<<std::endl;
    //    std::cout<<"cos(gamma1)为:"<<cos(gamma1)<<std::endl;
    double m1=(cos(alpha1))*(cos(gamma1));
    double n1=(sin(alpha1))*(cos(gamma1));
    double p1=sin(gamma1);
    double m2=(cos(alpha2))*(cos(gamma2));
    double n2=(sin(alpha2))*(cos(gamma2));
    double p2=sin(gamma2);

    std::cout<<"方向向量1为:"<<m1<<"，"<<n1<<"，"<<p1<<std::endl;
    std::cout<<"方向向量2为:"<<m2<<"，"<<n2<<"，"<<p2<<std::endl;
    double t;
    t=(p2*(y1-y2)+n2*(z2-z1))/(n2*p1-p2*n1);
    double Xtarget,Ytarget,Ztarget;
    Xtarget=x1+m1*t;
    Ytarget=y1+n1*t;
    Ztarget=z1+p1*t;
    std::cout<<"目标坐标为:"<<Xtarget<<"，"<<Ytarget<<"，"<<Ztarget<<std::endl<<std::endl;
    std::cout<<"分母为:"<<(n2*m1-m2*n1)<<std::endl<<std::endl;
    getchar();
    return 0;

}