ivu_linear.cxx

hl2 source code. Do not use it illegal.

// Copyright (C) Ipion Software GmbH 1999-2000. All rights reserved.

#ifndef WIN32
#	pragma implementation "ivu_linear.hxx"
#	pragma implementation "ivu_linear_software.hxx"
#	pragma implementation "ivu_linear_double.hxx"
#	pragma implementation "ivu_linear_ps2.hxx"
#	pragma implementation "ivu_linear_macros.hxx"
#	pragma implementation "ivu_matrix_macros.hxx"
#endif

/**** INCLUDES *****/
#include <ivp_physics.hxx>
#include <ivu_matrix_macros.hxx>

#if defined(PSXII) || defined(GEKKO)
#	include <string.h>
#elif defined(LINUX) || defined(WIN32) || defined(SGI) || (defined(__MWERKS__) && defined(__POWERPC__)) || defined(GEKKO)
#	include <memory.h>
#	include <string.h>
#endif



#include <ivu_float.hxx>


IVP_DOUBLE get_float(){
	return p_atof(p_str_tok(NULL, IVP_WHITESPACE));
}


IVP_FLOAT IVP_Inline_Math::isqrt_float(IVP_FLOAT quad){
    return IVP_Fast_Math::isqrt(quad,2);

}

IVP_DOUBLE IVP_Inline_Math::isqrt_double(IVP_DOUBLE quad){
    return IVP_Fast_Math::isqrt(quad,3);
}

#if !defined(IVP_NO_DOUBLE)
void IVP_U_Point::set_interpolate(const IVP_U_Point *p0,const IVP_U_Point *p1, IVP_DOUBLE s)
{
    IVP_DOUBLE is = 1.0f - s;
    IVP_DOUBLE a = p0->k[0] * is;
    IVP_DOUBLE a2 = p1->k[0] * s;
    IVP_DOUBLE b = p0->k[1] * is;
    IVP_DOUBLE b2 = p1->k[1] * s; a += a2;
    IVP_DOUBLE c = p0->k[2] * is;
    IVP_DOUBLE c2 = p1->k[2] * s; b += b2;

    k[0] = a; k[1] = b; k[2] = c + c2;
}

void IVP_U_Point::set_interpolate(const IVP_U_Float_Point *p0,const IVP_U_Float_Point *p1, IVP_DOUBLE s)
{
    IVP_DOUBLE is = 1.0f - s;
    IVP_DOUBLE a = p0->k[0] * is;
    IVP_DOUBLE a2 = p1->k[0] * s;
    IVP_DOUBLE b = p0->k[1] * is;
    IVP_DOUBLE b2 = p1->k[1] * s; a += a2;
    IVP_DOUBLE c = p0->k[2] * is;
    IVP_DOUBLE c2 = p1->k[2] * s; b += b2;

    k[0] = a; k[1] = b; k[2] = c + c2;
}
#endif

void IVP_U_Float_Point::set_interpolate(const IVP_U_Float_Point *p0,const IVP_U_Float_Point *p1, IVP_DOUBLE s)
{
    IVP_DOUBLE is = 1.0f - s;
    IVP_DOUBLE a = p0->k[0] * is;
    IVP_DOUBLE a2 = p1->k[0] * s;
    IVP_DOUBLE b = p0->k[1] * is;
    IVP_DOUBLE b2 = p1->k[1] * s; a += a2;
    IVP_DOUBLE c = p0->k[2] * is;
    IVP_DOUBLE c2 = p1->k[2] * s; b += b2;

    k[0] = (IVP_FLOAT)a; k[1] = (IVP_FLOAT)b; k[2] = (IVP_FLOAT)(c + c2);
}

IVP_DOUBLE IVP_U_Float_Point::real_length()const{
    return IVP_Inline_Math::sqrtd(this->quad_length());
}

#if !defined(IVP_NO_DOUBLE)
IVP_DOUBLE IVP_U_Point::real_length()const{
    return IVP_Inline_Math::sqrtd(this->quad_length());
}


IVP_RETURN_TYPE IVP_U_Point::fast_normize(){
    IVP_DOUBLE length = this->quad_length();
    if (length< P_DOUBLE_EPS ) return IVP_FAULT;
    IVP_DOUBLE f = IVP_Fast_Math::isqrt(length,2);
    this->mult(f);
    return IVP_OK;
}

