matrix.cpp

robocuo相关资料robocuo相关资料

#include "Matrix.h"
#include "Geometry.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <algorithm>

using namespace std;

void Matrix::swap_col(int a, int b)
{
	for(int i=0;i<4;i++)
	{
		double t=m[i][a];
		m[i][a]=m[i][b];
		m[i][b]=t;
	}
}

double Matrix::factor(int r, int c) const
{
	double tmp[3][3];
	for(int k=0;k<3;k++)
		for(int l=0;l<3;l++)
			tmp[k][l] = m[k+(k>=r)][l+(l>=c)];

	int i,j;
	int a[]={0,1,2};
	double sum=0;
	do
	{
		int sig=1;
		for(i=0;i<3;i++)
			for(j=0;j<i;j++)
				if(a[j]>a[i])
					sig=-sig;
		double p=1;
		for(i=0;i<3;i++)
			p*=tmp[i][a[i]];
		sum+=sig*p;
	}while(std::next_permutation(a,a+3));
	return sum;
}

double Matrix::determinant() const
{
	int i,j;
	int a[]={0,1,2,3};
	double sum=0;
	do
	{
		int sig=1;
		for(i=0;i<4;i++)
			for(j=0;j<i;j++)
				if(a[j]>a[i])
					sig=-sig;
		double p=1;
		for(i=0;i<4;i++)
			p*=m[i][a[i]];
		sum+=sig*p;
	}while(std::next_permutation(a,a+4));

	return sum;
}

Matrix Matrix::cofactors() const
{
	Matrix res;
	for(int i=0;i<4;i++)
		for(int j=0;j<4;j++)
			res.m[i][j] = factor(i,j)*((i+j)%2?-1:1);
	return res;
}

Matrix Matrix::transport() const
{
	Matrix cofac_mat = cofactors();
	Matrix res;
	for(int i=0;i<4;i++)
		for(int j=0;j<4;j++)
			res.m[i][j]=cofac_mat.m[j][i];
	return res;
}

Matrix::Matrix()
{
	for(int r=0;r<4;r++)
		for(int c=0;c<4;c++)
			m[r][c] = 0;
}

Vector3d Matrix::operator*(const Vector3d &v) const
{
	Vector3d res;
	res.X()=m[0][0]*v.X()+m[0][1]*v.Y()+m[0][2]*v.Z()+m[0][3];
	res.Y()=m[1][0]*v.X()+m[1][1]*v.Y()+m[1][2]*v.Z()+m[1][3];
	res.Z()=m[2][0]*v.X()+m[2][1]*v.Y()+m[2][2]*v.Z()+m[2][3];

	return res;
}

Matrix Matrix::operator*(const Matrix &a) const
{
	Matrix res;
	for(int r=0;r<4;r++)
		for(int c=0;c<4;c++)
			for(int k=0;k<4;k++)
				res.m[r][c] += m[r][k] * a.m[k][c];
	return res;
}

Matrix Matrix::operator -() const
{
	Matrix r = *this;
	double det = determinant();
	Matrix trans_mat=transport();
	for(int i=0;i<4;i++)
		for(int j=0;j<4;j++)
			r.m[i][j]=trans_mat.m[i][j]/det;
	return r;
}

Matrix::Matrix(const Vector2d &v)
{
	for(int i=0;i<16;i++)
		m[i/4][i%4]=0;
	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
	m[0][3] = v.x;
	m[1][3] = v.y;
}

Matrix::Matrix(const Vector3d &v)
{
	for(int i=0;i<16;i++)
		m[i/4][i%4]=0;
	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
	m[0][3] = v.X();
	m[1][3] = v.Y();
	m[2][3] = v.Z();
}

Vector2d Matrix::getVector2d()
{
	return Vector2d( m[0][3], m[1][3] );
}

Vector3d Matrix::getVector3d()
{
	return Vector3d( m[0][3], m[1][3], m[2][3] );
}

Matrix Matrix::transferX(double dis)
{
	Matrix r;
	r.m[0][0] = r.m[1][1] = r.m[2][2] = r.m[3][3] = 1.0;
	r.m[0][3] = dis;
	return r;
}

Matrix Matrix::transferY(double dis)
{
	Matrix r;
	r.m[0][0] = r.m[1][1] = r.m[2][2] = r.m[3][3] = 1.0;
	r.m[1][3] = dis;
	return r;
}

Matrix Matrix::transferZ(double dis)
{
	Matrix r;
	r.m[0][0] = r.m[1][1] = r.m[2][2] = r.m[3][3] = 1.0;
	r.m[2][3] = dis;
	return r;
}

Matrix Matrix::translate(double x, double y, double z)
{
	return Matrix(Vector3d(x,y,z));
}

Matrix Matrix::rotateX(double ang)
{
	Matrix r;
	r.m[0][0] = r.m[3][3] = 1.0;
	r.m[1][1] = Angle::cosDeg(ang);
	r.m[1][2] = -Angle::sinDeg(ang);
	r.m[2][1] = Angle::sinDeg(ang);
	r.m[2][2] = Angle::cosDeg(ang);
	return r;
}

Matrix Matrix::rotateY(double ang)
{
	Matrix r;
	r.m[1][1] = r.m[3][3] = 1.0;
	r.m[0][0] = Angle::cosDeg(ang);
	r.m[0][2] = Angle::sinDeg(ang);
	r.m[2][0] = -Angle::sinDeg(ang);
	r.m[2][2] = Angle::cosDeg(ang);
	return r;
}

Matrix Matrix::rotateZ(double ang)
{
	Matrix r;
	r.m[2][2] = r.m[3][3] = 1.0;
	r.m[0][0] = Angle::cosDeg(ang);
	r.m[0][1] = -Angle::sinDeg(ang);
	r.m[1][0] = Angle::sinDeg(ang);
	r.m[1][1] = Angle::cosDeg(ang);
	return r;
}

Matrix Matrix::rotate_arbitrary_axis(Vector3d a, double ang)
{
	Matrix r;
	double costheta = Angle::cosDeg(ang);
	double sintheta = Angle::sinDeg(ang);
	
	a = a/a.magnitude();
	
	r.m[0][0] = (costheta + (1 - costheta) * a.X() * a.X());
	r.m[0][1] = ((1 - costheta) * a.X() * a.Y() - a.Z() * sintheta);
	r.m[0][2] = ((1 - costheta) * a.X() * a.Z() + a.Y() * sintheta);
	r.m[1][0] = ((1 - costheta) * a.X() * a.Y() + a.Z() * sintheta);
	r.m[1][1] = (costheta + (1 - costheta) * a.Y() * a.Y());
	r.m[1][2] = ((1 - costheta) * a.Y() * a.Z() - a.X() * sintheta);
	r.m[2][0] = ((1 - costheta) * a.X() * a.Z() - a.Y() * sintheta);
	r.m[2][1] = ((1 - costheta) * a.Y() * a.Z() + a.X() * sintheta);
	r.m[2][2] = (costheta + (1 - costheta) * a.Z() * a.Z());
	r.m[3][3] = 1;
	return r;
}

void Matrix::print()
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
			printf("%10.6lf", m[i][j]);
		printf("\n");
	}
}