补传一下，好久之前做的。

理想下双灯塔求结果点

函数原型

bool lighthouseGeometryGetPositionFromRayIntersection(const baseStationGeometry_t* geo1, const baseStationGeometry_t* geo2, float angles1[2], float angles2[2], vec3d position, float *position_delta);
/**
 * @brief Get a normalized vector representing the direction of a ray in world reference frame, based on
 * sweep angles and base station orientation.
 *
 * @param baseStation - Geometry data for the base statsion (position and orientation)
 * @param angle1 - horizontal sweep angle
 * @param angle2 - vertical sweep angle
 * @param ray - (output) the resulting normalized vector
 */

提供的数据

灯塔

这个函数首先获取了两个baseStationGeometry_t记录了两个灯塔的信息

#define vec3d_size 3
typedef float vec3d[vec3d_size];
typedef float mat3d[vec3d_size][vec3d_size];

typedef struct {
  __attribute__((aligned(4))) vec3d origin;
  __attribute__((aligned(4))) mat3d mat;
  bool valid;
} __attribute__((packed)) baseStationGeometry_t;

vec3d代表了灯塔的具体位置，mat3d存储了三个向量，分别表示了前向量，上向量，右向量

角度

每个灯塔的两次扫描会得到两个角度，这两个角度表示的两个面交于一线来表示一条空间直线

两个灯塔各有两个角度，一共提供四个角度即可。

计算的结果

结果点

通过两个灯塔扫描出的点的坐标

偏差范围

两个灯塔的射线最近距离长度

计算流程

先获取向量

我们有角度a和角度b，角度a代表平面和xoz面的角度，角度b代表平面与xoy平面的角度，可得直线方程

叉乘两个平面的法向量即可获得交线的方向向量p

再对p进行单位化。此时的p是相对于灯塔，以灯塔作为坐标原点的，需要进行下一步转换。

利用灯塔的mat3d(记录了灯塔朝向)对该向量进行基变换

此时的向量d就被转化为了绝对向量，是基于地面的。

void lighthouseGeometryGetRay(const baseStationGeometry_t* baseStationGeometry, const float angleH, const float angleV, vec3d ray) {
    vec3d a = {arm_sin_f32(angleH), -arm_cos_f32(angleH), 0};  // Normal vector to X plane
    vec3d b = {-arm_sin_f32(angleV), 0, arm_cos_f32(angleV)};  // Normal vector to Y plane
    // 获取两个面  两个面的交线就是直线
    vec3d raw_ray = {};
    vec_cross_product(b, a, raw_ray); // Intersection of two planes -> ray vector.
    // 方向向量
    float len = vec_length(raw_ray);
    arm_scale_f32(raw_ray, 1 / len, raw_ray, vec3d_size); // Normalize raw ray length.
    // 单位化方向向量
    arm_matrix_instance_f32 source_rotation_matrix = {3, 3, (float32_t *)baseStationGeometry->mat};
    arm_matrix_instance_f32 ray_vec = {3, 1, raw_ray};
    arm_matrix_instance_f32 ray_rotated_vec = {3, 1, ray};
    mat_mult(&source_rotation_matrix, &ray_vec, &ray_rotated_vec);
    // 通过发出面的方向旋转当前向量
}

调整出发点偏移

激光发射器与灯塔坐标有一定的偏移，进行调整。

void lighthouseGeometryGetBaseStationPosition(const baseStationGeometry_t* bs, vec3d baseStationPos) {
    // TODO: Make geometry adjustments within base station.
    vec3d rotated_origin_delta = {};
    //vec3d base_origin_delta = {-0.025f, -0.025f, 0.f};  // Rotors are slightly off center in base station.
    // arm_matrix_instance_f32 origin_vec = {3, 1, base_origin_delta};
    // arm_matrix_instance_f32 origin_rotated_vec = {3, 1, rotated_origin_delta};
    // mat_mult(&source_rotation_matrix, &origin_vec, &origin_rotated_vec);
    baseStationGeometry_t* bs_unconst = (baseStationGeometry_t*)bs;
    arm_add_f32(bs_unconst->origin, rotated_origin_delta, baseStationPos, vec3d_size);
}

计算结果点

我们有两个基站的坐标以及它们发射出去的向量方向

此时产生两条射线，由于射线不一定相交，所以只能找到估计点。对于这两个射线的最短距离的线段，估计点就是这个线段的中点。

线段AB有点A在射线1上，点B在射线2上，容易理解A是在射线1上距离B最近的点，易得AB垂直于两射线。

设p1为射线1(灯塔1)的起始点，p2为射线2(灯塔2)的起始点，对于射线1上的点A有

OA = Op1 + t1 * d1

OB = Op2 + t2 * d2

上式同理(t1和t2都是待计算的值)

有以下推导

有了t1和t2，AB点的值就得出来了，

再求AB两点的中点即为答案点。

static bool intersect_lines(vec3d orig1, vec3d vec1, vec3d orig2, vec3d vec2, vec3d res, float *dist) {

    vec3d w0 = {};
    arm_sub_f32(orig1, orig2, w0, vec3d_size);

    float a, b, c, d, e;
    arm_dot_prod_f32(vec1, vec1, vec3d_size, &a);
    arm_dot_prod_f32(vec1, vec2, vec3d_size, &b);
    arm_dot_prod_f32(vec2, vec2, vec3d_size, &c);
    arm_dot_prod_f32(vec1, w0, vec3d_size, &d);
    arm_dot_prod_f32(vec2, w0, vec3d_size, &e);

    float denom = a * c - b * b;
    // 求|vec1 x vec2|^2的值，越小越相近

    if (fabsf(denom) < 1e-5f) {
        return false;
    }
    // 可能基站过近，两个向量几乎重合，无法计算

    // Closest point to 2nd line on 1st line
    float t1 = (b * e - c * d) / denom;
    vec3d pt1 = {};
    arm_scale_f32(vec1, t1, pt1, vec3d_size);
    arm_add_f32(pt1, orig1, pt1, vec3d_size);

    // Closest point to 1st line on 2nd line
    float t2 = (a * e - b * d) / denom;
    vec3d pt2 = {};
    arm_scale_f32(vec2, t2, pt2, vec3d_size);
    arm_add_f32(pt2, orig2, pt2, vec3d_size);

    // Result is in the middle
    vec3d tmp = {};
    arm_add_f32(pt1, pt2, tmp, vec3d_size);
    arm_scale_f32(tmp, 0.5f, res, vec3d_size);

    // Dist is distance between pt1 and pt2
    arm_sub_f32(pt1, pt2, tmp, vec3d_size);
    *dist = vec_length(tmp);

    return true;
}