ivu_linear.cxx

hl2 source code. Do not use it illegal.

}

/** Calculate one eigenvector for a given eigen value
  returns the degree of freedom of the eigenvector,
  the function assumes that there is at least one eigenvector !!!!
  */


int IVP_U_Matrix3::calc_eigen_vector(IVP_DOUBLE eigen_value, IVP_U_Point *eigen_vector_out){
    IVP_U_Matrix3 matrix = *this;
    IVP_DOUBLE m[3][3];		// row, column
    int col_index[3];		// the original index of a local col
    int row_index[3];		// the original index of a local row
    // search highest element in first column
    col_index[0] = 0;
    col_index[1] = 1;
    col_index[2] = 2;
    row_index[0] = 0;
    row_index[1] = 1;
    row_index[2] = 2;
    
    matrix.rows[0].k[0] -= eigen_value;
    matrix.rows[1].k[1] -= eigen_value;
    matrix.rows[2].k[2] -= eigen_value;
    
    {	// find maximum value in all values
	IVP_DOUBLE max = -1.0f;     // real maximum is always positiv
	int max_row = 0;
	int max_col= 0;
	
	for ( int row = 0;row<3;row++){
	    IVP_U_Point *current_row = &rows[row];
	    for ( int col = 0; col < 3; col++){
		IVP_DOUBLE v = current_row->k[col];
		v *= v;
		if (v > max) {
		    max = v;
		    max_col = col;
		    max_row = row;
		}
	    }
	}
    	row_index[0] = max_row;	// exchange columns to shift pivot element into m[0][0]
	row_index[max_row] = 0;

	col_index[0] = max_col;
	col_index[ max_col ] = 0;
	
	if (max < (P_DOUBLE_RES * P_DOUBLE_RES) ){	// 3 degree of freedom !!
	    eigen_vector_out->set(0,0,1);
	    return 3;
	}
    }

    {	// resort matrix
	for (int i=2;i>=0;i--){
	    IVP_U_Point *dest_row = &rows[i];
	    IVP_U_Point *source = &rows[row_index[i]];
	    dest_row->set(source);
	}
    }

    // get rid of first column values;
    {
	for (int r = 1; r<=2;r++){
	    IVP_DOUBLE f = m[r][0] / m[0][0];
	    m[r][1] -= f * m[0][1];
	    m[r][2] -= f * m[0][2];
	}
    }
    // get index of pivot element in 1 or 2rd row->flip rows
    {
	IVP_DOUBLE max = -1.0f;     // real maximum is always positiv
	int max_row=0,max_col=0;
    
	for ( int row = 1;row<3;row++){
	    IVP_DOUBLE *pm = &m[row][0];	// skip first row and column
	    for ( int col = 1; col < 3; col++){
		IVP_DOUBLE v = pm[col];
		v *= v;
		if (v > max) {
		    max = v;   max_col = col;    max_row = row;
		}
	    }
	}
	if (max_row != 1){	// switch rows  1 and 2
	    IVP_DOUBLE f;
	    f = m[1][1]; m[1][1] = m[2][1]; m[2][1] = f;
	    f = m[1][2]; m[1][2] = m[2][2]; m[2][2] = f;
	}
	if (max_col != 1){	// switch columns 1 and 2 and index pointer
	    IVP_DOUBLE h;
	    h = m[0][1]; m[0][1] = m[0][2]; m[0][2] = h;
	    h = m[1][1]; m[1][1] = m[1][2]; m[1][2] = h;
	    h = m[2][1]; m[2][1] = m[2][2]; m[2][2] = h;
	    int i;
	    i = col_index[2]; col_index[2] = col_index[1]; col_index[1] = i;
	}

    // check for 2 degrees of freedom !!
	if (max < (P_DOUBLE_RES * P_DOUBLE_RES) ){
	    eigen_vector_out->k[col_index[0]] = m[0][1] / m[0][0];
	    eigen_vector_out->k[col_index[1]] = 1.0f;
	    eigen_vector_out->k[col_index[2]] = 0.0f;
	    return 2;
	}
    }
    
    IVP_DOUBLE f2 = m[1][2]/m[1][1];
    
    // get rid of second column values;
    {
	m[2][2] -= f2 * m[2][1];		// this value should be zero, as we know we have an eigenvalue
    }
    // now we got an upper diagonal matrix,
    // set eigen_vector_out.k[index[2]] = 1.0
    eigen_vector_out->k[col_index[2]] = 1.0f;
    eigen_vector_out->k[col_index[1]] = -f2;
    eigen_vector_out->k[col_index[0]] = ( f2 * m[0][1] - m[0][2]) / m[0][0];
    return 1;
}



