ivu_linear.cxx

hl2 source code. Do not use it illegal.

    }
}

// init orthornomized matrix given a Bs-axis in As coords
// result is a matrix m_Bs_f_As
void IVP_U_Matrix3::init_normized3_col(const IVP_U_Point *vp, IVP_COORDINATE_INDEX coordinate) {
    IVP_U_Point v0 = *vp;
    v0.normize();
    IVP_U_Point v2(v0.k[1], v0.k[2] - v0.k[0], -v0.k[1]);		// orthogonal vector
    if (v2.normize() == IVP_FAULT){
	v2.set(v0.k[2], -v0.k[2], v0.k[1] - v0.k[0] );
	v2.normize();
    }
    IVP_U_Point v1;    v1.inline_calc_cross_product(&v2, &v0);

    unsigned int p[5] = {0, 1, 2, 0, 1};
    unsigned int i0 = coordinate;
    unsigned int i1 = i0 + 1;
    unsigned int i2 = i1 + 1;

    set_col( IVP_COORDINATE_INDEX(p[i0]) , &v0);
    set_col( IVP_COORDINATE_INDEX(p[i1]) , &v1);
    set_col( IVP_COORDINATE_INDEX(p[i2]) , &v2);
}

// init orthornomized matrix given an As-axis in Bs coords
// result is a matrix m_Bs_f_As
void IVP_U_Matrix3::init_normized3_row(const IVP_U_Point *vp, IVP_COORDINATE_INDEX coordinate) {
    IVP_U_Point v0 = *vp;
    v0.normize();
    IVP_U_Point v2(v0.k[1], v0.k[2] - v0.k[0], -v0.k[1]);		// orthogonal vector
    if (v2.normize() == IVP_FAULT){
	v2.set(v0.k[2], -v0.k[2], v0.k[1] - v0.k[0] );
	v2.normize();
    }
    IVP_U_Point v1;   v1.inline_calc_cross_product(&v2, &v0);

    unsigned int p[5] = {0, 1, 2, 0, 1};
    unsigned int i0 = coordinate;
    unsigned int i1 = i0 + 1;
    unsigned int i2 = i1 + 1;

    rows[p[i0]] .set( &v0 );
    rows[p[i1]] .set( &v1 );
    rows[p[i2]] .set( &v2 );
}

// init orthornomized matrix given a Bs-axis and a Cs-axis in As coords
// result is a matrix m_Bs_f_As
void IVP_U_Matrix3::init_normized3_col(const IVP_U_Point *vb, IVP_COORDINATE_INDEX index_b, const IVP_U_Point *vc) {
    IVP_U_Point v0 = *vb; v0.normize();
    IVP_U_Point v2;	 v2.inline_calc_cross_product_and_normize(&v0, vc);
    IVP_U_Point v1;   v1.inline_calc_cross_product(&v2, &v0);

    unsigned int p[5] = {0, 1, 2, 0, 1};
    unsigned int i0 = index_b;
    unsigned int i1 = i0 + 1;
    unsigned int i2 = i1 + 1;

    set_col( IVP_COORDINATE_INDEX(p[i0]) , &v0);
    set_col( IVP_COORDINATE_INDEX(p[i1]) , &v1);
    set_col( IVP_COORDINATE_INDEX(p[i2]) , &v2);
}


// init orthornomized matrix given an As-axis in Bs coords
// result is a matrix m_Bs_f_As
void IVP_U_Matrix3::init_normized3_row(const IVP_U_Point *vb, IVP_COORDINATE_INDEX index_b, const IVP_U_Point *vc) {
    IVP_U_Point v0 = *vb; v0.normize();
    IVP_U_Point v2;    v2.inline_calc_cross_product_and_normize(&v0, vc);
    IVP_U_Point v1;    v1.inline_calc_cross_product(&v2, &v0);

    unsigned int p[5] = {0, 1, 2, 0, 1};
    unsigned int i0 = index_b;
    unsigned int i1 = i0 + 1;
    unsigned int i2 = i1 + 1;

    rows[p[i0]] .set( &v0);
    rows[p[i1]] .set( &v1);
    rows[p[i2]] .set( &v2);
}

