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); };