/**
******************************************************************************
* @file :geometry_3d.c
* @date :2025-12-16
* @brief :三维平面几何函数定义
*
*------------------------------History Record-------------------------------
* @modifier :
*
* @date :
******************************************************************************
**/
/*Include ---------------------------------------------------------------*/
#include "geometry_3d.h"
#include <math.h>
/*private define --------------------------------------------------------*/
/*private TypeDef -------------------------------------------------------*/
/*Fucntions--------------------------------------------------------------*/
/* Private Functions--------------------------------------*/
/****
* @brief 向量减法（a - b）
* @param a 点坐标 b 点坐标
**/
static point_3d_t vector_sub(point_3d_t a, point_3d_t b)
{
return (point_3d_t){a.x - b.x, a.y - b.y, a.z - b.z};
}
/****
* @brief 向量加法（a + b）
* @param a 点坐标 b 点坐标
**/
static point_3d_t vector_add(point_3d_t a, point_3d_t b)
{
return (point_3d_t){a.x + b.x, a.y + b.y, a.z + b.z};
}
/****
* @brief 向量缩放
* @param v 输入向量 s 缩放因子
**/
static point_3d_t vector_scale(point_3d_t v, float s)
{
return (point_3d_t){v.x * s, v.y * s, v.z * s};
}
/****
* @brief 向量点乘（a ·b = ax*bx + ay*by + az*bz）
* @param a 点坐标 b 点坐标
**/
static float vector_dot(point_3d_t a, point_3d_t b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
/****
* @brief 向量叉乘（a ×b，用于计算平面法向量）
* @param a 点坐标 b 点坐标
**/
static point_3d_t vector_cross(point_3d_t a, point_3d_t b)
{
return (point_3d_t){
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x
};
}
/****
* @brief 标准化射线方向向量（转为单位向量）
* @param
**/
static point_3d_t vector_normalize(point_3d_t v)
{
float mag = sqrtf(vector_dot(v, v));
if (mag < EPSILON_3D) return v;
return vector_scale(v, 1.0f / mag);
}
/* Public Functions --------------------------------------*/
/****
* @brief 计算两点之间的三维欧氏距离
* @param a 点坐标 b 点坐标
**/
float point_distance_3d(point_3d_t a, point_3d_t b)
{
float dx = a.x - b.x;
float dy = a.y - b.y;
float dz = a.z - b.z;
return sqrtf(dx*dx + dy*dy + dz*dz);
}
/****
* @brief 球管射线方向计算 (已知起点和 Z轴夹角，创建射线结构体)
* @param origin：起点 angle_z_deg：Z轴夹角
**/
ray_3d_t create_ray_from_angle(point_3d_t origin, float theta_z)
{
// θ_z 是射线与Z轴的夹角（弧度）
ray_3d_t ray;
ray.origin = origin;
// 计算方向向量（假设在XZ平面）
ray.direction.x = sinf(theta_z);
ray.direction.y = 0.0f;
ray.direction.z = cosf(theta_z);
// 归一化方向向量
ray.direction = vector_normalize(ray.direction);
return ray;
}
/****
* @brief 创建与Z轴平行的空间矩形 空间矩形坐标计算(翻斗不翻转情况)
* @param mid_point：射线中线点（矩形中心点），width：X轴方向长度，height：Y轴方向长度
**/
rect_3d_t create_rect_from_ray(ray_3d_t ray, float width, float height)
{
rect_3d_t rect;
rect.center = ray.origin;
// 计算局部坐标系基向量
point_3d_t world_up = {0, 1, 0};
point_3d_t local_x = vector_normalize(vector_cross(world_up, ray.direction));
point_3d_t local_y = vector_normalize(vector_cross(ray.direction, local_x));
// 计算半宽高
float half_width = width / 2.0f;
float half_height = height / 2.0f;
// 计算四个角点
rect.left_top = vector_add(rect.center, vector_add(
vector_scale(local_x, -half_width),
vector_scale(local_y, half_height)
));
rect.left_bot = vector_add(rect.center, vector_add(
vector_scale(local_x, -half_width),
vector_scale(local_y, -half_height)
));
rect.right_bot = vector_add(rect.center, vector_add(
vector_scale(local_x, half_width),
vector_scale(local_y, -half_height)
));
rect.right_top = vector_add(rect.center, vector_add(
vector_scale(local_x, half_width),
vector_scale(local_y, half_height)
));
return rect;
}
/****
* @brief 三维矩形转换为平面方程（Ax + By + Cz + D = 0）
* @param
**/
plane_3d_t rect_to_plane(rect_3d_t rect)
{
plane_3d_t plane;
// 计算两个边向量
point_3d_t v1 = vector_sub(rect.right_top, rect.left_top);
point_3d_t v2 = vector_sub(rect.left_bot, rect.left_top);
// 计算法向量（叉积）
point_3d_t normal = vector_cross(v1, v2);
normal = vector_normalize(normal);
// 平面方程: Ax + By + Cz + D = 0
plane.A = normal.x;
plane.B = normal.y;
plane.C = normal.z;
plane.D = -(plane.A * rect.center.x + plane.B * rect.center.y + plane.C * rect.center.z);
return plane;
}
/****
* @brief 判断射线与平面是否有交点
* @param
**/
// 射线方程：P = origin + t*direction（t≥0）
// 平面方程：A*x + B*y + C*z + D = 0
// 推导：A*(ox + t*dx) + B*(oy + t*dy) + C*(oz + t*dz) + D = 0 → t = -(A*ox + B*oy + C*oz + D)/(A*dx + B*dy + C*dz)
bool ray_plane_intersection(ray_3d_t ray, plane_3d_t plane, point_3d_t* intersection)
{
float denom = vector_dot(ray.direction, (point_3d_t){plane.A, plane.B, plane.C});
// 检查平行情况
if (fabsf(denom) < EPSILON_3D)
{
return false;
}
// 计算交点参数 t
point_3d_t diff = vector_sub(ray.origin, (point_3d_t){0, 0, 0});
float num = -(plane.A * ray.origin.x + plane.B * ray.origin.y + plane.C * ray.origin.z + plane.D);
float t = num / denom;
// 检查是否在射线正方向
if (t < 0) return false;
// 计算交点坐标
intersection->x = ray.origin.x + t * ray.direction.x;
intersection->y = ray.origin.y + t * ray.direction.y;
intersection->z = ray.origin.z + t * ray.direction.z;
return true;
}
/****
* @brief 判断点是否在三维矩形内
* @param
**/
// 方法：通过向量投影判断点是否在矩形的X-Y边界内（矩形平面垂直于Z轴时等价于2D判断）
bool is_point_in_rect(point_3d_t point, rect_3d_t rect)
{
// 计算局部坐标系基向量
point_3d_t v1 = vector_sub(rect.right_top, rect.left_top);
point_3d_t v2 = vector_sub(rect.left_bot, rect.left_top);
point_3d_t normal = vector_cross(v1, v2);
// 构建局部坐标系
point_3d_t local_x = vector_normalize(v1);
point_3d_t local_y = vector_normalize(vector_cross(normal, local_x));
// 计算点到矩形中心的向量
point_3d_t vec_to_point = vector_sub(point, rect.center);
// 计算在局部坐标系中的投影
float proj_x = vector_dot(vec_to_point, local_x);
float proj_y = vector_dot(vec_to_point, local_y);
// 计算矩形半宽高
float half_width = point_distance_3d(rect.left_top, rect.right_top) / 2.0f;
float half_height = point_distance_3d(rect.left_top, rect.left_bot) / 2.0f;
// 检查是否在矩形边界内
return (fabsf(proj_x) <= half_width + EPSILON_3D) &&
(fabsf(proj_y) <= half_height + EPSILON_3D);
}
/****
* @brief 计算射线与三维矩形的交点
* @param
**/
point_3d_t ray_rect_intersection(ray_3d_t ray, rect_3d_t rect)
{
point_3d_t intersection = {NULL, NULL, NULL};//NAN
// 转换矩形为平面方程
plane_3d_t plane = rect_to_plane(rect);
// 求射线与平面交点
if (ray_plane_intersection(ray, plane, &intersection))
{
// 检查交点是否在矩形内
if (is_point_in_rect(intersection, rect))
{
return intersection;
}
}
// 无交点返回
return (point_3d_t){NULL, NULL, NULL};
}
/****
* @brief 计算点在平面上的垂直投影
* @param
**/
point_3d_t project_point_to_plane(point_3d_t point, plane_3d_t plane)
{
// 计算平面法向量
point_3d_t normal = {plane.A, plane.B, plane.C};
float mag = sqrtf(vector_dot(normal, normal));
if (mag < EPSILON_3D) return point;
// 归一化法向量
normal = vector_scale(normal, 1.0f / mag);
// 计算点到平面的距离
float dist = (plane.A * point.x + plane.B * point.y + plane.C * point.z + plane.D) / mag;
// 计算投影点
return (point_3d_t){
point.x - dist * normal.x,
point.y - dist * normal.y,
point.z - dist * normal.z
};
}
/****
* @brief 已知球管旋转角度，求射线在目标平面的投影
* @param
**/
point_3d_t ray_plane_3d_project_with_rotation(ray_3d_t ray, rect_3d_t* target_rect, float rotate_angle_y_deg)
{
if (target_rect == NULL) return ray.origin;
// 将射线起点绕 Y轴旋转（模拟球管旋转）
float angle_rad = rotate_angle_y_deg * M_PI / 180.0f;
float cos_a = cosf(angle_rad);
float sin_a = sinf(angle_rad);
point_3d_t rotated_origin;
// 绕 Y轴旋转矩阵：X'=X*cosθ - Z*sinθ，Y'=Y，Z'=X*sinθ + Z*cosθ
rotated_origin.x = ray.origin.x * cos_a - ray.origin.z * sin_a;
rotated_origin.y = ray.origin.y;
rotated_origin.z = ray.origin.x * sin_a + ray.origin.z * cos_a;
// 获取目标平面方程
plane_3d_t target_plane = rect_3d_to_plane(target_rect);
// 计算旋转后射线起点在目标平面的垂直投影
point_3d_t project_point = point_plane_3d_project(rotated_origin, target_plane);
return project_point;
}
/*-------------------------end of file --------------------------------*/
根据我当前代码，生成一个测试用例代码。帮我写个测试用例代码来进行测试，名字叫geometry_3d_test.c和geometry_3d_test.h。我使用stm32f207vet6使用keil5写代码,使用SEGGER_RTT_printf来进行log信息打印，帮我写个测试用例代码
最新发布
本文详细阐述了射线与平面的定义、参数方程及其在几何学中的应用，包括交点求解过程。
扫一扫
1123
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