void IVP_U_Matrix3::init_rows3(const IVP_U_Point *v0, const IVP_U_Point *v1, const IVP_U_Point *v2 ){
    rows[0].set(v0);
    rows[1].set(v1);
    rows[2].set(v2);
}

void IVP_U_Matrix3::init_columns3(const IVP_U_Point *v0, const IVP_U_Point *v1, const IVP_U_Point *v2 ){
    set_elem(0,0, v0->k[0]);	set_elem(1,0, v0->k[1]);	set_elem(2,0, v0->k[2]);
    set_elem(0,1, v1->k[0]);	set_elem(1,1, v1->k[1]);	set_elem(2,1, v1->k[2]);
    set_elem(0,2, v2->k[0]);	set_elem(1,2, v2->k[1]);	set_elem(2,2, v2->k[2]);
}

void IVP_U_Matrix::init_columns4(const IVP_U_Point *v0, const IVP_U_Point *v1, const IVP_U_Point *v2, const IVP_U_Point *shift ){
    set_elem(0,0, v0->k[0]);	set_elem(1,0, v0->k[1]);	set_elem(2,0, v0->k[2]);
    set_elem(0,1, v1->k[0]);	set_elem(1,1, v1->k[1]);	set_elem(2,1, v1->k[2]);
    set_elem(0,2, v2->k[0]);	set_elem(1,2, v2->k[1]);	set_elem(2,2, v2->k[2]);
    if (shift){
	vv.set(shift);
    }else{
	vv.set_to_zero();
    }
}

void IVP_U_Matrix::init_rot_multiple(const IVP_U_Point *angles, IVP_DOUBLE factor){// angles.k[0] = beta, [1] = gamma [2] = alpha,
    // mout = m_gamma * m_beta * m_alpha = m_k[1] * m_k[0] * m_k[2]
    IVP_DOUBLE x,y;
    if ( angles->k[0] ){
	x = IVP_Inline_Math::cosd( angles->k[0] * factor );
	y = IVP_Inline_Math::sind( angles->k[0] * factor);
    }else{
	x = 1.0f;
	y = 0.0f;
    }

    IVP_DOUBLE u,w;
    if ( angles->k[1] ){
	u = IVP_Inline_Math::cosd( angles->k[1] * factor );
	w = IVP_Inline_Math::sind( angles->k[1] * factor);
    }else{
	u = 1.0f;
	w = 0.0f;
    }

    IVP_DOUBLE c,s;
    if ( angles->k[2] ){
	c = IVP_Inline_Math::cosd( angles->k[2] * factor );
	s = IVP_Inline_Math::sind( angles->k[2] * factor);
    }else{
	c = 1.0f;
	s = 0.0f;
    }

    vv.set_to_zero();

    set_elem(0,0, u*c   + w*y*s);
    set_elem(0,1, w*y*c - u * s);
    set_elem(0,2, w * x);
    set_elem(1,0, x * s);
    set_elem(1,1, x * c);
    set_elem(1,2, -y);
    set_elem(2,0, y * u * s - w * c);
    set_elem(2,1, u * y * c + w * s);
    set_elem(2,2, u * x);
}


void IVP_U_Matrix::mmult4(const IVP_U_Matrix *mb, IVP_U_Matrix *m_out) const {
    inline_mmult4(mb,m_out);
}



void IVP_U_Matrix::mimult4(const IVP_U_Matrix *mb, IVP_U_Matrix *m_out) const {
    inline_mimult4(mb,m_out);
}


void IVP_U_Matrix3::mmult3(const IVP_U_Matrix3 *mb, IVP_U_Matrix3 *m_out) const {
    inline_mmult3(mb,m_out);
}

void IVP_U_Matrix3::mimult3(const IVP_U_Matrix3 *mb, IVP_U_Matrix3 *m_out) const {
    inline_mimult3(mb,m_out);
}

void IVP_U_Matrix::mi2mult4(const IVP_U_Matrix *mb, IVP_U_Matrix *m_out) const {
    /* m_out = this * mb^T */
    this->mi2mult3(mb,m_out);
    IVP_U_Point v_shift_korr;
    m_out->vmult3(&mb->vv,&v_shift_korr);
    m_out->vv.subtract(&this->vv, &v_shift_korr);
}