IVP_DOUBLE IVP_U_Point::real_length_plus_normize() {
    IVP_DOUBLE qlength = this->quad_length();
    if (qlength < P_DOUBLE_EPS ){
      //k[0] = 1.0; k[1] = 0; k[2] = 0;
      return 0.0;
    }
    IVP_DOUBLE f = IVP_Fast_Math::isqrt(qlength,3);
    this->mult(f);
    return f * qlength; //qlength * f;
}
#endif



IVP_DOUBLE IVP_U_Float_Point::real_length_plus_normize() {
    IVP_DOUBLE qlength = this->quad_length();
    if (qlength < P_DOUBLE_EPS ){
      //k[0] = 1.0; k[1] = 0; k[2] = 0;
      return 0.0f;
    }
    IVP_DOUBLE f = IVP_Fast_Math::isqrt(qlength,3);
    this->mult(f);
    return f * qlength; //qlength * f;
}


IVP_RETURN_TYPE IVP_U_Float_Point::fast_normize(){
    IVP_DOUBLE length = this->quad_length();
    if (length< P_DOUBLE_EPS ) return IVP_FAULT;
    IVP_DOUBLE f = IVP_Fast_Math::isqrt(length,2);
    this->mult(f);
    return IVP_OK;
}


IVP_RETURN_TYPE IVP_U_Float_Point::normize(){
    IVP_DOUBLE length = this->quad_length();
    if (length< P_DOUBLE_EPS ) return IVP_FAULT;
    IVP_DOUBLE f = IVP_Fast_Math::isqrt(length,2);
    this->mult(f);
    return IVP_OK;
}

#if !defined(IVP_NO_DOUBLE)
IVP_RETURN_TYPE IVP_U_Point::normize(){
    IVP_DOUBLE length = this->quad_length();
    if (length< P_DOUBLE_EPS ) return IVP_FAULT;
    IVP_DOUBLE f = IVP_Fast_Math::isqrt(length,3);
    this->mult(f);
    return IVP_OK;
}
#endif

void IVP_U_Float_Point::line_sqrt(){
    k[0] = IVP_Inline_Math::ivp_sqrtf(k[0]);
    k[1] = IVP_Inline_Math::ivp_sqrtf(k[1]);
    k[2] = IVP_Inline_Math::ivp_sqrtf(k[2]);
}

void IVP_U_Point::set_orthogonal_part(const IVP_U_Point *vector,const IVP_U_Point *normal_v) {
    IVP_DOUBLE part_direction=normal_v->dot_product(vector);
    this->add_multiple(vector,normal_v,-part_direction);
}

void IVP_U_Point::set_orthogonal_part(const IVP_U_Point *vector,const IVP_U_Float_Point *normal_v) {
    IVP_DOUBLE part_direction= vector->dot_product(normal_v);
    this->add_multiple(vector,normal_v,-part_direction);
}


void IVP_U_Float_Point::set_orthogonal_part(const IVP_U_Float_Point *vector,const IVP_U_Float_Point *normal_v) {
    IVP_DOUBLE part_direction= vector->dot_product(normal_v);
    this->add_multiple(vector,normal_v,-part_direction);
}

#if !defined(IVP_NO_DOUBLE)
void IVP_U_Point::calc_cross_product(const IVP_U_Point *v1,const IVP_U_Point *v2){
    inline_calc_cross_product(v1,v2);
}
#endif

void IVP_U_Float_Point::calc_cross_product(const IVP_U_Float_Point *v1,const IVP_U_Float_Point *v2)
{
    inline_calc_cross_product(v1,v2);
}

void IVP_U_Point::solve_quadratic_equation_fast(const IVP_U_Point *p){	// points stores quadratic equ. k[2] + x*k[1] * x^2 * k[0]
    if (IVP_Inline_Math::fabsd(p->k[0])<P_DOUBLE_EPS){ // linear equation
	k[0] = 0;
	if (IVP_Inline_Math::fabsd(p->k[1])> P_DOUBLE_EPS){
	    k[0] = -1.0f;	// no result
	    return;
	}
	k[1] = k[2] = - p->k[2] / p->k[1];
	return;
    }
    IVP_DOUBLE b2 = p->k[1] * p->k[1];
    IVP_DOUBLE ac4 = p->k[0] * p->k[2] * 4.0f;
    k[0] = b2 - ac4;
    if (k[0] > 0){
	IVP_DOUBLE i2a = .5f/p->k[0];
	IVP_DOUBLE sr = IVP_Fast_Math::sqrt(k[0]);
	IVP_DOUBLE l1 = (-p->k[1] - sr ) * i2a;
	IVP_DOUBLE l2 = (-p->k[1] + sr ) * i2a;
	k[1] = l1;
	k[2] = l2;
    }
}

