3D图形库四元数相关函数示例源代码

#include <math.h> #include "CPPYIN.Math.h" // 命名空间内全局变量定义 namespace _CPPYIN_Math { double CosTable[360]; double SinTable[360]; } int _CPPYIN_Math::InitSinCosTable() { for (int ang = 0; ang < 360; ++ang) { double theta = (double)ang * PI / (double)180; CosTable[ang] = cos(theta); SinTable[ang] = sin(theta); } return 1; } double _CPPYIN_Math::FastSin(double theta_du) { // 对可能的正负小数取模 theta_du = fmod(theta_du, 360); // 将负角度转正 if (theta_du < 0) { theta_du += 360.0; } // 对小数和整数部分分别处理 int theta_int = int(theta_du); double theta_frac = theta_du - theta_int; // 插值 double result = SinTable[theta_int] + theta_frac * (SinTable[theta_int+1] - SinTable[theta_int]); return result; } double _CPPYIN_Math::FastCos(double theta_du) { // 对可能的正负小数取模 theta_du = fmod(theta_du, 360); // 将负角度转正 if (theta_du < 0) { theta_du += 360.0; } // 对小数和整数部分分别处理 int theta_int = int(theta_du); double theta_frac = theta_du - theta_int; // 插值 double result = CosTable[theta_int] + theta_frac * (CosTable[theta_int+1] - CosTable[theta_int]); return result; } void _CPPYIN_Math::CooTransPOINT2DtoPOLAR2D(POINT2D_PTR point2d, POLAR2D_PTR polar2d) { polar2d->r = sqrt((point2d->x * point2d->x) + (point2d->y * point2d->y)); polar2d->theta = atan((point2d->y) / (point2d->x)); } void _CPPYIN_Math::CooTransPOLAR2DtoPOINT2D(POLAR2D_PTR polar2d, POINT2D_PTR point2d) { point2d->x = polar2d->r * cos(polar2d->theta); point2d->y = polar2d->r * sin(polar2d->theta); } void _CPPYIN_Math::CooTransPOINT3DtoCYLINDRICAL3D(POINT3D_PTR point3d, CYLINDRICAL3D_PTR cylindrical3d) { cylindrical3d->r = sqrt((point3d->x * point3d->x) + (point3d->y * point3d->y)); cylindrical3d->theta = atan((point3d->y) / (point3d->x)); cylindrical3d->z = point3d->z; } void _CPPYIN_Math::CooTransCYLINDRICAL3DtoPOINT3D(CYLINDRICAL3D_PTR cylindrical3d, POINT3D_PTR point3d) { point3d->x = cylindrical3d->r * cos(cylindrical3d->theta); point3d->y = cylindrical3d->r * sin(cylindrical3d->theta); point3d->z = cylindrical3d->z; } void _CPPYIN_Math::CooTransPOINT3DtoSPHERICAL3D(POINT3D_PTR point3d, SPHERICAL3D_PTR spherical3d) { spherical3d->p = sqrt((point3d->x * point3d->x) + (point3d->y * point3d->y) + (point3d->z * point3d->z)); double r = sqrt((point3d->x * point3d->x) + (point3d->y * point3d->y)); spherical3d->phi = asin(r / spherical3d->p); spherical3d->theta = atan(point3d->y / point3d->x); } void _CPPYIN_Math::CooTransSPHERICAL3DtoPOINT3D(SPHERICAL3D_PTR spherical3d, POINT3D_PTR point3d) { double r = spherical3d->p * sin(spherical3d->phi); point3d->x = r * cos(spherical3d->theta); point3d->y = r * sin(spherical3d->theta); point3d->z = spherical3d->p * cos(spherical3d->phi); } void _CPPYIN_Math::VectorAdd(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum) { vsum->x = va->x + vb->x; vsum->y = va->y + vb->y; } void _CPPYIN_Math::VectorAdd(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vsum) { vsum->x = va->x + vb->x; vsum->y = va->y + vb->y; vsum->z = va->z + vb->z; } void _CPPYIN_Math::VectorAdd(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vsum) { vsum->x = va->x + vb->x; vsum->y = va->y + vb->y; vsum->z = va->z + vb->z; vsum->w = 1; } void _CPPYIN_Math::VectorSub(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum) { vsum->x = va->x - vb->x; vsum->y = va->y - vb->y; } void _CPPYIN_Math::VectorSub(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vsum) { vsum->x = va->x - vb->x; vsum->y = va->y - vb->y; vsum->z = va->z - vb->z; } void _CPPYIN_Math::VectorSub(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vsum) { vsum->x = va->x - vb->x; vsum->y = va->y - vb->y; vsum->z = va->z - vb->z; vsum->w = 1; } void _CPPYIN_Math::VectorScale(double k, VECTOR2D_PTR va, VECTOR2D_PTR vscaled) { vscaled->x = k * va->x; vscaled->y = k * va->y; } void _CPPYIN_Math::VectorScale(double k, VECTOR3D_PTR va, VECTOR3D_PTR vscaled) { vscaled->x = k * va->x; vscaled->y = k * va->y; vscaled->z = k * va->z; } void _CPPYIN_Math::VectorScale(double k, VECTOR4D_PTR va, VECTOR4D_PTR vscaled) { vscaled->x = k * va->x; vscaled->y = k * va->y; vscaled->z = k * va->z; vscaled->w = 1; } double _CPPYIN_Math::VectorDot(VECTOR2D_PTR va, VECTOR2D_PTR vb) { return (va->x * vb->x) + (va->y * vb->y); } double _CPPYIN_Math::VectorDot(VECTOR3D_PTR va, VECTOR3D_PTR vb) { return (va->x * vb->x) + (va->y * vb->y) + (va->z * va->z); } double _CPPYIN_Math::VectorDot(VECTOR4D_PTR va, VECTOR4D_PTR vb) { return (va->x * vb->x) + (va->y * vb->y) + (va->z * va->z); } void _CPPYIN_Math::VectorCross(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vn) { vn->x = ((va->y * vb->z) - (va->z * vb->y)); vn->y = -((va->x * vb->z) - (va->z * vb->x)); vn->z = ((va->x * vb->y) - (va->y * vb->x)); } void _CPPYIN_Math::VectorCross(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vn) { vn->x = ((va->y * vb->z) - (va->z * vb->y)); vn->y = -((va->x * vb->z) - (va->z * vb->x)); vn->z = ((va->x * vb->y) - (va->y * vb->x)); vn->w = 1; } double _CPPYIN_Math::VectorLength(VECTOR2D_PTR va) { return sqrt(va->x * va->x + va->y * va->y); } double _CPPYIN_Math::VectorLength(VECTOR3D_PTR va) { return sqrt(va->x * va->x + va->y * va->y + va->z * va->z); } double _CPPYIN_Math::VectorLength(VECTOR4D_PTR va) { return sqrt(va->x * va->x + va->y * va->y + va->z * va->z); } void _CPPYIN_Math::VectorNormalize(VECTOR2D_PTR va, VECTOR2D_PTR vn) { vn->x = 0; vn->y = 0; double length = VectorLength(va); if (length < EPSILON) { return; } else { double lengthdao = 1 / length; vn->x = va->x * lengthdao; vn->y = va->y * lengthdao; } } void _CPPYIN_Math::VectorNormalize(VECTOR3D_PTR va, VECTOR3D_PTR vn) { vn->x = 0; vn->y = 0; vn->z = 0; double length = VectorLength(va); if (length < EPSILON) { return; } else { double lengthdao = 1 / length; vn->x = va->x * lengthdao; vn->y = va->y * lengthdao; vn->z = va->z * lengthdao; } } void _CPPYIN_Math::VectorNormalize(VECTOR4D_PTR va, VECTOR4D_PTR vn) { vn->x = 0; vn->y = 0; vn->z = 0; vn->w = 0; double length = VectorLength(va); if (length < EPSILON) { return; } else { double lengthdao = 1 / length; vn->x = va->x * lengthdao; vn->y = va->y * lengthdao; vn->z = va->z * lengthdao; vn->w = 1; } } double _CPPYIN_Math::VectorCos(VECTOR2D_PTR va, VECTOR2D_PTR vb) { return VectorDot(va, vb) / (VectorLength(va) * VectorLength(vb)); } double _CPPYIN_Math::VectorCos(VECTOR3D_PTR va, VECTOR3D_PTR vb) { return VectorDot(va, vb) / (VectorLength(va) * VectorLength(vb)); } double _CPPYIN_Math::VectorCos(VECTOR4D_PTR va, VECTOR4D_PTR vb) { return VectorDot(va, vb) / (VectorLength(va) * VectorLength(vb)); } void _CPPYIN_Math::MatrixCreate(MATRIX3X3_PTR pm, double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22) { pm->M00 = m00; pm->M01 = m01; pm->M02 = m02; pm->M10 = m10; pm->M11 = m11; pm->M12 = m12; pm->M20 = m20; pm->M21 = m21; pm->M22 = m22; } void _CPPYIN_Math::MatrixCreate(MATRIX4X4_PTR pm, double m00, double m01, double m02, double m03, double m10, double m11, double m12, double m13, double m20, double m21, double m22, double m23, double m30, double m31, double m32, double m33) { pm->M00 = m00; pm->M01 = m01; pm->M02 = m02; pm->M03 = m03; pm->M10 = m10; pm->M11 = m11; pm->M12 = m12; pm->M13 = m13; pm->M20 = m20; pm->M21 = m21; pm->M22 = m22; pm->M23 = m23; pm->M30 = m30; pm->M31 = m31; pm->M32 = m32; pm->M33 = m33; } void _CPPYIN_Math::MatrixAdd(MATRIX3X3_PTR ma, MATRIX3X3_PTR mb, MATRIX3X3_PTR msum) { for (int