739 lines
23 KiB
C++
739 lines
23 KiB
C++
/***********************************************************************************************************************
|
|
*
|
|
* Copyright (c) 2010 - 2025 by Tech Soft 3D, Inc.
|
|
* The information contained herein is confidential and proprietary to Tech Soft 3D, Inc., and considered a trade secret
|
|
* as defined under civil and criminal statutes. Tech Soft 3D shall pursue its civil and criminal remedies in the event
|
|
* of unauthorized use or misappropriation of its trade secrets. Use of this information by anyone other than authorized
|
|
* employees of Tech Soft 3D, Inc. is granted only under a written non-disclosure agreement, expressly prescribing the
|
|
* scope and manner of such use.
|
|
*
|
|
***********************************************************************************************************************/
|
|
|
|
#ifndef A3DMATRIX_H
|
|
#define A3DMATRIX_H
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <A3DSDKErrorCodes.h>
|
|
#include <A3DVector.h>
|
|
#include <float.h>
|
|
|
|
#define A3D_PI 3.1415926535897932384626433832795028841971693993751
|
|
#define A3D_HALF_PI 1.5707963267948966192313216916397514420985846996875
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static void MultMatrixVecd(const double matrix[16],
|
|
const double in[4],
|
|
double out[4])
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
out[i] =
|
|
in[0] * matrix[0 * 4 + i] +
|
|
in[1] * matrix[1 * 4 + i] +
|
|
in[2] * matrix[2 * 4 + i] +
|
|
in[3] * matrix[3 * 4 + i];
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
/*
|
|
** Invert 4x4 matrix.
|
|
** Contributed by David Moore (See Mesa bug #6748)
|
|
*/
|
|
static bool InvertMatrixd(const double m[16], double invOut[16])
|
|
{
|
|
double inv[16], det;
|
|
int i;
|
|
|
|
inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15]
|
|
+ m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
|
|
inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15]
|
|
- m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
|
|
inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15]
|
|
+ m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
|
|
inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14]
|
|
- m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
|
|
inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15]
|
|
- m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
|
|
inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15]
|
|
+ m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
|
|
inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15]
|
|
- m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
|
|
inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14]
|
|
+ m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
|
|
inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15]
|
|
+ m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
|
|
inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15]
|
|
- m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
|
|
inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15]
|
|
+ m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
|
|
inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14]
|
|
- m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
|
|
inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11]
|
|
- m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
|
|
inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11]
|
|
+ m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
|
|
inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11]
|
|
- m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
|
|
inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10]
|
|
+ m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
|
|
|
|
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
|
|
if (det == 0)
|
|
return false;
|
|
|
|
det = 1.0 / det;
|
|
|
|
for (i = 0; i < 16; i++)
|
|
invOut[i] = inv[i] * det;
|
|
|
|
return true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static void MultMatricesd(const double a[16], const double b[16],
|
|
double r[16])
|
|
{
|
|
int i, j;
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
for (j = 0; j < 4; j++) {
|
|
r[i * 4 + j] =
|
|
a[i * 4 + 0] * b[0 * 4 + j] +
|
|
a[i * 4 + 1] * b[1 * 4 + j] +
|
|
a[i * 4 + 2] * b[2 * 4 + j] +
|
|
a[i * 4 + 3] * b[3 * 4 + j];
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus UnProject(double winx,
|
|
double winy,
|
|
double winz,
|
|
const double modelMatrix[16],
|
|
const double projMatrix[16],
|
|
const int viewport[4],
|
|
double *objx, double *objy, double *objz)
|
|
{
|
|
double finalMatrix[16];
|
|
double in[4];
|
|
double out[4];
|
|
|
|
MultMatricesd(modelMatrix, projMatrix, finalMatrix);
|
|
if (!InvertMatrixd(finalMatrix, finalMatrix))
|
|
return A3D_ERROR;
|
|
|
|
in[0] = winx;
|
|
in[1] = winy;
|
|
in[2] = winz;
|
|
in[3] = 1.0;
|
|
|
|
/* Map x and y from window coordinates */
|
|
in[0] = (in[0] - viewport[0]) / viewport[2];
|
|
in[1] = (in[1] - viewport[1]) / viewport[3];
|
|
|
|
/* Map to range -1 to 1 */
|
|
in[0] = in[0] * 2 - 1;
|
|
in[1] = in[1] * 2 - 1;
|
|
in[2] = in[2] * 2 - 1;
|
|
|
|
MultMatrixVecd(finalMatrix, in, out);
|
|
if (out[3] == 0.0)
|
|
return A3D_ERROR;
|
|
out[0] /= out[3];
|
|
out[1] /= out[3];
|
|
out[2] /= out[3];
|
|
*objx = out[0];
|
|
*objy = out[1];
|
|
*objz = out[2];
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus Project(double objx,
|
|
double objy,
|
|
double objz,
|
|
const double modelMatrix[16],
|
|
const double projMatrix[16],
|
|
const int viewport[4],
|
|
double *winx,
|
|
double *winy,
|
|
double *winz)
|
|
{
|
|
double in[4];
|
|
double out[4];
|
|
|
|
in[0] = objx;
|
|
in[1] = objy;
|
|
in[2] = objz;
|
|
in[3] = 1.0;
|
|
MultMatrixVecd(modelMatrix, in, out);
|
|
MultMatrixVecd(projMatrix, out, in);
|
|
if (in[3] == 0.0) return A3D_ERROR;
|
|
in[0] /= in[3];
|
|
in[1] /= in[3];
|
|
in[2] /= in[3];
|
|
/* Map x, y and z to range 0-1 */
|
|
in[0] = in[0] * 0.5 + 0.5;
|
|
in[1] = in[1] * 0.5 + 0.5;
|
|
in[2] = in[2] * 0.5 + 0.5;
|
|
|
|
/* Map x,y to viewport */
|
|
in[0] = in[0] * viewport[2] + viewport[0];
|
|
in[1] = in[1] * viewport[3] + viewport[1];
|
|
|
|
*winx = in[0];
|
|
*winy = in[1];
|
|
*winz = in[2];
|
|
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus MultiplyMatrix(const double* padFather, const double* pdThisMatrix, double* pdResult)
|
|
{
|
|
A3DStatus iRet = A3D_SUCCESS;
|
|
pdResult[0] = padFather[0] * pdThisMatrix[0] + padFather[4] * pdThisMatrix[1] + padFather[8] * pdThisMatrix[2] + padFather[12] * pdThisMatrix[3];
|
|
pdResult[1] = padFather[1] * pdThisMatrix[0] + padFather[5] * pdThisMatrix[1] + padFather[9] * pdThisMatrix[2] + padFather[13] * pdThisMatrix[3];
|
|
pdResult[2] = padFather[2] * pdThisMatrix[0] + padFather[6] * pdThisMatrix[1] + padFather[10] * pdThisMatrix[2] + padFather[14] * pdThisMatrix[3];
|
|
pdResult[3] = padFather[3] * pdThisMatrix[0] + padFather[7] * pdThisMatrix[1] + padFather[11] * pdThisMatrix[2] + padFather[15] * pdThisMatrix[3];
|
|
pdResult[4] = padFather[0] * pdThisMatrix[4] + padFather[4] * pdThisMatrix[5] + padFather[8] * pdThisMatrix[6] + padFather[12] * pdThisMatrix[7];
|
|
pdResult[5] = padFather[1] * pdThisMatrix[4] + padFather[5] * pdThisMatrix[5] + padFather[9] * pdThisMatrix[6] + padFather[13] * pdThisMatrix[7];
|
|
pdResult[6] = padFather[2] * pdThisMatrix[4] + padFather[6] * pdThisMatrix[5] + padFather[10] * pdThisMatrix[6] + padFather[14] * pdThisMatrix[7];
|
|
pdResult[7] = padFather[3] * pdThisMatrix[4] + padFather[7] * pdThisMatrix[5] + padFather[11] * pdThisMatrix[6] + padFather[15] * pdThisMatrix[7];
|
|
pdResult[8] = padFather[0] * pdThisMatrix[8] + padFather[4] * pdThisMatrix[9] + padFather[8] * pdThisMatrix[10] + padFather[12] * pdThisMatrix[11];
|
|
pdResult[9] = padFather[1] * pdThisMatrix[8] + padFather[5] * pdThisMatrix[9] + padFather[9] * pdThisMatrix[10] + padFather[13] * pdThisMatrix[11];
|
|
pdResult[10] = padFather[2] * pdThisMatrix[8] + padFather[6] * pdThisMatrix[9] + padFather[10] * pdThisMatrix[10] + padFather[14] * pdThisMatrix[11];
|
|
pdResult[11] = padFather[3] * pdThisMatrix[8] + padFather[7] * pdThisMatrix[9] + padFather[11] * pdThisMatrix[10] + padFather[15] * pdThisMatrix[11];
|
|
pdResult[12] = padFather[0] * pdThisMatrix[12] + padFather[4] * pdThisMatrix[13] + padFather[8] * pdThisMatrix[14] + padFather[12] * pdThisMatrix[15];
|
|
pdResult[13] = padFather[1] * pdThisMatrix[12] + padFather[5] * pdThisMatrix[13] + padFather[9] * pdThisMatrix[14] + padFather[13] * pdThisMatrix[15];
|
|
pdResult[14] = padFather[2] * pdThisMatrix[12] + padFather[6] * pdThisMatrix[13] + padFather[10] * pdThisMatrix[14] + padFather[14] * pdThisMatrix[15];
|
|
pdResult[15] = padFather[3] * pdThisMatrix[12] + padFather[7] * pdThisMatrix[13] + padFather[11] * pdThisMatrix[14] + padFather[15] * pdThisMatrix[15];
|
|
return iRet;
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static void MatrixVectMultiply(const double* pdMatrix,
|
|
const double* pdVect,
|
|
double* pdresult)
|
|
{
|
|
unsigned int uI;
|
|
|
|
for (uI = 0; uI < 4; uI++)
|
|
{
|
|
pdresult[uI] =
|
|
pdVect[0] * pdMatrix[uI] +
|
|
pdVect[1] * pdMatrix[4 + uI] +
|
|
pdVect[2] * pdMatrix[8 + uI] +
|
|
pdVect[3] * pdMatrix[12 + uI];
|
|
}
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus Calculate(double* pdX, double* pdY, double* pdZ, double* pdMat)
|
|
{
|
|
double dLength = sqrt(pdX[0] * pdX[0] + pdX[1] * pdX[1] + pdX[2] * pdX[2]);
|
|
if (dLength < 1e-12)
|
|
return A3D_ERROR;
|
|
pdX[0] /= dLength; pdX[1] /= dLength; pdX[2] /= dLength;
|
|
|
|
dLength = sqrt(pdY[0] * pdY[0] + pdY[1] * pdY[1] + pdY[2] * pdY[2]);
|
|
if (dLength < 1e-12)
|
|
return A3D_ERROR;
|
|
pdY[0] /= dLength; pdY[1] /= dLength; pdY[2] /= dLength;
|
|
|
|
dLength = sqrt(pdZ[0] * pdZ[0] + pdZ[1] * pdZ[1] + pdZ[2] * pdZ[2]);
|
|
if (dLength < 1e-12)
|
|
return A3D_ERROR;
|
|
pdZ[0] /= dLength; pdZ[1] /= dLength; pdZ[2] /= dLength;
|
|
|
|
pdMat[0] = pdX[0];
|
|
pdMat[1] = pdX[1];
|
|
pdMat[2] = pdX[2];
|
|
pdMat[4] = pdY[0];
|
|
pdMat[5] = pdY[1];
|
|
pdMat[6] = pdY[2];
|
|
pdMat[8] = pdZ[0];
|
|
pdMat[9] = pdZ[1];
|
|
pdMat[10] = pdZ[2];
|
|
|
|
pdMat[3] = pdMat[7] = pdMat[11] = pdMat[12] = pdMat[13] = pdMat[14] = pdMat[15] = 0;
|
|
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static double det2x2(double a, double b, double c, double d)
|
|
{
|
|
double det;
|
|
det = a * d - b * c;
|
|
return det;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static double det3x3(double a1, double a2, double a3,
|
|
double b1, double b2, double b3,
|
|
double c1, double c2, double c3)
|
|
{
|
|
double det;
|
|
det = a1 * det2x2(b2, b3, c2, c3)
|
|
- b1 * det2x2(a2, a3, c2, c3)
|
|
+ c1 * det2x2(a2, a3, b2, b3);
|
|
return det;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static void adjoint(double* pdMatrix)
|
|
{
|
|
double a1, a2, a3, a4, b1, b2, b3, b4;
|
|
double c1, c2, c3, c4, d1, d2, d3, d4;
|
|
|
|
|
|
a1 = pdMatrix[0]; b1 = pdMatrix[1];
|
|
c1 = pdMatrix[2]; d1 = pdMatrix[3];
|
|
a2 = pdMatrix[4]; b2 = pdMatrix[5];
|
|
c2 = pdMatrix[6]; d2 = pdMatrix[7];
|
|
a3 = pdMatrix[8]; b3 = pdMatrix[9];
|
|
c3 = pdMatrix[10]; d3 = pdMatrix[11];
|
|
a4 = pdMatrix[12]; b4 = pdMatrix[13];
|
|
c4 = pdMatrix[14]; d4 = pdMatrix[15];
|
|
|
|
pdMatrix[0] = det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
|
|
pdMatrix[4] = -det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
|
|
pdMatrix[8] = det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
|
|
pdMatrix[12] = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
|
|
|
|
pdMatrix[1] = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
|
|
pdMatrix[5] = det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
|
|
pdMatrix[9] = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
|
|
pdMatrix[13] = det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
|
|
|
|
pdMatrix[2] = det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
|
|
pdMatrix[6] = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
|
|
pdMatrix[10] = det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
|
|
pdMatrix[14] = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
|
|
|
|
pdMatrix[3] = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
|
|
pdMatrix[7] = det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
|
|
pdMatrix[11] = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
|
|
pdMatrix[15] = det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus Invert4x4(const double* pdMat, double* pdMatInv)
|
|
{
|
|
double det;
|
|
int i, j;
|
|
double scale = pdMat[15];
|
|
|
|
if (fabs(scale) < FLT_MIN)
|
|
{
|
|
/*"Matrice avec Scale == 0.: inversion impossible\n"*/
|
|
return A3D_ERROR;
|
|
}
|
|
|
|
for (i = 0; i < 4; i++)
|
|
for (j = 0; j < 4; j++)
|
|
pdMatInv[i * 4 + j] = pdMat[i * 4 + j];
|
|
|
|
adjoint(pdMatInv);
|
|
det = det3x3(pdMat[0], pdMat[4], pdMat[8],
|
|
pdMat[1], pdMat[5], pdMat[9],
|
|
pdMat[2], pdMat[6], pdMat[10]);
|
|
|
|
if (fabs(det) < FLT_MIN)
|
|
{
|
|
/*"Matrice non reguliere: inversion impossible\n"*/
|
|
return A3D_ERROR;
|
|
}
|
|
|
|
for (i = 0; i < 4; i++)
|
|
for (j = 0; j < 4; j++)
|
|
pdMatInv[i * 4 + j] /= (scale*det);
|
|
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus UnProject(const double * pPt,
|
|
const double* pdModelMatrix,
|
|
const double* pdProjMatrix,
|
|
const int* viewport,
|
|
double* pPtresult)
|
|
{
|
|
double finalMatrix[16];
|
|
double in[4];
|
|
double out[4];
|
|
|
|
MultiplyMatrix(pdModelMatrix, pdProjMatrix, finalMatrix);
|
|
if (!Invert4x4(finalMatrix, finalMatrix))
|
|
return A3D_ERROR;
|
|
|
|
in[0] = pPt[0];
|
|
in[1] = pPt[1];
|
|
in[2] = pPt[2];
|
|
in[3] = 1.0;
|
|
|
|
/* Map x and y from window coordinates */
|
|
in[0] = (in[0] - viewport[0]) / viewport[2];
|
|
in[1] = (in[1] - viewport[1]) / viewport[3];
|
|
|
|
/* Map to range -1 to 1 */
|
|
in[0] = in[0] * 2 - 1;
|
|
in[1] = in[1] * 2 - 1;
|
|
in[2] = in[2] * 2 - 1;
|
|
|
|
MatrixVectMultiply(finalMatrix, in, out);
|
|
if (out[3] == 0.0)
|
|
return A3D_ERROR;
|
|
|
|
out[0] /= out[3];
|
|
out[1] /= out[3];
|
|
out[2] /= out[3];
|
|
|
|
*(pPtresult) = out[0];
|
|
*(pPtresult + 1) = out[1];
|
|
*(pPtresult + 2) = out[2];
|
|
|
|
double objx, objy, objz;
|
|
UnProject(pPt[0], pPt[1], pPt[2],
|
|
pdModelMatrix,
|
|
pdProjMatrix,
|
|
viewport,
|
|
&objx, &objy, &objz);
|
|
|
|
if (fabs(pPtresult[0] - objx) > 1e-12
|
|
|| fabs(pPtresult[1] - objx) > 1e-12
|
|
|| fabs(pPtresult[2] - objx) > 1e-12)
|
|
return A3D_ERROR;
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus Identity(double* pdIdentity)
|
|
{
|
|
memset(&pdIdentity[0], 0, 16 * sizeof(double));
|
|
pdIdentity[0] = pdIdentity[5] = pdIdentity[10] = pdIdentity[15] = 1;
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DStatus Ortho(const double* pdCurrentMatrix,
|
|
double dLeft,
|
|
double dRight,
|
|
double dTop,
|
|
double dBottom,
|
|
double dNear,
|
|
double dFar,
|
|
double* pdOutput)
|
|
{
|
|
double adOrthoMatrix[16];
|
|
double tx = -(dRight + dLeft) / (dRight - dLeft);
|
|
double ty = -(dTop + dBottom) / (dTop - dBottom);
|
|
double tz = -(dFar + dNear) / (dFar - dNear);
|
|
|
|
adOrthoMatrix[0] = 2 / (dRight - dLeft);
|
|
adOrthoMatrix[1] = 0;
|
|
adOrthoMatrix[2] = 0;
|
|
adOrthoMatrix[3] = tx;
|
|
|
|
adOrthoMatrix[4] = 0;
|
|
adOrthoMatrix[5] = 2 / (dTop - dBottom);
|
|
adOrthoMatrix[6] = 0;
|
|
adOrthoMatrix[7] = ty;
|
|
|
|
adOrthoMatrix[8] = 0;
|
|
adOrthoMatrix[9] = 0;
|
|
adOrthoMatrix[10] = (-2) / (dFar - dNear);
|
|
adOrthoMatrix[11] = tz;
|
|
|
|
adOrthoMatrix[12] = 0;
|
|
adOrthoMatrix[13] = 0;
|
|
adOrthoMatrix[14] = 0;
|
|
adOrthoMatrix[15] = 1;
|
|
|
|
MatrixVectMultiply(pdCurrentMatrix, adOrthoMatrix, pdOutput);
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
static A3DVector3dData CrossProduct(const A3DVector3dData* X, const A3DVector3dData* Y)
|
|
{
|
|
A3DVector3dData Z;
|
|
Z.m_dX = X->m_dY*Y->m_dZ - X->m_dZ*Y->m_dY;
|
|
Z.m_dY = X->m_dZ*Y->m_dX - X->m_dX*Y->m_dZ;
|
|
Z.m_dZ = X->m_dX*Y->m_dY - X->m_dY*Y->m_dX;
|
|
return Z;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
class A3DMatrix4x4
|
|
{
|
|
public:
|
|
double m_adM[16];
|
|
//column first
|
|
//m_adM[0] = m00 m_adM[4] = m01 m_adM[8] = m02 m_adM[12] = m03
|
|
//m_adM[1] = m10 m_adM[5] = m11 m_adM[9] = m12 m_adM[13] = m13
|
|
//m_adM[2] = m20 m_adM[6] = m21 m_adM[10] = m22 m_adM[14] = m23
|
|
//m_adM[3] = m30 m_adM[7] = m31 m_adM[11] = m32 m_adM[15] = m33
|
|
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
A3DMatrix4x4(double* pdMat)
|
|
{
|
|
memcpy(&m_adM[0], pdMat, sizeof(double) * 16);
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
A3DMatrix4x4()
|
|
{
|
|
ResetToIdentity();
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
operator const double*() const
|
|
{
|
|
return m_adM;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
static A3DStatus CreateIdentity(A3DMatrix4x4*& psIdentity)
|
|
{
|
|
// by default A3DMatrix4x4() call a ResetToIdentity
|
|
psIdentity = new A3DMatrix4x4();
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
A3DMatrix4x4 operator * (const A3DMatrix4x4& matrix) const
|
|
{
|
|
A3DMatrix4x4 result;
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
for (int j = 0; j < 4; j++)
|
|
{
|
|
result.m_adM[i + 4 * j] = 0.;
|
|
for (int k = 0; k < 4; k++)
|
|
result.m_adM[i + 4 * j] += m_adM[i + 4 * k] * matrix.m_adM[k + 4 * j];
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
A3DVector3d operator * (const A3DVector3d& v) const
|
|
{
|
|
A3DVector3d Result;
|
|
Result.x = m_adM[0] * v.x + m_adM[4] * v.y + m_adM[8] * v.z + m_adM[12];
|
|
Result.y = m_adM[1] * v.x + m_adM[5] * v.y + m_adM[9] * v.z + m_adM[13];
|
|
Result.z = m_adM[2] * v.x + m_adM[6] * v.y + m_adM[10] * v.z + m_adM[14];
|
|
return Result;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
A3DStatus Invert(A3DMatrix4x4& sResult)
|
|
{
|
|
return Invert4x4(m_adM, sResult.m_adM);
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
A3DStatus GetTranslation(A3DVector3d& sTranslation)
|
|
{
|
|
sTranslation.x = m_adM[12];
|
|
sTranslation.y = m_adM[13];
|
|
sTranslation.z = m_adM[14];
|
|
return A3D_SUCCESS;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
void ResetToIdentity()
|
|
{
|
|
memset(m_adM, 0, sizeof(double) * 16);
|
|
m_adM[0] = m_adM[5] = m_adM[10] = m_adM[15] = 1;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
bool IsIdentity() const
|
|
{
|
|
if ((m_adM[0] != 1.0) || (m_adM[1] != 0.0) || (m_adM[2] != 0.0) || (m_adM[3] != 0.0)
|
|
|| (m_adM[4] != 0.0) || (m_adM[5] != 1.0) || (m_adM[6] != 0.0) || (m_adM[7] != 0.0)
|
|
|| (m_adM[8] != 0.0) || (m_adM[9] != 0.0) || (m_adM[10] != 1.0) || (m_adM[11] != 0.0)
|
|
|| (m_adM[12] != 0.0) || (m_adM[13] != 0.0) || (m_adM[14] != 0.0) || (m_adM[15] != 1.0))
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
void GetScales(double& rdSX, double& rdSY, double& rdSZ) const
|
|
{
|
|
rdSX = sqrt(m_adM[0] * m_adM[0] + m_adM[1] * m_adM[1] + m_adM[2] * m_adM[2]);
|
|
rdSY = sqrt(m_adM[4] * m_adM[4] + m_adM[5] * m_adM[5] + m_adM[6] * m_adM[6]);
|
|
rdSZ = sqrt(m_adM[8] * m_adM[8] + m_adM[9] * m_adM[9] + m_adM[10] * m_adM[10]);
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
void GetTranslation(double& rdTX, double& rdTY, double& rdTZ) const
|
|
{
|
|
rdTX = m_adM[12];
|
|
rdTY = m_adM[13];
|
|
rdTZ = m_adM[14];
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
void GetXYZRadianAngles(double& rdXAngle, double& rdYAngle, double& rdZAngle) const
|
|
{
|
|
double dSx, dSy, dSz;
|
|
GetScales(dSx, dSy, dSz);
|
|
double rot[9];
|
|
rot[0] = m_adM[0] / dSx; rot[3] = m_adM[4] / dSy; rot[6] = m_adM[8] / dSz;
|
|
rot[1] = m_adM[1] / dSx; rot[4] = m_adM[5] / dSy; rot[7] = m_adM[9] / dSz;
|
|
rot[2] = m_adM[2] / dSx; rot[5] = m_adM[6] / dSy; rot[8] = m_adM[10] / dSz;
|
|
|
|
if (rot[1] > 0.998)
|
|
{
|
|
rdZAngle = atan2(rot[6], rot[8]);
|
|
rdYAngle = A3D_HALF_PI;
|
|
rdXAngle = 0.0;
|
|
}
|
|
else if (rot[1] < -.998)
|
|
{
|
|
rdZAngle = atan2(rot[6], rot[8]);
|
|
rdYAngle = -A3D_HALF_PI;
|
|
rdXAngle = 0.0;
|
|
}
|
|
else
|
|
{
|
|
rdZAngle = atan2(-rot[2], rot[0]);
|
|
rdYAngle = asin(rot[1]);
|
|
if (rot[7] == 0.0 && rot[4] == 0)
|
|
{
|
|
int sign = (rot[7] > 0) - (rot[7] < 0);
|
|
if (sign >= 0)
|
|
rdXAngle = A3D_HALF_PI;
|
|
else
|
|
rdXAngle = -A3D_HALF_PI;
|
|
}
|
|
else
|
|
rdXAngle = atan2(-rot[7], rot[4]);
|
|
}
|
|
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
void setRotations(double const& dXAngle, double const& dYAngle, double const& dZAngle)
|
|
{
|
|
double dCos, dSin;
|
|
dCos = cos(dXAngle);
|
|
dSin = sin(dXAngle);
|
|
|
|
A3DMatrix4x4 AX;
|
|
AX.m_adM[5] = dCos;
|
|
AX.m_adM[6] = -dSin;
|
|
AX.m_adM[9] = dSin;
|
|
AX.m_adM[10] = dCos;
|
|
|
|
dCos = cos(dYAngle);
|
|
dSin = sin(dYAngle);
|
|
|
|
A3DMatrix4x4 AY;
|
|
AY.m_adM[0] = dCos;
|
|
AY.m_adM[2] = dSin;
|
|
AY.m_adM[8] = -dSin;
|
|
AY.m_adM[10] = dCos;
|
|
|
|
dCos = cos(dZAngle);
|
|
dSin = sin(dZAngle);
|
|
|
|
A3DMatrix4x4 AZ;
|
|
AZ.m_adM[0] = dCos;
|
|
AZ.m_adM[1] = -dSin;
|
|
AZ.m_adM[4] = dSin;
|
|
AZ.m_adM[5] = dCos;
|
|
|
|
|
|
*this = AX * (AY * AZ);
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
A3DMatrix4x4& operator = (const A3DMatrix4x4& rFrom)
|
|
{
|
|
memcpy(m_adM, rFrom.m_adM, sizeof(double) * 16);
|
|
return *this;
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
//////////////////////////////////////
|
|
bool Compare(A3DMatrix4x4 const& rOther, double dTol = 0.0) const
|
|
{
|
|
if (dTol != 0.0)
|
|
{
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
if (m_adM[i] != rOther.m_adM[i])
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
if (fabs(m_adM[i] - rOther.m_adM[i]) < dTol)
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
};
|
|
|
|
////////////////////////////////////////
|
|
////////////////////////////////////////
|
|
static const A3DMatrix4x4 stIdentity;
|
|
|
|
#endif
|