void IVP_U_Hesse::proj_on_plane(const IVP_U_Point *p, IVP_U_Point *result) const
{
    result->add_multiple(p, this, - get_dist(p)/quad_length());
                                  // div unnecessary, if hesse is normized
}

void IVP_U_Hesse::calc_hesse_val(const IVP_U_Point *p0){
	this->hesse_val= -this->dot_product(p0);
}

void IVP_U_Hesse::mult_hesse(IVP_DOUBLE factor){
    mult(factor);
    hesse_val *= factor;
}



void IVP_U_Hesse::calc_hesse(const IVP_U_Point *tp0,const IVP_U_Point *tp1,const IVP_U_Point *tp2)
{
	IVP_DOUBLE a0, a1, a2, b0, b1, b2;

	// calculate (non-normized) normal
	IVP_DOUBLE fp0 = tp0->k[0];
	IVP_DOUBLE fp1 = tp0->k[1];
	IVP_DOUBLE fp2 = tp0->k[2];
	a0 = tp1->k[0] - fp0;
	a1 = tp1->k[1] - fp1;
	a2 = tp1->k[2] - fp2;
	b0 = tp2->k[0] - fp0;
	b1 = tp2->k[1] - fp1;
	b2 = tp2->k[2] - fp2;

	IVP_DOUBLE c0 = b1*a2 - a1*b2;
	IVP_DOUBLE c1 = b2*a0 - a2*b0;
	IVP_DOUBLE c2 = b0*a1 - a0*b1;


	// calculate hesse-area
	this->k[0]=	c0;
	this->k[1]=	c1;
	this->k[2]=	c2;
	this->hesse_val=	-(this->k[0]*fp0 +
		this->k[1]*fp1 + this->k[2]*fp2);
};

void IVP_U_Hesse::calc_hesse(const IVP_U_Float_Point *tp0,
							 const IVP_U_Float_Point *tp1,
							 const IVP_U_Float_Point *tp2)
{
	IVP_DOUBLE a0, a1, a2, b0, b1, b2;

	// calculate (non-normized) normal
	IVP_DOUBLE fp0 = (IVP_DOUBLE)tp0->k[0];
	IVP_DOUBLE fp1 = (IVP_DOUBLE)tp0->k[1];
	IVP_DOUBLE fp2 = (IVP_DOUBLE)tp0->k[2];
	a0 = tp1->k[0] - fp0;
	a1 = tp1->k[1] - fp1;
	a2 = tp1->k[2] - fp2;
	b0 = tp2->k[0] - fp0;
	b1 = tp2->k[1] - fp1;
	b2 = tp2->k[2] - fp2;

	IVP_DOUBLE c0 = b1*a2 - a1*b2;
	IVP_DOUBLE c1 = b2*a0 - a2*b0;
	IVP_DOUBLE c2 = b0*a1 - a0*b1;


	// calculate hesse-area
	this->k[0]=	c0;
	this->k[1]=	c1;
	this->k[2]=	c2;
	this->hesse_val=	-(this->k[0]*fp0 +
		this->k[1]*fp1 + this->k[2]*fp2);
};


void IVP_U_Hesse::normize()
{
    IVP_DOUBLE l = 1.0f/this->real_length();
    this->mult(l);
    hesse_val *= l;
}

void IVP_U_Float_Hesse::mult_hesse(IVP_DOUBLE factor){
    mult(factor);
    hesse_val *= factor;
}


void IVP_U_Float_Hesse::proj_on_plane(const IVP_U_Float_Point *p, IVP_U_Float_Point *result) const
{
    result->add_multiple(p, this, - get_dist(p)/quad_length());
                                  // div unnecessary, if hesse is normized
}

