lili/Hoops_Exchange
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
2b56cf87a863f9317ca8a3522aeb57ad15fa6741
Hoops_Exchange/exchange/exchangesource/Collision/visitor/Matrix.cpp
ninja 2b56cf87a8 Init
2025-12-15 22:06:49 +08:00

689 lines
20 KiB
C++
Raw Blame History

/***********************************************************************************************************************
*
* Copyright (c) 2010 - 2022 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.
*
***********************************************************************************************************************/
#include "Matrix.h"
#include <math.h>
#include <float.h>
#include "A3DVector.h"
////////////////////////////////////////////////////////////////////////
// A3DMatrix4x4
///////////////////////////////////////////////////////////////////////
A3DStatus A3DMatrix4x4::CreateIdentity(A3DMatrix4x4*& psIdentity)
{
// by default A3DMatrix4x4() call a ResetToIdentity
psIdentity = new A3DMatrix4x4();
return A3D_SUCCESS;
}
A3DMatrix4x4 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 A3DMatrix4x4 :: 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 A3DMatrix4x4::Invert(A3DMatrix4x4& sResult)
{
return Invert4x4(m_adM, sResult.m_adM);
}
A3DStatus A3DMatrix4x4::GetTranslation(A3DVector3d& sTranslation)
{
sTranslation.x = m_adM[12];
sTranslation.y = m_adM[13];
sTranslation.z = m_adM[14];
return A3D_SUCCESS;
}
void A3DMatrix4x4::ResetToIdentity()
{
memset(m_adM, 0, sizeof(double) * 16);
m_adM[0] = m_adM[5] = m_adM[10] = m_adM[15] = 1;
}
bool A3DMatrix4x4::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 A3DMatrix4x4::GetTranslation( double& rdTX, double& rdTY, double& rdTZ ) const
{
rdTX = m_adM[12];
rdTY = m_adM[13];
rdTZ = m_adM[14];
}
void A3DMatrix4x4::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 A3DMatrix4x4::GetXYZRadianAngles( double& rdXAngle, double& rdYAngle, double& rdZAngle ) const
{
//matrix3x3 of rotation without scale
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;
// rdYAngle = asin(rot[2]); //attitude :
// if(rdYAngle < A3D_HALF_PI)
// if(rdYAngle > - A3D_HALF_PI)
// {
// rdXAngle = atan2(-rot[5], rot[8]);//bank : rotation along the "look at axis" (1 0 0)
// rdZAngle = atan2(-rot[1], rot[0]);//heading : rotation along up axis (0 0 1)
// }
// else
// {
// rdZAngle = 0.0; // multiple solutions
// rdXAngle = -atan2(rot[4], rot[5]);
// }
// else
// {
// rdZAngle = 0.0; // multiple solutions
// rdXAngle = atan2(rot[4], rot[5]);
// }
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]);
}
//
// double cX = cos(rdXAngle);
// double sX = sin(rdXAngle);
// double cY = cos(rdYAngle);
// double sY = sin(rdYAngle);
// double cZ = cos(rdZAngle);
// double sZ = sin(rdZAngle);
//
// A3DMatrix4x4 testResult;
// testResult.m_adM[0] = cZ*cY; testResult.m_adM[4] = -cZ*sY*cX + sZ*sX; testResult.m_adM[8] = cZ*sY*sX + sZ*cX;
// testResult.m_adM[1] = sY; testResult.m_adM[5] = cY*cX; testResult.m_adM[9] = -cY*sX;
// testResult.m_adM[2] = -sZ*cY; testResult.m_adM[6] = sZ*sY*cX + cZ*sX; testResult.m_adM[10] = -sZ*sY*sX + cZ*cX;
// bool bRes = testResult.Compare(this);
}
void A3DMatrix4x4::setRotations(double const& dXAngle, double const& dYAngle, double const& dZAngle)
{
// Real fCos, fSin;
// fCos = Math::Cos(fYAngle);
// fSin = Math::Sin(fYAngle);
// Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
//
// fCos = Math::Cos(fPAngle);
// fSin = Math::Sin(fPAngle);
// Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
//
// fCos = Math::Cos(fRAngle);
// fSin = Math::Sin(fRAngle);
// Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
//
// *this = kXMat*(kYMat*kZMat);
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);
}
bool A3DMatrix4x4::Compare( A3DMatrix4x4 const& rOther, double dTol ) 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 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];
}
}
}
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;
}
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;
}
/*******************************************************
Multiply Matrix
********************************************************/
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];
}
}
/*******************************************************
Invert Matrix
********************************************************/
A3DStatus Calculate(double* pdX, double* pdY, double* pdZ, double* pdMat)
{
double dLenght = sqrt(pdX[0] * pdX[0] + pdX[1] * pdX[1] + pdX[2] * pdX[2]);
if(dLenght < 1e-12)
return A3D_ERROR;
pdX[0] /= dLenght; pdX[1] /= dLenght; pdX[2] /= dLenght;
dLenght = sqrt(pdY[0] * pdY[0] + pdY[1] * pdY[1] + pdY[2] * pdY[2]);
if(dLenght < 1e-12)
return A3D_ERROR;
pdY[0] /= dLenght; pdY[1] /= dLenght; pdY[2] /= dLenght;
dLenght = sqrt(pdZ[0] * pdZ[0] + pdZ[1] * pdZ[1] + pdZ[2] * pdZ[2]);
if(dLenght < 1e-12)
return A3D_ERROR;
pdZ[0] /= dLenght; pdZ[1] /= dLenght; pdZ[2] /= dLenght;
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;
}
/*******************************************************
Invert Matrix
********************************************************/
double det2x2(double a, double b, double c, double d)
{
double det;
det = a * d - b * c;
return det;
}
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;
}
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);
}
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;
}
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;
}
A3DStatus Identity(double* pdIdentity)
{
memset(&pdIdentity[0], 0, 16 * sizeof(double));
pdIdentity[0] = pdIdentity[5] = pdIdentity[10] = pdIdentity[15] = 1;
return A3D_SUCCESS;
}
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;
}
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;
}
Reference in New Issue View Git Blame Copy Permalink