vector.cpp

basic mathematic classes used for math programming

#include "mathclass.h"


vector       
catmullrom_interpolate(m_real t, vector const& p1, vector const& p2, vector const& p3, vector const& p4){

	vector result;
	m_real t2,t3;
	
	t2 = t*t;
    t3 = t2*t;

	result = 0.5 *((2 * p2) + (-p1 + p3) * t + (2*p1 - 5*p2 + 4*p3 - p4) * t2 +
			(-p1 + 3*p2 - 3*p3 + p4) * t3); 

	return result;

}


vector       
bezier_interpolate(m_real t, vector const& p1, vector const& p2, vector const& p3, vector const& p4, int mark)
{


	quater q1;
	quater q2;
	quater q3;
    quater q4;
	quater a;
	quater b;
	quater tmp;



	vector pp1,pp2,pp3,pp4;

	pp2[0] = p2.z()*M_PI/180.0;
	pp2[1] = p2.y()*M_PI/180.0;
	pp2[2] = p2.x()*M_PI/180.0;

	pp3[0] = p3.z()*M_PI/180.0;
	pp3[1] = p3.y()*M_PI/180.0;
	pp3[2] = p3.x()*M_PI/180.0;


  switch (mark) {
	
	case 0:{
		// At the beginning of the interpolating point

			pp4[0] = p4.x()*M_PI/180.0;
			pp4[1] = p4.y()*M_PI/180.0;
			pp4[2] = p4.z()*M_PI/180.0;

			q2 =  EulerAngle2Quater( pp2 );
	        q2.normalize();

			q3 =  EulerAngle2Quater( pp3 );
	        q3.normalize();

			q4 =  EulerAngle2Quater( pp4 );
	        q4.normalize();

			q3 = closetofirst(q2,q3);
			q4 = closetofirst(q3,q4);


			a = slerp(q2,q3,1.0/3.0);
			tmp = slerp(q3,bisect(doublequater(q2,q3),q4),1.0/3.0);
			b = doublequater(tmp,q3);

			
			a = closetofirst(q2,a);
			b = closetofirst(a, b);
			q3 = closetofirst(b,q3);

			tmp = decasteljau(q2,a,b,q3,t);

	
		break;
		   }
	case 1:{
		// At the middle part of the interpolating point
		
			pp4[0] = p4.z()*M_PI/180.0;
			pp4[1] = p4.y()*M_PI/180.0;
			pp4[2] = p4.x()*M_PI/180.0;

			pp1[0] = p1.z()*M_PI/180.0;
			pp1[1] = p1.y()*M_PI/180.0;
			pp1[2] = p1.x()*M_PI/180.0;

	
			q1 =  EulerAngle2Quater( pp1 );
	        q1.normalize();

			q2 =  EulerAngle2Quater( pp2 );
	        q2.normalize();

			q3 =  EulerAngle2Quater( pp3 );
	        q3.normalize();

			q4 =  EulerAngle2Quater( pp4 );
	        q4.normalize();

			q2 = closetofirst(q1,q2);
			q3 = closetofirst(q2,q3);
			q4 = closetofirst(q3,q4);

	
			a = slerp(q2,bisect(doublequater(q1,q2),q3),1.0/3.0);
			tmp = slerp(q3,bisect(doublequater(q2,q3),q4),1.0/3.0);
			b = doublequater(tmp,q3);

			a = closetofirst(q2,a);
			b = closetofirst(a, b);
			q3 = closetofirst(b,q3);

			tmp = decasteljau(q2,a,b,q3,t);


		    break;
		   }
	case 2:{
		// At the end of the interpolating point
		
		
		   pp1[0] = p1.z()*M_PI/180.0;
			pp1[1] = p1.y()*M_PI/180.0;
			pp1[2] = p1.x()*M_PI/180.0;


  
			q1 =  EulerAngle2Quater( pp1 );
	        q1.normalize();

			q2 =  EulerAngle2Quater( pp2 );
	        q2.normalize();

			q3 =  EulerAngle2Quater( pp3 );
	        q3.normalize();


			q2 = closetofirst(q1,q2);
			q3 = closetofirst(q2,q3);

	
			a = slerp(q2,bisect(doublequater(q1,q2),q3),1.0/3.0);

			b = slerp(q3,q2,1.0/3.0);


			a = closetofirst(q2,a);
			b = closetofirst(a, b);
			q3 = closetofirst(b,q3);


			tmp = decasteljau(q2,a,b,q3,t);
		//	tmp.normalize();


			break;
		   }

	default:{
		 cout<<"error Bezier segment type.\n"<<endl;
			}
	}



	
	vector rtmp, result;

	rtmp = Quater2EulerAngle(tmp);

	
	result[0] = rtmp.z()*180.0/M_PI;
	result[1] = rtmp.y()*180.0/M_PI;
	result[2] = rtmp.x()*180.0/M_PI;
	

	return result;


}


