449 lines
9.2 KiB
C++
449 lines
9.2 KiB
C++
//-------------------------------------------------------------------------------------------------------+
|
|
// Copyright (C), 1998-2007, Beijing Tangent Software Co. Ltd.
|
|
// = FileName : TGMatrix 类
|
|
// = Version : ver2.0
|
|
// = Author : wlw
|
|
// = CreateDate : 2002-09-09
|
|
// = Description: TGMatrix 定义
|
|
// = Maintainers:
|
|
//
|
|
//-------------------------------------------------------------------------------------------------------+
|
|
#include "StdAfx.h"
|
|
#include "TGMatrix.h"
|
|
|
|
TGMatrix::TGMatrix()
|
|
{
|
|
InitIdentity();
|
|
}
|
|
|
|
TGMatrix::~TGMatrix()
|
|
{
|
|
|
|
}
|
|
|
|
double* TGMatrix::operator [](int i) const
|
|
{
|
|
return (double*)(matrix[i]);
|
|
}
|
|
|
|
bool TGMatrix::CanReverse() const
|
|
{
|
|
double temp = Rank();
|
|
return !EQUAL_ZERO(temp);
|
|
}
|
|
|
|
void TGMatrix::GetCoordSys(TPGeCoordSys&sys) const
|
|
{
|
|
sys.Reset();
|
|
sys.TransformBy(this);
|
|
}
|
|
|
|
void TGMatrix::Init()
|
|
{
|
|
memset((void*)matrix,0,sizeof(double)*16);
|
|
matrix[3][3] = 1.0;
|
|
}
|
|
|
|
void TGMatrix::InitIdentity()
|
|
{
|
|
Init();matrix[0][0] = 1.0;
|
|
matrix[1][1] = 1.0;
|
|
matrix[2][2] = 1.0;
|
|
}
|
|
|
|
TGMatrix::TGMatrix(const TGMatrix&m2)
|
|
{
|
|
memcpy(matrix,m2[0],sizeof(double)*16);
|
|
}
|
|
|
|
TGMatrix& TGMatrix::operator = (const TGMatrix&m2)
|
|
{
|
|
memcpy(matrix,m2[0],sizeof(double)*16);
|
|
return *this;
|
|
}
|
|
|
|
TGMatrix& TGMatrix::operator = (const float*m)
|
|
{
|
|
for (int i = 0;i < 4;i++)
|
|
{
|
|
for (int j = 0;j < 4;j++)
|
|
{
|
|
matrix[i][j] = m[i*4+j];
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
void TGMatrix::Export(float*m) const
|
|
{
|
|
for (int i = 0;i < 4;i++)
|
|
{
|
|
for (int j = 0;j < 4;j++)
|
|
{
|
|
m[i*4+j] = (float)(matrix[i][j]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void TGMatrix::Transform3D(TGPoint&point) const
|
|
{
|
|
double a = matrix[0][0]*point.x+matrix[1][0]*point.y+matrix[2][0]*point.z+matrix[3][0];
|
|
double b = matrix[0][1]*point.x+matrix[1][1]*point.y+matrix[2][1]*point.z+matrix[3][1];
|
|
double c = matrix[0][2]*point.x+matrix[1][2]*point.y+matrix[2][2]*point.z+matrix[3][2];
|
|
double w = matrix[0][3]*point.x+matrix[1][3]*point.y+matrix[2][3]*point.z+matrix[3][3];
|
|
point.x = a/w;
|
|
point.y = b/w;
|
|
point.z = c/w;
|
|
}
|
|
|
|
void TGMatrix::Transform2D(TGPoint&point) const
|
|
{
|
|
double a = matrix[0][0]*point.x+matrix[1][0]*point.y+matrix[3][0];
|
|
double b = matrix[0][1]*point.x+matrix[1][1]*point.y+matrix[3][1];
|
|
double w = matrix[0][3]*point.x+matrix[1][3]*point.y+matrix[3][3];
|
|
point.x = a/w;
|
|
point.y = b/w;
|
|
}
|
|
|
|
void TGMatrix::Transform(TGVector3D&vt) const
|
|
{
|
|
double a = matrix[0][0]*vt.x+matrix[1][0]*vt.y+matrix[2][0]*vt.z;
|
|
double b = matrix[0][1]*vt.x+matrix[1][1]*vt.y+matrix[2][1]*vt.z;
|
|
double c = matrix[0][2]*vt.x+matrix[1][2]*vt.y+matrix[2][2]*vt.z;
|
|
double w = matrix[0][3]*vt.x+matrix[1][3]*vt.y+matrix[2][3]*vt.z+matrix[3][3];
|
|
vt.x = a/w;
|
|
vt.y = b/w;
|
|
vt.z = c/w;
|
|
}
|
|
|
|
void TGMatrix::Transform(TGVector2D&vt) const
|
|
{
|
|
double a = matrix[0][0]*vt.x+matrix[1][0]*vt.y;
|
|
double b = matrix[0][1]*vt.x+matrix[1][1]*vt.y;
|
|
double w = matrix[0][3]*vt.x+matrix[1][3]*vt.y+matrix[3][3];
|
|
vt.x = a/w;
|
|
vt.y = b/w;
|
|
}
|
|
|
|
TGMatrix& TGMatrix::operator *= (const TGMatrix &m2)
|
|
{
|
|
GTransformMatrix r;
|
|
for (int i = 0;i < 4;i++)
|
|
{
|
|
for (int j = 0;j < 4;j++)
|
|
{
|
|
r[i][j] = 0.0;
|
|
for (int k = 0;k < 4;k++)
|
|
{
|
|
r[i][j] += matrix[i][k]*m2[k][j];
|
|
}
|
|
}
|
|
}
|
|
memcpy(matrix,r,sizeof(double)*16);
|
|
return *this;
|
|
}
|
|
|
|
TGMatrix TGMatrix::operator *(const TGMatrix&m2) const
|
|
{
|
|
TGMatrix r;
|
|
for (int i = 0;i < 4;i++)
|
|
{
|
|
for (int j = 0;j < 4;j++)
|
|
{
|
|
r[i][j] = 0.0;
|
|
for (int k = 0;k < 4;k++)
|
|
{
|
|
r[i][j] += matrix[i][k]*m2[k][j];
|
|
}
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
bool TGMatrix::Reverse()
|
|
{
|
|
double r = Rank();
|
|
if (EQUAL_ZERO(r))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
TGMatrix m(*this);
|
|
double row[4];
|
|
for (long j = 0;j < 4;j++)
|
|
{
|
|
memcpy(row,m[j],4*sizeof(double));
|
|
memset(m[j],0,4*sizeof(double));
|
|
for (long i = 0;i < 4;i++)
|
|
{
|
|
m[j][i] = 1;
|
|
(*this)[i][j] = m.Rank()/r;
|
|
m[j][i] = 0;
|
|
}
|
|
memcpy(m[j],row,4*sizeof(double));
|
|
}
|
|
return true;
|
|
}
|
|
|
|
double TGMatrix::Rank() const
|
|
{
|
|
const TGMatrix& m = *this;
|
|
double r = 0;
|
|
r += m[0][0]*m[1][1]*m[2][2]*m[3][3];
|
|
r -= m[0][0]*m[1][1]*m[2][3]*m[3][2];
|
|
r -= m[0][0]*m[1][2]*m[2][1]*m[3][3];
|
|
r += m[0][0]*m[1][2]*m[2][3]*m[3][1];
|
|
r -= m[0][0]*m[1][3]*m[2][2]*m[3][1];
|
|
r += m[0][0]*m[1][3]*m[2][1]*m[3][2];
|
|
|
|
r -= m[0][1]*m[1][0]*m[2][2]*m[3][3];
|
|
r += m[0][1]*m[1][0]*m[2][3]*m[3][2];
|
|
r += m[0][1]*m[1][2]*m[2][0]*m[3][3];
|
|
r -= m[0][1]*m[1][2]*m[2][3]*m[3][0];
|
|
r -= m[0][1]*m[1][3]*m[2][0]*m[3][2];
|
|
r += m[0][1]*m[1][3]*m[2][2]*m[3][0];
|
|
|
|
r += m[0][2]*m[1][0]*m[2][1]*m[3][3];
|
|
r -= m[0][2]*m[1][0]*m[2][3]*m[3][1];
|
|
r -= m[0][2]*m[1][1]*m[2][0]*m[3][3];
|
|
r += m[0][2]*m[1][1]*m[2][3]*m[3][0];
|
|
r += m[0][2]*m[1][3]*m[2][0]*m[3][1];
|
|
r -= m[0][2]*m[1][3]*m[2][1]*m[3][0];
|
|
|
|
r -= m[0][3]*m[1][0]*m[2][1]*m[3][2];
|
|
r += m[0][3]*m[1][0]*m[2][2]*m[3][1];
|
|
r += m[0][3]*m[1][1]*m[2][0]*m[3][2];
|
|
r -= m[0][3]*m[1][1]*m[2][2]*m[3][0];
|
|
r -= m[0][3]*m[1][2]*m[2][0]*m[3][1];
|
|
r += m[0][3]*m[1][2]*m[2][1]*m[3][0];
|
|
|
|
return r;
|
|
}
|
|
|
|
void TGMatrix::Swap()
|
|
{
|
|
Exchange((*this)[0][1],(*this)[1][0]);
|
|
Exchange((*this)[0][2],(*this)[2][0]);
|
|
Exchange((*this)[0][3],(*this)[3][0]);
|
|
Exchange((*this)[1][2],(*this)[2][1]);
|
|
Exchange((*this)[1][3],(*this)[3][1]);
|
|
Exchange((*this)[2][3],(*this)[3][2]);
|
|
}
|
|
|
|
void TGMatrix::CreateMoveMatrix(double cx,double cy,double cz)
|
|
{
|
|
matrix[3][0] = cx;
|
|
matrix[3][1] = cy;
|
|
matrix[3][2] = cz;
|
|
}
|
|
|
|
void TGMatrix::CreateMoveMatrix(double cx,double cy)
|
|
{
|
|
matrix[3][0] = cx;
|
|
matrix[3][1] = cy;
|
|
}
|
|
|
|
void TGMatrix::CreateMoveMatrix(const TGVector3D&vt)
|
|
{
|
|
matrix[3][0] = vt.x;
|
|
matrix[3][1] = vt.y;
|
|
matrix[3][2] = vt.z;
|
|
}
|
|
|
|
void TGMatrix::CreateMoveMatrix(const TGVector2D&vt)
|
|
{
|
|
matrix[3][0] = vt.x;
|
|
matrix[3][1] = vt.y;
|
|
}
|
|
|
|
void TGMatrix::CreateScaleMatrix(double sx,double sy,double sz)
|
|
{
|
|
matrix[0][0] = sx;
|
|
matrix[1][1] = sy;
|
|
matrix[2][2] = sz;
|
|
}
|
|
|
|
void TGMatrix::CreateScaleMatrix(double sx,double sy)
|
|
{
|
|
matrix[0][0] = sx;
|
|
matrix[1][1] = sy;
|
|
}
|
|
|
|
void TGMatrix::CreateRotateMatrix(double angle,const TGPoint&basePoint)
|
|
{
|
|
double C = cos(angle),S = sin(angle);
|
|
matrix[0][0] = C;
|
|
matrix[0][1] = S;
|
|
matrix[1][0] = -S;
|
|
matrix[1][1] = C;
|
|
matrix[3][0] = (1-C)*basePoint.x+basePoint.y*S;
|
|
matrix[3][1] = (1-C)*basePoint.y-basePoint.x*S;
|
|
}
|
|
|
|
void TGMatrix::CreateScaleMatrix(double sx,double sy,double sz,const TGPoint&basePoint)
|
|
{
|
|
matrix[0][0] = sx;
|
|
matrix[1][1] = sy;
|
|
matrix[2][2] = sz;
|
|
matrix[3][0] = (1-sx)*basePoint.x;
|
|
matrix[3][1] = (1-sy)*basePoint.y;
|
|
matrix[3][2] = (1-sz)*basePoint.z;
|
|
}
|
|
|
|
void TGMatrix::CreateScaleMatrix(double sx,double sy,const TGPoint&basePoint)
|
|
{
|
|
matrix[0][0] = sx;
|
|
matrix[1][1] = sy;
|
|
matrix[3][0] = (1-sx)*basePoint.x;
|
|
matrix[3][1] = (1-sy)*basePoint.y;
|
|
}
|
|
|
|
void TGMatrix::CreateXRotateMatrix(double angle)
|
|
{
|
|
matrix[1][1] = cos(angle);
|
|
matrix[2][2] = matrix[1][1];
|
|
matrix[1][2] = sin(angle);
|
|
matrix[2][1] = -matrix[1][2];
|
|
}
|
|
|
|
void TGMatrix::CreateYRotateMatrix(double angle)
|
|
{
|
|
matrix[0][0] = cos(angle);
|
|
matrix[2][2] = matrix[0][0];
|
|
matrix[2][0] = sin(angle);
|
|
matrix[0][2] = -matrix[2][0];
|
|
}
|
|
|
|
void TGMatrix::CreateZRotateMatrix(double angle)
|
|
{
|
|
matrix[0][0] = cos(angle);
|
|
matrix[1][1] = matrix[0][0];
|
|
matrix[0][1] = sin(angle);
|
|
matrix[1][0] = -matrix[0][1];
|
|
}
|
|
|
|
void TGMatrix::CreateRotateMatrix(double angle,const TPGeLine3D&axis)
|
|
{
|
|
TGVector3D vt(axis);
|
|
if (!vt.Normalize())
|
|
{
|
|
return;
|
|
}
|
|
|
|
double a = vt.x,b = vt.y,c = vt.z;
|
|
double x = axis.basePoint.x,y = axis.basePoint.y,z = axis.basePoint.z;
|
|
double v = sqrt(c*c+b*b);
|
|
TGMatrix &taMat = *this,uaMat,rxMat,uxMat,ryMat,uyMat,rzmat;
|
|
|
|
taMat[3][0] = -x;
|
|
taMat[3][1] = -y;
|
|
taMat[3][2] = -z;
|
|
|
|
uaMat[3][0] = x;
|
|
uaMat[3][1] = y;
|
|
uaMat[3][2] = z;
|
|
|
|
rxMat[1][1] = rxMat[2][2] = c/v;
|
|
rxMat[1][2] = b/v;
|
|
rxMat[2][1] = -rxMat[1][2];
|
|
|
|
uxMat[1][1] = uxMat[2][2] = c/v;
|
|
uxMat[1][2] = -b/v;
|
|
uxMat[2][1] = -uxMat[1][2];
|
|
|
|
ryMat[0][0] = ryMat[2][2] = v;
|
|
ryMat[0][2] = a;
|
|
ryMat[2][0] = -a;
|
|
|
|
uyMat[0][0] = uyMat[2][2] = v;
|
|
uyMat[0][2] = -a;
|
|
uyMat[2][0] = a;
|
|
|
|
rzmat[0][0] = rzmat[1][1] = cos(angle);
|
|
rzmat[0][1] = sin(angle);
|
|
rzmat[1][0] = -rzmat[0][1];
|
|
|
|
taMat *= rxMat;
|
|
taMat *= ryMat;
|
|
taMat *= rzmat;
|
|
taMat *= uyMat;
|
|
taMat *= uxMat;
|
|
taMat *= uaMat;
|
|
}
|
|
|
|
bool TGMatrix::CreateXYProjectMatrix(const TGPoint&pt,bool bParallel)
|
|
{
|
|
if (EQUAL_ZEROD(pt.z))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
CreateXYProjectMatrix();
|
|
matrix[2][0] = -pt.x/pt.z;
|
|
matrix[2][1] = -pt.y/pt.z;
|
|
if (!bParallel)
|
|
{
|
|
matrix[2][3] = -1/pt.z;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool TGMatrix::CreateProjectMatrix(const TPGePlane&plane,const TGPoint&pt,bool bParallel)
|
|
{
|
|
if (bParallel)
|
|
{
|
|
double A = plane.a;
|
|
double B = plane.b;
|
|
double C = plane.c;
|
|
double D = plane.d;
|
|
TGVector3D vt = pt;
|
|
vt.Normalize();
|
|
double dot = A*vt[0]+B*vt[1]+C*vt[2];
|
|
if (EQUAL_ZERO(dot))
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
matrix[0][0] = 1.0-vt[0]*A/dot;
|
|
|
|
matrix[1][0] = -vt[0]*B/dot;
|
|
matrix[2][0] = -vt[0]*C/dot;
|
|
matrix[3][0] = -vt[0]*D/dot;
|
|
|
|
matrix[0][1] = -vt[1]*A/dot;
|
|
|
|
matrix[1][1] = 1.0-vt[1]*B/dot;
|
|
matrix[2][1] = -vt[1]*C/dot;
|
|
matrix[3][1] = -vt[1]*D/dot;
|
|
|
|
matrix[0][2] = -vt[2]*A/dot;
|
|
|
|
matrix[1][2] = -vt[2]*B/dot;
|
|
matrix[2][2] = 1.0-vt[2]*C/dot;
|
|
matrix[3][2] = -vt[2]*D/dot;
|
|
|
|
return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (plane.HitTest(pt).Is(GR_IN))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
TGPoint pt2(pt);
|
|
TGMatrix mPlane,mProject;
|
|
mPlane.SetCoordSys(plane);
|
|
(*this) *= mPlane;
|
|
mPlane.Transform3D(pt2);
|
|
mProject.CreateXYProjectMatrix(pt2,bParallel);
|
|
(*this) *= mProject;
|
|
mPlane.Reverse();
|
|
(*this) *= mPlane;
|
|
return true;
|
|
}
|
|
}
|