void IVP_U_Point::solve_quadratic_equation_accurate(const IVP_U_Point *p){	// points stores quadratic equ. k[2] + x*k[1] * x^2 * k[0]
    if (IVP_Inline_Math::fabsd(p->k[0])< P_DOUBLE_EPS){ // linear equation
	k[0] = 0;
	if (IVP_Inline_Math::fabsd(p->k[1])<P_DOUBLE_EPS){
	    k[0] = -1.0f;	// no result
	    return;
	}
	k[1] = k[2] = - p->k[2] / p->k[1];
	return;
    }
    IVP_DOUBLE b2 = p->k[1] * p->k[1];
    IVP_DOUBLE ac4 = p->k[0] * p->k[2] * 4.0f;
    k[0] = b2 - ac4;
    if (k[0]>=0){
	IVP_DOUBLE i2a = .5f/p->k[0];
	IVP_DOUBLE sr = IVP_Inline_Math::sqrtd(k[0]);
	IVP_DOUBLE l1 = (-p->k[1] - sr ) * i2a;
	IVP_DOUBLE l2 = (-p->k[1] + sr ) * i2a;
	k[1] = l1;
	k[2] = l2;
    }
}


IVP_BOOL IVP_U_Point::is_parallel(const IVP_U_Point *v_in, IVP_DOUBLE eps)const {
    IVP_U_Point v1, v2, c;
    v1.set(this);
    v2.set(v_in);
    c.calc_cross_product(&v1, &v2);
    return( (IVP_BOOL)(c.quad_length() <= eps*eps * v1.quad_length() * v2.quad_length()) );
}


#if !defined(IVP_NO_DOUBLE)
IVP_DOUBLE IVP_U_Point::fast_real_length() const {
    IVP_DOUBLE ql = this->quad_length();
    return IVP_Fast_Math::sqrt(ql);
}
#endif

IVP_DOUBLE IVP_U_Float_Point::fast_real_length() const {
    IVP_DOUBLE ql = this->quad_length();
    return IVP_Fast_Math::sqrt(ql);
}

void IVP_U_Point::print(const char *comment){
    if(comment){
	printf("%s Point %f %f %f\n",comment, k[0],k[1],k[2]);
    }else{
	printf("Point %f %f %f\n", k[0],k[1],k[2]);
    }
}

void IVP_U_Float_Point::print(const char *comment)const{
    if(comment){
	printf("%s Point %f %f %f\n",comment, k[0],k[1],k[2]);
    }else{
	printf("Point %f %f %f\n", k[0],k[1],k[2]);
    }
}


void IVP_U_Point::line_min(const IVP_U_Point *p){
    int i;
    for (i=2;i>=0;i--){
	if (p->k[i] < k[i]) k[i] = p->k[i];
    }
}

void IVP_U_Point::line_max(const IVP_U_Point *p){
    int i;
    for (i=2;i>=0;i--){
	if (p->k[i] > k[i]) k[i] = p->k[i];
    }
}


IVP_RETURN_TYPE IVP_U_Point::set_crossing(IVP_U_Hesse *h0, IVP_U_Hesse *h1, IVP_U_Hesse *h2)
{
    IVP_RETURN_TYPE rval;
    
    IVP_U_Matrix lin_equ_matrix;
    lin_equ_matrix.init_rows4(h0, h1, h2,0);

    rval = lin_equ_matrix.real_invert();

    IVP_U_Point right_side;
    right_side.set(-h0->hesse_val, -h1->hesse_val, -h2->hesse_val);

    lin_equ_matrix.vmult3(&right_side, this);

    return(rval);
}

