我的3D引擎数学库实现文件

#include "stdafx.h"#include <math.h>#include "MyMath.h"float FastSin(float theta){theta = fmodf(theta, 360) ;if (theta < 0)theta += 360 ;int thetaIntPart = (int)theta ;float thetaFracPart = theta - thetaIntPart ;return sinLookUpTable[thetaIntPart] +thetaFracPart * (sinLookUpTable[thetaIntPart + 1] - sinLookUpTable[thetaIntPart]) ;}float FastCos(float theta){theta = fmodf(theta, 360) ;if (theta < 0)theta += 360 ;int thetaIntPart = (int)theta ;float thetaFracPart = theta - thetaIntPart ;return cosLookUpTable[thetaIntPart] +thetaFracPart * (cosLookUpTable[thetaIntPart + 1] - cosLookUpTable[thetaIntPart]) ;}int Fast_Distance_2D(int x, int y){x = abs(x) ;y = abs(y) ;int mn = MIN(x, y) ;return (x + y - (mn >> 1) - (mn >> 2) + (mn >> 4)) ;}float Fast_Distance_3D(float x, float y, float z){int temp ;int newX, newY, newZ ;newX = fabs(x) * 1024 ;newY = fabs(y) * 1024 ;newZ = fabs(z) * 1024 ;if (newY < newX)SWAP(newX, newY, temp) ;if (newZ < newY)SWAP(newZ, newY, temp) ;if (newY < newX)SWAP(newX, newY, temp) ;int distance = (newZ + 11 * (newY >> 5) + (newX >> 2)) ;return ((float)(distance >> 10)) ;}void POLAR2D_To_POINT2D(POLAR2D_PTR polar, POINT2D_PTR rect){rect ->y = FastSin(polar ->theta) * polar ->r ;rect ->x = FastCos(polar ->theta) * polar ->r ;}void POINT2D_To_POLAR2D(POINT2D_PTR rect , POLAR2D_PTR polar){polar ->theta = atanf(rect ->y / rect ->x) ;polar ->r = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y)) ;}void POINT2D_To_PolarRTh(POINT2D_PTR rect, float * pR, float * pTheta){*pTheta = atanf(rect ->y / rect ->x) ;*pR = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y)) ;}void CYLINDRICAL3D_To_POINT3D(CYLINDRICAL3D_PTR cyl, POINT3D_PTR rect){rect ->x = FastCos(cyl ->theta) * cyl ->r ;rect ->y = FastSin(cyl ->theta) * cyl ->r ;rect ->z = cyl ->z ;}void CYLINDRICAL3D_To_POINT3D(CYLINDRICAL3D_PTR cyl, float *pX, float *pY, float *pZ){*pX = FastCos(cyl ->theta) * cyl ->r ;*pY = FastSin(cyl ->theta) * cyl ->r ;*pZ = cyl ->z ;}void POINT3D_To_CYLINDRICAL3D(POINT3D_PTR rect, CYLINDRICAL3D_PTR cyl){cyl ->theta = atanf(rect ->y / rect ->x) ;cyl ->r = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y)) ;cyl ->z = rect ->z ;}void SPHERICAL3D_To_POINT3D(SPHERICAL3D_PTR sph, POINT3D_PTR rect){float r = sph ->p * FastSin(sph ->theta) ;rect ->x = r * FastCos(sph ->phi) ;rect ->y = r * FastSin(sph ->phi) ;rect ->z = FastCos(sph ->theta) * sph ->p ;}void SPHERICAL3D_To_RectXYZ(SPHERICAL3D_PTR sph, float * pX, float * pY, float * pZ){float r = sph ->p * FastSin(sph ->theta) ;*pX = r * FastCos(sph ->phi) ;*pY = r * FastSin(sph ->phi) ;*pZ= FastCos(sph ->theta) * sph ->p ;}void POINT3D_To_SPHERICAL3D(POINT3D_PTR rect, SPHERICAL3D_PTR sph){sph ->p = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y) + (rect ->z * rect ->z)) ;sph ->phi = atanf(rect ->y / rect ->x) ;float r = rect ->y / FastSin(sph ->phi) ;sph ->theta = asinf(r / sph ->p) ;}void POINT3D_To_SphericalPThPh(POINT3D_PTR rect, float * pP, float * pTheta, float * pPhi){*pP = sqrtf((rect ->x * rect ->x) + (rect ->y * rect ->y) + (rect ->z * rect ->z)) ;*pPhi = atanf(rect ->y / rect ->x) ;float r = rect ->y / FastSin(*pPhi) ;*pTheta = asinf(r / *pP) ;}void VECTOR2D_Add(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum){vsum ->x = va ->x + vb ->x ;vsum ->y = va ->y + vb ->y ;}VECTOR2D VECTOR2D_Add(VECTOR2D_PTR va, VECTOR2D_PTR vb){VECTOR2D result ;result.x = va ->x + vb ->y ;result.y = va ->y + vb ->y ;return result ;}void VECTOR2D_Sub(VECTOR2D_PTR va, VECTOR2D_PTR vb, VECTOR2D_PTR vsum){vsum ->x = va ->x - vb ->x ;vsum ->y = va ->y - vb ->y ;}VECTOR2D VECTOR2D_Sub(VECTOR2D_PTR va, VECTOR2D_PTR vb){VECTOR2D result ;result.x = va ->x - vb ->x ;result.y = va ->y - vb ->y ;return result ;}void VECTOR3D_Add(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 ;}VECTOR3D VECTOR3D_Add(VECTOR3D_PTR va, VECTOR3D_PTR vb){VECTOR3D result ;result.x = va ->x + vb ->x ;result.y = va ->y + vb ->y ;result.z = va ->z + vb ->z ;return result ;}void VECTOR3D_Sub(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vdiff){vdiff ->x = va ->x - vb ->x ;vdiff ->y = va ->y - vb ->y ;vdiff ->z = va ->z - vb ->z ;}VECTOR3D VECTOR3D_Sub(VECTOR3D_PTR va, VECTOR3D_PTR vb){VECTOR3D result ;result.x = va ->x - vb ->x ;result.y = va ->y - vb ->y ;result.z = va ->z - vb ->z ;return result ;}//4D向量加法void VECTOR4D_Add(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 ;}VECTOR4D VECTOR4D_Add(VECTOR4D_PTR va, VECTOR4D_PTR vb){VECTOR4D result ;result.x = va ->x + vb ->x ;result.y = va ->y + vb ->y ;result.z = va ->z + vb ->z ;result.w = 1 ;return result ;}//4D向量减法void VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vdiff){vdiff ->x = va ->x - vb ->x ;vdiff ->y = va ->y - vb ->y ;vdiff ->z = va ->z - vb ->z ;vdiff ->w = 1 ;}VECTOR4D VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb){VECTOR4D result ;result.x = va ->x - vb ->x ;result.y = va ->y - vb ->y ;result.z = va ->z - vb ->z ;result.w = 1 ;return result ;}void VECTOR2D_Scale(float k, VECTOR2D_PTR va, VECTOR2D_PTR vscaled){vscaled ->x = va ->x * k ;vscaled ->y = va ->y * k ;}void VECTOR2D_Scale(float k, VECTOR2D_PTR va){va ->x *= k ;va ->y *= k ;}void VECTOR3D_Scale(float k, VECTOR3D_PTR va, VECTOR3D_PTR vscaled){vscaled ->x = va ->x * k ;vscaled ->y = va ->y * k ;vscaled ->z = va ->z * k ;}void VECTOR3D_Scale(float k, VECTOR3D_PTR va){va ->x *= k ;va ->y *= k ;va ->z *= k ;}void VECTOR4D_Scale(float k, VECTOR4D_PTR va, VECTOR4D_PTR vscaled){vscaled ->x = va ->x * k ;vscaled ->y = va ->y * k ;vscaled ->z = va ->z * k ;vscaled ->w = 1 ;}void VECTOR4D_Scale(float k, VECTOR4D_PTR va){va ->x *= k ;va ->y *= k ;va ->z *= k ;va ->w = 1 ;}float VECTOR2D_Dot(VECTOR2D_PTR va, VECTOR2D_PTR vb){return va ->x * vb ->x + va ->y * vb ->y ;}float VECTOR3D_Dot(VECTOR3D_PTR va, VECTOR3D_PTR vb){return va ->x * vb ->x + va ->y * vb ->y + va ->z * vb ->z ;}float VECTOR4D_Dot(VECTOR4D_PTR va, VECTOR4D_PTR vb){return va ->x * vb ->x + va ->y * vb ->y + va ->z * vb ->z ;}void VECTOR3D_Cross(VECTOR3D_PTR va, VECTOR3D_PTR vb, VECTOR3D_PTR vn){vn ->x = va ->y * vb ->z - va ->z * vb ->y ;vn ->y = va ->z * vb ->x - va ->x * vb ->z ;vn ->z = va ->x * vb ->y - va ->y * vb ->x ;}VECTOR3D VECTOR3D_Cross(VECTOR3D_PTR va, VECTOR3D_PTR vb){VECTOR3D result ;result.x = va ->y * vb ->z - va ->z * vb ->y ;result.y = va ->z * vb ->x - va ->x * vb ->z ;result.z = va ->x * vb ->y - va ->y * vb ->x ;return result ;}void VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vn){vn ->x = va ->y * vb ->z - va ->z * vb ->y ;vn ->y = va ->z * vb ->x - va ->x * vb ->z ;vn ->z = va ->x * vb ->y - va ->y * vb ->x ;vn ->w = 1 ;}VECTOR4D VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb){VECTOR4D result ;result.x = va ->y * vb ->z - va ->z * vb ->y ;result.y = va ->z * vb ->x - va ->x * vb ->z ;result.z = va ->x * vb ->y - va ->y * vb ->x ;result.w = 1 ;return result ;}float VECTOR2D_Length(VECTOR2D_PTR va){return sqrtf(va ->x * va ->x + va ->y * va ->y) ;}float VECTOR3D_Length(VECTOR3D_PTR va){return sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;}float VECTOR4D_Length(VECTOR4D_PTR va){return sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;}float VECTOR2D_Length_Fast(VECTOR2D_PTR va){return ((float) Fast_Distance_2D(va ->x, va ->y)) ;}float VECTOR3D_Length_Fast(VECTOR3D_PTR va){return (Fast_Distance_3D(va ->x, va ->y, va ->z)) ;}float VECTOR4D_Length_Fast(VECTOR4D_PTR va){return (Fast_Distance_3D(va ->x, va ->y, va ->z)) ;}void VECTOR2D_Normalize(VECTOR2D_PTR va){float length = sqrtf(va ->x * va ->x + va ->y * va ->y) ;if (length < EPSILON_E5)return ;va ->x /= length ;va ->y /= length ;}void VECTOR3D_Normalize(VECTOR3D_PTR va){float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;if (length < EPSILON_E5)return ;va ->x /= length ;va ->y /= length ;va ->z /= length ;}void VECTOR4D_Normalize(VECTOR4D_PTR va){float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;if (length < EPSILON_E5)return ;va ->x /= length ;va ->y /= length ;va ->z /= length ;va ->w = 1 ;}void VECTOR2D_Normalize(VECTOR2D_PTR va, VECTOR2D_PTR vn){float length = sqrtf(va ->x * va ->x + va ->y * va ->y) ;if (length < EPSILON_E5)return ;vn ->x = va ->x / length ;vn ->y = va ->y / length ;}void VECTOR3D_Normalize(VECTOR3D_PTR va, VECTOR3D_PTR vn){float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;if (length < EPSILON_E5)return ;vn ->x = va ->x / length ;vn ->y = va ->y / length ;vn ->z = va ->z / length ;}void VECTOR4D_Normalize(VECTOR4D_PTR va, VECTOR4D_PTR vn){float length = sqrtf(va ->x * va ->x + va ->y * va ->y + va ->z * va ->z) ;if (length < EPSILON_E5)return ;vn ->x = va ->x / length ;vn ->y = va ->y / length ;vn ->z = va ->z / length ;va ->w = 1 ;}void VECTOR2D_Build(VECTOR2D_PTR init, VECTOR2D_PTR term, VECTOR2D_PTR result){result ->x = term ->x - init ->x ;result ->y = term ->y - init ->y ;}void VECTOR3D_Build(VECTOR3D_PTR init, VECTOR3D_PTR term, VECTOR3D_PTR result){result ->x = term ->x - init ->x ;result ->y = term ->y - init ->y ;result ->z = term ->z - init ->z ;}void VECTOR4D_Build(VECTOR4D_PTR init, VECTOR4D_PTR term, VECTOR4D_PTR result){result ->x = term ->x - init ->x ;result ->y = term ->y - init ->y ;result ->z = term ->z - init ->z ;result ->w = 1 ;}float VECTOR2D_CosTh(VECTOR2D_PTR va, VECTOR2D_PTR vb){return (VECTOR2D_Dot(va, vb) / (VECTOR2D_Length(va) * VECTOR2D_Length(vb))) ;}float VECTOR3D_CosTh(VECTOR3D_PTR va, VECTOR3D_PTR vb){return (VECTOR3D_Dot(va, vb) / (VECTOR3D_Length(va) * VECTOR3D_Length(vb))) ;}float VECTOR4D_CosTh(VECTOR4D_PTR va, VECTOR4D_PTR vb){return (VECTOR4D_Dot(va, vb) / (VECTOR4D_Length(va) * VECTOR4D_Length(vb))) ;}void Mat_Init_2X2(MATRIX2X2_PTR ma, float m00, float m01, float m10, float m11){ma ->M00 = m00 ;ma ->M01 = m01 ;ma ->M10 = m10 ;ma ->M11 = m11 ;}void Mat_Add_2X2(MATRIX2X2_PTR ma, MATRIX2X2_PTR mb, MATRIX2X2_PTR msum){msum ->M00 = ma ->M00 + mb ->M00 ;msum ->M01 = ma ->M01 + mb ->M01 ;msum ->M10 = ma ->M10 + mb ->M10 ;msum ->M11 = ma ->M11 + mb ->M11 ;}void Mat_Mul_2X2(MATRIX2X2_PTR ma, MATRIX2X2_PTR mb, MATRIX2X2_PTR mprod){mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 ;mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 ;mprod ->M10 = ma ->M10 * mb ->M00 + ma ->M11 * mb ->M10 ;mprod ->M11 = ma ->M10 * mb ->M01 + ma ->M11 * mb ->M11 ;}void Mat_Mul_1X2_3X2(MATRIX1X2_PTR ma, MATRIX3X2_PTR mb, MATRIX1X2_PTR mprod){mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + 1 * mb ->M20 ;mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + 1 * mb ->M21 ;}float Mat_Det_2X2(MATRIX2X2_PTR m){return m ->M00 * m ->M11 - m ->M01 * m ->M10 ;}bool Mat_Inverse_2X2(MATRIX2X2_PTR m, MATRIX2X2_PTR mi){float det = Mat_Det_2X2(m) ;if (fabs(det) < EPSILON_E5)return false ;mi ->M00 = m ->M11 / det ;mi ->M01 = -(m ->M01 / det) ;mi ->M10 = -(m ->M10 / det) ;mi ->M11 = m ->M00 / det ;return true ;}void Mat_Init_3X3(MATRIX3X3_PTR ma,float m00, float m01, float m02,float m10, float m11, float m12,float m20, float m21, float m22){ma ->M00 = m00 ; ma ->M01 = m01 ; ma ->M02 = m02 ;ma ->M10 = m10 ; ma ->M11 = m11 ; ma ->M12 = m12 ;ma ->M20 = m20 ; ma ->M21 = m21 ; ma ->M22 = m22 ;}void Mat_Add_3X3(MATRIX3X3_PTR ma, MATRIX3X3_PTR mb, MATRIX3X3_PTR msum){msum ->M00 = ma ->M00 + mb ->M00 ;msum ->M01 = ma ->M01 + mb ->M01 ;msum ->M02 = ma ->M02 + mb ->M02 ;msum ->M10 = ma ->M10 + mb ->M10 ;msum ->M11 = ma ->M11 + mb ->M11 ;msum ->M12 = ma ->M12 + mb ->M12 ;msum ->M20 = ma ->M20 + mb ->M20 ;msum ->M21 = ma ->M21 + mb ->M21 ;msum ->M22 = ma ->M22 + mb ->M22 ;}void Mat_Mul_3X3_3X3(MATRIX3X3_PTR ma, MATRIX3X3_PTR mb, MATRIX3X3_PTR mprod){mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 ;mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 ;mprod ->M02 = ma ->M00 * mb ->M02 + ma ->M01 * mb ->M12 + ma ->M02 * mb ->M22 ;mprod ->M10 = ma ->M10 * mb ->M00 + ma ->M11 * mb ->M10 + ma ->M12 * mb ->M20 ;mprod ->M11 = ma ->M10 * mb ->M01 + ma ->M11 * mb ->M11 + ma ->M12 * mb ->M21 ;mprod ->M12 = ma ->M10 * mb ->M02 + ma ->M11 * mb ->M12 + ma ->M12 * mb ->M22 ;mprod ->M20 = ma ->M20 * mb ->M00 + ma ->M21 * mb ->M10 + ma ->M22 * mb ->M20 ;mprod ->M21 = ma ->M20 * mb ->M01 + ma ->M21 * mb ->M11 + ma ->M22 * mb ->M21 ;mprod ->M22 = ma ->M20 * mb ->M02 + ma ->M21 * mb ->M12 + ma ->M22 * mb ->M22 ;}float Mat_Det_3X3(MATRIX3X3_PTR m){return m ->M00 * (m ->M11 * m ->M22 - m ->M21 * m ->M12) -m ->M01 * (m ->M10 * m ->M22 - m ->M20 * m ->M12) +m ->M02 * (m ->M10 * m ->M21 - m ->M20 * m ->M11) ;}bool Mat_Inverse_3X3(MATRIX3X3_PTR m, MATRIX3X3_PTR mi){float det = Mat_Det_3X3(m) ;if (fabs(det) < EPSILON_E5)return false ;mi ->M00 = (m ->M11 * m ->M22 - m ->M21 * m ->M12) / det ;mi ->M10 = -(m ->M10 * m ->M22 - m ->M20 * m ->M12) / det ;mi ->M20 = (m ->M10 * m ->M21 - m ->M20 * m ->M11) / det ;mi ->M01 = -(m ->M01 * m ->M22 - m ->M21 * m ->M02) / det ;mi ->M11 = (m ->M00 * m ->M22 - m ->M20 * m ->M02) / det ;mi ->M21 = -(m ->M00 * m ->M21 - m ->M20 * m ->M01) / det ;mi ->M02 = (m ->M01 * m ->M12 - m ->M11 * m ->M02) / det ;mi ->M12 = -(m ->M00 * m ->M12 - m ->M10 * m ->M02) / det ;mi ->M22 = (m ->M00 * m ->M11 - m ->M10 * m ->M01) / det ;return true ;}void Mat_Mul_VECTOT3D_3X3(VECTOR3D_PTR va, MATRIX3X3_PTR mb, VECTOR3D_PTR vprod){vprod ->x = va ->x * mb ->M00 + va ->y * mb ->M10 + va ->z * mb ->M20 ;vprod ->y = va ->x * mb ->M01 + va ->y * mb ->M11 + va ->z * mb ->M21 ;vprod ->z = va ->x * mb ->M02 + va ->y * mb ->M12 + va ->z * mb ->M22 ;}void Mat_Mul_1X3_3X3(MATRIX1X3_PTR ma, MATRIX3X3_PTR mb, MATRIX1X3_PTR mprod){mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 ;mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 ;mprod ->M02 = ma ->M00 * mb ->M11 + ma ->M01 * mb ->M21 + ma ->M02 * mb ->M22 ;}void Mat_Init_4X4(MATRIX4X4_PTR ma,float m00, float m01, float m02, float m03,float m10, float m11, float m12, float m13,float m20, float m21, float m22, float m23,float m30, float m31, float m32, float m33){ma ->M00 = m00 ; ma ->M01 = m01 ; ma ->M02 = m02 ; ma ->M03 = m03 ;ma ->M10 = m10 ; ma ->M11 = m11 ; ma ->M12 = m12 ; ma ->M13 = m13 ;ma ->M20 = m20 ; ma ->M21 = m21 ; ma ->M22 = m22 ; ma ->M23 = m23 ;ma ->M30 = m30 ; ma ->M31 = m31 ; ma ->M32 = m32 ; ma ->M33 = m33 ;}void Mat_Add_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR msum){msum ->M00 = ma ->M00 + mb ->M00 ;msum ->M01 = ma ->M01 + mb ->M01 ;msum ->M02 = ma ->M02 + mb ->M02 ;msum ->M03 = ma ->M03 + mb ->M03 ;msum ->M10 = ma ->M10 + mb ->M10 ;msum ->M11 = ma ->M11 + mb ->M11 ;msum ->M12 = ma ->M12 + mb ->M12 ;msum ->M13 = ma ->M13 + mb ->M13 ;msum ->M20 = ma ->M20 + mb ->M20 ;msum ->M21 = ma ->M21 + mb ->M21 ;msum ->M22 = ma ->M22 + mb ->M22 ;msum ->M33 = ma ->M33 + mb ->M33 ;}void Mat_Mul_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR mprod){//for (int row = 0; row < 4; ++row)//{//for (int col = 0; col < 4; ++col)//{//float sum = 0 ;//for (int index = 0; index < 4; ++index)//{//sum += ma ->M[row][index] * mb ->M[index][col] ;//}//mprod ->M[row][col] = sum ;//}//}mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 + ma ->M03 * mb ->M30 ;mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 + ma ->M03 * mb ->M31 ;mprod ->M02 = ma ->M00 * mb ->M02 + ma ->M01 * mb ->M12 + ma ->M02 * mb ->M22 + ma ->M03 * mb ->M32 ;mprod ->M03 = ma ->M00 * mb ->M03 + ma ->M01 * mb ->M13 + ma ->M02 * mb ->M23 + ma ->M03 * mb ->M33 ;mprod ->M10 = ma ->M10 * mb ->M00 + ma ->M11 * mb ->M10 + ma ->M12 * mb ->M20 + ma ->M13 * mb ->M30 ;mprod ->M11 = ma ->M10 * mb ->M01 + ma ->M11 * mb ->M11 + ma ->M12 * mb ->M21 + ma ->M13 * mb ->M31 ;mprod ->M12 = ma ->M10 * mb ->M02 + ma ->M11 * mb ->M12 + ma ->M12 * mb ->M22 + ma ->M13 * mb ->M32 ;mprod ->M13 = ma ->M10 * mb ->M03 + ma ->M11 * mb ->M13 + ma ->M12 * mb ->M23 + ma ->M13 * mb ->M33 ;mprod ->M20 = ma ->M20 * mb ->M00 + ma ->M21 * mb ->M10 + ma ->M22 * mb ->M20 + ma ->M23 * mb ->M30 ;mprod ->M21 = ma ->M20 * mb ->M01 + ma ->M21 * mb ->M11 + ma ->M22 * mb ->M21 + ma ->M23 * mb ->M31 ;mprod ->M22 = ma ->M20 * mb ->M02 + ma ->M21 * mb ->M12 + ma ->M22 * mb ->M22 + ma ->M23 * mb ->M32 ;mprod ->M23 = ma ->M20 * mb ->M03 + ma ->M21 * mb ->M13 + ma ->M22 * mb ->M23 + ma ->M23 * mb ->M33 ;mprod ->M30 = ma ->M30 * mb ->M00 + ma ->M31 * mb ->M10 + ma ->M32 * mb ->M20 + ma ->M33 * mb ->M30 ;mprod ->M31 = ma ->M30 * mb ->M01 + ma ->M31 * mb ->M11 + ma ->M32 * mb ->M21 + ma ->M33 * mb ->M31 ;mprod ->M32 = ma ->M30 * mb ->M02 + ma ->M31 * mb ->M12 + ma ->M32 * mb ->M22 + ma ->M33 * mb ->M32 ;mprod ->M33 = ma ->M30 * mb ->M03 + ma ->M31 * mb ->M13 + ma ->M32 * mb ->M23 + ma ->M33 * mb ->M33 ;}bool Mat_Inverse_4X4(MATRIX4X4_PTR m, MATRIX4X4_PTR mi){float det = Mat_Det_3X3(reinterpret_cast<MATRIX3X3_PTR>(m)) ;if (fabs(det) < EPSILON_E5)return false ;mi ->M00 = (m ->M11 * m ->M22 - m ->M12 * m ->M21) / det ;mi ->M01 = -(m ->M01 * m ->M22 - m ->M02 * m ->M21) / det ;mi ->M02 = (m ->M01 * m ->M12 - m ->M02 * m ->M11 ) / det ;mi ->M03 = 0 ;mi ->M10 = -( m ->M10 * m ->M22 - m ->M12 * m ->M20 ) / det ;mi ->M11 =  ( m ->M00 * m ->M22 - m ->M02 * m ->M20 ) / det ;mi ->M12 = -( m ->M00 * m ->M12 - m ->M02 * m ->M10 ) / det ;mi ->M13 = 0 ;mi ->M20 = ( m ->M10 * m ->M21 - m ->M11 * m ->M20 ) / det ;mi ->M21 = -( m ->M00 * m ->M21 - m ->M01 * m ->M20 ) / det ;mi ->M22 = ( m ->M00 * m ->M11 - m ->M01 * m ->M10 ) / det ;mi ->M23 = 0 ;mi ->M30 = -( m ->M30 * mi ->M00 + m ->M31 * mi ->M10 + m ->M32 * mi ->M20 ) / det ;mi ->M31 = -( m ->M30 * mi ->M01 + m ->M31 * mi ->M11 + m ->M32 * mi ->M21 ) / det ;mi ->M32 = -( m ->M30 * mi ->M02 + m ->M31 * mi ->M12 + m ->M32 * mi ->M22 ) / det ;mi ->M33 = 1.0f;return true ;}void Mat_Mul_VECTOR3D_4X4(VECTOR3D_PTR va, MATRIX4X4_PTR mb, VECTOR3D_PTR vprod){for (int col = 0; col < 3; ++col){float sum = 0 ;for (int row = 0; row < 3; ++row){sum += va ->M[row] * mb ->M[row][col] ;}sum += mb ->M[3][col] ;vprod ->M[col] = sum ;}}void Mat_Mul_VECTOR3D_4X3(VECTOR3D_PTR va, MATRIX4X3_PTR mb, VECTOR3D_PTR vprod){for (int col = 0; col < 3; ++col){float sum = 0 ;for (int row = 0; row < 3; ++row){sum += va ->M[row] * mb ->M[row][col] ;}sum += mb ->M[3][col] ;vprod ->M[col] = sum ;}}void Mat_Mul_VECTOR4D_4X4(VECTOR4D_PTR va, MATRIX4X4_PTR mb, VECTOR4D_PTR vprod){for (int col = 0; col < 4; ++col){float sum = 0 ;for (int row = 0; row < 4; ++row){sum += va ->M[col] * mb ->M[row][col] ;}vprod ->M[col] = sum ;}}void Mat_Mul_VECTOR4D_4X3(VECTOR4D_PTR va, MATRIX4X3_PTR mb, VECTOR4D_PTR vprod){for (int col = 0; col < 3; ++col){float sum = 0 ;for (int row = 0; row < 4; ++row){sum += va ->M[col] * mb ->M[col][row] ;}vprod ->M[col] = sum ;}}void Mat_Mul_1X4_4X4(MATRIX1X4_PTR ma, MATRIX4X4_PTR mb, MATRIX1X4_PTR mprod){mprod ->M00 = ma ->M00 * mb ->M00 + ma ->M01 * mb ->M10 + ma ->M02 * mb ->M20 + ma ->M03 * mb ->M30 ;mprod ->M01 = ma ->M00 * mb ->M01 + ma ->M01 * mb ->M11 + ma ->M02 * mb ->M21 + ma ->M03 * mb ->M31 ;mprod ->M02 = ma ->M00 * mb ->M02 + ma ->M01 * mb ->M12 + ma ->M02 * mb ->M22 + ma ->M03 * mb ->M32 ;mprod ->M03 = ma ->M00 * mb ->M03 + ma ->M01 * mb ->M13 + ma ->M02 * mb ->M23 + ma ->M03 * mb ->M33 ;}void Init_Parm_Line2D(POINT2D_PTR p_init, POINT2D_PTR p_term, PARMLINE2D_PTR p){VECTOR2D_INIT(&(p ->p0), p_init) ;VECTOR2D_INIT(&(p ->p1), p_term) ;VECTOR2D_Build(p_init, p_term, &(p ->v)) ;}void Compute_Parm_Line2D(PARMLINE2D_PTR p, float t, POINT2D_PTR pt){pt ->x = p ->p0.x + p ->v.x * t ;pt ->y = p ->p0.y + p ->v.y * t ;}int Intersect_Parm_Line2D(PARMLINE2D_PTR p1, PARMLINE2D_PTR p2, float * t1, float * t2){//y1/x1 - y2/x2 = y1 * x2 - y2 * x1float det_p1p2 = (p1 ->v.x * p2 ->v.y - p1 ->v.y * p2 ->v.x) ;if (fabs(det_p1p2) <= EPSILON_E5)return PARM_LINE_NO_INTERSECT ;*t1 = (p1 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p2 ->v.y * (p1 ->p0.x - p2 ->p0.x)) / det_p1p2 ;*t2 = (p1 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p1 ->v.y * (p1 ->p0.x - p2 ->p0.x)) / det_p1p2 ;if (*t1 >= 0 && *t1 <= 1 &&*t2 >= 0 && *t2 <= 1){return PARM_LINE_INTERSECT_IN_SEGMENT ;}else{return PARM_LINE_INTERSECT_OUT_SEGMENT ;}}int Intersect_Parm_Lines2D(PARMLINE2D_PTR p1, PARMLINE2D_PTR p2, POINT2D_PTR pt){float t1 ;float t2 ;float det_p1p2 = (p1 ->v.x * p2 ->v.y - p1 ->v.y * p2 ->v.x) ;if (fabs(det_p1p2) <= EPSILON_E5)return PARM_LINE_NO_INTERSECT ;t1 = (p2 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p2 ->v.y * (p1 ->p0.x - p2 ->p0.x))/ det_p1p2;t2 = (p1 ->v.x * (p1 ->p0.y - p2 ->p0.y) - p1->v.y * (p1 ->p0.x - p2 ->p0.x))/ det_p1p2;pt ->x = p1 ->p0.x + p1 ->v.x * t1 ;pt ->y = p1 ->p0.y + p1 ->v.y * t1 ;if (t1 >= 0 && t1 <= 1 &&t2 >= 0 && t2 <= 1){return PARM_LINE_INTERSECT_IN_SEGMENT ;}else{return PARM_LINE_INTERSECT_OUT_SEGMENT ;}}void Init_Parm_Line3D(POINT3D_PTR p_init, POINT3D_PTR p_term, PARMLINE3D_PTR p){VECTOR3D_INIT(&(p ->p0), p_init) ;VECTOR3D_INIT(&(p ->p1), p_term) ;VECTOR3D_Build(p_init, p_term, &(p ->V)) ;}void Compute_Parm_Line3D(PARMLINE3D_PTR p, float t, POINT3D_PTR pt){pt ->x = p ->p0.x + p ->V.x * t ;pt ->y = p ->p0.y + p ->V.y * t ;pt ->z = p ->p0.z + p ->V.z * t ;}void POINT3D_COPY(POINT3D_PTR vdst, POINT3D_PTR vsrc){vdst ->x = vsrc ->x ;vdst ->y = vsrc ->y ;vdst ->z = vsrc ->z ;}void VECTOR3D_COPY(VECTOR3D_PTR vdst, VECTOR3D_PTR vsrc){vdst ->x = vsrc ->x ;vdst ->y = vsrc ->y ;vdst ->z = vsrc ->z ;}void PLANE3D_Init(PLANE3D_PTR plane, POINT3D_PTR p0, VECTOR3D_PTR normal, bool normalize){POINT3D_COPY(&(plane ->p0), p0) ;if (!normalize)VECTOR3D_COPY(&(plane ->n), normal) ;elseVECTOR3D_Normalize(normal, &(plane ->n)) ;}float Compute_Point_In_Plane3D(POINT3D_PTR pt, PLANE3D_PTR plane){float hs = plane ->n.x * (pt ->x - plane ->p0.x) +plane ->n.y * (pt ->y - plane ->p0.y) +plane ->n.z * (pt ->z - plane ->p0.z) ;return hs ;}int Intersect_Parm_Line3D_Plane3D(PARMLINE3D_PTR pline, PLANE3D_PTR plane, float *t, POINT3D_PTR pt){float plane_dot_line = VECTOR3D_Dot(&(pline ->V), &(plane ->n)) ; if (fabs(plane_dot_line) <= EPSILON_E5){if (fabs(Compute_Point_In_Plane3D(&(pline ->p0), plane)) <= EPSILON_E5)return PARM_LINE_INTERSECT_EVERYWHERE ;elsereturn PARM_LINE_NO_INTERSECT ;}*t = -(plane ->n.x * pline ->p0.x + plane ->n.y * pline ->p0.y + plane ->n.z * pline ->p0.z +(-plane ->n.x * plane ->p0.x - plane ->n.y * plane ->p0.y - plane ->n.z * plane ->p0.z)) /plane_dot_line ;pt ->x = pline ->p0.x + pline ->V.x * (*t) ;pt ->y = pline ->p0.y + pline ->V.y * (*t) ;pt ->z = pline ->p0.z + pline ->V.z * (*t) ;if (*t >= 0 && *t <= 1)return PARM_LINE_INTERSECT_IN_SEGMENT ;elsereturn PARM_LINE_INTERSECT_OUT_SEGMENT ;}void VECTOR3D_Theta_To_QUAT(QUAT_PTR q, VECTOR3D_PTR v, float theta){float theta_div_2 = theta / 2 ;float sinf_theta = sinf(theta_div_2) ;q ->x = sinf_theta * v ->x ;q ->y = sinf_theta * v ->y ;q ->z = sinf_theta * v ->z ;q ->w = cosf(theta_div_2) ;}void VECTOR4D_Theta_To_QUAT(QUAT_PTR q, VECTOR4D_PTR v, float theta){float theta_div_2 = theta / 2 ;float sinf_theta = sinf(theta_div_2) ;q ->x = sinf_theta * v ->x ;q ->y = sinf_theta * v ->y ;q ->z = sinf_theta * v ->z ;q ->w = cosf(theta_div_2) ;}void EulerZYX_To_QUAT(QUAT_PTR q, float theta_z, float theta_y, float theta_x){float cos_z_2 = cosf(theta_z) / 2 ;float cos_y_2 = cosf(theta_y) / 2 ;float cos_x_2 = cosf(theta_x) / 2 ;float sin_z_2 = sinf(theta_z) / 2 ;float sin_y_2 = sinf(theta_y) / 2 ;float sin_x_2 = sinf(theta_x) / 2 ;q ->w = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2 ;q ->x = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2 ;q ->y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2 ;q ->z = cos_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2 ;}void QUAT_To_VECTOR3D_Theta(QUAT_PTR q, VECTOR3D_PTR v, float * theta){*theta = acosf(q ->w) ;float sin_theta = sinf(*theta) ;v ->x = q ->x / sin_theta ;v ->y = q ->y / sin_theta ;v ->z = q ->z / sin_theta ;*theta *= 2 ;}void QUAT_Add(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum){qsum ->x = q1 ->x + q2 ->x ;qsum ->y = q1 ->y + q2 ->y ;qsum ->z = q1 ->z + q2 ->z ;qsum ->w = q1 ->w + q2 ->w ;}void QUAT_Sub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff){qdiff ->x = q1 ->x - q2 ->x ;qdiff ->y = q1 ->y - q2 ->y ;qdiff ->z = q1 ->z - q2 ->z ;qdiff ->w = q1 ->w - q2 ->w ;}void QUAT_Conjugate(QUAT_PTR q, QUAT_PTR qconj){qconj ->x = -q ->x ;qconj ->y = -q ->y ;qconj ->z = -q ->z ;qconj ->w = q ->w ;}void QUAT_Scale(QUAT_PTR q, float scale, QUAT_PTR qs){qs ->x = scale * q ->x ;qs ->y = scale * q ->y ;qs ->z = scale * q ->z ;qs ->w = scale * q ->w ;}void QUAT_Scale(QUAT_PTR q, float scale){q ->x *= scale ;q ->y *= scale ;q ->z *= scale ;q ->w *= scale ;}float QUAT_Norm(QUAT_PTR q){return sqrtf(q ->w * q ->w + q ->x * q ->x + q ->y * q ->y + q ->z * q ->z) ;}float QUAT_Norm2(QUAT_PTR q){return q ->w * q ->w + q ->x * q ->x + q ->y * q ->y + q ->z * q ->z ;}void QUAT_Normalize(QUAT_PTR q, QUAT_PTR qn){float length = QUAT_Norm(q) ;qn ->w = q ->w / length ;qn ->x = q ->x / length ;qn ->y = q ->y / length ;qn ->z = q ->z / length ;}void QUAT_Normalize(QUAT_PTR q){float length = QUAT_Norm(q) ;q ->w /= length ;q ->x /= length ;q ->y /= length ;q ->z /= length ;}void QUAT_Unit_Inverse(QUAT_PTR q, QUAT_PTR qi){qi ->w = q ->w ;qi ->x = -q ->x ;qi ->y = -q ->y ;qi ->z = -q ->z ;}void QUAT_Unit_Inverse(QUAT_PTR q){q ->x = -q ->x ;q ->y = -q ->y ;q ->z = -q ->z ;}void QUAT_Mul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod){qprod ->w = q1 ->w * q2 ->w - q1 ->x * q2 ->x - q1 ->y * q2 ->y - q1 ->z * q2 ->z ;qprod ->x = q1 ->w * q2 ->w + q2 ->x * q2 ->w + q1 ->y * q2 ->z - q1 ->z * q2 ->y ;qprod ->y = q1 ->w * q2 ->y - q1 ->x * q2 ->z + q1 ->y * q2 ->w - q1 ->z * q2 ->x ;qprod ->z = q1 ->w * q2 ->z + q1 ->x * q2 ->y - q1 ->y * q2 ->x + q1 ->z * q2 ->w ;}void QUAT_Triple_Product(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod){QUAT qtemp ;QUAT_Mul(q1, q2, &qtemp) ;QUAT_Mul(&qtemp, q3, qprod) ;}bool Solve_2X2_System(MATRIX2X2_PTR A, MATRIX1X2_PTR X, MATRIX1X2_PTR B){float det_A = Mat_Det_2X2(A) ;if (fabs(det_A) < EPSILON_E5)return false ;MATRIX2X2 work_mat ;MAT_COPY_2X2(A, &work_mat) ;MAT_COLUMN_SWAP_2X2(&work_mat, 0, B) ;float det_ABx = Mat_Det_2X2(&work_mat) ;X ->M00 = det_ABx / det_A ;MAT_COPY_2X2(A, &work_mat) ;MAT_COLUMN_SWAP_2X2(&work_mat, 1, B) ;float det_ABy = Mat_Det_2X2(&work_mat) ;X ->M01 = det_ABy / det_A ;return true ;}bool Solve_3X3_System(MATRIX3X3_PTR A, MATRIX1X3_PTR X, MATRIX1X3_PTR B){float det_A = Mat_Det_3X3(A);if (fabs(det_A) < EPSILON_E5)return false ;MATRIX3X3 work_mat; MAT_COPY_3X3(A, &work_mat);MAT_COLUMN_SWAP_3X3(&work_mat, 0, B);float det_ABx = Mat_Det_3X3(&work_mat);X->M00 = det_ABx/det_A;MAT_COPY_3X3(A, &work_mat);MAT_COLUMN_SWAP_3X3(&work_mat, 1, B);float det_ABy = Mat_Det_3X3(&work_mat);X->M01 = det_ABy/det_A;MAT_COPY_3X3(A, &work_mat);MAT_COLUMN_SWAP_3X3(&work_mat, 2, B);float det_ABz = Mat_Det_3X3(&work_mat);X->M02 = det_ABz/det_A;return true ;}