The matrix_x*v.x + y*v.y + z*v.z-CSDN博客

// 最后修改: 2004.8.杭州

//vector3d.h

#ifndef VECTOR3D_HEADER

#define VECTOR3D_HEADER

#include <cmath>

#include <iostream>

using namespace std;

template<typename T>

class vector3d

{

public:

vector3d(T _x=0,T _y=0, T _z=0.0)

{

x=_x;

y=_y;

z=_z;

}

vector3d(const vector3d &v )

{

x=v.x;

y=v.y;

z=v.z;

}

template<typename In>

vector3d(const In pv)

{

x=pv[0];

y=pv[1];

z=pv[2];

}

inline T Magnitude()

{

return sqrt(x*x+y*y+z*z);

}

inline void Normalize()

{

T magnitude=Magnitude();

if(!magnitude) return;

x/=magnitude;

y/=magnitude;

z/=magnitude;

}

inline vector3d<T> GetNormalizedVector()

{

T magnitude=Magnitude();

if(!magnitude) return *this;

return (*this)/magnitude;

}

inline vector3d<T> operator +(vector3d<T> &v)

{

return vector3d<T>(x+v.x,y+v.y,z+v.z);

}

inline vector3d<T> operator -(vector3d<T> &v)

{

return vector3d<T>(x-v.x,y-v.y,z-v.z);

}

inline T operator *(vector3d<T> &v)

{

return x*v.x+y*v.y+z*v.z;

}

inline vector3d<T> X(vector3d<T> &v)

{

return vector3d<T>(y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*z.x);

}

inline vector3d<T> operator +(T t)

{

return vector3d<T>(x+t,y+t,z+t);

}

inline vector3d<T> operator -(T t)

{

return vector3d<T>(x-t,y-t,z-t);

}

inline vector3d<T> operator *(T t)

{

return vector3d<T>(x*t,y*t,z*t);

}

inline vector3d<T> operator /(T t)

{

return vector3d<T>(x/t,y/t,z/t);

}

T & operator [](size_t index)//nonconst,but without & (can be left-value)

{

switch(index)

{

case 0:

return x;

case 1:

return y;

default:

return z;

}

friend ostream & operator <<(ostream &os,const vector3d<T> &v)

{

return os<<v.x<<"/t"<<v.y<<"/t"<<v.z<<endl;

}

T x,y,z;

};

#endif

//matrix.h

#ifndef MATRIX_H

#define MATRIX_H

#include "vector3d.h"

#include <vector>

#include <cmath>

using namespace std;

template<typename T>

class matrix

{

public:

template<typename In>

matrix(const In pData,size_t nR,size_t nC):nRow(nR),nCol(nC)//pData 连续的容器(数组)

{

T *p=(T*)(&(pData[0]));

copy(p,p+nR*nC,inserter(data,data.begin()));//data.reserve(nR*nC);copy(p,p+nR*nC,data.begin());更快

}

matrix()

{

nCol=0;nRow=0;

}

matrix(size_t _nRow,size_t _nCol=0)

{

if(_nCol==0) _nCol=_nRow;

nRow=_nRow;

nCol=_nCol;

if(nRow>0)

Identity();

}

void Identity()

{

data.resize(nRow*nCol,0);

for(size_t i=0;i<(nRow<nCol?nRow:nCol);i++)

{

data[i*nCol+i]=1.0;

}

friend ostream & operator <<(ostream &os,const matrix<T> & m)

{

if(m.nRow<=0 || m.nCol<=0) return os;

for(size_t i=0;i<m.nRow;i++)

{

for(size_t j=0;j<m.nCol;j++)

{

os<<m.data[i*m.nCol+j]<<"/t";

}

os<<endl;

}

return os;

}

void SetValue(T v,size_t r,size_t c)

{

data[t*nRow+c]=v;

}

T GetValue(size_t r,size_t c)const

{

return data[t*nRow+c];

}

void GetRotor(matrix<T> &out)

{

out.data.reserve(data.size());

out.nCol=nRow;

out.nRow=nCol;

for(size_t i=0;i<nCol;i++)

{

for(size_t j=0;j<nRow;j++)

{

out.data[i*nRow+j]=data[j*nCol+i];

}

matrix<T> GetRotor()

{

matrix<T> out;

out.data.resize(data.size(),0);

out.nCol=nRow;

out.nRow=nCol;

for(size_t i=0;i<nCol;i++)

{

for(size_t j=0;j<nRow;j++)

{

out.data[i*nRow+j]=data[j*nCol+i];

}

return out;

}

void ExchangeRowPos(size_t r1,size_t r2)

{

if(r1>=nRow || r2>=nRow || r2==r1 ) return;

T tmp;

for(size_t i=0;i<nCol;i++)

{

tmp=data[r1*nCol+i];

data[r1*nCol+i]=data[r2*nCol+i];

data[r2*nCol+i]=tmp;

}

void ExchangeColPos(size_t c1,size_t c2)

{

if(c1>=nRow || c2>=nRow || c2==c1 ) return;

T tmp;

for(size_t i=0;i<nRow;i++)

{

tmp=data[i*nRow+c1];

data[i*nRow+c1]=data[i*nRow+c2];

data[i*nRow+c2]=tmp;

}

vector<T> GetX(vector<T> B)

{

vector<T> x;

x.resize(nRow,0);

if(nRow<2 || nCol<2 || nCol!=nRow || B.size() !=nRow) return x;

matrix<T> m=*this;

size_t i,j,k;

for(k=0;k<nRow;k++)

{

if(m.data[k*nCol+k]==0)

{

for(i=k+1;i<nRow;i++)

{

if(m.data[i*nCol+k]!=0)

{

m.ExchangeRowPos(k,i);

B[k]+=B[i];

B[i]=B[k]-B[i];

B[k]=B[k]-B[i];

break;

}

if(i==nRow)

return x;

}

for(i=k+1;i<nRow;i++)

{

T rate=m.data[i*nCol+k]/m.data[k*nCol+k];

for(j=k;j<nCol;j++)

{

m.data[i*nCol+j]-=rate*m.data[k*nCol+j];

}

B[i]-=rate*B[k];

}

x[nRow-1]=B[nRow-1]/m.data[m.data.size()-1];

for(int a=(int)nRow-2;a>=0;a--)

{

T tmpSum=0;

for(int b=(int)nCol-1;b>a;b--)

{

tmpSum+=x[b]*(m.data[a*nCol+b]);

}

x[a]=(B[a]-tmpSum)/m.data[a*nCol+a];

}

return x;

}

T GetModel()

{

if(nRow<2 || nCol<2 || nCol!=nRow ) return 0;

matrix<T> m=*this;

size_t i,j,k;

for(k=0;k<nRow;k++)

{

if(m.data[k*nCol+k]==0)

{

for(i=k+1;i<nRow;i++)

{

if(m.data[i*nCol+k]!=0)

{

m.ExchangeRowPos(k,i);

break;

}

if(i==nRow)

return 0;

}

for(i=k+1;i<nRow;i++)

{

T rate=m.data[i*nCol+k]/m.data[k*nCol+k];

for(j=k;j<nCol;j++)

{

m.data[i*nCol+j]-=rate*m.data[k*nCol+j];

}

T tmp=1;

for(k=0;k<m.nRow;k++)

{

tmp*=m.data[k*nCol+k];

}

return tmp;

}

void GetNegativeMatrix(matrix &out)

{

if(nRow<2 || nCol<2 || nCol!=nRow ) return ;

out.data.clear();

out=*this;

size_t k,i,j;

vector<Row_Col> rc_list;

size_t rci,rcj;

T tmp;

for(k=0;k<nRow;k++)

{

//选主元,稳定,能保证稳定的情况下可以去掉这部分

tmp=fabs(out.data[k*nCol+k]);

rci=k;

rcj=k;

for(i=k;i<nRow;i++)

{

for(j=k;j<nCol;j++)

{

if(fabs(out.data[i*nCol+j])>tmp)

{

rci=i;

rcj=j;

tmp=fabs(out.data[i*nCol+j]);

}

//保存选主元时的行列交换信息

if(rci!=k)

{

out.ExchangeRowPos(k,rci);

Row_Col rc;

rc.rc1=k;

rc.rc2=rci;

rc.bRow=true;

rc_list.push_back(rc);

}

if(rcj!=k)

{

out.ExchangeColPos(k,rcj);

Row_Col rc;

rc.rc1=k;

rc.rc2=rcj;

rc.bRow=false;

rc_list.push_back(rc);

}

//选主元结束

out.data[k*nCol+k]=1/out.data[k*nCol+k];

for(j=0;j<nCol;j++)

{

if(k!=j)

out.data[k*nCol+j]=out.data[k*nCol+j]*out.data[k*nCol+k];

}

for(i=0;i<nRow;i++)

{

for(j=0;j<nCol;j++)

{

if(k!=j && k!=i)

out.data[i*nCol+j]=out.data[i*nCol+j]-out.data[i*nCol+k]*out.data[k*nCol+j];

}

for(i=0;i<nCol;i++)

{

if(k!=i)

out.data[i*nCol+k]*=-out.data[k*nCol+k];

}

//恢复选主元时的行列交换

for(k=rc_list.size();k>0;k--)

{

i=k-1;

if(rc_list[i].bRow)

{

out.ExchangeColPos(rc_list[i].rc1,rc_list[i].rc2);

}

else

{

out.ExchangeRowPos(rc_list[i].rc1,rc_list[i].rc2);

}

void GetAdjoint(matrix<T>& out)

{

out.data.clear();

GetNegativeMatrix(out);

vector<T>::iterator iter=out.data.begin();

T m=GetModel();

for(;iter!=out.data.end();iter++)

{

(*iter)*=m;

}

matrix<T> operator +(const matrix<T> &m)

{

matrix<T> out=*this;

for(size_t i=0;i<data.size();i++)

{

out.data[i]=data[i]+m.data[i];

}

return out;

}

matrix<T> operator -(const matrix<T> &m)

{

matrix<T> out=*this;

for(size_t i=0;i<data.size();i++)

{

out.data[i]=data[i]-m.data[i];

}

return out;

}

matrix<T> operator *(const matrix<T> &m)

{

if(nCol!=m.nRow) return matrix<T>();

vector<T> out;

out.resize(nRow*m.nCol,0);

size_t i,j,k;

for(i=0;i<nRow;i++)

{

for(j=0;j<m.nCol;j++)

{

for(k=0;k<nCol;k++)

out[i*m.nCol+j]+=data[i*nCol+k]*m.data[k*m.nCol+j];

}

return matrix<T>(out,nRow,m.nCol);

}

void Add(const matrix<T> &m)

{

for(size_t i=0;i<data.size();i++)

{

data[i]+=m.data[i];

}

void Sub(const matrix<T> &m)

{

for(size_t i=0;i<data.size();i++)

{

data[i]-=m.data[i];

}

void Multiply(const matrix<T> &m)

{

if(nCol!=m.nRow) return ;

vector<T> out;

out.resize(nRow*m.nCol,0);

size_t i,j,k;

for(i=0;i<nRow;i++)

{

for(j=0;j<m.nCol;j++)

{

for(k=0;k<nCol;k++)

out[i*m.nCol+j]+=data[i*nCol+k]*m.data[k*m.nCol+j];

}

data=out;

nCol=m.nCol;

}

vector3d<T> Multiply(const vector3d<T> &v,T r=1.0)

{

if(nRow!=4 || nCol!=4) return vector3d<T>();

size_t i,k;

T out[4]={0,0,0,0};

T in[4]={v.x,v.y,v.z,r};

for(i=0;i<4;i++)

{

for(k=0;k<4;k++)

out[i]+=data[i*nCol+k]*in[k];

}

return vector3d<T>(out);

}

///

vector<T> data;

size_t nRow,nCol;

private:

struct Row_Col

{

size_t rc1;

size_t rc2;

bool bRow;

};

#endif