mirror of
https://github.com/jorio/Pomme.git
synced 2024-09-28 18:55:16 +00:00
461 lines
13 KiB
C++
461 lines
13 KiB
C++
/*
|
|
|
|
Adapted from Quesa's math routines.
|
|
Original copyright notice below:
|
|
|
|
Copyright (c) 1999-2020, Quesa Developers. All rights reserved.
|
|
|
|
For the current release of Quesa, please see:
|
|
<https://github.com/jwwalker/Quesa>
|
|
|
|
Redistribution and use in source and binary forms, with or without modification, are permitted
|
|
provided that the following conditions are met:
|
|
|
|
o Redistributions of source code must retain the above copyright notice, this list of conditions
|
|
and the following disclaimer.
|
|
|
|
o Redistributions in binary form must reproduce the above copyright notice, this list of conditions
|
|
and the following disclaimer in the documentation and/or other materials provided with the
|
|
distribution.
|
|
|
|
o Neither the name of Quesa nor the names of its contributors may be used to endorse or promote
|
|
products derived from this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
|
|
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
|
|
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
|
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
|
|
IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
|
|
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
*/
|
|
|
|
#include "QD3DMath.h"
|
|
#include <stdexcept>
|
|
|
|
void Q3Point3D_To3DTransformArray(
|
|
const TQ3Point3D *inPoints3D,
|
|
const TQ3Matrix4x4 *matrix4x4,
|
|
TQ3Point3D *outPoints3D,
|
|
TQ3Uns32 numPoints)
|
|
{
|
|
TQ3Uns32 i;
|
|
|
|
// In the common case of the last column of the matrix being (0, 0, 0, 1),
|
|
// we can avoid some divisions and conditionals inside the loop.
|
|
if ( (matrix4x4->value[3][3] == 1.0f) &&
|
|
(matrix4x4->value[0][3] == 0.0f) &&
|
|
(matrix4x4->value[1][3] == 0.0f) &&
|
|
(matrix4x4->value[2][3] == 0.0f) )
|
|
{
|
|
for (i = 0; i < numPoints; ++i)
|
|
{
|
|
Q3Point3D_TransformAffine( inPoints3D, matrix4x4, outPoints3D );
|
|
inPoints3D++;
|
|
outPoints3D++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Transform the points - will be in-lined in release builds
|
|
for (i = 0; i < numPoints; ++i)
|
|
{
|
|
Q3Point3D_Transform(inPoints3D, matrix4x4, outPoints3D);
|
|
inPoints3D++;
|
|
outPoints3D++;
|
|
}
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
#pragma mark Q3Matrix4x4
|
|
|
|
TQ3Matrix4x4* Q3Matrix4x4_Transpose(
|
|
const TQ3Matrix4x4 *matrix4x4,
|
|
TQ3Matrix4x4 *result)
|
|
{
|
|
int i, j;
|
|
|
|
if (result != matrix4x4)
|
|
{
|
|
for (i = 0; i < 4; ++i)
|
|
for (j = 0; j < 4; ++j)
|
|
result->value[i][j] = matrix4x4->value[j][i];
|
|
}
|
|
else
|
|
{
|
|
__Q3Float_Swap(result->value[1][0], result->value[0][1]);
|
|
__Q3Float_Swap(result->value[2][0], result->value[0][2]);
|
|
__Q3Float_Swap(result->value[3][0], result->value[0][3]);
|
|
__Q3Float_Swap(result->value[2][1], result->value[1][2]);
|
|
__Q3Float_Swap(result->value[3][1], result->value[1][3]);
|
|
__Q3Float_Swap(result->value[2][3], result->value[3][2]);
|
|
}
|
|
return(result);
|
|
}
|
|
|
|
|
|
//=============================================================================
|
|
// e3matrix4x4_extract3x3 : Select the upper left 3x3 of a 4x4 matrix
|
|
//-----------------------------------------------------------------------------
|
|
static void e3matrix4x4_extract3x3( const TQ3Matrix4x4& in4x4, TQ3Matrix3x3& out3x3 )
|
|
{
|
|
for (int row = 0; row < 3; ++row)
|
|
{
|
|
for (int col = 0; col < 3; ++col)
|
|
{
|
|
out3x3.value[row][col] = in4x4.value[row][col];
|
|
}
|
|
}
|
|
}
|
|
|
|
//=============================================================================
|
|
// e3matrix3x3_invert : Transforms the given 3x3 matrix into its inverse.
|
|
//-----------------------------------------------------------------------------
|
|
// Note : This function uses Gauss-Jordon elimination with full pivoting
|
|
// to transform the given matrix to the identity matrix while
|
|
// transforming the identity matrix to the inverse. As the given
|
|
// matrix is reduced to 1's and 0's column-by-column, the inverse
|
|
// matrix is created in its place column-by-column.
|
|
//
|
|
// See Press, et al., "Numerical Recipes in C", 2nd ed., pp. 32 ff.
|
|
//-----------------------------------------------------------------------------
|
|
static void e3matrix3x3_invert(TQ3Matrix3x3* a)
|
|
{
|
|
#define A(x,y) a->value[x][y]
|
|
|
|
TQ3Int32 irow = 0, icol = 0;
|
|
TQ3Int32 i, j, k; // *** WARNING: 'k' must be a SIGNED integer ***
|
|
float big, element;
|
|
TQ3Int32 ipiv[3], indxr[3], indxc[3];
|
|
|
|
// Initialize ipiv: ipiv[j] is 0 (1) if row/column j has not (has) been pivoted
|
|
for (j = 0; j < 3; ++j)
|
|
ipiv[j] = 0;
|
|
|
|
// Loop over 3 pivots
|
|
for (k = 0; k < 3; ++k)
|
|
{
|
|
// Search unpivoted part of matrix for largest element to pivot on
|
|
big = -1.0f;
|
|
for (i = 0; i < 3; ++i)
|
|
{
|
|
if (ipiv[i])
|
|
continue;
|
|
|
|
for (j = 0; j < 3; ++j)
|
|
{
|
|
if (ipiv[j])
|
|
continue;
|
|
|
|
// Calculate absolute value of current element
|
|
element = A(i,j);
|
|
if (element < 0.0f)
|
|
element = -element;
|
|
|
|
// Compare current element to largest element so far
|
|
if (element > big)
|
|
{
|
|
big = element;
|
|
irow = i;
|
|
icol = j;
|
|
}
|
|
}
|
|
}
|
|
|
|
// If largest element is 0, the matrix is singular
|
|
// (If there are "nan" values in the matrix, "big" may still be -1.0.)
|
|
if (big <= 0.0f)
|
|
{
|
|
throw std::runtime_error("e3matrix3x3_invert: non-invertible matrix");
|
|
return;
|
|
}
|
|
|
|
// Mark pivot row and column
|
|
++ipiv[icol];
|
|
indxr[k] = irow;
|
|
indxc[k] = icol;
|
|
|
|
// If necessary, exchange rows to put pivot element on diagonal
|
|
if (irow != icol)
|
|
{
|
|
for (j = 0; j < 3; ++j)
|
|
__Q3Float_Swap(A(irow,j), A(icol,j));
|
|
}
|
|
|
|
// Divide pivot row by pivot element
|
|
//
|
|
// Note: If we were dividing by the same element many times, it would
|
|
// make sense to multiply by its inverse. Since we divide by a given
|
|
// elemen only 3 (4) times for a 3x3 (4x4) matrix, it doesn't make sense
|
|
// to pay for the extra floating-point operation.
|
|
element = A(icol,icol);
|
|
A(icol,icol) = 1.0f; // overwrite original matrix with inverse
|
|
for (j = 0; j < 3; ++j)
|
|
A(icol,j) /= element;
|
|
|
|
// Reduce other rows
|
|
for (i = 0; i < 3; ++i)
|
|
{
|
|
if (i == icol)
|
|
continue;
|
|
|
|
element = A(i,icol);
|
|
A(i,icol) = 0.0f; // overwrite original matrix with inverse
|
|
for (j = 0; j < 3; ++j)
|
|
A(i,j) -= A(icol,j)*element;
|
|
}
|
|
}
|
|
|
|
// Permute columns
|
|
for (k = 3; --k >= 0; ) // *** WARNING: 'k' must be a SIGNED integer ***
|
|
{
|
|
if (indxr[k] != indxc[k])
|
|
{
|
|
for (i = 0; i < 3; ++i)
|
|
__Q3Float_Swap(A(i,indxr[k]), A(i,indxc[k]));
|
|
}
|
|
}
|
|
|
|
#undef A
|
|
}
|
|
|
|
//=============================================================================
|
|
// e3matrix4x4_invert : Transforms the given 4x4 matrix into its inverse.
|
|
//-----------------------------------------------------------------------------
|
|
// Note : This function uses Gauss-Jordon elimination with full pivoting
|
|
// to transform the given matrix to the identity matrix while
|
|
// transforming the identity matrix to the inverse. As the given
|
|
// matrix is reduced to 1's and 0's column-by-column, the inverse
|
|
// matrix is created in its place column-by-column.
|
|
//
|
|
// See Press, et al., "Numerical Recipes in C", 2nd ed., pp. 32 ff.
|
|
//-----------------------------------------------------------------------------
|
|
static void e3matrix4x4_invert(TQ3Matrix4x4* a)
|
|
{
|
|
#define A(x,y) a->value[x][y]
|
|
|
|
TQ3Int32 irow = 0, icol = 0;
|
|
TQ3Int32 i, j, k; // *** WARNING: 'k' must be a SIGNED integer ***
|
|
float big, element;
|
|
TQ3Int32 ipiv[4], indxr[4], indxc[4];
|
|
|
|
// Initialize ipiv: ipiv[j] is 0 (1) if row/column j has not (has) been pivoted
|
|
for (j = 0; j < 4; ++j)
|
|
ipiv[j] = 0;
|
|
|
|
// Loop over 4 pivots
|
|
for (k = 0; k < 4; ++k)
|
|
{
|
|
// Search unpivoted part of matrix for largest element to pivot on
|
|
big = -1.0f;
|
|
for (i = 0; i < 4; ++i)
|
|
{
|
|
if (ipiv[i])
|
|
continue;
|
|
|
|
for (j = 0; j < 4; ++j)
|
|
{
|
|
if (ipiv[j])
|
|
continue;
|
|
|
|
// Calculate absolute value of current element
|
|
element = A(i,j);
|
|
if (element < 0.0f)
|
|
element = -element;
|
|
|
|
// Compare current element to largest element so far
|
|
if (element > big)
|
|
{
|
|
big = element;
|
|
irow = i;
|
|
icol = j;
|
|
}
|
|
}
|
|
}
|
|
|
|
// If largest element is 0, the matrix is singular
|
|
// (If there are "nan" values in the matrix, "big" may still be -1.0.)
|
|
if (big <= 0.0f)
|
|
{
|
|
throw std::runtime_error("e3matrix4x4_invert: non-invertible matrix");
|
|
return;
|
|
}
|
|
|
|
// Mark pivot row and column
|
|
++ipiv[icol];
|
|
indxr[k] = irow;
|
|
indxc[k] = icol;
|
|
|
|
// If necessary, exchange rows to put pivot element on diagonal
|
|
if (irow != icol)
|
|
{
|
|
for (j = 0; j < 4; ++j)
|
|
__Q3Float_Swap(A(irow,j), A(icol,j));
|
|
}
|
|
|
|
// Divide pivot row by pivot element
|
|
//
|
|
// Note: If we were dividing by the same element many times, it would
|
|
// make sense to multiply by its inverse. Since we divide by a given
|
|
// element only 3 (4) times for a 3x3 (4x4) matrix, it doesn't make sense
|
|
// to pay for the extra floating-point operation.
|
|
element = A(icol,icol);
|
|
A(icol,icol) = 1.0f; // overwrite original matrix with inverse
|
|
for (j = 0; j < 4; ++j)
|
|
A(icol,j) /= element;
|
|
|
|
// Reduce other rows
|
|
for (i = 0; i < 4; ++i)
|
|
{
|
|
if (i == icol)
|
|
continue;
|
|
|
|
element = A(i,icol);
|
|
A(i,icol) = 0.0f; // overwrite original matrix with inverse
|
|
for (j = 0; j < 4; ++j)
|
|
A(i,j) -= A(icol,j)*element;
|
|
}
|
|
}
|
|
|
|
// Permute columns
|
|
for (k = 4; --k >= 0; ) // *** WARNING: 'k' must be a SIGNED integer ***
|
|
{
|
|
if (indxr[k] != indxc[k])
|
|
{
|
|
for (i = 0; i < 4; ++i)
|
|
__Q3Float_Swap(A(i,indxr[k]), A(i,indxc[k]));
|
|
}
|
|
}
|
|
|
|
#undef A
|
|
}
|
|
|
|
TQ3Matrix4x4* Q3Matrix4x4_Invert(
|
|
const TQ3Matrix4x4 *matrix4x4,
|
|
TQ3Matrix4x4 *result)
|
|
{
|
|
if (result != matrix4x4)
|
|
*result = *matrix4x4;
|
|
|
|
// The 4x4 matrices used in 3D graphics often have a last column of
|
|
// (0, 0, 0, 1). In that case, we want the inverse to have exactly the same
|
|
// last column, and we can compute the inverse with fewer floating point
|
|
// multiplies and divides. The inverse of the matrix
|
|
// A 0
|
|
// v 1
|
|
// (where A is 3x3 and v is 1x3) is
|
|
// inv(A) 0
|
|
// -v * inv(A) 1 .
|
|
if ( (result->value[3][3] == 1.0f) && (result->value[0][3] == 0.0f) &&
|
|
(result->value[1][3] == 0.0f) && (result->value[2][3] == 0.0f) )
|
|
{
|
|
TQ3Matrix3x3 upperLeft;
|
|
e3matrix4x4_extract3x3( *result, upperLeft );
|
|
int i, j;
|
|
|
|
e3matrix3x3_invert( &upperLeft );
|
|
|
|
for (i = 0; i < 3; ++i)
|
|
{
|
|
for (j = 0; j < 3; ++j)
|
|
{
|
|
result->value[i][j] = upperLeft.value[i][j];
|
|
}
|
|
}
|
|
|
|
TQ3RationalPoint3D v = {
|
|
result->value[3][0], result->value[3][1], result->value[3][2]
|
|
};
|
|
Q3RationalPoint3D_Transform( &v, &upperLeft, &v );
|
|
|
|
result->value[3][0] = -v.x;
|
|
result->value[3][1] = -v.y;
|
|
result->value[3][2] = -v.w;
|
|
}
|
|
else
|
|
{
|
|
e3matrix4x4_invert(result);
|
|
}
|
|
|
|
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);
|
|
}
|