void IVP_U_Float_Hesse::calc_hesse(const IVP_U_Float_Point *tp0,const IVP_U_Float_Point *tp1,const IVP_U_Float_Point *tp2)
{
	IVP_DOUBLE a0, a1, a2, b0, b1, b2;

	// calculate (non-normized) normal
	IVP_DOUBLE fp0 = tp0->k[0];
	IVP_DOUBLE fp1 = tp0->k[1];
	IVP_DOUBLE fp2 = tp0->k[2];
	a0 = tp1->k[0] - fp0;
	a1 = tp1->k[1] - fp1;
	a2 = tp1->k[2] - fp2;
	b0 = tp2->k[0] - fp0;
	b1 = tp2->k[1] - fp1;
	b2 = tp2->k[2] - fp2;

	IVP_DOUBLE c0 = b1*a2 - a1*b2;
	IVP_DOUBLE c1 = b2*a0 - a2*b0;
	IVP_DOUBLE c2 = b0*a1 - a0*b1;


	// calc hesse-plane
	this->k[0]=	(IVP_FLOAT)c0;
	this->k[1]=	(IVP_FLOAT)c1;
	this->k[2]=	(IVP_FLOAT)c2;
	this->hesse_val=(IVP_FLOAT)	-(this->k[0]*fp0 +
		this->k[1]*fp1 + this->k[2]*fp2);
};

void IVP_U_Float_Hesse::calc_hesse_val(const IVP_U_Float_Point *p0){
	this->hesse_val= -this->dot_product(p0);
}

void IVP_U_Float_Hesse::normize()
{
    IVP_DOUBLE l = 1.0f/this->real_length();
    this->mult(l);
    hesse_val *= l;
}




void IVP_U_Matrix::interpolate(const IVP_U_Matrix *m1, const IVP_U_Matrix *m2,
		    IVP_DOUBLE i_factor)
{
    // i_factor aus [0,1] -> this = interpolation between m1 & m2
    for( int r=0;r<3;r++){
	rows[r].set_interpolate( &m1->rows[r], &m2->rows[r], i_factor);
    }
    vv.set_interpolate( m1->get_position(), m2->get_position(), i_factor);
}



IVP_RETURN_TYPE IVP_U_Matrix::real_invert(IVP_DOUBLE epsilon)
{
    // real inversion using determinant algorithm
    
    IVP_U_Point r0; r0.inline_calc_cross_product( &rows[1], &rows[2] );
    IVP_U_Point r1; r1.inline_calc_cross_product( &rows[2], &rows[0] );
    IVP_U_Point r2; r2.inline_calc_cross_product( &rows[0], &rows[1] );

    IVP_DOUBLE D = rows[0].dot_product(&r0);	// main determinant
    
    if(IVP_Inline_Math::fabsd(D)< epsilon){
		return IVP_FAULT;  // cannot invert, may happen
    }    
    IVP_DOUBLE DI = 1.0f/D;
    r0.mult(DI);
    r1.mult(DI);
    r2.mult(DI);
    set_col(IVP_INDEX_X, &r0);
    set_col(IVP_INDEX_Y, &r1);
    set_col(IVP_INDEX_Z, &r2);
    
    IVP_U_Point p;
    this->vmult3(&this->vv, &p);
    vv.set_negative(&p);
    
    return IVP_OK;
}


IVP_DOUBLE IVP_U_Matrix3::get_determinante() const{
    IVP_U_Point r0; r0.inline_calc_cross_product( &rows[1], &rows[2] );
    IVP_DOUBLE D = rows[0].dot_product(&r0);	// main determinant
    return D;
}

