From 4b6df39318dc8831ac99ac81069b9b6ba218cb25 Mon Sep 17 00:00:00 2001 From: Iliyas Jorio Date: Sat, 20 Feb 2021 17:22:22 +0100 Subject: [PATCH] QD3D: New math features --- src/QD3D/QD3D.h | 9 +++ src/QD3D/QD3DMath.cpp | 76 +++++++++++++++++++ src/QD3D/QD3DMath.h | 171 +++++++++++++++++++++++++++++++++++++----- 3 files changed, 237 insertions(+), 19 deletions(-) diff --git a/src/QD3D/QD3D.h b/src/QD3D/QD3D.h index 2595d6b..1f3f1d6 100644 --- a/src/QD3D/QD3D.h +++ b/src/QD3D/QD3D.h @@ -209,6 +209,7 @@ typedef struct TQ3ColorRGB float b; } TQ3ColorRGB; +// WARNING: this structure differs from QD3D typedef struct TQ3Vertex3D { TQ3Point3D point; @@ -221,6 +222,14 @@ typedef struct TQ3BoundingBox TQ3Boolean isEmpty; } TQ3BoundingBox; +typedef struct TQ3BoundingSphere +{ + TQ3Point3D origin; + float radius; + TQ3Boolean isEmpty; +} TQ3BoundingSphere; + + typedef struct TQ3Matrix3x3 { float value[3][3]; diff --git a/src/QD3D/QD3DMath.cpp b/src/QD3D/QD3DMath.cpp index 4512dbb..359e14b 100644 --- a/src/QD3D/QD3DMath.cpp +++ b/src/QD3D/QD3DMath.cpp @@ -382,3 +382,79 @@ TQ3Matrix4x4* Q3Matrix4x4_Invert( return(result); } + +//----------------------------------------------------------------------------- +#pragma mark Q3BoundingBox + +TQ3BoundingBox* Q3BoundingBox_SetFromPoints3D( + TQ3BoundingBox *bBox, + const TQ3Point3D *points3D, + TQ3Uns32 numPoints, + TQ3Uns32 structSize) +{ + if (structSize != sizeof(TQ3Point3D)) + throw std::runtime_error("Q3BoundingBox_SetFromPoints3D: unsupported struct size"); + + if (numPoints == 0) + { + bBox->isEmpty = kQ3True; + } + else + { + TQ3Uns32 i = 0; + + bBox->isEmpty = kQ3False; + bBox->min = points3D[0]; + bBox->max = points3D[0]; + + // We have already accounted for the first point, so if the number of + // points is odd, we can handle the other points in pairs, and need not + // look at the first point again. But if the number of points is even, + // we will start at the first point just so we can work in pairs. + if ( (numPoints % 2) == 1 ) + { + i = 1; + } + + for (; i < numPoints; i += 2) + { + TQ3Point3D pt0 = points3D[i]; + TQ3Point3D pt1 = points3D[i+1]; + + if (pt0.x < pt1.x) + { + if (pt0.x < bBox->min.x) bBox->min.x = pt0.x; + if (pt1.x > bBox->max.x) bBox->max.x = pt1.x; + } + else // pt1.x <= pt0.x + { + if (pt1.x < bBox->min.x) bBox->min.x = pt1.x; + if (pt0.x > bBox->max.x) bBox->max.x = pt0.x; + } + + if (pt0.y < pt1.y) + { + if (pt0.y < bBox->min.y) bBox->min.y = pt0.y; + if (pt1.y > bBox->max.y) bBox->max.y = pt1.y; + } + else // pt1.y <= pt0.y + { + if (pt1.y < bBox->min.y) bBox->min.y = pt1.y; + if (pt0.y > bBox->max.y) bBox->max.y = pt0.y; + } + + if (pt0.z < pt1.z) + { + if (pt0.z < bBox->min.z) bBox->min.z = pt0.z; + if (pt1.z > bBox->max.z) bBox->max.z = pt1.z; + } + else // pt1.z <= pt0.z + { + if (pt1.z < bBox->min.z) bBox->min.z = pt1.z; + if (pt0.z > bBox->max.z) bBox->max.z = pt0.z; + } + } + } + + return(bBox); +} diff --git a/src/QD3D/QD3DMath.h b/src/QD3D/QD3DMath.h index 578a309..9cfc59d 100644 --- a/src/QD3D/QD3DMath.h +++ b/src/QD3D/QD3DMath.h @@ -40,6 +40,7 @@ extern "C" { #include "QD3D.h" #include +#include #include #ifdef FLT_EPSILON @@ -137,25 +138,6 @@ static inline void Q3Point3D_CrossProductTri( result->z = (v1_x * v2_y) - (v1_y * v2_x); } -static inline TQ3Vector3D* Q3Vector3D_Transform( - const TQ3Vector3D *vector3D, - const TQ3Matrix4x4 *matrix4x4, - TQ3Vector3D *result) -{ - // Save input to avoid problems when result is same as input - float x = vector3D->x; - float y = vector3D->y; - float z = vector3D->z; - -#define M(x,y) matrix4x4->value[x][y] - result->x = x*M(0,0) + y*M(1,0) + z*M(2,0); - result->y = x*M(0,1) + y*M(1,1) + z*M(2,1); - result->z = x*M(0,2) + y*M(1,2) + z*M(2,2); -#undef M - - return result; -} - static inline TQ3Point3D* Q3Point3D_Transform( const TQ3Point3D *point3D, const TQ3Matrix4x4 *matrix4x4, @@ -210,6 +192,7 @@ static inline TQ3Point3D* Q3Point3D_TransformAffine( return(result); } +// This function's signature differs from vanilla QD3D's. void Q3Point3D_To3DTransformArray( const TQ3Point3D *inPoints3D, const TQ3Matrix4x4 *matrix4x4, @@ -272,6 +255,41 @@ static inline void Q3Vector2D_Normalize( Q3Vector2D_Scale(v1, 1.0f / theLength, result); } +static inline float Q3Vector2D_Cross( + const TQ3Vector2D* v1, + const TQ3Vector2D* v2) +{ + return (v1->x * v2->y) - (v1->y * v2->x); +} + +//============================================================================= +// E3Vector2D_Transform : Transform 2D vector by 3x3 matrix. +//----------------------------------------------------------------------------- +// Note : 'result' may be the same as 'vector2D'. +// +// Note that the translation and perspective components of the +// matrix is ignored (as if it were really a 2x2 matrix). +// +// Contrast with E3Point2D_Transform, which does the full 3x3 +// transformation. +//----------------------------------------------------------------------------- +static inline TQ3Vector2D* Q3Vector2D_Transform( + const TQ3Vector2D *vector2D, + const TQ3Matrix3x3 *matrix3x3, + TQ3Vector2D *result) +{ + // Save input to avoid problems when result is same as input + float x = vector2D->x; + float y = vector2D->y; + +#define M(x,y) matrix3x3->value[x][y] + result->x = x*M(0,0) + y*M(1,0); + result->y = x*M(0,1) + y*M(1,1); +#undef M + + return(result); +} + //----------------------------------------------------------------------------- #pragma mark Q3Vector3D @@ -335,6 +353,25 @@ static inline void Q3Vector3D_Normalize( Q3Vector3D_Scale(v1, 1.0f / theLength, result); } +static inline TQ3Vector3D* Q3Vector3D_Transform( + const TQ3Vector3D *vector3D, + const TQ3Matrix4x4 *matrix4x4, + TQ3Vector3D *result) +{ + // Save input to avoid problems when result is same as input + float x = vector3D->x; + float y = vector3D->y; + float z = vector3D->z; + +#define M(x,y) matrix4x4->value[x][y] + result->x = x*M(0,0) + y*M(1,0) + z*M(2,0); + result->y = x*M(0,1) + y*M(1,1) + z*M(2,1); + result->z = x*M(0,2) + y*M(1,2) + z*M(2,2); +#undef M + + return result; +} + //----------------------------------------------------------------------------- #pragma mark Q3Matrix3x3 @@ -372,6 +409,37 @@ static inline TQ3Matrix3x3* Q3Matrix3x3_SetTranslate( return matrix3x3; } +static inline TQ3Matrix3x3* Q3Matrix3x3_SetRotateAboutPoint( + TQ3Matrix3x3 *matrix3x3, + const TQ3Point2D *origin, + float angle) +{ + float cosAngle = cosf(angle); + float sinAngle = sinf(angle); + +#define M(x,y) matrix3x3->value[x][y] +#define Dx origin->x +#define Dy origin->y + + M(0,0) = cosAngle; + M(0,1) = sinAngle; + M(0,2) = 0.0f; + + M(1,0) = -sinAngle; + M(1,1) = cosAngle; + M(1,2) = 0.0f; + + M(2,0) = Dx - Dx*cosAngle + Dy*sinAngle; // = Dx - Dx*M(0,0) - Dy*M(1,0) + M(2,1) = Dy - Dx*sinAngle - Dy*cosAngle; // = Dx - Dx*M(0,1) - Dy*M(1,1) + M(2,2) = 1.0f; + +#undef M +#undef Dx +#undef Dy + + return(matrix3x3); +} + //----------------------------------------------------------------------------- #pragma mark Q3Matrix4x4 @@ -602,6 +670,62 @@ static inline TQ3Matrix4x4* Q3Matrix4x4_SetRotate_XYZ( return(matrix4x4); } +//============================================================================= +// E3Matrix4x4_SetRotateAboutPoint : Set 4x4 matrix to rotate about axes through +// point and parallel to X, Y, Z axes +// (in that order -- which is rarely useful). +//----------------------------------------------------------------------------- +static inline TQ3Matrix4x4* Q3Matrix4x4_SetRotateAboutPoint( + TQ3Matrix4x4 *matrix4x4, + const TQ3Point3D *origin, + float xAngle, + float yAngle, + float zAngle) +{ + float cosX = cosf(xAngle); + float sinX = sinf(xAngle); + float cosY = cosf(yAngle); + float sinY = sinf(yAngle); + float cosZ = cosf(zAngle); + float sinZ = sinf(zAngle); + + float sinXsinY = sinX*sinY; + float cosXsinY = cosX*sinY; + +#define M(x,y) matrix4x4->value[x][y] +#define Dx origin->x +#define Dy origin->y +#define Dz origin->z + + M(0,0) = cosY*cosZ; + M(0,1) = cosY*sinZ; + M(0,2) = -sinY; + M(0,3) = 0.0f; + + M(1,0) = sinXsinY*cosZ - cosX*sinZ; + M(1,1) = sinXsinY*sinZ + cosX*cosZ; + M(1,2) = sinX*cosY; + M(1,3) = 0.0f; + + M(2,0) = cosXsinY*cosZ + sinX*sinZ; + M(2,1) = cosXsinY*sinZ - sinX*cosZ; + M(2,2) = cosX*cosY; + M(2,3) = 0.0f; + + M(3,0) = Dx - Dx*M(0,0) - Dy*M(1,0) - Dz*M(2,0); + M(3,1) = Dy - Dx*M(0,1) - Dy*M(1,1) - Dz*M(2,1); + M(3,2) = Dz - Dx*M(0,2) - Dy*M(1,2) - Dz*M(2,2); + M(3,3) = 1.0f; + +#undef M +#undef Dx +#undef Dy +#undef Dz + + return(matrix4x4); +} + + TQ3Matrix4x4* Q3Matrix4x4_Transpose( const TQ3Matrix4x4 *matrix4x4, TQ3Matrix4x4 *result); @@ -610,6 +734,15 @@ TQ3Matrix4x4* Q3Matrix4x4_Invert( const TQ3Matrix4x4 *inMatrix, TQ3Matrix4x4 *result); +//----------------------------------------------------------------------------- +#pragma mark Q3BoundingBox + +TQ3BoundingBox* Q3BoundingBox_SetFromPoints3D( + TQ3BoundingBox *bBox, + const TQ3Point3D *points3D, + TQ3Uns32 numPoints, + TQ3Uns32 structSize); + #ifdef __cplusplus } #endif