math_vector math_vector::rotate(const math_vector& offset, const math_vector& displacement)const
{
if( this->num_elements() != offset.num_elements() )
{
std::stringstream errMsg;
errMsg << "math_vector::rotate called with offset of size " << offset.num_elements()
<< " on vector of size " << this->num_elements()
<< ". They must be of the same dimension." << std::endl;
throw FlexibleExeption(errMsg.str());
}
if( offset.num_elements() != displacement.num_elements() )
{
std::stringstream errMsg;
errMsg << "math_vector::rotate called with offset of size " << offset.num_elements()
<< " and displacment of size " << displacement.num_elements()
<< ". They must be of the same dimension." << std::endl;
throw FlexibleExeption(errMsg.str());
}
if( (offset.magnatude() < sEFFECTIVELY_ZERO) || (displacement.magnatude() < sEFFECTIVELY_ZERO))
{
return math_vector(*this);
}
cout<<"rotate"<<endl;
double theta = (displacement.perpendicular(offset).magnatude())/(offset.magnatude());
theta /= 10;
math_vector X_unit = offset/offset.magnatude();
cout<<"X unit: "<<X_unit.write()<<endl;
math_vector Y_unit = displacement.perpendicular(offset);
Y_unit /= Y_unit.magnatude();
cout<<"Y unit: "<<Y_unit.write()<<endl;
cout<<"X: ";
//double X ((parallel(X_unit)).magnatude());
double X(along(X_unit));
cout<<X<<endl;
cout<<"Y: ";
//double Y ((parallel(Y_unit)).magnatude());
double Y(along(Y_unit));
cout<<Y<<endl;
//cout<<"Z"<<endl;
math_vector Z(perpendicular(offset).perpendicular(displacement));
math_vector u = X_unit*(X*cos(theta) - Y*sin(theta));
math_vector v = Y_unit*(Y*cos(theta) + X*sin(theta));
//math_vector w = Z;
math_vector result((u+v)+Z);
cout<<"input: "<<write()<<" offset: "<<offset.write()<<" displacement: "<<displacement.write()<<endl;
cout<<"theta: "<<theta<<endl;
// cout<<"X: "<<X.write()<<" Y: "<<Y.write()<<" Z: "<<Z.write()<<endl;
cout<<"u: "<<u.write()<<" v: "<<v.write()<<" w: "<<Z.write()<<endl;
cout<<"output: "<<result.write()<<endl;
cout<<"magnatude: "<<result.magnatude()<<endl<<endl;
return result;//since w = Z
}
math_vector math_vector::createFromPolar(double angle, double magnitude){
//printf("angle: %g, angle/180: %g, in radians: %g\n",angle,angle/180,M_PI*(angle/180));
double x=magnitude*sin(M_PI*(angle/180));
double y=magnitude*cos(M_PI*(angle/180));
// x=round(x*100000)/100000;
//y=round(y*100000)/100000;
// printf("(%g,%g)\n",x,y);
return math_vector(x,-y); //-y, to cause 0 to be north to be 0,-1
};
math_vector math_vector::rotate(double degrees){
// printf("%g degrees + %g degrees\n",getAngle(),degrees);
if (!degrees) return math_vector(x,y);
if (degrees==270) return rightAngle();
if (degrees==180) return (*this)*-1;
if (degrees==90) return rightAngle()*-1;
double angle=getAngle()+degrees;
while (angle<0) angle+=360;
while (angle>360) angle-=360;
return createFromPolar(angle,getMagnitude());
}
math_vector math_vector::rightAngle(){
return math_vector(y,-x);
}
math_vector math_vector::operator/(double magnitude){
if (!magnitude) magnitude=.001; //HACK!
return math_vector(x/magnitude,y/magnitude);
}
math_vector math_vector::operator*(double magnitude){
return math_vector(x*magnitude,y*magnitude);
}
math_vector math_vector::operator-(math_vector& op){
return math_vector(x-op.x,y-op.y);
};
math_vector math_vector::operator+(math_vector& op){
return math_vector(x+op.x,y+op.y);
};