IVP_RETURN_TYPE IVP_U_Matrix3::real_invert(IVP_DOUBLE epsilon)
{
    // real inversion using determinant algorithm
    
    IVP_U_Point r0; r0.inline_calc_cross_product( &rows[1], &rows[2] );
    IVP_U_Point r1; r1.inline_calc_cross_product( &rows[2], &rows[0] );
    IVP_U_Point r2; r2.inline_calc_cross_product( &rows[0], &rows[1] );

    IVP_DOUBLE D = rows[0].dot_product(&r0);	// main determinant
    
    if(IVP_Inline_Math::fabsd(D)< epsilon){
		return IVP_FAULT;  // cannot invert, may happen
    }    
    IVP_DOUBLE DI = 1.0f/D;
    r0.mult(DI);
    r1.mult(DI);
    r2.mult(DI);
    set_col(IVP_INDEX_X, &r0);
    set_col(IVP_INDEX_Y, &r1);
    set_col(IVP_INDEX_Z, &r2);
    
    return IVP_OK;
}

void IVP_U_Matrix::init(){
    P_MEM_CLEAR(this);
    set_elem(0,0,1.0f);
    set_elem(1,1,1.0f);
    set_elem(2,2,1.0f);
}

void IVP_U_Matrix3::init3(){
    P_MEM_CLEAR(this);
    set_elem(0,0,1.0f);
    set_elem(1,1,1.0f);
    set_elem(2,2,1.0f);
}

IVP_RETURN_TYPE IVP_U_Matrix:: real_invert(const IVP_U_Matrix *m, IVP_DOUBLE epsilon)
{
     // overloaded
    this->set_matrix(m);
    return this->real_invert(epsilon);
}


void IVP_U_Matrix::rotate(IVP_COORDINATE_INDEX axis, IVP_FLOAT angle, IVP_U_Matrix *m_out) {
    static int pm[5] = {1, 2, 0, 1, 2};
    const int *p = &pm[axis];

    IVP_DOUBLE SA = IVP_Inline_Math::sind(angle);
    IVP_DOUBLE CA = IVP_Inline_Math::cosd(angle);
    IVP_U_Matrix m_rot;
    P_MEM_CLEAR(&m_rot);
    m_rot.set_elem( p[2], p[2] ,1);
    m_rot.set_elem( p[0], p[0], CA );
    m_rot.set_elem( p[1], p[1], CA );
    m_rot.set_elem( p[0], p[1], -SA);
    m_rot.set_elem( p[1], p[0], SA);
    m_rot.mmult4(this, m_out);
}

void IVP_U_Matrix::rotate_invers(IVP_COORDINATE_INDEX axis, IVP_FLOAT angle, IVP_U_Matrix *m_out) {
    static const int pm[5] = {1, 2, 0, 1, 2};
    const int *p = &pm[axis];

    IVP_DOUBLE SA = IVP_Inline_Math::sind(angle);
    IVP_DOUBLE CA = IVP_Inline_Math::cosd(angle);
    IVP_U_Matrix m_rot;
    P_MEM_CLEAR(&m_rot);
    m_rot.set_elem(p[2], p[2], 1);
    m_rot.set_elem(p[0], p[0], CA);
    m_rot.set_elem(p[0], p[1], SA);
    m_rot.set_elem(p[1], p[0], -SA);
    m_rot.set_elem(p[1], p[1], CA);
    this->mi2mult4(&m_rot, m_out);
}

void IVP_U_Matrix3::init_rotated3(IVP_COORDINATE_INDEX axis, IVP_FLOAT angle) {
    static int pm[5] = {1, 2, 0, 1, 2};
    const int *p = &pm[axis];

    IVP_DOUBLE SA = IVP_Inline_Math::sind(angle);
    IVP_DOUBLE CA = IVP_Inline_Math::cosd(angle);
    P_MEM_CLEAR(this);
    this->set_elem( p[2], p[2] ,1.0f);
    this->set_elem( p[0], p[0], CA );
    this->set_elem( p[1], p[1], CA );
    this->set_elem( p[0], p[1], -SA);
    this->set_elem( p[1], p[0], SA);
}



void IVP_U_Matrix::init_rows4( const IVP_U_Point *v0, const IVP_U_Point *v1, const IVP_U_Point *v2,const IVP_U_Point *shift){
// initialize transponed from v0v1v2, vx is stored in rows
    rows[0].set(v0);
    rows[1].set(v1);
    rows[2].set(v2);

    if (shift){
	vv.set(shift);
    }else{
	vv.set_to_zero();