void IVP_U_Matrix3::mi2mult3(const IVP_U_Matrix3 *mb, IVP_U_Matrix3 *m_out) const {
    IVP_U_Point row[3];
    for (int r = 0;r<3;r++){
	mb->inline_vmult3(&rows[r], &row[r] );
    }
    m_out->rows[0].set(&row[0]);
    m_out->rows[1].set(&row[1]);
    m_out->rows[2].set(&row[2]);
}

void IVP_U_Matrix::transpose(){
	for (int i=1;i<3;i++) {
		for (int j=0;j<i;j++){
		    IVP_DOUBLE v = get_elem(i,j);
			set_elem( i,j, get_elem(j,i));
			set_elem( j,i,v);
		}
	}
	IVP_U_Point v_shift_korr;	
	this->vmult3(&this->vv,&v_shift_korr);
	vv.set_negative(&v_shift_korr);
};

void IVP_U_Matrix3::transpose3(){
	for (int i=1;i<3;i++) {
		for (int j=0;j<i;j++){
		    IVP_DOUBLE v = get_elem(i,j);
			set_elem( i,j, get_elem(j,i));
			set_elem( j,i,v);
		}
	}
};

void IVP_U_Matrix3::set_transpose3(const IVP_U_Matrix3 *in){
	set_elem( 0,0, in->get_elem(0,0));
	set_elem( 1,1, in->get_elem(1,1));
	set_elem( 2,2, in->get_elem(2,2));
	for (int i=1;i<3;i++) {
		for (int j=0;j<i;j++){
		    IVP_DOUBLE v = in->get_elem(i,j);
			set_elem( i,j, in->get_elem(j,i));
			set_elem( j,i,v);
		}
	}
};

void IVP_U_Matrix::set_transpose(const IVP_U_Matrix *in){
	set_elem( 0,0, in->get_elem(0,0));
	set_elem( 1,1, in->get_elem(1,1));
	set_elem( 2,2, in->get_elem(2,2));
	for (int i=1;i<3;i++) {
		for (int j=0;j<i;j++){
		    IVP_DOUBLE v = in->get_elem(i,j);
			set_elem( i,j, in->get_elem(j,i));
			set_elem( j,i,v);
		}
	}

	IVP_U_Point v_shift_korr;	
	this->vmult3(&in->vv,&v_shift_korr);
	vv.set_negative(&v_shift_korr);
};

#if !defined(IVP_NO_DOUBLE)
void IVP_U_Matrix::vimult4( const IVP_U_Point *p_in, IVP_U_Float_Point * p_out )const{
    inline_vimult4(p_in,p_out);
};
void IVP_U_Matrix::vimult4( const IVP_U_Point *p_in, IVP_U_Point * p_out )const{
    inline_vimult4(p_in,p_out);
};
#endif

void IVP_U_Matrix::vimult4( const IVP_U_Float_Point *p_in, IVP_U_Float_Point * p_out )const{
    inline_vimult4(p_in,p_out);
};


void IVP_U_Matrix::vmult4( const IVP_U_Float_Point *p_in, IVP_U_Float_Point * p_out )const{
    inline_vmult4(p_in,p_out);
};

#if !defined(IVP_NO_DOUBLE)
void IVP_U_Matrix::vmult4( const IVP_U_Point *p_in, IVP_U_Point * p_out )const{
    inline_vmult4(p_in,p_out);
};

void IVP_U_Matrix::vmult4( const IVP_U_Float_Point *p_in, IVP_U_Point * p_out )const{
    inline_vmult4(p_in,p_out);
};
#endif

void IVP_U_Matrix3::vmult3( const IVP_U_Float_Point *p_in, IVP_U_Float_Point * p_out )const{
    inline_vmult3(p_in,p_out);
};

#if !defined(IVP_NO_DOUBLE)
void IVP_U_Matrix3::vmult3( const IVP_U_Point *p_in, IVP_U_Point * p_out )const{
    inline_vmult3(p_in,p_out);
};

void IVP_U_Matrix3::vimult3( const IVP_U_Point *p_in, IVP_U_Point * p_out )const{
    inline_vimult3(p_in,p_out);
};
#endif

void IVP_U_Matrix3::vimult3( const IVP_U_Float_Point *p_in, IVP_U_Float_Point * p_out )const{
    inline_vimult3(p_in,p_out);
};


void IVP_U_Matrix::shift_ws( const IVP_U_Point *v_in ) {
    this->vv.add(v_in);
};