vector       
bezier_interpolatep(m_real t, vector const& p1, vector const& p2, vector const& p3, vector const& p4, int mark)
{


	vector result,tmp, a,b;
	
	switch (mark) {
	
	case 0:{
		// At the beginning of the interpolating point

			

		//	b = doublequater(tmp,q3);


			a = p2+(p3-p2)*1.0/3.0;
				//interploate(p2,p3,1/3);
			tmp = (((p3+(p3-p2))+p4)/2-p3)*1.0/3.0;
			b = p3-tmp; 
		//	b = slerp(q3,bisect(q2,doublequater(q4,q3)),1/3);
			
		

			result = decasteljaup(p2,a,b,p3,t);
		//	tmp.normalize();
	
		break;
		   }
	case 1:{
		// At the middle part of the interpolating point
		
		   
	//		a = slerp(q2,bisect(doublequater(q1,q2),q3),1/3);
	//		tmp = slerp(q3,bisect(doublequater(q2,q3),q4),1/3);

		    tmp = (((p2+(p2-p1))+p3)/2.0-p2)*1.0/3.0;
			a = p2+tmp;
		       

			tmp = (((p3+(p3-p2))+p4)/2.0-p3)*1.0/3.0;
			b = p3-tmp; 
		
			result = decasteljaup(p2,a,b,p3,t);


		    break;
		   }
	case 2:{
		// At the end of the interpolating point
		
			//a = bisect(doublequaterk(q1,q2,0.5),q3);
			tmp = (((p2+(p2-p1))+p3)/2.0-p2)*1.0/3.0;
			a = p2+tmp;
			
			b = p3+(p2-p3)*1.0/3.0;

			result = decasteljaup(p2,a,b,p3,t);
		//	tmp.normalize();


			break;
		   }

	default:{
		 cout<<"error Bezier segment type.\n"<<endl;
			}
	}

	return result;


}



	
vector 
slerp_fixangle_interpolate(m_real t, vector const& a, vector const& b){

	quater aq;
	quater bq;
	quater rq;

	vector aa,bb;

	aa[0] = a.x()*M_PI/180;
	aa[1] = a.y()*M_PI/180;
	aa[2] = a.z()*M_PI/180;

	bb[0] = b.x()*M_PI/180;
	bb[1] = b.y()*M_PI/180;
	bb[2] = b.z()*M_PI/180;

    aq =  EulerAngle2Quater( aa );
	aq.normalize();
	bq =  EulerAngle2Quater( bb );
	bq.normalize();

	rq = slerp(aq,bq,t);
	rq.normalize();
	
	vector rtmp, result;

	rtmp = Quater2EulerAngle(rq);

	result[0] = rtmp.x()*180/M_PI;
	result[1] = rtmp.y()*180/M_PI;
	result[2] = rtmp.z()*180/M_PI;
	return result;
	
}

vector
interpolate( m_real t, vector const& a, vector const& b )
{
	return (1.0-t)*a + t*b;
}


vector
decasteljaup(vector const& q1, vector const& q2,vector const& q3, vector const& q4,m_real t){
// simplified version
//	quater pp = slerp(slerp(q1,q4,t),slerp(q2,q3,t),2*t*(1-t));

	vector p1,p2,p3,p12,p23,p;
	
	p1 = interpolate(t,q1,q2);
	p2 = interpolate(t,q2,q3);
	p3 = interpolate(t,q3,q4);

	p12 = interpolate(t,p1,p2);
	p23 = interpolate(t,p2,p3);

	p= interpolate(t,p12,p23);

	return p;
}



m_real len( vector const& v )
{
    return sqrt( v.p[0]*v.p[0] + v.p[1]*v.p[1] + v.p[2]*v.p[2] );
}

vector::vector(const vector& a){

	for(int i=0;i<3;i++){
	
		p[i] = a.p[i];
	}
	
}
vector& 
vector::operator=(const vector& a){

	for(int i=0;i<3;i++){
	
		p[i] = a.p[i];
	}

	return (*this);

}

void 
vector::normalizself(){

    m_real s = length();

    if ( s != 0.0 ){

		p[0] = p[0]/s;
		p[1] = p[1]/s;
		p[2] = p[2]/s;

    };

}
	
void   
vector::clear(){

	for(int i=0;i<3;i++){
	
		p[i] = 0;
	}
}
m_real
vector::length2() const
{
	return ( p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );

}
m_real
vector::length() const
{
    return sqrt( p[0]*p[0] + p[1]*p[1] + p[2]*p[2] );
}

matrix
vector::cross() const
{
    return matrix(	vector(  0 ,-z(), y()),
					vector( z(),  0 ,-x()),
					vector(-y(), x(),  0 ) );
}

m_real angle( vector const& a, vector const& b )
{
    return ACOS( (a%b)/(len(a)*len(b)) );
}

position vector2position( vector const& v )
{
    return position( v.x(), v.y(), v.z() );
}

vector position2vector( position const& p )
{
    return vector( p.x(), p.y(), p.z() );
}

ostream& operator<<( ostream& os, vector const& a )
{
    os << "( " << a.p[0] << " , " << a.p[1] << " , " << a.p[2] << " )";
    return os;
}

istream& operator>>( istream& is, vector& a )
{
	static char buf[256];

    is >> buf >> a.p[0] >> buf >> a.p[1] >> buf >> a.p[2] >> buf;
    return is;
}