// rotate the point around origin
void IVP_U_Point::rotate(IVP_COORDINATE_INDEX axis, IVP_FLOAT angle) {
    int hp[5] = {0, 1, 2, 0, 1};
    int *p = &hp[axis];
    IVP_DOUBLE c = IVP_Inline_Math::cosd(angle);
    IVP_DOUBLE s = IVP_Inline_Math::sind(angle);
    IVP_DOUBLE x_new = c * k[p[1]] - s * k[p[2]];
    k[p[2]] = s * k[p[1]] + c * k[p[2]];
    k[p[1]] = x_new;
}

// rotate the point around origin
void IVP_U_Float_Point::rotate(IVP_COORDINATE_INDEX axis, IVP_FLOAT angle) {
    int hp[5] = {0, 1, 2, 0, 1};
    int *p = &hp[axis];
    IVP_DOUBLE c = IVP_Inline_Math::ivp_cosf(angle);
    IVP_DOUBLE s = IVP_Inline_Math::ivp_sinf(angle);
    IVP_DOUBLE x_new = c * k[p[1]] - s * k[p[2]];
    k[p[2]] = s * k[p[1]] + c * k[p[2]];
    k[p[1]] = x_new;
}



/****************************** Matrix Operations ******************************/

IVP_RETURN_TYPE IVP_U_Matrix3::get_angles(IVP_FLOAT *alpha, IVP_FLOAT *beta, IVP_FLOAT *gamma)
{
    // returns angles in alpha, beta and gamma
    // return value of 0 means error (while normizing)

    // third column of 'this' is my looking direction
    
    IVP_U_Matrix3 mat;
    mat = *this;

    if(!mat.orthonormize()){
	printf("No valid matrix in get_angles!\n");
	return IVP_FAULT;	// could not be normized
    }
    
    IVP_U_Point v;
    IVP_U_Matrix em, rot, mat_no_g, mat_no_g_no_b;
    em.init();
    
    // calc gamma
    v.set(mat.get_elem(0,2), 0.0f, mat.get_elem(2,2));
    if(!v.normize()){
	*gamma = 0.0f;
    }else{
	*gamma = IVP_Inline_Math::atan2d(v.k[0], v.k[2]);
    }
    em.rotate(IVP_INDEX_Y,*gamma, &rot);
    rot.mimult3(&mat, &mat_no_g); // no gamma

 
    // calc beta
    v.set(0.0f, mat_no_g.get_elem(1,2), mat_no_g.get_elem(2,2));
    if(!v.normize()){
	*beta = 0.0f;
    }else{
	*beta = -IVP_Inline_Math::atan2d(v.k[1], v.k[2]);
    }
    em.rotate(IVP_INDEX_X, *beta, &rot);
    rot.mimult3(&mat_no_g, &mat_no_g_no_b); // no gamma no beta
    
   // calc alpha
    v.set(mat_no_g_no_b.get_elem(0,0), mat_no_g_no_b.get_elem(1,0), 0.0f); // OS ?? or:  1 and 4
    if(!v.normize()){
	*alpha = 0.0f;
	printf("very strange: no alpha vec in get_angles!\n");
    }else{
	*alpha = (IVP_FLOAT)IVP_Inline_Math::atan2d(v.k[1], v.k[0]); // or vice versa...
    }
    em.rotate(IVP_INDEX_Z, *alpha, &rot);

    // rot.mimult4(&mat_no_g_no_b, &mat_equals_em);
    // mat_equals_em should be the ident matrix now!

    return IVP_OK;
}

void IVP_U_Matrix3::orthogonize(){
    /* 3x3 orthogonize */
    IVP_U_Point vz, vy, v2, v3;
    
    vz.set(get_elem(0,2), get_elem(1,2), get_elem(2,2)); // z-vector shall remain (= v1)
    vy.set(get_elem(0,1), get_elem(1,1), get_elem(2,1)); // y-vector

    v2.calc_cross_product(&vy, &vz);
    v3.calc_cross_product(&vz, &v2);

    // vz, v2, v3 are in a right system
    this->init_columns3(&v2,&v3,&vz);
    return;
}

IVP_RETURN_TYPE IVP_U_Matrix3::normize(){
    /* only 3x3, sets shift to zero */
    if (!rows[0].normize()) return IVP_FAULT;
    if (!rows[1].normize()) return IVP_FAULT;
    if (!rows[2].normize()) return IVP_FAULT;
    return IVP_OK;
}

IVP_RETURN_TYPE IVP_U_Matrix3::orthonormize()
{
    this->orthogonize();
    return this->normize();