// recursion possible , works in object coords space
void IVP_U_Matrix::shift_os( const IVP_U_Point *v_in ) {
    IVP_U_Point v_out;
    vmult3(v_in,&v_out);
    this->vv.add(&v_out);
};



IVP_ERROR_STRING IVP_U_Matrix::write_to_file(FILE *fp, const char *key)
{
    // mark beginning
    if (key){
	fprintf(fp,"\t\t%s\n",key);
    }else{
	fprintf(fp,"\t\tMATRIX_START\n");
    }

    fprintf(fp,"\t\t\tMATRIX_ROT %g %g %g %g %g %g %g %g %g\n",
	    this->get_elem(0,0), this->get_elem(0,1), this->get_elem(0,2),
	    this->get_elem(1,0), this->get_elem(1,1), this->get_elem(1,2),
	    this->get_elem(2,0), this->get_elem(2,1), this->get_elem(2,2));

    fprintf(fp,"\t\t\tMATRIX_POS %g %g %g\n",
	    this->vv.k[0], this->vv.k[1], this->vv.k[2]);

    // mark end
    fprintf(fp,"\t\tMATRIX_END\n");
    return IVP_NO_ERROR;
}


void IVP_U_Matrix::print(const char *headline){
    if (headline) printf("%s\n",headline);
    printf("mm\t%g %g %g\n\t%g %g %g\n\t%g %g %g\n",
	   this->get_elem(0,0), this->get_elem(0,1), this->get_elem(0,2),
	   this->get_elem(1,0), this->get_elem(1,1), this->get_elem(1,2),
	   this->get_elem(2,0), this->get_elem(2,1), this->get_elem(2,2));

    printf("vv\t%g %g %g\n",
	   this->vv.k[0], this->vv.k[1], this->vv.k[2]);
}

IVP_ERROR_STRING IVP_U_Matrix::read_from_file(FILE *fp)
{
    char *cmd;
    while ((cmd = p_read_first_token(fp))){

	if( strcasecmp("MATRIX_START",cmd)==0 ){
	    continue;	// skip
	}

	if( strcasecmp("MATRIX_ROT",cmd)==0 ){
	    int i;
	    for (int r=0;r<3;r++){
		for(i=0; i<3; i++){
		    set_elem(r,i,get_float());
		}
	    }
	    continue;
	}

	if( strcasecmp("MATRIX_POS",cmd)==0 ){
	    this->vv.k[0] = get_float();
	    this->vv.k[1] = get_float();
	    this->vv.k[2] = get_float();
	    continue;
	}

	if( strcasecmp("MATRIX_END",cmd)==0 ){
	    break;
	}
	printf("Load IVP_U_Matrix: Unknown Command\n'%s'!\n",
	       cmd);
	CORE;
	return "ERROR in read Matrix";

    }
    return IVP_NO_ERROR;
}


IVP_DOUBLE IVP_U_Matrix3::quad_rot_distance_to(const IVP_U_Matrix3 *m2){
    
    IVP_DOUBLE sum =  this->rows[0].quad_distance_to(&m2->rows[0]) +
	this->rows[1].quad_distance_to(&m2->rows[1]) +
	this->rows[2].quad_distance_to(&m2->rows[2]);
    return sum;
}

IVP_DOUBLE IVP_U_Matrix::quad_distance_to(const IVP_U_Matrix *m2){
    return quad_rot_distance_to(m2) + vv.quad_distance_to(&m2->vv);
}

void IVP_U_Point::calc_an_orthogonal(const IVP_U_Point *ip)
{
    // change sign of biggest coordinate
    this->set(ip);
    
    int max_pos = 0;
    IVP_DOUBLE max = 0.0f;
    int i;
    for(i=2; i>=0; i--){
	IVP_DOUBLE h = IVP_Inline_Math::fabsd(k[i]);
	if(h>max){
	    max = h;
	    max_pos = i;
	}
    }
    k[max_pos] *= -1;
    IVP_DOUBLE h;
    int other_pos = max_pos-1;
    if(other_pos<0) other_pos = 2;
    h = k[max_pos];
    k[max_pos] = k[other_pos];
    k[other_pos] = h;

    this->calc_cross_product(this, ip);	// geht auf sich selber
}